Angle measurement for balancing robot.

[uffi]

Hi Henrik,

I am also working on a balancing robot. My platform is an ATMEGA16 with L293D motor controller ICs and I am using a two-axis accelerometer board built with Freescale MMA2260DR2 for x-axis and MMA1260DR2 for z-axis with a bandwidth of 50Hz, which should be fast enough for a pure accelerometer solution. The robot's mechanical structure is built with Fischertechnik components. I also use a rate gyro CRS03-02 by silicon sensors (bandwidth 10 Hz, which could be a little too slow).
I also asked myself whether it could be possible to use only the accelerometer alone or the rate gyro alone. To solve the problem of the acceleration of the robot influencing the measurement values of the two-axis accelerometer I would like to suggest the following to you:

First of all I would recommend to use both readings of the two-axis accelerometer. The accelerometer should be placed in the height of the motors. If you use the x-axis in direction of the robots acceleration and the z-axis in the direction to the sky or ground (starting initially without tilt), both the tilt and the acceleration can be calculated from the data using trigonometric relations and solving a squared equation. I used the drawing in the paper of Joe Le Pendulum for reference, see attached file Tilt_Angle.doc or their paper on the web, Figure 5.

Then the forward (or backward) acceleration a of the robot can be calculated to:

a= +- SqareRoot( Square(x) + Square(z) - 1)
+ for the case a>0
- for the case a<0

and

tilt=arcsin((x-z*SquareRoot(Square(x)+Square(z)-1))/(Square(x)+Square(z)))
# for the case a>0

tilt=arcsin((x+z*SquareRoot(Square(x)+Square(z)-1))/(Square(x)+Square(z)))
# for the case a<0

tilt=arcsin(x)
# for the case a=0

All values are normalized to the earth's acceleration g (g=1).

The remaining unknown figure is the sign or direction of the acceleration a to get the correct result for the tilt. This could be derived from the motor control comparing the motor power during the last two control steps before this measurement.

For the motor control it is then essential to use in addition to the tilt also the tilt change rate (for a PD regulation) by comparing the last two tilt values (differentiating), because if you only use the tilt itself, the reaction of the motors will be too slow.

If you only want to use the rate gyro alone (as was done for Joe Le Pendulum), then you need a way to compensate for the gyro's drift. A simple way of doing this could be to work with a running average for the offset. Make a reference measurement of the offset as a starting point and keep the robot in an upright position for a short time after releasing the reset button. Then update the offset with a weighted value of the rate measurement, such as:

offset_new= offset*(999/1000)+ rate/1000

for every new measurement of the rate. The weighting should be adapted to the timing period of your measurement updates. By this the drift of the gyro could be compensated. I am working on this tuning at the moment. Assuming a measurement timing period of 10ms (100Hz), the above equation would correspond to a low pass filter with 100Hz/1000=0,1 Hz filtering the dc component of the measurement signal to be used for the offset.

All this I am writing here is still in an experimental state and are thoughts I developed during my research over the last weeks. I hope you can make use of these ideas. If so, please let me know. My own robot is not yet perfectly balancing, I am tuning the PD regulation constants and the offset filtering at the moment based on the pure gyro rate approach. Next step I will do then is use the pure accelerometer approach.

Kind regards, uffi.