改進(jìn)D*算法下的無人機(jī)三維路徑規(guī)劃

汪小帥,朱其新,朱永紅

(1.蘇州科技大學(xué) 電子與信息工程學(xué)院,江蘇 蘇州 215009;2.蘇州科技大學(xué) 機(jī)械工程學(xué)院/建筑智慧節(jié)能江蘇省重點(diǎn)實(shí)驗(yàn)室/蘇州市共融機(jī)器人技術(shù)重點(diǎn)實(shí)驗(yàn)室,江蘇 蘇州 215009;3.景德鎮(zhèn)陶瓷大學(xué) 機(jī)電工程學(xué)院,江西 景德鎮(zhèn) 333001)

0 引 言

近年來,隨著無人機(jī)在軍事、工業(yè)和生活等多個(gè)領(lǐng)域內(nèi)的廣泛應(yīng)用,無人機(jī)的自主飛行能力要求越來越高。路徑規(guī)劃作為無人機(jī)飛行高效、穩(wěn)定、安全和自主地完成任務(wù)的關(guān)鍵技術(shù),一直是國內(nèi)外相關(guān)技術(shù)領(lǐng)域?qū)W者的重點(diǎn)研究對象之一。

無人機(jī)路徑規(guī)劃[1-2]問題的本質(zhì)在于根據(jù)先驗(yàn)知識和無人機(jī)所需完成的任務(wù),在三維環(huán)境下找到一條從起始點(diǎn)到目標(biāo)點(diǎn)的安全無障礙路徑。目前,三維路徑規(guī)劃算法可分為傳統(tǒng)算法和智能算法2大類。傳統(tǒng)算法包括RRT算法[3-4]、人工勢場法[5-6]、A*算法[7-8]等;智能算法有遺傳算法[9-10]、蟻群算法[11-13]、神經(jīng)網(wǎng)絡(luò)算法[14]、粒子群算法[15]等。文獻(xiàn)[3]在RRT算法中引入目標(biāo)點(diǎn)為導(dǎo)引點(diǎn)的啟發(fā)函數(shù),同時(shí)設(shè)置動態(tài)擴(kuò)展步長,解決了算法用作三維路徑規(guī)劃時(shí)易出現(xiàn)效率低下以及路徑冗長、不平滑等問題。文獻(xiàn)[7]結(jié)合無人機(jī)物理約束和環(huán)境威脅約束建立數(shù)學(xué)模型,并以此提出一種基于模型約束的改進(jìn)A*算法。文獻(xiàn)[9]在遺傳算法中引入差分進(jìn)化變異策略,并結(jié)合模擬退火算法,加強(qiáng)遺傳算法變異的多樣性,避免陷入局部最優(yōu)。文獻(xiàn)[11]結(jié)合蟻群算法和人工勢場法,提出一種新的信息素更新機(jī)制,并改進(jìn)啟發(fā)函數(shù),算法收斂速度顯著提高。文獻(xiàn)[14]應(yīng)用傳統(tǒng)算法規(guī)劃路徑,同時(shí)采用強(qiáng)化學(xué)習(xí)訓(xùn)練無人機(jī)避障,無人機(jī)動態(tài)避障能力得到顯著提高。

相較于上述算法,D*算法在面對動態(tài)障礙物以及尋找安全路徑方面一直有著良好表現(xiàn),尤其與基于強(qiáng)化學(xué)習(xí)、深度學(xué)習(xí)的智能算法相比,D*算法不需要龐大的訓(xùn)練數(shù)據(jù)和漫長的訓(xùn)練時(shí)間,且不必?fù)?dān)心所得路徑會受訓(xùn)練數(shù)據(jù)的影響。但傳統(tǒng)D*算法在用作路徑規(guī)劃時(shí)也經(jīng)常出現(xiàn)搜索效率低以及路徑復(fù)雜性高等問題。文獻(xiàn)[16-18]將全局地圖環(huán)境分解為多個(gè)局部環(huán)境,優(yōu)化子節(jié)點(diǎn)的選取方式,同時(shí)改進(jìn)代價(jià)函數(shù)并引入平滑函數(shù),所得路徑復(fù)雜性有所降低。文獻(xiàn)[19-20]改進(jìn)有向D*算法,引入關(guān)鍵節(jié)點(diǎn)逐級搜索,改善移動機(jī)器人路徑規(guī)劃的性能。但這些方法一般運(yùn)用在二維環(huán)境中。針對三維環(huán)境中遇到的問題, 文獻(xiàn)[21]先采用卡爾曼濾波算法預(yù)測目標(biāo)位置,然后使用算法完成路徑規(guī)劃,雖然有效縮短航程,但路徑復(fù)雜性依舊很高。文獻(xiàn)[22-23]結(jié)合遺傳算法提出一種不同的改進(jìn)方案,根據(jù)特定策略從D*算法規(guī)劃的初始航跡中提取特征點(diǎn)路徑,并以此生成初始種群,設(shè)計(jì)適應(yīng)度函數(shù)。但在方向約束下的D*算法會產(chǎn)生大量無用節(jié)點(diǎn)。

以上方法在一定程度上改進(jìn)了傳統(tǒng)D*算法的搜索效率和規(guī)劃路徑。但傳統(tǒng)D*算法在實(shí)際用作路徑規(guī)劃時(shí)依然存在一些弊端:①規(guī)劃階段計(jì)算量大,算法效率不高;②所得路徑總是長于實(shí)際可到達(dá)目標(biāo)點(diǎn)的路徑,冗余節(jié)點(diǎn)過多。針對這些問題,本文提出一種改進(jìn)的D*算法,該算法采用變步長的方式擴(kuò)展節(jié)點(diǎn),同時(shí)利用切比雪夫距離和曼哈頓距離的融合式(切-曼融合式)作為代價(jià)值,并對算法所得路徑進(jìn)行優(yōu)化處理,保存關(guān)鍵節(jié)點(diǎn),刪除冗余、不必要的路徑點(diǎn)。

1 D*算法理論

D*算法的核心代價(jià)函數(shù)[24]:

F(n)=G(n)+H(n)

(1)

式中:n為當(dāng)前節(jié)點(diǎn);G(n)為當(dāng)前所在節(jié)點(diǎn)到起始點(diǎn)的實(shí)際代價(jià)值;H(n)為當(dāng)前所在節(jié)點(diǎn)到目標(biāo)點(diǎn)的代價(jià)估計(jì)值。

D*算法的代價(jià)估計(jì)值H(n)可以具體表達(dá)為

式中:Succ(n)為當(dāng)前節(jié)點(diǎn)n的擴(kuò)展節(jié)點(diǎn)集合;n′為當(dāng)前所在節(jié)點(diǎn)n的擴(kuò)展節(jié)點(diǎn);T為目標(biāo)節(jié)點(diǎn);H(n′)為n′的代價(jià)估計(jì)值;C(n,n′)為2個(gè)相鄰節(jié)點(diǎn)的代價(jià)值。

D*算法在三維環(huán)境下進(jìn)行路徑規(guī)劃時(shí),會創(chuàng)建OPEN表和CLOSED表來完成擴(kuò)展節(jié)點(diǎn)和選取最優(yōu)節(jié)點(diǎn)的任務(wù)。從目標(biāo)點(diǎn)柵格開始,對目標(biāo)點(diǎn)柵格周圍的26個(gè)柵格節(jié)點(diǎn)進(jìn)行搜索并存入OPEN表,然后計(jì)算它們的代價(jià)值和代價(jià)估計(jì)值,選取最優(yōu)的柵格節(jié)點(diǎn)(代價(jià)值最小的柵格節(jié)點(diǎn))繼續(xù)擴(kuò)展,被擴(kuò)展過的點(diǎn)從OPEN表中刪除,存入CLOSED表中。重復(fù)上述操作,依次擴(kuò)展節(jié)點(diǎn),直到起始點(diǎn)從OPEN表中刪除,執(zhí)行過程如圖1所示。

圖 1 OPEN和CLOSED表執(zhí)行過程Fig.1 The execution of the list OPEN and list CLOSED

圖1中,當(dāng)遍歷到當(dāng)前柵格節(jié)點(diǎn)N時(shí),將節(jié)點(diǎn)N存入CLOSED表,遍歷該柵格周圍的26個(gè)柵格并存入OPEN表;經(jīng)過代價(jià)函數(shù)計(jì)算,將代價(jià)值最小的柵格節(jié)點(diǎn)N1從OPEN表中刪除,存入CLOSED表中。然后遍歷節(jié)點(diǎn)N1周圍的26個(gè)柵格節(jié)點(diǎn),計(jì)算出最小的代價(jià)值節(jié)點(diǎn)N1-2從OPEN表中刪除,存入CLOSED表中。之后繼續(xù)向前遍歷,直到起始點(diǎn)S存入CLOSED表中。

2 D*算法的改進(jìn)

傳統(tǒng)D*算法在搜尋無障礙路徑時(shí),通常因?yàn)橐?guī)劃階段的龐大計(jì)算量以及生成路徑復(fù)雜性高等問題導(dǎo)致算法效率低下,本文對傳統(tǒng)D*算法提出以下改進(jìn)。

2.1 擴(kuò)展節(jié)點(diǎn)選取

在三維環(huán)境下,傳統(tǒng)D*算法以當(dāng)前柵格為基準(zhǔn),選擇其相鄰的26個(gè)柵格作為擴(kuò)展節(jié)點(diǎn)。本質(zhì)是從當(dāng)前柵格出發(fā)向,以步長為1,向x、y、z方向(可以反方向)進(jìn)行遍歷。當(dāng)?shù)貓D模型較大,起始點(diǎn)和目標(biāo)點(diǎn)跨越整個(gè)地圖時(shí),需要耗費(fèi)大量的時(shí)間去遍歷節(jié)點(diǎn)選取路徑。本文采用變步長的方式,根據(jù)擴(kuò)展柵格節(jié)點(diǎn)(xR,yR,zR)和目標(biāo)點(diǎn)柵格(xT,yT,zT)的位置關(guān)系定義步長h,當(dāng)擴(kuò)展節(jié)點(diǎn)柵格未到目標(biāo)點(diǎn)柵格時(shí),加快遍歷速度,步長較大;當(dāng)擴(kuò)展節(jié)點(diǎn)柵格超過目標(biāo)節(jié)點(diǎn)柵格,則選取較小的步長;以x方向?yàn)槔?具體實(shí)施如下:

①當(dāng)xR

(3)

②當(dāng)xR>xT時(shí),步長h為-1。

同理,y方向與z方向也采用同樣的方法。這樣的方式在加快遍歷速度的同時(shí),保證路徑的精確性、安全性。

2.2 代價(jià)函數(shù)優(yōu)化

傳統(tǒng)D*算法采用歐氏距離作為代價(jià)值的計(jì)算需要進(jìn)行開方運(yùn)算,使得算法整體的計(jì)算量較大,在規(guī)劃階段選取代價(jià)值最小節(jié)點(diǎn)時(shí),這一特點(diǎn)會導(dǎo)致算法效率低下。尤其在三維環(huán)境下,還需要考慮高度因素,從而使歐氏距離的計(jì)算復(fù)雜度成倍增加。

綜上所述,本文對代價(jià)函數(shù)中代價(jià)值的評判依據(jù)做出了優(yōu)化,用切-曼融合式替代了傳統(tǒng)歐氏距離。將當(dāng)前節(jié)點(diǎn)、目標(biāo)點(diǎn)所在直線與當(dāng)前節(jié)點(diǎn)、擴(kuò)展節(jié)點(diǎn)所在直線的夾角值定為α,如圖2所示,并將sin α作為切比雪夫距離和曼哈頓距離融合的權(quán)重值,得到距離融合式如下:

d=sin α·dc+(1-sin α)·dm

(4)

式中:dc為三維空間內(nèi)兩點(diǎn)間切比雪夫距離;dm為三維空間內(nèi)兩點(diǎn)間曼哈頓距離。

圖 2 權(quán)重夾角Fig.2 Weight angle

圖2中,s為當(dāng)前節(jié)點(diǎn),k為擴(kuò)展節(jié)點(diǎn),T為目標(biāo)點(diǎn),直線sT與直線sk夾角為α∈(0°,90°)。當(dāng)夾角α趨向于0°時(shí),s、k、T應(yīng)三點(diǎn)共線,代價(jià)距離近似于曼哈頓距離較為精確;當(dāng)夾角α接近直角時(shí),代價(jià)距離近似于切比雪夫距離為最優(yōu)。

2.3 路徑優(yōu)化

傳統(tǒng)D*算法找到的無障礙路徑常常會存在不必要的拐點(diǎn),這會導(dǎo)致生成的路徑長于實(shí)際可到達(dá)目標(biāo)點(diǎn)的路徑。為提高無人機(jī)的效率,本文對已生成的路徑進(jìn)行優(yōu)化處理,將一些冗余的拐點(diǎn)剪除。

路徑優(yōu)化過程從目標(biāo)點(diǎn)開始,將目標(biāo)點(diǎn)設(shè)置為第1個(gè)判斷點(diǎn),依次遍歷前面的路徑節(jié)點(diǎn)并判斷當(dāng)前遍歷的節(jié)點(diǎn)與判斷點(diǎn)之間的連線是否穿過障礙物,如果不穿過障礙物則繼續(xù)向前遍歷路徑節(jié)點(diǎn);如果穿過障礙物,則將當(dāng)前遍歷到的節(jié)點(diǎn)的后一個(gè)節(jié)點(diǎn)更新為判斷點(diǎn),繼續(xù)向前遍歷判斷,直到判斷點(diǎn)與起始點(diǎn)的連線不穿過障礙物,如圖3所示。

圖 3 路徑優(yōu)化過程Fig.3 Path optimization process

從圖3可以看出,從目標(biāo)點(diǎn)T開始向前優(yōu)化路徑,將目標(biāo)點(diǎn)T作為第一個(gè)判斷點(diǎn)。目標(biāo)點(diǎn)T與點(diǎn)p1之間的連線不會穿過障礙物;繼續(xù)向前遍歷路徑點(diǎn),發(fā)現(xiàn)目標(biāo)點(diǎn)T與點(diǎn)p2連線發(fā)現(xiàn)也不會穿過障礙物,此處可以刪除多余路徑點(diǎn)p1;繼續(xù)向前遍歷路徑點(diǎn),目標(biāo)點(diǎn)T與點(diǎn)p3連線穿過了障礙物,所以不能優(yōu)化出目標(biāo)點(diǎn)T直達(dá)點(diǎn)p3的路徑,將點(diǎn)p3的后一點(diǎn)p2更新為判斷點(diǎn),從點(diǎn)p2開始繼續(xù)向前遍歷判斷,優(yōu)化整條路徑。

上述優(yōu)化路徑的過程中,如何在三維環(huán)境下判斷兩點(diǎn)之間的連線是否穿過障礙物是問題的關(guān)鍵,其本質(zhì)是判斷空間內(nèi)兩點(diǎn)間線段是否與立方體有交點(diǎn)。如果采用在三維空間內(nèi)直接判斷的方式,計(jì)算量較大且在柵格邊緣處難以判斷清楚。本文采用投影法,將三維路徑及三維障礙物地圖投影到二維平面上,然后根據(jù)路徑段投影與障礙物投影間的關(guān)系判斷路徑段是否穿過障礙物,如圖4所示。

(a) 路徑與障礙物的二維投影

由此可得,路徑段p5p6所在直線與平面的夾角θ的正切值為

另一方面，組織與組織之間操作化為組織與媒體、組織嵌入方式和組織與國家、民間、企業(yè)三個(gè)方面。媒體的介入起到的效果是雙方面的，大眾傳媒有利于整個(gè)社工行業(yè)的宣傳和監(jiān)管，而在整個(gè)社會對社工職業(yè)的認(rèn)知都尚處于初級階段，社工組織自身的能力仍需不斷提升的情況下，是需要謹(jǐn)慎看待媒體的介入，例如雙方在理念上差異而導(dǎo)致服務(wù)對象的權(quán)益受到損害。后兩者可以從影響力來看，即目前外界給予社工職業(yè)的影響力量較強(qiáng)，而反之社工對于外界的影響力較弱。

路徑段p5p6上任意一點(diǎn)q的高度qq′為

若qq′小于z,則說明在三維環(huán)境下,路徑段p5p6穿過障礙物O1。

2.4 改進(jìn)D*算法的實(shí)現(xiàn)

改進(jìn)D*算法的執(zhí)行過程如圖5所示。

3 仿真實(shí)驗(yàn)和結(jié)果分析

3.1 環(huán)境模型和無人機(jī)動力學(xué)模型

本文使用MATLAB作為實(shí)驗(yàn)平臺進(jìn)行仿真實(shí)驗(yàn)。為驗(yàn)證算法改進(jìn)結(jié)果的有效性,將傳統(tǒng)D*算法和改進(jìn)后的D*算法進(jìn)行對比仿真實(shí)驗(yàn)。三維城區(qū)場景設(shè)置如圖6所示。

圖6中,Start所在位置為起始點(diǎn),Target所在位置為目標(biāo)點(diǎn),其余立方體表示城區(qū)樓房建筑,屬于障礙物,白色區(qū)域表示無人機(jī)可以自由行駛的區(qū)域。從起始點(diǎn)出發(fā),選擇一條到達(dá)目標(biāo)點(diǎn)的無障礙路徑,從而完成路徑規(guī)劃。

本文主要研究四旋翼無人機(jī)的動力學(xué)模型[25]。在無人機(jī)的飛行過程中,可以將動力學(xué)系統(tǒng)劃分為3個(gè)子系統(tǒng),即高度子系統(tǒng)、偏航子系統(tǒng)和水平位置子系統(tǒng)。針對這3個(gè)子系統(tǒng),本文建立如下動力學(xué)模型,以驗(yàn)證改進(jìn)后的D*算法是否符合無人機(jī)的動力學(xué)要求。

式中:子系統(tǒng)∏1主要控制無人機(jī)的升降運(yùn)動;子系統(tǒng)∏2負(fù)責(zé)控制無人機(jī)的朝向和轉(zhuǎn)向運(yùn)動,子系統(tǒng)∏1和∏2均屬于全驅(qū)動系統(tǒng);子系統(tǒng)∏3則主要負(fù)責(zé)控制無人機(jī)的水平飛行運(yùn)動,屬于欠驅(qū)動系統(tǒng)。由于系統(tǒng)存在建模不確定性,本文假設(shè)無人機(jī)轉(zhuǎn)動慣量J1、J2、J3、空氣動力學(xué)參數(shù)Ki(i=1,2,…,6),無人機(jī)重心與螺旋槳軸心之間的距離l以及力-力矩比例常數(shù)c均為未知常數(shù)。

由于四旋翼無人機(jī)在飛行過程中不會做出過大機(jī)動動作,本文提出如下合理假設(shè):四旋翼無人機(jī)的俯仰角θ(t)和滾轉(zhuǎn)角φ(t)滿足以下不等式

-π/2<θ(t)<π/2,-π/2<φ(t)<π/2

(10)

3.2 仿真實(shí)驗(yàn)

為驗(yàn)證本文采用切-曼融合式作為代價(jià)值的優(yōu)越性,將改進(jìn)后D*算法與使用切比雪夫距離或曼哈頓距離作為代價(jià)值的D*算法進(jìn)行對比實(shí)驗(yàn),結(jié)果如圖7所示。

(a) 切比雪夫距離作為代價(jià)值的D*算法

從圖7可以看出,采用切-曼融合式有效避免了使用切比雪夫距離作為代價(jià)值時(shí)路徑拐點(diǎn)多、波動幅度大和精確度低的現(xiàn)象,還解決了使用曼哈頓距離作為代價(jià)值時(shí)路徑拐點(diǎn)多且緊貼障礙物邊緣的問題。本文采用切-曼融合式作為代價(jià)值不僅在算法規(guī)劃階段降低了計(jì)算量,提高了算法效率,并且加強(qiáng)了算法生成路徑的精確性、安全性。

另外,為驗(yàn)證改進(jìn)后D*算法的優(yōu)越性,并且保證仿真實(shí)驗(yàn)結(jié)果的真實(shí)性、普遍性,避免特殊環(huán)境的影響,傳統(tǒng)D*算法、改進(jìn)后的D*算法以及蟻群算法將在同一三維環(huán)境下、同一臺計(jì)算機(jī)上分別進(jìn)行多組不同目標(biāo)點(diǎn)的仿真實(shí)驗(yàn)。實(shí)驗(yàn)起始點(diǎn)固定為(5,5,10),而在選取目標(biāo)點(diǎn)時(shí),應(yīng)該重視環(huán)境因素對算法效率的影響。為了達(dá)到更好的效果,選擇跨越整個(gè)地圖的目標(biāo)點(diǎn)和地圖的不同部位,包括前、中、后段位置。作為具體示例,考慮使用坐標(biāo)(83,90,45)、(45,65,35)和(86,65,45)分別代表地圖上的這些位置。這3組仿真實(shí)驗(yàn)結(jié)果對比如圖8～10所示。

(a) 傳統(tǒng)D*算法規(guī)劃的無障礙路徑

(a) 傳統(tǒng)D*算法規(guī)劃的無障礙路徑

(a) 傳統(tǒng)D*算法規(guī)劃的無障礙路徑

從圖8～10可以看出,傳統(tǒng)D*算法的主要缺陷體現(xiàn)在以下幾點(diǎn),即:具有較高的轉(zhuǎn)彎頻率,且轉(zhuǎn)彎路徑總是緊貼障礙物邊緣,不具備較高的安全性。如圖9(b)所示,對于改進(jìn)后D*算法生成的路徑進(jìn)行優(yōu)化處理,上述問題有了明顯改進(jìn),路徑轉(zhuǎn)彎頻率顯著降低,路徑安全性得到提高。對比蟻群算法容易陷入局部最優(yōu),所得路徑拐點(diǎn)數(shù)過多、復(fù)雜性高的問題,改進(jìn)后D*算法減少了冗余拐點(diǎn),路徑更加簡單。同時(shí),觀察改進(jìn)后D*算法得出的圖像結(jié)果,優(yōu)化路徑中拐點(diǎn)的轉(zhuǎn)角均符合無人機(jī)動力學(xué)模型的假設(shè),符合無人機(jī)飛行任務(wù)要求。

改進(jìn)后D*算法在三維實(shí)驗(yàn)環(huán)境中的表現(xiàn)比傳統(tǒng)D*算法更加高效。這是因?yàn)椴捎昧俗儾介L的方法擴(kuò)展節(jié)點(diǎn),并且使用切-曼融合式作為代價(jià)值,從而明顯降低了算法規(guī)劃階段的計(jì)算量。此外,將生成路徑投影到二維平面上進(jìn)行了優(yōu)化處理,減少了路徑轉(zhuǎn)彎頻率、縮短了路徑長度并降低了路徑復(fù)雜性。值得注意的是,在算法總規(guī)劃時(shí)間中,僅有0.12%被用于優(yōu)化路徑的計(jì)算判斷,所以不會抵消其他優(yōu)化帶來的好處。

相比于使用蟻群算法的方案,改進(jìn)后D*算法在規(guī)劃時(shí)間和路徑復(fù)雜度方面具有顯著優(yōu)勢。更重要的是,改進(jìn)后D*算法克服了傳統(tǒng)算法轉(zhuǎn)彎次數(shù)太多的缺陷,提高了無人機(jī)的飛行安全性,當(dāng)路徑?jīng)]有過多轉(zhuǎn)向的要求時(shí),可以有效地保護(hù)發(fā)動機(jī)。在復(fù)雜的地圖環(huán)境或更長的任務(wù)距離下,改進(jìn)后D*算法優(yōu)越性更加明顯,能夠更好地滿足無人機(jī)飛行任務(wù)的需求,提升整體的飛行效率。

4 結(jié) 語

本文針對無人機(jī)使用傳統(tǒng)D*算法進(jìn)行三維路徑規(guī)劃過程中,存在效率低、拐點(diǎn)多,路徑復(fù)雜等問題,提出一種改進(jìn)D*算法。首先在擴(kuò)展節(jié)點(diǎn)時(shí),采用變步長的方式,根據(jù)當(dāng)前節(jié)點(diǎn)與目標(biāo)節(jié)點(diǎn)的遠(yuǎn)近,選擇不同的步長,這樣可以提高D*算法在規(guī)劃階段搜索空間的效率;同時(shí)在代價(jià)函數(shù)方面,算法利用切-曼融合式代替?zhèn)鹘y(tǒng)歐式距離作為代價(jià)值,在保證路徑精確性的前提下使得算法計(jì)算量進(jìn)一步減少,D*算法效率低下的問題得到有效解決;最后對算法所得路徑進(jìn)行優(yōu)化處理,保存關(guān)鍵節(jié)點(diǎn),刪除冗余、不必要的路徑點(diǎn),優(yōu)化后的路徑更符合無人機(jī)飛行任務(wù)的要求,無人機(jī)的飛行效率顯著提高。