无人机姿态解算代码讲解
姿态解算,对于无人机飞控算法来说极其重要,无人机的自稳飞行以及后面的一系列的高度数据融合,GPS数据融合、光流数据融合等都会有影响,所以理解好姿态解算这个算法还是很有必要的。
关于太多的理论的知识已经有很多的网上的解说,这次我写的是我这几年对姿态结算的代码的理解,怎么去调参数,哪些代码起着什么样的作用。
先上个姿态解算的结构框图:
姿态解算输入数据为三轴加速计,三轴陀螺仪。输出数据为欧拉角。
void IMUupdate(float gxi,float gyi,float gzi,float axi,float ayi,float azi,__Euler *euler)//姿态解算输入数据为三轴加速计,三轴陀螺仪。
{
float recipNormmag=0.0f;
float halfvx=0.0f, halfvy=0.0f, halfvz=0.0f;
float halfex=0.0f, halfey=0.0f, halfez=0.0f;
float qa=0.0f, qb=0.0f, qc=0.0f;
float q0q0 = q0*q0;
float q0q1 = q0*q1;
float q0q2 = q0*q2;
float q0q3 = q0*q3;
float q1q1 = q1*q1;
float q1q2 = q1*q2;
float q1q3 = q1*q3;
float q2q2 = q2*q2;
float q2q3 = q2*q3;
float q3q3 = q3*q3;
static int last_time=0;int now_time=0;
//前面都是算法的变量定义
if(euler==NULL)
return;
now_time=SystemGetMirco();
if(last_time!=0)
{
sampleFreq=1000000.0f/(float)(SystemGetMircoTim());
//定义姿态解算时间周期,算法运行时间跟实际时间必须一致,否则算法会出错
}
last_time=now_time;
ax=axi;ay=ayi;az=azi;gx=gxi;gy=gyi;gz=gzi;
// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f)))
//
{
// Normalise accelerometer measurement
//加速计做归一化,确保计算的数据比例一样
recipNormmag = Fylib_Qrsqrt(ax * ax + ay * ay + az * az);
ax *= recipNormmag;
ay *= recipNormmag;
az *= recipNormmag;
// Estimated direction of gravity and vector perpendicular to magnetic flux
halfvx = 2*(q1 * q3 - q0 * q2);
halfvy = 2*(q0 * q1 + q2 * q3);
halfvz = q0*q0 - q1*q1 - q2*q2 + q3*q3;
//计算各轴XYZ在重力方向上的分量,后面互补滤波需要用到这个数据
// Error is sum of cross product between estimated and measured direction of gravity
halfex = (ay * halfvz - az * halfvy);// + Mag_ex * _w_mag;
halfey = (az * halfvx - ax * halfvz);// + Mag_ey * _w_mag;
halfez = (ax * halfvy - ay * halfvx);// + Mag_ez * _w_mag;//(Mag_ez - corrrect_values1) * _w_mag;
//各轴分量与加速计在重力下各轴分量之差,这个是互补滤波的核心,halfvx 是估计分量误差,acc x y z是加速计分量,不一样的角度加速计在重力作用下的分量是固定的,所以互补滤波以此作为绝对参数数据
integralFBx += twoKi * halfex * (1.0f / sampleFreq); // integral error scaled by Ki
integralFBy += twoKi * halfey * (1.0f / sampleFreq);
integralFBz += twoKi * halfez * (1.0f / sampleFreq);
//互补滤波KI分量,KI参数可调,会影响更相信加速计或陀螺仪
gx += integralFBx; // apply integral feedback
gy += integralFBy;
gz += integralFBz;
//KI分量加入欧拉角补偿
// Apply proportional feedback
gx += twoKp * halfex;
gy += twoKp * halfey;
gz += twoKp * halfez;
互补滤波KP分量加入补偿
qa = q0;
qb = q1;
qc = q2;
q0 += (-qb * gx - qc * gy - q3 * gz);
q1 += (qa * gx + qc * gz - q3 * gy);
q2 += (qa * gy - qb * gz + q3 * gx);
q3 += (qa * gz + qb * gy - qc * gx);
四元素更新
// Normalise quaternion
recipNormmag = Fylib_Qrsqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
q0 *= recipNormmag;
q1 *= recipNormmag;
q2 *= recipNormmag;
q3 *= recipNormmag;
//四元素归一化
euler->pitch_x = atan2(2*q0*q1 + 2*q2*q3, 1 -2*q1*q1 - 2*q2*q2)* MV_RAD_DEG;//pitch
euler->roll_y = asin(2*q0*q2 - 2*q1*q3)*MV_RAD_DEG;//roll
euler->yaw_z = atan2(2*q0*q3+ 2*q1*q2 , 1 -2*q2*q2 - 2*q3*q3)*MV_RAD_DEG + 270;// - Gps_declination;//yaw
//用四元素来代表欧拉角,这个公式很多论文都有推导方法,看懂其推倒也是需要一定的数学功底
}
以上为传统的四元素互补滤波解算姿态角,用的过程主要调试KP KI两个分量。另外加速计跟陀螺仪在时间上会有一定的反应时间差,所以加速计陀螺仪理论上是要加时间延迟缓冲的。熟悉使用这个姿态解算,不仅在无人机可用,其他很多需要平衡类的产品均可用。关注我,我会持续更新有关无人机各个模块的知识,带你完整的熟悉无人机各个模块。技术交流也可以加我扣扣1836703877.