I made a Self Balancing Robot in 10 days!

/

I made a Self Balancing Robot in 10 days. I will explain how I made it.

I chose a Raspberry Pi zero. And I use SSH, SCP on Mac terminal to transfer the Robot program and to control it. Here, I don't explain how to setup a Rasbian OS.

My Robot program is executed in Python2.x. You can see the Robot program on my Github Repository.

A journey of a thousand miles begins with a single step

I don't know how to control motors with PWM. So I started with a very simple step before controlling motors. That's a blinking LED. Isn't it very very easy?

First, I installed gpiozero module to control GPIO of the Raspberry Pi.
Install as follows:
sudo apt install python-gpiozero

Then I wrote a simple program that blinks LED 5 times every second.

LED blinking is okay, but I'm bored with it. So I tried to blink LED with a sine curve as the next step. I wanted to start with the LED off, so I used the following formula.
$$LED = 0.5*sin(t-π/2)+0.5$$

I used PWMLED in gpiozero to control LED brightness. PWM is pulse-width modulation, is a method of reducing the average power delivered by an electrical signal, by effectively chopping it up into discrete parts.
I could see a shape like a measuring worm by my oscilloscope. Now I'm ready to control the motor with PWM.

Motor driver test

I went to Akizuki-Denshi at Akihabara to buy DC motor drivers, but it was close for Obon week. Next Sengoku-Densho was open, so I bought BD6211F.

I learned that controlling DC motors with driver is very easy. To control the motor is sending signals to FIN and RIN of a driver. Then we can control forward, backward and brake with PWM in a range 20kHz to 100kHz.

FIN RIN OPERATION
L L Stop
PWM L Forward
L PWM Backward
H H Brake

To change the pulse frequency of gpiozero's PWMLED, declare it as FIN_L = PWMLED (20, frequency = 100000).

I choose the TAMIYA's double gearbox as the power of my Robot. This module can move the left and right tires independently. If you want to make a Robot just stand, you can choose a single gearbox too.

Anyway I assembled it without any sensor. I loaded the battery on the Robot and moved it just forward and backward with the motor drivers.

Accelerometer sensor test

I bought MMA8452Q of an accelerometer at Sengoku Densho. I thought it had a gyroscope and a accelerometer, but it had just a accelerometer. I tried to calc the Robot's angle from just the accelerometer.

To get values of acceleration from MMA8452Q needs to I2C communication. Fortunately I had use I2C in the previous article, so it wasn't so difficult for me. But one point needs attention, is the sensor's SDA connects to the GPIO number 2 of the Raspberry Pi and SCL to the 3rd. It is GPIO numbers, not physical numbers.

I got the Robot's angle with the accelerometer. So I thought I could make a Self Balancing Robot only an accelerometer without a gyroscope. But while the accelerometer itself is moving, it couldn't get precise angles. Because the accelerometer can't judge whether acceleration is due to gravity or motion. Therefore, while an accelerometer is moving we should get angles with a gyroscope. But a gyroscope are also not perfect, that's thier angles drift with time. In order to remove the drift of a gyroscope, it must be corrected using an accelerometer.

The graph below shows degrees of the accelerometer when it is rested on a desk for 3 minutes.

It is not centered around 0 degrees because the desk is already slightly tilted. So it goes back and forth between plus and minus 0.5 degrees around -1.5 degrees. The angles could be calculated using arcsin or arctan2.

As you can see, the accelerometer has high frequency noise. I thought it would be nice to use a low-pass filter to remove the noise. Then the graph below shows degrees of the accelerometer with a low-pass filter when it is tilted 90 degrees twice.

An algorithm of the low-pass filter used exponential moving average. Although the low-pass filter remove the noise, it seems that angle phases are delayed. The sensor cannot follow fast movements.

Well, I tryed whether the Robot was vertical with the accelerometer and moved the Robot forward and backward.

Update my Robot

I reworked my Robot's framework with wood. I chose a thicker tire, and RS-385 motors that's 9V power supply. I thougt the Robots could move without gearbox, but it was failed. The motors doesn't have enough torque to move the Robot without no gears.

So I changed the motors to geared motors.

Wonderfully, the geared motors moved Robots so powerfully. I was confident of the success of this project.

Troubleshooting I2C communication errors

By the way, I was faced with a problem that my Robot program stopped often. The following error is issued when the program stops.
bus.write_byte_data(0x69, 0x0F, 0x04)
IOError: [Errno 121] Remote I/O error


The error is likely to occur when the direction of the motors rotation is reversed. I tried to attach a 100μF capacitor to lower the output impedance of the power supply, but the Robot had still the error. In the first place, the power supply for the motors and Raspberry pi are separate, so there seems to be no relation.

By writing exception handling as follows, the program is no longer forcibly terminated.
try:
except IOError as e:


However, the error itself has not been resolved. I thought that the noise caused by the back electromotive force of the motors might be related to the error. I tried to put 0.1uF bypass capacitors in the motors.

Attach three capacitors each a motor as shown in the picture. Insert one capacitor between the terminals of the motor and two capacitor between each terminal of the motors and the chassis. As a result, the error never appeared again.

Use a gyroscope and an acceleration sensor with BOSCH BMX055

I bought a BOSCH BMX055 sensor, which combines a gyroscope and an acceleration sensor, at Akizuki-Denshi. Note that the angles obtained with the gyroscope and the acceleration sensor are opposite. If the value calculated by the acceleration sensor is 20 degrees, the gyroscope will be -20 degrees.

Now, the graph below shows degrees of the gyroscope when the tilting process is repeated 90 degrees.

I found that the gyroscope is easy to use for instantaneous angle detection. However, since it drifts with time, it does not seem to be suitable for absolute angle detection. Therefore, it is necessary to correct with the acceleration sensor to remove the drifts.

I found two ways to remove the drifts. One is Kalman filter and the other is Complementary filter. It was too difficult for me to understand Kalman filter. On the other hand, it was easy to understand Complementary filter. It seems that there is not much difference in the result of using either of them, so I chose simple Complementary filter.
The filter formula is as follows:
$$angle = k * (angle + xGyro * dt)\\ + (1 - k) * angleAccel$$

Like the previous graph, the next graph shows angles.
However, the values are calculated using the complementary filter. The value of k is set to 0.9.

The drifts of the gyroscope has been removed brilliantly.

I tried to make the Robot stand up like this. Although it is still unstable, the success of the project is coming soon.

PID controller

It was difficult to make the Robot stand stably. Just then, I learned about PID controller from my friend. And I found that PID controller can be programmed very easily. I wanted to know the mechanism of PID controller. So I bought a control engineering book and studied it.

The book was written in comics so it was easy to read. However, since control engineering requires advanced mathematics and physics knowledge, it is difficult to understand perfectly. I think you can make a Self Balancing Robot without knowledge of control engineering. But if you are interested in control engineering even a little, I recommend that you study it.

Well, PID controller is an abbreviantion for Proportional-Integral-Differential Controller. Until now I have used only proportional control for my robot. That is why the robot did not stand up stably. By using the integral, it becomes strong against external force. By using differentiation, machine vibration can be smoothed. This video explains the PID controller in an easy-to-understand manner.

As I said, PID control programming was very easy. On the other hand, it took time to adjust the PID parameters.　I spent a few days adjusting it. Finally, my robot was able to stand stably.

The middle battery is for fine adjustment of the center of gravity. If the center of gravity shifts, the robot will move forward. The balance of the robot seems to be important. Also, the complementary filter coefficients, the timer interval of the program while loop, and the PID parameters are sensitive to each other, so it was difficult to optimize. It is useful to know how PID controller works.

At the end

I started this project as if it were my summer homework. It took about a month, but it was completed in just 10 days. I would like to thank the various people who posted how to make them on the web site. I hope my article is also useful for someone.

You can see my robot program source here. Thank you.

Reference sites

ラズベリーパイ　gpiozeroの使い方
Gpiozero Document
Data sheet BD6211F
Data sheet　MMA8452Q
MMA8452Q を Raspberry Pi から使う

【村田製作所】垂直面の回転角計測

The Balance Filter

Androidの加速度センサー(Accelerometer)から重力の影響を省いて値を取得する（ローパスフィルタでノイズも除去）
ハイパスフィルタ（High Pass Filter）
Rasperry Pi 3で9軸センサ(BMX055)を使う
MicroPythonで「ＢＭＸ０５５使用９軸センサーモジュール」を駆動するクラス
Data sheet TA8428K
9軸ジャイロセンサをJetson TK1で使う。

Data sheet BMX055
M5StickC で倒立振子 PID制御編 ー倒立振子への道 3ー
ジャイロのドリフト補正と比較（カルマン、相補フィルター）

SainSmart InstaBots Upright Rover(Self-Balancing Robot With Arduino)
ジャイロのドリフト補正を改良

システム同定による倒立振子のLQR制御
ＤＣモータの基礎