由于加工、装配、工作环境等因素，机械臂的实际结构参数与理论值存在一定偏差，使得机械臂运动学模型并不准确，从而影响机械臂定位精度. 机械臂标定就是应用先进的测量手段和基于模型的参数识别方法辨识出机械臂模型的准确参数，从而提高机械臂绝对定位精度^[1]，标定的结果是一组被识别的机械臂参数. Judd等^[2]的标定实验研究表明，运动学参数误差对机械臂定位精度的影响占所有因素的90%左右. 目前，国内外常用的运动学标定方法主要分为2种：运动学回路法和轴线测量法^[3]. 运动学回路法根据是否利用外界测量设备又可分为开环法和闭环法2类^[4]. Stone等^[5-6]利用轴线测量法建立各关节轴线的空间方程，轴线的相对位置即真实的运动学参数；Newman等^[7-9]分别利用激光跟踪仪、OPTOTRAK- CERTUSTM和Vxtrack测量得到机械臂实际末端位姿数据，都属于开环法. 上述方法都需要借助于外界先进的测量设备，成本高，且测量过程复杂，需要专业人员进行较长时间操作，因此比较适用于实验室环境下的标定而不适用于现场标定^[10]. 为了降低成本，许多学者提出闭环标定法（也称自标定），通过施加物理约束如点、线、面等，限制机器人末端执行器的运行空间，从各个关节编码器获得关节角度并通过正运动学得到末端位姿. 点约束对操作者要求较高，需低速点动操作机器人，以保证末端测头与孔或球契合^[11-12]，或者需要借助激光装置，从而增加标定成本与复杂度^[13]. 相比之下平面约束操作简单且成本低^[14-16]. Ikits等^[17]指出，在基于平面约束的机械臂标定中，根据机械臂末端各工具中心点（tool center point, TCP）位置数据拟合出的平面与实际约束平面往往存在一定偏差（点、线约束也存在此偏差），该偏差会直接影响标定精度. 虽然可以通过修正误差模型减小该偏差，但建立的误差模型复杂，且不能从根本上消除该偏差^{[16, 18]}.

针对上述情况，研究发现出现偏差的根本原因在于理论平面（即拟合平面）是利用机械臂在各位形下的TCP空间位置数据拟合得到的. 拟合必然存在偏差，如果存在某种方法，能够避免拟合过程而得到约束平面准确的空间方程，即可避免平面出现偏移现象. 因此提出一种方法，在实际约束平面上建立平面坐标系（planar coordinate system, PCS），即可得到约束平面在PCS中的准确方程，将TCP在基坐标系（base coordinate system, BCS）中的理论位置转换到PCS，并通过接触式测量头对约束平面进行接触式测量，得到TCP在PCS中的实际位置. 该方法由于PCS、机械臂末端工具坐标系（tool coordinate system, TCS）的存在，引入了冗余参数，因此将建立最小完整连续运动学模型，以减小冗余参数的影响. 为了实现自动化测量，提高标定效率，利用双目视觉定位约束平面并规划理论测量点位置，并利用改进的最小二乘法对参数误差进行辨识.

1 机械臂运动学误差模型 1.1 平面约束误差模型

基于平面约束的机械臂闭环标定，其原理是通过在机械臂工作空间内施加一个或多个物理约束平面，限制机械臂末端接触式测量头的运行空间，从各个关节编码器获得关节角度并通过正运动学得到机械臂在各个位形下TCP在BCS中的位置数据，这一系列名义TCP位置坐标在理想情况下都应满足平面方程：

$ax_{\rm N}^i + by_{\rm N}^i + cz_{\rm N}^i = [a,b,c]{{p}}_{\rm N}^i = 1.$

(1)

式中： $i = 1,2, \cdots ,n$ ，表示 $n$ 个位形； ${{p}}_{\rm N}^i = {[x_{\rm N}^i,y_{\rm N}^i,z_{\rm N}^i]^{\rm{T}}}$ 为对应的TCP位置坐标； $[a,b,c]$ 为拟合所得平面的平面系数. 该平面方程是在机械臂BCS中的一般表达式（平面不经过BCS原点）.

由于各连杆参数均存在误差，各名义TCP不在式（1）的平面上，与平面存在一定偏差：

$S = \sum\limits_{i = 1}^n {{{([a,b,c]{{p}}_{\rm N}^i - 1)}^2}\;} .$

(2)

${{p}}_{\rm N}^i$ 可以表达为机械臂各连杆参数以及各关节变量的非线性函数：

$[a,b,c]{ p}_{\rm N}^i{\rm{ = }}F(a,b,c,{{\phi}} ).$

(3)

式中： ${{\phi}} $ 包含了机械臂所有的运动学参数. 综合式（2）、（3），可得

$S = \sum\limits_{i = 1}^n {{{[F(a,b,c,{{\phi}} ) - 1]}^2}\;} .$

(4)

当 $S$ 取最小值时，得到的约束平面为最佳拟合平面. 对机械臂的标定问题最终转化为式（4）中 $S$ 最小值的求解问题，解集 ${{\phi}} $ 被认为是真实的运动学参数. $S$ 取最小值时得到的最佳拟合平面与实际的约束平面不可能完全重合，两者间的偏差如图1所示. 图中，实际平面通过光学测量设备测量得到，视作标准平面；拟合平面通过各TCP位置拟合得到. 这意味着如果用 $S$ 取最小值时对应的运动学参数更新运动学模型，重新通过正运动学计算机械臂在各位形下末端TCP的位置坐标，这些位置坐标点集只能更接近于拟合平面，而不是实际平面，显然降低了机械臂的标定精度.

如果存在某种方法，能够避免拟合过程而得到约束平面准确的空间方程，这将避免偏移现象. 如图2所示，在实际平面上建立PCS， $z$ 轴方向为平面法向，原点及 $x$ 、 $y$ 轴方向可任意选定. 先假设PCS与机械臂BCS的相对关系已知（具体方法将在2.2节说明），将机械臂末端TCP在BCS中的理论位置转换到PCS，通过正运动学得到TCP在PCS中的理论位置 ${{{P}}_{\rm N}}$ ；通过接触式测量头对约束平面进行接触式测量，得到TCP在PCS中的实际位置 ${{P}}$ ，如图2所示，实际位置在PCS的 $z$ 轴方向的分量 ${P_z}$ 恒等于0，即

${P_z} = 0,$

(5)

则

${P_z} - {P_{{\rm N}z}} = - {P_{{\rm N}z}} = \Delta {P_z} = {{{J}}_z}\Delta {{X}}.$

(6)

式中： ${P_{{\rm N}z}}$ 为 ${{{P}}_{\rm N}}$ 在PCS的 $z$ 轴方向的分量， $\Delta {P_z}$ 为 ${P_z}$ 与 ${P_{{\rm N}z}}$ 在 $z$ 轴方向的偏差， ${{{J}}_z}$ 为辨识雅克比向量， $\Delta {{X}}$ 为所需要辨识的所有运动学参数的误差向量. 式（6）即为所提出的平面约束误差模型.

1.2 最小完整连续运动学模型

建立合适、完整、准确的机械臂运动学模型是标定过程中首要的一步，根据该模型可以得知机械臂末端位姿与各运动学参数之间的函数关系，而要建立机械臂运动学模型，首先要确定各连杆坐标系. 目前，常用的连杆坐标系建立方法有2种^[19]：Paul法（标准D-H）和Craig法（改进D-H），都是在Denavit和Hartenberg提出的连杆参数（运动学参数）基础上提出或改进的，前者将坐标系建立在连杆的末端，后者建立在连杆首端，本研究使用Craig方法建立连杆坐标系. 研究对象是一般串联工业机械臂，根据Craig法建立的D-H坐标系如图3所示，其中4、5两连杆坐标系原点重合.

相邻两连杆坐标系 $i - 1$ 到 $i$ 的齐次坐标变换矩阵 ${ A}_i$ 可由4个连杆参数描述，如下：

$\begin{split}{{{A}}_i} = {{{R}}_{{x_{i - 1}}}}({\alpha _{i - 1}}){{{T}}_{{x_{i - 1}}}}({a_{i - 1}}){{{T}}_{{z_{i - 1}}}}({d_i}){{{R}}_{{z_{i - 1}}}}({\theta _i})=\quad\quad\quad\quad\quad\quad\\\left[\!\! {\begin{array}{*{20}{c}} {{\rm cos}\;{\theta _i}} &\!\! { - {\rm sin}\;{\theta _i}} &\!\! 0 &\!\! {{a_{i - 1}}}\\ {{\rm sin}\;{\theta _i}\;{\rm cos}\;{\alpha _{i - 1}}} &\!\!\!\! {{\rm cos}\;{\theta _i}\;{\rm cos}\;\,{\alpha _{i - 1}}} &\!\!\!\! { - {\rm sin}\;{\alpha _{i - 1}}} &\!\!\!\! { - {d_i}\;{\rm sin}\;{\alpha _{i - 1}}}\\{{\rm sin}\;{\theta _i}\;{\rm sin}\;{\alpha _{i - 1}}} &\!\!\!\! {{\rm cos}\;{\theta _i}\;{\rm sin}\;{\alpha _{i - 1}}} &\!\!\!\! {{\rm cos}\;{\alpha _{i - 1}}} &\!\!\!\! {{d_i}\;{\rm cos}\;{\alpha _{i - 1}}}\\0 &\!\!\!\! 0 &\!\! 0 &\!\! 1 \end{array}} \!\!\right]. \;\;\;\;\end{split}$

(6)

式中： ${a_{i{\rm{ - }}1}}$ 为 ${z_{i{\rm{ - }}1}}$ 沿 ${x_{i{\rm{ - }}1}}$ 方向平移至 ${z_i}$ 的距离， ${\alpha _{i{\rm{ - }}1}}$ 为 ${z_{i{\rm{ - }}1}}$ 绕 ${x_{i{\rm{ - }}1}}$ 轴旋转至 ${z_i}$ 的角度， ${d_i}$ 为 ${x_{i{\rm{ - }}1}}$ 沿 ${z_i}$ 或 ${z_{i{\rm{ - }}1}}$ （此时 ${z_i}$ 与 ${z_{i{\rm{ - }}1}}$ 重合）平移至 ${x_i}$ 的距离， ${\theta _i}$ 为 ${x_{i{\rm{ - }}1}}$ 绕 ${z_i}$ 或 ${z_{i{\rm{ - }}1}}$ 旋转至 ${x_i}$ 的角度. 需要注意的是，由于矩阵乘法不具有交换律，当用Craig法建立坐标系时，必须先关于 $x$ 轴变换，再关于 $z$ 轴变换，与Paul法变换顺序相反，因此2种方法对应的中间转换矩阵是完全不同的，不能混用^[19].

对于机械臂标定研究，各连杆之间不能视作完全平行或垂直，因此相关理论参数值存在微小偏差；在考虑相邻关节轴线近似平行（2、3关节）时，微小的不平行度会引起参数突变，式（7）所表示的4参数模型不再适用，需要增加绕 $y$ 轴旋转的参数 $\beta $ 才能准确描述 ${{{A}}_i}(i = 3)$ . 基于Craig法所建立的连杆坐标系，改进后的D-H模型完整表达式如下：

${{{A}}_i} = {{{R}}_{{x_{i - 1}}}}({\alpha _{i - 1}}){{{T}}_{{x_{i - 1}}}}({a_{i - 1}}){{{R}}_{{y_{i - 1}}}}({\beta _i}){{{T}}_{{z_{i - 1}}}}({d_i}){{{R}}_{{z_{i - 1}}}}({\theta _i}).$

(8)

式中： ${\beta _i} = 0\;(i \ne 3)$ ， ${d_i} = 0\;(i = 3)$ ，对于值为0的参数，不需要对其进行辨识.

由于PCS与机械臂BCS之间不遵循上述D-H参数建立规则，两坐标系之间的齐次矩阵可表示为

$\begin{split}{}^{\rm{p}}{{ A}_0} = {{ T}_{{x_{\rm{p}}}}}({d_{{x_{\rm{p}}}}}){{ T}_{{y_{\rm{p}}}}}({d_{{y_{\rm{p}}}}}){{ T}_{{z_{\rm{p}}}}}({d_{{z_{\rm{p}}}}}) {{ R}_{{x_{\rm{p}}}}}({\delta _{{x_{\rm{p}}}}}){{ R}_{{y_{\rm{p}}}}}({\delta _{{y_{\rm{p}}}}}){{ R}_{{z_{\rm{p}}}}}({\delta _{{z_{\rm{p}}}}}).\end{split}$

(9)

同样地，第6连杆坐标系与TCS之间的坐标变换矩阵表示如下：

$\begin{split}{}^6{{ A}_{\rm t}} = {{ T}_{{x_6}}}({d_{{x_6}}}){{ T}_{{y_6}}}({d_{{y_6}}}){{ T}_{{z_6}}}({d_{{z_6}}}) {{ R}_{{x_6}}}({\delta _{{x_6}}}){{ R}_{{y_6}}}({\delta _{{y_6}}}){{ R}_{{z_6}}}({\delta _{{z_6}}}).\end{split}$

(10)

式中： $d\_$ 和 $\delta\_$ 分别为关于其下标（_）的平移分量和旋转分量. 至此，共出现36（ $4 \times 6 + 1 - $ $1 + 6 \times 2$ ）个运动学参数.

Schröer等^[20]指出，机械臂的D-H参数完整表达姿态最小个数为 $4N + 6$ （ $N$ 为机械臂自由度），所以上述36个运动学参数中有6个参数是冗余的，这会造成参数难以辨识，因此需要剔除冗余参数. 按照D-H规则获得的参数不存在冗余问题，冗余参数主要存在于从PCS到第1连杆坐标系及第5连杆坐标系到TCS中.

首先讨论从PCS到第1连杆坐标系的冗余参数，由式（8）、（9）结合图2、3分析可知，从PCS到BCS中，沿 $z_{\rm p}$ 轴方向的偏移量 ${d_{ z_{\rm p}}}$ 及绕 $z_{\rm p}$ 轴的旋转量 ${\delta _{z_{\rm p}}}$ ，与从BCS到第1连杆坐标系沿 ${z_0}$ 轴的偏移量 ${d_1}$ 及绕 ${z_0}$ 轴的旋转量 ${\theta _1}$ 是线性相关的，所以，从PCS到BCS只需要4个参数即可（ ${d_{z_p}}$ 、 ${\delta _{z_p}}$ 可分别被包含在 ${d_1}$ 、 ${\theta _1}$ 中）：

${}^{\rm{p}}{{ A}_0} = {{ T}_{{x_{\rm{p}}}}}({d_{{x_{\rm{p}}}}}){{ T}_{{y_{\rm{p}}}}}({d_{{y_{\rm{p}}}}}){{ R}_{{x_{\rm{p}}}}}({\delta _{{x_{\rm{p}}}}}){{ R}_{{y_{\rm{p}}}}}({\delta _{{y_{\rm{p}}}}})\;.$

(11)

其次讨论从第5连杆坐标系到TCS的冗余参数，由于工具是固定安装在末端法兰上的，不存在自由度，通常认为TCS的姿态与第6连杆坐标系保持一致. 从第6连杆坐标系到TCS中，沿 ${z_6}$ 轴的偏移 ${d_{z_6}}$ ，与从第5连杆坐标系到第6连杆坐标系中沿 ${z_5}$ 轴的偏移量 ${d_6}$ 是线性相关的，所以从第6连杆坐标系到TCS只需要2个参数即可（姿态一致， ${d_{z_6}}$ 被包含在 ${d_6}$ 中）：

${}^6{{ A}_{\rm{t}}} = {{ T}_{{x_6}}}({d_{{x_6}}}){{ T}_{{y_6}}}({d_{{y_6}}})\;.$

(12)

综上所述，从PCS到TCS共有30（ $4 \times 6 + 1 - 1 +4+2 $ ）个运动学参数，符合最小完整连续运动学要求. 将 ${}^6 { A}_{\rm t} $ 表示成A₇，那么机械臂TCS相对于PCS的位姿矩阵 ${{ T}_{\rm t}}$ 可以表示为

$\begin{array}{l}\!\!\!\!\!\!\!\!\!{{ T}_{\rm{t}}} ={}^{\rm{p}}{{ A}_0}{{ A}_1} \cdots {{ A}_n}{}= \\\;\;{ f}_0^{\rm{p}}({b_1}, \cdots ,{b_4}){ f}_6^0({b_5}, \cdots ,{b_{28}}){ f}_{\rm{7}}^6({b_{29}},{b_{30}});\;n=7.\end{array}$

(13)

式中： ${b_i}\;(i = 1 ,\cdots, 30)$ 为需要辨识的各运动学参数. 特别需要注意的是，式（13）中有3个运动学参数已经被修正，具体如下：

$\left. \begin{split}d_1'&= {d_1} + {d_{{z_{\rm{p}}}}},\\{\theta _1'}&= {\theta _1} + {\delta _{{z_{\rm{p}}}}},\\d_6'&= {d_6} + {d_{{z_6}}}.\;\end{split} \right\}$

(14)

1.3 末端位置误差模型

由于运动学参数名义值存在误差，相邻两连杆间的实际坐标变换矩阵 ${{A}}_i^{\rm R}$ 与名义矩阵 ${{{A}}_i}$ 之间存在偏差. 根据微分变换思想，可知

${\rm d}{{{A}}_i} = {{A}}_i^{\rm R} - {{{A}}_i} = {{{A}}_i}{{{\varDelta}} _i}\;.$

(15)

式中： ${{{\varDelta}} _i}$ 为相邻两连杆坐标系间的微分变换矩阵.

对 ${{{A}}_i}$ 进行全微分，得到由连杆几何参数误差造成的相邻坐标系间的微分摄动齐次矩阵：

$\begin{split}{\rm d}{{{A}}_i} =& \displaystyle\frac{{\partial {{{A}}_i}}}{{\partial {\alpha _{i{\rm{ - }}1}}}}\Delta {\alpha _{i{\rm{ - }}1}} + \displaystyle\frac{{\partial {{{A}}_i}}}{{\partial {a_{i{\rm{ - }}1}}}}\Delta {a_{i{\rm{ - }}1}} + \displaystyle\frac{{\partial {{{A}}_i}}}{{\partial {\theta _i}}}\Delta {\theta _i} + \\&\displaystyle\frac{{\partial {{{A}}_i}}}{{\partial {d_i}}}\Delta {d_i} + \displaystyle\frac{{\partial {{{A}}_i}}}{{\partial {\beta _i}}}\Delta {\beta _i};\quad i = 1, \cdots ,6\;.\end{split}$

(16)

可以得到 ${{{\varDelta}} _i} = {({{{A}}_i})^{ - 1}}{\rm d}{{{A}}_i}$ ， ${{{\varDelta}} _i}$ 可以认为是由微分运动矢量 ${{{e}}_i}$ 构成的：

${{ e}_i} = {[{d_{{x_i}}},{d_{{y_i}}},{d_{{z_i}}},{\delta _{{x_i}}},{\delta _{{y_i}}},{\delta _{{z_i}}}]^{\rm{T}}}.$

(17)

式中：前3个元素为位置误差，后3个元素为姿态误差. 将e_i表示成运动学参数误差的形式：

${{{e}}_i} = {{{G}}_i}\Delta {{{X}}_i}\;.$

(18)

式中： $\Delta {{{X}}_i} \;=\; {[\;\Delta {\alpha _{i - 1}},\;\Delta {a_{i - 1}},\;\Delta {\theta _i},\;\Delta {d_i}\;]^{\rm T}}(i \ne 3) ,\;\Delta {{{X}}_i}\;\;=$ $[\;\Delta {\alpha _{i-1}},\;\Delta {a_{i-1}},\;\Delta {\theta_{i}},\;\Delta{\beta_{i}}\;]^{\rm T} \;(i=3),\;$ G_i $(i=0,\cdots,6) $ 为6×4误差系数矩阵.

${}^{\rm p}{{{A}}_0}$ 和 ${ A}_{7}$ 可以表示成式（18）的形式，区别在于： $\Delta {{{X}}_0} = {[\Delta {d_{x_{\rm p}}},\Delta {d_{y_{\rm p}}},\Delta {\delta _{x_{\rm p}}},\Delta {\delta _{y_{\rm p}}}]^{\rm T}},$ ${{{G}}_0}$ 为 $6 \times 4$ 误差系数矩阵； $\Delta {{{X}}_{7}} = {[\Delta {d_{x_6}},\Delta {d_{y_6}}]^{\rm T}},$ ${{{G}}_{\rm 7}}$ 为 $6 \times 2$ 误差系数矩阵.

${{{e}}_i}$ 只是相邻连杆间的误差，需要变换到TCS中：

${{e}}_i^{\rm t} = {{J}}_i^{\rm t}{{{e}}_i};\;\;i = 0,\; \cdots,\; 7.$

(19)

式中： ${{e}}_i^{\rm t}$ 为由第 $i$ 个连杆几何参数误差所引起的末端误差， ${{J}}_i^{\rm t}$ 为第 $i$ 个坐标系到TCS的 $6 \times 6$ 微分误差传递矩阵，可以通过第 $i$ 个连杆坐标系与TCS之间的变换矩阵直接求得.

机械臂末端位姿误差 ${{{e}}_{\rm T}}$ 是所有 ${{e}}_i^{\rm t}$ 的和：

${{{e}}_{\rm T}} = \sum\limits_{i = 0}^{\rm 7} {{{J}}_i^{\rm t}{{{G}}_i}} \Delta {{{X}}_i}.$

(20)

表示成矩阵形式为

${{{e}}_{\rm T}} = [{{J}}_0^{\rm t}{{{G}}_0} ,\cdots ,{{J}}_{\rm 7}^{\rm t}{{{G}}_{\rm 7}}]\left[ {\begin{array}{*{20}{c}}{{b_1}}\\ \vdots \\{{b_{30}}}\end{array}} \right] = {{{J}}^{\rm t}}\Delta {{X}}\;.$

(21)

式中： ${{{J}}^{\rm t}} = [{{J}}_0^{\rm t}{{{G}}_0}, \cdots ,{{J}}_{\rm 7}^{\rm t}{{{G}}_{\rm 7}}]$ ， ${{J}}_{\rm 7}^{\rm t}$ 实际上是 $6 \times 6$ 的单位阵.

可对式（6）作相关说明，取 ${{{e}}_{\rm T}}$ 的第3个元素 $\Delta {{P}_z}$ ，取 ${({ J}_0^{\rm {\rm t}})^{ - 1}}{{ J}^{\rm t}}$ 的第3行元素 ${{{J}}_z}$ ，即

$\Delta{{{P}}_z}={{ J}_z}\Delta {{X}},$

(22)

则

$ - {{ P}_{{\rm N}z}} = {{{J}}_z}\Delta {{X}}\;.$

(23)

测量获得 $n$ 组数据，可建立 $n$ 个约束方程：

${{H}}\Delta {{X}} = {{S}}\;.$

(24)

式中： ${{H}} = {[{{{J}}_{z1}}, \cdots ,{{{J}}_{zn}}]^{\rm T}}$ ， ${{S}} = {[ - {P_{{\rm N}z1}}, \cdots , - {P_{{\rm N}zn}}]^{\rm T}}$ .

2 数据测量 2.1 靶点三维定位

一般的平面约束标定方法，数据测量通过手动示教完成，本研究采用双目视觉系统定位约束平面基准，实现自动化测量. 由于双目视觉公共视场范围狭小，在约束平面上粘贴3个靶点，如图4所示，由此对约束平面的定位即可等效成对靶点的定位.

利用张正友标定法对左右2个相机分别进行内参标定，得到4参数内参矩阵：

${{{M}}_j} = \left[ {\begin{array}{*{20}{c}} {{f_{xj}}}&0&{{u_{j0}}}&0 \\ 0&{{f_{yj}}}&{{v_{j0}}}&0 \\ 0&0&1&0 \end{array}} \right].$

(25)

式中： ${f_{xj}},{f_{yj}}\;(j = 1,2)$ 分别为 $x$ 和 $y$ 方向的尺度因子， ${u_{j0}},{v_{j0}}\;(j = 1,2)$ 为左右2个相机的主点坐标.

对左右2个相机分别进行手眼标定，即确定摄像机坐标系相对于第6连杆坐标系的变换矩阵，利用传统手眼标定方法 ${{AX}} = {{XB}}$ ，使机械臂至少运动3个不同的姿态，分别计算获得左右2个相机的手眼矩阵 ${{{X}}_1}$ 、 ${{{X}}_2} $ ^[21]. 建立世界坐标系使之与第6连杆坐标系重合，并令 ${{L}} = {{{M}}_1}{{X}}_1^{ - 1}$ ， ${{R}} = $ $ {{{M}}_2}{{X}}_2^{ - 1}$ ，得到

$\left. {\begin{array}{*{20}{l}}{{ L} = \left[ {\begin{array}{*{20}{c}}{{l_{11}}}&\;{{l_{12}}}&\;{{l_{13}}}&\;{{l_{14}}}\\{{l_{21}}}&\;{{l_{22}}}&\;{{l_{23}}}&\;{{l_{24}}}\\{{l_{31}}}&\;{{l_{32}}}&\;{{l_{33}}}&\;{{l_{34}}}\end{array}} \right],}\\{{ R} = \left[ {\begin{array}{*{20}{c}}{{r_{11}}}&{{r_{12}}}&{{r_{13}}}&{{r_{14}}}\\{{r_{21}}}&{{r_{22}}}&{{r_{23}}}&{{r_{24}}}\\{{r_{31}}}&{{r_{32}}}&{{r_{33}}}&{{r_{34}}}\end{array}} \right].}\end{array}} \right\}$

(26)

设目标点在第6连杆坐标系（世界坐标系）中的三维位置坐标为 ${{{P}}_{Oi}} = {[{x_i},{y_i},{z_i}]^{\rm T}}(i = 1,2,3)$ ，可建立如下三维定位模型：

$Z_{{i}}^{ {\rm l}}\left[ {\begin{array}{*{20}{c}} u_{{i}}^{\rm l} \\ v_{{i}}^{\rm l} \\ 1\end{array}} \right] = \left[ {\begin{array}{*{20}{c}} {{l_{11}}}&{{l_{12}}}&{{l_{13}}}&{{l_{14}}} \\ {{l_{21}}}&{{l_{22}}}&{{l_{23}}}&{{l_{24}}} \\ {{l_{31}}}&{{l_{32}}}&{{l_{33}}}&{{l_{34}}} \end{array}} \right]\left[ {\begin{array}{*{20}{c}} {{x_i}} \\ {{y_i}} \\ {{z_i}} \\ 1\end{array}} \right],$

(27)

$Z_{{i}}^{\rm r}\left[ {\begin{array}{*{20}{c}} u_{{i}}^{\rm r} \\ v_{{i}}^{\rm r} \\ 1 \end{array}} \right] = \left[ {\begin{array}{*{20}{c}} {{r_{11}}}&{{r_{12}}}&{{r_{13}}}&{{r_{14}}} \\ {{r_{21}}}&{{r_{22}}}&{{r_{23}}}&{{r_{24}}} \\ {{r_{31}}}&{{r_{32}}}&{{r_{33}}}&{{r_{34}}} \end{array}} \right]\left[ {\begin{array}{*{20}{c}} {{x_i}} \\ {{y_i}} \\ {{z_i}} \\ 1 \end{array}} \right].$

(28)

式中： $Z_{{i}}^{ {\rm l}}$ 、 $ Z_{{i}}^{\rm r}$ 分别为目标点在摄像机坐标系下的深度信息. 联立式（27）、（28），可得

$\left. \begin{array}{l}\!\!\!\!(u_i^{\rm{l}}{l_{31}} - {l_{11}}){x_i} + (u_i^{\rm{l}}{l_{32}} - {l_{12}}){y_i} + (u_i^{\rm{l}}{l_{33}} - {l_{13}}){z_i} = {l_{14}} - u_i^{\rm{l}}{l_{34}},\\\!\!\!\!(v_i^{\rm{l}}{l_{31}} - {l_{21}}){x_i} + (v_i^{\rm{l}}{l_{32}} - {l_{22}}){y_i} + (v_i^{\rm{l}}{l_{33}} - {l_{23}}){z_i} = {l_{24}} - v_i^{\rm{l}}{l_{34}},\\\!\!\!\!(u_i^{\rm{r}}{r_{31}} \!-\! {r_{11}}){x_i} \!+\! (u_i^{\rm{r}}{r_{32}} \!-\! {r_{12}}){y_i} \!+\! (u_i^{\rm{r}}{r_{33}} \!-\! {r_{13}}){z_i} \!=\! {r_{14}} \!-\! u_i^{\rm{r}}{r_{34}},\\\!\!\!\!(v_i^{\rm{r}}{r_{31}} \!-\! {r_{21}}){x_i} \!+\! (v_i^{\rm{r}}{r_{32}} \!-\! {r_{12}}){y_i} \!+\! (v_i^{\rm{r}}{r_{33}} - {r_{23}}){z_i} \!=\! {r_{24}} - v_i^{\rm{r}}{r_{34}}.\end{array}\!\!\!\! \right\}$

(29)

式中： ${[u_{{i}}^{\rm l},v_{{i}}^{\rm l}]^{\rm T}},{[u_{{i}}^{\rm r},v_{{i}}^{\rm r}]^{\rm T}}$ 分别为目标点在左右2像素坐标系中的位置值，由此，可以计算得到 ${[{x_i},{y_i},{z_i}]^{\rm T}}$ ，即通过立体匹配定位 ${O_1}$ 、 ${O_2} $ 、 $ {O_3}$ 在BCS下的位置信息.

2.2 构建平面坐标系（PCS）

PCS原点取在 ${O_1}({x_1},{y_1},{z_1})$ 处，取 ${{{O}}_1}{{{O}}_2}$ 方向为 $x$ 轴方向并单位化：

${{x}} = \frac{{({x_2} - {x_1},{y_2} - {y_1},{z_2} - {z_1})}}{{\sqrt {{{({x_2} - {x_1})}^2} + {{({y_2} - {y_1})}^2} + {{({z_2} - {z_1})}^2}} }}.$

(30)

同理，取 ${{{O}}_1}{{{O}}_2} \times {{{O}}_1}{{{O}}_3}$ 的方向为 $z$ 轴方向，通过右手法则确定 $y$ 轴的方向，并分别进行单位化，确定最终的平面坐标系 $\{ oxyz\} $ ，如图5所示，坐标系参数均可通过 ${O_1},{O_2},{O_3}$ 的位置信息计算得到.

平面上的所有点在BCS下的三维位置都可以通过BCS的原点以及 $x,y$ 轴方向矢量描述：

${{P}} = { o}({x_1},{y_1},{z_1}) + a{{x}} + b{{y}}.$

(31)

式中： $o$ 点坐标及 ${{x}},{{y}}$ 均已知，通过改变参数 $a$ 、 $ b$ ，即可确定各约束点在基坐标系下的三维信息.

然而，由于双目视觉定位、机械臂等都存在一定量的误差，在实际测量过程中，测量头不能到达由式（31）所规划的理论坐标值，甚至会出现测量头已经接触实际约束平面，而控制器仍然控制其运动到理论约束点的情况. 因此，考虑实际因素，对式（31）进行如下修正：

${{P}} = { o}({x_1},{y_1},{z_1}) + a{{x}} + b{{y}} + c{{z}}\;.$

(32)

式中： $c$ 为约束平面的法向偏置参数，该参数保证了在实际测量过程中，在测量头到达由式（32）规划的理论约束点时，没有与实际约束平面相接触，从而避免碰撞. 然后继续沿着 $z$ 轴负方向逼近实际约束平面，最终通过测量头发出的接触信号来判断测量头是否与实际约束平面相接触. 若有信号，则立即记录机械臂当前的各关节角度值，并控制机械臂运动至下一个理论约束点，如此完成整个标定数据的测量过程.

3 实验及结果分析 3.1 实验系统

实验对象为埃夫特ER3C-C60本体，采用课题组自主研发的6R型串联工业机械臂控制系统；双目摄像机选用德国IMAGING工业相机；接触式测量头选用英国RENISHAW公司生产的MCP型测量头，在测量的5个方向上精度均达到 0.75 μm；大理石各表面（除底面外）的平面度均为3.5 μm，满足标定所需要的约束要求. 整个标定实验系统如图6所示.

3.2 实验过程与数据处理

通过Matlab相机标定工具箱，分别得到左右相机的4个内参数；通过 ${{AX}} = {{XB}}$ ，得到相机与机械臂之间的手眼关系矩阵；分别对左右相机采集得到的靶点图像进行灰度化、图像增强、分割、填充、特征提取等操作后得到各靶点的圆心坐标，为了提高精度，采用亚像素值. 截取其中部分图像如图7所示（实际采集的图像大小为 $640 \times 480$ ，单位：像素）. 图中，各坐标分别为靶点圆心 ${O_1}$ 、 $ {O_2}$ 、 $ {O_3}$ 在左右相机图像像素坐标系下的坐标值，分别代入式（29），即可得到各靶点在世界坐标系（第6连杆坐标系）下的三维位置坐标.

依次取 $a,b = 10n\;(n = 1,2, \cdots )$ ， $c$ =20，可得到约束平面上按矩形阵列分布的一系列约束点，在约束平面上的投影及测量路径如图8所示. 将约束点坐标导入到机械臂控制器，从而控制测量头对约束平面实现接触式测量. 如此在3个平面上共测量120个点，得到120组关节角数据，通过机械臂正运动学并转换至PCS中计算名义位置值，将其代入式（24）. 通过改进的最小二乘法对式（24）进行求解：

$\Delta { X} = {({{ H}^{\rm T}}{ H} + \mu { I})^{ - 1}}{{ H}^{\rm T}}{ S}.$

(33)

式中： ${ I}$ 为单位阵； $\mu $ 为方程的组合系数，取值0.001.

最终参数误差辨识结果如表1所示. 另外，由于构建了PCS及TCS而引入的其他运动学参数如下， $\Delta {d_{x_{\rm p}}}$ =1.552 4 mm， $\Delta {d_{y_{\rm p}}}$ = − 1.243 mm， $\Delta {\delta _{x_{\rm p}}}$ = − 0.126 6°， $\Delta {\delta _{y_{\rm p}}}$ =0.097 5°， $\Delta {d_{x_6}}$ = − 0.886 mm， $\Delta {d_{y_6}}$ = − 1.032 mm.

3.3 结果验证与分析

控制器使机械臂任意运动到50个位形，通过加拿大Creaform公司精度可达22 μm的光学测量设备Vxtrack380，测量得到每个位形的实际位置数据，同时记录下对应的关节角度值；通过理论运动学参数及修正后的运动学参数，分别计算得到机械臂末端对应的名义位置值及修正位置值；以测量得到的实际位置数据为基准，计算修正运动学参数前、采用基于一般平面约束误差模型修正后（修正1）、采用本研究提出的误差模型修正后（修正2）的机械臂在 $x,y,z$ 方向的位置误差. 如图9所示，取前10组数据，进行比较分析. 如表2、3所示，分别列出了不同位形下，利用2种方法修正前后的误差分布，各取5组数据. 表中， $ {\theta _{\rm{1}}}$ 、 ${\theta _{\rm{2}}} $ 、 ${\theta _{\rm{3}}} $ 、 ${\theta _{\rm{4}}} $ 、 ${\theta _{\rm{5}}} $ 、 ${\theta _{\rm{6}}} $ 分别为关节角度，（x_r，y_r，z_r）为测量得到的机械臂末端实际位置，（x_b，y_b，z_b）、（x_a，y_a，z_a）分别为一般标定方法修正前后的位置，（x_c，y_c，z_c）、（x_d，y_d，z_d）分别为所提方法修正前后的位置.

对于位置误差，计算公式如下：

$\Delta p = \sqrt {{{(x - {x_{\rm r}})}^2} + {{(y - {y_{\rm r}})}^2} + {{(z - {z_{\rm r}})}^2}} .$

(34)

如图9所示，修正运动学参数后，机械臂在 $x,y,z$ 方向的定位误差（ $\Delta p_x,\;\Delta p_y,\;\Delta p_z $ ）都明显减小. 比较50组数据发现，基于一般平面约束误差模型，可将机械臂绝对定位精度由1.234 mm提高至0.692 mm，而基于所提出的误差模型，可将绝对定位精度进一步提高至0.405 mm.

4 讨　论

（1）与一般平面约束标定方法不同，需要构建PCS，引入了PCS与BCS之间的参数误差，不过系统对该参数误差也进行了辨识，尽可能减小此误差对标定精度的影响，而由图9可知，平面偏移误差比坐标系间的转换误差对标定精度的影响更大. 通过双目视觉构建了PCS，但完全可以通过其他方式建立，比如控制机械臂末端运动至平面上不共线的3个点，获取各个点在BCS中的位置，从而建立PCS. 应用双目视觉是为了辅助定位约束平面，实现自动化测量数据，这为机械臂生产厂家对机械臂实现批量标定提供了思路. 使用视觉的另一个好处是可以规划测量点的位置，使其按一定规律有序分布（如图8所示），这为研究机械臂标定位形对参数辨识的影响奠定了基础.

（2）构建PCS时只对其 $z$ 轴方向提出了要求，因此在建立平面约束误差模型时，只能利用TCP在 $z$ 轴方向的信息. 然而，如果同时使用3个互相垂直的约束平面，建立PCS时可将其原点定在3个平面的交点处，坐标轴方向为各平面法向，对 $xoz$ 平面内的点测量时， ${P_y} = 0$ ，而对 $yoz$ 平面内的点测量时， ${P_x} = 0$ ，误差模型类似.

（3）目前很多标定方法虽然提高了机械臂绝对定位精度，但是大多需要使用先进的测量设备，操作复杂，比较适用于在实验室环境下由专业人员进行标定，并不适用于工业现场标定. 但在工业现场使用中，机械臂在受到碰撞或者使用一段时间后疲劳磨损，往往需要重新标定. 本研究提出的标定方法，成本低，只需要一个精度相对较高的平面即可完成，只要在机械臂工作空间内，可任意摆放平面位置，如果有双目视觉辅助定位，可以实现自动化标定并补偿，减轻了机械臂后期的维护工作.

5 结　语

基于平面约束，提出一种工业机械臂的闭环标定方法及其误差模型，消除了一般平面约束标定方法拟合平面与实际约束平面之间的偏差；为了避免冗余参数对参数辨识造成影响，建立了最小连续完整运动学模型；为了提高标定效率，应用双目视觉辅助定位约束平面，实现自动化测量数据；最终通过改进的最小二乘法辨识出了真实的运动学参数误差. 实验结果表明，修正运动学参数后，机械臂的绝对定位精度由1.234 mm提高到0.405 mm.