Self-Balancing Robot: Inverted Pendulum Physics and PID Control

A two-wheel self-balancing robot stands upright the same way you balance a stick on your palm: when it starts to fall forward, the wheels drive forward to get back under it. It is one of the most satisfying student projects because it combines everything from this series: motor drivers, an IMU, a fast control loop and careful PID tuning.

This tutorial explains the physics that makes balancing hard, the mechanical choices that make it easier, and a complete Arduino program that uses the MPU6050 complementary filter from the previous tutorial with a PID controller on the tilt angle. It finishes with a tuning procedure and an outline of the cascaded speed loop that stops the robot from wandering.

Inverted Pendulum Physics

Model the robot as a mass m at height l above the wheel axle. When it tilts by a small angle theta, gravity produces a torque that increases the tilt. For a point mass on a light rod the equation of motion is:

theta_double_dot = (g / l) * theta        (small angles, wheels fixed)

Solution grows like e^(t * sqrt(g/l))
l = 0.20 m -> sqrt(9.81 / 0.20) = 7.0 per second -> time constant about 0.14 s
l = 0.40 m -> sqrt(9.81 / 0.40) = 5.0 per second -> time constant about 0.20 s

Two lessons follow. First, the robot falls on a time scale of a fraction of a second, so the control loop must run far faster, typically 100 to 200 Hz or more. Second, a taller robot falls more slowly and is easier to balance, which surprises many beginners. Placing the battery high, not low, often helps.

Mechanical and Electrical Design

Choice	Recommendation	Reason
Motors	Metal gear motors with low backlash, ideally with encoders	Gear play acts as a dead zone that causes wobble
Wheels	Larger diameter with rubber tyres	More ground speed per RPM and good grip
IMU position	Close to the wheel axle, firmly mounted	Less vibration and less acceleration error
Frame	Rigid, two or three stacked plates	Flexing frames feed vibration into the IMU
Battery	2S or 3S Li-ion pack mounted high	Raises the centre of mass and supplies motor current
Driver	TB6612FNG for small motors, a higher-current driver for larger ones	Fast response with little voltage loss

Balancing Code

Wiring: MPU6050 on A4 and A5; TB6612FNG with PWMA 5, AIN1 7, AIN2 8, PWMB 6, BIN1 9, BIN2 10, STBY 4. The IMU's X axis must point along the wheel axle so that forward tilt changes the angle.

Two-wheel balancing robot: complementary filter plus PID at 200 Hz

#include <Wire.h>

// ---------- Pins ----------
const int PWMA = 5, AIN1 = 7, AIN2 = 8;
const int PWMB = 6, BIN1 = 9, BIN2 = 10, STBY = 4;
const uint8_t MPU = 0x68;

// ---------- Tuning ----------
float Kp = 25.0, Ki = 150.0, Kd = 0.8;   // starting points only: tune on your robot
float setpoint = 0.0;     // angle where the robot balances (find by experiment)
const int DEADBAND = 25;  // minimum PWM that makes the wheels move
const float FALL_LIMIT = 40.0;
const float ALPHA = 0.98;
const unsigned long LOOP_US = 5000;      // 200 Hz

float angle = 0, gyroOffset = 0, integral = 0;
unsigned long lastUs;

void writeReg(uint8_t r, uint8_t v) {
  Wire.beginTransmission(MPU); Wire.write(r); Wire.write(v); Wire.endTransmission();
}

void readIMU(float &accAngle, float &gyroRate) {
  Wire.beginTransmission(MPU); Wire.write(0x3B); Wire.endTransmission(false);
  Wire.requestFrom(MPU, (uint8_t)14);
  int16_t d[7];
  for (int i = 0; i < 7; i++) {
    uint8_t hi = Wire.read();
    uint8_t lo = Wire.read();
    d[i] = (int16_t)((hi << 8) | lo);
  }
  accAngle = atan2((float)d[1], (float)d[2]) * 180.0 / PI;   // accel Y, Z
  gyroRate = (d[4] - gyroOffset) / 131.0;                    // gyro X, deg/s
}

void motor(int pwmPin, int in1, int in2, int s) {
  s = constrain(s, -255, 255);
  digitalWrite(in1, s > 0 ? HIGH : LOW);
  digitalWrite(in2, s < 0 ? HIGH : LOW);
  analogWrite(pwmPin, abs(s));
}

void setMotors(float u) {
  int pwm = (int)u;
  // Deadband compensation: jump past the PWM range where wheels do not move
  if (pwm > 0) pwm += DEADBAND;
  else if (pwm < 0) pwm -= DEADBAND;
  motor(PWMA, AIN1, AIN2, pwm);
  motor(PWMB, BIN1, BIN2, pwm);
}

void setup() {
  int outs[] = {PWMA, AIN1, AIN2, PWMB, BIN1, BIN2, STBY};
  for (int p : outs) pinMode(p, OUTPUT);
  digitalWrite(STBY, HIGH);

  Wire.begin(); Wire.setClock(400000);
  writeReg(0x6B, 0x00); writeReg(0x1A, 0x03);
  writeReg(0x1B, 0x00); writeReg(0x1C, 0x00);

  delay(500);
  long sum = 0;                                   // gyro calibration, keep still
  for (int i = 0; i < 1000; i++) {
    Wire.beginTransmission(MPU); Wire.write(0x43); Wire.endTransmission(false);
    Wire.requestFrom(MPU, (uint8_t)2);
    uint8_t hi = Wire.read(); uint8_t lo = Wire.read();
    sum += (int16_t)((hi << 8) | lo);
    delay(2);
  }
  gyroOffset = sum / 1000.0;

  float a, g;
  readIMU(a, g);
  angle = a;
  lastUs = micros();
}

void loop() {
  if (micros() - lastUs < LOOP_US) return;
  unsigned long now = micros();
  float dt = (now - lastUs) / 1000000.0;
  lastUs = now;

  float accAngle, gyroRate;
  readIMU(accAngle, gyroRate);
  angle = ALPHA * (angle + gyroRate * dt) + (1 - ALPHA) * accAngle;

  if (fabs(angle - setpoint) > FALL_LIMIT) {     // fallen over: stop everything
    setMotors(0);
    integral = 0;
    return;
  }

  float error = angle - setpoint;
  integral = constrain(integral + error * dt, -1.0, 1.0);
  // Use the gyro rate directly as the derivative: cleaner than differencing angle
  float u = Kp * error + Ki * integral + Kd * gyroRate;
  setMotors(u);
}

Key Points in the Code

The derivative term uses the gyro rate directly. The rate of change of the angle is exactly what the gyro measures, so this avoids the noise of differencing the filtered angle.
The sign convention assumes that a positive angle means leaning towards the direction in which positive PWM drives. If the robot accelerates away from its fall, negate u or swap both motor wires.
Deadband compensation adds a fixed PWM so that small corrections actually move the wheels instead of being lost in motor friction.
The integral is clamped and reset after a fall so that it does not wind up while the robot lies on the floor.

Tuning Procedure

Find the setpoint: hold the robot at the point where it balances by itself without help and read the angle on the Serial Monitor in a separate test sketch. Enter that value as setpoint.
Set Ki and Kd to zero. Increase Kp until the robot tries to stand but oscillates back and forth around the setpoint.
Add Kd in small steps until the oscillation is damped and the robot feels stiff when lightly pushed.
Add a small Ki to stop it leaning and slowly running in one direction.
If it still drifts across the floor, add an outer speed loop: use wheel encoder speed to adjust the setpoint by a small amount, so the robot leans back against unwanted motion.

This last step is called cascaded control. The fast inner loop keeps the robot upright around a target angle, and the slower outer loop, perhaps at 20 to 50 Hz, chooses that angle to hold a target speed. Remote steering is then done by adding opposite offsets to the two motors.

Summary

A balancing robot is an unstable inverted pendulum that falls within a fraction of a second, so it needs a fast, fixed-rate loop. A rigid frame, low-backlash motors and a well-mounted IMU make the control problem easier. A PID controller on the filtered tilt angle, with the gyro rate as derivative, deadband compensation and a fall cutoff, gets it standing; an outer speed loop keeps it in place.