陳明強(qiáng),馮樹(shù)娟,李奇峰
(中國(guó)民用航空飛行學(xué)院 飛行技術(shù)學(xué)院,四川 廣漢 618307)
無(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í)際可飛的航線(xiàn)[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)題。
物流無(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)境。
判斷航跡規(guī)劃好與不好的一個(gè)重要因素是規(guī)劃的航跡長(zhǎng)度,在滿(mǎn)足各種需求的條件下規(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)航程。
物流無(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ì)量。
物流無(wú)人機(jī)在飛行的過(guò)程中受到自身性能和貨物的影響,飛行高度應(yīng)該限制在最低飛行高度和最高飛行高度之間。約束為:
hmin≤h≤hmax,
(5)
式中:hmin為最低飛行高度,hmax為最高飛行高度。
為了保證物流無(wú)人機(jī)運(yùn)輸?shù)恼w安全,物流無(wú)人機(jī)在持續(xù)飛行過(guò)程中的垂直旋轉(zhuǎn)角度受到限制,不可能實(shí)現(xiàn)革命性的大角度旋轉(zhuǎn)[7],約束為:
(6)
式中:b為最大俯仰角。
傳統(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.1 目標(biāo)不可達(dá)
物體在目標(biāo)點(diǎn)附近時(shí)斥力大于引力,此時(shí)物流無(wú)人機(jī)無(wú)法到達(dá)目標(biāo)點(diǎn)的情況稱(chēng)為目標(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)以及障礙物在同一直線(xià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
針對(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ī)之間的連線(xiàn)并指向無(wú)人機(jī),Frep2的方向是無(wú)人機(jī)與目標(biāo)點(diǎn)之間的連線(xià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)行分類(lèi)討論:
① 當(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)之間的連線(xià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)之間的連線(xià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)之間的連線(xiàn)并指向目標(biāo)點(diǎn)的,因此,當(dāng)目標(biāo)點(diǎn)與障礙物之間的距離較近時(shí),物流無(wú)人機(jī)也能夠在斥力分量Frep2(X)和引力Fatt二者合力的共同作用下安全到達(dá)目標(biāo)點(diǎn)。
考慮到物流無(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ù)條切線(xiàn),這些切線(xiàn)會(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
改進(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
圖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
為了驗(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)共線(xià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ī)劃中的可行性。
本文針對(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)。