The ball and beam system is a classic control theory problem. This simple system is an easily built testbed for control algorithms. Like the inverted pendulum, this system is open-loop unstable and therefore cannot be controlled without sensor feedback. Goal: Using a beam able to rotate via a servo, the position of a ball rolling on the beam should be maintained at a set point on the beam. This system should be robust to disturbance of the ball and and position the ball in a reasonable amount of time with little positional overshoot. The controller will be tested with static input and by tracking different functions. I will use a LEGO NXT, motor and light sensors.
<html> <iframe width=“420” height=“315” src=“http://www.youtube.com/embed/KSlPmbNaPN8” frameborder=“0” allowfullscreen></iframe> <iframe width=“420” height=“315” src=“http://www.youtube.com/embed/YYZXVt4dxAE” frameborder=“0” allowfullscreen></iframe> </html><br>
Note: If the videos do not load, just refresh the page.<br>
After drawing a free body diagram of a ball on a tilted beam, it can be seen that the acceleration due to gravity is proportional to sin(theta) where theta is the angle of the beam with 0 degrees being the horizontal orientation.
m*g*sin(theta) = m*x''
Based on the small angle theorem, this relation can be approximated by in the form:
g*theta = x''
As seen in the approximated equation of motion above, acceleration of the ball on the beam is proportional to the angle of the beam. This angle of the beam (theta), along with the position of the ball (x) and the velocity of the ball (x') can be either measured directly or obtained analytically from the accessible data.
Two controllers must be developed to control this system: am 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.
For PID control of the NXT motor on Port A:
OnFwdRegPID(byte outputs, char pwr, byte regmode, byte p, byte i, byte d )
For Velocity and Acceleration control of the NXT motor on Port A:
PosRegEnable(OUT_A); PosRegSetMax(byte output, byte max_speed, byte max_acceleration ) PosRegSetAngle (OUT_A, angle);
The angular position of the motor is accessed via a built in encoder using the following command:
curAngleInDegrees = MotorRotationCount(OUT_A);
To control the ball on the beam, a PD controller was employed to control the ball position on the beam and its positional derivative, velocity. Control of position allows the ball to be maintained at a set point on the beam while control of the velocity slows the ball as it heads toward the set point.
To control these parameters, the ball's position on the beam must be measured on the beam. This was done using two infrared range finders positioned at either end of the beam. The difference between the values obtained from each sensor was used to determine the ball position on the beam.
The sensor used was the High Precision Medium Range Infrared distance sensor for NXT (DIST-Nx-Medium-v3) from Mindsensors.com
Documentation and examples for using this sensor can be found on their website.
SetSensorLowspeed(S1); DISTNxSendCommand(S1, DIST_CMD_ENERGIZED, DIST_ADDR); DR = DISTNxReadValue(S1, DIST_REG_DIST_LSB, 2, DIST_ADDR); //Read Sensor Value
Before the control loop started, the sensors were calibrated to provide a difference of zero (0) when the ball was in the center of the beam. This was done by taking the average of many readings to calculate an offset necessary for each sensor to return a zero at the origin. This offset was then applied to every subsequent reading used for control.
int calibNum = 30; int i; ... for (i = 0; i < calibNum; i++) { DL_offset += DISTNxReadValue(S2, DIST_REG_DIST_LSB, 2, DIST_ADDR); DR_offset += DISTNxReadValue(S1, DIST_REG_DIST_LSB, 2, DIST_ADDR); Wait(100); TextOut(10, LCD_LINE3, "Calibrating..."); } DL_offset = DL_offset/calibNum; DR_offset = DR_offset/calibNum; DL_old = DISTNxReadValue(S2, DIST_REG_DIST_LSB, 2, DIST_ADDR)-DL_offset; DR_old = DISTNxReadValue(S1, DIST_REG_DIST_LSB, 2, DIST_ADDR)-DR_offset;
A very simple data averaging filter was employed to help smooth the very noisy range sensor data. This filter, before releasing a data point, took multiple readings from each sensors and provided an average reading for each sensor every iteration. Although simple, this filter helped reduce the impact of outlying data points and slowed the control loop down minimally.
int inLoopAvg = 3; int j; ... DL = 0; DR = 0; for (j = 0; j < inLoopAvg; j++) { DL += DISTNxReadValue(S2, DIST_REG_DIST_LSB, 2, DIST_ADDR)-DL_offset; DR += DISTNxReadValue(S1, DIST_REG_DIST_LSB, 2, DIST_ADDR)-DR_offset; } DL = DL/inLoopAvg; DR = DR/inLoopAvg;
The P and D gains were chosen by trial, error and observation. First, P was chosen to react to the ball's existence on either side of the zero position (center of the beam). With just a P controller, the beam would tilt clockwise with the ball on the left and counter-clockwise with the ball on the right. This controller provides the ability to react to a non-central ball location, tilting to move the ball in the direction of the center of the beam.
The position of the ball is proportional to the calibrated difference between the two range sensors.
The D gain was chosen to dampen the velocity of the ball as it rolled past the set point. When tuning the value, it is beneficial to attempt to control the ball manually (tilting the beam by hand). This trial will give insight into what type of motion is necessary to slow the ball down before it passes the set point (zero). The derivative (velocity) controller should be tuned to prevent overshoot while still achieving quick rising and settling times.
Velocity is calculated as the difference between the ball position from the last position and the current measured position divided by the time elapsed during the iteration.<br>
(Current - Previous) / deltaT
For the ball and beam system, built with a LEGO NXT motor geared down 1:5, the values used were:
KP = 0.15 KD = 0.04 MotorAngle = KP*Position + KD*Velocity
Note: If the videos do not load, just refresh the page.
// Program: BallAndBeam.nxc // Author: Alex Alspach (alexalspach@gmai.com) // Date: March 2009 // Status: Working but needs Tuning and redesign of the syste #include "dist-nx-lib.nxc" // Range Finder Device Address #define DIST_ADDR 0x02\ #define Kp 0.15 #define Kd 0.04 float DL_offset; float DR_offset; float DL; float DR; float X; long curAngleInDegrees; float DL_old; float DR_old; float errorX; float errorX_old; float DerX; float angle; float dT = 0.1; float alpha = 0.5; int calibNum = 30; int inLoopAvg = 3; int i; int j; task main() { SetSensorLowspeed(S1); SetSensorLowspeed(S2); PosRegEnable(OUT_A); PosRegSetMax (OUT_A, 80, 50); DISTNxSendCommand(S1, DIST_CMD_ENERGIZED, DIST_ADDR); DISTNxSendCommand(S2, DIST_CMD_ENERGIZED, DIST_ADDR); DL_old = DISTNxReadValue(S2, DIST_REG_DIST_LSB, 2, DIST_ADDR); DR_old = DISTNxReadValue(S1, DIST_REG_DIST_LSB, 2, DIST_ADDR); int count = 0; for (i = 0; i < calibNum; i++) { DL_offset += DISTNxReadValue(S2, DIST_REG_DIST_LSB, 2, DIST_ADDR); DR_offset += DISTNxReadValue(S1, DIST_REG_DIST_LSB, 2, DIST_ADDR); Wait(100); TextOut(10, LCD_LINE3, "Calibrating..."); } DL_offset = DL_offset/calibNum; DR_offset = DR_offset/calibNum; DL_old = DISTNxReadValue(S2, DIST_REG_DIST_LSB, 2, DIST_ADDR)-DL_offset; DR_old = DISTNxReadValue(S1, DIST_REG_DIST_LSB, 2, DIST_ADDR)-DR_offset; while (1) { DL = 0; DR = 0; for (j = 0; j < inLoopAvg; j++) { DL += DISTNxReadValue(S2, DIST_REG_DIST_LSB, 2, DIST_ADDR)-DL_offset; DR += DISTNxReadValue(S1, DIST_REG_DIST_LSB, 2, DIST_ADDR)-DR_offset; } DL = DL/inLoopAvg; DR = DR/inLoopAvg; X = (DR-DL); curAngleInDegrees = MotorRotationCount(OUT_A); ClearScreen(); NumOut(0, LCD_LINE1, DL, false); NumOut(0, LCD_LINE2, DR, false); NumOut(0, LCD_LINE4, X, false); NumOut(0, LCD_LINE6, dT, false); NumOut(0, LCD_LINE7, DerX, false); NumOut(0, LCD_LINE8, curAngleInDegrees, false); errorX = X; DerX = (errorX-errorX_old)/dT; //PD control angle = (Kp*X) + (Kd*DerX); // Limit the beam angle range if (angle > 200) angle = 200; if (angle < -200) angle = -200; PosRegSetAngle (OUT_A, angle); DR_old = DR; DL_old = DL; errorX_old = errorX; count = count + 1; } DISTNxSendCommand(S1, DIST_CMD_DEENERGIZED, DIST_ADDR); }
The ball and beam system shown in the video is a work in progress and performs poorly at this point. In this first go at the ball and beam system design and control, many lessons were learned that can be used to create a much more stable system and controller.
The beam built, along with the frame on which it rotates, is flimsy. This beam is so flimsy that it actually arcs so that either end, when horizontal, is lower than the center. This became noticeable when attempting to tune a gentle proportional controller. Although the beam would tilt, the ball would remain in a low point at either end of the beam, not reacting to the corrective motions.
The frame is also not nearly rigid enough. It allows not only the intended degree of freedom but fast motion and offset weight of the range sensors cause rotation about an upward pointing axis. This was reduced by tightening a zip-tie around the frame but ultimately, these motions still interfered with the dynamics to be controlled and, therefore, the stability of the system.
Backlash in the NXT motor amounts to almost five degrees of free motion while the motor is meant to be rotationally static. This “slop” was reduced by gearing the system down 5:1 but this was not enough. This error, along with even more rotational error due to beam driving axle torsional deformation, causes hardly precise measurements of the motor angle and poor ability to reach a specified angular position. This slop could be reduced by gearing down further or using more high precision servos.
The motor position control needs more attention. Currently, the acceleration is too high, causing a very jerky system. These jerky motion causes quick changes in velocity and therefore quick over-compensations from the velocity controllers. This motion and reactionary motion causes the system to approach instability.
The ball position data is extremely noisy, inconsistent and, therefore, unreliable. This is for many reasons. The sensors are not mounted securely enough. Sudden motion causes the mounts to flex and the range data, even if only slightly, to change. Even with a data filter, this system property causes errors from the very beginning. Secondly, the sensors are not focused perfectly on the ball as it moves and the relation between sensor output and ball location is not exactly linear.
The filtering method used (averaging multiple data points each iteration) is very basic and slows the loop more as more data is averaged. A more advanced filtering algorithm may be necessary if sensor data stays this noisy.