This is an old revision of the document!
Ball and Beam v2
=Motivation= The ball and beam system is also called ‘balancing a ball on a beam’.It is generally linked to real control problems such as horizontally stabilizing an airplane during landing and in turbulent airflow. There are two degrees of freedom in this system. One is the ball rolling up and down the beam, and the other is beam rotating through its central axis. The aim of the system is to control the position of the ball to a desired reference point, and reject disturbances such as a push from a finger. The control signal can be derived by feeding back the position information of the ball. The control power signal goes to the servo-motor，then the torque generated from the motor drives the beam to rotate to the desired angle. Thus, the ball can be located at the desired position.<br> It is important to point out that the open loop of the system is unstable and nonlinear. The problem of ‘instability’ can be overcome by closing the open loop with a feedback controller. The nonlinear property is not significant when the beam only deflects a small angle from the horizontal position. In this case, it is possible to linearize the system. However, the non-linearities become significant when the angle of the beam from the horizontal is larger than 30 degrees, or smaller than -30 degrees. In this case, a simple linear approximation is not accurate. Thus a more advanced control technique such as nonlinear control should work better.
=Assumptions=
In order to theoretically model the ball and beam system, the assumptions are listed in the following: * The ball rolls on the beam without slip * The gearbox embedded in the motor exhibit no backlash effect * The base of the system is static with respect to the ground * The beam is able to rotate between -30 degrees to 30 degrees relative to the horizontal
=Design Solutions=
The whole structure is made up of pieces of lego mindstorm kit ; There is a lego servo-motor that tilt the beam by using a wire, and the beam is hinged on the other side as shown the picture below.
Ball's position is detected by a Infrared distance sensor for NXT (DIST-Nx-v3)that is fixed on the beam.
=Mathematical Model=
According to physics laws, mathematical equations are derived to model a system. Therefore, a detailed derivation of those equations is presented. The following equation can be derived from analysis of the force balance on the ball employing Newton’s law.
<pre> m*g*sin(α) - Fr = m*x + b*x'</pre>
where m is the mass of the ball, g is acceleration of gravity, x is the vertical
distance between the center of ball and center of the shaft, b1 is the friction constant while the ball rolls on the channel of the beam, α is the beam’s tilt angle from the horizontal position, Fr represents the externally applied force.
<br>
Based on the small angle theorem, this relation can be approximated by in the form:
<pre> m*g*α - Fr = m*x + b*x'</pre>
The angle of the beam α is linked to the angle of the motor θ according the following equation:
<pre> d*θ = α*L </pre>
where d is the radius of the pulley and L is the length of the beam;
The torque balance of the ball is expressed as:
<pre> Fr*R = Jball*(x/R) </pre>
where x is the linear acceleration of the ball, Jball is the moment of inertia of the ball, given by:
<pre> Jball = 2/5 m*R^2 </pre>
Finally, the following result can be derived (neglecting the friction constant b1):
<pre> (m*g*d*θ)/L = (m + Jball/(R^2))*x </pre>
=Control=
The open loop of the system is unstable so it can be overcome by closing the open loop with a feedback controller.
500px
The error between set point and current position of the ball must be handled before being sent to the system, and this will be the task of the PID control:
500px
The control signal is thus a sum of three terms: the P-term(which is proportional to the error), the I-term(which is proportional to the integral of the error), and the D-term(which is proportional to the derivative of the error). The controller parameters
are proportional gain Kp, integral gain Ki, and derivative gain Kd. The integral, proportional and derivative part can be interpreted as control actions based on the past, the present and the future.
Two controllers must be developed to control this system: an angular position controller for the motor controlling beam angle and a controller for the ball position and velocity on the beam. For this project, a stock controller available through the NXC programing language was used to control the motor position. This controller uses Proportional-Integral-Derivative (PID) control to set a motor position with an appropriate rise time and minimal overshoot of the set value. This controller can be accessed in two ways. One way allows the PID gains to be set manually. The second way (used in the following controller code) allows the user to control the maximum motor velocity and acceleration effectively dampening the system.
<br>
To control ball position and velocity it will be used a PD control because theoretically there isn't a steady state error and then Ki will be useless.
===Motor Position Control===
For PID control of the NXT motor on Port A:
<source lang=“c”>
OnFwdRegPID(byte outputs,
char pwr,
byte regmode,
byte p,
byte i,
byte d
)
</source>
<br>
For Velocity and Acceleration control of the NXT motor on Port A:
<source lang=“c”>
PosRegEnable(OUT_A);
PosRegSetMax(byte output,
byte max_speed,
byte max_acceleration
)
PosRegSetAngle (OUT_A, angle);
</source>
<br>
The angular position of the motor is accessed via a built in encoder using the following command:
<source lang=“c”>
curAngleInDegrees = MotorRotationCount(OUT_A);
</source>
<br>
===Ball Position and velocity Control===
To control the ball on the beam it is required the use of a sensor that is able to recognize the current position of the ball so 'High Precision Medium Range Infrared distance sensor for NXT (DIST-Nx-Medium-v3)' was chosen from Mindsensors.com:
<br>http://www.mindsensors.com/index.php?module=pagemaster&PAGE_user_op=view_page&PAGE_id=72
<br>
Documentation and examples for using this sensor can be found on their website.
<source lang=“c”>
SetSensorLowspeed(S1);
DISTNxSendCommand(S1, DIST_CMD_ENERGIZED, DIST_ADDR);
DR = DISTNxReadValue(S1, DIST_REG_DIST_LSB, 2, DIST_ADDR); Read Sensor Value
</source>
<br>
The current position of the ball will be compared with the set point in order to give the error that will be managed by the PD controller.
In order to understand the accuracy of the sensor, two tests were performed:
1) the ball is stack on the central position of the beam:
900px
where Error is the difference between the set point (the central position) and the current position of the ball (now it is stuck).
2) the ball goes from the central position to one side of the beam:
900px
Finally it is possible to understand that the sensor accuracy is very good.
===Set Point===
The PD controller needs an error between the current position of the ball (given by the sensor) and the set point. Before the control loop started, the sensor detects the central position of the ball in order to save the set point of the close loop; This was done by taking the average of many readings (ReadNum = 30) to calculate it.
<source lang=“c”>
float Error;
float set_point;
int ReadNum = 30;
int i;
…
for (i = 0; i < ReadNum; i) //cycle for calibration
{
set_point += DISTNxReadValue(S1, DIST_REG_DIST_LSB, 2, DIST_ADDR);
Wait(100);
TextOut(10, LCD_LINE3, "Calibrating...");
}
set_point = set_point/ReadNum;
Error = DISTNxReadValue(S1, DIST_REG_DIST_LSB, 2, DIST_ADDR) - set_point; // give the Error = current position - set point
</source>
<br>
===Filtering===
To improve the quality of sensor data, a very simple filter has been implemented. Before sending the signal to the controller, the sensor reads 3 times the current position of the ball provided an average reading.
<source lang="c">
int FiltNum = 3;
int j;
...
Error = 0;
for (j = 0; j < FiltNum; j++)
{
Error += DISTNxReadValue(S1, DIST_REG_DIST_LSB, 2, DIST_ADDR)- set_point;
}
Error = Error/FiltNum; // current error
</source>
<br>
===Sensor Data===
In order to understand what happens on the field, the sensor data was written on two files and was plotted on Matlab:
<source lang="c">
byte fileHandle1;
short fileSize1;
short bytesWritten1;
byte fileHandle2;
short fileSize2;
short bytesWritten2;
int t0;
int time;
...
DeleteFile("Tempo.txt");
DeleteFile("Data.txt");
CreateFile("Tempo.txt", 10000, fileHandle1);
CreateFile("Data.txt", 10000, fileHandle2);
t0 = CurrentTick();
X = Error ; // current error X //write on file time and error
time = CurrentTick()-t0;
string tmp1 = NumToStr(time);
WriteLnString(fileHandle1,tmp1, bytesWritten1);
string tmp2 = NumToStr(X);
WriteLnString(fileHandle2,tmp2, bytesWritten2);
</source>
<br>
[[File:PallaRegulate092040sec.png
integral = integral + 1/2 *(X + X_old)*dT; // integral error (useless)
angle = (Kp*X) + (Kd*DerX) + (Ki*integral); // command for motor after PD regulation PosRegSetAngle (OUT_A, -angle); // command for the motor
X_old = X; //save current error
</source> <br> =Video= F3ontAu0pzI 600px =Code= <source lang=“c”> #include “dist-nx-lib.nxc” Range Finder Device Address #define DIST_ADDR 0x02\ 0.5 1.5 #define Kp 0.92 non superare 1.1 (0.9) -92 #define Kd 0.40 0.40 #define Ki 0 byte fileHandle1; short fileSize1; short bytesWritten1; byte fileHandle2; short fileSize2; short bytesWritten2; float set_point=0; float X; long curAngleInDegrees; float Error; float X_old=0; float DerX; float integral=0; float angle; float dT = 0.1; int t0; int time; int ReadNum = 30; number of mesures for sensor calibration int FiltNum = 3; number of mesures read by the sesor during regolation int i; int j; task main() { DeleteFile(“Tempo.txt”); DeleteFile(“Data.txt”); CreateFile(“Tempo.txt”, 10000, fileHandle1); CreateFile(“Data.txt”, 10000, fileHandle2); SetSensorLowspeed(S1); empower the port 1 for sensor PosRegEnable(OUT_A); empower the port A for motor
0 0 N 0 0 0 0 1 0 0 0 -1/tau ]
B = [ 0
0 0 -k/tau ]
C = [ 1 0 0 0
0 0 1 0 ]
D = [ 0
0 ]
</pre> where : <pre> N = (m*g*d/L)/(m + (Jball/ (r^2))) tau = time constant of the motor k = steady state constant of the motor </pre> ===Matlab & Simulink simulation=== In order to solve the Riccati equation and find the vector of gains K, it is helpful the use of Matlab code: <source lang=“matlab”> clc; close all; clear all; % Iordine.m % this code find the K gains for Simulink model using “lqr” command % Antonio Tota tau= 0.34-0.120; k = 0.1750; R =0.020; %radius of the ball (m) m = 0.0026; %mass of the ball (kg) g = 9.8; %gravitational accelaration L =0.645; %0.390; %lenght of the beam (m) d = 0.015; % motor radius Jball = (2/5)*(m*(R^2)); %moment of inertia of the ball N = (m * g * d / L)/(m + (Jball/(R^2))); A = [0 1 0 0; 0 0 N 0; 0 0 0 1; 0 0 0 -1/tau]; B = [0; 0; 0; k/tau]; C = [1 0 0 0;0 0 1 0]; D = [0;0]; Cm = eye(4); Dm = [0;0;0;0]; Control = ctrb(A,B); Observe = obsv(A,C); controllability = rank(Control); Observability = rank(Observe); Tc = 0.054; %10 ms ts = ss(A,B,C,D); sysd = c2d(ts,Tc); Control1 = ctrb(sysd.a,sysd.b); Observe1 = obsv(sysd.a,sysd.c); controllability1 = rank(Control1) Observability1 = rank(Observe1) poles = eig(A) tsi = 5; %s desired settling time x1max = 0.01; %m max overshoot desired x2max = 0.3; %m/s x3max = 6.6904; %rad max motor angle desired(10° of the beam is good for linearization!!) x4max = 15; %rad/s (max angular speed of motor) umax = 50; %%dc max desired input ro = 100; %according Bryson's Rule, this is the tradeoff between control and energy (input) effort %according Bryson's Rule : Q = [1/(tsi*(x1max^2)) 0 0 0 ; 0 1/(tsi*(x2max^2)) 0 0 ; 0 0 1/(tsi*(x3max^2)) 0 ; 0 0 0 1/(tsi*(x4max^2)) ]; RR = ro/(umax^2) ; K = lqr(A,B,Q,RR) KK=dlqr(sysd.a,sysd.b,Q,RR) sim ('lqrfinalidI.mdl')
</source> Finally, the gains obtained with “lqr” command are used for the following Simulink model: 800px ===NXC code for LQR control=== ATTENTION : before running the following code, place the beam in horizontal position and the ball close to the IR sensor ( 5 cm from it) in order to simulate a disturbance rejection (for more details see the video and observe the initial position of the beam and of the ball) After simulation, It's time to test the real system by using NXC code: [http://www.mindsensors.com/index.php?module=documents&JAS_DocumentManager_op=viewDocument&JAS_Document_id=49 Required Library File: dist-nx-lib.nxc] <source lang=“c”> #include “dist-nx-lib.nxc” Range Finder Device Address #define DIST_ADDR 0x02\ 0.5 1.5 #define P 96 useless #define I 75 useless #define D 39 useless byte fileHandle1; short fileSize1; short bytesWritten1; byte fileHandle2; short fileSize2; short bytesWritten2; byte fileHandle3; short fileSize3; short bytesWritten3; byte fileHandle4; short fileSize4; short bytesWritten4; byte fileHandle5; short fileSize5; short bytesWritten5; float set_point=0.0; float K1=459.1388; float K2=594.0909; float K3=5.3849; float K4=0.4687; float X; float curAngleInDegrees; float curAngleInDegreesOld=0.0; float CurPos; float vel; float thetam; float X_old=0; float DerX; float integral=0; long angle; float r = 0.0; float dT = 0.1; float t0; float time=0.0; float time_old; int ReadNum = 30; number of mesures for sensor calibration int FiltNum = 3; number of mesures read by the sesor during regolation int i; int j; task main() { DeleteFile(“Tempo.txt”); DeleteFile(“Data.txt”); DeleteFile(“LinVel.txt”); DeleteFile(“Theta.txt”); DeleteFile(“AngVel.txt”); CreateFile(“Tempo.txt”, 1000, fileHandle1); CreateFile(“Data.txt”, 1000, fileHandle2); CreateFile(“LinVel.txt”, 1000, fileHandle3); CreateFile(“Theta.txt”, 1000, fileHandle4); CreateFile(“AngVel.txt”, 5000, fileHandle5); SetMotorRegulationTime (10); PosRegEnable(OUT_A,P,I,D); PosRegSetMax (OUT_A, 0, 0); SetSensorLowspeed(S1); empower the port 1 for sensor DISTNxSendCommand(S1, DIST_CMD_ENERGIZED, DIST_ADDR); energize the sensor for (i = 0; i < ReadNum; i++) cycle for calibration {
set_point = set_point + DISTNxReadValue(S1, DIST_REG_DIST_LSB, 2, DIST_ADDR);
Wait(100);
TextOut(10, LCD_LINE3, "Calibrating...");
}
ClearScreen();
set_point = ((set_point/ReadNum)*0.001) + 0.2; give the set point float dato = 0.0; long theta = 0; float AngVel = 0.0; float LinVel = 0.0; float xapCur = 0.0; float xaCur = 0.0 ; float u = 0.0; char Pwr; float time_old = 0.0; float thetaOld = 0.0; float CurPosOld = DISTNxReadValue(S1, DIST_REG_DIST_LSB, 2, DIST_ADDR)*0.001 - set_point; float CurPos = 0.0; float xapOld = 0.0; t0 = CurrentTick(); while (1) { cycle for regulation
for (j = 0; j < FiltNum; j++)
{
CurPos = CurPos + DISTNxReadValue(S1, DIST_REG_DIST_LSB, 2, DIST_ADDR)*0.001 - set_point;
}
CurPos = CurPos/FiltNum; // current error
X = CurPos ; // current error X //write on file time and error time = (CurrentTick()-t0) * 0.001;
theta = MotorRotationCount(OUT_A);
thetam = theta * 0.0175 ;
AngVel = (thetam - thetaOld)/(time - time_old);
LinVel = (CurPos - CurPosOld)/(time - time_old);
u = -1 * (K1*CurPos + K2*LinVel + K3*thetam + K4*AngVel);
ClearScreen(); NumOut(0, LCD_LINE1, u, false); NumOut(0, LCD_LINE2, CurPos, false); NumOut(0, LCD_LINE3, thetam, false); PosRegSetAngle(OUT_A,u);
Pwr = MotorActualSpeed (OUT_A);
string tmp1 = NumToStr(time); WriteLnString(fileHandle1,tmp1, bytesWritten1); string tmp2 = NumToStr(X); WriteLnString(fileHandle2,tmp2, bytesWritten2); string tmp3 = NumToStr(u); WriteLnString(fileHandle3,tmp3, bytesWritten3); string tmp4 = NumToStr(thetam); WriteLnString(fileHandle4,tmp4, bytesWritten4); string tmp5 = NumToStr(Pwr); WriteLnString(fileHandle5,tmp5, bytesWritten5);
time_old = time; thetaOld = thetam; CurPosOld = CurPos; xapOld = xapCur; CurPos = 0.0; //Wait(20); }
DISTNxSendCommand(S1, DIST_CMD_DEENERGIZED, DIST_ADDR); // deenegize the sensor
}
</source>
=== Video and Plot ===
Finally we start simulation and real experiments with NXT and we can see how system react to a disturbance on ball position ( x0 = -0.2 m):
sv_BwBzuJQ0
x is equal to zero when the ball is in the center position of the beam, so if a disturbance is introduced the system react to it as shown in the picture below :
800px
=== Disturbance Rejection Control ===
cCA49c4TRDc
==PD vs LQR==
It was shown how singular controller works in presence of external disturbance, but which of them is the best one?
1)First of all, in PD control I use a second microcontroller (PosRegSetAngle) inside the servomotor which is not necessary in LQR control
2)The setting of PD gains required a lot of trials and experience
3)'The PD control doesn’t consider an optimitation of energy in input used during regolation (that’s the big difference from LQR control)'
Let's understand better what does it means:
600px
600px
600px
600px
===Real Behavior ===
The previous analysis was only on simulation scenario, but it is possible to reach the same conclusions on real scenario by using nxt lego;
The previous simulink model were «ideal» in the sense that it doesn't consider some nonlinearities which are present on real system.
Firstly, I use on NXT the same gains obtained from previous ideal simulation:
600px
600px
Of course there is a discrepancy between real and simulated behavior but what I stated previously is still visible.
Overshoot and time settling is quite similar but there is a big difference in control effort as I discussed before in the simulation (about energy saving).
=== Nonlinearities ===
The difference from ideal simulation and actual reality could be due to several possible reasons:
1)System identification of motor system
2)Deadzones
3)Resolution and accuracy of sensors
4)Discrete system (and not continuos)
5)Friction between ball and beam
It could be possible to implement some of these nonlinearity in the simulink model in order to reach a simulation closer to reality:
900px
700px
By using this new Simulink model, we can observe the nonlinearities also in the simulation:
700px
In my opinion the friction between ball an beam plays important role, especially at the beginning of the regulation where u is nonzero value but the ball is stuck, and this could be bring a sort of delay as picture shown.
== Track System Control ==
Previous experiments show how the system react to an external disturbance; It is possible to implement a Track System Control which means that the system starts in Zero position (all states are equal to 0) and a command on the ball position will send to it.
In this way, it is possible to implement also an integrator which will reduce the steady state error ( mainly due to the friction ).
900px
=== Step Response ===
Let we see how the system react to a step command of x = -0.2 m :
700px
700px
In this situation , without changing Q and R matrix, the control effort (time settling = 5 s ; overshoot = 1%; umax = 50%) is not accomplished. This is mainly due to the presence of the discrete integrator and the Gain Compensation M which ,at the same time, cancel the steady state error.
=== Video ===
YSxQCgTm5DI
=Building Instructions=
In order to build by yourself the Ball and Beam System, building instructions of the final project design are proposed in the following link:
[Building Instructions = http://speedy.sh/6n2Rv/Ball-and-Beam-new-step-by-step-build-plan.pdf]
