姿态估计（1）——互补滤波（Complementary filter ）

注：本文转载自 博主：Gen_Ye 博客：《Pixhawk-姿态解算-互补滤波》    

    

    想要了解用于姿态解算的扩展卡尔曼滤波的可以看这里姿态估计（2）—— 扩展卡尔曼滤波（Extended Kalman Filter—EKF）

 

    终于说到了正题，姿态解算这一部分很重要，主要的基础就是惯性导航和多传感器数据融合，很多公司都在招这方面的人才，如百度的无人驾驶在招传感器数据融合，网易的人机交互工程师也在找这方面的人，因为它是信息流的源泉，准确的姿态信息需要靠他们解算出来才能进行后续的步骤。

 

    鉴于加速度计低频特性比较好，因为加速度的角度可以直接算出来，没有累积误差，所以长时间后也比较准。而陀螺仪长时间后由于积分误差的累加，会造成输出误差比较大，甚至无法使用。所以用互补滤波法根据他们的特性取长补短进行姿态解算，每过一段时间就让加速度计去校准一下陀螺仪。互补滤波就是在短时间内采用陀螺仪得到的角度做为最优值，定时对加速度采样来的加速度值进行取平均值来校正陀螺仪的得到的角度。短时间内用陀螺仪比较准确，以它为主；长时间用加速度计比较准确，这时候加大它的比重，这就是互补了，不过加速度计要滤掉高频信号，陀螺仪要滤掉低频信号，互补滤波器就是根据传感器特性不同，通过不同的滤波器（高通或低通，互补的），然后再相加得到整个频带的信号。互补是给他们不同的权重加权求和。

 

    当然这里面还有一些问题：如加速度无法区分惯性加速度和运动加速度，在固定翼上这个问题更为显著，再者磁力计准确的偏角怎么得到？

 

   下面融合的一些框架图，先建立一个整体的概念：

 

          

    下面这幅图才是准确的阐述了互补滤波的过程。正常情况下用陀螺仪的数据就可以进行姿态的更新，但是由于陀螺仪的积分误差，这里用acc和mag去校正，求出他们的误差用PI去弥补。注意看看pid的公式和作用，pid是作用于误差（实际个期望之间的差值），最终反复调节，让实际值=期望值。

 

   下面先说点基础内容，之后再贴源码：

   下面介绍三部分内容：

    1、姿态的表示方法，在源码之中姿态的表示方法有DCM、四元数，欧拉角。欧拉角法在求解姿态时存在奇点（万向节死锁），不能用于全姿态的解算；方向余弦可用于全姿态的解算但计算量大，不能满足实时性要求。四元数法，其计算量小，无奇点且可以满足飞行器运动过程中姿态的实时解算。
 

   2、阐述一下姿态解算的原理。

 

    姿态就是指飞行器的俯仰/横滚/航向情况。在咱们地球上，就是指飞行器在地球坐标系中的俯仰/横滚/航向情况。飞行器需要实时知道当前自己的姿态，才能够根据需要操控其接下来的动作，例如保持平稳，例如实现翻滚。
姿态是用来描述一个刚体的固连坐标系和参考坐标系之间的角位置关系，有一些数学表示方法。很常见的就是欧拉角，四元数，矩阵，轴角。

地球坐标系又叫做地理坐标系，是固定不变的。正北，正东，正向上构成了这个坐标系的X，Y，Z轴，我们用坐标系R表示。四轴飞行器上固定着一个坐标系，我们一般称之为机体坐标系，用坐标系r表示。那么我们就可以用欧拉角，四元数等来描述r和R的角位置关系。这就是四轴飞行器姿态解算的数学模型和基础。

 

    欧拉角的姿态表示方法最为直观，可以看做飞机绕固定轴的三次旋转达到现在的姿态。

分解之后就是每次的旋转：

 

方向余弦矩阵是一个3*3阶的矩阵，矩阵的列表示载体坐标系中的单位矢量在参考坐标系中的投影。

 

这是一个总的旋转的表达，分解为三次旋转，可以理解为R=R3*R2*R1。

 

    四元数姿态表达式是一个四参数的表达式。它的基本思路是：一个坐标系到另一个坐标系的变换可以通过绕一个定义在参考系中的矢量 的单次转动来实现。四元数用符号q表示，它是一个具有4个元素的矢量，这些元素是该矢量方向和转动大小的函数。定义 的大小和方向是使参考系绕 转动一个角度 ，就能与载体坐标系重合。

    他们三者都可以表示姿态，求出一个就相当于知道其他的了。具体用什么形式表达，可以按照你的要求自己去换算，他们的之间的关系如下：

 

 

 

2、下面对姿态解算的原理进行阐述。

   姿态解算常用的算法有欧拉角法、方向余弦法和四元数法。 欧拉角法在求解姿态时存在奇点（万向节死锁），不能用于全姿态的解算； 方向余弦可用于全姿态的解算但计算量大，不能满足实时性要求。 四元数法，其计算量小，无奇点且可以满足飞行器运动过程中姿态的实时解算。

   姿态解算的原理：对于一个确定的向量，用不同的坐标系表示时，他们所表示的大小和方向一定是相同的。但是由于这两个坐标系的旋转矩阵存在误差，那么当一个向量经过这么一个有误差存在的旋转矩阵后，在另一个坐标系中肯定和理论值是有偏差的，我们通过这个偏差来修正这个旋转矩阵。这个旋转矩阵的元素是四元数，我们修正的就是四元数，这样姿态就被修正了。

   陀螺仪动态响应特性良好，但计算姿态时会产生累积误差。 磁力计和加速度计测量姿态没有累积误差，但动态响应较差。因此他们在频域上特性互补，所以采用互补滤波器融合这三种传感器的数据，提高测量精度和系统的动态性能。

 

3、四元数姿态解算的步骤：

 

 

 

 

好了到这里姿态解算的四路已经比较清晰了，最后给大家一个“杀手锏”：

 

   看到这些之后再结合源码，想必思路会清晰很多!