Enhancing Robot Stability: Sensor Fusion with MPU6050

 

Whether you are building a bipedal android or a quadruped robot, stability is the ultimate challenge. If a robot cannot calculate its orientation in 3D space, it cannot balance. Academic studies in biomimetics heavily emphasize "Sensor Fusion"—the mathematical process of combining data from multiple sensors to achieve a highly accurate reading.

In practical embedded systems, this is almost exclusively done using an Inertial Measurement Unit (IMU) like the MPU6050. This article bridges the gap between the theory of Sensor Fusion and the C++ code required to keep an ESP32-powered robot standing upright.

The Academic Theory: The Noise Problem

The MPU6050 contains two critical sensors:

1. The Accelerometer: Measures gravitational force. It is highly accurate over long periods but extremely sensitive to sudden vibrations and "noise."

2. The Gyroscope: Measures the rate of rotation. It is highly accurate in the short term, but over time, its readings "drift" away from reality.

Research shows that using either sensor alone leads to failure. Sensor Fusion solves this. While advanced bionics use complex Kalman Filters, a highly effective and computationally cheaper method for microcontrollers is the Complementary Filter.

The Complementary Filter applies a "Low-Pass" filter to the accelerometer (trusting its long-term data and ignoring sudden shakes) and a "High-Pass" filter to the gyroscope (trusting its quick changes but ignoring long-term drift).

The Engineering Application: C++ on the ESP32

To implement this on an ESP32 or Arduino, we use the Wire.h library to read raw I2C data from the MPU6050, process the math, and output a clean Pitch and Roll angle.

The C++ Code

C++

#include <Wire.h>

const int MPU_ADDR = 0x68; // Standard I2C address for MPU6050

// Variables for sensor data
int16_t accX, accY, accZ;
int16_t gyroX, gyroY, gyroZ;

// Variables for calculated angles
float accAngleX, accAngleY;
float gyroAngleX = 0, gyroAngleY = 0;
float pitch = 0, roll = 0;

// Timing variables
unsigned long previousTime, currentTime;
float elapsedTime;

void setup() {
  Serial.begin(115200);
  Wire.begin();
  
  // Wake up the MPU6050
  Wire.beginTransmission(MPU_ADDR);
  Wire.write(0x6B); 
  Wire.write(0);     
  Wire.endTransmission(true);
  
  previousTime = millis();
}

void loop() {
  // 1. Calculate time elapsed since last loop
  currentTime = millis();
  elapsedTime = (currentTime - previousTime) / 1000.0; // Convert to seconds
  previousTime = currentTime;

  // 2. Read Raw Data
  Wire.beginTransmission(MPU_ADDR);
  Wire.write(0x3B); // Starting register for Accel Readings
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_ADDR, 14, true); 
  
  accX = Wire.read() << 8 | Wire.read();
  accY = Wire.read() << 8 | Wire.read();
  accZ = Wire.read() << 8 | Wire.read();
  // Skip temperature data
  Wire.read(); Wire.read(); 
  gyroX = Wire.read() << 8 | Wire.read();
  gyroY = Wire.read() << 8 | Wire.read();
  gyroZ = Wire.read() << 8 | Wire.read();

  // 3. Calculate Accelerometer Angles
  // Convert raw values to degrees using basic trigonometry
  accAngleX = (atan(accY / sqrt(pow(accX, 2) + pow(accZ, 2))) * 180 / PI);
  accAngleY = (atan(-1 * accX / sqrt(pow(accY, 2) + pow(accZ, 2))) * 180 / PI);

  // 4. Calculate Gyroscope Angles
  // Convert raw value to degrees/second (based on standard sensitivity 131.0)
  gyroAngleX = gyroAngleX + (gyroX / 131.0) * elapsedTime;
  gyroAngleY = gyroAngleY + (gyroY / 131.0) * elapsedTime;

  // 5. THE COMPLEMENTARY FILTER (Sensor Fusion)
  // 96% Trust in Gyro (short term), 4% Trust in Accel (long term)
  roll = 0.96 * gyroAngleX + 0.04 * accAngleX;
  pitch = 0.96 * gyroAngleY + 0.04 * accAngleY;

  // Output the clean, fused data
  Serial.print("Pitch: "); Serial.print(pitch);
  Serial.print(" | Roll: "); Serial.println(roll);
  
  delay(10); // Loop delay
}

Practical Considerations

Once this data is fused, the pitch and roll variables can be plugged directly into a PID (Proportional-Integral-Derivative) controller to adjust the robot's servo motors. By understanding how to write your own Complementary Filter, you remove the reliance on bulky external libraries and gain complete control over your robot's central nervous system.

by Malik Hassan