一种基于单目相机与激光雷达信息融合构建致密地图方法的实现

杨涛，芦迪，翟凯，陈旭

（中国矿业大学（北京） 机电与信息工程学院，北京， 100083）

0 引言

精确和鲁棒的环境地图是移动机器人进行自主移动的前提，通过构建致密的环境地图，可以为移动机器人进行的定位、导航和路径规划等操作提供良好的依据[1～4]。

建图的方法根据使用传感器的不同分为两类：一类为视觉传感器建图，包括单目相机、双目相机和深度相机等；另一类为激光雷达建图[5]。单一传感器建图存在着明显的缺陷：使用单目相机建图虽然能够获取丰富的环境纹理信息，却无法得到具体的深度信息；双目相机可以按照三角测量的原理，由极线匹配的方式实现深度值的估计，但是受到基线长度的限制仅在近距离有良好的测距效果；深度相机具有良好的测距效果，但是在室外环境受光照的影响大；激光雷达通过发出的激光束能准确地测量出深度信息，但是受到激光点云稀疏性的限制导致无法获得致密地图。

本文将采用多传感器信息融合的方式，用激光雷达弥补单目相机无法获取深度信息的缺陷，用单目相机弥补激光雷达场景稀疏的不足，研究融合单目相机、三维激光雷达的三维致密地图构建方法。

1 联合标定

单目相机与激光雷达的联合标定是进行信息融合的前提，也是致密地图构建的基础。良好的联合标定结果可以使信息融合更加准确，从而使构建的地图能够更好地还原和表达环境信息，为机器人的自主导航和路径规划提供精确的参考依据。激光雷达与相机的联合标定的本质是分别在图像和三维点云中获得足够的像素点坐标和空间点云坐标，找到其对应关系，通过对多对匹配点的计算得出联合标定的外参结果[6～7]。

本文中进行的联合标定主要是对标定板进行联合标定。标定板选用的是8行6列的棋盘标定板，标定板中每一个小方格的边长为12cm，在进行标定时同时采集棋盘标定板的激光和相机数据。

图1 联合标定示意图

联合标定在autoware的Calibration Toolkit模块中进行，标定结束后自动计算出标定结果。这种标定方式通过对标定板识别，在抓取的激光点云中找到同一时间图像中标定版的位置，可以实时标定，也可以通过录制数据包实现离线标定。其中离线标定受到环境条件的影响较小，所以本文采用离线标定的方式。

在录制数据包的时候，标定板的站位以及姿态也要满足要求。棋盘标定板放置在距离镜头大约5m左右的位置，在镜头的视野中对标定板设置三个定位点，分别位于相机视野的左侧、中间和右侧。为了使标定结果更加准确，需要抓取不同姿态下的标定板。因此在三个定位点上要进行不同姿态的摆放，其姿态依次为：正向、下俯、上仰和向内侧偏移。在标定过程中，通过对这12组姿态的信息进行标定，可以得到准确的联合标定效果。

标定过程中，需要对点云的视角进行缩放和平移，将点云和图像的视角进行同步，在两个视角中可以分别清晰地辨别出标定版的位置和姿态，在每个定位点对两个视角进行捕获，通过观察点云视角上的点云信息并与图片视角中标定板的位置做比较，在其上标注出标定版的位置，通过标定可以得到相机外参矩阵、相机内参矩阵、畸变参数和图像的尺寸。

具体结果如图2所示。

图2 联合标定结果

由标定结果可知，相机坐标系与雷达坐标系进行转换的旋转矩阵R和平移向量t分别为：

2 激光点云的投影与信息融合

单目相机和激光雷达的联合标定为空间任意一点从三维点云到相机图像平面的转换提供了转换矩阵，在此基础上实现传感器数据的信息融合。图3为激光雷达坐标系LO-XL-LY-ZL，相机坐标系cO-Xc-cY-Zc、图像坐标系O-X-Y和像素坐标系u-v之间的关系。

图3 坐标系关系

设空间一点P，其在雷达坐标系下的坐标为(X L,YL,ZL)，在相机坐标系下的坐标为（X C,YC,ZC），在图像坐标系下的坐标为（x,y），在像素坐标系下的坐标为（u，v）。

由联合标定所求出来的转换矩阵可得，点P从雷达坐标系向相机坐标系的变换表示为：

由相机坐标系向图像坐标系的变换表示为：

由图像坐标系向像素坐标系的变换表示为：

将式(3)、式(4)、式(5)联立，可得激光雷达转换为像素坐标系的关系表示为：

由此将激光雷达坐标系转换为像素坐标系的问题，转化为求解矩阵D的问题，其中相机内参和焦距f由出厂数据可查到，坐标系之间的转换矩阵K由已经由联合标定获得，因此可以求解出矩阵D，从而实现将三维激光点云数据投影到二维图像上。

由公式(7)可以较为精确地将三维点云中的点与二维图像上的像素进行对应，通过计算将点云投影到图像平面，利用与像素的对应关系，将每个激光点云点所携带的深度信息融合到相应的像素点中，完成深度赋值，使每个像素点包含了(x,y,z,R,G,B)六个参数信息，即在图像上的位置信息、深度信息和环境彩色信息，完成信息的融合。

3 三角测量

由于激光雷达获取的点云数据是稀疏的，导致激光雷达与单目相机的信息融合使部分像素点获得了深度信息。为了能够构建致密的环境地图，本文利用三角测量对其余的像素深度进行估计。

三角测量的方法最初被广泛运用于单目相机地图构建当中，因为在建图时，仅凭单张图像是无法获取环境深度值的，需要通过两张甚至多张对目标进行深度估计。三角测量具体指的是通过两个不同的视角对同一个目标进行观测，根据两个位置的观测夹角以及两个位置之间的距离计算出观测目标的距离。三角测量最早由高斯提出，如今在多个领域都发挥着重要的作用[8]。

设I1和I2分别为两个图像平面，矩阵T为从平面I1到平面I2的变换矩阵。两个平面内分别有一个特征点p1和p2，O1和O2分别为相机在两个位置拍摄时的光心。如果不考虑其他因素的影响，理论上直线O1p1与O2p2会相交于特征点p1和p2在三维场景中的真实位置，即图4中的点P。但是由于环境噪声等因素的影响，直线O1p1与O2p2往往不会相交于点P。

图4 三角测量示意图

根据极几何中的定义中，它们满足：

式中s1和s2分别为特征点的深度；x1和x2为特征点的归一化坐标，R和t为两个图像平面之间的旋转矩阵和平移向量。

现在已经知道了R,t，想要求解的是s1和s2。当然这两个深度是可以分开求的。如果要算s2，那么先对上式两侧左乘一个得：

当式(9)左侧为零时，右侧就只含有s2一个未知数，可直接求出s2。当s2已知时，s1也可以求出。据此得到点P的深度。以上情况是不考虑环境噪声的影响而求出来的深度值。考虑到噪声的存在，式(9)不能精确得为零。所以在实际中，需要通过最小二乘法来估计。

4 图像的反投影

为了使构建的环境地图更加直观清晰，需要将信息融合后的图像反投影回三维空间形成三维点云地图。由单目相机和激光雷达的联合标定结果可以得到各传感器坐标系之间的转化关系，通过这种转化关系能够找到二维图像中的像素点在空间中所对应的三维点的位置，并且将其所携带的深度信息和彩色信息表示在三维空间，形成三维点云。

二维图像投影到三维空间其本质还是坐标的变换，其公式为：

其中x，y，z是点云坐标系，x′，y′是图像坐标系，D为深度值。

图5为图像中任意点m（u，v）向世界坐标系进行投影的过程。设投影后在世界坐标系下的坐标为M（Xw，Yw，Zw）。

图5 世界坐标向图像坐标的投影

公式化表示如下：

式（11）中u和v为像素坐标系中任意一点的坐标，u0和v0为像素坐标系的中心坐标，xω，yω，zω表示在世界坐标系中任意一点的坐标，zc表示相机坐标的z轴值，即目标到相机的距离。

在本文中，由于相机坐标系与世界坐标系三个轴的指向都相同，为了减少计算量，将世界坐标系与相机坐标系设为了同一个坐标系，即相机没有旋转和平移，同时在相机坐标系和世界坐标系下任何的环境深度都相同，即zc=zω，于是公式可进一步简化为：

由式（12）的变化，可以计算得到像素点(u,v)到世界坐标点(xω,yω,zω)的变换公式：

5 致密地图构建

本次实验所用的传感器系统如图6所示。本实验中使用的移动平台为Scout移动机器人，整个系统包括搭载了计算平台为Intel Core i7-8700K CPU的迷你主机、单目相机和激光雷达等传感器。其中，3D激光雷达VLP-16安放在移动平台前端钢梁的最顶层，这能够使得激光雷达具有良好的扫描视角，在其工作进行扫描时，避免了车身系统的遮挡物对扫描的干扰；单目相机安装在钢架的第二层，激光雷达的正下方，可以和激光雷达保持相同的视野，在联合标定以及后续建图时提供了很大的方便。

图6 建图平台

如图7和图8所示，单目相机只能获得环境的纹理信息和彩色信息，无法获取深度信息，只能构建二维地图，激光雷达获取了环境深度信息，但其稀疏性使得对环境表达不够丰富。将两种传感器获取的信息进行融合致密化后得到的三维点云地图既包含了环境的深度信息，又能准确地表达环境的纹理和色彩，构建的地图具有较高的还原性和鲁棒性。

图7 对标定板的建图

图8 实验室一角建图

图9为对实验室内一个边长大约6m的场景进行建图。整体效果展现较好。由三维点云地图可清楚地看到信息融合以及深度估计都取得良好效果，场景纹理信息还原度较高，实验桌上书架、显示器、水杯等物品轮廓容易辨别，环境表达清晰，证明本文提出的致密地图构建方法可行。

6 结束语

单一传感器受自身特性的局限性无法单独构建三维环境的致密地图。而多传感器的信息融合能够使两种传感器进行优势互补，弥补单一传感器建图的不足，并且该建图方法能够适用于大场景和复杂的室外场景，具有良好的研究价值。