基于改進人工勢場的物流無人機三維航跡規(guī)劃

陳明強，馮樹娟，李奇峰

(中國民用航空飛行學院 飛行技術(shù)學院，四川 廣漢 618307)

0 引言

無人機作為新技術(shù)和先進生產(chǎn)力的重要載體,已經(jīng)成為航空新業(yè)態(tài)發(fā)展的核心驅(qū)動力[1]。隨著電商的興起,最為突出的問題就是最后一公里難以送達或配送效率不高,因此物流無人機的應用隨之涌現(xiàn)[2-4]。利用無人機技術(shù)不僅可以提高配送效率,而且可以最大限度地減少能源消耗,達到節(jié)約能源的效果。采用無人機進行配送,這一途徑受到了各大物流企業(yè)的青睞,然而仍然存在一些技術(shù)上的瓶頸,阻礙了物流無人機的進一步發(fā)展。本文主要對其中的一項難題進行了研究,即物流無人機的航跡規(guī)劃問題。簡而言之,為了將運輸物品及時、準確、安全地運送到消費者手中,規(guī)劃出一條把貨物從服務商的最后一站配送點送至消費者,盡可能最優(yōu)、盡量避開障礙物的實際可飛的航線[5]。

孟想[6]在傳統(tǒng)人工勢場法的斥力函數(shù)中引入旋轉(zhuǎn)斥力、校正因子和安全距離,并將改進后的算法應用于動態(tài)航路規(guī)劃中,有效地解決了物流無人機三維航路規(guī)劃問題。徐宏飛[7]在傳統(tǒng)人工勢場法中改進引力算法為固定值、引入調(diào)節(jié)分量,驗證了面向智慧避障的物流無人機航跡規(guī)劃模型、三維人工勢場算法的有效性,但規(guī)劃的路徑不夠平滑。張凌云[8]結(jié)合量子局部搜索和反向?qū)W習策略對獅群算法進行改進,使所求的物流無人機航跡合理可靠。但只考慮了物流無人機在實際可飛行環(huán)境中基礎(chǔ)的物理量,忽略了一個關(guān)鍵的約束條件——貨物質(zhì)量約束。李志錕等[9]將人工勢場法與蟻群算法進行融合,引入目標點距離影響因子改進斥力場函數(shù),避免移動機器人受到較大斥力而無法實現(xiàn)路徑規(guī)劃,構(gòu)造勢場合力衰減系數(shù)避免了融合人工勢場蟻群算法陷入局部最優(yōu)解,但該方法相對復雜且僅適用于二維環(huán)境中。王兵等[10]通過引力補償增益系數(shù)來避免引力過大的問題;對于局部極小值點問題根據(jù)環(huán)境信息采取不同的虛擬目標點來解決,提高了安全性,縮短了路徑長度。但只在二維環(huán)境中進行了仿真驗證,并沒有擴展至三維環(huán)境中。Fan等[11]提出的改進人工勢場算法主要思想是構(gòu)建一個虛擬正六邊形模型,當運動目標陷入局部極小值時,利用正六邊形模型引導運動目標跳出局部極小值的陷阱。雖然該方法解決了傳統(tǒng)人工勢場法局部極小值的問題,但存在路徑震蕩和轉(zhuǎn)彎角過大的問題。

本文針對傳統(tǒng)人工勢場算法目標不可達和局部極小值的缺點,設計了一種改進的人工勢場算法。通過在斥力勢場函數(shù)中添加無人機和目標點之間的相對距離來解決目標不可達問題。采用球體模型策略有效地改善局部極小值問題。

1 無人機的性能約束

物流無人機的貨物運輸需要從多方面進行考慮,本文從物流無人機性能約束和任務要求兩方面著手,建立多約束條件下的物流無人機航跡規(guī)劃模型。設物流無人機運輸任務的區(qū)域是以長、寬、高分別為x、y、z的空間環(huán)境。

1.1 最遠航程約束

判斷航跡規(guī)劃好與不好的一個重要因素是規(guī)劃的航跡長度,在滿足各種需求的條件下規(guī)劃的航跡越短越好。因此物流無人機的運輸距離不得大于最遠航程,(xi,yi,zi)表示無人機在三維空間直角坐標系中第i個點的位置,約束為:

(1)

di≤dmax,

(2)

式中:dmax為無人機最遠航程。

1.2 最大貨物質(zhì)量約束

物流無人機能夠承載貨物的質(zhì)量是有限的,物流無人機的總質(zhì)量由物流無人機自身質(zhì)量及貨物質(zhì)量兩部分組成。而物流無人機自身的質(zhì)量是恒定不變的,所以只能對貨物進行限制。上限約束為:

M=m+n,

(3)

n≤nmax,

(4)

式中:m為物流無人機自身的質(zhì)量,n為貨物的質(zhì)量,nmax為最大載貨質(zhì)量。

1.3 飛行高度約束

物流無人機在飛行的過程中受到自身性能和貨物的影響,飛行高度應該限制在最低飛行高度和最高飛行高度之間。約束為:

hmin≤h≤hmax,

(5)

式中:hmin為最低飛行高度,hmax為最高飛行高度。

1.4 俯仰角約束

為了保證物流無人機運輸?shù)恼w安全,物流無人機在持續(xù)飛行過程中的垂直旋轉(zhuǎn)角度受到限制,不可能實現(xiàn)革命性的大角度旋轉(zhuǎn)[7],約束為:

(6)

式中:b為最大俯仰角。

2 傳統(tǒng)人工勢場法及其缺陷

2.1 傳統(tǒng)人工勢場法

傳統(tǒng)人工勢場(Traditional Artificial Potential Field, TAPF)法是由Khatib于1985年提出的一種解決路徑規(guī)劃的有效方法[12],其核心思想是構(gòu)造虛擬勢場,目標位置會對無人機產(chǎn)生引力Fatt作用,障礙物在一定范圍內(nèi)對無人機產(chǎn)生斥力Frep作用,最終在二者的合力F作用下使得無人機沿著無碰撞的軌跡運動至目標位置。無人機受力圖如圖1所示。

圖1 無人機受力圖Fig.1 Drone force diagram

引力勢場函數(shù):

Uatt(X)=0.5kattρ2(X,Xg),

(7)

引力函數(shù):

Fatt(X)=-▽Uatt(X)=-kattρ(X,Xg),

(8)

斥力勢場函數(shù):

(9)

斥力函數(shù):

Frep(X)=-▽Urep(X)=

(10)

合力:

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

(11)

式中:Xg表示目標點位置,Xo表示障礙物位置,katt表示引力增益系數(shù),krep表示斥力增益系數(shù),ρ表示2點之間的距離,ρ0表示斥力作用范圍,Fatt為勢場Uatt的負梯度,Frep為勢場Urep的負梯度,F為無人機所受到的合力。

2.2 TAPF法的缺陷

2.2.1 目標不可達

物體在目標點附近時斥力大于引力,此時物流無人機無法到達目標點的情況稱為目標不可達問題[13]。如圖2所示。

圖2 目標不可達示意Fig.2 Schematic of unreachable target

2.2.2 局部最優(yōu)

當物流無人機、目標點以及障礙物在同一直線上時總會出現(xiàn)一點使得引力和斥力大小相等、方向相反且合力為零,此時無人機就陷入局部極小值,如圖3所示。為了解決該問題,本文提出基于改進人工勢場法的無人機三維航跡規(guī)劃。

圖3 局部極小值示意Fig.3 Schematic of local minimums

3 TAPF算法的改進

3.1 改進斥力函數(shù)

針對目標不可達的問題,常用的方法是在斥力勢場函數(shù)中添加無人機和目標點之間的相對距離[14]。

改進后的斥力勢場函數(shù):

(12)

改進后的斥力函數(shù):

Frep(X)=-▽Urep(X)=

(13)

(14)

(15)

ρt(X,Xg)=|(x-xg)t|+|(y-yg)t|+|(z-zg)t|。

(16)

式(13)中的Frep1和Frep2見式(14)和式(15)。式(16)為無人機和目標點之間的相對距離,t可取大于零的任意常數(shù)。Frep1的方向是障礙物與無人機之間的連線并指向無人機,Frep2的方向是無人機與目標點之間的連線并指向目標點。無人機受力分析如圖4所示。

圖4 改進斥力后無人機受力圖Fig.4 Drone force diagram with improved repulsion

隨著t的取值發(fā)生變化,物流無人機的受力情況也隨之發(fā)生變化。接下來對t進行分類討論:

① 當t為0～1時,

(17)

(18)

因此,當t為0～1時,隨著物流無人機和目標點之間的距離不斷減小,Frep1(X)趨向于0,Frep2(X)趨向于∞,而Frep2(X)的方向是物流無人機與目標點之間的連線并指向目標點。此時無人機只受到斥力分量Frep2(X)和引力Fatt的作用,所以物流無人機在共同力的作用下朝著目標點的方向運動。

② 當t=1時,

(19)

(20)

因此,當t=1時,隨著物流無人機和目標點之間的距離不斷減小,Frep1(X)趨向于0,Frep2(X)趨向于-0.5Krep(1/ρ(X,X0)-1/ρ0)2,而Frep2(X)的方向是物流無人機與目標點之間的連線并指向目標點。此時無人機只受到斥力分量Frep2(X)和引力Fatt的作用,所以物流無人機在共同力的作用下朝著目標點的方向運動。

③ 當t﹥1時,

(21)

(22)

因此,當t﹥1時,隨著物流無人機和目標點之間的距離不斷縮小,Frep1(X)趨向于0,Frep2(X)趨向于0,此時無人機只受到引力Fatt的作用不受斥力的影響,且在引力的作用下使物流無人機朝著目標點的方向運動。

由此可知,無論t取任何大于零的常數(shù)時,斥力分量Frep1(X)都趨向于0,所以物流無人機在運動的過程中只受到斥力分量Frep2(X)和目標引力Fatt的影響。而斥力分量Frep2(X)以及引力Fatt的方向均是物流無人機與目標點之間的連線并指向目標點的,因此,當目標點與障礙物之間的距離較近時,物流無人機也能夠在斥力分量Frep2(X)和引力Fatt二者合力的共同作用下安全到達目標點。

3.2 球體模型策略

考慮到物流無人機在復雜環(huán)境下可能遇到多個局部極小值點問題,無法直接到達目標點,提出一種球體模型策略,通過在合適的位置設定虛擬目標點,打破原有力的平衡,使物流無人機在障礙物、目標點以及虛擬目標點的共同作用下跳出局部極小值陷阱,繼續(xù)向目標點移動。

首先,需要判斷物流無人機是否陷入局部極小值點。當物流無人機陷入局部極小值時,就會在某一點暫停或是在特別小的范圍內(nèi)前后移動,本文通過設定一個閾值與第i(i≥2) 次迭代時的位置和第i+1次迭代時的位置之間的相對距離進行比較,若2個點位置之間的相對距離小于設定的閾值,則物流無人機陷入局部極小值的陷阱;反之,則沒有[15-17]。或依據(jù)合力是否為0判斷物流無人機是否陷入局部極小值的陷阱。

針對上述問題,本文提出一種球體模型策略來解決。具體如下:若物流無人機陷入局部極小值,則以物流無人機陷入局部極小值為原點,建立三維空間直角坐標系XYZ,如圖5所示。本文把障礙物及其影響范圍抽象為一個球體(圖中的橙色區(qū)域),該球體是以障礙物的坐標(O)為球心,障礙物的影響范圍(r)為半徑。在XYZ坐標系中畫一個以局部極小值點為球心,局部極小值點(A)和目標點(G)的歐式距離為半徑(s)的球體N。物流無人機在局部極小值點掃射障礙物,以障礙物影響范圍的邊界為切點會有無數(shù)條切線,這些切線會與球體N相交形成一個圓(圓心為e,半徑為R,圖中的紫色區(qū)域),圓上的點可作為虛擬目標點,從而使物流無人機跳出局部最小值的陷阱。

圖5 球體模型Fig.5 Sphere model

設空間中的局部極小值點坐標為A(x1,y1,z1),障礙物的坐標為O(x0,y0,z0),目標點為G(xg,yg,zg),建立模型。

局部極小值與障礙物之間的距離:

(23)

局部極小值與切點之間的距離:

(24)

局部極小值與目標點之間的距離:

(25)

由式(24)和式(25)得圓的半徑:

(26)

帶入?yún)?shù)得:

(27)

由式(23)、式(24)和式(25)得圓心:

(28)

帶入?yún)?shù)得:

(29)

由式(27)和式(29)得圓的標準方程:

[x-(x1+e)]2+(y-yg)2=R2。

(30)

帶入?yún)?shù)得:

(31)

局部極小值問題通過在空間坐標系XYZ中設立虛擬目標點進行處理。當物流無人機陷入局部極小值的陷阱后,通過采用球體模型策略的方法在目標點附近建立虛擬目標點進行處理,使無人機跳出局部極小值的陷阱,順利到達目標點。設立虛擬目標點后物流無人機受力分析如圖6所示。

圖6 設立虛擬目標點后無人機受力圖Fig.6 Drone force diagram after setting up a virtual target point

3.3 算法流程

改進后的算法流程如下:

① 初始化參數(shù),設置物流無人機的初始位置、目標點位置和障礙物的位置。

② 根據(jù)式(8)、式(11)和式(13)計算合力。

③ 依據(jù)合力是否為零判斷物流無人機是否陷入局部極小值的陷阱,若是則轉(zhuǎn)至步驟④;否則轉(zhuǎn)至步驟⑤。

④ 采用球體模型的策略使物流無人機跳出局部極小值的陷阱。

⑤ 以改進后的人工勢場法驅(qū)動物流無人機繼續(xù)向前行駛,判斷物流無人機是否到達目標點,若是則轉(zhuǎn)至步驟⑥;否則轉(zhuǎn)至步驟② 繼續(xù)進行迭代。

⑥ 結(jié)束流程。

算法流程如圖7所示。

圖7 算法流程Fig.7 Algorithm flowchart

4 仿真分析

4.1 基于改進斥力函數(shù)的仿真

圖8 目標不可達仿真結(jié)果Fig.8 Simulation results for unreachable target

在斥力勢場函數(shù)中添加無人機和目標點之間的相對距離,解決了上述目標不可達的問題,仿真結(jié)果如圖9所示。可以看出,實際到達點和目標點重合,即物流無人機到達目標點。

圖9 改進斥力函數(shù)仿真結(jié)果Fig.9 Simulation results for improved repulsion function

4.2 球體模型策略的仿真

為了驗證本文所提出的球體模型策略對物流無人機跳出局部極小值陷阱的有效性,在Matlab仿真軟件上進行仿真實驗。設物流無人機的起始點為(0,0,0),終點為(900,900,900)。在只有一個障礙物的環(huán)境中,障礙物的坐標為(313,313,313)。由于起點、障礙物和終點3點共線,且在其中的一點合力為0,導致傳統(tǒng)人工勢場法陷入局部極值的陷阱,如圖10所示。

圖10 局部最優(yōu)仿真結(jié)果Fig.10 Simulation results for local optimal solution

判斷物流無人機已陷入局部極小值的陷阱,用本文提出的球體模型策略,產(chǎn)生虛擬目標點,打破原有力的平衡,使物流無人機跳出局部極小值的陷阱并安全到達目標點,球體模型策略規(guī)劃結(jié)果如圖11所示。

圖11 單障礙物球體模型策略規(guī)劃結(jié)果Fig.11 Planning results for single-obstacle sphere model strategy

上述仿真實驗驗證的是在只有一個礙物的場景中,當物流無人機陷入局部極值的陷阱時采用球體模型的策略跳出局部極小值陷阱是可行且有效的。接下來進行多障礙物情景中物流無人機的運動。設起點坐標(0,0,0),終點坐標(900,900,900),障礙物坐標分別為(313,313,313)、(400,700,100)、(530,610,600)、(800,750,300)、(700,400,600)、(730,710,710)、(132,625,440),使用TAPF法進行仿真,結(jié)果如圖12所示。

圖12 TAPF法規(guī)劃結(jié)果Fig.12 Planning results for TAPF method

由圖12可以看出,當前物流無人機已陷入局部極小值陷阱,采用本文提出的球體模型的策略,產(chǎn)生虛擬目標點,打破原有力的平衡,使物流無人機跳出局部極小值的陷阱并安全到達目標點,如圖13所示。

圖13 多障礙物球體模型策略規(guī)劃結(jié)果Fig.13 Planning results for multi-obstacle sphere model strategy

通過上述2組實驗可以看出,物流無人機靈巧地避開了所有障礙物,逃離局部極小點的陷阱,到達最終目標點。驗證了本文提出的改進人工勢場方法尤其是球體模型策略在物流無人機三維航跡規(guī)劃中的可行性。

5 結(jié)束語

本文針對物流無人機三維航跡規(guī)劃問題,分析了TAPF法及其存在的缺陷并提出了解決方案。通過改變斥力勢場函數(shù)解決目標不可達問題;當無人機處于局部極值時,采用球體模型策略構(gòu)造虛擬目標點使無人機在障礙物的斥力、目標點的引力以及虛擬目標點引力的共同作用下解決局部極小值問題。仿真結(jié)果顯示,改進的人工勢場法能夠有效地規(guī)劃出符合要求的最佳航跡,使物流無人機成功避開所有障礙物,安全到達目標點。