雙臂潔凈機器人運動控制算法研究

孫若懷

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

0 引言

隨著近年來半導體制造領域的快速發展，制造企業對于產能也提出了更高的要求。相較于傳統的SCARA單臂真空傳輸機械手，雙臂潔凈機器人具有更大的工作范圍、可執行更復雜的搬運動作，使得產線的產能有著顯著提高，使得雙臂潔凈機器人成為行業的熱點研究話題。由于該機器人結構更加復雜所以給控制帶來了更大的挑戰。

針對雙臂機器人的研究學術界已經有了一定的研究成果：張波等人針對欠驅動三指手爪進行了研究，優化了機構控制效果[1]；韓冬等人通過優化空間雙臂機器人軌跡來改善機器人的震顫問題[2]。

1 機器人的運動學正解

機器人的運動學正解是根據機器人各個關節的轉角計算得到機器人末端在操作空間中的位置和姿態的過程，此過程實現關節空間到操作空間的映射。本文所研究的雙臂真空傳輸機器人，其機械構型和關節坐標系分布如圖1所示。

圖1 機械構型

如圖1所示，該機器人共有四個主動關節，分別負責豎直Z軸升降、總體旋轉、左臂伸縮和右臂伸縮。

■1.1 關節坐標系建立

分別對左右雙臂采用工程法構建機器人的關節坐標系：

右臂需經過如下變換：

①坐標系XBOYB繞Z軸逆時針旋轉–31°（兩臂夾角118°），變換為坐標系XR0OYR0；

②坐標系XR0OYR0沿Z軸平移1l并繞Z軸逆時針旋轉θR2=θ2后，變換為坐標系XR1OYR1；

③坐標系XR1OYR1沿X軸平移L3后繞Z軸逆時針旋轉θR3變換為坐標系XR2OYR2；

④坐標系XR2OYR2沿X軸平移L4后繞Z軸逆時針旋轉θR4變換為XR3OYR3；

⑤坐標系XR3OYR3沿X軸平移L5后變換為坐標系XREOYRE。

左臂需經過如下變換：

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

②坐標系XL0OYL0沿Z軸平移?1l并繞Z軸逆時針旋轉θL2=?θ2后，變換為坐標系XL1OYL1；

③坐標系XL1OYL1沿X軸平移L3后繞Z軸逆時針旋轉θL3變換為坐標系XL2OYL2；

④坐標系XL2OYL2沿X軸平移L4后繞Z軸逆時針旋轉θL4變換為XL3OYL3；

⑤坐標系XL3OYL3沿X軸平移L5后變換為坐標系

⑥坐標系 XLE'OYLE'繞X軸逆時針旋轉180°后變換為

■1.2 機器人特殊性質

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

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

表1 關節軟限位

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

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

左臂各坐標系間變換關系如下：

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

■1.3 運動學正解

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

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

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

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

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

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

2 運動學逆解

雙臂機器人的逆解是已知機器人末端的位姿，反向求解機器人各關節的角度，通常是由規劃器給出機器人期望到達的操作空間位置經由逆解計算獲得各個關節電機的轉角，實現操作空間向關節空間的映射。

■2.1 右臂逆解

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

其中：

結合公式(1)，設：

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

可解得：

Z軸分量1l不參與運算，升降關節的關節值為pzR。

■2.2 左臂逆解

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

其中：

結合公式(1)，設：

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

可解得：

■2.3 機器人位姿的極坐標描述

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

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

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

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

基于上述特點，可得到左右兩臂在極坐標中的極徑和極角，替換原有的正逆傳動關系解析/逆解析函數。極徑計算：

極角計算：

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

3 仿真

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

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

(2)對空間軌跡上每一點應用逆解求得所對應的關節空間向量。

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

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

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

圖2 正弦跟蹤曲線

圖3 圓弧跟蹤曲線

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

4 結論

本文針對雙臂真空傳輸機器人分析其結構特點、分析了機器人機構上存在的特殊性質，分別針對左右雙臂建立了正反運動學并給出了極坐標描述。仿真結果驗證了所給出的運動學的準確性，同時正解–逆解操作在數據上能夠形成閉環，可確保運動不發生偏移。