MPU6050 values drifting.

Other Hardware Robotics
1

Hello!

I'm currently working on a 2WD self-balancing robot. The problem I'm having is that the gyro/acc values are drifting and therefore the robot eventually will fall over.
The robot balances for about 5 seconds but after that it always falls over into the same direction. When I read the values on the serial monitor i noticed that even if the robot is still, the roll value increases in a steady pace.
Here's my code:

#include <PID_v1.h>
#include <Wire.h>

const int MPU = 0x68; // MPU6050 I2C address
float AccX, AccY, AccZ;
float GyroX, GyroY, GyroZ;
float AX_angle, AY_angle, GX_angle, GY_angle;
float roll, pitch, yaw;
float elapsedTime, currentTime, previousTime = 0;
// Motor control
const int LM1 = 2;
const int LM2 = 4;
const int RM1 = 6;
const int RM2 = 7;
const int LMpwm = 3;
const int RMpwm = 5;
const int ctrl5V = 10;


// PID
double setPoint;
double input = 0;
double output = 0;
const double Kp = 0.3;
const double Ki = 0.5;
const double Kd = 0.025;
PID pid(&input, &output, &setPoint, Kp, Ki, Kd, DIRECT);



void setup(){
  Serial.begin(19200);
  Wire.begin();
  Wire.beginTransmission(MPU);
  Wire.write(0x6B);
  Wire.write(0x00);
  Wire.endTransmission(true);
  // Motor control
  pinMode(LM1, OUTPUT);
  pinMode(LM2, OUTPUT);
  pinMode(RM1, OUTPUT);
  pinMode(RM2, OUTPUT);
  pinMode(LMpwm, OUTPUT);
  pinMode(RMpwm, OUTPUT);
  pinMode(ctrl5V, OUTPUT);
  digitalWrite(ctrl5V, HIGH);
  // PID
  setPoint = roll;
  pid.SetMode(AUTOMATIC);
  pid.SetSampleTime(10);
  pid.SetOutputLimits(-255, 255);
}

void loop(){
  // Accelerometerns vinklar:

  Wire.beginTransmission(MPU);
  Wire.write(0x3B); // ACC Address 
  Wire.endTransmission(false);
  Wire.requestFrom(MPU, 6, true);
  AccX = (Wire.read() << 8 | Wire.read()) / 16384.0 ; // Kombinerar 2 mätvärden
  AccY = (Wire.read() << 8 | Wire.read()) / 16384.0 ;
  AccZ = (Wire.read() << 8 | Wire.read()) / 16384.0 ;

 
  
  AX_angle = ((atan((AccY) / sqrt(pow((AccX), 2) + pow((AccZ), 2))) * 180 / PI) - 1.22 );
  AY_angle = ((atan(-1 * (AccX) / sqrt(pow((AccY), 2) + pow((AccZ), 2))) * 180 / PI) + 0.73 );


  // Gyroskopets vinklar:
  
  currentTime = millis();            
  elapsedTime = (currentTime - previousTime) / 1000;
  
  Wire.beginTransmission(MPU);
  Wire.write(0x43);
  Wire.endTransmission(false);
  Wire.requestFrom(MPU, 6, true);

  GyroX = (Wire.read() << 8 | Wire.read()) / 131;
  GyroY = (Wire.read() << 8 | Wire.read()) / 131; 
  GyroZ = (Wire.read() << 8 | Wire.read()) / 131; 

  //GyroX -= 0.32;
  //GyroY += 0.38;
  
  GX_angle += GyroX * elapsedTime;
  GY_angle += GyroY * elapsedTime;
  previousTime = currentTime;  
  

  // Complementary filter

  roll = 0.9 * GX_angle + 0.1 * AX_angle;
  pitch = 0.96 * GY_angle + 0.04 * AY_angle;

  input = roll;
  Serial.println(input);

  pid.Compute();
  if(roll > setPoint)
    forward();
  else if(roll < setPoint)
    backwards();
  /*else
    halt();*/
    
}

void forward(){
  digitalWrite(LM1, HIGH);
  digitalWrite(LM2, LOW);
  digitalWrite(RM1, LOW);
  digitalWrite(RM2, HIGH);
  analogWrite(LMpwm, output);
  analogWrite(RMpwm, output);

  
}

void backwards(){
  digitalWrite(LM1, LOW);
  digitalWrite(LM2, HIGH);
  digitalWrite(RM1, HIGH);
  digitalWrite(RM2, LOW);
  analogWrite(LMpwm, -output);
  analogWrite(RMpwm, -output);

  
}

void halt(){
  analogWrite(LMpwm, 0);
  analogWrite(RMpwm, 0);
}

I've read that gyro values drift over time but I'm using a complementary filter, and the values increase in such a steady pace that i have a hard time believing that's the problem.

What causes the problem?

// Markus A

2

A calibration routine that gets drift rate would allow you to compensate for the drift.

3

Hi markusAnd,

I think the issue is that in your complementary filter, the gyro angle estimation needs to take the angle from the filter output (variables: roll and pitch) and not the previous gyroscope angles (variables GX_angle and GY_angle):

GX_angle = GyroX * elapsedTime + roll;
GY_angle = GyroY * elapsedTime + pitch;

It's probably not an issue for a 2 wheeled robot, but pitch and roll are not independent variables and are affected by yaw motion. To cross couple yaw motion into pitch and roll angles:

gyroRollAngle = gyroRollRate * dt + lastFilteredRollAngle -                // Integrate the gyro roll rate and add the last computed roll angle
  lastFilteredPitchAngle * DEGREES_TO_RADIANS * gyroYawRate * dt;          // This line adds yaw coupling - using small angle approximation
//lastFilteredPitchAngle * sin(DEGREES_TO_RADIANS * gyroYawRate * dt);     // Formula without small angle approximaion
gyroPitchAngle = gyroPitchRate * dt + lastFilteredPitchAngle +             // Integrate the gyro pitch rate and add the last computed pitch angle
  lastFilteredRollAngle * DEGREES_TO_RADIANS * gyroYawRate * dt;           // This line adds yaw coupling - using small angle approximation
//lastFilteredRollAngle * sin(DEGREES_TO_RADIANS * gyroYawRate * dt);      // Formula without small angle approximation
4

Yes, the complementary filter is wrong.

You really need to do the full 3D AHRS to get the angles correct. The Madgwick/Mahony AHRS filter works quite well.

5

Thanks for the answers! I changed to a Kalman filter which is giving me more accurate readings.

// Markus