復(fù)雜城市環(huán)境下無(wú)人機(jī)三維路徑規(guī)劃①

盧成陽(yáng), 王文格

(湖南大學(xué) 機(jī)械與運(yùn)載工程學(xué)院, 長(zhǎng)沙 410082)

電子商務(wù)的快速發(fā)展, 讓我國(guó)快遞數(shù)量在2017 年就進(jìn)入了“單日億件”時(shí)代, 為了降低地面貨運(yùn)交通的壓力, 提高快遞服務(wù)質(zhì)量, 國(guó)內(nèi)外各大物流公司早早將旋翼無(wú)人機(jī)加入物流配送環(huán)節(jié), 作為解決“最后一公里”問(wèn)題的答案[1]; 另一方面, “無(wú)人機(jī)+行業(yè)細(xì)分”的發(fā)展模式也讓旋翼無(wú)人機(jī)在城市內(nèi)的安防和搶險(xiǎn)救災(zāi)等方面發(fā)揮著重要作用[2].

面對(duì)復(fù)雜的城市環(huán)境, 旋翼無(wú)人機(jī)的路徑規(guī)劃技術(shù)格外重要. 一條好的飛行路徑, 直接關(guān)系到無(wú)人機(jī)的運(yùn)行效率、自身安全和對(duì)地面的安全影響[3]. 如何在復(fù)雜城市環(huán)境下, 快速找到距離更短、能耗更低、安全性更高、更符合旋翼無(wú)人機(jī)運(yùn)動(dòng)約束的路徑, 是目前急需解決的問(wèn)題.

傳統(tǒng)的路規(guī)劃如A*算法和生物啟發(fā)類算法, 計(jì)算量大、規(guī)劃時(shí)間長(zhǎng), 不能很好地應(yīng)對(duì)高維復(fù)雜環(huán)境[4-5];數(shù)學(xué)模型法、人工勢(shì)場(chǎng)法, 對(duì)環(huán)境和機(jī)器人的建模要求較高, 且容易陷入局部最優(yōu)[6-7]; 基于采樣的路徑規(guī)劃方法[8-10], 通過(guò)隨機(jī)性的節(jié)點(diǎn)采樣, 能快速尋找到空間中的路徑, 受維度變化的影響小, 但較強(qiáng)的隨機(jī)性和盲目的路徑尋找策略, 導(dǎo)致求解效率較低, 較難找到高質(zhì)量路徑解.

LaValle 教授在1999 年提出了快速搜索隨機(jī)樹(shù)(RRT)算法[11], 以其獨(dú)特的節(jié)點(diǎn)擴(kuò)展機(jī)制, 成功應(yīng)用于多種路徑規(guī)劃場(chǎng)景中, 如覆蓋路徑規(guī)劃、機(jī)械臂路徑規(guī)劃、車輛路徑規(guī)劃等. 隨后, RRT*算法[12]被提出,在RRT 算法的基礎(chǔ)上增加了父節(jié)點(diǎn)重新選擇和局部節(jié)點(diǎn)重連步驟, 彌補(bǔ)了RRT 算法的一些不足, 且被證實(shí)能夠達(dá)到路徑最優(yōu)[13]. 然而以上算法及其變體只能面向[0, 1]地圖環(huán)境, 針對(duì)機(jī)器人在連續(xù)成本變化空間的路徑規(guī)劃問(wèn)題, Jaillet 等人提出了T-RRT 算法[14], 在RRT 算法的基礎(chǔ)上引入了模擬退火的思想, 自行判斷新成本節(jié)點(diǎn)能否被擴(kuò)展, 讓路徑的探索偏向低成本區(qū)域, 可用于機(jī)器人在三維連續(xù)成本空間的路徑規(guī)劃. 緊隨其后, T-RRT*算法[15]被提出, 將RRT*算法的思想與T-RRT 算法相結(jié)合, 進(jìn)一步降低了路徑成本.

另一方面, 針對(duì)RRT 算法隨機(jī)性強(qiáng)的問(wèn)題, 文獻(xiàn)[16]提出的I-RRT 算法, 以目標(biāo)點(diǎn)概率性偏置的方式, 讓搜索樹(shù)的新節(jié)點(diǎn)擴(kuò)展指向目標(biāo)點(diǎn); 文獻(xiàn)[17]提出Informed-RRT*算法, 通過(guò)約束地圖上的采樣區(qū)域, 減少盲目性的同時(shí), 較好地提高了路徑質(zhì)量; 文獻(xiàn)[18,19]結(jié)合人工勢(shì)場(chǎng)法的思想, 提出APF-RRT 算法, 將目標(biāo)點(diǎn)方向參與到每一次新樹(shù)節(jié)點(diǎn)擴(kuò)展中, 影響節(jié)點(diǎn)擴(kuò)展. 然而,在面對(duì)城市環(huán)境下的旋翼無(wú)人機(jī)路徑規(guī)劃工況, 上述方法還存在以下問(wèn)題:

(1)空間成本類型考慮不充足;

(2)路徑節(jié)點(diǎn)的擴(kuò)展質(zhì)量差;

(3)路徑抖動(dòng)性強(qiáng).

針對(duì)這些問(wèn)題, 本文在T-RRT 算法的基礎(chǔ)上, 提出基于探索、啟發(fā)和轉(zhuǎn)移的EHT-RRT (exploring heuristic transition-based RRT)算法, 綜合全局路徑規(guī)劃和局部路徑規(guī)劃的特點(diǎn), 通過(guò)多層球形危險(xiǎn)度探索、啟發(fā)式成本估計(jì)、局部節(jié)點(diǎn)滑移、添加節(jié)點(diǎn)的局部最好方向?qū)傩? 以及改進(jìn)了的節(jié)點(diǎn)擴(kuò)展機(jī)制, 讓算法在三維空間中的規(guī)劃路徑更加穩(wěn)定、平滑和安全, 減少路徑后處理和軌跡規(guī)劃的難度, 更適合解決旋翼無(wú)人機(jī)在復(fù)雜三維環(huán)境下的路徑規(guī)劃問(wèn)題.

1 研究基礎(chǔ)

1.1 RRT 算法

RRT 算法首先將起點(diǎn)Xinit加入樹(shù)中, 然后在無(wú)障礙空間Dfree中生成隨機(jī)采樣點(diǎn)Xrand, 遍歷當(dāng)前樹(shù)節(jié)點(diǎn),找到離Xrand點(diǎn)最近的Xnear節(jié)點(diǎn), 隨后以Xnear-Xrand的方向, 按照固定步長(zhǎng)d生成可行新節(jié)點(diǎn)Xnew, 重復(fù)以上步驟直到新節(jié)點(diǎn)Xnew距目標(biāo)點(diǎn)Xgoal一定距離, 從而反向找到路徑解, 原理如圖1 所示.

圖1 RRT 算法擴(kuò)展原理

1.2 T-RRT 算法

T-RRT 算法在RRT 的框架上改進(jìn)而來(lái), 引入了模擬退火的思想, 通過(guò)對(duì)比兩節(jié)點(diǎn)間的成本變化, 決定新節(jié)點(diǎn)能否被擴(kuò)展. 算法在生成無(wú)碰撞的新節(jié)點(diǎn)Xnew后,需要進(jìn)行轉(zhuǎn)移測(cè)試審核: 如果新節(jié)點(diǎn)成本c(Xnew)比最近點(diǎn)的成本c(Xnear)低, 那么通過(guò)測(cè)試, 如果比最近點(diǎn)的高, 需要通過(guò)模擬退火思想下的概率公式, 才可通過(guò)轉(zhuǎn)移測(cè)試. 轉(zhuǎn)移測(cè)試函數(shù)如算法1 所示.

算法1. TransitionTest(c(Xnear), c(Xnew), d(Xnear-Xnew))begin if c(Xnew) ＞ cmax then return False;if c(Xnear) ＞ c(Xnew) then return True;p=exp(-(c(Xnew)-c(Xnear))/d(Xnear-Xnew)K·T )if Rand(0, 1) ＜ p then T = T/α;nFail = 0;return True;else if nFail ＞ nFailmax then T = T×α;nFail = 0;else nFail = nFail + 1;return False;end

算法1 中,K為起始點(diǎn)和目標(biāo)點(diǎn)的空間成本算數(shù)平均數(shù);T為模擬的溫度值;α為溫度變化系數(shù);nFail為失敗擴(kuò)展次數(shù);nFailmax為最大擴(kuò)展失敗次數(shù);c(·)為節(jié)點(diǎn)的空間成本, 這里用危險(xiǎn)度進(jìn)行表示;d(·)為內(nèi)部向量的模.

2 EHT-RRT 算法

EHT-RRT 算法基于T-RRT 算法改進(jìn), 保留了完整的轉(zhuǎn)移測(cè)試函數(shù), 去除節(jié)點(diǎn)細(xì)化/擴(kuò)展審核函數(shù), 在此基礎(chǔ)上, 提出節(jié)點(diǎn)滑移策略和啟發(fā)式成本探索策略, 利用球形節(jié)點(diǎn)結(jié)構(gòu), 將探索出的滑移方向危險(xiǎn)度和滑移節(jié)點(diǎn)帶來(lái)的啟發(fā)式路徑長(zhǎng)度、路徑偏角變化、路徑高度變化, 共同組成總的啟發(fā)式成本, 用于路徑節(jié)點(diǎn)的局部滑移, 并分析出局部最好方向, 添加到擴(kuò)展基點(diǎn)的屬性中, 以改進(jìn)節(jié)點(diǎn)的擴(kuò)展過(guò)程.

2.1 啟發(fā)式成本探索

由于城市環(huán)境下的樓宇、信號(hào)、人流等因素的影響, 算法借鑒了車輛路徑規(guī)劃中的多層Morphin 搜索樹(shù)的局部路徑規(guī)劃方法[20]和A*算法中的啟發(fā)式成本估計(jì)思想, 提出節(jié)點(diǎn)滑移策略, 對(duì)滑移方向及節(jié)點(diǎn)進(jìn)行啟發(fā)式成本探索, 為后續(xù)步驟做準(zhǔn)備.

2.1.1 危險(xiǎn)度索引和計(jì)算

全局地圖環(huán)境下的城市障礙危險(xiǎn)物如圖2 所示,分為樓宇障礙物、信號(hào)干擾和地面人流密度.

圖2 城市環(huán)境模型

樓宇障礙物, 用[0, 1]模型表示, 0 代表可行區(qū)域,1 代表樓宇障礙物, 則節(jié)點(diǎn)的樓宇危險(xiǎn)度為:

其中,Cbuilding為樓宇障礙物的索引矩陣.

信號(hào)干擾模型, 用干擾源三維坐標(biāo)和干擾半徑的列表[x,y,z,r]表示, 則節(jié)點(diǎn)的信號(hào)危險(xiǎn)度為:

其中,Csignal為節(jié)點(diǎn)到信號(hào)干擾源的距離,M為信號(hào)干擾源數(shù)量.

地面人流密度模型, 由地面人流密度二維矩陣表示, 等級(jí)為1-100, 則節(jié)點(diǎn)的人流危險(xiǎn)度為:

其中, (p1,p2,p3)為3 種危險(xiǎn)的權(quán)重.

算法在進(jìn)行節(jié)點(diǎn)擴(kuò)展時(shí), 只是從歐式距離上選擇了離Xrand更近的Xnear節(jié)點(diǎn)作為擴(kuò)展基點(diǎn), 卻沒(méi)有考慮Xnear節(jié)點(diǎn)的周圍環(huán)境情況. 從文獻(xiàn)[21]可知, 對(duì)節(jié)點(diǎn)的路徑碰撞檢查會(huì)消耗大量時(shí)間, 所以本文采用更加細(xì)密的多層球形節(jié)點(diǎn)結(jié)構(gòu)進(jìn)行危險(xiǎn)度索引和計(jì)算,以避免路徑偏向建筑障礙物附近, 提高路徑擴(kuò)展的成功率, 減少碰撞檢查次數(shù).

如圖3 所示, 以3 層結(jié)構(gòu)為例, 為了保證較好的方向擴(kuò)展密度, 結(jié)構(gòu)的相鄰方向偏角采用45°.

圖3(a) 顯示的為探索球的二維3 層結(jié)構(gòu), 共有8 個(gè)方向, 每個(gè)方向按半徑不同有3 個(gè)節(jié)點(diǎn). 首層節(jié)點(diǎn)因?yàn)橐糜诼窂降木植抗?jié)點(diǎn)滑移, 不宜過(guò)大, 所以設(shè)置為標(biāo)準(zhǔn)擴(kuò)展步長(zhǎng)d的1/3, 末層節(jié)點(diǎn)考慮到局部節(jié)點(diǎn)滑移帶來(lái)的動(dòng)態(tài)擴(kuò)展步長(zhǎng), 所以設(shè)置為標(biāo)準(zhǔn)步長(zhǎng)的4/3,具體取值關(guān)系如式(6).

圖3 多層危險(xiǎn)度探索球

抽出圖3(a)中的一層, 以任意方向每45°進(jìn)行一次旋轉(zhuǎn)復(fù)制, 即可得到圖3(b)所示的探索球的三維單層結(jié)構(gòu), 共26 個(gè)方向. 完整結(jié)構(gòu)的每個(gè)方向有3 個(gè)探索點(diǎn), 則單一方向上的危險(xiǎn)度為:

其中, (C1,C2,C3)分別為1-3 層節(jié)點(diǎn)的總危險(xiǎn)度值, 可由式(5)計(jì)算而得.

2.1.2 路徑啟發(fā)成本

多層危險(xiǎn)度探索, 只能了解到當(dāng)前節(jié)點(diǎn)周圍的危險(xiǎn)度情況, 然而算法還需要考慮無(wú)人機(jī)的運(yùn)動(dòng)約束和能耗問(wèn)題. 旋翼無(wú)人機(jī)不同于固定翼無(wú)人機(jī), 它能實(shí)現(xiàn)位置的精準(zhǔn)懸停, 亦能實(shí)現(xiàn)短距離剎車和轉(zhuǎn)彎, 但這樣的操作, 必定會(huì)浪費(fèi)自身能量. 文獻(xiàn)[22]中通過(guò)大量的實(shí)驗(yàn)證明, 除路徑長(zhǎng)度外, 不斷的姿態(tài)改變, 也嚴(yán)重影響著旋翼無(wú)人機(jī)的續(xù)航.

如圖4 所示, 對(duì)于選定滑移節(jié)點(diǎn)Xslip的路徑, 計(jì)算最近點(diǎn)到滑移節(jié)點(diǎn)Xnear-Xslip的歐式距離d1, 滑移節(jié)點(diǎn)到目標(biāo)點(diǎn)Xslip-Xgoal的歐式距離d2; 計(jì)算最近點(diǎn)到滑移節(jié)點(diǎn)到目標(biāo)節(jié)點(diǎn)Xnear-Xslip-Xgoal的偏轉(zhuǎn)角度θ(水平偏角θ1、豎直偏角θ2); 計(jì)算Xnear-Xslip和Xslip-Xgoal的高度變化絕對(duì)值(h1,h2). 則滑移節(jié)點(diǎn)下的啟發(fā)式路徑長(zhǎng)度d、偏轉(zhuǎn)角度θ和高度變化h可定義為:

圖4 路徑啟發(fā)成本

2.1.3 總啟發(fā)成本

綜上, 由A*算法中的啟發(fā)式成本估計(jì)思想, 從滑移點(diǎn)帶來(lái)的方向危險(xiǎn)度、路徑長(zhǎng)度、路徑偏轉(zhuǎn)角度和高度變化4 個(gè)方面, 力求路徑的平滑、安全, 減少無(wú)人機(jī)的能耗, 總的啟發(fā)成本為:

其中, (w1,w2,w3,w4)為4 個(gè)啟發(fā)成本的權(quán)重.

2.2 局部節(jié)點(diǎn)滑移

為了提高路徑質(zhì)量, 提出了局部節(jié)點(diǎn)滑移策略. 由上文可得滑移層26 個(gè)節(jié)點(diǎn)的啟發(fā)式估計(jì)成本, 然后找出最小成本滑移點(diǎn)作為新節(jié)點(diǎn)Xnew. 如圖5 所示, 選定了A點(diǎn)作為新節(jié)點(diǎn)Xnew, 路徑也從臨時(shí)新節(jié)點(diǎn)Tnew(O點(diǎn))滑移到新節(jié)點(diǎn)Xnew上.

圖5 局部滑移擴(kuò)展

以此能讓路徑偏向更好位置, 并實(shí)現(xiàn)了擴(kuò)展步長(zhǎng)的動(dòng)態(tài)改變, 這有利于算法逃離陷阱區(qū)域.

2.3 局部最好方向

人工勢(shì)場(chǎng)法和RRT 算法的融合, 讓路徑的尋找更有方向性, 但面對(duì)密集的樓宇建筑區(qū)域, 很可能讓當(dāng)前點(diǎn)的下次擴(kuò)展失效, 從而進(jìn)行不必要的路徑碰撞檢查.所以, 為了避免上述狀況的發(fā)生, 算法中對(duì)每個(gè)節(jié)點(diǎn)添加了局部最好方向?qū)傩詁est_dir, 以提高路徑臨時(shí)新節(jié)點(diǎn)的擴(kuò)展質(zhì)量.

在第2.1 節(jié)中, 可得到臨時(shí)新節(jié)點(diǎn)周圍26 個(gè)滑移點(diǎn), 以及26 個(gè)方向的成本. 局部最好方向的選取過(guò)程偽碼如算法2 所示.

算法2. best_dir Intput: the cost vector CM;the infinite cost quantity: cost_inf_num;the point Tnew & Xgoal; Pmin_cost the threshold of variance a.Output: best_dir.begin if cost_inf_num = 0;cost_var = variance(CM);if cost_var ＜ a best_dir = Xgoal- Tnew;else best_dir = Pmin_cost- Tnew;else best_dir = Pmin_cost- Tnew;end

通過(guò)矩陣中無(wú)窮大成本的數(shù)量cost_inf_num和成本均方差cost_var與閾值a的大小, 即可確定當(dāng)前范圍下最好擴(kuò)展方向的向量best_dir, 將其加入此次擴(kuò)展得到的新節(jié)點(diǎn)屬性中, 用于未來(lái)可能的節(jié)點(diǎn)擴(kuò)展. 局部最好方向best_dir如圖5 所示.

2.4 臨時(shí)新節(jié)點(diǎn)擴(kuò)展

為了更好地應(yīng)對(duì)復(fù)雜環(huán)境下的路徑擴(kuò)展尋找, 本文結(jié)合了I-RRT 算法中目標(biāo)點(diǎn)的概率性影響, 以及APF-RRT 的目標(biāo)點(diǎn)全時(shí)刻影響, 結(jié)合上文提出的best_dir屬性, 對(duì)臨時(shí)新節(jié)點(diǎn)的擴(kuò)展做出改進(jìn).

如圖6 所示, 同樣是通過(guò)隨機(jī)節(jié)點(diǎn)Xrand找到最近節(jié)點(diǎn)Xnear作為擴(kuò)展基點(diǎn). 算法首先讀取節(jié)點(diǎn)Xnear的局部最好方向best_dir, 然后按照式(10), 計(jì)算得出3 個(gè)方向上的標(biāo)準(zhǔn)步長(zhǎng)擴(kuò)展向量.

圖6 臨時(shí)新節(jié)點(diǎn)擴(kuò)展

其中, (u1,u2,u3)為3 個(gè)向量的權(quán)重.

2.5 算法流程

算法首先將起點(diǎn)Xinit加入樹(shù)中, 接下來(lái)在無(wú)障礙空間Dfree中生成隨機(jī)采樣點(diǎn)Xrand, 遍歷當(dāng)前樹(shù)節(jié)點(diǎn),找到離Xrand點(diǎn)最近的Xnear節(jié)點(diǎn), 隨后以Xnear-Xrand、Xnear-Xgoal、best_dir的3 方向擴(kuò)展機(jī)制, 生成可行臨時(shí)新節(jié)點(diǎn)Tnew, 接著對(duì)節(jié)點(diǎn)Tnew依次進(jìn)行路徑碰撞檢查和轉(zhuǎn)移測(cè)試審核, 通過(guò)后再以節(jié)點(diǎn)Tnew為球心, 構(gòu)建多層危險(xiǎn)度探索球, 通過(guò)索引計(jì)算每個(gè)方向的危險(xiǎn)度值,將此值和滑移層節(jié)點(diǎn)的啟發(fā)式路徑長(zhǎng)度、路徑偏轉(zhuǎn)角度、高度變化的權(quán)重和, 作為滑移層節(jié)點(diǎn)的啟發(fā)式成本. 通過(guò)成本分析, 找到成本最低的滑移點(diǎn)作為此次擴(kuò)展的新節(jié)點(diǎn)Xnew, 并確定新節(jié)點(diǎn)的局部最好方向向量best_dir屬性, 接著進(jìn)行路徑碰撞檢測(cè), 若通過(guò)則將新節(jié)點(diǎn)加入樹(shù)中. 重復(fù)以上步驟直到新節(jié)點(diǎn)Xnew距目標(biāo)點(diǎn)Xgoal一定距離, 從而反向找到路徑解. 完整的算法流程, 如圖7 所示.

圖7 EHT-RRT 算法流程圖

3 實(shí)驗(yàn)仿真與分析

為了驗(yàn)證EHT-RRT 的性能, 實(shí)驗(yàn)設(shè)置了如圖8 所示的規(guī)模較大、信號(hào)干擾較多、人流較稀疏的環(huán)境1和規(guī)模較小、信號(hào)干擾較少、人流較密集的環(huán)境2, 兩種城市環(huán)境進(jìn)行仿真實(shí)驗(yàn), 并通過(guò)環(huán)境1 來(lái)選定算法的各種參數(shù).

圖8 城市環(huán)境

具體任務(wù)省略了起飛和降落過(guò)程, 只考慮空間中兩點(diǎn)間的三維路徑規(guī)劃, 并通過(guò)多算法對(duì)比實(shí)驗(yàn)和消融對(duì)比實(shí)驗(yàn)驗(yàn)證EHT-RRT 算法的有效性.

仿真程序在64 bit Matlab 2018 平臺(tái)下運(yùn)行, 電腦系統(tǒng)環(huán)境為Windows 10, 配置為Inter(R) Core(TM) i7-1165G7 CPU @2.8 GHz, 16 GB RAM.

3.1 仿真參數(shù)設(shè)置

在求解路徑時(shí), 算法中的各種參數(shù)都影響著求解性能. 算法中的終點(diǎn)檢測(cè)半徑和標(biāo)準(zhǔn)擴(kuò)展步長(zhǎng)可依據(jù)城市環(huán)境中的樓宇和道路的相關(guān)建設(shè)標(biāo)準(zhǔn)和實(shí)際情況自行設(shè)定, 本文取終點(diǎn)檢測(cè)半徑為30 m, 標(biāo)準(zhǔn)擴(kuò)展步長(zhǎng)為18 m. 轉(zhuǎn)移測(cè)試函數(shù)中的各項(xiàng)參數(shù)與T-RRT 算法設(shè)置保持一致. 其他參數(shù)的選取均由仿真環(huán)境中20 次平均規(guī)劃結(jié)果進(jìn)行選定.

探索球?qū)訑?shù). 探索球?qū)訑?shù)越多, 越能準(zhǔn)確了解空間中的危險(xiǎn)情況, 但同時(shí)也會(huì)影響算法的效率. 不同層數(shù)探索球下的路徑搜索時(shí)間和路徑危險(xiǎn)度變化如圖9 所示. 綜合兩項(xiàng)指標(biāo), 可以看出層數(shù)為3 時(shí)情況較好, 所以選取探索球?qū)訑?shù)為3.

圖9 不同層數(shù)的性能對(duì)比

路徑危險(xiǎn)度的定義為, 最終路徑的平均路徑節(jié)點(diǎn)危險(xiǎn)度與路徑長(zhǎng)度的乘積, 計(jì)算公式為:

其中,N為最終路徑的節(jié)點(diǎn)數(shù),L為路徑長(zhǎng)度,Cxi為單一節(jié)點(diǎn)的危險(xiǎn)度, 可由式(5)計(jì)算.

危險(xiǎn)度權(quán)重. 不同的危險(xiǎn)對(duì)無(wú)人機(jī)的影響也有所不同, 本文利用層次分析法來(lái)確定上文所述的3 種危險(xiǎn)權(quán)值(p1,p2,p3)的大小. 由文獻(xiàn)[23]的介紹可得如表1 所示的判斷矩陣, 并計(jì)算得3 種權(quán)重值.

表1 不同危險(xiǎn)權(quán)重選取

啟發(fā)函數(shù)權(quán)重. 啟發(fā)函數(shù)的設(shè)置, 可以從不同的側(cè)重點(diǎn)去影響路徑的規(guī)劃結(jié)果, 文中設(shè)置了危險(xiǎn)度、路徑長(zhǎng)度、偏轉(zhuǎn)角度和高度變化, 共4 個(gè)啟發(fā)量, 可采用對(duì)照實(shí)驗(yàn)法來(lái)確定最終的權(quán)重值.

首先確定危險(xiǎn)度的權(quán)重值. 方法為改變危險(xiǎn)度的權(quán)重大小, 其余3 項(xiàng)均分剩余權(quán)重, 采用平均規(guī)劃時(shí)間、平均路徑危險(xiǎn)度和平均路徑長(zhǎng)度作為評(píng)判標(biāo)準(zhǔn),則不同權(quán)重組合下的規(guī)劃結(jié)果如表2 所示.

表2 危險(xiǎn)度啟發(fā)權(quán)重選取

從表2 中可以看出, 3 號(hào)實(shí)驗(yàn)組的路徑規(guī)劃結(jié)果相對(duì)較好, 所以危險(xiǎn)度的權(quán)重定為0.4.

接著確定路徑長(zhǎng)度的權(quán)重值. 保持危險(xiǎn)度權(quán)重不變, 改變路徑長(zhǎng)度的權(quán)重, 偏轉(zhuǎn)角度和高度變化均分剩余權(quán)重, 選取同樣的評(píng)判標(biāo)準(zhǔn), 實(shí)驗(yàn)組別及結(jié)果如表3所示. 從表3 中可以看出, 2 號(hào)實(shí)驗(yàn)組的各項(xiàng)結(jié)果都較好, 所以取路徑長(zhǎng)度的權(quán)重為0.2.

表3 路徑長(zhǎng)度啟發(fā)權(quán)重選取

同理, 從表4 和圖10 的結(jié)果可以確定偏轉(zhuǎn)角度和高度變化的權(quán)重分別為0.3, 0.1.

圖10 不同偏轉(zhuǎn)角度和高度變化權(quán)重的對(duì)比

表4 偏轉(zhuǎn)角度和高度變化啟發(fā)權(quán)重選取

偏轉(zhuǎn)角度為每?jī)蓷l相鄰路徑線的水平偏角和豎直偏角之和; 高度變化為每相鄰兩個(gè)路徑點(diǎn)間的高度變化絕對(duì)值.

3 方向擴(kuò)展權(quán)重. 隨機(jī)方向的設(shè)置, 有助于算法擺脫陷阱區(qū)域, 所以設(shè)置其權(quán)重為0.5, 同樣利用對(duì)照實(shí)驗(yàn)法得到如表5 所示結(jié)果. 可以看出2 號(hào)試驗(yàn)的規(guī)劃效果最好, 所以權(quán)重選定為(0.5, 0.2, 0.3).

表5 3 方向權(quán)重選取

由此可得EHT-RRT 算法的完整參數(shù)如表6 所示,并設(shè)置算法在兩個(gè)環(huán)境中的起點(diǎn)、終點(diǎn).

表6 仿真參數(shù)設(shè)置

3.2 仿真結(jié)果與分析

3.2.1 多算法對(duì)比實(shí)驗(yàn)

多算法對(duì)比實(shí)驗(yàn)將同樣擁有20%目標(biāo)偏置的TRRT、BT-RRT (雙向T-RRT)和T-RRT*算法進(jìn)行比較. 4 種算法均不進(jìn)行路徑迭代優(yōu)化和路徑剪枝處理,只對(duì)比首次找到路徑時(shí)的20 次結(jié)果均值. 各算法的路徑尋找情況俯視圖如圖11 所示, 其中路徑搜索樹(shù)為藍(lán)色, 最終路徑為綠色.

圖11 各算法仿真圖

為便于對(duì)比觀察, 僅將各算法路徑規(guī)劃結(jié)果作如圖12 所示的立體展示.

從圖11 可以看出, 4 種算法在兩個(gè)環(huán)境下都能較好地避開(kāi)人流稠密區(qū)域, 實(shí)現(xiàn)較為安全的路徑規(guī)劃, 但T-RRT 和 T-RRT*算法在規(guī)劃時(shí)探索了較多無(wú)效區(qū)域,BT-RRT 算法路徑較長(zhǎng)且曲折, 而EHT-RRT 算法能將探索范圍約束在目標(biāo)方向附近, 并朝向目標(biāo)點(diǎn). 結(jié)合圖12可以看出T-RRT、BT-RRT 和T-RRT*算法的規(guī)劃路徑抖動(dòng)大, 而EHT-RRT 算法規(guī)劃的路徑更加平滑和穩(wěn)定, 便于后期處理.

圖12 4 種算法的路徑規(guī)劃結(jié)果對(duì)比

表7 為20 次仿真結(jié)果中的常規(guī)路徑規(guī)劃參數(shù)對(duì)比, 可以反映算法的一般性能.

表7 常規(guī)參數(shù)對(duì)比

從20 次的平均結(jié)果看, 雙向探索機(jī)制的BT-RRT算法規(guī)劃時(shí)間最短, EHT-RRT 算法的規(guī)劃時(shí)間較長(zhǎng),但比T-RRT*算法短, 作為全局路徑規(guī)劃, 這些時(shí)間都在可接受范圍內(nèi); 路徑長(zhǎng)度參數(shù)上, EHT-RRT 算法的平均路徑長(zhǎng)度可比T-RRT 算法減少8.5%-13.9%, 可比BT-RRT 算法減少12.8%-14.3%, 可比T-RRT*算法減少6.0%-11.2%; 平均路徑危險(xiǎn)度參數(shù)上, EHT-RRT算法可比T-RRT 算法減少8.5%-14.5%, 可比BT-RRT算法減少12.8%-15.4%, 可比T-RRT*算法減少6.0%-11.3%.

為了更好地了解算法的規(guī)劃效果, 將4 種算法20 次的規(guī)劃路徑長(zhǎng)度進(jìn)行圖13 所示的對(duì)比展示. 結(jié)合圖11和表7 可以看出, EHT-RRT 算法的規(guī)劃結(jié)果, 路徑長(zhǎng)度更短, 一致性更好, 這有利于算法在實(shí)際中的應(yīng)用.

圖13 不同算法路徑長(zhǎng)度對(duì)比

表8 為20 次仿真結(jié)果的路徑抖動(dòng)參數(shù)對(duì)比, 一定程度上能反映規(guī)劃路徑對(duì)無(wú)人機(jī)能耗, 以及對(duì)路徑后處理工作量的影響.

表8 路徑抖動(dòng)參數(shù)對(duì)比

平均偏角變化參數(shù)上, EHT-RRT 算法可比T-RRT算法減少53.1%-59.7%, 可比BT-RRT 算法減少35.6%-64.6%, 可比T-RRT*算法減少2.9%-51.1%; 平均高度變化參數(shù)上, EHT-RRT 算法可比T-RRT 算法減少54.7%-60.5%, 可比BT-RRT 算法減少2.9%-47.9%, 可比T-RRT*算法減少52.2%-52.8%.

圖14 為4 種算法20 次規(guī)劃路徑的平均偏角變化和平均高度變化對(duì)比, 可以看出在環(huán)境2 中BT-RRT算法和EHT-RRT 算法大致處于一個(gè)性能上, 其余情況下, EHT-RRT 算法都能保持最低的路徑抖動(dòng)性. 且結(jié)果相對(duì)穩(wěn)定, 而 T-RRT、BT-RRT 和EHT-RRT 算法規(guī)劃結(jié)果波動(dòng)較大, 一致性較差.

圖14 不同算法的路徑抖動(dòng)參數(shù)對(duì)比

3.2.2 消融對(duì)比實(shí)驗(yàn)

為了驗(yàn)證EHT-RRT 算法改進(jìn)的有效性, 以環(huán)境1 為基礎(chǔ), 針對(duì)算法的改進(jìn)項(xiàng)進(jìn)行了消融對(duì)比實(shí)驗(yàn).

因?yàn)樗惴ǖ母倪M(jìn)項(xiàng)間存在繼承關(guān)系(如: 啟發(fā)式成本探索的結(jié)果是局部節(jié)點(diǎn)滑移和局部最好方向的數(shù)據(jù)基礎(chǔ); 局部最好方向應(yīng)用于改進(jìn)的節(jié)點(diǎn)擴(kuò)展機(jī)制), 所以僅查看EHT-RRT 算法在局部節(jié)點(diǎn)滑移和局部最好方向這兩個(gè)方面的改進(jìn)成效.

消融處理僅將對(duì)比項(xiàng)功能刪減, 整體的算法流程順序保持不變. 實(shí)驗(yàn)仍然采取20 次的平均仿真結(jié)果作為對(duì)比數(shù)據(jù), 具體如表9 所示.

表9 消融對(duì)比實(shí)驗(yàn)

從表9 中數(shù)據(jù)可以看出, 完整的EHT-RRT 算法能提供更好的規(guī)劃效果. 消去局部節(jié)點(diǎn)滑移, 導(dǎo)致路徑長(zhǎng)度、路徑危險(xiǎn)度以及路徑的抖動(dòng)性都增長(zhǎng)較大; 消去局部最好方向?qū)е侣窂降臄U(kuò)展出現(xiàn)局部盲目性, 造成規(guī)劃時(shí)間較長(zhǎng). 可以分析出: 局部節(jié)點(diǎn)滑移策略能更好地提升路徑質(zhì)量; 局部最好方向能更好地避免局部環(huán)境陷阱, 提升路徑搜索效率.

綜上分析, EHT-RRT 相比T-RRT、BT-RRT 和TRRT*, 在平均路徑長(zhǎng)度、平均路徑危險(xiǎn)度和平均高度變化參數(shù)上, 都有較好的表現(xiàn). 算法在各方面的改進(jìn)也為尋找更好的路徑帶來(lái)積極效果, 所以EHT-RRT 算法更適合在連續(xù)復(fù)雜的成本空間中, 找到一致性更好、長(zhǎng)度更短、危險(xiǎn)度更低且更加平滑安全的無(wú)人機(jī)飛行規(guī)劃路徑.

4 結(jié)論

本文針對(duì)復(fù)雜城市環(huán)境下的旋翼無(wú)人機(jī)路徑規(guī)劃問(wèn)題, 提出了EHT-RRT 算法, 具體包括以下4 個(gè)方面:

(1)對(duì)節(jié)點(diǎn)采用啟發(fā)式成本探索, 利用多層球形節(jié)點(diǎn)結(jié)構(gòu), 從危險(xiǎn)度、路徑長(zhǎng)度、路徑偏轉(zhuǎn)角度和高度變化4 個(gè)方面, 共同影響路徑的偏向;

(2)提出局部節(jié)點(diǎn)滑移策略, 可讓規(guī)劃路徑遠(yuǎn)離障礙物和危險(xiǎn)地區(qū), 降低路徑危險(xiǎn)度;

(3)為節(jié)點(diǎn)添加局部最好方向?qū)傩? 可讓每一次的節(jié)點(diǎn)擴(kuò)展, 考慮當(dāng)前節(jié)點(diǎn)的環(huán)境危險(xiǎn)情況, 提高擴(kuò)展質(zhì)量和效率;

(4)綜合隨機(jī)方向、局部最好方向和目標(biāo)點(diǎn)方向,改進(jìn)節(jié)點(diǎn)的擴(kuò)展機(jī)制, 減少盲目性, 提高規(guī)劃效率.

仿真結(jié)果表明, 本文算法能生成路徑更短、安全性更高、更加平滑的三維路徑, 可用于旋翼無(wú)人機(jī)在復(fù)雜成本空間中的路徑規(guī)劃.