STM32F103之实验7一阶互补滤波求解移动机器人的姿态代码

上一篇博客，采用的是DMP解算姿态，且给出了底层硬件配置及驱动方式，此处摒弃DMP直接由单片机自己实现姿态解算，接下来介绍由三轴陀螺仪和加速度计的值来使用四元数软件解算姿态的方法。

我们先来看看如何用欧拉角描述一次平面旋转(坐标变换)：

 

设坐标系绕旋转α角后得到坐标系，在空间中有一个矢量在坐标系中的投影为，在内的投影为由于旋转绕进行，所以Z坐标未变，即有。

转换成矩阵形式表示为：

整理一下：

所以从旋转到可以写成上面仅仅是绕一根轴的旋转，如果三维空间中的欧拉角旋转要转三次:

上面得到了一个表示旋转的方向余弦矩阵。

不过要想用欧拉角解算姿态，其实我们套用欧拉角微分方程就行了：

上式中左侧，是本次更新后的欧拉角，对应row、pit、yaw。右侧，是上个周期测算出来的角度，三个角速度由直接安装在四轴飞行器的三轴陀螺仪在这个周期转动的角度，单位为弧度，计算间隔时T陀螺角速度，比如0.02秒0.01弧度/秒=0.0002弧度。间因此求解这个微分方程就能解算出当前的欧拉角。

虽然欧拉角求解姿态表达清晰，但是欧拉角微分方程中包含了大量的三角运算，不利于实时运算。而且当俯仰角为90度时方程式会万向锁问题。所以欧拉角方法只适用于水平姿态变化不大的情况，而不适用于全姿态飞行器的姿态确定。

四元数法只求解四个未知量的线性微分方程组，计算量小，易于操作，是比较实用的工程方法。

我们知道在平面(x,y)中的旋转可以用复数来表示，同样的三维中的旋转可以用单位四元数来描述。我们来定义一个四元数：

四元数可以表示一个完整的旋转。只有单位四元数才可以表示旋转，因为这就是四元数表示旋转的约束条件。

而刚才用欧拉角描述的方向余弦矩阵用四元数描述则为：

所以在软件解算中，我们要首先把加速度计采集到的值(三维向量)转化为单位向量，即向量除以模，传入参数是陀螺仪x、y、z值和加速度计x、y、z值：

void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az) 

{ float norm; 

float vx, vy, vz; float ex, ey, ez;           

norm = sqrt(ax*ax + ay*ay + az*az);       

ax = ax / norm; 

ay = ay / norm; 

az = az / norm;  

下面把四元数换算成方向余弦中的第三行的三个元素。刚好vx、vy、vz 。其实就是上一次的欧拉角（四元数）的机体坐标参考系换算出来的重力的单位向量。

estimated direction of gravity vx = 2*(q1*q3 - q0*q2);

vy = 2*(q0*q1 + q2*q3); 

vz = q0*q0 - q1*q1 - q2*q2 + q3*q3; 

axyz是机体坐标参照系上，加速度计测出来的重力向量，也就是实际测出来的重力向量。

axyz是测量得到的重力向量，vxyz是陀螺积分后的姿态来推算出的重力向量，它们都是机体坐标参照系上的重力向量。

那它们之间的误差向量，就是陀螺积分后的姿态和加计测出来的姿态之间的误差。

向量间的误差，可以用向量叉积（也叫向量外积、叉乘）来表示，exyz就是两个重力向量的叉积。

这个叉积向量仍旧是位于机体坐标系上的，而陀螺积分误差也是在机体坐标系，而且叉积的大小与陀螺积分误差成正比，正好拿来纠正陀螺。（你可以自己拿东西想象一下）由于陀螺是对机体直接积分，所以对陀螺的纠正量会直接体现在对机体坐标系的纠正。

integral error scaled integral gain exInt = exInt + ex*Ki;

eyInt = eyInt + ey*Ki; 

ezInt = ezInt + ez*Ki; 

用叉积误差来做PI修正陀螺零偏

integral error scaled integral gain exInt = exInt + ex*Ki; 

eyInt = eyInt + ey*Ki; 

ezInt = ezInt + ez*Ki;         // adjusted gyroscope measurements 

gx = gx + Kp*ex + exInt; 

gy = gy + Kp*ey + eyInt; 

gz = gz + Kp*ez + ezInt; 

四元数微分方程，其中T为测量周期，为陀螺仪角速度，都是已知量，使用了一阶龙哥库塔求解四元数微分方程：

integrate quaternion rate and normalise 

q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT; 

q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT; 

q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT; 

q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;   

最后根据四元数方向余弦阵和欧拉角的转换关系，把四元数转换成欧拉角：

 

最终：

ANGLE.Yaw = atan2(2 * q1 * q2 + 2 * q0 * q3, -2 * q2*q2 - 2 * q3* q3 + 1)* 57.3; // yaw 

ANGLE.Y= asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch 

ANGLE.X= atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll

上述过程如下图所示：

接下来采用一节互补滤波求解姿态：

 

#defineKp 10.0f                        // 这里的KpKi是用于调整加速度计修正陀螺仪的速度
#defineKi 0.008f                        
#definehalfT 0.001f             // 采样周期的一半，用于求解四元数微分方程时计算角增量
floatq0 = 1, q1 = 0, q2 = 0, q3 = 0;    // 初始姿态四元数，由上篇博文提到的变换四元数公式得来
floatexInt = 0, eyInt = 0, ezInt = 0;    //当前加计测得的重力加速度在三轴上的分量
                                //与用当前姿态计算得来的重力在三轴上的分量的误差的积分
voidIMUupdate(float gx, float gy, float gz, float ax, float ay, float az)//g表陀螺仪，a表加计
{

  float q0temp,q1temp,q2temp,q3temp;//四元数暂存变量，求解微分方程时要用
  float norm; //矢量的模或四元数的范数
  float vx, vy, vz;//当前姿态计算得来的重力在三轴上的分量
  float ex, ey, ez;//当前加计测得的重力加速度在三轴上的分量
              //与用当前姿态计算得来的重力在三轴上的分量的误差

  // 
  float q0q0 = q0*q0;
  float q0q1 = q0*q1;
  float q0q2 = q0*q2;
  float q1q1 = q1*q1;
  float q1q3 = q1*q3;
  float q2q2 = q2*q2;
  float q2q3 = q2*q3;
  float q3q3 = q3*q3;      
  if(ax*ay*az==0)//加计处于自由落体状态时不进行姿态解算，因为会产生分母无穷大的情况
        return;
  norm = sqrt(ax*ax + ay*ay + az*az);//单位化加速度计，
  ax = ax /norm;// 归一化了
  ay = ay / norm;
  az = az / norm;
  //用当前姿态计算出重力在三个轴上的分量，
  //参考坐标n系转化到载体坐标b系的用四元数表示的方向余弦矩阵第三列即是
  vx = 2*(q1q3 - q0q2);        
  vy = 2*(q0q1 + q2q3);
  vz = q0q0 - q1q1 - q2q2 + q3q3 ;
  //计算测得的重力与计算得重力间的误差，向量外积可以表示这一误差

  ex = (ay*vz - az*vy) ;                                                                  
  ey = (az*vx - ax*vz) ;
  ez = (ax*vy - ay*vx) ;

  exInt = exInt + ex * Ki;                                           //对误差进行积分
  eyInt = eyInt + ey * Ki;
  ezInt = ezInt + ez * Ki;
  // adjusted gyroscope measurements
  gx = gx + Kp*ex + exInt;  //将误差PI后补偿到陀螺仪，即补偿零点漂移
  gy = gy + Kp*ey + eyInt;
  gz = gz + Kp*ez + ezInt;    //这里的gz由于没有观测者进行矫正会产生漂移，表现出来的就是积分自增或自减。

  //下面进行姿态的更新，也就是四元数微分方程的求解
  q0temp=q0;//暂存当前值用于计算
  q1temp=q1;//网上传的这份算法大多没有注意这个问题，在此更正
  q2temp=q2;
  q3temp=q3;
  //采用龙格库塔法，得到新的四元数
  q0 = q0temp + (-q1temp*gx - q2temp*gy -q3temp*gz)*halfT;
  q1 = q1temp + (q0temp*gx + q2temp*gz -q3temp*gy)*halfT;
  q2 = q2temp + (q0temp*gy - q1temp*gz +q3temp*gx)*halfT;
  q3 = q3temp + (q0temp*gz + q1temp*gy -q2temp*gx)*halfT;
  //单位化四元数在空间旋转时不会拉伸，仅有旋转角度
  norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
  q0 = q0 / norm;
  q1 = q1 / norm;
  q2 = q2 / norm;
  q3 = q3 / norm;
  //四元数到欧拉角的转换
  //其中YAW航向角由于加速度计对其没有修正作用，下次用磁力计整合，此处不管，用陀螺仪积分代替。
  Q_ANGLE.Z = GYRO_I.Z; // yaw
  Q_ANGLE.Y = asin(-2 * q1 * q3 + 2 * q0* q2)*57.3; // pitch
  Q_ANGLE.X = atan2(2 * q2 * q3 + 2 * q0 * q1,-2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll
}