董元元,崔祜濤,田陽

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

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

董元元,崔祜濤,田陽

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

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

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

0 引言

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

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

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

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

1.1 柵格地圖建立

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

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

1.2 A*算法

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

S為初始節(jié)點;

G為目標節(jié)點;

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

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

gi,x,gi,y分別為節(jié)點的橫、縱坐標;

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

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

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

OPEN為等待擴展節(jié)點的集合;

CLOSE為已擴展節(jié)點集合;

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

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

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

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

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

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

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

1.3 PAA*算法

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

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

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

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

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

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

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

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

2.1 軌跡組建立

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

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

其中: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 局部軌跡選擇

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

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

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

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

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

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

3 結(jié) 論

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

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

2)建立符合火星車運動學約束的軌跡組,之后以得到的全局路徑為中間目標點,通過避障和選擇中間目標點的共同作用,得到了合適的局部避障軌跡以及對應(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—),男,碩士研究生,主要研究方向為火星車導航及路徑規(guī)劃。

[責任編輯:宋宏]

2014-10-14

2014-10-30

國家重點基礎(chǔ)研究發(fā)展計劃項目(2012CB720005);國家自然科學基金資助項目(61174201)

電話:15004542438