裂縫區(qū)域優(yōu)化隨機(jī)擴(kuò)展航路規(guī)劃方法

何仁珂, 魏瑞軒, 張啟瑞, 許卓凡, 周凱

(空軍工程大學(xué) 航空航天工程學(xué)院, 陜西 西安 710038)

裂縫區(qū)域優(yōu)化隨機(jī)擴(kuò)展航路規(guī)劃方法

何仁珂, 魏瑞軒, 張啟瑞, 許卓凡, 周凱

(空軍工程大學(xué) 航空航天工程學(xué)院, 陜西 西安 710038)

摘要:針對(duì)密集障礙代價(jià)空間中存在的局部裂縫區(qū)域,基于采樣的路徑規(guī)劃方法隨機(jī)生成的節(jié)點(diǎn)無(wú)法進(jìn)行有效擴(kuò)展,形成可行路徑的概率極低的問(wèn)題,采用電勢(shì)原理建立環(huán)境威脅模型,提出局部區(qū)域啟發(fā)模式轉(zhuǎn)換機(jī)制及節(jié)點(diǎn)一步檢測(cè)方法。采用路徑代價(jià)改進(jìn)Transition-based RRT算法的節(jié)點(diǎn)選擇機(jī)制,實(shí)現(xiàn)節(jié)點(diǎn)擴(kuò)展的雙啟發(fā)。仿真結(jié)果表明,該算法能夠有效地解決上述問(wèn)題,其算法性能和路徑生成優(yōu)于同類算法。

關(guān)鍵詞:航路規(guī)劃; 快速擴(kuò)展隨機(jī)樹(shù); 裂縫問(wèn)題

0引言

航路規(guī)劃是保證移動(dòng)機(jī)器人及無(wú)人飛行器等各類智能體安全飛行和執(zhí)行任務(wù)的基礎(chǔ)環(huán)節(jié)。隨著這一領(lǐng)域研究的不斷深入,對(duì)于航路規(guī)劃問(wèn)題的求解由簡(jiǎn)單約束條件向復(fù)雜約束條件延伸,由二維空間向高維空間拓展。這一趨勢(shì)對(duì)傳統(tǒng)航路規(guī)劃方法提出了巨大的挑戰(zhàn),特別在解決密集障礙空間航路規(guī)劃問(wèn)題上,傳統(tǒng)航路規(guī)劃算法如拓?fù)鋱D[1]、人工勢(shì)場(chǎng)法[2]、柵格法[3]、智能優(yōu)化算法[4-5]等在算法適用性和計(jì)算效率方面有諸多限制。以快速擴(kuò)展隨機(jī)樹(shù)[6](Rapidly exploring Random Tree,RRT)算法為代表的基于采樣的路徑規(guī)劃方法,在處理二維、多維空間的復(fù)雜約束規(guī)劃問(wèn)題方面有著廣泛的應(yīng)用[7];但由于該算法是建立在隨機(jī)采樣的基礎(chǔ)上的,因此路徑生成帶有很大的隨機(jī)性。為了更好地處理節(jié)點(diǎn)選擇問(wèn)題,更為復(fù)雜的代價(jià)地圖被應(yīng)用于解決路徑優(yōu)化問(wèn)題[8-11]。Karaman等[12]證明了RRT算法得到最優(yōu)解的概率為零。近年來(lái),研究人員又提出了許多改進(jìn)算法用于求解最優(yōu)或漸近最優(yōu)路徑。Li等[13]利用A*算法的啟發(fā)函數(shù)改進(jìn)了RRT算法,但無(wú)法避免局部最小問(wèn)題。Lee等[14]從控制節(jié)點(diǎn)原始生成的角度減小了算法的計(jì)算代價(jià)。文獻(xiàn)[15-16]提出將改進(jìn)的RRT方法應(yīng)用于二維空間的動(dòng)態(tài)環(huán)境。Leonard等[17]提出Transition-based RRT(T-RRT)算法,利用隨機(jī)優(yōu)化的方法引導(dǎo)節(jié)點(diǎn)向低代價(jià)空間擴(kuò)展,其性能明顯優(yōu)于一般啟發(fā)的RRT算法。隨機(jī)生成的節(jié)點(diǎn),無(wú)法保證有效擴(kuò)展到高代價(jià)區(qū)域中存在的局部低代價(jià)裂縫區(qū)域,而在實(shí)際任務(wù)中,這一區(qū)域是縮短航路、執(zhí)行縱深突防打擊任務(wù)的重要“安全走廊”,有著極為重要的戰(zhàn)場(chǎng)價(jià)值。Berenson等[18]提出Gradien T-RRT算法,采用局部梯度下降法引導(dǎo)節(jié)點(diǎn)穿越裂逢區(qū)域;但是梯度下降方向是單一的,在代價(jià)空間中解決該類問(wèn)題時(shí),結(jié)果并非均優(yōu)于T-RRT算法。

本文運(yùn)用電學(xué)中關(guān)于空間電場(chǎng)和電勢(shì)分布原理構(gòu)建了飛行威脅場(chǎng),構(gòu)成代價(jià)空間,提出了局部區(qū)域啟發(fā)模式轉(zhuǎn)換機(jī)制和一步檢測(cè)方法。在局部區(qū)域擴(kuò)展受限的情況下,采用A*算法啟發(fā)引導(dǎo)節(jié)點(diǎn)對(duì)T-RRT算法在全局和局部進(jìn)行雙啟發(fā),實(shí)現(xiàn)路徑節(jié)點(diǎn)在局部裂縫區(qū)域的有效擴(kuò)展。

1基于電勢(shì)原理構(gòu)建飛行威脅場(chǎng)

傳統(tǒng)的建模方法通常是以威脅或障礙的邊界確定威脅半徑,建立圓形或球形的威脅模型。由于威脅的影響與相對(duì)距離有密切聯(lián)系,因此這種方法無(wú)法有效地描述與距離有關(guān)的威脅程度。本文利用電學(xué)中有關(guān)電荷激發(fā)形成電場(chǎng)原理[19]建立以威脅點(diǎn)、目標(biāo)點(diǎn)為元素的威脅場(chǎng),通過(guò)電勢(shì)公式對(duì)電場(chǎng)強(qiáng)度和距離的關(guān)系表達(dá),較好地表征了飛機(jī)逼近威脅的緊迫度,以此構(gòu)建代價(jià)空間。

假設(shè)有n個(gè)點(diǎn)電荷q1,q2,…,qn,根據(jù)電勢(shì)場(chǎng)疊加原理,在點(diǎn)P處激發(fā)形成的電勢(shì)為:

(1)

式中：ri為P點(diǎn)與qi的距離。

電場(chǎng)強(qiáng)度為:

(2)

設(shè)qoi為第i個(gè)威脅,威脅均為正電荷,目標(biāo)點(diǎn)qt為負(fù)電荷。引入目標(biāo)點(diǎn)后,電勢(shì)場(chǎng)變化為:

(3)

式中：rt為目標(biāo)點(diǎn)與飛行器的距離。電場(chǎng)強(qiáng)度的大小取決取決于激發(fā)電場(chǎng)的電荷,與電場(chǎng)中的受力電荷無(wú)關(guān)。設(shè)飛行器為受力試探電荷,不影響電勢(shì)分布,以此建立完整的代價(jià)空間。

2T-RRT算法

首先定義標(biāo)準(zhǔn)RRT變量。對(duì)于空間C,存在若干障礙區(qū)域。設(shè)Cfree為一非障礙的自由狀態(tài)空間,Cfree∈C;Tk為一個(gè)具有K個(gè)節(jié)點(diǎn)的擴(kuò)展樹(shù),x為Tk的節(jié)點(diǎn),且Tk∈Cfree;qinit為起點(diǎn),qgoal為終點(diǎn);令qrand為非障礙區(qū)空間的一個(gè)隨機(jī)采樣點(diǎn),即qrand∈Cfree;qnear為樹(shù)中找到距離qrand最近的節(jié)點(diǎn)。設(shè)p,q∈Cfree,令d(p,q)為狀態(tài)點(diǎn)的距離,則d(qnear,qrand)≤d(q,qrand)。在qrand與qnear兩點(diǎn)直線間取qnew,qnew∈Cfree,并滿足d(qnear,qnew)=ρ,其中ρ>0,為樹(shù)生長(zhǎng)的最小單位長(zhǎng)度,稱為步長(zhǎng)。

T-RRT算法能較好地解決在多維代價(jià)空間尋找合適低代價(jià)路徑的問(wèn)題。其主要思想是利用RRT算法向全局?jǐn)U展的優(yōu)勢(shì),結(jié)合Monte Carlo優(yōu)化和模擬退火的方法判斷是否接受新的節(jié)點(diǎn),形成類似于隨機(jī)優(yōu)化方法的轉(zhuǎn)換檢測(cè)(Transition Tests)的判斷機(jī)制,在代價(jià)空間中運(yùn)用最小機(jī)械功的思想計(jì)算出代價(jià)最小的路徑。當(dāng)新節(jié)點(diǎn)為低代價(jià)點(diǎn),則加入擴(kuò)展樹(shù)中;若為高代價(jià)點(diǎn),則根據(jù)Metropolis準(zhǔn)則函數(shù)判斷是否能加入,取決于函數(shù)中溫度T的值:

(4)

T的取值隨擴(kuò)展失敗的次數(shù)nfailmax而調(diào)整,拒絕次數(shù)越多,則溫度值越高,接受的概率越小,排斥高代價(jià)點(diǎn),較好地處理了路徑節(jié)點(diǎn)尋優(yōu)與擴(kuò)展的關(guān)系。

3優(yōu)化隨機(jī)擴(kuò)展算法

基于采樣方式的T-RRT算法在選擇路徑節(jié)點(diǎn)上有很大隨機(jī)性。雖然該算法從機(jī)械功的角度對(duì)所有節(jié)點(diǎn)選擇進(jìn)行概率檢驗(yàn),確保篩選出低代價(jià)節(jié)點(diǎn);但對(duì)于在高代價(jià)區(qū)域中存在的局部低代價(jià)空間無(wú)法進(jìn)行有效的節(jié)點(diǎn)搜索,樹(shù)擴(kuò)展的概率極低。從根本上來(lái)講,基于采樣的方法的隨機(jī)性在該問(wèn)題上存在固有缺陷,因此,本文提出利用A*算法在低代價(jià)點(diǎn)上的搜索優(yōu)勢(shì)來(lái)改進(jìn)T-RRT算法在裂縫問(wèn)題上的不足。A*算法并不依賴于基于采樣的方法獲得節(jié)點(diǎn),節(jié)點(diǎn)生成不受裂縫區(qū)域限制。值得注意的是,對(duì)于復(fù)雜密集阻礙區(qū)域,尤其是局部低代價(jià)區(qū)域,如何有效地解決有啟發(fā)與局部最小的矛盾是關(guān)鍵問(wèn)題。這就需要合理地將Metropolis準(zhǔn)則函數(shù)與A*算法啟發(fā)函數(shù)結(jié)合起來(lái),從而在有效引導(dǎo)節(jié)點(diǎn)穿越裂縫區(qū)域的同時(shí),保證避免局部最小問(wèn)題。

首先進(jìn)行威脅環(huán)境建模,將威脅設(shè)為正電荷,目標(biāo)點(diǎn)設(shè)為負(fù)電荷,飛行器為正的點(diǎn)電荷,構(gòu)建空間C。擴(kuò)展樹(shù)Tk進(jìn)行隨機(jī)節(jié)點(diǎn)qrand的生成,然后進(jìn)行碰撞檢測(cè),當(dāng)節(jié)點(diǎn)出現(xiàn)在高電勢(shì)區(qū)時(shí),判定該節(jié)點(diǎn)位于威脅區(qū)域。當(dāng)qrand?Cfree時(shí),計(jì)數(shù)器加1,否則進(jìn)入轉(zhuǎn)換檢測(cè),根據(jù)相鄰節(jié)點(diǎn)的電勢(shì)差選擇符合條件的qnear節(jié)點(diǎn);當(dāng)d(qnear,qrand)≤d(q,qrand)時(shí),以步長(zhǎng)ρ取qnew。檢測(cè)條件為:

(5)

式中:cparent為擴(kuò)展出qnew的父節(jié)點(diǎn)的代價(jià);(cparent-cnew)/dij為代價(jià)斜率。當(dāng)cnew代價(jià)低于cparent時(shí)接受該節(jié)點(diǎn),否則當(dāng)rand(0,1)

當(dāng)計(jì)數(shù)器達(dá)到門限值nfail>nfailmax時(shí)停止計(jì)數(shù),進(jìn)入局部啟發(fā)模式。以相同步長(zhǎng)ρ計(jì)算當(dāng)前節(jié)點(diǎn)qparent與周圍8個(gè)節(jié)點(diǎn)的相對(duì)代價(jià)值。估價(jià)函數(shù)為:

(6)

(7)

式中:g(n)為路徑長(zhǎng)度;h(n)為當(dāng)前節(jié)點(diǎn)qparent與目標(biāo)節(jié)點(diǎn)qgoal的歐氏距離;(xi,yi)為周圍節(jié)點(diǎn)。

接下來(lái)進(jìn)行節(jié)點(diǎn)擴(kuò)展測(cè)試。如果h(qparent,qgoal)135°,以此為條件篩選節(jié)點(diǎn)。

4仿真試驗(yàn)及結(jié)果分析

建立局部復(fù)雜密集障礙區(qū)域飛行威脅場(chǎng),將本文算法與RRT, T-RRT, Gradien T-RRT進(jìn)行對(duì)比分析,以檢驗(yàn)算法在復(fù)雜密集障礙環(huán)境以及裂縫區(qū)域的性能。仿真試驗(yàn)環(huán)境:軟件Matlab 7.0;計(jì)算機(jī)配置:Windows XP操作系統(tǒng)、CPU為Inter Core i3、主頻3.3 GHz。仿真條件:設(shè)定11處威脅點(diǎn),威脅強(qiáng)度由電量大小確定,電量為+1 C和+2 C,目標(biāo)點(diǎn)電量為-10 C;飛行器為元電荷,電量為+1 C。飛行威脅場(chǎng)大小為50 km×50 km,設(shè)K為1,ρ=1,ε0=1/(4π),α≥135°。試驗(yàn)結(jié)果如圖1所示。

圖1　各種算法路徑規(guī)劃結(jié)果Fig.1　Planning results of various algorithms

由圖1可以看出,對(duì)于密集障礙區(qū)域,各算法均能找到從起點(diǎn)到目標(biāo)點(diǎn)的路徑。RRT算法的隨機(jī)擴(kuò)展具有未知搜索優(yōu)勢(shì),幾乎遍布全局;但并不能很好地?cái)U(kuò)展到裂縫區(qū)域,路徑隨機(jī)性強(qiáng),沒(méi)有考慮運(yùn)動(dòng)約束條件。T-RRT算法能根據(jù)代價(jià)大小在全局中選擇合適的擴(kuò)展節(jié)點(diǎn),從而路徑擴(kuò)展有很好的指向性,當(dāng)在威脅區(qū)域擴(kuò)展失敗時(shí)可退出重點(diǎn)選取節(jié)點(diǎn),很好地避開(kāi)威脅所在的高代價(jià)區(qū)域。Gradien T-RRT算法在密集障礙區(qū)域進(jìn)行了有效地?cái)U(kuò)展試探,路徑在梯度引導(dǎo)下有良好的方向性;但由于梯度方向的限制,在靠近威脅時(shí)梯度易受威脅勢(shì)能的影響,沒(méi)有成功通過(guò)裂縫區(qū)域。本文算法能很好地穿越裂縫區(qū)域,且路徑擴(kuò)展有很好的方向性,局部啟發(fā)能有效引導(dǎo)節(jié)點(diǎn)通過(guò)密集障礙區(qū)域。

為了更加清晰地反映上述算法在計(jì)算時(shí)間和路徑代價(jià)上的優(yōu)劣,本文設(shè)置最大擴(kuò)展失敗次數(shù)nfailmax為10～80次,每一層級(jí)在相同的條件下進(jìn)行10次試驗(yàn),結(jié)果如圖2所示。由于RRT算法不具有此項(xiàng)性能,故不進(jìn)行比較。

圖2　各種算法的計(jì)算時(shí)間和路徑代價(jià)Fig.2　Computing time and path cost of various algorithms

由圖2可以看出,在相同條件下,T-RRT算法的計(jì)算時(shí)間和路徑代價(jià)最大,且計(jì)算時(shí)間隨nfailmax的增加逐漸變大,而對(duì)其他兩種算法的影響不大。這是由于在密集障礙區(qū)域,T-RRT算法選擇低代價(jià)節(jié)點(diǎn)的難度增大,擴(kuò)展溫度不斷升高,隨著nfailmax的增加,在同一節(jié)點(diǎn)擴(kuò)展次數(shù)增加,計(jì)算時(shí)間隨之提高;路徑代價(jià)下降是因?yàn)閚failmax增加,能夠保證選擇合適節(jié)點(diǎn)的概率增大,生成的路徑代價(jià)降低。Gradien T-RRT和本文算法由于在節(jié)點(diǎn)擴(kuò)展失敗后進(jìn)行了不同模式的節(jié)點(diǎn)增長(zhǎng)機(jī)制,從而在根本上改變了隨機(jī)擴(kuò)展的節(jié)點(diǎn)生成方式,因此計(jì)算時(shí)間和路徑代價(jià)受nfailmax影響較小。對(duì)于密集障礙區(qū)域生成的路徑，本文算法在計(jì)算時(shí)間和代價(jià)方面優(yōu)于其他兩種方法。

5結(jié)束語(yǔ)

本文針對(duì)基于采樣的路徑規(guī)劃方法在密集障礙區(qū)域裂縫問(wèn)題上的不足,在建立新型威脅模型的基礎(chǔ)上,提出用A*算法改進(jìn)T-RRT算法,從而有效地解決了算法在高代價(jià)局部區(qū)域節(jié)點(diǎn)擴(kuò)展困難的問(wèn)題,較好地實(shí)現(xiàn)了復(fù)雜密集障礙“雙啟發(fā)式”引導(dǎo)。與傳統(tǒng)威脅模型相比,該威脅模型能有效表征威脅與相對(duì)距離的關(guān)系,更具有應(yīng)用價(jià)值。本文的不足在于“一步檢測(cè)”時(shí)需進(jìn)行兩次節(jié)點(diǎn)篩選,在數(shù)據(jù)規(guī)模較大時(shí)影響計(jì)算速度。下一步將對(duì)路徑規(guī)劃實(shí)時(shí)性和計(jì)算效率方面進(jìn)行改進(jìn)。

參考文獻(xiàn)：

[1]Israel L,Flores G,Salazar S,et al.Dubins path generation for a fixed wing UAV[C]//International Conference on Unmanned Aircraft Systems (ICUAS).Orlando:IEEE,2014:339-346.

[2]Tingbin C,Qisong Z.Robot motion planning based on improved artificial potential field[C]//2013 3rd International Conference on Computer Science and Network Technology (ICCSNT).Dalian:IEEE,2013:1208-1211.

[3]Robert J S,Peggy G I S,Clickstein,et al.Robust algorithm for algorithm for real-time route planning[J].IEEE Transaction on Aerospace and Electronic System,2000,36(3):869-878.

[4]Tuncer A,Yildirim M.Dynamic path planning of mobile robots with improved genetic algorithm[J].Computer Electrical Engineering,2012,38(6):1564-1572.

[5]Chaari I,Koubaa A,Bennaceur H,et al.SmartPATH:a hybrid ACO-GA algorithm for robot path planning[C]//2012 IEEE Congress on Evolutionary Computation (CEC).Brisbane:IEEE,2012:1-8.

[6]Lavalle S M,Kuffner J J.Randomized kinodynamic planning[J].International Journal of Robotics Research,2001,20(3):378-400.

[7]Erion P,Kostas E B,Brjan Y C,et al.Samplng-based roadmap of trees for parallel motion planning[J].IEEE Transactions on Robotics,2005,21(4):597-608.

[8]Urmson C,Simmons R.Approaches for heuristically biasing RRT growth[C]//Proceedings of the IEEE/RSJ International Conference on Intelligent Robotics and Systems (Volume:2).Las Vegas:IEEE,2003:1178-1183.

[9]Lee J,Pippin C,Balch T.Cost based planning with RRT in outdoor environment[C]//IEEE/RSJ International Conference on Intelligent Robots and Systems.Nice:IEEE,2008:684-689.

[10]Ettlin A,Bleuler H.Rough-terrain robot motion planning based on obstacleness[C]//9th International Conference on Control,Automation,Robotics and Vision.Singapore:IEEE,2006:1-6.

[11]Xu Y,Choi J,Oh S.Mobile sensor network navigation using Gaussian processes with truncated observations[J].IEEE Transactions on Robotics,2011,27(6):1118-1131.

[12]Karaman S,Frazzoli E.Sampling-based algorithms for optimal motion planning[J].International Journal of Robotics Research,2011,30(7):846-894.

[13]Li J,Liu S,Zhang B.RRT-A*motion planning algorithm for non-holonomic mobile robot[C]//2014 Proceedings of the SICE Annual Conference.Sapporo:IEEE,2014:1833-1838.

[14]Lee D,Shim D H.Spline-RRT*based optimal path planning of terrain following flights for fixed-wing UAVs[C]//2014 11th International Conference on Ubiquitous Robots and Ambient Intelligence (URAI 2014) Hilton.Kuala Lumpur:IEEE,2014:257-261.

[15]Svenstrup M,Bak T,Andersen H J.Trajectory planning for robots in dynamic human environments [C]//2010 IEEE/RSJ International Conference on Intelligent Robots and Systems.Taipei:IEEE,2010:4293-4298.

[16]Jaillet L,Hoffman J,Berg J,et al.EG-RRT:environment-guided random trees for kinodynamic motion planning with uncertainty and obstacles [C]//IEEE/RSJ International Conference on Intelligent Robots and Systems.San Francisco:IEEE,2011:2646-2652.

[17]Leonard J,Juan C,Thierry S. Sampling-based path planning on configuration-space costmaps[J].IEEE Transaction on Robotics,2010,26(4):635-646.

[18]Berenson D,Simeon T,Srinivasa S S.Addressing cost-space chasms in manipulation planning[C]//2011 International Conference on Robotics and Automation.Shanghai:IEEE,2011:4561-4568.

[19]馬文蔚.物理學(xué)[M].第5版.北京:高等教育出版社,2006:149-186.

(編輯：李怡)

Addressing chasm in optimization rapidly exploring path planning

HE Ren-ke, WEI Rui-xuan, ZHANG Qi-rui, XU Zhuo-fan, ZHOU Kai

(Aeronautics and Astronautics Engineering College, AFEU, Xi’an 710038, China)

Abstract:Narrow low-cost regions called chasm exits in dense obstacle cost space; it is difficult for sampling-based methods to extend nodes in this area. This paper built model based on electric potential, proposed local heuristic mechanism and step detection method, improved Transition-based RRT algorithm by path cost function. The results show that the proposed method could effectively solve the problem mentioned above, its algorithm performance and path generation method are better than others.

Key words:path planning; rapidly exploring random tree; chasm problem

收稿日期:2015-09-07;

修訂日期:2015-11-28; 網(wǎng)絡(luò)出版時(shí)間:2016-02-29 16:37

基金項(xiàng)目:國(guó)家自然科學(xué)基金資助(61573373)；航空科學(xué)基金資助(20135896027)

作者簡(jiǎn)介:何仁珂(1993-)，男，河南信陽(yáng)人，碩士研究生，主要研究方向?yàn)闊o(wú)人機(jī)自主控制。

中圖分類號(hào)：V249.1

文獻(xiàn)標(biāo)識(shí)碼：A

文章編號(hào)：1002-0853(2016)03-0026-04