標題: STM32四旋翼飛控定高算法，含源代碼 [打印本頁]
作者: lishenliang    時間: 2019-10-21 20:53
標題: STM32四旋翼飛控定高算法，含源代碼
之前做電子設計競賽做的定高算法，硬件是基于STM32F103的飛控，算法主要是串級PID，使用的是超聲波測距模塊。由于整個飛控工程比較大，現(xiàn)將核心算法貼出來供大家參考。

單片機源程序如下:
  1. #include "height_ctrl.h"
  2. #include "include.h"
  3. #include "mymath.h"

  5. extern float Thr_Weight;
  6. extern Gravity V;//重力分量

  8. /******************高度控制變量********************/
  9. float height_ctrl_out;
  10. float wz_acc;
  11. float keepheight_thr;
  12. /*-------------------------------------------------*/

  14. /******************超聲波變量********************/
  15. #define ULTRA_SPEED                  300    // mm/s
  16. #define ULTRA_INT        300    // 積分幅度
  17. u8 lock=0;
  18. extern s8 ultra_start_f;
  19. extern float US100_Alt_delta;
  20. float exp_height_speed,exp_height;
  21. float ultra_speed,ultra_speed_acc;
  22. float ultra_dis_lpf;
  23. float ultra_ctrl_out;
  24. _st_height_pid_v ultra_ctrl;
  25. _st_height_pid ultra_pid;
  26. /*-------------------------------------------------*/

  28. /******************速度環(huán)變量********************/
  29. float wz_speed;
  30. float wz_acc_mms2;
  31. float tempacc_lpf;
  32. u8 lock_spd_crl=0;
  33. _st_height_pid_v wz_speed_pid_v;
  34. _st_height_pid wz_speed_pid;
  35. _st_height_pid ultra_wz_speed_pid;
  36. _st_height_pid baro_wz_speed_pid;
  37. /*-------------------------------------------------*/

  39. /******************氣壓計變量********************/
  40. float baro_height,baro_height_old;
  41. #define BARO_SPEED                  300    // mm/s
  42. #define BARO_INT        300    // 積分幅度

  44. u8 lock_BARO=0;//定高鎖定當前以及清零用
  45. #define BARO_SPEED_NUM 10
  46. float baro_speed_arr[BARO_SPEED_NUM + 1];

  48. #define ACC_SPEED_NUM 50
  49. float acc_speed_arr[ACC_SPEED_NUM + 1];

  51. float baro_dis_lpf,baro_dis_kalman;
  52. float baro_ctrl_out;
  53. _st_height_pid_v baro_ctrl;
  54. _st_height_pid baro_pid;

  56. u16 baro_cnt[2];
  57. u16 acc_cnt[2];
  58. float baro_speed,baro_speed_lpf;
  59. float baro_height;
  60. float Pressure_groud;
  61. /*-------------------------------------------------*/

  63. void Height_Ctrl(float T,float thr)
  64. {        
  65.         static u8 height_ctrl_start_f;
  66.         static float thr_lpf;
  67.         static float height_thr;
  68.         static float wz_acc_temp,wz_acc1;
  69.         static float hc_speed_i,wz_speed_0;
  70.         static float h_speed;
  71.         
  72.         switch( height_ctrl_start_f )
  73.         {
  74.                 case 0:
  75.         
  76.                                 if( sensor.acc.averag.z > 7000 )//注意這里，指水平靜態(tài)下的Z值要比這個大，表示水平垂直
  77.                                 {
  78.                                           height_ctrl_start_f = 1;
  79.                                 }
  80.                                 break;
  81.                
  82.                 case 1:
  83.                         
  84.                                 LockForKeepHigh(thr_lpf);//一旦切到控高模式，立即保存當前的距離值，油門值、相關清零。
  85.                                 if((flag.FlightMode==ULTRASONIC_High && lock==1 &&lock_spd_crl==0)|| (flag.FlightMode==ATMOSPHERE_High && lock_BARO==1 &&lock_spd_crl==0))
  86.                                 {
  87.                                                 keepheight_thr=thr_lpf;
  88.                                                 wz_speed_0=0;
  89.                                                 hc_speed_i=0;
  90.                                                 hc_speed_i=0;
  91.                                                 wz_speed=0;
  92.                                                 wz_speed_pid_v.err_old=0;
  93.                                                 wz_speed_pid_v.err=0;
  94.                                                 wz_speed_pid_v.err_i=0;
  95.                                                 lock=2;
  96.                                                 lock_BARO=2;
  97.                                                 lock_spd_crl=1;
  98.                                 }
  99.                                 else if(flag.FlightMode==MANUAL_High)
  100.                                 {
  101.                                                 lock=0;
  102.                                                 lock_BARO=0;
  103.                                                 lock_spd_crl=0;//切回手動模式后置零，以使下次進入定高模式是能順利置零速度內(nèi)環(huán)        
  104.                                 }
  105.                         
  106.                                 height_thr = LIMIT( thr , 0, 700 );
  107.                                 thr_lpf += ( 1 / ( 1 + 1 / ( 2.0f *3.14f *T ) ) ) *( height_thr - thr_lpf );//對油門值低通濾波修正
  108.                                 userdata1[0]=        thr_lpf;//調(diào)試用
  109.                                 /*下面的低通濾波用于測試對比效果，最終沒有選用*/                                
  110.                     //wz_acc += ( 1 / ( 1 + 1 / ( 8 *3.14f *T ) ) ) *my_deathzoom( (V.z *(sensor.acc.averag.z- sensor.acc.quiet.z) + V.x *sensor.acc.averag.x + V.y *sensor.acc.averag.y - wz_acc),100 );//
  111.                     //wz_acc_mms2 = (float)(wz_acc/8192.0f) *9800 ;
  112.                                 //加速度的靜態(tài)零點是讀取的EEPROM數(shù)據(jù)，但是每次飛行都可能不一樣，能每次起飛前校準一次最好，可以單獨校準Z軸的，自行設計程序
  113.                                 wz_acc_temp = V.z *(sensor.acc.averag.z- sensor.acc.quiet.z) + V.x *sensor.acc.averag.x + V.y *sensor.acc.averag.y;//
  114.                                 Moving_Average( wz_acc_temp,acc_speed_arr,ACC_SPEED_NUM,acc_cnt ,&wz_acc1 );
  115.                                 tempacc_lpf= (float)(wz_acc1/8192.0f) *9800;//9800 *T;由于是+-4G共8G，65535/8g=8192 g，加速度，mms2毫米每平方秒
  116.                                 if(abs(tempacc_lpf)<50)tempacc_lpf=0;//簡單消除下噪聲
  117.                                 userdata1[2]=tempacc_lpf;
  118.                                 wz_speed_0 += tempacc_lpf *T;//加速度計積分成速度
  119.                                                                
  120.                                 if( ultra_start_f == 1 )////不管是啥模式，只要更新了超聲波數(shù)據(jù)就進行運算
  121.                                 {        
  122.                                          Ultra_dataporcess(15.0f*TT);
  123.                                          ultra_start_f=2;
  124.                                 }
  125.                                 
  126.                                 if(baro_start_f==1)//不管是啥模式，只要更新了氣壓數(shù)據(jù)就進行運算
  127.                                 {
  128.                                          Baro_dataporcess(8.0f*TT);                         //8.0f*TT這么長時間更新一次氣壓計數(shù)據(jù)
  129.                                          userdata2[10]=baro_height;//Baro_Height_Source
  130.                                          baro_dis_delta=baro_height-baro_dis_old;
  131.                                          baro_dis_old=baro_height;//氣壓計速度可以繼續(xù)優(yōu)化，這里比較粗糙                        
  132.                        //Moving_Average( (float)( baro_dis_delta/(8.0f*TT)),baro_speed_arr,BARO_SPEED_NUM,baro_cnt ,&baro_speed ); //單位mm/s
  133.                                          userdata1[11]=baro_speed_lpf=0.4* baro_dis_delta/(8.0f*TT);//baro_speed這里乘以系數(shù)以削減該值                 
  134.                                          baro_start_f=2;
  135.                                 }        
  136.                
  137.                                 if(flag.FlightMode==ULTRASONIC_High)
  138.                                 {
  139.                                          h_speed=ultra_speed;
  140.                                          wz_speed_0 = 0.99        *wz_speed_0 + 0.01*h_speed;//超聲波垂直速度互補濾波
  141.                                 }                                
  142.                                 else if(flag.FlightMode==ATMOSPHERE_High)
  143.                                 {
  144.                                          h_speed=baro_speed_lpf;
  145.                                          wz_speed_0 = 0.99        *wz_speed_0 + 0.01*h_speed;//氣壓計垂直速度互補濾波，系數(shù)可調(diào)
  146.                                 }        

  148.                                 userdata1[3] =h_speed;//h_speed是高度環(huán)傳到速度環(huán)的實測高度方向速度【但可能是錯誤的，氣壓計速度不可靠】
  149.                                 userdata1[4]=wz_speed_0;//調(diào)試用
  150.                                 hc_speed_i += 0.4f *T *( h_speed - wz_speed );//速度偏差積分，乘以了0.4系數(shù)
  151.                                 hc_speed_i = LIMIT( hc_speed_i, -500, 500 );//積分限幅        
  152.                                 userdata1[5] =hc_speed_i;//這個沒顯示
  153.                                 wz_speed_0 += ( 1 / ( 1 + 1 / ( 0.1f *3.14f *T ) ) ) *( h_speed - wz_speed_0  ) ;//0.1實測速度修正加速度算的速度
  154.                                 userdata1[6] = wz_speed=wz_speed_0 + hc_speed_i;//經(jīng)過修正的速度+經(jīng)過限幅的增量式速度積分

  156.                           if( flag.FlightMode == ATMOSPHERE_High)
  157.                                 {
  158.                                                 if(baro_start_f==2)//說明有新數(shù)據(jù)且被上面移動均值濾波使用過了
  159.                                                 {
  160.                                                         baro_start_f = -1;
  161.                                                         Baro_Ctrl(8.0f*TT,thr_lpf);
  162.                                                 }        
  163.                                                 height_speed_ctrl(T,keepheight_thr,baro_ctrl_out,baro_speed_lpf);                                                        
  164.                                 }

  166.                                 if( flag.FlightMode == ULTRASONIC_High)
  167.                                 {
  168.                                                 height_speed_ctrl(T,keepheight_thr,ultra_ctrl_out,ultra_speed);//系數(shù)原來是0.4
  169.                                                         
  170.                                                 if( ultra_start_f == 2 )//超聲波數(shù)據(jù)更新了且被運算了
  171.                                                 {                                
  172.                                                                 Ultra_Ctrl(15.0f*TT,thr_lpf);//realtime是周期為 TT 秒的，#define TT 0.0025//控制周期2.5ms
  173.                                                                 ultra_start_f = -1;
  174.                                                 }
  175.                                 }
  176.                
  177.               /*******************************************控制高度輸出********************************************/
  178.                                 if(flag.FlightMode==ULTRASONIC_High || flag.FlightMode==ATMOSPHERE_High || flag.FlightMode==ACC_High)//注意這里，模式
  179.                                 {               
  180.                                           height_ctrl_out = wz_speed_pid_v.pid_out;
  181.                                 }
  182.                                 else
  183.                                 {
  184.                                           height_ctrl_out = thr;
  185.                                 }
  186.                                 userdata2[0]=exp_height;
  187.                         
  188.                                 break;         
  189.                                 default: break;
  190.         }
  191. }

  193. void WZ_Speed_PID_Init()
  194. {
  195.                 ultra_wz_speed_pid.kp = 0.25;// //超聲波定高的速度環(huán)PID，會被EEPROM里的重新賦值的
  196.                 ultra_wz_speed_pid.ki = 0.08; //0.12
  197.                 ultra_wz_speed_pid.kd = 8;
  198.                 baro_wz_speed_pid.kp = 0.1;// 氣壓定高的速度環(huán)PID，會被EEPROM里的重新賦值的
  199.                 baro_wz_speed_pid.ki = 0.06;
  200.                 baro_wz_speed_pid.kd = 8;//1.4*10，在計算式乘以了10，已刪除
  201.                 wz_speed_pid.kp = 0.1;//速度環(huán)默認PID，真正用不到被重新賦值
  202.                 wz_speed_pid.ki = 0.08;
  203.                 wz_speed_pid.kd = 8;
  204. }

  206. void height_speed_ctrl(float T,float thr_keepheight,float exp_z_speed,float h_speed)
  207. {
  208.     userdata1[1] =        thr_keepheight;
  209.                 //wz_speed被用于參與超聲波高度D運算
  210.                 userdata1[7]=wz_speed_pid_v.err = wz_speed_pid.kp *( exp_z_speed - wz_speed );//
  211.                 userdata1[8]=wz_speed_pid_v.err_d = wz_speed_pid.kd * (-tempacc_lpf) *T;//(wz_speed_pid_v.err - wz_speed_pid_v.err_old);
  212.                 //wz_speed_pid_v.err_i += wz_speed_pid.ki *wz_speed_pid_v.err *T;
  213.                 wz_speed_pid_v.err_i += wz_speed_pid.ki *( exp_z_speed - h_speed ) *T;//期望速度與實際速度誤差積分
  214.                 wz_speed_pid_v.err_i = LIMIT(wz_speed_pid_v.err_i,-Thr_Weight *200,Thr_Weight *200);
  215.                 userdata1[9]=wz_speed_pid_v.err_i;
  216.                 wz_speed_pid_v.pid_out = thr_keepheight + Thr_Weight *LIMIT((wz_speed_pid.kp *exp_z_speed + wz_speed_pid_v.err + wz_speed_pid_v.err_d + wz_speed_pid_v.err_i),-300,300);//積分原來沒有乘wz_speed_pid.kp
  217.                 userdata1[10]=wz_speed_pid_v.pid_out-thr_keepheight;
  218.                 wz_speed_pid_v.err_old = wz_speed_pid_v.err;
  219. }

  221. void Baro_PID_Init()
  222. {
  223.                 baro_pid.kp = 0.15;//氣壓定高的高度位置環(huán)PID，會被EEPROM里的重新賦值的
  224.                 baro_pid.ki = 0;
  225.                 baro_pid.kd = 1.5;        //2.5
  226. }

  228. void Baro_dataporcess(float T)
  229. {
  230.                 float baro_dis_tmp;        
  231.                
  232.                 baro_dis_tmp = Moving_Median(2,5,baro_height);//對超聲波測量的距離進行移動中位值濾波

  234.                 if( baro_dis_tmp < Baro_MAX_Height )
  235.                 {        
  236.                         if( ABS(baro_dis_tmp - baro_dis_lpf) < 100 )
  237.                         {        
  238.                                  baro_dis_lpf += ( 1 / ( 1 + 1 / ( 2.0f *3.14f *T ) ) ) *(baro_dis_tmp - baro_dis_lpf) ;
  239.                         }
  240.                         else if( ABS(baro_dis_tmp - baro_dis_lpf) < 500 )
  241.                         {
  242.                                  baro_dis_lpf += ( 1 / ( 1 + 1 / ( 1.0f *3.14f *T ) ) ) *(baro_dis_tmp- baro_dis_lpf) ;
  243.                         }
  244.                         else
  245.                         {
  246.                                  baro_dis_lpf += ( 1 / ( 1 + 1 / ( 0.5f *3.14f *T ) ) ) *(baro_dis_tmp- baro_dis_lpf) ;
  247.                         }
  248.                 }
  249.                 userdata2[2]=baro_dis_lpf;//調(diào)試用
  250.                 userdata2[3]+= ( 1 / ( 1 + 1 / ( 1.0f *3.14f *T ) ) ) *(Baro_Height_Source- userdata2[3]) ;//調(diào)試用        
  251.                 userdata2[4]=baro_speed;//調(diào)試用
  252. }

  254. void Baro_Ctrl(float T,float thr)
  255. {
  256.                 exp_height_speed = BARO_SPEED *my_deathzoom_2(thr - 500,100)/400.0f; //20這里具體根據(jù)自己起飛油門定，油門控制高度+-ULTRA_SPEEDmm / s
  257.                 exp_height_speed = LIMIT(exp_height_speed ,-BARO_SPEED,BARO_SPEED);
  258.                
  259.                 if( exp_height > Baro_MAX_Height )//限定高度，可以根據(jù)實際修改
  260.                 {
  261.                         if( exp_height_speed > 0 )
  262.                         {
  263.                                 exp_height_speed = 0;
  264.                         }
  265.                 }
  266.                 else if( exp_height < Baro_MIN_Height )
  267.                 {
  268.                         if( exp_height_speed < 0 )
  269.                         {
  270.                                 exp_height_speed = 0;
  271.                         }
  272.                 }
  273.                
  274.                 exp_height += exp_height_speed *T;//累積期望高度，因為期望速度可能改變了                        
  275.                 baro_ctrl.err = baro_pid.kp*(exp_height - baro_dis_lpf);//baro_dis_lpfmm 對高度誤差乘以Kp
  276.                 userdata2[5]=baro_ctrl.err;
  277.                 baro_ctrl.err_i += baro_pid.ki *baro_ctrl.err *T;//對高度誤差積分
  278.                 userdata2[6]=baro_ctrl.err_i = LIMIT(baro_ctrl.err_i,-Thr_Weight *BARO_INT,Thr_Weight *BARO_INT);//積分限幅，融合了油門權(quán)重
  279.                 //對于D，融合了加速度計運算得到的距離        
  280.                 userdata2[7]=0.4f *(baro_ctrl.err - baro_ctrl.err_old);
  281.                 baro_ctrl.err_d = baro_pid.kd *( 0.6f *(-wz_speed*T) + 0.4f *(baro_ctrl.err - baro_ctrl.err_old) );
  282.                 userdata2[8]=baro_ctrl.err_d;
  283.                 baro_ctrl.pid_out = baro_ctrl.err + baro_ctrl.err_i + baro_ctrl.err_d;
  284.                 baro_ctrl.pid_out = LIMIT(baro_ctrl.pid_out,-300,300);        
  285.                 userdata2[9]=baro_ctrl_out = baro_ctrl.pid_out;        
  286.                 baro_ctrl.err_old = baro_ctrl.err;
  287. }

  289. void Ultra_PID_Init()
  290. {
  291.                 ultra_pid.kp = 0.5;//超聲波定高的高度位置環(huán)PID，會被EEPROM里的重新賦值的
  292.                 ultra_pid.ki = 0;
  293.                 ultra_pid.kd = 2.5;        //2.5
  294. }

  296. void Ultra_dataporcess(float T)
  297. {
  298.                 float ultra_sp_tmp,ultra_dis_tmp;        
  299.                 ultra_dis_tmp = Moving_Median(1,5,US100_Alt);//對超聲波測量的距離進行移動中位值濾波
  300.                 userdata2[1]=ultra_dis_tmp;
  301.                 if( ultra_dis_tmp < Ultrasonic_MAX_Height )
  302.                 {        
  303.                         if( ABS(ultra_dis_tmp - ultra_dis_lpf) < 100 )
  304.                         {        
  305.                                 ultra_dis_lpf += ( 1 / ( 1 + 1 / ( 4.0f *3.14f *T ) ) ) *(ultra_dis_tmp - ultra_dis_lpf) ;
  306.                         }
  307.                         else if( ABS(ultra_dis_tmp - ultra_dis_lpf) < 200 )
  308.                         {                        
  309.                                 ultra_dis_lpf += ( 1 / ( 1 + 1 / ( 2.2f *3.14f *T ) ) ) *(ultra_dis_tmp- ultra_dis_lpf) ;
  310.                         }
  311.                         else if( ABS(ultra_dis_tmp - ultra_dis_lpf) < 400 )
  312.                         {
  313.                                 ultra_dis_lpf += ( 1 / ( 1 + 1 / ( 1.2f *3.14f *T ) ) ) *(ultra_dis_tmp- ultra_dis_lpf) ;
  314.                         }
  315.                         else
  316.                         {
  317.                                 ultra_dis_lpf += ( 1 / ( 1 + 1 / ( 0.2f *3.14f *T ) ) ) *(ultra_dis_tmp- ultra_dis_lpf) ;
  318.                         }
  319.                 }
  320.                
  321.                 //注意超聲波測量的 距離的時效性問題，避免反復被計算
  322.                 ultra_sp_tmp = Moving_Median(0,5,US100_Alt_delta/T); //對超聲波測距出來兩次之間距離差計算的速度中值濾波 ultra_delta/T;

  324.                 if( ultra_dis_tmp < Ultrasonic_MAX_Height )//小于3米,注意這里，萬一超出了呢？
  325.                 {
  326.                         if( ABS(ultra_sp_tmp) < 100 )//運動速度小于100mm/s
  327.                         {
  328.                                 ultra_speed += ( 1 / ( 1 + 1 / ( 4 *3.14f *T ) ) ) * ( (float)(ultra_sp_tmp) - ultra_speed );
  329.                         }//ultra_speed會傳遞給速度環(huán)，作為當前速度的反饋,原來是4
  330.                         else
  331.                         {
  332.                                 ultra_speed += ( 1 / ( 1 + 1 / ( 1.0f *3.14f *T ) ) ) * ( (float)(ultra_sp_tmp) - ultra_speed );
  333.                         }//系數(shù)越大作用越小，原來是1
  334.                 }
  335. }

  337. void Ultra_Ctrl(float T,float thr)
  338. {
  339.                 exp_height_speed = ULTRA_SPEED *my_deathzoom_2(thr - 500,100)/300.0f; //20這里具體根據(jù)自己起飛油門定，油門控制高度+-ULTRA_SPEEDmm / s
  340.                 exp_height_speed = LIMIT(exp_height_speed ,-ULTRA_SPEED,ULTRA_SPEED);
  341.                
  342.                 if( exp_height > Ultrasonic_MAX_Height )//超出超聲波高度穩(wěn)定范圍則不執(zhí)行
  343.                 {
  344.                         if( exp_height_speed > 0 )
  345.                         {
  346.                                 exp_height_speed = 0;
  347.                         }
  348.                 }
  349.                 else if( exp_height < Ultrasonic_MIN_Height )
  350.                 {
  351.                         if( exp_height_speed < 0 )
  352.                         {
  353.                                 exp_height_speed = 0;
  354.                         }
  355.                 }
  356.                
  357.                 exp_height += exp_height_speed *T;//累積期望高度，因為期望速度可能改變了

  359.                 ultra_ctrl.err = ( ultra_pid.kp*(exp_height - ultra_dis_lpf) );//mm 對高度誤差乘以Kp
  360.                 userdata2[5]=ultra_ctrl.err;
  361.                 ultra_ctrl.err_i += ultra_pid.ki *ultra_ctrl.err *T;//對高度誤差積分
  362.                 userdata2[6]=ultra_ctrl.err_i = LIMIT(ultra_ctrl.err_i,-Thr_Weight *ULTRA_INT,Thr_Weight *ULTRA_INT);//積分限幅，融合了油門權(quán)重
  363.                                         //對于D，融合了加速度計運算得到的距離        
  364.                 userdata2[7]=0.4f *(ultra_ctrl.err_old - ultra_ctrl.err);
  365. ……………………

  367. …………限于本文篇幅 余下代碼請從51黑下載附件…………
復制代碼

所有資料51hei提供下載:
height_ctrl.rar (4.54 KB, 下載次數(shù): 97)



作者: FMCH    時間: 2019-11-2 11:53
樓主你好能不能簡單分享一下定高是PID的參數(shù)測定方法嗎



歡迎光臨 (http://www.torrancerestoration.com/bbs/) Powered by Discuz! X3.1