一种基于激光数据的移动机器人点云地图创建方法与流程

本发明属于智能机器人导航领域，特别地涉及一种基于激光数据的移动机器人点云地图创建方法。

背景技术：

地图创建是移动机器人相关技术中的一个基本而且十分重要的问题，在移动机器人导航定位和全局路径规划等相关领域有着广泛的应用。为了实现移动机器人的自主导航，必须通过自定位来获取机器人在环境中的准确位姿。根据先验地图进行的移动机器人定位和导航技术得到了广泛的研究，并取得了很好的应用效果。但是实际环境往往是未知的，不能提供诸如环境大小、形状、障碍物位置等先验信息，而且实际环境中也往往不存在路标、灯塔等人为设定的标志物，这给移动机器人的自主导航带来了很大的问题。所以，在未知环境中，如何通过传感器提取出环境和障碍物信息，并快速地创建环境地图是移动机器人自定位、导航和路径规划问题解决的关键。

近年来，由于导航定位以及路径规划等技术的迫切需要，关于移动机器人基于激光扫描数据而进行地图创建的研究发展迅速。匹配是基于激光测距仪环境地图创建的一种核心方法。匹配就是利用两组激光数据点集并根据两组点集间相关数据的配准来估计两组点集的位姿变化，使得两组激光数据点集能够最大程度的重合，从而完成机器人环境地图的创建。国际上有许多学者进行了大量的研究，提出了大量的数据点集匹配方法。总体上分为两类：一是采用经过高精度标定的仪器来获取不同视点的数据以及它们之间的变换关系进行数据间的配准计算法。但是由于受到条件的限制，很难得到这些数据采集位置信息，因此应用范围并不广泛。二是在没有任何控制之下，基于数据点集信息对相邻数据点集的配准。由于这种方法应用范围广泛，得到了大量的研究应用。

基于激光数据点集匹配算法大致可分为非迭代匹配算法和迭代匹配算法。

直方图匹配算法是典型的非迭代匹配算法，该算法利用角度直方图和位移直方图进行匹配，其最大优点是容易实现，匹配速度快，但是由于与直方图的分辨率有关，直方图的匹配精度十分有限。迭代最近点(iterative closest point)算法是一种典型的迭代匹配算法。该算法是一种通过在迭代过程中不断降低配准误差来获取采样点相对位置变换的扫描匹配方法。总体而言，非迭代匹配算法的匹配速度较快但是匹配精度不高，结合基于激光扫描数据进行地图创建的要求，迭代匹配算法更适用于移动机器人导航系统。

技术实现要素：

本发明的目的在于解决现有技术中存在的问题，并提供一种基于激光数据的移动机器人点云地图创建方法。本发明所采用的具体技术方案如下:

一种基于激光数据的移动机器人点云地图创建方法，包括如下步骤：

步骤1：移动机器人与客户端之间建立实时通信和数据传输；

步骤2：激光传感器和里程计随移动机器人的运动采集外部环境数据并传输至工控机，生成本地数据文件；

步骤3：针对本地数据文件，对其已存储的数据进行预处理，删除匹配度低于阈值的数据帧；

步骤4：根据预先采集的激光和里程计信息进行离线计算，形成机器人所在环境的二维点云地图。

作为优选，基于步骤1的具体过程为：

步骤1.1：移动机器人与客户端连接于同一个局域网，并基于UDP连接实现实时通信；

步骤1.2：移动机器人上搭载的工控机与解算板通过RS232串口进行通信，工控机与激光传感器通过TCP/IP连接进行数据传输。

作为优选，基于步骤2的具体过程为：

步骤2.1：激光传感器以一定的频率和角度范围平面扫描外部环境，获取激光传感器所在位置与周边环境间的距离数据，并实时传输至工控机；

步骤2.2：里程计随着机器人的平面运动实时记录机器人的位置和姿态信息，同时传输至工控机；位置信息包括X、Y轴坐标值，姿态信息包括旋转角度值；

步骤2.3：工控机将接收到的激光扫描数据和位置姿态信息存储于本地，生成本地数据文件。

作为优选，基于步骤3的具体过程为：

步骤3.1：针对本地数据文件，从数据帧首帧开始，采用迭代最近点算法分别匹配相邻两帧数据，并获得相邻两数据帧的匹配度值；

步骤3.2：设定匹配度阈值，删除匹配度低于阈值的数据帧，保留匹配度不低于阈值的数据帧；

步骤3.3：将所保留的数据帧重新输出至本地文件进行存储。

进一步的，基于步骤4的具体过程为：

步骤4.1：将步骤3.3中存储的本地数据文件中各数据帧以点云图的形式显示；

步骤4.2：选择该本地数据文件中待拼接的数据帧的范围为第p帧至第q帧，其中q-p>＝0；

步骤4.3：采用点云库中迭代最近点算法从第p帧至第q帧数据进行逐帧自动点云匹配和数据融合；待匹配完成后，更新显示匹配后的数据集点云地图，将点云地图以*.map的形式存储于本地文件。

更进一步的，基于步骤4.3已创建的点云地图进行路径规划和机器人手动控制，具体步骤为：

步骤5.1：客户端加载*.map点云地图，并显示该地图文件所表示的点云；

步骤5.2：通过UDP连接，客户端从移动机器人实时获取里程计记录的机器人位置和姿态信息，并实时模拟显示移动机器人在地图的相应位置。

步骤5.3：通过客户端进行地图路径规划，添加直线路径和曲线路径，其中曲线形式为三次贝塞尔曲线；

步骤5.4：通过客户端添加停靠点，选择已绘制的直线路径，并在此路径上添加点作为移动机器人导航时的停靠点；

步骤5.5：通过客户端传输运动控制命令至移动机器人上的工控机，实现机器人前后左右手动移动控制、原地旋转控制以及运动速度设置。

步骤5.6：将路径的起始点、长度、方向以及贝塞尔曲线的控制点信息存储至数据库中，同时也将停靠点坐标信息等存储至数据库中。

再进一步的，步骤5.4中，客户端接收到用户所选的点P(x₀,y₀)后，计算该点到各直线路径的距离值，取最小距离值，若该距离值d₁小于一定的阈值β₁，则将该距离值对应的直线路径作为所选路径；点到直线的距离计算公式如下：

其中，公式中的直线路径的方程为Ax+By+C＝0；

选定路径后，进行添加停靠点的操作，客户端接收到用户所选的停靠点Q(x₁,y₁)后，通过上述距离公式计算Q点到所选路径的距离d₂，若d₂小于一定的阈值β2，则将Q点作为停靠点。

所述的数据库中创建有路径信息数据表和停靠点信息数据表，路径信息数据表中包含路径起止点、直线路径长度、直线路径方向、曲线路径控制点信息；停靠点信息数据表中包含停靠点坐标、所停靠的路径信息。

本发明数据采集操作简单，且无需预先了解环境结构空间，也无需额外铺设地标，成本低廉、地图创建速度快精度高，可实现移动机器人在未知环境下地图的多次创建和快速更新。

附图说明

图1为本发明的移动机器人的结构框架图；

图2为本发明实施例中加载数据帧后的示意图；

图3为本发明实施例中拼接效果示意图；

图4为本发明实施例中添加的直线路径示意图。

具体实施方式

下面结合附图和具体实施方式对本发明做进一步阐述，以便更好的理解本发明。本发明的技术特征在没有冲突的情况下，均可进行相互组合。

如图1所示，本发明的移动机器人内置工控机、解算板、激光传感器和里程计，工控机与解算板通过RS232串口进行通信。工控机通过wifi与客户端进行远程通信。客户端可采用PC机、服务器等形式实现。

本实施方式中，基于激光数据的移动机器人点云地图创建方法，包括如下步骤和子步骤：

步骤1：移动机器人与客户端之间建立实时通信和数据传输。具体子步骤：

步骤1.1：移动机器人与客户端连接于同一个局域网，并基于UDP连接实现实时通信；

步骤1.2：移动机器人上搭载的工控机与解算板通过RS232串口进行通信，工控机与激光传感器通过TCP/IP连接进行数据传输。

步骤2：激光传感器和里程计随移动机器人的运动采集外部环境数据并传输至工控机，生成本地数据文件。具体子步骤：

步骤2.1：激光传感器以一定的频率和角度范围平面扫描外部环境，获取激光传感器所在位置与周边环境间的距离数据，并实时传输至工控机；

步骤2.2：里程计随着机器人的平面运动实时记录机器人的位置(包括X、Y轴坐标值)和姿态信息(包括旋转角度值)，同时传输至工控机；

步骤2.3：工控机将接收到的激光扫描数据和位置姿态信息存储于本地，生成本地数据文件，格式为*.2d。

步骤3：针对本地数据文件*.2d，对其已存储的数据进行预处理，删除匹配度低于阈值的数据帧。具体子步骤：

步骤3.1：针对本地数据文件，从数据帧首帧开始，采用迭代最近点算法分别匹配相邻两帧数据，并获得相邻两数据帧的匹配度值；

步骤3.2：设定匹配度阈值，删除匹配度低于阈值的数据帧，保留匹配度不低于阈值的数据帧；

步骤3.3：将所保留的数据帧重新输出至本地文件进行存储。

步骤4：根据预先采集的激光和里程计信息进行离线计算，形成机器人所在环境的二维点云地图。具体子步骤：

步骤4.1：将步骤3.3中存储的本地数据文件(*.2d)中各数据帧以点云图的形式显示；

步骤4.2：选择该本地数据文件中待拼接的数据帧的范围为第p帧至第q帧(如图2所示，其中(q-p)>＝0)，同时在图像显示区用不同颜色分别标注出第p帧数据和第q帧数据；

步骤4.3：采用点云库(PCL)中迭代最近点(ICP)算法从第p帧至第q帧数据进行逐帧自动点云匹配和数据融合，如图3所示。待匹配完成后，更新显示匹配后的数据集点云地图，将点云地图以*.map的形式存储于本地文件。

ICP算法分为两大部分，对应点的搜索和变换参数的求解。整个算法就是在对对应点对的搜素——变换参数的计算——对应点进行刚性变换——检验目标函数值的迭代过程中不断降低匹配误差以获得两组数据间的相对位姿变化的最优变换参数(R,T)，其中R为旋转矩阵，T为平移矩阵。ICP算法的具体实现过程如下：

给定两组临近时刻扫描所得的激光数据点集P＝{Pi,i＝0,1,2,…,m}及Q＝{Qi,i＝0,1,2,…,n}，设置最小误差度量值ε和最大迭代次数D。

估计配准：数据点集P与U之间的估计配准由同时存储在2d文件中与这两组数据点对应的里程计数值给出。没有估计配准，匹配算法会陷入局部极小值。

寻找对应点：根据欧式距离最小原则寻找，欧式距离定义如下：

对于集合Q中的各点，在集合P中找出距该点最近的对应点，且设集合P中由这些对应点组成的新点集为P‘＝{P’i,i＝0,1,2,…,n}。作为求解变换的匹配点。

寻找变换(R,T)：使得两组数据点间的误差度量值最小，计算公式如下：

其中ω为旋转弧度，旋转矩阵平移向量Q_i为参考点集，P’_i为对应的带匹配的点集。通过最小化E_k，可以得到T_x、T_y和ω的解。

应用变换，更新机器人位姿。用配准变换矩阵R、T进行坐标变换，得到新的点集Q₁，即Q₁＝RQ+T，计算E_k+1。

迭代：当|E_k-E_k+1|>ε，重复步骤(3)-(5)。否则，跳出迭代，计算结束，匹配成功。

由此，完成移动机器人点云地图的创建步骤，基于该点云地图可以进行路径规划和机器人手动控制，具体如下：

步骤5：基于步骤4.3已创建的点云地图进行路径规划和机器人手动控制，各子步骤为：

步骤5.1：客户端加载*.map点云地图，并显示该地图文件所表示的点云；

步骤5.2：通过UDP连接，客户端从移动机器人实时获取里程计记录的机器人位置和姿态信息，并实时模拟显示移动机器人在地图的相应位置。

步骤5.3：通过客户端进行地图路径规划，添加直线路径和曲线路径，其中曲线形式为三次贝塞尔曲线B(t)，公式如下：

B(t)＝P₀(1-t)³+3P₁t(1-t)²+3P₂t²(1-t)+P₃t³,t∈[0,1]

上式P₀、P₁、P₂、P₃四个点在平面中定义了三次方贝塞尔曲线，P₀、P₃为曲线起始点，P₁、P₂为曲线控制点，贝塞尔曲线始于P₀走向P₁，并从P₂的方向来到P₃。

步骤5.4：通过客户端添加停靠点，选择已绘制的直线路径，并在此路径上添加点作为移动机器人导航时的停靠点。

该步骤中，具体实现方式为：客户端接收到用户所选的点P(x₀,y₀)后，计算该点到各直线路径的距离值。如通过鼠标点击选择一条已绘制的直线路径。计算鼠标当前点击的点P(x0,y0)到各直线路径的距离值。取最小距离值，若该距离值d₁小于一定的阈值β₁，则将该距离值对应的直线路径作为所选路径；点到直线的距离计算公式如下：

其中，公式中的直线路径的方程为Ax+By+C＝0；

选定路径后，进行添加停靠点的操作，客户端接收到用户所选的停靠点Q(x₁,y₁)后，通过上述距离公式计算Q点到所选路径的距离d₂，若d₂小于一定的阈值β2，则将Q点作为停靠点。并且可将Q点作为停靠点并用不同颜色标记出来，以示标记。本实例中，添加的直线路径如图4所示。

步骤5.5：通过客户端传输运动控制命令至移动机器人上的工控机，实现机器人前后左右手动移动控制、原地旋转控制以及运动速度设置。

步骤5.6：将路径的起始点、长度、方向以及贝塞尔曲线的控制点信息存储至数据库中，同时也将停靠点坐标信息等存储至数据库中，供后续调用。数据库中创建路径信息数据表，表中包含路径起止点、直线路径长度、直线路径方向、P1、P2曲线路径控制点等信息。数据库中创建停靠点信息数据表，表中包含停靠点坐标、所停靠的路径等信息。

上述实施例仅用于解释说明本发明要求保护的内容，但并不是用于限制本发明的要求保护的范围。本领域技术人员在本发明精神内所做的改进和替换，均属于保护范围内。