Some help needed model for self-balancing

[micman2]

Hi ,
I am trying to work with IMU , work very well.
In my system I have :
Angle
Angle rate
Meters (encoder apply in motor)
Meters/sec

I need a model LQR in simulink for balancing robot

[LubinKerhuel]

Hi,

I guess that you need a state space model. LQR is the method to compute your controller (the K gain).
State space model can be done very easily using simulink using only gain blocks (that can make calculation with vectors and matrix) and a block 1/s or 1/z depending if you are modeling your system in continuous time or discrete time.

To implement the state space model (observator and controller) on a dsPIC, you must use a discrete time model...

Simulink fixed point optimization system will then give you the possibility to implement your calculation in fixed point rather than using floating point calculation. Even fixed point 32 bit calculation is faster than single or double calculations.
Lubin

[micman2]

Hi ,
Thanks you for your answer.

The state space model is: (screen)


I have taken int the site
http://robotics.ee.uwa.edu.au/theses/20 ... ce-Ooi.pdf

How can implement it in simulink, thanks to the availability

Michele

[LubinKerhuel]

Hi,

I could not open the pdf link you provide ; so I realized the model in discret time on simulink (Te = 1ms) :

Discret implementation of the Robot with a Simple Controlleur (Gain K)

ModeleRobot.png (3.61 KiB) Viewed 36867 times

If you want a model in continuous time, change 1/z with a block 1/s. You also have to change the Az,Bz,Cz,Dz and K coefficient to their continuous time equivalent.

I made few assumption and compute a controllor to stabilize the angular position of the "pendulum".
The position is not controlled.
I assume that only the angle of the "pendulum" is measured.

Note that this controllor will not work in "real life". You need to add an observator to estimate the internal state of your model or to add enough sensors to measure all internal states. Also, before implementing it, you will probably need to convert the calculation into a fixed point in order to get all calculation finished within the sampling time choosen (1ms here).

The script is :

Code: Select all

clear all;
close all;

Te=1e-3;    %% discret time
% Robot's model (continuous time)
A = [0 1 0 0 ; 0 -0.0097 11.1591 0 ; 0 0 0 1 ; 0 -0.0293 172.116 0 ];   % robot's dynamic
B = [0 1 0 0]';  % input : suppose you can apply a translating force on the robot ?
C = [0 0 1 0]; % Measurement : suppose you measure the angle
D = 0 ;     %% this is probably not 0 ?

Robot_s = ss(A,B,C,D);

%% Discretizing :
Robot_z = c2d(Robot_s,Te,'tustin');
[Az,Bz,Cz,Dz,Ts]=ssdata(Robot_z);

rank(obsv(Robot_s)) %% System not observable but
rank(ctrb(Robot_s)) %% controllable

%% LQR synthesis
R=1;
Q = diag([0 1e-2 1 1e-3]);  % you must find the best ponderation ...
[K,S,e] = dlqr(Az,Bz,Q,R);
Poles_s = log(abs(eig(Az-Bz*K)))/Te   % en continue 

Find the model and the script file that must be run first.

BalancingRobot.mdl

Simulink model

(26.38 KiB) Downloaded 1223 times

Robot_Script.m

m script

(755 Bytes) Downloaded 1178 times

[micman2]

Hi ,
Thanks you for your answer.

Opss the link not work excuse me, I upload the document in the forum.

Note that this controllor will not work in "real life". You need to add an observator to estimate the internal state of your model or to add enough sensors to measure all internal state

Yes, but is implement another sensor, angle, rate angle, encoder position of wheel and velocity and is describe in state space

mistake?

[LubinKerhuel]

If you have all sensor and if they are not too noisy, it may work

The state space model must be correct otherwise the controllor will not be adapted.

I modify the B vector that I missed up on the previous script. With this new vector, the position of the robot can be controlled.
I also set the sampling time to 0.02s (50Hz) to have the same parameters than the Thesis of Richi Chi Ooi.

The script modification are the following :

Code: Select all

...
Te=2e-2;    %% discret time
...
B = [0 0.0815 0 0.2456]';  % input : suppose you can apply a translating force on the robot ?
...
Q = diag([5e1 0 1e-2 0]);  % you must find the best ponderation ...
...

Note that at a sampling rate of 20ms, you will have time to get all calculation done within few ms (< 20ms) without problem in double precision in a dsPIC. Thus, you could directly use the dsPIC blockset to implement the calculation onboard your chip, making the design process fast.


New script and simulink model :

Robot_Script.m

new script

(860 Bytes) Downloaded 1128 times

BalancingRobot.mdl

Model with a 20ms sampling rate

(30.03 KiB) Downloaded 1209 times

If you implement this on a real robot, I would be interrested to see the results and the parameters you tune...

[LubinKerhuel]

Oups, mistakes,

zoh method must be used to discretize the model, not tustin (becaue input is fixed for 20ms). The parameters for the Q vector changes.

the script is :

Robot_Script.m

Corrected script using zoh discretization method

(781 Bytes) Downloaded 1306 times

[mecch]

I justed tested the embedded coder robot toolbox on the Lego mindstorm NXT platform for a self-balancing robot with one gyro sensor. The performance is quite good. The implementation details are well described in the attached document. Since It also uses the embedded coder for code generation, it will be very helpful.
http://www.mathworks.com/matlabcentral/ ... tType=File

Chen

[micman2]

Thanks!