徐昭輝, 齊臣坤*,, 王衛(wèi)軍, , 韓亮亮, 陳 萌

(1上海交通大學(xué) 機械與動力工程學(xué)院,上海 200240,E-mail: chenkqi@sjtu.edu.cn;2上海宇航系統(tǒng)工程研究所,上海 201109)

月球是人類探索宇宙的重要目標(biāo),月球探測是我國航天工程的重要任務(wù)[1]。按照我國深空探測和載人月球探測發(fā)展戰(zhàn)略,在探測準(zhǔn)備階段,由月面探測機器人提前開展航天員登月區(qū)域的預(yù)先詳查,獲取著陸區(qū)域詳細(xì)的數(shù)據(jù)信息,為載人登月提供數(shù)據(jù)基礎(chǔ)[2-4]。目前各國的探月活動主要針對月海較為平緩的地形進(jìn)行探測,但這樣的探測活動并不能全面地了解月球。對于復(fù)雜地形的探測可以擴大探測的范圍和地形種類,有助更深入了解和研究月球。

目前,國內(nèi)外登陸成功的星表移動機器人均采用輪式移動系統(tǒng)和被動懸掛,以適應(yīng)外星球的不平坦地形。例如美國1997年的“探路者”號火星車、2004年的“勇氣號”和“機遇號”火星車[5]、2012年的“好奇號”火星車[6]、中國2013年的“玉兔”號月球車和2019年的“玉兔”二號月球車都是采用同樣方式適應(yīng)不平地形。盡管輪式移動系統(tǒng)能夠適應(yīng)一定程度的不平地形,但難以進(jìn)入復(fù)雜的崎嶇地形,因此研究能夠適應(yīng)月面復(fù)雜地形的足式機器人具有重要的意義。

美國卡內(nèi)基梅隆大學(xué)(CMU)早在1992年就研發(fā)了一款名為Dante[7]的八足探測機器人嘗試進(jìn)行火山口探測,而1994年研發(fā)的Dante Ⅱ[8]對火山口進(jìn)行了考察,在回程途中失去穩(wěn)定性,掉入火山口中。CMU之后在前兩代機器人的技術(shù)基礎(chǔ)上,針對火星探測任務(wù)研發(fā)了六足探測機器人Ambler[9],但由于機器人質(zhì)量體積過大未被用于實際探測。德國人工智能研究中心(DFKI)曾研發(fā)八足機器人SCORPION Ⅰ-Ⅳ[10]與四足機器人ARAMIES[11],之后基于經(jīng)驗開發(fā)了一款針對月表探測的六足機器人SpaceClimber[12]。此外一些實驗性質(zhì)的研究成果還有:德國DLR機構(gòu)研發(fā)了微型六足機器人DLR-Crawler[13],實現(xiàn)了起伏地形下的自主運動規(guī)劃;德國Karlsruhe大學(xué)研發(fā)了LAURON Ⅰ-Ⅴ[14]用于救援、勘探、監(jiān)測等危險任務(wù);瑞士蘇黎世聯(lián)邦理工學(xué)院(ETH)研發(fā)了一款適用于低重力條件下運動的四足機器人SpaceBok[15];國內(nèi)哈爾濱工業(yè)大學(xué)也曾研發(fā)一款重載月面探測機器人ELSpider[16]等等?？傮w來說,目前針對月面足式機器人研究已經(jīng)有了一定進(jìn)展,但離實際落地完成月面機器人任務(wù)還有不小差距。在月面機器人任務(wù)中,為保證后續(xù)載人登月任務(wù)的安全性,月面探測機器人需開展登月著陸區(qū)域的先導(dǎo)巡視詳查,因此需要對月面探測機器人的運動路徑進(jìn)行合理的規(guī)劃。而移動機器人的路徑規(guī)劃算法的開發(fā)與研究是當(dāng)下移動機器人的發(fā)展帶來的需求[17]。目前主要的路徑規(guī)劃算法主要有:

(1) A*算法:A*算法是一種典型的啟發(fā)式算法,A*算法的優(yōu)點體現(xiàn)在搜索路徑直接,對環(huán)境反應(yīng)快, 缺點是隨著地圖增大由于每一節(jié)點計算量大,導(dǎo)致運算時間變長,并且規(guī)劃路徑不考慮機器人運動模型而過于理想化。為了保證移動機器人能實際跟蹤規(guī)劃出的路徑,文獻(xiàn)[18]提出將動態(tài)窗口法(DWA)和A*算法融合,實現(xiàn)了路徑全局最優(yōu)和局部避障效果。

(2) 快速隨機搜索樹算法(Rapidly-exploring Random Tree, RRT):快速隨機搜索樹是一種采用隨機采樣的搜索方法。其優(yōu)點體現(xiàn)在理論簡單、不需要對狀態(tài)空間進(jìn)行預(yù)處理。其缺點是搜索方向隨機生成導(dǎo)致計算量大、生成路徑轉(zhuǎn)折點多,不適合機器人直接行走。文獻(xiàn)[19]討論了RRT算法在三維環(huán)境下的運用,以及利用貝塞爾曲線對路徑進(jìn)行優(yōu)化,有效的避免了RRT算法的缺陷。

(3) 蟻群算法等啟發(fā)式算法:蟻群算法是一種模擬螞蟻覓食行為的仿生算法[20]。其優(yōu)點是具有較強的魯棒性,但是依然存在收斂慢、計算效率低和容易陷入局部最優(yōu)等問題。文獻(xiàn)[21]提出一種自適應(yīng)優(yōu)化算法,在最優(yōu)路徑與規(guī)劃時間之間做到均衡。文獻(xiàn)[22]針對蟻群算法易陷入局部最優(yōu)、搜索時間過長等問題,使用Logistic 模型改進(jìn),加快了蟻群算法后期收斂速度。文獻(xiàn)[23]采取將動態(tài)窗口方法和改進(jìn)蟻群算法相結(jié)合,實現(xiàn)了機器人在車間動態(tài)環(huán)境下安全導(dǎo)航。

(4) 人工勢場法(APF):人工勢場法是一種虛擬力場法。人工勢場法的優(yōu)點是結(jié)構(gòu)簡單,生成的路徑一般比較平滑且安全,它的缺點是對障礙有一定要求,并且經(jīng)過狹窄通道時會發(fā)生擺動現(xiàn)象,易陷入局部極小點。文獻(xiàn)[24]結(jié)合了雙向RRT算法與人工勢場法實現(xiàn)了月面環(huán)境下的路徑規(guī)劃。

(5) 基于人工智能的方法:近年來結(jié)合強化學(xué)習(xí)的路徑規(guī)劃算法研究趨于熱門,比如文獻(xiàn)[25]中提出了一種基于Q學(xué)習(xí)和Sarsa算法的路徑規(guī)劃算法,并結(jié)合RRT算法成功實現(xiàn)了兼顧全局路徑和局部動態(tài)避障。

總的來說,當(dāng)前路徑規(guī)劃算法雖然種類繁多并且研究深度較深,但是這些算法沒有與實際環(huán)境做很好的結(jié)合應(yīng)用,并且主要考慮的是輪式移動平臺的運動模型而非足式機器人。因此需要結(jié)合月面足式機器人的運動模型與約束,建立適用于月面足式機器人的路徑規(guī)劃算法?；谏鲜鰡栴},本文先通過對月面地形的分析建立了二維路徑規(guī)劃仿真模型,之后建立六足機器人的運動模型分析得到其運動約束,再結(jié)合勢場法和六足機器人運動約束提出一種適用于月面六足機器人的路徑規(guī)劃方法,并通過仿真實驗驗證了該方法的性能和可行性。

1 月面地形分析與路徑規(guī)劃仿真環(huán)境建模

1.1 月面地形分析

月球表面主要分為月海和月陸兩大地理模塊。人類在地球上用肉眼看到的月面暗黑色斑塊,稱為“月海”。月海由古代玄武巖熔巖凝而成,是月面上寬廣的平原,約占月表面積的17%。月球上顏色較淺的區(qū)域被稱為月陸,約占月表面積的83%。月陸地區(qū)有大量的山脈,雖然月球比地球小得多,但月球表面的最大起伏可達(dá)16 km(地球最高處和最低處相差約20 km)。月陸地區(qū)較月海地區(qū)的起伏更大。

月海和月陸均覆蓋有大大小小的石塊和撞擊坑。石塊是指月面上分布的巖石,主要由月球表面或其附近由巖漿直接結(jié)晶和固化形成;撞擊坑是指布滿月球表面的環(huán)形凹坑構(gòu)造,包括環(huán)形山、輻射紋以及與撞擊坑有關(guān)的隆起構(gòu)造,這些是因為小行星、彗星等宇宙內(nèi)星體與月球表面直接相撞,常年累月積累形成。此外,月球表面覆蓋有松軟的月壤,月壤顆粒直徑以小于1 mm為主。月面復(fù)雜地形實景照片如圖1所示。

圖1 月面復(fù)雜地形

1.2 月面路徑規(guī)劃仿真環(huán)境

路徑規(guī)劃往往是將三維環(huán)境投影到二維平面。目前月面環(huán)境難以在現(xiàn)實中復(fù)刻,因此采取仿真環(huán)境進(jìn)行路徑規(guī)劃算法的測試和實現(xiàn)。從圖2月面俯瞰圖可得,月面的障礙物主要由撞擊坑和大型石塊等組成。月面環(huán)境由于缺少空氣流動和地質(zhì)運動,大型石塊的分布比較隨機,形狀也近似規(guī)則形狀,因此可以用二維平面內(nèi)的圓形障礙物替代。本文提出了一種月面二維地圖模型生成算法,生成地圖效果如圖3所示。

圖2 月面俯瞰圖

圖3 仿真模型

月面二維地圖模型隨機生成算法流程如下:

算法1 隨機月面仿真環(huán)境二維平面生成算法

輸入 地圖的最大尺寸size與地圖內(nèi)部的障礙物數(shù)量num

輸出 帶有障礙物的二維地圖

(1) 根據(jù)地圖尺寸size和障礙物數(shù)量num在地圖隨機選取障礙物中心坐標(biāo)。

(2) 為每一個障礙物隨機生成半徑。

(3) 檢測是否發(fā)生干涉,如果沒有則輸出地圖,否則重復(fù)1-2。

2 六足機器人運動模型與約束

當(dāng)?shù)匦苇h(huán)境較復(fù)雜時,如石塊、坑洼、斜坡等崎嶇地形,腿式機器人可以選擇較好的落腳支撐點,通過性較好。足式機器人通過抬腿邁步可以避免機器人受困于松軟月壤,具有較好的適應(yīng)能力[26]。而與雙足機器人和四足機器人對比,六足機器人行走總是處于靜態(tài)穩(wěn)定狀態(tài),穩(wěn)定裕度大,更適合月面復(fù)雜崎嶇的地形[27]。因此,六足機器人是月面復(fù)雜崎嶇地形環(huán)境下探測的較優(yōu)方案。六足機器人如圖4所示。

圖4 六足機器人

2.1 六足機器人腿部運動學(xué)模型

六足機器人腿機構(gòu)的坐標(biāo)系和參數(shù)定義如圖5所示,ΣL為腿坐標(biāo)系,與機架固定連接,位于腿的髖關(guān)節(jié),三個主動驅(qū)動的軸線交點,x軸向前,與側(cè)擺電機轉(zhuǎn)動軸線同軸,y軸向左,z軸向上。

圖5 六足機器人腿機構(gòu)

記ΣL腿坐標(biāo)系下腿部構(gòu)機構(gòu)的輸出廣義坐標(biāo)Pep為(xep,yep,zep),其中,xep,yep,zep為足尖末端在ΣL中的笛卡爾坐標(biāo)。另一方面,記腿部機構(gòu)的輸入廣義坐標(biāo)系q為(γ,α,β),其中,γ,α,β分別為如圖所示的驅(qū)動側(cè)擺、大腿和小腿電機的轉(zhuǎn)角。該機構(gòu)大腿長度為Lt,小腿長度為Lc。根據(jù)這些參數(shù)可以計算腿機構(gòu)的運動學(xué)位置正解。運動學(xué)位置正解,即已知腿的輸入廣義坐標(biāo)q,求解得到輸出廣義坐標(biāo)Pep:

(1)

根據(jù)式(1)求腿機構(gòu)的運動學(xué)位置反解,即已知腿的輸出廣義坐標(biāo)Pep,求解輸入廣義坐標(biāo)q,整理可得:

(2)

六足機器人的坐標(biāo)系主要有機身坐標(biāo)系、各腿機構(gòu)坐標(biāo)系。機身與各腿機構(gòu)的關(guān)系為:ΣLi(i=1,2,…,6)為第i條腿的腿機構(gòu)坐標(biāo)系。ΣB為機器人機身坐標(biāo)系,位于ΣLi(i=1,2,…,6)的幾何中心。機器人各腿坐標(biāo)系與機身坐標(biāo)系ΣB之間旋轉(zhuǎn)變換矩陣和位移變換矩陣定義如表1所示。

表1 機器人各腿坐標(biāo)系與機身坐標(biāo)系ΣB之間變換矩陣

六足機器人在設(shè)計上具有中心對稱布局,因此各腿坐標(biāo)系位移變換矩陣可以通過機身寬度DW和機身長度DL表示出來,旋轉(zhuǎn)變換矩陣一般視為無旋轉(zhuǎn)變換的單位矩陣。根據(jù)上述定義,若已知第i條腿末端在腿坐標(biāo)系ΣLi下的位置Pep(ΣLi),則在機身坐標(biāo)系ΣB下的描述Pep,Li(ΣB)為:

Pep,Li(ΣB)=TB,Li+RB,LiPep(ΣLi)

(3)

2.2 六足機器人運動模型

機器人二維平面內(nèi)當(dāng)前坐標(biāo)為(x,y,θ), 當(dāng)前運動狀態(tài)為(v,ω),其中x,y為當(dāng)前坐標(biāo),θ為當(dāng)前角度,v為當(dāng)前線速度,ω為當(dāng)前角速度。若當(dāng)前輪式機器人的轉(zhuǎn)向半徑為r,輪式機器人的運動模型為:

(3)

輪式機器人一般通過控制線速度v和轉(zhuǎn)向半徑r的大小來控制機器人的運動。與輪式機器人相比,六足機器人主要通過控制單步步長d和當(dāng)前轉(zhuǎn)向變化角Δθ來控制運動。設(shè)六足機器人邁出一步前后的坐標(biāo)分別為(xi,yi,θi)、(xi+1,yi+1,θi+1),其運動模型為:

(4)

對于輪式機器人,由于線速度v和轉(zhuǎn)向半徑r的大小可以在運動過程中隨時調(diào)整,所以輪式機器人的運動軌跡是連續(xù)的。但是六足機器人的運動模型是離散化的,其運動軌跡是一系列連續(xù)的點。這是因為單步步長d和當(dāng)前轉(zhuǎn)向變化角Δθ在邁步過程中是無法修改的。其運動軌跡如圖6所示。

圖6 六足機器人運動軌跡

圖7 人工勢場

2.3 六足機器人運動約束

由式 (5)總結(jié)可以得到,六足機器人的機身中心在二維平面內(nèi)的運動軌跡是離散的,而根據(jù)式(3)可以得到,在世界坐標(biāo)系下,第i條腿末端位置Pep,Li(W)為:

(7)

由于支撐腿末端在運動過程中在世界坐標(biāo)系下是靜止不動的,所以六足機器人的足端支撐軌跡也是一系列離散的點。而擺動腿末端的運動目標(biāo)位置是下一次支撐腿-擺動腿輪換的支撐腿末端位置,所以六足機器人與障礙物之間需要進(jìn)行碰撞檢測Collision(x)的位置就是每一步的支撐腿末端位置(Collision(x)=0視為未發(fā)生碰撞),該位置可以通過式(6)計算得到。而在實際行走過程中,六足機器人的單步步長d一般也是固定值,所以六足機器人受到的運動控制主要是每一步的轉(zhuǎn)向變化角Δθ,該轉(zhuǎn)向變化角受到最大值和最小值約束。因此六足機器人的運動約束為:

(7)

3 路徑規(guī)劃算法

基于式(7)的六足機器人運動約束,本文提出了一種適用于月面六足機器人的路徑規(guī)劃算法,其核心是一個基于勢場和運動約束的代價函數(shù)F(xi),具體組成在本節(jié)后續(xù)內(nèi)容中會詳細(xì)介紹。算法具體流程如下所示:

算法2 結(jié)合勢場的六足機器月面路徑規(guī)劃算法

輸入 二維地圖模型,起始點,目標(biāo)點,轉(zhuǎn)向變化角限制

輸出 算法規(guī)劃的月面六足機器人運動路徑

1.計算當(dāng)前六足機器人運動狀態(tài)

2.代入代價函數(shù)F(xi),建立最優(yōu)化問題模型

3求解該最優(yōu)化問題

4.將最優(yōu)解代入,計算下一步六足機器人的位置

5.重復(fù)1-4,直到六足機器人到達(dá)目標(biāo)點

3.1 勢場法

勢場法是用人工虛擬的一種模仿重力勢場的方法來規(guī)劃控制機器人的避障行駛。這種路徑規(guī)劃算法的具體實現(xiàn)思路是:在移動機器人運行的環(huán)境空間中構(gòu)造一個虛擬的人工勢場,該勢場由兩部分組成:源于目標(biāo)點的引力勢場Uatt(xi),和源于障礙物的斥力勢場Ureq(xi),移動機器人采用

基于人工勢場法的路徑規(guī)劃時,其前進(jìn)方向是由當(dāng)前所處位置的總勢場的梯度下降方向決定的。

移動機器人、障礙物中心點、目標(biāo)點以點的形式在以起始點為原點的環(huán)境二維坐標(biāo)系空間中表示,每個運動點xi可表示為:

(8)

當(dāng)前環(huán)境內(nèi)的全局人工虛擬勢場函數(shù)由目標(biāo)終點所產(chǎn)生的引力勢場和各個障礙物所產(chǎn)生的斥力勢場共同組成,全局勢場函數(shù)為:

U(xi)=Ureq(xi)+Uatt(xi)

(9)

在虛擬勢場中,當(dāng)機器人當(dāng)前位置的總勢能為零時,則說明機器人已經(jīng)到達(dá)目標(biāo)點。相比于其他避障算法,人工勢場法具有計算空間較小、計算速度快等優(yōu)點,但也存在難以解決的問題,如目標(biāo)點不可到達(dá)問題、局部最小點問題等。目標(biāo)點不可到達(dá)問題即移動機器人在目標(biāo)點周圍不停的振蕩,且始終無法到達(dá)目標(biāo)點問題,可以通過適當(dāng)調(diào)整勢場函數(shù)克服。人工勢場的數(shù)學(xué)模型主要由斥力函數(shù)Ureq(xi)和引力函數(shù)Uatt(xi)構(gòu)成。斥力勢場和引力勢場函數(shù)的數(shù)學(xué)公式設(shè)計如下:

(10)

(11)

式(11)中引力函數(shù)可以保證機器人在距離目標(biāo)點較近的情況下仍舊保有一定勢場梯度,從而避免機器人無法到達(dá)目標(biāo)點的情況。然而在全局勢場中,障礙物的分布可能會導(dǎo)致局部最優(yōu)點。下面通過結(jié)合六足步行機器人運動約束突破困境。

3.2 結(jié)合運動約束的地圖代價函數(shù)設(shè)計

根據(jù)地圖內(nèi)障礙物位置尺寸信息以及目標(biāo)點位置信息,基于式(10)和(11)計算地圖內(nèi)人工勢場函數(shù)U(x)。結(jié)合六足步行機器人運動特性,為了保證機器人能夠完全避免碰撞,將機器人每條腿末端的坐標(biāo)作為代價函數(shù)計算的變量,構(gòu)造代價函數(shù)f(θi):

(12)

(13)

3.3 路徑規(guī)劃的平滑優(yōu)化方法

為了使得規(guī)劃出來的路徑更加平滑并且保證最后可以收斂到目標(biāo)點位置,在式(12)的代價函數(shù)中添加平滑函數(shù)s(θi)和收斂函數(shù)h(θi):

s(θi)=c3|θi|

(14)

h(θi)=c4|θi-θgoal|

(15)

式中:θgoal為當(dāng)前路徑點對目標(biāo)位置點的朝向角。代價函數(shù)F(θi)修改為:

F(θi)=f(θi)+s(θi)+h(θi)

(16)

根據(jù)以上代價函數(shù)求解最優(yōu)化問題:

minF(θi)
s.t.θi∈[θi-1+θd,min,θi-1+θd,max]

(17)

為了加快求解速度,可以通過均勻采樣方法求解該最優(yōu)化問題,得到θi,計算該情況下機器人下一步新坐標(biāo)。重復(fù)該最優(yōu)化求解過程,直到當(dāng)前路徑點到達(dá)目標(biāo)位置點。

4 實驗與分析

4.1 平滑優(yōu)化方法

為了驗證算法中的平滑優(yōu)化方法的有效,c1、c2、c4)的值固定,調(diào)整c3的值,以c3、c4)的比值ratio為變量,實驗比較不同情況下算法規(guī)劃的路徑長度和路徑點平均轉(zhuǎn)向變化角絕對值,規(guī)劃的路徑樣例結(jié)果見圖8,實驗結(jié)果見表2。

表2 平滑優(yōu)化參數(shù)調(diào)整影響

圖8 ratio=1.0的規(guī)劃路徑

根據(jù)實驗結(jié)果可得,當(dāng)比值越大時,平均轉(zhuǎn)向變化角絕對值越小,即規(guī)劃路徑越光滑,但是當(dāng)比值大于等于2.0時,會導(dǎo)致規(guī)劃出來的路徑無法到達(dá)目標(biāo)點,即平滑代價函數(shù)在總代價函數(shù)中的占比過高。此外,比值大小對規(guī)劃路徑長度影響較小。因此選擇一個合適的比值主要影響算法的可行性和規(guī)劃路徑平滑性。

4.2 算法性能對比

為了驗證算法的性能,本文算法命名為HEX-APF,與常用的A*算法、RRT算法和傳統(tǒng)APF算法在二維月面仿真環(huán)境中進(jìn)行對比實驗,具體實驗結(jié)果如下所示:

根據(jù)表3可得,HEX-APF算法相比于A*算法和RRT算法具有更高的運行效率,因此更適合機器人機載平臺電腦算力有限的實際限制,而APF算法盡管運行效率更高,但由于局部最優(yōu)問題存在一定失敗概率,魯棒性較差。而根據(jù)表4可得,HEX-APF規(guī)劃出來的路徑長度小于APF和RRT算法結(jié)果,略高于A*算法結(jié)果。此外根據(jù)圖9和圖10,A*算法和RRT算法規(guī)劃的路徑點中一些點距離障礙物太近,有可能發(fā)生碰撞。因此綜合考慮下,HEX-APF是這些算法中性能表現(xiàn)最好的算法。

表3 算法運行時間/s

表4 規(guī)劃路徑長度/m

圖9 算法性能對比實驗1結(jié)果

5 結(jié)論

針對月面環(huán)境六足機器人的需求,提出了一種有效魯棒的路徑規(guī)劃算法。通過人工勢場法和結(jié)合六足機器人運動約束,引入平滑函數(shù)和收斂函數(shù),算法具有較高的運行效率和性能。在根據(jù)月面地形建立的二維月面仿真環(huán)境中,本文算法與常用的A*算法、RRT算法和APF算法相比,具有與A*算法相近的運行結(jié)果和更高的運行效率,比RRT算法性能更好,比APF算法具有更高魯棒性。

下一步工作:考慮月面崎嶇地形的影響,引入結(jié)合傳感器的實時路徑規(guī)劃和六足機器人步態(tài)規(guī)劃。