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 :?:

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

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

Hi,



I could not open the pdf link you provide ; so I realized the model in discret time on simulink (Te = 1ms) :
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 :

<i>
</i>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.

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?

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 :

<i>
</i>...
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 :


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

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 :

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

Thanks!