2 Attachment(s)
Angle measurement for balancing robot.
Hi,
(Long winded story...)
I'm working on a two wheel inverted pendulum balancing robot. (photo attached). The basic idea is to sense the angle of the robot relative to gravity and drive the wheels so that they are right under the robots center of gravity.
To do this I need to meassure the tilt-angle of the robot and for this I have an ADXL203 dual axis accelerometer (only one axis in use). Now this would work perfect if it wasn't for the fact the accelerometer measures, eeh...well acceleration.
As soon as the robot accelerates to compensate as it starts to fall over the reading from the accelerometer is no longer only proportional to the robots tilt-angle.
In comes the gyro....an ADXRS150 in my case. This is a rate gyro, ie it
outputs a voltage proportional to how fast it's turning. If this voltage is integrated over time it's proportional to the difference in tilt-angle compared to the angle at power-on. In a perfect world, that is. The problem here, as you probably know, is that the gyro's bias is drifting pretty fast so the 'angle' value isn't accurate for very long.
Now, in researching other inverted pendulum robots, (like the nBot, bBot, gyroBot, etc) they all seem to 'merge' the readings from the acceleromter and the gyro via either a complementary filter or a Kalman filter. I have found code (GNU) from a heli auto pilot project for a Kalman filter
but the math is a bit daunting and it's written in C (whatever that is... :-)... just kidding).
Anyway, have anybody of you done anything like this with PBP and/or is willing to have a look at/work with me on porting the Kalman code to PBP (if possible). Or do you have any other ideas on how to acomplish this.
(Except for buing a FAS-G inclinometer for $795, that is)
Sorry for the long post, just wanted to show that I've done my homework ;-)
I do have a short video of it actually balancing (briefly) but I can't seem to upload it here.
TIA
/Henrik Olsson.
1 Attachment(s)
balancing two-wheel robot using accelerometer or rate gyro
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.
I got my two-wheel balancer working
Henrik,
during the last weekend I had a breakthrough using the kalman filter code you provided here from the nuts&volts article. I got my robot balancing based on combined gyro and accelerometer approach. Up to now I also used only one axis of my 2-axis accelerometer (therefore containing also an error due to the measurement of the robots acceleration itself: this error seems to be filtered away by the kalman code). The regulation is only PD-control, not PID, but I plan to extend it to PID to get better control of absolute position. I have a demonstration video (6MB) which is too large to be uploaded here. If you send me your email address, I will mail it to you.
All approaches based on using only the accelerometer or the gyro were not successful so far: the pure accelerometer approach has a too noisy signal to be used for this kind of control and for the pure gyro approach I was not yet able to really compensate for the drift only by software.
Have fun, Dirk.
