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

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

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

機器人的運動規(guī)劃就是在一定的工作空間中，求解使得機器人能夠從起始位姿達到終止目標位姿的中間運動序列的問題[1].因此，運動規(guī)劃問題就可以看作是滿足一定優(yōu)化指標的，針對一系列空間位姿的機器人逆運動學的求解問題.目前，較為廣泛采用的求解機器人逆運動學的方法有解析法、代數(shù)法、幾何法、數(shù)值解法等.前三種方法由于涉及到矩陣與反三角函數(shù)運算，不適合處理多關(guān)節(jié)的機器人逆運動學問題;數(shù)值方法模型簡單，但是不能求得所有解，而且計算量隨著關(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)，為機器人的逆運動學求解提供了便捷的方式.以上算法都有各自的優(yōu)點，但是當自變量數(shù)較多而且之間有耦合作用時，存在著收斂速度慢，結(jié)果不精確等一些缺點.人工魚群算法作為一種新的優(yōu)化算法已經(jīng)得到了越來越多的認可，在很多領(lǐng)域中得到了應(yīng)用[9－10].

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

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

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

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

2 機器人越障運動性能分析

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

2.1 越障模型分析

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

圖2 機器人翻越垂直障礙模型

表1 機器人參數(shù)

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

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

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

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

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

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

2.2 單單元越障模式

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

當抬起第1單元越障時(如圖3(1)所示)，

當抬起第2單元時(如圖3(2)所示)，

當抬起第3單元時(如圖3(3)所示)，

當抬起第4單元時(如圖3(4)所示)，

2.3 雙單元越障模式

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

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

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

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

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

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

3 基于NAFSA的自主越障運動規(guī)劃

3.1 機器人越障運動規(guī)劃優(yōu)化問題

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

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

3.2 人工魚群算法簡介

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

3.3 基于改進的人工魚群算法的越障逆運動學求解

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

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

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

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

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

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

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

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

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

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

3.3.3 縮小搜索域

對于優(yōu)化算法來說，搜索域范圍越大其搜索的速度就會越慢，從而導致收斂速度變慢.而且對于很多有約束條件的優(yōu)化問題，其初始的搜索域往往存在很多不滿足條件的解，從而浪費了大量的運行時間.本文采用一種變步長快速隨機搜索的方法來縮小自變量的搜索域，進而提高收斂速度.由于搜索自變量上邊界與下邊界方法一樣，所以以θ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)時，θ3與θ4是否滿足約束條件:

如果不滿足約束條件，

如果滿足約束條件，則令

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

Step 3 對于k＞2判斷

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

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

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

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

3.3.4 公告板的使用

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

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

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

Step 2 對魚群中個體θ3i分別進行覓食、聚群與追尾操作.

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

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

圖5 改進的嵌套魚群算法求解過程

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

為了驗證本文所用方法的正確性以及優(yōu)良的收斂性能，針對相同的高度值H=500 mm，在matlab中分別對傳統(tǒng)人工魚群方法與改進的嵌套魚群方法進行了仿真驗證.設(shè)置傳統(tǒng)人工魚群算法的群體規(guī)模N=50，迭代次數(shù)K=500.對改進的人工魚群算法的各個參數(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.兩種算法的機器人的優(yōu)化的目標函數(shù)值與進化代數(shù)的關(guān)系分別如圖6(1)和6(2)所示.可見傳統(tǒng)人工魚群算法需要280代才能收斂到最優(yōu)解，并且不能保證收斂到全局最優(yōu)解，而本算法可以在31代內(nèi)很快地收斂到全局最優(yōu)解.算法的效率有了顯著的提高.

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

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

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

5 實驗驗證

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

圖7 雙單元模式越障實驗

6 結(jié)束語

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

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

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

4)機器人真實越障實驗證明，算法規(guī)劃結(jié)果正確，可以用于解決這一類機器人基于逆運動學的越障規(guī)劃.

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

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

[3]周友行，何清華，鄧伯祿.一種改進的爬山法優(yōu)化求解冗余機械手運動學逆解[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]何廣忠，高洪明，張廣軍，等.弧焊機器人放置規(guī)劃問題的研究[J].機器人，2006，28(3):249－254.

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

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

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

[10]張梅鳳，邵誠，甘勇，等.基于變異算子與模擬退火混合的人工魚群優(yōu)化算法[J].電子學報，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].小型微型計算機系統(tǒng)，2009，30(8):1663－1667.