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:
- Much more attitude stability
- Much faster response on disturbances or new setpoints
Con:
- You need to tune the control loop again after this change
- 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:
- Reload your last setup and level the copter.
- Set all parameters for stabilize STB_***_P and STB_***_I to zero.
- Set all parameters for rate control RATE_***_P, RATE_***_I and RATE_***_D to zero.
- 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.
- 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.
- Proceed with the P-Terms. Increase and test until it gets unstable. Then reduce it to 50..70 percent of the original value.
- 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.
- 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.
- Always leave the STABILIZE I-Term STB_***_I zero in this controller configuration!
- 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.
- 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
Comments
Go ahead and try it. There is nothing like learning by doing. That is how I started.
Having said that, the double loop system is common. There are a lot of different algorithms for determining attitude, but the Attitude ==> Rotation_Rate ==> ESC type of control loop scheme is often used even when it's not obvious. Off the top of my head, Arducopter, Openpilot, Multiwii, Aeroquad all use the dual loop system.
Yes, I totally got what you have done, thanks for the explanation. So, you think one-loop controller will also work? The reason I asked is that I already designed a controller and I want to implement it on PX4 flight controller. Additionally, I have seen many controllers in different articles designed by different control methods but I'd never seen this kind off controller that is used in Arducopter.
Regards,
Maziar
The short answer is that it could be done, but is a vastly inferior result to using a double loop system. Although it is counter intuitive, dividing the control loop into two pieces is both easier and provides better results than a single loop system.
Stability requires fast response to disturbances. In a flying machine, the gyro outputs in terms of of rotation per unit time (usually radian/s or deg/s). This output from the gyro is more resistant to noise than any other sensor on the machine. This makes it very easy, and fast to compare the expected rotational rate to the measured rotational rate as indicated by the gyro and adjust the ESC output to compensate. The stability generated by this control loop is all that is required to fly in Acro mode. This is often referred to as the inner loop.
The outer control loop looks at the expected orientation of the machine and compares this to the observed orientation estimate generated by some fancy algorithm and the input of multiple sensors (usually gyro, accelerometer and magnetometer). This control loop outputs rotational rates to the inner loop to try to correct for the errors it observes.
Out side of this loop may be a control system relating to navigating in the world to get to and between way points. This is not considered part of stability control.
I hope this helps a little.
Phillip
Hi everyone,
I have a fundamental question. Why did you divide the control process into two loops? Can't we just have one controller? The inputs would be desired Euler angles and the outputs would go to ESCs? Can everyone clarify this to me?
@Roy,
palo sestak has told me where these objects are defined. Many thanks to him. Thank you all well.
Actually in your original assumptation, what you described is a trajectory tracking problem, i.e., tracking a desired attitude with a speed of 10 deg/sec. But for multicopters' control, the robot usually works close to its operating point, i.e., it's a stabilization problem. PID isn't a model-based control technique, but IMO it's the most popular one. But of course for different MAV configurations, the control structure might be different.
Thanks,
Yangbo
@Yangbo,
Sorry, I am not familiar enough with the Ardu code to tell you where those functions live, but I should think a simple search would be able to find them.
@Igor (sorry so late with my reply)
My experience is with full-size vehicles, not with paprazzi nor arducopter specifically.
All,
In general, I do not favor the "PID" approach - it assumes you know very little about what you are trying to control (which is certainly understandable from the perspective of the typical developer on projects such as this one). In reality, you know that you're trying to control a multirotor, or a fixed wing plane, or a helicopter, etc. Therefore, the dynamics are more constrained and you don't have to resort to these "black box" techniques.
- Roy
Hi Roy,
I read the discussion and comments here. I also read the PID libraries. But I have difficulities to find these functions or objects:
wrap_180, constrain, ahrs.roll_sensor, roll_rate_d_filter.apply
Because I failed to set up Eclipse on my laptop, it's really difficult for me to find many variables, functions and objects. Can you show me where they are defined?
In my opinion, the P-PID control strcture is more like a cascaded design. Two controllers are responsible for two loops, inner angular rate loop and outer attitude loop. As you assumed, the "Desired Rate" is not a real desired rate, I think that's because essentially the outer loop P controller is not able to control the attitude loop without steady error. A similar strcture to this is the motor control, which includes a current loop, a speed loop and a position loop, each loop is controlled individually.
The control code in MK is like this:
varibles_roll.e = (Dst_eula.roll - Real_eula.roll - Speed.b)*parameter_roll.Kp;
varibles_roll.integral += varibles_roll.e*parameter_roll.Ki;
varibles_roll.differential=(varibles_roll.e-varibles_roll.last_e)*parameter_roll.Kd;
varibles_roll.last_e = varibles_roll.e;
varibles_roll.output = varibles_roll.e //* parameter_roll.Kp
+ varibles_roll.integral //* parameter_roll.Ki
+ varibles_roll.differential; //* parameter_roll.Kd;
somehow similar to the P-PID control structure, with the standalone kp=1.
Thank you in advance if anybody can let me know where the objects, wrap_180, constrain, ahrs.roll_sensor and roll_rate_d_filter.apply, are defined in the PID libraries. Thank you for your help.
Yangbo
@Robert: If I got it right, you say that the I-term of the rate PIDT1 will generate overshots on disturbance all the time. From practical experiments, the P-PIDT1 work much better than the P->PI ArduCopter used bevore. And you can tune the P-PIDT1 in a way that there is no overshot. Of cause you can tune nearly each contol loop also in a way that it generates overshot.
@Roy: Maybe the paparazzi structure for stabilize mode is better. We should try it! Do you fly paparazzi and arducopter? Can you compare it directly? Or have you tried this structure in ArduCopter?
@Thomas: Thanx, but I have not implemented it into 2.3.It is the same if you leave the dampening controller STAB_D zero. If you dont want to use the DT1, set all D parameters to zero and you get the same as AC 2.1.
The vibration issue is not solved with controller settings, because the vibrations go directly into the attitude estimation.
Fast responding ESCs are an improvement, because the whole system gets faster. No matter which control structure you use.
And about tuning of the parameters: I think a lot of users quite dont understand that this is really important. You can tune it to work insuficient, to work perfectly, or to overshot, oscillation and crash. And your parameters depend on many things like weight, diameter, akku, escs, motors, propellers... so, parameters that work for one airframe perfectly can crash another.
Oy, Igor!
The 2.3 code, with some sort of implementation of the PIDT1 seems to have brought us noticeable improvement on stabilize mode. Though many users reports difficulties to really use the roll D-term, due to oscillation and vibration sensitivity.
Just wondering, have you been involved in the implementation of PIDT1 into 2.3, and is does the implementation truly reflect the PIDT1 you had in mind?
About the sensivity of vibrations/ sampling/aliasing noise and oscillations - do you believe that a well tuned low-pass filter on the D-term input would make it less sensitive?
And would more responsive ESC:s increase the potential for D-term output to do it´s job better?
Tomas
As Robert Lefebvre and I said earlier, it is not a good idea to integrate the rate feedback term as you have it shown. If you want to see what a good control loop architecture looks like, try this:
http://paparazzi.enac.fr/wiki/Control_Loops#Reference_generators_2
As for angular acceleration or double-D feedback, yes the Stanford guys are using a better sensor package. But, Bill Premerlani is using similar sensors on his UAVDevBoard, and he's considering using this term as well:
http://www.diydrones.com/profiles/blog/show?id=705844%3ABlogPost%3A...
Bill does not report results yet that I have seen, but I hope this community would have learned by now to take his ideas seriously!
- Roy
-
1
-
2
-
3
-
4
-
5
of 6 Next