CN107175660B - 一种基于单目视觉的六自由度机器人运动学标定方法 - Google Patents

Abstract

本发明涉及一种基于单目视觉的六自由度机器人运动学标定方法，包括：建立六自由度机器人的MDH模型，并根据初始标定变量U得到六自由度机器人末端的指令位移和安装于六自由度机器人上的单目视觉传感器与六自由度机器人末端之间的坐标变换关系根据六自由度机器人的初始标定变量U，利用绝对值编码棋盘对安装于六自由度机器人上的单目视觉传感器的多组位移Δ'进行分级测距并优化；得到六自由度机器人末端的实际位移Δ，并得到多组观测变量V_k；利用约束关系，结合多组观测变量V_k对标定变量U进行优化，判断标定变量U是否达到定位精度，若是则完成少自由度机器人的运动学标定，若否则返回。与现有技术相比，本发明具有测距精度高、实用性强以及节省成本等优点。

Description

一种基于单目视觉的六自由度机器人运动学标定方法

技术领域

本发明涉及工业机器人标定领域，尤其是涉及一种基于单目视觉的六自由度机器人运动学标定方法。

背景技术

精确测量机器人工作末端位移是完成机器人标定的核心。通常借助外界传感器测量其位姿。常用设备包括三坐标测量仪、经纬仪等精密测量仪器。但这些方案的共同缺点在于设备价格昂贵(数十万甚至上百万)，使用方法繁琐复杂，往往需要专业人员现场校正操控，使用门槛较高；另一方面，此类设备通常需要占据较大的空间，难以满足生产现场对于高效便捷的要求。

受制于上述设备，工业现场往往采用根据机器人工作实际位置误差，手动拟合补偿函数，但这样并没有将模型中各个机械参数造成的误差解耦，难以满足精确度的要求。且标定结果适用范围具有很大的局限性，往往仅适用于标定过的位置，而其他位置存在较大误差。

基于摄像头的运动学标定方案，以其经济、便捷的优点，能够很好的满足现场标定的要求。测量时，将相机固定在机器人末端，标靶位置固定且已知(或反之)。其基本原理是利用相机相对于固定标靶的移动，得到末端的位移信息。然而，现有的方案往往需要保持整个标靶始终处于视野范围内，利用标靶上所有的特征点，计算所需信息。这种方法很难利用普通相机做到高精度标定。主要原因是以下两点：

1.机器人往往需要较大的工作空间，为了保持标靶始终处于视野范围内，这就要求相机的视野范围很大，相应的每个像素对应的尺寸就很大(很难控制在1mm²以内)，难以做到精确定位。

2.标靶会较多的出现在视野中图片的边缘区域。而该部分有很严重的径向畸变。即使经过摄像头标定，其带来的误差也会使测量结果产生较大的波动，给结果的精准度造成干扰。

另外，传统的对于机器人进行标定的方法，多是通过DH模型对机器人进行建模，这种建模方法存在一个根本缺陷，即当存在两轴平行或近于平行时，两轴线相对位置误差的建模不符合微小误差模型，可能会引起很大的杆件偏移量误差。而且现有的方法多是默认机器人末端和摄像头之间为平移关系，二者之间的坐标转换较为简单，对于六自由度机器人来讲，由于机器人自由度较高，这种坐标转换方式将会引起较大的误差。

发明内容

本发明的目的是针对上述问题提供一种基于单目视觉的六自由度机器人运动学标定方法。

本发明的目的可以通过以下技术方案来实现：

一种基于单目视觉的六自由度机器人运动学标定方法，所述方法包括下列步骤：

1)建立六自由度机器人的MDH模型，并根据初始标定变量U得到六自由度机器人末端的指令位移和安装于六自由度机器人上的单目视觉传感器与六自由度机器人末端之间的坐标变换关系

2)根据六自由度机器人的初始标定变量U，利用绝对值编码棋盘对安装于六自由度机器人上的单目视觉传感器的多组位移Δ'进行分级测距并优化；

3)标定棋盘格坐标系与基坐标系的转换角α，并根据转换角α对位移Δ'进行坐标变换得到六自由度机器人末端的实际位移Δ，并得到多组观测变量V_k；

4)利用约束关系，结合步骤2)得到的多组观测变量V_k对标定变量U进行优化，判断标定变量U是否达到定位精度，若是则完成六自由度机器人的运动学标定，若否则返回步骤2)。

所述六自由度机器人的MDH模型具体为5参数MDH模型，所述5参数包括关节角θ_i、杆件偏移量d_i、杆件长度L_i、扭转角α_i和扭转角β_i。

所述通过初始标定得到安装于六自由度机器人上的单目视觉传感器与六自由度机器人之间的坐标变换关系的方法包括单轴旋转法或双轴旋转法。

所述单轴旋转法具体为：保持六自由度机器人的5个轴静止，只旋转最后1个轴，得到单目视觉传感器采集的图片，测量单目视觉传感器相对于棋盘格坐标系的位置，通过拟合得到单目视觉传感器的轨迹方程，结合当前机器人的末端执行器的位置即可得到单目视觉传感器与六自由度机器人末端之间的坐标变换关系。

所述双轴旋转法具体为：保持六自由度机器人的4个轴静止，只旋转最后2个轴，拟合最后2个轴的轴线并将2个轴线的交点作为六自由度机器人的第六轴的坐标系的原点，通过单目视觉传感器与第六轴的坐标系的原点的位置关系得到单目视觉传感器与六自由度机器人末端之间的坐标变换关系。

所述步骤2)具体为：

21)对绝对值编码棋盘的角点在像素坐标系下的位置坐标进行优化；

22)利用优化后的绝对值编码棋盘对安装于六自由度机器人上的单目视觉传感器的位移进行分级测距，得到多组位移Δ'，同时测量与位移Δ'对应的电机角度Θ。

所述步骤21)具体为：

211)根据绝对值编码棋盘的局部特征对角点在像素坐标系下的位置坐标进行优化；

212)根据绝对值编码棋盘的整体属性，对优化后的角点在像素坐标系下的位置坐标进行进一步优化。

所述位移Δ'为单目视觉传感器在绝对值编码棋盘下的中心栅格位置变化T与单目视觉传感器在中心栅格内部的相对位移T'之和。

所述单目视觉传感器在绝对值编码棋盘下的中心栅格位置变化T具体为：根据识别中心栅格阵列的编码，确定单目视觉传感器所在的中心栅格的位置，继而确定单目视觉传感器在绝对值编码棋盘下的中心栅格位置变化T。

所述单目视觉传感器在中心栅格内部的相对位移T'具体为：

T'＝t_(i+1)k-t_ik

sp_ik＝A[R_ik t_ik]P_ik

其中，i＝1,2,...表示单目视觉传感器的不同位置，k表示角点的编号，p_ik和P_ik分别表示角点在像素坐标系与棋盘格坐标系下的坐标，s表示尺度参数，A表示单目视觉传感器内部参数，R_ik和t_ik分别表示单目视觉传感器当前位姿在棋盘格坐标系下的旋转和平移。

与现有技术相比，本发明具有以下有益效果：

(1)通过对六自由度机器人建立MDH模型，克服原有DH模型在存在两轴平行或近于平行时，两轴线相对位置误差的建模不符合微小误差模型，可能会引起很大的杆件偏移量误差的缺陷，使得机器人的建模更为准确，提高后续标定的准确程度。

(2)本发明提出的方法，通过初始标定确定机器人末端和单目视觉传感器之间的坐标变换关系，而非传统的默认为二者是简单的平移关系，对于六自由度机器人来讲，此方法更符合实际，充分考虑到六自由度机器人活动的灵活程度，大大减小了标定的误差。

(3)利用绝对值编码棋盘，实现单目视觉传感器的分级测距，保证了单目视觉传感器始终工作在较小的视野范围内，且图片有效信息集中在图像中央畸变较小的部分，提高单目视觉传感器的测距精度，进而提高了标定的准确性。

(4)分别根据绝对值编码棋盘的局部特征和整体属性对角点的位置坐标进行了优化，使角点的精度达到了亚像素的级别，进一步提高了单目视觉传感器的测距准确度，提高了标定的准确性。

附图说明

图1为本发明的方法流程图；

图2为分级测距的示意图，其中(2a)为中心栅格的位置变化示意图，(2b)为光心相对位移示意图；

图3为8阶反馈移位寄存器的示意图；

图4为对伪随机序列进行二维延扩的示意图；

图5为角点位置优化的示意图，其中(5a)为利用棋盘格的局部特征进行角点位置优化，(5b)为利用棋盘格的整体属性进行角点位置优化；

图6为六自由度机器人的近似平行关节示意图；

图7为两个位姿之差示意图。

具体实施方式

下面结合附图和具体实施例对本发明进行详细说明。本实施例以本发明技术方案为前提进行实施，给出了详细的实施方式和具体的操作过程，但本发明的保护范围不限于下述的实施例。

机器人标定的通常步骤是根据机器人的结构特征，构建运动学模型。并利用模型特性建立观测变量与待标定变量的约束关系；通过外部传感器，高精度地测量所需观测变量；利用约束关系与多组实测值，通过相关数学优化方法，实现对待标定变量的优化；以此修正模型中的机械参数或进行末端的误差补偿，获得高精度的空间定位。

如图1所示，本实施例提供的是一种基于单目视觉的六自由度机器人运动学标定方法，针对的是空间少自由度机器人的标定，即机器人末端存在空间上的位移，没有姿态上的旋转，具体的标定步骤如下：

1)建立六自由度机器人的MDH模型，并根据初始标定变量U得到六自由度机器人末端的指令位移和安装于六自由度机器人上的单目视觉传感器与六自由度机器人末端之间的坐标变换关系

2)根据六自由度机器人的初始标定变量U，利用绝对值编码棋盘对安装于六自由度机器人上的单目视觉传感器的多组位移Δ'进行分级测距并优化；

3)标定棋盘格坐标系与基坐标系的转换角α，并根据转换角α对位移Δ'进行坐标变换得到六自由度机器人末端的实际位移Δ，并得到多组观测变量V_k；

4)利用约束关系，结合步骤2)得到的多组观测变量V_k对标定变量U进行优化，判断标定变量U是否达到定位精度，若是则完成六自由度机器人的运动学标定，若否则返回步骤2)。

下面对上述步骤进行具体的阐述：

机器人模型构建

本实施例针对的是空间六自由度机器人的标定。一般而言，具有6个关节的六自由度机器人***能驱动末端执行器达到三维空间中的任意位置和姿态。目前工业机器人多采用DH参数法进行建模，但是DH参数法存在一个根本缺陷，即当存在两轴平行或近于平行时，两轴线相对位置误差的建模不符合微小误差模型。

如图6所示，当关节i和关节i+1公称平行时，杆件偏移量|d_i|的变化很敏感。这样造成：参数对偏移平行条件(相邻两轴线扭转角α_i为0)下的误差非常敏感了；当α_i趋近于0时，d_i和矩阵接近无穷大；该处关节建立的坐标轴远远偏离机器人手臂。

因此，本实施例中在采用5参数MDH方法。5个参数分别是关节角θ_i、杆件偏移量d_i、杆件长度L_i、扭转角α_i和β_i。其中，扭角β_i是相邻关节轴Z_i-1和Z_i在平行于X_i和Z_i平面上的夹角，其他参数的定义与DH模型一致。其变换矩阵为：

当相邻关节不平行时，扭角β_i的值为0，MDH模型与DH模型一致；当相邻关节平行时，MDH杆件坐标系的建立与DH模型相邻关节平行的规则一致，d＝0,β是否等于0，则有机械误差决定，当相邻平行关节轴有微小的偏差时，β不等于0。因此，MDH模型能够克服DH模型的不足。

相机分级测距方案

视觉测量***主要组成部分为单个摄像头传感器和带有绝对编码的棋盘格。

如图2所示，图中栅格中的两个角点分别记为G₁和G₂，图中虚线表示摄像头的主光轴。在相机的从初始位置运动到终点位置的一次移动过程中，引入过渡位置，使得该位置摄像头主光轴与初始位置时主光轴在同一栅格中。并且保证在过渡位置处摄像头与在终点位置处姿态相同，摄像头主光轴平行。这样就可以将整个移动过程等效为从初始位置先运动到过渡位置以及从过渡位置运动到终点位置两个运动过程t₁和t₂。

以固定方式在每个角点处自定义局部角点阵列坐标系，这样，每个局部角点阵列坐标系与棋盘格坐标系之间只有简单平移关系。利用摄像头成像原理以及刚体变换等原理，可以根据在棋盘格各个角点处的图像来确定该角点阵列坐标系与摄像头坐标系之间的变换关系。从而可以获得摄像头坐标系和棋盘格坐标系之间的变换关系。

t₂过程等于从过渡位置运动到终点位置的过程可以看作是在棋盘格坐标系下两个角点阵列坐标系的平移运动，从角点G₁移动到角点G₂，即为单目视觉传感器在绝对值编码棋盘下的中心栅格位置变化T。

t₁过程等于终点位置相对于角点G₂的位置与姿态与初始位置相对于角点G₁的位置与姿态之差，由于姿态的变化导致摄像头传感器中心投影位置的偏移，在角点G₁的内部形成C₁到C₂的位置差，即为单目视觉传感器在中心栅格内部的相对位移T'。

通过将整***移和局部位姿变换的分离，实现了位移分级测量。相比于现在的测量方案，在位移分级测量中，机器人末端执行器的运动范围更大。只要保证摄像头的视野范围内包含棋盘格上的一个编码序列就行，无需看到完整的标靶就能测出末端的位置变化。并且由于角点阵列基本均在摄像头视野中间，大大降低了由于摄像头成像畸变造成的误差对结果的影响，提高了测量精度。

对棋盘格各个栅格进行编码参考如下：

选取中心栅格周围n*n(如n＝8)的棋盘格阵列，那么T可以通过阵列本身的特定标示确定，T'可以利用阵列求算相机外部参数解算。这样的划分能够保证相机工作在足够小的视野范围，从而获得更加精准且鲁棒的测量值。

确定中心栅格的在棋盘上的绝对位置需要每个栅格具有独特的标示。可以对其周围n*n的棋盘格阵列进行二进制编码，使每个栅格对应的阵列具有唯一的标示。通过扩展伪随机序列到二维，可以实现栅格的绝对编码。

构造一个长度为m＝2ⁿ-1的伪随机序列需要n阶多项式h(x)。以n＝8为例，反馈移位寄存器如图3。即在给定a_i...a_i+7后，以此类推。

F.Jessie等提出上述构造方式并证明寄存器的循环周期为255＝2⁸-1。下面将其延扩为50*50的二维阵列。若棋盘格边长为2cm，则会构成1m*1m棋盘，满足使用要求。

如图4所示，取前215组编码，分为模块1～5，每个模块43组，每组长度为50。第一行用模块1伪随机编码即43组不同的编码(长度为50)，使得在序列中任取8个数都有唯一的一组8位二进制编码与其对应。将1*8的标识延拓为8*8的方阵，后7行与第1行相同，得到8*50的矩阵。其余4个8*50的矩阵可以用类似的方法得到，组合后得到40*50的矩阵，最后10*50的矩阵可由两个5*50的矩阵组合得到，分别采用模块1、2的编码数据延拓。

这样，通过图像识别栅格阵列的编码，就能够唯一的确定相机光心所在的栅格位置。

六自由度机器人和单目视觉传感器之间的转化

六自由度机器人末端执行器和摄像头是刚性连接，而通过视觉测量获取的是摄像头相对于栅格板坐标系的位置和姿态。所以，在标定之前，首先需要获取机器人末端执行器和摄像头之间的转换关系，即摄像头坐标系与机器人末端坐标系之间的齐次变换矩阵

本方案中选择视觉测量方法获取该变换矩阵在测量时，在某给定六自由度机器人位姿下使机器人末端仅随第6个轴转动，保持另5个轴不动。由于摄像头是刚性固定在末端执行器上，所以摄像头光心的轨迹为空间中一个圆(或圆弧)。而该圆(或圆弧)的旋转轴线即为第6个轴的轴线。

另外根据机器人的MDH模型，可以通过各轴电机当前角度计算出此时末端的位姿。根据多级通过视觉测量的光心位姿的拟合，可以求出摄像头的旋转轨迹和该轨迹方程和轴线的方程。而末端执行器一定在该轴线上，所以根据这两个方程和末端执行器的位置，就可以利用视觉测量方法获得机器人末端执行器坐标系和摄像头坐标系的齐次变换矩阵

也可以将机器人最后两个轴均进行旋转，通过上述方法对最后两个轴的轴线进行拟合，有MDH模型中坐标系的建立过程，可以知道这两个轴线的交点就是最后一个关节坐标系的坐标原点。所以就可以直接获取摄像头位置到最后一个关节位置的平移，即可直接得到最后一个关节坐标系到摄像头坐标系的变换关系。

选择合理的误差函数迭代优化

当前对工业机器人精度的定义一般采用位姿精确度和位姿重复度来衡量。但是在实际测量中，机器人末端执行器的实际位姿是不易获取的。这是因为标定***是利用指令位姿和实际位姿之差作为修正机械参数的依据，只有当指令位姿和实际位姿是在同一坐标系(也就是基坐标系)下的描述，才能得到完全理想的误差函数。然而，在实际标定过程中，末端执行器的实际位姿都是通过外部设备获取的，是当前实际末端执行器相对于测量坐标系的位姿，而通过个关节电机参数以及机械参数算出的指令位姿是相对于机器人基坐标系的位姿。两者之间相差一个从测量坐标系到基坐标系的变换，而现实中这个变换是不易获取的。这就导致了末端执行器相对于基坐标系的实际位姿是很难得到的。

但是，对于末端执行器的两种不同位姿状态，它们之间的差值是可以精确测量的。并且因为摄像头光心与末端执行器之间是刚性连接，所以在同一坐标系下，二者的位移相等，可以将摄像头光心的位姿变化差值等价于末端执行器的位姿变化差值。

如图7所示，设A_c与B_c分别是两个指令位姿，相应的实际位姿分别是A和B。若仅考虑位置上的差值，根据距离准确度的定义，可以得到：

另外也可以考虑距离精密度在不同坐标轴方向上的定义：

上式的向量坐标也需要在同一坐标系下表达，但相比于位置的坐标变换要更简单一下。因为它并不受坐标系的平移的影响，因此求取时无需关注两坐标系之间的平移变换。

本实施例中，通常选取电机转角值与末端执行器位姿变化值作为观测变量V，如下所示：

V_k＝[Θ₁...Θ_m ΔP_k]

上式表示第k组观测标量，其中Θ表示m个电机转动角度，ΔP_k表示末端执行器位姿变化值。而待定标量U由机器人需要标定的机械参数组成。这是结合机器人模型与使用需求确定的。约束条件可以表征为：

Γ_k＝f(V_k,U)

测得V后即可获得多组约束条件。在理想的U下，Γ为零。可以选用适合的数学优化方法最小化Γ，已获得U的最佳值。

高斯牛顿法是常用的解决非线性回归问题的手段。可以将t组测量值表达成误差矩阵的形式：

E(U,V)＝[Γ₁...Γ_t]

记误差矩阵的雅克比矩阵为J(U,V)，则可逐步优化待标定向量：

U_n+1＝U_n-J(U_n,V)⁺E(U_n,V)

视觉测量优化

本实施例中选择将棋盘格进行特殊编码，使得每个栅格对应位置都有一串唯一表示序列。这样可以保证在标定时运动范围可以更大，不需要让整个棋盘格都在摄像头视野中。

利用棋盘格的局部特征与整体属性，可以进行两个方面的优化。

如图5所示，考虑到角点q应满足性质：在q的图像邻域内的一点p处(p₀和p₁为样例点，p为这些点的集合)，其梯度与向量q-p垂直。即综合邻域内所有点，可以得到上式的最优解：

重复迭代这一过程可以得到精确到亚像素级别的角点位置。

另一方面，棋盘格打印的尺寸精度可以到0.01mm的数量级。利用尺寸信息可以在整体布局上对角点进行约束。下述为基于RANSAC对角点布局优化的算法：

若迭代达到指定次数或误差小于给定阈值，输出当前角点坐标为最优；否则继续。随机选取若干实测角点，根据其坐标计算出理想角点坐标集合。计算理想角点集合每对相邻点距离与给定尺寸之间的差的平方之和为当前误差。若当前误差小于最优误差，记录当前坐标集合和误差为最优值。返回1。

Claims (10)

1.一种基于单目视觉的六自由度机器人运动学标定方法，其特征在于，所述方法包括下列步骤：

1)建立六自由度机器人的MDH模型，并根据初始标定变量U得到六自由度机器人末端的指令位移和安装于六自由度机器人上的单目视觉传感器与六自由度机器人末端之间的坐标变换关系

2)根据六自由度机器人的初始标定变量U，利用绝对值编码棋盘对安装于六自由度机器人上的单目视觉传感器的多组位移Δ'进行分级测距并优化；

3)标定棋盘格坐标系与基坐标系的转换角α，并根据转换角α对位移Δ'进行坐标变换得到六自由度机器人末端的实际位移Δ，并得到多组观测变量V_k；

4)利用约束关系，结合步骤2)得到的多组观测变量V_k对标定变量U进行优化，判断标定变量U是否达到定位精度，若是则完成六自由度机器人的运动学标定，若否则返回步骤2)。

2.根据权利要求1所述的基于单目视觉的六自由度机器人运动学标定方法，其特征在于，所述六自由度机器人的MDH模型具体为5参数MDH模型，所述5参数包括关节角θ_i、杆件偏移量d_i、杆件长度L_i、扭转角α_i和扭转角β_i。

3.根据权利要求1所述的基于单目视觉的六自由度机器人运动学标定方法，其特征在于，根据初始标定变量U得到安装于六自由度机器人上的单目视觉传感器与六自由度机器人末端之间的坐标变换关系的方法包括单轴旋转法或双轴旋转法。

4.根据权利要求3所述的基于单目视觉的六自由度机器人运动学标定方法，其特征在于，所述单轴旋转法具体为：保持六自由度机器人的5个轴静止，只旋转最后1个轴，得到单目视觉传感器采集的图片，测量单目视觉传感器相对于棋盘格坐标系的位置，通过拟合得到单目视觉传感器的轨迹方程，结合当前机器人的末端执行器的位置即可得到单目视觉传感器与六自由度机器人末端之间的坐标变换关系。

5.根据权利要求3所述的基于单目视觉的六自由度机器人运动学标定方法，其特征在于，所述双轴旋转法具体为：保持六自由度机器人的4个轴静止，只旋转最后2个轴，拟合最后2个轴的轴线并将2个轴线的交点作为六自由度机器人的第六轴的坐标系的原点，通过单目视觉传感器与第六轴的坐标系的原点的位置关系得到单目视觉传感器与六自由度机器人末端之间的坐标变换关系。

6.根据权利要求1所述的基于单目视觉的六自由度机器人运动学标定方法，其特征在于，所述步骤2)具体为：

21)对绝对值编码棋盘的角点在像素坐标系下的位置坐标进行优化；

22)利用优化后的绝对值编码棋盘对安装于六自由度机器人上的单目视觉传感器的位移进行分级测距，得到多组位移Δ'，同时测量与位移Δ'对应的电机角度Θ。

7.根据权利要求6所述的基于单目视觉的六自由度机器人运动学标定方法，其特征在于，所述步骤21)具体为：

211)根据绝对值编码棋盘的局部特征对角点在像素坐标系下的位置坐标进行优化；

212)根据绝对值编码棋盘的整体属性，对优化后的角点在像素坐标系下的位置坐标进行进一步优化。

8.根据权利要求6所述的基于单目视觉的六自由度机器人运动学标定方法，其特征在于，所述位移Δ'为单目视觉传感器在绝对值编码棋盘下的中心栅格位置变化T与单目视觉传感器在中心栅格内部的相对位移T'之和。

9.根据权利要求8所述的基于单目视觉的六自由度机器人运动学标定方法，其特征在于，所述单目视觉传感器在绝对值编码棋盘下的中心栅格位置变化T具体为：根据识别中心栅格阵列的编码，确定单目视觉传感器所在的中心栅格的位置，继而确定单目视觉传感器在绝对值编码棋盘下的中心栅格位置变化T。

10.根据权利要求8述的基于单目视觉的六自由度机器人运动学标定方法，其特征在于，所述单目视觉传感器在中心栅格内部的相对位移T'具体为：

T'＝t_(i+1)k-t_ik

sp_ik＝A[R_ik t_ik]P_ik

其中，i＝1,2,...表示单目视觉传感器的不同位置，k表示角点的编号，p_ik和P_ik分别表示角点在像素坐标系与棋盘格坐标系下的坐标，s表示尺度参数，A表示单目视觉传感器内部参数，R_ik和t_ik分别表示单目视觉传感器当前位姿在棋盘格坐标系下的旋转和平移。