多个激光雷达同时校准、定位和建图的框架

点击下方卡片，关注“自动驾驶之心”公众号

ADAS巨卷干货，即可获取

点击进入→自动驾驶之心技术交流群

文章：A decentralized framework for simultaneous calibration, localization and mapping with multiple LiDARs

作者：  Jiarong Lin, Xiyuan Liu and Fu Zhang

编译：点云PCL

代码：https://github.com/hku-mars/decentralized_loam

摘要

激光雷达在自动驾驶车辆目标检测、自定位和地图绘制方面发挥着越来越重要的作用，由于恶劣的车辆环境（例如，温度，振动等），单个激光雷达经常会出现硬件故障（例如暂时失去连接），或者由于缺乏足够的几何特征，性能下降，尤其是对于具有小视场（FoV）的固态激光雷达。为了提高系统在自定位和建图方面的鲁棒性和性能，我们开发了一个分布式框架，用于多个激光雷达同时校准、定位和建图。我们提出的框架基于扩展卡尔曼滤波器（EKF），但专门用于分散实现。这样的实现可能会将密集的计算分布在每个激光雷达专用的较小计算设备或资源中，并消除单点故障问题。然后，在一辆无人地面车辆（UGV）上实施这种分散式配置，该车辆搭载5个低成本激光雷达，在城市环境中以1.3m/s的速度移动。实验结果表明，该方法能够成功地同时估计车辆状态和所有激光雷达外部参数。在我们收集的两个数据集上，定位精度高达0.2%，为了分享我们的发现并为社区做出贡献，同时让读者能够验证我们的工作，我们将在Github上发布所有源代码和硬件设计：

https://github.com/hku-mars/decentralized_loam

主要贡献

多个分布式激光雷达的使用给其定位和建图带来了许多新的挑战：

（1）外参校准。由于激光雷达安装在车身的不同位置（通常相距很远），它们的相对姿态不是完全刚性的，可能会随时间漂移。这需要在线外参校准。当两个相邻的激光雷达FOV重叠很小时，这将更具挑战性；

（2） 高网络带宽和计算要求。激光雷达通常以非常快的速度生成原始点数据，将所有激光雷达数据发送到中央计算机，不仅容易干扰车辆网络和中央计算机，导致单点故障，而且会显著增加计算能力（意味着高功耗、大噪声等）。一种可能更可靠的方法是在专用计算机中处理每个激光雷达的点云数据，并通过车辆网络传输处理后的数据（例如，车辆状态，通常是非常小的数据）。

图1：上图：我们的分散式多激光雷达车辆平台。边界框的颜色与相同激光雷达生成的点云一致（如下面的B、C所示）。底部：A）：从场景1构建的地图。点云颜色由反射率计算。B） ，C）：地图起点和拐角处的详细点云。该颜色表示激光雷达的原点。

在本文中，我们提出了一个分散的多激光雷达校准、定位和建图系统。该系统基于EKF算法的分散公式，可同时在所有激光雷达计算机上运行，所有EKF副本都执行相同的步骤：保持一个由几何中心的姿态和所有激光雷达的外参参数组成的增强状态向量，根据从网络中其余EKF副本接收到的最新状态更新进行预测，使用来自其局部的激光雷达的新帧更新状态向量，以及将更新的状态向量发布到网络以供其他EKF副本使用。

总之，我们的贡献是：

• 我们提出了一种利用等速模型和EKF的校准、定位和建图系统，该系统能够在线估计和更新激光雷达外部w.r.t.几何中心

•  我们提出了一个分散的多传感器校准和融合框架，该框架可以以分布式方式实现，并且对中央计算机或单个传感器的故障可能更为鲁棒

•  我们在实际系统上验证了该框架的收敛性和准确性，并与以前的单激光雷达SLAM解决方案相比，取得了高精度的定位和建图结果。

主要内容

我们的系统配置如图2所示，在汽车平台上安装了五个不同的激光雷达，它们的正面看起来是“前”、“左”、“右”、“左后”和“右后”。LiDAR-1是Livox-MID1003，具有98.4度水平和38.4度垂直视野。其他激光雷达均为Livox-MID40，38.4度圆形视野。由于视野有限，任何两个激光雷达之间都没有重叠区域（见图3）。防止在崎岖地面上旋转车轮4引起的振动，这可能会对激光雷达点云造成高频运动模糊效应。我们在激光雷达与汽车平台的连接处添加了橡胶阻尼球，可以有效地补偿振动。因此，我们不会将激光雷达组视为一个刚性系统，我们的平台是低成本的，我们所有的机械部件都是3D打印的，总价约为4k美元。使用下面几节中提出的算法，我们可以实现大约0.2%的精度。

图2：我们的数据采集平台，汽车平台上安装了5个激光雷达。为了防止车轮在粗糙地面上旋转引起的机械振动，我们在激光雷达与平台的连接处（标记在黄色虚线圆圈内）添加了一些阻尼球

图3：我们的激光雷达安装配置

 等速模型

将机器人视为刚体，其姿势可由参考框架表示（例如，图3中的几何中心）。此外，我们使用等速模型来模拟机器人的6自由度运动。表示r 为机器人姿态，t 为平移，ω为角速度，v 为线速度，所有时间均为k，然后恒速模型得出如下状态方程：

外参模型

假设有N个激光雷达，Ti表示第i个激光雷达框架的外参，w.r.T参考框架，我们将第i个激光雷达在时间k处的姿态T 表示为

全状态模型

表示x与第i个LiDAR外部参数关联的状态，然后是完整状态i

其状态表示为

测量模型

我们的分散式EKF在其专用计算设备上为每个激光雷达运行一个激光雷达里程计和映射（LOAM）算法，通常是具有调制计算性能的车载计算机或从高性能服务器分配的虚拟计算资源。以第i个激光雷达为例，LOAM通过最小化当前扫描和局部扫描之间的边缘特征rp2e和平面特征rp2p的距离来求解时间tk+1（即T i k+1）处的第i个激光雷达姿态

分散扩展卡尔曼滤波器

这里介绍我们的分散EKF算法，与通常在中央计算机上运行单个实例的现有EKF算法不同，我们的系统并行运行多个EKF实例，每个激光雷达一个。单个实例通常在各自的激光雷达专用计算资源上运行，并负责处理该激光雷达数据。如图4所示，每个EKF从网络读取完整状态向量x，通过配准相应的LiDAR数据对其进行更新，并将更新后的状态发布到网络以供其他EKF实例使用。

图4：分散系统的框架，每个激光雷达通过订阅来自网络的消息来刷新最新状态x，完成点云与最新数据的配准后，将向网络公布选定的要素点云和更新状态

 如图5所示的状态预测

图5：分散EKF算法的一步更新：一旦在时间tk+1接收到点云扫描，第i个激光雷达将检索最新的状态更新，该更新是在其本地时间tk从网络接收的，然后，它使用扫描来更新状态，并将更新后的状态发布给网络

多个激光雷达的分散校准、定位和映射算法综上所述，我们总结了之前的EKF公式，如下所示的算法

实验

定制的机器人平台如图6所示，顶部安装有差分全球定位系统（D-GPS）移动站，用于提供高精度里程计参考，以评估我们的算法。我们在一台高性能PC上实现了我们的分散式框架，该PC嵌入Intel i7-9700K处理器和32GB RAM。与真实的分布式系统类似，在我们的实现中，每个激光雷达EKF算法通过发布和订阅彼此的消息在单个ROS节点中运行。我们在港口区域以相对恒定的速度、良好的GPS信号和一些移动的行人运行车辆平台。我们试验场地的卫星图像如图11所示。

图6：我们的远程操作车辆平台包括5个激光雷达、车载微型计算机、D-GPS移动站和单目摄像头

记录了两条轨迹，场景1和场景2，分别耗时约400秒和320秒。场景1是单向轨迹，而在场景2中，我们选择沿相对直线行走，并返回到靠近起点的终点，如图8所示。

在线外参校准

结果图7绘制了所有5个激光雷达相对于几何中心的外参估计，左栏和右栏分别以欧拉角和米为单位描述旋转的外在形式，我们用两组初始值来测试我们的算法：第一组是从地图对齐结果（实线）获得的，第二组是直接设置为0（虚线）。如图7所示，旋转和平移的外部变量都快速收敛到一个稳定值，这表明我们的算法能够校准外部变量，即使初始值不准确。收敛的外参值也与每个激光雷达位置一致。

图7：5个激光雷达之间关于几何中心的外部参数更新（左：旋转，右：平移）。实线以根据mapalignment计算的初始猜测开始。虚线以零初始值开始

状态估计的结果

除了外参外，我们还进一步给出了车辆几何中心的状态估计。在场景1中，几何体的姿势和速度估计如图9所示，我们可以看到速度估计可以很好地反映姿势的变化。以xaxis的位置及其相应的线速度为例，在时间间隔[18.9s，300s]内，Pos x的值从16m增加到约400m，恒定速度约为1.36m/s，这与图9所示的估计速度相匹配。场景2的结果相似，由于空间限制，此处不显示

图9：场景1中几何中心的位置、线速度、旋转和角速度的估计，所有激光雷达的配置均已启用。数据图在地图对齐后开始，即t=18.9s。

定位精度评估

虽然所提出的方法在定性上收敛，但在本小节中，我们通过与差分全球定位系统（D-GPS）6进行比较，对我们算法的定位精度进行了定量评估，该系统可以提供1cm+1ppm精度的定位参考。我们在两个场景中使用不同数量的激光雷达来评估我们的算法。不同轨迹的比较如图8所示

其中单前激光雷达的轨迹最初很好地遵循地面真实情况，但由于小视场内缺乏足够的特征，长期来看失败。表二显示了不同配置之间的最大绝对/相对误差，表明多个激光雷达对提高定位精度有很大影响。此外，我们在图10中绘制了距离上的绝对误差，以供详细参考。

图10：在启用所有激光雷达配置的情况下，随时间变化的绝对平移误差

在两个场景中，我们都达到了约0.2%的精度，这表明我们的算法具有较高的精度。

建图结果

在场景1中，我们构建的地图如图11所示，从不同激光雷达采样的点云数据以不同的颜色渲染。从鸟瞰图（图11.A）和详细视图（图11.（C-E）），我们可以看到，来自不同激光雷达的点云数据很好地对齐在一起，并在局部和全局保持一致性。综上所述，我们用实际数据在实际系统上验证了所提算法的收敛性和精度。

图11:我们使用场景1中收集的数据重建的地图鸟瞰图。从不同的激光雷达采样的点云数据将使用不同的颜色进行渲染。白色、红色、深蓝色、青色和绿色点分别是安装在字体、左、右、左后和右后的激光雷达采集的数据

总结

本文提出了一种分布式EKF算法，用于多个激光雷达的同时校准、定位和建图。在市区进行了实验。结果表明，该算法收敛稳定，在低速运动时精度达到0.2%。我们当前的实现基于单个高性能PC，其中所有通信都在PC内完成，不会出现消息同步或通信丢失等问题，也不会考虑这些问题。此外，受PC计算能力的限制，当前的实现是离线运行的。未来的工作将在每个激光雷达专用计算机上实施，解决其中的问题（例如，时间同步），并在激光雷达出现故障时验证其鲁棒性。

【自动驾驶之心】全栈技术交流群

自动驾驶之心是首个自动驾驶开发者社区，聚焦目标检测、语义分割、全景分割、实例分割、关键点检测、车道线、目标跟踪、3D目标检测、多传感器融合、SLAM、光流估计、轨迹预测、高精地图、规划控制、AI模型部署落地等方向；

加入我们：自动驾驶之心技术交流群汇总！

自动驾驶之心【知识星球】

想要了解更多自动驾驶感知（分类、检测、分割、关键点、车道线、3D目标检测、多传感器融合、目标跟踪、光流估计、轨迹预测）、自动驾驶定位建图（SLAM、高精地图）、自动驾驶规划控制、领域技术方案、AI模型部署落地实战、行业动态、岗位发布，欢迎扫描下方二维码，加入自动驾驶之心知识星球（三天内无条件退款），日常分享论文+代码，这里汇聚行业和学术界大佬，前沿技术方向尽在掌握中，期待交流！