基于狀態改變的機器人路徑規劃算法

陳麒杰，晉玉強，王陶昱

(海軍航空大學岸防兵學院，煙臺 264000)

0 引言

隨著科學技術的不斷進步和制造工藝的提高，機器人在軍事[1]、運輸[2]、救援[3]、商業、娛樂和農業[4]等領域的應用越來越廣泛。機器人在執行任務過程中，合理的規劃路徑是實現有效避障和快速到達目標點的基本要求，也是高效完成任務的前提條件。因此，對于機器人的路徑規劃研究越來越成為目前研究的熱點之一。

目前，機器人路徑規劃算法主要有以A*算法[5]、路線圖法[6]、細胞分解法[7]和人工勢場法[8]為代表的經典路徑規劃算法，以及以遺傳算法[9]、粒子群算法[10]、蜂群算法[11]和布谷鳥算法[12]為代表的智能路徑規劃算法。經典算法在面對多障礙物、規劃面積較大時，其計算量顯著增加，解算速度明顯降低，導致時效性降低；智能規劃算法對于任務規劃區域的信息獲取度要求高，并需要多次學習和模擬，很難實現在未知環境中的路徑規劃，降低了路徑規劃的使用范圍，減小了能夠使用智能算法的任務區域。而人工勢場法作為一種簡單高效的經典路徑規劃方法，得到了廣泛的研究與應用。文獻[13]中將人工勢場法改進后應用于機器人的路徑規劃，該方法的優點是在提高時效性的同時，有效降低了路徑規劃的難度。文獻[14]中通過建立動態調節因子改進人工勢場法中的斥力場，實現無人機逃離極小值點，從而到達預定目標，但需要計算的候選路徑多，降低了時效性和全局性。文獻[15]中利用流函數法對人工勢場進行拓展，利用沿墻隨機行走和改變目標引力的方法避免滯點和目標不可達問題，但該算法在設置虛擬障礙物時，需要預先獲得區域內的障礙物信息，降低了實時性。文獻[16]中利用神經網絡的自學能力改進人工勢場法的系數選擇，以實現逃離局部極小值，但該方法的準確程度依賴于樣本的數量和訓練次數，對于樣本較少、學習量低的規劃區域，很難計算出有效的自適應系數，導致避障失敗。文獻[8]中利用搜索算法改進人工勢場法，以避免無人機陷入極小值點，但在算法運算過程中，需要對極小值點內的障礙物進行全局搜索，降低了算法的時效性。

本文針對上述問題，參考電路中理想電感元件能夠在直流電壓變化后，由感抗狀態恢復到穩定狀態的現象，提出了一種基于狀態改變的機器人路徑規劃算法。以機器人速度和方向為基礎，建立機器人狀態改變檢測量，利用克羅內克函數實現對機器人速度狀態在不同時刻的對比，確定避障時機和避障策略，從而實現機器人避障，最終到達目標點。仿真結果表明，該方法能夠成功有效地實現機器人的自主避障，同時規劃出符合條件設定的路徑。

1 基于狀態改變的路徑規劃算法基礎

設機器人所在平面為xoy平面，空間中任一點的向量可以表示為P=(x,y)，無人機器人起始點的向量為P0=(x0,y0)，在任意時刻的位置為Pt=(xt,yt)，目標點位置向量為Pg=(xg,yg)。

本文中為了簡化模型條件，對機器人和障礙物作出如下假設：

1)平面內障礙物根據其最大半徑，膨脹為圓形障礙物，則圓形障礙物模型可表示為

(1)

其中，Pobsn=(xobsn,yobsn)為第n個障礙物中心點向量；rn為障礙物半徑。

2)機器人運動時，可以實時獲得當前時刻的位置、速度和速度方向。

4)假設機器人的質量為1，直徑為dU，行進速度為固定的巡航速度VR，且滿足VR≤Vmax，其中Vmax為機器性能決定的最大巡航速度。

2 基于狀態改變的二維路徑規劃

在二維平面中，將機器人向目標點行進作為系統的穩定狀態，每一個障礙物作用的疊加則是對該穩定狀態改變的激勵狀態。類比電路中理想電感元件在電源電壓發生改變后，當電源電壓穩定時，電感元件兩端電壓能夠從感抗狀態恢復到初始狀態的現象。因此，提出了基于狀態改變的二維路徑規劃算法。

假設機器人在場內任意一點受到向量場的作用向目標點前進，得到機器人在任意一點由該點向目標點的方向向量，即機器人初始的穩定狀態為

(2)

當平面場內沒有障礙物時，機器人將沿著由起點到終點的直線向量場前進，根據機器人本身的性能確定速度。

當平面內存在障礙物時，機器人為了盡快恢復到初始的運動狀態，會選擇路徑較短的方向，沿著障礙物邊緣的切線與目標點方向夾角小于90°的方向，通過改變機器人的前進方向，達到避障的目的。由機器人和障礙物中心所在點可得障礙物方向向量為

(3)

根據式(3)得障礙物切向量為

(4)

引入克羅內克函數[17]，該函數滿足

(5)

根據式(2)、式(4)和式(5)可得單個障礙物對機器人的作用力為

Fobsn=

(6)

對于多障礙物存在的行進平面內，定義第n個障礙物的影響系數為

(7)

根據式(7)可以確定，通過改變障礙物作用半徑，可以改變機器人避障時機，確保機器人的避障成功。根據式(6)和式(7)可得N個障礙物對機器人的作用合力為

(8)

則機器人的加速度變化量為

(9)

定義t時刻機器人的速度向量為

(10)

根據式(2)和式(9)可得機器人在障礙物作用下的中間變化向量為

VR+aobsN·Δt

(11)

根據機器人性能得最大轉角為α，定義矩陣T為機器以最大轉角逆時針轉向的旋轉矩陣

(12)

由式(12)可得，矩陣T作用時，機器人將按照最大轉角逆時針旋轉。

綜上可得，機器人在t+1時刻的速度方程為

vuavt+1=(vF+vuavt·δ(〈vF,vuavt〉)·

(13)

通過對速度量的積分，可以得到機器人的下一個路徑點為

Pt+1=Pt+vuavt+1·Δt

(14)

式中，Δt為計算步長。通過迭代得到每一個步長下的位置點，將每一個路徑點相連，可以得到機器人前進的最終路徑。

3 避障分析

3.1 單個障礙物避障分析

單個障礙物時，選取圓形障礙物的圓心位置為Pobs=(a,b)，半徑為r，斥力作用半徑為R，單個障礙物模型可化為

Γ：(x-a)2+(y-b)2=r2

(15)

根據式(13)可得，機器人運動分為2個狀態，即直線運動狀態和避障狀態。

設機器人當前坐標為Pt=(xt,yt)，則直線前進階段

(16)

當機器人首次進入障礙物作用范圍內時，其路徑規劃方程為

(17)

機器人在結束初次進入障礙物作用范圍的運動后，其運動方程為

(18)

機器人避障成功，脫離避障區域后，其運動方程為

(19)

由以上公式可得，機器人在避障前后的狀態沒有改變，只有當進入到避障區域內，受到障礙物影響，狀態發生改變的情況下，按照避障策略實現避障。

3.2 目標不可達問題改進

針對目標不可達問題，對避障過程中的障礙物系數添加角度和障礙物間距的判定條件，對不滿足條件的障礙物，其作用系數為0 ，根據式(1)可得，在考慮機器人直徑的情況下，障礙物方程可以等效為

Γ(P)：(x-xobsn)2+(y-yobsn)2=(rn+dU)2

(20)

增加判定條件Q

(21)

根據式(7)和式(21)可得,改進后的障礙物作用系數為

(22)

根據式(22)，當目標點附近存在障礙物時，機器人進入障礙物作用區域后，該障礙物作用系數為0，因此機器人能夠有效到達目標點。

3.3 滯點問題改進應對策略

利用機器人對障礙物之間距離的實時檢測，將滯點問題分兩種情況討論。

情況一：障礙物間距小于機器人直徑的情況

當機器人運行至對稱障礙物時，因為相同障礙物系數的作用，使得機器人避障失敗(如圖1所示)。

圖1 極小值點示意圖(情況一)Fig.1 Schematic diagram of minimum point (Case 1)

當機器人運行至圖1所示位置時，障礙物產生的作用向量可由式(9)和式(13)計算得到

vuavt=

(23)

根據式(23)可得，當障礙物間距小于機器人直徑時，機器人將按照最大轉角實現避障。

情況二：障礙物間距大于機器人直徑的情況

當障礙物間距大于機器人直徑時(如圖2所示)，引入作用向量為0的條件，可以實現快速路徑規劃，以較少的路徑點到達目標點，避免陷入陷阱。

圖2 極小值點示意圖(情況二)Fig.2 Schematic diagram of minimum point (Case 2)

4 數值仿真

根據以上理論推導，在本算法中，主要是對機器人的狀態進行檢測，包括速度的方向和大小，根據其速度相較于初始狀態的不同，得到算法中下一步的執行狀態。算法的流程圖如圖3所示。

圖3 基于狀態改變的機器人路徑規劃算法流程圖Fig.3 Flow chart of robot path planning algorithm based on state change

4.1 仿真環境構建

利用MATLAB按照以上流程針對不同情況對算法進行仿真，仿真條件如表1所示。

表1 仿真條件

4.2 仿真結果分析

當傳統人工勢場法遇到滯點時，如圖4和圖5所示，軌跡點會在障礙物附近徘徊，直到耗盡設定的最大步數為止；當目標點附近存在障礙物時,如圖6所示，機器人會在目標點附近徘徊，直到走完所有的規劃步數，不能到達目標點。

圖4 人工勢場法仿真圖(情況一)Fig.4 Simulation diagram of artificial potential field method (Case 1)

圖5 人工勢場法仿真圖(情況二)Fig.5 Simulation diagram of artificial potential field method (Case 2)

圖6 人工勢場法仿真圖(目標不可達)Fig.6 Simulation diagram of artificial potential field method (Target unreachable)

相同環境下，采用本文算法進行規劃，針對以上問題，均成功規劃出一條合理的路徑軌跡，如圖7和圖8所示。

圖7 基于狀態改變的路徑規劃算法仿真圖(情況一)Fig.7 Simulation diagram of path planning algorithm based on state change (Case 1)

圖8 基于狀態改變的路徑規劃算法仿真圖(情況二)Fig.8 Simulation diagram of path planning algorithm based on state change (Case 2)

由圖7和圖8可以看出，機器人能夠避免人工勢場法中的極小值點，并且能夠按照不同情況實現避障。當障礙物間距小于機器人直徑時，機器人按照最大轉角實現避障；當障礙物間距大于機器人直徑時，從障礙物中間穿過，以降低路徑規劃計算量；在目標點附近存在障礙物時，不受障礙物影響，能夠到達目標點。

5 結論

本文借鑒電場出現外界激勵后恢復到穩定狀態的現象，提出了一種基于狀態改變的機器人二維路徑規劃算法，主要完成以下工作：

1)引入機器人狀態檢測中間量，實現機器人避障時機和避障策略的選擇；

2)引入障礙物作用條件，當滿足障礙物作用條件時，機器人狀態受到障礙物影響，避免人工勢場法中的極小值點以及目標不可達問題；

3)引入克羅內克函數，實現對最大轉角矩陣作用條件的判斷，避免障礙物間距小于機器人直徑時產生極小值點；

4)文中圖7和圖8的路徑規劃時間分別為23.76s和16.57s，路徑長度分別為3527.334m和2918.546m，最大轉角分別為75°和43°。從時間上看，本文算法能夠保證路徑規劃的時效性；從路徑長度和最大轉角看，本文算法不能夠保證得到的規劃路徑是最優路徑，并且沒有充分考慮機器人實際的物理性能。

通過虛擬環境的仿真實驗，驗證了本文算法的有效性和優越性。為下一步移動障礙物避障和路徑規劃研究，以及移動機器人平臺上的實際應用奠定了理論基礎。下一步的工作將進一步對算法進行研究，拓展算法適用的避障條件，以及對算法規劃路徑的平滑，降低對機器人本身條件的約束。