雙七自由度臂防碰撞協(xié)同控制算法研究①

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

(*中國(guó)科學(xué)院沈陽(yáng)自動(dòng)化研究所機(jī)器人學(xué)國(guó)家重點(diǎn)實(shí)驗(yàn)室 沈陽(yáng) 110016)

(**沈陽(yáng)理工大學(xué)自動(dòng)化與電氣工程學(xué)院 沈陽(yáng) 110159)

(***中國(guó)科學(xué)院大學(xué) 北京 100049)

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

(*****沈陽(yáng)化工大學(xué)信息工程學(xué)院 沈陽(yáng) 110142)

0 引言

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

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

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

1 自碰撞檢測(cè)系統(tǒng)設(shè)計(jì)

1.1 正運(yùn)動(dòng)學(xué)模型

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

表1 MDH 參數(shù)

圖1 機(jī)器人三維模型

圖2 機(jī)械臂MDH 圖

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

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

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

根據(jù)上式求各個(gè)關(guān)節(jié)相對(duì)于基坐標(biāo)系的齊次變換可得

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

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

1.2 OBB 包圍盒建模

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

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

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

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

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

圖3 包圍盒模型

1.3 改進(jìn)GJK 碰撞檢測(cè)算法

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

GJK 算法是由Gilbert、Johnson 和Keerthi 在1988 年開(kāi)發(fā)的一種迭代算法。通過(guò)輸入空間內(nèi)任意兩凸體的頂點(diǎn)集,可以計(jì)算出凸體間的歐式距離。假設(shè)存在兩凸體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)成的圖形與原點(diǎn)的距離,如圖4 所示。圖4(a)中2 個(gè)三角形之間的距離等價(jià)于圖4(b)中2 個(gè)三角形求Minkowski 差所構(gòu)成的六邊形與到原點(diǎn)的距離。具體流程如下所示。

圖4 距離等價(jià)圖

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

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

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

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

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

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

圖6 算法流程對(duì)比圖

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

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

并對(duì)M進(jìn)行代數(shù)余子式展開(kāi),則有:

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

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

圖8 二維單純形與原點(diǎn)投影

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

圖9 yOz 面投影

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

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

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

2 機(jī)械臂控制方法

2.1 逆運(yùn)動(dòng)學(xué)求解

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

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

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

記機(jī)械臂當(dāng)前末端位姿為T(mén)curr,目標(biāo)末端位姿為T(mén)end,則

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

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

(2)根據(jù)機(jī)械臂目標(biāo)末端姿態(tài)計(jì)算機(jī)械臂的微分運(yùn)動(dòng)矩陣Δ。

(3)根據(jù)機(jī)械臂的微分運(yùn)動(dòng)矩陣Δ與當(dāng)前角度qcurr計(jì)算出機(jī)械臂的雅可比矩陣。

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

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

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

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

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

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

在機(jī)器人實(shí)際應(yīng)用中,各個(gè)關(guān)節(jié)角的角度、角速度和角加速度都不能發(fā)生突變,否則會(huì)大大加重電機(jī)的負(fù)載,嚴(yán)重時(shí)可能導(dǎo)致電機(jī)損壞。五次多項(xiàng)式插補(bǔ)可以解決三次多項(xiàng)式插值方法中角速度變化不平滑、角加速度容易發(fā)生突變的問(wèn)題。

五次多項(xiàng)式插補(bǔ)方程可以寫(xiě)作如下形式:

在此方程的基礎(chǔ)上添加對(duì)起始點(diǎn)與終點(diǎn)處角度、角速度、角加速度的約束方程。

解方程得:

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

3 仿真與實(shí)驗(yàn)驗(yàn)證

3.1 碰撞檢測(cè)算法仿真

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

圖11 算法耗時(shí)對(duì)比

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

3.2 機(jī)械臂控制仿真

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

圖12 終止?fàn)顟B(tài)機(jī)器人位姿

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

圖13 均勻插值各關(guān)節(jié)運(yùn)動(dòng)過(guò)程

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

圖14 五次多項(xiàng)式插值各關(guān)節(jié)運(yùn)動(dòng)過(guò)程

圖15 改良五次多項(xiàng)式插值各關(guān)節(jié)運(yùn)動(dòng)過(guò)程

3.3 真實(shí)場(chǎng)景協(xié)同工作實(shí)驗(yàn)

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

圖16 協(xié)同工作實(shí)驗(yàn)

此外,機(jī)器人還與冰壺機(jī)器人進(jìn)行了傳遞火炬實(shí)驗(yàn)。在冰壺機(jī)器人位姿不變的基礎(chǔ)上,由雙臂機(jī)器人對(duì)冰壺機(jī)器人所持火炬進(jìn)行定位,并通過(guò)引導(dǎo)手臂末端位姿達(dá)到指定位置,使得夾爪所持火炬能夠盡可能接近冰壺機(jī)器人所持火炬,進(jìn)而達(dá)到火炬?zhèn)鬟f的目標(biāo),實(shí)驗(yàn)過(guò)程如圖17 所示。

圖17 與冰壺機(jī)器人火炬?zhèn)鬟f實(shí)驗(yàn)

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

4 結(jié)論

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