IROS2020开源软硬件！多激光雷达的协同定位建图及在线外参自标定

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

干货第一时间送达

本文由作者林家荣授权转载，二次转载请联系作者

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

----多图预警！请在wifi环境下食用！----

首先，先放我儿子镇下楼先

自从上次在知乎上分（吹）享（水）了我们的工作后，

https://www.zhihu.com/question/332926945/answer/836032023

我们的工作收到了很多的关注，并陆陆续续收到了来着各位朋友的咨询邮件，截止到目前，我们的开源库

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

已经收到了517个star(感谢各位老铁)。于是乎，我们趁(划)胜(水)追(摸)击(鱼)，苟(不敢出去，年前怕被废青干，年后怕被疫情干)在实验室里面，积(通)极(宵)开展了新的研(组)究(团)工(开)作(黑)。

而今天，我要介绍的是我们最近被IROS 2020接收的工作” A decentralized framework for simultaneous calibration, localization and mapping with multiple LiDARs”即“一个多激光雷达同时定位建图以及外参的自标定的分布式框架”。我们的工作不仅能实现多（5）个雷达同时定位建图以及在线外参标定，而且分布式的去中心框架能使得我们整个系统的效率更高，鲁棒性更强。我们的定位结果和RTK的GPS相比，误差仅为0.2%。我们的论文现在已经挂在arxiv上面的了，

https://arxiv.org/abs/2007.01483

硬件平台已经开源在了，

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

软件算法在后面会开源在

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

一．背景介绍

在介绍我们的工作之前，先需要回答以下2个问题，即：

为啥要用多激光雷达
为什么要做去中心化的分布式系统

对于第一个问题，我引用@瓦力（经作者同意）同学的PPT来解释，目前大部分的自动驾驶方案现在的自动驾驶方案都配备了多个激光雷达，如下图所示：

为什么这么他们要用这么多雷达呢？瓦力的ppt给出了以下三个观点：

A.多雷达组合的方案远比单雷达方案便宜：以velodyne家的雷达的为例，如下图所示，同样的价钱，我用买一个64线（32线），的雷达的价钱，去买20（10）个雷达他不香吗？说不定还能留一下一大笔钱雇几个博士生天天996，岂不美哉？

B. 部分区域点云覆盖率稀疏：使用一个雷达作为传感器，总会出现部分区域雷达测量稀疏的情况，例如置放在车顶的旋转机械式雷达，对于车周围的近距离范围，只会出现寥寥几条扫描线的情况，如下图的B区域

C．单雷达容易出现遮挡问题：这就很好理解了，由于车本身就不是一个质点，安装雷达的时候，多多少少有些雷达的测量被车本体遮挡到，这个时候就会存在视野盲区

除了PPT提到的以上三点之外，我们认为还有以下几点考虑使用多激光雷达有优势：

D. 鲁棒性的考虑：单个雷达的鲁棒性不够，万一这个雷达失效了怎么办？例如因为通讯问题，数据传输中断了，长时间雷达数据丢包了怎么办？又例如雷达故障问题，机械式旋转雷达旋转部件老化了，行驶过程中被不明物体了gank（如鸟粪，树叶）了怎么办？

E．感知的多元化需求：为了提高在某些范围的感知能力，如加几个雷达提高近距离的点云密度，使detection，segmentation的成功率更高，对于高速场景，补一个长距离（如Livox刚出的Tele 雷达就能看到距离1000米的物体）的雷达以提高避障能力 …

为什么要做成去中心的分布式系统？

基于性能的考虑：实际上由于各种原因，如驱动，通讯延时，雷达不是同一个厂商，甚至基于鲁棒性的考虑，雷达数据都不会全交给一个节点来同步以及发包，所以雷达的数据都不是一下去全部同步好然后发包的。那么问题来了，雷达的数据陆陆续续来包的时候，怎么去实时处理这些数据？加之雷达的数据又大又无序，处理储存起来也是个蛋疼的问题。在一个中央平台上无脑堆算力当然可以解决问题，但是边际效应太了，到达某个程度之后，把算力堆上去所需要花的代价太高（如同样价格的顶端处理器可以买好几个中低端的处理器了），如果你的系统能并行化，把不同时刻的雷达数据分配给分布式平台处理，最后再想办法融合在一起，那么这个问题将得到很大的缓解。
基于鲁棒性的考虑：如果你的系统只有一个处理平台的话，那万一这个平台出bug了，或者压力太大泉水挂机了怎么办？（项管表示: 开发人员也要做成去中心化并分布式的，万一996的那个博士生加班猝死了，或者删库跑路了就彻底完犊子了）。

好处这么多，那么，狗蛋，代价是什么呢？

多传感器安装的需求：首先你得把多个雷达安装上去对吧，如果车是定制的还好，不是定制的还需要考虑改装的成本；其次，雷达和雷达之间的连接应该足够刚性对吧，baseline比较短的话还好，baseline一长的话就要花很大的代价去保证连接的刚性；最后，雷达安装的位置如果机械振动太大了怎么办？是不是要加一些阻尼器消除掉一些机械振动？

对外参标定的需求: 标定的需求包括离线标定和在线标定的需求，离线标定相对来说简单一些，我总可以使用各种marker，或者周围建筑规则的面线来完成离线标定；在线标定主要是考虑到机械形变的带来的外参变化问题，如使用时间久了材料变形了呀，冬暖夏凉，从凉爽的地下室停车场到骄阳似火的马路上，材料的热胀冷缩带来的机械形变…

对数据融合算法的需求: 这个就是老生常谈的问题了，这么多传感器我怎么把数据结果融合在一起，多传感器的多个测量值我应该相信谁？

二．实验平台

既然我们的工作是多雷达的工作，那首先得有一个平台吧，但是我们的实验室刚成立不久，实验室的foundings非常有限，于是乎我们基于Robomaster的小车底盘，用3D打印机打印的3D支撑件，搭建了下面的多雷达数据采集平台。

我们的雷达系统由1个Livox-Mid100以及4个Livox-Mid40组成，整套弄下来不到4K美刀，还抵不上人家一个velodyne VLP16的价格，可想而知我们的系统（实验室）是有多低（穷）成本（PS. 没错，我们组的Livox雷达也是白嫖的）。

雷达的配置和FoV覆盖图如下所示：

三．实现方法

想要了解这里面细节的同学可以查阅我们的论文，以论文为主。这里我只简单介绍下我们的大致流程以及算法。

简单地说，我们的思路是：如论文中Fig.4和Fig.5 所示，分布式的LiDAR节点从网络里面订阅（由其他雷达更新的）最新的系统EKF状态接收到最新帧的雷达点云是，EKF根据运动学模型预测当前雷达6D姿态，然后使用这个预测值作为LiDAR odometry and mapping (LOAM)的起始点完成当前帧点云的配准（registration），点云配准完成之后，节点更新最新的系统EKF状态，并将注册完的点云以及最新的EKF状态广播到网络里面去。

EKF模型：

我们文章的Section V主要介绍了我们的模型（vehicle model），在Section IV.A 我们介绍了我们的一些notation，例如：T表示一个SE3的刚体转换，R,t 分别代表旋转和位移等等…

在Section IV.B 我们介绍了小车的恒速模型，根据小车在第k帧的角速度以及线速度我们就可以估计∆t时间后第k+1 帧小车的为旋转角度以及位置。