张青春 何孝慈 姚胜 郭振久

摘要：为提高智能移动机器人的感知能力，针对智能机器人相机与激光雷达融合提出一种基于直线与平面拟合算法，并将其应用于 ROS 智能移动机器人系统。基于 PPB 方法对相机图像进行畸变校正获得角点的二维坐标，采用点云反射强度（PCI）的方法获得角点的三维坐标。通过解决 N 点透视位姿求解问题（PNP 问题）的方法来获得三维坐标与二维坐标之间的变换，变换后完成相机与激光雷达的点云融合。将激光雷达与深度相机安装与机器人顶部并通过比较选择 ORB 算法作为机器人的 SLAM 方式，实验结果表明融合后机器人能够更有效地识别小体积障碍物，具有一定的应用前景。

关键词： ROS 系统;深度相机;激光雷达;融合

中图分类号： TP241.2;TB9文献标志码： A文章编号：1674–5124（2021）12–0120–04

Research on the fusion technology of camera and lidar based on ROS intelligent mobile robot

ZHANG Qingchun，HE Xiaoci，YAO Sheng，GUO Zhenjiu

（Automation Faculty， Huaiyin Institute of Technology， Huaian 223003， China）

Abstract： In order to improve the perception ability of intelligent mobile robots， an algorithm based on straight line and plane fitting is proposed for the fusion of intelligent robot camera and lidar， and it is applied to the ROS intelligent mobile robot system. Based on the PPB method， the camera image is subjected to distortion correctiontoobtainthetwo-dimensionalcoordinatesof thecornerpoints，andthepointcloudreflection intensity （PCI） methodisusedtoobtainthethree-dimensionalcoordinatesofthecornerpoints. The transformation between three-dimensional coordinates and two-dimensional coordinates is obtained by solving the N-point perspective pose problem （PNP problem）， and the point cloud fusion between the camera and the lidar is completed after the transformation. The lidar and depth camera are installed on the top of the robot， and the ORB algorithm is selected as the SLAM method of the robot through comparison. The experimental results show that the fusion robot can more effectively identify small-volume obstacles， which has certain application prospects.

Keywords： ROS system; depth camera; lidar; fusion

0引言

隨着科学技术的发展，机器人技术已广泛应用于各个行业。相较于用于传统工业的机器人，机器人领域的研究重点已经转移到了智能移动机器人上，智能移动机器人的应用变得愈加广泛，并能完成一部分人类的工作[1]。本文使用 ROS 系统，ROS 设计的主要目的是提高机器人研发中的代码重用度，是一个分层架构。尽管 ROS 系统仍然存在一些缺点，但是对其系统体系结构的研究将对机器人的整体开发和设计带来很大的帮助[2]。在智能移动机器人系统中，如何使用传感器对获得的数据进行更精确的映射，定位，感知和导航变得十分重要。自主移动机器人的传感器技术是指通过摄像头，激光雷达，毫米波雷达，超声波雷达等传感器与外界环境交互[3]。一种传感器必定存在其自身的缺点，必须采取有效措施来杜绝其缺点，以便于机器人更好地完成环境感知的任务。

1系统方案及部分模块选型

移动机器人可以自主导航也可以接受上位机控制移动，并在未知环境中实时构建地图，移动机器人机箱中携带的微计算机和5G 模块，机器人使用5G 信号从本地计算机接收控制命令，进而通过决策层微控制器配备的 Ubuntu操作系统携带的 ROS 框架，以计算每种算法并规划路径，并控制中间层（MCU），控制板卡将 PWM 扩展到驱动电机以使其旋转，完成机器人的移动并在测试场景中前进、后退、转弯等。通过激光雷达和深度相机传感器获取未知的环境信息和数据，最后由决策层的软件系统获取的环境数据进行处理，进一步进行路线轨迹，自主导航，地图构建和地图更新避障等操作。

该移动机器人的硬件系统由三部分组成。第一部分是底层，主要由电源、电机和各种传感器组成。第二部分是中间层，中间层以 MCU 为主要控制器控制下层单元，并与上层交互。第三部分是上层， NVIDIA JESTON NANO 嵌入式智能开发板，可以接收下面的数据并发送控制命令，并实现人机交互。系统结构示意图如图1所示。

上层部分主控模块选用英伟达 JETSON NANO 嵌入式智能开发板，激光雷达选择 RPLIDAR A3激光雷达扫描测距仪，深度相机选择英特尔 Intel RealSense 双目深度摄像头。

2相机与激光雷达数据融合

当前，多源传感器的融合[4]是无人驾驶环境传感的研究重点，必要将激光雷达和摄像头融合起来，利用它们各自的优势互相促进以改善系统环境感知的能力。相机数据和激光雷达数据的提取和矫正是进一步融合两个传感器的重要前提。

2.1相机图像角点提取

采取多边形平面板的方法，简称为PPB方法[5]来提取角点，在提取角度点的过程中，拍摄图像并校正失真后，手动调整像素位置以直接选择拐角点，对于角点标记的位置，程序会准确读取角点坐标。

2.2激光雷达点云数据的提取

采用测量点云反射强度的方法来解决激光雷达点云数据的角点提取问题，激光雷达角点提取示意图如图2所示。

为了确定获得的边缘点之间互相的位置关系，可以使用点云数据精确地读取每个点，并且可以使用该点的3D 坐标值计算其所属的激光束的俯仰角，从而可以计算该点所属的激光线的条数。假设所获得的3D 点云数据中的一个点 P=（x，y，z），计算线束 n 的方法如下：

式中：Angle——激光线束的一个俯仰角且是由点的三维坐标所求得;

gene——比例因子且为一固定的数值;

n?所要求取的点 P 所位于的激光线束的条数。

可以求出每条激光线点云上与标定平面的交点或者距离平面几何距离最小的点，并且可以获得这些点的拟合直线，以获得每个边缘三维空间中的通用表达式。最后分别处理四个边缘，并获得3D 空间中两条相邻线的交点，即校准板和相机的对应四个角，传统的方程组方法不一定能够求得所需要的角点坐标，且由于调整后的直线不一定是空间中的共面直线因此需要使用别的方法来计算出所需的角点坐标。

2.3直线与平面拟合算法

为了能够求出激光雷达数据的角点坐标，提出了一种直线与平面拟合的算法，具体方法如下：

知道空间直线的一般方程的情况下，设点 p1=（x1 ，y1 ，z1）和点p2=（x2 ，y2 ，z2）为直线L1上的两点，点p3=（x3 ，y3 ，z3）和点p4=（x4 ，y4 ，z4）为直线L2上的两点，则两直线对应的方向向量分别表示为：

此时可以求得同时垂直于直线L1和L2的法向量：

根据空间解析几何的原理可知，一条直线与直线外一点可以确定一个平面，所以借助直线L1上一个任意的点P1与直线的法向量 c 可以构造一个同时过直线L1和向量 c 的平面，记作：

通过式（5）所确定的平面与L2联立一个线性方程组，可以求得一点Q1，记作Q1=（X1 ， Y1 ， Z1），该点则为平面与L2的交点。同样的思想可以由直线L2上的一点p3与法向量 c 可以构造一个同时过直线L2和向量 c 的平面，记作：

通过式（6）所确定的平面与L1联立一个线性方程组，可以求得一点Q2，记作Q2=（X2 ， Y2 ， Z2），则该点为平面与L1的交点。由于L1与L2为两条互相异面的直线，则通过拟合所得到的直线交点估算为上面所求得的Q1与Q2的坐标平均值。

2.43D 与2D 投影的求解方法

在知道角点的3D 与2D 坐标之后，可以使用求解 PNP 问题的方法来求取它们之间的变换。

在软件OpenCV3.4中，用solvePNP迭代法求解3维到2维的转换中，设空间中的点为[X0 ， Y0 ， Z0 ， 1]T，二维图像上的对应点为[x，y， 1]T，则三维点变换到二维点的求解原理为公式为：

其中，f1、f2、u0、v0是可以从相机参数中读到的固定系数，那么才这个变换中就有 R11、R12、R13、R21、 R22、R23、R31、R32、R33、T1、T2、T3共12个未知数，角点的2D 和3D 结构提供了由X0 ， Y0 ， Z0 ， x，y确定的两个方程。从数学方程式基本求解性质上可以得知6个未知数需要6个方程式联立求解，具体算法流程图如图3所示。

2.5路径规划

本文在杨博等人[6]为了保障机器人安全的研究基础上采用 A*算法将机器人 SLAM 构建出的地图简化成栅格地图，地图中的每一个栅格用0或1表示其特定的信息。若栅格的数值为1，则代表该区域可以通过;若栅格的数值为0，则代表该区域存在障碍物，机器人无法通过该单元格。在地图中设置起点和终点，则机器人会找出从起点栅格到终点栅格将要经过的所有可以通过的栅格，这些栅格就是机器人起点到终点的行驶路径。

在 A*算法中，搜索移动机器人当前位置所在位置的周围节点，计算从起始点至当前位置周围节点的实际值与该节点至目标位置和的最小值节点，将该节点作为机器人下一步运行方向。继续重复进行上述过程，直到移动机器人到达目标位置，从而获得从起始位置到目标位置的最终路径[7]。

3实验结果与分析

实验使用基于 ROS 平台的移动机器人。将激光雷达与深度相机安装于机器人的顶端并且通过 USB 接口与决策层连接，采集到的数据直接由英伟达 JETSON NANO 嵌入式智能开发板的主处理器进行处理，使得機器人可以独立进行环境感知。

3.1 SLAM 方法选择

在本文中，构建了包含小障碍物的精确地图，以提高检测小障碍物的能力。使用Gmapping算法通过融合传感器数据来构建地图，该方法是一种有效的 Rao-Blackwellized粒子滤波算法，并且网格地图是由传感器使用该算法从周围环境中获取的数据构建而来。在 SLAM 中使用此粒子过滤时，每个粒子点代表一个单独的环境图[8]。找到什么样的方法来减少粒子的数量成为了问题的关键，为了减少粒子数量，有Gmapping算法的研究人员提出了几种算法包括 ORB 算法、SURF 算法以及 SIFT 算法[9]，通过对比算法平均匹配运行时间选出平均匹配时间最短的方法来进行接下来的数据融合实验，实验结果如表1所示。

通过表1可以看出本实验中 ORB 算法所用的运行总时间以及平均匹配时间均少于另外两种算法且精度也能达到要求，因此将使用 ORB 算法进行 slam 地图构建。

3.2数据融合实验

单线激光雷达无法扫描障碍物，此外还会严重影响移动机器人的地图构建和导航避障，因此将融合单线激光和深度摄像头的信息来构建地图，该软件平台为 ROS。首先，使用 ROS 中的 PCL 库将深度相机的3D 点云数据转换为2D 点云数据，并将单线激光雷达中的Laserscan数据类型转换为2D 点云，然后使用修改过的 IRA 工具包源代码的 Laser 部分，使用它来合并深度相机和激光雷达数据，生成虚拟传感器，然后转换虚拟传感器坐标，最后将融合到的数据流放入Gmapping包中。如图4所示的实验场景及其前后的结果，在融合之前无法用单线激光雷达在实验场景中检测到障碍物，融合后可以准确地检测出低障碍物。

4结束语

为了使智能移动机器人能够更好的进行环境识别，结合 PNP 问题求解方法，提出了一种基于直线与平面拟合的算法，完成了相机与激光雷达的融合。在实验过程中采用Gmapping算法来构建网格地图，大大地减少了Rao-Blackwellized粒子滤波的粒子数量，通过实验选择了Gmapping方法中运行时间较短且算法精度较高的 ORB 算法进行 slam，数据融合后成功地识别了较小障碍物，更好地完成了机器人的环境识别任务，具有一定的应用前景。

参考文献

[1]陈志贤.面向复杂环境的服务机器人自主规划方法研究[D].深圳：中国科学院大学， 2019.

[2]胡春旭. ROS 机器人开发实践[M].北京：机械工业出版社，2018.

[3]王鹏飞.基于 ROS 的多传感器信息融合自主导航控制系统设计与实现[D].南京：南京邮电大学， 2019.

[4] GROVES P D. GNSS 与惯性与多传感器组合导航系统原理[M].练军想， 唐康华， 译.北京：国防工业出版社出版社， 2015：28-39.

[5]丁业峰.室内移动机器人激光雷达导航算法研究.[D]哈尔滨：哈尔滨工业大学， 2019.

[6]杨博， 张缓缓， 江忠顺.智能车辆的避障路径规划与跟踪控制仿真分析[J/OL].中国测试：1-8[2021-06-20]. http：//kns.cnki. net/kcms/detail/51.1714.TB.20201016.1753.015.html.

[7] GU J J， CAO Q X. Path planning for mobile robot in a 2.5- dimensionalgrid-basedmap[J]. IndustrialRobot： An International Journal， 2011（3）：315-321.

[8]曾奇.激光测距与视觉融合的室内导航系统研究[D].成都：電子科技大学， 2018.

[9] ZI J Y ， GIM HL .3DFeat-Net： weakly supervised local 3D features for point cloud registration[C]// European Conference on Computer Vision， 2018.

（编辑：徐柳）