李群智 申振榮 張 伍 賈 陽(yáng)
(北京空間飛行器總體設(shè)計(jì)部,北京 100094)
月球是人類向外層空間發(fā)展的理想中轉(zhuǎn)基地和前哨站。目前,包括中國(guó)在內(nèi)的世界多個(gè)國(guó)家的研究機(jī)構(gòu)均開展了對(duì)月探測(cè)的研發(fā)工作[1-2]。其中,路徑規(guī)劃的研究[3],作為月面巡視探測(cè)器(簡(jiǎn)稱巡視器)進(jìn)行巡視探測(cè)的前提和基礎(chǔ),直接影響巡視探測(cè)的效率,即影響地面遙操作和巡視器自主運(yùn)行的工作效率,同時(shí),會(huì)影響巡視器的生存安全。因此,開展巡視器路徑規(guī)劃的研究,具有重要的意義和工程實(shí)用價(jià)值。
經(jīng)研究發(fā)現(xiàn),巡視器在月面進(jìn)行巡視探測(cè),其運(yùn)動(dòng)主要包括移動(dòng)和機(jī)械臂攜帶有效載荷運(yùn)動(dòng)兩個(gè)方面的內(nèi)容[4]。針對(duì)這兩個(gè)方面,本文開展了帶機(jī)械臂的月面巡視探測(cè)器的路徑規(guī)劃方法研究,主要內(nèi)容包括:巡視器移動(dòng)的路徑規(guī)劃(簡(jiǎn)稱巡視器的路徑規(guī)劃)、巡視器機(jī)械臂的路徑規(guī)劃(簡(jiǎn)稱機(jī)械臂的路徑規(guī)劃)、巡視器攜帶機(jī)械臂就位探測(cè)時(shí)巡視器與機(jī)械臂的路徑規(guī)劃(簡(jiǎn)稱器臂動(dòng)態(tài)聯(lián)合的路徑規(guī)劃)。
巡視器的路徑規(guī)劃方法(Lunar Rover Path Planning M ethod, LRPPM)是指在一個(gè)可視區(qū)域內(nèi),巡視器以某一個(gè)或幾個(gè)因素作為優(yōu)化條件,發(fā)現(xiàn)較優(yōu)行進(jìn)路線的過(guò)程。與地面的自主移動(dòng)機(jī)器人不同,巡視器在完成路徑規(guī)劃的過(guò)程中,只能靠自身的環(huán)境建模和航位推算,而不能借助外部的導(dǎo)航和標(biāo)定系統(tǒng),路徑規(guī)劃過(guò)程具有動(dòng)態(tài)性和不完整性。此外,路徑規(guī)劃還需要考慮能量的限制、巡視器運(yùn)行的安全性和平穩(wěn)性、車體的最小轉(zhuǎn)彎半徑、適宜弧線、越障通過(guò)能力等特殊要求。本文巡視器的路徑規(guī)劃方法涉及到環(huán)境表達(dá)、路徑評(píng)估和路徑搜索。
針對(duì)月面環(huán)境以非結(jié)構(gòu)化為顯著特征,巡視器的路徑規(guī)劃基于三維數(shù)字高程圖(DEM),采用改進(jìn)的A*算法完成路徑的搜索與擇優(yōu)。
月面巡視探測(cè)器所在環(huán)境是實(shí)際的物理環(huán)境,而進(jìn)行路徑規(guī)劃需要將物理環(huán)境抽象成為能被計(jì)算機(jī)理解和表達(dá)的環(huán)境模型,稱為環(huán)境建模,即地圖創(chuàng)建(M ap-building)[5]。地圖創(chuàng)建首先要確定環(huán)境地圖的描述方法,即數(shù)字地圖的表示方法,必須便于計(jì)算機(jī)存儲(chǔ)和處理。目前各國(guó)研究者已經(jīng)提出了多種表示方法,常用的環(huán)境建模方法有柵格法、幾何法、C 空間(C-space)法和拓?fù)鋱D等。每種數(shù)字地圖的表示方法都有自己的優(yōu)點(diǎn)和缺點(diǎn),根據(jù)月面巡視探測(cè)器環(huán)境感知的特點(diǎn),一般采用柵格法。
基于柵格的環(huán)境表示方法最早由Elfes 和M orave 提出[6]。所謂柵格法就是將地形環(huán)境用柵格進(jìn)行覆蓋,每個(gè)柵格的屬性值代表所覆蓋地形的屬性,如高度屬性或區(qū)域?qū)傩?自由空間或障礙物等)。最簡(jiǎn)單的柵格法是將地圖分成大小相同的柵格,這樣地圖就可以用數(shù)組或矩陣表示,數(shù)組的索引號(hào)與柵格在地圖中的位置對(duì)應(yīng),而元素值則與柵格屬性對(duì)應(yīng),形成數(shù)字高程圖(DEM)。
利用柵格法進(jìn)行環(huán)境建模時(shí),一般規(guī)定只能向其水平和豎直方向的相鄰柵格中心移動(dòng)(稱為4-鄰域移動(dòng))或向其周圍相鄰的八個(gè)柵格中心移動(dòng)(稱為8-鄰域移動(dòng))。柵格法的優(yōu)點(diǎn)包括:柵格法容易創(chuàng)建和維護(hù),便于計(jì)算機(jī)存儲(chǔ)、處理和使用;在路徑搜索時(shí),柵格對(duì)應(yīng)圖的節(jié)點(diǎn),節(jié)點(diǎn)間連通性已經(jīng)暗含在柵格的相鄰性當(dāng)中,便于算法搜索。柵格法的缺點(diǎn)包括:空間效率不高;柵格的大小影響搜索路徑的完備性;4-鄰域或8-鄰域移動(dòng)準(zhǔn)則限制搜索路徑的最優(yōu)性。但是,柵格法的這些缺點(diǎn)可以通過(guò)改進(jìn)搜索算法和進(jìn)行規(guī)劃路徑的后續(xù)處理來(lái)克服。
因此,本文采用柵格的描述方法來(lái)對(duì)月面環(huán)境進(jìn)行描述,充分考慮巡視器的可通過(guò)性,給不同地形對(duì)應(yīng)的柵格賦予不同的值,并在此基礎(chǔ)上進(jìn)行路徑評(píng)估與路徑搜索。
路徑規(guī)劃的評(píng)估與搜索是路徑規(guī)劃的核心內(nèi)容。國(guó)外應(yīng)用過(guò)的路徑規(guī)劃搜索算法包括啟發(fā)式搜索(A*)算法研究、幾何搜索算法研究和統(tǒng)計(jì)搜索算法研究等。巡視器路徑規(guī)劃方法根據(jù)不同的原則可以有不同的分類方法,按是否使用智能算法大致可以分為傳統(tǒng)方法和智能方法[7]。根據(jù)工作環(huán)境,路徑規(guī)劃模型可分為全局路徑規(guī)劃和局部路徑規(guī)劃[8]。在巡視探測(cè)器中,使用得比較多的是人工勢(shì)場(chǎng)法、A*算法和動(dòng)態(tài)啟發(fā)式搜索(D*)算法[3]、廣度優(yōu)先(Bug)算法、深度優(yōu)先(ISE)算法等。本文路徑規(guī)劃的方法采用了具有啟發(fā)、智能和全局特性的改進(jìn)的A*算法。
A*算法使用了啟發(fā)式方法,這種方法通過(guò)充分利用圖給出的信息來(lái)動(dòng)態(tài)地做出決定,使搜索次數(shù)大大降低。它通過(guò)一個(gè)評(píng)估函數(shù)(Heuristic Function)f(n)來(lái)估計(jì)圖中的當(dāng)前點(diǎn)到終點(diǎn)的距離(帶權(quán)值),并由此決定它的搜索方向,當(dāng)這條路徑失敗時(shí),它會(huì)嘗試其它路徑。A*算法成功與否的關(guān)鍵在于評(píng)估函數(shù)的正確選擇。
A*算法的評(píng)估函數(shù)為
式中,n 為搜索路徑, f(n)為預(yù)測(cè)的全程移動(dòng)消耗,g(n)為從起點(diǎn)到當(dāng)前點(diǎn)的實(shí)際移動(dòng)消耗,h(n)為當(dāng)前點(diǎn)到終點(diǎn)的移動(dòng)消耗估計(jì)值。
本文選取只對(duì)目標(biāo)點(diǎn)感興趣的Manhattan 估算函數(shù)作為啟發(fā)因子,以達(dá)到最佳運(yùn)算效率。M anhattan 距離為
式中,A.x 為當(dāng)前點(diǎn)的x 坐標(biāo)值,A.y 為當(dāng)前點(diǎn)的y 坐標(biāo)值;goal.x 為目標(biāo)點(diǎn)的x 坐標(biāo)值,goal.y為目標(biāo)點(diǎn)的y 坐標(biāo)值;h(A)為當(dāng)前點(diǎn)的Manhattan估計(jì)值。
由上式可知,M anhattan 估算函數(shù)與當(dāng)前點(diǎn)到目標(biāo)點(diǎn)的x 方向距離絕對(duì)值及y 方向距離絕對(duì)值的和成正比,即具有估算因子的功能,又擁有較高的運(yùn)算效率。然而,巡視器可以在地圖上作斜線方向的運(yùn)動(dòng),故將M ahattan 距離修正為
此時(shí),路徑會(huì)明顯分為兩個(gè)階段:第一階段,路徑點(diǎn)首先向著靠近目標(biāo)點(diǎn)斜對(duì)角線的方向延伸,當(dāng)規(guī)劃的路徑走到與目標(biāo)點(diǎn)斜對(duì)角時(shí),路徑走向進(jìn)入第二階段;第二階段,路徑點(diǎn)以斜向移動(dòng)快速靠近目標(biāo)點(diǎn),這種方法相對(duì)于鋸齒狀移動(dòng)的路徑,消耗大為減少。
此外,本文在A*算法的處理上還做了兩個(gè)方面的修正。首先,對(duì)自排序鏈表的構(gòu)建進(jìn)行了優(yōu)化,即open 列表在組建時(shí)根據(jù)f 值大小進(jìn)行排序, f 值小的排在表頭處,使得插入點(diǎn)的排序工作僅在表尾幾個(gè)節(jié)點(diǎn)進(jìn)行,甚至可能不需要移動(dòng)任何節(jié)點(diǎn)直接將新節(jié)點(diǎn)排在表頭,不僅節(jié)省了運(yùn)算時(shí)間,還有利于保留曾經(jīng)走過(guò)的路徑信息。其次,在尋路結(jié)束之后,在返回規(guī)劃路徑之前對(duì)路徑鏈表進(jìn)行了優(yōu)化處理,通過(guò)兩個(gè)指針從路徑鏈表的兩端向另一端遍歷,尋找出路徑鏈表中的迂回路徑,并切除多余部分的路徑節(jié)點(diǎn)。算法的流程圖如圖1 所示??傊?改進(jìn)的A*算法不僅考慮了距離和柵格的可通過(guò)性,還提高了路徑規(guī)劃的效率,適合用于非結(jié)構(gòu)化月面柵格地圖上的路徑規(guī)劃。
圖1 算法流程圖Fig.1 Flow chart of arithmetic
采用改進(jìn)的A*算法,在構(gòu)建的柵格月面地圖上進(jìn)行路徑規(guī)劃, 如圖2 所示。已知單位柵格為10mm ×10mm, 路徑的起始點(diǎn)坐標(biāo)為(140mm,930mm),目標(biāo)點(diǎn)坐標(biāo)為(910mm ,150mm),則路徑搜索過(guò)程耗時(shí)1.293s,路徑長(zhǎng)度為1 381.5mm,滿足巡視器的路徑規(guī)劃的需求。
圖2 采用改進(jìn)A*算法的路徑規(guī)劃Fig.2 Path planning using improved A*arithmetic
機(jī)械臂的路徑規(guī)劃方法(Manipulator Path Planning Method,MPPM)與巡視器的路徑規(guī)劃方法有著本質(zhì)的不同,巡視器的路徑規(guī)劃受限于地形地貌和巡視器的行駛性能;而機(jī)械臂的規(guī)劃過(guò)程受外界的影響一般僅是碰撞檢測(cè)方面,其余則受限于自身的特性,如工作空間、臂長(zhǎng)和構(gòu)型設(shè)計(jì)的影響等。
機(jī)械臂的路徑規(guī)劃M PPM,首先在機(jī)械臂Denavit-Harterberg(D-H)建模的基礎(chǔ)上,采用蒙特卡羅法求得機(jī)械臂的可達(dá)工作空間,然后規(guī)劃得出機(jī)械臂工作空間內(nèi)的路徑點(diǎn),最后通過(guò)運(yùn)動(dòng)學(xué)逆解計(jì)算和關(guān)節(jié)插值,得到機(jī)械臂的運(yùn)動(dòng)路徑。
巡視器機(jī)械臂簡(jiǎn)化模型如圖3a 所示,由三個(gè)關(guān)節(jié)組成。其中,a1=110mm ,指關(guān)節(jié)1 與關(guān)節(jié)2 軸線之間的距離;a2=350mm,指關(guān)節(jié)2 與關(guān)節(jié)3 軸線之間的距離;a3=110mm,指有效載荷末端到關(guān)節(jié)3 軸線的距離。
圖3 機(jī)械臂建模Fig.3 Model of the manipulato r
根據(jù)D-H 坐標(biāo)系建立法則,建立機(jī)械臂的DH 模型,如圖3b 所示。其中,Z1指關(guān)節(jié)1 的轉(zhuǎn)動(dòng)軸線,Z2指關(guān)節(jié)2 的轉(zhuǎn)動(dòng)軸線,Z3指關(guān)節(jié)3 的轉(zhuǎn)動(dòng)軸線,Z 4 的坐標(biāo)原點(diǎn)位于有效載荷末端面的中心。機(jī)械臂模型的D-H 參數(shù)如表1 所示。
表1 D-H參數(shù)表Table 1 D-H parameter
由機(jī)械臂模型的D-H 參數(shù),求機(jī)械臂末端點(diǎn)在x,y,z 坐標(biāo)系下的位置參數(shù)方程為
機(jī)械臂工作空間的求解采用蒙特卡羅法[9]。蒙特卡羅法是一種基于“隨機(jī)數(shù)”的計(jì)算方法,屬于概率統(tǒng)計(jì)方法的一種,同時(shí)也具有一般數(shù)值方法的優(yōu)越性。求解機(jī)械臂的工作空間時(shí),對(duì)關(guān)節(jié)變量通過(guò)均勻分布,賦予一定數(shù)量的、符合關(guān)節(jié)變化要求的隨機(jī)量,從而得到工作空間由隨機(jī)點(diǎn)構(gòu)成的圖形,稱之為“云圖”,這樣就構(gòu)成了機(jī)械臂的可達(dá)工作空間。求解方法的程序流程圖,如圖4 所示。
圖4 蒙特卡羅方法求解工作空間的程序流程圖Fig.4 Flow chart of finding workspace using Monte Carlo method
根據(jù)D-H 參數(shù),考慮機(jī)械臂的關(guān)節(jié)空間約束,使用式(4)~(6),采用蒙特卡羅法,對(duì)機(jī)械臂進(jìn)行工作空間分析,確定機(jī)械臂的可達(dá)工作空間如圖5 所示。由圖5 可以看出,三自由度機(jī)械臂的構(gòu)型對(duì)應(yīng)的可達(dá)工作空間為一個(gè)帶半球空腔的半球體,機(jī)械臂與其可達(dá)工作空間的關(guān)系如圖5 a 所示,可達(dá)工作空間的兩個(gè)方向的視圖如圖5 b、c 所示。
圖5 機(jī)械臂的可達(dá)工作空間Fig.5 Reachable w orkspace of the manipulator
在機(jī)械臂的工作空間內(nèi),根據(jù)機(jī)械臂的起始點(diǎn)和目標(biāo)點(diǎn),確定多個(gè)路徑段,每個(gè)路徑段用小直線段表示。
首先,通過(guò)運(yùn)動(dòng)學(xué)反解計(jì)算,得出每個(gè)路徑段內(nèi)每個(gè)關(guān)節(jié)需要運(yùn)動(dòng)的范圍,設(shè)起始關(guān)節(jié)角度為θ0,終止關(guān)節(jié)角度為θf(wàn)。
然后,使用關(guān)節(jié)插值計(jì)算,本文采用三次多項(xiàng)式插值,可以得出機(jī)械臂的運(yùn)動(dòng)路徑(軌跡)。
每個(gè)關(guān)節(jié)的運(yùn)動(dòng)軌跡為
運(yùn)動(dòng)軌跡上關(guān)節(jié)的角速度為
運(yùn)動(dòng)軌跡上關(guān)節(jié)的角加速度為
其中,t 為運(yùn)動(dòng)軌道的時(shí)間變量, a0=θ0,a1=
這種機(jī)械臂的路徑規(guī)劃的方法比較簡(jiǎn)單、直觀,是在計(jì)算出機(jī)械臂的工作空間的基礎(chǔ)上,人為確定路徑點(diǎn),然后通過(guò)機(jī)械臂運(yùn)動(dòng)學(xué)計(jì)算最終得出其運(yùn)動(dòng)路徑。此外,可以通過(guò)碰撞檢測(cè),由機(jī)械臂自主確定路徑點(diǎn),完成整個(gè)路徑規(guī)劃過(guò)程。但是,碰撞檢測(cè)的設(shè)置需要機(jī)械臂所處外部環(huán)境的精確建模和機(jī)械臂本體與外部環(huán)境的接觸判斷,受多方面因素的限制,相對(duì)復(fù)雜,在一定程度上會(huì)影響機(jī)械臂的探測(cè)效率,故暫不采用。
巡視器攜帶機(jī)械臂就位探測(cè)時(shí)器臂動(dòng)態(tài)聯(lián)合的路徑規(guī)劃方法(Rover-M anipulator Path Planning M ethod,RM PPM),將上述巡視器和機(jī)械臂的路徑規(guī)劃結(jié)合起來(lái),巡視器路徑規(guī)劃的目標(biāo)點(diǎn)和巡視器于目標(biāo)點(diǎn)的探測(cè)姿態(tài),將作為機(jī)械臂路徑規(guī)劃的初始點(diǎn)和初始姿態(tài),如圖6 所示。整個(gè)規(guī)劃過(guò)程描述如下:
首先,確定巡視器路徑規(guī)劃的目標(biāo)點(diǎn)和巡視器于目標(biāo)點(diǎn)的探測(cè)姿態(tài),使機(jī)械臂末端點(diǎn)和探測(cè)目標(biāo)點(diǎn)均處于機(jī)械臂的工作空間范圍內(nèi),如圖6 中石塊6 處的放大圖部分所示;
其次,采用改進(jìn)的A*算法,在三維柵格數(shù)字高程地形圖上,進(jìn)行巡視器路徑規(guī)劃,得出較優(yōu)的巡視器規(guī)劃路徑,其中,巡視器初始點(diǎn)根據(jù)柵格地形確定,目標(biāo)點(diǎn)為上一步確定的巡視器路徑規(guī)劃的目標(biāo)點(diǎn);
再次,于目標(biāo)點(diǎn)調(diào)整巡視器到探測(cè)姿態(tài),在調(diào)整過(guò)程中,有巡視器最佳探測(cè)方向的判斷,需要精確的求解機(jī)械臂的可達(dá)工作空間,使機(jī)械臂末端點(diǎn)和探測(cè)目標(biāo)點(diǎn)均處于機(jī)械臂的工作空間范圍內(nèi),同時(shí)保證巡視器處于最穩(wěn)定的狀態(tài),以免機(jī)械臂探測(cè)時(shí)巡視器出現(xiàn)滑動(dòng)、傾斜等問(wèn)題;
最后,在機(jī)械臂的工作空間內(nèi),確定機(jī)械臂的路徑點(diǎn),用小直線段連接,通過(guò)運(yùn)動(dòng)學(xué)反解計(jì)算和三次多項(xiàng)式插值計(jì)算,得出機(jī)械臂的規(guī)劃路徑。
整個(gè)路徑規(guī)劃過(guò)程之所以稱為動(dòng)態(tài)聯(lián)合,是因?yàn)槭芊墙Y(jié)構(gòu)化月面地形的影響,巡視器于目標(biāo)點(diǎn)的位姿與初始規(guī)劃的位姿一般會(huì)有較大的偏差,需要根據(jù)遙測(cè)信息進(jìn)行修正,才能作為機(jī)械臂路徑規(guī)劃的輸入。
圖6 器臂動(dòng)態(tài)聯(lián)合的路徑規(guī)劃示意圖Fig.6 Sketch map of Rover-Manipulator Path Planning
綜上所述,巡視器的路徑規(guī)劃有兩種:一種是針對(duì)移動(dòng)的巡視器的路徑規(guī)劃方法(LRPPM),一種是針對(duì)機(jī)械臂的路徑規(guī)劃方法(M PPM),兩種結(jié)合起來(lái)(RMPPM),才能夠完成就位探測(cè)。因此,帶機(jī)械臂的月面巡視探測(cè)器的路徑規(guī)劃方法的研究?jī)?nèi)容,不僅包括巡視器的路徑規(guī)劃和機(jī)械臂的路徑規(guī)劃,還需要完成巡視器攜帶機(jī)械臂就位探測(cè)時(shí)器臂動(dòng)態(tài)聯(lián)合的路徑規(guī)劃。
帶機(jī)械臂的月面巡視探測(cè)器的路徑規(guī)劃方法的特點(diǎn)歸納總結(jié)如下:
1)巡視器的路徑規(guī)劃過(guò)程具有動(dòng)態(tài)性和不完整性。規(guī)劃方法基于月面三維數(shù)字高程圖(DEM),采用改進(jìn)的A*算法完成路徑的搜索與擇優(yōu)。
2)機(jī)械臂的路徑規(guī)劃僅在自己的可達(dá)工作空間內(nèi)進(jìn)行,需要針對(duì)障礙進(jìn)行碰撞檢測(cè)。規(guī)劃方法基于D-H 建模,采用蒙特卡羅法求得機(jī)械臂的可達(dá)工作空間,然后人為輔助通過(guò)運(yùn)動(dòng)學(xué)反解計(jì)算和三次多項(xiàng)式插值計(jì)算,規(guī)劃得出機(jī)械臂工作空間內(nèi)的運(yùn)動(dòng)路徑。
3)巡視器攜帶機(jī)械臂完成就位探測(cè)的過(guò)程,需要上述兩種規(guī)劃的動(dòng)態(tài)聯(lián)合。實(shí)際過(guò)程中,還需要地面操作人員的參與,尤其是機(jī)械臂初始方位的輸入和機(jī)械臂到位的判斷。
)
[1]葉培建,孫澤洲,饒煒.嫦娥一號(hào)月球探測(cè)衛(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)華.基于不確定信息的移動(dòng)機(jī)器人地圖創(chuàng)建研究進(jìn)展[J].機(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]張穎,吳成東, 原寶龍.機(jī)器人路徑規(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)計(jì)物理中的應(yīng)用[M].鄭州:河南科學(xué)技術(shù)出版社,1991