【开源】分享一个经典的串级PID算法，附源代码

一直想搞一下四轴飞行器，不过总是么有时间，前段时间在家休息，好好研究一下四轴飞行器的PID算法。现在凑个热闹，分享传说中的串级PID，内环为角速度环，外环为角速度。整定PID参数的时候先调外环，再调内环。

不多说，先上文件：

chuanPID.zip(1.75 KB)
(下载次数: 1533, 2015-6-5 10:50 上传)

下载积分: 积分 -1 分

想直接看的可以看看这个代码，都是一样的

1. #include 'CONTROL.h'
   
2. #include 'IMU1.h'      
   
3. #include 'moto.h'
   
4. #include 'RFdate.h'
   
5. #include
   
6. extern T_RC_Data                         Rc_D;                //遥控通道数据;
   
7. 
   
8. extern u8 txbuf[4];         //发送缓冲
   
9. extern u8 rxbuf[4];         //接收缓冲
   
10. extern u16 test1[3]; //接收到NRf24L01数据
    
11. extern S_INT16_XYZ ACC_F,GYRO_F;
    
12. 
    
13. PID PID_ROL,PID_PIT,PID_YAW;
    
14. 
    
15. extern S_INT16_XYZ        MPU6050_ACC_LAST,MPU6050_GYRO_LAST;      
    
16. 
    
17. 
    
18. int Motor_Ele=0;                                           //俯仰期望
    
19. int Motor_Ail=0;                                           //横滚期望
    
20. 
    
21. //u8 ARMED = 0;
    
22. 
    
23. //float rol_i=0,pit_i=0,yaw_p=0;
    
24. float thr=0;
    
25. 
    
26. S_FLOAT_XYZ EXP_ANGLE ,DIF_ANGLE;
    
27. PID1 PID_Motor;
    
28. /*********************************/
    
29. float Pitch_i,Roll_i,Yaw_i;                                   //积分项
    
30. float Pitch_old,Roll_old,Yaw_old;                 //角度保存
    
31. float Pitch_d,Roll_d,Yaw_d;          //微分项
    
32. float RC_Pitch,RC_Roll,RC_Yaw;                       //姿态角
    
33. float Pitch_shell_out,Roll_shell_out,Yaw_shell_out;//外环总输出
    
34.         //外环PID参数
    
35. float Pitch_shell_kp=280;//30 140
    
36. float Pitch_shell_kd=0;//
    
37. float Pitch_shell_ki=0;//
    
38. /*********************************/
    
39. float Roll_shell_kp=250;//30
    
40. float Roll_shell_kd=0;//10                 
    
41. float Roll_shell_ki=0;//0.08
    
42. /*********************************/
    
43. float Yaw_shell_kp=1.5;//10;//30
    
44. float Yaw_shell_kd=0;//10                 
    
45. float Yaw_shell_ki=0;//0.08;//0.08
    
46. float Gyro_radian_old_x,Gyro_radian_old_y,Gyro_radian_old_z;//陀螺仪保存
    
47. float pitch_core_kp_out,pitch_core_kd_out,Roll_core_kp_out,Roll_core_kd_out,Yaw_core_kp_out,Yaw_core_kd_out;//内环单项输出
    
48. float Pitch_core_out,Roll_core_out,Yaw_core_out;//内环总输出      
    
49.       
    
50. //内环PID参数
    
51. //float Pitch_core_kp=0.040;
    
52. //float Pitch_core_kd=0.008;////0.007;//0.07;
    
53. float Pitch_core_kp=0.040;
    
54. float Pitch_core_kd=0.002;////0.007;//0.07;
    
55. 
    
56. float Roll_core_kp=0.040;//;
    
57. float Roll_core_kd=0.002;////0.007;//06;//0.07;
    
58. 
    
59. float Yaw_core_kp=0.046;//;
    
60. float Yaw_core_kd=0.012;////0.007;//06;//0.07;
    
61. 
    
62. 
    
63. int16_t moto1=0,moto2=0,moto3=0,moto4=0;
    
64. 
    
65. float tempjd=0;
    
66. void CONTROL(float rol, float pit, float yaw)
    
67. {
    
68.       
    
69.          RC_Pitch=(Rc_D.PITCH-1500)/20;
    
70.       
    
71.         ////////////////////////外环角度环(PID)///////////////////////////////
    
72.   Pitch_i+=(Q_ANGLE.Pitch-RC_Pitch);
    
73. //-------------Pitch积分限幅----------------//
    
74.   if(Pitch_i>300) Pitch_i=300;
    
75.   else if(Pitch_i<-300) pitch_i="">
    
76. //-------------Pitch微分--------------------//
    
77.   Pitch_d=Q_ANGLE.Pitch-Pitch_old;
    
78. //-------------Pitch  PID-------------------//
    
79.   Pitch_shell_out = Pitch_shell_kp*(Q_ANGLE.Pitch-RC_Pitch) + Pitch_shell_ki*Pitch_i + Pitch_shell_kd*Pitch_d;
    
80. //角度保存
    
81.   Pitch_old=Q_ANGLE.Pitch;
    
82. /*********************************************************/      
    
83.       
    
84.         RC_Roll=(Rc_D.ROLL-1500)/20;
    
85.         Roll_i+=(Q_ANGLE.Rool-RC_Roll);
    
86. //-------------Roll积分限幅----------------//
    
87.   if(Roll_i>300) Roll_i=300;
    
88.   else if(Roll_i<-300) roll_i="">
    
89. //-------------Roll微分--------------------//
    
90.   Roll_d=Q_ANGLE.Rool-Roll_old;
    
91. //-------------Roll  PID-------------------//
    
92.   Roll_shell_out  = Roll_shell_kp*(Q_ANGLE.Rool-RC_Roll) + Roll_shell_ki*Roll_i + Roll_shell_kd*Roll_d;
    
93. //------------Roll角度保存------------------//
    
94.   Roll_old=Q_ANGLE.Rool;
    
95.       
    
96.       
    
97.         RC_Yaw=(Rc_D.YAW-1500)*10;
    
98. //-------------Yaw微分--------------------//
    
99.   Yaw_d=MPU6050_GYRO_LAST.Z-Yaw_old;
    
100. //-------------Roll  PID-------------------//
     
101.   Yaw_shell_out  = Yaw_shell_kp*(MPU6050_GYRO_LAST.Z-RC_Yaw) + Yaw_shell_ki*Yaw_i + Yaw_shell_kd*Yaw_d;
     
102. //------------Roll角度保存------------------//
     
103.   Yaw_old=MPU6050_GYRO_LAST.Z;
     
104.       
     
105.       
     
106.         ////////////////////////内环角速度环(PD)///////////////////////////////      
     
107.   pitch_core_kp_out = Pitch_core_kp * (Pitch_shell_out + MPU6050_GYRO_LAST.Y * 3.5);
     
108.   pitch_core_kd_out = Pitch_core_kd * (MPU6050_GYRO_LAST.Y   - Gyro_radian_old_y);
     
109. 
     
110.   Roll_core_kp_out  = Roll_core_kp  * (Roll_shell_out  + MPU6050_GYRO_LAST.X *3.5);
     
111.   Roll_core_kd_out  = Roll_core_kd  * (MPU6050_GYRO_LAST.X   - Gyro_radian_old_x);
     
112. 
     
113.   Yaw_core_kp_out  = Yaw_core_kp  * (Yaw_shell_out  + MPU6050_GYRO_LAST.Z * 1);
     
114.   Yaw_core_kd_out  = Yaw_core_kd  * (MPU6050_GYRO_LAST.Z   - Gyro_radian_old_z);
     
115.       
     
116.       
     
117.         Pitch_core_out = pitch_core_kp_out + pitch_core_kd_out;
     
118.   Roll_core_out  = Roll_core_kp_out  + Roll_core_kd_out;
     
119.   Yaw_core_out   = Yaw_core_kp_out   + Yaw_core_kd_out;
     
120. 
     
121.   Gyro_radian_old_y = MPU6050_GYRO_LAST.X;
     
122.   Gyro_radian_old_x = MPU6050_GYRO_LAST.Y;
     
123.   Gyro_radian_old_z = MPU6050_GYRO_LAST.Z;   //储存历史值
     
124.       
     
125. //--------------------将输出值融合到四个电机--------------------------------//
     
126. 
     
127.       
     
128.         if(Rc_D.THROTTLE>1020)
     
129.         {
     
130.   thr=Rc_D.THROTTLE- 1000;
     
131. 
     
132. //                if(Rc_D.THROTTLE<>
     
133. //                {
     
134. //  moto1=(int16_t)(thr  - Pitch_core_out);//- yaw);
     
135. //        moto2=(int16_t)(thr  - Pitch_core_out);//+ yaw);      
     
136. //        moto3=(int16_t)(thr  + Pitch_core_out);// - yaw);
     
137. //        moto4=(int16_t)(thr  + Pitch_core_out);//+ yaw);      
     
138.    
     
139. //  moto1=(int16_t)(thr  - Roll_core_out);//- yaw);
     
140. //        moto2=(int16_t)(thr  + Roll_core_out);//+ yaw);      
     
141. //        moto3=(int16_t)(thr  + Roll_core_out);// - yaw);
     
142. //        moto4=(int16_t)(thr  - Roll_core_out);//+ yaw);
     
143. 
     
144. //  moto1=(int16_t)(thr  - Yaw_core_out);//- yaw);
     
145. //        moto2=(int16_t)(thr  + Yaw_core_out);//+ yaw);      
     
146. //        moto3=(int16_t)(thr  - Yaw_core_out);// - yaw);
     
147. //        moto4=(int16_t)(thr  + Yaw_core_out);//+ yaw);                       
     
148.                        
     
149. //moto1=(int16_t)(thr - Roll_core_out - Pitch_core_out);
     
150. //moto2=(int16_t)(thr + Roll_core_out - Pitch_core_out);      
     
151. //moto3=(int16_t)(thr + Roll_core_out + Pitch_core_out);
     
152. //moto4=(int16_t)(thr - Roll_core_out + Pitch_core_out);      
     
153. //                       
     
154.   moto1=(int16_t)(thr - Roll_core_out - Pitch_core_out- Yaw_core_out);
     
155.         moto2=(int16_t)(thr + Roll_core_out - Pitch_core_out+ Yaw_core_out);      
     
156.         moto3=(int16_t)(thr + Roll_core_out + Pitch_core_out- Yaw_core_out);
     
157.         moto4=(int16_t)(thr - Roll_core_out + Pitch_core_out+ Yaw_core_out);                       
     
158.                        
     
159. //                }
     
160.   }
     
161.         else
     
162.         {
     
163.                 moto1 = 0;
     
164.                 moto2 = 0;
     
165.                 moto3 = 0;
     
166.                 moto4 = 0;
     
167.         }
     
168.         MOTO_PWMRFLASH(moto1,moto2,moto3,moto4);//        Moto_PwmRflash(moto1,moto2,moto3,moto4);
     
169. }