PID control for drone rotations

[Joel Graff]

Hello,

I'm playing around with simulating drones (quadcopters) in Gazebo (an open source robotics simulator).

The control system is made up of six PIDs (one for each degree of freedom) and I'm encountering trouble tuning the pids for pitch / roll control.

In this case, the linear x / y and roll / pitch control are managed by two cascaded pairs of PIDs. That is, the X velocity is passed into the outer pid of one pair, which then supplies an angle of rotation to the inner pid. The inner pid then calculates the angular acceleration for a direct calculation of the resulting torque.

The physics works fine, but the problem is overcoming the angular momentum. In this case, if I fix the drone in free space and just command a constant 0.392 radian rotation (about 45 degrees), the drone will oscillate between 0 and 45 degrees. The reason, of course, is because as the PID scales it's output w.r.t the limit, it can't take into account the angular momentum that will continue to carry the rotation. Thus, the overcorrection that returns the drone to horizontal.

Not being a big physics guy (or drone guy), I'm at a bit of a loss as to how to address the issue...

Any tips? (Or pointers to forums that are more suited to the problem?) :)

 

[anorlunda]

The reason, of course, is because as the PID scales it's output w.r.t the limit, it can't take into account the angular momentum that will continue to carry the rotation. Thus, the overcorrection that returns the drone to horizontal.

Not account directly, but indirectly via tuning. With PID you have three tuning adjustments to make and finding the best adjustment is a bit of art.
Here is a pretty good general method for tuning.
https://en.wikipedia.org/wiki/Ziegler–Nichols_method

But if it is oscillating they way you said, I suspect a configuration error is more likely than a tuning problem.

Can you post your configuration. What is the set point and the feedback, and where does the output go for each of the PID controllers?

 

[Joel Graff]

Yeah, I've opted to use a genetic algorithm to tune it. Not convinced it's the best approach, but in the linear velocity case (no rotation pids), it seemed to consistently demonstrate convergence... May take a shot at it with gradient descent here eventually, but I need to be able to capture the response metrics a bit better than I am (rise / settle time, overshoot, decay, etc)... I haven't tried Ziegler-Nichols because it requires extra work to capture the different error metrics. I may end up giving it a shot, though, if nothing else works.

Still, I'm pretty sure this doesn't have anything to do with gain tuning - watching the output tells me the angular momentum is what's really driving the response and no amount of gain is going to overcome that. The reason is because if I use strong gains to correct more quickly, it drives the angular mometum further in the other direction, and the oscillation remains.

The PID configuration is derived from the source code here: https://github.com/tu-darmstadt-ros...ns/src/gazebo_quadrotor_simple_controller.cpp

The code of interest is in the 'void GazeboQuadrotorSimpleController::Update()' function.

The most relevant code is as follows, it's my code that is modified somewhat from what's found in the Hector gazebo codebase:
----------------------------

mPose = mLink->GetWorldPose();
mAngularVelocity = mLink->GetWorldAngularVel();
ang_accel = mLink->GetWorldAngularAccel() - mAngAccel;
mAcceleration = (mLink->GetWorldLinearVel() - mVelocity) / dt;
mVelocity = mLink->GetWorldLinearVel();
mAngAccel = mLink->GetWorldAngularAccel();

// Get gravity
math::Vector3 gravity_body = pose.rot.RotateVector(mWorld->GetPhysicsEngine()->GetGravity());
double gravity = gravity_body.GetLength();
loadFactor = gravity * gravity / mWorld->GetPhysicsEngine()->GetGravity().Dot(gravity_body); // Get gravity

// update controllers
mForce.Set(0.0, 0.0, 0.0);
mTorque.Set(0.0, 0.0, 0.0);

roll = mPose.rot.GetRoll();
pitch = mPose.rot.GetPitch();
yaw = mPose.rot.GetYaw();

pitchCommand = .392; // -mPids[PidType::VelocityX]->update(0, mVelocity.x, mAcceleration.x, dt);
rollCommand = mPids[PidType::VelocityY]->update(0, mVelocity.y, mAcceleration.y, dt);

aa_x = mPids[PidType::Roll]->update(rollCommand, roll, mAngularVelocity.x, dt);
aa_y = mPids[PidType::Pitch]->update(pitchCommand, pitch, mAngularVelocity.y, dt);

mTorque.x = mInertia.x * aa_x;
mTorque.y = mInertia.y * aa_y;
mTorque.z = mInertia.z * mPids[PidType::Yaw]->update(ang_vel.z, mAngularVelocity.z, 0, dt);
mForce.z = mMass * (mPids[PidType::VelocityZ]->update(9, mVelocity.z, mAcceleration.z, dt) + loadFactor * gravity);

if (mMaxForce > 0.0 && mForce.z > mMaxForce) mForce.z = mMaxForce;
if (mForce.z < 0.0) mForce.z = 0.0;

mLink->AddRelativeForce(mForce);
mLink->AddRelativeTorque(mTorque - mLink->GetInertial()->GetCoG().Cross(mForce));

---------------------------

Basically, the system computes the forces / torques relative to the body frame of the drone. Linear velocity PIDs (X and Y) are stacked with angular velocity PIDs (outer / inner loop scheme), such that the linear velocity PIDs output an angle of rotation for the angular PID setpoints. X velocity drives pitch and Y velocity drives roll. The angular pids then compute an angular acceleration to compute the resultant torques according to T = I*a.

Currently, I'm using 0.392 radians as my setpoint for the pitch / roll PIDs with no horizontal velocity and a 9 m/s vertical velocity.

The pid code applies both input and output limits (a bit redundant, but I went with it anyway). Integral terms are limited by the output limit to overcome integral windup. For the X/Y velocity pids, the input limit is 18, and the integral/output limit is 0.392. Likewise, my angular pids limit input to 0.392 and output angular acceleration to 27.7 rads/s^2. max_force_ is limited to 25N.

The force and angular acceleration limits are back-calculated from the rotor performance properties that I found when I dug through the Hector quadrotor model source code (they specified a max torque / force per volt, I assumed 11.1V rotors, and worked it out from there...)

FWIW, here's the PID source code I've used (I've actually tried multiple implementations, to the same effect)... Note that I don't actually use the dx / dt parameters here because the update interval is constant:

-------------------------------------------------
double LearnerPid::update(double setPoint, double x, double dx, double dt)
{
//limit input to valid ranges
if (in_limit > 0.0 && fabs(setPoint) > in_limit)
setPoint = (setPoint < 0 ? -1.0 : 1.0) * in_limit;

double error = setPoint - x;
mIterm += (gain_i * error);

//limit integral to valid output ranges to avoid windup
if (out_limit > 0.0 && fabs(mIterm) > out_limit)
mIterm = (mIterm < 0 ? -1.0 : 1.0) * out_limit;

double dInput = (x - mLastInput);

output = gain_p * error + mIterm + gain_d * dInput;

//limit output to valid ranges
if (out_limit > 0.0 && fabs(output) > out_limit)
output = (output < 0 ? -1.0 : 1.0) * out_limit;

return output;
}

My apologies if my terminology or explanations are off... I'm not really a robotics guy and physics was never my best subject.

 

[Joel Graff]

This is what the system needs to do (links to an animated gif):
http://vignette4.wikia.nocookie.net...imation.gif/revision/latest?cb=20141124024926
http://vignette4.wikia.nocookie.net...imation.gif/revision/latest?cb=20141124024926
So I basically need to pulse the rotational PIDs at just the right time to start / stop rotation. Seems like this is more of an orbital mechanics problem. :)

Anyway, The problem is obvious to me, as is the general nature of the solution... but I'm a bit out of my depth on trying to figure out how to implement it.

 

[anorlunda]

It looks like fun. I was not able to understand all your code, but I do have some questions/comments.

How do you define roll, pitch, and yaw, as angles?

You calculate pitchcommand and rollcommand from the integral of velocities, but then you compare with with angular roll or pitch. That sounds like the wrong units. I don't see any sin or cos expressions to relate x,y,z with roll,pitch,yaw. Tell me how you define x,y,z,roll,pitch,yaw.

You calculate rollcommand with a PID acting on y velocity, but then you use it with another PID to set aa_x. Your pitch cals similarly cross x and y. Shouldn't x go with x and y with y?

The animated gif didn't help at all. I have no idea what it is trying to show relative to your problem.

 

[Joel Graff]

For the purposes of solving this problem, I guess it's best just to ignore the velocity PIDs. You'll note that I actually set pitchCommand to a fixed 0.392, regardless of what the X velocity PID is outputing, and the Y / roll pair is set to zero because all I want to do is test pitch control.

But to explain how that works (briefly), a drone can't achieve linear acceleration without inducing angular acceleration (it pitches to fly forward / backward or rolls to fly left / right). But since control is typically specified in linear velocities, we use the PIDs to convert. Ultimately, a linear velocity is converted to an angular acceleration, but to provide the ability to lock out at a specific pitch / roll angle, it's a two-step process. The first (outer) PID outputs a value that's scaled to the range of 0 - 0.392 (or at least is limited at 0.392), providing an "angle" for the second (inner) PID's setpoint.

The point, though is that a PID takes the difference between two values and produces an output that scales linearly with time. It's essentially unitless (technically, it's really outputing the time derivative of the input). But since we scale and limit the inputs / outputs, we can simply look at it as a time-sensitive, unitless controller.

I'm no PID expert, but this seems to be the way it gets used, anyway.

So the GIF is exactly the problem I'm trying to solve. Let me try to explain - I apologize if I'm being overly simplistic:

1. In my simplified model, X velocity is omitted. The pitch pid setpoint is simply locked to 0.392 radians. In other words, I want my pitch pid to rotate the drone until the drone's attitude is 0.392 radians pitch.

2. When it starts, the drone is perfectly level and the pitch PID sees a big difference between 0 and 0.392. It needs to output an angular acceleration (which maxes at 27.7 rad / s^2), so the PID scales the error to provide an output in the range of 0 - 27.7. To start, it's going to spit out a max value of 27.7.

3. Over time, the error the PID sees between the current angle and target drops, so it drops its output (the angular acceleration) as well.

4. As the pitch reaches the target angle (setpoint = 0.392), pid output = angular acceleration = zero.

5. But, of course, because of the conservation of angular momentum, the drone will continue to rotate. So now, in linear fashion, the pid has to apply reverse torque to counteract the angular momentum that's overdriving the rotation and try to bring that angle back to the 0.392 setpoint.

6. However, this "reversing" effort causes exactly the same problem as before, and generates an angular momentum that drives the rotation angle past the setpoint (this time, toward zero radians), and again, the PID has to reverse the torque to counteract.

The problem is that the drone never achieves equilibrium because the PID is constantly applying positive or negative output. That is, even though the PID output drops as the drone's rotation approaches the setpoint, it's still applying a positive angular acceleration, even if it is smaller. This smaller acceleration continues to generate extra angular momentum.

What I need is a way to apply torque to the system to generate a *small* amount of angular momentum that, at some point closer to the target value, I can reverse with a reverse torque, so the drone decelerates and stops exactly at the desired value.

The animated GIF demonstrates exactly what I mean - except that I'm not generating forces - just torques.

 

[anorlunda]

Thanks for the description. It does indeed look like fun. Probably over my head. But I may be able to offer some "old timers" control system advice that might be helpful.

Forget the P and D part of the PID controllers. Consider them just integral controllers. The key point is that the input to every integral must be zero in the steady state. Otherwise it won't be steady by definition.

Therefore, to be able to achieve a steady state, then you have a new equation for each PID, that it's error (set point minus feedback) must be zero. Examine your whole problem, if there is no solution where the errors of all the PIDs are zero at the same time, then there can be no steady state and things will oscillate. My advice is for you to write those equations and see if there is a steady state solution for the whole thing.