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

加速计、气压计、GPS数据融合

在无人机控制当中，传感器的参与那是必不可少的，特别是陀螺仪，最经典的为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[i] = x_vel_history[i+1];
  y_vel_history[i] = y_vel_history[i+1];
  
  x_pos_history[i] = x_pos_history[i+1];
  y_pos_history[i] = 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。无人机四轴本人做了五年，以上代码全部自己写，上传的没来的及添加注释，但是代码时成功在飞机上跑的并且无人机出货的。