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

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

0 引言

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

圖1 超冗余機(jī)器人靈巧運(yùn)動(dòng)

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

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

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

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

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

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

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

1 等效桿件法逆運(yùn)動(dòng)學(xué)及避障規(guī)劃

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

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

圖8 等效桿件法示意圖

(1)

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

L=2l0cosβ。

(2)

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

(3)

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

(4)

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

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

(5)

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

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

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

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

2 基于Webots的仿真驗(yàn)證

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

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

圖11 仿真平臺(tái)搭建流程

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

圖13 末端圓運(yùn)動(dòng)軌跡

2.2 避障運(yùn)動(dòng)仿真

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

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

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

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

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

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

3 結(jié)論

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