陳麒杰，晉玉強(qiáng)，王陶昱

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

0 引言

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

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

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

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

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

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

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

(1)

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

2)機(jī)器人運(yùn)動(dòng)時(shí)，可以實(shí)時(shí)獲得當(dāng)前時(shí)刻的位置、速度和速度方向。

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

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

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

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

(2)

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

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

(3)

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

(4)

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

(5)

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

Fobsn=

(6)

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

(7)

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

(8)

則機(jī)器人的加速度變化量為

(9)

定義t時(shí)刻機(jī)器人的速度向量為

(10)

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

VR+aobsN·Δt

(11)

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

(12)

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

綜上可得，機(jī)器人在t+1時(shí)刻的速度方程為

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

(13)

通過(guò)對(duì)速度量的積分，可以得到機(jī)器人的下一個(gè)路徑點(diǎn)為

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

(14)

式中，Δt為計(jì)算步長(zhǎng)。通過(guò)迭代得到每一個(gè)步長(zhǎng)下的位置點(diǎn)，將每一個(gè)路徑點(diǎn)相連，可以得到機(jī)器人前進(jìn)的最終路徑。

3 避障分析

3.1 單個(gè)障礙物避障分析

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

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

(15)

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

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

(16)

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

(17)

機(jī)器人在結(jié)束初次進(jìn)入障礙物作用范圍的運(yùn)動(dòng)后，其運(yùn)動(dòng)方程為

(18)

機(jī)器人避障成功，脫離避障區(qū)域后，其運(yùn)動(dòng)方程為

(19)

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

3.2 目標(biāo)不可達(dá)問(wèn)題改進(jìn)

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

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

(20)

增加判定條件Q

(21)

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

(22)

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

3.3 滯點(diǎn)問(wèn)題改進(jìn)應(yīng)對(duì)策略

利用機(jī)器人對(duì)障礙物之間距離的實(shí)時(shí)檢測(cè)，將滯點(diǎn)問(wèn)題分兩種情況討論。

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

當(dāng)機(jī)器人運(yùn)行至對(duì)稱障礙物時(shí)，因?yàn)橄嗤系K物系數(shù)的作用，使得機(jī)器人避障失敗(如圖1所示)。

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

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

vuavt=

(23)

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

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

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

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

4 數(shù)值仿真

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

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

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

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

表1 仿真條件

4.2 仿真結(jié)果分析

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

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

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

圖6 人工勢(shì)場(chǎng)法仿真圖(目標(biāo)不可達(dá))Fig.6 Simulation diagram of artificial potential field method (Target unreachable)

相同環(huán)境下，采用本文算法進(jìn)行規(guī)劃，針對(duì)以上問(wèn)題，均成功規(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可以看出，機(jī)器人能夠避免人工勢(shì)場(chǎng)法中的極小值點(diǎn)，并且能夠按照不同情況實(shí)現(xiàn)避障。當(dāng)障礙物間距小于機(jī)器人直徑時(shí)，機(jī)器人按照最大轉(zhuǎn)角實(shí)現(xiàn)避障；當(dāng)障礙物間距大于機(jī)器人直徑時(shí)，從障礙物中間穿過(guò)，以降低路徑規(guī)劃計(jì)算量；在目標(biāo)點(diǎn)附近存在障礙物時(shí)，不受障礙物影響，能夠到達(dá)目標(biāo)點(diǎn)。

5 結(jié)論

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

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

2)引入障礙物作用條件，當(dāng)滿足障礙物作用條件時(shí)，機(jī)器人狀態(tài)受到障礙物影響，避免人工勢(shì)場(chǎng)法中的極小值點(diǎn)以及目標(biāo)不可達(dá)問(wèn)題；

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

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

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