基于改進人工勢場法的移動機器人局部路徑規劃

張國勝，李彩虹，張耀玉，李永迪，周瑞紅

(山東理工大學 計算機科學與技術學院，山東 淄博 255049)

路徑規劃技術作為實現移動機器人自主導航的核心技術受到廣泛關注,其目的是使移動機器人按照某些特定的評價標準規劃出一條從起點到目標點的最優或次優的避障路徑[1]。根據對工作環境信息的理解層次,路徑規劃可分為全局路徑規劃和局部路徑規劃[2]。全局路徑規劃應用在工作環境已知的場景下,是一種離線規劃方法,實時性和靈活性較差,可以規劃出全局最優或次優的路徑。局部路徑規劃應用在工作環境未知或部分已知的場景下,移動機器人根據局部環境信息進行在線路徑規劃,具有較高的實時性和靈活性;由于缺少全局環境信息,傳感器的探測范圍有限,所以規劃路徑一般是局部最優。常見的路徑規劃方法有A*算法[3]、蟻群算法[4]、動態窗口法[5-6]、人工勢場法[7-8]和模糊控制法[9]等。其中,人工勢場法由于結構簡單、計算量小、實時性好且路徑較為平滑等優點得到了廣泛應用,但是存在局部極小值以及目標不可達等問題。

針對傳統人工勢場法存在的局部極小值和目標不可達等問題,國內外學者做了大量的研究工作。林潔等[10]提出沿邊走的詳細策略來解決局部極小點,但在陷入型障礙物以及復雜障礙物環境下會增加路徑長度;黃開啟等[11]設置模糊控制模塊為移動機器人提供輔助作用力以擺脫局部極小值點,但在虛擬力引導過程中路徑容易出現連續振蕩;Fan等[12]將人工勢場法與正六邊形引導方法相結合,通過繞行障礙物克服局部極小值,但在復雜障礙物環境中無疑加大了計算時間和耗能;Zhou等[13]提出在人工勢場法中構建基于障礙物的切向量作為避障過程的虛擬力,使移動機器人逃脫局部極小值點,但未有效解決引力過大和障礙物環境誤判問題;趙炳巍等[14]通過模擬退火算法,在移動機器人關于障礙物右對稱點區域設置隨機目標以擺脫局部極小值陷阱,但未充分考慮當前障礙物信息,算法效率無法保證;羅強等[15]采用切線法以及搜索法得到最優逃逸力,移動機器人在新的合力作用下擺脫局部極小值點,但應用場景簡單,面對復雜障礙物環境時無法有效逃出陷阱區域。

針對上述算法的不足,本文在已改進勢場函數的基礎上,設計自適應速度調節機制,提高路徑規劃效率。針對局部極小值問題,分別提出了APF-v1算法和APF-v2算法。通過對當前障礙物環境判斷,兩種算法采取不同的虛擬目標點設置策略打破受力平衡狀態,引導移動機器人擺脫局部極小值陷阱。最后利用ROS機器人操作系統對APF-v1和APF-v2算法進行可行性的驗證。

人工勢場法是由Khatib于1986年提出的一種虛擬力法[16],其基本思想是在移動機器人的工作環境中構造一個虛擬力場,目標點產生引力勢場,障礙物產生斥力勢場,移動機器人在引力場和斥力場的共同作用下向目標點移動。

人工勢場的勢場函數U(X)定義:

U(X)=Uatt(X)+Urep(X),

(1)

式中:Uatt(X)為目標點產生的引力勢場;Urep(X)為障礙物產生的斥力勢場;X為移動機器人的當前位置。

人工勢場中機器人所受的力F(X):

F(X)=Fatt(X)+Frep(X),

(2)

式中:Fatt(X)為目標點對移動機器人的引力;Frep(X)為障礙物對移動機器人的斥力。

1.1 引力場

引力場是全局勢場,引力勢能與移動機器人和目標點之間距離成正比,距離越大,移動機器人所受的引力勢能越大。引力勢場函數定義:

(3)

式中:ε為引力增益系數;Xg為目標點的位置;ρ(X,Xg)為移動機器人到目標點的距離。

目標點對移動機器人的引力為引力勢場函數的負梯度:

Fatt(X)=-?Uatt(X)=-ε(X-Xg)。

(4)

1.2 斥力場

斥力場是局部勢場,在障礙物影響范圍內,斥力勢能與移動機器人和障礙物之間的距離成反比,距離越大,所受的斥力勢能越小,在障礙物影響范圍外,移動機器人所受的斥力勢能為零。斥力勢場函數定義:

0,ρ(X,Xobs)>ρ0,

(5)

式中:η為斥力增益系數;Xobs為障礙物的位置;ρ(X,Xobs)為移動機器人到障礙物的距離;ρ0為障礙物的影響范圍。

障礙物對移動機器人的斥力為斥力勢場函數的負梯度:

Frep(X)=

0,ρ(X,Xobs)>ρ0。

(6)

1.3 合力

人工勢場為引力場和斥力場的疊加,移動機器人所受的合力為引力與斥力的矢量和,合力的方向即為移動機器人運動的方向。移動機器人受力分析如圖1所示。

圖1 移動機器人受力分析

移動機器人在合力的作用下向目標點移動,但是存在以下問題:

1) 引力過大問題。當移動機器人與目標點之間的距離過大時,其所受的引力將遠大于斥力,在移動過程中可能會出現碰撞障礙物的情況。

2) 目標不可達問題。當目標點位于障礙物附近時,隨著移動機器人不斷向目標點靠近,其所受的引力逐漸減小,斥力逐漸增大,斥力過大,引力相對較小,導致移動機器在某一區域徘徊或靜止。

3) 局部極小值問題。當移動機器人運動到某一非目標點位置時,其所受合力為零或引力與斥力方向相反而陷入局部極小值陷阱,移動機器人無法確定下一步的運動方向,導致其在局部極小值點附近反復震蕩或靜止。

2.1 改進的引力勢場

針對移動機器人距離目標點過遠導致的引力過大問題,本文對引力勢場函數進行修正,設置引力作用閾值將其分段,當距離大于此閾值時,采用修正的引力勢場函數,避免引力過大問題。修正的引力勢場函數:

Uatt(X)=

(7)

式中dg為引力作用閾值。

目標點對移動機器人的引力為引力勢場函數的負梯度:

Fatt(X)=

(8)

2.2 改進的斥力勢場

針對目標不可達問題,本文引入一種改進的斥力勢場函數,相較于傳統的斥力勢場函數,改進的斥力勢場函數加入了目標點和移動機器人之間的距離,保證僅目標點處的勢場為全局最小。改進的斥力勢場函數:

Urep(X)=

(9)

斥力為改進的斥力勢場函數的負梯度:

Frep(X)=

(10)

式中Frep1(X)和Frep2(X)分別為斥力Frep(X)在兩個不同方向上的分力。矢量Frep1(X)的方向由障礙物指向移動機器人,為斥力分量;矢量Frep2(X)的方向由移動機器人指向目標點,為引力分量。

(11)

(12)

改進勢場函數后移動機器人受力分析如圖2所示。

圖2 改進勢場函數后移動機器人受力分析

2.3 自適應速度調節

傳統人工勢場法進行路徑規劃任務時線速度采用固定值。結合實際工作經驗,根據移動機器人工作環境的復雜程度,提出了一種自適應速度調節機制,以降低發生碰撞的概率。

移動機器人線速度v的取值取決于當前所處的環境。移動機器人通過自身搭載的傳感器獲取周圍環境信息,通過傳感器返回的數據得到周圍障礙物的數量obs_count以及移動機器人與障礙物之間的距離dis_r_o,由此判斷當前工作環境的復雜程度。當移動機器人處于簡單環境時,可適當增大線速度,提高路徑規劃效率;當移動機器人處于復雜環境時,適當減小線速度,降低發生碰撞的概率;當移動機器人陷入局部極小值陷阱時,則需進一步降低移動機器人運動速度,防止在虛擬目標點引導過程中出現震蕩。線速度v取值如下:

(13)

式中:Nmax和Nmin分別為障礙物數量最大預設值和最小預設值;Dmax和Dmin分別為移動機器人與障礙物之間距離最大預設值和最小預設值;vmax、vmid、vmin和vmmin分別表示線速度取得的較大、中等、較小和最小值。

2.4 虛擬目標點法

移動機器人在未知環境中依靠局部地圖信息進行路徑規劃,當遇到局部極小值問題時,僅靠對勢場函數進行改進無法有效解決。典型的局部極小值情況如圖3所示。當移動機器人處于障礙物影響范圍邊緣,其所受到的斥力較小,合力方向由移動機器人指向目標點;當靠近障礙物時,其所受到的斥力大于引力,合力方向反轉,移動機器人陷入局部極小值陷阱,表現為在該區域反復震蕩或靜止。

圖3 局部極小值情況

局部極小值問題是由移動機器人所受合力為零達到平衡狀態所致,本文分別提出APF-v1和APF-v2兩種設置虛擬目標點的方法打破該平衡狀態,引導移動機器人擺脫局部極小值陷阱。在設置虛擬目標點時,兩種算法都需要對周邊的障礙物環境進行檢測,選擇障礙物數量少的區域設置虛擬目標點。其中,APF-v1算法在斥力分量偏轉90°方向的合適位置設置虛擬目標點,APF-v2算法則將虛擬目標點設置在目標障礙物外側的合適位置。

2.4.1APF-v1

移動機器人的工作環境復雜多樣,虛擬目標點的設置位置也直接影響規劃路徑的優劣,在設置虛擬目標點時應充分利用傳感器感知到的環境信息。當系統檢測到移動機器人陷入局部極小值陷阱時,通過直線連接移動機器人與目標點,該直線將移動機器人的工作環境分為兩部分,分別計算兩區域障礙物的數量,虛擬目標點設置在障礙物數量少的區域。

移動機器人通過傳感器反饋的數據計算得到各個力的大小和方向。以移動機器人為圓心沿斥力分量Frep1向虛擬目標點設置區域偏轉90°,沿此方向選取合適位置設置虛擬目標點,期間忽略原目標點的引力。為避免忽略障礙物斥力所導致移動機器人與障礙物發生碰撞的問題,需保留障礙物產生的斥力。移動機器人在虛擬目標點產生的引力和障礙物產生的斥力共同作用下逃離局部極小值點。生成虛擬目標點的位置(xvir,yvir):

(14)

式中:(xcur,ycur)為移動機器人的當前位置;ρvir為設置虛擬目標點的距離;θ(Frep1)為斥力分量Frep1相對地圖坐標系的角度;Ovir為生成虛擬目標點的方向,Ovir為1表示虛擬目標點生成在移動機器人的左側區域,Ovir為-1表示虛擬目標點生成在移動機器人的右側區域。APF-v1算法設置虛擬目標點后的移動機器人受力分析如圖4所示。

圖4 APF-v1移動機器人受力分析

2.4.2 APF-v2

APF-v1算法分析當前受力情況,通過在斥力分量Frep1偏轉90°方向上選擇合適距離設置虛擬目標點,APF-v2算法則根據目標障礙物分布情況設置虛擬目標點。

當系統檢測到移動機器人陷入局部極小值陷阱時,設置障礙物存儲模塊,記錄使其陷入局部極小值陷阱的障礙物位置信息,形成局部極小值障礙物組,在系統判斷的虛擬目標點設置區域選擇障礙物組中最外側的為目標障礙物,虛擬目標點設置在移動機器人當前位置相對于目標障礙物的對稱點附近范圍內。忽略原目標點引力,移動機器人在虛擬目標點產生的引力和障礙物產生的斥力共同作用下擺脫局部極小值點。生成的虛擬目標點位置(xvir,yvir):

(15)

式中:(xobs,yobs)為目標障礙物的位置;β1和β2為距離調節參數,取值為正。APF-v2算法設置虛擬目標點后的移動機器人受力分析如圖5所示。

圖5 APF-v2移動機器人受力分析

虛擬目標點采用一步一設的原則。移動機器人在擺脫局部極小值陷阱的過程中可能會設置多個虛擬目標點,直至脫離局部極小值點的影響范圍。移動機器人擺脫局部極小值陷阱后撤銷虛擬目標點,在原目標點和障礙物的共同作用下繼續移動。

本文使用的兩輪差速移動機器人模型是TurtleBot3系列的Burger版本,運動學模型如圖6所示。其中左右兩側是驅動輪,分別由兩個電機驅動;前后的萬向輪是從動輪;ICR表示瞬時旋轉中心;rrob表示移動機器人轉向半徑;drob表示移動機器人直徑;vl和vr分別表示移動機器人左輪和右輪的線速度。

圖6 移動機器人運動學模型

運動參數包括線速度v和角速度w,其中線速度v∈[0, 0.22],單位為m/s;角速度w∈[-2.84, 2.84],單位為rad/s。通過調節左右驅動輪速度來控制移動機器人運動。移動機器人的線速度與角速度和左右驅動輪速度之間的關系:

(16)

TurtleBot3移動機器人通過自身搭載的激光雷達獲取周圍環境信息。傳感器探測范圍為360°,探測距離為0.12～3.5 m,每15°返回一條激光雷達數據di(i=0～23),di表示該方向上最近障礙物的距離信息。移動機器人激光雷達模型如圖7所示。

圖7 移動機器人激光雷達模型

為驗證本文所提方法的有效性,在ROS機器人操作系統中利用Gazebo物理仿真平臺進行仿真環境搭建,在離散型障礙物、一型障礙物、U型障礙物及混合障礙物環境中分別對傳統的人工勢場法和改進的人工勢場法進行對比實驗。實驗所采用的計算機配置是:Ubuntu 16.04操作系統,ROS Kinetic機器人操作系統,處理器為Intel i3-4160,運行內存4 GB,主頻3.6 GHz。

為更直觀地觀察仿真結果,將仿真環境以及移動機器人運行軌跡實時顯示在Rviz可視化工具中。其中,移動機器人初始位置為起點,方框為目標點,圓柱體為障礙物,實線為移動機器人運行的軌跡,方格地板邊長為1 m。仿真過程算法主要參數設定見表1。

4.1 離散型障礙物環境下的仿真

Gazebo中離散稀疏障礙物環境和移動機器人如圖8(a)所示,在Rviz中的投影如圖8(b)所示。其中,移動機器人初始位置為起點,起點坐標為(-2, -1),目標點坐標為(4.5, 2.0)。

表1 仿真過程算法參數設定

(a)Gazebo中的仿真環境 (b) Rviz中的投影

APF-v1和APF-v2算法僅在解決局部極小值時采取不同的虛擬目標點策略。當未遇到局部極小值問題時,兩種算法規劃的路徑相同且花費步數和耗時一致,故離散稀疏和密集障礙物環境中,將APF-v1和APF-v2算法統稱為改進的人工勢場法。傳統人工勢場法與改進的人工勢場法規劃的路徑如圖9(a)和圖9(b)所示。改進的人工勢場法采用自適應速度調節機制,根據當前工作環境的復雜程度實時對速度進行調整。在離散稀疏障礙物環境中,傳統人工勢場法完成路徑規劃任務所用步數為253;采用自適應速度調節機制時,完成路徑規劃所用步數為238,且運行軌跡相似,說明自適應調節機制能夠提高路徑規劃的效率。

(a)傳統人工勢場法路徑 (b) 改進人工勢場法路徑

在離散密集障礙物環境中調整起始位置、目標點位置以及障礙物布局如圖10(a) 所示,在Rviz中的投影如圖10(b)所示。其中,起點坐標為 (-2, -2),目標點坐標為(4.2, -2.8)。

(a)Gazebo中的仿真環境 (b) Rviz中的投影

傳統人工勢場法與改進的人工勢場法規劃的路徑如圖11(a)和圖11(b)所示。當目標點距離障礙物較近時,傳統人工勢場法出現斥力過大問題,移動機器人在目標點附近反復震蕩直至步數耗盡;改進的人工勢場法在斥力勢場函數中引入目標點與移動機器人之間的距離,保證僅目標點處的勢能為全局最小,完成路徑規劃任務所用步數為288。

(a)傳統人工勢場法路徑 (b) 改進人工勢場法路徑

4.2 一型障礙物環境下的仿真

Gazebo中的一型障礙物環境和移動機器人如圖12(a)所示,在Rviz中的投影如圖12(b)所示。其中,起點坐標為(-2.5, 0),目標點坐標為(2.5, 0)。

(a)Gazebo中的仿真環境 (b) Rviz中的投影

傳統人工勢場法、APF-v1和APF-v2算法規劃的路徑如圖13所示。僅使用傳統人工勢場法時,移動機器人陷入局部極小值陷阱,在一型障礙物附近反復震蕩,導致路徑規劃任務失敗;本文提出的APF-v1算法在斥力分量Frep1偏轉90°方向上選擇合適距離設置虛擬目標點;APF-v2算法通過對當前環境分析選擇在目標障礙物外側設置虛擬目標點,兩種算法均能使移動機器人在虛擬目標點的引導下擺脫局部極小值陷阱。

(a)傳統人工勢場法路徑 (b) APF-v1算法路徑

4.3 U型障礙物環境下的仿真

Gazebo中的U型障礙物環境和移動機器人如圖14(a)所示,在Rviz中的投影如圖14(b)所示。其中,起點坐標為(-3.5, 0),目標點坐標為(3, 0)。

(a)Gazebo中的仿真環境 (b) Rviz中的投影

傳統人工勢場法、APF-v1和APF-v2算法在U型障礙物環境中規劃的路徑如圖15所示。傳統人工勢場法使移動機器人陷入U型障礙物陷阱,在局部極小值點附近反復震蕩直至步數耗盡;APF-v1算法在擺脫U型障礙物陷阱時,由于移動機器人在U型障礙物邊緣所受斥力分量Frep1角度變化較大,虛擬目標點設置位置變化較大,導致移動機器人擺脫U型障礙物陷阱所需步數較多;而APF-v2算法通過在目標障礙物外側設置虛擬目標點的方法,能規劃出相對更優的路徑。

4.4 混合障礙物環境下的仿真

將離散型、一型和U型障礙物組合形成混合障礙物環境。Gazebo中的混合障礙物環境和移動機器人如圖16(a)所示,在Rviz中的投影如圖16(b)所示。其中,起點坐標為(-4, -2),目標點坐標為(4, 4)。

(a)傳統人工勢場法路徑 (b) APF-v1算法路徑

(a)Gazebo中的仿真環境 (b) Rviz中的投影

傳統人工勢場法、APF-v1和APF-v2算法規劃的路徑如圖17所示。由于傳感器探測范圍有限,移動機器人沿勢場的負梯度方向移動進入U型障礙物陷阱,傳統人工勢場法使移動機器人在U型障礙物陷阱區域反復震蕩,導致路徑規劃任務失敗;而APF-v1算法和APF-v2算法使移動機器人進入U型和一型障礙物陷阱時,對當前障礙物環境進行判斷,通過設置虛擬目標點的方法引導機器人擺脫局部極小值陷阱。其中,APF-v1算法在繞行障礙物邊緣時步數相對較多,APF-v2算法能規劃出相對更優的路徑。

對傳統人工勢場法、APF-v1算法和APF-v2算法在上述五種仿真環境中從起點到目標點的路徑規劃步數和時間進行匯總,見表2。

從表2數據對比可以看出,在各種仿真環境中,APF-v1算法和APF-v2算法均能規劃出從起點到目標點的路徑, 且在一型、U型和混合障礙物環境中,APF-v2算法規劃的路徑效果較好,所用的步數較少、耗時較短。

(a)傳統人工勢場法路徑 (b) APF-v1算法路徑

表2 仿真環境中算法對比數據

針對未知環境下的移動機器人局部路徑規劃問題,本文提出了APF-v1算法和APF-v2算法,并在ROS系統中設計不同的仿真環境,將本文所提算法與傳統人工勢場法進行了對比實驗,驗證了改進算法的有效性。APF-v1算法和APF-v2算法具有以下特點:

1)通過改進引力和斥力勢場函數,有效解決了引力過大和目標不可達問題;根據當前環境復雜程度設計自適應速度調節機制,提高路徑規劃的效率。

2)對存在局部極小值陷阱的復雜障礙物區域,算法對當前障礙物環境判斷,分別在斥力分量偏轉90°方向上與目標障礙物外側合適位置設置虛擬目標,引導移動機器人擺脫局部極小值陷阱。

3)當障礙物環境較為復雜時,APF-v2算法完成路徑規劃任務花費步數較少,用時較短,效率較高。

考慮到移動機器人的實際工作環境可能更為復雜,下一步工作考慮將人工勢場法與模糊控制法以及強化學習方法結合,解決移動機器人在更為復雜場景下的路徑規劃問題,并進一步縮短路徑長度,使移動機器人在各種環境下規劃出較優的路徑。