Self Balancing Robot - Part 2

In my previous post I described the problems my initial attempt at coding a balance bot ran into. Specifically the vibrations or shockwaves from the motors turning on and off swamp the accelerometer readings in noise. So instead of measuring the direction of gravity acting on my robot, the accelerometer is measuring the accerleration forces of these shock waves through the chassis. The robot just vibrated noisily and fell over because I could not measure angle of tilt of the chassis this way. Then it shot off across the room lying down. This second aspect was more concerning initially as it risked damaging my robot. So I dealt with that first.

Once the robot had toppled over, the motors are not able to right it again. There is an angle of tilt beyond which gravity wins over the power of the motors I have. At less steep angles of tilt the robot tries to drive at speed across the room, attempting to get the wheels back underneath the centre of gravity of the robot. I did not know exactly what angle the robot was unable to recover balance beyond, so I just picked 45 degrees from vertical as a starting point (I can tune this value later when I have the fundamentals working). If the robot was tilted at an angle of over 45 degrees from upright, then I set the code to keep the motors off. That way the drive is automatically off if the robots is lying down, and turns off pretty quickly when it falls over.

Now I just needed to work out how to effectively measure the angle of tilt using the IMU when the motor vibrations mask this information read from the accelerometers alone. My IMU is a 9dof chip (that's 9 degrees of freedom). These come from 3 sets of sensors each of which is measuring along 3 orthogonal vectors (x, y and z axes). We've already talked about the accelerometer (measuring changes in directional forces acting on the chip). The other 2 types of sensor are magnetic field strength, and gyroscopic. The gyros measure the rate of rotation around each axis. If we know how fast something is rotating and we measure this for a short enough time interval (so that the reading we take represents the average over the whole time period) then we can work out how many degrees we have rotated during that time. This allows us to work out the change in tilt angle of our robot since we last knew the angle of tilt, so we can update the angle in our code.

I found example code doing this where the time interval was stored in a constant for their program, based on how long their code loop took between readings. But this appeared flawed to me as what if the code in my loop varied in how long it took to run? So instead I just captured the time just after taking a reading, and then on the next reading I could work out how long had elapsed since the last reading. If you mount your IMU chip so that one of the 3 axes is parallel to the axle of your balance bot wheels then you only need to read one of the 3 axes of the IMU gyros. So with this code we can determine how much our robot tilt angle has changed in the last loop cycle. To work out the actual angle of tilt we also need to know what angle we started at. The gyros cannot tell us this, so we need to use the angle calculated from the accelerometers first. This is OK as long as we read this when the robot motors are off and the robots is not being knocked about. The perfect time is when the robot is being tilted upright to start with, or it is lying down with the motors off not moving at all.

My code design so far is structured as follows.

1. The loop is timed at the start. This means the first time around the loop the calculated time elapsed will be wrong, but we don't use it on the first pass round the loop so that does not matter. By the time we start reading the gyro we will know now long we have been in the loop. 
2. We start in accelerometer mode, and stay in that mode until we get stable tilt readings. This ensures we know the true tilt angle read from the accelerometers while the robot is stationary.
3. Once we know the tilt we can switch to gyro mode. By this time we have been round the loop more than once, so the time elasped calculation is correct for the current loop iteration. We use it to work our how much we rotated according to the gyros in that time.
4. We add/subtract the amount of rotation since the previous loop iteration from the tilt angle we last calculated, to update it to the new tilt angle.

Initialise:

    Set measure mode to Accelerometers
    Initialise last loop time to zero

Loop:
    Get the time now
    Calculate the time elapsed since last loop iteration
    (time_now - last_loop_time)
    Update last_loop_time = time_now

    If Measure mode is Accelerometers:
        Calculate angle of tilt measured from the accelerometers

        If angle is very close to last measured angle:
            Switch measure mode to Gyros

    Else (measure mode is gyros)
        Read rate of rotation around axis of wheels (using the gyro)
        Calculate how far we rotated since last tilt calc
        (degrees rotated per second * seconds elapsed)

The psuedo-code above hopefully explains the basic approach, but there is a problem with it. Once we switch to gyro mode, we only ever calculate tilt based on the previously calculated tilt. So any inaccuracy in the measurements can result in the calculated value drifting from the actual tilt angle. Only a small amount of drift will cause the robot to try to maintain an angle different to the perfect balancing angle, and the only way to keep that angle is to keep moving. So the robot will quickly start to head off at speed across the room. Now we are driving the motors we cannot rely on a precise reading from the accelerometers, so how do we correct for this drift in the gyro calculated reading?

I get a lot of help from fellow makers on Twitter, and in this case extra credit must go to Keith's Pi Tutorials for pointing me at the following resources. The problem of determining when something is level against the background vibration of motors is the same for autoleveling flying drones like quadcopters. Keith pointed me to the following website http://www.brokking.net/yabr_main.html which in turn links to a couple of tutorial videos on YouTube on autoleveling quadcopters. The first video is here and links to part 2 in the comments:

After thinking and playing with these ideas I realised that while the quadcopter example is having to level in 2 directions (pitch and roll), our balance bot case is much simpler. We only need to consider one axis of rotation, the axis of our wheels axle. Our robot only needs to rotate around this one axis to self balance. The technique in summary is to mix a small amount of the tilt angle calculated from the accelerometers alone with the tilt angle calculated from the gyros. The accelerometer calculated tilt values are very noisy, but over time their average represents the constant force of gravity. So mixing in a very small amount (~0.05%) to the gyro readings does not throw off the calculated tilt significantly, but it is enough to correct for the drift in the gyro readings over time.

With this knowledge I was able to get some initial code working which gave me a reliable tilt angle whether the motors were on or off. I was able to start experimenting with PID tuning next, but that will have to be another post. What I had realised was that this was going to take me too long to perfect for my first Pi Wars competition entry, and another idea had come along which looked much more suitable for my submission for a place in the 2019 competition. We'll look at this in my next post.