一種多關(guān)節(jié)履帶式機(jī)器人自主越障運(yùn)動(dòng)規(guī)劃

朱 磊，樊繼壯，趙 杰，吳曉光，韓震峰

(哈爾濱工業(yè)大學(xué)機(jī)器人技術(shù)與系統(tǒng)國家重點(diǎn)實(shí)驗(yàn)室，150001哈爾濱，rayjew@sohu.com)

機(jī)器人的運(yùn)動(dòng)規(guī)劃就是在一定的工作空間中，求解使得機(jī)器人能夠從起始位姿達(dá)到終止目標(biāo)位姿的中間運(yùn)動(dòng)序列的問題[1].因此，運(yùn)動(dòng)規(guī)劃問題就可以看作是滿足一定優(yōu)化指標(biāo)的，針對(duì)一系列空間位姿的機(jī)器人逆運(yùn)動(dòng)學(xué)的求解問題.目前，較為廣泛采用的求解機(jī)器人逆運(yùn)動(dòng)學(xué)的方法有解析法、代數(shù)法、幾何法、數(shù)值解法等.前三種方法由于涉及到矩陣與反三角函數(shù)運(yùn)算，不適合處理多關(guān)節(jié)的機(jī)器人逆運(yùn)動(dòng)學(xué)問題;數(shù)值方法模型簡單，但是不能求得所有解，而且計(jì)算量隨著關(guān)節(jié)數(shù)目的增加呈指數(shù)增長[2－3];傳統(tǒng)的方法已經(jīng)無法滿足需要.隨著遺傳算法[4]、神經(jīng)網(wǎng)絡(luò)算法[5]、模擬退火算法[6]、蟻群算法[7]、粒子群算法[8]等現(xiàn)代啟發(fā)式數(shù)值優(yōu)化方法的出現(xiàn)，為機(jī)器人的逆運(yùn)動(dòng)學(xué)求解提供了便捷的方式.以上算法都有各自的優(yōu)點(diǎn)，但是當(dāng)自變量數(shù)較多而且之間有耦合作用時(shí)，存在著收斂速度慢，結(jié)果不精確等一些缺點(diǎn).人工魚群算法作為一種新的優(yōu)化算法已經(jīng)得到了越來越多的認(rèn)可，在很多領(lǐng)域中得到了應(yīng)用[9－10].

本文提出了一種改進(jìn)的人工魚群算法，用于求解多關(guān)節(jié)履帶式機(jī)器人的越障運(yùn)動(dòng)學(xué)逆解.首先將要求解的問題轉(zhuǎn)化為幾個(gè)嵌套的子問題分別應(yīng)用人工魚群算法，然后通過一種隨機(jī)搜索的方法縮小自變量的搜索范圍，并且自適應(yīng)地調(diào)整視野和步長，從而可以離線精確地獲得機(jī)器人越障的各個(gè)關(guān)鍵步驟的關(guān)節(jié)角度，滿足目標(biāo)函數(shù)的要求.仿真結(jié)果證明了方法的正確性與可靠性.利用得到的關(guān)節(jié)角度值對(duì)機(jī)器人進(jìn)行越障規(guī)劃實(shí)驗(yàn)，結(jié)果顯示機(jī)器人能夠順利地越過高度為500 mm的垂直障礙.

1 機(jī)器人系統(tǒng)本體結(jié)構(gòu)設(shè)計(jì)

由于履帶式移動(dòng)機(jī)器人可以較好地適應(yīng)復(fù)雜地形，而多關(guān)節(jié)的機(jī)器人具有較大的靈活性，本文根據(jù)機(jī)器人的應(yīng)用環(huán)境和實(shí)際功能，設(shè)計(jì)了一種用于礦難后井下環(huán)境探測(cè)的多關(guān)節(jié)履帶式礦難搜索機(jī)器人.該機(jī)器人采用4個(gè)履帶單元串聯(lián)的多關(guān)節(jié)鉸接的蛇形結(jié)構(gòu)(如圖1所示)，可以在崎嶇地形中自由行走，并且可翻越一定高度的障礙.機(jī)器人的運(yùn)動(dòng)主要由4個(gè)行走單元模塊以及3個(gè)十字軸俯仰偏航關(guān)節(jié)模塊來完成.依據(jù)功能可以分為傳感器單元、控制與其他單元、中間俯仰單元以及節(jié)點(diǎn)單元.俯仰偏航關(guān)節(jié)模塊則通過兩對(duì)錐齒輪分別驅(qū)動(dòng)一個(gè)偏心的十字軸的兩個(gè)垂直軸，可以輕松地實(shí)現(xiàn)機(jī)器人各個(gè)關(guān)節(jié)之間的俯仰與偏航運(yùn)動(dòng).

1 機(jī)器人總體結(jié)構(gòu)示意圖

2 機(jī)器人越障運(yùn)動(dòng)性能分析

由機(jī)器人的自由度分析可知，對(duì)于高度比履帶輪半徑還小的障礙，機(jī)器人可以輕松越過，而不需要對(duì)姿態(tài)做任何改變;對(duì)于不方便直接越過而橫截面積很小且可以通過轉(zhuǎn)彎繞行避開的障礙，機(jī)器人可以通過對(duì)關(guān)節(jié)偏航運(yùn)動(dòng)實(shí)現(xiàn)各個(gè)單元之間的運(yùn)動(dòng)，控制也相對(duì)簡便;但是對(duì)于無法繞過或者很難繞過的較大障礙，機(jī)器人只能通過抬起模塊單元的方式進(jìn)行越障，此方法是最為復(fù)雜的，也是能量消耗最大的方式.因此，本文將重點(diǎn)針對(duì)這種方式的運(yùn)動(dòng)性能進(jìn)行分析.

2.1 越障模型分析

根據(jù)機(jī)器人的本體結(jié)構(gòu)可以建立如圖2所示的機(jī)器人越障模型，關(guān)節(jié)角度的變化范圍及桿件的長度如表1所示，并且均假定每一單元的質(zhì)心位置為其中心位置.

圖2 機(jī)器人翻越垂直障礙模型

表1 機(jī)器人參數(shù)

由文獻(xiàn)[11]可知，只有抬起的最前端的單元的質(zhì)心超過障礙物的高度，機(jī)器人才能平穩(wěn)越過障礙，因此，對(duì)于單單元越障模式機(jī)器人所能越過的高度應(yīng)該滿足(如圖3(1)).

當(dāng)θ4=45°時(shí)，通過式(1)可以得到機(jī)器人所能越過的最大高度H1max=166.59 mm.圖3(1)中最右邊的為傳感器單元(圖3與圖4中的各單元放置順序均與此相同).

圖3 單單元模式越障動(dòng)作規(guī)劃

對(duì)于雙單元越障模式機(jī)器人能夠翻越的高度應(yīng)該滿足(如圖4(2))所示)

當(dāng) θ3=45°，θ4=45°時(shí)，通過式(2)可以得到機(jī)器人所能翻過的最高障礙為H2max=620.05 mm.

圖4 雙單元越障動(dòng)作規(guī)劃

2.2 單單元越障模式

由上面分析可知，對(duì)于高度小于166 mm的垂直障礙可以采用單單元越障模式，如圖3所示.設(shè)機(jī)器人初始狀態(tài)各個(gè)關(guān)節(jié)角度均為0.每一步驟中由于只有一個(gè)角度是自變量，因此很容易得到各個(gè)角度在不同階段與高度之間的關(guān)系.

當(dāng)抬起第1單元越障時(shí)(如圖3(1)所示)，

當(dāng)抬起第2單元時(shí)(如圖3(2)所示)，

當(dāng)抬起第3單元時(shí)(如圖3(3)所示)，

當(dāng)抬起第4單元時(shí)(如圖3(4)所示)，

2.3 雙單元越障模式

當(dāng)要翻越的垂直障礙的高度大于166 mm、小于620 mm時(shí)，機(jī)器人只能采用雙單元越障模式，如圖4所示.設(shè)機(jī)器人初始狀態(tài)各個(gè)關(guān)節(jié)角度均為0.

對(duì)于步驟1與步驟2，機(jī)器人的角度參數(shù)與高度的關(guān)系應(yīng)該滿足

對(duì)于步驟3與步驟4，機(jī)器人的關(guān)節(jié)角度參數(shù)與高度的關(guān)系應(yīng)該滿足

對(duì)于步驟5，機(jī)器人的關(guān)節(jié)角度參數(shù)與高度的關(guān)系應(yīng)該滿足

對(duì)于步驟 6 與步驟7，θ3?= θ4?=0，θ2?=θ2″，θ1?= － θ2″.對(duì)于步驟 8，機(jī)器人的關(guān)節(jié)角度全部都為0.

因此，可以依據(jù)要翻越障礙的高度與優(yōu)化指標(biāo)決定各個(gè)狀態(tài)變化時(shí)刻的關(guān)節(jié)角度值，完成機(jī)器人基于逆運(yùn)動(dòng)學(xué)的自主越障規(guī)劃.

3 基于NAFSA的自主越障運(yùn)動(dòng)規(guī)劃

3.1 機(jī)器人越障運(yùn)動(dòng)規(guī)劃優(yōu)化問題

由上面的分析可知，單單元越障模式每步驟中的關(guān)節(jié)角度可以容易地得到解析解.而雙單元越障模式中每一個(gè)步驟則與多個(gè)關(guān)節(jié)角度有關(guān)，存在多組解，并且關(guān)節(jié)角度相互之間有制約關(guān)系，難于求解，需要針對(duì)雙單元越障模式進(jìn)行合理的運(yùn)動(dòng)學(xué)優(yōu)化.由于機(jī)器人是通過電池提供能量，每一次運(yùn)動(dòng)如果能量消耗最低將會(huì)增加機(jī)器人的運(yùn)動(dòng)時(shí)間，從而擴(kuò)大搜索范圍，對(duì)救援工作提供更多的支持.而翻越垂直障礙時(shí)，機(jī)器人的能量耗費(fèi)與關(guān)節(jié)角度的變化大小有關(guān)系，因此本文將機(jī)器人在越障中各個(gè)步驟間的關(guān)節(jié)角度的變化值作為目標(biāo)函數(shù)進(jìn)行優(yōu)化，即

其中，θi、θi'、θi″分別對(duì)應(yīng)了步驟1 與步驟2 中、步驟3與步驟4中以及步驟5中的機(jī)器人所對(duì)應(yīng)的第i個(gè)關(guān)節(jié)角度.因此可以將越障運(yùn)動(dòng)規(guī)劃理解成為滿足式(3)～(5)并且使得式(6)取得最小值的最優(yōu)關(guān)節(jié)角度值的求解問題.由于式(6)具有多變量、非線性的特點(diǎn)，傳統(tǒng)的解析方法很難獲得最優(yōu)解，提出了一種基于改進(jìn)人工魚群算法的機(jī)器人越障的逆運(yùn)動(dòng)學(xué)求解方法.為了使問題簡化，由式(3)～ (5)可知，θ4、θ4'，θ2″可以由 θ3、θ2'、θ3'、θ3″來表示，從而使得式(6)優(yōu)化自變量數(shù)由7個(gè)減少至4個(gè).

3.2 人工魚群算法簡介

人工魚群算法是2002年李曉磊等[9]提出的一種基于動(dòng)物行為的自治體尋優(yōu)模式.是模擬自然界中魚的覓食、聚群和追尾行為以及魚群之間的相互協(xié)助從而達(dá)到全局尋優(yōu)的目的.但是，人工魚群算法在求解優(yōu)化問題中依然存在著一些問題:1)當(dāng)搜素域較大或處于變化平坦的區(qū)域時(shí)，搜索性能變差，收斂速度變慢;2)算法一般在初期收斂較快，后期卻往往收斂較慢;3)AFSA具有把握搜索方向和在一定程度上逃出局部最優(yōu)的特性，但當(dāng)很多人工魚處于漫無目的地隨機(jī)移動(dòng)或人工魚群在非全局極值點(diǎn)出現(xiàn)較嚴(yán)重聚集情況時(shí)，大大降低了收斂速度，使得精度也大大降低[10].

3.3 基于改進(jìn)的人工魚群算法的越障逆運(yùn)動(dòng)學(xué)求解

3.3.1 多層分組優(yōu)化思想

考慮到本文目標(biāo)函數(shù)的自變量有4個(gè)，如果同時(shí)對(duì)其進(jìn)行優(yōu)化，運(yùn)算的復(fù)雜程度將會(huì)很高，而且自變量之間互相影響很難收斂到理想的結(jié)果.由于θ3與θ3″對(duì)于求解目標(biāo)函數(shù)的最優(yōu)值是互不相關(guān)的，所以利用分組的思想將目標(biāo)函數(shù)分解為兩個(gè)子函數(shù)，即

這樣可以將問題改進(jìn)成為了一個(gè)三層四組單自變量的嵌套人工魚群算法，從而將各個(gè)參數(shù)之間的耦合作用降到最低，提高了收斂速度.具體的分層原則如下.

第1層人工魚群(最外層):求出使得目標(biāo)函數(shù)F最小的自變量θ2'.

第2層人工魚群(中間層):求出在θ2'一定的情況下使得目標(biāo)函數(shù)F取得最小值的自變量θ3'.

第3層人工魚群(最內(nèi)層):求出在θ2'、θ3'一定的情況下使得目標(biāo)函數(shù)f1與f2分別取得最小值的自變量 θ3與 θ3″.

通過這三層人工魚群使得對(duì)于自變量θ2'的任意值，可以找到唯一的一組 θ3、θ3'與 θ3″使得F最小，因此只需要對(duì)自變量θ2'進(jìn)行魚群行為來尋優(yōu)即可，使得魚群行為方向明確，算法效率提高，有利于得到最優(yōu)解.

3.3.2 自適應(yīng)地調(diào)節(jié)視野與步長

視野的大小決定了收斂的速度，而步長的大小決定了求解的精度.可以動(dòng)態(tài)調(diào)整人工魚群的視野和步長.在算法運(yùn)行前期，采用較大的視野和步長，可以增強(qiáng)算法的全局搜索能力和收斂速度，使人工魚在更大的范圍內(nèi)進(jìn)行粗搜索;隨著搜索的進(jìn)行，視野V和步長S逐步減小;在算法運(yùn)行后期，算法逐步演化為局部搜索，定位在最優(yōu)解附近區(qū)域并進(jìn)行精細(xì)搜索，從而提高了算法的局部搜索能力和尋優(yōu)結(jié)果的精度.視野V和步長S可按式(7)動(dòng)態(tài)調(diào)整[12]:

其中，s為大于1 的整數(shù)，一般取s=5、20、30，Vmin為最小視野，Smin為最小步長，t為迭代次數(shù)，Tmax為最大迭代次數(shù).

3.3.3 縮小搜索域

對(duì)于優(yōu)化算法來說，搜索域范圍越大其搜索的速度就會(huì)越慢，從而導(dǎo)致收斂速度變慢.而且對(duì)于很多有約束條件的優(yōu)化問題，其初始的搜索域往往存在很多不滿足條件的解，從而浪費(fèi)了大量的運(yùn)行時(shí)間.本文采用一種變步長快速隨機(jī)搜索的方法來縮小自變量的搜索域，進(jìn)而提高收斂速度.由于搜索自變量上邊界與下邊界方法一樣，所以以θ3下邊界的搜索為例說明.

Step 1 設(shè) θ3的下邊界為 xl，初始步長為S=2，精度為θjd=0.01°，迭代次數(shù)為K=4 000.首先令 θ3=xl，求出 θ4，判斷 θ3與 θ4是否滿足約束條件，如果滿足則直接輸出xl，結(jié)束搜索;令k=1開始迭代.

Step 2 判斷θ3=x1(k)時(shí)，θ3與θ4是否滿足約束條件:

如果不滿足約束條件，

如果滿足約束條件，則令

其中fix為取整函數(shù).

Step 3 對(duì)于k＞2判斷

是否成立，如果成立結(jié)束迭代，取max((x1(k)，x1(k－1))作為結(jié)果輸出，跳出迭代.

Step 4 依據(jù)公式(7)動(dòng)態(tài)調(diào)整S，取S=2，Smin=0.01°.

Step 5 如果k＞K成立，結(jié)束迭代，說明變量的下界不存在;如果不成立k=k+1轉(zhuǎn)Step 2.

求上界與求下界類似，但要把Step 2中的加號(hào)換成減號(hào)，把減號(hào)換成加號(hào)，在Step 3中取最小值即可.

3.3.4 公告板的使用

公告板是用來保存人工魚群中的最優(yōu)人工魚群個(gè)體的.各人工魚個(gè)體在尋優(yōu)過程中，每次行動(dòng)完畢就檢驗(yàn)自身狀態(tài)與公告板的狀態(tài)，如果自身的狀態(tài)優(yōu)于公告板狀態(tài)，就將公告板的狀態(tài)改寫為自身狀態(tài)，這樣就使公告板記錄下了歷史最優(yōu)的狀態(tài).采用末位淘汰的方法，用公告板中的最優(yōu)的狀態(tài)替換掉每次迭代后的最差的狀態(tài).

綜上所述，每層的人工魚群算法的目標(biāo)函數(shù)都是相似的，只是自變量有所不同，因此具體的步驟都是相同的.現(xiàn)在以基于自變量θ3的人工魚群算法的步驟為例來說明.

Step 1 根據(jù)變步長快速隨機(jī)搜索算法確定θ3的取值范圍，并且對(duì)人工魚群的參數(shù)初始化，生成魚群 θ31，θ32，…，θ3N1，令 i=1.

Step 2 對(duì)魚群中個(gè)體θ3i分別進(jìn)行覓食、聚群與追尾操作.

Step 3 判斷i＞N1+1是否成立，如果不成立，i=i+1轉(zhuǎn)Step 2;如果成立，判斷θ3i是否滿足迭代收斂條件，不滿足，令i=1，并且記錄最優(yōu)個(gè)體更新公告板，轉(zhuǎn)Step 2;如果成立輸出θ3，對(duì)θ3尋優(yōu)結(jié)束.

設(shè)帶*的為每一層人工魚群求出的最優(yōu)解，則整個(gè)算法的主要流程如圖5所示.

圖5 改進(jìn)的嵌套魚群算法求解過程

4 仿真結(jié)果及分析

為了驗(yàn)證本文所用方法的正確性以及優(yōu)良的收斂性能，針對(duì)相同的高度值H=500 mm，在matlab中分別對(duì)傳統(tǒng)人工魚群方法與改進(jìn)的嵌套魚群方法進(jìn)行了仿真驗(yàn)證.設(shè)置傳統(tǒng)人工魚群算法的群體規(guī)模N=50，迭代次數(shù)K=500.對(duì)改進(jìn)的人工魚群算法的各個(gè)參數(shù)設(shè)置如下，第1層人工魚的群體規(guī)模N=20，迭代次數(shù)K=100，第2層人工魚的群體規(guī)模N=10，迭代次數(shù)K=100，第3層兩組人工魚的群體規(guī)模N=10，迭代次數(shù)K=50.兩種人工魚群的公共相同參數(shù)分別為擁擠度因子δ=0.618，人工魚群的最大試探次數(shù)ntry=3，初始視野V0=5°，Vmin=0.04°，初始步長S0=2°，Smin=0.01°，s=30.兩種算法的機(jī)器人的優(yōu)化的目標(biāo)函數(shù)值與進(jìn)化代數(shù)的關(guān)系分別如圖6(1)和6(2)所示.可見傳統(tǒng)人工魚群算法需要280代才能收斂到最優(yōu)解，并且不能保證收斂到全局最優(yōu)解，而本算法可以在31代內(nèi)很快地收斂到全局最優(yōu)解.算法的效率有了顯著的提高.

圖6 目標(biāo)函數(shù)值變化過程比較

為了驗(yàn)證算法的穩(wěn)定性，利用本文所提的算法對(duì)機(jī)器人要越過的3種高度(500、525、550 mm)分別進(jìn)行了10次優(yōu)化計(jì)算，所得的結(jié)果如表2所示.由表2可以看出，算法每次都能收斂到對(duì)應(yīng)高度的全局最優(yōu)解，從而證明了算法的魯棒性.

表2 針對(duì)3種高度算法連續(xù)運(yùn)行10次的結(jié)果比較

5 實(shí)驗(yàn)驗(yàn)證

為了證明本文方法的正確性，在高度為500 mm的垂直障礙上依據(jù)算法得到的關(guān)節(jié)角度進(jìn)行了越障實(shí)驗(yàn)，如圖7所示.結(jié)果表明本文的規(guī)劃方法能夠?qū)崿F(xiàn)機(jī)器人的越障，計(jì)算得到的結(jié)果是合理的.

圖7 雙單元模式越障實(shí)驗(yàn)

6 結(jié)束語

1)依據(jù)多關(guān)節(jié)履帶式礦難搜索移動(dòng)機(jī)器人的本體結(jié)構(gòu)，分別建立了機(jī)器人的單單元與雙單元的越障模型，并對(duì)其越障能力進(jìn)行了分析.

2)設(shè)計(jì)了一種基于三層四組嵌套人工魚群算法的機(jī)器人越障逆運(yùn)動(dòng)學(xué)優(yōu)化算法.該算法只需要完成多次單變量尋優(yōu)，避免了多變量之間的相互干擾，并且在自變量進(jìn)行初始化前通過一種變步長的隨機(jī)搜索方法將變量的搜素域縮小，提高了計(jì)算效率.

3)仿真分析表明，這種方法的收斂代數(shù)很少，精度也很高，并且可以保證每次都能收斂到最優(yōu)解.

4)機(jī)器人真實(shí)越障實(shí)驗(yàn)證明，算法規(guī)劃結(jié)果正確，可以用于解決這一類機(jī)器人基于逆運(yùn)動(dòng)學(xué)的越障規(guī)劃.

[1]宣俊.高維空間機(jī)器人運(yùn)動(dòng)規(guī)劃算法研究[D].杭州:浙江大學(xué)，2005:1－4.

[2]榮盤祥，楊晶，胡林果，等.基于RBF網(wǎng)絡(luò)的SCARA機(jī)器人的運(yùn)動(dòng)學(xué)逆解[J].電機(jī)與控制學(xué)報(bào)，2007，11(3):303－305.

[3]周友行，何清華，鄧伯祿.一種改進(jìn)的爬山法優(yōu)化求解冗余機(jī)械手運(yùn)動(dòng)學(xué)逆解[J].機(jī)器人，2003，25(1):35－38.

[4]ALBERT F Y C，KOH S P，TIONG S K，et al.Inverse kinematic solution in handling 3R manipulator via realtime genetic algorithm [C]//Proceedings of the 2008 IEEE International Symposium on Information Technology.Kuala Lumpur:IEEE，2008:1－6.

[5]ZHOU You Hang，TANG Wen Zhuang，ZHANG Jian Xun.Arithmetic for multi-joint redundant robot inverse kinematics based on the bayesian-BP neural network[C]//Proceedings of the 2008 IEEE International Conference on Intelligent Computation Technology and Automation Mechatronics and Automation.Changsha:IEEE，2008:173－178.

[6]何廣忠，高洪明，張廣軍，等.弧焊機(jī)器人放置規(guī)劃問題的研究[J].機(jī)器人，2006，28(3):249－254.

[7]梅紅，王勇，趙榮齊.基于蟻群神經(jīng)網(wǎng)絡(luò)算法的機(jī)器人逆解[J].山東大學(xué)學(xué)報(bào):工學(xué)版，2008，38(5):72－76.

[8]芮挺，朱經(jīng)緯，周游，等.多自由度機(jī)器人逆運(yùn)動(dòng)粒子群優(yōu)化求解方法[J].系統(tǒng)仿真學(xué)報(bào)，2009，21(10):2930－2932.

[9]李曉磊，邵之江，錢積新.一種基于動(dòng)物自治體的尋優(yōu)模式:魚群算法[J].系統(tǒng)工程理論與實(shí)踐，2002，22(11):32－38.

[10]張梅鳳，邵誠，甘勇，等.基于變異算子與模擬退火混合的人工魚群優(yōu)化算法[J].電子學(xué)報(bào)，2006，30(8):1381－1385.

[11]TAKAYAMA T，HIROSE S.Development of"Souryu I＆ II"[J].Joumal of Robotics and Mcchahonics，2003，15(1):61－69.

[12]王聯(lián)國，洪毅，趙付青，等.一種簡化的人工魚群算法[J].小型微型計(jì)算機(jī)系統(tǒng)，2009，30(8):1663－1667.