双臂洁净机器人运动控制算法研究

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后变换为坐标系

⑥坐标系 X LE'O YLE'绕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)的右侧，通过三角函数的和差化积公式化简可得

当时，C R ,SR均为0。因此有：

可解得：

Z轴分量1l不参与运算，升降关节的关节值为pzR。

■2.2 左臂逆解

左臂逆解即为求解矩阵方程组(8)的解，其左侧为已知，右侧为未知，根据公式(8)中 两个分量的特点可得出：

其中：

结合公式(1)，设：

对公式(10)的右侧，通过三角函数的和差化积公式化简可得：

当时，C L ,SL均 为0。因此有：

可解得：

■2.3 机器人位姿的极坐标描述

在工程上，洁净机器人的运动可拆解为伸缩运动和旋转运动，通常采用极坐标来表征机器人的位姿，更加直观。机器人具有如下特征：

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

(2)手指末端指向的方向始终与基坐标原点到末端所形成的向量共线；

(3)手指的伸缩长度由θ 3L (θ3R)决定。

基于上述特点，可得到左右两臂在极坐标中的极径和极角，替换原有的正逆传动关系解析/逆解析函数。极径计算：

极角计算：

由公式(6)和公式(8)可得左右两臂的姿态角度分别为：

3 仿真

为验证所提出的双臂运动学算法的准确性，通过MATLAB对机械手的正逆运动学计算结果进行验证。验证按照如下步骤进行：

(1)规划生成空间轨迹。

(2)对空间轨迹上每一点应用逆解求得所对应的关节空间向量。

(3)将(2)中计算得到的关节空间向量代入运动学正解中，求解得到对于规划轨迹的跟踪结果。

(4)对比规划轨迹和跟踪轨迹的差异可判断机器人运动学正解、逆解的准确性，同时可验证正解–逆解操作是否能够形成严格的闭环。

通过规划器分别生成正弦轨迹和空间圆弧轨迹，所设计正逆解算法对于规划结果的跟踪效果分别如图2和图3所示。

图2 正弦跟踪曲线

图3 圆弧跟踪曲线

由图2和图3可见，经由上述仿真实验步骤所产生的跟踪轨迹与规划器所产生的规划轨迹完全重合，可说明所设计的正逆解算法的准确性，同时正解–逆解操作可形成严格的闭环确保机器人位姿不产生偏移。

4 结论

本文针对双臂真空传输机器人分析其结构特点、分析了机器人机构上存在的特殊性质，分别针对左右双臂建立了正反运动学并给出了极坐标描述。仿真结果验证了所给出的运动学的准确性，同时正解–逆解操作在数据上能够形成闭环，可确保运动不发生偏移。