[发明专利]一种视觉伺服切换控制方法及系统

权利要求书

1.一种视觉伺服切换控制方法，其特征在于，包括：

获取带有二维码的图像、图像特征点的期望坐标、相机位姿的期望轴角和相机参数；所述图像特征点为二维码的角点；

根据所述图像确定图像特征点的实际坐标；

根据所述图像和所述相机参数确定实际相机位姿；

判断坐标误差的李雅普诺夫函数值是否小于或等于控制结束阈值，得到第一判断结果；所述坐标误差为所述图像特征点的实际坐标与所述图像特征点的期望坐标之间的误差；

若所述第一判断结果为是，则结束控制；

若所述第一判断结果为否，则判断所述图像特征点的实际坐标与图像边界的距离是否大于边界阈值，得到第二判断结果；

若第二判断结果为是，则采用基于图像特征的视觉伺服控制方法计算机械臂第一关节角速度，根据所述第一关节角速度对机器人进行控制，并返回步骤“获取带有二维码的图像、图像特征点的期望坐标、相机位姿的期望轴角和相机参数”；

若第二判断结果为否，则判断所述图像特征点的实际坐标与图像边界的距离是否等于边界阈值，得到第三判断结果；

若所述第三判断结果为是，则在所述坐标误差的李雅普诺夫函数值小于或等于基于图像特征的视觉伺服控制阈值时采用基于图像特征的视觉伺服控制方法计算所述第一关节角速度，根据所述第一关节角速度对机器人进行控制，并返回步骤“获取带有二维码的图像、图像特征点的期望坐标、相机位姿的期望轴角和相机参数”；或者在相邻两次轴角误差的李雅普诺夫函数值差值小于或等于0，并且在基于图像特征的视觉伺服控制运行时间大于运行时间阈值时，采用基于图像位置的视觉伺服控制方法计算机械臂第二关节角速度，根据所述第二关节角速度对机器人进行控制，并返回步骤“获取带有二维码的图像、图像特征点的期望坐标、相机位姿的期望轴角和相机参数”；

若所述第三判断结果为否，则在相邻两次轴角误差的李雅普诺夫函数值差值小于或等于0，并且在基于图像特征的视觉伺服控制运行时间大于运行时间阈值时，采用基于图像位置的视觉伺服控制方法计算所述第二关节角速度，根据所述第二关节角速度对机器人进行控制，并返回步骤“获取带有二维码的图像、图像特征点的期望坐标、相机位姿的期望轴角和相机参数”；所述轴角误差为实际相机位姿的轴角与所述相机位姿的期望轴角之间的误差。

2.根据权利要求1所述的视觉伺服切换控制方法，其特征在于，所述采用基于图像特征的视觉伺服控制方法计算机械臂第一关节角速度，具体包括：

获取机械臂末端当前位姿、机械臂末端期望位姿和机械臂末端期望速度；

根据所述机械臂末端期望位姿与所述机械臂末端当前位姿的差值和所述机械臂末端期望速度计算所述第一关节角速度。

3.根据权利要求2所述的视觉伺服切换控制方法，其特征在于，所述采用基于图像位置的视觉伺服控制方法计算机械臂第二关节角速度，具体包括：

对图像的像素坐标进行归一化处理，得到归一化的像素坐标；

根据所述归一化的像素坐标确定交互作用矩阵；

根据所述交互作用矩阵和所述坐标误差确定笛卡尔坐标系下的相机速度；

获取坐标系变换矩阵；所述坐标系变换矩阵为笛卡尔坐标系变换为相机坐标系的矩阵；

根据所述坐标系变换矩阵，将所述笛卡尔坐标系下的相机速度转换为相机坐标系下的相机速度；

根据所述相机坐标系下的相机速度计算所述第二关节角速度。