腿式机器人激光SLAM系统

点击上方“3D视觉工坊”，选择“星标”

干货第一时间送达

腿式机器人激光SLAM系统-编程知识网

作者：robot L | 来源：知乎

https://zhuanlan.zhihu.com/p/148855755

本文仅做学术分享，如有侵权，请联系删除。

主要内容

基于点云分割和CNN的LiDAR点云回环检测方法。

改进的点云配准失效检测方法，效率更高。

基于运动惯性里程计，AICP，回环的因子图LiDAR-SLAM系统，可以在腿式机器人上运行。

不足

处理频率只有1Hz，远低于激光频率10Hz。

没有和SOTA LiDAR-SLAM算法对比。

M. Ramezani, G. Tinchev, E. Iuganov, M. Fallon. Online LiDAR-SLAM for Legged Robots with Robust Registration and Deep-Learned Loop Closure. IEEE International Conference on Robotics and Automation, 2020.

摘要

在本文中，我们提出了一种基于因子图的3D LiDAR SLAM系统，组合了SOTA深度学习的基于特征的回环检测子，使得腿式机器人能够在工业环境中定位和建图。点云先通过惯性运动学状态估计器对齐，然后再使用ICP配准对齐。为了闭环，我们使用使用一种回环策略，匹配工业环境中的分割块和点云。我们离线训练描述子来匹配这些分割快。我们方法的高效性来自小心地设计网络结构以最小化参数数量，从而深度学习方法能够使用CPU实时配置，这也是本文的最大贡献。里程计和闭环因子通过位姿图优化被更新。最终我们提供了高效的风险对齐预测方法，验证了配准的可靠性。在一个工业环境中的实验结果证实了我们系统的鲁棒性和灵活性，包括在SLAM地图中的自主路径跟随。

1. 介绍

机器人建图和定位已经被研究了20多年，为许多不同任务提供了感知基础。大量研究被执行使得机器人能够在未知环境中定位，导航，并完成任务。虽然付出了大量的努力，但是自主机器人在复杂环境中长期操作仍然是开放的研究领域。

视觉SLAM已经展示出大量的进步[1][2]，专注于克服光照条件变化的挑战[3]。替代地，本文专注于LiDAR。激光测量的运动估计和建图更准确。因此，我们专注于腿式机器人的LiDAR-SLAM。

我们SLAM的核心基于ICP，用于3D配准的常见方法[13]，来对齐3D LiDAR采集的点云。我们的方法基于之前的自动调参ICP（AICP）[4]，通过分析点云内容来鲁棒化对齐。AICP的初始化由机器人的状态估计器提供[5]，通过融合惯性测量和运动学信息来估计机器人状态。我们的LiDAR里程计漂移低于1.5%。

本文主要贡献如下：

1）一种在线的3D LiDAR SLAM系统用于腿式机器人基于ICP配准。

2）一种验证度量来量化点云配准的可靠性。

3）一种基于学习的闭环检测，在移动CPU上实时运行。

4）算法在ANYmal四足机器人上的实时验证。

图1. 在ANYmal机器人上的3D LiDAR建立的工业场地地图的俯视角。

2. 相关工作

本节提供了文献综述在行走机器人的感知和LiDAR-SLAM系统方面。

A 行走机器人的感知

SLAM对于行走机器人是一项关键能力。示例系统包括运行在HRP-2人形机器人上的单目视觉SLAM系统[6]，其基于EKF框架。相反地，[7]利用粒子滤波器来估计SLAM方法的后验，也在HRP-2上进行。Oriolo等人[8]验证了视觉里程计在Nano bipedal机器人上通过紧耦合视觉信息和机器人的运动学和惯性感知在EKF框架中。但是，这些方法需要环境的先验稀疏地图，限制了机器人的感知。

[9]和[10]中的工作利用KinectFusion[11]或ElasticFusion[12]的帧-模型视觉跟踪，其使用从粗到精的ICP算法。然而，基于视觉的SLAM技术受到变化光照的干扰。虽然对光照的鲁棒性已经被探索，例如[14]和[3]，但是仍然是很大的挑战对于任何视觉导航系统来说。

与双足机器人相比，四足机器人的灵活性和鲁棒性更强，当在具有挑战的环境中运动时。因此，它更适合长期任务，例如工业场景的探索。在极端情况下，机器人需要跳过障碍物区域。Park等人[15]使用一个Hokuyo激光来检测40cm的障碍，其然后允许MIT Cheetah-2的控制器跳过障碍物，虽然LiDAR测量没有被用于定位。

Nobili等人[16]提供了一个状态估计器用于Hydraulic四足机器人（HyQ），基于单目惯性EKF。机器人的基坐标系速度和位置被传播使用IMU测量，腿式里程计，视觉里程计和激光里程计。系统容易漂移由于导航系统没有回环检测技术。

B 激光SLAM系统

一个有效的LiDAR里程计方法是LOAM[17]。LOAM提取边缘和表面点特征在点云中通过评估局部表面的平滑度。表面被重投影到下一帧的开始基于运动模型，并在下一帧中找到点对应。最终3D运动被估计通过最小化点对应的距离和。LOAM达到了高精度和低计算代价。

类似LOAM，Deschaud[18]发展了LiDAR里程计，IMLS-LiDAR，具有类似的精度通过使用帧-模型匹配。

Shan等人[19]扩展了LOAM到LeGO-LOAM，其分别优化横向和纵向参数。此外，LeGO-LOAM检测回环通过使用ICP算法来创建一个全局一致的位姿图使用iSAM2[20]。

Behley等人[21]提出了一种基于面元的LiDAR SLAM系统通过使用当前帧和面元模型的投影数据关联。

Dube等人[22]发展了一种在线的协作LiDAR SLAM系统。使用两到三个机器人，每个装配3D LiDAR，增量稀疏位姿图被生成通过连续场景识别约束。场景识别约束被确定通过SegMatch算法[23]，其表示LiDAR点云为一组紧凑但是具有区分度的特征。描述子被用于匹配过程以确定回环。

3. 方法

我们的目标是提供ANYmal长时间的无漂定位和准确的周围地图。图2和图3给出了系统的不同成分分别从硬件和软件的角度。

图2. LiDAR-SLAM系统的硬件组成部分。

图3. LiDAR-SLAM系统的框图。

A 运动-惯性里程计

我们使用默认的状态估计器用于机器人估计增量运动使用本体感知。这是两状态隐式滤波器（TSIF）[24]。从前向运动学得到的惯性坐标系腿式机器人激光SLAM系统-编程知识网中的脚位置，被当做时序测量来估计机器人的位姿在固定的里程计坐标系中：

腿式机器人激光SLAM系统-编程知识网 (1)

其中腿式机器人激光SLAM系统-编程知识网是基坐标系到里程计坐标系的变换。

基坐标系的估计随着时间漂移，因为它不适用外部传感器，并且四足的位置和绕z轴的旋转是不可观的。下面，我们定义LiDAR-SLAM系统用于估计机器人相对地图坐标系腿式机器人激光SLAM系统-编程知识网的位姿。值得注意的是，TSIF框架估计状态的协方差[24]，我们使用它在我们的几何回环检测中。

B LiDAR-SLAM系统

我们的LiDAR-SLAm系统是一个位姿图SLAM系统，建立在我们的ICP配准方法上，称之为自动调参ICP（AICP）[4]。AICP自动调整ICP的外点滤波器通过计算重叠参数腿式机器人激光SLAM系统-编程知识网，因为常数重叠率是传统外点滤波器的重用假设，违背了实际情形的配准。

给定初始的估计位姿通过运动-惯性里程计，我们获得一个参考点云，其中我们对齐每个连续的点云。

这样，连续的点云被准确地和参考点云对齐，极大降低了漂移。对应每个点云的机器人位姿被获得如下：

腿式机器人激光SLAM系统-编程知识网 (2)

其中腿式机器人激光SLAM系统-编程知识网是机器人在地图坐标系中的位姿，并且是AICP计算得到的对齐变换。

计算对应点云的修正位姿后，我们计算连续参考点云之间的相对变换以创建里程计因子，如公式(3)所示。

里程计因子，腿式机器人激光SLAM系统-编程知识网，被定义为：

腿式机器人激光SLAM系统-编程知识网 (3)

其中腿式机器人激光SLAM系统-编程知识网和是对节点和的机器人的AICP估计位姿，同时和是无噪声的变换。

一个先验因子腿式机器人激光SLAM系统-编程知识网，取自运动-惯性里程计的位姿估计，被加入因子图来设置机器人初始的原点和朝向。

为了修正里程计漂移，回环因子被加入因子图一旦机器人重新访问环境区域。我们使用两种方法来提出回环：a）基于当前位姿和因子图中位姿距离的几何方法，适合小环境，和b）一种基于学习的方法用于全局闭环，详见3.C.1节，其对于大环境也适用。每种方法提供初始猜测，由ICP精修得到。

每个独立的回环变成一个因子，被加入因子图。回环因子的终点是当前位姿，起点是其中一个参考点云，存储在机器人的历史中。参考点云必须满足最近邻和与当前参考点云有足够重叠的两个原则。一个可接受的回环因子腿式机器人激光SLAM系统-编程知识网被定义为

腿式机器人激光SLAM系统-编程知识网 (4)

其中腿式机器人激光SLAM系统-编程知识网和是地图坐标系中由AICP修正的机器人位姿。是当前参考点云和参考点云之间的AICP修正。

一旦所有的因子，包括里程计和回环因子，都被加入到因子图，我们优化该图从而找到机器人相对参考点云的位姿的最大后验（MAP）。为了执行在变量腿式机器人激光SLAM系统-编程知识网上的推理，其中是机器人在因子图中的位姿，所有因子的乘积必须被最大化：

腿式机器人激光SLAM系统-编程知识网 (5)

假设因子遵循高斯分布，并且所有测量被白噪声干扰，即噪声服从零均值正态分布，等式(5)中的优化问题等价于最小化非线性最小二乘：

腿式机器人激光SLAM系统-编程知识网 (6)

其中腿式机器人激光SLAM系统-编程知识网表示测量，数学模型和协方差矩阵。

MAP估计是可靠的当等式(6)服从正态分布。但是，ICP对于较少的几何特征环境敏感，例如走廊或者门前，可能得到不好的街而过当优化该位姿图时。在第3.D节中，我们提出了一种快速的验证技术用于检测可能的AICP配准失效。

C 回环方法

本节介绍了我们的基于学习的回环方法和几何回环检测。

1)深度学习的回环方法：我们使用Tinchev等人的方法[26]，基于匹配单独的分割对使用基于学习的特征描述子。它使用一个较浅的网络，从而它不需要一个GPU。我们提供一个方法总结，称之为高效分割匹配（ESM），但是建议读者参考[26]。

首先，一个神经网络被离线训练使用单个LiDAR观测（分割）。通过使用里程计，我们能够匹配分割实例不需要人工的干预。网络输入是一个三元组——锚点，正和负分割。锚点和正样本是来自连续帧的相同物体，而负样本是20m外的分割。该方法直接在原始点云数据上执行一系列X-卷积算子，基于PointCNN[27]，接着是三个全连接层，其中最后一层用于分割的描述子。

在我们的实验中，当SLAM系统接收一个新的参考点云时，它被预处理，然后被分割为一系列点云簇。对于每个参考点云中的簇，一个描述子向量被计算使用高效的Tensorflow C++应用通过使用训练模型的权重进行前向传播。这允许一批分割被同时预处理。

一旦参考点云的描述子被计算后，它们和之前参考点云的地图进行比较。ESM使用特征空间中的腿式机器人激光SLAM系统-编程知识网距离来检测匹配分割，和一个鲁棒的估计器来检索6维位姿。这产生一个当前参考点云相对之前参考点云的变换。该变换被用于AICP来添加一个闭环作为基于图优化的约束。最终，ESM的地图表示被更新，当优化结束后。

2）几何回环检测：为了几何检测回环，我们使用腿式估计器（TSIF）的协方差来定义当前机器人位姿附近的一个动态搜索窗口。然后搜索窗口内的机器人位姿被检验基于两种原则：最近邻和点云配准的验证（第3.D节）。最终，几何回环被计算在当前点云中，并且得到对应的位姿使用AICP。

D 点云配准的快速验证

本节详细解释了ICP点云配准的验证方法来确定两个点云是否安全配准。我们提高之前提出的对齐性度量[28]，使其具有更快的速度。

[28]的方法：首先，点云被分割为一组平面表面。然后，一个矩阵腿式机器人激光SLAM系统-编程知识网被计算，其中每一行对应平面的法向量，按照重叠率排序。是匹配平面数量在两个点云的重叠区域中。最后，对齐度量被定义为最小和最大特征值的比值。

匹配得分腿式机器人激光SLAM系统-编程知识网被计算为两个平面和的重叠，其中，。和是输入点云中平面数量。此外，为了找到最大可能的重叠，算法在所有可能的平面中迭代。这导致了，其中和是两个点云的平面的点数目。

提出的改进：为了降低逐点计算量，我们首先计算每个平面腿式机器人激光SLAM系统-编程知识网和的中心，并对齐它们使用给定的变换。我们然后存储中心在K-D树中。对每个询问平面，，我们寻找K近邻。我们计算重叠对于K近邻，然后使用最高重叠。结果是，其中。实际上，我们发现腿式机器人激光SLAM系统-编程知识网已经足够了。伪代码如算法1所示。

算法1. 伪代码

4. 实验验证

表1给出了机器人传感器的参数。

表1. 传感器参数

我们首先分析了验证方法，然后探究了基于学习的回环检测方法对于速度的可靠性。然后证实了我们SLAM系统的表现在室内和室外试验中。视频链接：ori.ox.ac.uk/lidar-slam

A 验证表现

我们专注于对齐风险预测的可对齐度量腿式机器人激光SLAM系统-编程知识网，因为他是我们的首要贡献。我们计算室外实验中连续点云之间的。如图6（左）所示，基于k-d树的可对齐滤波器快于原始滤波器。如第3.D节所示，我们的方法很少依赖点云大小，由于使用了平面中心。而原始方法依赖分割中的点数，导致了更高的计算时间。平均时间少于0.5s（几乎提高了15倍），适合实时表现。

图6（右）展示了我们的方法与原始方法高度相关。最终，我们建议读者参考原始文章[30][31]。

图6. 我们提出的验证方法和原始方法比较。时间比较（左），表现比较（右）。

B 学习回环检测器的评估

在下一个实验中，我们探索不同回环方法的表现使用室外数据。

1）对视角变化的鲁棒性：图5展示了我们具有学习的回环方法的SLAM系统的预览。我们只选择两个点云来创建地图，它们的位置是图5中的蓝色箭头。

图5. 基于学习的回环验证。

机器人在环境中转了两圈，包括249帧点云。我们故意在第二次进行路径反转已测量回环对视角和平移变换的鲁棒性。总共50个回环被检测到（绿色箭头）在两个地图点附近。有趣的是，该方法不仅检测到偏移6.5m的回环，而且能够检测到朝向差180度的回环——视觉定位不能做到，这也是使用LiDAR的动机。在所有50个回环中，每个点云平均识别5.24个点云簇。计算的变换具有腿式机器人激光SLAM系统-编程知识网的RMSE。近似每个询问点云486ms。

2）计算时间：图8展示了计算时间图。几何回环的计算时间依赖遍历相同区域的次数。几何回环在最近N个点云中迭代，基于半径；遍历的协方差和距离导致它变慢。类似地，验证方法需要在最大重叠的点云中迭代，影响了实时操作。

替代地，学习的回环方法对地图大小的扩展性更强。它具有低维特征描述子，比上千个欧式空间中的数据点更快。

C 室内和室外实验

为了验证完整的SLAM系统，机器人在室内和室外分别走了100m和250m。每个实验持续45min。图1（底部）和图4展示了测试位置：工业建筑。为了评估建图准确性，我们比较了SLAM系统，AICP，和基准腿式里程计（TSIF）使用真值。如图4所示，我们使用Leica TS16来自动跟踪装在机器人身上的360度棱镜。这样，我们能够记录机器人的位置以厘米级别在7Hz左右。