CN106681320A - 一种基于激光数据的移动机器人导航控制方法 - Google Patents

Abstract

本发明公开了一种基于激光数据的移动机器人导航控制方法。该方法包括以下步骤：步骤1：全局地图中添加和设置路径；步骤2：通过迭代匹配算法获取移动机器人的实时定位信息；步骤3：通过最短路径算法规划移动机器人导航所需最短路径；步骤4：通过左右轮差速控制实现移动机器人沿所规划路径导航行驶；步骤5：通过减速控制实现移动机器人准确停靠在路径上的目标停靠点。本发明数据采集操作简单，也无需额外铺设地标，成本低廉、定位精度高，可实现移动机器人在室内及室外环境下的自主定位和导航控制。

Description

一种基于激光数据的移动机器人导航控制方法

技术领域

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

背景技术

在机器人研究领域中，机器人导航技术快速发展。随着时代的进步以及科技水平的不断提高，在环境中对机器人进行准确的定位也变得越来越重要，导航包含了多种不同的技术和应用：数据关联、映射算法、定位算法和同步定位与地图构建等。每一个步骤和细节可以通过不同的方法或传感器获取。

移动机器人导航方法主要可以分为两种：一种是基于地图的导航方法，另一种是无地图导航方法。基于地图的导航方法主要是通过自动或者半自动方法构建地图，通常采用激光测距仪、超声波传感器、或视觉系统来构建地图。地图主要有四种：栅格地图、拓扑地图、特征地图以及混合地图。栅格地图通常是由激光测距仪产生的，有些方法使用超声波传感器也可以得到类似的栅格地图。拓扑地图的构建也是使用相同的传感器但是地图的思想不一样。机器人在特定的位置记录路标，使用路标之间的关联来对机器人进行定位。大多数的特征地图的构建是通过照相机获取然后用计算机视觉算法处理得到。然后，机器人通过对观测值和期望值之间的比较对自身进行定位。如果映射方法能够综合以上两种或三种方法或传感器，称之为混合地图。

目前正在运行的移动机器人主要是采用磁轨迹规划巡检路线，以及无线射频识别(RFID)等技术实现导航。这种方法需预先设定巡检机器人的巡检路线，然后在运行路线上铺设磁轨道，并且在机器人停靠的位置预先设置RFID标识，存在地面施工复杂工作量大、机器人运行路线不灵活等问题。而采用二维激光导航，不仅定位准确度高、成本低、不受光线影响，而且具备抗电磁干扰，对配置系统要求不高等优点。

发明内容

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

基于激光数据的移动机器人导航控制方法，包括如下步骤：

步骤1：在全局地图中添加、设置路径和停靠点；

步骤2：获取移动机器人的实时定位信息；

步骤3：规划移动机器人导航所需最短路径；

步骤4：移动机器人沿所规划路径导航行驶，且在行驶过程中，实时计算机器人中心点到目标路径的距离、与目标路径的方向差以及中心点到目标节点的距离，并实时调整机器人的两侧轮速，使其逐渐靠近目标节点；

步骤5：当到达目标节点附近时，进行减速，使移动机器人停靠在目标节点附近；依次对后续的目标节点重复上述步骤，使移动机器人最终停靠在目标停靠点上。

作为优选，所述步骤1的具体过程为：

步骤1.1：进行地图路径设置，在点云地图中添加节点，同时连接节点绘制路径；

步骤1.2：选择一条已绘制的路径，并在此路径上添加点作为移动机器人导航时的一个停靠点；

步骤1.3：将各路径的起始节点坐标、终止节点坐标、路径长度、路径方向信息存储至数据库中，同时也将停靠点的编号信息、坐标信息存储至数据库中。

作为优选，所述步骤2的具体过程为：

步骤2.1：移动机器人自带的激光传感器以一定的频率和角度范围平面扫描外部环境，获取激光传感器所在位置与周边环境间的距离数据，构成此次扫描的点云数据；

步骤2.2：移动机器人初始定位时，预设一个初始位置值作为迭代匹配算法的估计配准量，非初始定位时将前一帧点云数据匹配所得的定位值作为后一帧激光数据用于匹配的估计配准量；

步骤2.3：通过迭代匹配算法将扫描所得点云数据与现有全局地图的整体点云数据进行匹配，获得移动机器人上的激光传感器在全局地图坐标系下的定位信息，包括X、Y轴坐标值(x_L，y_L)和方向角θ_L；

步骤2.4：将获得的激光传感器的定位信息转化为移动机器人中心点P_R的坐标值(x_R，y_R)和方向角θ_R，并以中心点的定位值作为整个移动机器人的定位信息；

其中，x_R＝x_L-L·cosθ_L，y_R＝y_L-L·sinθ_L，θ_R＝θ_L,其中L为激光传感器到移动机器人中心点的平面距离。

迭代匹配算法可选择现有技术中的方法，例如ICP算法，就是一种典型的迭代匹配算法。该算法是一种通过在迭代过程中不断降低配准误差来获取采样点相对位置变换的扫描匹配方法。主要分为两大部分，对应点的搜索和变换参数的求解。整个算法就是在对对应点对的搜素——变换参数的计算——对应点进行刚性变换——检验目标函数值的迭代过程中不断降低匹配误差以获得两组数据间的相对位姿变化的最优变换参数(R,T)，其中R为旋转矩阵，T为平移矩阵。

作为优选，所述步骤3的具体过程为：

步骤3.1：移动机器人获取数据库中该目标停靠点的信息；

步骤3.2：获取移动机器人的初始定位信息(x_R0，y_R0，θ_R0)；

步骤3.3：通过最短路径算法计算移动机器人的初始位置到目标停靠点位置P_s(x_s，y_s)运行距离最短的路径组合；

步骤3.4：存储步骤3.3中获得的最短距离的路径信息，包括所需途经的节点顺序(P₁，P₂…P_n-1，P_s)和路径顺序(L₁、L₂…L_n-1，L_n)。

最短路径算法可采用现有技术中的算法，例如Dijkstra算法，是典型的最短路径算法，用于计算一个节点到其他所有节点的最短路径。将地图中规划的所有路径、节点、停靠点的组合视为带权有向图，每条路径均为双向，且各路径长度为该路径的权值。将机器人初始位置作为源点，通过Dijkstra算法计算源点到图中其他各点的最短路径，而后选出源点到目标停靠点的最短路径。

作为优选，所述步骤4的具体过程为：

步骤4.1：移动机器人在导航初始时刻，将步骤3中得到的最短路径中的第一个节点P₁和第一条路径L₁分别作为目标节点和目标路径；

步骤4.2：计算移动机器人当前方向角与目标路径方向的角度差Δθ₀，移动机器人以角度Δθ₀进行原地旋转，调整行进方向与目标路径方向保持一致；

步骤4.3：移动机器人向前行进，在行进过程中实时计算机器人中心点P_R(x_R，y_R)到目标路径的垂直距离Δs、机器人行进方向与目标路径的方向差Δθ以及机器人中心点到目标节点的直线距离d；例如，中心点到目标路径的距离计算公式如下：其中，公式中目标路径所在的直线路径方程为Ax+By+C＝0；中心点到目标节点的距离公式如下：

步骤4.4：计算移动机器人轮速控制量ΔV＝k_s·Δs+k_θ·Δθ，其中k_s,k_θ分别为预设的距离Δs和方向差Δθ的权值；从而根据前一时刻的左轮轮速V_L-before和右轮轮速V_R-before，实时更新并控制移动机器人当前时刻的左轮轮速V_L-after和右轮轮速V_R-after，其中V_L-after＝V_L-before+ΔV，V_R-after＝V_R-before-ΔV。

作为优选，所述步骤5的具体过程为：

步骤5.1：在移动机器人行进过程中，判断机器人中心与目标节点的距离d是否小于一定阈值d₀，若d小于d₀，则视为移动机器人已经到达目标节点附近；

步骤5.2：然后控制移动机器人进行减速，对左右轮速度增加动态控制量控制左轮轮速为V’_L-after和右轮轮速为V’_R-after，其中V_L-after’＝k_d·V_L-after，V_R-after’＝k_d·V_R-after；当移动机器人不断靠近目标节点，即d越小时k_d值也越小直至0，使移动机器人在目标节点处停下；

步骤5.3：将步骤3中得到的最短路径中的下一个节点P₂和下一条路径L₂分别作为目标节点和目标路径，重复步骤4.2至步骤5.2；依次类推，直至运动至路径L_n，并停靠在目标停靠点P_s。

本发明数据采集操作简单，且无需预先了解环境结构空间，也无需额外铺设地标，成本低廉、地图创建速度快精度高，可实现移动机器人在室内室外环境下的准确导航控制。其他技术效果将在具体实施方式中进一步说明。

附图说明

图1全局地图路径规划；

图2移动机器人导航时各控制量示意图。

具体实施方式

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

本实施方式中，移动机器人并没有构造和型号上的限制，但其用于驱动的左轮和右轮需要能够分别调整轮速。其正前端搭载用于扫描周边环境的激光传感器，激光传感器中心与移动机器人中心点的连线与机器人的行进方向一致。

基于激光数据的移动机器人导航控制方法，包括如下步骤和子步骤：

步骤1：全局地图中添加和设置路径，具体子步骤：

步骤1.1：首先构建一个用于控制机器人和输入指令、信息的客户端，通过客户端进行地图路径设置，在点云地图中添加节点，同时连接节点绘制路径。本实施例中地图路径节点如图1所示。

步骤1.2：通过客户端添加停靠点，选择一条已绘制的路径，并在此路径上添加点作为移动机器人导航时的一个停靠点，该停靠点将作为最后一个目标节点。

步骤1.3：将各路径的起始节点坐标、终止节点坐标、路径长度、路径方向信息存储至数据库中，同时也将停靠点的编号信息、坐标信息存储至数据库中，供机器人的控制系统调用。

步骤2：获取移动机器人的实时定位信息，具体子步骤：

步骤2.1：移动机器人自带的激光传感器以一定的频率和角度范围平面扫描外部环境，获取激光传感器所在位置与周边环境间的距离数据，构成此次扫描的点云数据；

步骤2.2：移动机器人第一次开始定位时，为了提高定位的速度和准确性，可事先由用户估计一个初始位置值，并输入客户端，然后将初始位置值作为迭代匹配算法的估计配准量。当非初始定位时，可以将前一帧点云数据匹配所得的定位值作为后一帧激光数据用于匹配的估计配准量；

步骤2.3：通过迭代匹配算法将扫描所得点云数据与现有全局地图的整体点云数据进行匹配，获得激光传感器在全局地图坐标系下的定位信息，包括X、Y轴坐标值(x_L，y_L)和方向角θ_L(机器人行进方向与X轴的夹角)。在机器人后续的行进过程中，也通过该方法不断地确定自身的定位信息，以计算各种参数。

本实施例中采用ICP算法作为迭代匹配算法。该算法的具体实现过程如下：

给定激光数据点集P＝{P_i,i＝0,1,2,…,m}及全局地图数据点集Q＝{Q_i,i＝0,1,2,…,n}，设置最小误差度量值ε和最大迭代次数D。

(1)估计配准：在数据点集P与Q匹配之前，将前一次迭代匹配完成后获得的匹配结果作为P与Q的估计配准值。没有估计配准，匹配算法会陷入局部极小值。

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

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

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

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

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

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

步骤2.4：由于激光传感器测量得到的定位信息实际上是其自身的定位，但其与移动机器人的中心点存在一定的位置差异，因此需要进行校正：首先测量激光传感器到移动机器人中心点的平面距离L，将步骤2.3中获得的激光传感器的定位信息转化为移动机器人中心点P_R的坐标值(x_R，y_R)和方向角(θ_R)，并以中心点的定位值作为整个移动机器人的定位信息。

其中，x_R＝x_L-L·cosθ_L，y_R＝y_L-L·sinθ_L，θ_R＝θ_L。

步骤3：规划移动机器人导航所需最短路径。具体子步骤：

步骤3.1：通过客户端确定目标停靠点，移动机器人获取数据库中该目标停靠点的信息；

步骤3.2：通过步骤2获取移动机器人的初始定位信息(x_R0，_yR0，θ_R0)；

步骤3.3：通过最短路径算法计算移动机器人初始位置到目标停靠点位置P_s(x_s，_ys)运行距离最短的路径；

Dijkstra算法是典型的最短路径算法，该算法的具体实现过程如下：

(1)算法思想：设G＝(V,E)是一个带权有向图，把图中顶点集合V分成两组，第一组为已求出最短路径的顶点集合(用S表示，初始时S中只有一个源点，以后每求得一条最短路径,就将加入到集合S中，直到全部顶点都加入到S中，算法就结束了)，第二组为其余未确定最短路径的顶点集合(用U表示)，按最短路径长度的递增次序依次把第二组的顶点加入S中。在加入的过程中，总保持从源点_v到S中各顶点的最短路径长度不大于从源点v到U中任何顶点的最短路径长度。此外，每个顶点对应一个距离，S中的顶点的距离就是从v到此顶点的最短路径长度，U中的顶点的距离，是从v到此顶点只包括S中的顶点为中间顶点的当前最短路径长度。

(2)算法步骤：

a.初始时，S只包含源点，即S＝{v}，v的距离为0。U包含除v外的其他顶点，即:U＝{其余顶点}，若v与U中顶点u有边，则<u,v>正常有权值，若u不是v的出边邻接点，则<u,v>权值为∞。

b.从U中选取一个距离v最小的顶点m，把m，加入S中(该选定的距离就是v到m的最短路径长度)。

c.以m为新考虑的中间点，修改U中各顶点的距离；若从源点v到顶点u的距离(经过顶点m)比原来距离(不经过顶点m)短，则修改顶点u的距离值，修改后的距离值的顶点m的距离加上边上的权。

d.重复步骤b和c直到所有顶点都包含在S中。

步骤3.4：存储步骤3.3中获得的最短距离的路径信息，包括所需途经的节点顺序(P₁，P₂…P_n-1，P_s)和路径顺序(L₁、L₂…L_n-1，L_n)，P_s表示第s个节点，L_n表示第n个条路径。

步骤4：移动机器人沿所规划路径导航行驶。如图2，具体子步骤：

步骤4.1：移动机器人在导航初始时刻，将步骤3中得到的最短路径中的第一个节点P₁和第一条路径L₁分别作为目标节点和目标路径，机器人第一步的目的是移动到该目标节点；

步骤4.2：计算移动机器人当前方向角与目标路径方向的角度差Δθ₀，移动机器人以角度Δθ₀进行原地旋转，调整自身方向与目标路径方向保持一致。该步骤有利于机器人在行进方向与目标路径方向差异较大时，即在需要进行较大角度转弯的地方，快速的调整其自身的行进方向；

步骤4.3：移动机器人直线行走过程中，通过激光传感器不断地确定自身的定位信息，实时计算机器人中心点P_R(x_R，y_R)到目标路径的垂直距离Δs、机器人行进方向与目标路径的方向差Δθ以及中心点到目标节点P’(x_p，y_p)的距离d。中心点到目标路径的距离计算公式如下：其中，公式中目标路径所在的直线路径方程为Ax+By+C＝0；中心点到目标节点的距离公式如下：

步骤4.4：移动机器人可设定一定时间调整一次左右轮速，已调整行进方向。移动机器人的轮速控制量计算方法为：ΔV＝k_s·Δs+k_θ·Δθ，其中k_s,k_θ分别为预设的距离Δs和方向差Δθ的权值，其具体取值可通过试验进行不断优化。两者的取值不一定要采用定值，可以在不同的情况下采用不同的权重，比如根据优先调整的参数，先设置其权值较大，当该参数被调整至目标范围内时，在使另外一个参数的权值较大，对其进行调整。根据前一时刻的左轮轮速V_L-before和右轮轮速V_R-before，可以计算当前时刻的左轮轮速V_L-after＝V_L-before+ΔV，当前时刻的右轮轮速V_R-after＝V_R-before-ΔV。然后利用这两个计算得到的轮速，实时更新并控制移动机器人的左轮和右轮，实现通过左右轮差速调整机器人的前进方向。

步骤5：使移动机器人准确停靠在路径上的目标停靠点，具体子步骤：

步骤5.1：当移动机器人运动至目标节点附近时，需对其进行减速控制。判断步骤4.3中移动机器人中心与目标节点的距离d是否小于一定阈值d₀，若d小于d₀，则视为移动机器人已经到达目标节点附近；

步骤5.2：然后控制移动机器人进行减速，具体方法为：对前面计算得到的左右轮速度增加一个动态控制量k_d(0≤k_d≤1)，使V_L-after’＝k_d·V_L-after，V_R-after’＝k_d·V_R-after。然后分别利用V_L-after’和V_R-after’控制左轮和右轮轮速。当移动机器人不断靠近目标节点，即d越小时k_d值也越小直至0，最终使移动机器人在目标节点处停下。

步骤5.3：到达一个节点后，将步骤3中得到的最短路径中的下一个节点P₂和下一条路径L₂分别作为目标节点和目标路径，重复步骤4.2至步骤5.2。依次类推，直至运动至路径L_n，并停靠在目标停靠点P_s。

Claims (6)

1.一种基于激光数据的移动机器人导航控制方法，其特征在于，包括如下步骤：

步骤1：在全局地图中添加、设置路径和停靠点；

步骤2：获取移动机器人的实时定位信息；

步骤3：规划移动机器人导航所需最短路径；

步骤4：移动机器人沿所规划路径导航行驶，且在行驶过程中，实时计算机器人中心点到目标路径的距离、与目标路径的方向差以及中心点到目标节点的距离，并实时调整机器人的左右轮速，使其逐渐靠近目标节点；

步骤5：当到达目标节点附近时，进行减速，使移动机器人停靠在目标节点附近；依次对后续的目标节点重复上述步骤，使移动机器人最终停靠在目标停靠点上。

2.根据权利要求书1所述的基于激光数据的移动机器人导航控制方法，其特征在于，所述步骤1的具体过程为：

步骤1.1：进行地图路径设置，在点云地图中添加节点，同时连接节点绘制路径；

步骤1.2：选择一条已绘制的路径，并在此路径上添加点作为移动机器人导航时的一个停靠点；

步骤1.3：将各路径的起始节点坐标、终止节点坐标、路径长度、路径方向信息存储至数据库中，同时也将停靠点的编号信息、坐标信息存储至数据库中。

3.根据权利要求书1所述的基于激光数据的移动机器人导航控制方法，其特征在于，所述步骤2的具体过程为：

步骤2.1：移动机器人自带的激光传感器以一定的频率和角度范围平面扫描外部环境，获取激光传感器所在位置与周边环境间的距离数据，构成此次扫描的点云数据；

步骤2.2：移动机器人初始定位时，预设一个初始位置值作为迭代匹配算法的估计配准量，非初始定位时将前一帧点云数据匹配所得的定位值作为后一帧激光数据用于匹配的估计配准量；

步骤2.3：通过迭代匹配算法将扫描所得点云数据与现有全局地图的整体点云数据进行匹配，获得移动机器人上的激光传感器在全局地图坐标系下的定位信息，包括X、Y轴坐标值(x_L，y_L)和方向角θ_L；

步骤2.4：将获得的激光传感器的定位信息转化为移动机器人中心点P_R的坐标值(x_R，y_R)和方向角θ_R，并以中心点的定位值作为整个移动机器人的定位信息；其中，x_R＝x_L-L·cosθ_L，y_R＝y_L-L·sinθ_L，θ_R＝θ_L,其中L为激光传感器到移动机器人中心点的平面距离。

4.根据权利要求书1所述的基于激光数据的移动机器人导航控制方法，其特征在于，所述步骤3的具体过程为：

步骤3.1：移动机器人获取数据库中该目标停靠点的信息；

步骤3.2：获取移动机器人的初始定位信息(x_R0，y_R0，θ_R0)；

步骤3.3：通过最短路径算法计算移动机器人的初始位置到目标停靠点位置P_s(x_s，y_s)运行距离最短的路径组合；

步骤3.4：存储步骤3.3中获得的最短距离的路径信息，包括所需途经的节点顺序(P₁，P₂…P_n-1，P_s)和路径顺序(L₁、L₂…L_n-1，L_n)。

5.根据权利要求书1所述的基于激光数据的移动机器人导航控制方法，其特征在于，所述步骤4的具体过程为：

步骤4.1：移动机器人在导航初始时刻，将步骤3中得到的最短路径中的第一个节点P₁和第一条路径L₁分别作为目标节点和目标路径；

步骤4.2：计算移动机器人当前方向角与目标路径方向的角度差Δθ₀，移动机器人以角度Δθ₀进行原地旋转，调整行进方向与目标路径方向保持一致；

步骤4.3：移动机器人向前行进，在行进过程中实时计算机器人中心点P_R(x_R，y_R)到目标路径的垂直距离Δs、机器人行进方向与目标路径的方向差Δθ以及机器人中心点到目标节点的直线距离d；

步骤4.4：计算移动机器人轮速控制量ΔV＝k_s·Δs+k_θ·Δθ，其中k_s,k_θ分别为预设的距离Δs和方向差Δθ的权值；根据前一时刻的左轮轮速V_L-before和右轮轮速V_R-before，实时更新并控制移动机器人当前时刻的左轮轮速V_L-after和右轮轮速V_R-after，其中V_L-after＝V_L-before+ΔV，V_R-after＝V_R-before-ΔV。

6.根据权利要求书1所述的基于激光数据的移动机器人导航控制方法，其特征在于，所述步骤5的具体过程为：

步骤5.1：在移动机器人行进过程中，判断机器人中心与目标节点的距离d是否小于一定阈值d₀，若d小于d₀，则视为移动机器人已经到达目标节点附近；

步骤5.2：然后控制移动机器人进行减速，对左右轮速度增加动态控制量控制左轮轮速为V’_L-after和右轮轮速为V’_R-after，其中V_L-after’＝k_d·V_L-after，V_R-after’＝k_d·V_R-after；当移动机器人不断靠近目标节点，即d越小时k_d值也越小直至0，使移动机器人在目标节点处停下；

步骤5.3：将步骤3中得到的最短路径中的下一个节点P₂和下一条路径L₂分别作为目标节点和目标路径，重复步骤4.2至步骤5.2；依次类推，直至运动至路径L_n，并停靠在目标停靠点P_s。