栅格地图环境下机器人速度势实时路径规划

杨凌耀，张爱华，张 洁，宋季强

上海工程技术大学 机械与汽车工程学院，上海201620

移动机器人依托高效智能的数字化管理和规划可以方便地完成复杂环境的搬运、爆发式增长的物流快件分拣等任务。在智能制造场景中，移动机器人的运动路径上通常会有各种形状的障碍物存在，移动机器人想要到达目标点必须躲避环境中的障碍物，这就要求机器人在向目标点运动的同时即时地产生避障行为。随着智能制造业的发展，现代工业对移动机器人的智能化水平提出了更高的要求，尤其是避障路径规划问题，已成为近年来国内外研究的热点[1]。

路径规划通常可以划分为环境建模和路径搜索两部分。合理的环境建模方法可以极大提高路径搜索效率和避障精准度[2]。常用的环境建模方法有栅格法、可视图法、维诺图法等。其中栅格法因其强大的处理不规则障碍物的能力成为了当前较为常用的建模方法，主要思想是将环境进行离散分解，用一定大小栅格来表示基本单元[3]。文献[4]在设计路径规划算法时就先利用栅格对机器人工作环境进行分区预处理，从而提前量化环境的复杂度，提高了算法搜索效率、精度和灵活性。

路径搜索已有多种算法，包括人工势场法[5]、蚁群算法[6]、神经网络避障[7]、遗传算法[8]等，其中人工势场法是较为基础的算法。人工势场法最初是由Khatib在1986年提出的[9]，人工势场法的基本思想是根据“趋利避害”的原则，移动机器人在一个虚拟的力场中运动，目标点被引力和斥力势场包围，产生的合力使机器人朝向目标点运动，其优点是系统的路径生成与控制直接与环境实现了闭环，从而大大加强了系统的适应性与避障性能。文献[10]针对传统人工势场在复杂的障碍物下难以规划出完整路径问题，提出了一种改进的人工势场，改进了斥力生成和计算以及添加了识别周围环境的机制，有效地解决了传统算法容易出现路径规划中断的情况。文献[11]将势场法与蚁群算法相结合，解决了原有算法收敛速度过快、易出现局部最优解、路径不平滑等问题。但是上述的人工势场法是针对静态环境提出来的，在实际情况下机器人往往需要实时避开动态障碍物或实时跟踪动态目标点。文献[12]在传统的人工势场法加入障碍物的速度和加速度矢量修正势场角度方向，相当于把原来的静态势场变为动态势场，满足了某些特定条件下的动态避障需求。文献[13]在文献[12]的基础上将机器人自身的速度和加速度也考虑进来，根据当前的机器人、障碍物、目标物的位置对下一步的动作做出预判，避免由于不必要的增加修正力而使得机器人的规避路径无谓的增加。文献[14]提出将速度障碍思想和人工势场理论相结合的船舶避碰决策方法，保证船舶在障碍物、来船和可航水域边界组成的复杂动态环境下的避障性能。应用人工势场法规划出来的路径是比较平滑和安全的，但是存在障碍物附近不可达和局部极小点等问题[15]，并且在势场对机器人的合力计算较为繁琐，计算效率较低。文献[16]提出了一种新的排斥势函数，当检测到局部极小值时，通过激活虚拟逃逸力来脱离局部极小点。文献[17]在人工势场算法的基础上，设计新的势能函数以减少运动过程中计算势场对智能体作用力合力的繁琐步骤，提高了算法效率。

受到前人的研究工作启发，提出一种基于栅格地图的移动机器人速度势实时避障路径规划方法，将工作环境进行二值化的栅格划分，再利用栅格搜索的方法计算移动机器人和障碍物的实时距离，通过设计速度势函数产生与实时距离相关的速度增量用来驱动移动机器人运动，既省去了传统势场法繁琐的合力计算过程，又达到实时路径规划的目的。同时在零势点处设置最小速度增量，避免局部极小，并在目标点附近设立距离阈值，避免机器人运动到目标点附近可能出现的速度增量趋于无穷的情况。最后，仿真模拟移动机器人在复杂多变的障碍物环境下的实时避障规划，验证算法的有效性。

1 问题描述

在移动机器人的现实场景中存在不同形状的障碍物，假设通过校准摄像机摄取工作环境的图像，使用多个摄影机捕捉整个工作区的不同部分，并通过融合以创建整体地图，通过对障碍物形状进行简化，得到工作场景地图的示意图如图1所示。图1中不同形状黑色物块表示障碍物，它们随机地分布在机器人工作的环境中，移动机器人需要在此环境中完成两点之间的路径规划，假设机器人的中心点为f，机器人运动的目标点为g，机器人的速度为v；在700 dm×700 dm的工作地图中运动，移动机器人需要根据自身的当前位置即时地躲避随机分布在工作场景中的障碍物，最终到达目标点完成工作任务。因为机器人驶出工作区域会产生无法预测的结果，故工作区域的边界也视为障碍物处理。

图1 移动机器人工作场景示意图Fig.1 Sketch map of mobile robot working scene

选取XOY为大地坐标系，设定移动机器人t时刻位姿为f(t)=[x(t)y(t)θ(t)]T，其中[x(t)y(t)]T∈R2表示中心点f在XOY中的实时位置，θ(t)表示机器人前进方向与x轴之间的实时夹角，t时刻到t+1时刻的时间微分为dt。

假设移动机器人驱动轮与地面不滑移，其运动学方程可表示如下：

2 栅格地图与栅格搜索

为充分表达整个环境的分布情况，以栅格法构建地图，以机器人和障碍物的实体长度尺寸作栅格的基本单元，根据障碍物的实际位置来确定栅格的状态，有障碍物的栅格被赋值0，无障碍物的栅格被赋值1，如图2所示。栅格越密能够表示的工作环境越详细，对障碍物外形特点描绘的就越逼真，路径规划精准度就越高。从机器人f出发向四周不同的方向进行栅格搜索，如搜索到1，表示是自由区域，继续沿着该方向搜索；如搜索到0，表示到达障碍物边缘，搜索停止，计算栅格数，得到该搜索方向上机器人距离障碍物的距离。

图2 栅格地图构建与二值化Fig.2 Construction and binarization of raster map

3 速度势避障算法

考虑到距离障碍物越近的区域机器人越难到达，距离目标点越近的区域机器人越容易到达，障碍物对机器人有排斥作用，可产生负的速度增量aw(t)，目标点对障碍物有吸引作用，可产生正的速度增量av(t)，如图3所示。进而合成移动机器人的实时速度增量函数a͂(t)：

图3 中箭头指向表示势向量的方向，由公式（2）可以看到，此处通过速度势分布定义速度增量函数，这样既可达到速度驱动的目的，又能实现避障行为。

图3 速度势场分布示意图Fig.3 Schematic diagram of velocity potential field distribution

进一步，为体现目标对移动机器人的吸引作用，机器人越靠近目标点，目标点对机器人的吸引越大，越远离目标点，目标点对机器人的吸引越小，定义目标点对机器人的速度增量矩阵a͂v(t)：

为防止距离目标点过远导致吸引势过小，引入最小吸引势速度增量矩阵，方向与相同；为避免机器人接近目标点时，速度增量趋于无穷，引入目标点附近的距离阈值dgm(t)，即当dg(t)＜dgm(t)时，dg(t)=dgm(t)。dgm(t)的取值与上一时刻吸引势速度增量的单位时间增幅有关，当单位时间增幅急剧增大就表示机器人进入目标点附近的小邻域内。

式中，ε为距离阈值增益，‖‖2为向量的二范数，ρ为距离阈值半径，从而得到新目标点对机器人吸引的速度增量矩阵为：

接着，根据障碍物对机器人有排斥作用的思想，机器人越靠近障碍物，障碍物对机器人的排斥越大，越远离障碍物，障碍物对机器人的排斥越小，定义障碍物对机器人排斥的速度增量矩阵：

式中，j为栅格搜索的方向的个数，β为排斥势的程度参数，ηj为各排斥势的影响比例因子，与障碍物的形状和大小有关，awx(t)和awy(t)分别为aw(t)在x方向和y方向上的速度增量，θbj(t)为第j个方向的测量角度，dbj(t)为第j个方向的测量距离。dm为障碍物外围的最小安全距离，σ(t)∝v(t)表示机器人对障碍物的敏感度，他和机器人的实时运动速度大小成正比，当最小安全距离dm一定，机器人的运动速度增大，机器人对障碍物的敏感度会变高，障碍物外围的实时安全距离σ(t)dm会随之增大，最大程度避免机器人和障碍物发生碰撞。

此外，为防止速度增量累加产生零势点，使机器人陷入局部极小点，引入最小合成速度增量a͂(t)min，增量方向与零势点处相同，当机器人陷入局部极小点时，最小合成速度增量会帮助机器人离开零势点区域，得到新的速度增量矩阵：

δ为最小实时速度增量的二范数，用于界定零势点范围。

4 基于栅格法的实时避障路径规划

4.1 实时轨迹生成

通过对式（1）中的运动学模型得出的速度矩阵施加实时速度增量a(t)，即可得到t+1时刻机器人的速度矩阵为：

通过速度矩阵，可以得到移动机器人穿越障碍物的实时轨迹曲线：

机器人下一时刻的方位角为：

4.2 实时路径规划流程

栅格搜索方法和速度势增量函数建立之后，根据机器人的实时位置的调整在工作环境中的位姿，从而实时规划一条可靠的通往目标点的路径，实时路径规划流程如图4所示，路径规划步骤如下：

步骤1初始化，设定计算机和机器人初始参数。

步骤2对工作环境摄取并栅格化，再对栅格做二值化处理。

步骤3判断机器人是否到达目标，如到达路径规划结束，如未到达，机器人开始路径规划。

步骤4机器人向四周不同方向进行栅格搜索，测得在不同方向上机器人与障碍物的实时距离dbj(t)，并根据自身坐标位置计算与目标点的距离dg(t)。

步骤5判断dg(t)是否达到距离阈值dm(t)，如达到，dg(t)=dgm(t)。

步骤6根据dg(t)、dbj(t)计算得出吸引势和排斥势速度增量

步骤8 av(t)和aw(t)累加合成机器人在t时刻速度增量

步骤10 a(t)对机器人作速度驱动，得到机器人下一时刻的目标点。

5 仿真与分析

为验证避障方法的有效性，本章针对不同障碍物环境以及不同起始点和目标点的避障规划仿真。通过栅格搜索测量机器人周围所有角度的障碍物与机器人的距离来计算合成的速度增量a(t)。在仿真中，挑选了5个特定角度的距离来计算排斥势，它们是向前、左侧、右侧、向前左对角线和向前右对角线。

仿真环境根据坐标环境而定，在700 dm×700 dm的工作区域内，坐标系中的每个整数坐标对应栅格地图中的一个网格单元，每个单元根据障碍物的位置被赋予二值化信息，有障碍物的区域设置为0，无障碍物的区域设置为1。设定吸引势的影响比例因子λ为400 000和各排斥势的影响比例因子ηj为300 000，排斥势的影响程度为2和吸引势的影响程度为3。设定最小吸引势速度增量，最小速度增量机器人初始转角为，机器人初始速度为0。

设定不同的起点和终点，检验避障算法对于地图上任意可通过点的有效性，并得到路径在坐标系中的曲线方程，仿真如下：

（1）起点[20，20]，终点[650，20]的情况

（2）起点[20，20]，终点[20，650]的情况

（3）起点[20，20]，终点[650，650]的情况

图5 起点[20，20]，终点[650，20]时避障路径及曲线特征图。图6为起点[20，20]，终点[20，650]时避障路径及曲线特征图。图7为起点[20，20]，终点[650，650]时避障路径及曲线特征图。在图5（a）、图6（a）和图7（a）仿真图中，路径中的小方块表示机器人的实际形状，黑色的物块表示障碍物的形状和位置。机器人从自身的中心点出发向指定的各个角度进行栅格搜索，求得各个角度上机器人和障碍物之间的距离，再通过增量函数求得实时的速度增量，得出机器人在下一时刻期望位置。不同时刻的位置被记录在地图中，形成了机器人的避障路径。从仿真结果可以看出，尽管障碍物的形状较为复杂多样，但在对地图中任意位置的目标点的避障运动过程中，机器人总是实时规划出了安全路径。通过设置不同的终点以引导机器人穿越不同形状的障碍物，来验证处理不同类型实时路径规划问题的能力。

图5 起点[20，20]，终点[650，20]时避障路径及曲线特征Fig.5 Obstacle avoidance path and curve characteristics at starting point[20，20]and ending point[650，20]

图6 起点[20，20]，终点[20，650]时避障路径及曲线特征Fig.6 Obstacle avoidance path and curve characteristics at starting point[20，20]and ending point[20，650]

图7 起点[20，20]，终点[650，650]时避障路径及曲线特征Fig.7 Obstacle avoidance path and curve characteristics at starting point[20，20]and ending point[650，650]

图5 （b）、图6（b）和图7（b）为不同终点情况下机器人在避障过程中机器人中心在笛卡尔坐标系中的曲线特征，可以看出实时路径规划出的曲线均较为平滑，这非常有利于后续轨迹跟踪控制器的设计。图5（c）、图6（c）和图7（c）为三种情况下机器人的期望速度增量曲线变化图，可以看出，避障开始阶段机器人因为离目标点过远，排斥势的影响远大于吸引势，故合成速度增量数值非常大。随着机器人向目标点前进，吸引势的影响逐渐增大，合成速度增量数值在小范围内波动。但是合成速度增量始终未趋于零，说明机器人在运动过程中未陷入局部极小陷阱。结果表明所提出的充分考虑了避障过程中机器人和障碍物外形特点，具有更好的实时性和容错性。

6 结论

针对未知且复杂多变的障碍物环境，提出了一种基于栅格地图的速度势实时避障路径规划方法，该方法利用栅格划分对移动机器人工作环境进行二值化处理，再通过栅格搜索计算出不同方向上机器人与障碍物之间的距离。进而，基于障碍物对移动机器人有排斥作用以及目标点对机器人有吸引作用的思想，并将障碍物的形状和最小安全距离等因素考虑进来，在机器人运动学的基础上，引入速度势产生与实时距离相关的速度增量对移动机器人进行实时的速度驱动，减少势场计算时的工作量和提高实时避障的效率。最后通过仿真软件进行验证，结果表明：

（1）所提的算法在包含随机分布的形状多样的障碍物的工作环境中，总是能实时规划出一条平滑、安全的无碰撞路径。

（2）所提的方法在针对工作地图中安全区域的任意点都普遍具有可达性。

（3）移动机器人在运动过程中，实时的速度增量大小仅在小范围波动但是没有趋于零的时刻，说明该避障算法对机器人的运动性能要求较低且不存在局部极小的问题。

但是，在栅格环境下需要根据障碍物的密度来的决定栅格的密度大小，而栅格密度越小，障碍物的轮廓描述的越详细，避障精确度越高，但计算的代价也会越大，因此如何找到障碍物的密度来决定栅格密度的关系是下一步研究方向。