CN105955262A

CN105955262A - 一种基于栅格地图的移动机器人实时分层路径规划方法

Info

Publication number: CN105955262A
Application number: CN201610299576.1A
Authority: CN
Inventors: 尤波; 李智; 丁亮; 高海波; 许家忠; 李东洁; 王明睿; 张乐超
Original assignee: Harbin University of Science and Technology
Current assignee: Harbin University of Science and Technology
Priority date: 2016-05-09
Filing date: 2016-05-09
Publication date: 2016-09-21

Abstract

本发明涉及一种基于栅格地图的移动机器人实时分层路径规划方法，该方法在栅格地图中使用，该方法包括：根据外部环境信息进行二值分类并建立栅格地图，根据基于速度的人工势场法进行外层路径规划，在路径即将陷入局部极小和震荡时，根据A星算法进行基于内层路径规划。本发明解决了传统人工势场法可能陷入局部极小和规划路径振荡的问题，解决了A星算法不适用于实时路径规划的问题，保证了移动机器人的实时、安全和稳定运行。

Description

一种基于栅格地图的移动机器人实时分层路径规划方法

技术领域

本发明属于移动机器人路径规划领域，涉及一种基于栅格地图的移动机器人实时分层路径规划方法。

背景技术

移动机器人的路径规划是指在含有障碍物的工作空间中，按照一定的评价指标，搜索一条可以完成指定任务的移动路径。移动机器人工作环境的描述方式主要包括栅格地图、拓扑地图、几何地图与以上几种地图的混合地图。栅格地图一般指以大小相等或不等的方形栅格将外部环境进行划分，根据栅格区域是否被障碍物占据将栅格进行二值划分。这种建模方式便于计算机存储与批量处理，因为是目前最为被广泛应用的地图建立方式。

移动机器人的路径规划主要可以分为单元分解法和势场法等。

单元分解法主要是将空间分解为一系列可以重新进行分类的单元，并对不同单元空间赋予特征信息以表征实际环境特征，通过对不同单元的信息的进行分析得到符合规划需求的最终路径。路径规划的需求跟所选取的具体方法不同而不同，例如A星算法、Dijkstra算法等搜索最优或次优路径的方法，以及他们的改进方法。但实际应用中，由于A星算法等单元分解法需要频繁的对内存进行读写，在地图较大时算法实时性差，常用于离线路径规划。

人工势场法是另一种常用路径规划方法，它借物理学中的场论来解决机器人学中的路径规划问题。该类方法最早由Khatib于1986年提出，现在已经被广泛应用于移动机器人的路径规划与实时避障。但传统人工势场法主要针对静态情况，且规划的路径可能出现局部极小或震荡的情形导致路径不适用于机器人运动或者规划失败。有很多学者针对以上问题进行了针对性的改进，如添加新的代价函数，如速度、惯性和能量等等，但仍不能完全解决问题。

文献《基于栅格-几何混合地图的移动机器人分层路径规划》针对机器人规划问题，建立了栅格-几何混合地图，在栅格地图下进行了改进A星算法的上层路径规划，在几何地图下进行基于惯性人工势场法的底层路径规划，通过惯性人工势场法对改进A星算法生成的轨迹进行修正。该方法针对静态环境建立的混合地图在实际机器人控制系统中实现较为复杂，且每次规划过程中改进A星算法与惯性人工势场二者都要运行一次后并进行路径融合，无法保证移动机器人的实时运行，不能应对动态变化场景。

已授权专利《一种基于障碍物运动预测的移动机器人避障方法》阐述了一种基于速度的人工势场法的机器人避障方法，并使用IMM方法进行障碍物运动预测。该方法重点研究了移动机器人避障，但没有解决用人工势场法进行路径规划带来的局部极小或震荡问题，且该方法并不是全局路径的规划方法，不适用于移动机器人实时路径规划。

发明内容

本发明的主要目的是利用栅格地图便于计算机建立、储存和分析的优点，使用基于速度的人工势场法进行外层路径规划，保留势场法实时性和简洁性，当势场法的规划路径发生震荡或遇到局部极小问题时，切换到A星算法进行内层路径规划，并及时切回，最大限度避免移动机器人路径规划失败，从而保证机器人的实时、安全和稳定运行。

一种基于栅格地图的移动机器人实时分层路径规划方法，该方法包括以下步骤：

步骤一：明确机器人路径规划的目标，并进行初始化：机器人搭载的计算机通过采集设备建立并实时更新包含外部环境信息的栅格地图，确定规划的起点和目标点，应用基于速度的人工势场法和A星算法进行分层路径规划，保证机器人实时、安全和稳定地运行到目标点；

步骤二：进行基于速度的人工势场法外层路径规划：计算出基于速度的人工势场下机器人所受的合力矢量，该合力矢量在机器人规划的路径不处于震荡和局部极小时，直接指导机器人运动，并储存在该矢量指导下生成规划路径。合力矢量方程为：

；

其中为引力增益系数；表示一个矢量，矢量的大小为当前位置与目标点位置之间的欧几里得距离，矢量的方向为与两个位置连线上从机器人位置指向目标点的位置；其中为斥力增益函数；为正常数，表示障碍物对移动机器人产生影响的最大距离；表示一个矢量，矢量大小为当前位置点与障碍物点的欧几里得距离，矢量方向为障碍物位置指向机器人位置；其中为可调的速度常量，为机器人与障碍物的相对运动速度，为与的夹角，为机器人所在点指向障碍物点的向量；

步骤三：进行基于A星算法的内层路径规划：在机器人规划的路径即将处于震荡和局部极小时，路径规划方法切换到A星算法，以当前点为起点，选取临时的目标点，规划出到临时目标点的路径，转到步骤四；如果临时目标点同时是整个规划周期的目标点，则到达目标点后，转到步骤五；

步骤四：内层和外层的规划路径合并后，从A星算法的临时目标点开始进行基于速度的人工势场法外层路径规划，即回到步骤二；

步骤五：规划周期结束，将所有路径合并后，命令机器人按照规划路径运行，同时如果需要重新开始规划周期就回到步骤一，重新启动路径规划流程，直到机器人停止运动。

附图说明

图1为一种只用基于速度的人工势场法由于局部极小而规划失败的情况。

图2为基于栅格地图的移动机器人实时分层路径规划法的概要流程图。

图3外层基于速度的人工势场法路径规划流程图。

图4内层基于的A星路径规划流程图。

图5为机器人在基于速度的人工势场法下受力分析图。

图6为边长10个单位长的栅格地图内A星路径规划方法的路径。

图7为边长10个单位长的栅格地图内分层路径规划方法的路径。

图8为边长25个单位长的栅格地图内A星路径规划方法的路径。

图9为边长25个单位长的栅格地图内分层路径规划方法的路径。

图10为边长50个单位长的栅格地图内A星路径规划方法的路径。

图11为边长50个单位长的栅格地图内分层路径规划方法的路径。

图12以上三种情况分层路径规划方法与A星规划方法的规划时间对比图。

其中，在方形网状栅格地图中起始点在左下角用圆圈表示，目标点在右上角用五角星表示，障碍物所在栅格用黑色填充，障碍物膨胀栅格用斑点填充，最终规划得到的路径栅格用斜网格线填充。

具体实施方式

具体实施方式一：明确机器人路径规划的目标，并进行初始化：移动机器人搭载的计算机通过采集设备建立并实时更新包含外部环境信息的栅格地图，确定规划的起点和目标点，应用基于速度的人工势场法和A星算法进行分层路径规划，保证机器人实时、安全和稳定地运行到目标点。

具体实施方式二：本实施是对一种基于栅格地图的移动机器人实时分层路径规划方法作进一步说明，当栅格地图规模等于时，A*路径规划方法的计算复杂度可达。如果将该环境模型中的栅格粒度变为原来的二分之一，则栅格规模将变为，而计算量则会增至。因此尽管A*可以保证找到一条最优路径，但是并不能保证移动机器人实时运动。

尽管基于速度的人工势场法由于其算法的简洁性更适用于实时性的路径规划，但局部极小现象和可能出现的震荡都会引发机器人运行不稳定。由于势场的本质，如果参数选择不当，机器人有忽视物理上的接触，仅视作场的相互作用，机器人有过于接近障碍并引发危险的可能，如图1为一种只用基于速度的人工势场法或常规人工势场法由于局部极小而规划失败的情况。

因此本发明设计了实时切换的分层轨迹规划策略，图2为其概要流程说明。在地图信息栅格化完成之后，会设置好当前规划起点与目标点，进行外层基于速度的人工势场运动规划，图3为外层路径规划流程，当判断到规划路径机器人将处于局部极小、震荡等状况的时候，切换到内层A星算法路径规划，图4为外层路径规划流程。

具体实施方式三：本实施是对一种基于栅格地图的移动机器人实时分层路径规划方法作进一步说明，本实施方式中，在外层基于速度的人工市场法路径规划过程中，移动机器人受到的引力势能与目标点的欧几里得距离呈反比关系，当引力势能为零时，移动机器人到达目标点。引力势能函数表示为：

；

其中为引力增益系数；表示一个矢量，矢量的大小为当前位置与目标点位置之间的欧几里得距离，矢量的方向为与两个位置连线上从机器人位置指向目标点的位置。

则由引力势场产生的引力为：

；

引力方向与的方向相同。

移动机器人收到的斥力势能与引力势能与目标点的欧几里得距离也呈反比关系。当斥力势能为零时，机器人已经脱离障碍物的影响范围，本发明设定为。斥力势能函数表示为：

其中为斥力增益函数；为正常数，表示障碍物对移动机器人产生影响的最大距离；表示一个矢量，矢量大小为当前位置点与障碍物点的欧几里得距离，矢量方向为障碍物位置指向机器人位置。

相应的斥力方程为：

。

除了可能遇到的震荡、局部极小问题，传统人工势场适用于静态环境的路径规划。而在动态和不确定性的环境中，机器人携带空间测绘设备由于自身所处位置受限，无法预先知道下一周规划周期尚未测绘到障碍，从而降低机器人避障的能力。因此本发明采用了基于速度的人工势场法，提高机器人在在实时环境中对动态环境变化适应性。

动态变化主要来自于空间障碍，需要对斥力势能进行改进。

机器人与障碍物之间的相对速度势场：

；

其中为可调的速度常量，为机器人与障碍物的相对运动速度，为与的夹角，为机器人所在点指向障碍物点的向量，当即障碍物不向机器人运动，或机器人不在障碍影响范围内时，则速度斥力势能可忽略。

速度斥力为：

。

加入速度因素机器人收到的总斥力势能为：

。

相应的总斥力为：

。

根据上述的引力势能、斥力势能，可以得到机器人在某一位置所受到的全局势能。全局势能函数为：

。

在考虑速度因素后，机器人最终所受的合力为：

。

图5为对移动机器人的受力分析。

具体实施方式四：本实施是对一种基于栅格地图的移动机器人实时分层路径规划方法作进一步说明，本实施方式中，内层规划所采用的A星算法路径规划方法启发式搜索算法，它根据局部信息不断地逼近目标位置，最终得到有效路径，其评估公式如公式：

；

其中表示从初始结点到任意结点的代价，表示从任意结点到目标点的启发式评估代价。

恰当的启发函数会提高A星算法效率，且路径更优。本发明中的A星算法认为机器人可以沿着栅格的对角线移动，考虑到机器人对角线移动的需要转弯等操作，那么假设一个位置水平或垂直移动到邻近位置的最小代价，对角线移动的代价设为。启发函数中移动步数表示为：

；

其中为沿着斜线可以移动的步数，其中代表当前节点；为曼哈顿距离，即X，Y方向上所有沿直线移动的步数之和。

最终的启发函数为：

；

最终的评估函数为

。

具体实施方式四：本实施方式是对一种基于栅格地图的移动机器人实时分层路径规划方法作进一步说明，在进行一个完整的路径规划周期结束之后，机器人会按照规划好的路径进行运动，同时更新外部环境的栅格地图，直到新的路径规划周期结束后机器人获得新的规划路径。其每个规划周期内完整的规划流程如下。

1、地图预处理过程：

首先进行当前规划周期中的地图数据更新，建立存储空间所有栅格运行代价的数组，设定障碍点的运行代价为4，非障碍点运行代价为1。然后进行膨胀运算，设定包围障碍物的栅格区域运行代价也为4。地图预处理之后，建立用于存储当前周期全局规划路径的数组，开始进行路径规划。

2、外层路径规划流程：

外层规划流程选用实时性好的基于速度的人工势场法：

Step 1 设定引力增益系数、斥力增益系数，障碍物影响半径和单步步长，这四个参数要根据实际情况变化；

Step 2 判断当前位置是否到达当前周期内规划路径的目标点，如果到达就跳转到Step 9，如果未到达则执行下一步；

Step 3 判断当前规划路径是否有局部极小、震荡的问题，如果没有则继续执行下一步，否则切换到内层A星算法路径规划；

Step 4 计算机器人当前位置与目标之间的角度，进而计算出引力对机器人在X，Y方向上的分力和；

Step 5 计算机器人当前位置与各障碍点之间的每个夹角，计算出斥力对机器人在X，Y方向上的分力和；

Step 6 计算机器人当前与各障碍点相对速度，计算速度斥力对机器人在X，Y方向上的分力，；

Step 7 计算机器人在X，Y方向上受到的合力和，并计算合力与X轴的夹角；

Step 8 根据步长、和当前机器人位置，计算并取整得出下一步的位置，将坐标存储到，并回到Step2；

Step 9 输出最终全局路径，结束当前规划周期。

3、内层路径规划流程：

本发明的内层A星算法中最为重要的部分是对临时目标点的选取。本发明假设在规划周期开始时，移动机器人通过采集可用于地图构建的传感器的信息，如激光雷达、摄像头等，可以实时更新所处环境的栅格化地图，使机器人有足够的依据进行临时目标点的选取：

Step 1 建立数组和，保存代价待考察的节点，保存已经考察过其代价的节点，建立数组保存节点的代价，建立保存局部规划路径，将切换到内层规划时的规划位置存入，作为内层A星规划的起点；

Step 2 建立局部栅格地图并选取临时目标点。在当前的规划周期内，在当前位置与目标所连的直线上选取一临时目标点，当前位置与局部目标点构成了新的正方形局部栅格地图，其边长根据障碍物分布密度进行选取，且满足以下公式：

；

判断全局目标是否处于局部地图内，如果存在则将全局目标点坐标设为临时目标点，如果选取的临时目标点不可达，如选取在障碍栅格或膨胀栅格上，需要同时将临时目标点坐标和增加一个步长，再判断临时目标点是否可达，如果不可达则继续增加，直到其可达或者全局目标点被包络进来；

Step 3 根据当前起始点和临时目标点，进行A星路径规划，并得到内层规划路径；

Step 4 将内部路径添加到全局路径，结束内层A星路径规划，切换回到外层规划流程。

具体实施方式五：本实施方式是对一种基于栅格地图的移动机器人实时分层路径规划方法作进一步说明，本实施方式中为了测试本方法的实时性，将其与只使用A星路径规划方法的情况进行了对比。在障碍物为5个且对移动机器人产生影响的时候，在尺寸为10，25和50个单位长的地图中进行了MATLAB仿真实验，其中栅格地图尺寸为10个单位长时不进行膨胀运算，分层规划有关参数为=0.5，=200，=3单位长，=1单位长， =3单位长，=1单位长/s，图6~图11为以上路径规划的结果图，图12为以上路径规划时间统计结果对比图。

Claims

1.一种基于栅格地图的移动机器人实时分层路径规划方法，其特征在于，该方法包括以下步骤：

步骤一：地图预处理过程：开始新的规划周期，进行当前规划周期中的栅格地图数据更新，给定所有栅格的运行代价，进行膨胀运算，设定当前规划周期的起点和目标点；

步骤二：基于速度的人工势场法外层路径规划过程：设定基于速度的人工势场法参数，根据引力势能、斥力势能所产生合力矢量的方程计算移动机器人所受到的合力矢量，合力矢量方程为：

；

该合力矢量直接指导移动机器人运行，并对所规划路径的状态进行循环检测，当规划路径即将处于局部极小和震荡状态时且未达到全局目标点时，转到步骤三；当机器人到达目标点时，转到步骤四；

步骤三：基于A星算法的内层路径规划过程：暂停外层基于速度的人工势场法路径规划过程，选取临时目标点并切换到A星算法进行机器人路径规划；当规划路径不再处于局部极小和震荡状态且未到达临时目标点时，转到步骤二；当机器人到达全局目标点时，转到步骤四；

步骤四：路径规划结果输出：合并内外层规划路径，将此规划周期生产的路径保存并让机器人按照规划路径移动；如果不满足停止路径规划条件，回到步骤一，否则停止路径规划，机器人也停止运动。

2.根据权利要求1所述的一种基于栅格地图的移动机器人实时分层路径规划方法，其特征在于步骤一所述的停止路径规划条件为机器人到达了全局的目标或人为干预、程序要求或机器人处于危险状态，使得机器人停止运动；

步骤一所述的栅格地图为正方形，由许多单元栅格构成，可分为障碍栅格，非障碍栅格和膨胀栅格；其中障碍栅格的运行代价为较大值，非障碍栅格运行代价为较低值，膨胀运算使围绕障碍物一圈的非障碍栅格为膨胀栅格，其运行代价与障碍栅格相同，但不对机器人产生势场。

3.根据权利要求1所述的一种基于栅格地图的移动机器人实时分层路径规划方法，其特征在于步骤二所述的引力势能方程为：

；

其中为引力增益系数；表示一个矢量，矢量的大小为当前位置与目标点位置之间的欧几里得距离，矢量的方向为与两个位置连线上从机器人位置指向目标点的位置；

引力势能产生的引力方程为：

；

步骤二所述的总斥力势能由障碍物产生的斥力势能和机器人与障碍物相对速度产生的斥力势能共同构成，总斥力势能中障碍物产生的斥力势能方程为：

；

其中为斥力增益函数；为正常数，表示障碍物对移动机器人产生影响的最大距离；表示一个矢量，矢量大小为当前位置点与障碍物点的欧几里得距离，矢量方向为障碍物位置指向机器人位置；

总斥力势能中机器人与障碍物相对速度产生的斥力势能方程为：

；

其中为可调的速度常量，为机器人与障碍物的相对运动速度，为与的夹角，为机器人所在点指向障碍物点的向量，当即障碍物不向机器人运动，或机器人不在障碍影响范围内时，则速度斥力势能可忽略；

总斥力势能方程为：

；

总斥力势能产生的总斥力方程为：

；

除了可能遇到的路径局部极小和震荡问题，传统人工势场适用于静态环境的路径规划；而在动态和不确定性的环境中，机器人携带空间测绘设备由于自身所处位置受限，无法预先知道下一周规划周期尚未测绘到障碍，从而降低了机器人动态避障的能力；因此本文采用了基于速度的人工势场法，提高机器人在在实时环境中对动态环境变化的适应性。

4.根据权利要求1所述的一种基于栅格地图的移动机器人实时分层路径规划方法，步骤三中临时目标点选取的方法为：

在当前的规划周期内，在切换到A星规划时移动机器人的当前位置与目标所连的直线上选取临时目标点，当前位置与临时目标点包含在局部新的正方形栅格地图内，其边长根据障碍物分布密度进行选取，且边长满足以下公式：

；

判断临时目标点是否处于局部地图内，如果存在则将全局目标点坐标设为临时目标点，如果选取的临时目标点不可达，在则同时将临时目标点坐标和增加一个步长，判断临时目标点是否可达，如果不可达则继续增加一个步长并重新判断，直到可达或者全局目标点被包络进来，如果全局目标被包络进来，则直接将其设为临时目标点，随后按照A星算法对机器人进行路径规划。