There are numerous instances of balancing bots as evidenced in many video of this topics in Youtube. The challenge here is to turn the cheap robot chassis from the previous Alexa controlled robot project into a balancing robot. I wanted to see if the cheap motor is responsive enough to balance and examine its stability using a basic PID-based mechanism. Also the “shape” of the robot is not symmetrical along its pivoting axis which generates lots of headache and challenges along the way (but also learn a lot because of it). I also like to see if the balancing action is precise to perform motion control given all these irregularities. This last objective is inconclusive at the time of writing.
- 1x Robot chassis platform (with the 3 wheeled mount configuration; but the front guiding wheel is removed).
- 2x cheap DC motors
- toy structure parts to build up the tower
- 2x 4-cells AA battery packs to drive the motors (wired in series)
- 1x mobile phone battery to power the pi
- 1x Raspberry Pi Model 3 (with built in wifi)
- 1x Adafruit motor driver hat
- 1x MPU 6050 with gyro and accelerometer
- 1x Pi camera to stream what the robot sees (optional). I added a nifty cover on it as windshield which I improvised from an old iPod casing.
The balancing robot acts as an inverted pendulum system. The weighs of the pendulum is predominately at the top of robot where all the battery packs are placed. The MPU and the PI and motor driver electronics are protected in the middle of the structure. Since the toy structure and the base chassis are not compatible with no easy mounting to clip them together; they are basically double-sided taped together and then tied down with elastic bands. This is truly a duck-tape DIY project. It turns out the structure and the elastic band works very well together to cover for both tensile and compression forces when the robot rocks back and forth.
- The responsiveness of the “plant” (using PID lingo) is very important. Therefore doubling up the battery pack (i.e. in series which gives 12V output) turns out to be quite essential to the balancing.
- The cheap dc motor has no motion below certain control level. At 6V, this setting is about 40-45 out of 255; whereas with 12V pack, it is around 18-22 out of 255. The code needs to account for this dead zone.
The Pi runs the main gyro and accelerometer reader with a complementary filter in order to reduce the noise; plus it runs the main PID loop. Much of the effort is in the tuning as well as in the special calculation to compensate for the cheap laggy plant (i.e. the dc motors) ; which is detailed below.
The PID loop and gyro angle reader are implemented in Python on the Pi. The whole loop is approximate 12ms. The delta time is calculated dynamically in each loop to give it better precision.
There is a great tool to model and understand the inverted pendulum from myPhysicsLab.
Note that the angular acceleration is proportion to the sine of the offset angle and is inversely proportion to the ideal mass at the top of the pendulum.
Therefore putting weights along the tower effectively “shorten” the inverted pendulum, thus increases the angular acceleration. If you have balanced an inverted pendulum before, you noticed that it is easier to balance when the object is at the top. Same theory goes here. With this robot setup, it turns out that the robot swings between +/- 1.5 deg or so.
Reading the angle with complementary filter
The MPU 6050 is an inexpensive 6DOF sensor (3 for gyro and 3 for accelerometer). Gyro provides reading in angular velocity (deg per sec) whereas accelerometer provides value in deg. The accelerometer output is quite noisy; therefore the gyro reading and accelerometer is combined together to form the instantaneous angle offset value.
The complementary filter is a simple yet effective filter for reducing the noise from the sensors. The K value of 0.95 was used to bias towards the gyro reading. I found that this is good pick. A higher value would make the signal even cleaner but it induced a latency which is problematic in a balancing setting (the plant needs to be very responsive). A smaller value would be more responsive but the resulting signal is quite noise. This chosen K value turns out to be a good compromise.
(Note the illustration above used a K value of 0.98).
- A separate python routine is written to take a set of sample angle values and it is averaged to form the setpoint for the overall robots.
- For the purpose of this balance robot, we only use one axis. Based on how the MPU was placed, the Y-axis is what I needed. Important: do remember to use the registers for both the gyro and accelerometer of the same direction (Y direction in my case).
- Gyro, due to its implementation nature, has a drift. This infamous error is called the gyro drift. To compensate for this drift, I took a sample of 50-100 readings initially and treat that as the offset for the gyro during run times. It is important to remember that these readings must be performed when the robot is not moving. See this post for a reference to the drift compensation strategy.
- In the future, more sophisticated form of the robot can incorporate the yaw dimension.
PID design and tuning
The setpoint is the target offset angle at which the balancing robot is aiming for. A negative error will induce a positive motor action and vice versa. The output of the PID is input directly into the motor control which is restricted by Adafruit library to be a value between 0 and 255. It also needs to be an integer. A basic PID loop is shown below. There are many tutorial on the internet. See my other reference section if you want good tips for tuning. Some of my tuning procedures are documented below.
The P (proportional) term is the present difference between the actual offset angle and the targeted angle. This turns out to the main contributor to the balancing action. The main changes to the classical P term is that the dead zone for the motor is added to the P term such that the output of the PID would not be below the dead zone value – i.e. the motor is always moving; albeit not visible to the naked eyes.
Note that in classical PID theory, the P term introduces the steady state error which is a permanent offset from the target setpoint. This is evidenced in the drift of the robot in one direction after the first few seconds of balancing. Eventually there is enough disturbance to cause the robot to fall over. In theory, large P gain will make steady state error small but it starts to introduce larger oscillation as well.
The P term is also responsible for the responsive of the initial correction towards the setpoint; therefore if you want to allow the robot to sustain some disturbance while it is balanced (such as a light push), a large P gain is required but then you have to use the D term to reduce that oscillation.
The D (derivative) term is responsible for the short term fast response aspect of the control. It effectively estimates where the error will be and projects into the future the anticipated error. As such it suppresses the large oscillation motion of the P and I effects but it introduces high frequency noise. The robot no longer has big swing but becomes jittery when the D term is introduced.
Instead of the classical derivative calculation, I have opt for setpoint-based calculation advocated by Brett B. . This new method is suppose to reduce the overshoot noise of the derivative term. Since I didn’t collect enough data points to do numeric analysis, I take it at face value that this works.
In general the D terms help center and make the robot roughly stationary at a point in space. In reality, the robot performs less than ideal.
The I (integral) term turns out to the hardest to manage in this robot setting. The integral term is responsible for the compensation of the long term error – which typically is introduced by the steady state error caused by the P term. When the I term is introduced, the robot still tend to drift a bit but it wasn’t as bad as when the robotonly has the P term.
The robot’s telemetry was instrumented and the key values were printed on a remote PC. (TightVNC was used to connect wirelessly as to the Pi’s remote desktop). After examining the dynamics of the P, I, D values, the I value tends to exhibit some “stuck” behavior when it drifts and falls. I contended that it must be due to the infamous “wind-up” problem associated with the I term. This is usually fixed by capping the I term to the max of the plant can sustain (which is 255 in this case). It turns that 255 is far too late for holding back the dynamics of the I term. A reasonable good recipe was found as follow.
- keep the I term to within the limit of +/- 35 degree
- reset the integration term to zero when the error is beyond +/- 1.5 degree
Item 2 above seems to have a good effect in minimizing the drift of the robot. I think there must be more elegant ways to control integration – will need to research that as future enhancement.
PID tuning sequence and tricks
Many formal tuning methods exist for PID tuning; including the Ziegler-Nichols method. This method is relatively easy to follow if you can find the ultimately gain (Ku) of the system. Without proper telemetry data collection, it turns out that it is quite hard to determine exactly where Ku is. It is suppose to be the point when the system starts to oscillate into instability; but not more. A simple summary can be found here. Many elaborate information on Z-N can be found throughout the internet. After trialing with it once, I decided to abundant it and just follow the traditional method:
- Set all Ks to zero
- Increase Kp to a level when it starts to balance but with oscillation. Increase it to a level such that it starts to resist your push a bit. It is ok if the robot drift after a while. This is likely due to the accumulated steady state error.
- Increase Kd to a level such that the robot is jittery but the wild (low frequency) oscillation and the eventual drift disappear (or is reduced).
- Increase Ki to a level such that it would remain, largely speaking, at a fixed location. See the I term section for other finer tuning techniques to tackle the wind-up problem.
- Once you get a feel of what each term does, go back to step 2/3/4 for finer trimming.
Overall the balancing precision is sensitive to these factors (not in particular order):
- How close the setpoint is with respect to the real CG point of the robot. The initial calibration helps but observing the drift and tuning it manually certainly helps a lot. There are other design that estimate this drift dynamically and automatically adjust the setpoint to compensate. This is a to-do item for later.
- The K value in the complementary factor which trade off between noise and latency in the output signal.
- High enough voltage (thus current) drive is extremely important in the end. Of course upgrade to better motor with encoder will help but that defeats the objective of this project.
- The dead zone value for the motor also affect the responsiveness of the system. But it seems that programming at the bare minimum level at +/- 0 deg is sufficient. I did experiment with some dead zone in the error dimension (i.e. within a band of error, the motor does not move); it turns out that such set up significantly reduces the responsiveness of the plant and eventually led to instability.
- P/I/D gains
- Sufficient P gain for basic balancing but not enough to cause huge oscillation
- Sufficient D gain to reduce the large oscillation and keep the robot at the same location (roughly); yet not too jittery
- Sufficient I gain to eliminate the drift due to the steady state error. But then you have deal with the headache of the wind up dynamics.
- The floor surface: the tuning of the above factor would lead to reasonable balancing for a type of surface (e.g. in my case the carpet). But it does not seem to perform that way when the robot is moved to a hard surface.
I like to improve the stability of the robot and then layer in the motor control using the same Alexa control setup from previous project. It would be nice to introduce the camera and watch remotely the scenery of the robot’s line of sight – given the rocky motion of the robot, I think I might get motion sickness!