李群智 申振榮 張 伍 賈 陽

(北京空間飛行器總體設計部,北京 100094)

1 引言

月球是人類向外層空間發(fā)展的理想中轉基地和前哨站。目前,包括中國在內的世界多個國家的研究機構均開展了對月探測的研發(fā)工作[1-2]。其中,路徑規(guī)劃的研究[3],作為月面巡視探測器(簡稱巡視器)進行巡視探測的前提和基礎,直接影響巡視探測的效率,即影響地面遙操作和巡視器自主運行的工作效率,同時,會影響巡視器的生存安全。因此,開展巡視器路徑規(guī)劃的研究,具有重要的意義和工程實用價值。

經(jīng)研究發(fā)現(xiàn),巡視器在月面進行巡視探測,其運動主要包括移動和機械臂攜帶有效載荷運動兩個方面的內容[4]。針對這兩個方面,本文開展了帶機械臂的月面巡視探測器的路徑規(guī)劃方法研究,主要內容包括:巡視器移動的路徑規(guī)劃(簡稱巡視器的路徑規(guī)劃)、巡視器機械臂的路徑規(guī)劃(簡稱機械臂的路徑規(guī)劃)、巡視器攜帶機械臂就位探測時巡視器與機械臂的路徑規(guī)劃(簡稱器臂動態(tài)聯(lián)合的路徑規(guī)劃)。

2 巡視器的路徑規(guī)劃方法

巡視器的路徑規(guī)劃方法(Lunar Rover Path Planning M ethod, LRPPM)是指在一個可視區(qū)域內,巡視器以某一個或幾個因素作為優(yōu)化條件,發(fā)現(xiàn)較優(yōu)行進路線的過程。與地面的自主移動機器人不同,巡視器在完成路徑規(guī)劃的過程中,只能靠自身的環(huán)境建模和航位推算,而不能借助外部的導航和標定系統(tǒng),路徑規(guī)劃過程具有動態(tài)性和不完整性。此外,路徑規(guī)劃還需要考慮能量的限制、巡視器運行的安全性和平穩(wěn)性、車體的最小轉彎半徑、適宜弧線、越障通過能力等特殊要求。本文巡視器的路徑規(guī)劃方法涉及到環(huán)境表達、路徑評估和路徑搜索。

針對月面環(huán)境以非結構化為顯著特征,巡視器的路徑規(guī)劃基于三維數(shù)字高程圖(DEM),采用改進的A*算法完成路徑的搜索與擇優(yōu)。

2.1 環(huán)境表達

月面巡視探測器所在環(huán)境是實際的物理環(huán)境,而進行路徑規(guī)劃需要將物理環(huán)境抽象成為能被計算機理解和表達的環(huán)境模型,稱為環(huán)境建模,即地圖創(chuàng)建(M ap-building)[5]。地圖創(chuàng)建首先要確定環(huán)境地圖的描述方法,即數(shù)字地圖的表示方法,必須便于計算機存儲和處理。目前各國研究者已經(jīng)提出了多種表示方法,常用的環(huán)境建模方法有柵格法、幾何法、C 空間(C-space)法和拓撲圖等。每種數(shù)字地圖的表示方法都有自己的優(yōu)點和缺點,根據(jù)月面巡視探測器環(huán)境感知的特點,一般采用柵格法。

基于柵格的環(huán)境表示方法最早由Elfes 和M orave 提出[6]。所謂柵格法就是將地形環(huán)境用柵格進行覆蓋,每個柵格的屬性值代表所覆蓋地形的屬性,如高度屬性或區(qū)域屬性(自由空間或障礙物等)。最簡單的柵格法是將地圖分成大小相同的柵格,這樣地圖就可以用數(shù)組或矩陣表示,數(shù)組的索引號與柵格在地圖中的位置對應,而元素值則與柵格屬性對應,形成數(shù)字高程圖(DEM)。

利用柵格法進行環(huán)境建模時,一般規(guī)定只能向其水平和豎直方向的相鄰柵格中心移動(稱為4-鄰域移動)或向其周圍相鄰的八個柵格中心移動(稱為8-鄰域移動)。柵格法的優(yōu)點包括:柵格法容易創(chuàng)建和維護,便于計算機存儲、處理和使用;在路徑搜索時,柵格對應圖的節(jié)點,節(jié)點間連通性已經(jīng)暗含在柵格的相鄰性當中,便于算法搜索。柵格法的缺點包括:空間效率不高;柵格的大小影響搜索路徑的完備性;4-鄰域或8-鄰域移動準則限制搜索路徑的最優(yōu)性。但是,柵格法的這些缺點可以通過改進搜索算法和進行規(guī)劃路徑的后續(xù)處理來克服。

因此,本文采用柵格的描述方法來對月面環(huán)境進行描述,充分考慮巡視器的可通過性,給不同地形對應的柵格賦予不同的值,并在此基礎上進行路徑評估與路徑搜索。

2.2 改進的A*算法

路徑規(guī)劃的評估與搜索是路徑規(guī)劃的核心內容。國外應用過的路徑規(guī)劃搜索算法包括啟發(fā)式搜索(A*)算法研究、幾何搜索算法研究和統(tǒng)計搜索算法研究等。巡視器路徑規(guī)劃方法根據(jù)不同的原則可以有不同的分類方法,按是否使用智能算法大致可以分為傳統(tǒng)方法和智能方法[7]。根據(jù)工作環(huán)境,路徑規(guī)劃模型可分為全局路徑規(guī)劃和局部路徑規(guī)劃[8]。在巡視探測器中,使用得比較多的是人工勢場法、A*算法和動態(tài)啟發(fā)式搜索(D*)算法[3]、廣度優(yōu)先(Bug)算法、深度優(yōu)先(ISE)算法等。本文路徑規(guī)劃的方法采用了具有啟發(fā)、智能和全局特性的改進的A*算法。

A*算法使用了啟發(fā)式方法,這種方法通過充分利用圖給出的信息來動態(tài)地做出決定,使搜索次數(shù)大大降低。它通過一個評估函數(shù)(Heuristic Function)f(n)來估計圖中的當前點到終點的距離(帶權值),并由此決定它的搜索方向,當這條路徑失敗時,它會嘗試其它路徑。A*算法成功與否的關鍵在于評估函數(shù)的正確選擇。

A*算法的評估函數(shù)為

式中,n 為搜索路徑, f(n)為預測的全程移動消耗,g(n)為從起點到當前點的實際移動消耗,h(n)為當前點到終點的移動消耗估計值。

本文選取只對目標點感興趣的Manhattan 估算函數(shù)作為啟發(fā)因子,以達到最佳運算效率。M anhattan 距離為

式中,A.x 為當前點的x 坐標值,A.y 為當前點的y 坐標值;goal.x 為目標點的x 坐標值,goal.y為目標點的y 坐標值;h(A)為當前點的Manhattan估計值。

由上式可知,M anhattan 估算函數(shù)與當前點到目標點的x 方向距離絕對值及y 方向距離絕對值的和成正比,即具有估算因子的功能,又擁有較高的運算效率。然而,巡視器可以在地圖上作斜線方向的運動,故將M ahattan 距離修正為

此時,路徑會明顯分為兩個階段:第一階段,路徑點首先向著靠近目標點斜對角線的方向延伸,當規(guī)劃的路徑走到與目標點斜對角時,路徑走向進入第二階段;第二階段,路徑點以斜向移動快速靠近目標點,這種方法相對于鋸齒狀移動的路徑,消耗大為減少。

此外,本文在A*算法的處理上還做了兩個方面的修正。首先,對自排序鏈表的構建進行了優(yōu)化,即open 列表在組建時根據(jù)f 值大小進行排序, f 值小的排在表頭處,使得插入點的排序工作僅在表尾幾個節(jié)點進行,甚至可能不需要移動任何節(jié)點直接將新節(jié)點排在表頭,不僅節(jié)省了運算時間,還有利于保留曾經(jīng)走過的路徑信息。其次,在尋路結束之后,在返回規(guī)劃路徑之前對路徑鏈表進行了優(yōu)化處理,通過兩個指針從路徑鏈表的兩端向另一端遍歷,尋找出路徑鏈表中的迂回路徑,并切除多余部分的路徑節(jié)點。算法的流程圖如圖1 所示?？傊?改進的A*算法不僅考慮了距離和柵格的可通過性,還提高了路徑規(guī)劃的效率,適合用于非結構化月面柵格地圖上的路徑規(guī)劃。

2.3 仿真驗證

圖1 算法流程圖Fig.1 Flow chart of arithmetic

采用改進的A*算法,在構建的柵格月面地圖上進行路徑規(guī)劃, 如圖2 所示。已知單位柵格為10mm ×10mm, 路徑的起始點坐標為(140mm,930mm),目標點坐標為(910mm ,150mm),則路徑搜索過程耗時1.293s,路徑長度為1 381.5mm,滿足巡視器的路徑規(guī)劃的需求。

圖2 采用改進A*算法的路徑規(guī)劃Fig.2 Path planning using improved A*arithmetic

3 機械臂的路徑規(guī)劃方法

機械臂的路徑規(guī)劃方法(Manipulator Path Planning Method,MPPM)與巡視器的路徑規(guī)劃方法有著本質的不同,巡視器的路徑規(guī)劃受限于地形地貌和巡視器的行駛性能;而機械臂的規(guī)劃過程受外界的影響一般僅是碰撞檢測方面,其余則受限于自身的特性,如工作空間、臂長和構型設計的影響等。

機械臂的路徑規(guī)劃M PPM,首先在機械臂Denavit-Harterberg(D-H)建模的基礎上,采用蒙特卡羅法求得機械臂的可達工作空間,然后規(guī)劃得出機械臂工作空間內的路徑點,最后通過運動學逆解計算和關節(jié)插值,得到機械臂的運動路徑。

3.1 機械臂D-H 建模

巡視器機械臂簡化模型如圖3a 所示,由三個關節(jié)組成。其中,a1=110mm ,指關節(jié)1 與關節(jié)2 軸線之間的距離;a2=350mm,指關節(jié)2 與關節(jié)3 軸線之間的距離;a3=110mm,指有效載荷末端到關節(jié)3 軸線的距離。

圖3 機械臂建模Fig.3 Model of the manipulato r

根據(jù)D-H 坐標系建立法則,建立機械臂的DH 模型,如圖3b 所示。其中,Z1指關節(jié)1 的轉動軸線,Z2指關節(jié)2 的轉動軸線,Z3指關節(jié)3 的轉動軸線,Z 4 的坐標原點位于有效載荷末端面的中心。機械臂模型的D-H 參數(shù)如表1 所示。

表1 D-H參數(shù)表Table 1 D-H parameter

由機械臂模型的D-H 參數(shù),求機械臂末端點在x,y,z 坐標系下的位置參數(shù)方程為

3.2 機械臂可達工作空間分析

機械臂工作空間的求解采用蒙特卡羅法[9]。蒙特卡羅法是一種基于“隨機數(shù)”的計算方法,屬于概率統(tǒng)計方法的一種,同時也具有一般數(shù)值方法的優(yōu)越性。求解機械臂的工作空間時,對關節(jié)變量通過均勻分布,賦予一定數(shù)量的、符合關節(jié)變化要求的隨機量,從而得到工作空間由隨機點構成的圖形,稱之為“云圖”,這樣就構成了機械臂的可達工作空間。求解方法的程序流程圖,如圖4 所示。

圖4 蒙特卡羅方法求解工作空間的程序流程圖Fig.4 Flow chart of finding workspace using Monte Carlo method

根據(jù)D-H 參數(shù),考慮機械臂的關節(jié)空間約束,使用式(4)～(6),采用蒙特卡羅法,對機械臂進行工作空間分析,確定機械臂的可達工作空間如圖5 所示。由圖5 可以看出,三自由度機械臂的構型對應的可達工作空間為一個帶半球空腔的半球體,機械臂與其可達工作空間的關系如圖5 a 所示,可達工作空間的兩個方向的視圖如圖5 b、c 所示。

圖5 機械臂的可達工作空間Fig.5 Reachable w orkspace of the manipulator

3.3 機械臂的路徑規(guī)劃

在機械臂的工作空間內,根據(jù)機械臂的起始點和目標點,確定多個路徑段,每個路徑段用小直線段表示。

首先,通過運動學反解計算,得出每個路徑段內每個關節(jié)需要運動的范圍,設起始關節(jié)角度為θ0,終止關節(jié)角度為θf。

然后,使用關節(jié)插值計算,本文采用三次多項式插值,可以得出機械臂的運動路徑(軌跡)。

每個關節(jié)的運動軌跡為

運動軌跡上關節(jié)的角速度為

運動軌跡上關節(jié)的角加速度為

其中,t 為運動軌道的時間變量, a0=θ0,a1=

這種機械臂的路徑規(guī)劃的方法比較簡單、直觀,是在計算出機械臂的工作空間的基礎上,人為確定路徑點,然后通過機械臂運動學計算最終得出其運動路徑。此外,可以通過碰撞檢測,由機械臂自主確定路徑點,完成整個路徑規(guī)劃過程。但是,碰撞檢測的設置需要機械臂所處外部環(huán)境的精確建模和機械臂本體與外部環(huán)境的接觸判斷,受多方面因素的限制,相對復雜,在一定程度上會影響機械臂的探測效率,故暫不采用。

4 器臂動態(tài)聯(lián)合的路徑規(guī)劃方法

巡視器攜帶機械臂就位探測時器臂動態(tài)聯(lián)合的路徑規(guī)劃方法(Rover-M anipulator Path Planning M ethod,RM PPM),將上述巡視器和機械臂的路徑規(guī)劃結合起來,巡視器路徑規(guī)劃的目標點和巡視器于目標點的探測姿態(tài),將作為機械臂路徑規(guī)劃的初始點和初始姿態(tài),如圖6 所示。整個規(guī)劃過程描述如下:

首先,確定巡視器路徑規(guī)劃的目標點和巡視器于目標點的探測姿態(tài),使機械臂末端點和探測目標點均處于機械臂的工作空間范圍內,如圖6 中石塊6 處的放大圖部分所示;

其次,采用改進的A*算法,在三維柵格數(shù)字高程地形圖上,進行巡視器路徑規(guī)劃,得出較優(yōu)的巡視器規(guī)劃路徑,其中,巡視器初始點根據(jù)柵格地形確定,目標點為上一步確定的巡視器路徑規(guī)劃的目標點;

再次,于目標點調整巡視器到探測姿態(tài),在調整過程中,有巡視器最佳探測方向的判斷,需要精確的求解機械臂的可達工作空間,使機械臂末端點和探測目標點均處于機械臂的工作空間范圍內,同時保證巡視器處于最穩(wěn)定的狀態(tài),以免機械臂探測時巡視器出現(xiàn)滑動、傾斜等問題;

最后,在機械臂的工作空間內,確定機械臂的路徑點,用小直線段連接,通過運動學反解計算和三次多項式插值計算,得出機械臂的規(guī)劃路徑。

整個路徑規(guī)劃過程之所以稱為動態(tài)聯(lián)合,是因為受非結構化月面地形的影響,巡視器于目標點的位姿與初始規(guī)劃的位姿一般會有較大的偏差,需要根據(jù)遙測信息進行修正,才能作為機械臂路徑規(guī)劃的輸入。

圖6 器臂動態(tài)聯(lián)合的路徑規(guī)劃示意圖Fig.6 Sketch map of Rover-Manipulator Path Planning

5 結論

綜上所述,巡視器的路徑規(guī)劃有兩種:一種是針對移動的巡視器的路徑規(guī)劃方法(LRPPM),一種是針對機械臂的路徑規(guī)劃方法(M PPM),兩種結合起來(RMPPM),才能夠完成就位探測。因此,帶機械臂的月面巡視探測器的路徑規(guī)劃方法的研究內容,不僅包括巡視器的路徑規(guī)劃和機械臂的路徑規(guī)劃,還需要完成巡視器攜帶機械臂就位探測時器臂動態(tài)聯(lián)合的路徑規(guī)劃。

帶機械臂的月面巡視探測器的路徑規(guī)劃方法的特點歸納總結如下:

1)巡視器的路徑規(guī)劃過程具有動態(tài)性和不完整性。規(guī)劃方法基于月面三維數(shù)字高程圖(DEM),采用改進的A*算法完成路徑的搜索與擇優(yōu)。

2)機械臂的路徑規(guī)劃僅在自己的可達工作空間內進行,需要針對障礙進行碰撞檢測。規(guī)劃方法基于D-H 建模,采用蒙特卡羅法求得機械臂的可達工作空間,然后人為輔助通過運動學反解計算和三次多項式插值計算,規(guī)劃得出機械臂工作空間內的運動路徑。

3)巡視器攜帶機械臂完成就位探測的過程,需要上述兩種規(guī)劃的動態(tài)聯(lián)合。實際過程中,還需要地面操作人員的參與,尤其是機械臂初始方位的輸入和機械臂到位的判斷。

)

[1]葉培建,孫澤洲,饒煒.嫦娥一號月球探測衛(wèi)星研制綜述[J].航天器工程, 2007, 16(6):9-15

[2]Huntsville, Alabama.A brief history of the lunar roving vehicle[R].NASA Marshall Space Flght Center, 2002:3

[3]Joseph Carsten, Arturo Rankin, et al.Global path planning on board the Mars exploration rovers[C]//Aerospace Conference 2007, IEEE, Big sky, MT,2007:1-11

[4]Tunstel E, M aimone M, Trebi O A, et al.Mars exploration rover mobility and robotic arm operational performance[C]// System, M an and Cybernetics, 2005 Internationl Conference on IEEE, 2005:1807 -1814

[5]席裕庚, 陳衛(wèi)東, 王衛(wèi)華.基于不確定信息的移動機器人地圖創(chuàng)建研究進展[J].機器人, 2001,23(6):563-568

[6]Kambhampati S, Davis L S.M ultiresolution path planning for mobile robot[J].IEEE Journal of Robotics and Automation, 1986, RA2(3):135-145

[7]張穎,吳成東, 原寶龍.機器人路徑規(guī)劃方法綜述[J].控制工程, 2003, 5(10):152-155

[8]Sanjiv Singh, Reid Simmons, T rey Smith, et al.Recent progress in local and global traversability for planetary rovers[C]// IEEE International Conference on Robotics and Automation.San Francisco, 2000, 4:1-7

[9]Denavit J, Hartenberg R S.A kinematic notation for lower-pair mechanism s based on matices[J].T rans.ASME, J of Applied Mechanics, 1955, 77(6):215-221

[10]張孝澤.蒙特卡羅方法在統(tǒng)計物理中的應用[M].鄭州:河南科學技術出版社,1991