单片机论坛

 找回密码
 立即注册

QQ登录

只需一步,快速开始

搜索
查看: 94|回复: 5
收起左侧

无人机加速计、气压计、GPS数据融合

[复制链接]
涂覆莉娜 发表于 2019-8-9 22:54 | 显示全部楼层 |阅读模式
在无人机控制当中,传感器的参与那是必不可少的,特别是陀螺仪,最经典的为MPU6050,目前大部分的无人机都是用的这个器件。熟悉MPU6050的都知道,这个传感器可以输出三轴加速计、三轴陀螺仪;加速计,顾名思义,直接测得物体的加速度。理论上  加速度积分得到速度,速度再积分得到位移:S = V0 * t + 1/2 * a * t ^ 2;V = v0 + a * t;按照此理想情况下:一个加速计直接积分算得速度跟位移就得了呀,为啥还要气压计、GPS呢。无人机为了定点悬停,需要计算XYZ三轴的速度跟位移,而加速度计有一个致命的缺点就是它积分会漂移,而且是漂移非常大的那种,如果直接用这个数据做控制,飞机都不知道要飘到哪里去了。所以才借助外部传感器气压计、GPS修正加速计的漂移。加速计在无人机三轴位置速度计算时也有很大的优点,就是其灵敏度相对很高,无人机稍微动一点加速计都可以感受的出来,然后把数据传到无人机,无人机得以做相应的调整。

数据融合

具体数据怎么融合呢?我们先来总结一下加速计、气压计、gps各自的优缺点:加速计:优点反应速度快,缺点积分漂移严重。气压计GPS:优点绝对位置比较平稳,缺点反应速度比较慢。所以就有了互补滤波这个伟大的,简单易懂的滤波算法。

以下为互补滤波代码:

void Gps_Acc_Delay(void){ volatile uint8 i; for (i = 0; i < XY_VEL_HISTORY - 1; i++) {  x_vel_history = x_vel_history[i+1];  y_vel_history = y_vel_history[i+1];    x_pos_history = x_pos_history[i+1];  y_pos_history = y_pos_history[i+1]; } x_vel_history[XY_VEL_HISTORY - 1] = x_est[1]; y_vel_history[XY_VEL_HISTORY - 1] = y_est[1]; x_pos_history[XY_VEL_HISTORY - 1] = x_est[0]; y_pos_history[XY_VEL_HISTORY - 1] = y_est[0];}void ObservationOPT_Update(float pos_x,float vel_x,float pos_y,float vel_y)  //have data and update{ uint8 i; corrOPT_xpos = pos_x-x_est[0]; corrOPT_xvel = vel_x-x_est[1]; corrOPT_ypos = pos_y-y_est[0]; corrOPT_yvel = vel_y-y_est[1]; return;}void ObservationUpdate(int baro_disdance,int16 baro_vel){ uint8 i; corr_baro = (float)baro_disdance - z_est[0]; if (fly_status.HOLD_STATUS == HOLD_STOP) {  z_est[0] = baro_disdance;  z_est[1] = 0; } return;}void TimeUpdate(struct vertical_information *pAltitude,realacc* acc){ static float high_pass_zvel = 0; z_est[2]=acc->zz-z_bias; accel_filter_predict(INAV_T,z_est); z_est[0] += corr_baro * w_z_baro * INAV_T; z_bias -= corr_baro * 0.001f * INAV_T; z_est[1] += corr_baro * baro_vel_gain * INAV_T; high_pass_zvel = z_est[1]; pAltitude->altitude_hat=z_est[0]; pAltitude->velocity_hat=high_pass_zvel; ObservationOPT_Update(x_est[0],optflow.velx,y_est[0],optflow.vely); x_est[2]=acc->xx-x_bias; accel_filter_predict(INAV_TXY,x_est);  x_est[0] += corrGPS_xpos * x_pos_gain * INAV_TXY; x_est[1] += (corrOPT_xvel*0 + corrGPS_xpos * 1.0f + corrGPS_xvel * 0.2f) * x_vel_gain * INAV_TXY; y_est[2]=acc->yy-y_bias; accel_filter_predict(INAV_TXY,y_est);  y_est[0] += corrGPS_ypos * y_pos_gain * INAV_TXY; y_est[1] += (corrOPT_yvel*0 + corrGPS_ypos * 1.0f + corrGPS_yvel * 0.2f) * y_vel_gain * INAV_TXY; if (GPS_is_Good) {  if (corrGPS_xvel > -10 && corrGPS_xvel < 10)    x_bias -= (corrGPS_xpos*0.3f + corrGPS_xvel) * INAV_TXY * 0.1f;  if (corrGPS_yvel > -10 && corrGPS_yvel < 10)    y_bias -= (corrGPS_ypos*0.3f + corrGPS_yvel) * INAV_TXY * 0.1f; } else {  x_est[1] = Body_dat.vel_x;  y_est[1] = Body_dat.vel_y;  x_est[0] = Body_dat.pos_x1;  y_est[0] = Body_dat.pos_y1; } pos_vel_xy.posx = Body_dat.pos_x1; pos_vel_xy.posy = Body_dat.pos_y1; pos_vel_xy.velx = x_est[1]; pos_vel_xy.vely = y_est[1];}

以上代码就是无人机里的互补滤波修正。如有不懂欢迎留言或扣我1836703877。无人机四轴本人做了五年,以上代码全部自己写,上传的没来的及添加注释,但是代码时成功在飞机上跑的并且无人机出货的。

评分

参与人数 1黑币 +50 收起 理由
admin + 50 共享资料的黑币奖励!

查看全部评分

回复

使用道具 举报

51hei团团 发表于 2019-8-10 16:10 | 显示全部楼层
好东东啊,楼主能把代码放在代码框中吗?
回复

使用道具 举报

 楼主| 涂覆莉娜 发表于 2019-8-12 19:55 | 显示全部楼层
可以的,待我整理一下
回复

使用道具 举报

007cad 发表于 2019-8-13 15:30 | 显示全部楼层
无人机 未来发展前途广阔
大家说一下哪家的无人机技术优秀
回复

使用道具 举报

 楼主| 涂覆莉娜 发表于 2019-8-14 10:36 | 显示全部楼层
目前来看,还是大疆的比较牛逼,各方面稳定,控制,都没得说
回复

使用道具 举报

 楼主| 涂覆莉娜 发表于 2019-8-14 10:38 | 显示全部楼层
  1. _POS_NOWstatus pos_vel_xy;
  2. float z_est[3]={0,0,0},x_est[3]={0,0,0},y_est[3]={0,0,0};
  3. typedef struct {
  4. float R[3][3];                    /**< Rotation matrix body to world, (Tait-Bryan, NED)*/
  5. }vehicle_attitude_s;

  6. vehicle_attitude_s      vehicle_att;
  7. vehicle_attitude_s      vehicle_optical;
  8. realacc real_acceleration;
  9. _vertical_information  baro_acc_alt;

  10. //#define ALT_VEL_HISTORY 10
  11. //volatile float alt_vel_history[ALT_VEL_HISTORY] = {0};

  12. //#define XY_VEL_HISTORY 3
  13. //volatile float x_vel_history[XY_VEL_HISTORY] = {0}, y_vel_history[XY_VEL_HISTORY] = {0};

  14. static float z_bias = 0,x_bias = 0,y_bias = 0;
  15. float w_z_baro=0.5f;
  16. static float corr_baro = 0;//corr_baro_filter = 0;
  17. static float corr_xvel = 0,corr_yvel = 0;
  18. static float corr_xpos = 0,corr_ypos = 0;

  19. static float acck = 0;

  20. void accel_filter_predict(float dt,float x[3])
  21. {
  22.         x[0] += x[1] * dt + x[2] * dt * dt / 2.0f;
  23.         x[1] += x[2] * dt;
  24. }

  25. void inertial_filter_correct(float e,float dt,float x[3],int i,float w,float vp)
  26. {
  27.   float ewdt = e * w * dt;
  28.         x[i] += ewdt;
  29.         if(i == 0)
  30.         {
  31.                 x[1] += vp*w * ewdt;
  32.         }
  33. }

  34. void check_stop(int16 accz_velocity,int16 accz)
  35. {
  36.         static uint8 slow_down_count=0;
  37.         static uint16 down_time=0;
  38.         down_time++;
  39.         if(fly_status.HOLD_STATUS == HOLD_DOWN)
  40.         {
  41.                 if(down_time>=200)
  42.                 {
  43.                         if((accz_velocity>=0&&accz<50&&accz>-50) || (accz_velocity>=50) || (accz>300 || accz<-300))
  44.                         {
  45.                                 slow_down_count++;
  46.                                 if(slow_down_count>3)
  47.                                 {
  48.                                         fly_status.HOLD_STATUS=HOLD_STOP;
  49.                                 }
  50.                         }
  51.                         else if(slow_down_count>0)
  52.                         {
  53.                                 if(accz_velocity<0)
  54.                                         slow_down_count=0;
  55.                                 else
  56.                                         slow_down_count--;
  57.                         }
  58.                 }
  59.         }
  60.         else
  61.         {
  62.                 down_time=0;
  63.                 slow_down_count=0;
  64.         }
  65. }


  66. void accel_baro_mag_gps_calculator(int16 accel_x,int16 accel_y,int16 accel_z)
  67. {
  68.         static uint8 usart1_count = 0;
  69.        
  70.         static int16 acc_xyz[3];
  71.         static float no_filteraccxx,no_filteraccyy,no_filteracczz;
  72.        
  73.         if (accel_x != 0 && accel_y != 0 && accel_z != 0)
  74.         {
  75.                 acck=(1/Fylib_Qrsqrt(accel_x*accel_x + accel_y*accel_y+accel_z*accel_z)-MV_ACC_RP)/MV_ACC_RP;
  76.                         if(acck<0) acck=-acck;
  77.         }
  78.        
  79.         acc_xyz[0]=accel_x*980/MV_ACC_RP;acc_xyz[1]=accel_y*980/MV_ACC_RP;acc_xyz[2]=accel_z*980/MV_ACC_RP;
  80.         //&Eacute;&Iuml;&Otilde;&yacute;&Iuml;&Acirc;&cedil;&ordm;&pound;&not;±±&Otilde;&yacute;&Auml;&Iuml;&cedil;&ordm;&pound;&not;&para;&laquo;&Otilde;&yacute;&Icirc;÷&cedil;&ordm;
  81.         no_filteracczz=-acc_xyz[1]*vehicle_att.R[2][0]-acc_xyz[0]*vehicle_att.R[2][1]+acc_xyz[2]*vehicle_att.R[2][2]-980;
  82.         no_filteraccxx=-(-acc_xyz[1]*vehicle_optical.R[0][0]-acc_xyz[0]*vehicle_optical.R[0][1]+acc_xyz[2]*vehicle_optical.R[0][2]);
  83.         no_filteraccyy=(acc_xyz[1]*vehicle_optical.R[1][0]+acc_xyz[0]*vehicle_optical.R[1][1]-acc_xyz[2]*vehicle_optical.R[1][2]);
  84.        
  85. //        no_filteracczz += (float)(agx + agy);
  86. //        no_filteraccxx += (float)(agy + agz);
  87. //        no_filteraccyy += (float)(agx + agz);
  88.        
  89.        
  90.         //high_pass
  91.         real_acceleration.xx=(int16)no_filteraccxx;
  92.         real_acceleration.yy=(int16)no_filteraccyy;
  93.         real_acceleration.zz=(int16)no_filteracczz;
  94.        
  95.         TimeUpdate(&baro_acc_alt,(realacc*)&real_acceleration);
  96.        
  97.         check_stop(baro_acc_alt.velocity_hat,no_filteracczz);
  98.        
  99.         usart1_count++;
  100.         if(usart1_count>=1)
  101.         {
  102.                 usart1_count = 0;
  103. //          SendDatShowINT((int)z_est[0],(int)(baro_data.baro_home),(int)(baro_data.velocity*3.1f),(int)baro_to_alt);
  104.         SendDatShow((int)y_est[1],(int)x_est[1],(int)(acck * 100),(int)baro_data.baro_home);
  105.         }
  106.        
  107.        
  108. }

  109. void ahrs_update_R_bf_to_ef(float angle_pitch,float angle_roll,float angle_yaw)
  110. {
  111.         float sin_pitch = sin(angle_pitch * M_DEG_TO_RAD);
  112.         float cos_pitch = cos(angle_pitch * M_DEG_TO_RAD);
  113.         float sin_roll = sin(angle_roll * M_DEG_TO_RAD);
  114.         float cos_roll = cos(angle_roll * M_DEG_TO_RAD);
  115.         float sin_yaw = sin(angle_yaw * M_DEG_TO_RAD);
  116.         float cos_yaw = cos(angle_yaw * M_DEG_TO_RAD);
  117.        
  118.         vehicle_att.R[0][0] = cos_pitch * cos_yaw;
  119.         vehicle_optical.R[0][0]=cos_pitch;
  120.         vehicle_att.R[0][1] = sin_roll * sin_pitch * cos_yaw + cos_roll * sin_yaw;
  121.         vehicle_optical.R[0][1] = 0;//sin_roll * sin_pitch + cos_roll;
  122.         vehicle_att.R[0][2] = cos_roll * sin_pitch * cos_yaw - sin_roll * sin_yaw;
  123.         vehicle_optical.R[0][2] = sin_pitch;//cos_roll * sin_pitch - sin_roll;
  124.        
  125.         vehicle_att.R[1][0] = cos_pitch * sin_yaw;
  126.         vehicle_optical.R[1][0] = 0;//cos_pitch;
  127.         vehicle_att.R[1][1] = -sin_roll * sin_pitch * sin_yaw - cos_roll * cos_yaw;
  128.         vehicle_optical.R[1][1] =cos_roll;// -sin_roll * sin_pitch - cos_roll;
  129.         vehicle_att.R[1][2] = cos_roll * sin_pitch * sin_yaw + sin_roll * cos_yaw;
  130.         vehicle_optical.R[1][2] =-sin_roll;// cos_roll * sin_pitch;// + sin_roll;
  131.        
  132.         vehicle_att.R[2][0] = -sin_pitch;
  133.         vehicle_att.R[2][1] = sin_roll * cos_pitch;
  134.         vehicle_att.R[2][2] = cos_roll * cos_pitch;       
  135.        
  136.        
  137. }

  138. #define HIGH_PASS_PARAM 0.999f
  139. float baro_vel_gain = 0.2f, x_vel_gain = 1.8f, y_vel_gain = 1.8f;

  140. float baro_gain=0.05f;

  141. void TimeUpdate(struct vertical_information *pAltitude,realacc* acc)
  142. {
  143.         static float high_pass_zvel = 0;//, z_est1f = 0;
  144.         z_est[2]=acc->zz-z_bias;
  145.         accel_filter_predict(INAV_T,z_est);
  146.         z_est[0] += corr_baro * w_z_baro * INAV_T;
  147.         z_bias -= corr_baro * 0.001f * INAV_T;
  148.        
  149.        
  150.         z_est[1] += corr_baro * baro_vel_gain * INAV_T;
  151.        
  152. //        if (z_est[1] >-20 && z_est[1] <20)
  153. //          high_pass_zvel = (high_pass_zvel + z_est[1] - z_est1f) * HIGH_PASS_PARAM;
  154. //        else
  155.                 high_pass_zvel = z_est[1];
  156. //        z_est1f = z_est[1];
  157.        
  158.         pAltitude->altitude_hat=z_est[0];
  159.         pAltitude->velocity_hat=high_pass_zvel;
  160.        
  161.         ObservationOPT_Update(x_est[0],optflow.velx,y_est[0],optflow.vely);
  162.        
  163.         x_est[2]=acc->xx-x_bias;
  164.         accel_filter_predict(INAV_T,x_est);
  165.         x_est[1] += corr_xvel * x_vel_gain * INAV_T;
  166.        
  167.         y_est[2]=acc->yy-y_bias;
  168.         accel_filter_predict(INAV_T,y_est);
  169.         y_est[1] += corr_yvel * y_vel_gain * INAV_T;
  170.        
  171.         pos_vel_xy.posx = 0;
  172.         pos_vel_xy.posy = 0;
  173.         pos_vel_xy.velx = optflow.velx;//x_est[1];
  174.         pos_vel_xy.vely = optflow.vely;//y_est[1];
  175. }

  176. __AVERAGE_DATAF altitude_vcal={0,0};

  177. float ABS_f(float values)
  178. {
  179.         if (values < 0 )
  180.                 return -values;
  181.         else
  182.                 return values;
  183. }

  184. void ObservationUpdate(int baro_disdance,int16 baro_vel)
  185. {
  186.         uint8 i;
  187.         corr_baro = (float)baro_disdance - z_est[0];
  188.        
  189.         if (corr_baro >20 || corr_baro <-20)
  190.                 baro_vel_gain = 0.4f;
  191.         else
  192.                 baro_vel_gain = 0.2f;
  193.        
  194.         if (fly_status.HOLD_STATUS == HOLD_STOP)
  195.         {
  196.                 z_est[0] = baro_disdance;
  197.                 z_est[1] = 0;
  198.         }
  199.        
  200.         return;
  201. }

  202. void ObservationOPT_Update(float pos_x,float vel_x,float pos_y,float vel_y)
  203. {
  204.         uint8 i;
  205.         corr_xpos = pos_x-x_est[0];
  206.         corr_xvel = vel_x-x_est[1];//x_vel_history[0]);
  207.         corr_ypos = pos_y-y_est[0];
  208.         corr_yvel = vel_y-y_est[1];//y_vel_history[0]);
  209.        
  210. //        for (i = 0; i < XY_VEL_HISTORY - 1; i++)
  211. //        {
  212. //                x_vel_history[i] = x_vel_history[i+1];
  213. //                y_vel_history[i] = y_vel_history[i+1];
  214. //        }
  215. //        x_vel_history[XY_VEL_HISTORY - 1] = x_est[1];
  216. //        y_vel_history[XY_VEL_HISTORY - 1] = y_est[1];
  217.         return;
  218. }
复制代码


以上代码是我的加速计跟光流,气压计的融合算法
回复

使用道具 举报

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

本版积分规则

QQ|手机版|小黑屋|单片机论坛 |51黑电子论坛单片机 联系QQ:125739409;技术交流QQ群582644647

Powered by 单片机教程网

快速回复 返回顶部 返回列表