江亦涵 王 挺 李亞偉 邵士亮 王 寧 畢健康 姚 辰

(*中國科學院沈陽自動化研究所機器人學國家重點實驗室 沈陽 110016)

(**沈陽理工大學自動化與電氣工程學院 沈陽 110159)

(***中國科學院大學 北京 100049)

(****沈陽工業(yè)大學人工智能學院 沈陽 110870)

(*****沈陽化工大學信息工程學院 沈陽 110142)

0 引言

近年來,雙臂機器人由于其協(xié)調(diào)性強、可操作性強、工作效率高等特點,逐漸在制造業(yè)和服務業(yè)等領域取得應用[1-3]。雙臂機器人可以完成多種復雜的任務,例如協(xié)同搬運和協(xié)調(diào)操縱。但控制器設計的難度也遠大于單臂機器人,因為增加的手臂不僅導致復雜的結(jié)構(gòu)和機構(gòu),而且增加了強耦合非線性以及被抓取物體產(chǎn)生的內(nèi)力。為了解決這些問題,雙臂機器人的末端位置控制得到了廣泛的研究[4-10]。Liang 等人[4]通過計算雙四元數(shù)對雙臂機器人進行運動控制。Phukan 等人[5]設計了一個滑?？刂破饕詤f(xié)調(diào)雙機械臂位置。Liu 等人[6]通過對機械臂進行體素化分割,計算出了空間中每一點相對于機械臂的可達性。Kivel 等人[7]為機械臂建立了坐標軸平行(axis-aligned bounding box,AABB)包圍盒,并采用了分離軸定理法進行碰撞檢測。Kim 等人[8]采用了一種基于調(diào)和函數(shù)的新型人工勢函數(shù)來構(gòu)建人工勢場并進行軌跡規(guī)劃。Palmieri 等人[10]提出了一個冗余機械手的運動規(guī)劃和控制框架,采用了離線路徑規(guī)劃算法與在線運動控制組合的控制策略。文獻[11]與文獻[12]分別采用了快速搜索隨機樹(rapid-exploration random tree,RRT)及A*算法探索未知環(huán)境,但以上方法均只適用于機械臂在已知環(huán)境信息的情況下規(guī)避靜態(tài)障礙物。

根據(jù)機器人在生產(chǎn)和生活中的應用可知,障礙物包括靜態(tài)障礙物和動態(tài)障礙物2 種。對于大多數(shù)雙臂機器人來說,兩臂的工作區(qū)域存在重疊部分,因此當一個機械臂移動時,該機械臂會被另一個機械臂視為動態(tài)障礙物。因此,任何一個機器人機械手都應該避開動態(tài)障礙物以完成路徑規(guī)劃。李龍等人[13]提出了一種基于接近覺的機械臂避障路徑規(guī)劃方法,通過讀取連桿運動方向上的距離傳感器數(shù)據(jù)作為碰撞檢測的依據(jù),并采用基于勢函數(shù)與關節(jié)空間的人工勢場法解決避障路徑規(guī)劃問題。但該方法需要在機械臂上安裝額外的傳感器,增加了機械臂的成本。劉建春等人[14]采用球體和膠囊體包圍盒簡化機械臂模型,根據(jù)包圍盒之間的相交情況判斷是否發(fā)生碰撞。通過二次投影法,將包圍盒投影為線段實現(xiàn)快速檢測。但二次投影法對包圍盒角度要求較高,存在失效風險。

本文針對仿人雙機械臂協(xié)同運動規(guī)劃中防碰撞的問題,基于實驗室自行研發(fā)的雙七自由度臂機器人,通過理論計算、仿真實驗,提出了一種適用于冗余自由度機器人協(xié)同工作的控制算法。建立了機器人正逆運動學模型與包圍盒模型,采用了基于單純形退化的改進吉爾伯特-約翰遜-基爾特(Gilbert-Johnson-Keerth,GJK)算法設計了基于安全工作范圍的改良五次多項式軌跡插補法。對此算法分別進行了Matlab 仿真、插拔鑰匙實驗與火炬?zhèn)鬟f實驗。在環(huán)境中不存在靜態(tài)障礙物僅需避免機器人自碰撞的情況下該方法具有非?？斓挠嬎闼俣?得到的軌跡也較為平滑,具備一定的工程應用價值。

1 自碰撞檢測系統(tǒng)設計

1.1 正運動學模型

本文研究對象為圖1 所示的雙七自由度臂機器人。為準確獲取其運動過程中的各關節(jié)位置,并以此為基礎構(gòu)建與更新包圍盒,需要對其進行運動學建模。目前主流的運動學建模方法有2 種,一種是標準D-H 參數(shù)法,一種是改進D-H 參數(shù)法[15]。標準DH 參數(shù)法僅適用于串聯(lián)機器人,而改進D-H 參數(shù)法適用于串聯(lián)機器人、樹結(jié)構(gòu)機器人、并聯(lián)機器人、閉回路結(jié)構(gòu)機器人與移動機器人。由于本文的研究對象為6 輪移動機器人,故建立如圖2 所示的機械臂坐標系,得到的改進D-H 參數(shù)如表1 所示。其中θi代表x軸轉(zhuǎn)角,αi-1代表z軸轉(zhuǎn)角,di-1代表x軸偏移量,ai代表z軸偏移量。

表1 MDH 參數(shù)

圖1 機器人三維模型

圖2 機械臂MDH 圖

由于機械臂采用仿人手結(jié)構(gòu),因此對每個機械臂分別采用4 個OBB 包圍盒進行碰撞模型建模。

為了在機械臂各個關節(jié)旋轉(zhuǎn)的過程中及時更新包圍盒的位置與角度,需要對機械臂進行正運動學計算,以確定各關節(jié)的位置坐標,分別確定第2 關節(jié)到第4 關節(jié)、第5 關節(jié)到第5 關節(jié)、第7 關節(jié)與末端抓手的向量方向與長度,以此為基礎實時地更新包圍盒的位置與歐拉角。通過MDH 參數(shù)表構(gòu)建齊次變換矩陣,可得到:

其中cθi為cosθi的簡寫,sθi為sinθi的簡寫,θ1～θ7為第1 關節(jié)至第7 關節(jié)的旋轉(zhuǎn)角度。

根據(jù)上式求各個關節(jié)相對于基坐標系的齊次變換可得

其中r11～r33可以構(gòu)成3×3 旋轉(zhuǎn)矩陣,px、py、pz即為第i個關節(jié)相對于基坐標系的坐標,三軸歐拉角為

其中px、py、pz與θx、θy、θz均為關于θ1～θ7的函數(shù)。

1.2 OBB 包圍盒建模

基于這些數(shù)據(jù),對于本文所研究的機械臂進行有向包圍盒(oriented bounding box,OBB)建模,則依照式(1)和(2)通過DH 參數(shù)計算出手臂第2 關節(jié)變換矩陣并取其最后一列,可以得到該關節(jié)幾何中心的三維坐標px、py、pz。取其前3 列并依照式(3)轉(zhuǎn)換為歐拉角,可以得到關節(jié)相對基坐標系旋轉(zhuǎn)的角度θx、θy、θz。以此類推,對第4 關節(jié)、第5關節(jié)、第6 關節(jié)與第7 關節(jié)進行同樣的計算。由此可得所有關節(jié)的坐標以及包圍盒旋轉(zhuǎn)的角度,以此可以構(gòu)建出高為

的OBB 包圍盒,長與寬依照三維模型尺寸確定。針對本文的機器人形狀參數(shù),包圍盒的長與寬取為

并按照θx、θy、θz對包圍盒進行旋轉(zhuǎn),由此在機械臂旋轉(zhuǎn)過程基于各關節(jié)角度實時更新包圍盒的坐標與歐拉角。

除此之外,對于雙臂機器人,不僅應當考慮雙臂之間的自碰撞,也應當考慮雙臂與軀干之間的自碰撞。但由于軀干部分相對于機器人坐標系不發(fā)生相對轉(zhuǎn)動,因此生成的OBB 包圍盒不需要實時更新位置與歐拉角,僅需在基坐標系的基礎上增加一個固定的偏移量。針對軀干部分生成的OBB 包圍盒尺寸如下:

得到了如圖3 所示的機器人的包圍盒建模。

圖3 包圍盒模型

1.3 改進GJK 碰撞檢測算法

根據(jù)機械臂的包圍盒模型,將雙臂機器人碰撞問題轉(zhuǎn)化為判斷各包圍盒是否與其他包圍盒相交的問題。本文采用基于單純形退化的改進GJK 算法計算包圍盒之間的距離,避免了原始GJK 算法需要搜索所有原點可能出現(xiàn)的區(qū)域的缺點,計算量小,魯棒性較強。

GJK 算法是由Gilbert、Johnson 和Keerthi 在1988 年開發(fā)的一種迭代算法。通過輸入空間內(nèi)任意兩凸體的頂點集,可以計算出凸體間的歐式距離。假設存在兩凸體A和B,則A和B距離d(A,B)=min{‖X-Y‖},其中X∈A,Y∈B。根據(jù)兩凸體的Minkowski 差的定義,即D={X-Y|X∈A,Y∈B},兩凸體間的距離可以等效為二者的Minkowski 差構(gòu)成的圖形與原點的距離,如圖4 所示。圖4(a)中2 個三角形之間的距離等價于圖4(b)中2 個三角形求Minkowski 差所構(gòu)成的六邊形與到原點的距離。具體流程如下所示。

圖4 距離等價圖

(2)在凸體A中選取在方向投影長度最長的點p1,在凸體B中選取在方向投影長度最長的點p2,令v1=p1-p2,此時v1即為三維單純形的一個頂點。

(3)判斷該三維單純形是否包含原點,若包含原點,說明發(fā)生了碰撞,算法結(jié)束。若不包含原點,繼續(xù)步驟(4)。

(4)在v1、v2、v3、v4構(gòu)成的三維單純形中找出距離原點最近的面S,移除不屬于這個面的點,并將向量的方向更新為面S的垂線方向,以新的向量方向重新取點,構(gòu)建單純形;重復步驟(4),直到取到的點與之前重復,此時迭代完成,未發(fā)生碰撞,計算三維單純形與原點的距離,即為兩凸體間的歐氏距離。

對于原始GJK 算法而言,雖然這種算法在數(shù)學上是完全可實現(xiàn)且可靠的,但實際的應用場景對于實時性的要求非常高,原始的GJK 算法在處理多面體時需要進行大量的計算,可能導致實時性受到影響。為了應對這一問題,本文采用了一種基于單純形退化的改進GJK 算法,通過改良原始GJK 算法的距離子算法來獲得更高的計算效率。

如圖5 所示,原始GJK 算法從底部向上迭代,依次搜索圖5 中(1,2)區(qū)域,(1,3)區(qū)域與(2,3)區(qū)域中是否存在原點,而改進GJK 算法則通過將單純形退化,排除不可能包含原點的先驗區(qū)域,大幅簡化了計算量,算法流程對比圖如圖6 所示。改進GJK算法的詳細步驟如下。

圖5 GJK 算法搜索區(qū)域

圖6 算法流程對比圖

對于如圖7(a)所示的由v1、v2、v3、v4構(gòu)成的三維單純形,構(gòu)造矩陣:

圖7 根據(jù)指定三維單純形構(gòu)建虛擬三維單純形

并對M進行代數(shù)余子式展開,則有:

如圖7(b)～(e)所示,分別舍棄單純形的頂點之一,并將剩余3 個頂點與原點相連,構(gòu)建出4 個虛擬單純形,此時A4,1、A4,2、A4,3、A4,4的絕對值分別與這4 個虛擬單純形的體積成正比。在計算出了detM與所有的A4,i之后,將detM的符號與A4,i逐個進行比較,若detM與A4,i符號均相同,則說明原點在單純形內(nèi)部;若detM與A4,i符號不同,則舍棄點vi,將原有的三維單純形退化為二維單純形,對于圖7的情況,經(jīng)過計算可知:

因此,舍棄點v4,構(gòu)成新的二維單純形s1、s2、s3,如圖8 所示。對于二維單純形,取向量:

圖8 二維單純形與原點投影

將原點O按向量pO投影至單純形所在平面中,得到點PO,此時式(5)中已計算得到的|A4,1|、|A4,2|、|A4,3|與單純形在xOy、yOz、xOz上的投影面積成正比,選取|A4,1|、|A4,2|、|A4,3|中的最大值,并將單純形連同點p向相應的面進行投影。對于本文列出的情況,|A4,1|=1.5,因此在yOz平面進行投影。投影后的結(jié)果如圖9 所示。此時,設矩陣為

圖9 yOz 面投影

得到λ1、λ2、λ3,算法結(jié)束,返回點s1、s2、s3,根據(jù)λ1、λ2、λ3得到新的搜索方向。

綜上所述,可得到如圖10 所示的算法流程圖。

圖10 單純形退化算法流程圖

2 機械臂控制方法

2.1 逆運動學求解

對于任意三維空間機械臂,末端的姿態(tài)信息可以由一個6×1 的向量α=[x,y,z,θx,θy,θz] 完全描述。而對于七自由度機械臂聯(lián)立方程組可得:

在這個方程組中,共有7 個變量θ1,θ2,…,θ7以及6 個方程,變量個數(shù)大于方程個數(shù),屬于欠定方程組,因此在空間中存在無窮多解,即在笛卡爾空間中存在無窮多組滿足指定位置與角度要求的手臂位姿。對于該問題,本文采用牛頓-辛普森法求取滿足末端位置要求的數(shù)值解。

當機械臂末端的位置與角度僅發(fā)生微小變化,且變化耗時較短時,可以將這樣的微小運動等效為機械臂末端的運動速度,這種運動稱作機器人的微分運動。設d為機械臂位置的微分運動,δ為機械臂角度的微分運動,則微分運動可以表示為

記機械臂當前末端位姿為Tcurr,目標末端位姿為Tend,則

使用牛頓迭代法的步驟如下所示。

(1)根據(jù)機械臂當前姿態(tài)下各關節(jié)角度值qcurr=[q1,q2,q3,q4,q5,q6,q7] 計算出機械臂當前末端位姿Tcurr。

(2)根據(jù)機械臂目標末端姿態(tài)計算機械臂的微分運動矩陣Δ。

(3)根據(jù)機械臂的微分運動矩陣Δ與當前角度qcurr計算出機械臂的雅可比矩陣。

(4)根據(jù)式(14)求出dq的大小,其中雅可比矩陣的逆即為機械臂末端位姿矩陣的偽逆。

(5)根據(jù)當前計算出的dq,疊加到目前的關節(jié)角度中,即關節(jié)的角度變化成為qcurr=dq+qcurr。

(6)根據(jù)新的qcurr計算出新的末端姿態(tài)Tcurr,回到步驟(1),以此繼續(xù)迭代。

以上是迭代計算的計算過程。此外,還需要為該迭代過程設置迭代終止條件。常見的做法是設置一個誤差量E,當?shù)Y(jié)果與目標末端位置的差小于一定范圍時即可停止迭代。此外再設置一個迭代次數(shù)上限I,當超出迭代次數(shù)上限時停止計算,并反饋一條錯誤信息。

2.2 機械臂軌跡規(guī)劃

在完成逆解計算之后可以得到2 組關節(jié)角,分別為當前狀態(tài)關節(jié)角qcurr與目標狀態(tài)關節(jié)角qend?？紤]到在插拔火炬鑰匙與傳遞火炬的需求中均不存在坐標及形狀已知的靜態(tài)障礙物,而動態(tài)障礙物通過碰撞檢測可以保證安全性,因此沒有采用常用的諸如快速搜索隨機樹法一類的空間避障路徑規(guī)劃算法。為保證傳遞火炬與插拔鑰匙過程中機械臂運動的平穩(wěn)性,采用了結(jié)合包圍盒歐氏距離的改進五次多項式插補方法。

在機器人實際應用中,各個關節(jié)角的角度、角速度和角加速度都不能發(fā)生突變,否則會大大加重電機的負載,嚴重時可能導致電機損壞。五次多項式插補可以解決三次多項式插值方法中角速度變化不平滑、角加速度容易發(fā)生突變的問題。

五次多項式插補方程可以寫作如下形式:

在此方程的基礎上添加對起始點與終點處角度、角速度、角加速度的約束方程。

解方程得:

在雙臂機器人正常平穩(wěn)工作時,五次多項式插補可以較為輕松地滿足機械臂運動的各項需要,但發(fā)生不在路徑規(guī)劃中的包圍盒碰撞時,仍難以避免電機速度與加速度的突變。因此需要對五次多項式得到的軌跡點逐個進行測試,在發(fā)生碰撞或包圍盒間最小距離小于安全值時取前一個軌跡點,重新進行五次多項式插值。

3 仿真與實驗驗證

3.1 碰撞檢測算法仿真

為驗證本文采用的改進GJK 算法的高效性,在空間中多次隨機生成2 個正四面體、正方體、正八面體以及正十二面體,分別應用分離軸定理(separating axis theorem,SAT)算法、原始GJK 算法與改進GJK 算法進行距離計算。結(jié)果如圖11 所示。

圖11 算法耗時對比

從圖中不難看出,隨著多面體形狀變得復雜,SAT 算法耗時上升較快,而GJK 算法更適合用于處理復雜的三維物體。對于各種形狀、位于空間中隨機位置的多面體,使用改進GJK 算法進行碰撞檢測耗時均比原始GJK 算法少5%～25%,應用于機械臂的碰撞檢測系統(tǒng)可以大幅提升實時性。

3.2 機械臂控制仿真

為了驗證本文提出方法的可行性,在CoppeliaSim軟件中設定了一條有碰撞發(fā)生的機械臂運動軌跡,并使用其提供的Matlab API 進行輔助計算。在機械臂由初始狀態(tài)運動至圖12 所示的終止狀態(tài)的過程中,機械臂由的初始狀態(tài)運動至

圖12 終止狀態(tài)機器人位姿

將這段運動進行不同方式的插值,并在Matlab中計算手臂各關節(jié)與其余包圍盒的距離,同時記錄各關節(jié)參數(shù)變化狀況。參數(shù)變化狀況如圖13 所示。

圖13 均勻插值各關節(jié)運動過程

由圖13 可以看出,采用均勻插值法進行軌跡規(guī)劃時,起始階段與終止階段均存在較大的角速度突變。如圖14 所示,采用五次多項式插值時,起始階段的角速度變化較為平滑,但在發(fā)生了規(guī)劃之外的自碰撞時,仍然不可避免地產(chǎn)生了較大的加速度。如圖15所示,采用了基于安全工作范圍的改良五次多項式軌跡插補法之后,得到了較好的運動軌跡,各關節(jié)角度變化平滑,角速度與角加速度無突變。

圖14 五次多項式插值各關節(jié)運動過程

圖15 改良五次多項式插值各關節(jié)運動過程

3.3 真實場景協(xié)同工作實驗

在火炬?zhèn)鬟f項目中,為了體現(xiàn)雙臂機器人協(xié)同工作的能力,進行了插拔火炬點火鑰匙的實驗。在該實驗中,機器人左手夾持一直徑為5mm的桿型鑰匙,右手夾持的火炬?zhèn)让骈_有一直徑為5.2 mm的小孔,機器人的插拔鑰匙過程完全自主進行,多次實驗均能順利完成指定工作。實驗過程如圖16 所示。

圖16 協(xié)同工作實驗

此外,機器人還與冰壺機器人進行了傳遞火炬實驗。在冰壺機器人位姿不變的基礎上,由雙臂機器人對冰壺機器人所持火炬進行定位,并通過引導手臂末端位姿達到指定位置,使得夾爪所持火炬能夠盡可能接近冰壺機器人所持火炬,進而達到火炬?zhèn)鬟f的目標,實驗過程如圖17 所示。

圖17 與冰壺機器人火炬?zhèn)鬟f實驗

實驗中,雖然機器人順利完成了指定的任務,但由于冰壺機器人屬于動態(tài)障礙物,難以進行包圍盒建模與碰撞檢測,故仍存在碰撞風險,后續(xù)在這類多機器人協(xié)作的動態(tài)障礙物密集的環(huán)境,仍需安裝力傳感器以保證安全工作。

4 結(jié)論

本文針對機器人安全工作要求,為雙七自由度臂機器人設計了一套控制系統(tǒng),通過采用OBB 包圍盒建模與基于單純形退化的改進GJK 算法求歐氏距離相結(jié)合的方法,獲得了良好的避碰撞能力。通過牛頓-辛普森迭代法以及改良五次多項式插值法,可以獲得平穩(wěn)、無突變的機械臂末端運動軌跡。機器人在實際場景中進行了插拔火炬點火鑰匙與傳遞火炬實驗,均能較好地完成指定工作。實驗結(jié)果表明,所設計的雙七自由度臂機器人控制系統(tǒng)具有良好的控制精度、較高的工作可靠性和較廣的環(huán)境適應性,能夠滿足雙機械臂協(xié)同工作的需求。