Inverted pendulum, balancing robot

October 12th, 2008

It was a very long-timed project, albeit not a very intensive one. The robot was developed pretty slowly: if I'd place photos of each version in a line it would resemble homo sapiens evolution chart... the first and the current version differ even more then monkey and human. One film, some source code and brief description follow.

A movie as it balances can be seen on Youtube, or from this avi. The film was made just after the robot could hold it's position, so there's still plenty of things to tune but it's not very important. Mechanical part of the robot is a rubbish. It wobbles which makes it hard to estimate acceleration used for calculating an inclination, has only one encoder and a common wheel axis. Alsa it has the mass center located pretty high, but that makes the things interesting.

Controller board for this design is described here -- it uses ATmega644 µC. Currently the code reached 10kB mostly because of floating point arithmetic required for the Kalman filter (KF). That's only about 1.5k lines of code. Most of the time it was developed in the C language, then for some experiments I switched to C++ and it worked... and I liked gain made by a few C++ features (mostly by namespaces) and learnt I can control what ASM the compiler generates so I decided to stick to it (I had some minor problems like bug #36310) then when I omitted constructions like templates parametrized by templates everything worked just fine.

Program is splitted into few .cc files, but compiled as a whole. All source files (version from the movie, so working, but soon to be old) are here. They are all under GNU GPLv3+ license (KalmanSimple.cc under GPLv2+). The main program modules are:

  • Global.cc - type definitions, LED signaling, delay functions, some rather not used functions...
  • Config.cc - reads PID configuration from eeprom (faster than full-flashing everytime you change one parameter)
  • Serial.cc - buffered, interrupt-driven serial debugging
  • Motor.cc - controlling motors -- speed and direction
  • RunAvg.cc - some templates of static low-pass filters
  • Main.cc - initializes everything, calls periodically Kalman filter and steering/balancing PIDs

Next, there're few files to read data from sensors and calculate our inclination:

  • Angle.cc - using ADC reads data alternately from accel and gyro. Does some low-pass filtering.
  • Encoder.cc - reads encoder data, determines direction and keeps track of our position from the start of a program
  • KalmanSimple.cc - Kalman filter based on the source from autopilot project
Code retaining balance is in Control.cc file.

I'm able to do about 6000 measurements of acceleration and angular speed per second, but KF is pretty slow (it takes about 40800 cycles [0.00204s] to calculate single angle -- using both gyro and accel parts). That's why I call gyro updates only every 0.0028s (357 times per s) and every five such updates I additionally update angle using the accel measurement. So to do something with this 5k of pretty unnecessary accel/gyro measurements I just smooth measurement with a low-pass filter.

Because of this problems I'd really love to rewrite KF from scratch. Measure the gyro bias offline. Use simpler state matrix, then check if P matrix and kalman gain stabilize around some value (they should), when they do -- hard-code the values into the code. Then try to rewrite it using only integers in calculations.

Update: Kalman rewritten using simpler state matrix and no bias auto-tunning. Now it reads all gyro measurements (6000/s) and minimum of 357/s accel readings.

Power sent to motors in order to stabilize the robot is calculated as often as accelerometer part of KF but with a phase shift. First I call the Steer function -- a PID controller with wheel position and speed as input. It calculates the inclination we'd like to achieve to steer correctly, e.g. not to have too big speed, to keep our position etc. Then second PID takes the robot tilt from defined "destination angle" and calculates power for motors. That's it, pretty simple.

To be able to calibrate PID controllers I sent all data about measurements, PIDs input and output over a serial. The nice thing was a simple python script which received it and plotted in the realtime. For example the angle estimated by KF during balancing:

Python plotter

It wasn't very fast (it would be faster when done in OCaml though) but it was very straight-forward to write using pylab module.

During this project I've read a lot of texts about other inverted pendulum robots and they helped me a lot. If you have any questions regarding this design, used tools etc. I'll answer them with pleasure. ;)

Comment by Jose Vicente

submitted on August 3rd, 2009 at 03:06

Very nice project !!

Where can I get information on plotting with Python and reading the serial port ? . You mentioned a simple script..
Thanks in advance for any info or orientation on this matter !!

Jose

Comment by bla

submitted on August 3rd, 2009 at 08:40

http://temp.thera.be/AVR.py http://temp.thera.be/plot.py
See this. I should do some tidying up; the project evolved a lot, but the site didn't changed a bit. ;s

Comment by Jose Vicente

submitted on August 5th, 2009 at 01:09

Thank you very much for sharing this. I am learning Python I have a little more practice with Tcl Tk. I've modified your script to read bytes but I am still working to get them plotted. I have accelerometer data ( single axis) coming byte by byte like 4 times per second. I've seen you had used like a synch byte. I was thinking on the script to send a read command to the ucontroller and then read the byte, but seeing you approach I may use a synch byte too. not sure.THanks!!!

Comment by bla

submitted on August 5th, 2009 at 01:16

You can send data in ascii if you can afford it (takes more time) separated with standard \n. Then parsing is very easy. Or you can send it in packets with synchronization bytes, or if this data is really comming byte-by-byte (8bit data, not 16bit) then you don't need to synchronize. But you will for sure get to the place when you send more data. ;d (I send currently in ASCII lines containing, angle, wheel speed, accel, gyro measurements, P, I, D parts, PWM output to wheels and incoming radio control). Using gnuplot I can plot any combination of them (not in realtime though) or one in realtime. But binary sending would be much faster (see python struct module if you're doing this).

Comment by Jose Vicente

submitted on August 5th, 2009 at 06:14

Thanks, excuse my ignorance , what do you mean by ASCII ? like sending the ASCII code of the numbers ?. For instance, what the microcontroller is doing is just spitting the accelerometer 8 bit data about 4 times per second in pure hexadecimal; ie if I receive 0A , the value is 10 in dec.

I do not have a big rate , so I believe I can afford the ASCII you suggest, but not sure how to implement it.

Concerning the new line , yes it seems to be a good option , just check that with an IF and data will come right after each \n right ?

Is GNUplot able to read the serial port ?

Thanks again....Jose

Comment by bla

submitted on August 5th, 2009 at 08:18

Say you've got 10 bit measurement 0 - 1023; you can send it in 'ascii'. Just as you would printf("1023"); This will take 5 bytes max per transmission (bytes: 1, 0, 2, 3, \n) and minimum 2 bytes (0, \n). And you can send it's binary representation which will take ALWAYS 2 bytes (16 bits - measurement is 10 bit wide so it will fit). By transmitting it in hex you have range 0 - 3FF, so 4 bytes to transmit. Binary is most effective, but as you send pair of bytes for each measurement you have to know which byte is "first" (most significant), therefore you need some kind of synchronization.

Comment by Jose Vicente

submitted on August 5th, 2009 at 15:24

Thank you again. It is clear , I understand now. In my case is pretty easy because I've only use one byte 0 to FF , so in binary will be always 1 byte ( not even use \n), in ASCII it will be up to four like 2 5 6 and \n.

So, summing up, I have a char variable accel , then if I have a raw send bytes to the UART like send_byte(accel) this will be pure bynary. If I decided to use printf ( accel, "\n") then this is ASCII right ?

Thanks,

Comment by mag

submitted on December 12th, 2011 at 12:01

For plots in python, definitly Matplotlib:
http://matplotlib.sourceforge.net/

mag

Comment by bla

submitted on December 12th, 2011 at 12:35

Yeah, pylab uses matplotlib even. I guess I was pretty much confused about pylab/matplotlib when was writting this article. Nevertheless I should update it. Since that time I've redone the robot steering interface completely. It's able to plot multiple values in real-time currently, including all PID variables.

Add a comment [+] Hide the comment form [-]

I'm a bot, please ignore this comment.

I'm not a bot. Tick this box.

This is a mail of my good friend who loves rubbish email. Send him some if you want to land on my not-so-welcome lists: John Sparrow john@thera.be john(at)thera.be