ANO匿名飞控分析（3）— 姿态解算原理及代码分析

• 准备电赛，简单写一下匿名飞控的分析
• 基于TM4C主控的匿名拓空者飞控，介绍见匿名科创–匿名拓空者PRO—TI版全开源飞控使用入门—TM4C123

---

一、姿态解算原理相关

1、简介

• 对于四旋翼无人机来说，有两个相关坐标系，即地理坐标系$O_n{X}_n{Y}_n{Z}_n$和机体坐标系$O_b{X}_b{Y}_b{Z}_b$。
• 姿态角的检测对于四旋翼控制是至关重要的。比如在做飞行器姿态控制的时候，通常使用串级pid的方法，需要得到飞行器姿态角（即欧拉角）来做角度位置环反馈。再如利用装置在机腹垂直向下拍摄的摄像头，进行机体视觉定位时，也需要靠姿态角进行视觉反馈补偿。
• 要解算姿态角，就要研究从地理坐标系到机体坐标系的转换过程。这个转换过程不是唯一的，比如可以先绕X轴旋转$\theta$，再绕Y轴旋转$\gamma$，最后绕Z轴旋转$\psi$，这样得到的一组姿态角称为卡尔丹角；也可以按ZYX的顺序旋转，这样得到的一组姿态角称为欧拉角（这是最常用的，pitch,roll,yaw）。不管按照什么顺序，得到的角度都可以称作广义欧拉角。实际理论分析时，旋转顺序不是很重要，这个顺序会影响四元数与欧拉角的关系，但是都可以进行解算。

2、坐标变换和旋转矩阵

下面从最基本的坐标变换入手开始讲解，请看如下坐标系旋转：$O_A{X}_A{Y}_A{Z}_A$绕OX旋转$\theta$到$O_B{X}_B{Y}_B{Z}_B$

对于原系$O_A{X}_A{Y}_A{Z}_A$中的一个向量$[r_{x_A},r_{y_A},r_{z_A}]$，转换到新系$O_B{X}_B{Y}_B{Z}_B$中的向量$[r_{x_B},r_{y_B},r_{z_B}]$，有：

$$\begin{gathered} r_{x_B}=r_{x_A} \\ {r}_{y_B}=cos(\theta )r_{y_A}+sin(\theta )r_{z_A} \\ {r}_{z_B}=-sin(\theta )r_{y_A}+cos(\theta )r_{z_A} \end{gathered}$$

可以用旋转矩阵表示

$$\left[\begin{array}{c}{r}_{x_B}\\ r_{y_B}\\ r_{z_B}\end{array}\right]=\left[\begin{array}{ccc}1& 0& 0\\ 0& cos(\theta )& sin(\theta )\\ 0& -sin(\theta )& cos(\theta )\end{array}\right]\left[\begin{array}{c}{r}_{x_A}\\ r_{y_A}\\ r_{z_A}\end{array}\right]$$

• 中间的就是 绕x轴旋转$\theta$ 的旋转矩阵，记作
  $$C_{x_{(}\theta )}=\left[\begin{array}{ccc}1& 0& 0\\ 0& cos(\theta )& sin(\theta )\\ 0& -sin(\theta )& cos(\theta )\end{array}\right]$$

• 同样的，还有 绕y轴旋转$\gamma$ 的旋转矩阵，记作
  $$C_{y_{(}\gamma )}=\left[\begin{array}{ccc}cos(\gamma )& 0& -sin(\gamma )\\ 0& 1& 0\\ sin(\gamma )& 0& cos(\gamma )\end{array}\right]$$

• 同样的，还有 绕z轴旋转$\psi$ 的旋转矩阵，记作
  $$C_{z_{(}\psi )}=\left[\begin{array}{ccc}cos(\psi )& -sin(\psi )& 0\\ sin(\psi )& cos(\psi )& 0\\ 0& 0& 1\end{array}\right]$$

• 假如现在我们按ZXY的顺序，从地理坐标系$O_n{X}_n{Y}_n{Z}_n$旋转到机体坐标系$O_b{X}_b{Y}_b{Z}_b$，整体旋转矩阵为
  $$C_n^b=C_{y_{(}\gamma )}{C}_{x_{(}\theta )}{C}_{z_{(}\psi )}=\left[\begin{array}{ccc}cos(\gamma )cos(\psi )+sin(\gamma )sin(\psi )sin(\theta )& -cos(\gamma )sin(\psi )+sin(\gamma )cos(\psi )cos(\theta )& -sin(\psi )cos(\theta )\\ sin(\psi )cos(\theta )& cos(\psi )cos(\theta )& sin(\theta )\\ sin(\gamma )cos(\psi )-cos(\gamma )sin(\psi )sin(\theta )& -sin(\gamma )sin(\psi )-cos(\gamma )cos(\psi )sin(\theta )& cos(\gamma )cos(\theta )\end{array}\right]$$

• 注意一点，如果从n系到b系的旋转矩阵为$C_n^b$，则从b系到n系的旋转矩阵为${C_n^b}^T$，显然有$C_n^b\ast {C_n^b}^T=E$，所以旋转矩阵是正交矩阵

• 这个结论不能直接使用，一方面这个直接是不可解的，另一方面大量三角函数运算会导致大量资源占用。

3、四元数

• 四元数是一种超复数，由四个元构成：$Q(q_0,q_1,q_2,q_3)=q_0+q_{1}i+q_{2}i+q_{3}k$
• 关于四元数更详细的介绍可以参考四元数
• 利用四元数我们可以提出另一种描述空间矩阵的方法，具体可以参考秦永元的《惯性导航》和【教程】四旋翼飞行器姿态解算算法入门学习-Rick Grimes

（1）四元数和欧拉角的关系

• 注意这三个式子中，$\theta$是绕Y轴转角，$\varphi$是绕X轴转角，$\psi$是绕Z轴转角，和前文有所不同

1. 按ZYX的顺序从地理坐标系$O_e{X}_e{Y}_e{Z}_e$旋转到机体坐标系$O_b{X}_b{Y}_b{Z}_b$，整体旋转矩阵为
   
2. 对应的四元数表示法为：
   
3. 解算出的欧拉角为
   

（2）四元数的求解

• 飞行器姿态的改变，可以对应到旋转矩阵的改变，进一步对应到四元数的改变。实时姿态计算，实际上也就是实时更新四元数。
• 我们可以构建四元数关于时间的微分方程，来研究四元数关于时间的变化规律，求解该微分方程，即可解出四元数

1、建立微分方程

• 从四元数的三角表示法入手推到微分方程，具体推到过程可以参考秦永元的《惯性导航》
  $$Q=\frac{cos(\theta )}{2}+\frac{\lambda sin(\theta )}{2}$$$$\frac{dQ}{dt}=1/2\left[\begin{array}{cccc}0& -{\omega }_x& -{\omega }_y& -{\omega }_x\\ {\omega }_x& 0& {\omega }_z& -{\omega }_y\\ {\omega }_y& -{\omega }_z& 0& {\omega }_x\\ {\omega }_z& {\omega }_y& -{\omega }_x& 0\end{array}\right]\cdot \left[\begin{array}{c}{q}_0\\ q_1\\ q_2\\ q_3\end{array}\right]$$$$记为\frac{dQ}{dt}=\Phi \cdot Q$$

2、一阶龙格库塔法求解微分方程

一阶龙格库塔法是求解微分方程常用的工程方法，原理是把微分转化为微元增量，利用递推迭代的方法求解

• 设有微分方程$\frac{dy}{dx}=f(x,y)$

  求解y的迭代公式为$y(\lambda +\Delta \lambda )=y(\lambda )+\Delta \lambda \cdot f(x(\lambda ),y(\lambda ))$

• 对应到四元数微分方程：
  $$Q(t-\Delta t)=Q(t)+\Delta t\cdot \Phi (t)\cdot Q(t)$$
  整理得
  $${\left[\begin{array}{c}{q}_0\\ q_1\\ q_2\\ q_3\end{array}\right]}_{t+\Delta t}={\left[\begin{array}{c}{q}_0\\ q_1\\ q_2\\ q_3\end{array}\right]}_t+\frac{1}{2}\cdot \Delta t\cdot \left[\begin{array}{c}-{\omega }_x\cdot q_1-{\omega }_y\cdot q_2-{\omega }_z\cdot q_3\\ {\omega }_x\cdot q_0-{\omega }_y\cdot q_3+{\omega }_z\cdot q_2\\ {\omega }_x\cdot q_3+{\omega }_y\cdot q_0-{\omega }_z\cdot q_1\\ -{\omega }_x\cdot q_2+{\omega }_y\cdot q_1+{\omega }_z\cdot q_0\end{array}\right]$$
  这里$\omega$就是三轴角速度，可以用陀螺仪直接测得

4、旋转矩阵中的一列

• 旋转矩阵中的一列有特殊意义，它代表原坐标系中一个方向的单位向量（X/Y/Z），在新坐标系中对应的向量值
• 用方向向量（如X方向$[1,0,0{]}^T$）左乘旋转矩阵可以轻易看出，这里不写了

二、匿名四轴姿态解算分析

按Ano_Imu.c文件顺序进行分析

1、坐标系定义

• 匿名四轴姿态解算部分主要写在Ano_Imu.c中
• 匿名四轴程序中一共有三个坐标系：

1. 地理坐标系，标记为w（官方定义如下）
2. 机体坐标系，标记为a（官方定义如下）
   
3. 航向坐标系，标记为h（专门解释一下，四旋翼飞行时，姿态角pitch和roll一般很小，在任意时刻，如果我们粗略地认为飞行器始终与底面平行（把飞机摆正，使pitch=roll=0），此时的机体坐标系即为航向坐标系）

2、坐标系转换

（1）匿名旋转坐标系

• 匿名的坐标系变换，是按是按ZYX顺序从地理坐标系转到机体坐标系的，这个旋转矩阵是前面提过的 (注意这三个式子中，$\theta$是绕Y轴转角，$\varphi$是绕X轴转角，$\psi$是绕Z轴转角)

$$C_w^a=\left[\begin{array}{ccc}cos(\theta )cos(\psi )& sin(\psi )cos(\theta )& -sin(\theta )\\ sin(\varphi )sin(\theta )cos(\psi )-cos(\varphi )sin(\psi )& sin(\varphi )sin(\theta )sin(\psi )+cos(\varphi )cos(\psi )& sin(\varphi )cos(\theta )\\ cos(\varphi )sin(\theta )cos(\psi )+sin(\varphi )sin(\psi )& cos(\varphi )sin(\theta )sin(\psi )-sin(\varphi )cos(\psi )& cos(\varphi )cos(\theta )\end{array}\right]$$

• 程序中用二维数组att_matrix表示从机体坐标系转到地理坐标系的旋转矩阵，是上面那个的转置
  $$att\_matrix=C_a^w=(C_w^a{)}^T=\left[\begin{array}{ccc}cos(\theta )cos(\psi )& sin(\varphi )sin(\theta )cos(\psi )-cos(\varphi )sin(\psi )& cos(\varphi )sin(\theta )cos(\psi )+sin(\varphi )sin(\psi )\\ sin(\psi )cos(\theta )& sin(\varphi )sin(\theta )sin(\psi )+cos(\varphi )cos(\psi )& cos(\varphi )sin(\theta )sin(\psi )-sin(\varphi )cos(\psi )\\ -sin(\theta )& sin(\varphi )cos(\theta )& cos(\varphi )cos(\theta )\end{array}\right]$$
• 注意这里绕Y轴转角$\theta$ (pitch)，绕X轴转角$\varphi$ (roll)都是很小的角

（2）相关代码分析

#include "Ano_Imu.h"
#include "Ano_Math.h"
#include "Ano_Filter.h"

//#include "ANO_RC.h"

/*参考坐标，定义为ANO坐标*

俯视，机头方向为x正方向
     +x
     |
 +y--|--
     |
		 
*/	

//世界坐标平面XY转平面航向坐标XY
void w2h_2d_trans(float w[VEC_XYZ],float ref_ax[VEC_XYZ],float h[VEC_XYZ])
{
     
	h[X] =  w[X] *  ref_ax[X]  + w[Y] *ref_ax[Y];
	h[Y] =  w[X] *(-ref_ax[Y]) + w[Y] *ref_ax[X];
	
}

//平面航向坐标XY转世界坐标平面XY
void h2w_2d_trans(float h[VEC_XYZ],float ref_ax[VEC_XYZ],float w[VEC_XYZ])
{
     
	w[X] = h[X] *ref_ax[X] + h[Y] *(-ref_ax[Y]);
	w[Y] = h[X] *ref_ax[Y] + h[Y] *  ref_ax[X];
}

//载体坐标转世界坐标（ANO约定等同与地理坐标）
float att_matrix[3][3]; //必须由姿态解算算出该矩阵
void a2w_3d_trans(float a[VEC_XYZ],float w[VEC_XYZ])
{
     
		for(u8 i = 0;i<3;i++)
		{
     
			float temp = 0;
			for(u8 j = 0;j<3;j++)
			{
     
				temp += a[j] *att_matrix[i][j];
			}
			w[i] = temp;
		}
}

• 这是Ano_Imu.c文件的起始部分，提供了三个坐标系转换函数，可以实现w系和h系XY方向向量互转，以及从a系到w系三轴方向向量转换
• 这里实际上是三个旋转矩阵运算，分别为

1. void w2h_2d_trans(float w[VEC_XYZ],float ref_ax[VEC_XYZ],float h[VEC_XYZ])

$$\left[\begin{array}{c}h[X]\\ h[Y]\end{array}\right]=\left[\begin{array}{cc}ref\_ax[X]& ref\_ax[Y]\\ -ref\_ax[Y]& ref\_ax[X]\end{array}\right]\left[\begin{array}{c}w[X]\\ w[Y]\end{array}\right]$$

2. void h2w_2d_trans(float h[VEC_XYZ],float ref_ax[VEC_XYZ],float w[VEC_XYZ])

$$\left[\begin{array}{c}w[X]\\ w[Y]\end{array}\right]=\left[\begin{array}{cc}ref\_ax[X]& -ref\_ax[Y]\\ ref\_ax[Y]& ref\_ax[X]\end{array}\right]\left[\begin{array}{c}h[X]\\ h[Y]\end{array}\right]$$

3. void a2w_3d_trans(float a[VEC_XYZ],float w[VEC_XYZ])

$$\left[\begin{array}{c}w[X]\\ w[Y]\\ w[Z]\end{array}\right]=\left[\begin{array}{ccc}att\_matrix[0][0]& att\_matrix[0][1]& att\_matrix[0][2]\\ att\_matrix[1][0]& att\_matrix[1][1]& att\_matrix[1][2]\\ att\_matrix[2][0]& att\_matrix[2][1]& att\_matrix[2][2]\end{array}\right]\cdot \left[\begin{array}{c}a[X]\\ a[Y]\\ a[Z]\end{array}\right]$$

• 第三个很简单，就是直接带入旋转矩阵运算，这里分析一下前两个是啥意思

1. 首先找到调用处，发现形参ref_ax的实参是imu_data.hx_vec
2. 查看imu_data.hx_vec是什么，找到

	//水平面方向向量
	//my_sqrt_reciprocal求开方的倒数；my_pow求平方
	float hx_vec_reci = my_sqrt_reciprocal(my_pow(att_matrix[0][0]) + my_pow(att_matrix[1][0]));
	imu->hx_vec[X] = att_matrix[0][0] *hx_vec_reci;
	imu->hx_vec[Y] = att_matrix[1][0] *hx_vec_reci;

3. 还记得嘛
   $$att\_matrix=\left[\begin{array}{ccc}cos(\theta )cos(\psi )& sin(\varphi )sin(\theta )cos(\psi )-cos(\varphi )sin(\psi )& cos(\varphi )sin(\theta )cos(\psi )+sin(\varphi )sin(\psi )\\ sin(\psi )cos(\theta )& sin(\varphi )sin(\theta )sin(\psi )+cos(\varphi )cos(\psi )& cos(\varphi )sin(\theta )sin(\psi )-sin(\varphi )cos(\psi )\\ -sin(\theta )& sin(\varphi )cos(\theta )& cos(\varphi )cos(\theta )\end{array}\right]$$
   所以
   $$hx\_vec\_reci=\frac{1}{\sqrt{co{s}^2(\theta )co{s}^2(\psi )+co{s}^2(\theta )si{n}^2(\psi )}}=\frac{1}{\sqrt{co{s}^2(\theta )}}=\frac{1}{cos(\theta )}$$$$imu->hx\_vec[X]=cos(\psi )$$$$imu->hx\_vec[Y]=sin(\psi )$$
   物理意义上，hx_vec[X]和hx_vec[Y]来自机体系转地面系的第一列，表示机体系中的X单位向量（机头方向）$[1,0,0{]}^T$，在$\theta =\varphi =0$时，仅绕Z轴旋转$\psi$后，得到的向量中的XY项。如下图所示，不难看出这指示了航向坐标的机头方向，所以匿名在注释中把它们称为水平面方向向量
   

4. 回代，有以下关系，可见航向坐标系h就是地理系w只绕Z轴旋转(yaw)，忽略pitch和roll旋转的结果

$$\left[\begin{array}{c}h[X]\\ h[Y]\end{array}\right]=\left[\begin{array}{cc}cos(\psi )& sin(\psi )\\ -sin(\psi )& cos(\psi )\end{array}\right]\left[\begin{array}{c}w[X]\\ w[Y]\end{array}\right]$$

$$\left[\begin{array}{c}w[X]\\ w[Y]\end{array}\right]=\left[\begin{array}{cc}cos(\psi )& -sin(\psi )\\ sin(\psi )& cos(\psi )\end{array}\right]\left[\begin{array}{c}h[X]\\ h[Y]\end{array}\right]$$

3、IMU_update函数分析

在三个坐标系变换函数后，下一个就是IMU_update函数了，下面进行分析

（1）整体分析

• 此函数原型void IMU_update(float dT,_imu_state_st *state,float gyr[VEC_XYZ], s32 acc[VEC_XYZ],s16 mag_val[VEC_XYZ],_imu_st *imu)，参数包括计算周期dT，Imu标志*state，滤波转换后的三轴陀螺仪数据gyr，滤波转换后的三轴加速度计数据acc，滤波转换后的三轴磁力计数据gyr，Imu数据结构体*Imu

• 任务：

  1. 利用互补滤波方法，用加速度计和磁力计数据对陀螺仪数据进行补偿。
  2. 使用一阶龙格库塔法更新四元数和旋转矩阵。
  3. 利用旋转矩阵计算三个坐标系下的各项数据值，存入*Imu。
  4. 根据*state中的标记，对互补滤波系数进行修正，进一步提高解算准确度。

（2）互补滤波原理相关

• 姿态角可以用两个传感器计算

  1. 陀螺仪数据积分（陀螺仪输出三轴角速度）
  2. 加速度计数据正交分解（加速度计输出三轴平动加速度，当飞机匀速运动时，数据是重力加速度在三坐标轴方向上的分量）

• 注意这两个传感器测到的数据都是在传感器坐标系中的，因为传感器和机体固连，所以也可以看做测得的数据在机体坐标系下

• 两个传感器的数据漂移有不同的特点：

  1. 陀螺仪积分数据会缓慢漂移，也就是具有低频误差
  2. 加速度计非常灵敏，会受到抖动影响出现数据抖动，也就是具有高频误差

  针对这种特点，如果我们可以找到一个方法融合两个传感器的数据，对误差进行补偿，就可以大大提高获取姿态角的精度 ，这种方法就是互补滤波

• 要补偿的对象：还记得吗，四元数更新方法如下
  $${\left[\begin{array}{c}{q}_0\\ q_1\\ q_2\\ q_3\end{array}\right]}_{t+\Delta t}={\left[\begin{array}{c}{q}_0\\ q_1\\ q_2\\ q_3\end{array}\right]}_t+\frac{1}{2}\cdot \Delta t\cdot \left[\begin{array}{c}-{\omega }_x\cdot q_1-{\omega }_y\cdot q_2-{\omega }_z\cdot q_3\\ {\omega }_x\cdot q_0-{\omega }_y\cdot q_3+{\omega }_z\cdot q_2\\ {\omega }_x\cdot q_3+{\omega }_y\cdot q_0-{\omega }_z\cdot q_1\\ -{\omega }_x\cdot q_2+{\omega }_y\cdot q_1+{\omega }_z\cdot q_0\end{array}\right]$$
  最终的姿态角是用旋转矩阵，也就是用四元数解算出来的，而四元数依靠的唯一参数就是来自陀螺仪的三轴角速度数据，因此我们最终要补偿的是陀螺仪数据

• 补偿数据来自：补偿数据来自两个传感器测算同一数据的误差，不妨称它为标准值，当补偿为0时，输出为陀螺仪积分数据，也就是完全置信于陀螺仪；当补偿为全部误差时，输出为加速度计数据，也就是完全置信于加速度计。补偿的比例即为置信于两个传感器的比例

• “标准值”是什么数据：通常我们用机体系下的地理重力加速度方向向量作为上面所说的标准值

  1. 回顾一下上面旋转矩阵中的一列相关内容，直接拿出旋转矩阵的第三列，就是利用陀螺仪测算的机体系下地理重力加速度方向向量（可以看做是积分计算得到的理论重力加速度分量）
  2. 加速度计可以直接计算加速度，当飞机匀速运动时，它可以直接输出机体系下地理重力加速度方向向量（可以看做是实际重力加速度分量）

• 误差的计算：机体系下地理重力加速度方向向量实质是一个三维向量，所谓误差，实质就是两个三维方向向量的夹角，求这个夹角的方法相信大家高中都用过，就是计算向量叉积
  $$有向量A,B，夹角为\theta ，则$$$$A\times B=|A|\cdot |B|\cdot sin\theta$$

  1. 对于两个传感器的数据，我们可以认为数据偏差（也就是两个三维方向向量的夹角$\theta$）很小，这时我们近似认为$sin\theta =\theta$
  2. 在计算误差前，处理向量A,B为单位向量，这样就有$A\times B=|A|\cdot |B|\cdot sin\theta =sin\theta =\theta$

• 补偿的方法：构造一个PI (比例-积分）控制器进行补偿，
  $$Gyro\_compenstae=K_p\cdot error+K_i\cdot \int error$$
  虽然形式上完全一致，但最好不要把这个理解成自动控制的那种控制器。请这样理解：积分项用来消除静态误差，比例项用来控制传感器的可信度

• 小结一下加速度计和陀螺仪的互补融合：

  1. 标准值（被测的）：地理系中重力方向$[0,0,1{]}^T$在机体系的投影
  2. 加速度计测算（实际值）：加速度计测算的加速度向量
  3. 陀螺仪测算（理论值）： 旋转矩阵第三列，来自角速度积分
  4. 偏差计算：两个传感器数据单位化，然后叉积
  5. 补偿方法：PI控制器
  6. 注意：被补偿的陀螺仪数据是机体系的，这里都是在机体系进行的计算，所以可以直接补偿

• 匿名对互补融合滤波做了优化，进一步融合了磁力计的数据，下面类比加速度计的融合方法进行分析。

  1. 磁力计测算什么：磁力计测算的是地理磁感线（正北方向）在机体坐标系下的投影。需要注意的是，虽然磁感线曲率和地球曲率不同，但是差距实际比较小，程序里直接把Z方向分量看做0了，这样可以简化单位化之类的计算
  2. 磁力计的误差特点：和加速度计一样，也是高频误差
  3. 标准值(真实值)：地理系中的正北方向向量（二维向量，忽略Z方向），根据匿名坐标系定义，这是个定值$[1,0]$
  4. 磁力计测算（理论值）：磁力计数据右乘旋转矩阵转到地理系，取XY分量组成二维向量
  5. 偏差计算：传感器数据单位化，然后和$[1,0]$叉积。注意这里和加速度计融合不太一样，因为那个偏差一般不超过45°，而这里可能很大。为了简化处理，直接用$sin\psi$作为偏差值。还要注意，$sin\psi$的极值1和-1出现在90°和-90°，对于超过这个范围的偏差角$\psi$，直接把误差值$sin\psi$设为极值1或-1
  6. 补偿方法：纯P控制器（我不太清楚为什么匿名的程序里为啥不用PI控制器了）
  7. 注意：被补偿的陀螺仪数据是机体系的，这里都是在地理系进行的计算，所以补偿前要转换到机体系。（可以看出这里坐标系变来变去很麻烦，或许可以直接把地面系的[1 0]变换到机体系，直接和磁力计数据叉乘，进一步优化，不过我们没有试过这样可行不）
     

（3）互补滤波相关代码分析

以下内容是IMU_update完整函数，前半部分是互补滤波相关的，后半部分是一些校准标志处理什么的（这里不分析了）。带括号的是我补充的注释，没有的是匿名官方注释

//输入：acc是载体系a下的三轴加速度测量值(经过滤波和变换什么的)，是加速度计直接反馈的（包含重力加速度成分，匀速时是重力加速度在机体下的投影）
//		gyr:	陀螺仪数据（经过滤波和变换什么的），表示（载体系）相对地面系的三轴角速度
//		mag_val:磁力计数据（没滤过波），表示地理系正比方向在机体系的三轴投影
//		dT:		计算周期
//		*state:	imu的状态结构体
//		*imu:	imu相关数据，要向这里赋值
void IMU_update(float dT,_imu_state_st *state,float gyr[VEC_XYZ], s32 acc[VEC_XYZ],s16 mag_val[VEC_XYZ],_imu_st *imu)
{
     
//	const float kp = 0.2f,ki = 0.001f;
//	const float kmp = 0.1f;
	
	//互补滤波系数
	static float kp_use = 0;		//(加速度计误差比例系数，越大越倾向加速度计)
	static float ki_use = 0;		//(加速度计误差积分积分系数)
	static float mkp_use = 0;		//(磁力计修正系数)

	
	float acc_norm_l;				//(加速度向量的模)
	float acc_norm_l_recip;			//(加速度向量的模的倒数)
	float q_norm_l;				    //(四元数的模)
		
	float acc_norm[VEC_XYZ];		//(单位化的加速度向量)

	float d_angle[VEC_XYZ];			//(微元转角，即滤波修正后的角速度值)
	


	//(旋转矩阵需要的一些四元数，先算好调用快一点)
//		q0q0 = imu->w * imu->w;							
		q0q1 = imu->w * imu->x;
		q0q2 = imu->w * imu->y;
		q1q1 = imu->x * imu->x;
		q1q3 = imu->x * imu->z;
		q2q2 = imu->y * imu->y;
		q2q3 = imu->y * imu->z;
		q3q3 = imu->z * imu->z;
		q1q2 = imu->x * imu->y;
		q0q3 = imu->w * imu->z;
	
		//(这里实际执行不到)
		if(state->obs_en)
		{
     
			//计算机体坐标下的运动加速度观测量。坐标系为北西天
			for(u8 i = 0;i<3;i++)
			{
     
				s32 temp = 0;
				for(u8 j = 0;j<3;j++)
				{
     
					
					temp += imu->obs_acc_w[j] *att_matrix[j][i];//t[i][j] 转置为 t[j][i]
				}
				imu->obs_acc_a[i] = temp;
				
				imu->gra_acc[i] = acc[i] - imu->obs_acc_a[i];
			}
		}
		//(只执行这句)
		else																																									    
		{
     
			for(u8 i = 0;i<3;i++)
			{
     			
				imu->gra_acc[i] = acc[i];					//（把加速度计测量值传给结构体保存）																								
			}
		}
    
    	//(加速度向量模的倒数)
		acc_norm_l_recip = my_sqrt_reciprocal(my_pow(imu->gra_acc[X]) + my_pow(imu->gra_acc[Y]) + my_pow(imu->gra_acc[Z]));	
		//(加速度向量模)
		acc_norm_l = safe_div(1,acc_norm_l_recip,0);																																				
		
		// 加速度计的读数，单位化。
		for(u8 i = 0;i<3;i++)
		{
     
			acc_norm[i] = imu->gra_acc[i] *acc_norm_l_recip;																																	
		}

		

	//(att_matrix是从机体系a到地理系w的转换矩阵)
	// 载体坐标下的（地理）x方向向量，单位化。	(地面系北方x [1 0 0]T转到机体系中，w到a系旋转矩阵第一列，可以看作地理x方向在机体系的三个分量)																			
    att_matrix[0][0] = imu->x_vec[X] = 1 - (2*q2q2 + 2*q3q3);
    att_matrix[0][1] = imu->x_vec[Y] = 2*q1q2 - 2*q0q3;
    att_matrix[0][2] = imu->x_vec[Z] = 2*q1q3 + 2*q0q2;
		
	// 载体坐标下的（地理）y方向向量，单位化。	(地面系西方y [0 1 0]T转到机体系中，w到a系旋转矩阵第二列，可以看作地理y方向在机体系的三个分量)																		
    att_matrix[1][0] = imu->y_vec[X] = 2*q1q2 + 2*q0q3;
    att_matrix[1][1] = imu->y_vec[Y] = 1 - (2*q1q1 + 2*q3q3);
    att_matrix[1][2] = imu->y_vec[Z] = 2*q2q3 - 2*q0q1;
			
    // 载体坐标下的（地理）z方向向量（等效重力向量、重力加速度向量），单位化。	(地面系天空z [0 0 1]T转到机体系中，w到a系旋转矩阵第三列，可以看作地理z方向在机体系的三个分量)			
    att_matrix[2][0] = imu->z_vec[X] = 2*q1q3 - 2*q0q2;
    att_matrix[2][1] = imu->z_vec[Y] = 2*q2q3 + 2*q0q1;
    att_matrix[2][2] = imu->z_vec[Z] = 1 - (2*q1q1 + 2*q2q2);
		
		//水平面方向向量	(a到w系第一列XY，机体系x [1 0 0]T 转到地面系后的XY向量，也就是航向坐标系的机头指向)																														
		float hx_vec_reci = my_sqrt_reciprocal(my_pow(att_matrix[0][0]) + my_pow(att_matrix[1][0]));
		imu->hx_vec[X] = att_matrix[0][0] *hx_vec_reci;
		imu->hx_vec[Y] = att_matrix[1][0] *hx_vec_reci;
	
	
		//计算载体坐标下的运动加速度	(加速度传感器测量值去掉重力加速度成分，得到机体系中的a_acc,与姿态解算无关)																								
		for(u8 i = 0;i<3;i++)
		{
     
			imu->a_acc[i] = (s32)(acc[i] - 981 *imu->z_vec[i]);
		}
		
    
		//计算世界坐标下的运动加速度。坐标系为北西天	(利用旋转矩阵变换a_acc,得到地理系中的加速度值w_acc)																		
		for(u8 i = 0;i<3;i++)
		{
     
			s32 temp = 0;
			for(u8 j = 0;j<3;j++)
			{
     
				
				temp += imu->a_acc[j] *att_matrix[i][j];
			}
			imu->w_acc[i] = temp;
		}
		
		//(再从地理系转换到航向系，得到航向系中加速度值h_acc)
		w2h_2d_trans(imu->w_acc,imu_data.hx_vec,imu->h_acc);															
		
		
    // 测量值与等效重力向量的叉积（计算向量误差）。		
	/*(这里是把加速度计测量的三轴向量，和载体坐标下的（地理）z方向向量imu->z_vec做叉乘，得到和二者垂直的新向量,
	  acc_norm表示的重力方向来自加速度计，imu->z_vec表示的重力方向来自陀螺仪，这里计算向量外积实质上是在算二者夹角，准备进行融合滤波)		*/
    vec_err[X] =  (acc_norm[Y] * imu->z_vec[Z] - imu->z_vec[Y] * acc_norm[Z]);
    vec_err[Y] = -(acc_norm[X] * imu->z_vec[Z] - imu->z_vec[X] * acc_norm[Z]);
    vec_err[Z] = -(acc_norm[Y] * imu->z_vec[X] - imu->z_vec[Y] * acc_norm[X]);

//(进行磁力计互补融合滤波)
#ifdef USE_MAG

		//电子罗盘赋值为float矢量
		for(u8 i = 0;i<3;i++)
		{
     
			mag_val_f[i] = (float)mag_val[i];
		}	
			
		if(!(mag_val[X] ==0 && mag_val[Y] == 0 && mag_val[Z] == 0))
		{
     
			//把载体坐标下的罗盘数据转换到地理坐标下
			a2w_3d_trans(mag_val_f,imu->w_mag);
			//计算方向向量归一化系数（模的倒数）
			float l_re_tmp = my_sqrt_reciprocal(my_pow(imu->w_mag[0]) + my_pow(imu->w_mag[1]));
			//计算南北朝向向量
			mag_2d_w_vec[1][0] = imu->w_mag[0] *l_re_tmp;
			mag_2d_w_vec[1][1] = imu->w_mag[1] *l_re_tmp;
			//计算南北朝向误差(叉乘)，地理坐标中，水平面磁场方向向量应恒为南北 (1,0)
			mag_yaw_err = vec_2_cross_product(mag_2d_w_vec[1],mag_2d_w_vec[0]);
			//计算南北朝向向量点乘，判断同向或反向
			mag_err_dot_prudoct = vec_2_dot_product(mag_2d_w_vec[1],mag_2d_w_vec[0]);
			//若反向，直接给最大误差
			if(mag_err_dot_prudoct<0)
			{
     
				mag_yaw_err = my_sign(mag_yaw_err) *1.0f;
			}			
			//
			
		}
#endif
	
		for(u8 i = 0;i<3;i++)
		{
     
//(这个没用)
#ifdef USE_EST_DEADZONE	
			if(state->G_reset == 0 && state->obs_en == 0)
			{
     
				vec_err[i] = my_deadzone(vec_err[i],0,imu->gacc_deadzone[i]);
			}
#endif	
//(用了这个，这里是利用加速度模长判断，如果水平方向加速度太大，就不能直接把加速度数据看作重力加速度数据，这一轮计算的偏差清零不补偿了)
#ifdef USE_LENGTH_LIM			
			if(acc_norm_l>1060 || acc_norm_l<900)
			{
     
				vec_err[X] = vec_err[Y] = vec_err[Z] = 0;
			}
#endif
		//误差积分	（用于互补滤波的pi控制器）																																				
		vec_err_i[i] +=  LIMIT(vec_err[i],-0.1f,0.1f) *dT *ki_use;

		
	// 构造增量旋转（含融合纠正）。	
	//    d_angle[X] = (gyr[X] + (vec_err[X]  + vec_err_i[X]) * kp_use - mag_yaw_err *imu->z_vec[X] *kmp_use *RAD_PER_DEG) * dT / 2 ;
	//    d_angle[Y] = (gyr[Y] + (vec_err[Y]  + vec_err_i[Y]) * kp_use - mag_yaw_err *imu->z_vec[Y] *kmp_use *RAD_PER_DEG) * dT / 2 ;
	//    d_angle[Z] = (gyr[Z] + (vec_err[Z]  + vec_err_i[Z]) * kp_use - mag_yaw_err *imu->z_vec[Z] *kmp_use *RAD_PER_DEG) * dT / 2 ;
			
			
#ifdef USE_MAG
			//（用的这个，在普通互补融合滤波基础上增加了磁力计补偿）
			d_angle[i] = (gyr[i] + (vec_err[i]  + vec_err_i[i]) * kp_use + mag_yaw_err *imu->z_vec[i] *mkp_use) * dT / 2 ;			
#else
			//（这个没用，普通的互补融合滤波，用加速度计pi控制器输出补偿陀螺仪）
			d_angle[i] = (gyr[i] + (vec_err[i]  + vec_err_i[i]) * kp_use ) * dT / 2 ;																						
#endif
		}

    // 计算姿态（一节龙格库塔法，wxyz初值为1000）

    imu->w = imu->w            - imu->x*d_angle[X] - imu->y*d_angle[Y] - imu->z*d_angle[Z];
    imu->x = imu->w*d_angle[X] + imu->x            + imu->y*d_angle[Z] - imu->z*d_angle[Y];
    imu->y = imu->w*d_angle[Y] - imu->x*d_angle[Z] + imu->y            + imu->z*d_angle[X];
    imu->z = imu->w*d_angle[Z] + imu->x*d_angle[Y] - imu->y*d_angle[X] + imu->z;
		
	q_norm_l = my_sqrt_reciprocal(imu->w*imu->w + imu->x*imu->x + imu->y*imu->y + imu->z*imu->z);
    imu->w *= q_norm_l;
    imu->x *= q_norm_l;
    imu->y *= q_norm_l;
    imu->z *= q_norm_l;
  /修正开关(以下内容不是姿态解算不分析了)
#ifdef USE_MAG
		if(state->M_fix_en==0)//磁力
		{
     
			mkp_use = 0;//不修正
			state->M_reset = 0;//罗盘修正不复位，清除复位标记
		}
		else
		{
     
			if(state->M_reset)//
			{
     
				//通过增量进行对准
				mkp_use = 10.0f;
				if(mag_yaw_err != 0 && ABS(mag_yaw_err)<0.02f)
				{
     
					state->M_reset = 0;//误差小于2的时候，清除复位标记
				}
			}
			else
			{
     
				mkp_use = state->mkp; //正常修正
			}
		}
#endif
		
		if(state->G_fix_en==0)//重力方向修正
		{
     
			kp_use = 0;//不修正
		}
		else
		{
     
			if(state->G_reset == 0)//正常修正
			{
     			
				kp_use = state->gkp;
				ki_use = state->gki;
			}
			else//快速修正，通过增量进行对准
			{
     
				kp_use = 10.0f;
				ki_use = 0.0f;
//				imu->est_speed_w[X] = imu->est_speed_w[Y] = 0;
//				imu->est_acc_w[X] = imu->est_acc_w[Y] = 0;
//				imu->est_acc_h[X] = imu->est_acc_h[Y] =0;
				
				//计算静态误差是否缩小
//				imu_reset_val += (ABS(vec_err[X]) + ABS(vec_err[Y])) *1000 *dT;
//				imu_reset_val -= 0.01f;
				imu_reset_val = (ABS(vec_err[X]) + ABS(vec_err[Y]));
				
				imu_reset_val = LIMIT(imu_reset_val,0,1.0f);
				
				if((imu_reset_val < 0.05f) && (state->M_reset == 0))
				{
     
					//计时
					reset_cnt += 2;
					if(reset_cnt>400)
					{
     
						reset_cnt = 0;
						state->G_reset = 0;//已经对准，清除复位标记
					}
				}
				else
				{
     
					reset_cnt = 0;
				}
			}
		}
}

4、姿态角输出函数函数calculate_RPY()

终于到最后了，这是imu.c中最后一个函数，利用旋转矩阵反解欧拉角，不多解释了，注意避免一下机头垂直向上时的特殊情况就行了（好像是处理陀螺仪死锁问题）

static float t_temp;
void calculate_RPY()
{
     
	///输出姿态角///
	
		t_temp = LIMIT(1 - my_pow(att_matrix[2][0]),0,1);
		
		//imu_data.pit = asin(2*q1q3 - 2*q0q2)*57.30f;
	
		if(ABS(imu_data.z_vec[Z])>0.05f)//避免奇点的运算
		{
     
			imu_data.pit =  fast_atan2(att_matrix[2][0],my_sqrt(t_temp))*57.30f;
			imu_data.rol =  fast_atan2(att_matrix[2][1], att_matrix[2][2])*57.30f; 
			imu_data.yaw = -fast_atan2(att_matrix[1][0], att_matrix[0][0])*57.30f; 
		}
}

三、参考

• 1. 这里给出出了姿态计算的学习路径，和一些解惑，还是蛮好的
• 2. 这个视频讲的非常清晰，我主要是看这个学的
• 3. 大佬队友的文章，大佬很强解答了我不少问题，文章也值得一看

四、Q&A

• 文章发布后，很多朋友在评论区提出的问题，很感谢大家进行讨论。
• 这里的很多问题应该也是大家关心的，但因为CSDN评论有时候加载不出来，所有把相关讨论搬运到文章中

（1）讨论1

• Q：加速度计和陀螺仪融合那里有处错误，理论值是旋转矩阵第三列，即陀螺仪测算的角速度，不是角速度积分，积分就是角度了
• A：这个地方是把地理系下重力加速度方向转换到机体系下，旋转矩阵是用陀螺仪数据构造的。我想表达的意思是：角速度经过积分和变形得到四元数，构成旋转矩阵，直接说角速度积分确实不太严谨，感谢您指出

（2）讨论2

• Q：我想问一下磁力计计算出sin角之后在融合时为什么要乘Z轴向量
• A：这里不用直接计算sin角。磁力计测算的是地理磁感线（正北方向）在机体坐标系下的投影，把这个矩阵旋转到地理坐标系下，取出南北方向，规格化后和地理系中的正北方向[1,0]叉乘得到偏差。在做叉乘时，两个方向向量的夹角可能太大(就是叉积展开后的那个sin角)，所以还要点乘一下判断它们的夹角绝对值是否超过90度。
• Q：讨论2没看懂回答。我的理解是： imu->z_vec[i]是r系z轴在b系三轴的投影，也是r转b系旋转矩阵的第三列。这列的物理意义是：将地理z轴数据转到机体系。 我们补偿的这个误差，其实就是：推导出的正北，与真实正北方向之间的偏离角度。通俗说就是z轴的旋转误差。 所以只需要乘旋转矩阵的第三列。即只转z轴到机体系。 简而言之：z轴误差是在地理系计算的，我们最后得将其转回机体系才能补偿到磁力计数据上。只转Z轴就可以了，所以乘以Z。
• A：是的是的，乘Z是为了变换到机体系。我之前回答的时候只顾着说那个角度了。航向角比较特殊，它的偏差可以很大，所以这里叉乘得到sin后还要按取值范围限制一下，之后乘Z就是为了坐标变换没错。谢谢您的补充

（3）讨论3

• Q：关于mag_2d_w_vec[1][0] 和 mag_2d_w_vec[1][1] 的含义
• A：这个是这样定义的：static float mag_2d_w_vec[2][2] = { {1,0},{1,0}};，mag_2d_w_vec[0]就是地理系里的实际正北方向。叉乘那句，就是磁力计测出的南北方向和正北方向的叉乘，来算误差的。

（4）讨论4

• Q：174行这里是利用加速度模长判断，如果水平方向加速度太大，就不能直接把加速度数据看作重力加速度数据中, 代码是通过加速度的模长超出[900,1060]这个区间才舍掉加速度数据的, 并不是根据’水平方向的加速度太大’而舍弃掉加速度数据的. 不知道我的理解是否正确?
• A：这是这里光看代码确实是这样的。不过这里本质上是要避免水平加速度太大导致加速度计数据不准。飞机水平加速度为0时，只有重力加速度，模为9.8（980），就是[900,1600]中点嘛。
• Q：因为acc_norm的3个分量中包含了飞机本身的加速度和重力加速度分量, 所以应该是’要避免飞机本身在载体系3个方向的加速度太大’ 吧? 因为飞机毕竟会有横滚和俯仰的飞行姿态并同时加速啊.
• A：我觉得问题的关键不是加速度有多大，而是这个姿态融合算法进行了这样的假设 “程序假设加速度计测量的就是重力加速度”，它直接用加速度计的数据去融合。如果飞机运动太剧烈，这个假设就不成立了，所以要限制一下

（5）讨论5

• Q：在摘录的本文代码中, 计算出来的去掉重力加速度分量后的机体本身加速度量a_acc, w_acc, h_acc 并没有在后续代码中进一步用到. 这三个量在没被你截取的代码中被用到了么?
• A：在pid控制的时候用到了w_acc 和 h_acc，主要用来做速度环反馈

//位置速度环代码节选
		LPF_1_(5.0f,dT_ms*1e-3f,imu_data.h_acc[X],vel_fb_d_lpf[X]);
		LPF_1_(5.0f,dT_ms*1e-3f,imu_data.h_acc[Y],vel_fb_d_lpf[Y]);		
 
		//位置_速度环反馈=上次检测的光流速度+20ms*当前加速度=当前理论速度（20ms为Loc_1level_Ctrl的执行周期）
		loc_ctrl_1.fb[X] = OF_DX2 + 0.02f *vel_fb_d_lpf[X];
		loc_ctrl_1.fb[Y] = OF_DY2 + 0.02f *vel_fb_d_lpf[Y];

（6）讨论6

• Q：博主,在代码的184~186行, mag_yaw_err前面是’-’, 而在191行, mag_yaw_err前面是’+’. 知道这是什么原因么?
• A：这是匿名写的，我只能猜测一下。这里就是用了一个p控制器修正磁力计偏差，有点像pid控制器，式子里正负号不重要，因为参数mkp_use可以取负值。他那个mkp_use=10.0我不能确定是怎么得到的，如果我来做，我会先试一下100和-100，看看哪个发散了，这样就能确定符号。然后再调到一个合适的值

（7）讨论7

• Q：博主,calculate_RPY()中的第8,12,14行, 等式右侧的符号是不是搞反了? 是否应该为: imu_data.pit = -asin; imu_data.pit =- fast_atan2; imu_data.yaw =+fast_atan2
• A：emm这个代码我上飞机运行挺稳定的，所以应该没错
• Q：但是这和正文中3. (1) 里面的公式不匹配啊. 能否把代码改成这样, 看飞机运行是否也稳定?
• A：我开程序仔细看了看，这里写成这样没问题，不能改的。fast_atan()函数原型REAL fast_atan2(REAL y, REAL x)，参数可以看成一个点的坐标（写形参（sin(a),cos(a)）可以解出a的弧度值）。3.(1)公式中绕三个轴的转角，正方向是按右手螺旋定则确定的，这和匿名规定的欧拉角正方向不同，匿名规定正方向为：pit-仰角；roll-右倾；yaw-右转。以俯仰角为例，此这是绕Y轴的转角θ，对应的欧拉角为pit，pit=-θ，带入参数atan(-sin(θ)，cos(θ))，解出的是-θ；其他两个轴同理