SELF BALANCING BOT (INVERTED PENDULUM STABILIZATION)

INTRODUCTION

A two wheeled bot is unstable when the bot is standing upright. But this bot can
be made stable by using proper control engineering. Since, it is an unstable system,
a PID controller-based approach is used for stabilization which can even handle small
disturbances. For this project Arduino Nano is used as a micro-controller. Components
used and their quantities are shown in Table 1.

Table 1: List of the components

Components Quantities

Ardunio Nano 1

MPU6050 1

Stepper (NEMA 17, 4.2kg-cm) 2

A4988 stepper motor driver module 2

LiPo battery (11.1V) 1

Arduino Nano expansion shield (optional) 1

A4988 driver module expansion board (optional) 2

10cm dia wheel 2

BASIC PHYSICS

If the bot is left untouched, it falls on either side. But to prevent it from falling,
the bot must accelerate in the falling direction with the controlled acceleration
(computed by the controller). When the bot accelerates, the inertial force acting on the
CG are in the opposite of the falling direction and this inertia force prevents it from
falling. The full situation is depicted in the diagram below.
 Figure 1: Basic Physics of Self Balancing Bot

CONTROLLER DESIGN
Design of the controller is based on the tilt angle. When the controller senses the tilt
angle deviates from equilibrium position (𝜃 = 900 ), then it commands the motor with
controlled speed in the direction of tilt (falling). Therefore, the input to controller is the
tilt angle and to design the controller, first it is required to compute the tilt angle of the
bot from its equilibrium position.

MEASURING ANGLES FROM MPU6050

MPU6050 is a micro elector mechanical system (MEMS) which measures

accelerations (𝑎𝑥 , 𝑎𝑦 and 𝑎𝑧 ) and angular speeds (𝜔𝑥 , 𝜔𝑦 and 𝜔𝑧 ) along the x, y, and
z-axes (in body frame which are attached to the sensor. The values obtained from the
sensor are in the local or body frame. If the sensor is placed on the flat ground, it reads
acceleration in x, y, and z as zero, zero and -1, respectively. But if the sensor is places
on an inclined surface, then the readings are different for example: (𝑎𝑥 , 𝑎𝑦 , and 𝑎𝑧 ) are
0.2, 0.4, and -0.8). The inclination angles can be computed from the vales of the
accelerations by transform the acceleration (measured from MPU6050 sensor) to
global (earth – fixed coordinate) system.

An orthogonal rotation matrix is multiplied to transform vectors from local

coordinates (Sensor system) to global coordinates (earth – fixed system).
 (a) Earth fixed coordinate frame (b) Local coordinate frame

Rotation matrix from body frame to earth fixed frame:

𝐶𝜓 𝐶𝜃 𝐶𝜓 𝑆𝜃 𝑆𝜙 − 𝑆𝜓 𝐶𝜙 𝐶𝜓 𝑆𝜃 𝐶𝜙 + 𝑆𝜓 𝑆𝜙
𝑹 = 𝑹𝒙 𝑹𝒚 𝑹𝒛 = [𝑆𝜓 𝐶𝜃 𝑆𝜓 𝑆𝜃 𝑆𝜙 + 𝐶𝜓 𝐶𝜙 𝑆𝜓 𝑆𝜃 𝐶𝜙 − 𝐶𝜓 𝑆𝜙 ]
−𝑆𝜃 𝐶𝜃 𝑆𝜙 𝐶𝜃 𝐶𝜙

Where, 𝑹𝒙 , 𝑹𝒚 , and 𝑹𝒛 are rotation matrix about x, y and z – axes respectively.

When the sensor is placed on flat earth then in earth fixed coordinate frame, the force
vector is –

0
𝒈 = {0}
𝑔

This force vector (𝒈), is transformed to local frame from earth fixed frame by
multiplying this vector to 𝑹𝑻 . Transformed force vector (𝒇) –

𝑎𝑥 −𝑆𝜃
𝒇 = {𝑎𝑦 } = 𝑹 𝒈 = {𝐶𝜃 𝑆𝜙 } 𝑔
𝑻
𝑎𝑧 𝐶𝜃 𝐶𝜙

Where, 𝐶() and 𝑆() are cos() and sin(), respectively and 𝑎𝑥 , 𝑎𝑦 and 𝑎𝑧 are the
acceleration values obtained from the sensor. Form the above equation the roll and
pitch angle can be computed as –

𝑎𝑦
tan 𝜙 =
𝑎𝑧

𝑎𝑥
tan 𝜃 = −
√𝑎𝑦2 + 𝑎𝑧2
 Since, MPU6050 sensor also measures angular rotation rates (𝜔𝑥 , 𝜔𝑦 and 𝜔𝑧 ).
Roll, pitch and yaw angles can also be computed from angular rates by integration.

𝜃 = ∫ 𝜔𝑦 𝑑𝑡

𝜃𝑘 = 𝜃𝑘−1 + 𝜔𝑦 𝑘 Δ𝑡

Both the methods described above, have their own disadvantages. Angles
computed from accelerometer are very sensitive to vibration and noise whereas
angles obtained from gyro are drifting because during integration (summation), the
error also gets added (shown in figure below).
Angle

Time
Figure 2: Angle Vs Time from accelerometer and gyro
To minimize the disadvantages of the both the methods two different
approaches are used which are given as below –

1. Complementary Filter
2. Kalman Filter

In complementary filter, weighted sum of angles computed from both the

methods are used.

Angle = 𝑤1 (AngleGyro) + 𝑤2 (AngleAccelerometer)

𝐺𝑦𝑟𝑜
𝜃𝑘 = 𝑤1 𝜃𝑘 + 𝑤2 𝜃𝑘𝐴𝑐𝑐𝑒𝑙𝑒𝑟𝑜𝑚𝑒𝑡𝑒𝑟

𝐺𝑦𝑟𝑜
𝜙𝑘 = 𝑤1 𝜙𝑘 + 𝑤2 𝜙𝑘𝐴𝑐𝑐𝑒𝑙𝑒𝑟𝑜𝑚𝑒𝑡𝑒𝑟

Where, 𝑤1 + 𝑤2 = 1
One example is shown in figure below, in which 𝑤1 = 0.995 and 𝑤2 =
0.005.
Roll angle

Roll angle Vs time

THE KALMAN FILTER (1D)

Kalman filter is based on the weighted approaches as described above but slightly
advanced. The difference lies in the updating of weights after receiving the readings
from MPU 6050 sensor or in each iteration. In each iteration, values of weights are
updated based on the errors or uncertainties present in the measurement and
estimation. Kalman filter assumes errors or uncertainties as normally distributed
random variables. These uncertainties are computed or updated in each iteration. This
can be explained with an example given below –

Ex: A physical quantity 𝑥 is measured as 𝑥𝑚 and estimated (using system dynamics)

as 𝑥𝑒 . Then, the estimated value from the Kalman filter is –

𝑥̂ = 𝑤1 𝑥𝑚 + 𝑤2 𝑥𝑒

𝑤1 + 𝑤2 = 1

𝑥̂ = 𝑤1 𝑥𝑚 + (1 − 𝑤1 )𝑥𝑒

If the variance in measurement is ′𝑟′ and variance in estimation is ′𝑝′, then variance
in 𝑥̂ is given below –

𝑣𝑎𝑟(𝑥̂) = 𝑤12 𝑟 + (1 − 𝑤1 )2 𝑝
Kalman Filter is an optimum complementary filter which minimizes the variance
in the final estimation.

Variance in 𝑥̂ depends on weight (𝑤1). Minimum variance is given by –

𝑑[𝑣𝑎𝑟(𝑥̂)]
= 2𝑤1 𝑟 − 2(1 − 𝑤1 )𝑝 = 0
𝑑𝑤1
𝑝
𝑤1 =
𝑝+𝑟

𝑑2 [𝑣𝑎𝑟(𝑥̂)]
=𝑟+𝑝 >0
𝑑𝑤12
𝑝
Therefore, for 𝑤1 = 𝑝+𝑟, 𝑣𝑎𝑟(𝑥̂) is minimum.

𝑉𝑎𝑟𝑖𝑎𝑛𝑐𝑒 𝑖𝑛 𝐸𝑠𝑡𝑖𝑚𝑎𝑡𝑖𝑜𝑛
Kalman Gain (𝑤1 ) =
𝑉𝑎𝑟𝑖𝑎𝑛𝑐𝑒 𝑖𝑛 𝐸𝑠𝑡𝑖𝑚𝑎𝑡𝑖𝑜𝑛+𝑉𝑎𝑟𝑖𝑎𝑛𝑐𝑒 𝑖𝑛 𝑀𝑒𝑎𝑠𝑢𝑟𝑒𝑚𝑒𝑛𝑡
𝑝
𝑥̂ = 𝑥𝑒 + (𝑥 − 𝑥𝑒 )
𝑝+𝑟 𝑚

Angle form MPU6050 is measured from accelerometer and gyro. Standard deviation
in angle from accelerometer is ′𝜎𝑚 ′ (which is ~30) and variance in angle estimated from
gyro is given by –

𝑝 = (Δ𝑡)2 𝜎𝜔2

Where, 𝜎𝜔 is standard deviation in angular rates.

ALGORITHM OF THE KALMAN FILTER (1D)

𝑛
𝑎𝑥
𝜃𝑚 = − tan−1 ( )
√𝑎𝑦2 + 𝑎𝑧2

𝜃𝑒𝑛 = 𝜃𝑒𝑛−1 + 𝜔𝑦n Δ𝑡

𝑝𝜃𝑛−1 = 𝑝𝜃𝑛−1 + 𝜎𝜔2 𝑦 (Δ𝑡)2

𝑝𝜃𝑛−1
𝜃̂𝑒𝑛 = 𝜃𝑒𝑛 + 𝑘𝜃𝑛 (𝜃𝑚
𝑛
− 𝜃𝑒𝑛 ) = 𝜃𝑒𝑛 + 𝑛−1 2
𝑛
(𝜃𝑚 − 𝜃𝑒𝑛 )
𝑝𝜃 + 𝜎𝑚

𝑝𝜃𝑛 = (1 − 𝑘𝜃𝑛 ) 𝑝𝜃𝑛−1

CONTROLLER DESIGN

The block diagram of the controller and self-balancing bot are presented below –

Desired tilt Arduino Self-balancing Tilt angle

angle Nano bot

Controller Plant

MPU 6050
Measured
tilt angle

Figure 3: Block diagram of Self balancing bolt

The algorithm of the controller is based on the PID controller. The output of the controller is
the motor speed.
𝑑𝑒
motorSpeed = 𝐾𝑝 𝑒 + 𝐾𝑖 ∫ 𝑒𝑑𝑡 + 𝐾𝑑
𝑑𝑡
In the digital micro-controller, the above equation can be written as –

(𝑒−𝑒𝑝 )
motorSpeed = 𝐾𝑝 𝑒 + 𝐾𝑖 (𝑒 + 𝑒𝑝 )Δ𝑡 + 𝐾𝑑
Δ𝑡
Where, 𝑒: present error
𝑒𝑝 : previous iteration error

Δ𝑡: Loop time taken by micro-controller

For the present project, following are the values of PID gains:
𝐾𝑝 = 180, 𝐾𝑖 = 40 and 𝐾𝑑 = 0.3 and Δ𝑡 = 20ms
ASSEMBLY OF SELF BALANCING BOT

Assembly of the bot

STABILIATION OF THE BOT

Stabilization of the bot – 1

Stabilization of the bot – 2

Arduino Code:
#include <AccelStepper.h>
#include <Wire.h>

// Define the stepper motor and the pins that is connected to

AccelStepper stepper1(1, 5, 6);
AccelStepper stepper2(1, 3, 4);

float motorSpeed;
int ledPin = 13;
const int MPU = 0x68;
float AccX, AccY, AccZ;
float offsetAccX = 0; float offsetAccY = 0; float offsetAccZ = 0;
float Ts = 20.0;
float offestRollRate = 0.0;
float meaRoll;
float rollRate;
float roll;
float preError = 0;
float sumError = 0;
float error = 0;
float target = 0;

//1D-Kalman Filter Parameters

float rollEstVar = 0.0; float rollMeaStd = 3.0;
float rollRateStd = 4.0; float kalGain = 0.0;

//PID Gain Values

//1/8 step
/*int kp = 400;
int ki = 20;
int kd = 3.6;*/

//1/4 step
int kp = 180;
int ki = 40;
int kd = 0.3 ;

void initializeSensor()
{
// Perform full reset as per MPU-6000/MPU-6050 Register Map and Descriptions, Section 4.28,
pages 40 to 41.
//Serial.print(F("Performing full reset of MPU-6050..."));

Wire.beginTransmission(MPU); // Device address.

Wire.write(0x6B); // PWR_MGMT_1 register.
Wire.write(0b10001000); // DEVICE_RESET, TEMP_DIS.
Wire.endTransmission();
delay(100); // Wait for reset to complete.

Wire.beginTransmission(MPU); // Device address.

Wire.write(0x68); // SIGNAL_PATH_RESET register.
Wire.write(0b00000111); // GYRO_RESET, ACCEL_RESET, TEMP_RESET.
Wire.endTransmission();
delay(100); // Wait for reset to complete.

//Serial.println(F(" Done."));

// Disable SLEEP mode because the reset re-enables it. Section 3, PWR_MGMT_1 register, page 8.
//Serial.print(F("Disabling sleep mode of MPU-6050..."));
 Wire.beginTransmission(MPU); // Device address.
Wire.write(0x6B); // PWR_MGMT_1 register.
Wire.write(0b00001000); // SLEEP = 0, TEMP_DIS = 1.
Wire.endTransmission();
//Serial.println(F(" Done."));

// Writing to gyro config register.

Serial.print(F("Configuring gyro of MPU-6050..."));

Wire.beginTransmission(MPU); // Device address.
Wire.write(0x1B); // GYRO_CONFIG register.
Wire.write(0b00001000); // +/-500 deg/s.
Wire.endTransmission();
Serial.println(" Done.");
}

void gyro_signals(void){
Wire.beginTransmission(MPU);

// Low Pass Filter

Wire.write(0x1A);
Wire.write(0x05);
Wire.endTransmission();

// Range of MPU Accelerometer

Wire.beginTransmission(MPU);
Wire.write(0x1C);
Wire.write(0x10);
Wire.endTransmission();

//Getting Accelerations
Wire.beginTransmission(MPU);
Wire.write(0x3B);
Wire.endTransmission();
Wire.requestFrom(MPU, 6);

int16_t AccXLSB = Wire.read()<<8|Wire.read();

int16_t AccYLSB = Wire.read()<<8|Wire.read();
int16_t AccZLSB = Wire.read()<<8|Wire.read();

AccX = (float)AccXLSB/4096 - offsetAccX;

AccY = (float)AccYLSB/4096 - offsetAccY;
AccZ = (float)AccZLSB/4096 + offsetAccZ ;

//Range of GyroRates
Wire.beginTransmission(MPU);
Wire.write(0x1B);
Wire.write(0x8);
Wire.endTransmission();

//Getting GyroRates
Wire.beginTransmission(MPU);
Wire.write(0x43);
Wire.endTransmission();
Wire.requestFrom(MPU, 6);

int16_t GyroX = Wire.read()<<8|Wire.read();

int16_t GyroY = Wire.read()<<8|Wire.read();
int16_t GyroZ = Wire.read()<<8|Wire.read();
 rollEstVar += pow((0.001*Ts*rollRateStd), 2);
kalGain = rollEstVar/(rollEstVar + pow(rollMeaStd, 2));
rollRate = (float)GyroX/65.5 - offestRollRate;
roll += 0.001*Ts*rollRate;
meaRoll = atan(AccY/AccZ)*180/3.1416;
roll += kalGain*(meaRoll - roll);
rollEstVar = (1 - kalGain)*rollEstVar;
}

void setup() {
// put your setup code here, to run once:
Serial.begin(9600);
stepper1.setMaxSpeed(16000);
stepper2.setMaxSpeed(16000);
pinMode(ledPin, OUTPUT);
initializeSensor();
Wire.setClock(400000);
Wire.begin();
delay(250);
Wire.beginTransmission(MPU);
Wire.write(0x6B);
Wire.write(0x00);
Wire.endTransmission();

//Calibration of Gyro
Serial.println("\nCalibration is going on.....");
digitalWrite(ledPin, HIGH);
int N = 2000; float sumRoll = 0;
float sumX =0; float sumY = 0; float sumZ = 0;
for (int i = 1; i < N; i++){
gyro_signals();
sumRoll += rollRate;
sumX += AccX; sumY += AccY; sumZ += AccZ;
}
offestRollRate = sumRoll/N;
offsetAccX = sumX/N;
offsetAccY = sumY/N;
offsetAccZ = 1 - sumZ/N;
digitalWrite(ledPin, LOW);
roll = 0; meaRoll = 0;
Serial.println("Calibration is completed.");
}

void loop() {
static unsigned long timer = 0;
if ((millis() - timer) > Ts){
timer = millis();
gyro_signals();
error = target - roll;
motorSpeed = kp*error + kd*(error - preError)/(Ts/1000) + ki*(0.5*(error+preError) +
sumError)*(Ts/1000);
sumError += error;
preError = error;
if (abs(error) < 0.1 || abs(error) > 70){
motorSpeed = 0;}

/*Serial.print(error);
Serial.print("\t");
Serial.println(motorSpeed);*/
}
if (motorSpeed > 8000){
 motorSpeed = 8000;
}
if (motorSpeed < -8000){
motorSpeed = -8000;
}

stepper1.setSpeed(-motorSpeed);
stepper2.setSpeed(-motorSpeed);
stepper1.runSpeed();
stepper2.runSpeed();
}