User Tools

Site Tools


lego_ball_and_beam_v2

Differences

This shows you the differences between two versions of the page.

Link to this comparison view

Next revision
Previous revision
lego_ball_and_beam_v2 [2016/11/01 15:56] – created dwallacelego_ball_and_beam_v2 [2016/11/09 15:44] (current) dwallace
Line 1: Line 1:
 ====== Ball and Beam v2 ====== ====== Ball and Beam v2 ======
  
-=Motivation= +===== 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>+ 
 +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. 
 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. 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=+===== Assumptions =====
  
 In order to theoretically model the ball and beam system, the assumptions are listed in the following: 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 ball rolls on the beam without slip 
-* The gearbox embedded in the motor exhibit no backlash effect +  * The gearbox embedded in the motor exhibit no backlash effect 
-* The base of the system is static with respect to the ground +  * 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+  * The beam is able to rotate between -30 degrees to 30 degrees relative to the horizontal
  
-=Design Solutions=+===== 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. 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.
  
-[[File:BallAndBeam.JPG|400px]] +{{dylanw:BallAndBeam.JPG}}{{dylanw:BallAndBeam2.JPG}}\\ 
-[[File:BallAndBeam2.JPG|400px]]+
  
 Ball's position is detected by a Infrared distance sensor for NXT (DIST-Nx-v3)that is fixed on the beam. Ball's position is detected by a Infrared distance sensor for NXT (DIST-Nx-v3)that is fixed on the beam.
  
-=Mathematical Model=+===== Mathematical Model =====
  
 According to physics laws, mathematical equations are derived to model a system. Therefore, a detailed derivation of those equations is presented. 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. The following equation can be derived from analysis of the force balance on the ball employing Newton’s law.
  
-[[File:Immagine.png|500px]]+{{dylanw:Immagine.png}}\\ 
  
-<pre> m*g*sin(α) - Fr = m*x'' + b*x'</pre>+  m*g*sin(α) - Fr = m*x'' + b*x' 
 + 
 +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.
  
-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: Based on the small angle theorem, this relation can be approximated by in the form:
  
-<pre> m*g*α - Fr = m*x'' + b*x'</pre>+  m*g*α - Fr = m*x'' + b*x'
  
-The angle of the beam ''α'' is linked to the angle of the motor ''θ'' according the following equation:+The angle of the beam **α** is linked to the angle of the motor **θ** according the following equation:
  
-<pre> d*θ = α*L </pre>+  d*θ = α*L
  
-where ''d'' is the radius of the pulley and L is the length of the beam; +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: The torque balance of the ball is expressed as:
  
-<pre> Fr*R = Jball*(x''/R) </pre>+  Fr*R = Jball*(x''/R)
  
-where ''x'''' is the linear acceleration of the ball, Jball is the moment of inertia of the ball, given by:+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>+  Jball = 2/5 m*R^2
  
-Finally, the following result can be derived (neglecting the friction constant b1):+Finally, the following result can be derived (neglecting the friction constant **b1**):
  
-<pre> (m*g*d*θ)/L = (m + Jball/(R^2))*x'' </pre>+  (m*g*d*θ)/L = (m + Jball/(R^2))*x''
  
-=Control=+===== Control =====
  
 The open loop of the system is unstable so it can be overcome by closing the open loop with a feedback controller.  The open loop of the system is unstable so it can be overcome by closing the open loop with a feedback controller. 
  
-[[File:OpenClosedLoop.PNG|500px]]+{{dylanw:OpenClosedLoop.PNG}}\\ 
  
 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: 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:
  
-[[File:800px-Pid-feedback-nct-int-correct.png|500px]]+{{dylanw:800px-Pid-feedback-nct-int-correct.png}}\\ 
  
 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 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
Line 70: Line 71:
 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. 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. 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===+==== Motor Position Control ====
  
 For PID control of the NXT motor on Port A: For PID control of the NXT motor on Port A:
-<source lang="c">+ 
 +code c>
 OnFwdRegPID(byte outputs, OnFwdRegPID(byte outputs,
   char pwr,   char pwr,
Line 84: Line 85:
   byte d     byte d  
   )   )
-</source> +</code>
-<br>+
  
 For Velocity and Acceleration control of the NXT motor on Port A: For Velocity and Acceleration control of the NXT motor on Port A:
-<source lang="c">+ 
 +<code c>
 PosRegEnable(OUT_A); PosRegEnable(OUT_A);
  
Line 97: Line 98:
  
 PosRegSetAngle (OUT_A, angle); PosRegSetAngle (OUT_A, angle);
-</source+</code
-<br>+
 The angular position of the motor is accessed via a built in encoder using the following command: 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===+  curAngleInDegrees MotorRotationCount(OUT_A);
  
-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'': +==== Ball Position and velocity Control ==== 
-<br>http://www.mindsensors.com/index.php?module=pagemaster&PAGE_user_op=view_page&PAGE_id=72 + 
-<br>+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 [[http://www.mindsensors.com/index.php?module=pagemaster&PAGE_user_op=view_page&PAGE_id=72|Mindsensors.com]]
  
 Documentation and examples for using this sensor can be found on their website. Documentation and examples for using this sensor can be found on their website.
  
-<source lang="c">+<code c>
 SetSensorLowspeed(S1); SetSensorLowspeed(S1);
  
Line 119: Line 116:
  
 DR = DISTNxReadValue(S1, DIST_REG_DIST_LSB, 2, DIST_ADDR); //Read Sensor Value DR = DISTNxReadValue(S1, DIST_REG_DIST_LSB, 2, DIST_ADDR); //Read Sensor Value
-</source> +</code>
- +
-<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. 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: 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: 
  
-[[File:PallaFerma.png|900px]]+  1) the ball is stack on the central position of the beam: 
 + 
 +{{dylanw:PallaFerma.png}}\\ 
  
 where Error is the difference between the set point (the central position) and the current position of the ball (now it is stuck). 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:+  2) the ball goes from the central position to one side of the beam:
  
-[[File:PallaMuove.png|900px]]+{{dylanw:PallaMuove.png}}\\ 
  
 Finally it is possible to understand that the sensor accuracy is very good. Finally it is possible to understand that the sensor accuracy is very good.
  
-===Set Point===+==== 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. 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.
  
- +<code c>
-<source lang="c">+
 float Error; float Error;
 float set_point; float set_point;
Line 165: Line 160:
 Error = DISTNxReadValue(S1, DIST_REG_DIST_LSB, 2, DIST_ADDR) - set_point; // give the Error = current position - set point Error = DISTNxReadValue(S1, DIST_REG_DIST_LSB, 2, DIST_ADDR) - set_point; // give the Error = current position - set point
  
-</source> +</code>
-<br>+
  
-===Filtering===+==== 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. 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">+<code c>
 int FiltNum = 3; int FiltNum = 3;
 int j; int j;
Line 189: Line 183:
      Error = Error/FiltNum;  // current error      Error = Error/FiltNum;  // current error
  
-</source+</code
-<br>+ 
 +==== Sensor Data ====
  
-===Sensor Data=== 
 In order to understand what happens on the field, the sensor data was written on two files and was plotted on Matlab: 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">+<code c>
 byte fileHandle1; byte fileHandle1;
 short fileSize1; short fileSize1;
Line 223: Line 217:
 string tmp2 = NumToStr(X); string tmp2 = NumToStr(X);
 WriteLnString(fileHandle2,tmp2, bytesWritten2); WriteLnString(fileHandle2,tmp2, bytesWritten2);
-</source+</code
-<br> + 
-[[File:PallaRegulate092040sec.png|600px]]+{{dylanw:PallaRegulate092040sec.png}}\\  
 + 
 +==== Controller ====
  
-===Controller=== 
 The error found as the difference between the set point and the current position of the ball is processed by the PD controller in order to send a command to the motor. The error found as the difference between the set point and the current position of the ball is processed by the PD controller in order to send a command to the motor.
 The Kp and Kd gains were chosen by trial, error and observation. Before Kp were chosen in order to give stability to the system, and after Kd were chosen in order to reduce oscillations: use Kp to decrease the rise time and Kd to reduce overshoot and settling time. The Kp and Kd gains were chosen by trial, error and observation. Before Kp were chosen in order to give stability to the system, and after Kd were chosen in order to reduce oscillations: use Kp to decrease the rise time and Kd to reduce overshoot and settling time.
  
-<source lang="c">+<code c>
 float X float X
 float DerX float DerX
Line 253: Line 248:
  
     X_old = X; //save current error     X_old = X; //save current error
-</source+</code
-<br>+ 
 +===== Video ===== 
 + 
 +{{youtube>F3ontAu0pzI?large}}\\ 
  
-=Video= +{{dylanw:PallaRegulate092040.png}}\\ 
-{{#ev:youtube|F3ontAu0pzI}} +
-[[File:PallaRegulate092040.png|600px]]+
  
-=Code=+===== Code =====
  
-<source lang="c">+<code BallandBeamv2.nxc>
 #include "dist-nx-lib.nxc" #include "dist-nx-lib.nxc"
  
Line 427: Line 423:
 } }
  
-</source>+</code>
  
-=Cosiderations= +===== Cosiderations ===== 
-[[File:PallaRegulate092040.png|600px]]+ 
 +{{dylanw:PallaRegulate092040.png}}\\ 
  
 The picture above shows how does PD control work. The PD control is enough stable and the ball reach the central position in a few seconds but there is a problem when the ball is close to steady state (central position in the beam). The picture above shows how does PD control work. The PD control is enough stable and the ball reach the central position in a few seconds but there is a problem when the ball is close to steady state (central position in the beam).
 This might be due to reasons of structural type (stiffness of the structure or the presence of backlash in the internal gear box) or to problems related to the type of control. This might be due to reasons of structural type (stiffness of the structure or the presence of backlash in the internal gear box) or to problems related to the type of control.
  
-===Possible Solutions===+==== Possible Solutions ===
 First of all it was increasing the structural stiffness with a new support: First of all it was increasing the structural stiffness with a new support:
  
-[[File:motorsupport1.JPG|250px]] +{{dylanw:motorsupport1.JPG}}{{dylanw:DSCF1830.JPG}}\\ 
-[[File:DSCF1830.JPG|300px]]+
  
 The vibrations of the building are greatly reduced but the problem is still present. The vibrations of the building are greatly reduced but the problem is still present.
  
-=A new controller : the Linear Quadratic Regulator=+===== A new controller : the Linear Quadratic Regulator ===== 
 A state space model of a typical plant is given by: A state space model of a typical plant is given by:
  
-xdot = A*x + B*u+  xdot = A*x + B*u
  
-y = C*x + D*u+  y = C*x + D*u
  
 where A is the state matrix, B is the input matrix, C is the output matrix, D is the direct transmission matrix, x is the plant states, y is the output of the plant, and u is the vector of input signals. where A is the state matrix, B is the input matrix, C is the output matrix, D is the direct transmission matrix, x is the plant states, y is the output of the plant, and u is the vector of input signals.
Line 454: Line 452:
 The LQR stands for Linear Quadratic Regulator, which seeks a gain matrix that minimizes some performance index J: The LQR stands for Linear Quadratic Regulator, which seeks a gain matrix that minimizes some performance index J:
  
-J = ∫ [x'(t)*Q*x(t) + u'(t)R*u(t)]dt+  J = ∫ [x'(t)*Q*x(t) + u'(t)R*u(t)]dt
  
 where Q is the state weighting matrix, and R is the control weighting matrix. The optimal gain matrix is calculated by following Equation: where Q is the state weighting matrix, and R is the control weighting matrix. The optimal gain matrix is calculated by following Equation:
  
-K = R^-1*B'*P+  K = R^-1*B'*P
  
 where P is determined by solving the algebraic Riccati Equation, show in following: where P is determined by solving the algebraic Riccati Equation, show in following:
  
-A'*P + P*A - P*B*R^-1*B'*P + Q=0+  A'*P + P*A - P*B*R^-1*B'*P + Q=0
  
 It is important to note that 'a unique, positive definite solution P, which minimizes the performance index J', can be determined by last equation, given that the system is controllable. It is important to note that 'a unique, positive definite solution P, which minimizes the performance index J', can be determined by last equation, given that the system is controllable.
  
-==Mathematical Model ==+===== Mathematical Model =====
  
 It is very important to find a mathematical model very close to real system for implementing a good LQR. It is very important to find a mathematical model very close to real system for implementing a good LQR.
 The system can be divided into two subsystems in order to find transfer function from input (power to the motor) to output (ball position). The system can be divided into two subsystems in order to find transfer function from input (power to the motor) to output (ball position).
  
-[[File:paint.jpg|600px]]+{{dylanw:paint.jpg}}\\ 
  
-=== I system : motor identification ===+==== I system : motor identification ====
 In some cases, plants or systems are so complicated or unknown that dynamics cannot be derived using simple physical laws. Therefore, it is necessary to use other methods to identify mathematical models of those systems. In some cases, plants or systems are so complicated or unknown that dynamics cannot be derived using simple physical laws. Therefore, it is necessary to use other methods to identify mathematical models of those systems.
 Inputs and outputs of the system have been measured through experiments. With this data, a system identification technique can be used to estimate both structures and parameters of the system. Inputs and outputs of the system have been measured through experiments. With this data, a system identification technique can be used to estimate both structures and parameters of the system.
  
-[[File:Cattura.PNG|300px]][[File:1.PNG|400px]]+{{dylanw:Cattura.PNG}}{{dylanw:1.PNG}}\\ 
  
 In order to understand the transfer function from input power and angular speed of the motor, a plot of the step response of the motor (-30% of max power) is shown in the picture;  In order to understand the transfer function from input power and angular speed of the motor, a plot of the step response of the motor (-30% of max power) is shown in the picture; 
 Observing the response, it is possible to assume a I order for the motor system and finally  calculate the characteristic parameters: Observing the response, it is possible to assume a I order for the motor system and finally  calculate the characteristic parameters:
-[[File:2.PNG|500px]][[File:3.PNG|500px]] 
  
-=== II system : ball and beam ===+{{dylanw:2.PNG}}{{dylanw:3.PNG}}\\  
 + 
 +==== II system : ball and beam ===
 In this second subsystem it is possible the use of dynamic equations as was shown in the previous section which are summarized in this picture : In this second subsystem it is possible the use of dynamic equations as was shown in the previous section which are summarized in this picture :
  
-[[File:4.PNG|500px]]+{{dylanw:4.PNG}}\\ 
  
 Finally the transfer function of this two subsystem are used to build a good mathematical model of the ball and beam system; Finally the transfer function of this two subsystem are used to build a good mathematical model of the ball and beam system;
Line 493: Line 493:
 In order to understand the power and efficacy of LQR control, I'm going to compare it with a PD control in order to show advantages. In order to understand the power and efficacy of LQR control, I'm going to compare it with a PD control in order to show advantages.
  
-== PD CONTROL (PosRegSetAngle)==+===== PD CONTROL (PosRegSetAngle) ===== 
 The Ball and Beam is a MIMO system because a motor position regulator is required for ball position control. The Ball and Beam is a MIMO system because a motor position regulator is required for ball position control.
 Therefore, for PD simulation, I used a specific function called PosRegSetAngle in order to use an internal microcontroller (PID) of servomotor; Therefore, for PD simulation, I used a specific function called PosRegSetAngle in order to use an internal microcontroller (PID) of servomotor;
 In this way I can make a PD  control of the ball position  by introducing a second system identification on the system motor+internalcontroller: In this way I can make a PD  control of the ball position  by introducing a second system identification on the system motor+internalcontroller:
  
-[[File:5.PNG|500px]][[File:6.PNG|500px]]+{{dylanw:5.PNG}}{{dylanw:6.PNG}}\\ 
  
-=== Simulation with Simulink ===+==== Simulation with Simulink ====
  
 By using Simulink, it is possible to compare actual system with a simulated system that is build from the dynamic equations writing before. By using Simulink, it is possible to compare actual system with a simulated system that is build from the dynamic equations writing before.
  
-[[File:7.PNG|800px]]+{{dylanw:7.PNG}}\\ 
  
 In order to obtain all system parameters, a Matlab code was running before simulation : In order to obtain all system parameters, a Matlab code was running before simulation :
  
-<source lang="matlab">+<code matlab>
 clc; clc;
 close all; close all;
Line 529: Line 530:
 N = (m * g * d / L)/(m + (Jball/(R^2))); N = (m * g * d / L)/(m + (Jball/(R^2)));
  
-</source>+</code> 
 + 
 +==== NXC code for PD  control ====
  
-=== NXC code for PD  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) 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)
  
-<source lang="c">+<code PDControl.nxc>
 #include "dist-nx-lib.nxc" #include "dist-nx-lib.nxc"
  
Line 730: Line 732:
  DISTNxSendCommand(S1, DIST_CMD_DEENERGIZED, DIST_ADDR); // deenegize the sensor  DISTNxSendCommand(S1, DIST_CMD_DEENERGIZED, DIST_ADDR); // deenegize the sensor
 } }
-</source>+</code>
  
-=== Video and Plot ===+==== 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): 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):
  
-{{#ev:youtube|X-692rr3KUY}}+{{youtube>X-692rr3KUY?large}}\\ 
  
 +{{dylanw:8.PNG}}\\ 
  
-[[File:8.PNG|800px]] +===== LQR CONTROL =====
- +
-== LQR CONTROL ==+
  
 Before applying the LQR system is necessary to convert the transfer function, previously identified, into the state space configuration. Before applying the LQR system is necessary to convert the transfer function, previously identified, into the state space configuration.
 A possible state space representation can be : A possible state space representation can be :
  
-<pre>  +  Xdot = A*X + B*u  
-Xdot = A*X + B*u  +   
- +  Y = C*X + D*u 
-Y = C*X + D*u  +
-</pre>+
  
 where X is the state vector which consisting of : where X is the state vector which consisting of :
  
-x1 = x = ball position+  x1 = x = ball position
  
-x2 = xdot = ball linear velocity+  x2 = xdot = ball linear velocity
  
-x3 = theta = angular position of the motor+  x3 = theta = angular position of the motor
  
-x4 = thatadot = angular velocity of the motor+  x4 = thatadot = angular velocity of the motor
  
 u is the input command (% duty cycle) and the matrices are: u is the input command (% duty cycle) and the matrices are:
  
-<pre> +  A =  
-A =  +  [ 0 1 0 0 
-[ 0 1 0 0 +    0 0 N 0 
-  0 0 N 0 +    0 0 0 1 
-  0 0 0 1 +    0 0 0 -1/tau ]
-  0 0 0 -1/tau ]+
  
-B = +  B = 
-[ 0 +  [ 0 
-  +    
-  0  +    0  
-  -k/tau ]+    -k/tau ]
  
-C =  +  C =  
-[ 1 0 0 0 +  [ 1 0 0 0 
-  0 0 1 0 ]+    0 0 1 0 ]
  
-D =  +  D =  
-[ 0 +  [ 0 
-  0 ] +    0 ]
-</pre>+
  
 where : where :
  
-<pre> +  N = (m*g*d/L)/(m + (Jball/ (r^2)))
-N = (m*g*d/L)/(m + (Jball/ (r^2)))+
  
-tau = time constant of the motor+  tau = time constant of the motor
  
-k = steady state constant of the motor +  k = steady state constant of the motor
-</pre>+
  
-===Matlab & Simulink simulation===+==== 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: 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">+<code matlab>
 clc; clc;
 close all; close all;
Line 880: Line 875:
 sim ('lqrfinalidI.mdl') sim ('lqrfinalidI.mdl')
        
-</source>+</code>
  
 Finally, the gains obtained with "lqr" command are used for the following Simulink model: Finally, the gains obtained with "lqr" command are used for the following Simulink model:
  
-[[File:9.PNG|800px]]+{{dylanw:9.PNG}}\\  
 + 
 +==== NXC code for LQR control ====
  
-===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) 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: +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]]
-[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">+<code LQRControl.nxc>
 #include "dist-nx-lib.nxc" #include "dist-nx-lib.nxc"
  
Line 1091: Line 1086:
  DISTNxSendCommand(S1, DIST_CMD_DEENERGIZED, DIST_ADDR); // deenegize the sensor  DISTNxSendCommand(S1, DIST_CMD_DEENERGIZED, DIST_ADDR); // deenegize the sensor
 } }
-</source>+</code> 
 + 
 +==== Video and Plot ====
  
-=== 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): 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):
  
-{{#ev:youtube|sv_BwBzuJQ0}}+{{youtube>sv_BwBzuJQ0?large}}\\ 
  
 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 :  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 : 
  
-[[File:10.PNG|800px]]+{{dylanw:10.PNG}}\\ 
  
-=== Disturbance Rejection Control ===+==== Disturbance Rejection Control ====
  
- +{{youtube>cCA49c4TRDc?large}}\\ 
  
-{{#ev:youtube|cCA49c4TRDc}} +===== PD vs LQR =====
- +
-==PD vs LQR==+
  
 It was shown how singular controller works in presence of external disturbance, but which of them is the best one? 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+  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+  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)'''+  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: Let's understand better what does it means:
  
-[[File:11.PNG|600px]] +{{dylanw:11.PNG}}{{dylanw:12.PNG}}\\ 
-[[File:12.PNG|600px]]+
  
-[[File:13.PNG|600px]] +{{dylanw:13.PNG}}{{dylanw:14.PNG}}\\ 
-[[File:14.PNG|600px]]+
  
-===Real Behavior ===+==== 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 analysis was only on simulation scenario, but it is possible to reach the same conclusions on real scenario by using nxt lego;
Line 1132: Line 1124:
 Firstly, I use on NXT the same gains obtained from previous ideal simulation: Firstly, I use on NXT the same gains obtained from previous ideal simulation:
  
-[[File:15.PNG|600px]] +{{dylanw:15.PNG}}{{dylanw:16.PNG}}\\ 
-[[File:16.PNG|600px]]+
  
 Of course there is a discrepancy between real and simulated behavior but what I stated previously is still visible. 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). 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 ===+==== Nonlinearities ====
  
 The difference from ideal simulation and actual reality could be due to several possible reasons: The difference from ideal simulation and actual reality could be due to several possible reasons:
-1)System identification of motor system 
  
-2)Deadzones+  1)System identification of motor system
  
-3)Resolution and accuracy of sensors+  2)Deadzones
  
-4)Discrete system (and not continuos) +  3)Resolution and accuracy of sensors
-  +
-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:+  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:
  
-[[File:17 - Copia.PNG|900px]] +{{dylanw:17 - Copia.PNG}}{{dylanw:18 - Copia.PNG}}\\ 
-[[File:18 - Copia.PNG|700px]]+
  
 By using this new Simulink model, we can observe the nonlinearities also in the simulation: By using this new Simulink model, we can observe the nonlinearities also in the simulation:
  
-[[File:19.PNG|700px]]+{{dylanw:19.PNG}}\\ 
  
 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. 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 ==+===== 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. 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 ). In this way, it is possible to implement also an integrator which will reduce the steady state error ( mainly due to the friction ).
  
-[[File:20.PNG|900px]]+{{dylanw:20.PNG}}\\ 
  
-=== Step Response ===+==== Step Response ====
  
 Let we see how the system react to a step command of x = -0.2 m : Let we see how the system react to a step command of x = -0.2 m :
  
-[[File:21.PNG|700px]]+{{dylanw:21.PNG}}\\ 
  
-[[File:22.PNG|700px]]+{{dylanw:22.PNG}}\\ 
  
 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. 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 ===+==== Video ====
  
-{{#ev:youtube|YSxQCgTm5DI}}+{{youtube>YSxQCgTm5DI?large}}\\ 
  
-=Building Instructions=+===== 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: 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]+[[http://speedy.sh/6n2Rv/Ball-and-Beam-new-step-by-step-build-plan.pdf|Building Instructions]]
lego_ball_and_beam_v2.txt · Last modified: 2016/11/09 15:44 by dwallace