The BalanceBot needs to know if it’s upright to balance. If it starts tipping over in one direction the robot needs to sense how far it has tipped over and speed up the motors in that direction to counter the tipping motion. This will compensate for the tipping motion by driving the bottom of the robot under its centre of gravity and stand it back upright again.

Sensing the angle of the robot on a simple level could be achieved with a single g-force sensor or accelerometer. The accelerometer measures acceleration. Our planets gravity, acting on an object, is acceleration, so the accelerometer can be used to measure gravity. Gravity always pulls down towards ground at a constant value. By knowing the value you can calculate the angle of the sensor. If the sensor reads 1g, the sensor is “vertical” with the ground. If the accelerometer reads 0g the sensor is “horizontal” with the ground. By using trigonometry, the value of g can be used to calculate any angle of the sensor.

However this is only true if the sensor is perfectly still, and the only acceleration acting on it is gravity. As we are going to mount this in a moving robot, the acceleration of the robot will cause the accelerometer to read not just the gravitational force, but also any acceleration of the robot, which will render the reading inaccurate when calculating the angle of the robot.

Another method of measuring the angle of the robot is with a gyroscope. A gyroscope measures rotation velocity around an axis. If you know the angle of the sensor to start with, then continuously measure the rate at which it rotates, you can calculate the current angle. This has two issues; firstly you need to know your angle to start with and secondly any error in the calculation is compounded with time, so the calculated value “drifts” over time and become less accurate. However a gyroscope is immune to interface from acceleration.

So, if you can combine the accelerometer data and gyroscope data together you can use both readings to compensate for the inaccuracies of the other. The accelerometers can compensate for the gyroscope drift as they give exact readings at a known time. The gyroscopes compensate for the non-gravitational acceleration as they only measure rotation velocity.

This process is called “sensor fusion”. There are lots of method of processing the accelerometer and gyroscope data to give a single accurate angular reading. These include Kalman Filters and Complimentary Filters. The maths can get a bit complex but it’s all based on trigonometry. You could write your own code, but there are lots of existing, tested, libraries that do this for you.

Using such a library and the appropriate sensor can give your orientation in 3D space, your Roll, Pitch and Yaw angles, or Euler Angles. This traditionally uses an additional sensor to measure yaw, as yaw cannot be measured with a g sensor, as yaw rotates around the vertical axis, and so the force of gravity doesn’t change. The easiest way to measure yaw is with a magnetometer, this measures the earth magnetic field and can calculate your orientation to “North” (magnetic north).

The individual sensors all work in a single plane, so a single gyroscope only measures rotational velocity in one direction. An accelerometer only measures acceleration in one direction. Depending on how much your sensor, or robot, moves, you may need three of each sensor in each orientation (x, y & z) to calculate your orientation in 3D space. For example, as the “X” axis accelerometer rotates it will no longer be vertical and will become horizontal at which point, depending on the orientation, of the sensor either the “Y” or “Z” accelerometer begin to point down. Using 3D trigonometry of the X, Y & Z accelerometer reading, you can begin to calculate the orientation in 360 degrees. This is called “three degrees of freedom” you are measuring 3 independent parameters that define its orientation. If you add three gyroscopes, and three magnetometers, you end up with 9 Degrees of Freedom, or 9DOF.

Using a 9DOF sensor and a maths library, you can calculate (almost) any orientation accurately. This combination of sensors and maths is called an Inertial Measurement Unit or IMU. There are lots of these, ranging in accuracy and cost. They can cost thousands of pounds. However there are lots of hobby level devices too for less than £100.

You have the choice of separate sensors and use your own CPU for the maths, or use an all in one device that includes its own CPU. I have an UM6-LT Orientation Sensor from CH Robotics for a different project. They are easy to interface to and do all the Kalman filtering for you on board and provide the filtered, stable Euler Angles via a serial interface. However I was using this device in a different project.

There are many 9 DoF boards containing just the sensors. FreeIMU is a popular board, cheap and compact. SparkFun produce a 9DoF Sensor Stick which I decided on, as its cheap, small and easy to interface to.

The FreeIMU site provides a GNU GPL license library for Arduino which supports not only the FreeIMU boards but also the SparkFun boards and the DIYDrones ArduIMU. This provided the sensor fusion calculations to provide a stable combined output from the raw sensors.

The SparkFun board uses an I2C interface to communicate with the Arduino. This is a bidirectional serial interface, using just two data wires, and power.

To be clear I didn’t need a 9DOF board in this simple robot, as I only needed to calculate the robots orientation in one axis (the axis that rotates around the wheels) however, by using a 9DOF board I get full 3D orientation which means it doesn’t matter which direction I mount the sensor in, plus I intend to use the magnetometer reading to capture the “heading” the robot is traveling on so I can make it travel in a straight line. It was also fun to learn.

There are two common ways to represent a 3D orientation; Euler Angles and Quaternions.

Euler Angles can be understood easily if you imagine an old fashioned gyroscope in an aeroplane. A spinning mass is suspended in space on 3 gimbal bearings, that each rotate in a perpendicular axis. As the aeroplane tilts left and right (rolls) the gimbals rotate to keep the mass level. This angle of roll is the Roll Euler Angle. Same for Pitch, the movement of the nose of the plane up and down, and Yaw the rotation of the plane caused by the movement of the rudder. These three Euler angles define the Roll, Pitch and Yaw of the object. However Euler Angles have a flaw, as one of the axis rotates 90 degrees, it aligns with one of the other axis. In fact all three axis can align. This is called Gimbal Lock.

Gimbal lock is the loss of one degree of freedom in a three-dimensional space that occurs when the axes of two of the three gimbals are driven into a parallel configuration, “locking” the system into rotation in a degenerate two-dimensional space.

This means in some situations Euler Angles cannot accurately define our orientation. However they work fine for movements that do not exceed 90 degree rotations, which is our case if fine, as we will only be “wobbling” a few degrees from vertical. (I hope)

An alternative system is called Quaternions which does not suffer from Gimbal Lock. This uses imaginary numbers to define our three dimensional orientation, and is way too complex to try and explain here. Plus I don’t really understand it enough 🙂

So, I stuck with Euler Angles.

The FreeIMU library provides a simple function to calculate the Euler Angles :

my3IMU.getYawPitchRoll(ypr);

This fills a 3 dimensional variable with three floating point numbers, which contains the Pitch, Roll and Yaw angles.

With my sensor mounted as it is, the “Roll” angle is the rotation around the wheels, this is the only value I use:

angle = ypr[IMU_ROLL];

With just this value I can balance the robot. If the angle is positive, I drive the wheels in one directions and it the angle is negative I drive the wheels in the other direction. By proportionally driving the wheel speed, relative to the angle, the robot balances. I tested it using a cube function. i.e. Speed = Angle ^ 3

If the angle is 1 degree, the speed is 1 (slow), if the angle is 2 degrees the speed is 8 (faster), if the angle is 3 degrees the speed is 27 (quite quick), etc. The more the robot falls over the faster the wheels rotate to stand it back up. When the robot is upright the speed is very slow so the robot doesn’t overshoot and start to oscillate.

Of course it does oscillate with this very simplistic control algorithm and falls over. We need a more complicated control algorithm to stop the oscillation.

A PID Controller works well. More on that next…