基于绳驱多关节冗余驱动机械臂的视觉伺服补偿控制方法

摘要

本发明公开了一种基于绳驱多关节冗余驱动机械臂的视觉伺服控制方法。建带有ARUCO码的视觉伺服硬件系统，利用视觉伺服硬件系统采集绳驱多关节冗余驱动机械臂的姿态图像；对姿态图像，采用图像处理方法解算出绳驱多关节冗余驱动机械臂中每个绳驱关节两个方向的转动角度作为绳驱关节的关节角度；根据第二步解算出的机械臂的关节角度，建立视觉伺服反馈的控制器，作为运动学控制的外环，进行视觉伺服补偿控制。本发明将视觉反馈信息与基于机械臂运动学的控制相结合，实现了绳驱机械臂关节角度的实时反馈控制，提高了绳驱机械臂的末端控制精度。

权利要求

1.一种基于绳驱多关节冗余驱动机械臂的视觉伺服补偿控制方法，其特征在于，方法包括以下步骤：

第一步：搭建带有ARUCO码的视觉伺服硬件系统，利用视觉伺服硬件系统采集绳驱多关节冗余驱动机械臂的姿态图像；

第二步：对姿态图像，采用图像处理方法解算出绳驱多关节冗余驱动机械臂中每个绳驱关节两个方向的转动角度Φi、θi作为绳驱关节的关节角度；

所述的第二步，具体为：

2.1)对采集的姿态图像进行自适应二值化，提取轮廓，提取获得ARUCO码的图像区域，根据ARUCO码所在的图像区域分析处理获得ARUCO码所在平面在空间中的位置，进而得到ARUCO码所在平面相对于相机坐标系的三个轴的夹角；

2.2)将ARUCO码的图像区域平均分割成4*4方格，统计每个方格中黑白色块的个数，然后进行以下判断：

若方格中黑色块的数量大于白色块的数量，则该方格为黑色，赋值为1；若方格中黑色块的数量小于等于白色块的数量，则该方格为白色，赋值为0；

以方格为基本单元将ARUCO码重新编码形成二进制码；

2.3)将二进制码和ARUCO字典对比得到此ARUCO码在ARUCO字典中的编号，利用opencv工具中与ARUCO码在ARUCO字典中编号对应的子库函数输入2.1)获得的三个轴的夹角解算出相机坐标系下每个关节筒的1*3大小的旋转矢量和1*3大小的平移矢量；

2.4)然后利用旋转矢量和平移矢量，得到相机坐标系下的4*4大小的坐标转换矩阵T[i]，进而利用坐标转换矩阵T[i]采用以下公式获得每一关节筒相对前一关节筒的姿态：Ti＝T[i]/T[i‑1]

其中，T[i]表示第i个关节筒相对于相机坐标系的坐标转换矩阵，坐标转换矩阵的大小是4*4，Ti表示第i个关节筒相对于第i‑1个关节筒的姿态矩阵，i表示关节筒的序数；

2.5)取姿态矩阵Ti中左上角3*3大小的部分作为参考矩阵Ri，表示为：根据上述的关系式解算θi和Φi：

其中，θi和φi分别为第i个关节筒两端的绳驱关节中更靠近基座的绳驱关节的两个转动角度，θi为第i‑1个关节筒与第i个关节的万向结铰接的转动方向，φi为第i个关节筒与第i个关节的万向结铰接的转动方向；其中Ri(m,n)为上述参考矩阵Ri的第m行第n列的元素，m,n＝1,2,3；

第三步：根据第二步解算出的机械臂的关节角度，建立视觉伺服反馈的控制器，作为运动学控制的外环，进行视觉伺服补偿控制。

2.根据权利要求1所述的一种基于绳驱多关节冗余驱动机械臂的视觉伺服补偿控制方法，其特征在于：所述第一步具体为：视觉伺服硬件系统包括安装在绳驱多关节冗余驱动机械臂的每个关节筒上的ARUCO码和全局相机，在绳驱多关节冗余驱动机械臂的每个关节筒安装上ARUCO码，不同的关节筒安装不同的ARUCO码，第i个关节筒安装上编号为i的ARUCO码，各个关节筒的ARUCO码布置于同一侧，在面对ARUCO码的方向放置全局相机，全局相机实时采集绳驱多关节冗余驱动机械臂的姿态图像，姿态图像中包含覆盖有所有关节筒上的ARUCO码。

3.根据权利要求1所述的一种基于绳驱多关节冗余驱动机械臂的视觉伺服补偿控制方法，其特征在于：所述的第三步中，根据机械臂的关节角度，建立以下公式的视觉伺服反馈的PI控制器：

T

u＝[u1,u2,...u3N]

e＝qd‑q

T

q＝[θ1,φ1,θ2,φ2,...θN,φN]其中，u为电机输入脉冲，u1,u2,...u3N为3N根绳的电机输入脉冲，e为关节变量的误差，q为关节变量，t表示时间， 表示关节变量q和电机输入脉冲u之间的转化矩阵；

kp表示控制器的比例系数，qd表示输入给定的关节变量，ki表示控制器的积分系数， 表示第i个关节的第j根绳在第i个关节筒上的穿线孔与第i个关节筒轴线的连线，和第i个关节筒的后端面参考线的夹角， 表示第i个关节的第j根绳的绳长。

4.根据权利要求1所述的一种基于绳驱多关节冗余驱动机械臂的视觉伺服补偿控制方法，其特征在于：所述绳驱多关节冗余驱动机械臂，主要由基座和机械臂关节部分构成；机械臂关节部分主要由N个关节筒通过绳驱关节串联构成，N个关节筒通过绳驱关节串联后再通过绳驱关节连接到基座；将各个关节筒及其两端的绳驱关节中更靠近基座的绳驱关节均按顺序编号为i，i＝1,2,3,...,N，每个绳驱关节包括三根绳和一个万向结，每根绳连接在相邻关节筒之间，其中编号为i的关节的每根绳编号为j，选取每个关节筒靠近基座的端面上的任意一条确定的直径线作为第i个关节筒的后端面参考线；相邻关节筒端面之间通过万向结铰接，每个绳驱关节具有两个方向转动角度Φi、θi，两个方向转动角度Φi、θi所在的转动平面相互垂直且均垂直于关节筒截面，关节筒截面垂直于关节筒轴向所在的平面；N个绳驱关节共具有3N根绳进行控制，每根绳子连接电机端部，由一个电机控制伸缩，3N个电机均在基座后布置。

说明书

基于绳驱多关节冗余驱动机械臂的视觉伺服补偿控制方法

技术领域

[0001] 本发明属于绳驱多关节冗余驱动机械臂的控制领域的一种机械臂控制方法，具体来说是涉及了一种基于绳驱多关节冗余驱动机械臂的视觉伺服补偿控制方法。

背景技术

[0002] 在海洋勘探与资源开发、管网检测、微创手术、灾害救援、石油化工、核辐射设备检修等方面，必然涉及复杂管网、狭窄深腔等空间受限环境下的作业任务，由于作业空间狭
小，不适合人或大型装备开展工作。绳驱机械臂因其姿态灵活，特别适合此类运动空间受限
环境下的作业要求。为满足复杂作业条件下机械臂较高的冗余度，绳躯机械臂的关节数一
般较多，且由于机械臂上无关节传感器，若单纯依靠以磁栅传感器反馈绳长的运动学控制
将不可避免的会出现机械臂末端定位控制精度不佳的问题，会使得机械臂的作业效果大打
折扣。因此需要在运动学控制的基础上增加视觉伺服控制进行补偿，以提高控制精度，增强
机械臂的作业性能。虽然国内外科研机构针对机械臂的视觉伺服控制已经取得了一定研究
成果，但大多数研究是基于传统的工业机械臂，极少有专门针对绳驱多冗余自由度机械臂
的视觉伺服控制。仅有的研究也只是局限于运动学分析和视觉伺服方法的应用，并未发挥
绳驱冗余自由度机械臂的优势。

[0003] 发明目的

[0004] 针对绳驱多关节冗余驱动机械臂，在现有的运动学建模，以及以磁栅传感器反馈绳长构建的运动学控制器的基础上，增加视觉伺服反馈系统，反馈机械臂姿态信息，并设计
基于视觉伺服反馈的控制器作为运动学控制器的补偿，来显著提高机械臂的末端定位控制
精度，解决了绳驱机械臂由于关节数增加引起的的末端定位控制不精的问题，使得机械臂
具有良好的的避障能力。

[0005] 本发明技术方案具体如下：

[0006] 第一步：搭建带有ARUCO码的视觉伺服硬件系统，利用视觉伺服硬件系统采集绳驱多关节冗余驱动机械臂的姿态图像；

[0007] 第二步：对姿态图像，采用图像处理方法解算出绳驱多关节冗余驱动机械臂中每个绳驱关节两个方向的转动角度Φi、θi作为绳驱关节的关节角度；

[0008] 第三步：根据第二步解算出的机械臂的关节角度，建立视觉伺服反馈的控制器，作为运动学控制的外环，进行视觉伺服补偿控制，显著提高了机械臂的末端控制精度。

[0009] 所述第一步具体为：

[0010] 视觉伺服硬件系统包括安装在绳驱多关节冗余驱动机械臂的每个关节筒上的ARUCO码和全局相机，在绳驱多关节冗余驱动机械臂的每个关节筒安装上ARUCO码，不同的
关节筒安装不同的ARUCO码，第i个关节筒安装上编号为i的ARUCO码，各个关节筒的ARUCO码
布置于绳驱多关节冗余驱动机械臂的同一侧，在面对ARUCO码的方向放置全局相机，全局相
机实时采集绳驱多关节冗余驱动机械臂的姿态图像，姿态图像中包含覆盖有所有关节筒上
的ARUCO码。

[0011] 所述的第二步，ARUCO码检测以及图像处理步骤，具体为：

[0012] 2.1)对采集的姿态图像进行自适应二值化，提取轮廓，提取获得每个ARUCO码的图像区域，根据ARUCO码所在的图像区域分析处理获得ARUCO码所在平面在空间中的位置，进
而得到ARUCO码所在平面相对于相机坐标系的三个轴的夹角；相机坐标系是以全局相机而
建立的笛卡尔坐标系。

[0013] ARUCO码是包含有黑色块和白色块。

[0014] 2.2)将每个ARUCO码的图像区域平均分割成4*4方格，统计每个方格中黑白色块的个数，然后进行以下判断：

[0015] 若方格中黑色块的数量大于白色块的数量，则该方格为黑色，赋值为1；若方格中黑色块的数量小于等于白色块的数量，则该方格为白色，赋值为0；

[0016] 以方格为基本单元将ARUCO码重新编码形成二进制码，实现特征提取；

[0017] 2.3)将二进制码和ARUCO字典对比得到此ARUCO码在ARUCO字典中的编号，利用opencv工具中与ARUCO码在ARUCO字典中编号对应的子库函数输入2.1)获得的三个轴的夹
角解算出相机坐标系下每个关节筒的1*3大小的旋转矢量和1*3大小的平移矢量；

[0018] 所述的opencv工具中自带ARUCO字典的库函数，库函数中包含了对于不同二进制码的子库。

[0019] 2.4)然后利用旋转矢量和平移矢量，得到相机坐标系下的4*4大小的坐标转换矩阵T[i]，进而利用坐标转换矩阵T[i]采用以下公式获得每一关节筒相对前一关节筒的姿
态：

[0020] Ti＝T[i]/T[i‑1]

[0021] 其中，T[i]表示第i个关节筒相对于相机坐标系的坐标转换矩阵，坐标转换矩阵的大小是4*4，Ti表示第i个关节筒相对于第i‑1个关节筒的姿态矩阵，i表示关节筒的序数；

[0022] 对于第1个关节筒，当全局相机和基座相对位置不变的情况下，以事先测量出基座相对全局相机的姿态作为坐标转换矩阵T[0]带入作为初始值进行运算。

[0023] 2.5)取姿态矩阵Ti中左上角旋转部分的3*3大小的部分作为参考矩阵Ri，表示为：

[0024]

[0025] 根据上述的关系式解算θi和Φi：

[0026]

[0027]

[0028] 其中，θi和φi分别为第i个关节筒两端的绳驱关节中更靠近基座的绳驱关节的两个转动角度，θi为第i‑1个关节筒与第i个关节的万向结铰接的转动方向，φi为第i个关节筒
与第i个关节的万向结铰接的转动方向；其中Ri(m,n)为上述参考矩阵Ri的第m行第n列的元
素，m,n＝1,2,3,例如；Ri(3,1)为上述参考矩阵Ri的第3行第1列的元素。

[0029] 所述的第三步中，根据机械臂的关节角度，建立以下公式的视觉伺服反馈的PI控制器：

[0030]

[0031] u＝[u1,u2,...u3N]T

[0032] e＝qd‑q

[0033]

[0034] 其中，u为针对关节筒数为N的情况，3N根绳对应的电机输入脉冲，u1,u2,...u3N为3N根绳的电机输入脉冲，e为关节变量的误差，q为关节变量，t表示时间， 表
示关节变量q和电机输入脉冲u之间的转化矩阵；kp表示控制器的比例系数，qd表示输入给定
的关节变量，ki表示控制器的积分系数， 表示第i个关节的第j根绳在第i个关节筒上的穿
线孔与第i个关节筒轴线的连线，和第i个关节筒的后端面参考线的夹角。 表示第i个关节
的第j根绳的绳长。

[0035] 本发明对象针对的所述绳驱多关节冗余驱动机械臂，其结构如图1所示，主要由基座和机械臂关节部分构成；

[0036] 机械臂关节部分主要由N个关节筒通过绳驱关节串联构成，N个关节筒通过绳驱关节串联后再通过绳驱关节连接到基座，使得相邻关节筒之间通过绳驱关节连接；将各个关
节筒及其两端的绳驱关节中更靠近基座的绳驱关节均按顺序编号为i，i＝1,2,3,...,N，每
个绳驱关节如图2所示，包括三根绳和一个万向结，每根绳连接在相邻关节筒之间，其中编
号为i的关节的每根绳编号为j，j＝3(i‑1)+1,2,3，选取每个关节筒靠近基座的端面上的任
意一条确定的直径线作为第i个关节筒的后端面参考线；三根绳沿圆周均布，每根绳均连接
一个电机，每根绳穿过相邻关节筒之间中更靠近基座的一个关节筒后连接到更远离基座的
一个关节筒端面，相邻关节筒端面之间通过万向结铰接，关节筒由三根绳驱动通过一个万
向结连接，每个绳驱关节具有两个方向转动角度Φi、θi，两个方向转动角度Φi、θi所在的
转动平面相互垂直且均垂直于关节筒截面，关节筒截面垂直于关节筒轴向所在的平面；N个
绳驱关节共具有3N根绳进行控制，每根绳子连接电机端部，由一个电机控制伸缩，3N个电机
均在基座后布置。

[0037] 本发明通过步骤1，搭建了视觉伺服硬件系统。给机械臂第i个关节筒，安装上编号为i的ARUCO码。在机械臂侧边面对ARUCO码的方向，放置全局相机实时采集机械臂的姿态图
像。通过步骤2，对全局相机采集的装上ARUCO码的绳驱机械臂的图像，编写图像处理算法程
序，解算出每个关节的角度Φi、θi。最后通过步骤3，根据步骤2中解算出的机械臂的每个关
节Φi、θi，设计视觉伺服反馈控制器，设置控制频率为30帧/s，作为运动学控制的补偿，显
著提高了机械臂末端的控制精度。

[0038] 与现有技术相比，本发明具有如下有益效果：

[0039] 1、本发明基于绳驱多关节冗余驱动机械臂，通过将视觉伺服反馈与运动学控制相结合，针对绳驱机械臂的结构特点设计了控制器。相较于绳驱机械臂单纯的运动学控制，解
决了由于绳驱机械臂关节数较多冗余度较高所导致的末端定位控制精度不高的问题。

[0040] 2、由于工作条件和结构限制，绳驱机械臂只使用磁栅传感器反馈绳长，通过正运动学求解推算出实时的关节角度和机械臂姿态。而本发明利用全局相机和ARUCO码，可以直
接实时反馈机械臂姿态和关节角度，使得机械臂具有了一定的避障能力。

附图说明

[0041] 图1是本发明基于的对象绳驱多关节冗余驱动机械臂整体结构示意图。

[0042] 图2是基于的对象绳驱多关节冗余驱动机械臂的绳驱关节连接的示意图。

[0043] 图3是本发明的基于视觉伺服的角度控制器设计框图。

[0044] 图4是本发明五关节绳驱机械臂基于视觉伺服的运动实验结果图，其中图(a)～(j)分别是每个关节输入的期望角度Φid、θid和实际角度Φi、θi(i＝1,2，3,4,5)，随时间t
的变化曲线图。

具体实施方式

[0045] 为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅用以解释本发明，并不
用于限定本发明。此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼
此之间未构成冲突就可以相互组合。

[0046] 本发明的具体实施例及其实施情况过程如下：

[0047] 第一步：搭建带有ARUCO码的视觉伺服硬件系统，利用视觉伺服硬件系统采集绳驱多关节冗余驱动机械臂的姿态图像；

[0048] 视觉伺服硬件系统包括安装在绳驱多关节冗余驱动机械臂的每个关节筒上的ARUCO码和全局相机，在绳驱多关节冗余驱动机械臂的每个关节筒安装上ARUCO码，不同的
关节筒安装不同的ARUCO码，第i个关节筒安装上编号为i的ARUCO码，各个关节筒的ARUCO码
布置于绳驱多关节冗余驱动机械臂的同一侧，在面对ARUCO码的方向放置全局相机，全局相
机实时采集绳驱多关节冗余驱动机械臂的姿态图像，姿态图像中包含覆盖有所有关节筒上
的ARUCO码。

[0049] 第二步：对姿态图像，采用图像处理方法解算出绳驱多关节冗余驱动机械臂中每个绳驱关节两个方向的转动角度Φi、θi作为绳驱关节的关节角度；

[0050] 2.1)对采集的姿态图像进行自适应二值化，提取轮廓，提取获得每个ARUCO码的图像区域，根据ARUCO码所在的图像区域分析处理获得ARUCO码所在平面在空间中的位置，进
而得到ARUCO码所在平面相对于相机坐标系的三个轴的夹角；相机坐标系是以全局相机而
建立的笛卡尔坐标系。

[0051] ARUCO码是包含有黑色块和白色块。

[0052] 2.2)将每个ARUCO码的图像区域平均分割成4*4方格，统计每个方格中黑白色块的个数，然后进行以下判断：

[0053] 若方格中黑色块的数量大于白色块的数量，则该方格为黑色，赋值为1；若方格中黑色块的数量小于等于白色块的数量，则该方格为白色，赋值为0；

[0054] 以方格为基本单元将ARUCO码重新编码形成二进制码，实现特征提取；

[0055] 2.3)将二进制码和ARUCO字典对比得到此ARUCO码在ARUCO字典中的编号，利用opencv工具中与ARUCO码在ARUCO字典中编号对应的子库函数输入2.1)获得的三个轴的夹
角解算出相机坐标系下每个关节筒的1*3大小的旋转矢量和1*3大小的平移矢量；

[0056] opencv工具中自带ARUCO字典的库函数，库函数中包含了对于不同二进制码的子库。

[0057] 2.4)然后利用旋转矢量和平移矢量，得到相机坐标系下的4*4大小的坐标转换矩阵T[i]，进而利用坐标转换矩阵T[i]采用以下公式获得每一关节筒相对前一关节筒的姿
态：

[0058] Ti＝T[i]/T[i‑1]

[0059] 其中，T[i]表示第i个关节筒相对于相机坐标系的坐标转换矩阵，坐标转换矩阵的大小是4*4，Ti表示第i个关节筒相对于第i‑1个关节筒的姿态矩阵，i表示关节筒的序数；

[0060] 对于第1个关节筒，当全局相机和基座相对位置不变的情况下，以事先测量出基座相对全局相机的姿态作为坐标转换矩阵T[0]带入作为初始值进行运算。

[0061] 2.5)取姿态矩阵Ti中左上角旋转部分的3*3大小的部分作为参考矩阵Ri，表示为：

[0062]

[0063] 根据上述的关系式解算θi和Φi：

[0064]

[0065]

[0066] 其中，θi和φi分别为第i个关节筒两端的绳驱关节中更靠近基座的绳驱关节的两个转动角度，θi为第i‑1个关节筒与第i个关节的万向结铰接的转动方向，φi为第i个关节筒
与第i个关节的万向结铰接的转动方向。其中Ri(m,n)(m,n＝1,2,3)为上述参考矩阵Ri的第m
行第n列的元素。Ri(3,1)为上述参考矩阵Ri的第3行第1列的元素。

[0067] 第三步：根据第二步解算出的机械臂的关节角度，建立视觉伺服反馈的控制器，作为运动学控制的外环，进行视觉伺服补偿控制，显著提高了机械臂的末端控制精度。

[0068] 具体根据机械臂的关节角度，建立以下公式的视觉伺服反馈的控制器：

[0069]

[0070] u＝[u1,u2,...u3N]T

[0071] e＝qd‑q

[0072]

[0073] 其中，u为针对关节筒数为N的情况，3N根绳对应的电机输入脉冲，u1,u2,...u3N为3N根绳的电机输入脉冲，e为关节变量的误差，q为关节变量，t表示时间， 表示
关节变量q和电机输入脉冲u之间的转化矩阵；kp表示控制器的比例系数，qd表示输入给定的
关节变量，ki表示控制器的积分系数， 表示第i个关节的第j根绳在第i个关节筒上的穿线
孔与第i个关节筒轴线的连线，和第i个关节筒的后端面参考线的夹角。 表示第i个关节的
第j根绳的绳长。

[0074] 本发明对象针对的绳驱多关节冗余驱动机械臂，其结构如图1所示，主要由基座和机械臂关节部分构成；

[0075] 机械臂关节部分主要由N个关节筒通过绳驱关节串联构成，N个关节筒通过绳驱关节串联后再通过绳驱关节连接到基座，使得相邻关节筒之间通过绳驱关节连接；将各个关
节筒及其两端的绳驱关节中更靠近基座的绳驱关节均按顺序编号为i，i＝1,2,3,...,N，每
个绳驱关节如图2所示，包括三根绳和一个万向结，每根绳连接在相邻关节筒之间，其中编
号为i的关节的每根绳编号为j，j＝3(i‑1)+1,2,3，选取每个关节筒靠近基座的端面上的任
意一条确定的直径线作为第i个关节筒的后端面参考线；三根绳沿圆周均布，每根绳均连接
一个电机，每根绳穿过相邻关节筒之间中更靠近基座的一个关节筒后连接到更远离基座的
一个关节筒端面，相邻关节筒端面之间通过万向结铰接，关节筒由三根绳驱动通过一个万
向结连接，每个绳驱关节具有两个方向转动角度Φi、θi，两个方向转动角度Φi、θi所在的
转动平面相互垂直且均垂直于关节筒截面，关节筒截面垂直于关节筒轴向所在的平面；N个
绳驱关节共具有3N根绳进行控制，每根绳子连接电机端部，由一个电机控制伸缩，3N个电机
均在基座后布置。

[0076] 最后，进行五关节绳驱机械臂基于视觉伺服的运动实验，通过验证每个关节角度控制精度，来体现机械臂末端定位精度的控制效果。

[0077] 在本实验中，给每一个关节输入随时间变化的关节角度Φid、θid，输入到控制器中进行运算。观察机械臂的运动效果，并用角度传感器实时采集每个关节的角度Φi、θi(i
＝1，2,3，4,5)，绘制出关节角度随时间的变化图。如图4所示，观察每个关节角度的控制误
差。

[0078] 实时采集实验过程各个关节的角度，如图4所示，最后得到加入视觉伺服后的角度控制误差基本在0.1rad以内，相对于单纯依靠运动学控制的2rad左右控制精度得到了显著
地提高。

[0079] 由此实验可见，本发明是针对了绳驱机械臂多关节和冗余驱动的特点，利用全局相机和ARUCO码给绳驱机械臂搭建了视觉伺服反馈系统，进行机械臂关节部分视觉信息的
测量，将视觉反馈信息与基于机械臂运动学的控制相结合，实现了绳驱机械臂关节角度的
实时反馈控制，显著提高了绳驱机械臂的末端控制精度。

[0080] 以上内容仅为本发明的技术思想，不能以此限定本发明的保护范围，凡是按照本发明提出的技术思想，在技术方案基础上所做的任何改动，均落入本发明权利要求书的保
护范围之内。