汪小帥,朱其新,朱永紅

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

0 引 言

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

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

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

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

1 D*算法理論

D*算法的核心代價函數[24]:

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

(1)

式中:n為當前節(jié)點;G(n)為當前所在節(jié)點到起始點的實際代價值;H(n)為當前所在節(jié)點到目標點的代價估計值。

D*算法的代價估計值H(n)可以具體表達為

式中:Succ(n)為當前節(jié)點n的擴展節(jié)點集合;n′為當前所在節(jié)點n的擴展節(jié)點;T為目標節(jié)點;H(n′)為n′的代價估計值;C(n,n′)為2個相鄰節(jié)點的代價值。

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

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

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

2 D*算法的改進

傳統(tǒng)D*算法在搜尋無障礙路徑時,通常因為規(guī)劃階段的龐大計算量以及生成路徑復雜性高等問題導致算法效率低下,本文對傳統(tǒng)D*算法提出以下改進。

2.1 擴展節(jié)點選取

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

①當xR

(3)

②當xR>xT時,步長h為-1。

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

2.2 代價函數優(yōu)化

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

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

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

(4)

式中:dc為三維空間內兩點間切比雪夫距離;dm為三維空間內兩點間曼哈頓距離。

圖 2 權重夾角Fig.2 Weight angle

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

2.3 路徑優(yōu)化

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

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

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

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

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

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

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

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

路徑段p5p6上任意一點q的高度qq′為

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

2.4 改進D*算法的實現

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

3 仿真實驗和結果分析

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

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

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

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

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

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

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

(10)

3.2 仿真實驗

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

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

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

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

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

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

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

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

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

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

4 結 語

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