imu存在的问题_8. IMU 姿态估计的新方法

这篇文章主要讨论姿态估计算法在 MEKF 之后的一些理论进展。我会首先介绍 MEKF 在几何上的直观解释，并由此引出它的一些缺点，然后介绍几种试图解决这些缺点的思路。目前这些思路大多还在开发过程中，因此这篇文章主要以综述的形式，着重介绍几种算法的几何意义，而不会深入介绍某种算法的计算过程。

1. 姿态估计为什么困难

相信大家在刚接触惯性传感器 IMU 时，都或多或少被灌输了这样的想法：姿态估计可以使用卡尔曼滤波器解决。但当学习了卡尔曼滤波的理论之后，却发现如果想直接套用 EKF 的公式，基本只能使用欧拉角作为状态量；然而我们都知道欧拉角有自锁的问题。如果想用四元数或者旋转矩阵，就要忍受用

或者

的协方差矩阵来描述一个只有三个自由度的状态量，这种做法一眼看上去就会觉得有问题。似乎不管怎么样，将 EKF 用在姿态估计算法中都会遇到一些不可避免的障碍，这和刚入门时四处听来的“卡尔曼滤波器可以用在 IMU 上”好像不太一样，实在是令人恼火。

造成这种困难最大的原因是：描述 IMU 姿态的 SO(3) 群是一个三维空间，但它不可能被嵌入到一个三维欧氏空间中。然而卡尔曼滤波器最基本的要求就是状态变量必须要在欧式空间中，否则没有办法定义正态分布，数学期望也失去了原有的意义。这就是为什么直接套用 EKF 的公式会遇到各种各样的问题。

但这些困难并非没有解决方法，因为任何一个三维光滑流形在“局部”都可以等价为一个三维欧氏空间，这个欧氏空间叫做切空间，SO(3) 也不例外。试想如果用一个概率密度来描述真实姿态，假设 IMU 的精度足够高，那么它很可能不会离用 IMU 估计的姿态太远，或者换一种说法：真实姿态距离估计值很远的概率几乎为零，所以我们只要知道真实值在估计值“附近”的概率密度就足够了。这就是 MEKF 的核心思想。在 MEKF 中，真实值用估计值“加上”一个小的旋转矢量

来表示，这个

实际就属于 SO(3) 的切空间，因为切空间是三维线性空间，所以正态分布，卡尔曼滤波器都可以拿来应用。更重要的是，这个切空间在

“附近”是等价于 SO(3) 的，因此如果描述

的正态分布集中在

附近，那么它的概率密度是真实姿态的概率密度极好的近似。

通过上面的叙述，MEKF 已经很完美了，只要传感器够精确，它用到近似的步骤是很少的。但 MEKF 并非没有进步空间：首先，如果传感器不够精确，

在距离

很远处的概率密度都不消失，那么它就不是真实姿态概率密度的很好近似了。其次，切空间的定义是依赖于姿态本身的，也就是说如果

和

是两个不同的姿态，那么在

处的切空间和在

处的切空间是不同的。具体到 MEKF 中，将

从第

步的

处的切空间转移到第

步

处的切空间不可避免地要用到线性近似。下面我们介绍一些试图解决第一个问题的方法。

2. 利用 SO(3) 上的概率分布

为了解决当估计的精度较低时，

无法很好地近似姿态的概率分布的问题，最好的方法就是使用直接定义在 SO(3) 群上的概率分布。在上一篇文章中我介绍了两种 SO(3) 上的分布，分别是 Matrix Fisher 分布和 Bingham 分布，并且这两种分布是等价的。Matrix Fisher 分布在姿态估计中最早的应用是 Markley 在 2003 年的 [1] 中做出的，但似乎 Markley 本人并不知道这种概率分布已经存在了。事实上在 [1] 中，Markley 自己定义了一种在

上分布，用来同时估计姿态和陀螺仪的零偏。他采用的方法是对陀螺仪运动方程对应的 Fokker Planck 方程做了近似，使近似后的解符合他所定义的概率分布的形式，然后求解参数的变化完成先验概率的递推，再用贝叶斯公式得到后验概率。

第一次在 Matrix Fisher 分布的框架下进行姿态估计是由 Lee 在 2018 年 [2] 中完成的，他的做法是近似求解旋转矩阵

的数学期望在陀螺仪运动方程下的变化，再用 Matrix Fisher 分布的最大似然估计将

的数学期望转化为该分布的参数，从而完成先验公式的递推。在贝叶斯公式下，直接进行姿态观测和间接观测一些惯性坐标系下固定的矢量，Matrix Fisher 分布的形式不发生变化，因此可以很容易求得后验分布。这也是使用 Matrix Fisher 分布的好处之一，可以直接使用矢量观测而不需要求解 Wahba 问题。使用 Bingham 分布进行姿态估计几乎是 Glover 等人在 2013 年 [3]，和 Kurz 等人在 2014 年 [4] 中同时做出的。因为 Bingham 分布和 Matrix Fisher 分布是等价的，所以这种滤波器和基于 Matrix Fisher 分布的滤波器几乎完全一致。

使用定义在 SO(3) 上的概率分布解决了低精度带来的问题，但代价是运算量几乎提高了上百倍。原因是从

或者

的数学期望，通过 Matrix Fisher 分布和 Bingham 分布的最大似然估计，反推它们的参数时，涉及到非常复杂的计算。我曾经比较过利用 Bingham 分布设计的滤波器和 MEKF [5]，得到的结果是使用 Bingham 分布对精度的提升非常有限，几乎可以忽略不计。目前我能看到的这种新的滤波方法的好处有两个：首先，它在数学上比 MEKF 更完美，使用了更少的近似（可能大多数人看来这不能算是好处）；其次，当估计姿态的误差实际很大，但滤波器的不确定性却很低时，新的滤波方法对比 MEKF 有很大的精度提升，而这种情况可能在 IMU 经历剧烈的运动（如碰撞）时遇到。另外，除了 Markley 在 [1] 中的算法，其它算法目前还不能估计陀螺仪的零偏。

3. 如何定义 SO(3) 上的随机过程

为了在数学上更严格地处理姿态在连续时间下的滤波问题，定义在 SO(3) 上演化的随机过程是最基础的一步。可能大家在学习卡尔曼滤波器时只接触到它的离散形式，但它确实是在连续时间随机过程的框架下推导的，离散时间的应用可以看作是一种特殊形式。

目前最经常被用到的 SO(3) 上的随机过程可以用下面的方程描述 [1,2,6]：

（1）

其中

是角速度，

是一个常数矩阵，

是标准三维维纳过程，

还是从

到

的同构映射。这种定义方法的直观理解就是角速度受到一个高斯白噪声的干扰，而这个高斯白噪声也要通过指数映射变为姿态

的噪声。方程 (1) 的积分只能使用 Stratonovich 定义，而不能使用 Ito 定义，否则积分出的姿态会脱离 SO(3) [1,6]。

上面介绍的定义方法是根据 SO(3) 的李群性质得到的，还有一种完全忽略其群的结构，直接用黎曼流形的性质来定义，具体做法可以参考 [7]。利用黎曼几何方法定义一般流形上的随机过程有一整套数学理论 [8]，但我还不清楚如果在 SO(3) 定义合适的度量，用这种方法定义的随机过程是否和 (1) 式定义相同。这一部分内容涉及到比较复杂的数学内容，我也还在学习过程中，另外一个相关的参考资料是 [9]。

最后，在 [6] 中作者给出了利用公式 (1) 设计的滤波器，但其核心思想还是用切空间中的正态分布来近似姿态的概率分布，因此该滤波器和 MEKF 几乎完全一致。在 [7] 中作者描述了使用利用黎曼几何定义的随机过程的滤波器，但没有给出具体算法。

4. 总结

这篇文章首先介绍了 MEKF 的几何解释，即用切空间中的正态分布近似姿态的概率分布，这种近似在 IMU 的精度较差时变得不太准确。有一种解决方案是利用定义在 SO(3) 上的概率分布，即 Matrix Fisher 分布和 Bingham 分布来设计滤波器。这种新的滤波器相比 MEKF 精度的提升非常有限，但计算量的代价却很大。此外如何定义在 SO(3) 上的随机过程，并基于这些定义设计连续时间的滤波器，还是一个没有完全解决的问题。

参考文献

[1] Markley, F. Landis. "Attitude filtering on SO (3)."The Journal of the Astronautical Sciences54.3-4 (2006): 391-413.

[2] Lee, Taeyoung. "Bayesian attitude estimation with the matrix Fisher distribution on SO (3)."IEEE Transactions on Automatic Control63.10 (2018): 3377-3392.

[3] Glover, Jared, and Leslie Pack Kaelbling. "Tracking 3-D rotations with the quaternion Bingham filter." (2013).

[4] Kurz, Gerhard, et al. "Recursive Bingham filter for directional estimation involving 180 degree symmetry."Journal of Advances in Information Fusion9.2 (2014): 90-105.

[5] Wang, Weixin, and Peter G. Adamczyk. "Comparison of Bingham Filter and Extended Kalman Filter in IMU Attitude Estimation."IEEE Sensors Journal19.19 (2019): 8845-8854.

[6] Barrau, Axel, and Silvere Bonnabel. "Intrinsic filtering on SO (3) with discrete-time observations."52nd IEEE Conference on Decision and Control. IEEE, 2013.

[7] Solo, Victor. "Attitude estimation and Brownian motion on SO (3)."49th IEEE Conference on Decision and Control (CDC). IEEE, 2010.

[8] Émery, Michel.Stochastic calculus in manifolds. Springer Science & Business Media, 2012.

[9] Chirikjian, Gregory S.Stochastic Models, Information Theory, and Lie Groups, Volume 2: Analytic Methods and Modern Applications. Vol. 2. Springer Science & Business Media, 2011.

后记

这篇文章应该是这个专栏的最后一篇文章了。到此为止，读者朋友如果参考专栏文章和其中的参考文献，应该已经可以基本应用 MEKF 算法，并对姿态估计的最新进展有一些了解。这个专栏没有谈到的一个重要话题，是如何同时估计 IMU 的姿态和陀螺仪的零偏。这一点 Mahony 的互补滤波器（第三篇文章中的 [2]），和 MEKF（第六篇文章中的 [5]）都可以做到。此外，在第六篇文章的 [5] 中，作者介绍了如何在 MEKF 的基础上同时估计姿态、速度、位置、陀螺仪和加速度计零偏等 15 个状态量，如果对用 IMU 做 slam 感兴趣的朋友可以参考。第六篇文章中的 [6] 的作者是用 IMU 做 slam 的大佬，请见

此外，IMU 的姿态估计还可以使用无迹卡尔曼滤波器，和粒子滤波器等，这些方法都是很标准的非线性滤波方法，可以很容易搜索到相关文献。

我现在的研究领域就是三维姿态的滤波方法，[2] 的作者是我目前的博士导师。具体研究的项目是改进 [1] 中定义在

上的概率分布，并用它设计同时估计姿态和陀螺仪零偏的滤波器。如果有感兴趣的老师同学欢迎和我私信讨论。