ArduCopter: Changing from PI to PID to improve stability

Hi,

I changed the attitude control loop a little bit to achieve more stability. Rate control with a PID(T1) instead of a PI controller. Here I present the necessary changes in the code to do this.

Pro:

  1. Much more attitude stability
  2. Much faster response on disturbances or new setpoints

Con:

  1. You need to tune the control loop again after this change
  2. This change is experimental, it works for me but you do it on your own risk ;)


I'm starting from Software AC 2.1. First I modified the library "PID" to save calculating time. I saved the following floating point operations per call to "get_pid": 2 x division, 4 x multiplication. You can download the modified library here. Unzip it and put the folder "PID_fast" in the "libraries" folder.

In file "ArduCopter.pde":

Add this line to the includes:

#include <PID_fast.h>            // PID library

For the following 4 blocks, I post it like the diff you get in GIT. The "-" means that you should look for this line and remove it. Replace it with the following lines starting with the "+". Of cause, the lines in the code have no "-" or "+". It's just to show what to remove and what to add instead.

-                g.pi_rate_roll.reset_I();
-                g.pi_rate_pitch.reset_I();
+                g.pid_rate_roll.reset_I();
+                g.pid_rate_pitch.reset_I();

-            g.pi_rate_roll.kP(tuning_value);
-            g.pi_rate_pitch.kP(tuning_value);
+            g.pid_rate_roll.kP(tuning_value);
+            g.pid_rate_pitch.kP(tuning_value);
 
-            g.pi_rate_roll.kI(tuning_value);
-            g.pi_rate_pitch.kI(tuning_value);
+            g.pid_rate_roll.kI(tuning_value);
+            g.pid_rate_pitch.kI(tuning_value);
 
-            g.pi_rate_yaw.kP(tuning_value);
+            g.pid_rate_yaw.kP(tuning_value);

In file "Attitude.pde":

In function "get_stabilize_roll(int32_t target_angle)":
 
-    rate         = g.pi_rate_roll.get_pi(error, G_Dt);
+    rate         = g.pid_rate_roll.get_pid(error, G_Dt);

In function "get_stabilize_pitch(int32_t target_angle)":

-    rate         = g.pi_rate_pitch.get_pi(error, G_Dt);
+    rate         = g.pid_rate_pitch.get_pid(error, G_Dt);

In function "get_stabilize_yaw(int32_t target_angle)":


-        rate     = g.pi_rate_yaw.get_pi(error, G_Dt);
+        rate     = g.pid_rate_yaw.get_pid(error, G_Dt);

-    rate         = g.pi_rate_yaw.get_pi(error, G_Dt);
+    rate         = g.pid_rate_yaw.get_pid(error, G_Dt);

In function "get_rate_pitch(int32_t target_rate)":
 
-    target_rate = g.pi_rate_yaw.get_pi(error, G_Dt);
+    target_rate = g.pid_rate_yaw.get_pid(error, G_Dt);

In file "Parameters.h":

Increase the number after "static const uint16_t k_format_version" by one. Warning: This will make the APM erase your EEPROM and Log after flashing the new compiled firmware. So save your settings first! After this, you have to reload your settings and level the copter again.

Later on in the file:

-    k_param_pi_rate_roll = 235,
-    k_param_pi_rate_pitch,
-    k_param_pi_rate_yaw,
+    k_param_pid_rate_roll = 235,
+    k_param_pid_rate_pitch,
+    k_param_pid_rate_yaw,

-    APM_PI        pi_rate_roll;
-    APM_PI        pi_rate_pitch;
-    APM_PI        pi_rate_yaw;
+    PID_fast    pid_rate_roll;
+    PID_fast    pid_rate_pitch;
+    PID_fast    pid_rate_yaw;

-    pi_rate_roll        (k_param_pi_rate_roll,            PSTR("RATE_RLL_"),    RATE_ROLL_P,        RATE_ROLL_I,        RATE_ROLL_IMAX * 100),
-    pi_rate_pitch        (k_param_pi_rate_pitch,            PSTR("RATE_PIT_"),    RATE_PITCH_P,        RATE_PITCH_I,        RATE_PITCH_IMAX * 100),
-    pi_rate_yaw            (k_param_pi_rate_yaw,            PSTR("RATE_YAW_"),    RATE_YAW_P,            RATE_YAW_I,            RATE_YAW_IMAX * 100),
-
+    pid_rate_roll        (k_param_pid_rate_roll,            PSTR("RATE_RLL_"),    RATE_ROLL_P,        RATE_ROLL_I,        RATE_ROLL_D,        RATE_ROLL_IMAX * 100),
+    pid_rate_pitch        (k_param_pid_rate_pitch,        PSTR("RATE_PIT_"),    RATE_PITCH_P,        RATE_PITCH_I,        RATE_PITCH_D,        RATE_PITCH_IMAX * 100),
+    pid_rate_yaw        (k_param_pid_rate_yaw,            PSTR("RATE_YAW_"),    RATE_YAW_P,            RATE_YAW_I,            RATE_YAW_D,            RATE_YAW_IMAX * 100),

In file "config.h":

You have to add a few lines. Lines to add are indicated by "+". Other lines are for reference to find the right place.

 #ifndef RATE_ROLL_I
 # define RATE_ROLL_I         0.0
 #endif
+#ifndef RATE_ROLL_D
+# define RATE_ROLL_D         0.0
+#endif

 #ifndef RATE_PITCH_I
 # define RATE_PITCH_I        0 //0.18
 #endif
+#ifndef RATE_PITCH_D
+# define RATE_PITCH_D         0.0
+#endif

 #ifndef RATE_YAW_I
 # define RATE_YAW_I     0.0
 #endif
+#ifndef RATE_YAW_D
+# define RATE_YAW_D     0.0
+#endif

In file "system.pde":

-    Log_Write_Data(12, g.pi_rate_roll.kP());
-    Log_Write_Data(13, g.pi_rate_pitch.kP());
+    Log_Write_Data(12, g.pid_rate_roll.kP());
+    Log_Write_Data(13, g.pid_rate_pitch.kP());

Now, all changes are done. Now save your current settings of the APM, compile and load the code... You will find 3 new parameters (RATE_RLL_D, RATE_PIT_D and RATE_YAW_D) you can edit like any other mavlink parameter. These are the D-Terms for all rate control loops.


Testing and tuning:

Be really carefull, propellers are dangerous!  I do the tuning of the control loops while I hold the copter in my hand and try if it compensates the disturbances without overshot or oscillation. You should decide for yourself if cou can hold your copter in one hand while you throttle up and test the stability. Even small propellers can harm your fingers! If you want to proceed, I do it like this:

  1. Reload your last setup and level the copter.
  2. Set all parameters for stabilize STB_***_P and STB_***_I to zero.
  3. Set all parameters for rate control RATE_***_P, RATE_***_I and RATE_***_D to zero.
  4. Hold the copter with one hand, throttle up until it becomes weightless, then turn it. Nothing should happen, because all control loop parameters are zero. You have just manual throttle control.
  5. Increase the D-Terms and test again while holding the copter in your hand. Remember, it can't fly with most parameters set to zero. Increase the D-Term and test again, until you got some oscillation. Let's call the value you found 100 percent. Now reduce it to 50..70 percent. Do this for ROLL and PITCH.
  6. Proceed with the P-Terms. Increase and test until it gets unstable. Then reduce it to 50..70 percent of the original value.
  7. Set the I-Terms to 30..50 percent of the P-Terms. Test again. If it is unstable, reduce P and I and maybe D a little bit.
  8. Now go to the STABILIZE control parameters. Increase the P-Term. Now the copter should return to level position automatically. Increasing of the P-Term will lead to a faster return to level position after disturbance or also to a fast respond to the desired angle by pilot input. Find a value where the copter returns fast from a disturbance without overshot or oscillation.
  9. Always leave the STABILIZE I-Term STB_***_I zero in this controller configuration!
  10. For YAW you can leave the D-Term zero and use your previous settings for STABILIZE and RATE control. Or you tune it, but I didn't try it at YAW yet.
  11. If everything works well and the copter returns to level position after disturbance or pilot input without overshot or oscillation, then you can try to hover and fly carefully. Not before!

Here are some controller parameters from my Tricopter (80 cm from motor to motor, 1440 g in flight weight), the D-Terms are quite small:

STB_PIT_I,0
STB_PIT_IMAX,1500
STB_PIT_P,9
STB_RLL_I,0
STB_RLL_IMAX,1500
STB_RLL_P,9
STB_YAW_I,0
STB_YAW_IMAX,1500
STB_YAW_P,4

RATE_PIT_D,0.015
RATE_PIT_I,0.02
RATE_PIT_IMAX,1000
RATE_PIT_P,0.09
RATE_RLL_D,0.012
RATE_RLL_I,0.02
RATE_RLL_IMAX,1000
RATE_RLL_P,0.09
RATE_YAW_D,0.004
RATE_YAW_I,0.002
RATE_YAW_IMAX,500
RATE_YAW_P,0.5

It would be really nice, if other people try this modification. For me, it was a significant increase in stability.

Regards, Igor

Views: 4305

Tags: PI, PID, arducopter, attitude, control loops, stabilize, tricopter

Comment by Tomas Soedergren on January 25, 2012 at 5:49am

Guys, I am so glad this conversation exists. It brings me hope.

While not having profound experience of all the FW versions, every time I have been flying, tweaking and testing STAB and ACRO I have been left with "ok, this might work but really it MUST be possible to make it better". A gut feeling that something is missing here. One could argue that "hey, you just have to be more rigorous with you PI-tuning, and it will be fine", but too many fellow reports indicate that there IS really a missing link between the AC firmware and that perfect locked-in-flight experience. And if we cannot get the ACRO mode perfect (well, the inner control loop), then we cannot expect to get the higher level functions perfect either, since they always depend on the basic inner loop. The higher functions will have to tolerate and compensate for the not fully performing inner loop, instead of just doing their job.

Even if it was many years ago, I have studied control theory, and after having penetrated Igor´s arguments I have to say that I concur. Implementing PIDT1 and cleaning up the loop arcitechture seems like a necessity to move on. Flight tests performed by Marco indicates likewise.

Seems to be some confusion on the "D-term" concept in this conversation. From my observers seat it looks like this confusion is due to jason referring to the attitude, while Igor refers to the basic, inner rate loop (rate). (Correct me if  I am wrong. ) So when Igor talk D-term he refers to angular acceleration, while Jason refers to angular speed (rate). If true, this explanins why Jason, a bit puzzled, insists that there IS already a D-term in action. Yes, for the attitude, but not for the angular rate.

To be continued...

Comment by Tomas Soedergren on January 25, 2012 at 9:30am

Continuing…

And I do share Igor´s conviction that the purpose of the outer ”extra” I-term, implemented to counteract outer disturbance like offset CG, could (and should) be handled by the inner (rate) loop I-factor, commanded by the attitude (Stable) P-term. Think of it. If we hang a battery pack on one of the copter arms:

1. The arm will initially dip, and simultaneously there will be an output from the attitude (Stable) P-term.

2. The attitude P-term output is immediately fed into the inner loop, the rate loop, which is a PIDT1. As long as there is an input (attitude error) it WILL be integrated by the rate loop I-term, which at some point (preferably close to instantly) will build up enough to force the arm up to level. And that is when the attitude P-term returns to zero, while the rate integrator remembers the value needed to correct for that stupid battery dangling on one arm.

3. If somebody disagrees, please specify exactly what was wrong in 1 or 2. Thanks.

So I firmly believe that ACRO (or the inner rate loop) has a clear purpose:

  • When stick input is zero, copter keeps present attitude. Quickly recovering from any outside disturbance.
  • When stick input is applied, copter (pitch/roll) angular velocity will be proportional to stick input. No more, no less.
  • Period.

I just came in from two brief test / tuning flights of my stock 3DR APM2 quad, AC 2.2.v4. And the ACRO mode is not capable of the purpose above. It is flyable, but attitude wanders without command. Copter obeys rate inputs a bit sloppy with slight overshoots. If somebody tells me that the ACRO control loop is already perfect, well then then the gyro references is not working properly. Something is just half baked. If somebody disagrees, PLEASE provide detailed tuning instructions on the two tunable terms:  ACRO Pitch and Roll Proportional and Integrating.

Stable mode works decently, but has artifacts. Like for example: I command tilt forward, the copter gains speed and flies forward. Then I ease off on the stick to neutral, thereby ordering horizontal level. Instead, the copter starts leaning slightly backwards (uncommanded) to me, starting backwards flight towards me. After a moment it has once again returned to level. It looks like a slow attitude oscillation, probably a result of the double I-terms existing in the control diagram. I do not understand how to get rid of that artifact with the tools at hand.

When writing this, I understand the PIDT1 is already a bit on the road to be included in next release.

I really look forward to that, with great expectations!

Comment by Igor van Airde on January 25, 2012 at 9:36am

The inner PIDT1 loop in my implementation controls the angular rate/velocity, not the angular acceleration. (Or do you mean the D term reacts on angular acceleration? Thats right.) The outer loop controls the angle with a simple P control.

And in my opinion outer loop with P and inner loop with PIDT1 is the best thing that you can do (with traditional controllers). You always have to choose the right controller structure. Putting PIDT1 everywhere is not a solution, because two I Terms in series generates oscillation.

Can someone with a strong background in control theory join in? I had control theory one year at university and have some hobby experiences with controllers. And I tried combinations of outer and inner loop for attitude stabilisation, and P->PIDT1 works the best.

Comment by Tomas Soedergren on January 25, 2012 at 10:12am

Yes Igor, I meant that the D term reacts on angular acceleration. But I think this has not been entirely clear for everybody in this thread. Thus the occasional confusion present in some posts on this topic...

Comment by robert bouwens on January 25, 2012 at 10:13am

nice!

nothing more to say.

Comment by Igor van Airde on January 25, 2012 at 10:18am

Hi Tomas, my comment above is to your first comment.

Now to your second: As I understand, there are different controllers used for acro mode than for the rate loop in the stabilize mode. In acro mode you use just a PI controller for rate. Its another controller than the rate controller in stabilize mode. In my opinion a double implementation that leads to confusion. But my PIDT1 mod has nothing changed in the acro mode.

About your assumption about acro mode "When stick input is zero, copter keeps present attitude. Quickly recovering from any outside disturbance":  No, it will quickly recover to the desired rate (angular velocity, e.g. zero on stick middle position) but not to an attitude. Best compraison of the acro flight behaviour is a traditional helicopter. You could implement the behaviour you described, but its not like this at the moment.

You can change the acro mode also from PI to PIDT1. Should bring more stability of cause. But the clean way would be to use the same control loop for acro and the inner rate loop in stab mode. It would also improve the ability to tune, because you could tune your PIDT1 acro loop until it is perfect. Then tuning the P term of stab mode is easy. Now you basically tuning 2 loops (or 4 parameters) at the same time.

About your in flight problem I dont know. The I term of your outer loop should be zero.

Comment by Tomas Soedergren on January 25, 2012 at 10:28am

It has been suggested, since the ESC response is a part of the overall system time constant, that control performance would benefit from ESC´s not having digital low-pass filters on their inputs (as they currently do). By this reason there are bold do-it-yourselfers flashing that filter away from the ESC firmware (there are forum threads about it).

Do you think that the standard ESC low-pass input filtering affects overall control performance significantly?

I ask because Castle Creations is doing alpha tests of a firmware that accepts high frame rate and enables instant output response on input signal, without the low-pass filter.
I was thinking, maybe some of our bold test pilots here on DIY would like to be push the limit by tweaking the PIDT1 in combination with unrestricted ESC response, to find out how far it is relevant to go in overall response speed. CC might even sponsor test pilots with some ESC:s... Not a promise, but a possibility :)

Anybody volunteer for that? ;-)

Comment by Tomas Soedergren on January 25, 2012 at 10:56am

Speaking of control structure,

I recall Robert Lefebvre some time ago proposed the concept of Gain scheduling control loops.

Is this concept in the ArduCopter developers tool box yet? If not, maybe it should be considered?

Situations I can think of where scheduling might be relevant are:

  • Steep descends into copters downwash.
  • Auto-land & auto take off, close to ground.
  • Altitude hold functions, differentiating gain on sonar / baro sensing guidance.
  • payload adaptive altitude regulation and rate response
  • future self-tuning algorithms
Comment by Igor van Airde on January 25, 2012 at 10:59am

I dont have the standard ESCs. But if there is a low pass filter, it will affect the over all performance significantly!

As I tried the original P->PI controller structure, even a heavy propeller instead of a lighter one could do the difference between stable and unstable. I started the PIDT1 change, because I need bigger propellers to get the thrust from the slow running motors. And with the original controller I didnt get it stable.

So you have to retune the control loops if you change battery, ESCs, motors, propellers, total weight, moment of inertia, ...

And about ESCs: They should be fast but they need a overcurrent protection. And you have to check if the ESC will not stop the motor due to fast change on input values BEVORE flying!

And about fast changing values: The control loop runs at 200 Hz. The output to the ESCs runs at 400 Hz. As I understand the code, they are not synchronized. Increasing the ESC update rate is not necessary, because the microcontroller cant calculate much faster than 200Hz. There is much more basic work to do until we have a sufficiant copter control.

Comment by Igor van Airde on January 25, 2012 at 11:08am

About Gain scheduling :  I think in ArduPlane is something that the controls depend on the airspeed. On a copter my concept was always to hand tune a easy but sufficiant control structure. You can make things better with gain scheduling or adaptive controls, but you can also make things worse. The more complex, the more possibilities to make mistakes. And my opinion is, that we really have to clean up the controller structure on ArduCopter first, bevore we can think of more. And keep in mind, that the calculating power is limited with the current µC.

Comment

You need to be a member of DIY Drones to add comments!

Join DIY Drones

Social Networking

Contests

Season Two of the Trust Time Trial (T3) Contest has now begun. The fourth round is an accuracy round for multicopters, which requires contestants to fly a cube. The deadline is April 14th.

A list of all T3 contests is here

Groups

Advertisement

© 2013   Created by Chris Anderson.   Powered by

Badges  |  Report an Issue  |  Terms of Service