Stabilising a quadcopter using YPR

Question

I'm using the MPU-6050 accelerometer + gyro with the library I2Cdev which outputs: quaternion, euler angles and YPR angles. The equations used for calculating the YPR are:

uint8_t MPU6050::dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity) {
  // yaw: (about Z axis)
  data[0] = atan2(2 * q -> x * q -> y - 2 * q -> w * q -> z, 2 * q -> w * q -> w + 2 * q -> x * q -> x - 1);
  // pitch: (nose up/down, about Y axis)
  data[1] = atan(gravity -> x / sqrt(gravity -> y * gravity -> y + gravity -> z * gravity -> z));
  // roll: (tilt left/right, about X axis)
  data[2] = atan(gravity -> y / sqrt(gravity -> x * gravity -> x + gravity -> z * gravity -> z));
  return 0;
}

I want to stabilize a quadcopter with these values and 3 PID regulators like this:

• FL = Throttle + (-PitchPID) + (-RollPID) + (+YawPID)
• FR = Throttle + (-PitchPID) + (+RollPID) + (-YawPID)
• RL = Throttle + (+PitchPID) + (-RollPID) + (+YawPID)
• RR = Throttle + (+PitchPID) + (+RollPID) + (-YawPID)

The pitch and roll values are between -90 and +90 degrees (0 degrees is horizontal and +-90 is vertical). The problem is that when the quad starts tipping over, the error will start decreasing and will stabilize upside down.

Answer 1

You can't use an absolute value for the feedback in a PID controller.

Integral error accumulates the control error, which is the difference between what you want and what you have. So, consider a basic case where you want level flight (reference = 0).

If you take the absolute value of the feedback, then in ALL cases your feedback is either zero or positive. That means that the control error (reference - feedback), is ALWAYS NEGATIVE.

The integral error term can ONLY accumulate negative values. Typically you would overshoot the reference, at which point the sign would flip on the feedback and you could begin accumulating POSITIVE values, which would "bleed" the integral error term.

You have no means to reduce integral error. It will be a large negative number that will only ever get more negative (until you reset it or it overflows).

You can't just flip the sign on the output of the PID controller when a feedback signal changes signs because that doesn't do anything to correct the error terms that drive the controller.

---

Contributing to the problems you're having with the IMU are the facts that it looks like you're not converting from the quaternion to Euler angles correctly, and maybe also that you're not even getting a quaternion.

Any time you see an atan function your first thought should be, "Something's wrong." The atan function cannot differentiate between $(-y/x)$ and $(y/-x)$, so it can't always give you a correct solution.

I can see where you got the calculations and I understand that a sqrt() function will always return a positive value, but that's a problem too, isn't it? If your denominator is always positive then you know that you're never going to get a value of $-x$ and thus you know that you're not going to get a full range of angles from the calculation.

I would suggest using the code provided on Wikipedia if you're going to do this conversion yourself:

static void toEulerianAngle(const Quaterniond& q, double& roll, double& pitch, double& yaw)
{
  double ysqr = q.y() * q.y();

  // roll (x-axis rotation)
  double t0 = +2.0 * (q.w() * q.x() + q.y() * q.z());
  double t1 = +1.0 - 2.0 * (q.x() * q.x() + ysqr);
  roll = std::atan2(t0, t1);

  // pitch (y-axis rotation)
  double t2 = +2.0 * (q.w() * q.y() - q.z() * q.x());
  t2 = t2 > 1.0 ? 1.0 : t2;
  t2 = t2 < -1.0 ? -1.0 : t2;
  pitch = std::asin(t2);

  // yaw (z-axis rotation)
  double t3 = +2.0 * (q.w() * q.z() + q.x() * q.y());
  double t4 = +1.0 - 2.0 * (ysqr + q.z() * q.z());  
  yaw = std::atan2(t3, t4);
}

But again, back to the source of the quaternion - how are you getting that value? From the documentation page you linked:

Currently, the source code available will only provide basic device configuration and raw accel/gryo readings.

When I searched the PDF files at the bottom of the product's website, there were NO mentions of "quaternion" or "Euler" or "angle," so I'm not sure what the "motion processing" feature of the chip is or what it's supposed to do.

So, in summary:

1. Double check how/where/if you are actually getting a quaternion,
2. Check the math on how you are converting a quaternion to an Euler angle and verify that math checks out for all angles, and
3. Always use signed feedback for a PID controller.