加速计、气压计、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。无人机四轴本人做了五年,以上代码全部自己写,上传的没来的及添加注释,但是代码时成功在飞机上跑的并且无人机出货的。
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)