[发明专利]一种多自由度开环步进串联机械臂及其控制方法

权利要求书

1.一种多自由度开环步进串联机械臂，其特征在于：所述机械臂各个轴的减速器为同步带或谐波减速器，各个轴不带抱闸，断电情况下机械臂能够实现自然下垂；机械臂各个关节上均安装有找零装置，找零装置用于检测机械臂各个关节的机械零位；在机械臂运动空间中安装有带深度信息的摄像头，机械臂末端安装有十字靶标，所述摄像头用于获取十字靶标的空间位置信息；

所述多自由度开环步进串联机械臂的控制方法，包括如下步骤：

(1)机械臂驱动上电前，将机械臂处于自然下垂位；

(2)给机械臂驱动上电，机械臂保持上电前位置；

(3)对自然下垂位时机械臂各关节角度的理论值与参考零位各关节角度的理论值进行对应求差，参考零位为机械臂设计完成时确定的零位；

(4)根据步骤(3)的结果，控制机械臂各关节运动至参考零位理论值；

(5)运动到位后，机械臂各关节通过找零装置找零；

(6)将机械臂各关节当前的坐标位置设置为参考零位的准确值，此时机械臂已精准找到零位，开始执行后续动作；

(7)机械臂运动过程中，通过带深度信息的摄像头采集十字靶标的位置，将其与当前机械臂的理论控制位置相比较，如果偏差大于阈值上限，判定机械臂大失步，进入步骤(8)；如果偏差小于阈值下限，判定机械臂没有失步，进入步骤(9)；如果偏差位于阈值上限和下限之间，则判定机械臂小失步，进入步骤(10)；

(8)机械臂断电，重复步骤(1)-(7)；

(9)机械臂继续执行后续动作，同时执行步骤(7)；

(10)控制机械臂各关节运动至步骤(6)中确定的准确的参考零位处，继续执行后续动作，同时执行步骤(7)。

2.根据权利要求1所述的一种多自由度开环步进串联机械臂，其特征在于：所述每个关节上的找零装置均包括光电开关(1)和挡片(2)；其中，挡片(2)为弧形片状结构；挡片(2)以o点为圆心做圆周转动；光电开关(1)固定设置在挡片(2)的圆周转动路径上；当挡片(2)旋转至遮挡光电开关(1)位置时，光电开关(1)的输出信号为1；当挡片(2)旋转至不遮挡光电开关(1)位置时，光电开关(1)的输出信号为0；光电开关(1)的输出信号从0到1跳变时，挡片(2)的位置即为所在关节的机械零位。

3.根据权利要求2所述的一种多自由度开环步进串联机械臂，其特征在于：转动挡片时，光电开关(1)不允许位于的区域A对应圆心o点的圆心角c为30°-60°。

4.根据权利要求1所述的一种多自由度开环步进串联机械臂，其特征在于：所述步骤(1)中，如机械臂不能位于自然下垂位，则应通过人工拖动机械臂到自然下垂位。

5.根据权利要求4所述的一种多自由度开环步进串联机械臂，其特征在于：所述步骤(1)中，机械臂各关节角度相对于标准自然下垂位的误差不超过±10°。

6.根据权利要求1所述的一种多自由度开环步进串联机械臂，其特征在于：所述步骤(7)中，十字靶标的位置与当前机械臂的理论控制位置的偏差Δδ通过如下公式计算：

其中，(x₁,y₁,z₁)为十字靶标在相机坐标系下的位置坐标，(x₂,y₂,z₂)为当前机械臂在相机坐标系下的理论控制位置，由当前机械臂在机械臂坐标系下的理论控制位置转换得到；

相机坐标系原点为摄像头光心，z轴为摄像头光轴，x轴垂直与机械臂的安装基座，且x、y轴符合右手定则。

7.根据权利要求1所述的一种多自由度开环步进串联机械臂，其特征在于：所述步骤(4)中，机械臂各关节运动的顺序为从初始关节到末端关节，依次运动。

8.根据权利要求7所述的一种多自由度开环步进串联机械臂，其特征在于：所述步骤(5)中，机械臂各关节找零的顺序为从末端关节到初始关节，依次找零。

9.根据权利要求7所述的一种多自由度开环步进串联机械臂，其特征在于：所述步骤(5)中，机械臂各关节同时找零。