基于视觉的六自由度机械臂运动学参数辨识

摘要

提出一种新型低成本的基于单目视觉的机器人末端位姿测量方法，设计并实现了六自由度机械臂的运动学参数辨识。采用分级测量方法和标定板绝对编码方法，解决了当前视觉测量过程中测量范围小、测量精度受相机畸变影响大等问题；使用基于位置误差的运动学参数辨识模型和单目视觉测量系统，简化了机器人的标定过程。最后，通过实验验证了方案的实用性和有效性。

关键词

运动学参数标定; 单目视觉; 分级测量; 位置误差

精度是评价工业机器人性能最重要的指标之一，可以分为重复定位精度和绝对定位精度。重复定位精度衡量的是机器人空间定位的可重复性，一般由机器人的材料、制作工艺等因素决定。实际运行过程中，温湿度变化及负载变化等对其产生的影响较小，在现有加工工艺下，机器人的重复定位精度能保持较高水准。绝对定位精度描述的是末端到达指定工作空间的准确程度。除了环境及负载等因素外，机器人的控制模型参数是否准确也是影响机器人绝对定位精度的重要因素之一。研究表明，运动学中机械参数偏差所导致的绝对定位误差占系统总误差的65%~95%^[1]。受机械加工误差、安装误差等因素影响，机器人的实际结构参数（如机械臂长等）与理想结构参数之间存在偏差，导致控制模型不准确，严重降低机器人的绝对精度。提高机器人绝对定位精度主要有两种途径： 提高零部件加工精度、装配精度； 利用运动学标定技术对控制模型参数进行辨识，对末端运动偏差加以修正或补偿，实现更高精度的绝对定位。目前大多数理论研究与工程设计围绕着通过标定改善精度这一思路展开。实践表明，运动学标定已经成为有效且主要的修正机器人绝对精度的途径。

根据标定过程中使用的系统信息或测量手段的不同，运动学标定有以下3类方法^[2]：外部标定法、约束标定法和自标定法^[3,4]。外部标定法是利用额外的高精度测量工具测量机器人末端的运动误差，根据建立的参数辨识模型进行参数辨识。约束标定法是通过在机器人工作空间设置带约束的示教点，使得机器人末端在这几个示教点间运动，得到相应的 约束条件来进行运动学参数辨识。自标定法则是利用机器人自身的几何约束条件进行机器人标定。其中，外部标定法以其易于计算和实践的优势，被广泛应用于科研及工程现场。文献[5]中将外部标定法分为4个步骤进行： 建立运动学参数辨识模型； 利用外部设备对机器人末端位姿误差进行测量； 利用参数辨识模型和测量误差进行参数辨识优化； 利用优化结果进行精度补偿。

末端位姿误差测量是外部标定法中最为关键的一步。一般需要借助额外的位姿测量设备，测量机器人末端对应的实际位姿。当前主流的测量方法可以分为以下两类：① 通过位姿约束构成坐标空间闭环。在机器人工作空间施加形状已知的物理约束；或设定特定的路径轨迹，测量末端在该路径上特定方向的偏差，建立约束方程来求解机器人运动学参数^[6]，该测量方法往往需要配合测量仪器使用。② 使用高精度位姿测量仪器进行测量。常用的测量系统包括三坐标测量仪^[3]、自动经纬仪^[7]和激光跟踪仪^[8]等。

为了降低测量成本和操作复杂度，越来越多的学者采用视觉的方式进行高精度测量。利用视觉进行运动学参数辨识的过程一般分为两步^[9]：相机自标定和运动学参数自标定。Zhu等^[10]提出一种更适应于工业环境的相机参数标定方案；文献[11]证明了当目标距离相机较远的时候单目视觉在位姿测量上的效果明显优于双目视觉。除了传统的视觉测量外，Wang等^[12]提出了一种利用视觉并基于神经网络的方式进行位置误差估计；Stepanova等^[13]利用RGB-D的立体相机进行末端位姿的测量；Aitor等^[14]利用三个机载相机进行六足机器人的运动学测量。 Andreff等^[15]讨论了将视觉技术应用于工作空间较小的并联机器人的运动学参数标定；孙月海等^[16]利用单目视觉实现了对Delta机器人的零点位置标定。文献[17]和文献[18]对串联工业机器人的误差产生原因进行了充分的分析和建模；文献[19]则对更为复杂的主臂并联拓扑的混合机器人进行了运动误差分析和标定。针对六自由度工业机器人，Wang等^[20]提出了一种两步标定法进行运动学参数辨识，Lembono等^[21]使用2D激光做测量工具进行运动学参数辨识。

本文针对当前高精度测量设备昂贵以及操作复杂等现状，提出了一种基于单目视觉的位姿测量方案和分级测量方法，克服了视觉测量中测量空间小的局限，降低了相机畸变对测量精度的影响；建立了基于距离误差的参数辨识模型，结合基于视觉的测量方法，简化了标定流程；最后通过实验验证了本方案的可行性和有效性。

1 参数辨识模型

对运动学参数的辨识是进行运动学标定的根本任务。通过比较机器人正运动学模型计算出的理论位姿和利用测量设备测量的实际位姿之间的误差，利用相应的优化算法，对运动学参数进行拟合辨识。

1.1　运动学模型

文献[22]提出,一个有效的标定模型必须满足完备性、比例性和连续性的要求。但是DH（Denavit-Hartenberg）建模方法在连续性上有着巨大的缺陷，特别是对于两个平行的相邻轴而言。当两平行轴间有微小误差，按照DH建模原则，相邻两轴不平行时坐标系原点建立在两轴的公垂线上，此时两坐标系之间的关节参数偏距$d_i$的变化对两轴之间的夹角误差异常敏感^[23]。为解决此问题，本文采用了Samad等^[24]提出的5参数MDH（modified-DH）模型，即在DH模型的基础上增加了一个绕Y轴的扭角${\beta }_i$。该扭角定义为相邻平行关节轴$Z_i$与$Z_{i+\mathrm{1}}$与$X_i{O}_i{Z}_i$平面的夹角。所以MDH模型中相邻两平行关节之间的变换矩阵为

$$\begin{array}{l}{\mathit{T}}_{i,i+\mathrm{1}}={\mathit{T}}_{\mathrm{r}\mathrm{o}\mathrm{t}}(Z,{\theta }_i){\mathit{T}}_{\mathrm{t}\mathrm{r}\mathrm{a}}(Z,d_i){\mathit{T}}_{\mathrm{t}\mathrm{r}\mathrm{a}}(X,a_i)\cdot \\ {\mathit{T}}_{\mathrm{r}\mathrm{o}\mathrm{t}}(X,{\alpha }_i){\mathit{T}}_{\mathrm{r}\mathrm{o}\mathrm{t}}(Y,{\beta }_i)\end{array}$$

(1)

式中：${\mathit{T}}_{\mathrm{r}\mathrm{o}\mathrm{t}}(Z,{\theta }_i)$为绕Z轴旋转$\theta$_i的变换矩；${\mathit{T}}_{\mathrm{t}\mathrm{r}\mathrm{a}}(Z,d_i)$为沿Z轴平移$d_i$的变换矩阵；${\mathit{T}}_{\mathrm{t}\mathrm{r}\mathrm{a}}(X,a_i)$为沿X轴平移$a_i$的变换矩阵；${\mathit{T}}_{\mathrm{r}\mathrm{o}\mathrm{t}}(X,{\alpha }_i)$为绕X轴旋转${\alpha }_i$的变换矩阵；${\mathit{T}}_{\mathrm{r}\mathrm{o}\mathrm{t}}(Y,{\beta }_i)$为绕Y轴旋转${\beta }_i$的变换矩阵。

当相邻两关节不平行时，MDH模型与DH模型一致；当相邻两关节平行时，MDH模型中关节坐标系的建立与DH模型中关节坐标系建立方式一致，$d_i=\mathrm{0}$时，${\beta }_i$是否等于0，由机械误差决定，当相邻两平行关节轴有微小偏差时不为0。因此，MDH模型能够克服DH模型的不足。

本文研究的实体对象是伊雪松公司生产的EDV6007机械臂，其MDH模型参数如表1所示，建立的关节坐标系如图1所示。 图1　EDV6007的正运动学模型

表1 MDH模型参数理论值

Tab.1 Theoretical value of MDH model parameters

关节	$a$/mm	$\alpha$ /(°)	$d$/mm	$\theta$/(°)	$\beta$/(°)
1	180	90	383	180
2	600	180	0	90	0
3	0	90	0	0
4	0	90	680	180
5	0	-90	0	90
6	0	0	-137	0

Fig.1 EDV60007 kinematic model

1.2　末端误差计算模型

对机器人绝对定位误差进行测量时，往往涉及到测量坐标系和机器人基坐标系之间的坐标变换关系。一方面，这两者之间的转换关系往往难以精确测量；另一方面，在标定前增加测量该转换矩阵的环节无疑加大了标定过程的复杂度与操作难度，加大了对非专业人员的要求。为了实现更容易操作的标定方案，在文献[25,26,27]的基础上，设计了一种基于位移精度的末端误差计算模型，利用空间两点间的距离精度来衡量机器人的绝对定位精度。这样不仅能避免求取测量坐标系和机器人基坐标系之间转换矩阵引起的误差，而且能简化整个标定流程。

根据1.1节中建立的正运动学模型，可以得到在机器人基坐标系下，机器人末端的位置为末端坐标系到基坐标系转换矩阵${\mathit{T}}_{\mathrm{0},\mathrm{e}}$中第4列的前3项，即

$$\left\{\begin{array}{c}{x}_{\mathrm{R}}=T_{\mathrm{1},\mathrm{e}}(\mathrm{1,4})\\ y_{\mathrm{R}}=T_{\mathrm{1},\mathrm{e}}(\mathrm{2,4})\\ z_{\mathrm{R}}=T_{\mathrm{1},\mathrm{e}}(\mathrm{3,4})\end{array}\right.$$

(2)

如果用$P_{\mathrm{m}}(i,j)$和$P_{\mathrm{R}}(i,j)$分别表示通过相机位姿估计测量和理想正运动学模型得到的机器人处于指令轨迹上i点和j点时末端运动的位移，则通过位移得到的理想化约束关系为

$$f(X)={‖P_{\mathrm{R}}(i,j)-P_{\mathrm{m}}(i,j)‖}_{\mathrm{2}}=\mathrm{0}$$

(3)

式中：$P(i,j)$为工作空间中第i个点到第j个点的位移；X为所有关节参数的集合。

对于机器人工作空间内任意两点的位移，可以建立如式（3）所示包含25个MDH模型关节参数的方程。在实际实验过程中，可以设定一个中心点c，根据其他n组点到该中心点的位移误差，建立如下方程组：

$$\left\{\begin{array}{c}{f}_{\mathrm{1}}(X)={‖P_{\mathrm{R}}(\mathrm{1},c)-P_{\mathrm{m}}(\mathrm{1},c)‖}_{\mathrm{2}}=\mathrm{0}\\ f_{\mathrm{2}}(X)={‖P_{\mathrm{R}}(\mathrm{2},c)-P_{\mathrm{m}}(\mathrm{2},c)‖}_{\mathrm{2}}=\mathrm{0}\\ ⋮\\ f_n(X)={‖P_{\mathrm{R}}(n,c)-P_{\mathrm{m}}(n,c)‖}_{\mathrm{2}}=\mathrm{0}\end{array}\right.$$

(4)

式（4）是一个超定的非线性方程组，将对机器人运动学参数辨识的过程变成了一个带约束的非线性回归问题。利用高斯‒牛顿迭代优化算法对该超定的非线性方程求解。每两组指定位置之间距离误差对应的雅可比矩阵定义如下：

$$\mathit{J}=\left[\begin{array}{ccccccc}{j}_{\mathrm{1}}& j_{\mathrm{2}}& j_{\mathrm{3}}& j_{\mathrm{4}}& j_{\mathrm{5}}& j_{\mathrm{6}}& j_{\beta \mathrm{2}}\end{array}\right]$$

(5)

式中：$j_i$为距离误差对第i组关节参数的偏导，$j_i=\left[\begin{array}{cccc}\frac{\partial f}{\partial {\theta }_i}& \frac{\partial f}{\partial {\alpha }_i}& \frac{\partial f}{\partial a_i}& \frac{\partial f}{\partial d_i}\end{array}\right]$；$j_{\beta \mathrm{2}}$为距离误差对MDH模型中引入的扭角参数${\beta }_{\mathrm{2}}$的偏导，$j_{\beta \mathrm{2}}=\frac{\partial f}{\partial {\beta }_{\mathrm{2}}}$。

对于m组数据，可以建立一个m行的雅可比辨识矩阵。

$$\mathit{J}={\left[\begin{array}{cccc}{J}_{\mathrm{1}}& J_{\mathrm{2}}& \cdots & J_m\end{array}\right]}^{\mathrm{T}}$$

(6)

利用高斯‒牛顿迭代法进行非线性回归的参数辨识的伪代码如表2所示。

表2 高斯牛顿迭代法的运动学参数辨识

Tab.2 Gauss-Newton iterative algorithm for ( kinematic identification )

输入：初始化关节参数集合${\mathrm{X}}_0$ 输出：辨识后关节参数集合$\mathrm{X}$ 初始化：计算初始误差$\mathrm{e}\mathrm{r}\mathrm{r}=\mathrm{f}({\mathrm{X}}_0)$ do 计算雅可比矩阵$\mathrm{J}$； 更新关节参数$\mathrm{X}={\mathrm{X}}_{\mathrm{l}\mathrm{a}\mathrm{s}\mathrm{t}}+\mathrm{\alpha }{\mathrm{J}}^{+}\cdot {\mathrm{X}}_{\mathrm{l}\mathrm{a}\mathrm{s}\mathrm{t}}$； 更新误差$\mathrm{e}\mathrm{r}\mathrm{r}=\mathrm{f}(\mathrm{X})$； while $\mathrm{e}\mathrm{r}\mathrm{r}<\mathrm{\epsilon }$ and $\mathrm{k}>{\mathrm{k}}_{\mathrm{m}\mathrm{a}\mathrm{x}}$

2 单目视觉测量方法

一般而言，单目视觉测量主要通过识别黑白相间的平面标定板上的角点信息，然后利用透视N点定位算法（PnP算法）进行位姿测量。实际应用中有眼在手(eye-in-hand)和眼在外(eye-to-hand)两种形式。本文中由于设计了一种特殊的标定板，其体积不适合固定在机械臂上，所以采用的是眼在手的形式，也就是将相机固定在末端。

利用单目视觉测量有两个主要缺陷： 相机视野范围过大导致精度下降； 相机图像畸变引起测量误差。为了提高相机的测量精度，在实际标定过程中一般需要遵循以下准则： 应保持相机尽可能工作在较小的视野范围； 应保持棋盘格尽可能处于图片中间位置，以消除边缘处畸变过大引起的巨大误差。

这两个准则制约着视觉测量在工作空间更大的机械臂中的应用。相比于并联机器人，六自由度机械臂工作空间更大，而且姿态变化也更为复杂。用于测量并联机器人位姿的棋盘格明显不适用于六自由度机械臂的高精度测量。为了克服这两个问题，本文设计了一种分级测量的方案。

2.1　标定板二元编码

为了扩大机器人的活动范围，本文将传统的A4纸大小的平面标定板进行扩充，使得机械臂在不同的位姿下，不需要利用全部的平面标定板信息，只需要检测识别视野中心畸变最小的标定板信息。设计一种特殊标志，使得每次识别到的位于视野中心的角点阵列能对应于一个特定的位置。这种特殊标志不能影响对角点阵列的识别，同时易于识别，且有一定的抗干扰性。因此通过扩大平面标定板的方式能够扩大机器人的活动空间，同时减小因标定板信息在图像角落而引入相机畸变造成的测量误差。

基于此，设计了如图2所示的二元编码的棋盘格。其中有半圆符号的表示为1，没有半圆符号的表示为0。而且该半圆符号仅分布在方格的上半部，这样就可以有效地明确棋盘格的朝向。

图2 二元编码简化示意图

Fig.2 Schematic diagram of binary coding simplification

除了保证能准确地识别到特殊标号，每个角点阵列的编号唯一性也尤为重要。该问题的数学描述为：给定四元组$(M,N;m,n)$，其中$(m<M,n<N)$，通过二元编码，使得矩阵$M\times N$中任意的子矩阵$m\times n$均具有不同的二维编码。其中相机的有效范围$(m\times n)$可根据实际应用场景来选取。

本文利用文献[28]给出的由r阶本原多项式生成长度为$l={\mathrm{2}}^r-\mathrm{1}$的伪随机序列算法，构造n阶（不失一般性，可假定$m<n$）移位寄存器，获取长度为$S={\mathrm{2}}^n-\mathrm{1}$的一维编码，并将编码连续的$N-n+\mathrm{1}$个状态归为一组。由于相邻编码之间有$n-\mathrm{1}$个编码重复，可知每组对应的伪随机序列长度为N，共可获得K= S/(N-n+1)组。接着将得到的K组编码集合进行二维拓展。从矩阵的第1行开始，按行填充矩阵。将第i组长度为N的序列放置在第(i-1)·m+1行，并重复m行。

2.2　分级测量方法

如图3所示，利用分级测量系统计算相机在测量坐标系中的位姿时，首先仅使用视野中心方格附近的角点阵列，利用透视N点定位算法（PnP算法）计算出相机在该角点阵列处坐标系的位姿。

图3 分级位移测量示意图

Fig.3 Schematic diagram of hierarchical measurement method

其次利用不同角点阵列之间仅存在相对位移的约束，利用视觉识别每个角点阵列的编码，获得其在平面标定板中的绝对位置，将相机的运动划分为两部分。利用这种方式，不仅可以始终保证每次利用的信息都是图像中靠近中心区域的畸变小的信息，而且可以通过扩充棋盘标定板的大小，增大机器人末端的活动空间。

2.3　末端位姿测量

利用视觉测量一般得到的都是相机在测量坐标系中的位姿，但是利用正运动学模型计算得到的是机器人末端的位姿。所以，如何准确地获得机器人末端和相机坐标系之间的转换关系尤为重要。

当把相机利用夹具刚性固定在机器人末端，若只考虑位移误差，则只需要获得相机坐标系和末端之间的相对位移即可准确地用相机位置表示出末端的位置。一些学者采用了手眼标定的方案。对于标定过程，利用不精确的正运动学模型计算出手和眼的关系，并将这种关系应用于标定运动学模型显然是不合适的。所以本文提出了一种单轴旋转法来测量手和眼之间的位置关系，如图4所示。

图4 单轴旋转法示意图

Fig.4 Schematic diagram of single axis rotation method

将相机刚性固定在末端，单独旋转一个关节时，相机的轨迹必然在一个空间圆上，且该空间圆的轴线就是该旋转轴的Z轴。利用这一性质，可以通过单独旋转最后的第6轴和第5轴，得到最后两个坐标系的Z轴方程。根据DH建模规则，可以得到第6个轴处的坐标原点。而末端坐标和第6个轴的坐标只存在沿着第6个轴处坐标系Z轴的平移，由于该平移关系完全由最后一个杆长决定，而单一杆长的加工精度远高于连杆之间的组装精度，因此，利用最后一个连杆的杆长作为实际杆长，可以建立相机坐标和末端坐标之间的准确位移关系。即可以利用相机准确测量末端的位置。

3 实验验证

本文使用配备普通USB摄像头的相机，获取的图像分辨率为1 080像素×1 920像素，相机视野角度为120°，标定板为挤压定制的亚克力板，其表面平整度能够达到0.3 mm。相机位姿的计算是通过整个角点阵列计算的，在小范围内的起伏不会影响计算过程。棋盘格的打印通过高精度打印机打印，每个像素点边长能控制在0.01 mm以下。

3.1　单目视觉测量精度验证

本文利用带有编码器的传送带（编码器一格对应传送带位移0.021 663 mm）作为测量工具，设计如下实验进行相机测量精度验证，如图5所示。将棋盘格近似垂直放置，相机固定在传送带上，朝向棋盘格。整个测量过程保证相机能完整识别到中心的角点阵列。实验分三组进行，通过改变相机或棋盘格的摆放方式，使传送带的运动方向依次近似沿测量坐标系的X、Y和Z轴运动。每组实验通过控制传送带带动相机运动。记录每次运动时对应的编码器数值和相机的图片；通过编码器变化值计算出相机实际运动距离作为真实距离；利用图像处理计算得到的运动距离作为测量距离；计算两者之间的差。

图5 相机测量精度验证实验示意图

Fig.5 Schematic diagram of camera measurement accuracy verification experiment

考虑到单目测量系统中，相机在Z轴方向上距离标定板的距离对测量结果影响较大，通过设计两组实验进行验证： 改变相机和标定板之间的距离，进行多组实验； 选择合适的距离后进行多次测量。实验结果如图6所示。从图6a中可以看出，当相机与标定板之间的距离小于400 mm左右时，单目视觉测量系统的误差均值在0.05 mm以下。图6b中则是保持相机和标定板之间距离在200~400 mm（距离的最小值根据角点阵列是否全部处于图像中选择）之间多次测量的结果，可以看出本方案中的视觉测量误差在该范围内能保持在0.05 mm以下。因此，此视觉测量系统能满足标定过程中的精度要求。

图6 单目视觉测量精度验证实验

Fig.6 Monocular vision measurement accuracy verification experiment

3.2　运动学参数标定实验

1.2节中基于距离的末端误差函数，计算方法并不是简单地利用笛卡尔空间中距离的不变性，而是分别计算X、Y、Z三个方向的位移误差，涉及到测量坐标系和基坐标系之间的转换关系。本文利用水平仪（型号为DXL360S，精度为0.01°）使得标定板平面和机器人基座平面保持平行。因此可以假定机器人基坐标系和测量坐标系之间仅存在绕Z轴的旋转和简单的平移关系。对于本文采用的末端误差函数，可以忽略两者之间的平移关系。本文中控制机器人分别沿着基坐标系的X轴和Y轴运动，利用视觉测量出末端在测量坐标系中的轨迹。分别对两条轨迹利用最小二乘法进行拟合，获得基坐标系中X轴和Y轴在测量坐标系中的表示。即获得了两者绕Z轴的转角。这样就可以准确测得末端实际运行的位移误差。

本文选择第1个点作为其他点的起点（距离标定板300 mm），然后在以该点为球心的空间球（半径为75 mm，根据3.1中实验确定）上均匀选取94个测量点。标定前后总共94个测量位形上位置误差的变化情况如图7所示。表3为经过标定后运动学参数的变化值。标定前位置误差最大值为1.837 9 mm，平均值为1.132 5 mm，误差标准差为0.336 5；标定后位置误差最大值为1.045 9 mm，平均值为0.528 8 mm，标准差为0.217 7 mm。

图7 标定前后末端位移误差

Fig.7 End displacement error before and after calibration

表3 MDH模型参数修正值

Tab.3 Corrected value of MDH model parameters

关节	$\mathrm{\Delta }a$/mm	$\mathrm{\Delta }\alpha$ /(°)	$\mathrm{\Delta }d$/mm	$\mathrm{\Delta }\theta$/(°)	$\mathrm{\Delta }\beta$/(°)
1	0.155 4	0.885 2	0	0
2	-0.999 9	0.999 7	0	-0.157 0	0.132 8
3	-0.999 7	0.189 5	0.260 6	-0.495 0
4	-0.025 3	0.192 1	-0.332 6	0.996 2
5	-0.999 6	-0.018 2	0	0.501 2
6	0	0	0	0

3 结论

本文设计了一种低成本、操作简单的单目视觉测量方案。利用分级测量方法和绝对编码定位方法有效地扩大了利用视觉的测量空间，同时避免了由于图片边缘畸变引起的测量误差；通过视觉精度测量实验对此测量方案的有效性进行验证，结果表明，此测量系统在较大的活动范围内能保持较高的测量精度。但是，此视觉测量系统操作过程中需要保证足够的视野，使得机器人的运动空间局限在距离标定板的一定高度且近似朝向标定板的范围内。这种限制是进一步提高运动学参数辨识性能的关键阻碍。

另外，本文结合视觉测量方案和基于位置误差的辨识模型，设计了一种针对串联机器人的运动学参数辨识方案，具有一定的泛化性。在伊雪松EDV6007机械臂上进行的实验结果表明，能将其位置误差平均值从1.132 5 mm降低到0.528 8 mm。本文提出的辨识方案使用低成本的工业相机作为测量设备，辨识过程操作要求低，后续能将相机自标定和机器人标定整合为全自动化标定系统。