移動(dòng)機(jī)器人動(dòng)態(tài)目標(biāo)規(guī)劃研究

山 丹，胡玉蘭

(沈陽(yáng)理工大學(xué)信息科學(xué)與工程學(xué)院，遼寧沈陽(yáng)110159)

人工勢(shì)場(chǎng)法(potential field)［1］是 Khatib 于1956年提出的，其基本思想是構(gòu)造目標(biāo)位姿引力場(chǎng)和障礙物周圍斥力場(chǎng)共同作用的人工勢(shì)場(chǎng)，搜索勢(shì)函數(shù)的下降方向來(lái)尋找無(wú)碰撞路徑。人工勢(shì)場(chǎng)法以其描述簡(jiǎn)單、計(jì)算效率高、對(duì)于傳感器要求低(只需要聲納就可實(shí)現(xiàn))，得到了廣泛的研究和應(yīng)用，特別適合于機(jī)器人在未知?jiǎng)討B(tài)環(huán)境中的避碰運(yùn)動(dòng)規(guī)劃。但是由于人工勢(shì)場(chǎng)法依據(jù)的是局部環(huán)境信息，缺乏宏觀的自我調(diào)控能力，從而容易產(chǎn)生局部極小的問(wèn)題。當(dāng)機(jī)器人處于局部極小值時(shí)，停止不前或來(lái)回?cái)[動(dòng)。局部極小點(diǎn)問(wèn)題的存在，極大的限制了人工勢(shì)場(chǎng)法的推廣運(yùn)用。本文對(duì)引力函數(shù)和斥力函數(shù)做改進(jìn)避免了人工勢(shì)場(chǎng)法的這一局限性。

1 改進(jìn)的人工勢(shì)場(chǎng)法

1.1 改進(jìn)的斥力場(chǎng)函數(shù)

通過(guò)對(duì)斥力場(chǎng)函數(shù)進(jìn)行優(yōu)化，讓機(jī)器人在接近目標(biāo)點(diǎn)時(shí)，不僅引力場(chǎng)的引力不斷減小，同時(shí)斥力場(chǎng)的斥力也在不斷減小，這樣就解決了機(jī)器人目標(biāo)不可到達(dá)問(wèn)題［2］。其斥力場(chǎng)函數(shù)表達(dá)式為

其中:ε為斥力場(chǎng)的正比例系數(shù);ρ(q，qobs)為機(jī)器人的當(dāng)前位置q到障礙物的表面位置qobs的距離;ρ(q，qgoal)為機(jī)器人的當(dāng)前位置q到目標(biāo)點(diǎn)位置qgoal之間的距離;ρ0為障礙物所能影響的范圍;n為常數(shù)。

根據(jù)公式(1)，當(dāng)機(jī)器人離目標(biāo)點(diǎn)越來(lái)越近時(shí)，雖然目標(biāo)點(diǎn)對(duì)機(jī)器人產(chǎn)生的引力越來(lái)越小，但是障礙物對(duì)機(jī)器人產(chǎn)生的斥力也越來(lái)越小，當(dāng)機(jī)器人到達(dá)目標(biāo)點(diǎn)時(shí)，機(jī)器人的引力和斥力均為零。

當(dāng)機(jī)器人不在目標(biāo)點(diǎn)時(shí)，即q≠qgoal，斥力可以表示為

其中，

i=▽?duì)?q，qobs)和 j= －▽?duì)?q，qgoal)是兩個(gè)單位向量，一個(gè)向量是由障礙物指向機(jī)器人，另一個(gè)矢量由機(jī)器人指向目標(biāo)點(diǎn)。該受力分析如圖1所示?？梢钥闯龀饬Ψ至縁rep1i讓機(jī)器人遠(yuǎn)離障礙物，分量Frep2j吸引著機(jī)器人向目標(biāo)前進(jìn)。

由公式(1)可知，當(dāng)n=0時(shí)改進(jìn)的斥力場(chǎng)函數(shù)變?yōu)閭鹘y(tǒng)的人工勢(shì)場(chǎng)函數(shù)，因此n不能取0，本文選擇n＞0。下面通過(guò)分析n的取值來(lái)說(shuō)明新的斥力場(chǎng)函數(shù)能夠保障機(jī)器人順利避開障礙物，最終到達(dá)目標(biāo)點(diǎn)。

圖1 機(jī)器人在改進(jìn)的斥立場(chǎng)函數(shù)作用下的受力分析

(1)當(dāng)0＜n＜1時(shí)

公式(1)在目標(biāo)點(diǎn)q=qgoal時(shí)，函數(shù)不可微，由公式(3)、(4)可知，當(dāng) ρ(q，qobs)＜ ρ0且 ρ(q，qgoal)≠0時(shí)，即機(jī)器人在障礙物的影響范圍之內(nèi)，但不在目標(biāo)點(diǎn)。當(dāng)機(jī)器人接近目標(biāo)點(diǎn)時(shí)［3］，ρ(q，qgoal)逐漸接近于零，從上面兩個(gè)公式可以看出，斥力函數(shù)第一個(gè)分量Frep1趨近于零，而第二個(gè)分量Frep2趨近于無(wú)窮大，這個(gè)分量是吸引機(jī)器人向目標(biāo)點(diǎn)運(yùn)動(dòng)，所以這時(shí)機(jī)器人相當(dāng)于，只受目標(biāo)點(diǎn)對(duì)它的吸引力，機(jī)器人能到達(dá)目標(biāo)點(diǎn)。

(2)當(dāng)n=1時(shí)

同樣 ρ(q，qobs)＜ ρ0且 ρ(q，qgoal)≠0，斥力兩分量為

當(dāng)機(jī)器人接近目標(biāo)點(diǎn)時(shí)，ρ(q，qgoal)逐漸為零，因此斥力第一個(gè)分量Frep1逐漸趨近于零，而第二個(gè)分量Frep2趨近于一個(gè)常量，第二個(gè)分量吸引著機(jī)器人向目標(biāo)點(diǎn)運(yùn)動(dòng)。所以當(dāng)n=1時(shí)保證了機(jī)器人向目標(biāo)點(diǎn)運(yùn)動(dòng)。

(3)當(dāng)n＞1時(shí)

斥力場(chǎng)函數(shù)在目標(biāo)點(diǎn)是可微的，隨著機(jī)器人接近目標(biāo)點(diǎn)，總的斥力趨近于零，只剩下目標(biāo)點(diǎn)對(duì)機(jī)器人的引力，同樣確保了機(jī)器人不斷地向目標(biāo)點(diǎn)運(yùn)動(dòng)并且最終能夠到達(dá)目標(biāo)點(diǎn)。

綜上所述，改進(jìn)的斥力場(chǎng)函數(shù)能夠很好地解決障礙物在目標(biāo)點(diǎn)附近的局部最小值問(wèn)題。

他是一個(gè)不知道自己有什么弱點(diǎn)的人，比如他說(shuō)話時(shí)結(jié)巴，可他自己不知道，或者說(shuō)他從來(lái)沒(méi)有承認(rèn)過(guò)這一點(diǎn)，他的妻子萍萍是一個(gè)漂亮的女人，留著很長(zhǎng)的頭發(fā)，不過(guò)大多數(shù)時(shí)間她都是把頭發(fā)盤起來(lái)，她知道自己的脖子很長(zhǎng)很不錯(cuò)，她有時(shí)候穿上豎領(lǐng)的衣服，她的脖子被遮住了大半以后，反而更加美妙了，那衣服的豎領(lǐng)就像是花瓣一樣。

1.2 基于動(dòng)態(tài)目標(biāo)的改進(jìn)引力場(chǎng)函數(shù)

傳統(tǒng)的人工勢(shì)場(chǎng)法只適合于目標(biāo)是靜態(tài)環(huán)境［4］，如果目標(biāo)是動(dòng)態(tài)的，傳統(tǒng)的人工勢(shì)場(chǎng)函數(shù)并不適合。所以本文提出一種基于目標(biāo)是動(dòng)態(tài)運(yùn)動(dòng)的改進(jìn)的引力場(chǎng)函數(shù)。為簡(jiǎn)化分析，做以下假設(shè):

假設(shè)1機(jī)器人為一質(zhì)點(diǎn)，其位置q和速率v都是已知的;

假設(shè)2目標(biāo)機(jī)器人的位置qtar和速率vtar都是已知的，且|vtar|＜ |vmax|。

通常，引力場(chǎng)被定義為一個(gè)只與機(jī)器人和目標(biāo)距離相關(guān)的函數(shù)，而且目標(biāo)被定義為空間中一個(gè)固定的質(zhì)點(diǎn)。但當(dāng)目標(biāo)在不斷移動(dòng)時(shí)，傳統(tǒng)的勢(shì)場(chǎng)函數(shù)就不能使用了，需要定義一個(gè)改進(jìn)的引力場(chǎng)函數(shù)來(lái)適應(yīng)這種情況。

改進(jìn)的引力場(chǎng)函數(shù)為

其中:q(t)和qtar(t)分別對(duì)應(yīng)t時(shí)刻圍捕機(jī)器人和移動(dòng)目標(biāo)機(jī)器人的位置;v(t)和vtar(t)分別是t時(shí)刻圍捕機(jī)器人和移動(dòng)目標(biāo)機(jī)器人的速度矢量;║qtar(t)－q(t)║是t時(shí)刻圍捕機(jī)器人與移動(dòng)目標(biāo)機(jī)器人的歐幾里德距離;║vtar(t)－v(t)║是t時(shí)刻圍捕機(jī)器人與移動(dòng)目標(biāo)機(jī)器人速度的相對(duì)值;аq和аv是正比例系數(shù);m和n是兩個(gè)正的常量。

從上式容易看出如果аv=0，m=2，那么新引力場(chǎng)函數(shù)就會(huì)退化為傳統(tǒng)引力場(chǎng)的二次函數(shù)形式

公式(8)不包括機(jī)器人與目標(biāo)機(jī)器人相對(duì)速度的信息。相應(yīng)地，傳統(tǒng)引力場(chǎng)函數(shù)的引力只被定義為相對(duì)位置的負(fù)梯度方向，同樣與機(jī)器人和目標(biāo)機(jī)器人的相對(duì)速度無(wú)關(guān)。

新引力場(chǎng)函數(shù)Uatt(q，v)是一個(gè)關(guān)于位置q和速度v的函數(shù)。因此，定義相應(yīng)的引力應(yīng)該定義為關(guān)于位置和速度的負(fù)梯度方向

式(7)中m和n的值對(duì)新引力場(chǎng)函數(shù)有很大影響，不同取值對(duì)引力場(chǎng)函數(shù)有著不同影響，因此必須選擇合理的m和n的值，下面將討論m和n的參數(shù)選取情況。

可以看出，當(dāng)0＜m＜1，在q=qtar時(shí)引力場(chǎng)函數(shù) Uatt(q，v)不可微;當(dāng) 0 ＜ n≤1，v=vtar時(shí)引力場(chǎng)函數(shù)Uatt(q，v)不可微。在對(duì)移動(dòng)目標(biāo)進(jìn)行構(gòu)建引力場(chǎng)函數(shù)時(shí)，可以將應(yīng)用情況大致分為兩類，一類可以稱為“軟圍捕”，另一類稱為“硬圍捕”。“軟圍捕”，即機(jī)器人以零相對(duì)速度接觸到移動(dòng)目標(biāo)之后以與目標(biāo)相同的速率進(jìn)行運(yùn)動(dòng)?！坝矅丁?，即機(jī)器人只考慮怎樣到達(dá)移動(dòng)目標(biāo)(q=qtar)而不考慮相對(duì)速度。

當(dāng) q≠qtar，v≠vtar時(shí)，將式(7)帶入式(10)，可得

其中，

其中，nRT是從機(jī)器人到目標(biāo)機(jī)器人的單位矢量，nVRT是圍捕機(jī)器人相對(duì)于目標(biāo)機(jī)器人的相對(duì)速度的單位矢量。引力場(chǎng)函數(shù)的引力以及機(jī)器人目標(biāo)的位置和速度關(guān)系如圖2所示。引力Fatt由兩個(gè)分量組成［5］:第一個(gè)分量Fatt1(q)將機(jī)器人向目標(biāo)吸引，縮短目標(biāo)和機(jī)器人的距離;第二個(gè)分量Fatt2(v)拉扯著機(jī)器人以相同速率運(yùn)動(dòng)。

圖2 在二維空間中的改進(jìn)引力場(chǎng)引力

從公式(12)和(13)可以看出，當(dāng)機(jī)器人接近目標(biāo)時(shí)，也就是║qtar(t)－q(t)║接近于零，即Fatt1(q)接近于零;當(dāng)機(jī)器人接近目標(biāo)點(diǎn)時(shí)，即Fatt2(v)接近與零。因此，當(dāng)機(jī)器人的位置和速度都接近與目標(biāo)時(shí)，虛擬引力Fatt也接近于零。當(dāng)圍捕機(jī)器人捕獲到目標(biāo)，然后以相同速率運(yùn)動(dòng)時(shí)，引力場(chǎng)引力為零，因此圍捕機(jī)器人可以與機(jī)器人一起運(yùn)動(dòng)。對(duì)于“軟圍捕”問(wèn)題，對(duì)m和n的選擇是必不可少的。而對(duì)于“硬圍捕”則沒(méi)有這樣的約束，只要m和n都大于零就行。

2 改進(jìn)人工勢(shì)場(chǎng)法仿真實(shí)驗(yàn)

為驗(yàn)證改進(jìn)式人工勢(shì)場(chǎng)法的有效性，本文在VisualC++6.0和Matlab7.1這兩種工具聯(lián)合開發(fā)的仿真平臺(tái)上進(jìn)行仿真實(shí)驗(yàn)。仿真結(jié)果如圖3～6所示。

圖3 圍捕機(jī)器人穿越障礙物

圖4 偵測(cè)到入侵機(jī)器人

圖5 對(duì)入侵機(jī)器人進(jìn)行圍捕圖

圖6 圍捕機(jī)器人成功圍捕入侵機(jī)器人

在靜態(tài)復(fù)雜環(huán)境下，通過(guò)給定圍捕機(jī)器人的起始點(diǎn)以及動(dòng)態(tài)目標(biāo)起始點(diǎn)和運(yùn)動(dòng)軌跡，驗(yàn)證算法的可行性。機(jī)器人圓形的全方位移動(dòng)機(jī)器人，機(jī)器人的偵查周期大于其運(yùn)動(dòng)周期，機(jī)器人僅依靠自身的傳感器進(jìn)行環(huán)境探測(cè)，探測(cè)范圍是機(jī)器人自身半徑的5倍。機(jī)器人在探測(cè)范圍外時(shí)，避障運(yùn)動(dòng)見(jiàn)圖3。進(jìn)入探測(cè)范圍時(shí)，機(jī)器人發(fā)現(xiàn)目標(biāo)，并靠攏且跟蹤動(dòng)態(tài)目標(biāo)點(diǎn)見(jiàn)圖4、圖5。圖6為入侵機(jī)器人停止運(yùn)動(dòng)，表明圍捕機(jī)器人成功圍捕入侵機(jī)器人。

3 結(jié)束語(yǔ)

通過(guò)對(duì)傳統(tǒng)的人工勢(shì)場(chǎng)法的引力函數(shù)和斥力函數(shù)做改進(jìn)，有效解決了機(jī)器人狹長(zhǎng)通道中無(wú)法找到路徑、在半封閉空間中不斷震蕩等問(wèn)題和無(wú)法跟蹤動(dòng)態(tài)目標(biāo)問(wèn)題。仿真實(shí)驗(yàn)結(jié)果表明了該改進(jìn)人工勢(shì)場(chǎng)法在機(jī)器人路徑規(guī)劃中的有效性。

［1］譚民，王碩，曹志強(qiáng).多機(jī)器人系統(tǒng)［M］.北京:清華大學(xué)出版社，2005:7－57.

［2］賈潤(rùn)亮.多機(jī)器人系統(tǒng)路徑規(guī)劃研究［D］.太原:太原理工大學(xué)，2008:2－27.

［3］陳剛，沈林成.復(fù)雜環(huán)境下路徑規(guī)劃問(wèn)題的遺傳路徑規(guī)劃算法［J］.機(jī)器人，2001，23(1):40 －44.

［4］方京華，魏然.人工勢(shì)場(chǎng)法在多機(jī)器人運(yùn)動(dòng)中的研究［J］.控制工程，2007，14(2):115 －117.

［5］S.S.GE，Y.J.CUI.Dynamic Motion Planning for Mobile Robots Using Potential Field Method［J］.Autonmous Robots，2002，13(3):207－222.