Hi, I have been lurking for a while, but recently I got my quad up in the air on ardupirates, and just this week, I started getting successful flights with AC2. I am very impressed with the progress since early this year.
Now, what is the yoyo effect? when manually or automatically attempting to control the throttle of a multirotor, the craft will have a tendency to slowly oscillate in a fairly wide altitude range. That is, given a step throttle input, a quad will not go up to some height, instead it will overshoot its equilibrium height, and oscillate for a long time before settling (if it settles at all) so to hold a position, very careful operator input is required.
While many experienced pilots probably know how to hover at a specified altitude, the fact is, there is an inherent instability in the system. Here is my analysis and my proposal on how to deal with this issue.
Consider typical second order systems: A weight hanging from a spring in a frictional medium: pluck the weight and it will bob up and down. The oscillation will die out only as quickly as the frictional medium (air, honey, water?) is able to dissipate the energy.
I believe the process of attempting to hold altitude in a quad is a second order system: The quad has a mass (which will hold kinetic energy) but where is the spring that holds the potential energy? well, it turns out that the thrust your propellers are able to create is proportional to air pressure, and for all intents and purposes, we can say that thrust will vary linear over the range of oscillation: the higher the quad is, the lower the thrust. This efect is quite obvious closer to the ground, where the oscillations become more rapid, but regardless of altitude, thrust varies.
Now, when we model the system as if it was a spring, it is easy to see why it oscillates, but also, it is clear that the rate of oscillation will be rather slow, and DECEIVING, since an inexperienced pilot (or hypothetically an autopilot) would not quite realize what is happening and will produce throttle inputs that are always reactive, amplifying the problem.
Now, if the system had greater damping, these oscillations would die out quickly and it would be easier to maintain altitude, but we can't just make air "thicker" or put a sail on the quad to do this. We can produce a damping force by applying a differential term to thrust, in other words, create a little extra thrust in direct proportion to how fast the quad is moving and in opposition to it: When the quad is bobbing up, automatically create a little "downward" force by cutting thrust, and vice-versa.
There is a problem though: For this to work we need an accurate vertical speed to derive the term from. And neither of the altitude sensors (sonar or baro) has the responsiveness required to achieve a well behaved damping.
My approach (which I have tested for a number of weeks now) is to derive the differential throttle term from the accelerometer Z axis.
Here is the pseudo code:
th_dampK = 0.05;
th_dampP = -0.10;
leakRate = 0.1;
deltaZ = Z - lowPass_Z;
lowPass += Z * th_dampK;
speed_estimate += deltaZ;
speed_estimate *= 1 - leakRate; // leak 10 percent each iteration
throttle_damping = speed_estimate * th_dampP;
From physics class you will remember that the integral of acceleration is speed. So we can integrate the accelerometer output to obtain a value that is proportional to speed. There is a little problem with this method: The accelerometer will give a constant negative reading because there is gravity. If we were to merely integrate, we would come up with a speed that is always getting larger and larger, as if the quad was under free fall.
So that won't work without a small change: we high pass filter the accelerometer signal before integration, or use something called a "leaky" integrator. On every iteration the integrator looses a percentage of the total integral.
But will that work? The leaks have effectively highpassed the accelerometer's integral reading, We now only have "high passed" vertical velocity, or relative changes in velocity over the short term, but we never get an absolute velocity. Well, it turns out that this still works very well, because our goal here is not to read velocity but to create a "loss" in the yoyo effect. As long as there is yoyo effect to be dealt with, there is a short term velocity reading worth using.
There are a couple of last things to make this method safe: First, its range of influence has to be limited, so the damping term has to be constrained to some reasonable range, Otherwise, the throttle response will be adversely affected and the user will feel like they are fighting molasses on the way up or down; we want the pilot to feel like they are fighting molasses vertically when they are *trying* to hold the quad still (a good thing, because this "friction is helping hold still as well), but not when they are trying to climb quickly or drop quickly (not that you should be trying to drop quickly!, but that is another story)
throttle_damping = constrain(throttle_damping, -200, 100);
The end result is a throttle response that is much closer to "do what I mean" and not "do what I told you to" and I happen to believe that it doesn't only benefit manual control, but that automated means of throttle control *should* be wrapped around a "rate throttle controller" which is exactly what this method describes. We already do this for other axes of control, why not do it with throttle?
And the last thing: I have not tested this method at a tilted angle, nor do I think it is necessary for it to work at an angle.
So throttle_damping is scaled down gradually as the quad deviates from the horizontal plane. That way, rapid forward flight won't be affected. Remember the point of this is to facilitate keeping the quad still.
I haven't fully tested altitude hold in AC2 due to tuning, but I hope to get it working today. I am not necessarily proposing that this method be used in the altitude holding loop, my main objective with this method is to use it as manual throttle augmentation to smooth inputs, but I have a very strong feeling that the performance of altitude holding control methods is likely to drastically improve with this term added to the throttle. I will be testing this both ways once I have fully tested and tuned altitude holding as it is now.
I welcome feedback!
I think you'd have to apply something like the DCM for it to be useful for flight (not hovering), in order to caclulate it's Z acceleration in the world frame.
I find this interesting though, What advantages does this method have over using a PD compensator?
I wonder if you could combine this idea, and also add some configurable controls to limit or scale max vertical speeds when under a certain altitude. By doing so, you might create very precise vertical movement, create a "virtual airbag", and simultaneously improve automatic landing and takeoff routines.
Thanks Allen, I haven't given any thought to using this method to augment throttle in flight, but intuition tells me it is incomplete to be used other than in hover. I think it would perhaps take more than DCM, the problem gets more complex in flight because throttle now influences altitude as well as horizontal displacement speed.
As far as a PD controller, I believe this method is in fact the D part of a PD controller. the pilot brain is providing the P part though. If this control loop was used as an inner loop to proportional altitude control, and let the proportional term use an error derived from sonar / baro, but the D term synthesize speed from accelerometer, as described here, I think it would be really good.
I don't believe that sonar or baro are quick enough to give a good error signal to drive a D term just by themselves though.
I think this is entirely possible, I suppose one could add a throttle slew limit to prevent the craft from jerking from bad inputs below a certain altitude, then the integrated speed can be fused from sonar / baro altitude derived speed, and this speed could be averaged over time and be closely correlated to real vertical speed. Then the D term can be scaled non-linearly to produce a "terminal velocity" effect. I guess there would be details to work out under a simulator.
This is god timing. I have just started building this for AC2. I started a few months ago, but didn't get far. I do have a rotated Z value from the DCM so it works at angles.
I've taken a similar approach, but have not had success yet.
Hi Jason, please let me know how I can be helpful. At least in the limited case of a hover, this method I am proposing has worked for me. Hopefully in the next few days I will write it for AC2 (on my own copy) and I could shoot a video and demonstrate it.
I haven't fully thought about this at all angles, but I suppose that there is a need for some sort of throttle enhancement in forward flight as well, otherwise there wouldn't be an angle boost in the code, which seems like a simple way to maintain altitude during forward flight
I find that the angle boost results in an unexpected lift when I suddenly stop forward flight, and so far I am trying to learn to anticipate and control that, I imagine a better form of this algorithm adapted for all angles could serve as a replacement for, or addition to angle boost.
In other words, angle boost is probably required for steady state forward flight, but the transitional moments of accelrate / decelerate would resist sudden changes in altitude by use of this new method. I can see how that would be helpful.
I haven't really analyzed forward flight to know whether it behaves like a second order system. In theory it should still be second order, thrust is always higher at higher pressure, but there could be aerodynamic effects involving forward speed I haven't considered. Nevertheless, this method is simply a rate controller that counters changes in altitude within a narrow window. So, even if forward flight is not a second order system, countering throttle in proportion to the short term vertical velocity could only help control vertical velocity, and ultimately altitude.
Since a true Z axis acceleration is known, independent of orientation, the next part is: Does the proportional term need to scale with how far off horizontal the quad is? I think that it would have to right? thrust times the cosine of how far off horizontal we are (the angle between the Z axis and a vertical line) is the actual thrust in the vertical component, so the inverse of that number would have to be multiplied by the damping term for it to have the same effect as it had during hover. But then again, upward thrust of a propeller slicing sideways at 10 - 30mph can't simply be exactly the same as when it sits still can it? how much to scale the damping term during forward flight would be an area to experiment with.
For that matter, this technique could be applied to the x and y axis so that short term variations in horizontal placement (wind gusts) are automatically resisted by the quad with minimal user input. Simply apply the same math in those axes and actuate roll or pitch within a narrow window of influence (small enough that it is not obvious to the pilot, but large enough to be effective) seems like something desirable for AP.
Ah, I get it now. Instead of calculating the D term from the verticle velocity, you use the acceleration measurements directly. Clever!
And no, I don't think sonar nor baro readings would be of much use for Z-rate dampening... since you'd then have to do a double-derivative (ouch).
Actually, one correction to this: I convert acceleration to velocity by integration of the accelerometer, but also throw away the DC and use a leaky integral. That is what I take to be "short term velocity" because it is not suitable for a long term velocity estimate.
And yes, you are correct, any velocity estimate from other sensors would not be nearly as quick.
I did try to use acceleration directly for the feedback (counter thrust in proportion to acceleration) but it was not stable for me. I don't think such a system can be stable, but my control theory is not good enough to demonstrate this mathematically or to be 100% certain of this.
Basically the equilibrium point is the midpoint of the yoyo.
If thrust in proportion to acceleration is used you have two choices: accelerate more towards the middle upon sensed acceleration or accelerate more away from the middle.
Thrusting towards the middle by added thrust is simply an increase in oscillation frequency (and possibly amplitude), and thrusting outwards would either lower the oscillation frequency (but maintain the magnitude) or continue to accelerate past the oscillation range, unless the value was just so perfect to nail the boundary, which it won't in practice.
The use of a D term based on velocity is most natural and well understood way to dampen a second order system, since that is what would occur in nature (friction), so it is not too surprising that gave the better results. At least that was what I found at the time of testing.
Jason, I think the value we need would be obtained by getting the projection of the accelerometer vector (straight from the IMU) onto the vector that describes "this way is down" as interpreted from the DCM. In other words, vertical acceleration regardless of orientation. I am not entirely sure the _accel_vector in the DCM implementation represents exactly that. I also don't know of the top of my head how to get that projection using rotation matrices, but I am sure somebody here can work out the solution. Thoughts?
Nice work there Aurelio. I was also starting to work on a similar issue, namely automatic take-off and landing for an indoor setup quadrocopter with sonar + accelerometer. I like the idea of a leaky-integrator for short term velocity estimation.
I have two hints: firstly, yes, altitude hold is a second order system, but it's also highly non-linear. Meaning, the equilibrium point of the yoyo, as you put it, won't be the desired altitude. I just made a test for you. I set I and D terms to zero, and this is what I got (the quadro bounced off the ground in the lower part of the chart, I've been testing at home so I didn't want it to hit the ceiling while setting desired altitude too high).
And another thing: I guess to design a really great altitude controller one would have to add a time delay compensation loop of some sort in the end. I observed for my quad, there is about a 0,4s actuation delay between a throttle impulse and an altitude response. Motor response is almost immediate. I'll work on that now too, so keep us up on your achievements, please.
Have you maybe made a block diagram of the controller you made?
I researched the leaky-integrator a little further and this is what I found. I measured accelerometer velocity estimate for 3 different leak rates (0,1; 0,01; 0,001) and recorded them against sonar provided altitudes. Acc vertical velocities are depicted above, measured in m/s, altitudes shown below, expressed in cm.
leakRate = 0,1
leakRate = 0,01
leakRate = 0,001
For the leakRate = 0,1 one can clearly see there is still a really big amount of acceleration in the velocity estimate. If it were speed we were measuring, the blue line should cross 0m/s exactly where altitude starts rising / falling. So for me 0,1 is too little. leakRate of 0,001 seems to high, because there is too much of bias already in the velocity estimate (which would only get bigger if motors were on). So I'd go with 0,01 - there is also some acceleration visible in the velocity estimate, but it's acceptable for our needs already.
Don't mind the noise in altitude measurement - the movement of quadrotor was smooth, I held it in my hand with motors off.
I made another analysis, trying to avoid anything from causing sonar reading distortions (like keeping my sleeves out of the way) and, more importantly, adding sonar dirty-derivative to show the negative phase shift induced by high-pass filtering the integral (leaky-integral). It clearly shows how much better this approach should be as D-term in altitude control or for climb-rate control while taking-off or landing. One would only have to estimate the phase shift (should work as-is as well). See the picture below.
Are you there Aurelio? :)