How to build a balancing robotic platform - Gyrobot

[Neo]

gryobot.jpg (40.25 KiB) Viewed 4959 times

To balance a platform one needs to know the forces acting upon it.

The force is related to the angle of tilt. The static acceleration is simply (g sin theta) where theta is the tilt away from the balance point and g is the acceleration due to gravity. With small angles sin theta can be modeled as simply theta (radians). In a dynamic system the mass of the object determines the required torque to counter the acceleration. Further more, as the system accelerates the potential torque available from the motors goes down.

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.

Gyro undressed.jpg (8.34 KiB) Viewed 4959 times

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.

adxl2002.jpg (41.05 KiB) Viewed 4959 times

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.

Wheel Encoder.jpg (35.66 KiB) Viewed 4959 times

The paper Motion Control discusses PWM motor drive, Quadrature encoding and basic motion control.

Tall gyrobot.jpg (60.4 KiB) Viewed 4959 times

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.

The Code

Code: Select all

'$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

The C code for Rate Gyro only

Rate Gyro Balance Bot.zip

(39.99 KiB) Downloaded 421 times

Its basic, its crude and it works, sort of. In the future look here for multi-tasking C code with real motion control and odometery and a user interface via radio telemetry.

Courtesy: Larry Barello