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

ti

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

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

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

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

参数调节：

还有代码分享：

void GET_EXPRAD(void) //¼ÆËãÆÚÍû½Ç¶È,²»¼Ó¿ØÖÆÊ±ÆÚÍû½Ç¶ÈÎª0,0
{
EXP_ANGLE.X = (float)(-(Rc_Get.ROLL-1500)/30.0f);
EXP_ANGLE.Y = (float)(-(Rc_Get.PITCH-1500)/30.0f);
EXP_ANGLE.Z = (float)(Rc_Get.YAW);
// 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));
// DIF_ANGLE.X = (ACC_AVG.Y*cos(Q_ANGLE.X/57.3)-ACC_AVG.Z*sin(Q_ANGLE.X/57.3))/500;
// DIF_ANGLE.Y = (ACC_AVG.X*cos(-Q_ANGLE.Y/57.3)-ACC_AVG.Z*sin(-Q_ANGLE.Y/57.3)/500);
DIF_ANGLE.X = EXP_ANGLE.X - Q_ANGLE.X;
DIF_ANGLE.Y = EXP_ANGLE.Y - Q_ANGLE.Y;
// DIF_ANGLE.Z = EXP_ANGLE.Z - GYRO_I[0].Z;
// DIF_ANGLE.X = EXP_ANGLE.X - GYRO_I[0].X;
// DIF_ANGLE.Y = EXP_ANGLE.Y - GYRO_I[0].Y;
// DIF_ANGLE.Z = EXP_ANGLE.Z - GYRO_I[0].Z;
}
void CONTROL(void)
{
static float thr=0,rool=0,pitch=0,yaw=0;
static float rool_i=0,pitch_i=0;
static float rool_dif=0,pitch_dif=0;
static float rool_speed_dif=0,pitch_speed_dif=0;
float rool_out,pitch_out;
uint16_t THROTTLE;
GET_EXPRAD();
rool = PID_ROL.P * DIF_ANGLE.X;
rool_i += PID_ROL.I * DIF_ANGLE.X * 0.002;
rool_i = between(rool_i,30,-30);
rool += rool_i;
rool += PID_ROL.D * (DIF_ANGLE.X-rool_dif) * 500;
rool_dif = DIF_ANGLE.X;
///////////
pitch = +PID_ROL.P * DIF_ANGLE.Y;
pitch_i += PID_ROL.I * DIF_ANGLE.Y * 0.002;
pitch_i = between(pitch,30,-30);
pitch += pitch_i;
pitch += PID_ROL.D * (DIF_ANGLE.Y-pitch_dif) * 500;
pitch_dif = DIF_ANGLE.Y;
///////////
//
rool -= GYRO_AVG.X* Gyro_G;
rool_out=PID_PIT.P*rool;
rool_out += PID_PIT.D*(rool- rool_speed_dif)*500;
rool_speed_dif = rool;
pitch -= GYRO_AVG.Y* Gyro_G;
pitch_out=PID_PIT.P*pitch;
pitch_out += PID_PIT.D*(pitch- pitch_speed_dif)*500;
pitch_speed_dif=pitch;
// rool=PID_PIT.I*(rool-GYRO_AVG.X* Gyro_G);
// pitch=PID_PIT.I*(pitch-GYRO_AVG.Y* Gyro_G);
// PID_YAW.dout = 20 * (MPU6050_GYRO_LAST.Z* Gyro_G-(Rc_Get.YAW-1500)/10);
PID_YAW.dout = 10 * (GYRO_AVG.Z* Gyro_G-(Rc_Get.YAW-1500)/10);
PID_ROL.OUT = rool_out;
PID_PIT.OUT = pitch_out;
PID_YAW.OUT = PID_YAW.dout;
/////////////
// GYRO_I[0].Z += EXP_ANGLE.Z/3000;
// yaw = -10 * GYRO_I[0].Z;
//
// yaw -= 3 * GYRO_F.Z;
THROTTLE=Rc_Get.THROTTLE;
if(THROTTLE>1050)
{
// if(THROTTLE>1950)
// {
// THROTTLE=1950;
// }
THROTTLE = THROTTLE/cos(Q_ANGLE.X/57.3)/cos(Q_ANGLE.Y/57.3);
moto1 = THROTTLE - 1000 + (int16_t)PID_ROL.OUT - (int16_t)PID_PIT.OUT - (int16_t)PID_YAW.OUT;
moto2 = THROTTLE - 1000 + (int16_t)PID_ROL.OUT + (int16_t)PID_PIT.OUT + (int16_t)PID_YAW.OUT;
moto3 = THROTTLE - 1000 - (int16_t)PID_ROL.OUT + (int16_t)PID_PIT.OUT - (int16_t)PID_YAW.OUT;
moto4 = THROTTLE - 1000 - (int16_t)PID_ROL.OUT - (int16_t)PID_PIT.OUT + (int16_t)PID_YAW.OUT;
}
else
{
moto1 = 0;
moto2 = 0;
moto3 = 0;
moto4 = 0;
}
if(Q_ANGLE.X>45||Q_ANGLE.Y>45||Q_ANGLE.X<-45||Q_ANGLE.Y<-45)
{
ARMED=0;
LED3_OFF;
}
// printf("moto=%d %d %d %dn",moto1,moto2,moto3,moto4);
if(ARMED) MOTO_PWMRFLASH(moto1,moto2,moto3,moto4);
else MOTO_PWMRFLASH(0,0,0,0);
}

复制代码

秦云鹤 · 2015-7-24 09:06:07

楼主陀螺仪传感器可以简化为什么样的系统呢？

刘威 · 2015-7-24 09:10:24

一直关注着楼主，目前也在做四周，底层写好了，只差pid调节了，学习了

嗷嗷~嗷呜~ · 2015-7-27 12:59:06

支持楼主。。。

正电荷的电子 · 2015-7-27 21:17:42

先留再看，感觉还不错的样子

lewis_y · 2015-8-6 18:22:13

感谢楼主的分享

白门学子 · 2015-8-10 19:19:09

正需要使用呢。。。。

mkx · 2015-9-20 15:17:49

学习一下~~，谢谢楼主

冯宇 · 2015-11-20 23:32:24

mark 顶楼主要是有注释就更好了

黄超 · 2016-2-2 12:15:37

学习，多谢分享。。

男神 · 2016-2-19 15:44:26

谢谢楼主无私分享~~先收藏；了

friend0720 · 2016-2-19 16:57:37

顶！！！！！！！！！！！！！！！！！！！！！！！！！！！！

雷豪 · 2016-2-21 21:40:54

很复杂啊，很复杂啊！

Benj · 2016-2-22 10:22:23

谢谢分享

浅时光ja · 2016-4-17 01:11:38

感谢楼主分享。。

lian494362816 · 2016-6-11 15:42:26

程序中的between 是什么函数啊，我查了一下C语言里没有

李林 · 2016-7-16 14:59:05

你这程序怎么感觉写的有问题呢？全程好多数据都被后面的覆盖了，确定没问题？

做一只牛 · 2016-7-18 22:35:18

赞，太有用了

钱生辉 · 2016-7-20 13:18:12

正在学习中

Benj · 2016-7-25 03:22:25

谢谢楼主分享

sjh_7366 · 2016-7-29 16:48:55

谢谢分享谢谢分享谢谢分享
   谢谢分享谢谢分享                      谢谢分享
   谢谢分享谢谢分享             谢谢分享
               谢谢分享             谢谢分享谢谢分享谢谢分享
         谢谢分享             谢谢分享       谢             谢
         谢谢分享             谢谢分享       谢             谢
         谢谢分享             谢谢分享       分             分
         谢谢分享             谢谢分享       享             享
         谢谢分享             谢谢分享       谢             谢
         谢谢分享             谢谢分享       谢             谢
谢       谢谢分享             谢谢分享    分             分
谢谢    谢谢分享             谢谢分享       享             享
谢谢分谢谢分享             谢谢分享       谢             谢
谢谢分谢谢分享                            谢    谢
谢谢分谢谢分享                      谢                谢
      谢谢谢谢                   分                            分
         谢谢                享                                  享

搜索历史

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

相关推荐

44 条评论

评论

精选推荐

站长推荐 /6