改進人工勢場法機器人避障路徑規劃研究

高飛翔,郝萬君,吳 宇,孫 誠

(1. 蘇州科技大學電子與信息工程學院,江蘇 蘇州 215009;2. 蘇州科技大學物理科學與技術學院,江蘇 蘇州 215009)

1 引言

隨著人工智能的快速發展,機器人技術得到進一步開發,具有各種功能的機器人開始逐漸融入到日常生活中[1]。機器人工作時都需要進行路徑規劃,路徑規劃分為全局路徑規劃和局部路徑規劃,它可以為機器人在各種復雜環境中規劃一條安全、快速的路徑。常用的路徑規劃算法有:蟻群算法(Ant Colony Optimization,ACO)、遺傳算法(Genetic Algorithm,GA)、快速搜索隨機樹法(Rapid-Exploration Random Tree,RRT)和人工勢場法(Artificial Potential Field,APF)等[1]。

APF法不同于其它的路徑規劃智能算法,APF是基于局部路徑規劃的智能算法,運用在路徑規劃中需要根據當前機器人的位姿以及距離障礙物的距離來規劃下一步動作,其算法本身具有計算量小、算法反應速度快、生成路徑順滑等優點[1]。徐小強[4]等人提出從安全距離概念避免多余路徑,引入預測距離解決目標不可達問題,在機器人陷入局部極小值時利用虛擬障礙點逃離局部極小點,但是未曾考慮復雜障礙物下機器人是否可以完整規劃路線。何乃峰[5]等人提出利用模擬退火算法重建APF函數模型,重新規劃全局路徑,雖然避免了局部極小值但是所規劃的路徑不是全局最優路徑。吳宇[6]等人提出的利用A*與DWA算法進行融合優化,提出自適應參數融合改進算法,在柵格地圖中可以有效避開障礙物建立安全且平滑的行進路線,但是這種方法的計算量大,運行緩慢,收斂精度低。楊周[7]等人提出的利用ACO與DWA算法融合,但融合的算法在面對復雜環境問題時,區域搜索最佳轉折點不完整可能會導致可行區域的收斂性低,從而影響整個路徑規劃的完整性。

針對以上TAPF應用于路徑規劃存在的弊端,提出了利用自適應切線偏轉角確定虛擬障礙點的APF算法,改進斥力函數引入機器人與目標點的動態影響距離以及調節因子系數,避免遇到目標不可達問題,利用當前機器人行徑位置提出自適應切線偏轉角法,以最遠障礙物為參照物在另一側確定虛擬障礙點,通過引入虛擬斥力解決振蕩易陷入局部最小值問題,使得機器人可以更快、平穩的到達目標點。然后通過MATLAB仿真驗證該算法的正確性,并進行了實際論證。

2 APF算法

2.1 地圖構建

ROS智能機器人進行路徑規劃前利用激光雷達對周圍復雜環境進行數據掃描,將掃描到的數據建立模擬地圖模型。本文利用柵格地圖進行復雜環境的搭建模擬,柵格地圖具有簡潔的特點,可以直觀的顯示出仿真結果,在眾多路徑規劃仿真中得到廣泛應用。

本文采用如圖1所示柵格地圖進行復雜環境復現,黑色柵格表示障礙物,白色部分表示無障礙物可以通過區域。

圖1 柵格地圖

2.2 APF算法基本原理

APF是路徑規劃中應用較廣泛的智能算法之一,最早由Khatib在80年代提出[4]。在APF算法中,整個機器人運動過程比作一顆運動粒子在帶有正負極的電勢場中運動的過程。將整個復雜的地圖環境看作一個虛擬電勢場,將機器人當作電勢場正極,目標點當作電勢場負極,障礙物視為正極,機器人利用與障礙物引起的斥力場以及與目標點產生的引力場的合力使得機器人有效地繞過障礙物最終到達目標點。

假設機器人在二維空間中的受力情況如圖2所示,且設機器人在二維空間內的位置表示為j=(x,y),目標點位置表示為jobj=(xT,yT),障礙物的位置表示為jobs=(xo,yo)。

圖2 傳統APF受力分析

設定目標點對機器人產生的引力場表示為Uatt(j),障礙物對機器人產生的斥力場表示為Urep(j),即機器人最終受到的合力場U(j),則合力如式(1)所示:

U(j)=Uatt(j)+Urep(j)

(1)

其中,引力場函數如式(2)所示:

(2)

式中,β為引力勢場增益常數,β>0;‖j-jobj‖為機器人當前位置與目標點的距離。機器人與障礙物之間的斥力勢場函數如式(3)所示

(3)

勢場力為負梯度時,機器人受到的勢場引力如式(4)所示

Fatt(j)=-?Uatt(j)=β(jobj-j)

(4)

機器人受到的勢場斥力如式(5)所示

Frep(j)=-?Uatt(j)=

(5)

則機器人受到的合力如式(6)所示

(6)

2.3 APF法的缺陷

TAPF法不同于其它智能算法,不需要進行全局路徑規劃找出全局關鍵節點,因此其行進路線都是利用障礙物斥力及目標點引力的合力來推動機器人前進。一旦途中沒有受到力的驅使或力達到平衡狀態時,就會出現以下問題:

1)由于力的不平衡才驅使機器人可以向目標點移動,一旦行徑策略中存在某點使其達到合力平衡的效果,機器人會產生目標不可達現象,如圖3所示。

圖3 傳統APF目標不可達

2)機器人行進策略位于兩個障礙物夾縫之中或遇到L型等復雜障礙物時,極易陷入徘徊抖動造成振蕩狀態,如圖4所示。

圖4 傳統APF“振蕩”

3 改進TAPF算法

3.1 改進斥力場函數

針對機器人在TAPF存在合力平衡導致目標不可達現象,引入機器人與目標點之間的動態影響距離參數,在每個障礙物斥力范圍之內,將其代入斥力場函數式

(7)

(8)

(9)

其中,Fm(j)表示機器人朝向與障礙物相反,Fn(j)表示機器人朝向與障礙物同向。

考慮改進的改進斥力函數打破力的平衡,因此引入協調力Fx,其定義為

(10)

式中,η為協調系數,η的取值范圍在0≤η≤1,一般取值0.5。由此,改進后人工勢場合力函數為

(11)

3.2 虛擬障礙點

在機器人行進策略中,遇到L型等障礙物或在障礙物中間穿行會出現振蕩情況,機器人就陷入死鎖。本文提出利用自適應偏轉角根據障礙物的分布情況引入額外虛擬障礙點,當機器人陷入振蕩情況時,利用虛擬障礙點提供的斥力使其合力發生改變推動機器人向目標點移動。在接下來行進過程中再遇此情況,重復進行以上操作,直到機器人到達目標點為止。

3.2 自適應切線偏轉角

當機器人在進行過程中遇到復雜結構障礙物時,例如U型障礙物、L型障礙物,機器人會產生振蕩狀態即陷入局部最小值中。當機器人處于振蕩狀態時,以機器人當前位置為切點,做當前振蕩軌跡的切線l1,如圖5所示;以當前切線為基準線建立坐標軸;再以機器人為圓心,距離機器人受到斥力最遠障礙物距離Ri為半徑順時針做圓;機器人與最遠障礙物之間的距離為dis(Robot,Obsm),利用式(12)計算得出自適應切線l1的偏轉角ζ的大小,將切線l1順時針旋轉偏轉角ζ得直線l2與圓相交得兩點γ1與γ2;將最遠障礙物另一側的為設定的虛擬障礙點Obsset,如圖6所示。

圖5 軌跡切線示意圖

圖6 設定模擬障礙點示意圖

自適應切線偏轉角ζ可表示為

(12)

4 改進APF算法流程

本文改進傳統APF避障算法,解決傳統APF在做局部路徑規劃時遇到的目標不可達以及振蕩現象。其具體步驟如下:

步驟 1:初始化柵格地圖及算法參數。

步驟2:確定柵格地圖中機器人的初始位置及機器人的目標點。

步驟3:利用傳統APF法計算機器人在行進過程中每一步的斥力場及引力場。

步驟4:通過機器人周圍的障礙物計算出斥力與目標點對其的引力得出合力,判斷合力是否為零,進而判斷是否遇到傳統APF法的兩大問題之一,使用改進型斥力場函數,尋找新的局部路徑;通過機器人判斷,對于仿真中出現振蕩狀態從而陷入局部最小值問題時,利用自適應偏轉角ζ找出位于最遠障礙物另一側的虛擬障礙點,添加額外斥力使得機器人擺脫局部最小值。

步驟5:通過APF法計算下一時刻機器人前進位置,最終抵達終點,如再遇上述問題返回步驟4重新找尋新的下一步路徑。

改進融合算法的機器人路徑規劃流程圖,如圖7所示。

圖7 改進APF算法的機器人避障示意圖

5 仿真及結果分析

為驗證本文所提算法的可行性以及普適性,利用MATLAB2016a仿真平臺分別在20×20、25×25、30×30三種大小的柵格地圖下進行改進后避障算法仿真。

將本文所提的改進APF算法與TAPF算法以及文獻[4]所提A-APF算法分別在三種大小的地圖中進行對比實驗。將移動機器人視為一個質點,黑色矩形代表柵格地圖中的障礙物,綠色正三角S表示起點,紅色正三角T表示目標點。主要設置參數如表(1)所示,環境一、二、三中障礙率分別為0.075、0.136、0.1444,如圖8-圖10所示。

表1 仿真參數設定

圖8 環境一下三種算法的路徑規劃圖

圖9 環境二下三種算法的路徑規劃圖

圖10 環境三下三種算法的路徑規劃圖

從圖8-圖10可以看出,在不同障礙率和不同尺寸的環境下,TAPF算法面對復雜障礙物通常會在行進途中陷入局部極小值從而走入死胡同,A-APF算法提出虛擬目標點方法以及本文所提利用自適應切線角所確定虛擬障礙點算法均能規劃出完整路徑。如表2-4所示,給出三種環境下,三種不同算法的性能參數。從表2-4可以看出:

表2 20×20復雜環境地圖中仿真數據

表3 25×25復雜環境地圖中仿真數據

表4 30×30復雜環境地圖中仿真數據

在20×20的柵格地圖環境中,TAPF算法由于復雜障礙物存在陷入局部極小值,無法規劃完整路線,而本文算法較A-APF算法在步長、時間方面分別降低了14.2%、13.1%;

在25×25的柵格地圖環境中,TAPF算法規劃出完整路線,但振蕩頻率較大,而本文算法較TAPF及A-APF算法在步長、時間方面分別降低了34.2%、26.3%,20.6%、15.8%;

在30×30的柵格地圖環境中,TAPF算法因復雜障礙物無法規劃出完整路線,而本文算法較A-APF算法在步長、時間方面分別降低了9.6%、12.5%;相同仿真環境下,利用TAPF、本文所提算法對TAPF算法所存在兩個缺陷進行對比仿真,如圖11、圖12所示。

圖11 TAP目標不可達情況及改進

圖12 TAPF陷入局部極小值情況及改進

如圖11所示,TAPF面對合力為零導致目標不可達就此算法進入死胡同,本文所提算法改進斥力場函數引入動態距離影響參數以及斥力場調節因子,避開了合力為零的狀態;如圖12所示,面對復雜障礙物例如L型障礙物等,TAPF會陷入局部極小值出現振蕩現象,TAPF又會進入死胡同,本文改進后的算法提出自適應切線偏轉角,確定合適的虛擬障礙點,從而使得機器人擺脫局部極小值,順利到達目標點。

通過上述分析可以看出本文所提算法與TAPF、A-APF算法在路徑長度、時間都有較大的優化;在路徑長度方面與A-APF算法相差不多,但本文所提算法的時間以及平滑度與A-APF算法相比有明顯提高。綜上分析,本文所提改進TAPF避障算法有進一步的改進,路徑更平滑,拐點更少,通過不同大小環境對比而言更加具有普適性。

6 實際論證

為了驗證在實際環境中本文所提算法的有效性以及可靠性,將本文所提改進TAPF避障算法應用到圖13(a)中的輪式激光雷達機器人中,進行實物論證。利用激光雷達對周圍環境進行數據掃描,如圖13(b)所示。

圖13 實際驗證環境

如圖14所示,為實驗驗證過程圖。黑色方框為輪式智能機器人,粉紅色為實際障礙物,紅色線為智能機器人局部路徑規劃路線。

圖14 輪式機器人行進策略軌跡圖

起點為圖中紅綠交叉直角處,設置好后,智能機器人向目標點移動,通過激光雷達掃描到障礙物時,通過改進算法可以成功避開障礙物,繼續向目標點前進。規避障礙物軌跡如圖14(a)-圖14(d)所示,通過圖15(a)、圖15(b)可以看出規劃局部路線時能夠與障礙物保持一定的安全距離并且避障軌跡較為平滑,符合機器人實際運行動力學特點。

圖15 輪式機器人實際避障圖

7 結論

1)針對TPF算法目標不可達情況,本文對其傳統斥力場函數進行改進,引入機器人與目標點之間影響距離以及障礙物斥力場調節因子,通過仿真與TAPF和文獻[4]所提算法A-APF進行對比,本文優化算法路徑更加平滑,用時更少。

2)針對復雜環境下TAPF會出現振蕩陷入死區現象,本文提出自適應偏轉角確定虛擬障礙點,通過額外斥力使得機器人擺脫局部極小值,繼續向目標點前進直到到達目標點。

3)本文利用ROS輪式智能機器人進行算法實際測試,測試結果顯示,在復雜環境下機器人也能夠安全,快速的進行障礙物避障直到抵達設定目標點。

綜上,本文所提避障算法能夠為智能機器人行進策略規劃好安全路線,為智能駕駛提供一定的行進決策借鑒意義。