R语言怎么写积分_2. 角速度的积分

IMU 姿态估计的第一步是将陀螺仪（gyroscope）测量的角速度积分成陀螺仪的姿态。在早期的惯性导航应用中，因为计算机计算能力有限，人们曾经使用双速度积分法：高速一阶积分和中速高阶积分，从而在保证积分精度的前提下降低计算量 [1]。在现在 IMU 的低精度应用中，一般单片机的计算性能足以保证在 IMU 的测量带宽内使用最精确的积分方法，因此我们没有必要讨论 [1] 中的双速度积分法。在这篇文章中，我们将介绍三种形式的积分方法：分别使用旋转矩阵、四元数和欧拉角。在这些方法中，我们不考虑地球自身的旋转和地球的曲率，因此只限于小范围内的 IMU 的应用。在开始之前，我们先做一个约定：在本文中所有的角速度

指旋转坐标系（IMU坐标系）相对于固定坐标系（地球坐标系）的角速度，并且

的坐标表示在旋转坐标系中。

1. 李群（Lie Group）的基础知识 [2]

尽管刚体的姿态有很多种不同的表示方法，它们的积分方法也不相同，但这其中有一些共通的地方，在这里作一些简要但不严格的介绍。刚体旋转群 SO(3) 是一个李群，所谓李群是指同时具有光滑流形和群的结构的空间，同时群内的运算也是光滑的。李群的定义需要花很长篇幅解释清楚，也不重要，在这篇文章中我们需要知道的是 SO(3) 上的每一个旋转，都有一个切空间。这个切空间是一个线性空间，它的维数与 SO(3) 相同，即具有三个自由度。在李群的语言中，切空间也叫做李代数（Lie Algebra）。切空间的直观理解是：切空间内的每一个向量代表了一个速度，并且这个速度可以通过李群的性质积分成一个位移，而这个积分就是李群的指数映射（exponential map）。可以证明指数映射在局部是一一对应的，也就是每个速度都对应了唯一一个位移。

刚体旋转群 SO(3) 的李代数

是三维的反对称矩阵：

(1)

作为一个线性空间，

和三维实数空间

是等距同构的，这也是为什么刚体的角速度可以表示在

中的原因。巧合的是，SO(3) 群的指数映射和上一篇文章中介绍的指数矩阵函数是相同的，因此

可以表示为

。

在单位四元数表示中，因为

和

表示相同的三维旋转，因此全部不同的单位四元数组成的空间是三维实射影空间

（real projective space，即在三维单位球面上关于原点对称的两点被认为是一个点）。

和 SO(3) 是李群同态的，因此 SO(3) 的指数映射对于单位四元数也适用，但有不同的表示方法：如果

，那么四元数的指数映射可以表示为四元数的指数函数

。

2. 旋转矢量的微分

如果刚体的姿态由旋转矢量

表示，同时刚体具有角速度

，那么旋转矢量关于时间

的导数可以写成如下的公式 [3]：

(2)

其中

。可以看到这个公式是关于

的微分方程，但它太过复杂了，很难找到解析解。因此在实际应用中，很少见到使用旋转矢量进行积分的方法。

3. 体坐标系

欧拉角的积分

如果用

（yaw-pitch-roll，航向角-俯仰角-横滚角）表示三个欧拉角

，那么它们关于时间

的导数可以表示为 [4]：

(3)

我从未见到过这个微风方程的解析解，在一些相对外行的文章中可以看到用一阶近似的形式对欧拉角积分：

。在 [4] 中，作者使用了二阶近似的形式对欧拉角积分，这样会提升一些精度，具体公式请参考 [4] 中的公式 (14)。而没有解析形式的解，也极大地限制了欧拉角在 IMU 姿态估计中的使用。

4. 旋转矩阵的积分

旋转矩阵

关于时间

的导数可以表示为 [5]:

(4)

不同于旋转矢量和欧拉角，这个微分方程是有解析解的，它的解可以用指数映射表示：

(5)

因此在精度上，旋转矩阵的积分相比于旋转矢量和欧拉角具有明显的优势，因为它在积分的过程中没有忽略任何高阶小量。关于指数映射的简便计算方法，请参考上一篇文章“姿态的表示方法”中的公式 (4)。因为指数映射的性质，

也是一个旋转矩阵，所以 (5) 自动保证了积分结果

是旋转矩阵，而不需要在积分后采用任何正交化的操作。

需要注意的是，在公式 (5) 中我们需要假设在

的时间内，角速度

是固定的，也就是说角加速度

。这需要我们在 IMU 的测量中使用高的采样频率，同时需要保证运动不是非常激烈（例如避免碰撞等高带宽的运动）。如果考虑角加速度

，也可以推导出比较复杂的积分公式（[5,6]中对四元数进行了推导），但这个公式不是解析解，需要舍去一些高阶小量，因此无法保证积分结果的正交性。

5. 单位四元数的积分

单位四元数

关于时间

的导数与旋转矩阵非常类似，但因为单位四元数和三维旋转是2：1的对应关系，在公式中需要对角速度减半：

(6)

其中

。因为单位四元数和旋转矩阵具有李群同态的关系，所以大部分旋转矩阵的结论可以平移到单位四元数上。微分方程 (6) 也有解析解：

(7)

关于单位四元数的指数映射的简便计算方法，请参考上一篇文章“姿态的表示方法”中的公式 (6)。这个解析解和旋转矩阵的解析解具有同样好的性质，它保证了积分结果

是单位四元数，而不需要额外的归一化的操作。如果考虑非零角加速度，积分公式可以参考 [5] 中的公式 (195) 或 [6] 中的公式 (131)。虽然这些公式提高了精度，但无法保证积分结果是单位四元数。

6. 总结

在这篇文章中，我们介绍了使用欧拉角、旋转矩阵和单位四元数对 IMU 测量得到的角速度进行积分。其中使用欧拉角的积分方法是其运动学方程的近似解，而使用旋转矩阵和单位四元数的积分方法是运动学方程的解析解。因此我更推荐使用旋转矩阵或四元数对角速度进行积分。另外，如果考虑非零的角加速度，也可以使用精度更高但更复杂的积分公式。

参考文献

[1] Savage, P.G., 1998. Strapdown inertial navigation integration algorithm design part 1: Attitude algorithms.Journal of guidance, control, and dynamics,21(1), pp.19-28.

[2] Hall, B., 2015.Lie groups, Lie algebras, and representations: an elementary introduction(Vol. 222). Springer.

[3] Shuster, M.D., 1993. A survey of attitude representations.Navigation,8(9), pp.439-517.

[4] Foxlin, E., 1996. Inertial head-tracker sensor fusion by a complementary separate-bias Kalman filter.

[5] Sola, J., 2017. Quaternion kinematics for the error-state Kalman filter.arXiv preprint arXiv:1711.02508.

[6] Trawny, N. and Roumeliotis, S.I., 2005. Indirect Kalman filter for 3D attitude estimation.University of Minnesota, Dept. of Comp. Sci. & Eng., Tech. Rep,2, p.2005.