Quadcopter stabilization algorithm

Hello everybody,

Im doing a quadcopter using Arduino, but I am stuck at the stabilization part.

I know that I have to something like this :

Pd - Pv * coef - which Pd is the desired angle (0 for me so) and Pv the actual angle. I got the Yaw and Pitch thanks to MPU6050 so I managed to get the actual angle. But now I dont know how to convert this result to a motor power value...

If you could help me with this !

Thank you

kakou19:
Im doing a quadcopter using Arduino, but I am stuck at the stabilization part.

Have you researched the control strategies used on other drones? I very much doubt that the simple proportional feedback algorithm you described will be sufficient.

I know that other strategies use PID algorithm, but I dont find well explanation or implementation.

I have a real simple solution I'm working on. Will post some uncompiled pseudocode here later today if you'd like. PID algorithms in the manufacturing process can be quite complex. I believe this problem is simpler to solve.

PID algorithms in the manufacturing process can be quite complex. I believe this problem is simpler to solve.

Please elaborate.

Around here (university engineering department that teaches controls) the consensus seems to be that PID is less effective than other control algorithms, but it is used because it is simpler (to implement and understand).

-j

I see where I have misled you. I am already using the KK board to control pitch and roll. It is only $12. KK v2.0 includes accelerometers to keep it level. I agree with your statement. I assert that P and I are all that is needed. Each is only a few lines of code combined with my existing assembly code in the KK board. The job of the KK board is to keep it stable in pitch, roll, and yaw. It does nothing for altitude. My code will keep the yaw from drifting using a magnetic compass. It will keep the altitude constant when you enable it. It will keep it in one position using GPS on top of the KK v2.0 leveling which does not take into account wind. In the end I may decide to modify the assembly code on KK, but for now I'm going to intercept the PWM between the receiver and KK, and modify it with a few short lines of C. One step at a time. Let's start with the altitude. Standby. I'm going to rip out the important part from my testing code. It's a work in progress.

kg4wsv:
Around here (university engineering department that teaches controls) the consensus seems to be that PID is less effective than other control algorithms, but it is used because it is simpler (to implement and understand).

PID can be applied to a wide variety of systems, and is conceptually simple. What 'other control algorithms' are you thinking of? The P, I and D elements each have a distinct purpose and are each simple to understand. P alone and P+I each represents subsets of the full PID which might work OK in specific situations, but if you omit the Integral term it may not converge on zero error, and if you omit the Derivative term it is liable to suffer from oscillations.

Thank everybody for your answer.

Can you explain exactly how PID works or post some pseudo code please. Im a bit confused...

No need for pseudocode - the PID library has the real thing and you can read about it here: Improving the Beginner’s PID – Introduction « Project Blog

I have searched a long time for this kind of tutorial ... thank you so much !

What 'other control algorithms' are you thinking of?

I think it was suggested sliding mode would be superior, but I've slept quite a few times since that discussion so I don't remember for sure.

-j

kakou19:
Can you explain exactly how PID works or post some pseudo code please. Im a bit confused...

If you really want to understand PID - I suggest that you take Udacity's CS373 class; in it, PID is described and implemented (in Python) in a way that makes it very clear how it works and why. This is only one small portion of the course, though; you can't just jump to that part and hope to understand it (unless you have a good grounding in the earlier parts of the course). Depending on your skill levels, the course may be considered from "easy" to "very difficult" (the latter being true if your understanding of linear algebra and probability/statistics is low to non-existant) - but in the end is well worth the struggle.

On a similar note:

Udacity recently announced that they have brought back the original AI course that Stanford ran last fall/winter (I am contemplating enrolling in it - I was in it last fall, and had to drop out halfway through due to personal issues - I am so glad they brought it back!)...

Here's how simple it can be using all 3 P,I,D:

loop
error = setpoint - measured_value
integral = integral + errordt
derivative = (error - previous_error)/dt
output = Kp
error + Kiintegral + Kdderivative
delay(dt)

While experimenting you can assign dt,gain to the value of R/C channel 5,6.
Let's start by controlling altitude only using the throttle channel, P term only:

void loop()
ft=get_altitude();
set_throttle((wasft-ft)*gain+wasthrottle); //gain is Kp
wasft=ft;
delay(dt);

A few more lines gives us I term:
const int NZEROSLOW=3;
const int NZEROSTOP=15;

ft=get_altitude();
chg=wasft-ft;
if last is in correct direction toward setpoint and j-lastj > NZEROSLOW then chg=0;
set_throttle(chg*gain+wasthrottle);
if chg then
last=chg;
lastj=j';
j++;
delay(dt);

It's not perfect, oversimplified a bit to make it easier to understand.
Another improvement in case it's moving too slow, not near the setpoint:

if ft-setpoint > 10ft and j-lastj > NZEROSTOP then chg= toward setpoint;

You can use the same method for pitch, roll, yaw using magnetic compass and GPS to correct towards setpoint.

Simple? Questions? Improvement? It works!

PS- Oh great library BTW! It's not needed for my solution here.

Can you tell me why it won't work properly before I make these changes?
Ideas for improvement?
Thanks for taking the time to read it.

sbright33:
Ideas for improvement?

How are you measuring altitude, and what sort of accuracy in the altitude hold are you trying to achieve?

I'm using the BMP085. Over a short time period it can detect changes of 1-2ft precision. I'd like to keep it within 10ft of the setpoint, where I activate it. Using the P term only loop, you have to sample often to keep it stable, between 1-5 samples/sec. At 2 s/sec it does not change very often when it's stable. That's why there are so many 0 values in chg. Instead of looking at the magnitude of chg, I have to count the number of 0's in a row for the I term. Am I making sense? Or am I in my own world?

Thank you everybody, my stabilization algorithm works well !

I'm from Missouri. Show me? I'd like to see how yours is different from mine. Since mine is so short, I'm sure you can offer me some improvements.

sbright33:
Ideas for improvement?

In the pseudo-code you seem to be controlling the timing by a fixed delay(dt) and then assuming that the sample interval is dt. Firstly I'd suggest using the actual loop interval in your calculation (either by explicitly controlling the loop interval, or by measuring it). Secondly, I'd have thought that reducing the sample interval would reduce the integration errors which suggests it might be desirable to run this control loop as fast as possible.

I appreciate your help! Let's talk about this. Without this interaction I'm too close to the project and will miss something. Talking about it out loud helps me think outside the box and become more creative. Since dt is about 500ms, the delay is about equal to the total loop time. In my code I don't use delay, I simplified it here to make it easier to read. You're right it is a fixed delay. During experiments I can vary dt by turning Channel 5 on my transmitter. Once I have chosen a value that works without oscillation, I can make it a constant which seems to work in all situations. I could measure the total interval it would be about 501ms. The way it is written without delay, I am actually doing what you said already. I can tell you are paying attention, because you noticed the problem in my simplified pseudocode.

I do not use dt anywhere in my calculations. So I'm confused about that comment. I don't think lowering dt would help. Since we're talking about the I term, let's look at the 3rd piece of code in my posting. Even using 200ms, chg is often 0. Because the sensor resolution is 1ft. There is no change for many seconds once it has stabilized. That's why I count the number of 0's instead of using the magnitude of chg for the I term. Simply: If it's moving slowly in the correct direction, don't chg throttle, until it arrives. I can tweek NZEROSLOW to keep it from oscillating. Maybe your comments apply to the first piece of code using all 3 PID. That's just an example showing what I'm NOT doing.