How to convert PID outputs to appropriate motor speeds for a quad copter

Question

I am building an autonomous robot using PID control algorithm. So, far I have implemented PID using online resources/references. I am testing for stabilizing an axis of the quad copter. However, I am not successful to stabilize even one axis. Description: My input for the PID is an angle value i.e the orientation of the quad copter measured by AHRS (a Gyroscope that measures angles) and the motors take integer values as speeds. What I am doing is,

motor_right_speed = base_speed + adjusted_value;
motor_left_speed = base_speed - adjusted_value;
adjusted_value += PID_output;

Where ajusted_value is a buffer that accumulates or subtracts the PID output value based on either PID output is +ve or -ve.

I also tried,

motor_right_speed = base_speed + PID_output;
motor_left_speed = base_speed - PID_output;

Both don't seem to be working.

I have tested using a wide range of P gain values (from very small to very large), but the quad copter only oscillates; it does not self-correct. Your help with suggestions would be greatly appreciated. Thanks!

Answer 1

I would recommend to double check the algorithm and make sure that is able to output negative values. You should post your pid code.

The approach you are using is a simple and quite effective control strategy in which the knowledge of the model is not required to get the job done. By 'knowledge of the model' I mean the system of equations that describes the dynamics of your quadcopter in space. You just need the reference value, the filtered measurement (the estimated angle) and three coefficients.

In your project you want your roll and yaw angle to be 0, at least in hovering mode right. Let's consider as a measured variable y(t), the roll angle theta(t) at time t (the pitch angle phi will be another one). So y(t) = theta(t).

• First of all, make sure that your estimated angles are accurate.

Then define the control error as the difference between the desired position, y_des = 0, and the actual measurement at time t, y(t).

e(t) = y_des - y(t)

In order to understand the effect of each coefficient, (Kp,Kd and Ki) we need to look at the control formula.

The control input u(t) is the sum of three different terms:

• The proportional term is described by the Kp gain which tells the control how intense should the response be compared to the error. What does it mean? If at time t, the quad is 3 degree away from your setpoint (e(t)=3) then apply a torque Kp times that value to get it back the correct position. If the value is to high you will pass your set point and you will notice oscillations around the 0 value. The steady state error will decrease with increasing gain, but the tendency towards oscillation will also increase. You can't get good results only with this term.

• The derivative term is characterized by the Kd gain. In the formula, this coefficient is multiplied by the derivative of the error. In other words, this gain tells the controller how fast should it react to changes in error's values. If you are going away from your set point, the error's derivative will be different than zero and positive (or negative) so apply a torque proportionally to this value. The derivative action will not help if the sample time is too large.

• The integral term is described by the Ki gain. This coefficient weights the contribution of how long does the quadcopter remains in a state of error. If you are testing the control only on one axis and you are using a too low Proportional gain, Kp, you will notice that the quad won't be able to reach the 0 degree reference value and will be "stuck" at a certain angle. Adding Ki != 0 will help by providing an extra torque value after an interval of time. The Ki gain will determine this extra torque value.

Here is example using arduino. (This is a basic code and what's above is pub version of the explanation)

  // Roll PID
  errorRoll = abs(SetpointRoll - estimatedKalmanRoll); //distance away from setpoint
  if(errorRoll<thresholdRoll)
  {  
    //we're close to setpoint, use conservative tuning parameters
    myRollPID.SetTunings(consKpRoll, consKiRoll, consKdRoll);
  }
  else
  {
     //we're far from setpoint, use other parameters
     myRollPID.SetTunings(aggKpRoll, aggKiRoll, aggKdRoll);
  }

  myRollPID.Compute(); // This computes rollpid   

Same thing for pitch, yaw and altitude

  // computes motor IN based on roll, pitch, yaw Pid OUT and throttle value
  motorA = thr + rollpid - yawpid;
  motorB = thr + pitchpid + yawpid;
  motorC = thr - rollpid - yawpid;
  motorD = thr - pitchpid + yawpid

Check this answer as well.

Hope this helps, Ciao.

Answer 2

I have been there before :) First double check that everything else is working. My IMU was not working in flight at first, but I didn't realise it because I only ever had tested it with the motors off. The vibrations made my algorithm go crazy. The sensors built-in low-pass filter helped.

Next a gyro measures angular rates, not angles. So you can know the speed at which you are turning around an axis, but not the angle itself. You could integrate the angular rate and get the angle, but the gyro drift would soon send you back down on the ground. In order to achieve angular stabilisation you need an accelerometer. And if you needed to maintain a precise heading you would need a magnetometer.

This said with just a gyro you can stabilise the angular rate in what some call the "manual" mode. Piloting in this mode requires skills but it's not impossible to learn. For this the input of the PID is an angular rate, and the output is a difference to apply between opposed motors (based on your pseudo-code I'm assuming you have a + configuration, X is is not much more complicated).

The correct formula to stabilise the roll rate only is the second one you used: motor_right_speed = base_speed + PID_output; motor_left_speed = base_speed - PID_output; You want to be careful about making sure the output remains within the motors bounds. My experience is that PD controllers work the best for roll/pitch stabilisation (leave the I term null). Start at 0 for all the constants, raise P until the quad oscillates slightly, then increase D until the oscillations from P dampen. Repeat until D cannot compensate the oscillations and use the last stable pair of constants. In order for this to work you have to use a test rig. If you try to stabilise only 1 anglular rate in flight you will crash.

I would recommend not assuming the problem is with the PID: based on what you said it's not unlikely the issue is somewhere else. In any case try to isolate the different units of your program and test them independently. It's much harder to debug a whole system than its separate components. Good luck, you'll be happy when it works!