经典文献阅读之--Kinematic-ICP(动态优化激光雷达与轮式里程计融合)

0. 简介

传统的激光雷达里程计系统通过点云配准来计算移动机器人的自运动（ego-motion），但它们通常没有考虑机器人的运动学特性，这可能导致不准确的运动估计，特别是在机器人不可能发生某些运动（如沿z轴的小幅移动）的情况下。

《Kinematic-ICP: Enhancing LiDAR Odometry with Kinematic Constraints for Wheeled Mobile Robots Moving on Planar Surfaces》的创新在于将机器人运动学模型整合到ICP优化中，使得估计的运动轨迹更符合机器人的实际运动特性。该系统特别针对在平面表面上移动的轮式移动机器人（如仓库、办公室、医院中的机器人），通过将轮式机器人的运动学约束引入传统的迭代最近点算法（ICP）优化过程中，从而提高里程计的精度。本文的研究重点在于改进传统的激光雷达里程计系统，通过动态调整激光雷达测量数据和轮式里程计数据的权重，使得系统能够应对诸如特征稀少的走廊等退化场景，并且能够在不同的环境中（如大型仓库或户外环境）取得更高的精度。这项研究已经在全球多个仓库的机器人系统中进行了部署，验证了其在实际场景中的有效性。相关的算法已经在Github上开源了。

图1：鉴于移动机器人局部一致但全局漂移的轮式里程计，我们的方法利用LiDAR数据和平台的运动学模型对里程计进行精细化处理。我们的方法计算出的轨迹大约为10公里。

1. 主要贡献

本文的主要贡献是一个新颖的3D LiDAR里程计系统，该系统与轮式移动机器人的运动学模型紧密结合。我们的系统能够准确且快速地估计机器人姿态，甚至在当前最先进系统表现不佳的挑战性场景中也能超越传感器的帧率。我们声称，我们的方法可以实现以下目标：

1. 在对估计运动施加运动学约束的同时，纠正移动机器人的轮式里程计；

2. 以与最先进的LiDAR里程计系统相当或更好的精度计算里程计；

3. 动态调整LiDAR测量与轮式里程计读数之间的权重，从而在多样化和具有挑战性的环境中提高鲁棒性和准确性。

2. 使用点对点ICP的激光雷达里程计

我们的方法将轮式机器人的运动学模型整合到配准方案中，以顺序估计其运动。主要组件基于KISS-ICP [29]，我们在本节中简要回顾以确保完整性并引入相关符号。

为了获得机器人在时刻 $t$ 的里程计框架中的位姿 $T_t\in \mathbb{SE}(3)$，我们首先对传感器框架中表达的输入点云 P = { p i ∣ p i ∈ R 3 } P = \{p_i | p_i \in \mathbb{R}^3\} P={pi​∣pi​∈R3} 进行预处理，通过去偏斜和体素下采样得到 ${\hat{P}}^{\ast }$。然后，我们通过传感器与机体框架之间的外部标定 $C\in \mathbb{SE}(3)$ 将该点云转换到机体框架，得到点云 S = { s i ∣ s i ∈ R 3 } S = \{s_i | s_i \in \mathbb{R}^3\} S={si​∣si​∈R3}。给定之前的机器人位姿估计 $T_{t-1}$ 和相对里程计测量 $O_t\in SE(3)$，我们计算当前机器人位姿的初始猜测为：

$${\hat{T}}_t=T_{t-1}{O}_t.(1)$$

然后，我们使用点对点ICP算法来细化该估计。在每次迭代中，我们获得源点云 $S$ 和我们的局部地图点 Q = { q i ∣ q i ∈ R 3 } Q = \{q_i | q_i \in \mathbb{R}^3\} Q={qi​∣qi​∈R3} 之间的一组对应关系，这些点存储在体素网格中，如KISS-ICP [29]所示。我们定义点 $q$ 和通过 $T$ 变换后的点 $s$ 之间的残差 $r$ 为：

$$r(T)=Ts-q.(2)$$

然后，我们定义我们的点对点代价函数为：

$$\chi ({\hat{T}}_t)=\frac{1}{|C|}\sum _{(s,q)\in C}{\Vert r\left({\hat{T}}_t\right)\Vert }_2^2,(3)$$

其中 $C$ 是最近邻对应关系的集合，$|C|$ 是此类对应关系的数量。我们可以以最小二乘的方式最小化方程 (3)：

$$\Delta u=\arg \underset{\Delta u}{\min }\chi ({\hat{T}}_t⊞\Delta u),(4)$$

其中 $\Delta u$ 是ICP修正向量，$\oplus$ 将修正向量应用于当前的位姿估计。这个过程，包括最近邻对应关系搜索和最小二乘优化，重复进行直到收敛，最终得到新的位姿估计 $T_t$。收敛后，我们使用注册扫描的下采样版本更新地图。

3. 在ICP中引入运动学约束

给定来自一次ICP迭代的修正向量 $\Delta u\in {\mathbb{R}}^N$，我们可以使用以下公式更新我们的姿态估计 ${\hat{T}}_t$：

$${\hat{T}}_t={\hat{T}}_t⊞\Delta u={\hat{T}}_t\cdot \text{Exp}(f(\Delta u)),\text{\hspace{1em}(5)}$$

其中 $f:{\mathbb{R}}^N\to {\mathbb{R}}^6$ 表示所使用的移动平台的集成运动学模型 [24]，而 $\text{Exp}$ 是$\mathbb{SE}(3)$的指数映射。函数 $f$ 用于修正来自平台里程计的初始猜测 $\hat{T}$。因此，可以使用与移动平台的物理配置不同的运动学模型，因为我们实际上是在建模运动修正，而不是两个扫描之间的相对运动。直观上，$\Delta u\in {\mathbb{R}}^N$ 表示需要施加于机器人以修正运动的 $N$ 个集成控制输入。虽然我们的框架可以应用于任何机器人运动学，但我们希望专注于基于单轮车的修正，因为这可以广泛应用于大多数轮式移动机器人 [24]、类人机器人 [4]，甚至船只 [30]。在这种情况下，$\Delta u=[\Delta x,\Delta \theta {]}^T\in {\mathbb{R}}^2$ 表示线性和角位移，且

$$f(\Delta u)=\left(\begin{array}{cccccc}\Delta x\frac{\sin (\Delta \theta )}{\Delta \theta +ϵ}& \frac{1-\cos (\Delta \theta )}{\Delta \theta +ϵ}& 0& 0& 0& \Delta \theta \end{array}\right),\text{\hspace{1em}(6)}$$

其中 $ϵ$ 是一个小常数，通常设置为可以表示为浮点数的最小正数。当以最小二乘法求解更新当前位姿估计方程 (4)时，我们需要计算当前激光雷达点云与前一帧点云进行对齐，通过寻找对应的点方程 (2) 对来计算关于修正向量 $\Delta u$ 的雅可比矩阵。通过观察方程 (5)，可以使用链式法则计算该雅可比矩阵：

其中 ${\hat{R}}_t$ 是 ${\hat{T}}_t$ 的旋转部分，$I\in {\mathbb{R}}^{3\times 3}$ 是单位矩阵，而 $[s{]}_{\times }\in {\mathbb{R}}^{3\times 3}$ 是从点 $s\in {\mathbb{R}}^3$ 计算得到的反对称矩阵。在通过方程 (6) 表达的基于单轮车的修正中，我们可以计算出雅可比矩阵 $J_{\text{kinematic}}\in {\mathbb{R}}^{6\times 2}$ 为：

$$J_{\text{kinematic}}=\left(\begin{array}{cccccc}1& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 1\end{array}\right),\text{\hspace{1em}(8)}$$

在轮式移动机器人系统中，由于轮滑、机械磨损或不平坦表面等因素，旋转估计通常比平移估计更为噪声较大。轮编码器通常提供可靠的平移估计，但在旋转方面的可靠性较低，尤其是在存在外部干扰的情况下。实际上，我们希望我们的激光雷达修正更加关注估计的旋转部分，同时更信任来自轮编码器的平移部分。这可以帮助系统在一些退化场景中，例如在一条没有特征的直走走廊中移动，或通过狭窄通道进入一个之前未见过的环境部分时，激光雷达扫描无法完全确定机器人姿态的情况下。这种知识可以通过向成本函数添加正则化项来引入优化中，形式为：

$$S({\hat{T}}_t)=\chi ({\hat{T}}_t)+\frac{1}{{\beta }_t}{\Vert {\text{Log}}_t\underset{D_t}{\underbrace{\left(T_{t-1}{O}_t{\hat{T}}_t^{-1}\right)}}\Vert }^2,\text{\hspace{1em}(9)}$$

其中 $D_t\in SE(3)$ 是当前估计与轮里程计初始猜测之间的偏差，而 ${\text{Log}}_t(D_t)$ 提取 $D_t$ 的平移部分。方程 (9) 是一个高度非线性的成本函数，但我们可以通过优雅地对 $\Delta u$ 的平移部分进行正则化来近似它，形式为：

$$G({\hat{T}}_t⊞\Delta u)=\chi ({\hat{T}}_t⊞\Delta u)+\frac{1}{{\beta }_t}\Delta x^2,\text{\hspace{1em}(10)}$$

其中 $\Delta x\in \mathbb{R}$ 是 $\Delta u$ 的平移部分，${\beta }_t$ 与我们希望施加在 $\Delta x$ 上的正则化量成反比。在我们的情况下，当轮里程计不可靠时，我们希望 ${\beta }_t$ 的值较大，因此系统应更加关注激光雷达测量。相反，当系统应更信任轮里程计时，我们希望 ${\beta }_t$ 的值较小。为了实现这种行为并避免手动参数调优，我们以数据驱动的方式计算 ${\beta }_t$ 的值为：

$${\beta }_t=\chi (T_{t-1}{O}_t).\text{\hspace{1em}(11)}$$

这意味着我们在轮里程计的初始猜测下评估最小化误差函数方程，即所有点对之间的残差平方和，来优化位姿$T_t$有效地考虑了轮里程计与激光雷达测量之间的一致性。正如我们在实验中所展示的，引入 ${\beta }_t$ 提高了系统的鲁棒性，特别是在退化场景中，同时也提升了系统的准确性。

即通过计算轮式里程计初始估计下的ICP误差来确定 ${\beta }_t$ 的值。具体步骤如下：
计算误差：系统首先计算轮式里程计提供的初始位姿估计与激光雷达数据进行ICP匹配的误差 $\chi (T_{t-1}{O}_t)$。
调整权重：根据该误差值来动态调整 ${\beta }_t$ 的大小：

• 如果误差较大（即轮式里程计的估计不准确），系统将减小 ${\beta }_t$ 的值，从而让系统更依赖激光雷达数据进行位姿估计。
• 如果误差较小（即轮式里程计提供了较为可靠的估计），则增大 ${\beta }_t$，让系统更多依赖轮式里程计数据。

通过动态调整激光与轮式里程计的权重，可以在下述场景中得到更高的里程计精度：

• 特征丰富的场景：例如复杂的室内环境，激光雷达能够获得充足的点云数据并提供高精度的位姿估计。此时 ${\beta }_t$ 会减小，让系统更多依赖激光雷达数据进行优化。

• 特征稀缺的场景：例如在狭窄走廊或空旷区域，激光雷达点云数据稀少或质量不佳。此时 ${\beta }_t$ 会增大，系统会更多依赖轮式里程计的估计来获得更稳定的位姿更新。

• 轮式里程计不可靠的场景：例如在不平整的地形上移动或轮胎打滑时，轮式里程计的精度可能会显著下降。此时，系统通过检测轮式里程计和激光雷达数据之间的偏差，动态减小 ${\beta }_t$，从而增加激光雷达数据的影响。

4. 环境编译

该开源框架是基于ros2的，编译方式如下：

cd <your_ros_workspace>/src
git clone https://github.com/PRBonn/kinematic-icp
cd ..rosdep install --from-paths src --ignore-src -r -ycolcon buildsource ./install/setup.bash

运行方式如下：

ros2 launch kinematic_icp online_node.launch.py lidar_topic:=<TOPIC> visualize:=true

5. 总结

在本文提出了Kinematic-ICP，这是一种新颖的激光雷达里程计方法，明确将移动机器人的运动学约束融入经典的点对点ICP算法中。该方法利用单轮车运动模型的知识，估计与轮式移动平台的自然运动更加一致的里程计。自适应正则化机制使系统能够适应退化条件，确保在传统激光雷达里程计系统面临挑战的场景中（如特征稀疏环境）仍能保持稳健的性能。