CN111360827A

CN111360827A - 一种视觉伺服切换控制方法及系统

Info

Publication number: CN111360827A
Application number: CN202010151757.6A
Authority: CN
Inventors: 高会军; 刘晨璐; 姜易木; 林伟阳; 佟明斯
Original assignee: Harbin Institute of Technology
Current assignee: Ningbo Intelligent Equipment Research Institute Co ltd; Harbin Institute of Technology
Priority date: 2020-03-06
Filing date: 2020-03-06
Publication date: 2020-07-03
Anticipated expiration: 2040-03-06
Also published as: CN111360827B

Abstract

本发明公开了一种视觉伺服切换控制方法及系统。该方法包括：确定带有二维码的图像特征点的实际坐标和实际相机位姿；图像特征点的实际坐标与图像边界的距离大于边界阈值，则采用基于图像特征的视觉伺服控制方法计算机械臂第一关节角速度，根据第一关节角速度对机器人进行控制；在图像特征点的实际坐标与图像边界的距离是否等于边界阈值，区分计算哪种关节角速度；在相邻两次轴角误差的李雅普诺夫函数值差值小于或等于0，并且在基于图像特征的视觉伺服控制运行时间大于运行时间阈值时，采用基于图像位置的视觉伺服控制方法计算第二关节角速度。采用本发明的方法及系统，具有能够增强机器人控制系统稳定性，改善视觉伺服效果的优点。

Description

一种视觉伺服切换控制方法及系统

技术领域

本发明涉及机器人视觉伺服控制技术领域，特别是涉及一种视觉伺服切换控制方法及系统。

背景技术

目前，各类机器人、机械臂在工业自动化中应用渐趋广泛，机器人的视觉伺服控制技术起到了十分关键的作用。然而，视觉伺服控制方法的选择是一个难点，常用的视觉伺服控制方法可分为基于图像位置的视觉伺服和基于图像特征的视觉伺服。

基于图像位置的视觉伺服适用于大范围的视觉跟踪，可细分为基于关节空间的控制和基于笛卡尔空间的控制，前者需要进行轨迹变换，在需要对位置与接触力都进行控制时使用不便；后者在每一个控制周期内都需要进行运动学正解，在相同计算性能下最大采样频率较低，系统稳定性和抗干扰性能也随之较差。

基于图像特征的视觉伺服适用于小范围的精确定位，但涉及到矩阵求逆，在图像特征参数与操作空间变量的非线性映射下，机器人可能出现奇异位形，这将引起不稳定或控制作用的饱和，以致机器人超出关节的运动极限。

因此，如何实现良好的视觉伺服效果，增强机器人控制系统的稳定性是亟待解决的问题。

发明内容

本发明的目的是提供一种视觉伺服切换控制方法及系统，具有能够增强机器人控制系统稳定性，改善视觉伺服效果的优点。

为实现上述目的，本发明提供了如下方案：

一种视觉伺服切换控制方法，包括：

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

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

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

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

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

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

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

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

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

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

可选的，所述采用基于图像特征的视觉伺服控制方法计算机械臂第一关节角速度，具体包括：

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

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

可选的，所述采用基于图像位置的视觉伺服控制方法计算机械臂第二关节角速度，具体包括：

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

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

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

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

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

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

可选的，所述根据所述机械臂末端期望位姿与所述机械臂末端当前位姿的差值和所述机械臂末端期望速度计算所述第一关节角速度，具体包括：

根据如下公式计算所述第一臂关节角速度：

式中，q为关节转角，

为第一关节角速度，

为第一雅克比矩阵的逆，

为机械臂末端期望平动速度，e_bp为机械臂末端期望位姿与机械臂末端当前位姿的位置误差矢量，K_p为对应e_bp的系数阵，ω_d为机械臂末端期望旋转角速度，e_bo为机械臂末端期望位姿与机械臂末端当前位姿的方向误差矢量，K_o为对应e_bo的系数阵，S(·)为反对称阵，n_d、o_d、a_d分别为期望姿态矩阵R_d的三个列向量，n_e、o_e、a_e分别为实际姿态矩阵R_e的三个列向量。

可选的，所述根据所述相机坐标系下的相机速度计算所述第二关节角速度，具体包括：

根据如下公式计算所述第二关节角速度：

式中，

为第二关节角速度，J^-1为第二雅克比矩阵的逆，R_c为旋转矩阵，

为相机在相机坐标系中的速度，T_i ^c为坐标系变换矩阵，

为笛卡尔坐标系下的相机速度，L_s为交互作用矩阵，

为交互作用矩阵的转置矩阵，K_s为增益系数矩阵，e_s为坐标误差。

本发明还提供一种视觉伺服切换控制系统，包括：

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

坐标确定模块，用于根据所述图像确定图像特征点的实际坐标；

位姿确定模块，用于根据所述图像和所述相机参数确定实际相机位姿；

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

结束控制模块，用于在所述第一判断结果为是时，结束控制；

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

第一关节角速度计算模块，用于在第二判断结果为是时，采用基于图像特征的视觉伺服控制方法计算机械臂第一关节角速度，根据所述第一关节角速度对机器人进行控制，并执行所述获取模块；

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

关节角速度计算区分模块，用于在所述第三判断结果为是时，在所述坐标误差的李雅普诺夫函数值小于或等于基于图像特征的视觉伺服控制阈值时采用基于图像特征的视觉伺服控制方法计算所述第一关节角速度，根据所述第一关节角速度对机器人进行控制，并执行所述获取模块；或者在相邻两次轴角误差的李雅普诺夫函数值差值小于或等于0，并且在基于图像特征的视觉伺服控制运行时间大于运行时间阈值时，采用基于图像位置的视觉伺服控制方法计算机械臂第二关节角速度，根据所述第二关节角速度对机器人进行控制，并执行所述获取模块；

第二关节角速度计算模块，用于在所述第三判断结果为否时，在相邻两次轴角误差的李雅普诺夫函数值差值小于或等于0，并且在基于图像特征的视觉伺服控制运行时间大于运行时间阈值时，采用基于图像位置的视觉伺服控制方法计算所述第二关节角速度，根据所述第二关节角速度对机器人进行控制，并执行所述获取模块。

可选的，所述第一关节角速度计算模块，具体包括：

位姿与速度获取单元，用于获取机械臂末端当前位姿、机械臂末端期望位姿和机械臂末端期望速度；

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

可选的，所述第二关节角速度计算模块，具体包括：

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

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

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

坐标系变换矩阵获取单元，用于获取坐标系变换矩阵；所述坐标系变换矩阵为笛卡尔坐标系变换为相机坐标系的矩阵；

第二相机速度确定单元，用于根据所述坐标系变换矩阵，将所述笛卡尔坐标系下的相机速度转换为相机坐标系下的相机速度；

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

可选的，所述第一关节角速度计算单元，具体包括：

第一关节角速度计算子单元，用于根据如下公式计算所述第一关节角速度：

式中，q为关节转角，

为第一关节角速度，

为第一雅克比矩阵的逆，

可选的，所述第二关节角速度计算单元，具体包括：

第二关节角速度计算子单元，用于根据如下公式计算所述第二关节角速度：

式中，

为相机在相机坐标系中的速度，T_i ^c为坐标系变换矩阵，

为笛卡尔坐标系下的相机速度，L_s为交互作用矩阵，

与现有技术相比，本发明的有益效果是：

本发明提出了一种视觉伺服切换控制方法及系统，将二维码的角点作为图像特征点，利用了二维码内部的编码信息，直接对特征点可能出现的区域进行一个准确的定位，精度高，稳定性好，并且通过阈值判断的方法，确保系统在视觉伺服过程中始终处于稳定的状态，同时能够保证图像处理和运动控制的实时性。通过将基于图像特征的视觉伺服控制(IBVS)方法与基于图像位置的视觉伺服控制(PBVS)方法进行切换，能够有效避免传统视觉伺服当中存在的诸多问题，包括PBVS特征点容易游离到视野之外、IBVS存在奇异点和相机后撤现象等，保证在整个控制过程中满足约束，同时增加系统的稳定域，提高收敛速度。本发明能够在实际装配作业当中减少装配误差，提高生产效率，节约加工成本，在视觉伺服相关的各种组装作业中有重要应用。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。

图1为本发明实施例中视觉伺服切换控制方法流程图；

图2为本发明实施例中基于图像位置的视觉伺服控制系统流程图；

图3为本发明实施例中基于图像位置的视觉伺服控制系统控制结构图；

图4为本发明实施例中基于图像特征的视觉伺服控制系统流程图；

图5为本发明实施例中基于图像特征的视觉伺服控制系统控制结构图；

图6为本发明实施例中视觉伺服切换控制系统结构图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。

实施例

图1为本发明实施例中视觉伺服切换控制方法流程图，如图1所示，一种视觉伺服切换控制方法，包括：

步骤101：获取带有二维码的图像、图像特征点的期望坐标、相机位姿的期望轴角和相机参数。图像特征点为二维码的角点。

步骤102：根据图像确定图像特征点的实际坐标。

步骤103：根据图像和相机参数确定实际相机位姿。

步骤104：判断坐标误差的李雅普诺夫函数值是否小于或等于控制结束阈值，得到第一判断结果。若第一判断结果为是，则执行步骤110；若第一判断结果为否，则执行步骤105。坐标误差为图像特征点的实际坐标与图像特征点的期望坐标之间的误差。

步骤105：判断图像特征点的实际坐标与图像边界的距离是否大于边界阈值，得到第二判断结果。若第二判断结果为是，则执行步骤106；若第二判断结果为否，则执行步骤107。

步骤106：采用基于图像特征的视觉伺服控制方法计算机械臂第一关节角速度，根据第一关节角速度对机器人进行控制，并返回步骤101。

采用基于图像特征的视觉伺服控制方法计算机械臂第一关节角速度，具体包括：

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

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

根据如下公式计算第一臂关节角速度：

式中，q为关节转角，

为第一关节角速度，

为第一雅克比矩阵的逆，

为机械臂末端期望平动速度，e_bp为机械臂末端期望位姿与机械臂末端当前位姿的位置误差矢量，K_p为对应e_bp的系数阵，ω_d为机械臂末端期望旋转角速度，e_bo为机械臂末端期望位姿与机械臂末端当前位姿的方向误差矢量，K_o为对应e_bo的系数阵，L表示中间矩阵，S(·)为反对称阵，n_d、o_d、a_d分别为期望姿态矩阵R_d的三个列向量，n_e、o_e、a_e分别为实际姿态矩阵R_e的三个列向量。

步骤107：判断图像特征点的实际坐标与图像边界的距离是否等于边界阈值，得到第三判断结果。若第三判断结果为是，则执行步骤108；若第三判断结果为否，则执行步骤109。

步骤108：在坐标误差的李雅普诺夫函数值小于或等于基于图像特征的视觉伺服控制阈值时采用基于图像特征的视觉伺服控制方法计算第一关节角速度，根据第一关节角速度对机器人进行控制，并返回步骤101；或者在相邻两次轴角误差的李雅普诺夫函数值差值小于或等于0，并且在基于图像特征的视觉伺服控制运行时间大于运行时间阈值时，采用基于图像位置的视觉伺服控制方法计算机械臂第二关节角速度，根据第二关节角速度对机器人进行控制，并返回步骤101。

步骤109：在相邻两次轴角误差的李雅普诺夫函数值差值小于或等于0，并且在基于图像特征的视觉伺服控制运行时间大于运行时间阈值时，采用基于图像位置的视觉伺服控制方法计算第二关节角速度，根据第二关节角速度对机器人进行控制，并返回步骤101。轴角误差为实际相机位姿的轴角与相机位姿的期望轴角之间的误差。

采用基于图像位置的视觉伺服控制方法计算机械臂第二关节角速度，具体包括：

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

2)根据归一化的像素坐标确定交互作用矩阵。

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

4)获取坐标系变换矩阵；坐标系变换矩阵为笛卡尔坐标系变换为相机坐标系的矩阵。

5)根据坐标系变换矩阵，将笛卡尔坐标系下的相机速度转换为相机坐标系下的相机速度。

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

根据如下公式计算第二关节角速度：

式中，

为相机在相机坐标系中的速度，T_i ^c为坐标系变换矩阵，

为笛卡尔坐标系下的相机速度，L_s为交互作用矩阵，

步骤110：结束控制。

具体的，为了实现良好的视觉伺服效果，增强机器人控制系统的稳定性，提高收敛速度，本发明提供一种基于图像位置和图像特征的视觉伺服切换控制方法。首先对机器人硬件建模分析，之后分别设计了两种不同的视觉伺服控制系统，并分析其各自存在的问题，最后设计切换控制律。本发明是在基于图像位置的视觉伺服和基于图像特征的视觉伺服两个子系统中进行切换。在满足约束的前提下设计子系统的控制器,当约束条件即将触发时切换到其他控制器，从而保证在整个控制过程中满足约束。该方法包含以下三部分：

第一，基于图像位置的视觉伺服控制系统。基于图像位置的视觉伺服的基本思想是利用图像，尤其是图像上的特征点，来计算机器人末端的位姿。在伺服过程中利用机器人当前位姿与所需位姿之间的误差进行控制，通过连续、快速地提取图像并计算姿态信息，并将其与给定位姿进行作差，形成闭环反馈。本发明使用二维码的四个角点作为特征点，用PnP算法求解末端相机的位姿用于控制。

基于图像位置的视觉伺服控制系统流程图如图2所示，将给定期望位姿

与估计位姿

作差，实现闭环反馈控制。PBVS控制这一模块的输入为位姿差值，输出为关节伺服电机的控制指令，控制指令可以是位置、速度或电流。将控制指令输入到关节控制器中，控制机器人各关节运动，并及时将编码器数据返回给关节控制器。在运动过程中，末端相机拍摄图像，通过特征提取得到图像特征坐标值f，进一步通过姿态估计得到机器人位姿估计值

主要实现步骤如下：

步骤一：使用二维码计算相机位姿。

将二维码角点检测与PnP位姿计算结合，得到相机的位姿，用于机器人的姿态估计。在这里，要获取二维码四个角点在图像中的像素坐标。由于之后要根据这四个坐标计算相机的位姿，因此这四个点在相机图像平面的坐标必须具有准确性、鲁棒性。得到相机位姿不只需要这四个点的坐标，还需要相机内参的标定。在这里使用棋盘格标定板与二维码定位识别算法进行标定，主要得到相机的焦距与相机的畸变参数，利用小孔成像的方法使现实中的三维点的坐标与相机像平面上的二维坐标产生联系。这句话的意思就是获取参数后利用PNP 计算相机位姿。现有技术通常使用对目标进行模板匹配，之后进行特征点提取、筛选的办法。这种办法由于光照强度的变化，无法得到准确、稳定的特征点，进而导致系统失稳。这里由于利用了二维码内部的编码信息，直接就对特征点可能出现的区域进行了一个准确的定位，因而精度高，稳定性好。使用的得到相机位姿的方法为EPnP算法，不同于传统PnP算法中的DLT算法，这种算法精度高，同时可扩展性好，即使对于点对数较多的PnP问题也依然有效。

步骤二：设计等效轴角控制器。

得到相机位姿后，就要设计等效轴角控制器以获取控制指令。采用基于笛卡尔空间的控制方法，将实际末端位姿与期望的任务空间位姿相比较，则可以得到任务空间当中的误差e，本发明采用的基于图像位置的视觉伺服控制系统控制结构如图3所示，其中x_d表示期望末端位置，

为期望末端速度，q_e为实际关节角度，x_e为实际末端位置。由于该方法需要将视觉定位包含在反馈环内部，因此每一个控制周期都需要进行PnP求解。涉及到的逆动力学算法如下：

根据机械臂关节转动角度与机械臂末端在任务空间中速度的关系：

式中，q为关节转角，

为关节角速度，

为末端速度，J₀为几何雅克比矩阵。

结合末端任务空间误差：

e_b＝x_d-x_e

式中，e_b为任务空间中末端位姿误差，根据当前位姿绕某一轴线旋转某一角度来刻画；

为任务空间中末端位姿误差对时间的导数。x_d为期望末端位姿，

为期望末端速度；x_e为当前末端位姿，

为当前末端速度。

为使系统镇定，取误差的动态微分方程满足

式中，e_b为任务空间中末端位姿误差，

为任务空间中末端位姿误差对时间的导数，K为增益矩阵，一般取对角正定阵。由以上公式可以得到关节速度输入：

式中，q为关节转角，

为关节角速度，

为几何雅克比矩阵的逆，

为期望末端速度，K为增益矩阵，e_b为任务空间中末端位姿误差。

根据等效轴角理论，得到的相关设计参数具有如下形式：

式中，q为关节转角，

为关节角速度，

为几何雅克比矩阵的逆，

为期望的末端平动速度，e_bp为期望位姿与实际位姿的位置误差矢量，K_p为对应e_bp的系数阵，ω_d为期望的末端旋转角速度，e_bo为期望位姿与实际位姿的方向误差矢量，K_o为对应e_bo的系数阵；S(·)为反对称阵，下标d表示该向量为期望姿态矩阵R_d的三列之一，下标e表示该向量为实际姿态矩阵R_e的三列之一。

第二，基于图像特征的视觉伺服控制系统。基于图像特征的视觉伺服是直接在图像空间中根据图像定义误差信号，直接映射到执行器的控制量上以控制末端运动，使图像特征收敛到期望特征。本发明采用六自由度的基于图像的视觉伺服控制器，可以完成三维笛卡尔空间中的任意运动。

基于图像特征的视觉伺服控制系统流程图如图4所示，将给定的期望图像特征坐标值f^*与实际图像特征坐标值f作差，实现闭环反馈控制。IBVS控制这一模块的输入为图像特征坐标差值，输出为关节伺服电机的控制指令，控制指令可以是位置、速度或电流。将控制指令输入到关节控制器中，控制机器人各关节运动，并及时将编码器数据返回给关节控制器。在运动过程中，末端相机拍摄图像，通过特征提取得到当前实际图像特征坐标值f。

本发明使用的基于图像特征的视觉伺服控制系统控制结构如图5所示，其中s_d为特征点在图像上的期望坐标值，s为特征点在图像上的实际坐标值，e_s为图像空间中图像特征点坐标误差，K_s为增益阵，

为图像雅克比矩阵的逆，z_c为在相机坐标系中的z轴坐标，x_c为在相机坐标系中的x轴坐标，q为关节角度， q′为关节角速度。主要实现步骤如下：

步骤一：获取二维码角点坐标。

构建共享内存，将二维码的角点信息存放在两层vector容器中，发送至共享内存供机器人控制程序使用。

步骤二：计算图像雅可比矩阵。

将图像像素归一化，代入计算得到图像雅可比矩阵，即交互作用矩阵。

图像像素坐标归一化：

设计如下的交互作用矩阵：

式中，s为特征点在图像平面的坐标，X,Y为该坐标具体值，z_c为点在相机坐标系中的z轴坐标。

步骤三：计算关节空间速度。

利用步骤二中交互作用矩阵建立的图像中特征点的速度与相机速度的联系，可以进一步求得关节空间速度。特征点在图像上的运动速度

和笛卡尔坐标系下相机速度

的联系可以用以下公式来表示：

式中，L_s是交互作用矩阵。

笛卡尔坐标系下的相机速度

可以由上式对L_s求伪逆得到：

式中，

为交互作用矩阵的转置矩阵，

表示对矩阵

与L_s的乘积求逆。

采用速度分解控制策略完成对图像特征的约束，设计比例控制器使图像特征点坐标误差收敛

式中，e_s为图像空间中图像特征点坐标误差，

为该误差对时间的导数， k为增益系数，

为特征点在图像上的运动速度。

结合以上两式，可得

式中，L_s为交互作用矩阵，e_s为图像空间中图像特征点坐标误差，K_s为增益系数矩阵。

根据相机坐标系中的速度和图像坐标系中计算的速度之间的关系式：

式中用到的变换矩阵T_i ^c如下：

将图像坐标系中的速度换算到了相机坐标系中。之后再用下式计算关节空间速度，输入机器人进行控制。

式中，q为关节角度，

为关节角速度，J^-1为雅克比矩阵的逆，R_c为旋转矩阵，

为相机在相机坐标系中的速度。

第三，切换控制律。为了消除基于图像位置的视觉伺服特征点容易游离到视野之外、基于图像特征的视觉伺服存在奇异点和相机后撤现象对视觉伺服性能的影响，本发明提出一种切换控制方法，利用位置误差和相平面误差的李雅普诺夫函数，并结合最短IBVS控制时间，在满足约束的前提下设计子系统的控制器，当约束条件即将触发时切换到其他控制器，从而保证在整个控制过程中满足约束，同时增加系统的稳定域，提高收敛速度。

整个视觉伺服切换控制算法如下：

步骤一：数据初始化。

步骤二：从共享内存中读取S_i,P_i计算D_i,LyPBVS,LyIBVS,Flag。

步骤三：根据Flag值确定此时是PBVS模式还是IBVS模式。

如果D_i≥D_imax或者LyIBVS≤IBVSLim，那么置Flag＝0，记录此时位置误差的李雅普诺夫函数LyPBVS与切换至IBVS模式后系统在该模式下运行时间的最小值IBVSTime，这里的位置误差是指当前表示位姿的轴角与期望轴角的差值；反之如果D_i≤D_imax,LyPBVS_now≤LyPBVS且IBVSTime＞TimeLim，那么Flag＝1。这里Flag代表子控制器的类型，0为基于图像特征的视觉伺服，1为基于图像位置的视觉伺服。其中，在LyPBVS_now≤LyPBVS中，LyPBVS_now表示当前的位置误差的李雅普诺夫函数值，LyPBVS表示前一次位置误差的李雅普诺夫函数值；TimeLim表示时间阈值。

步骤四：计算速度，并将对应的速度输入至驱动器；

步骤五：当LyIBVS≤γ·IBVSLim时结束视觉伺服过程，否则重复执行步骤二至步骤五。

上述步骤各个符号的说明如下所示：

S_i：第i个特征点图像坐标，可以用二维码信息和相机内参求解。其中横坐标记作S_ix，纵坐标记作S_iy。

P_i：此时相机的位姿，可以用EPnP算法求得。

D_i：第i个点的坐标与边界的距离，其中横坐标距离记作D_ix，纵坐标距离记作D_iy，单位是像素。

LyPBVS：位置误差的李雅普诺夫函数，这里的位置误差是指当前表示位姿的轴角与期望轴角的差值。计算公式为

e_p为实际位置到期望位置的平移旋转矢量，即误差矢量。

LyIBVS：相机图像平面实际坐标与期望坐标的误差的李雅普诺夫函数，计算公式为

e_i为图像空间中图像特征点实际坐标与期望坐标误差。

Flag：代表子控制器的类型，0为基于图像特征的视觉伺服，1为基于图像位置的视觉伺服。如果位置误差的李雅普诺夫函数的值大于阈值，那么就置Flag的值为0，否则将其置1。

D_imax：容许的特征点像素坐标距离图像边界的最大值。

IBVSLim：IBVS的阈值，当LyIBVS小于该值时，强制系统使用IBVS控制，以达到使图像特征收敛于期望值的效果。

γ：判断是否结束工作的系数，当LyIBVS小于它与IBVSLim相乘的值时，伺服过程结束。

IBVSTime：切换至IBVS模式后系统在该模式下运行时间的最小值。

图6为本发明实施例中视觉伺服切换控制系统结构图。如图6所示，一种视觉伺服切换控制系统，包括：

获取模块201，用于获取带有二维码的图像、图像特征点的期望坐标、相机位姿的期望轴角和相机参数。图像特征点为二维码的角点。

坐标确定模块202，用于根据图像确定图像特征点的实际坐标。

位姿确定模块203，用于根据图像和相机参数确定实际相机位姿。

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

第二判断模块205，用于在第一判断结果为否时，判断图像特征点的实际坐标与图像边界的距离是否大于边界阈值，得到第二判断结果。

第一关节角速度计算模块206，用于在第二判断结果为是时，采用基于图像特征的视觉伺服控制方法计算机械臂第一关节角速度，根据第一关节角速度对机器人进行控制，并执行获取模块201。

第一关节角速度计算模块206，具体包括：

位姿与速度获取单元，用于获取机械臂末端当前位姿、机械臂末端期望位姿和机械臂末端期望速度。

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

第一关节角速度计算单元，具体包括：

第一关节角速度计算子单元，用于根据如下公式计算第一关节角速度：

式中，q为关节转角，

为第一关节角速度，

为第一雅克比矩阵的逆，

第三判断模块207，用于在第二判断结果为否时，判断图像特征点的实际坐标与图像边界的距离是否等于边界阈值，得到第三判断结果。

关节角速度计算区分模块208，用于在第三判断结果为是时，在坐标误差的李雅普诺夫函数值小于或等于基于图像特征的视觉伺服控制阈值时采用基于图像特征的视觉伺服控制方法计算第一关节角速度，根据第一关节角速度对机器人进行控制，并执行获取模块201。或者在相邻两次轴角误差的李雅普诺夫函数值差值小于或等于0，并且在基于图像特征的视觉伺服控制运行时间大于运行时间阈值时，采用基于图像位置的视觉伺服控制方法计算机械臂第二关节角速度，根据第二关节角速度对机器人进行控制，并执行获取模块201。

第二关节角速度计算模块209，用于在第三判断结果为否时，在相邻两次轴角误差的李雅普诺夫函数值差值小于或等于0，并且在基于图像特征的视觉伺服控制运行时间大于运行时间阈值时，采用基于图像位置的视觉伺服控制方法计算第二关节角速度，根据第二关节角速度对机器人进行控制，并执行获取模块。

第二关节角速度计算模块209，具体包括：

归一化处理单元，用于对图像的像素坐标进行归一化处理，得到归一化的像素坐标。

交互作用矩阵确定单元，用于根据归一化的像素坐标确定交互作用矩阵；

第一相机速度确定单元，用于根据交互作用矩阵和坐标误差确定笛卡尔坐标系下的相机速度。

坐标系变换矩阵获取单元，用于获取坐标系变换矩阵；坐标系变换矩阵为笛卡尔坐标系变换为相机坐标系的矩阵。

第二相机速度确定单元，用于根据坐标系变换矩阵，将笛卡尔坐标系下的相机速度转换为相机坐标系下的相机速度。

第二关节角速度计算单元，用于根据相机坐标系下的相机速度计算第二关节角速度。

第二关节角速度计算单元，具体包括：

第二关节角速度计算子单元，用于根据如下公式计算第二关节角速度：

式中，

为相机在相机坐标系中的速度，T_i ^c为坐标系变换矩阵，

为笛卡尔坐标系下的相机速度，L_s为交互作用矩阵，

结束控制模块210，用于在第一判断结果为是时，结束控制。

对于实施例公开的系统而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。

本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处。综上，本说明书内容不应理解为对本发明的限制。