基于改进人工势场法的移动机器人局部路径规划

张国胜，李彩虹，张耀玉，李永迪，周瑞红

(山东理工大学 计算机科学与技术学院，山东 淄博 255049)

路径规划技术作为实现移动机器人自主导航的核心技术受到广泛关注,其目的是使移动机器人按照某些特定的评价标准规划出一条从起点到目标点的最优或次优的避障路径[1]。根据对工作环境信息的理解层次,路径规划可分为全局路径规划和局部路径规划[2]。全局路径规划应用在工作环境已知的场景下,是一种离线规划方法,实时性和灵活性较差,可以规划出全局最优或次优的路径。局部路径规划应用在工作环境未知或部分已知的场景下,移动机器人根据局部环境信息进行在线路径规划,具有较高的实时性和灵活性;由于缺少全局环境信息,传感器的探测范围有限,所以规划路径一般是局部最优。常见的路径规划方法有A*算法[3]、蚁群算法[4]、动态窗口法[5-6]、人工势场法[7-8]和模糊控制法[9]等。其中,人工势场法由于结构简单、计算量小、实时性好且路径较为平滑等优点得到了广泛应用,但是存在局部极小值以及目标不可达等问题。

针对传统人工势场法存在的局部极小值和目标不可达等问题,国内外学者做了大量的研究工作。林洁等[10]提出沿边走的详细策略来解决局部极小点,但在陷入型障碍物以及复杂障碍物环境下会增加路径长度;黄开启等[11]设置模糊控制模块为移动机器人提供辅助作用力以摆脱局部极小值点,但在虚拟力引导过程中路径容易出现连续振荡;Fan等[12]将人工势场法与正六边形引导方法相结合,通过绕行障碍物克服局部极小值,但在复杂障碍物环境中无疑加大了计算时间和耗能;Zhou等[13]提出在人工势场法中构建基于障碍物的切向量作为避障过程的虚拟力,使移动机器人逃脱局部极小值点,但未有效解决引力过大和障碍物环境误判问题;赵炳巍等[14]通过模拟退火算法,在移动机器人关于障碍物右对称点区域设置随机目标以摆脱局部极小值陷阱,但未充分考虑当前障碍物信息,算法效率无法保证;罗强等[15]采用切线法以及搜索法得到最优逃逸力,移动机器人在新的合力作用下摆脱局部极小值点,但应用场景简单,面对复杂障碍物环境时无法有效逃出陷阱区域。

针对上述算法的不足,本文在已改进势场函数的基础上,设计自适应速度调节机制,提高路径规划效率。针对局部极小值问题,分别提出了APF-v1算法和APF-v2算法。通过对当前障碍物环境判断,两种算法采取不同的虚拟目标点设置策略打破受力平衡状态,引导移动机器人摆脱局部极小值陷阱。最后利用ROS机器人操作系统对APF-v1和APF-v2算法进行可行性的验证。

人工势场法是由Khatib于1986年提出的一种虚拟力法[16],其基本思想是在移动机器人的工作环境中构造一个虚拟力场,目标点产生引力势场,障碍物产生斥力势场,移动机器人在引力场和斥力场的共同作用下向目标点移动。

人工势场的势场函数U(X)定义:

U(X)=Uatt(X)+Urep(X),

(1)

式中:Uatt(X)为目标点产生的引力势场;Urep(X)为障碍物产生的斥力势场;X为移动机器人的当前位置。

人工势场中机器人所受的力F(X):

F(X)=Fatt(X)+Frep(X),

(2)

式中:Fatt(X)为目标点对移动机器人的引力;Frep(X)为障碍物对移动机器人的斥力。

1.1 引力场

引力场是全局势场,引力势能与移动机器人和目标点之间距离成正比,距离越大,移动机器人所受的引力势能越大。引力势场函数定义:

(3)

式中:ε为引力增益系数;Xg为目标点的位置;ρ(X,Xg)为移动机器人到目标点的距离。

目标点对移动机器人的引力为引力势场函数的负梯度:

Fatt(X)=-∇Uatt(X)=-ε(X-Xg)。

(4)

1.2 斥力场

斥力场是局部势场,在障碍物影响范围内,斥力势能与移动机器人和障碍物之间的距离成反比,距离越大,所受的斥力势能越小,在障碍物影响范围外,移动机器人所受的斥力势能为零。斥力势场函数定义:

0,ρ(X,Xobs)>ρ0,

(5)

式中:η为斥力增益系数;Xobs为障碍物的位置;ρ(X,Xobs)为移动机器人到障碍物的距离;ρ0为障碍物的影响范围。

障碍物对移动机器人的斥力为斥力势场函数的负梯度:

Frep(X)=

0,ρ(X,Xobs)>ρ0。

(6)

1.3 合力

人工势场为引力场和斥力场的叠加,移动机器人所受的合力为引力与斥力的矢量和,合力的方向即为移动机器人运动的方向。移动机器人受力分析如图1所示。

图1 移动机器人受力分析

移动机器人在合力的作用下向目标点移动,但是存在以下问题:

1) 引力过大问题。当移动机器人与目标点之间的距离过大时,其所受的引力将远大于斥力,在移动过程中可能会出现碰撞障碍物的情况。

2) 目标不可达问题。当目标点位于障碍物附近时,随着移动机器人不断向目标点靠近,其所受的引力逐渐减小,斥力逐渐增大,斥力过大,引力相对较小,导致移动机器在某一区域徘徊或静止。

3) 局部极小值问题。当移动机器人运动到某一非目标点位置时,其所受合力为零或引力与斥力方向相反而陷入局部极小值陷阱,移动机器人无法确定下一步的运动方向,导致其在局部极小值点附近反复震荡或静止。

2.1 改进的引力势场

针对移动机器人距离目标点过远导致的引力过大问题,本文对引力势场函数进行修正,设置引力作用阈值将其分段,当距离大于此阈值时,采用修正的引力势场函数,避免引力过大问题。修正的引力势场函数:

Uatt(X)=

(7)

式中dg为引力作用阈值。

目标点对移动机器人的引力为引力势场函数的负梯度:

Fatt(X)=

(8)

2.2 改进的斥力势场

针对目标不可达问题,本文引入一种改进的斥力势场函数,相较于传统的斥力势场函数,改进的斥力势场函数加入了目标点和移动机器人之间的距离,保证仅目标点处的势场为全局最小。改进的斥力势场函数:

Urep(X)=

(9)

斥力为改进的斥力势场函数的负梯度:

Frep(X)=

(10)

式中Frep1(X)和Frep2(X)分别为斥力Frep(X)在两个不同方向上的分力。矢量Frep1(X)的方向由障碍物指向移动机器人,为斥力分量;矢量Frep2(X)的方向由移动机器人指向目标点,为引力分量。

(11)

(12)

改进势场函数后移动机器人受力分析如图2所示。

图2 改进势场函数后移动机器人受力分析

2.3 自适应速度调节

传统人工势场法进行路径规划任务时线速度采用固定值。结合实际工作经验,根据移动机器人工作环境的复杂程度,提出了一种自适应速度调节机制,以降低发生碰撞的概率。

移动机器人线速度v的取值取决于当前所处的环境。移动机器人通过自身搭载的传感器获取周围环境信息,通过传感器返回的数据得到周围障碍物的数量obs_count以及移动机器人与障碍物之间的距离dis_r_o,由此判断当前工作环境的复杂程度。当移动机器人处于简单环境时,可适当增大线速度,提高路径规划效率;当移动机器人处于复杂环境时,适当减小线速度,降低发生碰撞的概率;当移动机器人陷入局部极小值陷阱时,则需进一步降低移动机器人运动速度,防止在虚拟目标点引导过程中出现震荡。线速度v取值如下:

(13)

式中:Nmax和Nmin分别为障碍物数量最大预设值和最小预设值;Dmax和Dmin分别为移动机器人与障碍物之间距离最大预设值和最小预设值;vmax、vmid、vmin和vmmin分别表示线速度取得的较大、中等、较小和最小值。

2.4 虚拟目标点法

移动机器人在未知环境中依靠局部地图信息进行路径规划,当遇到局部极小值问题时,仅靠对势场函数进行改进无法有效解决。典型的局部极小值情况如图3所示。当移动机器人处于障碍物影响范围边缘,其所受到的斥力较小,合力方向由移动机器人指向目标点;当靠近障碍物时,其所受到的斥力大于引力,合力方向反转,移动机器人陷入局部极小值陷阱,表现为在该区域反复震荡或静止。

图3 局部极小值情况

局部极小值问题是由移动机器人所受合力为零达到平衡状态所致,本文分别提出APF-v1和APF-v2两种设置虚拟目标点的方法打破该平衡状态,引导移动机器人摆脱局部极小值陷阱。在设置虚拟目标点时,两种算法都需要对周边的障碍物环境进行检测,选择障碍物数量少的区域设置虚拟目标点。其中,APF-v1算法在斥力分量偏转90°方向的合适位置设置虚拟目标点,APF-v2算法则将虚拟目标点设置在目标障碍物外侧的合适位置。

2.4.1APF-v1

移动机器人的工作环境复杂多样,虚拟目标点的设置位置也直接影响规划路径的优劣,在设置虚拟目标点时应充分利用传感器感知到的环境信息。当系统检测到移动机器人陷入局部极小值陷阱时,通过直线连接移动机器人与目标点,该直线将移动机器人的工作环境分为两部分,分别计算两区域障碍物的数量,虚拟目标点设置在障碍物数量少的区域。

移动机器人通过传感器反馈的数据计算得到各个力的大小和方向。以移动机器人为圆心沿斥力分量Frep1向虚拟目标点设置区域偏转90°,沿此方向选取合适位置设置虚拟目标点,期间忽略原目标点的引力。为避免忽略障碍物斥力所导致移动机器人与障碍物发生碰撞的问题,需保留障碍物产生的斥力。移动机器人在虚拟目标点产生的引力和障碍物产生的斥力共同作用下逃离局部极小值点。生成虚拟目标点的位置(xvir,yvir):

(14)

式中:(xcur,ycur)为移动机器人的当前位置;ρvir为设置虚拟目标点的距离;θ(Frep1)为斥力分量Frep1相对地图坐标系的角度;Ovir为生成虚拟目标点的方向,Ovir为1表示虚拟目标点生成在移动机器人的左侧区域,Ovir为-1表示虚拟目标点生成在移动机器人的右侧区域。APF-v1算法设置虚拟目标点后的移动机器人受力分析如图4所示。

图4 APF-v1移动机器人受力分析

2.4.2 APF-v2

APF-v1算法分析当前受力情况,通过在斥力分量Frep1偏转90°方向上选择合适距离设置虚拟目标点,APF-v2算法则根据目标障碍物分布情况设置虚拟目标点。

当系统检测到移动机器人陷入局部极小值陷阱时,设置障碍物存储模块,记录使其陷入局部极小值陷阱的障碍物位置信息,形成局部极小值障碍物组,在系统判断的虚拟目标点设置区域选择障碍物组中最外侧的为目标障碍物,虚拟目标点设置在移动机器人当前位置相对于目标障碍物的对称点附近范围内。忽略原目标点引力,移动机器人在虚拟目标点产生的引力和障碍物产生的斥力共同作用下摆脱局部极小值点。生成的虚拟目标点位置(xvir,yvir):

(15)

式中:(xobs,yobs)为目标障碍物的位置;β1和β2为距离调节参数,取值为正。APF-v2算法设置虚拟目标点后的移动机器人受力分析如图5所示。

图5 APF-v2移动机器人受力分析

虚拟目标点采用一步一设的原则。移动机器人在摆脱局部极小值陷阱的过程中可能会设置多个虚拟目标点,直至脱离局部极小值点的影响范围。移动机器人摆脱局部极小值陷阱后撤销虚拟目标点,在原目标点和障碍物的共同作用下继续移动。

本文使用的两轮差速移动机器人模型是TurtleBot3系列的Burger版本,运动学模型如图6所示。其中左右两侧是驱动轮,分别由两个电机驱动;前后的万向轮是从动轮;ICR表示瞬时旋转中心;rrob表示移动机器人转向半径;drob表示移动机器人直径;vl和vr分别表示移动机器人左轮和右轮的线速度。

图6 移动机器人运动学模型

运动参数包括线速度v和角速度w,其中线速度v∈[0, 0.22],单位为m/s;角速度w∈[-2.84, 2.84],单位为rad/s。通过调节左右驱动轮速度来控制移动机器人运动。移动机器人的线速度与角速度和左右驱动轮速度之间的关系:

(16)

TurtleBot3移动机器人通过自身搭载的激光雷达获取周围环境信息。传感器探测范围为360°,探测距离为0.12～3.5 m,每15°返回一条激光雷达数据di(i=0～23),di表示该方向上最近障碍物的距离信息。移动机器人激光雷达模型如图7所示。

图7 移动机器人激光雷达模型

为验证本文所提方法的有效性,在ROS机器人操作系统中利用Gazebo物理仿真平台进行仿真环境搭建,在离散型障碍物、一型障碍物、U型障碍物及混合障碍物环境中分别对传统的人工势场法和改进的人工势场法进行对比实验。实验所采用的计算机配置是:Ubuntu 16.04操作系统,ROS Kinetic机器人操作系统,处理器为Intel i3-4160,运行内存4 GB,主频3.6 GHz。

为更直观地观察仿真结果,将仿真环境以及移动机器人运行轨迹实时显示在Rviz可视化工具中。其中,移动机器人初始位置为起点,方框为目标点,圆柱体为障碍物,实线为移动机器人运行的轨迹,方格地板边长为1 m。仿真过程算法主要参数设定见表1。

4.1 离散型障碍物环境下的仿真

Gazebo中离散稀疏障碍物环境和移动机器人如图8(a)所示,在Rviz中的投影如图8(b)所示。其中,移动机器人初始位置为起点,起点坐标为(-2, -1),目标点坐标为(4.5, 2.0)。

表1 仿真过程算法参数设定

(a)Gazebo中的仿真环境 (b) Rviz中的投影

APF-v1和APF-v2算法仅在解决局部极小值时采取不同的虚拟目标点策略。当未遇到局部极小值问题时,两种算法规划的路径相同且花费步数和耗时一致,故离散稀疏和密集障碍物环境中,将APF-v1和APF-v2算法统称为改进的人工势场法。传统人工势场法与改进的人工势场法规划的路径如图9(a)和图9(b)所示。改进的人工势场法采用自适应速度调节机制,根据当前工作环境的复杂程度实时对速度进行调整。在离散稀疏障碍物环境中,传统人工势场法完成路径规划任务所用步数为253;采用自适应速度调节机制时,完成路径规划所用步数为238,且运行轨迹相似,说明自适应调节机制能够提高路径规划的效率。

(a)传统人工势场法路径 (b) 改进人工势场法路径

在离散密集障碍物环境中调整起始位置、目标点位置以及障碍物布局如图10(a) 所示,在Rviz中的投影如图10(b)所示。其中,起点坐标为 (-2, -2),目标点坐标为(4.2, -2.8)。

(a)Gazebo中的仿真环境 (b) Rviz中的投影

传统人工势场法与改进的人工势场法规划的路径如图11(a)和图11(b)所示。当目标点距离障碍物较近时,传统人工势场法出现斥力过大问题,移动机器人在目标点附近反复震荡直至步数耗尽;改进的人工势场法在斥力势场函数中引入目标点与移动机器人之间的距离,保证仅目标点处的势能为全局最小,完成路径规划任务所用步数为288。

(a)传统人工势场法路径 (b) 改进人工势场法路径

4.2 一型障碍物环境下的仿真

Gazebo中的一型障碍物环境和移动机器人如图12(a)所示,在Rviz中的投影如图12(b)所示。其中,起点坐标为(-2.5, 0),目标点坐标为(2.5, 0)。

(a)Gazebo中的仿真环境 (b) Rviz中的投影

传统人工势场法、APF-v1和APF-v2算法规划的路径如图13所示。仅使用传统人工势场法时,移动机器人陷入局部极小值陷阱,在一型障碍物附近反复震荡,导致路径规划任务失败;本文提出的APF-v1算法在斥力分量Frep1偏转90°方向上选择合适距离设置虚拟目标点;APF-v2算法通过对当前环境分析选择在目标障碍物外侧设置虚拟目标点,两种算法均能使移动机器人在虚拟目标点的引导下摆脱局部极小值陷阱。

(a)传统人工势场法路径 (b) APF-v1算法路径

4.3 U型障碍物环境下的仿真

Gazebo中的U型障碍物环境和移动机器人如图14(a)所示,在Rviz中的投影如图14(b)所示。其中,起点坐标为(-3.5, 0),目标点坐标为(3, 0)。

(a)Gazebo中的仿真环境 (b) Rviz中的投影

传统人工势场法、APF-v1和APF-v2算法在U型障碍物环境中规划的路径如图15所示。传统人工势场法使移动机器人陷入U型障碍物陷阱,在局部极小值点附近反复震荡直至步数耗尽;APF-v1算法在摆脱U型障碍物陷阱时,由于移动机器人在U型障碍物边缘所受斥力分量Frep1角度变化较大,虚拟目标点设置位置变化较大,导致移动机器人摆脱U型障碍物陷阱所需步数较多;而APF-v2算法通过在目标障碍物外侧设置虚拟目标点的方法,能规划出相对更优的路径。

4.4 混合障碍物环境下的仿真

将离散型、一型和U型障碍物组合形成混合障碍物环境。Gazebo中的混合障碍物环境和移动机器人如图16(a)所示,在Rviz中的投影如图16(b)所示。其中,起点坐标为(-4, -2),目标点坐标为(4, 4)。

(a)传统人工势场法路径 (b) APF-v1算法路径

(a)Gazebo中的仿真环境 (b) Rviz中的投影

传统人工势场法、APF-v1和APF-v2算法规划的路径如图17所示。由于传感器探测范围有限,移动机器人沿势场的负梯度方向移动进入U型障碍物陷阱,传统人工势场法使移动机器人在U型障碍物陷阱区域反复震荡,导致路径规划任务失败;而APF-v1算法和APF-v2算法使移动机器人进入U型和一型障碍物陷阱时,对当前障碍物环境进行判断,通过设置虚拟目标点的方法引导机器人摆脱局部极小值陷阱。其中,APF-v1算法在绕行障碍物边缘时步数相对较多,APF-v2算法能规划出相对更优的路径。

对传统人工势场法、APF-v1算法和APF-v2算法在上述五种仿真环境中从起点到目标点的路径规划步数和时间进行汇总,见表2。

从表2数据对比可以看出,在各种仿真环境中,APF-v1算法和APF-v2算法均能规划出从起点到目标点的路径, 且在一型、U型和混合障碍物环境中,APF-v2算法规划的路径效果较好,所用的步数较少、耗时较短。

(a)传统人工势场法路径 (b) APF-v1算法路径

表2 仿真环境中算法对比数据

针对未知环境下的移动机器人局部路径规划问题,本文提出了APF-v1算法和APF-v2算法,并在ROS系统中设计不同的仿真环境,将本文所提算法与传统人工势场法进行了对比实验,验证了改进算法的有效性。APF-v1算法和APF-v2算法具有以下特点:

1)通过改进引力和斥力势场函数,有效解决了引力过大和目标不可达问题;根据当前环境复杂程度设计自适应速度调节机制,提高路径规划的效率。

2)对存在局部极小值陷阱的复杂障碍物区域,算法对当前障碍物环境判断,分别在斥力分量偏转90°方向上与目标障碍物外侧合适位置设置虚拟目标,引导移动机器人摆脱局部极小值陷阱。

3)当障碍物环境较为复杂时,APF-v2算法完成路径规划任务花费步数较少,用时较短,效率较高。

考虑到移动机器人的实际工作环境可能更为复杂,下一步工作考虑将人工势场法与模糊控制法以及强化学习方法结合,解决移动机器人在更为复杂场景下的路径规划问题,并进一步缩短路径长度,使移动机器人在各种环境下规划出较优的路径。