基于狀態(tài)改變的機器人路徑規(guī)劃算法

陳麒杰，晉玉強，王陶昱

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

0 引言

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

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

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

1 基于狀態(tài)改變的路徑規(guī)劃算法基礎(chǔ)

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

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

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

(1)

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

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

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

2 基于狀態(tài)改變的二維路徑規(guī)劃

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

假設(shè)機器人在場內(nèi)任意一點受到向量場的作用向目標點前進，得到機器人在任意一點由該點向目標點的方向向量，即機器人初始的穩(wěn)定狀態(tài)為

(2)

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

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

(3)

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

(4)

引入克羅內(nèi)克函數(shù)[17]，該函數(shù)滿足

(5)

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

Fobsn=

(6)

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

(7)

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

(8)

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

(9)

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

(10)

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

VR+aobsN·Δt

(11)

根據(jù)機器人性能得最大轉(zhuǎn)角為α，定義矩陣T為機器以最大轉(zhuǎn)角逆時針轉(zhuǎn)向的旋轉(zhuǎn)矩陣

(12)

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

綜上可得，機器人在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)

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

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

(16)

當(dāng)機器人首次進入障礙物作用范圍內(nèi)時，其路徑規(guī)劃方程為

(17)

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

(18)

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

(19)

由以上公式可得，機器人在避障前后的狀態(tài)沒有改變，只有當(dāng)進入到避障區(qū)域內(nèi)，受到障礙物影響，狀態(tài)發(fā)生改變的情況下，按照避障策略實現(xiàn)避障。

3.2 目標不可達問題改進

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

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

(20)

增加判定條件Q

(21)

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

(22)

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

3.3 滯點問題改進應(yīng)對策略

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

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

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

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

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

vuavt=

(23)

根據(jù)式(23)可得，當(dāng)障礙物間距小于機器人直徑時，機器人將按照最大轉(zhuǎn)角實現(xiàn)避障。

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

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

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

4 數(shù)值仿真

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

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

4.1 仿真環(huán)境構(gòu)建

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

表1 仿真條件

4.2 仿真結(jié)果分析

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

圖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)

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

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

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

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

5 結(jié)論

本文借鑒電場出現(xiàn)外界激勵后恢復(fù)到穩(wěn)定狀態(tài)的現(xiàn)象，提出了一種基于狀態(tài)改變的機器人二維路徑規(guī)劃算法，主要完成以下工作：

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

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

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

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

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