一種動(dòng)態(tài)環(huán)境下的速度勢場方法

一種動(dòng)態(tài)環(huán)境下的速度勢場方法

1運(yùn)動(dòng)規(guī)劃中的應(yīng)用在動(dòng)態(tài)環(huán)境下，機(jī)器人的運(yùn)動(dòng)規(guī)劃一直是移動(dòng)機(jī)器研究領(lǐng)域的一個(gè)很難解決的問題。在動(dòng)態(tài)環(huán)境下，機(jī)器人不僅是移動(dòng)的，障礙物是移動(dòng)的，機(jī)器人的目標(biāo)也是移動(dòng)的。機(jī)器人運(yùn)動(dòng)規(guī)劃的問題不是傳統(tǒng)的障礙避免問題，而是動(dòng)態(tài)逃避問題，包括動(dòng)態(tài)跟蹤問題?；谥暗难芯拷Y(jié)果，本文給出了相對正確的定義：機(jī)器人運(yùn)動(dòng)規(guī)劃是指根據(jù)機(jī)器人的初始狀態(tài)和目標(biāo)狀態(tài)，規(guī)劃一組運(yùn)動(dòng)，使機(jī)器人輕輕抵達(dá)目標(biāo)狀態(tài)。機(jī)器人的目標(biāo)狀態(tài)不是固定的點(diǎn)，而是一個(gè)特定的運(yùn)動(dòng)狀態(tài)。為了促進(jìn)目標(biāo)和運(yùn)動(dòng)，引入了新的問題：（1）當(dāng)機(jī)器人到達(dá)目標(biāo)時(shí)，機(jī)器人的速度與目標(biāo)的速度相同。（2）機(jī)器人的速度和目標(biāo)的速度不同，這是不同的。它們都是實(shí)際的應(yīng)用，如足球機(jī)器人的斷球、破球和跑位。在機(jī)器人運(yùn)動(dòng)規(guī)劃中比較常用的方法有:人工勢場法、向量場法、柵格法、微分幾何法、速度障礙物的概念和方法等.其中人工勢場法在運(yùn)動(dòng)規(guī)劃中引入虛力的概念對機(jī)器人的運(yùn)動(dòng)進(jìn)行規(guī)劃:機(jī)器人根據(jù)勢場(機(jī)器人所處的環(huán)境)確定它所受的虛力,然后以這個(gè)虛力作用于機(jī)器人,控制它的運(yùn)動(dòng),從而規(guī)劃出機(jī)器人的一組運(yùn)動(dòng)狀態(tài).在這里已沒有明顯的路徑規(guī)劃、軌跡跟蹤的界限,機(jī)器人的運(yùn)動(dòng)路徑(軌跡)即是規(guī)劃出的路徑,而這條路徑并不是顯式表示出來的,而是隱含在一段時(shí)間段內(nèi)連續(xù)作用于機(jī)器人的虛力當(dāng)中.一般將機(jī)器人的運(yùn)動(dòng)規(guī)劃分為兩個(gè)部分:(1)趨向于目標(biāo)的全局運(yùn)動(dòng)規(guī)劃;(2)躲避障礙物的運(yùn)動(dòng)規(guī)劃.這兩部分可以分別通過構(gòu)造引力勢場和斥力勢場來解決.早期對人工勢場的研究主要用于解決靜態(tài)環(huán)境下機(jī)器人的局部路徑規(guī)劃問題.而對于動(dòng)態(tài)環(huán)境下的運(yùn)動(dòng)規(guī)劃問題,傳統(tǒng)的人工勢場就勉為其難了:因?yàn)閭鹘y(tǒng)的人工勢場只考慮了位置的因素,是位置的函數(shù),忽略了動(dòng)態(tài)環(huán)境中的許多重要因素.近年來,許多學(xué)者對傳統(tǒng)勢場法做了不少改進(jìn),其中Ko與Lee在建立勢場時(shí)引入了障礙物的速度,Ge與Cui在建立勢場時(shí)考慮了機(jī)器人與障礙物不發(fā)生碰撞的最短允許距離、相對速度等因素.2動(dòng)態(tài)避障能力的實(shí)現(xiàn)動(dòng)態(tài)環(huán)境下移動(dòng)機(jī)器人的運(yùn)動(dòng)規(guī)劃問題是指在機(jī)器人、目標(biāo)、障礙物運(yùn)動(dòng)的條件下規(guī)劃和控制移動(dòng)機(jī)器人以期望的方式從初始狀態(tài)運(yùn)動(dòng)到目標(biāo)狀態(tài),機(jī)器人應(yīng)該能夠?qū)崿F(xiàn)對目標(biāo)點(diǎn)的快速停靠,也應(yīng)該能夠?qū)崿F(xiàn)對運(yùn)動(dòng)目標(biāo)的快速跟蹤,并且在這一過程中能夠?qū)崿F(xiàn)動(dòng)態(tài)避障.為了簡化分析,我們做如下假設(shè):(1)機(jī)器人的位置P、速度V已知,且機(jī)器人的外形為圓形,半徑為R.(2)目標(biāo)的位置Ptar和速度Vtar已知,且目標(biāo)的速度║Vtar║<Vmax.(3)障礙物的位置Pobsi、速度Vobsi可以被實(shí)時(shí)地檢測到,障礙物的外形為半徑為Robsi的圓,Pobsi為圓心,其中i表示第i個(gè)障礙物.為討論方便,在分析問題時(shí)我們假設(shè)機(jī)器人為一個(gè)質(zhì)點(diǎn),障礙物的外形半徑為Ri=R+Robsi.3力場函數(shù)和重力函數(shù)attackcapture函數(shù)3.1引力條件和相對速度引力場函數(shù)傳統(tǒng)的引力場函數(shù)僅僅定義為機(jī)器人與目標(biāo)的相對位置的函數(shù),即相對位置引力場函數(shù):Uattp=12α||Ρtar(t)-Ρ(t)||2(1)Uattp=12α||Ptar(t)?P(t)||2(1)這對于目標(biāo)點(diǎn)固定的情況比較適用,但對于動(dòng)態(tài)環(huán)境下目標(biāo)點(diǎn)運(yùn)動(dòng)的情況,效果就不明顯了,不能有效地跟蹤目標(biāo)點(diǎn),容易產(chǎn)生振蕩,我們在引力場中引入速度項(xiàng),即構(gòu)造相對速度引力勢場函數(shù):Uattv=12β||Vtar(t)-V(t)||2(2)Uattv=12β||Vtar(t)?V(t)||2(2)分別對相對位置引力場函數(shù)和相對速度引力場函數(shù)求負(fù)梯度得到相對位置引力和相對速度引力: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)在引入了相對速度引力勢場函數(shù)之后,引力場函數(shù)變?yōu)?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)機(jī)器人在該勢場環(huán)境下所受的引力為相對位置引力與相對速度引力的合力: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引力勢場的作用范圍是全局的,在整個(gè)機(jī)器人的作業(yè)空間內(nèi),無論機(jī)器人與目標(biāo)的相對位置如何(大小、方向),機(jī)器人的引力勢場總是非負(fù)的.當(dāng)且僅當(dāng)相對位置與相對速度均為0時(shí),引力勢才為0.相對位置引力Fattp使得機(jī)器人沿著到目標(biāo)的最短路徑運(yùn)動(dòng),其大小和機(jī)器人與目標(biāo)點(diǎn)的相對位置的大小成正比,方向由機(jī)器人的當(dāng)前位置指向目標(biāo)點(diǎn)的位置,即目標(biāo)點(diǎn)相對于機(jī)器人的位置方向.相對速度引力Fattv使得機(jī)器人的速度與目標(biāo)的速度保持一致,其大小與相對速度的大小成正比,方向?yàn)槟繕?biāo)點(diǎn)相對于機(jī)器人的運(yùn)動(dòng)方向.α、β為權(quán)重系數(shù),調(diào)整其大小可以調(diào)整速度引力和位置引力在合引力中所占的權(quán)重,當(dāng)β=0時(shí)引力場函數(shù)退化為傳統(tǒng)的引力勢場函數(shù).3.3引力場函數(shù)為了驗(yàn)證新的引力函數(shù)的有效性,我們對靜止目標(biāo)和運(yùn)動(dòng)目標(biāo)在傳統(tǒng)引力函數(shù)和新的引力函數(shù)作用下分別做了仿真實(shí)驗(yàn).對于目標(biāo)是靜止的情況,在傳統(tǒng)的引力函數(shù)作用下,機(jī)器人只能到達(dá)目標(biāo)點(diǎn),但不能?？吭谀繕?biāo)點(diǎn),而在新的引力場函數(shù)作用下,機(jī)器人能夠快速地停靠在目標(biāo)點(diǎn)處,如圖1所示.機(jī)器人的初始位置P(0,0),初始速度V(0,0),目標(biāo)點(diǎn)的位置Ptar(30,30),曲線表示機(jī)器人隨著時(shí)間的位置變化,直線表示固定的目標(biāo)點(diǎn).對于目標(biāo)點(diǎn)運(yùn)動(dòng)的情況,在傳統(tǒng)的引力函數(shù)作用下機(jī)器人只能隨著目標(biāo)運(yùn)動(dòng),但不能保持與目標(biāo)的運(yùn)動(dòng)狀態(tài)一致,而在新的引力函數(shù)作用下,機(jī)器人能夠快速地跟蹤目標(biāo)的運(yùn)動(dòng),如圖2所示.機(jī)器人的初始位置P(0,0),初始速度V(0,0),目標(biāo)的初始位置Ptar(0,10),初始速度Vtar(3,3),直線表示目標(biāo)隨時(shí)間的位置變化,曲線表示機(jī)器人隨時(shí)間的位置變化.4力場函數(shù)和阻力函數(shù)復(fù)合器函數(shù)和反演函數(shù)4.1相對速度斥力場函數(shù)傳統(tǒng)的斥力場函數(shù)一般定義為機(jī)器人與障礙物之間的距離函數(shù),即相對位置斥力場函數(shù),本文定義第i個(gè)障礙物的斥力場為:Ureppi=aln∥Ρobsi(t)-Ρ(t)∥(7)Ureppi=aln∥Pobsi(t)?P(t)∥(7)由于動(dòng)態(tài)環(huán)境下障礙物是運(yùn)動(dòng)的,僅考慮位置并不能完全反映環(huán)境信息,我們引入相對速度斥力場函數(shù):Urepvi=b|∥Vobsi(t)-V(t)∥sinθ|=bVor(t)|sinθ|(8)Urepvi=b|∥Vobsi(t)?V(t)∥sinθ|=bVor(t)|sinθ|(8)其中Vor表示障礙物相對于機(jī)器人的速度大小,θ表示機(jī)器人相對于障礙物的位置矢量與相對速度矢量的夾角,如圖3所示.我們對相對位置斥力場函數(shù)求位置微分得到相對位置斥力的函數(shù):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)為了討論方便起見,我們以障礙物相對于機(jī)器人的速度方向作為x軸,障礙物的位置作為坐標(biāo)原點(diǎn),建立平面直角坐標(biāo)系,如圖3所示,記機(jī)器人相對于障礙物的位置坐標(biāo)為(Xro,Yro),θ的取值為(-π,π),則速度斥力場函數(shù)可以寫為:Urepvi=bVor(t)|sinθ|=bVor(t)|Yro|√X2ro+Y2ro(10)Urepvi=bVor(t)|sinθ|=bVor(t)|Yro|X2ro+Y2ro√(10)定義速度斥力函數(shù)為速度斥力場函數(shù)的位置微分,則:Frepvi=▽p?UrepviFrepvi=▽p?Urepvi當(dāng)θ>0時(shí)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)當(dāng)θ<0時(shí)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)當(dāng)θ=0時(shí)斥力函數(shù)不能確定方向,因此需要根據(jù)不同的情況來做補(bǔ)充定義,本文在θ=0時(shí)按(11)～(14)式進(jìn)行計(jì)算.當(dāng)cosθ<0時(shí)機(jī)器人向著遠(yuǎn)離障礙物的方向運(yùn)動(dòng),此時(shí)即便沒有速度斥力也不會(huì)發(fā)生碰撞,因此我們在這種情況下令相對速度斥力為0.當(dāng)機(jī)器人距離障礙物比較遠(yuǎn)時(shí),障礙物就威脅不到機(jī)器人了,因此障礙物對機(jī)器人的斥力有一定的作用范圍,超過這個(gè)范圍,斥力就不起作用了.我們以一定半徑的圓來表征斥力的作用范圍.基于以上討論,第i個(gè)障礙物的相對位置斥力場函數(shù)寫為: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)相對速度斥力場函數(shù)寫為: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)相對位置斥力函數(shù)寫為: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)相對速度斥力函數(shù)寫為: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)在引入了相對速度斥力場之后機(jī)器人在人工勢場中所受的斥力為:Frep=Ν∑i=1(Freppi+Frepvi)(19)其中N為障礙物的個(gè)數(shù).4.2機(jī)器人與藻類相對速度斥力比m斥力場的作用范圍是局部的,位置斥力場和速度斥力場可以具有不同的作用范圍.相對位置斥力使得機(jī)器人快速遠(yuǎn)離障礙物,其大小與機(jī)器人到障礙物距離成反比,方向?yàn)橛烧系K物指向機(jī)器人.相對速度斥力垂直于機(jī)器人與障礙物的相對位置方向,當(dāng)夾角θ較小時(shí),速度斥力幾乎垂直于相對速度的方向,能夠使機(jī)器人較快地脫離障礙威脅.機(jī)器人與障礙物的相對位置越近,速度斥力越大;相對速度越大,斥力越大.a、b為權(quán)重系數(shù),調(diào)整其大小可以調(diào)整相對位置斥力和相對速度斥力在斥力中的權(quán)重.4.3考慮污染物的情況傳統(tǒng)的人工勢場法中,機(jī)器人很容易陷入局部極值點(diǎn),并且經(jīng)常在障礙物附近出現(xiàn)振蕩.在引入相對速度斥力之后,雖然不能完全地消除局部極值點(diǎn),但可以使極值點(diǎn)的出現(xiàn)幾率大大降低,并且消除了機(jī)器人在障礙物附近出現(xiàn)振蕩的可能.限于篇幅,我們只考慮障礙物在機(jī)器人與目標(biāo)之間的情況,如圖5、圖6.在傳統(tǒng)的勢場作用下,機(jī)器人向著目標(biāo)運(yùn)動(dòng),同時(shí)也向著障礙物運(yùn)動(dòng),若在機(jī)器人所受的虛力中斥力的權(quán)重比較大,則機(jī)器人到達(dá)不了目標(biāo)就會(huì)被斥力推向遠(yuǎn)處,若斥力所占的權(quán)重比較小,則機(jī)器人在慣性和所受引力作用下會(huì)撞上障礙物.一般的解決辦法是構(gòu)造出虛擬目標(biāo)點(diǎn),一步一步地將機(jī)器人引向目標(biāo).而在引入了速度斥力場函數(shù)之后,則不存在這樣的問題,機(jī)器人在新的勢場作用下能夠順利地繞過障礙