改进人工势场法的移动机器人路径规划研究

倪建云,杜合磊,谷海青,李 浩,吴 杰,薛晨阳

(1.天津市复杂系统控制理论与应用重点实验室, 天津 300384;2.天津理工大学 电气工程与自动化学院, 天津 300384)

0 引言

路径规划是移动机器人领域的重要分支之一,也是移动机器人导航技术的重点[1]。路径规划问题就是在一定约束条件下,按照一定的评价标准寻找一条最优或次优的无碰撞路径[2]。移动机器人的路径规划按照对环境的已知程度,可以划分为基于环境已知的全局路径规划和基于环境未知或局部已知的局部路径规划[3]。常用的方法有蚁群算法、栅格法、遗传算法、快速扩展随机树法、人工势场法等。人工势场法因具有反应速度快、实时性强等优点,被广泛应用于路径规划中。但是人工势场存在目标不可达和局部极小值的问题[4],如何更好地解决人工势场存在的问题,国内外学者做了广泛的研究,最终产生了2个主要的方向。一种是通过改进人工势场的势函数,降低人工势场陷入局部极小值的概率;另一种是与其他算法进行融合来克服人工势场的缺点。Lazarwaka等[5]提出了一种离散人工势场法,将移动机器人的运动环境离散化,找寻最低离散点,解决局部极小值问题。王迪等[6]提出了一种左转势场法,使移动机器人陷入局部极小值时强制转向,从而跳出局部极小值,但是这种方法在遇到类似封闭的陷阱区域时存在局限性,无法到达目标点。杨凯等[7]通过移动机器人当前位置、目标点位置和障碍物位置建立数学关系,设置虚拟目标点位置,以解决局部极小值问题。于振中等[8]提出了一种填平势场来引导移动机器人走出局部极小值,但这种方法需要不断将各种势场强度进行代数和叠加,计算量大且复杂。Sang等[9]将A*算法生成的全局路径点分为最优子目标点序列,引导移动机器人不断到达子目标点,从而解决局部极小值问题。刘翰培等[10]针对局部极小值问题将模糊控制思想引入到了人工势场法中,对移动机器人偏转角进行精确控制。赵炳巍等[11]结合模拟退火算法,在出现局部极小值位置附近增设随机目标点,用临时引力牵引移动机器人走出局部极小值。

本文对传统人工势场的基本原理进行了简要概述,分析了目标不可达和局部极小值形成的原因。在改进斥力场函数的基础上,通过椭圆形目标策略设置虚拟目标点,使移动机器人在陷入局部极小值时所受合力不为零,从而逃离局部极小值。另外,在椭圆形目标策略设置虚拟目标点的基础上提出椭圆形跟踪策略,采用模型预测控制算法跟踪既定的部分椭圆形到达所设置的虚拟目标点解决局部极小值。为了满足移动机器人速度与加速度连续的要求,采用了3次均匀B样条曲线对所规划出的路径进行拟合处理。最后,在仿真平台上对改进算法与传统算法进行仿真对比实验,实验结果表明,本文的方法能够有效解决传统人工势场的缺点。

1 传统人工势场基本原理

人工势场法是一种虚拟力法[12-13],于1986年首次被Khatib提出来的。该方法是将移动机器人抽象为一个点,该点在人工势场2种虚拟场源的作用下运动。虚拟场源分为2类,一类是引力势场源:将目标点视为引力极产生引力,引力极产生的引力是移动机器人位置与目标点位置之间的距离相关函数;另一类是斥力势场源:将障碍物视为斥力极产生斥力,斥力极产生的斥力是移动机器人位置与障碍物位置之间的距离相关函数。引力与斥力叠加的矢量大小和方向就是移动机器人运动的大小和方向,传统人工势场法如图1所示。

图1 传统人工势场法示意图

1.1 引力势场

假设移动机器人在运动环境中的位置向量为X=(x,y)T,目标点位置向量为Xg=(xg,yg)T,定义引力势场函数[14]:

(1)

式中:Uatt(X)为引力势场;ka为引力势场常量。相应的引力为Uatt(X)的负梯度:

Fatt(X)=-grad(Uatt(X))=

-ΔUatt(X)=-ka(X-Xg)

(2)

1.2 斥力势场

假设障碍物在运动环境中的位置向量为Xo=(xo,yo)T,定义斥力势场函数为:

式中:Urep(X)为斥力势场;kr为斥力势场常量;ρ为障碍物的最大影响范围。相应的斥力为Urep(X) 的负梯度:

Frep(X)=-grad(Urep(X))=-ΔUrep(X)=

(4)

1.3 总势场

总势场为引力势场与斥力势场的和:

Utotal(X)=Uatt(X)+Urep(X)

(5)

合力为引力与斥力的矢量和:

Ftotal(X)=Fatt(X)+Frep(X)=

(6)

2 传统人工势场存在的问题

2.1 目标不可达

传统人工势场法(T-APF)的目标不可达问题就是当目标点附近存在障碍物,即目标点此时处在障碍物最大影响范围之内时,移动机器人在运动时受到的斥力大于引力,导致移动机器人停止运动或在某一区域内徘徊,从而无法到达目标点的过程,如图2所示,此时斥力大于引力,合力方向偏于目标点位置方向,导致移动机器人无法到达目标点。

图2 目标不可达问题示意图

2.2 局部极小值

局部极小值就是移动机器人所受到的引力与斥力合力大小相同方向相反,即移动机器人受到的合力为零,移动机器人停止了运动或在某一区域徘徊的过程。针对运动环境中障碍物数量的不同,具体分析移动机器人陷入局部极小值的情况。

对于小于2个障碍物的简单运动环境,移动机器人陷入局部极小值最常见的情况有3种:一是障碍物在移动机器人与目标点中间;二是目标点在移动机器人与障碍物中间;三是2个障碍物分布在移动机器人与目标点连线的两边。3种情况分别如图3—图5所示。

图3 局部极小值1受力分析示意图

图4 局部极小值2受力分析示意图

图5 局部极小值3受力分析示意图

对于大于2个障碍物的复杂运动环境,移动机器人陷入局部极小值最常见的情况有两字形陷阱问题。如图6 U型陷阱区域受力分析和图7 L型陷阱区域受力分析所示,图中的Frep代表着斥力和。

图6 U型陷阱区域受力分析示意图

图7 L型陷阱区域受力分析示意图

3 改进人工势场法

为了能够更好地处理目标不可达问题,本文采用改进人工势场法(I-APF)进行解决,即在斥力势场函数中加入距离因子,距离因子是移动机器人当前位置与目标点位置之间的欧氏距离。则此时斥力场函数为:

(7)

相应的斥力为:

(8)

式(7)和式(8)中的(X-Xg)n是距离因子,Frep 1和Frep2是斥力Frep(X)的2个分力,Frep1所在方向与障碍物位置向量指向移动机器人位置向量的方向一致,Frep2所在方向与移动机器人位置向量指向目标点位置向量的方向一致。选取0

图8 改进斥力场函数受力分析示意图

对于图4中局部极小值2的情况,本文采用当移动机器人陷入局部极小值时忽略障碍物斥力的方法进行解决,此时移动机器人只受到目标点引力,解决局部极小值2问题如图9所示。

图9 解决局部极小值2问题示意图

3.1 椭圆形目标策略

本文提出了一种椭圆形目标策略(IEVAPF)解决人工势场法存在的局部极小值问题。该方法以移动机器人与障碍物之间的欧氏距离为长半轴,以障碍物膨胀距离为短半轴建立椭圆形,短轴所在的直线与椭圆形相交的顶点为虚拟目标点构建位置,此时移动机器人受到虚拟目标点的引力逃离局部极小值位置。由于运动环境不同,所以选取的障碍物有所不同,对于单一障碍物运动环境,障碍物只有一个选择,此时产生2个虚拟目标点可以随机选取其中一个;对于2个障碍物运动环境,2个障碍物可以随机选取,但是需要选取远离另一个障碍物位置的虚拟目标点;对于多个障碍物运动环境,尤其是2个陷阱区域,选取离移动机器人最远距离的障碍物实行椭圆形目标策略。各运动环境解决方法如图10椭圆形目标策略所示。

图10 椭圆形目标策略

针对图10(c)和图10(d)中椭圆形目标策略,为了提高规划效率,防止移动机器人在逃离局部极小值时再次陷入新的局部极小值点,对长半轴加入一个权重因子α,即长半轴:

L=α(X-Xo),α≥1

(9)

权重因子α的选择影响虚拟目标点的位置,应使得椭圆形可以包围边缘障碍物,在考虑移动机器人大小的前提下,可以有效避免在运动过程中碰撞到障碍物,致使规划失败。

3.2 椭圆形跟踪策略

本文提出了一种椭圆形跟踪策略(IEMAPF),在椭圆形目标策略确定椭圆形和虚拟目标点的基础上,采用结合移动机器人运动学模型的模型预测控制算法[15-16]设计控制器,进行跟踪既定的部分椭圆形到达虚拟目标点,从而解决陷入局部极小值的问题。移动机器人的运动学状态方程为:

(10)

式(10)中:(x,y)为移动机器人后轴中心坐标;φ为移动机器人的横摆角;v为后轮速度;δ为前轮转角;l为移动机器人前后轮之间轴距长度。针对于单个障碍物、2个障碍物和多个障碍物的运动环境,椭圆形跟踪策略的解决方法如图11所示。以图11(a)所示的单个障碍物环境为例,当机器人处于局部极小的状态时,在椭圆形目标策略确定了椭圆形和虚拟目标点的基础上,采用了椭圆形跟踪策略跟踪既定的部分椭圆形,即图中椭圆形的实线部分。针对图11(d)中“L”字形陷阱区域的情况,由于加入了权重因子α,在实行椭圆形跟踪策略时,根据椭圆形目标策略所构建的椭圆形,提前预测机器人陷入局部极小的位置坐标点,即图中虚线受力分析部分,继而进行椭圆形跟踪策略,即图中椭圆形的实线部分。

图11 椭圆形跟踪策略示意图

4 三次均匀B样条拟合处理

B样条曲线因其局部可修改性的优点,尤其是具有表达式简单的特点,使得B样条曲线在路径拟合处理中得到了广泛的应用。其中三次均匀B样条曲线在节点处具有二阶连续性的特点,可以满足移动机器人速度与加速度连续性要求[17],因此,本文针对所规划出的路径在拐点处不平滑且降低了移动机器人转弯效率的问题,引入了三次均匀B样条曲线对所规划路径进行拟合处理。B样条曲线的数学表达式为[18-19]:

(11)

式中:Pi为路径中给定的控制点坐标;Ni,k(t)为B样条曲线的基函数,基函数的具体表达式为:

(12)

(13)

将式(13)代入到B样条表达式式(11)中可得到三次均匀B样条曲线方程的矩阵表达形式为:

(14)

5 实验仿真

针对以上提到的所有运动环境,为了验证I-APF、IEVAPF和IEMAPF有效性,使用Matlab搭载运动环境进行仿真实验。对目标不可达和单个障碍物、2个障碍物和多个障碍物环境问题分别进行对比仿真。本文设置的人工势场参数如表1所示。下面所有的运动环境实验仿真的人工势场参数都与表1的参数相同。

表1 人工势场参数

5.1 目标不可达环境仿真

对于图2中目标不可达问题,采用图8所示的改进斥力场函数受力分析方法进行解决。图12所示为目标不可达问题的运动环境,蓝色星号(0,0) 为起点位置坐标,绿色实心圆(10,10)为目标点位置坐标,黑色实心圆(9,9.5)为障碍物位置坐标。图12(a)中,在传统人工势场作用下,移动机器人未能到达目标点位置,图12(b)中,在改进斥力场函数情况下,移动机器人最终到达目标点。图12(c)是三次均匀B样条曲线拟合处理(B-APF)后的路径结果,可以看出,在路径拐点处拟合后的路径相比之前更平滑。

图12 目标不可达问题的运动环境仿真示意图

5.2 局部极小值环境仿真

对于图4中局部极小值2情况,根据图9给出的解决方法进行实验验证。设置起点位置坐标(0,0),目标点位置坐标(10,10),障碍物位置坐标(11,11)。图13(a)表示传统人工势场,移动机器人在距离目标点一段距离处徘徊,导致路径规划失败,此时忽略障碍物斥力,如图13(b)所示,移动机器人顺利到达目标点。由图13(c)所示,经过三次均匀B样条曲线拟合处理后,去掉了路径上的冗余点,使路径更平滑。

图13 局部极小值2仿真示意图

5.2.1单一障碍物环境仿真

对于图3中局部极小值1情况,图10(a)和图11(a)分别给出了椭圆形目标策略和椭圆形跟踪策略解决方法。设置图3中运动环境起点位置坐标(0,0),目标点位置坐标(10,10),障碍物位置坐标(7,7)。图14(a)和图14(b)给出了2种策略的实验仿真,图14(c)是三次均匀B样条曲线拟合椭圆形目标策略(B-IEVAPF)的仿真结果,图14(d)是三次均匀B样条曲线拟合椭圆形跟踪策略(B-IEMAPF)的仿真结果。

5.2.2两个障碍物环境仿真

对于图5中2个障碍物造成局部极小值的情况,根据图10(b)和图11(b)2种策略进行解决。设置图5中运动环境起点位置坐标(0,0),目标点位置坐标(10,10),障碍物位置坐标(5.5,3.5)和(3.5,5.5)。图15(a)和图15(b)分别是2种策略实验仿真,图15(c)和图15(d)给出了2种策略经过三次均匀B样条曲线处理过的仿真结果。

图15 局部极小值3仿真

5.2.3复杂障碍物环境仿真

对于图6和图7中多障碍物复杂环境,分别按照图10(c)、图11(c)和图10(d)、图11(d)进行实验仿真。运动环境中起点位置坐标(0,0),目标点位置坐标(10,10),障碍物位置坐标排列成“U”字形和“L”字形。图16是图6中U形陷阱区域问题的仿真,且给出了三次均匀B样条曲线处理后的结果。图16中没有加入权重因子α,为了验证加入权重因子的效果,图17中L形陷阱区域仿真给出了仿真验证。由图10(d)和图11(d)中可以看出,在采用2种策略进行解决局部极小值问题时,椭圆形与障碍物相交导致设置的虚拟目标点位置可能使移动机器人再次陷入局部极小值,因此,根据式(9)选取权重因子α=1.2,改变椭圆形长半轴大小,让椭圆形可以包围边缘障碍物,提高移动机器人的规划效率,如图18所示。图17(a)和图17(b)中,在加入权重因子的情况下,可以顺利完成规划任务。图18(a)中,未加入权重因子,导致再次陷入局部极小值点,图18(b)中,在加入权重因子后,可以顺利逃离局部极小值点且避免了二次陷入局部极小值的问题。同时为了验证不同权重因子对路径规划的影响,分别设置了权重因子数值为1.2如图18(b),权重因子数值为2如图18(c)。图中可以看出两者对IEVAPF方法影响不大,但对于图18(d)中IEMAPF方法影响较大,相比图17(b),由于椭圆形变大使跟踪既定的部分椭圆形路径变长,通过B样条拟合处理后的图18(e)中路径也要相对变长,具体路径评价参数可见表4不同权重因子路径评价表所示。图16(c)、(d)和图17(c)、(d)分别给出了2种陷阱区域的2种策略经过三次均匀B样条曲线处理过的仿真结果。

图16 U形陷阱区域仿真

图17 L形陷阱区域仿真

图18 加入权重因子对比仿真示意图

针对IEMAPF方法,在局部极小值点位置时,通过当前位置姿态与下一运动时刻位置姿态计算获取移动机器人前轮转角,为了验证移动机器人不同速度下IEMAPF方法的有效性,图19给出了不同速度下IEMAPF方法对比图。图19(a)给出了不同速度下跟踪既定路径的仿真,为了能够清楚地分析,图19(c)对不同位置的跟踪效果进行了局部放大处理。从局部放大图和图19(b)跟踪误差图可以分析出,当速度v=10 m/s(图中蓝色表示)时,相比v=2 m/s(图中红色表示)时跟踪效果较差一点,但是在速度相差较大时,都能够保持良好的跟踪精度且误差小于0.031 m。为了更好地贴合既定路径,采用相对较小的跟踪速度。

为了验证IEVAPF和IEMAPF 2种策略的有效性,图20给出了复杂区域实验仿真。复杂区域运动环境中包括了目标不可达和所有局部极小值的环境。运动环境起点位置坐标设置成(0,0),目标点位置坐标为(14,14),51个障碍物组成各种陷阱环境。

由图20(a)和图20(b)所示,2种策略在陷入2次局部极小值后成功到达目标点,有效证明本文所提方法能够解决传统人工势场法的缺点。图20(c)、(d)是经过三次均匀B样条处理后的路径。

图19 不同速度IEMAPF仿真图

图20 复杂区域仿真图

5.3 路径评价对比

为了使图12目标不可达仿真图和图13局部极小值2仿真图的仿真结果更加直观,对传统人工势场法、改进人工势场法和经三次均匀B样条拟合后人工势场法的路径长度进行对比。由表2可知,传统人工势场法失效,导致机器人不能到达目标点,三次均匀B样条拟合后,人工势场法在路径长度上要短于改进人工势场法。

表2 路径评价表1

为了直观地验证2种策略能够有效解决传统人工势场法的缺点,表3以移动机器人路径长度和拟合后的路径长度为评价标准进行对比。

表3 路径评价表2

由表3可以看出:椭圆形跟踪策略要比椭圆形目标策略的路径上短,可以提高移动机器人的运动效率。经过三次均匀B样条曲线拟合后,路径长度都明显缩短,椭圆形跟踪策略仍然优于椭圆形目标策略。

由表4中不同权重因子可以分析出:当没有加入权重因子,即权重因子为1时,路径规划失败;当选取的权重因子不断变大时,对IEMAPF方法和B-IEMAPF方法影响较大,因此,设置权重因子时,应在保证成功规划的基础上,尽量选取较小的权重因子。

表4 不同权重因子路径评价

6 结论

针对传统人工势场法存在目标不可达和局部极小值问题,提出一种改进的人工势场法。在改进斥力场函数的基础上,提出椭圆形目标策略和椭圆形跟踪策略,当机器人陷入局部极小值后仍能够及时向目标点移动,完成路径规划任务。为了使规划路径更加平滑,采用三次均匀B样条曲线对规划路径进行拟合处理。最后在仿真平台中对传统人工势场法与IAPF、IEVAPF、IEMAPF、B-IEVAPF和B-IEMAPF进行不同运动环境的仿真实验,证实了所提改进人工势场法的可行性与有效性。