Some help needed model for self-balancing

Post Reply
micman2
Posts: 20
Joined: Wed Feb 13, 2008 9:24 am

Some help needed model for self-balancing

Post by micman2 » Sat Apr 19, 2008 1:31 pm

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
Site Admin - Expert
Posts: 616
Joined: Wed Mar 07, 2007 11:23 pm
Location: Bayonne- France
Contact:

Re: Some help needed model for self-balancing

Post by LubinKerhuel » Thu Apr 24, 2008 11:26 pm

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
Posts: 20
Joined: Wed Feb 13, 2008 9:24 am

Re: Some help needed model for self-balancing

Post by micman2 » Fri Apr 25, 2008 2:33 pm

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
Attachments
MWSnap.gif

LubinKerhuel
Site Admin - Expert
Posts: 616
Joined: Wed Mar 07, 2007 11:23 pm
Location: Bayonne- France
Contact:

Re: Some help needed model for self-balancing

Post by LubinKerhuel » Sat Apr 26, 2008 3:06 pm

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)
Discret implementation of the Robot with a Simple Controlleur (Gain K)
ModeleRobot.png (3.61 KiB) Viewed 18535 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 697 times
Robot_Script.m
m script
(755 Bytes) Downloaded 652 times

micman2
Posts: 20
Joined: Wed Feb 13, 2008 9:24 am

Re: Some help needed model for self-balancing

Post by micman2 » Sat Apr 26, 2008 3:35 pm

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?
Attachments
grasser_darrigo_colombi_rufer_ieee_02.pdf
(185.13 KiB) Downloaded 626 times
2003-Balance-Ooi.pdf
(885.99 KiB) Downloaded 631 times

LubinKerhuel
Site Admin - Expert
Posts: 616
Joined: Wed Mar 07, 2007 11:23 pm
Location: Bayonne- France
Contact:

Re: Some help needed model for self-balancing

Post by LubinKerhuel » Sat Apr 26, 2008 4:36 pm

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 626 times
BalancingRobot.mdl
Model with a 20ms sampling rate
(30.03 KiB) Downloaded 678 times
If you implement this on a real robot, I would be interrested to see the results and the parameters you tune...

LubinKerhuel
Site Admin - Expert
Posts: 616
Joined: Wed Mar 07, 2007 11:23 pm
Location: Bayonne- France
Contact:

Re: Some help needed model for self-balancing

Post by LubinKerhuel » Sat Apr 26, 2008 4:44 pm

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 737 times

mecch
Posts: 2
Joined: Wed Sep 19, 2007 8:17 am
Location: Taiwan

Re: Some help needed model for self-balancing

Post by mecch » Tue Sep 09, 2008 12:15 pm

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
Posts: 20
Joined: Wed Feb 13, 2008 9:24 am

Re: Some help needed model for self-balancing

Post by micman2 » Tue Sep 09, 2008 1:04 pm

Thanks!

Post Reply

Who is online

Users browsing this forum: No registered users and 13 guests