基于等效桿件法的超冗余機(jī)器人規(guī)劃仿真研究

王偉霖，王成疆，賈利慧 ，張魯楊 ，劉力源，李玉勝，牟宗高,3

(1.山東理工大學(xué) 機(jī)械工程學(xué)院，山東 淄博 255000；2.深圳市先進(jìn)智能技術(shù)研究所，廣東 深圳 518055；3.哈爾濱工業(yè)大學(xué)(深圳) 機(jī)電工程與自動化學(xué)院，廣東 深圳 518055)

0 引言

相比于傳統(tǒng)的工業(yè)機(jī)器人，超冗余機(jī)器人具有更多的自由度、更纖細(xì)的工作臂桿和更靈巧的運動方式，因此超冗余機(jī)器人在狹小的工作環(huán)境表現(xiàn)出更優(yōu)越的適應(yīng)性和相容性。近年來，超冗余機(jī)器人得到了國內(nèi)外專家學(xué)者的深入研究，并被廣泛應(yīng)用于狹小空間的作業(yè)任務(wù)[1]，例如：大型航天器設(shè)備的檢測維修、核電站廢墟環(huán)境的探生救援及狹小腔道環(huán)境的微創(chuàng)手術(shù)等。東京工業(yè)大學(xué)研制了一種包含9自由度的超冗余機(jī)器人[2]，該超冗余機(jī)器人不僅可以實現(xiàn)繞基座的轉(zhuǎn)動及在空間的運動，而且可以發(fā)揮其擁有較多自由度的優(yōu)勢，靈活的避開工作范圍內(nèi)的多障礙物到達(dá)指定的空間位置，如圖1所示。美國國防部高級研究計劃局提出的“鳳凰計劃”[3]中提出了開發(fā)下一代高靈巧性機(jī)器人的發(fā)展方向。該類超冗余機(jī)器人將具備照明和攝像等功能同時也將具有較大的工作空間以及障礙物回避等能力，如圖2所示。日本國家信息和通信技術(shù)研究中心提出了軌道維護(hù)系統(tǒng)的概念[4]，該系統(tǒng)將完成對通信衛(wèi)星的檢修以及空間碎片的清理等工作，如圖3所示。OC Robotics 公司推出了一種典型的超冗余機(jī)器人-型號II-X125[5]。該機(jī)器人臂桿直徑為125 mm，可在核電站等危險的狹小空間代替人類開展切割/焊接等作業(yè)任務(wù)。哈爾濱工業(yè)大學(xué)設(shè)計了一套應(yīng)用于核電站狹小空間探測的超冗余機(jī)器人[6, 7]。該機(jī)器人有18個自由度可以靈活的進(jìn)入狹小空間執(zhí)行視頻檢測任務(wù)。香港中文大學(xué)設(shè)計了一款應(yīng)用于微創(chuàng)手術(shù)的繩索驅(qū)動機(jī)器人[8, 9]。該機(jī)器人具有緊湊的結(jié)構(gòu)，相對于連續(xù)型機(jī)器人具有更好的定位精度，實驗中末端定位誤差小于4%。

圖1 超冗余機(jī)器人靈巧運動

圖2 超冗余機(jī)器人在軌照明

圖3 超冗余機(jī)器人在軌維護(hù)

圖4 超冗余機(jī)器人切割/焊接作業(yè)

圖5 超冗余機(jī)器人視覺檢測作業(yè)

圖6 超冗余機(jī)器人微創(chuàng)手術(shù)應(yīng)用

然而，由于大量自由度的存在，超冗余機(jī)器人的運動學(xué)逆解及軌跡規(guī)劃將十分復(fù)雜，因此運動學(xué)逆解及軌跡規(guī)劃也成為超冗余機(jī)器人研究中的熱點[10-13]。Chirikjian 首先提出了脊線表征超冗余機(jī)器人宏觀構(gòu)型的思想[14]，當(dāng)超冗余機(jī)器人宏觀構(gòu)型確定后，就可以依次確定每個關(guān)節(jié)的角度。Sunil 提出了使用圓弧表征超冗余機(jī)器人構(gòu)型的思路[15]。Samer 提出了一種從超冗余機(jī)器人無窮多解中確定唯一解的幾何法[16]，由于該方法設(shè)定相鄰關(guān)節(jié)角度相等，因此可以有效避免內(nèi)部奇異。為了實現(xiàn)超冗余機(jī)器人的避障規(guī)劃，F(xiàn)reund提出了一種在線避障規(guī)劃方法[17]，該方法使用了二次凸優(yōu)化理論實現(xiàn)超冗余機(jī)器人的避障優(yōu)化。Mayorga 提出了實現(xiàn)超冗余機(jī)器人避障的幾何包圍法[18]，該方法通過計算零空間及與障礙物包絡(luò)的距離簡單有效的實現(xiàn)了避障規(guī)劃目標(biāo)。Mu等提出了使用超二次曲面方程建立障礙物模型，為冗余機(jī)器人避障規(guī)劃提供判據(jù)的思路[19, 20]，該方法實現(xiàn)了冗余機(jī)器人的在三維環(huán)境下多障礙物的避障規(guī)劃。Xu提出了應(yīng)用等效模式函數(shù)法求解超冗余機(jī)器人逆運動學(xué)的思路[21]，該方法在避障方面具有較高的靈巧度，但在求解逆運動學(xué)時具有較高的復(fù)雜度。Mu提出了分段幾何法求解超冗余機(jī)器人的逆運動學(xué)的思路[6]，該方法提高了逆運動學(xué)的求解效率，但在避障規(guī)劃中機(jī)器人的肘部只有四種構(gòu)型可供選擇，降低了超冗余機(jī)器人的靈巧性。

目前，在避障規(guī)劃研究中為了獲得障礙物回避的高效性，在障礙物建模方面通常采用圓或球等簡單幾何模型包絡(luò)障礙物。而在避障算法優(yōu)化方面還需要進(jìn)一步研究超冗余機(jī)器人的逆運動學(xué)求解及軌跡規(guī)劃問題。針對以上問題，本文提出了應(yīng)用等效桿件法求解超冗余機(jī)器人的逆運動學(xué)及避障軌跡規(guī)劃問題的思路。該方法根據(jù)超冗余機(jī)器人避障規(guī)劃的特點將超冗余的機(jī)器人逐次等效為低冗余或非冗余的機(jī)器人，從而降低了避障規(guī)劃的求解復(fù)雜度，提高了避障規(guī)劃效率。本文首先介紹了等效桿件法的基本原理，并以二連桿等效桿件法為例介紹超冗余機(jī)器人的避障規(guī)劃方法。然后基于Webots建立超冗余機(jī)器人的仿真系統(tǒng)驗，證二連桿等效桿件法避障規(guī)劃算法。最后總結(jié)全文并給出結(jié)論。

1 等效桿件法逆運動學(xué)及避障規(guī)劃

一般情況下，障礙物都具有不規(guī)則的幾何形狀。本文針對平面障礙物，采用圓包絡(luò)的方法進(jìn)行描述。這種方法不僅建模簡單而且可以適度擴(kuò)大障礙物的空間，從而提高避障規(guī)劃的計算效率及機(jī)器人運動路徑的安全度，如圖7所示。為了降低設(shè)計復(fù)雜程度，采用模塊化思想設(shè)計超冗余機(jī)器人，使每個桿件的長度都是一致的。在傳統(tǒng)幾何法[16]中相鄰桿件之間的夾角均設(shè)定為相同，能有效避免相鄰連桿件共線的問題，因而體現(xiàn)出良好的避免內(nèi)部奇異性?；诔哂鄼C(jī)器人的特點，本文提出了等效桿件法逆運動學(xué)及避障規(guī)劃策略。在超冗余機(jī)器人的逆運動學(xué)求解方面可以將等效桿件法劃分為二連桿等效桿件法和三連桿等效桿件法，如圖8所示。二連桿等效桿件法(如圖 8虛線所示，n為超冗余機(jī)器人自由度，本文設(shè)定n=6)是以兩個連桿為等效單位，將n自由度的機(jī)器人劃分為(n/2)個等效桿件。同理，三連桿等效桿件法(如圖8雙點線所示)是以三個連桿為等效單位，將n自由度的機(jī)器人劃分為(n/3)個等效桿件。在逆運動學(xué)求解時，基于等效桿件法可以將超冗余的機(jī)器人逐次等效為低冗余或非冗余的機(jī)器人，從而降低超冗余機(jī)器人的逆運動學(xué)的難度。在超冗余機(jī)器人的避障規(guī)劃方面則將基于等效桿件法等效后的低冗余或非冗余機(jī)器人與障礙物的笛卡爾距離作為避障判據(jù)，通過判斷等效后的低冗余或非冗余機(jī)器人與障礙物的距離大小就可以判斷原超冗余機(jī)器人與障礙物的位置關(guān)系。因此，等效桿件法可以降低避障規(guī)劃的求解復(fù)雜度、提高了避障規(guī)劃的效率。

圖7 機(jī)器人及障礙物模型

圖8 等效桿件法示意圖

(1)

其中L為等效桿件的桿長，其值通過(2)式計算

L=2l0cosβ。

(2)

顯然，當(dāng)給定機(jī)器人與等效桿件之間的夾角β，依據(jù)分式(1)則可得出一組唯一確定的α和θ的解，從而得出了等效桿件組的關(guān)節(jié)角度。進(jìn)一步，可以依據(jù)公式(3)求得超冗余機(jī)器人各個關(guān)節(jié)的角度。

(3)

對于超冗余機(jī)器人在障礙物環(huán)境中工作，還需要研究的一個重要指標(biāo)是其工作空間。工作空間由超冗余機(jī)器人的自由度數(shù)目、關(guān)節(jié)角度極限和桿件長度共同決定。通常應(yīng)用蒙特卡羅法繪制機(jī)器人的工作空間。根據(jù)公式(1)可知，末端執(zhí)行器的位置是關(guān)節(jié)角度的函數(shù)，可以簡寫為

(4)

其中關(guān)節(jié)角度滿足θmin≤θi≤θmax,(i=1,2,…,6,θmin=-85,θmax=85)。對每個關(guān)節(jié)變量，在其規(guī)定的范圍內(nèi)，隨機(jī)抽取500個值可得具體參數(shù)

θi=θmin+(θmax-θmin)×RAND(500,1),

(5)

則應(yīng)用蒙特卡羅法，對所有角度組合，采用循環(huán)(多重循環(huán))語句依次將各關(guān)節(jié)變量值組合代入運動學(xué)正解方程，繪制工作空間如圖9所示。

圖9 超冗余機(jī)器人工作空間

在工作空間范圍內(nèi)，則可以確定超冗余機(jī)器人每個桿件的位置。并通過判斷桿件所在線段與障礙物包絡(luò)圓交點是否存在，作為確定桿件是否會與障礙物發(fā)生碰撞的判據(jù)。當(dāng)會發(fā)生碰撞時，則改變機(jī)器人與虛擬桿件的夾角β，從而改變機(jī)器人的姿態(tài)來實現(xiàn)避障，其具體算法流程如圖10所示。

圖10 障礙物避障算法流程圖

2 基于Webots的仿真驗證

2.1 仿真系統(tǒng)架構(gòu)

Webots可以實現(xiàn)對機(jī)器人和環(huán)境進(jìn)行建模，并能夠進(jìn)行編程、控制和仿真。在Webots軟件中，用戶可以快速創(chuàng)建三維虛擬環(huán)境，并賦予環(huán)境及環(huán)境中物體相應(yīng)的物理屬性，如光照強度、顏色、形狀、質(zhì)量、摩擦系數(shù)等，從而實現(xiàn)十分接近地模擬機(jī)器人現(xiàn)實的工作環(huán)境。Webots軟件界面包含4個主要的窗口：場景樹、三維窗口、代碼區(qū)和控制臺。在這些窗口中分別可以實現(xiàn)建模、顯示、編程和仿真等功能。圖11為仿真平臺搭建的基本流程。本文采用MATLAB語言編寫控制程序。將規(guī)劃好的關(guān)節(jié)角度數(shù)據(jù)儲存于txt文檔中，在運行程序時通過textread函數(shù)讀取關(guān)節(jié)角度數(shù)據(jù)；另外通過wb_robot_get_device函數(shù)獲取設(shè)備標(biāo)簽，并通過Webots中名稱為wb_motor_set_position的函數(shù)設(shè)置電機(jī)對應(yīng)的關(guān)節(jié)角度，在每設(shè)置完一組電機(jī)關(guān)節(jié)角度之后，采用wb_robot_step函數(shù)實現(xiàn)控制臺和仿真窗口之間數(shù)據(jù)的更新，搭建完成的Webots仿真模型如圖12所示。為了驗證模型的適用性，首先規(guī)劃出超冗余機(jī)器人末端運動的圓軌跡及對應(yīng)關(guān)節(jié)角度，然后使用關(guān)節(jié)角度控制超冗余機(jī)器人運動，通過Webots系統(tǒng)傳感器獲得超冗余機(jī)器人末端的位置數(shù)據(jù)。分別繪制期望末端位置和實際末端位置如圖13所示。圖中虛線為規(guī)劃的末端軌跡圓，實線為傳感器測得的末端軌跡圓。實線與虛線具有較好的重合度，說明了超冗余機(jī)器人Webots仿真系統(tǒng)的適用性。

圖11 仿真平臺搭建流程

圖12 超冗余機(jī)器人Webots仿真模型

圖13 末端圓運動軌跡

2.2 避障運動仿真

本文基于建立的超冗余機(jī)器人Webots仿真模型驗證提出的等效桿件法避障規(guī)劃方法。當(dāng)存在障礙物時，需要提前判斷超冗余機(jī)器人運動的下一個構(gòu)型是否會與障礙物發(fā)生碰撞。如果判斷出超冗余機(jī)器人的下一個構(gòu)型有碰撞則需要重新選擇一組滿足要求的構(gòu)型角度。本文基于提出的等效桿件法避障算法規(guī)劃合理的避障軌跡，在Webots仿真系統(tǒng)中驗證所規(guī)劃路徑的有效性。

在單障礙物避障規(guī)劃仿真中，設(shè)定超冗余機(jī)器人與障礙物之間的相對位置關(guān)系如圖14所示。超冗余機(jī)器人在靈活地避開單障礙物的同時沿著規(guī)劃的末端軌跡到達(dá)了期望位置。

圖14 單障礙物避障規(guī)劃仿真

在多障礙物避障規(guī)劃仿真中增加障礙物的數(shù)量同時改變障礙物與超冗余機(jī)器人的相對位置，具體末端的運動軌跡和多障礙物的分布如圖15所示，從圖16的運動軌跡可看出機(jī)器人末端按照規(guī)劃的“直線—圓弧—直線”的路徑進(jìn)行運動，并且軌跡在虛線圓外部。超冗余機(jī)器人的末端及臂桿不與障礙物發(fā)生碰撞，證明了所提出的等效桿件法對超冗余機(jī)器人回避工作空間內(nèi)障礙物的有效性。

圖15 多障礙物位置及末端軌跡

圖16 多障礙物避障規(guī)劃仿真

3 結(jié)論

本文提出的等效桿件法解決了超冗余機(jī)器人的逆運動學(xué)及避障軌跡規(guī)劃問題。該方法的主要思想是將超冗余的機(jī)器人等效為低冗余或非冗余的機(jī)器人；然后，根據(jù)在工作空間內(nèi)障礙物的特點將不規(guī)則的障礙物使用圓進(jìn)行包絡(luò)，通過判斷包絡(luò)圓與等效機(jī)器人的相交點個數(shù)判斷是否發(fā)生碰撞，根據(jù)碰撞的判斷結(jié)果調(diào)整超冗余機(jī)器人的運動構(gòu)型，從而降低了避障規(guī)劃的求解復(fù)雜度并且提高了避障規(guī)劃的效率。為了驗證方法的有效性，本文基于Webots實現(xiàn)了對超冗余機(jī)器人和環(huán)境的建模，基于等效桿件法完成了超冗余機(jī)器人回避多障礙物的編程控制和系統(tǒng)仿真。仿真結(jié)果證明了所提出的等效桿件法對超冗余機(jī)器人回避工作空間中障礙物的有效性。