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 帧小车的为旋转角度以及位置。
在Section IV.C 我们介绍了雷达的外参模型，我们使用估计的i-th雷达的外参以及Section IV.B估计的小车姿态既可以预测雷达在k+1帧的位置
在Section IV.E我们介绍了我们的观测模型，如之前提到的，我们使用k+1帧的EKF预测值作为此次LOAM的起始点，迭代地最小化最邻近“点-线”和“点-面”的距离，以完成k+1帧的点云配准。
去中心的EKF算法
我们文章的Section V主要介绍了我们的分布式EKF预测以及更新，值得注意的是，为了防止EKF发散，我们在Section V.B引进了对雷达外参的约束是，我们的算法如下图所示：
2.初始化：
为了给外参估计提供一个良好的初始值，我们可以使用手眼标定算法标定出雷达之间的外参，也可以使用CAD模型的设计的外参值作为EKF的迭代的初始值。除此之外，我们也可以尝试在一开始用ICP把不同雷达的外参尝试匹配到一起，算出一个更为准确的初始值。
初始值只要不是太离谱，我们的算法也能逐渐将外参收敛到一个最优值上，只不过一开始的外参不准确的话，一开始建出来的图可能会有重影
四. 实验以及结论
我们的实验室视频见：
https://zhuanlan.zhihu.com/p/157533731
外参在线标定的结果图：
估计的中心位置，角度角度，角速度和线速度
在2个测试场景下，我们的轨迹和D-GPS的轨迹的对比如下图所示（单雷达后面的轨迹飘了主要是因为场景退化了）：
最大误差值的表格如下：
误差分布的箱线图：
最后就是我们的建图效果啦（不同雷达的点云已经用不同的颜色上色了）：
五. 讨论和未来的工作
由于实验条件受限，一时半会儿找不到那么多电脑并安装到小车上面，所以我们的所有实验都跑在了一个PC上面，用多线程”反正”分布式的多节点，未来的工作可能会把这个系统实在在真正的多分布式系统上面，以探讨和解决可能出现的问题。
六．致谢
感谢帮我做实验的刘晰源以及帮我改了好几版论文的张富老师
感谢DJI在这个工作中提供的设备支持，包括但不限于：Robomaster的小车底盘， A3的RTK套件, Manifold机载计算机…
感谢Livox提供的免费雷达
推荐阅读：
专辑|相机标定
专辑|3D点云
专辑|SLAM
专辑|深度学习与自动驾驶
专辑|结构光
专辑|事件相机
专辑|OpenCV学习
专辑|学习资源汇总
专辑|招聘与项目对接
专辑|读书笔记
专辑|相机标定
专辑|3D点云
专辑|SLAM
专辑|深度学习与自动驾驶
专辑|结构光
专辑|事件相机
专辑|OpenCV学习
专辑|学习资源汇总
专辑|招聘与项目对接
专辑|读书笔记
重磅！3DCVer-学术论文写作投稿 交流群已成立
扫码添加小助手微信，可申请加入3D视觉工坊-学术论文写作与投稿 微信交流群，旨在交流顶会、顶刊、SCI、EI等写作与投稿事宜。
同时也可申请加入我们的细分方向交流群，目前主要有3D视觉、CV&深度学习、SLAM、三维重建、点云后处理、自动驾驶、CV入门、三维测量、VR/AR、3D人脸识别、医疗影像、缺陷检测、行人重识别、目标跟踪、视觉产品落地、视觉竞赛、车牌识别、硬件选型、学术交流、求职交流等微信群，请扫描下面微信号加群，备注：”研究方向+学校/公司+昵称“，例如：”3D视觉 + 上海交大 + 静静“。请按照格式备注，否则不予通过。添加成功后会根据研究方向邀请进去相关微信群。原创投稿也请联系。
▲长按加微信群或投稿
▲长按关注公众号
3D视觉从入门到精通知识星球：针对3D视觉领域的知识点汇总、入门进阶学习路线、最新paper分享、疑问解答四个方面进行深耕，更有各类大厂的算法工程人员进行技术指导。与此同时，星球将联合知名企业发布3D视觉相关算法开发岗位以及项目对接信息，打造成集技术与就业为一体的铁杆粉丝聚集区，近1000+星球成员为创造更好的AI世界共同进步，知识星球入口：
学习3D视觉核心技术，扫描查看介绍，3天内无条件退款
圈里有高质量教程资料、可答疑解惑、助你高效解决问题