基于勢(shì)場(chǎng)和運(yùn)動(dòng)約束的月面六足機(jī)器人路徑規(guī)劃算法

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

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

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

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

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

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

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

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

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

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

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

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)造,這些是因?yàn)樾⌒行恰㈠缧堑扔钪鎯?nèi)星體與月球表面直接相撞,常年累月積累形成。此外,月球表面覆蓋有松軟的月壤,月壤顆粒直徑以小于1 mm為主。月面復(fù)雜地形實(shí)景照片如圖1所示。

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

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

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

圖2 月面俯瞰圖

圖3 仿真模型

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

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

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

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

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

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

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

2 六足機(jī)器人運(yùn)動(dòng)模型與約束

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

圖4 六足機(jī)器人

2.1 六足機(jī)器人腿部運(yùn)動(dòng)學(xué)模型

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

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

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

(1)

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

(2)

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

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

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

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

(3)

2.2 六足機(jī)器人運(yùn)動(dòng)模型

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

(3)

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

(4)

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

圖6 六足機(jī)器人運(yùn)動(dòng)軌跡

圖7 人工勢(shì)場(chǎng)

2.3 六足機(jī)器人運(yùn)動(dòng)約束

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

(7)

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

(7)

3 路徑規(guī)劃算法

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

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

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

輸出 算法規(guī)劃的月面六足機(jī)器人運(yùn)動(dòng)路徑

1.計(jì)算當(dāng)前六足機(jī)器人運(yùn)動(dòng)狀態(tài)

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

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

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

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

3.1 勢(shì)場(chǎng)法

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

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

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

(8)

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

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

(9)

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

(10)

(11)

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

3.2 結(jié)合運(yùn)動(dòng)約束的地圖代價(jià)函數(shù)設(shè)計(jì)

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

(12)

(13)

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

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

s(θi)=c3|θi|

(14)

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

(15)

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

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

(16)

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

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

(17)

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

4 實(shí)驗(yàn)與分析

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

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

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

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

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

4.2 算法性能對(duì)比

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

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

表3 算法運(yùn)行時(shí)間/s

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

圖9 算法性能對(duì)比實(shí)驗(yàn)1結(jié)果

5 結(jié)論

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

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