Two-wheel self-balancing robot

After my first fast made self balancing robot, I want to better understand it and especially to try different control methods. For me it is necessary to simulate the whole stuff on computer (robot + control law).

The first step is therefore to model the robot with mathematical equations. Because of the complexity of the model I do make some assumption and simplification.

The model is separated into three part: the DC motor, the wheel and the inverted pendulum

The DC motor

electrical model:

V a = L d i d t + V E M F + i R

wtih

V E M F = k e ω

equation of motion for the motor:

I r ω ˙ = τ m - τ a - τ f

with

τ m = k m i

τ f = k f ω

If the motor inductance and friction torque are neglected, then

d w d t = - k m k e I r R ω + k m I r R V a - 1 I r τ a

in state space form:

[θ ˙ ω ˙] = [001 - k m k e I r R] [θ ω] + [0 k m I r R 0 - 1 I r]

The inputs to the motor is the applied voltage and applied torque on the rotor.

The wheel

in horizontal (x direction):

M w x ¨ = N f - N

rotation equation around the center of the wheel:

I w θ ω ¨ = τ m - N f r

by using motor dynamic equations, one can get

N f = - I w r ω w ˙ - k m k e r R ω w + k m r R V a

with

ω w = θ w ˙

then

M w x ¨ = - I w r ω w ˙ - k m k e r R ω w + k m r R V a - N

Above equation is valid for both wheels.

With following equations, one can translate the angular rotate to linear motion:

r ω w ˙ = r θ w ¨ = x ¨

r ω w = r θ w ˙ = x ˙

then

M w x ¨ = - I w r 2 x ¨ - k m k e r 2 R x ˙ + k m r R V a - N

After rearrangement, one has:

(M w + I w r 2) x ¨ = - k m k e r 2 R x ˙ + k m r R V a - N

The inverted pendulum

horizontal:

2 N + m l θ p ˙ 2 s i n θ p = m l θ p ¨ c o s θ p + m x ¨

The sum of forces perpendicular to the pendulum:

2 N c o s θ p - 2 P s i n θ p + m g s i n θ p = m l θ p ¨ + m x ¨ c o s θ p

The sum of motions around the center of mass of pendulum:

- 2 τ m - 2 l N c o s θ p + 2 l P s i n θ p = I p θ p ¨

with

τ m = - k m k e R ω + k m R V a = - k m k e R x ˙ r + k m R V a

one has:

2 k m k e R x ˙ r - 2 k m R V a - 2 l N c o s θ p + 2 l P s i n θ p = I p θ p ¨

then

2 k m k e R x ˙ r - 2 k m R V a + m g l s i n θ p = I p θ p ¨ + m l 2 θ p ¨ + m l x ¨ c o s θ p

then

I p θ p ¨ - 2 k m k e R x ˙ r + 2 k m R V a + m l 2 θ p ¨ - m g l s i n θ p = - m l x ¨ c o s θ p

and

2 (M w + I w r 2) x ¨ + m l θ p ¨ c o s θ p + m x ¨ = - 2 k m k e r 2 R x ˙ + 2 k m r R V a + m l θ p ˙ 2 s i n θ p

after arrangement, one can get the following state space expression:

⎡⎣⎢⎢⎢⎢ x ˙ x ¨ θ p ˙ θ p ¨ ⎤⎦⎥⎥⎥⎥ = ⎡⎣⎢⎢⎢⎢⎢ 00001 - 2 k m k e ( T + r m l ) r 2 R α 0 2 k m k e ( m l + r S ) r 2 R α 0 - m 2 g l 2 α 0 m g l S α 0010 ⎤⎦⎥⎥⎥⎥⎥ ⎡⎣⎢⎢⎢⎢ x x ˙ θ p θ p ˙ ⎤⎦⎥⎥⎥⎥ + ⎡⎣⎢⎢⎢⎢⎢ 0 2 k m ( r m l + T ) r R α 0 - 2 k m ( m l + r S ) r R α ⎤⎦⎥⎥⎥⎥⎥ V a

with

S = 2 M w + 2 I w r 2 + m

T = I p + m l 2

α = S T - m 2 l 2

On my first two-wheel self-balancing robot, I didn’t use the encoder for speed measurement.
So it can basically stand still and balance its weight with small movements of its two
wheels simultaneously. I preferred that the robot would work very soon without much effort
and then I could improve it by adding speed control, RC, etc.

1.Hardware

It’s not difficult to gather all the parts. Here is my list:
1.glass fiber board
2.control board (arduino uno)
3.sensor board (MPU6050 gyroscope + accelerameter)
4.two-channel motor driver (L298N)
5.two geared DC motors (25GA370)
6.two toy wheels and fixation kit

Following is my assembly diagram:

Following is the wiring diagram:

In L298N, each motor is controlled by a full bridge circuit (or H-bridge) composed of two legs. Following figure is a simplified diagram coming from wikipedia. S1 and S2 are always controlled in complementary manner. When S1 closes, S2 opens. S1 and S2 close simultaneously is not allowed since that creates a short circuit of the power supply. Idem for S3 and S4.

When S1 and S4 close simultaneously, the motor rotates in one direction. When S3 and S2 close simultaneously, the motor rotates in the other direction. I use PWM control to set the motors’ speed. the switching frequency is about 1kHz which is the default value of Arduino module.

After the wiring, the robot looks like:

2.Control

Then comes the control part. The following figure shows the control diagram. Only the inner
loop in blue is implemented this time. I use it to regulate the tilt angle of the robot. As
I want the robot stands straight, I simply set the reference angle (REF_ANGLE) to 0. The
gyro+acc sensor gives the information needed to calculate the actual tilt angle. The error
between the reference and the measurement is used by the PID controller to give the command
to the robot (the two wheel motors).

2.1 PID controller

The PID controller is a complexe topic. With google you can find tons of material about it.
Fortunately it is relatively simple to implement it in a microcontroller. With some basic
understanding and tuning techniques, one can make it work very easily.

The general mathematical form of the PID controller is (extracted from wikipedia):

$u(t)=K_pe(t)+K_i\int_{0}^{t}e(\tau)d\tau+K_{d}\frac{d }{dt}e(t)$

Here is a simple software loop that implements a PID algorithm (extracted from wikipedia):

previous_error = 0
integral = 0 
start:
  error = setpoint - measured_value
  integral = integral + error*dt
  derivative = (error - previous_error)/dt
  output = Kp*error + Ki*integral + Kd*derivative
  previous_error = error
  wait(dt)
  goto start

The purpose of the PID controller is to generate a suitable command to the system (the self
balancing robot in this case) so that the system is stable and reactive. There are some
creteria of both the static and dynamic performance. But they are out of the scope. The PID
controller uses three parameters to do the job, the proportional (Kp), integral (Ki),
derivative(Kd) parameter. The big Kp makes the tilt angle error small and the reaction fast. The
Ki makes the static angle error dispear. This is important since I want the robot stands
straight (tilt angle=0) all the time. I choose to put Kd=0 because Ki and Kp achieve
perfectly my desired performance. So in fact I use a PI controller. For the PI parameter
tuning, I follow Ziegler-Nichols method that you can find on wikipedia. Following table is
copied from wikipedia. It works well for me.

2.2 Tilt angle measurement

A sensor combining a gyroscope and an accelerometer is used to measure the tilt angle. The sensor don’t give the angle directly. Some calculation is needed. I use MP6050 as sensor. You can check its datasheet for more information.

The accelerometer gives the acceleration (unit g) in three dimension. If the robot is perpendicular to the earth (z axis for example), you should have only Z axis which is not zero. If you have another axis not equaling zero besides Z axis in static state, it means the robot has a none zero tilt angle. And you can get easily the tilt angle based on these two axis acceleration. However using accelerometer to get the tilt angle in this way is valid only for static state since we suppose that only gravity acts on the robot. When the two wheels act a force on the robot, the accelerometer can measure the resulting acceleration as well! In consequence, the measures are very noised. The good point is it is accurate in long period of time (or in DC after filtering).

$\theta_{acc}(t)=tan^{-1}\frac{A_x(t)}{A_z(t)}=sin^{-1}\frac{A_x(t)}{g}$

The gyroscope measures the angle speed (unit deg/s) in three dimension. In order to get the angle, we need to do the integration of it. It is accurate dynamically (in short period). But the integration action accumulates the error of measurement, which makes the result drift in long period of time.

$\theta_{gyro}(t)=\int_{t_0}^{t_2}G(t)dt+\theta_{gyro}(t_0)$

I use a complementary filter to combine the two sensor measurements. It is simple and needs little microcontroller calculation. It works well for me (so far). One can implement kalman filter for optimal control. But the knowledge of the system model and more calculation capability are required. It needs definitely more effort. I use following complementary filter to get the tilt angle estimation:

$0.98*\theta_{acc}(t)+0.02*\theta_{gyro}(t)$

3.Software

The sensor communicates to the microcontroller through I2C bus. By using the WIRE library of arduino, it is easy to read the data. I use analogWrite function to set PWM duty cycle for the motor driver L298N. The command (PID controller’s output) is a value between -255 and 255. Positive sign means the robot moves forward. 255 represents the max speed (100% duty cycle for the first half bridge). I use anti-windup code to prevent the command to be higher than 255 (or lower than -255) which is the limit of PWM register (8 bits) max value.

The calibration of the sensor should be done generally. I calibrated the gyroscope because I see big static offset. My accelerometer’s offset is not obvious. I use 10ms as sampling time.

The total code has only about 50 lines which I believe possible to be optimized. It’s simple, right?

4.Conclusion

The robot resists relatively well the disturbance from both sides. The stability and dynamic response are satisfactory. The next step is to install an encoder on each motor, to make the robot possible to move and to add a remote control. Following is a video of it standing still.