[发明专利]一种四自由度机械臂视觉伺服控制方法

权利要求书

1.一种四自由度机械臂视觉伺服控制方法，其特征在于，所述四自由度机械臂包含四个关节，机械臂的臂上设有臂上相机，机械臂的末端设有末端相机；针对所述四自由度机械臂的视觉伺服控制方法，具体为：

首先将所述机械臂的整臂模式设置为视觉伺服模式，在每个视觉伺服模式的控制周期内均判断视觉测量位姿数据是否有效，若视觉测量位姿数据连续多个周期无效，则机械臂停止运动，整臂模式转换为伺服待机模式，关节控制模式转换为位置伺服模式；

在每个视觉伺服模式的控制周期内，判断视觉测量位姿数据是否有效，若视觉测量位姿数据有效，选取有效的视觉测量位姿数据，整臂控制器下发关节控制模式切换指令，使得关节控制模式处于速度控制模式，采用如步骤(1)～步骤(5)的方式计算并输出规划的末端四维速度VW_POR；然后通过运动学逆解求得规划关节角速度和规划关节角位置，并作为控制指令进行输出，用于控制关节在下一控制周期的角速度和角位置；

所述视觉伺服模式中的步骤(1)～步骤(5)具体为：

步骤(1)由所述臂上相机或者所述末端相机获取视觉测量位姿数据；

步骤(2)机械臂压紧构型下测量得到臂上相机的相机坐标系与末端坐标系的位置关系向量和姿态关系矩阵，以及末端相机的相机坐标系与末端坐标系的位置关系向量为和姿态关系矩阵；

步骤(3)将视觉测量位姿数据下的最终到位终止点位姿转换到末端坐标系下，得到末端坐标系下的最终到位终止运动点位姿EndPE_des；

步骤(4)进行运动跟踪，计算末端偏差包括位置差和姿态差，规划末端线速度Vez和末端角速度Wez，其中末端线速度Vez为三维向量，由末端综合线速度分配到末端坐标系的三个轴向的分量组成；则得到规划的末端四维速度VW_POR；VW_POR(1:3)＝Vez；VW_POR(4)＝Wez；

步骤(5)判断末端偏差是否超出给定阈值，若是则跳出视觉伺服模式，输出规划的末端四维速度VW_POR；否则继续执行步骤(1)～(5)。

2.如权利要求1所述的方法，其特征在于，初始默认关节控制模式均为位置伺服模式。

3.如权利要求1所述的方法，其特征在于，所述在每个视觉伺服模式的控制周期内，判断视觉测量位姿数据是否有效，具体为，分别由臂上相机和末端相机出获取视觉测量位姿数据，若其中臂上相机或末端相机其中一个采集得到视觉测量位姿数据，则视觉测量位姿数据有效，否则视觉测量位姿数据无效。

4.如权利要求1或3所述的方法，其特征在于，所述若视觉测量位姿数据有效，选取有效的视觉测量位姿数据，具体为：若臂上相机和末端相机的视觉测量位姿数据均有效，则优先取末端相机的视觉测量数据作为有效的视觉测量位姿数据。

5.如权利要求1所述的方法，其特征在于，所述步骤(2)具体为：在机械臂压紧构型下测量得到：臂上相机的相机坐标系与末端坐标系的位置关系向量为p_por_vision0和姿态关系矩阵C_por_vision0、以及末端相机的相机坐标系与末端坐标系的位置关系向量为p_por_vision1和姿态关系矩阵C_por_vision1

若关节四转动，关节四的转动角度为jnt_pos_now(4)；则关节四的姿态变化矩阵为C_4h_4^T；

则关节四转动后p_por_vision0、C_por_vision0、p_por_vision1、C_por_vision1均前乘C_4h_4^T。