盧成陽, 王文格

(湖南大學 機械與運載工程學院, 長沙 410082)

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

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

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

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

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

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

(2)路徑節(jié)點的擴展質量差;

(3)路徑抖動性強.

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

1 研究基礎

1.1 RRT 算法

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

圖1 RRT 算法擴展原理

1.2 T-RRT 算法

T-RRT 算法在RRT 的框架上改進而來, 引入了模擬退火的思想, 通過對比兩節(jié)點間的成本變化, 決定新節(jié)點能否被擴展. 算法在生成無碰撞的新節(jié)點Xnew后,需要進行轉移測試審核: 如果新節(jié)點成本c(Xnew)比最近點的成本c(Xnear)低, 那么通過測試, 如果比最近點的高, 需要通過模擬退火思想下的概率公式, 才可通過轉移測試. 轉移測試函數如算法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為起始點和目標點的空間成本算數平均數;T為模擬的溫度值;α為溫度變化系數;nFail為失敗擴展次數;nFailmax為最大擴展失敗次數;c(·)為節(jié)點的空間成本, 這里用危險度進行表示;d(·)為內部向量的模.

2 EHT-RRT 算法

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

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

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

2.1.1 危險度索引和計算

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

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

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

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

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

其中,Csignal為節(jié)點到信號干擾源的距離,M為信號干擾源數量.

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

其中, (p1,p2,p3)為3 種危險的權重.

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

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

圖3(a) 顯示的為探索球的二維3 層結構, 共有8 個方向, 每個方向按半徑不同有3 個節(jié)點. 首層節(jié)點因為要用于路徑的局部節(jié)點滑移, 不宜過大, 所以設置為標準擴展步長d的1/3, 末層節(jié)點考慮到局部節(jié)點滑移帶來的動態(tài)擴展步長, 所以設置為標準步長的4/3,具體取值關系如式(6).

圖3 多層危險度探索球

抽出圖3(a)中的一層, 以任意方向每45°進行一次旋轉復制, 即可得到圖3(b)所示的探索球的三維單層結構, 共26 個方向. 完整結構的每個方向有3 個探索點, 則單一方向上的危險度為:

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

2.1.2 路徑啟發(fā)成本

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

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

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

2.1.3 總啟發(fā)成本

綜上, 由A*算法中的啟發(fā)式成本估計思想, 從滑移點帶來的方向危險度、路徑長度、路徑偏轉角度和高度變化4 個方面, 力求路徑的平滑、安全, 減少無人機的能耗, 總的啟發(fā)成本為:

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

2.2 局部節(jié)點滑移

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

圖5 局部滑移擴展

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

2.3 局部最好方向

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

在第2.1 節(jié)中, 可得到臨時新節(jié)點周圍26 個滑移點, 以及26 個方向的成本. 局部最好方向的選取過程偽碼如算法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

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

2.4 臨時新節(jié)點擴展

為了更好地應對復雜環(huán)境下的路徑擴展尋找, 本文結合了I-RRT 算法中目標點的概率性影響, 以及APF-RRT 的目標點全時刻影響, 結合上文提出的best_dir屬性, 對臨時新節(jié)點的擴展做出改進.

如圖6 所示, 同樣是通過隨機節(jié)點Xrand找到最近節(jié)點Xnear作為擴展基點. 算法首先讀取節(jié)點Xnear的局部最好方向best_dir, 然后按照式(10), 計算得出3 個方向上的標準步長擴展向量.

圖6 臨時新節(jié)點擴展

其中, (u1,u2,u3)為3 個向量的權重.

2.5 算法流程

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

圖7 EHT-RRT 算法流程圖

3 實驗仿真與分析

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

圖8 城市環(huán)境

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

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

3.1 仿真參數設置

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

探索球層數. 探索球層數越多, 越能準確了解空間中的危險情況, 但同時也會影響算法的效率. 不同層數探索球下的路徑搜索時間和路徑危險度變化如圖9 所示. 綜合兩項指標, 可以看出層數為3 時情況較好, 所以選取探索球層數為3.

圖9 不同層數的性能對比

路徑危險度的定義為, 最終路徑的平均路徑節(jié)點危險度與路徑長度的乘積, 計算公式為:

其中,N為最終路徑的節(jié)點數,L為路徑長度,Cxi為單一節(jié)點的危險度, 可由式(5)計算.

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

表1 不同危險權重選取

啟發(fā)函數權重. 啟發(fā)函數的設置, 可以從不同的側重點去影響路徑的規(guī)劃結果, 文中設置了危險度、路徑長度、偏轉角度和高度變化, 共4 個啟發(fā)量, 可采用對照實驗法來確定最終的權重值.

首先確定危險度的權重值. 方法為改變危險度的權重大小, 其余3 項均分剩余權重, 采用平均規(guī)劃時間、平均路徑危險度和平均路徑長度作為評判標準,則不同權重組合下的規(guī)劃結果如表2 所示.

表2 危險度啟發(fā)權重選取

從表2 中可以看出, 3 號實驗組的路徑規(guī)劃結果相對較好, 所以危險度的權重定為0.4.

接著確定路徑長度的權重值. 保持危險度權重不變, 改變路徑長度的權重, 偏轉角度和高度變化均分剩余權重, 選取同樣的評判標準, 實驗組別及結果如表3所示. 從表3 中可以看出, 2 號實驗組的各項結果都較好, 所以取路徑長度的權重為0.2.

表3 路徑長度啟發(fā)權重選取

同理, 從表4 和圖10 的結果可以確定偏轉角度和高度變化的權重分別為0.3, 0.1.

圖10 不同偏轉角度和高度變化權重的對比

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

偏轉角度為每兩條相鄰路徑線的水平偏角和豎直偏角之和; 高度變化為每相鄰兩個路徑點間的高度變化絕對值.

3 方向擴展權重. 隨機方向的設置, 有助于算法擺脫陷阱區(qū)域, 所以設置其權重為0.5, 同樣利用對照實驗法得到如表5 所示結果. 可以看出2 號試驗的規(guī)劃效果最好, 所以權重選定為(0.5, 0.2, 0.3).

表5 3 方向權重選取

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

表6 仿真參數設置

3.2 仿真結果與分析

3.2.1 多算法對比實驗

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

圖11 各算法仿真圖

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

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

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

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

表7 常規(guī)參數對比

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

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

圖13 不同算法路徑長度對比

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

表8 路徑抖動參數對比

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

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

圖14 不同算法的路徑抖動參數對比

3.2.2 消融對比實驗

為了驗證EHT-RRT 算法改進的有效性, 以環(huán)境1 為基礎, 針對算法的改進項進行了消融對比實驗.

因為算法的改進項間存在繼承關系(如: 啟發(fā)式成本探索的結果是局部節(jié)點滑移和局部最好方向的數據基礎; 局部最好方向應用于改進的節(jié)點擴展機制), 所以僅查看EHT-RRT 算法在局部節(jié)點滑移和局部最好方向這兩個方面的改進成效.

消融處理僅將對比項功能刪減, 整體的算法流程順序保持不變. 實驗仍然采取20 次的平均仿真結果作為對比數據, 具體如表9 所示.

表9 消融對比實驗

從表9 中數據可以看出, 完整的EHT-RRT 算法能提供更好的規(guī)劃效果. 消去局部節(jié)點滑移, 導致路徑長度、路徑危險度以及路徑的抖動性都增長較大; 消去局部最好方向導致路徑的擴展出現局部盲目性, 造成規(guī)劃時間較長. 可以分析出: 局部節(jié)點滑移策略能更好地提升路徑質量; 局部最好方向能更好地避免局部環(huán)境陷阱, 提升路徑搜索效率.

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

4 結論

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

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

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

(3)為節(jié)點添加局部最好方向屬性, 可讓每一次的節(jié)點擴展, 考慮當前節(jié)點的環(huán)境危險情況, 提高擴展質量和效率;

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

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