Gyrobot - a balancing robotic platform
To balance a platform one needs to know the forces acting upon it. The force is related to the angle of tilt. The base counteracts that force by applying torque to the wheels. Basically, one multiplies the angle of tilt by a factor and uses that to drive the wheels. With appropriate factors the base will balance, sort of.
The Controller (CPU)
Gyrobot is based upon the AVR Robot Controller board (ARC 1.0) which is based upon the Atmel at90s8535 processor. The ARC 1.0 board has everything needed to connect to sensors and drive the wheels with minimal bread boarding. The additional boards, below, mainly bring out the signals from the expansion headers to the switches, sensors and user LCD interface.
The sensors
On my base, I use a BEI Systron/Donner gyroscope to measure the angular displacement from vertical. Electronic Gyroscopes indicate the rate of turn. By integrating the rate, one can find the angular displacement at any given moment of time. There are two problems with this. First of all there is no absolute indication of "up". Everything is relative to when the unit started integrating. Second, all gyro's have drift over time. The unit I am using is pretty stable with a drift of only .05 deg/sec over 100 seconds. Below is a picture of the gyro removed from its protective case.
The solution to drift is to use an absolute tilt device. One such device is the Analog Devices ADXL2002 accelerometer. These devices are very sensitive to acceleration and only give an accurate indication of down (acceleration due to gravity) when the rest of the platform is stable. The basic idea is to use the tilt sensor to adjust the integrated rate gyro data when the system is at equilibrium (i.e. not rotating or accelerating relative to the ground). In the picture, below, the tilt sensor is the "squashed bug" (upside down chip) on the proto board.
The velocity, or position, of the base, relative to the ground is determined with Quadrature encoders on the left and right wheels. The encoders have two adjacent sensors that detect the boundaries between the black and white stripes on the encoder disks (below). The encoders both count the number of boundaries and keep track of which value came first (black or white) and thus the direction of rotation. This is used to keep an absolute value of wheel position. The wheel position can be used to direct the robot to a particular place in XY coordinates through the use of Odometry.
The paper Motion Control discusses PWM motor drive, Quadrature encoding and basic motion control.
5/14/2002 I modified the base to move the mass away from the wheels and made some larger wheels. The large the moment of inertia (the farther away the mass of the robot is from the wheels) the easier it is to balance. Check this out by trying to balance a broom on your hand. Then turn the broom upside down and try again. Which is easier?
The algorithm
The problem of a balancing platform can be broken into two parts:
Determining the attitude of the platform
Taking attitude information and driving the motors appropriately.
There are varying levels of sophistication that can be applied to determining the tilt of a platform. The least sophisticated solution has been hinted at above. The most sophisticated solutions use a Kalman Filter to model, estimate and refine a prediction of the current actual attitude. An intermediate solution is to simply filter the rate gyro and tilt sensor outputs and remove what is undesirable (high speed bumps/motion induced acceleration in the Tilt sensor, long term drift in the rate gyro) and sum the results to achieve a wide bandwidth, low or no drift indication of current tilt.
The current algorithms in Gyrobot only use the integrated output of the rate sensor to provide tilt information. As mentioned above, this is good for several minutes until the angular offset becomes too large for the robot and it falls over.
The balancing algorithm is pretty straightforward, once you get it to work. The power drive to the motors (e.g. the torque) is the sum of various inputs multiplied by factors. The factors are weighting values that are used to tune the response of the system. The current Gyrobot algorithm is:
Torque = AngleError * Kangle + AngularRate * Krate + VelocityError * Kvel + IntegratedVelocityError * Kivel
Angle error is the displacement from vertical (balance)
Angular rate is the rate of rotation (deg/sec)
Velocity Error is the difference between the commanded velocity and the actual velocity
Integral Velocity Error is an accumulation of velocity errors used to maintain the forward velocity over uneven terrain.
The various Kxxx are the gains for each term. The value range from 1-30 or so and are dependent upon lots of other factors like the mass of the robot, the resolution of the encoders, the loop rate for controlling the motors, etc. I determined all of my values by trial and error.
The first two terms do the balancing. The second term provides a "kicker" when the robot is bumped and also anticipates balancing when correcting (angular rate will be negative, so the faster it is rotating towards balance, the sooner that term will cancel out the Angle Error term and reduce torque to the motors) The second term essentially compensates for the rotational mass of the platform.
The last two terms are strictly for motion control. In effect they provide a "Tilt" input to the system. As the robot "tilts" forward it will start to move forward - the first two terms providing the drive. The integral term increases or decreases the tilt to keep the robot moving at the commanded speed. These terms also compensate for Gyro drift until the drift is so large that the math over or underflows.
The future
Well, better wheels, taller robot (more torque with less angle) and integrating the tilt sensor into the angular displacement code to perform long term correction of measured angle. In addition, I plan on replacing the velocity control with absolute position control and position profiling to emulate velocity. By monitoring the implied angle of the system and velocity the code should be able to tell when the robot has bumped into something and can't go over it, thus activating some sort of evasion routine.
Link to Site: http://www.barello.net/Robots/gyrobot/index.htm
To balance a platform one needs to know the forces acting upon it. The force is related to the angle of tilt. The base counteracts that force by applying torque to the wheels. Basically, one multiplies the angle of tilt by a factor and uses that to drive the wheels. With appropriate factors the base will balance, sort of.
The Controller (CPU)
Gyrobot is based upon the AVR Robot Controller board (ARC 1.0) which is based upon the Atmel at90s8535 processor. The ARC 1.0 board has everything needed to connect to sensors and drive the wheels with minimal bread boarding. The additional boards, below, mainly bring out the signals from the expansion headers to the switches, sensors and user LCD interface.
The sensors
On my base, I use a BEI Systron/Donner gyroscope to measure the angular displacement from vertical. Electronic Gyroscopes indicate the rate of turn. By integrating the rate, one can find the angular displacement at any given moment of time. There are two problems with this. First of all there is no absolute indication of "up". Everything is relative to when the unit started integrating. Second, all gyro's have drift over time. The unit I am using is pretty stable with a drift of only .05 deg/sec over 100 seconds. Below is a picture of the gyro removed from its protective case.
The solution to drift is to use an absolute tilt device. One such device is the Analog Devices ADXL2002 accelerometer. These devices are very sensitive to acceleration and only give an accurate indication of down (acceleration due to gravity) when the rest of the platform is stable. The basic idea is to use the tilt sensor to adjust the integrated rate gyro data when the system is at equilibrium (i.e. not rotating or accelerating relative to the ground). In the picture, below, the tilt sensor is the "squashed bug" (upside down chip) on the proto board.
The velocity, or position, of the base, relative to the ground is determined with Quadrature encoders on the left and right wheels. The encoders have two adjacent sensors that detect the boundaries between the black and white stripes on the encoder disks (below). The encoders both count the number of boundaries and keep track of which value came first (black or white) and thus the direction of rotation. This is used to keep an absolute value of wheel position. The wheel position can be used to direct the robot to a particular place in XY coordinates through the use of Odometry.
The paper Motion Control discusses PWM motor drive, Quadrature encoding and basic motion control.
5/14/2002 I modified the base to move the mass away from the wheels and made some larger wheels. The large the moment of inertia (the farther away the mass of the robot is from the wheels) the easier it is to balance. Check this out by trying to balance a broom on your hand. Then turn the broom upside down and try again. Which is easier?
The algorithm
The problem of a balancing platform can be broken into two parts:
Determining the attitude of the platform
Taking attitude information and driving the motors appropriately.
There are varying levels of sophistication that can be applied to determining the tilt of a platform. The least sophisticated solution has been hinted at above. The most sophisticated solutions use a Kalman Filter to model, estimate and refine a prediction of the current actual attitude. An intermediate solution is to simply filter the rate gyro and tilt sensor outputs and remove what is undesirable (high speed bumps/motion induced acceleration in the Tilt sensor, long term drift in the rate gyro) and sum the results to achieve a wide bandwidth, low or no drift indication of current tilt.
The current algorithms in Gyrobot only use the integrated output of the rate sensor to provide tilt information. As mentioned above, this is good for several minutes until the angular offset becomes too large for the robot and it falls over.
The balancing algorithm is pretty straightforward, once you get it to work. The power drive to the motors (e.g. the torque) is the sum of various inputs multiplied by factors. The factors are weighting values that are used to tune the response of the system. The current Gyrobot algorithm is:
Torque = AngleError * Kangle + AngularRate * Krate + VelocityError * Kvel + IntegratedVelocityError * Kivel
Angle error is the displacement from vertical (balance)
Angular rate is the rate of rotation (deg/sec)
Velocity Error is the difference between the commanded velocity and the actual velocity
Integral Velocity Error is an accumulation of velocity errors used to maintain the forward velocity over uneven terrain.
The various Kxxx are the gains for each term. The value range from 1-30 or so and are dependent upon lots of other factors like the mass of the robot, the resolution of the encoders, the loop rate for controlling the motors, etc. I determined all of my values by trial and error.
The first two terms do the balancing. The second term provides a "kicker" when the robot is bumped and also anticipates balancing when correcting (angular rate will be negative, so the faster it is rotating towards balance, the sooner that term will cancel out the Angle Error term and reduce torque to the motors) The second term essentially compensates for the rotational mass of the platform.
The last two terms are strictly for motion control. In effect they provide a "Tilt" input to the system. As the robot "tilts" forward it will start to move forward - the first two terms providing the drive. The integral term increases or decreases the tilt to keep the robot moving at the commanded speed. These terms also compensate for Gyro drift until the drift is so large that the math over or underflows.
The future
Well, better wheels, taller robot (more torque with less angle) and integrating the tilt sensor into the angular displacement code to perform long term correction of measured angle. In addition, I plan on replacing the velocity control with absolute position control and position profiling to emulate velocity. By monitoring the implied angle of the system and velocity the code should be able to tell when the robot has bumped into something and can't go over it, thus activating some sort of evasion routine.
Link to Site: http://www.barello.net/Robots/gyrobot/index.htm
| CODE |
'$regfile "m163def.dat" $regfile "8535def.dat" $crystal = 8000000 ' ' Gyro balance bot ' $include "arc_10.bas" Declare Function Limit9bit(byval Value As Integer) As Integer Dim Angle As Integer Dim Rate As Integer Dim Iangle As Integer Dim Velocity As Integer Dim Old_position As Integer Dim Temp As Integer Dim Servo_integral As Integer Dim Torque As Integer Dim Rate_offset As Integer Dim Ivelocity As Integer Dim Velocity_err As Integer Dim Velocity_sp As Integer ' ' Rate gyro output is 8.518 counts/deg/sec. Divide this by Tau (or multiply by ' servo loop rate in hz) to get #counts per degree angular displacement. ' Const Init_rate_offset = 564 ' initial value Const One_degree = 426 ' 8.517 / 20 ms Const One_tenth_deg = 43 ' Const Kgyro_offset = 1500 ' Rate which we adjust gyro offset Const Kvelocity = -10 ' Wheel velocity feedback Const Krate = -1 ' Rate term Const Kangle = -5 ' angle term Const Kscale = 20 ' scaling factor. ' ' Initialize Hardware ' Config Adc = Single , Prescaler = Auto Start Adc 'Turn on power to ADC ' Config Timer0 = Timer , Prescale = 256 Const Timer0_reload = 256 - 25 ' Reload value to get 1250hz interrupt rate. On Timer0 Timer0_isr ' Our timer interrupt hanlder Enable Timer0 Enable Interrupts ' Config Pinb.4 = Output ' LED for winkie ' Call Initialize_pwm() Call Initialize_encoder() ' Dim Main_timer As Byte , Led As Integer , Velocity_timer As Integer Rate_offset = Init_rate_offset Velocity_sp = 0 Wait 1 ' Do Temp = Left_encoder() Velocity = Temp - Old_position Old_position = Temp Velocity_err = Velocity_sp - Velocity Ivelocity = Ivelocity + Velocity_err ' velocity error (future) ' Print Ivelocity Rate = Getadc(left_sharp) Rate = Rate - Rate_offset Iangle = Iangle + Rate Angle = Iangle / One_tenth_deg ' Temp = Ivelocity / Kgyro_offset ' Temp = Temp / 256 ' Rate_offset = Rate_offset + Temp ' correct gyro drift Torque = Iangle * Kangle Temp = Rate * Krate Torque = Torque + Temp Temp = Velocity_err * Kvelocity Torque = Torque + Temp Torque = Torque / Kscale Torque = Limit9bit(torque) Temp = Abs(angle) If Temp > 150 Then Torque = 0 ' stop if angle exceeds 15 deg Call Set_left_motor_pwm(torque) Call Set_right_motor_pwm(torque) ' ' State machine to flash LED at 1 hz. Note: the main loop code can use ' the value of Sys_timer to measure out time in 8 ms increments. ' Incr Led If Led = 5 Then Prog_led = 0 'enable low output (ON) Elseif Led = 50 Then Prog_led = 1 'disable output (OFF) Led = 0 'reset timer End If ' While Main_timer > 0 Idle Wend Main_timer = 25 'reset timer to 20 ms (50 hz) Loop Timer0_isr: ' ' The reload value for the timer is calculated, above, for a precise .8 ms ' overflow (e.g. 25 ticks). Since the interrupts may be turned off during ' overflow, the timer might increment a count or two. To account for this ' and maintain precise timing we read the current value of the counter and ' adjust the reload value by that amount. ' Timer0 = Timer0_reload + Timer0 'reload timer adjusted for overflow If Main_timer > 0 Then Decr Main_timer 'Count down main loop timer. End If Call Doencoder Return ' ' Limit value to +/- 255 for 9 bit PWM ' ' limit to some lower number to preserve H-Bridge when things oscillate. ' Function Limit9bit(byval Value As Integer) As Integer If Value > 150 Then Limit9bit = 150 Elseif Value < -150 Then Limit9bit = -150 Else Limit9bit = Value End If End Function $include "arc_10_pwm.bas" $include "arc_10_polled_encoder.bas" End 'end program |