CN113146616B - 一种四自由度机械臂视觉伺服控制方法 - Google Patents

Abstract

本发明公开了一种四自由度机械臂视觉伺服控制方法，能够运动控制过程中低重力环境下柔性机械臂高精度定位控制问题，实现无人自主定位。该方法包括如下步骤：首先将机械臂的整臂模式设置为视觉伺服模式，在每个视觉伺服模式的控制周期内均判断视觉测量位姿数据是否有效，若连续多个周期无效，则机械臂停止运动，整臂模式转换为伺服待机模式，关节控制模式转换为位置伺服模式。在每个视觉伺服模式的控制周期内，若视觉测量位姿数据有效，关节控制模式处于速度控制模式，计算并输出规划的末端四维速度VW_POR；然后通过运动学逆解求得规划关节角速度和规划关节角位置，并作为控制指令进行输出，用于控制关节在下一控制周期的角速度和角位置。

Description

一种四自由度机械臂视觉伺服控制方法

技术领域

本发明涉及航天技术领域，具体涉及一种四自由度机械臂视觉伺服控制方法，用于低重力环境下柔性机械臂高精度定位控制工况。

背景技术

视觉伺服控制方法是机械臂运动控制器内的主要控制模式之一，用来实现机械臂针对合作目标的任务自主路径规划计算功能。它集成了机械臂运动学逆解、视觉伺服测量、轨迹规划控制等内容。

行星样品采集装置往往需要既能适应较大的采样范围，又要实现采集样品的精确转移，且整个装置有严格的重量限制，这使得采样装置往往采用由细长形状的臂杆、关节及末端采样器串联组成的机械臂。此种机械臂具有重量轻、驱动力小、耗能低、速度快等诸多优点。但因为机械臂柔性较大，尤其是当末端采样器较重或抓取样品容器时，机械臂在运动时会产生振动，且在行星重力作用下，机械臂末端位置会有较大的偏差。

因此，针对行星表面采样机械臂控制系统，如何提高其末端跟踪与定位精度是亟待解决的问题、

发明内容

有鉴于此，本发明提供了一种四自由度机械臂视觉伺服控制方法，能够运动控制过程中低重力环境下柔性机械臂高精度定位控制问题，实现无人自主定位。

为达到上述目的，本发明的技术方案为：一种四自由度机械臂视觉伺服控制方法，四自由度机械臂包含四个关节，机械臂的臂上设有臂上相机，机械臂的末端设有末端相机；针对四自由度机械臂的视觉伺服控制方法，具体为：

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

在每个视觉伺服模式的控制周期内，判断视觉测量位姿数据是否有效，若视觉测量位姿数据有效，选取有效的视觉测量位姿数据，整臂控制器下发关节控制模式切换指令，使得关节控制模式处于速度控制模式，采用如步骤(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)具体为：在机械臂压紧构型下测量得到：臂上相机的相机坐标系与末端坐标系的位置关系向量为p_por_vision0和姿态关系矩阵C_por_visio0n、以及末端相机的相机坐标系与末端坐标系的位置关系向量为p_por_vision1和姿态关系矩阵C_por_vision1

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

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

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

臂上相机的目标测量姿态矩阵为

C_vision0_data＝RPYzyx_C(vision0_data(4:6))

其中vision0_data为6维向量，前三位vision0_data(1:3)为臂上相机的目标测量位置数据，后三位vision0_data(4:6)为臂上相机的目标测量姿态数据；RPYzyx_C表示将坐标系姿态欧拉角转换为旋转矩阵；

末端相机的目标测量姿态矩阵为

C_vision1_data＝RPYzyx_C(vision1_data(4:6))

其中vision1_data为6维向量，前三位vision1_data(1:3)为末端相机的目标测量位置数据，后三位vision1_data(4:6)为末端相机的目标测量姿态数据；

当选择臂上相机应用工况时，臂上相机的视觉测量位姿数据vision0_data有效，则在末端坐标系下表示的目标容器测量位置向量P_ref_por和姿态旋转矩阵C_ref_por分别为

P_ref_por＝p_por_vision0+C_por_vision0·vision0_data(1:3)+C_por_vision0·C_vision0_data·[0 0 -ds]

C_ref_por＝C_por_vision0·C_vision0_data

其中ds为末端工具到目标的距离，即期望距离；

当选择末端相机应用工况时，如果末端相机的视觉测量位姿数据vision1_data有效，则在末端坐标系下表示的目标容器测量位置向量P_ref_por和姿态旋转矩阵C_ref_por分别为

P_ref_por＝p_por_vision1+C_por_vision1·vision1_data(1:3)+C_por_vision1·C_vision1_data·[0 0 -ds]

C_ref_por＝C_por_vision1·C_vision1_data

将目标测量姿态旋转矩阵转换为姿态向量E_ref_por＝C_RPYzyx(C_ref_por)；其中C_RPYzyx表示将将姿态旋转矩阵转化为姿态向量；

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

表示为

其中P_ref_por(1)、P_ref_por(2)、P_ref_por(3)分别为末端坐标系下表示的目标容器测量位置向量P_ref_por的第1～第3位数据，E_ref_por(1)为末端相机的目标测量姿态向量E_ref_por的第1位数据。

进一步地，步骤(4)，具体为：

末端偏差如下：

位置差为

姿态差为DoeE＝|EndPE_des(4)|

设定到位标识位CaptureCommand，初始机械臂末端未达到目标点时，则CaptureCommand初始值为0，规划此时的末端线速度为Vez：

其中AutoVel为期望末端速度，为输入值；

末端角速度为Wez：

当时，/>

当时，Wez＝-DoeE·AutoVel/DoeP

当时，Wez＝DoeE·AutoVel/DoeP

得到规划的末端四维速度VW_POR；

VW_POR(1:3)＝Vez；

VW_POR(4)＝Wez。

进一步地，判断末端偏差是否超出给定阈值，具体为：

设定位置差阈值DelDis，以及姿态差阈值DelAng，判断Abs(DoeP)＜DelDis且Abs(DoeE)＜DelAng，则到位标识位CaptureCommand置1，跳出视觉伺服模式，否则到位标识位CaptureCommand为0，继续执行视觉伺服模式；

进一步地，通过运动学逆解求得规划关节角速度和规划关节角位置，并作为控制指令进行输出，用于控制关节在下一控制周期的角速度和角位置，具体为：

将VW_POR的4个值经过一阶滤波器处理，分别得到平滑输出的线速度信号Vez和角速度信号Wez。

然后通过运动学逆解求得规划关节角速度其中Jacob_POR为雅克比矩阵；i为下一控制周期序号。

规划关节角位置为其中t₀为视觉伺服模式的控制周期；θ_i-1为当前控制周期的关节角位置，得到最终的关节控制指令进行输出。

有益效果：

本发明提供一种四自由度机械臂视觉伺服控制方法，解决运动控制过程中低重力环境下柔性机械臂高精度定位控制问题，实现无人自主定位。该方法基于位置的视觉伺服方案，即根据机械臂上安装的测量相机得到的相机图像，由目标的几何模型和相机模型估计出目标相对于相机的位置，得到当前机械臂的末端位姿和估计的目标位姿的误差，通过视觉相机和机械臂控制器进行调节。由于四自由度机械臂的特殊构型，采用三维位置矢量及末端执行器与机械臂根部所在平面的夹角来表示机械臂末端执行器的位姿。机械臂运动学逆解采用位置级逆解方法。

基于位置的视觉伺服通过图像进行三维重构，在三维笛卡尔空间计算误差。这种方法的优点在于误差信号和视觉伺服控制方法的输入信号都是空间位姿，实现起来比较容易。采用视觉伺服运动规划跟踪方式时，选择目标定位由粗到细、测量精度由低到高的工作模式。首先，通过安装在机械臂远端的臂上相机完成目标搜索和跟踪并进行初步定位，引导机械臂末端向目标方向运动；然后，当目标进入末端相机的有效视场后，使用末端相机对目标进行跟踪并测量目标位姿，引导机械臂末端进一步逼近目标；最后，完成对目标的精确定位，以引导末端执行器完成对目标的抓取等精度高、稳定性强的精准操作。解决了基座姿态不确定导致末端定位不准的问题，同时提高了任务自主性，有助于提高任务完成可靠性。

附图说明

图1为四自由度机械臂坐标系定义；

图2为行星作业机械臂应用工况图；

图3为视觉伺服原理图；

图4为视觉伺服模式流程图。

具体实施方式

下面结合附图并举实施例，对本发明进行详细描述。

本发明提供了一种四自由度机械臂视觉伺服控制方法，四自由度机械臂包含四个关节，机械臂的臂上设有臂上相机(例如在机械臂的第二个臂杆上设有臂上相机)，机械臂的末端设有末端相机，具体的星星作业机械臂应用工况如图2所示；针对四自由度机械臂的视觉伺服控制方法，具体为：

首先将机械臂的整臂模式设置为视觉伺服模式，在每个视觉伺服模式的控制周期内均判断视觉测量位姿数据是否有效，若视觉测量位姿数据连续多个周期无效(例如连续三个周期或以上，视觉测量位姿数据无效)，则机械臂停止运动，整臂模式转换为伺服待机模式，关节控制模式转换为位置伺服模式(初始默认关节控制模式均为位置伺服模式。)。

在每个视觉伺服模式的控制周期内，判断视觉测量位姿数据是否有效，其中分别由臂上相机和末端相机出获取视觉测量位姿数据，若其中臂上相机或末端相机其中一个采集得到视觉测量位姿数据，则视觉测量位姿数据有效，否则视觉测量位姿数据无效。

若视觉测量位姿数据有效，选取有效的视觉测量位姿数据；其中若臂上相机和末端相机的视觉测量位姿数据均有效，则优先取末端相机的视觉测量数据作为有效的视觉测量位姿数据。

整臂控制器下发关节控制模式切换指令，使得关节控制模式处于速度控制模式，采用如步骤(1)～步骤(5)的方式计算并输出规划的末端四维速度VW_POR；然后通过运动学逆解求得规划关节角速度和规划关节角位置，并作为控制指令进行输出，用于控制关节在下一控制周期的角速度和角位置。

本发明中视觉伺服的原理如图3所示，其中采用整臂控制器进行笛卡尔空间控制，输出关节控制器的控制指令，关节控制器控制机械臂对象从而调整末端位姿，臂上相机或者末端相机采集末端位姿的图像，送入视觉伺服控制器中，视觉伺服控制器首先对图像进行特征提取得到视觉测量位姿数据，然后执行步骤视觉伺服模式中的步骤(1)～步骤(5)进行笛卡尔空间位姿计算，输出控制反馈对给定位姿进行调整。

视觉伺服模式中的步骤(1)～步骤(5)如图4所示，具体为：

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

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

在机械臂压紧构型下测量得到：臂上相机的相机坐标系与末端坐标系的位置关系向量为p_por_vision0和姿态关系矩阵C_por_vision0、以及末端相机的相机坐标系与末端坐标系的位置关系向量为p_por_visi1o和姿态关系矩阵C_por_visi1o。其中坐标系的建立如图1所示。相机坐标系根据相机的视角进行定义，光轴方向是x轴，法线方向为z方向，根据右手定则确定y轴；末端坐标系独立的，末端工具方向为x轴，末端工具方向的法线为z轴，右手定则确定y轴

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

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

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

臂上相机的目标测量姿态矩阵为

C_vision0_data＝RPYzyx_C(vision0_data(4:6))

其中vision0_data为6维向量，前三位vision0_data(1:3)为臂上相机的目标测量位置数据，后三位vision0_data(4:6)为臂上相机的目标测量姿态数据；RPYzyx_C表示将坐标系姿态欧拉角转换为旋转矩阵；

末端相机的目标测量姿态矩阵为

C_vision1_data＝RPYzyx_C(vision1_data(4:6))

其中vision1_data为6维向量，前三位vision1_data(1:3)为末端相机的目标测量位置数据，后三位vision1_data(4:6)为末端相机的目标测量姿态数据；

当选择臂上相机应用工况时，臂上相机的视觉测量位姿数据vision0_data有效，则在末端坐标系下表示的目标容器测量位置向量P_ref_por和姿态旋转矩阵C_ref_por分别为

P_ref_por＝p_por_vision0+C_por_vision0·vision0_data(1:3)+C_por_vision0·C_vision0_data·[0 0 -ds]

C_ref_por＝C_por_vision0·C_vision0_data

其中ds为末端工具到目标的距离，即期望距离；

当选择末端相机应用工况时，如果末端相机的视觉测量位姿数据vision1_data有效，则在末端坐标系下表示的目标容器测量位置向量P_ref_por和姿态旋转矩阵C_ref_por分别为

P_ref_por＝p_por_vision1+C_por_vision1·vision1_data(1:3)+C_por_vision1·C_vision1_data·[0 0 -ds]

C_ref_por＝C_por_vision1·C_vision1_data

将目标测量姿态旋转矩阵转换为姿态向量E_ref_por＝C_RPYzyx(C_ref_por)；其中C_RPYzyx表示将将姿态旋转矩阵转化为姿态向量；

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

表示为

其中P_ref_por(1)、P_ref_por(2)、P_ref_por(3)分别为末端坐标系下表示的目标容器测量位置向量P_ref_por的第1～第3位数据，E_ref_por(1)为末端相机的目标测量姿态向量E_ref_por的第1位数据。

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

末端偏差如下：

位置差为

姿态差为DoeE＝|EndPE_des(4)|

设定到位标识位CaptureCommand，初始机械臂末端未达到目标点时，则CaptureCommand初始值为0，规划此时的末端线速度为Vez：

其中AutoVel为期望末端速度，为输入值；

末端角速度为Wez：

当时，/>

当时，Wez＝-DoeE·AutoVel/DoeP

当时，Wez＝DoeE·AutoVel/DoeP

得到规划的末端四维速度VW_POR；

VW_POR(1:3)＝Vez；

VW_POR(4)＝Wez。

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

设定位置差阈值DelDis，以及姿态差阈值DelAng，判断Abs(DoeP)＜DelDis且Abs(DoeE)＜DelAng，则到位标识位CaptureCommand置1，跳出视觉伺服模式，否则到位标识位CaptureCommand为0，继续执行视觉伺服模式；

将VW_POR的4个值经过一阶滤波器处理，分别得到平滑输出的线速度信号Vez和角速度信号Wez；

然后通过运动学逆解求得规划关节角速度其中Jacob_POR为雅克比矩阵；i为下一控制周期序号；

规划关节角位置为其中t₀为视觉伺服模式的控制周期；θ_i-1为当前控制周期的关节角位置，得到最终的关节控制指令进行输出。

综上所述，以上仅为本发明的较佳实施例而已，并非用于限定本发明的保护范围。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (9)

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。

6.如权利要求5所述的方法，其特征在于，所述步骤(3)将视觉测量位姿数据下的最终到位终止点位姿转换到末端坐标系下，得到末端坐标系下的最终到位终止运动点位姿EndPE_des，具体为

臂上相机的目标测量姿态矩阵为

C_vision0_data＝RPYzyx_C(vision0_data(4:6))

其中vision0_data为6维向量，前三位vision0_data(1:3)为臂上相机的目标测量位置数据，后三位vision0_data(4:6)为臂上相机的目标测量姿态数据；RPYzyx_C表示将坐标系姿态欧拉角转换为旋转矩阵；

末端相机的目标测量姿态矩阵为

C_vision1_data＝RPYzyx_C(vision1_data(4:6))

其中vision1_data为6维向量，前三位vision1_data(1:3)为末端相机的目标测量位置数据，后三位vision1_data(4:6)为末端相机的目标测量姿态数据；

当选择臂上相机应用工况时，臂上相机的视觉测量位姿数据vision0_data有效，则在末端坐标系下表示的目标容器测量位置向量P_ref_por和姿态旋转矩阵C_ref_por分别为

P_ref_por＝p_por_vision0+C_por_vision0·vision0_data(1:3)+C_por_vision0·C_vision0_data·[0 0 -ds]

C_ref_por＝C_por_vision0·C_vision0_data

其中ds为末端工具到目标的距离，即期望距离；

当选择末端相机应用工况时，如果末端相机的视觉测量位姿数据vision1_data有效，则在末端坐标系下表示的目标容器测量位置向量P_ref_por和姿态旋转矩阵C_ref_por分别为

P_ref_por＝p_por_vision1+C_por_vision1·vision1_data(1:3)+C_por_vision1·C_vision1_data·[0 0 -ds]

C_ref_por＝C_por_vision1·C_vision1_data

将目标测量姿态旋转矩阵转换为姿态向量E_ref_por＝C_RPYzyx(C_ref_por)；其中C_RPYzyx表示将将姿态旋转矩阵转化为姿态向量；

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

表示为

其中P_ref_por(1)、P_ref_por(2)、P_ref_por(3)分别为末端坐标系下表示的目标容器测量位置向量P_ref_por的第1～第3位数据，E_ref_por(1)为末端相机的目标测量姿态向量E_ref_por的第1位数据。

7.如权利要求6所述的方法，其特征在于，所述步骤(4)，具体为：

末端偏差如下：

位置差为

姿态差为DoeE＝|EndPE_des(4)|

设定到位标识位CaptureCommand，初始机械臂末端未达到目标点时，则CaptureCommand初始值为0，规划此时的末端线速度为Vez：

其中AutoVel为期望末端速度，为输入值；

末端角速度为Wez：

当时，/>

当时，Wez＝-DoeE·AutoVel/DoeP

当时，Wez＝DoeE·AutoVel/DoeP

得到规划的末端四维速度VW_POR；

VW_POR(1:3)＝Vez；

VW_POR(4)＝Wez。

8.如权利要求7所述的方法，其特征在于，所述判断末端偏差是否超出给定阈值，具体为：

设定位置差阈值DelDis，以及姿态差阈值DelAng，判断Abs(DoeP)＜DelDis且Abs(DoeE)＜DelAng，则到位标识位CaptureCommand置1，跳出视觉伺服模式，否则到位标识位CaptureCommand为0，继续执行视觉伺服模式。

9.如权利要求8所述的方法，其特征在于，所述通过运动学逆解求得规划关节角速度和规划关节角位置，并作为控制指令进行输出，用于控制关节在下一控制周期的角速度和角位置，具体为：

将VW_POR的4个值经过一阶滤波器处理，分别得到平滑输出的线速度信号Vez和角速度信号Wez；

然后通过运动学逆解求得规划关节角速度其中Jacob_POR为雅克比矩阵；i为下一控制周期序号；

规划关节角位置为其中t₀为视觉伺服模式的控制周期；θ_i-1为当前控制周期的关节角位置，得到最终的关节控制指令进行输出。