【开源】固定翼四轴通用的串级PID算法分享，非常稳定好调

声明：楼主是四轴飞行器的爱好者（废话，不喜欢会做这个！！？）以前学习过相关方面的知识，不过开始的时候没有太多资金，而且无刷电机很危险，所以先从小四轴开始玩起，经过一年的努力四轴终于可以比较稳定的飞行了（程序参考了匿名和蜂鸟四轴，感谢他们的无私奉献）

做的时候发很多人都在抱怨四轴参数难调，刚开始我也使用的是常见的pid控制，发现对参数确实十分敏感，很难达到稳定的效果，甚至直接使用陀螺仪的数据进行角速率反馈得到的效果都比它好。

通过查阅资料我发现四轴一般会被简化为一个低阻尼的二阶系统，角速率反馈恰恰可以增加它的阻尼（这个在二代的战斗机比如歼7中广泛采用，是改善阻尼的好方法）。于是我就是用了在固定翼飞机上常常采用的串级pid来调试，经过师兄理论上的指点，四轴已经可以比较稳定的飞行。

这个算法有固定翼的控制，和四轴控制原理相同。 首先使用角速率反馈作为内环，在用角度反馈作为外环，只要内环的参数调整合适，外环的参数从个位数变成六七十，四轴都可以稳定的飞行。

参数调节：

还有代码分享：

1.     void GET_EXPRAD(void)                        //¼ÆËãÆÚÍû½Ç¶È,²»¼Ó¿ØÖÆʱÆÚÍû½Ç¶ÈΪ0,0
   
2.     {
   
3.             EXP_ANGLE.X = (float)(-(Rc_Get.ROLL-1500)/30.0f);      
   
4.             EXP_ANGLE.Y = (float)(-(Rc_Get.PITCH-1500)/30.0f);
   
5.             EXP_ANGLE.Z = (float)(Rc_Get.YAW);
   
6.     //        printf("%f %fn",MPU6050_ACC_LAST.Y*cos(Q_ANGLE.X/57.3)-MPU6050_ACC_LAST.Z*sin(Q_ANGLE.X/57.3),MPU6050_ACC_LAST.X*cos(-Q_ANGLE.Y/57.3)-MPU6050_ACC_LAST.Z*sin(-Q_ANGLE.Y/57.3));
   
7.     //        DIF_ANGLE.X = (ACC_AVG.Y*cos(Q_ANGLE.X/57.3)-ACC_AVG.Z*sin(Q_ANGLE.X/57.3))/500;
   
8.     //        DIF_ANGLE.Y = (ACC_AVG.X*cos(-Q_ANGLE.Y/57.3)-ACC_AVG.Z*sin(-Q_ANGLE.Y/57.3)/500);
   
9.       DIF_ANGLE.X = EXP_ANGLE.X - Q_ANGLE.X;
   
10.       DIF_ANGLE.Y = EXP_ANGLE.Y - Q_ANGLE.Y;
    
11.     //        DIF_ANGLE.Z = EXP_ANGLE.Z - GYRO_I[0].Z;
    
12.     //        DIF_ANGLE.X = EXP_ANGLE.X - GYRO_I[0].X;
    
13.     //        DIF_ANGLE.Y = EXP_ANGLE.Y - GYRO_I[0].Y;
    
14.     //        DIF_ANGLE.Z = EXP_ANGLE.Z - GYRO_I[0].Z;
    
15.     }
    
16. 
    
17.     void CONTROL(void)
    
18.     {
    
19.             static float thr=0,rool=0,pitch=0,yaw=0;
    
20.             static float rool_i=0,pitch_i=0;
    
21.             static float rool_dif=0,pitch_dif=0;
    
22.             static float rool_speed_dif=0,pitch_speed_dif=0;
    
23.             float rool_out,pitch_out;
    
24.             uint16_t THROTTLE;
    
25. 
    
26.             GET_EXPRAD();
    
27.            
    
28.             rool         = PID_ROL.P * DIF_ANGLE.X;      
    
29.             rool_i += PID_ROL.I * DIF_ANGLE.X * 0.002;
    
30.             rool_i = between(rool_i,30,-30);
    
31.             rool += rool_i;
    
32.             rool += PID_ROL.D * (DIF_ANGLE.X-rool_dif) * 500;
    
33.             rool_dif = DIF_ANGLE.X;
    
34.       ///////////      
    
35.             pitch         = +PID_ROL.P * DIF_ANGLE.Y;      
    
36.             pitch_i += PID_ROL.I * DIF_ANGLE.Y * 0.002;
    
37.             pitch_i = between(pitch,30,-30);
    
38.             pitch += pitch_i;               
    
39.             pitch += PID_ROL.D * (DIF_ANGLE.Y-pitch_dif) * 500;
    
40.             pitch_dif = DIF_ANGLE.Y;
    
41.             ///////////
    
42.     //      
    
43.             rool -= GYRO_AVG.X* Gyro_G;
    
44.             rool_out=PID_PIT.P*rool;
    
45.             rool_out += PID_PIT.D*(rool- rool_speed_dif)*500;
    
46.             rool_speed_dif = rool;
    
47.            
    
48.             pitch -= GYRO_AVG.Y* Gyro_G;
    
49.             pitch_out=PID_PIT.P*pitch;
    
50.             pitch_out += PID_PIT.D*(pitch- pitch_speed_dif)*500;
    
51.             pitch_speed_dif=pitch;
    
52. 
    
53.     //        rool=PID_PIT.I*(rool-GYRO_AVG.X* Gyro_G);
    
54.     //        pitch=PID_PIT.I*(pitch-GYRO_AVG.Y* Gyro_G);
    
55.            
    
56.     //        PID_YAW.dout = 20 * (MPU6050_GYRO_LAST.Z* Gyro_G-(Rc_Get.YAW-1500)/10);
    
57.             PID_YAW.dout = 10 * (GYRO_AVG.Z* Gyro_G-(Rc_Get.YAW-1500)/10);
    
58. 
    
59.             PID_ROL.OUT = rool_out;
    
60.             PID_PIT.OUT = pitch_out;
    
61.             PID_YAW.OUT = PID_YAW.dout;
    
62.            
    
63.     /////////////
    
64.     //        GYRO_I[0].Z += EXP_ANGLE.Z/3000;
    
65.     //        yaw = -10 * GYRO_I[0].Z;
    
66.     //      
    
67.     //        yaw -= 3 * GYRO_F.Z;      
    
68.             THROTTLE=Rc_Get.THROTTLE;
    
69.             if(THROTTLE>1050)
    
70.             {
    
71.     //                if(THROTTLE>1950)
    
72.     //                {
    
73.     //                        THROTTLE=1950;
    
74.     //                }
    
75.                     THROTTLE = THROTTLE/cos(Q_ANGLE.X/57.3)/cos(Q_ANGLE.Y/57.3);      
    
76.                   
    
77.                     moto1 = THROTTLE - 1000 + (int16_t)PID_ROL.OUT - (int16_t)PID_PIT.OUT - (int16_t)PID_YAW.OUT;
    
78.                     moto2 = THROTTLE - 1000 + (int16_t)PID_ROL.OUT + (int16_t)PID_PIT.OUT + (int16_t)PID_YAW.OUT;
    
79.                     moto3 = THROTTLE - 1000 - (int16_t)PID_ROL.OUT + (int16_t)PID_PIT.OUT - (int16_t)PID_YAW.OUT;
    
80.                     moto4 = THROTTLE - 1000 - (int16_t)PID_ROL.OUT - (int16_t)PID_PIT.OUT + (int16_t)PID_YAW.OUT;
    
81.             }
    
82.             else
    
83.             {
    
84.                     moto1 = 0;
    
85.                     moto2 = 0;
    
86.                     moto3 = 0;
    
87.                     moto4 = 0;
    
88.             }
    
89.             if(Q_ANGLE.X>45||Q_ANGLE.Y>45||Q_ANGLE.X<-45||Q_ANGLE.Y<-45)
    
90.             {
    
91.                     ARMED=0;
    
92.                     LED3_OFF;
    
93.             }
    
94.     //        printf("moto=%d %d %d %dn",moto1,moto2,moto3,moto4);
    
95.             if(ARMED)        MOTO_PWMRFLASH(moto1,moto2,moto3,moto4);
    
96.             else                        MOTO_PWMRFLASH(0,0,0,0);
    
97.     }

复制代码

楼主太给力了   赞赞赞               

感谢楼主分享。我现在的串级PID参数还没有调整好，学习学习。

多谢分享。。学习

多谢分享。。学习

多谢分享。。学习

学习，多谢分享。。

楼主好棒····

   请问下，这种做法悬停效果好吗？能悬停多久？你这个有没有做360度翻滚功能？

谢谢 楼主分享 哈哈

谢谢楼主分享,谢谢!

感谢楼主分享。。。。。。

楼主好牛，谢谢分享

没有积分，行行好吧

32个赞，感谢楼主的分享。。。。希望楼主能在代码加些注释就好啦！

资料是很好 就是不知道自己能不能做

楼主好牛，谢谢分享

请问，和这个串级PID比较，差别大吗？

谢谢楼主无私分享~~

先收藏了再看。