找回密碼
 立即注冊

QQ登錄

只需一步，快速開始

搜索
查看: 5402|回復(fù): 1
打印 上一主題 下一主題
收起左側(cè)

固定翼四軸通用的串級PID算法分享，非常穩(wěn)定好調(diào)

[復(fù)制鏈接]
跳轉(zhuǎn)到指定樓層
樓主
ID:171096 發(fā)表于 2017-3-17 01:55 | 只看該作者 |只看大圖 回帖獎勵 |倒序瀏覽 |閱讀模式
聲明：樓主是四軸飛行器的愛好者（廢話，不喜歡會做這個�。�？）以前學(xué)習(xí)過相關(guān)方面的知識，不過開始的時候沒有太多資金，而且無刷電機很危險，所以先從小四軸開始玩起，經(jīng)過一年的努力四軸終于可以比較穩(wěn)定的飛行了（程序參考了匿名和蜂鳥四軸，感謝他們的無私奉獻）

做的時候發(fā)很多人都在抱怨四軸參數(shù)難調(diào)，剛開始我也使用的是常見的pid控制，發(fā)現(xiàn)對參數(shù)確實十分敏感，很難達到穩(wěn)定的效果，甚至直接使用陀螺儀的數(shù)據(jù)進行角速率反饋得到的效果都比它好。

通過查閱資料我發(fā)現(xiàn)四軸一般會被簡化為一個低阻尼的二階系統(tǒng)，角速率反饋恰恰可以增加它的阻尼（這個在二代的戰(zhàn)斗機比如殲7中廣泛采用，是改善阻尼的好方法）。于是我就是用了在固定翼飛機上常常采用的串級pid來調(diào)試，經(jīng)過師兄理論上的指點，四軸已經(jīng)可以比較穩(wěn)定的飛行。

這個算法有固定翼的控制，和四軸控制原理相同。 首先使用角速率反饋作為內(nèi)環(huán)，在用角度反饋作為外環(huán)，只要內(nèi)環(huán)的參數(shù)調(diào)整合適，外環(huán)的參數(shù)從個位數(shù)變成六七十，四軸都可以穩(wěn)定的飛行。

參數(shù)調(diào)節(jié)：



還有代碼分享：
  1.     void GET_EXPRAD(void)                     
  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 %f",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.     }

  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;

  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;

  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);

  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 %d",moto1,moto2,moto3,moto4);
  95.             if(ARMED)        MOTO_PWMRFLASH(moto1,moto2,moto3,moto4);
  96.             else                        MOTO_PWMRFLASH(0,0,0,0);
  97.     }
復(fù)制代碼
分享到:  QQ好友和群QQ好友和群 QQ空間QQ空間 騰訊微博騰訊微博 騰訊朋友騰訊朋友
收藏收藏4 分享淘帖 頂頂 踩踩
回復(fù)

使用道具 舉報

沙發(fā)
ID:204748 發(fā)表于 2017-5-27 13:01 來自手機 | 只看該作者
謝謝分享。謝謝奉獻。謝謝樓主。
回復(fù)

使用道具 舉報

您需要登錄后才可以回帖 登錄 | 立即注冊

本版積分規(guī)則

手機版|小黑屋|51黑電子論壇 |51黑電子論壇6群 QQ 管理員QQ:125739409;技術(shù)交流QQ群281945664

Powered by 單片機教程網(wǎng)

快速回復(fù) 返回頂部 返回列表