传感器数据融合及姿态估计总结

超长篇警告！

一、准备工作

该部分内容用于进行一些基本内容的普及以及标准统一。过于简单的内容此处不再涉及，直接给出一些参考的博文。

姿态旋转理论基础

陀螺仪、加速度计、磁力计的传感器介绍

接下来是将标准进行统一方便后续理解和表达（主要参考全权老师的《多旋翼飞行器设计与控制》）。

• 规定NED坐标系简称n系，然后机体坐标系表示为b系。
• 绕 X轴 进行的滚转运动，称为Roll ，角度符号用 ϕ 来表示。
• 绕 Y轴 进行的俯仰运动，称为Pitch ，角度符号用 θ 来表示。
• 绕 Z轴 进行的偏航运动，称为Yaw ，角度符号用 ψ 来表示。
• 由此，n系向b系的旋转矩阵$R_n^b$表示为
  
• 四元数规定用$q_0，q_1，q_2，q_3$表示。

二、原始数据

本文的原始数据来源：Open source IMU and AHRS algorithms

当然最好还是采用自己的实际数据，我这里图省事就直接用现成的。注意最好还是用的实际的飞控数据，尽管自己用手控制得到的IMU数据也可以用来测试数据融合的算法，但是实际的飞控数据会有很多人手调不出来的抖动及震荡，更利于你对算法的理解及实践。

用MATLAB先把原始数据画出来。

此处图片较小可能有点看不太出来。放大来说，加速度计和磁力计的数据在曲线细节方面显得更“毛糙”，也就是说临近点附近会有很多的抖动及噪声。

加速度数据细节：

磁力计数据细节：

但是陀螺仪对比之下，虽然曲线乍一看貌似有很多抖动的地方，因为陀螺仪还是比较灵敏的。但是你细看这些曲线的时候会发现陀螺仪的输出曲线还是比较平滑的，临近点附近都是很自然的过渡，对比加速度计和磁力计来说噪音会小很多。

此处我们先不展开细说，只需简单了解到陀螺仪和加速度计、磁力计的一个重要区别在于：

• 加速度计和磁力计的数据是足够准确的，但是缺点在于容易受到干扰，数据噪声较大；
• 陀螺仪陀螺仪基本不受外界干扰，并且对于线性加速度（振动）不敏感。

三、传感器直接估算姿态

由此我们能不能直接使用传感器的信息来估算姿态呢？答案当然是可以的。主要分为两种方法：（1）加速度计估算Roll和Pitch角，磁力计估算Yaw角；（2）陀螺仪积分得到欧拉角。

以下均假定传感器已经固定在机体上，并且X、Y、Z轴与机体的X、Y、Z轴完全对齐。

3.1 加速度计和磁力计解算姿态角

3.1.1 加速度计解算Roll和Pitch

在机体初始保持水平静止状态时，我们可以认为加速度计测得的加速度与重力加速度保持一致，即 $a=g=(0,0,1{)}^T$。假定机体在旋转过程中机体不出现剧烈运动的情况（加速度不发生剧烈变化），那么实际上在经过旋转$R_n^b$之后得到的加速度计数据满足
$$a=R_n^b\ast g$$

$R_n^b$的形式在之前已经给出了，由此我们推算可知，最后得到的测量值$a$实际上就是$R_n^b$的第三列，即
$$a=[a_x,a_y,a_z{]}^T=[-sin\theta ,sin\varphi cos\theta ,cos\varphi cos\theta {]}^T$$

又由于$R_n^b=R(-\varphi ,-\theta ,-\psi )$，那么可以得知Roll和Pitch的角度为
$$\varphi =arcsin(a_x)$$$$\theta =-arctan(\frac{ay}{az})$$

3.1.2 磁力计估算Yaw角

通过加速度计我们解算得到了Roll角和Pitch角，剩余的Yaw角则是通过磁力计数据来进行解算。但是区别在于重力加速度在地球表面任何位置的方向始终都是不变的，因此可以直接从n系转换到b系中，但是地磁场方向和大小并非固定不变。

由于磁力计时固定在机体上的，此时测得的数据是以机体坐标系为基表示的，因此首先需要使用$R_b^n$转换到n系下得到地理坐标的磁力向量，然后在用$R_n^b$转换到b系得到最终值。由于此处笔者也尚未完全搞明白，就先把推导的公式放在这里，公式也已经验证过，大家直接使用即可。

当地磁坐标m系和机体坐标b系重合的时候，磁力计的输出假定为 $M^m=[M_N,0,M_D{]}^T$，在b系下磁力计的输出为$M^b=[M_x^b,M_y^b,M_z^b]$，由此有如下关系：
$$M^b=R_n^b\ast M^m$$$$\left(\begin{array}{c}{M}_x^b\\ M_y^b\\ M_z^b\end{array}\right)=\left(\begin{array}{c}{M}_{N}cos\theta cos\psi -M_{D}sin\theta \\ (sin\varphi sin\theta cos\psi -cos\varphi sin\psi )M_N+sin\varphi cos\theta M_D\\ (cos\varphi sin\theta cos\psi +sin\varphi sin\psi )M_N+cos\varphi cos\theta M_D\end{array}\right)$$

根据上述方程解算出以下内容：
$$M_{N}cos\psi =M_x^{b}cos\theta +M_y^{b}sin\theta sin\psi +M_z^{b}cos\varphi sin\theta$$$$-M_{N}sin\psi =M_y^{b}cos\varphi -M_z^{b}sin\varphi$$

最后可以得到：
$$\psi =-arctan\frac{M_y^{b}cos\varphi -M_z^{b}sin\varphi }{M_x^{b}cos\theta +M_y^{b}sin\theta sin\psi +M_z^{b}cos\varphi sin\theta }$$

根据原始数据编写MATLAB代码解算出欧拉角表示如下，可以看到估算曲线还是包含非常多的噪声的，整条曲线看上去也很“毛躁”，非常符合加速度计和磁力计自身噪音比较多的特征。除此之外，在5S~8S左右的时间Pitch和Yaw角跳变非常剧烈，此时正好是y轴和z轴加速度和磁力均发生剧烈变化的时候。

3.2 陀螺仪积分

之前的推算似乎表明光使用加速度计和磁力计似乎就能够完全得到欧拉角，但是最后解算得到的曲线告诉我们这种结果不太可靠。因此我们此处使用9轴IMU中的另一个传感器进行姿态的估计，陀螺仪的一个巨大优势就是它的精度相比于加速度计和磁力计要高很多，并且对于外界干扰不太敏感。

首先明确一点，陀螺仪输出的是角速度值，那么稍微回顾一下积分的知识，我们就可以确定如下的公式内容：
$$\alpha (t)={\alpha }_0+\int w(t)dt$$

其中，$\alpha (t)$是当前时刻的角度，${\alpha }_0$是初始角度，$w(t)$则是当前角速度。当然，由于整个过程是离散的，因此，实际的当前t时刻的角度表示为，前一t-1时刻的角度加上此次陀螺仪测得的角速度乘以微小时间段。
$${\alpha }_t={\alpha }_{t-1}+w\ast dt$$

对于Roll、Pitch和Yaw角就可以通过陀螺仪测量值的积分得到，当然前提是对初始角度已知。

可以看到曲线相比于直接使用加速度和磁力计进行估计要来的平滑许多，并且在5s~8s时间内没有那么剧烈的变化，那么是否意味着我们可以直接使用陀螺仪而不需要其他传感器了呢？当然不是，如果仔细观察，你会发现陀螺仪积分得到的角度随着时间在不断地漂移，在到达最后时间段的时候整条曲线已经具备了非常大的误差。

这就是陀螺仪其中的一个缺陷，陀螺仪获取的数据实际上都带有一定的误差，使得在积分的过程中得到的角度不断飘移，随着时间的累加整个积分误差会变得非常大。

3.3 传感器总结

这里我们已经能够非常明显地看出陀螺仪、加速度计和磁力计这三种传感器的特点。依次来说一下。

首先是加速度计和磁力计，这两个传感器的显著特点就是足够准确，并且不具备任何的漂移误差，可以直接用于对姿态进行测量估计，但是缺点也十分明显，也就是自身的噪音非常大，只要一受到扰动，如机体的震动或者外界其他干扰因素，测量值就会发生剧烈变化，这一点对于磁力计来说更为明显，因此实际工程中会有很多人选择放弃磁力计，即不对Yaw角进行估计，直接开环控制，而重点处理Roll和Pitch这两个姿态角，对于Yaw角就进行速率控制即可。此外，这两个传感器需要在机体保持静止或者稳定运动过程中才能保持测量结果的精准。

然后是陀螺仪，该传感器是IMU中最重要的传感器，因此精度相比于其余两种会更高一些，此外，其对于一些干扰不会太敏感，也就是说能够较小程度地受到外界干扰。但是传感器自身存在一定的漂移，使得其使用时间越长，得到的误差也就会越大。此外，使用陀螺仪数据进行积分的时候必须得知道其初始姿态信息。

那么面对这两种类型的传感器，一种噪声较大，但是不存在漂移，另一种基本没有噪声，但是存在漂移，有没有办法能够将这两种结合起来估计机体的姿态呢？这就是我们接下来探讨的数据融合。

四、线性互补滤波

将2种传感器的数据进行融合，一种简单的方法就是将各个传感器的数据进行加权，也就是给每一个传感器数据分配一个确信度$k$，当$k$值较大的时候，表明我们更相信这个传感器的数据，反之，表示我们对于这个传感器数据的确信度较低。

由于我们此次只有2种传感器，因此可以得到一个最基本的线性互补滤波公式
$${\alpha }_{t+1}=(1-k)\ast ({\alpha }_t+w_t\ast dt)+k\ast {\alpha }_{measure}$$

等式右边的左半部分的${\alpha }_t+w_t\ast dt$即是当前时刻陀螺仪积分得到的角度值，右半部分的α_{measure}则是通过加速度计或磁力计数据解算出来的角度值，由于此时仅有2种传感器，因此$k$表示对于加速度计/磁力计测量得到的值的确信度，那么$1-k$则表示对于陀螺仪积分得到的角度的确信度。

当然，如果有多个传感器，那么等式即可变为 ${\alpha }_{t+1}=a\ast {\alpha }_1+b\ast {\alpha }_2+c\ast {\alpha }_3+...+n\ast {\alpha }_n$，其中 $a+b+c+...+n=1$。

此处我们将$k=0.1$，也就是说我们相信陀螺仪的积分结果，由此得到如下曲线

对比直接采用传感器的解算方式，线性互补滤波方式在结果上相比于加速度计和磁力计解算结果更为平滑，相比于陀螺仪积分则解决了漂移的问题。但是我们也可以看到在5~8s的时候仍然受到了非常大的干扰，应该是更多地来自于加速度计和磁力计数据方面的问题。尽管通过调节k值大小能够在一定程度上缓解这个问题，但是如果要真正解决这个问题，仍然需要更好的互补滤波算法。

线性滤波算法是在互补滤波中最为基础的，更深入的还有自适应互补滤波，基于模糊方法的互补滤波等等，这些算法会根据不同的情况，自动调节$k$值的大小，来获得更好的姿态解算结果，更为详细的研究请大家再去细看一些论文，此处便不再不多展开。

五、Mahony

Mahony算法实际上是一种非线性互补滤波，其应该是在飞控姿态估算领域内部网上流传最多的一种方法了。在开始之前希望你对于姿态旋转、矩阵运算以及一阶龙格库塔法具备一定的了解。

它的基本思想如下：首先将加速度计和磁力计的的测量值与根据数学模型推导出来的理论值对比求得误差，然后利用这个误差进行PI运算更新到陀螺仪的数据上，最后利用一阶龙格库塔法对四元数进行更新。具体过程如下：

5.1 加速度计误差计算

该过程较为简单，跟之前通过加速度计数据推算Roll和Pitch角类似，首先已知在n系下重力加速度方向和大小基本固定不变，保持为$g=[0,0,1{]}^T$，由n系向b系旋转的四元数矩阵$C_n^b$表示为
$$C_n^b=\left(\begin{array}{c}1-2(q_2^2+q_3^2)2(q_1{q}_2+q_0{q}_3)2(q_1{q}_3-q_0{q}_2)\\ 2(q_1{q}_2-q_0{q}_3)1-2(q_1^2+q_3^2)2(q_2{q}_3+q_0{q}_1)\\ 2(q_1{q}_3+q_0{q}_2)2(q_2{q}_3-q_0{q}_1)1-2(q_1^2+q_2^2)\end{array}\right)$$

那么由此推算得知在当前旋转角度下的理论机体重力为
$$v=\left(\begin{array}{c}{v}_x\\ v_y\\ v_z\end{array}\right)=C_n^b\ast g=\left(\begin{array}{c}2(q_1{q}_3-q_0{q}_2)\\ 2(q_2{q}_3+q_0{q}_1)\\ 1-2(q_1^2+q_2^2)\end{array}\right)$$

假定此时由加速度计测量得到的三轴上的加速度值为
$$a=\left(\begin{array}{c}{a}_x\\ a_y\\ a_z\end{array}\right)$$

向量之间的误差可以由外积得到，那么就有误差为

$$ea=\left(\begin{array}{c}e{a}_x\\ e{a}_y\\ e{a}_z\end{array}\right)=\left(\begin{array}{c}{a}_y\ast v_z-a_z\ast v_y\\ a_z\ast v_x-a_z\ast v_z\\ a_x\ast v_y-a_y\ast v_x\end{array}\right)$$

5.2 磁力计误差计算

磁力计误差计算对比之下会略显复杂一些。前面已经说过，地磁方向并非和重力一样是固定的，因此由于磁力计是固定在机体上的，此时测得的数据是以机体坐标系为基表示的，因此首先需要使用$C_b^n$