查看: 114|回复: 0

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

[复制链接]

主题

好友

5614

积分

状元

  • TA的每日心情

    2018-11-20 13:41
  • 签到天数: 3 天

    连续签到: 1 天

    [LV.2]偶尔看看I

    发表于 2020-7-27 11:08:28 |显示全部楼层
    声明:楼主是四轴飞行器的爱好者(废话,不喜欢会做这个!!?)以前学习过相关方面的知识,不过开始的时候没有太多资金,而且无刷电机很危险,所以先从小四轴开始玩起,经过一年的努力四轴终于可以比较稳定的飞行了(程序参考了匿名和蜂鸟四轴,感谢他们的无私奉献)

    做的时候发很多人都在抱怨四轴参数难调,刚开始我也使用的是常见的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 %f\n",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 %d\n",moto1,moto2,moto3,moto4);
    •             if(ARMED)        MOTO_PWMRFLASH(moto1,moto2,moto3,moto4);
    •             else                        MOTO_PWMRFLASH(0,0,0,0);
    •     }

    复制代码








    回复

    使用道具 举报

    您需要登录后才可以回帖 登录 | 立即注册

    本版积分规则

    关闭

    站长推荐上一条 /2 下一条



    手机版|电路城

    GMT+8, 2020-8-5 09:29 , Processed in 0.084982 second(s), 13 queries , MemCache On.

    ICP经营许可证 苏B2-20140176  苏ICP备14012660号-2   苏州灵动帧格网络科技有限公司 版权所有.

    苏公网安备 32059002001037号

    Powered by Discuz!

    返回顶部