一种动态环境下的速度势场方法

一种动态环境下的速度势场方法

1运动规划中的应用在动态环境下，机器人的运动规划一直是移动机器研究领域的一个很难解决的问题。在动态环境下，机器人不仅是移动的，障碍物是移动的，机器人的目标也是移动的。机器人运动规划的问题不是传统的障碍避免问题，而是动态逃避问题，包括动态跟踪问题。基于之前的研究结果，本文给出了相对正确的定义：机器人运动规划是指根据机器人的初始状态和目标状态，规划一组运动，使机器人轻轻抵达目标状态。机器人的目标状态不是固定的点，而是一个特定的运动状态。为了促进目标和运动，引入了新的问题：（1）当机器人到达目标时，机器人的速度与目标的速度相同。（2）机器人的速度和目标的速度不同，这是不同的。它们都是实际的应用，如足球机器人的断球、破球和跑位。在机器人运动规划中比较常用的方法有:人工势场法、向量场法、栅格法、微分几何法、速度障碍物的概念和方法等.其中人工势场法在运动规划中引入虚力的概念对机器人的运动进行规划:机器人根据势场(机器人所处的环境)确定它所受的虚力,然后以这个虚力作用于机器人,控制它的运动,从而规划出机器人的一组运动状态.在这里已没有明显的路径规划、轨迹跟踪的界限,机器人的运动路径(轨迹)即是规划出的路径,而这条路径并不是显式表示出来的,而是隐含在一段时间段内连续作用于机器人的虚力当中.一般将机器人的运动规划分为两个部分:(1)趋向于目标的全局运动规划;(2)躲避障碍物的运动规划.这两部分可以分别通过构造引力势场和斥力势场来解决.早期对人工势场的研究主要用于解决静态环境下机器人的局部路径规划问题.而对于动态环境下的运动规划问题,传统的人工势场就勉为其难了:因为传统的人工势场只考虑了位置的因素,是位置的函数,忽略了动态环境中的许多重要因素.近年来,许多学者对传统势场法做了不少改进,其中Ko与Lee在建立势场时引入了障碍物的速度,Ge与Cui在建立势场时考虑了机器人与障碍物不发生碰撞的最短允许距离、相对速度等因素.2动态避障能力的实现动态环境下移动机器人的运动规划问题是指在机器人、目标、障碍物运动的条件下规划和控制移动机器人以期望的方式从初始状态运动到目标状态,机器人应该能够实现对目标点的快速停靠,也应该能够实现对运动目标的快速跟踪,并且在这一过程中能够实现动态避障.为了简化分析,我们做如下假设:(1)机器人的位置P、速度V已知,且机器人的外形为圆形,半径为R.(2)目标的位置Ptar和速度Vtar已知,且目标的速度║Vtar║<Vmax.(3)障碍物的位置Pobsi、速度Vobsi可以被实时地检测到,障碍物的外形为半径为Robsi的圆,Pobsi为圆心,其中i表示第i个障碍物.为讨论方便,在分析问题时我们假设机器人为一个质点,障碍物的外形半径为Ri=R+Robsi.3力场函数和重力函数attackcapture函数3.1引力条件和相对速度引力场函数传统的引力场函数仅仅定义为机器人与目标的相对位置的函数,即相对位置引力场函数:Uattp=12α||Ρtar(t)-Ρ(t)||2(1)Uattp=12α||Ptar(t)−P(t)||2(1)这对于目标点固定的情况比较适用,但对于动态环境下目标点运动的情况,效果就不明显了,不能有效地跟踪目标点,容易产生振荡,我们在引力场中引入速度项,即构造相对速度引力势场函数:Uattv=12β||Vtar(t)-V(t)||2(2)Uattv=12β||Vtar(t)−V(t)||2(2)分别对相对位置引力场函数和相对速度引力场函数求负梯度得到相对位置引力和相对速度引力:Fattp=-▽p⋅Uattp=α[Ρtar(t)-Ρ(t)](3)Fattv=-▽v⋅Uattv=β[Vtar(t)-V(t)](4)Fattp=−▽p⋅Uattp=α[Ptar(t)−P(t)](3)Fattv=−▽v⋅Uattv=β[Vtar(t)−V(t)](4)在引入了相对速度引力势场函数之后,引力场函数变为:Uatt=Uattp+Uattv=12α||Ρtar(t)-Ρ(t)||2+12β||Vtar(t)-V(t)||2(5)Uatt=Uattp+Uattv=12α||Ptar(t)−P(t)||2+12β||Vtar(t)−V(t)||2(5)机器人在该势场环境下所受的引力为相对位置引力与相对速度引力的合力:Fatt=Fattp+Fattv=α[Ρtar(t)-Ρ(t)]+β[Vtar(t)-V(t)](6)Fatt=Fattp+Fattv=α[Ptar(t)−P(t)]+β[Vtar(t)−V(t)](6)3.2相对位置引力fattp引力势场的作用范围是全局的,在整个机器人的作业空间内,无论机器人与目标的相对位置如何(大小、方向),机器人的引力势场总是非负的.当且仅当相对位置与相对速度均为0时,引力势才为0.相对位置引力Fattp使得机器人沿着到目标的最短路径运动,其大小和机器人与目标点的相对位置的大小成正比,方向由机器人的当前位置指向目标点的位置,即目标点相对于机器人的位置方向.相对速度引力Fattv使得机器人的速度与目标的速度保持一致,其大小与相对速度的大小成正比,方向为目标点相对于机器人的运动方向.α、β为权重系数,调整其大小可以调整速度引力和位置引力在合引力中所占的权重,当β=0时引力场函数退化为传统的引力势场函数.3.3引力场函数为了验证新的引力函数的有效性,我们对静止目标和运动目标在传统引力函数和新的引力函数作用下分别做了仿真实验.对于目标是静止的情况,在传统的引力函数作用下,机器人只能到达目标点,但不能停靠在目标点,而在新的引力场函数作用下,机器人能够快速地停靠在目标点处,如图1所示.机器人的初始位置P(0,0),初始速度V(0,0),目标点的位置Ptar(30,30),曲线表示机器人随着时间的位置变化,直线表示固定的目标点.对于目标点运动的情况,在传统的引力函数作用下机器人只能随着目标运动,但不能保持与目标的运动状态一致,而在新的引力函数作用下,机器人能够快速地跟踪目标的运动,如图2所示.机器人的初始位置P(0,0),初始速度V(0,0),目标的初始位置Ptar(0,10),初始速度Vtar(3,3),直线表示目标随时间的位置变化,曲线表示机器人随时间的位置变化.4力场函数和阻力函数复合器函数和反演函数4.1相对速度斥力场函数传统的斥力场函数一般定义为机器人与障碍物之间的距离函数,即相对位置斥力场函数,本文定义第i个障碍物的斥力场为:Ureppi=aln∥Ρobsi(t)-Ρ(t)∥(7)Ureppi=aln∥Pobsi(t)−P(t)∥(7)由于动态环境下障碍物是运动的,仅考虑位置并不能完全反映环境信息,我们引入相对速度斥力场函数:Urepvi=b|∥Vobsi(t)-V(t)∥sinθ|=bVor(t)|sinθ|(8)Urepvi=b|∥Vobsi(t)−V(t)∥sinθ|=bVor(t)|sinθ|(8)其中Vor表示障碍物相对于机器人的速度大小,θ表示机器人相对于障碍物的位置矢量与相对速度矢量的夹角,如图3所示.我们对相对位置斥力场函数求位置微分得到相对位置斥力的函数:Freppi=▽p⋅Ureppi=-aΡobsi(t)-Ρ(t)||Ρobsi(t)-Ρ(t)||2(9)Freppi=▽p⋅Ureppi=−aPobsi(t)−P(t)||Pobsi(t)−P(t)||2(9)为了讨论方便起见,我们以障碍物相对于机器人的速度方向作为x轴,障碍物的位置作为坐标原点,建立平面直角坐标系,如图3所示,记机器人相对于障碍物的位置坐标为(Xro,Yro),θ的取值为(-π,π),则速度斥力场函数可以写为:Urepvi=bVor(t)|sinθ|=bVor(t)|Yro|√X2ro+Y2ro(10)Urepvi=bVor(t)|sinθ|=bVor(t)|Yro|X2ro+Y2ro√(10)定义速度斥力函数为速度斥力场函数的位置微分,则:Frepvi=▽p⋅UrepviFrepvi=▽p⋅Urepvi当θ>0时Frepvi=bVor(t)Xro(X2ro+Y2ro)3/2(-Yro,Xro)(11)Frepvi=bVor(t)Xro(X2ro+Y2ro)3/2(−Yro,Xro)(11)即:Frepvi=bVor(t)√X2ro+Y2rocosθ(-sinθ,cosθ)(12)Frepvi=bVor(t)X2ro+Y2ro√cosθ(−sinθ,cosθ)(12)当θ<0时Frepvi=-bVor(t)Xro(X2ro+Y2ro)3/2(-Yro,Xro)(13)Frepvi=−bVor(t)Xro(X2ro+Y2ro)3/2(−Yro,Xro)(13)即:Frepvi=-bVor(t)√X2ro+Y2rocosθ(-sinθ,cosθ)(14)Frepvi=−bVor(t)X2ro+Y2ro√cosθ(−sinθ,cosθ)(14)当θ=0时斥力函数不能确定方向,因此需要根据不同的情况来做补充定义,本文在θ=0时按(11)～(14)式进行计算.当cosθ<0时机器人向着远离障碍物的方向运动,此时即便没有速度斥力也不会发生碰撞,因此我们在这种情况下令相对速度斥力为0.当机器人距离障碍物比较远时,障碍物就威胁不到机器人了,因此障碍物对机器人的斥力有一定的作用范围,超过这个范围,斥力就不起作用了.我们以一定半径的圆来表征斥力的作用范围.基于以上讨论,第i个障碍物的相对位置斥力场函数写为:Ureppi={aln||Ρobsi(t)-Ρ(t)||alnRp||Ρobsi(t)-Ρ(t)||＜Rp||Ρobsi(t)-Ρ(t)||≥Rp(15)Ureppi={aln||Pobsi(t)−P(t)||alnRp||Pobsi(t)−P(t)||＜Rp||Pobsi(t)−P(t)||≥Rp(15)相对速度斥力场函数写为:Urepvi={b||Vobsi(t)-V(t)||sinθ0≤θ≤π2且||Ρobsi(t)-Ρ(t)||＜Rv-b||Vobsi(t)-V(t)||sinθ-π2≤θ＜0且||Ρobsi(t)-Ρ(t)||＜Rv0其它(16)Urepvi=⎧⎩⎨⎪⎪⎪⎪b||Vobsi(t)−V(t)||sinθ0≤θ≤π2且||Pobsi(t)−P(t)||＜Rv−b||Vobsi(t)−V(t)||sinθ−π2≤θ＜0且||Pobsi(t)−P(t)||＜Rv0其它(16)相对位置斥力函数写为:Freppi={aΡ(t)-Ρobsi(t)||Ρ(t)-Ρobsi(t)||2||Ρobsi(t)-Ρ(t)||＜Rp0||Ρobsi(t)-Ρ(t)||≥Rp(17)Freppi={aP(t)−Pobsi(t)||P(t)−Pobsi(t)||2||Pobsi(t)−P(t)||＜Rp0||Pobsi(t)−P(t)||≥Rp(17)相对速度斥力函数写为:Frepvi={bVor(t)√X2ro+Y2rocosθ(-sinθ,cosθ)0≤θ≤π2且∥Ρobsi(t)-Ρ(t)∥＜Rv-bVor(t)√X2ro+Y2rocosθ(-sinθ,cosθ)-π2≤θ＜0且∥Ρobsi(t)-Ρ(t)∥＜Rv0其它(18)在引入了相对速度斥力场之后机器人在人工势场中所受的斥力为:Frep=Ν∑i=1(Freppi+Frepvi)(19)其中N为障碍物的个数.4.2机器人与藻类相对速度斥力比m斥力场的作用范围是局部的,位置斥力场和速度斥力场可以具有不同的作用范围.相对位置斥力使得机器人快速远离障碍物,其大小与机器人到障碍物距离成反比,方向为由障碍物指向机器人.相对速度斥力垂直于机器人与障碍物的相对位置方向,当夹角θ较小时,速度斥力几乎垂直于相对速度的方向,能够使机器人较快地脱离障碍威胁.机器人与障碍物的相对位置越近,速度斥力越大;相对速度越大,斥力越大.a、b为权重系数,调整其大小可以调整相对位置斥力和相对速度斥力在斥力中的权重.4.3考虑污染物的情况传统的人工势场法中,机器人很容易陷入局部极值点,并且经常在障碍物附近出现振荡.在引入相对速度斥力之后,虽然不能完全地消除局部极值点,但可以使极值点的出现几率大大降低,并且消除了机器人在障碍物附近出现振荡的可能.限于篇幅,我们只考虑障碍物在机器人与目标之间的情况,如图5、图6.在传统的势场作用下,机器人向着目标运动,同时也向着障碍物运动,若在机器人所受的虚力中斥力的权重比较大,则机器人到达不了目标就会被斥力推向远处,若斥力所占的权重比较小,则机器人在惯性和所受引力作用下会撞上障碍物.一般的解决办法是构造出虚拟目标点,一步一步地将机器人引向目标.而在引入了速度斥力场函数之后,则不存在这样的问题,机器人在新的势场作用下能够顺利地绕过障碍