陳明強(qiáng)，馮樹(shù)娟，李奇峰

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

0 引言

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

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

本文針對(duì)傳統(tǒng)人工勢(shì)場(chǎng)算法目標(biāo)不可達(dá)和局部極小值的缺點(diǎn),設(shè)計(jì)了一種改進(jìn)的人工勢(shì)場(chǎng)算法。通過(guò)在斥力勢(shì)場(chǎng)函數(shù)中添加無(wú)人機(jī)和目標(biāo)點(diǎn)之間的相對(duì)距離來(lái)解決目標(biāo)不可達(dá)問(wèn)題。采用球體模型策略有效地改善局部極小值問(wèn)題。

1 無(wú)人機(jī)的性能約束

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

1.1 最遠(yuǎn)航程約束

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

(1)

di≤dmax,

(2)

式中:dmax為無(wú)人機(jī)最遠(yuǎn)航程。

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

物流無(wú)人機(jī)能夠承載貨物的質(zhì)量是有限的,物流無(wú)人機(jī)的總質(zhì)量由物流無(wú)人機(jī)自身質(zhì)量及貨物質(zhì)量?jī)刹糠纸M成。而物流無(wú)人機(jī)自身的質(zhì)量是恒定不變的,所以只能對(duì)貨物進(jìn)行限制。上限約束為:

M=m+n,

(3)

n≤nmax,

(4)

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

1.3 飛行高度約束

物流無(wú)人機(jī)在飛行的過(guò)程中受到自身性能和貨物的影響,飛行高度應(yīng)該限制在最低飛行高度和最高飛行高度之間。約束為:

hmin≤h≤hmax,

(5)

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

1.4 俯仰角約束

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

(6)

式中:b為最大俯仰角。

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

2.1 傳統(tǒng)人工勢(shì)場(chǎng)法

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

圖1 無(wú)人機(jī)受力圖Fig.1 Drone force diagram

引力勢(shì)場(chǎng)函數(shù):

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

(7)

引力函數(shù):

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

(8)

斥力勢(shì)場(chǎng)函數(shù):

(9)

斥力函數(shù):

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

(10)

合力:

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

(11)

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

2.2 TAPF法的缺陷

2.2.1 目標(biāo)不可達(dá)

物體在目標(biāo)點(diǎn)附近時(shí)斥力大于引力,此時(shí)物流無(wú)人機(jī)無(wú)法到達(dá)目標(biāo)點(diǎn)的情況稱為目標(biāo)不可達(dá)問(wèn)題[13]。如圖2所示。

圖2 目標(biāo)不可達(dá)示意Fig.2 Schematic of unreachable target

2.2.2 局部最優(yōu)

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

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

3 TAPF算法的改進(jìn)

3.1 改進(jìn)斥力函數(shù)

針對(duì)目標(biāo)不可達(dá)的問(wèn)題,常用的方法是在斥力勢(shì)場(chǎng)函數(shù)中添加無(wú)人機(jī)和目標(biāo)點(diǎn)之間的相對(duì)距離[14]。

改進(jìn)后的斥力勢(shì)場(chǎng)函數(shù):

(12)

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

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

(13)

(14)

(15)

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

(16)

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

圖4 改進(jìn)斥力后無(wú)人機(jī)受力圖Fig.4 Drone force diagram with improved repulsion

隨著t的取值發(fā)生變化,物流無(wú)人機(jī)的受力情況也隨之發(fā)生變化。接下來(lái)對(duì)t進(jìn)行分類討論:

① 當(dāng)t為0～1時(shí),

(17)

(18)

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

② 當(dāng)t=1時(shí),

(19)

(20)

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

③ 當(dāng)t﹥1時(shí),

(21)

(22)

因此,當(dāng)t﹥1時(shí),隨著物流無(wú)人機(jī)和目標(biāo)點(diǎn)之間的距離不斷縮小,Frep1(X)趨向于0,Frep2(X)趨向于0,此時(shí)無(wú)人機(jī)只受到引力Fatt的作用不受斥力的影響,且在引力的作用下使物流無(wú)人機(jī)朝著目標(biāo)點(diǎn)的方向運(yùn)動(dòng)。

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

3.2 球體模型策略

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

首先,需要判斷物流無(wú)人機(jī)是否陷入局部極小值點(diǎn)。當(dāng)物流無(wú)人機(jī)陷入局部極小值時(shí),就會(huì)在某一點(diǎn)暫?；蚴窃谔貏e小的范圍內(nèi)前后移動(dòng),本文通過(guò)設(shè)定一個(gè)閾值與第i(i≥2) 次迭代時(shí)的位置和第i+1次迭代時(shí)的位置之間的相對(duì)距離進(jìn)行比較,若2個(gè)點(diǎn)位置之間的相對(duì)距離小于設(shè)定的閾值,則物流無(wú)人機(jī)陷入局部極小值的陷阱;反之,則沒(méi)有[15-17]。或依據(jù)合力是否為0判斷物流無(wú)人機(jī)是否陷入局部極小值的陷阱。

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

圖5 球體模型Fig.5 Sphere model

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

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

(23)

局部極小值與切點(diǎn)之間的距離:

(24)

局部極小值與目標(biāo)點(diǎn)之間的距離:

(25)

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

(26)

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

(27)

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

(28)

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

(29)

由式(27)和式(29)得圓的標(biāo)準(zhǔn)方程:

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

(30)

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

(31)

局部極小值問(wèn)題通過(guò)在空間坐標(biāo)系XYZ中設(shè)立虛擬目標(biāo)點(diǎn)進(jìn)行處理。當(dāng)物流無(wú)人機(jī)陷入局部極小值的陷阱后,通過(guò)采用球體模型策略的方法在目標(biāo)點(diǎn)附近建立虛擬目標(biāo)點(diǎn)進(jìn)行處理,使無(wú)人機(jī)跳出局部極小值的陷阱,順利到達(dá)目標(biāo)點(diǎn)。設(shè)立虛擬目標(biāo)點(diǎn)后物流無(wú)人機(jī)受力分析如圖6所示。

圖6 設(shè)立虛擬目標(biāo)點(diǎn)后無(wú)人機(jī)受力圖Fig.6 Drone force diagram after setting up a virtual target point

3.3 算法流程

改進(jìn)后的算法流程如下:

① 初始化參數(shù),設(shè)置物流無(wú)人機(jī)的初始位置、目標(biāo)點(diǎn)位置和障礙物的位置。

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

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

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

⑤ 以改進(jìn)后的人工勢(shì)場(chǎng)法驅(qū)動(dòng)物流無(wú)人機(jī)繼續(xù)向前行駛,判斷物流無(wú)人機(jī)是否到達(dá)目標(biāo)點(diǎn),若是則轉(zhuǎn)至步驟⑥;否則轉(zhuǎn)至步驟② 繼續(xù)進(jìn)行迭代。

⑥ 結(jié)束流程。

算法流程如圖7所示。

圖7 算法流程Fig.7 Algorithm flowchart

4 仿真分析

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

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

在斥力勢(shì)場(chǎng)函數(shù)中添加無(wú)人機(jī)和目標(biāo)點(diǎn)之間的相對(duì)距離,解決了上述目標(biāo)不可達(dá)的問(wèn)題,仿真結(jié)果如圖9所示?？梢钥闯?實(shí)際到達(dá)點(diǎn)和目標(biāo)點(diǎn)重合,即物流無(wú)人機(jī)到達(dá)目標(biāo)點(diǎn)。

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

4.2 球體模型策略的仿真

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

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

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

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

上述仿真實(shí)驗(yàn)驗(yàn)證的是在只有一個(gè)礙物的場(chǎng)景中,當(dāng)物流無(wú)人機(jī)陷入局部極值的陷阱時(shí)采用球體模型的策略跳出局部極小值陷阱是可行且有效的。接下來(lái)進(jìn)行多障礙物情景中物流無(wú)人機(jī)的運(yùn)動(dòng)。設(shè)起點(diǎn)坐標(biāo)(0,0,0),終點(diǎn)坐標(biāo)(900,900,900),障礙物坐標(biāo)分別為(313,313,313)、(400,700,100)、(530,610,600)、(800,750,300)、(700,400,600)、(730,710,710)、(132,625,440),使用TAPF法進(jìn)行仿真,結(jié)果如圖12所示。

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

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

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

通過(guò)上述2組實(shí)驗(yàn)可以看出,物流無(wú)人機(jī)靈巧地避開(kāi)了所有障礙物,逃離局部極小點(diǎn)的陷阱,到達(dá)最終目標(biāo)點(diǎn)。驗(yàn)證了本文提出的改進(jìn)人工勢(shì)場(chǎng)方法尤其是球體模型策略在物流無(wú)人機(jī)三維航跡規(guī)劃中的可行性。

5 結(jié)束語(yǔ)

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