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

耿雙樂 管 啟

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

1 引言

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

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

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

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

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

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

此時，所受到的斥力如式（6）:

3 改進算法

3.1 引力方向模型

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

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

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

3.2 斥力方向模型

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

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

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

根據(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)注的夾角。夾角的大小則由各分力求得，夾角也可描述出合力在運動空間中的具體方向狀態(tài)。通過各合力分量之可獲得移動機器人所受合力的方向。此時可以根據(jù)分量求得合力角來表示圖中所標(biāo)注的四個角度。合力角表示如下:

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

4 勻速環(huán)境下規(guī)劃運動

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

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

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

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

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

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

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

5 仿真實驗

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

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

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

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

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

6 結(jié)語

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