王瓊,于登云,賈陽

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

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

王瓊1,2,于登云3,賈陽2

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

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

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

0 引 言

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

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

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

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

1 Basic Theta*算法

Basic Theta*算法與A*算法的主要差別在于擴(kuò)展節(jié)點(diǎn)時的程序。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′這兩條路徑的代價,選擇代價更小的路徑。

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

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

算法1 A*算法

算法2 Basic Theta*算法

2 Risk Theta*算法

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

2.1 地形特征分析

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

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

1)坡度

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

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

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

2)地形粗糙度

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

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

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

3)地形起伏度

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

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

2.2 地形危險度評估

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

1)斜坡

斜坡危險度定義為

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

2)顛簸

巡視器行駛在粗糙度為r的地塊上的危險度為

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

(3)臺階

巡視器行駛在起伏度為D的地塊上的危險度為

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

參考我國首個月面巡視器“玉兔號”設(shè)定仿真用巡視器模型的參數(shù)[8,9]:收攏狀態(tài)包絡(luò)尺寸為1.5 m×1.0 m×1.1 m,車輪直徑30 cm,最大爬坡能力20°,越障能力20 cm。將每7×7個柵格擬合成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*的路徑總危險度比A*平均減小了37.0%,路徑總長度平均僅增加10.1%;路徑總危險度比Basic Theta*平均減小了30.4%,路徑總長度平均僅增加15.9%。特別地,Risk Theta*在單位長度上的危險度比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*算法得到路徑的長度比A*和Basic Theta*雖略有增加,但總危險度和在每一個柵格上的危險度大大減小,因此要安全得多。

4 結(jié) 論

本文提出的Risk Theta*算法,以星球表面地形特征統(tǒng)計分析為基礎(chǔ)提出了地形危險度指標(biāo),建立了地形危險度地圖,并以危險度最低為方向搜索任意航向路徑,能夠有效解決星球表面復(fù)雜地形環(huán)境中基于二值障礙判斷的Theta*算法路徑解安全性較差的問題。仿真實(shí)驗(yàn)證明,該算法能夠在柵格地圖上找到比A*和Theta*算法危險度低得多、長度相當(dāng)?shù)娜我夂较蚵窂?既顯著提高了巡視器的安全性,又滿足了星球表面巡視對任意航向行駛的迫切需求,因此具有較強(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]湯國安,李發(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]張伍,黨兆龍,賈陽.月面數(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]賈陽,張建利,李群智,等.嫦娥三號巡視器遙操作系統(tǒng)設(shè)計與實(shí)現(xiàn)[J].中國科學(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號核建大廈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—),男,高級工程師,碩士,主要研究方向?yàn)樯羁仗綔y任務(wù)總體設(shè)計、星球巡視器任務(wù)規(guī)劃技術(shù)。

2014-10-01

2014-11-30

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