Many people have asked for an explanation of how TwoPotatoe solves the balancing problem, so here it is. There are, perhaps, other ways of looking at and understanding the operation of an inverted pendulum robot, but the resulting actions will always be the same. Below is a rough explanation of how I understand the robot’s operation and the solutions that result.
Algorithms
Computing the robot speed
Understanding how TwoPotatoe determines its speed and position is central to the whole algorithm. The wheels are obviously not a good indicator of the speed and position of the robot since at any given time, the wheels will be going faster or slower than the robot in order to keep it in balance. There is a point on the robot known as the Center of Oscillation (CO).
You can observe the CO by moving the speed control rapidly back and forth on a balancing robot so that the it seems to stay in one position even though the wheels are rapidly moving back and forth. The very top of the robot is also moving but in the opposite direction. There is a point between the wheels and the top of the robot where the robot moves neither forward or backwards. This is the CO.
We know the wheel speed, from the wheel encoders and we know the rate of change in the pitch (angle leaning forward or backward) from the gyroscope. We can compute the speed and position of the CO directly from this. The Center of Oscillation Speed, COS, is considered to the actual speed of the robot.
The COS is normally based on the speed of the wheels. However, the wheels are often off of the ground in jumps or over rough ground. Also, slippery surfaces, such as wet grass, loose gravel, snow or ice make this method unreliable. An alternate solution is to use the accelerometer to compute the speed. Accelerometer measurements will drift over time so these measurements must be anchored on wheel speed measurements. For TwoPotatoe, accelerometer-based COS is used in only special situations and is still under development. For SixPotatoe, the wheel speed is simply held constant whenever it is in a zero gravity situation (falling).
The Target Angle
The speed of TwoPotatoe is controlled directly from the joystick on the hand controller. The top speed of the wheels for TwoPotatoe is about 30 KPH (kilometers per hour). As a practical matter, the robot should not try go go more than about two thirds of this so that there is still some reserve left over so that it can stay upright. The speed control on the remote sets the target robot speed. So, for example, if the control is advanced 50% toward its limit, the controller sends a message to TwoPotatoe to go 10.0 KPH. The robot receives this message and goes through the following computation about 200 times a second:
- Find the Speed Error, which is the difference between the Target Speed, commanded from the controller, and COS calculated as described above.
- Convert the Speed Error to a Target Pitch Angle. This is the angle we want TwoPotatoe to be at so that it will accelerate to the Target Speed.
- Compute the Pitch Error, which is the difference between where TwoPotatoe currently is and where it needs to be.
- Compute the Target Wheel Speed from the Pitch Error. This speed is either faster or slower than the COS to make TwoPotatoe eventually reach the Target Angle. A command is sent to the motors to go at this speed.
So, for example, if TwoPotatoe is traveling at 5 KPH and is perfectly vertical — not leaning forward or backward — and the controller is set to 5 KPH, the above computation will result in a target wheel speed of 5 KPH.
Robot height
It is often stated that inverted pendulum robots should be tall. One reason given for this is that tall objects are easier to balance the short objects. For example, it is much easier to balance a broom stick than a toothpick. In fact, this is human limitation imposed by nerve conduction velocity and brain processing that shouldn’t be imposed on robots that can compute and react much much more quickly. I have built robots that are indeed very short. Furthermore, Segways and hover boards are very short and have no problem staying upright.
If a balancing robot is not performing correctly, things can often be improved by making it taller. Added height can often compensate for improper mechanical design or an inadequate balancing algorithm.
Compute Cycle
The robot starts its compute cycle every time it receives new information from the IMU. The result of the computation is a new value sent to the motor controllers. I have seen much discussion about the proper rate for this cycle with some advocating that this cycle needs to happen 400 times a second or more. I have not spent much time investigating this but I have made robots work quite well at rates as low as 20 per second. Also, in some cases TwoPotatoe takes into account data from the depth camera which can be as low as 30/second, which has no adverse affect on its performance. Mostly I run the compute cycle at about 200 per second, which is well above what is actually required. In fact, there isn’t really point in going much above 100/second since the data from the IMU is low-pass filtered such that there is little to be gained from higher rates. A higher rate confers little advantage but there is no penalty either.
Motor Control
The motor has a feedback loop such that if it is not running at the commanded speed, excess power is applied until the correct speed is reached. This way the motor is able to rapidly respond to changing conditions. So, for example, if the robot hits a steep curb, the accelerometer will detect the head pitching forward and the encoders will detect a reduction in speed. All of this will happen within 10-20 milliseconds and the motors will be able to correct this very rapidly.
Also, note that the algorithm falls apart badly at low speeds—especially at zero speed where then no interrupts are being generated. To take care of this problem, the TwoPotatoe Algorithm also monitors the motors each time the IMU is read and adds pulses when necessary.
The Code
Many people have asked to look at the code that TwoPotatoe uses so they can understand how TwoPotatoe works. The code is rather large and arcane so that it is difficult to understand. To add to this problem is the simple fact that it is still under constant modification. I intend to clean up the algorithm someday so that it is properly structured and understandable. I have wanted to do this for the past two years but I haven’t had the time to do so. Realistically, this is not going to happen any time soon. It is not that I am trying to keep the code a secret. I am simply too busy – or lazy – to publish it. Sorry.
There is nothing new or remarkable about the algorithm since it is just based on basic physical characteristics of the robot. The best way to look at these algorithms is to consult the code for FivePotatoe or the Instructable for SixPotatoe.
PID Rant
Many people have noted that most (all?) self-balancing robots use the PID algorithm to stay upright and that TwoPotatoe does not use this algorithm. This is because a PID algorithm always produces a sub-optimal solution to the problem. It is very difficult, if not impossible, to tune a PID algorithm do deal adequately with complex maneuvers, rough ground, and inclines. As the many videos on YouTube indicate, they can be made to work fairly well, but not ideally, on a flat surface. The Wikipedia article on PID algorithms states the issue very well:
While PID controllers are applicable to many control problems, and often perform satisfactorily without any improvements or only coarse tuning, they can perform poorly in some applications, and do not in general provide optimal control.
Since understanding the motion of a self-standing robot requires only high-school-level Newtonian physics, it should be possible to control the motors in a way that is optimal in all circumstances. TwoPotatoe attempts to solve the control problem in a way that is optimal in all circumstances. Ideally, the only limitation on performance of the robot should be the power of the motors and traction of the wheels. If the motors and wheels are capable of performing a particular action, then the algorithm should be capable of correctly controlling it. There should be rapid recovery from all situations with no overshoot or oscillation. A PID algorithm means that compromises must be made between rapid response and oscillation/overshoot.
PID algorithms were invented in the time of vacuum tubes and analog computation. Their simplicity is very attractive but with modern high-speed computation, there are better ways to solve this problem.
PID Thought Problem
It is interesting to go back to the original problem that the PID algorithm was designed to solve: steering a ship. Let’s say you are steering a ship and want to change course by 30 degrees. If you let a PID algorithm to do your steering for you, you simply adjust the PID parameters to achieve the new course relatively quickly with a minimum of oscillation at the end. This is simple and it works; it doesn’t really matter in the usual case that it is not as rapid as it could be or that it takes a little settling time at the end to eventually achieve the new course. If you are captain of the ship and you need to set the new course as rapidly as possible to avoid mines that have been planted by the enemy, you might have a different attitude. The captain would certainly disable the PID control, take control of the rudder and take the following actions:
- The captain would steer a hard right causing the ship to rotate as rapidly as possible toward the target orientation.
- At some point he would steer a hard left to stop the rotation.
- If everything was calculated correctly, the ship will stop rotating at the point that it has turned 30 degrees. At this point the rudder is brought back to the center position.
These actions produce the most rapid response possible.
TwoPotatoe thinks like the captain.