基于控制方向角改進(jìn)勢場法的移動(dòng)機(jī)器人路徑規(guī)劃?

耿雙樂 管 啟

（上海理工大學(xué)光電信息與計(jì)算機(jī)工程學(xué)院 上海 200082）

1 引言

移動(dòng)機(jī)器人的路徑規(guī)劃是智能機(jī)器人研究的熱點(diǎn)之一。路徑規(guī)劃就是機(jī)器人根據(jù)自身傳感器感知周圍環(huán)境，規(guī)劃出一條安全無碰撞的路線［1］。根據(jù)是否獲得環(huán)境信息，將在完全已知環(huán)境下的規(guī)劃稱為全局規(guī)劃。只獲得部分環(huán)境信息下的規(guī)劃稱為局部規(guī)劃。全局規(guī)劃是在完全已知的環(huán)境中進(jìn)行規(guī)劃，不具有實(shí)時(shí)性。常見的全局算法有Floyd、柵格法等。人工勢場法［2］是由Khatib提出的一種路徑規(guī)劃方法，因其優(yōu)秀的避障性能和良好的路徑平滑性，成為最常用的規(guī)劃方法之一。傳統(tǒng)勢場法引入物理上引力和斥力的概念。利用其目標(biāo)點(diǎn)對(duì)移動(dòng)機(jī)器人產(chǎn)生引力，障礙物對(duì)機(jī)器人產(chǎn)生斥力的這種方法進(jìn)行規(guī)劃。但傳統(tǒng)勢場法在目標(biāo)點(diǎn)周圍存在障礙物時(shí)，由于受到周圍障礙物斥力作用，會(huì)存在無法到達(dá)目標(biāo)點(diǎn)的情況［3］；并且容易陷入局部最優(yōu)點(diǎn)，導(dǎo)致無法到達(dá)目標(biāo)點(diǎn)。

2 傳統(tǒng)勢場原理

勢場法是將整個(gè)運(yùn)動(dòng)空間抽象成一個(gè)勢場，目標(biāo)點(diǎn)對(duì)移動(dòng)機(jī)器人會(huì)形成一個(gè)引力場，運(yùn)動(dòng)空間內(nèi)所存在的障礙物又會(huì)在一定范圍內(nèi)對(duì)機(jī)器人形成一個(gè)斥力場，機(jī)器人所在的空間即為引力場和斥力場所疊加的合力場。機(jī)器人始終沿著勢場梯度下降的方向運(yùn)動(dòng)。將斥力場的負(fù)梯度作為斥力虛擬力，引力場的負(fù)梯度作為引力虛擬力。因此，移動(dòng)機(jī)器人在空間中運(yùn)動(dòng)受到引力和斥力的合力作用。設(shè)機(jī)器人在運(yùn)動(dòng)空間中實(shí)時(shí)位置為X=[x，y]，式（1）～（2）中構(gòu)建了引力場和斥力場關(guān)于機(jī)器人位置變化的函數(shù)。

其中Uatt表示移動(dòng)機(jī)器人所受到目標(biāo)位置的引力場，katt是自定義的引力場系數(shù)， ||X-XG表示機(jī)器人與目標(biāo)點(diǎn)之間的歐幾里得距離。XG表示目標(biāo)點(diǎn)的位置。η為可自定義的斥力場系數(shù)，ρo為自定義的障礙物斥力場影響范圍，Xob為障礙物所在的空間位置。那么目標(biāo)與障礙物所產(chǎn)生的負(fù)梯度即引力和斥力如式（3）～（4）:

傳統(tǒng)勢場法將機(jī)器人與障礙物和目標(biāo)點(diǎn)的距離信息轉(zhuǎn)化為引力和斥力進(jìn)行規(guī)劃。但是當(dāng)目標(biāo)點(diǎn)周圍存在障礙物時(shí)，隨著移動(dòng)機(jī)器人靠近目標(biāo)點(diǎn)，引力會(huì)逐漸變小，斥力逐漸變大。當(dāng)所受到的斥力大于引力時(shí)，此時(shí)就會(huì)造成目標(biāo)點(diǎn)不可達(dá)的情況。

文獻(xiàn)［2］提出在斥力場函數(shù)中引入距離因子，即如式（5）所示:

此時(shí)，所受到的斥力如式（6）:

3 改進(jìn)算法

3.1 引力方向模型

在二維空間內(nèi)設(shè)置機(jī)器人起點(diǎn)位置為X0=[0，0]，機(jī)器人的實(shí)時(shí)位置為 X=[x，y]，目標(biāo)點(diǎn)位置為 XG=[xg，yg]。設(shè)坐標(biāo)正方向?yàn)榱Φ恼较?。機(jī)器人的實(shí)時(shí)位置與目標(biāo)點(diǎn)之間的位置關(guān)系為Δx=xg-x，Δy=yg-y。則機(jī)器人與目標(biāo)點(diǎn)間實(shí)時(shí)距離可為機(jī)器人引力方向根據(jù)位置參數(shù)變化，在運(yùn)動(dòng)空間中存在如圖1四種方向狀態(tài)。

圖1中四種位置關(guān)系下的引力始終與橫軸方向有如圖示夾角。角度范圍為(0，2π)。每種方向狀態(tài)下與橫軸的夾角均可使得引力的正弦分量分解到Y(jié)軸方向，引力的余弦分量分解到X軸方向。角度的余弦正負(fù)決定了引力橫向分力的正反，正弦的正負(fù)值決定了引力縱向分力的正反。因此四種位置關(guān)系下的角度可描述引力在運(yùn)動(dòng)空間中的所有方向狀態(tài)。由于正弦余弦角度的周期性，此時(shí)可以引入一個(gè)角度 θatt=sign(Δy)arcos(Δx/ratt)可統(tǒng)一表示上面的四個(gè)角度，即稱為引力角。

圖1 引力四種方向狀態(tài)

3.2 斥力方向模型

再對(duì)移動(dòng)機(jī)器人與障礙物之間的傳統(tǒng)斥力模型進(jìn)行方向狀態(tài)分析。設(shè)定障礙物的位置坐標(biāo)為Xob=[xob，yob]?？山y(tǒng)一規(guī)定移動(dòng)機(jī)器人與障礙物之間的實(shí)時(shí)位置坐標(biāo)變化的關(guān)系為Δxob=xob-x，Δyob=yob-y，則移動(dòng)機(jī)器人與障礙物間實(shí)時(shí)位置可表示為斥力方向根據(jù)移動(dòng)機(jī)器人與障礙物位置變化的關(guān)系，會(huì)存在如圖2四種方向狀態(tài)。

瀝青混凝土中的集料是可以稱作是它的骨架，集料的質(zhì)量能夠決定混凝土的整體強(qiáng)度，對(duì)混凝土的支撐能力有重要的影響，在集料的質(zhì)量控制中首先應(yīng)該保證集料的外形上接近立方體，保證其有良好的穩(wěn)定性，另外由于細(xì)集料和粗集料有不同的用處，例如細(xì)集料是為了對(duì)粗集料中間的空隙進(jìn)行填充，因此還應(yīng)該對(duì)集料的體積大小進(jìn)行嚴(yán)格的把控。

圖2 斥力四種方向狀態(tài)

根據(jù)移動(dòng)機(jī)器人與障礙物間存在的四種位置關(guān)系，可分別求出四種方向狀態(tài)下引力與橫軸方向的夾角。都可將斥力的余弦分量分解到橫軸方向。圖中所標(biāo)注的四種位置關(guān)系下的角度可以描述斥力在空間中的方向狀態(tài)。則引入θrep=pi+sign(Δxob)arcos(Δxob/rrep)作為斥力方向的斥力角，統(tǒng)一表示以上四種狀態(tài)下的角度。

3.3 合力方向模型

再將引力通過引力角分解到橫縱方向得到

橫縱坐標(biāo)方向受到引力與斥力的共同作用，也就是所受到的總合力在橫縱坐標(biāo)方向的分量為

根據(jù)橫縱方向分力存在的四種情況，則合力方向存在如圖3所示四種方向狀態(tài)

圖3 合力的四種方向狀態(tài)

圖3 中四種方向狀態(tài)下的合力與橫軸方向存在如圖所標(biāo)注的夾角。夾角的大小則由各分力求得，夾角也可描述出合力在運(yùn)動(dòng)空間中的具體方向狀態(tài)。通過各合力分量之可獲得移動(dòng)機(jī)器人所受合力的方向。此時(shí)可以根據(jù)分量求得合力角來表示圖中所標(biāo)注的四個(gè)角度。合力角表示如下:

合力角可實(shí)時(shí)反應(yīng)出合力的方向狀態(tài)。此時(shí)，對(duì)于傳統(tǒng)人工勢場虛擬力控制即可轉(zhuǎn)化為角度方向的控制。

4 勻速環(huán)境下規(guī)劃運(yùn)動(dòng)

通過如上勢場模型分析，已將勢場虛擬力模型轉(zhuǎn)化為合力角、引力角和斥力角的控制。下面通過在運(yùn)動(dòng)環(huán)境中設(shè)定勻速環(huán)境，設(shè)定移動(dòng)機(jī)器人在單位時(shí)間內(nèi)行駛固定步長L。下個(gè)時(shí)間段行駛的方向則不斷通過合力角度變化而變化。設(shè)當(dāng)前時(shí)刻移動(dòng)機(jī)器人的實(shí)時(shí)位置為Xk=[xk，yk]，下一時(shí)刻位置為 Xk+1=[xk+1，yk+1]。通過人工勢場力所轉(zhuǎn)化的角度控制，可得到下一時(shí)刻的位置為

整個(gè)運(yùn)動(dòng)規(guī)劃過程就是不斷迭代更新控制角度完成。當(dāng)前時(shí)刻移動(dòng)機(jī)器人的位置由上個(gè)時(shí)刻的合力角所決定。當(dāng)目標(biāo)點(diǎn)周圍存在障礙物時(shí)，就會(huì)出現(xiàn)傳統(tǒng)勢場所固有的缺陷即無法達(dá)到目標(biāo)點(diǎn)。

下面通過在目標(biāo)點(diǎn)周圍設(shè)定半圓閾值檢測，轉(zhuǎn)換方向控制角來解決目標(biāo)點(diǎn)不可達(dá)的問題。如圖4所示。

圖4 設(shè)定半圓閾值

假定移動(dòng)機(jī)器人從左側(cè)開始運(yùn)動(dòng)，則如圖4所示在目標(biāo)點(diǎn)為中心畫左側(cè)半圓，設(shè)定半圓檢測半徑為2L。在檢測閾值范圍之外，始終是合力角不斷牽引規(guī)劃運(yùn)動(dòng)。當(dāng)移動(dòng)機(jī)器人開始進(jìn)入閾值范圍之內(nèi)，可實(shí)時(shí)檢測閾值之內(nèi)障礙物到移動(dòng)機(jī)器人與目標(biāo)點(diǎn)之間連線的距離。將左半圓作為閾值檢測范圍，即可忽略右側(cè)障礙物到移動(dòng)機(jī)器人與目標(biāo)點(diǎn)之間連線距離的影響?？稍O(shè)定一個(gè)安全距離0.5L，如若檢測到的距離滿足安全距離要求，則轉(zhuǎn)換合力角為引力角直接導(dǎo)向目標(biāo)點(diǎn)［10］，即如下迭代所示。

如果不滿足安全距離要求，則繼續(xù)使用合力角導(dǎo)航直到檢測左邊障礙物滿足安全距離要求。

采用左側(cè)半圓檢測，滿足安全距離時(shí)，此時(shí)采用引力角牽引，也就是忽略了目標(biāo)點(diǎn)周圍障礙物斥力的影響。閾值范圍內(nèi)左側(cè)的障礙物看作移動(dòng)機(jī)器人是否滿足安全距離的影響因素。直到滿足安全距離要求時(shí)，即可忽略左側(cè)障礙物的斥力作用，直接通過變化合力角為引力角規(guī)劃的方式無碰撞地直達(dá)目標(biāo)點(diǎn)。以下通過仿真驗(yàn)證了此種方法的可行性，并且相對(duì)于原有算法和加距離因子方法均具有改良效果。

5 仿真實(shí)驗(yàn)

本次實(shí)驗(yàn)將通過閾值處理改進(jìn)的算法與傳統(tǒng)算法、加距離因子改進(jìn)的算法進(jìn)行對(duì)比。對(duì)于三種方式均設(shè)定相同系數(shù)和相同障礙物，最后用Matlab顯示三種方法的最終效果。統(tǒng)一設(shè)定引力系數(shù)為11，步長L=0.5，斥力為1，斥力影響范圍為2。如圖5～7分別表示傳統(tǒng)算法、加距離因子方法和半圓閾值處理改進(jìn)的方法在Matlab仿真下的效果圖。

圖5 傳統(tǒng)算法規(guī)劃效果圖

圖6 加距離因子算法規(guī)劃效果圖

圖7 閾值處理規(guī)劃效果圖

通過圖5、6、7中可以看出三種方法的最終規(guī)劃效果。圖5中傳統(tǒng)算法在周圍存在障礙物的情況下，未能到達(dá)目標(biāo)點(diǎn)。圖6是通過加距離因子的方法，可看出規(guī)劃的路徑相對(duì)傳統(tǒng)方法規(guī)劃的路徑較長，并且不平滑。圖7所示，是通過本次改進(jìn)算法所得到的結(jié)果，相對(duì)加距離因子方法，路徑平滑，并且規(guī)劃路程較短，又能夠在目標(biāo)點(diǎn)周圍存在障礙物的情況下，到達(dá)目標(biāo)點(diǎn)。

6 結(jié)語

本文提出的一種勻速模型下在目標(biāo)點(diǎn)周圍設(shè)定閾值的方式來解決傳統(tǒng)算法所固有的缺陷。通過將傳統(tǒng)虛擬力的控制，轉(zhuǎn)化為方向角度控制。進(jìn)而在目標(biāo)點(diǎn)周圍設(shè)定合適閾值，檢測閾值內(nèi)的障礙物到移動(dòng)機(jī)器人與目標(biāo)點(diǎn)連線之間的距離，直到滿足所設(shè)定的安全距離，則可將合力角轉(zhuǎn)化為引力角的方式進(jìn)行路徑規(guī)劃。當(dāng)移動(dòng)機(jī)器人在閾值范圍內(nèi)時(shí)，如滿足安全距離要求，則忽略了目標(biāo)點(diǎn)周圍的障礙物影響，并且可以安全到達(dá)目標(biāo)點(diǎn)。此種方法可解決傳統(tǒng)算法在目標(biāo)點(diǎn)周圍存在障礙物時(shí)，移動(dòng)機(jī)器人無法到達(dá)目標(biāo)點(diǎn)的情況。本文提出的改進(jìn)算法與加距離因子的方法相比較，既可保證規(guī)劃路徑的平滑性也可縮短路徑長度。