CN113172659B - 基于等效中心点识别的柔性机器人臂形测量方法及系统 - Google Patents

Abstract

本发明涉及基于等效中心点识别的柔性机器人臂形测量方法和系统。其方法主要包括：通过固定于刚性机器人上的双目相机采集所述的柔性机器人的臂段的完整侧面图像；根据双目相机所采集的双目图像中分别检测的臂段，识别和拟合臂杆轮廓，求解相应的臂段中心的图像像素坐标；根据人机交互设备的触发信息，确定左右目图像中同一臂段在两图像中的对应中心点；解算双目图像的两中心点像素坐标所对应的臂杆中心在双目相机坐标系中的位姿；通过运动学算法，确定所述的双目相机当前相对于刚性机械臂基座的位姿，并且根据所述的刚性机械臂相对于所述的柔性机器人的安装位置关系，将解算的位姿换算为臂段中的臂杆中心在柔性机器人基座坐标系中的位姿。

Description

基于等效中心点识别的柔性机器人臂形测量方法及系统

技术领域

本发明属于机器人技术领域，一般涉及用于绳驱柔性机器人臂形测量方法和系统。本发明尤其涉及基于等效中心点识别的柔性机器人臂形测量方法及系统。

背景技术

灾害救援、环境探测、设备检修及维护等应用场景，具有作业空间狭小、障碍物多等特点。传统机器人难以在这些场景完成任务，而运动灵活且体型纤细的绳驱柔性机械臂却能够发挥重要作用。绳驱柔性机器人主要通过绳索实现驱动，然而因绳索驱动过程中存在伸缩及与接触部位的摩擦阻力等，绳驱柔性机器人的运动学及动力学方程较为复杂并难以准确反映其实际的特性，这导致绳驱柔性机器人在执行任务的过程中，时常难以准确地到达预期给定的位置。

为此，有必要借助视觉等外部传感方式来获得绳驱柔性机器人各部分的位置和姿态，从而实现运动过程的视觉闭环反馈，以便纠正绳索误差及模型计算不准确导致的偏差，使得绳驱柔性机器人的运动更加准确。目前采用的测量方法主要是在绳驱柔性机器人的臂杆上安装二维码等典型特征，对于二维平面运动的臂杆测量也可以添加标志球。但这些方式都需要在绳驱柔性机器人上额外安装辅助测量的典型特征，安装精度难以保证，且会对绳驱柔性机器人本身的运动性能有一定影响，且在一些工作场合额外安装典型特征难度较大。因此，有必要研究绳驱柔性机器人上本身具备的自然特征的视觉测量方法，从而能简单方便地测量绳驱柔性机器人的臂形。

发明内容

本发明提供基于等效中心点识别的柔性机器人臂形测量方法及系统，旨在至少解决现有技术中存在的技术问题之一。本发明的方案利用柔性机器人机械臂本身具备的自然特征，针对臂杆工装与柔性机器人机械臂其它部分和环境的颜色和反光性能不同，提取臂杆的轮廓，拟合臂杆中心点，通过对双目相机拍摄柔性机器人机械臂图片中臂杆中心点的提取，求解臂杆中心点的位置，进而得到柔性机器人机械臂的臂杆相对于相机的位姿。本发明的方法对臂杆轮廓拟合精度要求低，对臂杆形状适应性强，可用于矩形侧面臂杆或圆柱体臂杆等，该方法对相机与臂杆间距离要求低，随距离增加识别精度降低较小。同时还间接提出了刚性臂辅助测量柔性机器人机械臂的臂形的方法，通过刚性臂带动末端手眼相机移动，调整相机观察柔性机器人机械臂的角度，能够极大程度的减少臂杆间相互遮挡等带来的问题，使臂杆侧面在图像中更加完整，提高识别精度。

本发明的技术方案涉及一种基于等效中心点识别的柔性机器人臂形测量方法，所用于的柔性机器人包括多个臂段，每个臂段包括多个的臂杆，臂杆之间通过万向节串联。所述方法包括以下步骤：

A、通过固定于刚性机器人上的且空间位姿可调的双目相机采集所述的柔性机器人的至少一个臂段的完整侧面图像；

B、根据双目相机所采集的双目图像中分别检测的一个或多个臂段，识别和拟合臂杆轮廓，求解相应的臂段的中心的图像像素坐标；

C、根据人机交互设备的触发信息，确定左目图像和右目图像中同一臂段在两图像中的对应中心点；

D、通过最小二乘法解算左目图像和右目图像的两中心点像素坐标所对应的臂杆中心在双目相机坐标系中的位姿；

E、根据当前刚性机械臂的关节角、机械臂杆长尺寸和所述的双目相机的安装尺寸，通过运动学算法，确定所述的双目相机当前相对于刚性机械臂基座的位姿，并且根据所述的刚性机械臂相对于所述的柔性机器人的安装位置关系，将所述步骤D中解算的位姿换算为臂段中的至少一个臂杆中心在柔性机器人基座坐标系中的位姿；

F、如果检测到还有柔性机器人的臂段在柔性机器人基座坐标系中的位置未被测量，至少返回执行所述的步骤C或者返回所述的步骤C之前的步骤。

进一步，所述步骤A包括以下步骤：发送运动控制指令到所述的刚性机器人的控制器，使固定于刚性机器人的机械臂或末端的双目相机随着机械臂运动，使双目相机从柔性机器人的柔性臂弯曲面的相对垂直的方向拍摄至少一个臂段的完整侧面图像，或者观察上一次未被完整观察到臂段的完整侧面图像。

进一步，所述步骤B包括以下步骤：

B1、将双目相机采集的图像数据转化为灰度图像数据后进行中值滤波；

B2、利用自适应阈值图像二值化算法分离目标臂杆部分与图像中的环境背景部分，获得二值化图像；

B3、在处理后的二值化图像中查找闭合轮廓，根据轮廓上点的数量及轮廓面积剔除干扰轮廓；

B4、对于剩余的轮廓，利用矩形拟合算法拟合出轮廓，然后根据已拟合的矩形的面积、长径比、与原轮廓的面积比，再排除干扰轮廓，然后将剩余轮廓整合标记为目标臂杆的轮廓；

B5、计算剩余轮廓的中心矩，求解中心点的像素坐标。

进一步，所述步骤C包括：

C1、将双目图像中识别的臂杆轮廓及对应的中心点标号叠加在双目相机所拍摄的双目图像中，以允许交互设备选取；

C2、捕获交互设备触发事件，以人机交互设备所触发的选择点是否在轮廓内为判断条件，确定在左目相机图像和右目相机图像中捕获到人机交互设备分别点选到同一臂段对应的轮廓及中心点，并解算两中心点的像素坐标。

进一步，所述步骤E包括：

从双目相机与刚性机器人的机械臂之间预先的视觉标定数据，双目相机与刚性机械臂末端间的相对位姿关系矩阵为^endT_cam；

从装配位置和尺寸，获取刚性机器人的基座与世界坐标系的相对位姿关系矩阵为^worlfT_baseg

预设柔性机器人的基座与世界坐标系的相对位姿关系矩阵为^worldT_baser，

获取当前刚性机器人的机械臂杆之间的旋转关节角，结合刚性机器人机械臂杆的结构尺寸，通过刚性机器人的运动学算法，计算得到安装双目相机的刚性臂末端相对于刚性机器人基座的位姿关系矩阵为^basegT_end；

利用解算的臂杆中心相对于双目相机的位姿得到转换矩阵^camT_bi，通过下式计算臂杆中心在柔性臂基座坐标系中的位姿为

^baserT_bi＝(^worldT_baser)^-1worldT_baser ^basegT^en _dT_cam ^camT_bi。

进一步，所述方法还包括以下步骤：G、以迭代的方式求解柔性机器人各臂段中的一个或多个臂杆的位置和姿态，然后根据柔性机器人的运动学算法在虚拟仿真环境中重建柔性机器人臂形，求解柔性臂的末端位姿。

进一步，所述步骤G包括以下步骤：

G1、对于每个臂段，以远离臂段根部的臂杆作为初始臂杆开始迭代计算该段柔性臂关节角；

G2、配置所述的初始臂杆的中心的期望位姿为P_q，相应的关节角初始值为θ₀，迭代标记为i；最大迭代次数为i_max；其中，所述的期望位姿P_q来自于所述步骤E中所换算的臂杆中心在柔性机器人基座坐标系中的位姿；

G3、令i初始值为零；

G4、利用柔性机器人的运动学算法，根据θ_i计算臂杆中心的当前运算位姿P；

G5、通过伪逆雅可比矩阵，计算θ_i+1＝θ_i+Δθ，其中

Δθ＝J+ΔP，ΔP＝P_q-P，J⁺＝J^T(JJ^T)^-1，J＝[v_2n-1v_2n]∈R^3×2，其中v_2n-1，v_2n分别表示关节段n的单位速度对末端线速度的影响：

v_2n-1＝e_n，1×r_n，1+e_n，4×r_n，4+e_n，5×r_n，5+e_n，8×r_n，8

v_2n＝e_n，2×r_n，2+e_n，3×r_n，3+e_n，6×r_n，6+e_n，7×r_n，7

式中e_n，i为关节段n第i的全局变量；r_n，i为第i节臂杆中心到柔性臂末端的全局变量；

G6、判断ΔP是否大于一阈值并且同时i小于i_max，是则令i叠加1，否则结束迭代运算，输出θ_i+1作为当前计算的臂杆的柔性臂关节角。

进一步，所述步骤G包括以下步骤：

利用重建的臂形和末端位姿与柔性臂运动规划的臂形和末端位姿结合，修正柔性机器人的柔性臂的运动学模型，补偿柔性臂运动误差。

本发明的技术方案还涉及一种计算机可读存储介质，其上储存有计算机程序指令，所述计算机程序指令被处理器执行时实施上述的方法。

本发明的技术方案还涉及一种基于等效中心点识别的柔性机器人臂形测量系统，包括：固定于刚性机器人上的双目相机，所述的双目相机被所述的刚性机器人带动在空间中调整位置和姿态时的相机视场覆盖所述的柔性机器人的一个或多个臂段；用于标记选择所述的柔性机器人的臂段的人机交互设备；以及所述的计算机可读存储介质。

与现有的技术相比，本发明具有以下的特点。

1、本发明提供了一种基于多连杆等效中心点识别的绳驱柔性机器人臂形测量方法，能够适应多种柔性机器人和不同的工作场合，无需在其上外置特征标志且对臂杆形状无特殊要求。

2、利用柔性臂本身具备的特征，针对臂杆工装与柔性臂其它部分和环境的颜色和反光性能不同，提取臂杆的轮廓，拟合臂杆中心点，通过对双目相机拍摄柔性臂图片中臂杆中心点的提取，求解臂杆中心点的位置，进而得到柔性臂臂杆相对于相机的位姿。因为对于每个臂杆提取的是臂杆中心点，可以大大降低拟合的轮廓范围略超过臂杆或不足臂杆对臂杆位置解算精度的影响，因此该方法对臂杆轮廓拟合精度要求低，对臂杆形状适应性强，可用于矩形侧面臂杆或圆柱体臂杆等，该方法对相机与臂杆间距离要求低，随距离增加识别精度降低较小。

3、利用人机交互方式选择左右目图像中的对应臂杆，提高了臂杆选择的精确度，避免了左右目图像中臂杆对应错误导致臂杆位置解算错误。

4.通过刚性臂与柔性臂多臂协作的方式拍摄柔性臂臂形照片，减少了臂杆观测不到、观测不清晰、观测方向过差的问题。且通过相机在不同位姿拍摄同一段臂杆，得到的多组臂杆位置可以通过数据处理得到平均值，提高柔性臂臂杆的位置测量精度。

附图说明

图1是根据本发明的柔性机器人臂形测量系统的立体图。

图2是示例性的单段柔性臂的模型图。

图3是根据本发明的柔性机器人臂形测量方法的流程图。

图4是基于柔性机器人逆运动学的计算臂杆角的流程图。

图5是根据本发明的臂形测量方法在视觉仿真环境中的测量结果示意图。

具体实施方式

以下将结合实施例和附图对本发明的构思、具体结构及产生的技术效果进行清楚、完整的描述，以充分地理解本发明的目的、方案和效果。

需要说明的是，如无特殊说明，当某一特征被称为“固定”、“连接”在另一个特征，它可以直接固定、连接在另一个特征上，也可以间接地固定、连接在另一个特征上。本文所使用的单数形式的“一种”、“所述”和“该”也旨在包括多数形式，除非上下文清楚地表示其他含义。此外，除非另有定义，本文所使用的所有的技术和科学术语与本技术领域的技术人员通常理解的含义相同。本文说明书中所使用的术语只是为了描述具体的实施例，而不是为了限制本发明。本文所使用的术语“和/或”包括一个或多个相关的所列项目的任意的组合。

应当理解，尽管在本公开可能采用术语第一、第二、第三等来描述各种元件，但这些元件不应限于这些术语。这些术语仅用来将同一类型的元件彼此区分开。例如，在不脱离本公开范围的情况下，第一元件也可以被称为第二元件，类似地，第二元件也可以被称为第一元件。本文所提供的任何以及所有实例或示例性语言(“例如”、“如”等)的使用仅意图更好地说明本发明的实施例，并且除非另外要求，否则不会对本发明的范围施加限制。此外，本文所采用的行业术语“位姿”是指某个元件相对于空间坐标系的位置和姿态。

参照图1，根据本发明的臂形测量系统的测量对象是绳索驱动的柔性机器人100的机械臂。所述的柔性机器人100包括多个臂段，每个臂段包括多个的臂杆，臂杆之间通过万向节串联。参照图1和图2，柔性机器人100的柔性臂总共可以分为四大段，每段柔性臂包括四个臂杆和万向节，通过联动绳保证每段柔性臂绕图2所示的y轴和z轴旋转的关节角分别相等。

继续参照图1，在一些实施例中，根据本发明的测量系统还可以包括固连在刚性机器人200的刚性臂上或者末端的手眼双目相机300、以及用于标记选择所述的柔性机器人的臂段的人机交互设备。双目相机300观测柔性臂的臂段，拍摄柔性臂照片，用于臂形测量算法解算柔性臂位姿。双目相机300可以作为全局相机使用，也可固连在刚性臂上或者末端。刚性机械臂用于固连双目相机，为保证观测绳驱柔性机器人的效果，测量过程中刚性臂各关节可以旋转，以便控制刚性臂末端，使双目相机300观测方向能够尽量垂直于柔性臂的弯曲面。

在一些实施例中，刚性机器人可以是多自由度串联工业机器人，这样可以使得安装在刚性机器人机械臂末端的双目相机不容易受到机械臂的遮挡，而且位姿调整幅度更大。

在一些实施例中，人机交互设备可以是显示器、鼠标/键盘、或触摸屏。在测量臂形的过程中，双目显示器将拍摄的臂段双目图片实时显示到显示器或触摸屏显示器中，然后用户可以通过鼠标、键盘或者触摸屏来点中选择两个双目图片中的相同的臂段，从而辅助识别两个双目图片中的相同臂段的等效中心。在另一些实施例中，人机交互设备可以是激光笔，在测量臂形的过程中，用户可以用激光笔照射某个臂段，此时双目相机采集的图像中双同的臂段就会出现激光红点，以此辅助识别两个双目图片中的相同臂段的等效中心。

参照图3，在一些实施例中，根据本发明的臂形测量方法，包括如下步骤：

A、通过一个固定于其他机械臂或其他载体的双目相机从尽量垂直于柔性臂弯曲面方向拍摄柔性臂侧面的图像。

B、在双目相机图像中检测臂段，求解臂段的中心，得到各个臂段中心的像素坐标；该步骤B具体可以包括如下步骤：

B1、图像预处理，将图片转化为灰度图后中值滤波图片，以减少图像噪声；

B2、利用自适应阈值图像二值化算法分离目标臂杆部分与背景环境；

B3、在经过B2步骤处理后的二值化图片中寻找闭合轮廓，根据轮廓上点的数量及轮廓面积删去一部分干扰轮廓；

B4、对于B3步骤后剩余的轮廓，利用矩形拟合轮廓，根据拟合矩形的面积、长径比、与原轮廓的面积比排除一部分轮廓，经过该步骤处理后，剩余轮廓基本为目标臂杆的轮廓，有少许干扰轮廓不影响后续操作；

B5、计算剩余轮廓的中心矩，求解中心点的像素坐标。

C、通过人机交互方式，在左右目图像中人工选择同一臂段在两图像中的对应中心点；该步骤C具体可以包括如下步骤：

C1、将步骤B得到的臂杆轮廓及对应的中心点标号绘制在原始图片上；

C2、添加鼠标触发事件，在左右臂杆中分别点击同一个臂杆轮廓的内部，以点击点是否在轮廓内为判断条件，则可以在双目相机图片中分别选择到同一段臂杆对应的轮廓及中心点，并得到两中心点的像素坐标。

D、利用最小二乘法解算两中心点像素坐标对应的笛卡尔空间位置，该位置即为臂杆中心在双目相机坐标系中的位置，设该位置为^camT_bi；具体计算过程如下。

对于左目相机：

其中，[u_l，v_l，1]^T为目标点在左目相机图像坐标系中的坐标，[X_cl，Y_cl，Z_cl，1]^T为目标点在左目相机坐标系下的坐标，[X_w，Y_w，Z_w，1]^T为目标点在世界坐标系下的坐标，R和t为坐标转换矩阵，m₁₁至m₃₄为拆分的矩阵元素。

同理可得对于右目相机：

联立可得：

E、双目相机与刚性机械臂末端间的相对位姿关系可预先标定，为^endT_cam；刚性臂基座与世界坐标系的相对位姿在装配时已知，为^worldT_baseg；柔性臂基座与世界坐标系的相对位姿在装配时已知，为^worldT_baser；根据当前刚性机械臂的关节角，结合刚性机械臂的机械结构尺寸和运动学，则可以得到刚性臂末端相对于刚性机械臂基座的位姿，为^basegT_end。利用步骤D中求得的臂杆中心相对于双目相机的位姿得到转换矩阵^camT_bi，则可以得到柔性臂臂杆中心在柔性臂基座坐标系中的位置^baserT_bi，求解公式如下：

^baserT_bi＝(^worldT_baser)^-1worldT_basegb^asegT_end ^endT_cam ^camT_bi。

F、如果检测到还有柔性机器人的臂段在柔性臂基座坐标系中的位置未被测量，至少返回执行所述的步骤C或者返回所述的步骤C之前的步骤(比如步骤A)。还可以重复上述步骤C、D、E，可得到柔性臂各个臂段在世界坐标系中的位置。

如果各臂杆间存在遮挡现象，且相机光轴与一些臂杆侧面夹角过大，远非垂直状态，导致臂杆侧面在图像中观察不完全，引入很多额外干扰，则可以发送指令到刚性机械臂的运动控制器，使刚性机械臂带动双目相机移动和偏转，使在上一刚性臂构型下未被较好观察的柔性臂的臂杆能够被完整观察到。

可多次运动并重复A-F步骤，直到所有臂杆或根据任务需求需要被测量的臂杆均被测量。

参照图3和图4，在一些实施例中，根据本发明的臂形测量方法，还包括臂形重建步骤G，如下。

G、求解柔性臂各臂段的位置和姿态，重建柔性臂臂形，求得柔性臂的末端位姿，也可利用重建的臂形和末端位姿与柔性臂运动规划的臂形和末端位姿结合，修正柔性臂的运动学模型，补偿柔性臂运动误差。

对于每一段柔性臂，知道其中一节臂杆的三维位置信息，则可以求解该段柔性臂的两个关节角，因此，整个柔性臂最少需要测量4节臂杆的位置，每段柔性臂测量1节臂杆。测量的多余臂杆可以用于减小计算误差，提高算法鲁棒性。同时，后段柔性臂关节角的求解需要利用前段柔性臂末端的位姿信息，因此，需要从柔性臂的根部向末端求解。首先计算利用柔性臂逆运动学计算根部柔性臂关节角，利用关节角求解第一段柔性臂末端位姿，在此基础上求解第二段关节角和末端位姿，依此类推，求解4段柔性臂关节角。

对于每段柔性臂，以远离根部的第4节臂杆为例计算关节角，具体计算过程如下：

采用基于速度级雅可比矩阵的数值迭代法求解其最小范数解。同时，根据关节段的段内联动等角度的特点，对雅可比矩阵简化，其雅可比矩阵如下：

J＝[v_2n-1 v_2n]∈R^3×2

其中v_2n-1，v2n分别表示关节段n的单位速度对末端线速度的影响，它们可以表示如下：

v_2n-1＝e_n，1×r_n，1+e_n，4×r_n，4+e_n，5×r_n，5+e_n，8×r_n，8

v_2n＝e_n，2×r_n，2+e_n，3×r_n，3+e_n，6×r_n，6+e_n，7×r_n，7

式中e_n，i为关节段n第i的全局变量；r_n，i为第i节臂杆中心到柔性臂末端的全局变量。

根据雅可比矩阵J计算其伪逆J⁺＝J^T(JJ^T)^-1，基于简化的雅可比矩阵，根据数值迭代法求解该段柔性臂关节角，参照图4的具体流程如下。

G1、对于每个臂段，以一臂杆作为初始臂杆开始迭代计算该段柔性臂关节角；

G2、配置所述的初始臂杆的中心的期望位姿为P_q，相应的关节角初始值为θ₀，迭代标记为i；最大迭代次数为i_max；其中，所述的期望位姿P_q来自于所述步骤E中所换算的臂杆中心在柔性机器人基座坐标系中的位姿；

G3、令i初始值为零；

G4、利用柔性机器人的运动学算法，根据θ_i计算臂杆中心的当前运算位姿P；

G5、通过伪逆雅可比矩阵，计算θ_i+1＝θ_i+Δθ，其中

Δθ＝J⁺ΔP，ΔP＝P_q-P，J⁺＝J^T(JJ^T)^-1，J＝[v_2n-1v_2n]∈R^3×2；

G6、判断ΔP是否大于一阈值并且同时i小于i_max，是则令i叠加1，否则结束迭代运算，输出θ_i+1作为当前计算的臂杆的柔性臂关节角。

图5是利用视觉仿真系统测试的臂形测量算法的测量结果示意图。比如，利用OSG图像仿真系统导入柔性臂模型，并设置双目相机的位置和参数，如表1。图5a为柔性机器人机械臂在笛卡尔空间的弯曲状态图，利用双目相机观察柔性臂臂杆。图5b、图5c分别为左目相机和右目相机拍摄的图片，其中的黑色点为提取到臂杆轮廓后拟合得到的臂杆中心点。通过鼠标点击的方式在双目相机图片中选择同一臂杆，解算臂杆相对于相机的位置，与仿真模型设置参数比较，相机与目标臂杆距离1.2m内时，单节臂杆识别位置误差在10mm内，相机与目标臂杆距离2.5m内时，位置误差在25mm内。根据测量得到的臂杆位置求解关节角，与理论关节角对比结果见表2，平均误差为0.2748°，最大误差小于0.7°。利用求解得到的关节角重建柔性臂臂形，末端位姿误差见表3，各方向位置误差小于8mm，姿态误差小于1.5°。

表1

参数	Value
		臂杆尺寸	Φ55mm x 62mm
臂杆数量	16
		自由度	8
臂杆间距离	28mm
		相机分辨率	1024x1024
相机视场角	50°
		右目相对左目姿态角	[0，0，0](°)
右目相对左目位置	[300，0，0](mm)

表2

	1	2	3	4	5	6	7	8
									理论关节角(°)	0	0	10	0	-20	0	20	0
测量关节角(°)	-0.0352	-0.2563	9.7935	0.6852	-19.7460	-0.3964	20.0763	0.2887
									误差(°)	0.0352	0.2563	0.2065	-0.6852	-0.2540	0.3964	-0.0763	-0.2887

表3

应当认识到，本发明实施例中的方法步骤可以由计算机硬件、硬件和软件的组合、或者通过存储在非暂时性计算机可读存储器中的计算机指令来实现或实施。所述方法可以使用标准编程技术。每个程序可以以高级过程或面向对象的编程语言来实现以与计算机系统通信。然而，若需要，该程序可以以汇编或机器语言实现。在任何情况下，该语言可以是编译或解释的语言。此外，为此目的该程序能够在编程的专用集成电路上运行。

此外，可按任何合适的顺序来执行本文描述的过程的操作，除非本文另外指示或以其他方式明显地与上下文矛盾。本文描述的过程(或变型和/或其组合)可在配置有可执行指令的一个或多个计算机系统的控制下执行，并且可作为共同地在一个或多个处理器上执行的代码(例如，可执行指令、一个或多个计算机程序或一个或多个应用)、由硬件或其组合来实现。所述计算机程序包括可由一个或多个处理器执行的多个指令。

进一步，所述方法可以在可操作地连接至合适的任何类型的计算平台中实现，包括但不限于个人电脑、迷你计算机、主框架、工作站、网络或分布式计算环境、单独的或集成的计算机平台、或者与带电粒子工具或其它成像装置通信等等。本发明的各方面可以以存储在非暂时性存储介质或设备上的机器可读代码来实现，无论是可移动的还是集成至计算平台，如硬盘、光学读取和/或写入存储介质、RAM、ROM等，使得其可由可编程计算机读取，当存储介质或设备由计算机读取时可用于配置和操作计算机以执行在此所描述的过程。此外，机器可读代码，或其部分可以通过有线或无线网络传输。当此类媒体包括结合微处理器或其他数据处理器实现上文所述步骤的指令或程序时，本文所述的发明包括这些和其他不同类型的非暂时性计算机可读存储介质。当根据本发明所述的方法和技术编程时，本发明还可以包括计算机本身。

计算机程序能够应用于输入数据以执行本文所述的功能，从而转换输入数据以生成存储至非易失性存储器的输出数据。输出信息还可以应用于一个或多个输出设备如显示器。在本发明优选的实施例中，转换的数据表示物理和有形的对象，包括显示器上产生的物理和有形对象的特定视觉描绘。

以上所述，只是本发明的较佳实施例而已，本发明并不局限于上述实施方式，只要其以相同的手段达到本发明的技术效果，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明保护的范围之内。在本发明的保护范围内其技术方案和/或实施方式可以有各种不同的修改和变化。

Claims (10)

1.一种基于等效中心点识别的柔性机器人臂形测量方法，所述的柔性机器人包括多个臂段，每个臂段包括多个的臂杆，臂杆之间通过万向节串联，

其特征在于，所述方法包括以下步骤：

A、通过固定于刚性机器人上的且空间位姿可调的双目相机采集所述的柔性机器人的至少一个臂段的完整侧面图像；

B、根据双目相机所采集的双目图像中分别检测的一个或多个臂段，识别和拟合臂杆轮廓，求解相应的臂段的中心的图像像素坐标；

C、根据人机交互设备的触发信息，确定左目图像和右目图像中同一臂段在两图像中的对应中心点；

D、通过最小二乘法解算左目图像和右目图像的两中心点像素坐标所对应的臂杆中心在双目相机坐标系中的位姿；

E、根据当前刚性机械臂的关节角、机械臂杆长尺寸和所述的双目相机的安装尺寸，通过运动学算法，确定所述的双目相机当前相对于刚性机械臂基座的位姿，并且根据所述的刚性机械臂相对于所述的柔性机器人的安装位置关系，将所述步骤D中解算的位姿换算为臂段中的至少一个臂杆中心在柔性机器人基座坐标系中的位姿；

F、如果检测到还有柔性机器人的臂段在柔性臂基座坐标系中的位置未被测量，至少返回执行所述的步骤C或者返回所述的步骤C之前的步骤。

2.根据权利要求1所述的方法，其中，所述步骤A包括以下步骤：

发送运动控制指令到所述的刚性机器人的控制器，

使固定于刚性机器人的机械臂或末端的双目相机随着机械臂运动，使双目相机从柔性机器人的柔性臂弯曲面的相对垂直的方向拍摄至少一个臂段的完整侧面图像，或者观察上一次未被完整观察到臂段的完整侧面图像。

3.根据权利要求1所述的方法，其中，所述步骤B包括以下步骤：

B1、将双目相机采集的图像数据转化为灰度图像数据后进行中值滤波；

B2、利用自适应阈值图像二值化算法分离目标臂杆部分与图像中的环境背景部分，获得二值化图像；

B3、在处理后的二值化图像中查找闭合轮廓，根据轮廓上点的数量及轮廓面积剔除干扰轮廓；

B4、对于剩余的轮廓，利用矩形拟合算法拟合出轮廓，然后根据已拟合的矩形的面积、长径比、与原轮廓的面积比，再排除干扰轮廓，然后将剩余轮廓整合标记为目标臂杆的轮廓；

B5、计算剩余轮廓的中心矩，求解中心点的像素坐标。

4.根据权利要求1所述的方法，其中，所述步骤C包括：

C1、将双目图像中识别的臂杆轮廓及对应的中心点标号叠加在双目相机所拍摄的双目图像中，以允许交互设备选取；

C2、捕获交互设备触发事件，以人机交互设备所触发的选择点是否在轮廓内为判断条件，确定在左目相机图像和右目相机图像中捕获到人机交互设备分别点选到同一臂段对应的轮廓及中心点，并解算两中心点的像素坐标。

5.根据权利要求1所述的方法，其中，所述步骤E包括：

从双目相机与刚性机器人的机械臂之间预先的视觉标定数据，双目相机与刚性机械臂末端间的相对位姿关系矩阵为^endT_cam；

从装配位置和尺寸，获取刚性机器人的基座与世界坐标系的相对位姿关系矩阵为^worldT_baseg；

预设柔性机器人的基座与世界坐标系的相对位姿关系矩阵为^worldT_baser，

获取当前刚性机器人的机械臂杆之间的旋转关节角，结合刚性机器人机械臂杆的结构尺寸，通过刚性机器人的运动学算法，计算得到安装双目相机的刚性臂末端相对于刚性机器人基座的位姿关系矩阵为^basegT_end；

利用解算的臂杆中心相对于双目相机的位姿得到转换矩阵^camT_bi，通过下式计算臂杆中心在柔性臂基座坐标系中的位姿为

^baserT_bi＝(^worldT_baser)^{-1 world}T_baser ^basegT_end ^endT_cam ^camT_bi。

6.根据权利要求1所述的方法，其中，所述方法还包括以下步骤：

G、以迭代的方式求解柔性机器人各臂段中的一个或多个臂杆的位置和姿态，然后根据柔性机器人的运动学算法在虚拟仿真环境中重建柔性机器人臂形，求解柔性臂的末端位姿。

7.根据权利要求6所述的方法，每段臂段包括四节臂杆，其中，所述步骤G包括以下步骤：

G1、对于每个臂段，以远离臂段根部的臂杆作为初始臂杆开始迭代计算该段柔性臂关节角；

G2、配置所述的初始臂杆的中心的期望位姿为P_q，相应的关节角初始值为θ₀，迭代标记为i；最大迭代次数为i_max；其中，所述的期望位姿P_q来自于所述步骤E中所换算的臂杆中心在柔性机器人基座坐标系中的位姿；

G3、令i初始值为零；

G4、利用柔性机器人的运动学算法，根据θ_i计算臂杆中心的当前运算位姿P；

G5、通过伪逆雅可比矩阵，计算θ_i+1＝θ_i+Δθ，其中

Δθ＝J⁺ΔP，ΔP＝P_q-P，J⁺＝J^T(JJ^T)^-1，J＝[v_2n-v_2n]∈R^3×2，其中v_2n-1，v_2n分别表示关节段n的单位速度对末端线速度的影响：

v_2n-1＝e_n，1×r_n，1+e_n，4×r_n，4+e_n，5×r_n，5+e_n，8×r_n，8

v_2n＝e_n，2×r_n，2+e_n，3×r_n，3+e_n，6×r_n，6+e_n，7×r_n，7

式中e_n，i为关节段n第i的全局变量；r_n，i为第i节臂杆中心到柔性臂末端的全局变量；

G6、判断ΔP是否大于一阈值并且同时i小于i_max，是则令i叠加1，否则结束迭代运算，输出θ_i+1作为当前计算的臂杆的柔性臂关节角。

8.根据权利要求6所述的方法，其中，所述步骤G包括以下步骤：

利用重建的臂形和末端位姿与柔性臂运动规划的臂形和末端位姿结合，修正柔性机器人的柔性臂的运动学模型，补偿柔性臂运动误差。

9.一种计算机可读存储介质，其上储存有程序指令，所述程序指令被处理器执行时实施如权利要求1至8中任一项所述的方法。

10.一种基于等效中心点识别的柔性机器人臂形测量系统，所述的柔性机器人包括多个臂段，每个臂段包括多个的臂杆，臂杆之间通过万向节串联，其特征在于，所述系统包括：

固定于刚性机器人上的双目相机，所述的双目相机被所述的刚性机器人带动在空间中调整位置和姿态时的相机视场覆盖所述的柔性机器人的一个或多个臂段；

用于标记选择所述的柔性机器人的臂段的人机交互设备；

根据权利要求9所述的计算机可读存储介质。

Images