Risk Theta*:一種基于地形危險(xiǎn)度的任意航向路徑規(guī)劃算法

王瓊,于登云,賈陽(yáng)

(1.探月與航天工程中心,北京100037;2.北京空間飛行器總體設(shè)計(jì)部,北京100094; 3.中國(guó)航天科技集團(tuán)公司,北京100048)

Risk Theta*:一種基于地形危險(xiǎn)度的任意航向路徑規(guī)劃算法

王瓊1,2,于登云3,賈陽(yáng)2

(1.探月與航天工程中心,北京100037;2.北京空間飛行器總體設(shè)計(jì)部,北京100094; 3.中國(guó)航天科技集團(tuán)公司,北京100048)

提出了一種基于地形危險(xiǎn)度的任意航向路徑規(guī)劃算法——Risk Theta*。首先以星球表面地形特征統(tǒng)計(jì)分析為基礎(chǔ)提出了地形危險(xiǎn)度指標(biāo),并建立地形危險(xiǎn)度地圖。在此基礎(chǔ)上應(yīng)用Basic Theta*搜索,以危險(xiǎn)度最低為方向搜索最優(yōu)路徑。仿真實(shí)驗(yàn)證明,該算法能夠在柵格地圖上找到比A*和Basic Theta*算法危險(xiǎn)度低得多、長(zhǎng)度相當(dāng)?shù)娜我夂较蚵窂?既顯著提高了巡視器的安全性,又滿足了星球巡視探測(cè)對(duì)任意航向行駛的迫切需求,因此具有較強(qiáng)的實(shí)用性。

Theta*;地形危險(xiǎn)度;路徑規(guī)劃;任意航向;啟發(fā)式搜索

0 引 言

星球巡視器的全局路徑規(guī)劃是指在已知的星球表面非結(jié)構(gòu)化復(fù)雜環(huán)境中,尋找一條從起始點(diǎn)到目標(biāo)點(diǎn)的最優(yōu)或近似最優(yōu)的無(wú)碰路徑,以實(shí)現(xiàn)巡視器的安全移動(dòng)。全局路徑規(guī)劃算法大多采用基于幾何模型搜索的思路,首先構(gòu)建巡視器環(huán)境空間的幾何模型,在此基礎(chǔ)上采用某種圖搜索算法,得到最優(yōu)路徑。幾何模型構(gòu)建主要有路徑圖法和單元分解法兩類,其中路徑圖法和精確單元分解法的最大缺點(diǎn)是圖的構(gòu)建過(guò)程比較復(fù)雜,建模成本較高;而柵格法是一種近似單元分解法,十分便于創(chuàng)建、存儲(chǔ)和使用,如數(shù)字高程圖(DEM)就是一種廣泛應(yīng)用的柵格地圖。圖搜索算法包括寬度優(yōu)先、深度優(yōu)先、迭代加深、Dijkstra、A*等各種算法,其中以A*算法為代表的啟發(fā)式算法具有搜索效率高、容易實(shí)現(xiàn)、算法完備等優(yōu)點(diǎn)。因此,基于柵格地圖的啟發(fā)式算法得到了廣泛的研究和應(yīng)用。

但是,A*算法在柵格地圖中的搜索方向和路徑方向受到相鄰8個(gè)柵格的限制,無(wú)法得到最優(yōu)路徑。而規(guī)劃出任意航向的最短路徑恰恰是星球巡視探測(cè)、野外車輛導(dǎo)航、計(jì)算機(jī)游戲等領(lǐng)域的迫切要求。為了解決這一問(wèn)題,在A*算法基礎(chǔ)上,研究人員提出了一些改進(jìn)算法。

A*with post-smoothing paths算法[1]對(duì)A*算法結(jié)果進(jìn)行后處理,得到更平滑路徑,但是不能保證找到最短路徑。Field D*算法[2]采用線性插值,使得路徑點(diǎn)不局限于柵格中心或四角,消除了相鄰柵格之間的離散狀態(tài)轉(zhuǎn)移所帶來(lái)的路徑不平滑問(wèn)題,但是路徑中經(jīng)常存在不必要的轉(zhuǎn)向。Block A*算法[3]分塊預(yù)存事先計(jì)算好的塊內(nèi)各邊界點(diǎn)之間距離,并按塊進(jìn)行操作和搜索,能夠得到任意方向的路徑,這種以空間換時(shí)間的策略極大地提高了計(jì)算速度,但是按塊拼接的路徑往往也不是最短路徑。A Nash等人結(jié)合A*算法和可視圖法的優(yōu)點(diǎn),提出了Basic Theta*和Angle-Propagation Theta*(統(tǒng)稱為Theta*)算法[4],通過(guò)引入父節(jié)點(diǎn)和后繼節(jié)點(diǎn)間的可視性檢查來(lái)尋找捷徑,可以找到任意方向的路徑,并且在計(jì)算效率和最優(yōu)性方面都有很好的表現(xiàn)。

本文在地形特征統(tǒng)計(jì)分析基礎(chǔ)上,設(shè)計(jì)了地形危險(xiǎn)度指標(biāo),提出了一種以危險(xiǎn)度最低為優(yōu)化方向的Theta*改進(jìn)算法——Risk Theta*。

1 Basic Theta*算法

Basic Theta*算法與A*算法的主要差別在于擴(kuò)展節(jié)點(diǎn)時(shí)的程序。Basic Theta*在A*的基礎(chǔ)上,將當(dāng)前節(jié)點(diǎn)s的相鄰節(jié)點(diǎn)s′與s的父節(jié)點(diǎn)sparent做一次可視性檢查,若可視,則比較從sparent直接到s′和從sparent經(jīng)s到s′這兩條路徑的代價(jià),選擇代價(jià)更小的路徑。

A*和Basic Theta*擴(kuò)展節(jié)點(diǎn)程序分別見算法1和算法2。其中,OPEN為一個(gè)用于存放當(dāng)前待擴(kuò)展節(jié)點(diǎn)的列表,CLOSED為一個(gè)用于存放已經(jīng)訪問(wèn)過(guò)節(jié)點(diǎn)的列表,LineOfSight(s,s′)表示判斷s與s′是否可視,Insert(·)和Remove(·)分別表示從列表中插入節(jié)點(diǎn)和移出節(jié)點(diǎn)。

但是,Basic Theta*算法僅僅將柵格簡(jiǎn)單區(qū)分為可通行和障礙,并且只在可通行柵格中搜索最優(yōu)路徑。但實(shí)際上在星球表面復(fù)雜地形環(huán)境中,即便是可通行柵格也存在著地形高低起伏帶來(lái)的行駛危險(xiǎn)度差異,這是不可忽視的。若長(zhǎng)期在這些高危險(xiǎn)度柵格上行駛,會(huì)導(dǎo)致巡視器磨損加重,甚至陷入困境。因此,在星球表面巡視探測(cè)任務(wù)中,基于二值障礙判斷的Theta*算法的路徑解安全性較差,應(yīng)用效果不佳。

算法1 A*算法

算法2 Basic Theta*算法

2 Risk Theta*算法

眾所周知,人類在陌生野外環(huán)境中進(jìn)行探險(xiǎn)時(shí)的本能反應(yīng)是保持高度警惕,實(shí)時(shí)評(píng)估自身所處危險(xiǎn)程度,及時(shí)應(yīng)對(duì)各種威脅。類比人類的這種行為決策方式,由于任務(wù)機(jī)會(huì)極其難得、造價(jià)昂貴等原因,巡視器在星球表面非結(jié)構(gòu)化未知環(huán)境中進(jìn)行探測(cè)時(shí)也應(yīng)遵循安全至上的法則,其次才考慮追求更短的距離和更高的效率?；谶@一思路,本文提出地形危險(xiǎn)度指標(biāo),來(lái)刻畫巡視器行駛在某一地形上所面臨的危險(xiǎn)程度,作為Theta*搜索的優(yōu)化方向。

2.1 地形特征分析

在定義地形危險(xiǎn)度指標(biāo)之前,先進(jìn)行地形分析,首要工作是提取坡面地形因子。地形因子包括微觀和宏觀兩大類,前者反映了微觀地表單元的形態(tài)、起伏或扭曲特征,后者反映了地貌的宏觀形態(tài)特征[5]?？紤]到與巡視器姿態(tài)穩(wěn)定性、越障能力等的相關(guān)性,本文選取了坡度這個(gè)微觀因子和地形粗糙度、地形起伏度兩個(gè)宏觀因子進(jìn)行計(jì)算。

提取地形因子通常需要開辟一個(gè)有固定分析半徑的分析窗口(可稱為地塊),并且在窗口內(nèi)進(jìn)行統(tǒng)計(jì)計(jì)算。按照巡視器車體尺寸劃分地塊,可以在計(jì)算量和精度之間達(dá)到較好的平衡。

1)坡度

坡度表示月表面在某一點(diǎn)的傾斜程度,在數(shù)值上等于過(guò)該點(diǎn)的月表微分單元的法矢量ni,j與坐標(biāo)系z(mì)軸單位矢量z的夾角,即

假設(shè)該月表微分單元的擬合平面方程為z= Ax+By+C,則平面法矢量為ni,j={A,B,-1},z軸單位矢量為z={0,0,1},故有

其取值范圍為0°～90°。

2)地形粗糙度

地形粗糙度是反映地形的起伏變化和侵蝕程度的指標(biāo),一般定義為地表單元的曲面面積S曲面與其在水平面上的投影面積S水平之比,即

地形粗糙度可以通過(guò)擬合殘差來(lái)計(jì)算,即地塊內(nèi)所有點(diǎn)與擬合平面高程偏差的平方和

式中:n為地塊內(nèi)的柵格數(shù)。

3)地形起伏度

通常地形起伏度定義為地塊內(nèi)最大高程與最小高程之差,但這樣的定義難以區(qū)分是斜坡還是障礙造成的地形起伏。為了刻畫障礙的影響,本文將地形起伏度定義為地塊內(nèi)每?jī)蓚€(gè)相鄰柵格的高程差的最大值。

式中:p表示除最外圈柵格外的當(dāng)前地塊柵格集合。

2.2 地形危險(xiǎn)度評(píng)估

目前星球巡視器大多采用輪式結(jié)構(gòu),星球表面地形對(duì)輪式巡視器移動(dòng)主要造成斜坡、顛簸、臺(tái)階三種危險(xiǎn)。其中,斜坡超過(guò)巡視器爬坡能力,容易造成車體傾覆;顛簸主要造成巡視器姿態(tài)大幅度變化,容易與星球表面發(fā)生碰撞,且降低車輪之間的協(xié)調(diào)性,甚至將其陷住;臺(tái)階是星球表面上的突起或深坑,容易造成車體與星球表面碰撞甚至被卡住。

1)斜坡

斜坡危險(xiǎn)度定義為

式中:θth為巡視器允許的最大爬坡能力;K1為加權(quán)系數(shù)。

2)顛簸

巡視器行駛在粗糙度為r的地塊上的危險(xiǎn)度為

式中:rmax為所有柵格粗糙度的最大值;K2為加權(quán)系數(shù)。

(3)臺(tái)階

巡視器行駛在起伏度為D的地塊上的危險(xiǎn)度為

式中:Dmax為所有柵格起伏度的最大值;K3為加權(quán)系數(shù)。

為統(tǒng)一尺度便于比較,取K1=K2=K3=4,將這三種障礙危險(xiǎn)度均正規(guī)化到[1,5]之間。那么,地形危險(xiǎn)度定義為

值得一提的是,盡管危險(xiǎn)度是以地塊為單位進(jìn)行計(jì)算的,但它僅被賦予該地塊的中心柵格,而不是整個(gè)地塊。也就是說(shuō),每一個(gè)柵格(xi,yi)都有其對(duì)應(yīng)的滑動(dòng)分析窗口和相應(yīng)地形危險(xiǎn)度R(xi,yi),如圖1所示。

圖1 地塊劃分及相應(yīng)的地形危險(xiǎn)度Fig.1 Patch division and related terrain risk

2.3 估價(jià)函數(shù)的選取

Risk Theta*的算法流程與Basic Theta*相同,但在估價(jià)函數(shù)中引入了地形危險(xiǎn)度。與A*、Basic Theta*類似,Risk Theta*的估價(jià)函數(shù)為

式中:g(s)為從起點(diǎn)s0到當(dāng)前點(diǎn)s的實(shí)際代價(jià)函數(shù); h(s)為從s到目標(biāo)點(diǎn)sgoal的啟發(fā)函數(shù)。

c(s,s′)取為從s到s′的連線所途經(jīng)柵格(即圖2中灰色柵格)的危險(xiǎn)度的加權(quán)和。

圖2 代價(jià)函數(shù)計(jì)算Fig.2 Calculation of cost function

式中:Ri為途經(jīng)的第i個(gè)柵格的地形危險(xiǎn)度;ωi為相應(yīng)權(quán)重。若連線在某個(gè)柵格的進(jìn)出點(diǎn)分別位于柵格的兩條對(duì)邊上,如圖2中柵格(4,3)、(7,5)和(8, 7)的情形,則ωi取1,否則取1/2。

c(s,s′)的具體計(jì)算借鑒了Bresenham畫線算法[6]的思路,采用大量邏輯和整數(shù)操作,計(jì)算速度很快。例如,圖2中的分別為

g(s)為從s0到s的各段實(shí)際路徑的代價(jià)函數(shù)和。

為保證可納性(admissible),h(s)取為從s到sgoal的歐氏距離與最小危險(xiǎn)度的乘積。

式中:Rmin是所有柵格的最小危險(xiǎn)度,很顯然Rmin=1。

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

本文在matlab7.0環(huán)境下對(duì)A*、Basic Theta *和Risk Theta*算法進(jìn)行比對(duì)實(shí)驗(yàn)。參考張伍等提出的直徑隨機(jī)生成方法[7],生成尺寸50 m× 50 m、分辨率20 cm、網(wǎng)格數(shù)250×250的模擬月面DEM圖,如圖3所示。在此基礎(chǔ)上根據(jù)前文所述方法生成地形危險(xiǎn)度地圖,如圖4的背景圖所示。

參考我國(guó)首個(gè)月面巡視器“玉兔號(hào)”設(shè)定仿真用巡視器模型的參數(shù)[8,9]:收攏狀態(tài)包絡(luò)尺寸為1.5 m×1.0 m×1.1 m,車輪直徑30 cm,最大爬坡能力20°,越障能力20 cm。將每7×7個(gè)柵格擬合成1.4 m×1.4 m、與車體尺寸相當(dāng)?shù)牡貕K。

在非障礙區(qū)域中隨機(jī)設(shè)置起始點(diǎn)和目標(biāo)點(diǎn),分別采用A*算法、Basic Theta*算法與本文提出的Risk Theta*算法進(jìn)行10次仿真實(shí)驗(yàn),三種算法的仿真實(shí)驗(yàn)結(jié)果比較見表1,單次仿真結(jié)果路徑見圖4。其中,Risk Theta*的路徑總危險(xiǎn)度比A*平均減小了37.0%,路徑總長(zhǎng)度平均僅增加10.1%;路徑總危險(xiǎn)度比Basic Theta*平均減小了30.4%,路徑總長(zhǎng)度平均僅增加15.9%。特別地,Risk Theta*在單位長(zhǎng)度上的危險(xiǎn)度比A*和Basic Theta*分別平均減小42.2%、39.5%。

圖3 模擬月面DEMFig.3 Simulated DEM of lunar surface

圖4 A*、Basic Theta*與Risk Theta*算法的單次仿真結(jié)果Fig.4 Single simulation results of A*,Basic Theta*and Risk Theta*algorithms

從仿真結(jié)果可以看出,Risk Theta*算法得到路徑的長(zhǎng)度比A*和Basic Theta*雖略有增加,但總危險(xiǎn)度和在每一個(gè)柵格上的危險(xiǎn)度大大減小,因此要安全得多。

4 結(jié) 論

本文提出的Risk Theta*算法,以星球表面地形特征統(tǒng)計(jì)分析為基礎(chǔ)提出了地形危險(xiǎn)度指標(biāo),建立了地形危險(xiǎn)度地圖,并以危險(xiǎn)度最低為方向搜索任意航向路徑,能夠有效解決星球表面復(fù)雜地形環(huán)境中基于二值障礙判斷的Theta*算法路徑解安全性較差的問(wèn)題。仿真實(shí)驗(yàn)證明,該算法能夠在柵格地圖上找到比A*和Theta*算法危險(xiǎn)度低得多、長(zhǎng)度相當(dāng)?shù)娜我夂较蚵窂?既顯著提高了巡視器的安全性,又滿足了星球表面巡視對(duì)任意航向行駛的迫切需求,因此具有較強(qiáng)的實(shí)用性。

表1 A*、Basic Theta*和Risk Theta*算法的仿真結(jié)果比較Table 1 Comparison among simulation results of A*,Basic Theta*and Risk Theta*algorithms

[1]Thorpe C.Path relaxation:path planning for a mobile robot [C]∥The 4th National Conference on Artificial Intelligence. Austin,Texas,USA:[s.n.]:1984.

[2]Ferguson D,Stentz A.Field D*:an interpolation-based path planner and replanner[C]∥The 12th International Symposium of Robotics Research.San Francisco,California, USA:[s.n.]:2005.

[3]Yap P,Burch N,Holte R,et al.Block A*:database-driven search with applications in any-angle path-planning[C]//The 25th AAAI Conference on Artificial Intelligence.San Francisco,California,USA:[s.n.],2011.

[4]Nash A,Daniel K,Koenig S,et al.Theta*:Any-angle path planning on grids[C]∥The 22nd AAAI Conference on Artificial Intelligence.Vancouver,Canada:[s.n.],2007.

[5]Bresenham J.Algorithm for computer control of a digital plotter[J].IBM Systems Journal,1965,4(1):25-30.

[6]湯國(guó)安,李發(fā)源,劉學(xué)軍.數(shù)字高程模型教程(第二版)[M].北京:科學(xué)出版社,2010.[Tang G A,Li F Y,Liu X J.A course in digital elevation model(2nd edition)[M].Beijing: Science Press,2010.]

[7]張伍,黨兆龍,賈陽(yáng).月面數(shù)字地形構(gòu)造方法研究[J].航天器環(huán)境工程,2008,25(4):35-40.[Zhang W,Dang Z L,Jia Y.Research on digital lunar terrain construction[J]. Spacecraft Environment Engineering,2008,25(4):35-40.]

[8]Sun Z Z,Jia Y,Zhang H.Technological advancements and promotion roles of Chang'e-3 lunar probe mission[J].Sci China Tech Sci,2013,56(11):2702-2708.

[9]賈陽(yáng),張建利,李群智,等.嫦娥三號(hào)巡視器遙操作系統(tǒng)設(shè)計(jì)與實(shí)現(xiàn)[J].中國(guó)科學(xué):技術(shù)科學(xué),2014,44(5):470-482.[Jia Y,Zhang J L,Li Q Z,et al.Design and realization for teleoperation system of the Chang'E-3 rover[J].Sci Sin Tech,2014,44(5):470-482.]

通信地址:北京市西城區(qū)車公莊大街12號(hào)核建大廈10層(100037)

電話:(010)88306151

E-mail:wangq2006@163.com

[責(zé)任編輯:高莎]

Risk Theta*:an Any-Angle Path Planning Algorithm based on Terrain Risk

WANG Qiong1,2,YU Dengyun3,JIA Yang2
(1.Lunar Exploration and Space Engineering Center,Beijing 100037,China; 2.Beijing Institute of Spacecraft System Engineering,Beijing 100094,China; 3.China Aerospace Science and Technology Corporation,Beijing 100048,China)

Risk Theta*:an any-angle path planning algorithm based on terrain risk is proposed in this paper. At first,based on statistical analysis on terrain feature of planetary surface,terrain risk index is proposed and the index map is built.Basic Theta*search is conducted on this index map to seek optimal path of lowest terrain risk. Simulation experiments show that the proposed algorithm could find out any-angle path of much lower terrain risk and comparable path length on grid map than A*and Basic Theta*which can significantly improve the rover safety,as well as satisfy the urgent demand of any-angle travel of planetary roving exploration,therefore it is fairly practical.

Theta*;terrain risk;path planning;any-angle;heuristic search

TP242.6;V448.2

:A

:2095-7777(2014)04-0269-06

10.15982/j.issn.2095-7777.2014.04.004

王瓊(1983—),男,高級(jí)工程師,碩士,主要研究方向?yàn)樯羁仗綔y(cè)任務(wù)總體設(shè)計(jì)、星球巡視器任務(wù)規(guī)劃技術(shù)。

2014-10-01

2014-11-30

國(guó)家中長(zhǎng)期科技發(fā)展規(guī)劃重大專項(xiàng)資助項(xiàng)目