无人机姿态解算代码讲解

无人机姿态解算代码讲解

姿态解算，对于无人机飞控算法来说极其重要，无人机的自稳飞行以及后面的一系列的高度数据融合，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.