董元元,崔祜濤,田陽

(哈爾濱工業(yè)大學(xué)深空探測基礎(chǔ)研究中心,哈爾濱150001)

基于柵格地圖的火星車路徑規(guī)劃方法

董元元,崔祜濤,田陽

(哈爾濱工業(yè)大學(xué)深空探測基礎(chǔ)研究中心,哈爾濱150001)

火星車路徑規(guī)劃是實(shí)現(xiàn)火星車完成預(yù)定探測任務(wù)的關(guān)鍵。然而傳統(tǒng)路徑規(guī)劃算法,如A*和D*等,存在計(jì)算速度較慢,算法復(fù)雜度較高等問題。本文將對傳統(tǒng)路徑規(guī)劃方法A*法改進(jìn)并且得到一種快速高效的全局路徑規(guī)劃算法,之后結(jié)合合適的局部避障算法,得到一種基于柵格地圖的完整的火星車路徑規(guī)劃方法,最后通過仿真驗(yàn)證了此方法的有效性及合理性。

路徑規(guī)劃;PAA*;局部避障;A*

0 引言

火星車的路徑規(guī)劃,是指火星車根據(jù)自身傳感器對周圍環(huán)境進(jìn)行感知,自行規(guī)劃出一條安全的運(yùn)行路徑?；鹦擒嚶窂揭?guī)劃問題主要包括兩個(gè)方面: 1)使火星車從初始點(diǎn)運(yùn)行到目標(biāo)點(diǎn);2)用一定的算法使火星車能夠繞開障礙物,并且經(jīng)過某些規(guī)劃好的點(diǎn)。對于火星車路徑規(guī)劃,按照選取目標(biāo)的范圍看,其可分為全局路徑規(guī)劃和局部路徑規(guī)劃。全局路徑規(guī)劃從全局出發(fā),得到一條從起點(diǎn)到終點(diǎn)的全局軌跡。而局部路徑規(guī)劃考慮機(jī)器人移動(dòng)中的局部細(xì)節(jié),得到符合運(yùn)動(dòng)學(xué)規(guī)律的路徑。

柵格法是一種廣泛應(yīng)用于各種移動(dòng)機(jī)器人的全局規(guī)劃方法,其通過創(chuàng)建柵格地圖來代表機(jī)器人當(dāng)前的三維環(huán)境。常見的柵格法有A*法、D*法等[1-2]。A*法是通過定義啟發(fā)式函數(shù)來得到全局路徑的方法,其算法較為原始,在路徑的二次規(guī)劃中速度較慢。D*法是基于A*法的一種改進(jìn)算法,能夠更好地運(yùn)用于動(dòng)態(tài)場景,基于D*法改進(jìn)的field-D*法已經(jīng)成功運(yùn)用于在“勇氣號”火星探測器全局規(guī)劃過程中[3],但是D*算法原理過于復(fù)雜,難以實(shí)現(xiàn)。并且對于火星車路徑規(guī)劃而言,使用柵格法得到的軌跡為離散的柵格,不符合火星車的運(yùn)動(dòng)學(xué)規(guī)律,不能直接適用于火星車運(yùn)動(dòng)控制。

因此,在本文中將首先對A*法改進(jìn),得到一種快速高效的路徑規(guī)劃算法:PAA*法來實(shí)現(xiàn)全局路徑規(guī)劃,之后結(jié)合符合火星車運(yùn)動(dòng)學(xué)約束的局部避障算法,建立一種完整的火星車路徑規(guī)劃方法。

1 全局路徑規(guī)劃方法

1.1 柵格地圖建立

在使用柵格法全局路徑規(guī)劃算法之前,需要建立當(dāng)前環(huán)境的柵格地圖。柵格地圖建立如文獻(xiàn)[4]中所介紹,根據(jù)當(dāng)前地形高程圖擬合平面,對火星大小的斑點(diǎn)進(jìn)行高度、傾斜角以及粗糙度分析,可以得到當(dāng)前的柵格地圖。柵格地圖一般為二值圖:其中0代表當(dāng)前柵格不可通行,即為障礙柵格;1代表當(dāng)前柵格為可行柵格。如圖1所示,其中黑色部分為障礙區(qū)域。通過建立柵格地圖A之后,可以得到當(dāng)前柵格中的障礙柵格集合Obs?A,和可行域Free?A。這樣路徑規(guī)劃算法可以描述為在/區(qū)域中規(guī)劃一條從起點(diǎn)到終點(diǎn)的最優(yōu)路徑。

圖1 柵格地圖示意圖Fig.1 The grid map

1.2 A*算法

A*算法[5]是一種靜態(tài)的柵格路徑規(guī)劃方法。其主要過程為:從起始節(jié)點(diǎn)出發(fā),依次對當(dāng)前節(jié)點(diǎn)的子節(jié)點(diǎn)權(quán)重進(jìn)行更新,并用子節(jié)點(diǎn)中權(quán)重f(n)最小者對當(dāng)前節(jié)點(diǎn)更新,直至遍歷所有節(jié)點(diǎn)(或者當(dāng)前節(jié)點(diǎn)為目標(biāo)節(jié)點(diǎn))為止,其中f(n)=g(n)+h(n),這3個(gè)函數(shù)將在下文中定義。當(dāng)擴(kuò)展完成之后,A*算法結(jié)合移動(dòng)機(jī)器人目標(biāo)點(diǎn)位置信息,沿著目標(biāo)點(diǎn)位置開始搜尋,到達(dá)起始點(diǎn)為止,得到一條起點(diǎn)到終點(diǎn)的最優(yōu)路徑。對于A*算法中函數(shù)給出如下定義:

S為初始節(jié)點(diǎn);

G為目標(biāo)節(jié)點(diǎn);

k為路徑規(guī)劃中已規(guī)劃過的某一節(jié)點(diǎn);

gi為路徑規(guī)劃搜索過程中的柵格節(jié)點(diǎn);

gi,x,gi,y分別為節(jié)點(diǎn)的橫、縱坐標(biāo);

g(k)為初始節(jié)點(diǎn)到k節(jié)點(diǎn)的距離;

h(k)為啟發(fā)式函數(shù),為k節(jié)點(diǎn)的啟發(fā)距離;

f(k)為k節(jié)點(diǎn)的路徑評價(jià)函數(shù);

OPEN為等待擴(kuò)展節(jié)點(diǎn)的集合;

CLOSE為已擴(kuò)展節(jié)點(diǎn)集合;

ne為擴(kuò)展節(jié)點(diǎn)函數(shù),節(jié)點(diǎn)不是障礙或者之前未擴(kuò)展時(shí),執(zhí)行節(jié)點(diǎn)擴(kuò)展;

ni為插入節(jié)點(diǎn)函數(shù),將節(jié)點(diǎn)按照f(n)值大小降序放入OPEN列表;

在柵格地圖中,A*啟發(fā)式函數(shù)/定義為從當(dāng)前k節(jié)點(diǎn)到目標(biāo)點(diǎn)的距離,其表達(dá)式為

其中:kx、ky、Gx、Gy分別為當(dāng)前節(jié)點(diǎn)和目標(biāo)點(diǎn)在柵格地圖中的坐標(biāo)位置。

通過上面定義的函數(shù),可以得到A*算法的具體計(jì)算流程圖,如圖2所示。通過計(jì)算可以得到火星車的初始全局路徑。

圖2 A*算法流程圖Fig.2 Flowchart of A*algorithm

從圖2中可以看出,A*算法選擇f(n)最小的節(jié)點(diǎn)擴(kuò)展,并且將其存入到CLOSED列表中,將擴(kuò)展的節(jié)點(diǎn)存入到OPEN列表中。當(dāng)擴(kuò)展到目標(biāo)點(diǎn),即將目標(biāo)點(diǎn)存入到CLOSED列表中時(shí),停止搜索。之后在CLOSED列表中,搜索最小g(n)值可以得到一條從起始點(diǎn)到目標(biāo)點(diǎn)的最優(yōu)軌跡。

1.3 PAA*算法

對于全局規(guī)劃路徑而言,在已規(guī)劃好的路徑中間,當(dāng)有一可行柵格變?yōu)檎系K柵格時(shí),需要重新計(jì)算全局路徑。使用A*算法時(shí),重新規(guī)劃過程需要對整個(gè)柵格地圖繼續(xù)按照A*方法遍歷,求出新的路徑,這樣之前已規(guī)劃好的路徑?jīng)]有參與到重新規(guī)劃中。因此,我們使用PAA*算法解決這種重復(fù)計(jì)算問題。其對于CLOSED列表中啟發(fā)式函數(shù)h(k)為

其中:g*為上一步得到的最小代價(jià)值路徑的代價(jià)值;g(k)定義如上節(jié)所示。

在之后規(guī)劃過程中,將增加障礙柵格之后的每個(gè)路徑點(diǎn)假設(shè)為中介目標(biāo)點(diǎn),搜索從增加的障礙柵格到某個(gè)中介目標(biāo)點(diǎn)合適的路徑,之后沿著這個(gè)中介路徑點(diǎn)到達(dá)目標(biāo)點(diǎn)。這樣減少了重新規(guī)劃時(shí)計(jì)算的柵格數(shù),增加了計(jì)算速度。

圖3顯示了使用PAA*算法時(shí)的全局路徑規(guī)劃結(jié)果。在圖3(a)中,當(dāng)在規(guī)劃完成路徑中(5,4)位置增加了新的障礙柵格后,算法重新規(guī)劃得到點(diǎn)劃線的路徑。圖3(b)中灰色柵格為重新規(guī)劃過程中所計(jì)算的柵格。

圖3 PAA*算法演示Fig.3 PAA*algorithm example

2 局部路徑規(guī)劃方法

在本部分將選擇簡化的GESTALT方法來實(shí)現(xiàn)局部路徑生成。在此過程中,忽略GESTALT建立優(yōu)異值地圖過程,并且忽略其確定值,僅考慮建立二值柵格地圖,如上述柵格地圖建立中所描述,將整個(gè)地圖標(biāo)記為可行區(qū)域與不可行區(qū)域。之后建立一系列的軌跡組,從中選擇接近最遠(yuǎn)全局路徑點(diǎn),并且不經(jīng)過不可行區(qū)域的軌跡為下一步執(zhí)行軌跡,如圖4所示。

圖4 火星車避障系統(tǒng)流程圖Fig.4 Flowchart of Mar's rovers'obstacle avoiding system

2.1 軌跡組建立

此過程需要建立一組符合火星車運(yùn)動(dòng)學(xué)的軌跡。對于類似車輛的火星車,定義V(α)=[v(α),ω (α)]T為一給定軌跡的速度矢量(其中α為軌跡參數(shù),不同值代表不同軌跡),由火星車的線速度和角速度構(gòu)成。令x(α)、y(α)、?(α)為火星車位姿參數(shù),其中(x(α),y(α))代表火星車當(dāng)前位置,而?(α)代表火星車的當(dāng)前姿態(tài)角,定義P(α)=[x(α,t),y(α, t),?(α,t)]T,則其運(yùn)動(dòng)學(xué)方程為

之后建立如圖5所示的圓弧形軌跡為局部路徑,其速度表達(dá)式為

其中:v0、w0為固定值;K=±1代表軌跡方向(向前或者向后)。對于火星車,我們選擇v0=0.2 m/s, w0=0.05 rad/s作為初始參數(shù),建立如圖6所示的局部軌跡組,在下一步中將從此軌跡組中選擇最優(yōu)軌跡。

圖5 圓弧形軌跡示意圖Fig.5 Circle trajectories

圖6 建立局部軌跡組Fig.6 Local circle trajectories with series of parameters

2.2 局部軌跡選擇

建立軌跡組之后,從中選擇接近最遠(yuǎn)全局路徑點(diǎn),并且不經(jīng)過不可行區(qū)域的軌跡為下一步執(zhí)行軌跡。具體為分析每條軌跡所經(jīng)過的柵格,得到每條軌跡經(jīng)過的全局路徑柵格和不可行柵格,將其經(jīng)過的離目標(biāo)點(diǎn)最近的柵格作為其最終選擇路徑點(diǎn)。之后選擇經(jīng)過最遠(yuǎn)全局路徑點(diǎn)并且在可行區(qū)域的軌跡為下一步執(zhí)行軌跡。如圖7所示,點(diǎn)劃線為全局路徑,細(xì)虛線弧線為規(guī)劃軌跡組,粗虛線軌跡為選擇的不經(jīng)過不可行區(qū)域,并且經(jīng)過第三個(gè)路徑點(diǎn)的軌跡。

由于全局路徑的精確度較差,因此規(guī)劃好的全局路徑點(diǎn)周圍有可能出現(xiàn)障礙物,導(dǎo)致此路徑點(diǎn)成為不可行區(qū)域,這種障礙物可稱為局部障礙物,如圖8中加入的局部障礙物。對于此種情況,使用上述介紹的方法并不能完成避障,并且可能會(huì)產(chǎn)生錯(cuò)誤,因此需要圖8的避障方案。當(dāng)沒有局部軌跡經(jīng)過路徑點(diǎn),并且沒有穿過不可行區(qū)域時(shí),選擇最容易實(shí)現(xiàn)的軌跡,也就是弧度最小的軌跡,先繞出局部障礙物,之后再按照上述方法,尋找經(jīng)過最遠(yuǎn)路徑點(diǎn)并且不經(jīng)過不可行區(qū)域的軌跡,直到到達(dá)目標(biāo)點(diǎn)為止。

圖7 局部路徑選擇示意Fig.7 Local trajectories selecting example

圖8 出現(xiàn)局部障礙物時(shí)情形Fig.8 Local obstacle avoiding example

在全局規(guī)劃完成后,使用上述所介紹的局部路徑規(guī)劃方法,得到如圖9所示的結(jié)果。其中點(diǎn)劃線軌跡為全局規(guī)劃路徑,而實(shí)線軌跡為局部跟蹤之后的路徑,這樣得到的局部路徑為平滑路徑,其符合運(yùn)動(dòng)學(xué)約束,可應(yīng)用于火星車運(yùn)動(dòng)控制中。

圖9 局部路徑規(guī)劃結(jié)果Fig.9 Result of complete rover path-planning method

3 結(jié) 論

本文提出了一種基于柵格地圖的火星車全局路徑規(guī)劃方法,其主要有以下特點(diǎn):

1)使用了PAA*法作為全局規(guī)劃方法,此方法在二次規(guī)劃時(shí)重新定義了啟發(fā)式函數(shù),相對于傳統(tǒng)的全局規(guī)劃方法A*和D*來說,有速度快和實(shí)現(xiàn)簡單等優(yōu)點(diǎn);

2)建立符合火星車運(yùn)動(dòng)學(xué)約束的軌跡組,之后以得到的全局路徑為中間目標(biāo)點(diǎn),通過避障和選擇中間目標(biāo)點(diǎn)的共同作用,得到了合適的局部避障軌跡以及對應(yīng)的速度向量。

[1]Koren Yoram,Johann Borenstein.Potential field methods and their inherent limitations for mobile robot navigation[C]∥Robotics and Automation,Proceedings of 1991 IEEE International Conference on.[S.l.]:IEEE,1991.

[2]Koenig Sven,Maxim Likhachev.Improved fast replanning for robot navigation in unknown terrain[C]∥Robotics and Automation,Proceedings of 2002.IEEE International Conference on.[S.l.]:IEEE,2002.

[3]Carsten Joseph.Global path planning on board the mars exploration rovers[C]∥/Aerospace Conference,2007 IEEE.[S.l.]:IEEE,2007.

[4]Goldberg Steven B,Mark Maimone M,Larry Matthies. Stereo vision and rover navigation software for planetary exploration[C]∥Proceedings of 2002 IEEE Aerospace Conference.[S.l.]:IEEE,2002.

[5]Durfee Edmund H,Charles L Ortiz Jr,Michael J Wolverton. A survey of research in distributed,continual planning[J]. AI Magazine,1999,20(4):13.

A Path-Planning Method for Mars Rovers based on Grid Map

DONG Yuanyuan,CUI Hutao,TIAN Yang
(Deep Space Exploration Research Center,Harbin Institute of Technology,Harbin 150001,China)

Path-planning is one of the major parts for Mars to complete exploration missions.With running slowly and higher computing complexity,traditional path-planning algorithms,such as A*and D*algorithms, are no longer in using.In this paper,a fast and efficient global path-planning method is got through improving the conretional A*algorithm.Then combining with local obstacle avoiding method,a complete rover path-planning method based on grid map is obtained.Finally,by using simulation provefs this method is effective and available.

path-planning;PAA*;local obstacles avoidance;A*

P2

:A

:2095-7777(2014)04-0289-05

10.15982/j.issn.2095-7777.2014.04.007

董元元(1991—),男,碩士研究生,主要研究方向?yàn)榛鹦擒噷?dǎo)航及路徑規(guī)劃。

[責(zé)任編輯:宋宏]

2014-10-14

2014-10-30

國家重點(diǎn)基礎(chǔ)研究發(fā)展計(jì)劃項(xiàng)目(2012CB720005);國家自然科學(xué)基金資助項(xiàng)目(61174201)

電話:15004542438