基于STM32F407四旋翼无人机 --- 姿态解算讲解（四元数）（叉积法融合陀螺仪数据和加速度数据）（五）

姿态解算

姿态解算定义

在机器人的姿态中是涉及了两个比较关键的坐标系，一个是机器人本身的坐标系，称为机体坐标系，该坐标系会跟随机器人的姿态变化而变化。另一个坐标系为地理坐标系，在看地图的时候我们只有一个固定的方位是东西南北，即该坐标系为东北天坐标系，X轴指向东方向，Y轴指向北方向，Z轴指向天，地理坐标系也可以称为导航坐标系。为什么要介绍这两个坐标系呢？第一是因为在做姿态解算的时候就是需要将机体坐标系和地理坐标系进行相互联系，计算出机体坐标系对于地理坐标系的变化。第二是做惯性导航需要将机体坐标系转到东北天坐标系（地理坐标系）。

这两个坐标系之间的转换其实就是对一个坐标系绕一个轴做旋转，两个坐标系的原点重合。而坐标系的定点转动可以通过一个矩阵来表示，矩阵中的各个元素包含了坐标系的姿态信息，对于机器人来说可以根据该矩阵中的一些元素计算出，横滚角，俯仰角和偏航角。将机体坐标系中的一个向量，左乘这个矩阵之后就可以推出该向量在地理坐标系中的大小。

我们的目的就是根据惯性元件（加速度计、陀螺仪和磁力计）输出的实时的输出数据来计算姿态矩阵，从而可以不断更新横滚、俯仰和偏航角。陀螺仪输出的为角速度。

欧拉角

欧拉角表示法具有简便、几何意义明显的优点，同时姿态敏感器可以直接测出这些参数，但是采用欧拉角的姿态描述方式存在一些问题，且需要好几次的三角函数运算，对于MCU而言，计算很多的三角函数很耗资源，而且计算量很大。而采用四元数表示方法则可以避免这些问题，因此开始采用四元数来描述飞行器的姿态。
根据欧拉定理，坐标系绕固定点的位移也可以是绕该点的若干次有限转动的合成。在欧拉转动中，在三次转动中每次的旋转轴是被转动坐标系的某一坐标轴，每次转动的角为欧拉角。

根据秦永元的《惯性导航》来讲解转换矩阵。

假设坐标系O-X1Y1Z1绕Z轴旋转α角度后得到坐标系O-X2Y2Z2，在O-X1Y1Z1则投影为在坐标系O-X2Y2Z2投影为
由于旋转轴为Z轴，那么Z轴的向量是没有变化的，有rz1 = rz2，根据图可以得出：
将上面的三个式子写成矩阵的形式为：

对于机器人，若机体坐标系向地理坐标系变换的过程可以分为三步。
（1）第一步，将机器人绕Z轴逆时针旋转，最终得到偏航角（YAW），大小为α，则旋转矩阵为：
（2）第二步，将机器人绕Y轴逆时针旋转，最终得到俯仰角（pitch），大小为ψ，则旋转矩阵为：
（3）第三步，将机器人绕X轴逆时针旋转，最终得到横滚角（ROLL），大小为φ，则旋转矩阵为：比如说机体坐标系旋转前向量是a，经过三次旋转后的向量变成b，则此时的机体坐标系到地理坐标系的旋转矩阵为：c = Rx(φ)*Ry(ψ)*Rz(α)

矩阵相乘结果为：
但是用欧拉角矩阵去解算的话会有一些问题，第一就是因为需要大量的三角函数运算，所以对于MCU来说是非常占用资源的，计算量很大，而且会有万向节锁死的问题，对此引入了用四元数去做姿态解算的方法。

四元数

（1）四元数定义：四元数是由四个元构成的数Q（q0,q1,q2,q3） = q0 + q1i + q2j + q3k;其中，q0,q1,q2,q3是实数，i,j,k既是互相正交的单位向量，又是虚单位根号-1。四元数即可看作四维空间中的一个向量，又可以看做一个超复数。对于后续有一个重要的变化需要记住：

四元数性质

（2）四元数的范数：
（3）规范化四元数：||Q|| = 1；
（4）坐标系旋转，坐标系OXYZ按照四元数Q转动，得到新坐标系OX’Y’Z’，设矢量V在两坐标系中的映像分别问Ve、Ve‘，则Ve’ = Q-1 * Ve * Q。

方向余弦矩阵

四元数方向余弦矩阵

一个矢量V相对于坐标系OXYZ固定：V = xi + yj + zk；坐标系OXYZ转动了Q得到一个新坐标系OX’Y’Z’:V = x’i’ + y’j‘ + z’k’；设四元数Ve、Ve‘
Ve = xi + yj + zk;
Ve’ = x’i + y’j + z’k;
则Ve’ = Q-1 * Ve * Q；

设Q = q0 + q1i + q2j + q3k;则Q-1 = q0 - q1i - q2j - q3k;
Ve’=x’i+y’j+z’k=(q0-q1i-q2j-q3k)(0+xi+yj+zk)(q0+q1i+q2j+q3k)；则可以算出
x’=(q0 ^2+q1 ^2-q2 ^2-q3 ^2)x+2(q1q2+ q1q3)y+2(q1q3-q0q2)z
y’ = 2(q1q2-q0q3)x+(q0 ^2-q1 ^2+q2 ^2-q3 ^2)y+2(q2q3+q0q1)z
z’ = 2(q1q3+q0q2)x+2(q2q3-q0q1)y+(q0 ^2-q1 ^2-q2 ^2+q3 ^2)z

那么中间那一堆就是将地理坐标系通过四元素转换为机体坐标系的方向余弦矩阵。如果做惯导的话，那么x’,y’,z’则需要进行逆转换，再次将这三个矩阵相乘得到的方向余弦矩阵就是将机体坐标系转为东北天坐标系，这样不管我们在做惯导的时候如何旋转航向，在经过变换到地理坐标系求出的加速度的方向是不变的。

q0 = 1/2 (1 + c11 + c22 + c33)^1/2;
q1 = 1/4q0 (c32 - c23);
q2 = 1/4q0 (c13 - c31);
q3 = 1/4q0 (c21 - c12);

叉积法融合陀螺仪数据和加速度数据

由于加速度计短时间内数据不算可靠，但是长时间加速度计的数据是可靠的，而陀螺仪数据是由积分得来的，所以长时间的话积分误差会不断的叠加，不断增大，所以就要通过算法融合加速度计和陀螺仪来不断矫正陀螺仪数据。在此使用叉积法去做融合。
世界坐标系重力分向量是通过方向旋转矩阵的最后一列的三个元素乘上加速度就可以算出机体坐标系中的重力向量。

 Gravity.x = 2*(Nq.q1*Nq.q3 - Nq.q0*Nq.q2); //由下向上方向的加速度在加速度计X分量               
 Gravity.y = 2*(Nq.q0*Nq.q1 + Nq.q2*Nq.q3);   //由下向上方向的加速度在加速度计Y分量           
 Gravity.z = 1 - 2*(Nq.q1*Nq.q1 + Nq.q2*Nq.q2);  //由下向上方向的加速度在加速度计Z分量

图1中模型中的小球处于失重状态，坐标系与世界坐标系重合。
图2中坐标系仍与世界坐标系重合，描述为[x,y,z],不同的是小球收到重力作用，模型坐标系中重力向量为[0,0,1],世界坐标系中也是[0,0,1]。
图3中在世界坐标系中与地面夹角为45度，但是坐标系仍是[x,y,z]，重力向量变为了[-0.71,0,-0.71]，注意这个向量是模型坐标系的向量值，转换到世界坐标系仍是[0,0,1]。
所以无论加计怎么旋转重力向量在世界坐标系中都是[0,0,1],但是我们有加速度计读到的值是基于模型坐标系的，也就是我们所说的机体坐标系。现在就明白了，刚才为什么要用四元数将[0，0，1]乘一个旋转矩阵，我们之后关于加速度计和陀螺仪的计算都是基于机体坐标系的。

叉积运算

详细过程可以通过代码来了解：
（1）将加计的三维向量转为单位向量

 NormQuat = my_sqrt(my_pow(Acc.x) + my_pow(Acc.y) + my_pow(Acc.z));   
 //把加计的三维向量转成单位向量。
  Acc.x = Acc.x / NormQuat;
  Acc.y = Acc.y / NormQuat;
  Acc.z = Acc.z / NormQuat;

（2）叉积运算开始。

   /* 叉乘得到误差 */
   why_err_Acc.x = Acc.y*Gravity.z - Acc.z*Gravity.y;
   why_err_Acc.y = Acc.z*Gravity.x - Acc.x*Gravity.z;
   why_err_Acc.z = Acc.x * Gravity.y - Acc.y * Gravity.x;

（3）在对叉乘得到的误差进行低通滤波,其中的HalfTime 表示在姿态解算中运行周期的一半。

/* 误差低通 */
   ref.err_lpf.x += 6.28f *HalfTime *( why_err_Acc.x  - ref.err_lpf.x );
   ref.err_lpf.y += 6.28f *HalfTime *( why_err_Acc.y  - ref.err_lpf.y );
  ref.err_lpf.z += 6.28f *HalfTime *( why_err_Acc.z  - ref.err_lpf.z );

（4）开始对误差进行积分运算，并进行积分限幅，在进行kp控制

#define IMU_INTEGRAL_LIM  0.034906585f
#define ANGLE_TO_RADIAN 0.017453293f

ref.err_Int.x += ref.err.x *Ki *2 *HalfTime ;
 ref.err_Int.y += ref.err.y *Ki *2 *HalfTime ;
 ref.err_Int.z += ref.err.z *Ki *2 *HalfTime ;
/* 积分限幅 */
 ref.err_Int.x = LIMIT(ref.err_Int.x, - IMU_INTEGRAL_LIM ,IMU_INTEGRAL_LIM );
 ref.err_Int.y = LIMIT(ref.err_Int.y, - IMU_INTEGRAL_LIM ,IMU_INTEGRAL_LIM );
 ref.err_Int.z = LIMIT(ref.err_Int.z, - IMU_INTEGRAL_LIM ,IMU_INTEGRAL_LIM );
 /***************与上式构成PI控制******************/
 ref.g.x = (Gyro.x - Gravity.x * yaw_correct ) *ANGLE_TO_RADIAN + ( Kp*(ref.err.x + ref.err_Int.x) ) ;  
 ref.g.y = (Gyro.y - Gravity.y * yaw_correct ) *ANGLE_TO_RADIAN + ( Kp*(ref.err.y + ref.err_Int.y) ) ;    
 ref.g.z = (Gyro.z - Gravity.z * yaw_correct ) *ANGLE_TO_RADIAN;

一阶龙格库塔法

已知一四元数:
对时间微分得:

令则有
由一阶龙格库塔法
，则可以推导出：

四元数更新

 // 一阶龙格库塔法, 更新四元数
   //矫正后的角速度值积分，得到两次姿态解算中四元数一个实部Q0，三个虚部Q1~3的值的变化
 Nq.q0 = Nq.q0 +(-Nq.q1*ref.g.x - Nq.q2*ref.g.y - Nq.q3*ref.g.z)*HalfTime;
 Nq.q1 = Nq.q1 + (Nq.q0*ref.g.x + Nq.q2*ref.g.z - Nq.q3*ref.g.y)*HalfTime;
 Nq.q2 = Nq.q2 + (Nq.q0*ref.g.y - Nq.q1*ref.g.z + Nq.q3*ref.g.x)*HalfTime;
 Nq.q3 = Nq.q3 + (Nq.q0*ref.g.z + Nq.q1*ref.g.y - Nq.q2*ref.g.x)*HalfTime; 
 /* 四元数规一化 normalise quaternion */
 NormQuat = my_sqrt(my_pow(Nq.q0) + my_pow(Nq.q1) + my_pow(Nq.q2) + my_pow(Nq.q3));
 
 Nq.q0 = Nq.q0 / NormQuat;
 Nq.q1 = Nq.q1 / NormQuat;
 Nq.q2 = Nq.q2 / NormQuat;
 Nq.q3 = Nq.q3 / NormQuat;

获得欧拉角

四元数转换为方向余弦矩阵的关键元素：

/**********四元数的旋转矩阵转换***************/
static void IMU_ComputeRotationMatrix(void)
{
 Eln_angle[0] = 2*(Nq.q0*Nq.q1 + Nq.q2*Nq.q3);
 Eln_angle[1] = 1 - 2*(Nq.q1*Nq.q1 + Nq.q2*Nq.q2);
 Eln_angle[2] = 2*(Nq.q1*Nq.q3 - Nq.q0*Nq.q2);
 Eln_angle[3] = 2*(-Nq.q1*Nq.q2 - Nq.q0*Nq.q3);
 Eln_angle[4] = 2*(Nq.q0*Nq.q0 + Nq.q1*Nq.q1) - 1;
}

/***********获得欧拉角**********/
void Get_Angle(void)
{
 IMU_ComputeRotationMatrix(); //四元数转旋转矩阵
 
 IMU_data.Pitch = asin(Eln_angle[2])*DEG_PER_RAD;
 IMU_data.Roll= fast_atan2(Eln_angle[0],Eln_angle[1])*DEG_PER_RAD;
 IMU_data.Yaw = fast_atan2(Eln_angle[3],Eln_angle[4])*DEG_PER_RAD;
}

最终得到横滚、俯仰和偏航角。但是需要注意的有一点，如果单独依靠陀螺仪和加速度计解算出来的姿态横滚和俯仰是没有问题的，但是航向是一直在漂移的，这个是因为解算出来就会漂移的，所以一般都使用磁力计去限航。通过之前的博客我们获取的磁力计数据。

/*磁力计限制偏航函数*/
void Mag_Limit(float DT)
{
  static xyz_t Mag;
  //计算磁力计向量的模 
  Mag_norm_xyz = my_sqrt(my_pow(WHY.MAGX) + my_pow(WHY.MAGY) + my_pow(WHY.MAGZ));
    
   if( Mag_norm_xyz != 0)
 {
  Mag.x += 20 *(6.28f *DT) *( (float)WHY.MAGX /( Mag_norm_xyz ) - Mag.x);
  Mag.y += 20 *(6.28f *DT) *( (float)WHY.MAGY /( Mag_norm_xyz ) - Mag.y); 
  Mag.z += 20 *(6.28f *DT) *( (float)WHY.MAGZ /( Mag_norm_xyz ) - Mag.z); 
 }
 mag_trans(&Gravity,&Mag,&Mag_process);
 Mag_norm = my_sqrt(Mag_process.x * Mag_process.x + Mag_process.y *Mag_process.y);
 if( Mag_process.x != 0 && Mag_process.y != 0 && Mag_process.z != 0 && Mag_norm != 0)
 {
  yaw_mag = fast_atan2( ( Mag_process.y/Mag_norm ) , ( Mag_process.x/Mag_norm) ) * DEG_PER_RAD;
 } 
 yaw_correct = Kp *0.8f *To_180_degrees(yaw_mag - IMU_data.Yaw);
 }

有很多的上位机可以通过你传输的欧拉角去看此时机器人的姿态，比如匿名的上位机，有能力的也可以自己写一个上位机，按照通信协议将所需显示的数据在PC端进行显示。

姿态解算C代码