孫若懷

（沈陽新松機器人自動化股份有限公司，遼寧沈陽，110168）

0 引言

隨著近年來半導(dǎo)體制造領(lǐng)域的快速發(fā)展，制造企業(yè)對于產(chǎn)能也提出了更高的要求。相較于傳統(tǒng)的SCARA單臂真空傳輸機械手，雙臂潔凈機器人具有更大的工作范圍、可執(zhí)行更復(fù)雜的搬運動作，使得產(chǎn)線的產(chǎn)能有著顯著提高，使得雙臂潔凈機器人成為行業(yè)的熱點研究話題。由于該機器人結(jié)構(gòu)更加復(fù)雜所以給控制帶來了更大的挑戰(zhàn)。

針對雙臂機器人的研究學(xué)術(shù)界已經(jīng)有了一定的研究成果：張波等人針對欠驅(qū)動三指手爪進(jìn)行了研究，優(yōu)化了機構(gòu)控制效果[1]；韓冬等人通過優(yōu)化空間雙臂機器人軌跡來改善機器人的震顫問題[2]。

1 機器人的運動學(xué)正解

機器人的運動學(xué)正解是根據(jù)機器人各個關(guān)節(jié)的轉(zhuǎn)角計算得到機器人末端在操作空間中的位置和姿態(tài)的過程，此過程實現(xiàn)關(guān)節(jié)空間到操作空間的映射。本文所研究的雙臂真空傳輸機器人，其機械構(gòu)型和關(guān)節(jié)坐標(biāo)系分布如圖1所示。

圖1 機械構(gòu)型

如圖1所示，該機器人共有四個主動關(guān)節(jié)，分別負(fù)責(zé)豎直Z軸升降、總體旋轉(zhuǎn)、左臂伸縮和右臂伸縮。

■1.1 關(guān)節(jié)坐標(biāo)系建立

分別對左右雙臂采用工程法構(gòu)建機器人的關(guān)節(jié)坐標(biāo)系：

右臂需經(jīng)過如下變換：

①坐標(biāo)系XBOYB繞Z軸逆時針旋轉(zhuǎn)–31°（兩臂夾角118°），變換為坐標(biāo)系XR0OYR0；

②坐標(biāo)系XR0OYR0沿Z軸平移1l并繞Z軸逆時針旋轉(zhuǎn)θR2=θ2后，變換為坐標(biāo)系XR1OYR1；

③坐標(biāo)系XR1OYR1沿X軸平移L3后繞Z軸逆時針旋轉(zhuǎn)θR3變換為坐標(biāo)系XR2OYR2；

④坐標(biāo)系XR2OYR2沿X軸平移L4后繞Z軸逆時針旋轉(zhuǎn)θR4變換為XR3OYR3；

⑤坐標(biāo)系XR3OYR3沿X軸平移L5后變換為坐標(biāo)系XREOYRE。

左臂需經(jīng)過如下變換：

①坐標(biāo)系XBOYB繞Y軸逆時針旋轉(zhuǎn)180°，再繞Z軸逆時針旋轉(zhuǎn)–31°（兩臂夾角118°），變換為坐標(biāo)系XL0OYL0；

②坐標(biāo)系XL0OYL0沿Z軸平移?1l并繞Z軸逆時針旋轉(zhuǎn)θL2=?θ2后，變換為坐標(biāo)系XL1OYL1；

③坐標(biāo)系XL1OYL1沿X軸平移L3后繞Z軸逆時針旋轉(zhuǎn)θL3變換為坐標(biāo)系XL2OYL2；

④坐標(biāo)系XL2OYL2沿X軸平移L4后繞Z軸逆時針旋轉(zhuǎn)θL4變換為XL3OYL3；

⑤坐標(biāo)系XL3OYL3沿X軸平移L5后變換為坐標(biāo)系

⑥坐標(biāo)系 XLE'OYLE'繞X軸逆時針旋轉(zhuǎn)180°后變換為

■1.2 機器人特殊性質(zhì)

值得注意的是，機器人的腕部從動關(guān)節(jié)與肘部的主動關(guān)節(jié)有著2：(–1)的機構(gòu)傳動約束關(guān)系。這個約束可以確保機器人腕部的姿態(tài)始終與手臂末端的姿態(tài)的一致。

按照機器人各關(guān)節(jié)的實際機械運動范圍，可以求解得出機器人控制算法中軟限位，如表1所示。

表1 關(guān)節(jié)軟限位

雙臂機器人肘部連桿相對于手指方向的法線的夾角為–15°，定義如下變量：

按照1.1節(jié)右臂的5步變換，可分別求得每步的變換矩陣，右臂各坐標(biāo)系間變換關(guān)系如下（E表示工具）：

左臂各坐標(biāo)系間變換關(guān)系如下：

按照1.1節(jié)左臂的6步變換，可分別求得每步的變換矩陣，左臂各坐標(biāo)系間變換關(guān)系如下（E表示工具）：

■1.3 運動學(xué)正解

機器人右臂正解即為式(3)中各矩陣順序相乘所得到的位姿矩陣，即：

上式中，n zR,o zR,a xR,ayR四項為0，azR為1。其余非零項為：

式(6)中部分關(guān)鍵角度的正余弦值如式(7)。

類似地，機器人左臂正解即為式(3)、(4)中各矩陣順序相乘所得到的位姿矩陣，即：

上式中，n zL,o zL,a xL,ayL四項為0，azL為1。其余非零項為：

式(8)中部分關(guān)鍵角度的正余弦值如式(9)。

2 運動學(xué)逆解

雙臂機器人的逆解是已知機器人末端的位姿，反向求解機器人各關(guān)節(jié)的角度，通常是由規(guī)劃器給出機器人期望到達(dá)的操作空間位置經(jīng)由逆解計算獲得各個關(guān)節(jié)電機的轉(zhuǎn)角，實現(xiàn)操作空間向關(guān)節(jié)空間的映射。

■2.1 右臂逆解

右臂逆解即為求解矩陣方程組(6)的解，其左側(cè)為已知，右側(cè)為未知，根據(jù)公式(6)中p xR,pyR兩個分量的特點可得出：

其中：

結(jié)合公式(1)，設(shè)：

對公式(9)的右側(cè)，通過三角函數(shù)的和差化積公式化簡可得

可解得：

Z軸分量1l不參與運算，升降關(guān)節(jié)的關(guān)節(jié)值為pzR。

■2.2 左臂逆解

左臂逆解即為求解矩陣方程組(8)的解，其左側(cè)為已知，右側(cè)為未知，根據(jù)公式(8)中兩個分量的特點可得出：

其中：

結(jié)合公式(1)，設(shè)：

對公式(10)的右側(cè)，通過三角函數(shù)的和差化積公式化簡可得：

可解得：

■2.3 機器人位姿的極坐標(biāo)描述

在工程上，潔凈機器人的運動可拆解為伸縮運動和旋轉(zhuǎn)運動，通常采用極坐標(biāo)來表征機器人的位姿，更加直觀。機器人具有如下特征：

(1)末端手指的姿態(tài)由θ2,θ3L(θ3R)決定；

(2)手指末端指向的方向始終與基坐標(biāo)原點到末端所形成的向量共線；

(3)手指的伸縮長度由θ3L(θ3R)決定。

基于上述特點，可得到左右兩臂在極坐標(biāo)中的極徑和極角，替換原有的正逆?zhèn)鲃雨P(guān)系解析/逆解析函數(shù)。極徑計算：

極角計算：

由公式(6)和公式(8)可得左右兩臂的姿態(tài)角度分別為：

3 仿真

為驗證所提出的雙臂運動學(xué)算法的準(zhǔn)確性，通過MATLAB對機械手的正逆運動學(xué)計算結(jié)果進(jìn)行驗證。驗證按照如下步驟進(jìn)行：

(1)規(guī)劃生成空間軌跡。

(2)對空間軌跡上每一點應(yīng)用逆解求得所對應(yīng)的關(guān)節(jié)空間向量。

(3)將(2)中計算得到的關(guān)節(jié)空間向量代入運動學(xué)正解中，求解得到對于規(guī)劃軌跡的跟蹤結(jié)果。

(4)對比規(guī)劃軌跡和跟蹤軌跡的差異可判斷機器人運動學(xué)正解、逆解的準(zhǔn)確性，同時可驗證正解–逆解操作是否能夠形成嚴(yán)格的閉環(huán)。

通過規(guī)劃器分別生成正弦軌跡和空間圓弧軌跡，所設(shè)計正逆解算法對于規(guī)劃結(jié)果的跟蹤效果分別如圖2和圖3所示。

圖2 正弦跟蹤曲線

圖3 圓弧跟蹤曲線

由圖2和圖3可見，經(jīng)由上述仿真實驗步驟所產(chǎn)生的跟蹤軌跡與規(guī)劃器所產(chǎn)生的規(guī)劃軌跡完全重合，可說明所設(shè)計的正逆解算法的準(zhǔn)確性，同時正解–逆解操作可形成嚴(yán)格的閉環(huán)確保機器人位姿不產(chǎn)生偏移。

4 結(jié)論

本文針對雙臂真空傳輸機器人分析其結(jié)構(gòu)特點、分析了機器人機構(gòu)上存在的特殊性質(zhì)，分別針對左右雙臂建立了正反運動學(xué)并給出了極坐標(biāo)描述。仿真結(jié)果驗證了所給出的運動學(xué)的準(zhǔn)確性，同時正解–逆解操作在數(shù)據(jù)上能夠形成閉環(huán)，可確保運動不發(fā)生偏移。