21ic电子技术开发论坛 › 非技术讨论区 › 活动专区 › 【多旋翼飞控】串级PID控制，飞行结贴
返回列表 发新帖我要提问本帖赏金: 10.00元(功能说明)

[活动] 【多旋翼飞控】串级PID控制，飞行结贴

[复制链接]
5318|16
 楼主| 冷不丁 发表于 2016-2-18 18:23 | 显示全部楼层 |阅读模式
PID控制, se, TE, IO, ST
本帖最后由 冷不丁 于 2017-5-11 19:37 编辑

经过接近两个月的努力（折腾了一个寒假QAQ），终于成功使用NXP K64实现了多旋翼飞控的制作，和程序编写。
接下来，我将进行控制算法部分的调参讲解。

飞行器的姿态控制算法使用的是串级PID，即角速度作为内环，角度为外环，两层PID级联的控制方式。
关于串级PID的一些比较好的资料，我以文档的方式进行分享。

我主要对于，串级PID参数整定分享一些经验。
以下是我使用到的串级PID整定策略：
单极PID适合线性系统，当输出量和被控制量呈线性关系时单极PID能获得较好的效果，但是四轴不是线性系统，现代学者认为，四轴通常可以简化为一个二阶阻尼系统。为什么四轴不是线性系统呢？首先，输出的电压和电机转速不是呈正比的，其次，螺旋桨转速和升力是平方倍关系，故单极PID在四轴上很难取得很好效果，能飞，但是不好飞。
为了解决这个问题，我们提出了串级PID这个解决方法。
串级PID就是两个PID串在一起，分为内环和外环PID。在此，我们使用内环PID控制，外环PI控制。
单极PID输入的是期望角度，反馈的是角度数据，串级PID中外环输入反馈的也是角度数据，内环输入反馈的便是角速度数据。通俗来讲，内环就是你希望将四轴以多少度每秒的速度运动，然后他给你纠正过来，外环就是根据角度偏差告诉内环你该以多少度一秒运动。这样，即使外环数据剧烈变化，四轴的效果也不会显得很僵硬。
在内环中，PID三个数据作用分别是：P（将四轴从偏差角速度纠正回期望角速度）D（抑制系统运动）I（消除角速度控制静差）
外环PI中，两个数据的作用是：P（将四轴从偏差角度纠正回期望角度）I（消除角度控制静差）
整定方法：
1，将内外环PID都归0，适当增加内环的P，调整P至四轴从正面朝上自然转动到正面朝下时能感受到阻力，且没有抖动，有抖动就应减小P，当P减小到无抖动或者轻微抖动时即可。
2，让内环的D慢慢增加，到你用手能明显感受到转动四轴产生排斥外力的阻力即可，D能抑制P产生的振荡，但是D过大也会导致高频振荡，调整D至系统无振荡且能抑制外界的力即可。
3，给内环一点点I，注意的是I的积分要在油门开启后才开始，油门关闭就清0，且必须有积分限幅。I推荐取越小越好，我取的是0.01，I取大了会导致系统振荡。
4，将内环P减半，将外环P调至内环的50-70倍，根据系统产生的高频振荡降低内环的D，直至高频振荡消除即可。
5，给外环一点点I，同3.
6，根据实际情况对参数进行优化调整，调整过程中要注意区分各个参数的作用，时刻记住，P是回复力，大了会低频振荡，D是抑制力，大了会高频振荡，I是静差消除力，越小越好，大了会产生振荡。
高度控制使用PD算法。
位置控制使用串级PID算法。贴出我的姿态控制，进行了部分注释，输入目前姿态和目标姿态，输出PWM直接控制电机。
复制
  1. void CONTROL(float rol_now, float pit_now, float yaw_now, float rol_tar, float pit_tar, float yaw_tar)
    

  2. {
    


  3. 

  4.         uint16_t moto1=0,moto2=0,moto3=0,moto4=0;
    

  5.         S_INT16_XYZ ACC_Angle_Offset;
    

  6.         
    

  7.         /*____________________________________________assignment argument______________________________________________________*/
    

  8.         float error_rol = (-rol_tar) - (-rol_now);
    

  9.         float error_pit = (-pit_tar) - (-pit_now);
    

  10.         float error_yaw = (-yaw_tar) - (-yaw_now);
    

  11.         float g_x = MPU6050_GYRO_NOW.Y*0.0610351;
    

  12.         float g_y = MPU6050_GYRO_NOW.X*0.0610351;
    

  13.         float g_z = MPU6050_GYRO_NOW.Z*0.0610351;
    

  14.         
    

  15.         round_error_rol=error_rol;
    

  16.         round_error_pit=error_pit;
    

  17.         /*__________________________________________==assignment argumen==_____________________________________________________*/
    


  18. 

  19.         /*__________________________________________________zero set___________________________________________________________*/
    

  20.         if((Temp_Throttle<180) && ((MPU6050_GYRO_LAST.Z > 1) || (MPU6050_GYRO_LAST.Z < -1)))//
    

  21.         {
    

  22.                 Gyro_Z_Offset = Gyro_Z_Offset + MPU6050_GYRO_LAST.Z;
    

  23.                 GYRO_I.Z = 0;
    

  24.         }
    

  25.         if((Temp_Throttle<180) && ((MPU6050_GYRO_LAST.X > 1) || (MPU6050_GYRO_LAST.X < -1)))//
    

  26.         {
    

  27.                 Gyro_X_Offset = Gyro_X_Offset + MPU6050_GYRO_LAST.X;
    

  28.                 GYRO_I.X = 0;
    

  29.         }
    

  30.         if((Temp_Throttle<180) && ((MPU6050_GYRO_LAST.Y > 1) || (MPU6050_GYRO_LAST.Y < -1)))//
    

  31.         {
    

  32.                 Gyro_Y_Offset = Gyro_Y_Offset + MPU6050_GYRO_LAST.Y;
    

  33.                 GYRO_I.Y = 0;
    

  34.         }
    

  35.         if(Temp_Throttle<180)//加速度计零偏值校验
    

  36.         {
    

  37.                 Dataanl_flag = 0;
    

  38.                 Accel_X_Offset = ACC_AVG.X;
    

  39.                 Accel_Y_Offset = ACC_AVG.Y;
    

  40.                 Accel_Z_Offset = ACC_AVG.Z - 8192;
    

  41.         }
    

  42.         else
    

  43.         {
    

  44.                         Dataanl_flag = 1;
    

  45.         }
    

  46.         /*________________________________________________==zero set==_________________________________________________________*/
    

  47.         
    

  48. //        PID_ROL.IMAX = 600;        
    

  49. //        PID_PIT.IMAX = PID_ROL.IMAX;
    

  50.   /*_____________________________________________________PID_____________________________________________________________*/        
    

  51.         /*_________________________________PID_X_OU_________________________________________*/                
    

  52.         PID_PIT.OU_I_integral = PID_PIT.OU_I_integral + error_pit;//外环角度误差积分
    

  53.         if(PID_PIT.OU_I_integral >1000)//积分限幅：100需调整
    

  54.                 PID_PIT.OU_I_integral = 1000;        
    

  55.         else if(PID_PIT.OU_I_integral < -1000)
    

  56.                 PID_PIT.OU_I_integral = -1000;
    

  57.         PID_PIT.OU_OUT = error_pit*PID_PIT.OU_P + PID_PIT.OU_I_integral*PID_PIT.OU_I;//角度环PI计算
    

  58.         /*_______________________________==PID_X_OU==_______________________________________*/        
    

  59.         /*_________________________________PID_X_IN_________________________________________*/
    


  60. 

  61. //        Last_Gyro_y = gy;//保存本次陀螺仪数据，下次使用        
    

  62.         PID_PIT.IN_I_integral = PID_PIT.IN_I_integral+(PID_PIT.OU_OUT - (-g_y));
    

  63.         if(PID_PIT.IN_I_integral>500)// 积分限幅
    

  64.                 PID_PIT.IN_I_integral = 500;
    

  65.         else if(PID_PIT.IN_I_integral < -500)
    

  66.                 PID_PIT.IN_I_integral = -500;
    

  67.         if(LOCK_THR==1)
    

  68.                 PID_PIT.IN_I_integral = 0;
    

  69.         /*____________________________________bu_wan_quan_wen_fen_______________________________________________________________*/
    

  70.         PID_PIT.in_dout = PID_PIT.NOW_IN_D_RATE*(last_g_y - g_y) + (1.0-PID_PIT.NOW_IN_D_RATE)*PID_PIT.in_dout;
    

  71.         /*__________________________________==bu_wan_quan_wen_fen==_____________________________________________________________*/
    

  72.         PID_PIT.IN_OUT =(PID_PIT.OU_OUT - (-g_y))*PID_PIT.IN_P + PID_PIT.IN_I_integral*PID_PIT.IN_I - PID_PIT.in_dout*PID_PIT.IN_D;//内环PID计算输出
    

  73.         if(PID_PIT.IN_OUT > 1000)        
    

  74.                 PID_PIT.IN_OUT = 1000;        
    

  75.         if(PID_PIT.IN_OUT < -1000)        
    

  76.                 PID_PIT.IN_OUT = -1000;        
    

  77.         last_g_y = g_y;//保存本次陀螺仪数据，下次使用        
    

  78.         /*_______________________________==PID_X_IN==_______________________________________*/
    

  79.         /*----------------------------------X_END-------------------------------------------*/
    

  80.         /*----------------------------------X_END-------------------------------------------*/
    

  81.         /*_________________________________PID_Y_OU_________________________________________*/
    

  82.         PID_ROL.OU_I_integral = PID_ROL.OU_I_integral + error_rol;//外环角度误差积分
    

  83.         if(PID_ROL.OU_I_integral >1000)//积分限幅：100需调整
    

  84.                 PID_ROL.OU_I_integral = 1000;        
    

  85.         else if(PID_ROL.OU_I_integral < -1000)
    

  86.                 PID_ROL.OU_I_integral = -1000;
    

  87.         PID_ROL.OU_OUT = error_rol*PID_ROL.OU_P + PID_ROL.OU_I_integral*PID_ROL.OU_I;//角度环PI计算        
    

  88.         /*_______________________________==PID_Y_OU==_______________________________________*/
    

  89.         
    

  90.         /*_________________________________PID_Y_IN_________________________________________*/
    

  91.         //        Last_Gyro_y = gy;//保存本次陀螺仪数据，下次使用        
    

  92.         PID_ROL.IN_I_integral = PID_ROL.IN_I_integral+ PID_ROL.OU_OUT - g_x;
    

  93.         if(PID_ROL.IN_I_integral>200)// 积分限幅
    

  94.                 PID_ROL.IN_I_integral = 200;
    

  95.         else if(PID_ROL.IN_I_integral < -200)
    

  96.                 PID_ROL.IN_I_integral = -200;
    

  97.         if(LOCK_THR==1)
    

  98.                 PID_ROL.IN_I_integral = 0;        
    

  99.         /*____________________________________bu_wan_quan_wen_fen_______________________________________________________________*/
    

  100.         PID_ROL.in_dout = PID_ROL.NOW_IN_D_RATE*(g_x - last_g_x) + (1.0-PID_ROL.NOW_IN_D_RATE)*PID_ROL.in_dout;
    

  101.         /*__________________________________==bu_wan_quan_wen_fen==_____________________________________________________________*/
    

  102.         PID_ROL.IN_OUT =(PID_ROL.OU_OUT - g_x)*PID_ROL.IN_P + PID_ROL.IN_I_integral*PID_ROL.IN_I  - PID_ROL.in_dout*PID_ROL.IN_D;//内环PID计算输出
    

  103.         if(PID_ROL.IN_OUT > 1000)        
    

  104.                 PID_ROL.IN_OUT = 1000;        
    

  105.         if(PID_ROL.IN_OUT < -1000)        
    

  106.                 PID_ROL.IN_OUT = -1000;
    

  107.         
    

  108.         Ax_del = (last_g_x - g_x);
    

  109.         
    

  110.         last_g_x = g_x;//保存本次陀螺仪数据，下次使用        
    

  111.         /*_______________________________==PID_Y_IN==_______________________________________*/
    

  112.         
    

  113.         /*__________________________________PID_Z___________________________________________*/
    

  114.         
    

  115.         PID_YAW.ou_pout = PID_YAW.OU_P * error_yaw;
    

  116.         int16_t yaw_d;
    

  117.         yaw_d = MPU6050_GYRO_NOW.Z*0.0610351;
    

  118.         PID_YAW.ou_dout = PID_YAW.OU_D * yaw_d/0.02;
    

  119.         PID_YAW.OU_OUT = PID_YAW.ou_pout  + PID_YAW.ou_dout;
    

  120.         /*________________________________==PID_Z==_________________________________________*/
    


  121. 

  122.         /*___________________________________________________==PID==___________________________________________________________*/                
    

  123.         
    

  124.         
    

  125.         PID_ROL.OUT = PID_ROL.IN_OUT;
    

  126.         PID_PIT.OUT = PID_PIT.IN_OUT;
    

  127.         PID_YAW.OUT = PID_YAW.OU_OUT;
    

  128.         //PID_YAW.OUT = PID_Z;
    

  129.         /*___________________________________________________To-moto___________________________________________________________*/                
    

  130.         if(throttle>=200)//Temp_Throttle
    

  131.         {   
    

  132.                 moto1 = throttle + PID_ROL.OUT - PID_PIT.OUT - PID_YAW.OUT + PID_HIGH.OUT - PID_POSITION_ROL.OUT + PID_POSITION_PIT.OUT;
    

  133.                 moto2 = throttle - PID_ROL.OUT - PID_PIT.OUT + PID_YAW.OUT + PID_HIGH.OUT + PID_POSITION_ROL.OUT + PID_POSITION_PIT.OUT;
    

  134.                 moto3 = throttle - PID_ROL.OUT + PID_PIT.OUT - PID_YAW.OUT + PID_HIGH.OUT + PID_POSITION_ROL.OUT - PID_POSITION_PIT.OUT;
    

  135.                 moto4 = throttle + PID_ROL.OUT + PID_PIT.OUT + PID_YAW.OUT + PID_HIGH.OUT - PID_POSITION_ROL.OUT - PID_POSITION_PIT.OUT;
    

  136.         }
    

  137.         else
    

  138.         {
    

  139.                 moto1 = 0;
    

  140.                 moto2 = 0;
    

  141.                 moto3 = 0;
    

  142.                 moto4 = 0;
    

  143.         }
    

  144.         moto_view1=moto1;
    

  145.         moto_view2=moto2;
    

  146.         moto_view3=moto3;
    

  147.         moto_view4=moto4;
    

  148.         
    

  149.         if (LOCK_THR == 0)        Moto_Control(moto1,moto2,moto3,moto4);
    

  150.         else Moto_Control(0,0,0,0);
    

  151.         
    

  152.         
    

  153.         /*____________________________________________________roun_____________________________________________________________*/
    

  154.         round_rol = error_rol;
    

  155.         round_pit = error_pit;
    

  156.         round_yaw = error_yaw;
    

  157.         round_g_x = g_x;
    

  158.         round_g_y = g_y;
    

  159.         round_g_z = g_z;
    

  160.         /*__________________________________________________==roun==___________________________________________________________*/
    

  161.         /*_________________________________________________==To-moto==_________________________________________________________*/                
    

  162. }

避障策略使用漫反射红外对管，实现可调距离的对目标检测，然后配合光流模块实现自主避障飞行。

主函数中wihile循环及执行策略是
2ms执行一次姿态计算和控制
20ms执行一次光流数据读取和位置及高度控制
复制
  1. while (1)
    

  2.     {
    

  3.                 if (true == mission_attitude)//2ms一次计算
    

  4.         {
    

  5.             Prepare_Data();                                                        
    

  6.                         Recursive_Fliter(&MPU6050_GYRO_LAST,&MPU6050_GYRO_NOW ,5);                        
    

  7.                         Get_Attitude();                //姿态计算
    

  8.                         round_x = Q_ANGLE.X;
    

  9.                         round_y = Q_ANGLE.Y;
    

  10.                         round_z = Q_ANGLE.Z;
    

  11.                         RC_to_tar_angle();
    

  12.                         CONTROL(Q_ANGLE.Y,Q_ANGLE.X, Q_ANGLE.Z, tar_angle_X, tar_angle_Y, 0);
    

  13.                         counter++;
    

  14.                         mission_attitude=false;
    

  15.         }
    

  16.                 if (true == mission_senser)//20ms一次计算
    

  17.                 {
    

  18.                         I2C_ReadFlow();
    

  19.                         if(LOCK_THR==0)
    

  20.                         {
    

  21.                                 senser.x_sumdistance = senser.x_sumdistance + (float)senser.flow_comp_x_speed*0.01;
    

  22.                                 senser.y_sumdistance = senser.y_sumdistance + (float)senser.flow_comp_y_speed*0.01;
    

  23.                         }
    

  24.                         mission_senser=false;
    

  25.                 }
    

  26.                 if(true == mission_high)            //10ms
    

  27.                 {
    

  28.                         if (LOCK_THR==0 && (position_star==1))
    

  29.                         {
    

  30.                                 High_control();
    

  31.                         }
    

  32.                         else
    

  33.                         {
    

  34.                                 PID_HIGH.OUT=0;
    

  35.                                 PID_HIGH.ou_iout = 0;
    

  36.                         }
    

  37.                         mission_high = false;
    

  38.                 }
    

  39.                 if(true == mission_position)  //20ms
    

  40.                 {
    

  41.                         if(Rc_Get.AUX1 > 1500 && Rc_Get.AUX1 < 1750 && position_star == 0) 
    

  42.                         {
    

  43.                                 senser.y_sumdistance_offset = senser.y_sumdistance;
    

  44.                                 senser.x_sumdistance_offset = senser.x_sumdistance;
    

  45.                                 position_star = 1;
    

  46.                         }
    

  47.                         if((Rc_Get.AUX1 > 1000 && Rc_Get.AUX1 < 1500) ||  LOCK_THR==1)
    

  48.                         {
    

  49.                                 senser.y_sumdistance_offset = 0;
    

  50.                                 senser.x_sumdistance_offset = 0;
    

  51.                                 position_star = 0;
    

  52.                         }
    

  53.                         if(position_star == 1)
    

  54.                         {
    

  55.                                 if(AUTO_FLY_FLAG == 1)
    

  56.                                 {
    

  57.                                         move_control(0,0); 
    

  58.                                 }
    

  59.                                 Position_control();
    

  60.                         }
    

  61.                         if(position_star == 0)
    

  62.                         {
    

  63.                                 PID_POSITION_ROL.ou_iout=0;
    

  64.                                 PID_POSITION_ROL.OUT=0;
    

  65.                                 PID_POSITION_PIT.ou_iout=0;
    

  66.                                 PID_POSITION_PIT.OUT=0;
    

  67.                         }
    

  68.                         mission_position = false;
    

  69.                 }
飞行视频包括定点悬停10min和一键起飞自主避障飞行，我已经上传B站bilibili，将在审核通过后将AV号分享至该帖。
至此我的多旋翼飞控制作就到此结束了...快开学了，祝21IC的各位工作学习愉快！

资料：
1，各个部分函数及最终程序（工程文件太大，分享到百度云上）
2，最小底板PCB原理图（系列贴中有专门的介绍 https://bbs.21ic.com/icview-1350282-1-1.html）
3，我搜集到的全部关于K64的有用的资料（太大了，只能分享到百度云上）
4，选用的配件模块清单和各模块的资料详细在以前的帖子都有介绍，详细可以翻我的【多旋翼飞控】系列帖查看

本帖子中包含更多资源

您需要 登录 才可以下载或查看，没有账号？注册

×
回复

收藏24 举报

打赏榜单

21ic小喇叭 打赏了 10.00 元 2016-02-24

相关帖子
Ketose 发表于 2016-2-18 21:10 | 显示全部楼层
这个必须要收藏。。。。
回复

收藏24 举报

liozhenbin92 发表于 2016-2-19 14:13 | 显示全部楼层
牛人，过来学习下。
回复

收藏24 举报

yuchl 发表于 2016-2-19 14:50 | 显示全部楼层
学习了，收藏！
回复

收藏24 举报

michael_llh 发表于 2016-2-19 16:19 | 显示全部楼层
不错，说的挺好。不过对于四轴的串级PID调整还是要靠实际调整观察现象，理论作为辅助作用！
回复

收藏24 举报

 楼主| 冷不丁 发表于 2016-2-19 17:50 | 显示全部楼层
michael_llh 发表于 2016-2-19 16:19
不错，说的挺好。不过对于四轴的串级PID调整还是要靠实际调整观察现象，理论作为辅助作用！ ...

嗯嗯，这种运动控制的东西和电机还有硬件情况有很大的关系，主要还是实际现象实际理解作调整最好。
回复

收藏24 举报

eydj2008 发表于 2016-2-20 09:02 | 显示全部楼层
还以为是多轴 结果是四轴。。。
回复

收藏24 举报

wyscjm 发表于 2016-2-21 09:19 | 显示全部楼层
怎么收藏呢？
回复

收藏24 举报

tliang1984 发表于 2016-2-24 09:14 | 显示全部楼层
这个不错哦，支持楼主
回复

收藏24 举报

gaon2 发表于 2016-2-24 09:40 | 显示全部楼层
较完整的设计分享，有时间谈谈设计心得体会，及设计遇到难点及解决思路，那就更赞了
回复

收藏24 举报

wahe2008 发表于 2016-2-25 11:01 | 显示全部楼层
不错的帖子，收藏了
回复

收藏24 举报

luochangqing112 发表于 2016-2-25 21:12 | 显示全部楼层
收藏
回复

收藏24 举报

netzhang 发表于 2016-2-26 15:35 | 显示全部楼层
有时间的时候跟楼主学习做四轴。
回复

收藏24 举报

8049401 发表于 2016-2-27 07:02 | 显示全部楼层
收藏了，谢谢分享！
回复

收藏24 举报

TyrQueen 发表于 2016-3-16 15:10 | 显示全部楼层
楼主好厉害，自己到现在还是没有看懂PID如何调。
回复

收藏24 举报

mega1702 发表于 2016-3-28 14:12 | 显示全部楼层
感谢楼主分享!!!
回复

收藏24 举报

蛊惑狼 发表于 2016-4-9 15:22 | 显示全部楼层
大神
回复

收藏24 举报

返回列表 发新帖
高级模式
B Color Image Link Quote Code Smilies
您需要登录后才可以回帖 登录 | 注册

本版积分规则

冷不丁
个人签名：hungry

10

主题

36

帖子

7

粉丝
快速回复 在线客服 返回列表 返回顶部