图 1为本文的研究平台，在改造的Turtlebot机器人平台上搭载一台Kinect相机，并在其上方安装一台Hokuyo UTM-30LX激光雷达。激光雷达和Kinect相机结合的工作思路如图 2所示，Kinect相机从周围环境中采集深度图像，经过“深度图像转换模块”转换成虚拟激光测距数据；二维激光雷达从周围环境中采集二维平面的激光测距数据；“测距数据融合”模块把虚拟激光测距数据和激光测距数据整合得到障碍物位置，并提交给“机器人导航模块”。

图  1  系统工作平台

图  2  系统工作思路

对于环境中的障碍物，二维激光雷达和Kinect相机各有不同的表示形式。图 3给出了同时处在激光雷达坐标系$({{O}_{L}}{{X}_{L}}{{Y}_{L}}{{Z}_{L}}) $与Kinect相机坐标系$ ({{O}_{K}}{{X}_{K}}{{Y}_{K}}{{Z}_{K}})$的目标点P，坐标分别为$({{x}_{L}}, {{y}_{L}}, {{z}_{L}}) $和$({{x}_{K}}, {{y}_{K}}, {{z}_{K}}) $，P点在图像平面坐标系$ ({{X}_{P}}{{O}_{P}}{{Y}_{P}})$中的投影$ P'$坐标为(u, v)。通过标定找到激光雷达坐标系与Kinect图像坐标系之间的关系，才能准确融合激光雷达测距数据与Kinect深度图像数据。

图  3  激光雷达与Kinect相机坐标系

P点在$ ({{O}_{L}}{{X}_{L}}{{Y}_{L}}{{Z}_{L}})$下的坐标为$({{x}_{L}}, {{y}_{L}}, {{z}_{L}}) $，根据激光雷达坐标系与Kinect坐标系的几何关系，得到坐标关系为：

式中，R是旋转矩阵；T是平移矩阵。Kinect相机采集得到的数据不是P点坐标$ ({{x}_{K}}, {{y}_{K}}, {{z}_{K}})$，而是$ {P}'$点像素坐标(u, v)和对应的深度值$ {{z}_{K}}$；激光雷达采集得到的不是P点坐标$ ({{x}_{L}}, {{y}_{L}}, {{z}_{L}})$，而是距离r和角度α。可以通过下面两小节分析得到$({{x}_{K}}, {{y}_{K}}, {{z}_{K}}) $与像素点三元组$ (u, v, {{z}_{K}})$的对应关系、$ ({{x}_{L}}, {{y}_{L}}, {{z}_{L}})$与测距点二元组$ (r,\alpha )$的对应关系。

根据经典相机模型^[6]，P点在$({{O}_{K}}{{X}_{K}}{{Y}_{K}}{{Z}_{K}}) $下的坐标为$({{x}_{K}}, {{y}_{K}}, {{z}_{K}}) $，其在图像平面投影为${P}' $，用像素表示坐标为$(u, v) $，可以得到Kinect坐标与像素点三元组的关系为：

式中，$ {{f}_{x}}$、$ {{f}_{_{y}}}$是X轴和Y轴方向上以像素为单位的等效焦距；${{c}_{x}} $、${{c}_{y}} $是基准点。这些都是相机参数，可以采用标准的相机参数标定方法计算出。

综合式(1)与式(2)，可以得到像素点三元组与激光雷达坐标的关系为：

对于激光雷达，保持极坐标系与平面直角坐标系$ ({{O}_{L}}{{X}_{L}}{{Z}_{L}})$在同一个平面内，原点同为激光雷达中心，激光束与$ {{Z}_{L}}$轴正向夹角为极坐标角度a(激光束偏角值)，激光束测距值为极坐标径向值r(激光雷达当前测距值)，如图 4所示。

图  4  激光雷达的直角坐标与极坐标关系

激光测距点二元组$ (r, \alpha )$与直角坐标$({{x}_{L}}, {{y}_{L}}, {{z}_{L}}) $的关系为：

对于像素点$ {P}'$的图像坐标$(u, v) $，保持激光雷达坐标原点与Kinect相机坐标原点在Y轴方向的垂直高度差为l，则有像素点三元组$ (u, v, {{z}_{K}})$与激光雷达测距点二元组$(r, \alpha ) $的关系为：

在确定相机参数后，把多对像素点三元组和测距点二元组代入式(5)，通过求解线性方程组得到未知的R、T矩阵，进而确定激光雷达坐标系与Kinect相机坐标系的旋转平移关系，完成联合标定。

本文中，Kinect相机采集得到深度图像后，深度图像转换模块把图像中转换窗口的每个像素列上深度值最小的像素点距离和角度提取出来，相当于将部分或者全部深度图像向中心平面压缩，在中心平面上形成一系列虚拟激光点。图 5a为Kinect相机采集到的RGB图像；图 5b为深度图像与转换窗口，s表示窗口第一个转换像素行号，h表示转换窗口高度(单位为像素行)，RGB图像$ ({{u}_{0}}, {{v}_{0}})$点对应深度图像的$(u, v) $点。

图  5  Kinect相机图像

假设当前需要转换的像素在第u个像素列，该列所有像素点被转换成一个虚拟激光点，该点在所有虚拟激光点中的序号是j，激光点距离值为：

式中，$ r_{j}^{K}$表示${{O}_{K}} $与目标点P的虚拟激光点(P点在中心平面上的投影)距离，具体计算方法参照图 6所示模型。P在深度图像的对应点$ {P}'(u, v)$，其在中心平面的投影点A就是虚拟激光点，C是$ {{O}_{K}}$的投影，那么，线段A${{O}_{K}} $的长度就是$ r_{j}^{K}(i)$。

图  6  Kincet相机的虚拟激光测距

具体计算$r_{j}^{K}(i) $的步骤如下：

1) 获取P在Kinect坐标系下坐标$ P({{x}_{K}}, {{y}_{K}}, {{z}_{K}})$，${{z}_{K}} $是${P}' $的深度值；

2) 计算直线A$ {{O}_{K}}$和C$ {{O}_{K}}$夹角β为：

3) 二维激光点的计算。

如果Kinect相机观测角范围为$({{\beta }_{\text{min}}}, {{\beta }_{\text{max}}}) $，沿AC轴向可转换成M个虚拟激光点，那么，P对应虚拟激光束的序号为：

4) 计算A到相机原点$ {{O}_{K}}$的距离为：

设激光雷达扫描得到的测距二元组集合为$ {{D}_{L}}$，融合Kinect采集得到的三元组信息后的测距二元组集合为$ {{D}_{F}}$。图 3中，P为$ {{D}_{L}}$中第i个测距点，其测距二元组为$ (d_{i}^{L}, \alpha )$，由式(1)和式(4)可得P在$({{O}_{K}}{{X}_{K}}{{Y}_{K}}{{Z}_{K}}) $下的坐标为：

利用第3节的方法，由$({{x}_{K}}, {{y}_{K}}, {{z}_{K}}) $得到P对应虚拟激光雷达测距数据d。于是，可得到融合后第i束激光测距二元组$(d_{i}^{F}, \alpha ) $为：

当Kinect相机与二维激光雷达同时工作，按照上述方法得到所有融合后的测距二元组集合${{D}_{F}} $。一般情况下二维激光雷达的激光测量角度大于Kinect相机观测角度，即$[{{\beta }_{\text{min}}}, {{\beta }_{\text{max}}}]\subset [{{\alpha }_{\text{min}}}, {{\alpha }_{\text{max}}}] $，从而${{D}_{F}} $为：

式中，$ \Delta \beta $表示Kinect相机的虚拟二维激光角增量；$ \Delta \alpha $代表激光雷达的角增量。深度图像与二维激光雷达数据融合算法伪代码为：

输入：$ {{D}_{L}}$//原始激光扫描二元组集合

输出：${{D}_{F}} $//融合Kinect数据的二元组集合

FuseRangerKinect(${{D}_{L}} $)

{    //从$ {{D}_{L}}$返回下一个二元组(r, α)

   (r, α, i )=GetNextElement($ {{D}_{L}}$)；

for (i in [0, ($ {{\alpha }_{\text{max}}}-{{\alpha }_{\text{min}}}$)/$\Delta \alpha $])

{    //返回二元组对应Kinect像素点坐标

     $({{x}_{K}}, {{y}_{K}}, {{z}_{K}}) $=LaserTOKinect(r, α, l)；

//返回像素点的虚拟激光点测距值

     (d, j)=VirtualLaserRange $({{x}_{K}}, {{y}_{K}}, {{z}_{K}}) $；

     if (j in [0, (${{\beta }_{\text{max}}}-{{\beta }_{\text{min}}} $)/ $ \Delta \beta $ ])

     { if (d < r)

    //把${{D}_{F}} $第i个元素设置为(d, α )

       {Insert($ {{D}_{F}}$, d, α, i)；

         Continue；

       }

     }

     Insert(r, α, i)；

     (r, α, i)=GetNextElement($ {{D}_{L}}$)；

}

   return($ {{D}_{L}}$)

}

其中，FuseRangeKinect()是主框架；GetNextElement()为从原始激光扫描点集$ {{D}_{L}}$返回下一个二元组；LaserTOKinect()实现式(10)；Insert()向融合激光点集$ {{D}_{F}}$中添加一个二元组。

为验证本文方法正确有效，在图 1的机器人平台上实现了上述环境障碍检测算法：主控计算机处理器为酷睿六代i7，主频为2.5 GHz，操作系统采用Ubuntu并运行了ROS系统；二维激光雷达为Hokuyo UTM-30LX，最大测距值30 m，角分辨率是$0.25{}^\circ $，测距精度能达到30 mm，最大扫描角度是$ 270{}^\circ $。

图 7给出了本文方法的测试结果，图 7a为Kinect相机采集的环境RGB图像，图 7b为相机采集的深度图像。本文通过4组测试对比二维激光雷达、Kinect相机和本文方法的障碍物检测结果。

图  7  障碍物检测实验

1) 第1组测试：图 7c表示h=50的测量区域，此时转换窗口很小，只能检测到桌面上的物品；图 7d表示h=50的障碍物边缘比较图，其中点划线为激光雷达检测到的障碍物边缘(来自$ {{D}_{L}}$)，点线为Kinect相机检测到的障碍物边缘，细实线为融合得到的障碍物边缘(来自于${{D}_{F}} $)。对比可发现，因为Kinect相机检测角度小于激光雷达，其检测出的障碍物边缘范围也小于激光雷达和融合方法；因为转换窗口很小，融合方法跟激光雷达一样没有发现桌子边沿、垃圾桶和纸箱。

2) 第2组测试：图 7e表示h=100的测量区域，转换窗口高度有所增大，检测到了桌面弧形边沿，图 7f表示h=100的障碍物边缘比较图。对比可发现，随着转换窗口增大，融合方法和Kinect相机可发现桌子边沿，但激光雷达依然不行。

3) 第3组测试：图 7g表示h=180的测量区域，转换窗口高度继续增大，检测到纸箱；图 7h表示h=180障碍物边缘比较图。对比发现，随着窗口继续增大，融合方法和Kinect相机可发现地面纸箱，相比Kinect，融合方法还能检测更宽的范围。

4) 第4组测试：图 7i表示h=240的测量区域，转换窗口高度进一步增大，进而检测到地面垃圾桶；图 7j表示h=240的障碍物边缘比较图。对比可发现，随着转换窗口进一步增大，融合方法和Kinect相机可发现地面纸篓，相比Kinect相机，融合方法还能检测更宽的范围。

由4次实验对比可见，本文的融合方法不仅能更准确地检测到环境中的障碍物，而且还具有较宽的检测范围，是一种成本较低的三维障碍检测方法，适合移动机器人在局部区域移动时避障。

针对现有视觉传感器探测障碍物时易受环境影响、检测精度低的问题，本文提出了一种结合Kinect相机和二维激光雷达判断障碍物的方法，具备了二维激光雷达平面探测范围广、精度高、实时性好的特点，同时兼有Kinect相机廉价的三维测量优势。在改造的Turtlebot机器人平台上，验证了该方法的正确性和有效性。