提高运动精度是机器人执行精密操作的基础。机器人在执行重载操作时，机器人本体和末端执行器易产生机械结构变形，从而引入运动学的非几何误差，因此必须对机器人运动的位置和姿态进行补偿。

提高机器人运动精度的方式有两种：一种是利用机器人定位精度的离线测量数据对机器人的运动学进行标定和补偿，另一种则是借助于外部测量设备为机器人运动提供在线全闭环反馈。

针对机器人运动学的标定和补偿问题，有学者在机器人运动学的基础^[1]上建立了考虑非几何误差的力学^[2]、热学^[3]机理的误差传递模型，但运动补偿的精度取决于模型的准确性，往往难以保证。一些学者则通过数据逼近的方式建立运动误差的数据模型，利用空间网格^[4]、多项式拟合^[5]、神经网络^[6]和Gauss过程回归^[7-8]等方法直接建立机器人构形空间与机器人运动学误差的映射关系，并取得了较好的实验结果。但建模补偿方法用于在线运动补偿时受采集的样本数据的分布和数量的制约。

现有的在线位姿测量方式主要包括视觉测量和激光测量两种。利用立体视觉^[9]系统获取位姿信息，实时性好，但由于受相机视野、分辨率等限制，一般取得的位置和姿态精度相对较低；激光跟踪仪^[10]则可利用反射靶球测量单个目标点的空间位置，如果将多个反射靶球固定于测量工具上，分别测量多个目标点的位置，则可以计算得到测量工具的位置和姿态^[11]，从而获得较高精度的6D位姿测量。但受限于一台激光跟踪仪同一时间只能测量一个目标点，这种测量方式只能在待测目标处在静止状态时进行测量。此外，还可通过3台以上的激光跟踪仪构建测量网络^[12]或者使用昂贵的6D位姿测量的T-Mac配件实现高精度位姿反馈，但这两种方式成本太高。

为了满足高精度的在线位姿反馈需求，本文提出一种结合两种运动补偿方式的6D位姿估计方法，如图 1所示。该方法利用一台激光跟踪仪进行在线测量，同时利用基于Gauss过程回归的运动学误差模型对其他目标点的位置进行预测，从而实现机器人末端的高精度的位姿测量。测量平差模型是一种基于最小二乘原理的测量数据处理方法，一些学者将其应用于不同测量设备的误差分配与综合^[13-14]，以提高测量精度，本文将利用测量平差法对机器人的末端位姿进行修正，得到的末端6D位姿可在运动精度要求较高的精密操作中，为机器人提供更精确的位姿反馈，从而实现更好的运动控制效果。

1 基于Gauss过程回归的运动误差模型 1.1 运动补偿问题描述

具有n个关节的机器人，假设其正逆运动学模型分别为Fkine(·)和Ikine(·)，则机器人末端位姿的理论值可表示为T^N=Fkine(q)，其中关节位形q ∈ $ {{\mathbb{R}}^{n\times 1}}$，T^N为4×4维齐次矩阵。

末端工件重力引入的非线性变形误差如图 2所示，是机器人关节位形的函数。假设建立运动学误差模型Ω(q)，则经补偿后的机器人末端的理论位姿可表示为

$ \boldsymbol{T}^{\mathrm{N}}=\operatorname{Fkine}(\boldsymbol{q}) \varOmega(\boldsymbol{q}). $

(1)

本文均采用D-H模型建立机器人运动学，记机器人末端的实测位姿为T^M，则补偿后的机器人运动学误差矩阵ΔT可表示为

$ \Delta \boldsymbol{T}=\left(\boldsymbol{T}^{\mathrm{N}}\right)^{-1} \boldsymbol{T}^{\mathrm{M}}=\varOmega(\boldsymbol{q})^{-1} \text { Fkine }(\boldsymbol{q})^{-1} \boldsymbol{T}^{\mathrm{M}} . $

(2)

当运动学误差较小时，误差矩阵可表示为

$ \Delta \boldsymbol{T}=\left[\begin{array}{cccc} 1 & -\delta \theta_{z} & \delta \theta_{y} & \delta_{x} \\ \delta \theta_{z} & 1 & -\delta \theta_{x} & \delta_{y} \\ -\delta \theta_{y} & \delta \theta_{x} & 1 & \delta_{z} \\ 0 & 0 & 0 & 1 \end{array}\right] . $

(3)

其中：δ_x、δ_y、δ_z分别表示沿着基础坐标系的X、Y、Z方向的微小位置偏差，δθ_x、δθ_y、δθ_z则分别表示绕X、Y、Z方向的微小旋转偏差。用e_p=(δ_x, δ_y, δ_z)表示机器人末端的位置误差向量，e_t=(δ_x, δ_y, δ_z, δθ_x, δθ_y, δθ_z)表示机器人末端的6D位姿误差向量。

1.2 运动学误差模型

Gauss过程回归在处理高维度、小样本、非线性等复杂的预测问题时具有较好的适应性。本节将利用Gauss过程回归建立机器人运动误差模型Ω(q)，实现机器人构形空间到工作空间位置误差的非线性映射。

假设由结构变形引入的机器人末端位置误差e_p服从Gauss分布，位置误差的测量值可表示为一个关于机器人关节位形的Gauss过程和一个独立分布的Gauss白噪声的叠加，即

$ \hat{e}=\varOmega(\boldsymbol{q})+\varepsilon. $

(4)

其中：Gauss过程Ω(q)是任意有限个具有联合Gauss分布的随机变量组成的集合，由均值函数m(q)和协方差函数k(q, q ′)确定^[15]；噪声ε服从Gauss分布ε_i~N(0, σ_n²)，σ_n²表示测量的综合误差。

记e =[e₁, e₂, …, e_m]^T为末端位置误差的某个分量的测量序列，作为已知输出样本，其中m=1, 2, …，为测量序号。相应的机器人的关节位形Q =[q₁, q₂, …, q_m]^T为输入样本，则已知输出样本的概率分布为

$ \boldsymbol{e} \sim N\left[0, \boldsymbol{K}(\boldsymbol{Q}, \boldsymbol{Q})+\sigma_{n}^{2} \boldsymbol{I}\right]. $

(5)

其中，K(Q, Q) 为关节位形Q的协方差矩阵。给定新的关节位形q^*，则由Gauss过程的定义可知，e和e^*的联合概率分布可表示为

$ \left[\begin{array}{c} \boldsymbol{e} \\ e^{*} \end{array}\right] \sim N\left(0,\left[\begin{array}{cc} \boldsymbol{K}(\boldsymbol{Q}, \boldsymbol{Q})+\sigma_{n}^{2} \boldsymbol{I} & \boldsymbol{K}\left(\boldsymbol{Q}, \boldsymbol{q}^{*}\right) \\ \boldsymbol{K}\left(\boldsymbol{q}^{*}, \boldsymbol{Q}\right) & k\left(\boldsymbol{q}^{*}, \boldsymbol{q}^{*}\right) \end{array}\right]\right). $

(6)

依据多维Gauss分布的性质，新输出的位置误差e^*服从后验分布e^*|Q, e, q^* ~N(e^*, cov(e^*))，令H=K(Q, Q) +σ_n²I，则e^*的均值和协方差分别为

$ \bar{e}^{*}=E\left[e^{*} \mid \boldsymbol{Q}, \boldsymbol{e}, \boldsymbol{q}^{*}\right]=\boldsymbol{K}\left(\boldsymbol{q}^{*}, \boldsymbol{Q}\right) \boldsymbol{H}^{-1} \boldsymbol{e}, $

(7)

$ \begin{gathered} \operatorname{cov}\left(e^{*}\right)=k\left(\boldsymbol{q}^{*}, \boldsymbol{q}^{*}\right)+\sigma_{n}^{2}- \\ \boldsymbol{K}\left(\boldsymbol{q}^{*}, \boldsymbol{Q}\right) \boldsymbol{H}^{-1} \boldsymbol{K}\left(\boldsymbol{Q}, \boldsymbol{q}^{*}\right). \end{gathered} $

(8)

至此，建立了基于Gauss过程回归的运动学误差模型，式(7)和(8)是Gauss过程回归的核心公式。可以看到，Gauss过程回归同时表示出了末端位置预测误差的均值和协方差，前者可作为新输出e^*的预测值，后者则描述了位置误差预测值e^*的置信域，这是后面目标点位置的测量平差的基础。

协方差函数常选用平方指数函数，即

$ k\left(\boldsymbol{q}_{a}, \boldsymbol{q}_{b}\right)=\sigma_{\mathrm{f}}^{2} \exp \left[-\frac{1}{2}\left(\boldsymbol{q}_{a}-\boldsymbol{q}_{b}\right)^{\mathrm{T}} \boldsymbol{M}_{\lambda}\left(\boldsymbol{q}_{a}-\boldsymbol{q}_{b}\right)\right], $

其中M_λ=diag{λ₁^-2, λ₂^-2, …, λ_n^-2}，λ_i为宽度参数，σ_f²为信号方差，其中n为输入样本中机器人关节自由度。函数参数λ_i和σ_f以及白噪声的方差σ_n²共同决定了Gauss过程回归模型的预测效果，可表示为参数集θ=[σ_f, σ_n, λ₁, λ₂, …, λ_n]。

Gauss过程回归模型的求解，就是利用输入输出样本数据求解出最优的参数集。本文采用最大似然估计法^[16]求解模型，取负对数似然函数为L(θ)=-log[p(e|Q, θ)]，其中p(e|Q, θ)为输出样本(位置误差e)的条件概率，可表示为

$ p(\boldsymbol{e} \mid \boldsymbol{Q}, \theta)=\frac{1}{(2 {\rm{ \mathsf{ π} }})^{\frac{n}{2}} \sqrt{|\boldsymbol{H}|}} \exp \left\{-\frac{1}{2} \boldsymbol{e}^{\mathrm{T}} \boldsymbol{H}^{-1} \boldsymbol{e}\right\}. $

(9)

对L(θ)求偏导数$ \frac{\partial L\left( \theta \right)}{\partial \theta }$，利用梯度下降法、牛顿法等优化方法则可求得似然函数L(θ)取最小值的参数集θ，即最优参数。

参数估计完毕，代入式(7)，就可以预测新输入的关节位形q^*下机器人末端的位置误差e^*。

2 基于模型和数据驱动的6D位姿估计

激光跟踪仪作为一种高精度、非接触式的坐标测量设备，能精确测量反射靶球在三维空间的位置信息，适合作为机器人末端位置的外部测量设备。如图 3所示，机器人末端装有测量工具，并固结多个位置不共线的反射靶球，作为待测目标点，由3个以上的目标点可确定机器人末端的位置和姿态。

3个目标点的位置数据的采集方式可分为离线测量和在线测量2种。离线测量需要在机器人执行操作之前对机器人在不同关节位形下的末端位置和姿态进行测量；在线测量只能实现机器人末端的单个位置的在线测量。

为了实现机器人末端位置和姿态的在线估计，本文提出将激光跟踪仪的在线测量和运动学误差模型的位置补偿结合在一起，如图 4所示，用于计算机器人的末端位姿的3个目标点位置，包括1个由激光跟踪仪测得的实测目标点位置和2个由运动学误差模型预测的目标点位置。预测目标点位置由机器人的理论运动学模型和Gauss过程回归的运动学误差模型叠加而得到，故上述测量方式是一种基于模型和数据驱动的6D位姿估计方法。具体测量步骤如下：

1) 标定坐标变换矩阵。

测量系统涉及多个坐标系，包括机器人基础坐标系{B}，激光跟踪仪坐标系{L}，测量工具坐标系{T}，机器人末端坐标系{E}。提前标定好机器人基础坐标系与激光跟踪仪坐标系的变换矩阵^LT_B和待量目标点在机器人末端TCP中心坐标系下的位置^EP₁、^EP₂和^EP₃。

2) 离线测量。

将机器人运动到不同关节位形q_i下，离线测量机器人末端的预测目标点在激光跟踪仪坐标系下的实测位置^LP_1i^M和^LP_2i^M，并依据机器人运动学模型，计算其理论位置^LP_1i^N和^LP_2i^N，其中i=1, 2, 3, …，表示关节位形序号，可得定位误差

$ \left\{\begin{array}{c} { }^{\mathrm{L}} \boldsymbol{\delta}_{1 i}={ }^{\mathrm{L}} \boldsymbol{P}_{1 i}^{\mathrm{M}}-{ }^{\mathrm{L}} \boldsymbol{P}_{1 i}^{\mathrm{N}}, \\ { }^{\mathrm{L}} \boldsymbol{\delta}_{2 i}={ }^{\mathrm{L}} \boldsymbol{P}_{2 i}^{\mathrm{M}}-{ }^{\mathrm{L}} \boldsymbol{P}_{2 i}^{\mathrm{N}}. \end{array}\right. $

(10)

式中^LP_1i^N=^LT_B·Fkine(q_i)·^EP₁。

3) 建立运动学误差模型。

以机器人的关节位形序列为输入样本，2个预测目标点的定位误差为输出样本，分别建立基于Gauss过程回归的机器人运动学误差模型Ω₁(q)和Ω₂(q)，并依据式(9)对模型参数进行训练。

4) 在线测量和模型预测。

机器人操作过程中，利用激光跟踪仪在线测量实测目标点的空间位置^LP_t^M，同时将机器人当前关节位形q_t作为机器人运动学误差模型的新的输入样本，由式(7)得到预测目标点的定位误差的预测值^Lδ_1t^Pre和^Lδ_2t^Pre，目标点的预测位置可表示为

$ \left\{\begin{array}{c} { }^{\mathrm{L}} \boldsymbol{P}_{1 t}^{\mathrm{Pre}}={ }^{\mathrm{L}} \boldsymbol{P}_{1 t}^{\mathrm{N}}+\varOmega_{1}\left(\boldsymbol{q}_{t}\right), \\ { }^{\mathrm{L}} \boldsymbol{P}_{2 t}^{\text {Pre }}={ }^{\mathrm{L}} \boldsymbol{P}_{2 t}^{\mathrm{N}}+\varOmega_{2}\left(\boldsymbol{q}_{t}\right). \end{array}\right. $

(11)

5) 计算6D位姿。

由当前关节位形下实测的目标点位置^LP_t^M和两组预测的目标点位置^LP_1t^Pre和^LP_2t^Pre，可计算测量工具坐标系在激光跟踪仪下的6D位姿^LT_T^Pre，通过坐标变换得到机器人末端TCP中心的6D位姿^BT_E^Pre=^LT_B^-1·^LT_T^Pre·^ET_T^-1。

3 基于测量平差的6D位姿修正

为了提高测量工具坐标系的位姿测量精度，本节提出基于冗余测量的6D位姿修正方法，利用平差模型对目标点的实测值和预测值进行修正。

3.1 冗余约束条件

如果将测量工具视为一个刚体，则反射靶球支座(目标点)之间的距离应当保持不变，可以作为测量平差的冗余约束条件。

如图 5所示，设某个时刻目标点的实测位置P^M坐标为(x₁, y₁, z₁)，2个目标点的预测位置P₁^Pre和P₂^Pre的坐标分别为(x₂, y₂, z₂)和(x₃, y₃, z₃)。

不同目标点i和目标点j之间的距离

$ d_{i j}=\sqrt{\left(x_{i}-x_{j}\right)^{2}+\left(y_{i}-y_{j}\right)^{2}+\left(z_{i}-z_{j}\right)^{2}}, $

$ i, j=1,2,3, $

且i≠j，则基于距离约束的平差条件方程可表示为

$ \left\{\begin{array}{l} \left(x_{1}-x_{2}\right)^{2}+\left(y_{1}-y_{2}\right)^{2}+\left(z_{1}-z_{2}\right)^{2}-d_{12}^{2}=0, \\ \left(x_{1}-x_{3}\right)^{2}+\left(y_{1}-y_{3}\right)^{2}+\left(z_{1}-z_{3}\right)^{2}-d_{13}^{2}=0, \\ \left(x_{2}-x_{3}\right)^{2}+\left(y_{2}-y_{3}\right)^{2}+\left(z_{2}-z_{3}\right)^{2}-d_{23}^{2}=0. \end{array}\right. $

(12)

令函数

$ \varGamma_{i j}=\left(x_{i}-x_{j}\right)^{2}+\left(y_{i}-y_{j}\right)^{2}+\left(z_{i}-z_{j}\right)^{2}-d_{i j}^{2}, $

则平差条件方程可简化为Γ = 0，其中Γ =[Γ₁₂, Γ₁₃, Γ₂₃]^T。

受随机误差的影响，目标点的实际测量结果并不能满足式(12)，测量工具坐标系的测量值由3个实测坐标，6个预测坐标以及3个实测距离组成，记为

$ \begin{gathered} \boldsymbol{L}_{\mathrm{M}}=\left[x_{1}, y_{1}, z_{1}, x_{2}, y_{2}, z_{2}, x_{3}, y_{3},\right. \\ \left.z_{3}, d_{12}, d_{13}, d_{23}\right]^{\mathrm{T}}, \end{gathered} $

则其闭合差可表示为E_M= Γ(L_M)。

测量值和条件方程是平差模型的已知量，而改正数为测量平差最终要求的未知量。

3.2 权阵的确定

权是衡量测量值的相对测量精度的数字特征，不同测量值之间权的取值决定了测量平差对改正数的分配。

1) 实测位置的权阵。

目标点的实测位置的方差由激光跟踪仪的测量误差确定。激光跟踪仪主要通过单频激光干涉测距和高精度的光栅编码度盘测角，分别获取测距值ρ_L、俯仰角V_L和水平角H_L，并以空间极坐标计算待测点空间三维坐标为(ρ_LsinV_LcosH_L, ρ_LsinV_LsinH_L, ρ_LcosV_L)，对空间三维坐标进行全微分，可得

$ \left[\begin{array}{c} \mathrm{d} X_{\mathrm{L}} \\ \mathrm{d} Y_{\mathrm{L}} \\ \mathrm{d} Z_{\mathrm{L}} \end{array}\right]=\boldsymbol{C}_{\mathrm{L}}\left[\begin{array}{c} \mathrm{d} H_{\mathrm{L}} \\ \mathrm{d} V_{\mathrm{L}} \\ \mathrm{d} \rho_{\mathrm{L}} \end{array}\right]. $

(14)

其中，

$ \boldsymbol{C}_{\mathrm{L}}=\left[\begin{array}{ccc} -\rho_{\mathrm{L}} \sin V_{\mathrm{L}} \sin H_{\mathrm{L}} & \rho_{\mathrm{L}} \cos V_{\mathrm{L}} \sin H_{\mathrm{L}} & \sin V_{\mathrm{L}} \cos H_{\mathrm{L}} \\ \rho_{\mathrm{L}} \sin V_{\mathrm{L}} \cos H_{\mathrm{L}} & \rho_{\mathrm{L}} \cos V_{\mathrm{L}} \sin H_{\mathrm{L}} & \sin V_{\mathrm{L}} \\ 0 & -\rho_{\mathrm{L}} \sin V_{\mathrm{L}} & \sin V_{\mathrm{L}} \end{array}\right], $

由协方差传播律得到直角坐标的协方差矩阵

$ \boldsymbol{D}_{{XYZ}}^{\mathrm{M}}=\boldsymbol{C}_{\mathrm{L}} \boldsymbol{D}_{\rho \mathrm{HV}}^{\mathrm{M}} \boldsymbol{C}_{\mathrm{L}}^{\mathrm{T}}. $

(15)

其中，由于距离和角度测量相互独立，球坐标的协方差矩阵D_ρHV^M=diag{σ_H², σ_V², σ_ρ²}为对角阵，σ_V和σ_H为俯仰角和水平角的测角均方差，σ_ρ为测距均方差。取距离1 m的测量均方差为单位权均方差σ₀，则工具坐标系目标点的实测位置的权阵可表示为

$ \boldsymbol{P}^{\mathrm{M}}=\left(\boldsymbol{D}_{X Y Z}^{\mathrm{M}}\right)^{-1} / \sigma_{0}^{2}. $

(16)

2) 预测位置的权阵。

预测目标点的测量误差源于激光跟踪仪的测量误差和Gauss过程回归预测的误差。Gauss过程回归不仅输出了基于样本数据的预测结果，也给出了预测值的置信度，即预测值的均方差σ^GPR，可用于运动学误差模型的预测误差的定权。实测误差和预测误差相互独立，则二者叠加后的预测值的总方差可表示为(σ^Pre)²=(σ^M)²+(σ^GPR)²。

由于目标点的预测位置的X、Y和Z坐标分别由不同的Gauss过程回归模型预测，则预测位置的协方差矩阵为对角阵，即

$ \boldsymbol{D}^{\mathrm{Pre}}=\left[\begin{array}{ccc} \left(\sigma_{X X}^{\mathrm{M}}\right)^{2}+\left(\sigma_{X}^{\mathrm{GPR}}\right)^{2} & 0 & 0 \\ 0 & \left(\sigma_{{YY}}^{\mathrm{M}}\right)^{2}+\left(\sigma_{{Y}}^{\mathrm{GPR}}\right)^{2} & 0 \\ 0 & 0 & \left(\sigma_{Z Z}^{\mathrm{M}}\right)^{2}+\left(\sigma_{Z}^{\mathrm{GPR}}\right)^{2} \end{array}\right]. $

(17)

其中，(σ_XX^M)²、(σ_YY^M)²和(σ_ZZ^M)²分别为激光跟踪仪X、Y、Z坐标的方差，也是协方差矩阵D_XYZ^M的对角元素。

目标点的预测位置的权阵可表示为

$ \boldsymbol{P}^{\text {Pre }}=\left(\boldsymbol{D}^{\text {Pre }}\right)^{-1} / \sigma_{0}^{2}. $

(18)

3) 距离测量的权阵。

目标点之间的距离可以使用激光跟踪仪或者更高精度的三坐标测量机进行测量。其中三坐标测量机的长度测量误差一般以固定误差和比例误差的形式，即σ_d=a_CMM+d/b_CMM，其中σ_d的单位为μm。不同的距离值的测量相互独立，故距离测量的协方差矩阵为对角阵，即

$ \boldsymbol{D}_{\mathrm{d}}^{\mathrm{M}}=\left[\begin{array}{cc} \left(a_{\mathrm{CMM}}+\frac{d_{12}}{b_{\mathrm{CMM}}}\right)^{2} & \\ & \left(a_{\mathrm{CMM}}+\frac{d_{13}}{b_{\mathrm{CMM}}}\right)^{2} \\ & &\left(a_{\mathrm{CMM}}+\frac{d_{23}}{b_{\mathrm{CMM}}}\right)^{2} \end{array}\right] . $

距离测量的权阵可表示为

$ \boldsymbol{P}_{\mathrm{d}}^{\mathrm{M}}=\left(\boldsymbol{D}_{\mathrm{d}}^{\mathrm{M}}\right)^{-1} / \sigma_{0}^{2}. $

(19)

至此，最终的权阵可由3部分独立测量的权阵共同组成，即

$ \boldsymbol{P}_{\mathrm{M}}=\left[\begin{array}{llll} \boldsymbol{P}^{\mathrm{M}} & & & \\ & \boldsymbol{P}_{1}^{\text {Pre }} & & \\ & & \boldsymbol{P}_{2}^{\text {Pre }} & \\ & & & \boldsymbol{P}_{\mathrm{d}}^{\mathrm{M}} \end{array}\right]_{12 \times 12}. $

(20)

3.3 平差模型的解算

条件方程是测量平差的数学模型，权阵是测量平差的随机模型，改正数则是平差解算的未知量，记为Δ_M，测量工具的目标点的平差值则可表示为

$ \boldsymbol{L}_{\mathrm{A}}=\boldsymbol{L}_{\mathrm{M}}+\boldsymbol{\varDelta}_{\mathrm{M}}. $

(21)

由于距离约束的条件方程为非线性方程，可通过泰勒展开线性化处理，得到改正数的线性条件方程^[17]

$ \boldsymbol{C}_{\mathrm{M}} \boldsymbol{\varDelta}_{\mathrm{M}}+\boldsymbol{E}_{\mathrm{M}}=\bf{0}. $

(22)

其中：$ \mathit{\boldsymbol{C}}{\mathit{\boldsymbol{}}_{\rm{M}}} = {\left. {\frac{{\partial \mathit{\boldsymbol{ \boldsymbol{\varGamma} }}}}{{\partial \mathit{\boldsymbol{L}}}}} \right|_{{\mathit{\boldsymbol{L}}_{\rm{M}}}}}$为3×12的系数矩阵，E_M为3×1的列向量，Δ_M为12×1的列向量，未知数有12个大于方程数。

改正数的选取一般遵循最小二乘准则，即要求Δ_M^TP_MΔ_M最小，故测量平差的解算问题实际上是一个带约束的最优化问题：

$ \begin{gathered} \min \boldsymbol{\varDelta}_{\mathrm{M}}^{\mathrm{T}} \boldsymbol{P}_{\mathrm{M}} \boldsymbol{\varDelta}_{\mathrm{M}} \\ \text { s. t. } \quad \boldsymbol{C}_{\mathrm{M}} \boldsymbol{\varDelta}_{\mathrm{M}}+\boldsymbol{E}_{\mathrm{M}}=\bf{0}. \end{gathered} $

(23)

可用拉格朗日乘数法求解，引入乘数λ，则可构造综合目标函数

$ \boldsymbol{\varPhi}=\boldsymbol{\varDelta}_{\mathrm{M}}^{\mathrm{T}} \boldsymbol{P}_{\mathrm{M}} \boldsymbol{\varDelta}_{\mathrm{M}}-2 \boldsymbol{\lambda}^{\mathrm{T}}\left(\boldsymbol{C}_{\mathrm{M}} \boldsymbol{\varDelta}_{\mathrm{M}}+\boldsymbol{E}_{\mathrm{M}}\right), $

求函数极值得到

$ \boldsymbol{\varDelta}_{\mathrm{M}}=\boldsymbol{P}_{\mathrm{M}}^{-1} \boldsymbol{C}_{\mathrm{M}}^{\mathrm{T}} \boldsymbol{\lambda}. $

(24)

代入式(23)，有

$ \boldsymbol{C}_{\mathrm{M}} \boldsymbol{P}_{\mathrm{M}}^{-1} \boldsymbol{C}_{\mathrm{M}}^{\mathrm{T}} \boldsymbol{\lambda}+\boldsymbol{E}_{\mathrm{M}}=\bf{0} . $

(25)

由于C_MP_M^-1C_M^T为满秩矩阵且可逆，方程有唯一的解λ =-(C_MP_M^-1C_M^T)^-1E_M，代入式(24)则可计算得到改正数Δ_M，将其代入式(21)，就得到最终的平差值L_A。

至此，测量平差模型解算完毕，利用测量平差修正3个目标点的位置，即可求得更高精度的6D位姿。

4 实验 4.1 实验平台

为了验证本文提出的机器人运动误差补偿方法，本文搭建了如图 6所示的测量实验平台，平台由ABB IRB 4600机器人、Leica AT901-B激光跟踪仪、多目标测量工具和主控上位机组成。多目标测量工具由铝材质本体和3个磁性靶座组成，尺寸为150 mm×200 mm，固定有3个1.5″反射靶球。

在进行运动补偿之前，需要预先标定激光跟踪仪相对于机器人基础坐标系的变换矩阵。设目标点在激光跟踪仪坐标系下的空间位置为^LP_T1、^LP_T2和^LP_T3，则可求出测量工具坐标系的实测位姿^LT_T^M。

4.2 变形误差补偿实验

为了获取不同负载下机器人末端的变形补偿效果，实验选取一个铝合金工件作为基础负载，并在此基础上增加了不同数量的Q235材质的配重，分别模拟4种不同负载下机器人的操作工况(5.25、12.10、41.15和46.05 kg)，其中最大负载接近IRB 4600机器人的最高负载。实验中分别测量了181组关节位形下目标点1在激光跟踪仪坐标系下的空间坐标。实验中选择前150组作为训练样本，后31组作为测试样本，分别对每组负载建立机器人的运动误差模型。由于IRB 4600机器人为6自由度机器人，输入样本为关节位形q ∈$ {\mathbb{R}^{6 \times 1}}$，输出样本为位置误差向量e_p∈$ {\mathbb{R}^{3 \times 1}}$，可由式(3)计算得到，训练样本容量m=150，误差模型的参数集θ={σ_f, σ_n, λ₁, λ₂, …, λ₆}，利用梯度下降法求解运动误差模型的最优参数。

图 7给出了46.05 kg负载下对运动学误差模型的预测效果，可以看到，模型的预测误差能够很好地逼近机器人运动学的实测误差。表 1列出了不同负载下模型预测误差的均值和均方差，结果显示随着负载的增大，模型的预测误差有所下降，但仍然能够将机器人运动学的误差均值控制在±0.025 mm以内，均方差控制在0.029 mm以内。

此外，实验对比了46.05 kg负载下基于Gauss过程回归(GPR)和基于径向基函数(radial basis function，RBF)神经网络的运动学误差模型的预测效果。RBF神经网络是一种强大的神经网络模型，具有自学习、自组织的特点，能够很好地逼近非线性连续函数^[18]。实验中从181组关节位形中随机选取不同数量的输入输出样本进行训练，并计算模型预测的误差，以观察样本数量对误差预测的影响。

图 8给出了不同模型的位置综合误差σ_xyz= $ \sqrt {\sigma _x^2 + \sigma _v^2 + \sigma _z^2} $随着训练样本数量的变化。可以看到，随着样本数量减小，基于RBF神经网络^[18]的运动学误差模型的预测误差明显增大，而基于Gauss过程回归的运动学误差模型则变化没有那么明显，在样本数量有限的情况下具有更好的预测效果。

上述结果表明，基于Gauss过程回归的运动学误差模型能够对机器人在不同负载下的结构变形误差起到很好的补偿效果。

4.3 6D位姿估计实验

实验测量了不同运动轨迹下测量工具坐标系的位置和姿态。将运动轨迹插值为多个路径点，机器人运动到指定路径点后，依次测量3个目标点的空间位置。依据每个路径点的3个目标点的空间位置可由式(3)计算测量工具坐标系的实测值^LT_T^M。

如图 9所示，实验分别对空间直线和空间圆两种不同的运动轨迹进行了测量，相关实验参数如表 2所示。将目标点1作为实测目标点，目标点2和3作为预测目标点，从路径点中随机选取120组作为离线测量的样本数据，建立运动学误差模型，并对剩余的路径点的目标点位置进行预测，结合目标点1的实测位置，即可计算得到测量工具坐标系的预测值^LT_T^Pre和位姿预测误差向量e_t。

图 10和图 11分别给出了空间直线和空间圆轨迹下的姿态的预测误差，可以看到姿态的预测值能很好地逼近实测值。表 3列出了位姿估计误差的统计结果。可以看到，空间直线轨迹的姿态误差均值都很小，Euler角EZ的均方差较大，主要是因为随机选取的样本所覆盖的范围有限，导致对目标点Z向坐标的预测效果差异较大。空间圆轨迹的姿态误差相对更小，Euler角EX和EY的误差主要源于目标点X向和Z向坐标的预测误差。

上述结果表明，本文提出的位姿估计方法能精确获取机器人末端测量工具的位置和姿态。

为了验证基于测量平差的6D位姿修正方法，实验使用海克斯康Global Plus三坐标测量机完成了测量工具上3个目标点之间的距离测量，结果如表 4所示。

实验中采用的三坐标测量机的长度测量误差为(2.5+3L/1 000)μm/m，AT901-B激光跟踪仪的测距误差的标称值为±0.5 μm/m，测角误差为1″~2″，代入式(20)、(21)可求解平差模型。表 5给出了空间圆轨迹的第163个路径点上目标点2和3的平差结果，包括平差前目标点的预测位置P^Pre、平差后的位置P^Adj以及改正数Δ_M。为了对比平差前后的效果，表 5同样列出了激光跟踪仪对目标点2和3的实测位置P^M，可以看到，平差后的坐标值更接近于激光跟踪仪的实测位置。

依据上述方法，实验计算了所有路径点下，平差前后的目标点2和3的预测位置到激光跟踪仪实测位置的距离L_i=‖ P_i^Pre- P_i^M‖，将其作为目标点2和目标点3的定位误差，其中i为路径点序号。统计所有路径点的定位误差，并计算其均值E(L_i)=ΣL_i和均方差RMSE(L_i)=Σ[L_i-E(L_i)]²/m，如表 6所示。

可以看到，经过平差模型的修正后，目标点2和目标点3的预测位置比平差前更接近于激光跟踪仪的实测值，平差后的定位精度的均值提高了20%左右，均方差变化不大。上述结果表明基于测量平差的位姿修正方法能够有效地提高机器人末端目标点的估计精度。

5 结论

本文针对机器人在重载操作下运动补偿问题展开研究，提出了基于模型和数据驱动的6D位姿在线估计方法，同时结合了基于离线测量的运动学误差模型和在线测量反馈，实验结果很好地验证了该方法能够对机器人末端位置和姿态进行精确测量。在机器人执行精密操作中，本文提出的6D位姿估计方法可提供更精确的位姿反馈，从而实现更好的运动控制效果。需要指出的是，本文所提出的方法受限于离线测量数据，仅适用于特定负载下的运动学补偿，否则，负载的改变同样会改变运动学误差模型，必须重新采集新的输入输出样本。理论上，在Gauss过程回归模型的输入样本信息中增加末端负载信息，建立更复杂的运动学误差模型，则可以适用于负载改变情况下的末端位姿估计。此外，如何通过减少离线测量数据的影响、增加在线测量数据的影响，以实现更精确的6D位姿反馈，是后续研究中值得关注的问题。