激光雷达和 IMU 标定中的运动方程

Title: 激光雷达和 IMU 标定中的运动方程

前言

阅读论文中^[1]遇到刚体运动学, 好久没用都忘了, 从新推导记录一下. 事实上, 关于自由刚体运动学和动力学有比较多的细节需要注意和留心, 不然很容易出错.

1 问题描述

激光 3D SLAM 中, 激光雷达和 IMU 一般都固定在一起, 或者 Lidar 已经自带 IMU, 或者在 Lidar 附近固定外置 IMU. Lidar 和 IMU 相对位置固定, 两者一起运动, 如 Fig. 1 所示. 即使是这样简单的固定关系, 运动学的加速度关系也不直观.

Fig. 1 Lidar 与 IMU 的位置关系

2 刚体运动

Fig 1. 中 Lidar、IMU、结构平板固联在一起, 作为一个刚体.

按照哈工大理论力学教科书上自由刚体运动建模方法^[2], 以 Lidar 的原点为基点, 在基点上安装一个始终保持平移的坐标系 { ξ } \{\xi\} {ξ}, 如 Fig. 2 所示.

- 平移坐标系 { ξ } \{\xi\} {ξ} 在全局坐标系 { G } \{G\} {G} 中只能自由平移 (不能转动). 平移运动通过基点在全局坐标系 { G } \{G\} {G} 中的坐标 ${}^G{\mathbf{p}}_{\xi }$、 对应速度 ${}^G{\mathbf{v}}_{\xi }$ 和对应加速度 ${}^G{\mathbf{a}}_{\xi }$ 来描述;

- 平移坐标系 { ξ } \{\xi\} {ξ} 与 Lidar 坐标系 { L } \{L\} {L} 原点 “钉” 在一起, Lidar 坐标系 { L } \{L\} {L} 绕平移坐标系 { ξ } \{\xi\} {ξ} 原点自由转动. 自由转动通过刚体相对于平移坐标系 { ξ } \{\xi\} {ξ} 的欧拉角、角速度 ${}^{\xi }{\mathbf{\omega }}_L$ 和角加速度 ${}^{\xi }{\mathbf{\Omega }}_L$ 来描述.

平移坐标系 { ξ } \{\xi\} {ξ} 与 Lidar 坐标系 { L } \{L\} {L} 原点重合, 故有
$${}^G{\mathbf{p}}_{\xi }={}^G{\mathbf{p}}_L\text{\hspace{1em}(2-1)}$$

$${}^G{\mathbf{v}}_{\xi }={}^G{\mathbf{v}}_L\text{\hspace{1em}(2-2)}$$

$${}^G{\mathbf{a}}_{\xi }={}^G{\mathbf{a}}_L\text{\hspace{1em}(2-3)}$$

刚体上的 Lidar、IMU、结构平板遵从同一个刚体运动, 故有
$${}^G{\mathbf{\omega }}_L={}^{\xi }{\mathbf{\omega }}_L={}^{\xi }{\mathbf{\omega }}_I\text{\hspace{1em}(2-4)}$$

$${}^G{\mathbf{\Omega }}_L={}^{\xi }{\mathbf{\Omega }}_L={}^{\xi }{\mathbf{\Omega }}_I\text{\hspace{1em}(2-5)}$$

Fig. 2 刚体自由运动的坐标系

3 位置方程

刚体上任意点的位置矢量需要根据该点与刚体基点的相对位置进行计算. IMU 的位置矢量为
$${}^G{\mathbf{p}}_I={}^G{\mathbf{p}}_{\xi }+{}^{\xi }{\mathbf{p}}_I\text{\hspace{1em}(3-1)}$$

4 速度方程

对式 (3-1) 求导获得 IMU 的速度矢量
$${}^G{\mathbf{v}}_I={}^G{\mathbf{v}}_{\xi }+{}^{\xi }{\mathbf{v}}_I\text{\hspace{1em}(4-1)}$$
其中相对速度 ${}^{\xi }{\mathbf{v}}_I$ 是由相对转动 (方向改变) 引起的, 故有
$${}^{\xi }{\mathbf{v}}_I={}^{\xi }{\dot{\mathbf{p}}}_I={}^{\xi }{\mathbf{\omega }}_I\times {{}^{\xi }\mathbf{p}}_I\text{\hspace{1em}(4-2)}$$
式 (4-2) 代入式 (4-1) 得到
$${}^G{\mathbf{v}}_I={}^G{\mathbf{v}}_{\xi }+{}^{\xi }{\mathbf{\omega }}_I\times {{}^{\xi }\mathbf{p}}_I\text{\hspace{1em}(4-3)}$$

5 加速度方程

对式 (4-3) 求导得到加速度方程
$${}^G{\mathbf{a}}_I={}^G{\mathbf{a}}_{\xi }+{}^{\xi }{\dot{\mathbf{\omega }}}_I\times {{}^{\xi }\mathbf{p}}_I+{}^{\xi }{\mathbf{\omega }}_I\times {{}^{\xi }\dot{\mathbf{p}}}_I\text{\hspace{1em}(5-1)}$$
已知
$${}^{\xi }{\dot{\mathbf{\omega }}}_I={}^{\xi }{\mathbf{\Omega }}_I\text{\hspace{1em}(5-2)}$$
式 (5-2) 和式 (4-2) 代入式 (5-1) 得到
$${}^G{\mathbf{a}}_I={}^G{\mathbf{a}}_{\xi }+{}^{\xi }{\mathbf{\Omega }}_I\times {{}^{\xi }\mathbf{p}}_I+{}^{\xi }{\mathbf{\omega }}_I\times {}^{\xi }{\mathbf{\omega }}_I\times {{}^{\xi }\mathbf{p}}_I\text{\hspace{1em}(5-3)}$$
IMU 加速度计测的是全局惯性坐标系 {G} 中的加速度值, 而输出的加速度测量值是以 IMU 坐标系在全局惯性空间中的瞬时重合的坐标系 { ι } \{\iota\} {ι} 为参照坐标系. 换句话就是, IMU 输出值是全局坐标系中的加速度测量值在 IMU 瞬时坐标系中的描述. IMU 原始加速度测量值记为 ${}^{\iota }{\mathbf{a}}_I$.

坐标系 { ι } \{\iota\} {ι} 仍是惯性坐标系, 和全局坐标系 { G } \{G\} {G} 存在姿态转换关系. 能够获得 IMU 加速度测量值的坐标转换关系如下
$${}^G{\mathbf{a}}_I={}^G{\mathbf{R}}_{\iota }{}^{\iota }{\mathbf{a}}_I={}^G{\mathbf{R}}_I{}^{\iota }{\mathbf{a}}_I\text{\hspace{1em}(5-4)}$$
将式 (5-4)、(2-1)、(2-3)、(2-4)、(2-5) 代入整理得到
$${}^G{\mathbf{R}}_I{}^{\iota }{\mathbf{a}}_I=\underset{基点加速度}{\underbrace{{}^G{\mathbf{a}}_L}}+\underset{转动加速度}{\underbrace{{}^G{\mathbf{\Omega }}_L\times {{}^{\xi }\mathbf{p}}_I}}+\underset{向心加速度}{\underbrace{{}^G{\mathbf{\omega }}_L\times {}^G{\mathbf{\omega }}_L\times {{}^{\xi }\mathbf{p}}_I}}\text{\hspace{1em}(5-5)}$$
向量乘可以写成 $[\cdot {]}_{\times }$ 形式
$${}^G{\mathbf{R}}_I{}^{\iota }{\mathbf{a}}_I={}^G{\mathbf{a}}_L+[{}^G{\mathbf{\Omega }}_L{]}_{\times }{{}^{\xi }\mathbf{p}}_I+[{}^G{\mathbf{\omega }}_L{]}_{\times }^2{{}^{\xi }\mathbf{p}}_I\text{\hspace{1em}(5-6)}$$
这里强调 ${{}^{\xi }\mathbf{p}}_I$, 与 ${{}^L\mathbf{p}}_I$ 之间存在坐标转换关系如下
$${{}^{\xi }\mathbf{p}}_I={{}^{\xi }\mathbf{p}}_L+{}^{\xi }{\mathbf{R}}_L{{}^L\mathbf{p}}_I={}^{\xi }{\mathbf{R}}_L{{}^L\mathbf{p}}_I={}^G{\mathbf{R}}_L{{}^L\mathbf{p}}_I\text{\hspace{1em}(5-7)}$$

6 夏莱定理及等价推导

上面的过程之所以能够顺利地几次求导后得到结果, 关键是因为设置了平移坐标系 { ξ } \{\xi\} {ξ}. 因为平移坐标系是惯性坐标系或者平移坐标系的方向不会改变, 不会在求导时考虑所在坐标系方向的改变. 事实上, 这里有一个刚体运动定理支撑

夏莱定理

刚体最一般的位移可以分解为随任选基点的平动位移和绕通过基点的某个轴的转动.

选择刚体上不同的点为基点, 这种分解方法不唯一;

选择不同的基点时平动位移的长度和方向将改变, 而转动轴的方向和转角不依赖于基点的旋转.

如果不引入平移坐标系 (不使用夏莱定理), IMU 的位置矢量可以写成
$${}^G{\mathbf{p}}_I={}^G{\mathbf{p}}_L+{}^G{\mathbf{R}}_L{}^L{\mathbf{p}}_I\text{\hspace{1em}(6-1)}$$
两边求导得到
$${}^G{\dot{\mathbf{p}}}_I={}^G{\dot{\mathbf{p}}}_L+{}^G{\dot{\mathbf{R}}}_L{}^L{\mathbf{p}}_I+{}^G{\mathbf{R}}_L{}^L{\dot{\mathbf{p}}}_I\text{\hspace{1em}(6-2)}$$
其中
$${}^L{\dot{\mathbf{p}}}_I=0\text{\hspace{1em}(6-3)}$$
参考流形上的运动学 - Notes for “Kalman Filter on Differentiable Manifolds“ - IKFoM (I) 中姿态矩阵的导数小节可知
$${}^G{\dot{\mathbf{R}}}_L=[{}^{\mathbf{G}}{\mathbf{\omega }}_{\mathbf{L}}{]}_{\times }{}^G{\mathbf{R}}_L\text{\hspace{1em}(6-4)}$$
式 (6-3) 和式 (6-4) 代入式 (6-2) , 并结合式 (5-7) 得到
$$\begin{gathered} \begin{array}{rl}{}^G{\dot{\mathbf{p}}}_I& ={}^G{\dot{\mathbf{p}}}_L+[{}^{\mathbf{G}}{\mathbf{\omega }}_{\mathbf{L}}{]}_{\times }{}^G{\mathbf{R}}_L{}^L{\mathbf{p}}_I\\ & ={}^G{\dot{\mathbf{p}}}_L+[{}^{\mathbf{G}}{\mathbf{\omega }}_{\mathbf{L}}{]}_{\times }{}^{\xi }{\mathbf{p}}_I\end{array} \\ \stackrel{(2-2),(2-4)}{\Rightarrow } \\ {}^G{\dot{\mathbf{v}}}_I={}^G{\dot{\mathbf{v}}}_{\xi }+[{}^{\mathbf{\xi }}{\mathbf{\omega }}_{\mathbf{I}}{]}_{\times }{}^{\xi }{\mathbf{p}}_I\text{\hspace{1em}(6-5)} \end{gathered}$$
这样又兜回到使用平移坐标系 { ξ } \{\xi\} {ξ} 时的式 (4-3).

7 外参标定方程

式 (5-6) 作为一般用途的运动方程, 却不太适合作为于激光雷达和 IMU 之间的外参标定. 原因是需要标定的外参 ${}^L{\mathbf{R}}_I$ 和 ${}^L{\mathbf{p}}_I$ 没有在方程中体现.

故需要由式 (5-6) 继续作坐标系的转换. 由姿态变换关系可知
$${}^G{\mathbf{R}}_I={}^G{\mathbf{R}}_L{}^L{\mathbf{R}}_I\text{\hspace{1em}(7-1)}$$
通过特征匹配与 Kalman 滤波方法预测得到 全局坐标系 { G } \{G\} {G} 下的 Lidar 加速度 ${}^G{\mathbf{a}}_L$, 可以转换为 Lidar 瞬时坐标系 { γ } \{\gamma\} {γ} 下的 Lidar 加速度 ${}^{\gamma }{\mathbf{a}}_L$. 所谓Lidar 瞬时坐标系 { γ } \{\gamma\} {γ} 就是指 Lidar 运动过程中与全局惯性空间瞬时重合形成的固定坐标系. 坐标转换关系为
$${}^G{\mathbf{a}}_L={}^G{\mathbf{R}}_{\gamma }{}^{\gamma }{\mathbf{a}}_L={}^G{\mathbf{R}}_L{}^{\gamma }{\mathbf{a}}_L\text{\hspace{1em}(7-2)}$$
将式 (7-1) 和式 (7-2) 代入式 (5-6) 得到
$$\begin{gathered} {}^G{\mathbf{R}}_I{}^{\iota }{\mathbf{a}}_I={}^G{\mathbf{a}}_L+[{}^G{\mathbf{\Omega }}_L{]}_{\times }{{}^{\xi }\mathbf{p}}_I+[{}^G{\mathbf{\omega }}_L{]}_{\times }^2{{}^{\xi }\mathbf{p}}_I \\ \Rightarrow {}^G{\mathbf{R}}_L{}^L{\mathbf{R}}_I{}^{\iota }{\mathbf{a}}_I={}^G{\mathbf{R}}_L{}^{\gamma }{\mathbf{a}}_L+[{}^G{\mathbf{\Omega }}_L{]}_{\times }{{}^{\xi }\mathbf{p}}_I+[{}^G{\mathbf{\omega }}_L{]}_{\times }^2{{}^{\xi }\mathbf{p}}_I\text{\hspace{1em}(7-3)} \end{gathered}$$
两边同乘以 ${}^G{\mathbf{R}}_L^{\mathrm{T}}$, 并将式 (5-7) 代入 得到
$${}^L{\mathbf{R}}_I{}^{\iota }{\mathbf{a}}_I={}^{\gamma }{\mathbf{a}}_L+{}^G{\mathbf{R}}_L^{\mathrm{T}}[{}^G{\mathbf{\Omega }}_L{]}_{\times }{}^G{\mathbf{R}}_L{{}^L\mathbf{p}}_I+{}^G{\mathbf{R}}_L^{\mathrm{T}}[{}^G{\mathbf{\omega }}_L{]}_{\times }^2{}^G{\mathbf{R}}_L{{}^L\mathbf{p}}_I\text{\hspace{1em}(7-4)}$$
参考 Notes for “A micro Lie theory for state estimation in robotics“ - 公式详细推导 (I) 可知
$$\begin{array}{rl}{}^G{\mathbf{R}}_L^{\mathrm{T}}[{}^G{\mathbf{\Omega }}_L{]}_{\times }{}^G{\mathbf{R}}_L& ={\left[{}^G{\mathbf{R}}_L^{\mathrm{T}}{}^G{\mathbf{\Omega }}_L\right]}_{\times }\\ & ={\left[{}^L{\mathbf{R}}_G{}^G{\mathbf{\Omega }}_L\right]}_{\times }\\ & ={\left[{}^{\gamma }{\mathbf{\Omega }}_L\right]}_{\times }\end{array}\text{\hspace{1em}(7-5)}$$
亦可知
$$\begin{array}{rl}{}^G{\mathbf{R}}_L^{\mathrm{T}}[{}^G{\mathbf{\omega }}_L{]}_{\times }^2{}^G{\mathbf{R}}_L& ={}^G{\mathbf{R}}_L^{\mathrm{T}}[{}^G{\mathbf{\omega }}_L{]}_{\times }{}^G{\mathbf{R}}_L{}^G{\mathbf{R}}_L^{\mathrm{T}}[{}^G{\mathbf{\omega }}_L{]}_{\times }{}^G{\mathbf{R}}_L\\ & ={\left[{}^G{\mathbf{R}}_L^{\mathrm{T}}{}^G{\mathbf{\omega }}_L\right]}_{\times }{\left[{}^G{\mathbf{R}}_L^{\mathrm{T}}{}^G{\mathbf{\omega }}_L\right]}_{\times }\\ & ={\left[{}^{\gamma }{\mathbf{\omega }}_L\right]}_{\times }^2\end{array}\text{\hspace{1em}(7-6)}$$
将式 (7-5) 和式 (7-6) 代入式 (7-4) 得到
$${}^L{\mathbf{R}}_I{}^{\iota }{\mathbf{a}}_I={}^{\gamma }{\mathbf{a}}_L+{\left[{}^{\gamma }{\mathbf{\Omega }}_L\right]}_{\times }{{}^L\mathbf{p}}_I+{\left[{}^{\gamma }{\mathbf{\omega }}_L\right]}_{\times }^2{{}^L\mathbf{p}}_I\text{\hspace{1em}(7-7)}$$
这是最终得到激光雷达和 IMU 标定中的运动方程.

总结

这是一个关于自由刚体运动学方程的推导过程.

后面的激光雷达和 IMU 的外参标定是利用得到的运动方程去做最小二乘法估计, 这是另一个有趣故事了.

（如有问题请指教, 谢谢！）

参考文献

[1] F. Zhu, Y. Ren and F. Zhang, “Robust Real-time LiDAR-inertial Initialization,” 2022 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Kyoto, Japan, 2022, pp. 3948-3955

[2] 哈尔滨工业大学理论力学教研室, 理论力学 (II), 第 8 版, 高等教育出版社, 2016