Angle measurement for balancing robot.

[HenrikOlsson]

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.