In my previous post, I explained how to design an inertial measurement unit or IMU for use with a self-balancing robot. The IMU is used to measure the current tilt angle of the robot. The idea is to use the tilt angle as a way to determine the direction the robot should move to bring it back to a near vertical position. This problem is similar to the situation most of us have intuitively solved when balancing something like a broom on the palm of our hand. We move our hand so that it is under the center of mass – if the broom tilts left, we move the bottom to the left to catch up with the rest of the broom before it falls. We make many small adjustments like this to keep the broom balanced. These adjustments are based on our observation that the broom is beginning to tilt too much in one direction and we need to move our hand fast enough so that the broom doesn’t fall. In this case, we may need to move our hand forward and backward as well as left and right.

### The PID and the Pendulum

[Apologizes to Edgar Allen Poe]. The self-balancing robot balancing act is similar and somewhat simpler than balancing the broom in that we only need to move the robot forward and backward – but how to do this? This problem happens to be a classic control theory problem called an inverted pendulum. Most descriptions of this problem go to the trouble of mathematically describing the motion equations of the inverted pendulum before describing how to control it. This is mostly for the benefit of those with system simulation software and ideal parameters to create the perfect solution. The math is frighteningly complex involving signal processing techniques like transforming the time domain problem into the frequency domain through Laplace transforms and bringing stability to the system by moving the roots into the negative s-plane. Do you need to understand that? I don’t think so – but if you do, that could help later. I haven’t touched Laplace transforms in a very very long time and I didn’t need to learn them again. So what do you need to know to control the robot? A little bit of control theory.

You have likely encountered control theory without even realizing it while filling a bath tub. You may start with both hot and cold water running while you monitor the water temperature in the tub. If the temperature is too cold, you might turn the cold water down or turn the hot water up. You monitor the temperature and make adjustments until the right water temperature and level is reached. What you’ve done is used the difference between the desired and actual water temperature as a guide to repeatedly adjust the incoming water temperature mix. Your hand is the *sensor* that provides feedback of the current temperature. The difference between the desired (or *reference*) and actual temperatures is called the *error*. You use the error signal to control adjustments to bring the temperature closer to the desired output – you are the *controller*.

You can see from the diagram above that “Your Program” is the controller and it uses the IMU output to decide how to move the robot. This type of feedback control has been extensively used for over one hundred years to control all sorts of machines. A typical approach to a controller is to use the Proportional, Integral, Derivative or PID design. This design combines the use of two or three of:

- A proportional adjustment (e.g. multiplier) of the current tilt angle. This adjustment by itself cannot result in a stable system and must be combined with one or both of the other adjustments.
- An adjustment based on the total accumulated error. Multiple adjustments to the robot should make the tilt angle approach zero degrees (e.g. vertical). The actual measurements will show that the adjustments may have fallen short or over shot the desired angle resulting in an error. This error is accumulated and should be accounted for by the controller. Because this is a sum over time, it is mathematically an integration and thus the name integral.
- An adjustment that considers the rate at which the error is changing so that the system can become stable more quickly. Because it is related to the rate of change of the error over time, it is mathematically a derivative of the error.

Controllers that use only the Proportional and Integral adjustments are referred to as PI controllers. Those that only use the Proportional and Derivative adjustments are called PD controllers. A very informative and comically dated MIT lecture on YouTube provides a very good detailed mathematical foundation for control feedback of an inverted pendulum. The lecture only uses a PD design, while I understand most systems rely on a PI design. This series uses all three adjustments just for fun. The PID controller output can be expressed mathematically as:

Where θ is the tilt angle. The tilt angle is considered the “error” since the reference is zero degrees so anything other than zero is considered an error. Each adjustment has a multiplier called a gain. The Kp, Ki, and Kd gains are for the proportional, integral and derivative adjustments respectively. All of these gains must be positive to achieve stability. There are other equivalent representations of this equation that we’ll discuss when it comes to determining the values of each of the gains. As you can see there is a need to do integration and differentiation. We need to find ways to approximate these functions.

Integration is the calculation of the area under a function. Fans of calculus also know that integration is the sum of infinitesimally wide rectangles as the width approaches zero. Your program can accomplish a close approximation of integration through numerical integration. This example (and all that I have seen elsewhere) specifically uses the rectangle method. Imagine a graph of the accumulated tilt angle on the y-axis versus time on the x-axis like the one to the right. The rectangle method sums the rectangles formed by a series of *equal sub-intervals* to calculate the area providing a nice approximation to integration. There is some error in the calculation as shown with the black triangles but is minimized through the use of equal sub-intervals. This approach led me to use a timer interrupt to ensure the time intervals were as equal as possible. Many other examples you might find tend to use the main Arduino loop for this evaluation and I suspect additional error may be introduced in the numeric integration as a result.

A derivative is the slope of a function and is accomplished through differentiation. Your program can implement a close approximation of a derivative through the use of numeric differentiation. This example uses the finite difference approach where the slope of a secant line between two adjacent tilt angle measurements is used. This is sufficiently accurate for our use provided the sub-interval time is very short.

So now that we have the controller output foundations established, how should that be used? Don’t worry about the gain values yet as we’ll cover them in detail when we finally get to some code.

### May The Torque Be With You

Ultimately we need to drive the motors of our robot and the details are quite implementation specific. Your program needs to determine a direction and speed to keep the robot balanced. The way to do that depends on the weight of the robot, the torque of the motors you’ve selected and how you’ve attached the motors to your Arduino. I use the Adafruit Motor/Stepper/Servo Shield because it’s convenient, powerful and relatively inexpensive at US$20. You might be able to put together a cheaper alternative with some extra work. A few things I like about this shield is that is has a dedicated PWM chip, connects over just two I2C pins and has a nice library to access it. The direction is set with a dedicated function call using FORWARD or BACKWARD constants. The speed is set by using another function call passing a value ranging from 0 to 255 – the realized speed depends on the motor, the current power supply level and the load.

As you may recall from my previous post, the IMU arrangement makes a positive rotation about the x-axis correspond to tilting forward. Your design may use a different convention for the axis or direction. As the tilt angle becomes positive, the robot needs to move forward to stay balanced. Looking at the control equation, we can see that a positive and increasing tilt angle θ produces a positive control output. The sign of the control value can be used to determine the required motor direction.

The absolute value of the control should not be directly used as the speed or throttle of the motor because that value must be within the range of 0 to 255. With some care of data types and ranges, the control value can be mapped into the proper throttle value through the use of the Arduino ‘constrain’ and ‘map’ functions.

This completes the background and foundational concepts to design a self-balancing robot. Next we will *finally* get to some code to make these concepts real and then determine the PID gain values to make it work.