（转载）4. 飞控与姿态互补滤波器

4. 飞控与姿态互补滤波器
Tam Alex
原文链接：https://zhuanlan.zhihu.com/p/34323865

中间停更了好久呢，一方面因为寒假了，另一方面这篇写起来不小心就变成了公式推导，有些违背这个专栏的初衷，所以考虑了很久，才有了这些难产的文字。

这篇文字主要和大家聊一聊用于确定飞行器姿态的互补滤波器，所谓飞行器姿态呢，可以是欧拉角、方向余弦矩阵(DCM)，四元数等等，在后面我们可以看到不论采用什么姿态表示方法，互补滤波器的思想都是一样的，那么就让我们开始吧。

1、互补滤波器的思想

互补滤波器的思想可以用下面这个图概括(图片来自师兄的论文)
 

可以说是一种相当直观的信号融合方案：假如我们对一个物理量有两种测量手段，一种呢容易受到高频噪声的干扰，实际的观感就是信号有很多的毛刺；另一种容易受到低频噪声的干扰，就是噪声会慢慢的变化。那么我们就可以设计互补的两个滤波器 [公式] 和 [公式] ，所谓互补就是 [公式] ，它们一个是低通滤波器，可以滤除信号中的高频噪声，一个是高通滤波器，能够滤除信号中的低频变化。这样分别把相应测量中的噪声滤除，就可以得到更精确的对这个物理量的测量了。

那么显然，互补滤波器的好坏，就取决于噪声滤除的干不干净。不幸的是，对实际实现的滤波器来说，噪声是不可能完全被滤除的。所以，互补滤波器并不是某种特定准则上的“最优”滤波器，这是它与后面我们要聊到的卡尔曼滤波器的本质区别。

2、基本的姿态互补滤波器

那么要使用互补滤波器获得对飞行器姿态的更准确的测量，按照上一节的思路，我们要找到两个对飞行器姿态的测量手段，刚好之前我们的专栏里提到过，陀螺仪可以通过积分获得一组姿态，而加速度计(配合磁强计)也能够获得一组姿态，而且又这么巧地，加速度计容易受到振动等高频噪声的干扰，陀螺仪容易受到漂移等低频噪声的干扰，一切都是如此的美好，我们就可以直接设计下面的姿态互补滤波器了。
 

    这里啰嗦几句，容易受到高频噪声和低频噪声影响这个说法，并不准确。实际接触过MEMS惯性器件的朋友可能都知道，加速度计解算出的姿态，相当相当的灵敏，稍微有些振动就会产生很大的变化，这实际上并不完全是噪声的影响，而是由加速度计的特性决定的；反之，陀螺仪积分得到的姿态几乎不会有什么毛刺，但是由于陀螺漂移的存在，积分的姿态角会一直跑偏。因此，不如说是我们希望姿态算法能够综合加速度计不漂移和陀螺仪受振动影响小的优点，也就是我们希望使用加速度计姿态的低频部分和陀螺仪姿态的高频部分。

可能有的朋友接触过开源飞控或是其他地方的姿态互补滤波器，会发现它们都是以PI控制器的形式实现的，与上面我们放的图的样子貌似差很远。实际上这是由于我们在选择飞行器姿态测量传感器时，使用了陀螺仪这个角速度传感器，以致于需要补偿陀螺漂移的缘故。至于具体的推导，这里就不详细讲了，感兴趣的可以读一读Mahony那篇"Nonlinear complementary filters on the SO group"的附录A。

因此我们实际中的姿态互补滤波器是这个样子滴：

图3：虽然看起来相差很多，但这幅图真的是和上面那副图是等价的，要是大家需要可以写篇番外推导给你们看哦

具体原理大家也可以参考Mahony那篇文章，有以下几个要点需要注意：

    互补滤波器的整体框架是利用飞行器的旋转速度来更新飞行器的姿态，即图中红色的框，采用欧拉角、四元数、DCM方法的差别就在这个框框里；但是陀螺测量的飞行器旋转速度是不准确的，怎么办？利用PI控制器去修正陀螺的测量~用简单的反馈控制的思想可以理解为，加速度计和磁强计产生的是参考信号，是滤波器要去跟踪的，所以参考姿态和滤波器输出姿态之间的差异就构成了新息，我们对新息进行PI控制操作，其中P环节是为了补偿滤波器的估计误差，I环节是为了补偿陀螺仪的漂移，为什么是这样？实际上是还是通过Mahony那篇文章附录A的推导得到的；可以这样理解这个互补滤波器，基本的框架是利用陀螺测量的旋转速度来计算姿态，但这样肯定将漂移和噪声引入到姿态中，所以我们希望陀螺更新的姿态能够向相对无偏移的加速度计姿态靠近，这样利用一个PI控制器来控制这个过程，这样一来，因为PI控制器不可能非常快速的把陀螺姿态控制到和加速度计姿态一致，所以我们能够避免加速度计姿态里面快速变化的部分，因为控制带宽不够跟踪不上。与此同时，陀螺姿态在不断试图跟踪加速度计姿态的过程中，也避免了漂移量带来的误差，实际上从控制的角度解释了“互补”的思想。也就是说红色框是滤波器主体框架，蓝色框是滤波器“互补”的实现，那黄色框是什么呢？黄色框就是Mahony算法做出的贡献。

3、Mahony的互补滤波器

我们来关注一下姿态误差（新息）的产生。Mahony对传统互补滤波器（图3架构的）做出的改进就在这里，传统互补滤波器我们可能使用欧拉角相减，DCM或四元数相乘来获取姿态的误差，这样的方法都有一个缺点，那就是需要通过加速度计和磁强计的测量计算出一组姿态。虽然常用的方法比较成熟，但也有一些问题，比如欧拉角法存在奇异点（如在俯仰角90度时），DCM和四元数的TRIAD或QUEST等方法又需要额外的计算，Mahony的突出贡献之一就是直接利用加速度计和磁强计的测量值，利用和惯性空间向量的叉乘，简简单单的就获得了姿态的误差。

通俗点讲就是，加速度计是用来测量重力矢量的，那么我把测量的机体坐标系的加速度转换到参考坐标系去和当地的重力矢量比较，差别不就是我转换矩阵（DCM，即姿态）的误差么？同理磁强计是用来测量地磁矢量的，也可以用同样的方法获得姿态的误差。利用这个思想再稍加处理，就是现在应用广泛的Mahony互补滤波器。

Mahony的另一个突出贡献就是证明了这类互补滤波器的稳定性。上面我们讲了，互补滤波器不存在某种准则下的最优，但对于滤波器是否能够收敛以往的证明也是不很完善的。Mahony在那篇论文里不但证明了这一类互补滤波器的几乎全局渐近稳定，还证明了其不稳定集是一个不变集，给实际使用提供了非常完备的理论支持。

近些年，Mahony团队又利用最优控制的思想提出了最小能量滤波器，这是一个具有“最优”准则的滤波器，并且脱离了卡尔曼滤波的框架，反而更像是一个LQR的对偶问题，实在是令人心生赞叹。

好啦这次就先聊到这里吧，中间省略了大量数学推导，主要是从思想上谈了谈个人对互补滤波器的看法。下次我们就来聊鼎鼎大名的卡尔曼滤波吧~