基于双目视觉的6D位姿测量误差补偿方法

A Compensation Method for 6D Pose Measurement Error Based on Binocular Vision

摘要

针对双目视觉设备测量误差较大且单点测量误差在视野范围内分布不一致， 导致测量6D位姿精度和稳定性较差的问题， 提出了一种测量误差补偿方法。为了解决传统回归模型在小样本多输入多输出情况下拟合效果较差的问题， 采用基于多输出最小二乘支持向量回归（MLSSVR）的算法， 实现了单点测量误差准确预测， 并采用遗传算法对模型超参数进行全局寻优； 为了实现跟踪坐标系的位姿测量误差补偿， 结合位姿测量原理， 将线性问题转化为最优估计问题， 通过最小化坐标变换误差函数， 将多个单点预测误差转化为多点耦合误差， 从而实现了6D位姿测量误差补偿。实验结果表明， 所提方法将跟踪坐标系的平均位置测量误差在3个方面上分别降低了63.4%、 45.2%和75.0%， 平均姿态测量误差分别降低50%、 48.4%和53.1%； 与其它现有补偿方法相比， 所提方法能显著提高双目视觉设备的测量精度， 并保证不同观测角度下的测量误差补偿有效性。

Abstract

To address the issue of large measurement errors in binocular vision devices and the inconsistent distribution of single-point measurement errors within the field of view， which leads to poor accuracy and stability in the measurement of 6D poses a measurement error compensation method has been proposed. To solve the problem of poor fitting effects of traditional regression models in the case of small samples and multiple inputs and outputs， an algorithm based on multi-output least squares support vector regression （MLSSVR） is adopted， which achieves accurate prediction of single-point measurement errors， and a genetic algorithm is used to optimize model hyperparameters globally. To realize the pose measurement error compensation of the tracking coordinate system， combined with the principle of pose measurement， the linear problem is transformed into an optimal estimation problem. By minimizing the coordinate transformation error function， multiple single-point prediction errors are converted into multi-point coupled errors， thereby achieving 6D pose measurement error compensation. Experimental results show that the proposed method reduces the average position measurement error of the tracking coordinate system by 63.4%， 45.2% and 75.0%， and the average attitude measurement error by 50%， 48.4% and 53.1%， respectively. Compared with other existing compensation methods， the method proposed can significantly improve the measurement accuracy of binocular vision devices and ensure the effectiveness of measurement error compensation from different observation angles.

Graphical abstract

关键词

双目视觉 / 6D位姿 / 误差补偿 / 遗传算法 / 最优估计

Key words

binocular vision / 6D pose / error compensation / genetic algorithms / optimal estimation

0 引　言

相比于传统的测量方式， 视觉测量作为一种非接触式的测量方式具有更大的测量范围与更快的测量速度， 被广泛应用于零件尺寸测量、 工业机器人及移动机器人导航等领域^［1‐3］。目前在机器人视觉引导领域， 为实现机器人的高精度定位需求， 大多采用商用的双目视觉设备直接测量机器人6D位姿信息^［4‐6］。然而， 由于双目视觉设备存在测量误差， 且测量误差在双目视觉设备视野范围内分布不一致， 导致双目视觉设备测量的机器人6D位姿数据不准确， 严重制约了双目视觉设备的进一步推广与应用^［7‐8］。因此， 有必要针对双目视觉设备的测量误差补偿方法展开研究。

目前， 国内外已有不少学者针对双目视觉设备的测量误差补偿进行研究， 并取得了一定的成果。孟环标^［9］建立了双目视觉设备测量误差模型， 从理论上分析了双目视觉设备各结构参数与测量误差之间的关系， 并通过实验进行了验证。Jin等^［10］针对双目视觉测量系统中误差因子灵敏度进行了分析， 通过BP神经网络建立总测量误差与各误差因子的映射关系， 得到了各误差因子对测量精度的影响大小。崔恩坤等^［11］提出了一种基于局部视场的双目视觉设备优化方法， 减小了标定和测量过程中信息非一致性引入的系统误差。Huo等^［12］提出了一种双目视觉测量系统精确标定方法， 利用固定约束关系将双目视觉测量系统的两个相机绑定成为一个相机， 并通过改进的迭代算法计算得到高精度的相机参数。Hong等^［13］提出了一种基于新型深度学习模型的高精度圆形标志点识别方法， 利用改进的YOLOv4模型检测圆形标记， 然后基于显著性目标检测模型提取轮廓， 最后采用最小二乘法得到圆形标志点中心， 进而提高了双目视觉装置三维测量精度。除了从单一环节进行测量误差补偿， 还有学者针对最终测量结果展开补偿方法研究。卫松^［14］提出了基于空间误差相似度的双目相机测量误差补偿方法， 通过空间采样建立了双目视觉设备测量误差与跟踪坐标系位置之间的映射模型， 并利用映射模型实现位置测量误差预测， 但该方法直接采样对象为坐标系而非视觉标志点， 未考虑双目视觉设备不同观测角度对坐标系测量误差所带来的影响。

通过对以上双目视觉设备测量误差补偿方法研究分析可知， 现有的补偿方法大多集中在由图像到最终位姿数据过程中的某一环节， 包括相机标定、 图像处理和匹配算法等， 没有对最终的跟踪模型进行补偿， 补偿效果相对有限； 少数研究从全局误差考虑建立预测模型， 虽能降低最终测量误差， 但模型的适用范围较差。

对于商用的双目视觉设备而言， 其底层算法通常不对用户开放， 难以对位姿数据计算过程进行优化。从整体角度考虑， 通过建立误差预测模型直接补偿最终测量结果， 理论上能达到补偿效果最大化， 但需要模型具有较好的普适性， 使其在不同测量状态下都能得到较为一致且稳定的预测结果。为此， 本文将分析双目视觉设备的跟踪原理， 并针对存在的测量误差分布不一致问题， 提出一种双目视觉设备测量误差补偿方法。首先， 建立单点测量误差预测模型， 并对模型参数进行优化； 然后， 结合位姿测量原理， 将预测得到的单点误差转化为多点耦合误差， 采用耦合误差对坐标系误差进行表征， 从而实现跟踪模型坐标系误差的有效补偿； 最后， 搭建实验平台验证所提方法的有效性。

1 双目视觉测量系统跟踪原理

双目视觉系统的单点测量几何关系模型如图 1 所示。其中， o₁X₁Y₁和o₂X₂Y₂分别为成像面1和 2 中的图像坐标系， 其原点与各自相机透镜中心的连线o₁O₁和o₂O₂形成两个相机的光轴， f₁和f₂分别为两个图像坐标系原点到透镜光学中心的距离， O₁O₂的连线形成基线， 长度为B， 以两镜头透镜中心连线O₁O₂的中点O为原点， O₁O₂所在直线为X轴建立双目视觉测量系统坐标系OXYZ。

由以上模型的几何关系可以得到测量目标点P在双目视觉测量系统坐标系OXYZ下的位置坐标为

\left\{\begin{array}{l}{X}_w=\frac{B\mathrm{c}\mathrm{o}\mathrm{t}({\alpha }_{\mathrm{1}}+{\beta }_{\mathrm{1}})}{\mathrm{c}\mathrm{o}\mathrm{t}({\alpha }_{\mathrm{1}}+{\beta }_{\mathrm{1}})+\mathrm{c}\mathrm{o}\mathrm{t}({\alpha }_{\mathrm{2}}+{\beta }_{\mathrm{2}})}+\frac{B}{\mathrm{2}},\\ Y_w=y_{\mathrm{1}}\cdot \frac{z\mathrm{c}\mathrm{o}\mathrm{s}{\beta }_{\mathrm{1}}}{f_{\mathrm{1}}\mathrm{s}\mathrm{i}\mathrm{n}({\alpha }_{\mathrm{1}}+{\beta }_{\mathrm{1}})}=y_{\mathrm{2}}\cdot \frac{z\mathrm{c}\mathrm{o}\mathrm{s}{\beta }_{\mathrm{1}}}{f_{\mathrm{2}}\mathrm{s}\mathrm{i}\mathrm{n}({\alpha }_{\mathrm{2}}+{\beta }_{\mathrm{2}})},\\ Z_w=\frac{B}{\mathrm{c}\mathrm{o}\mathrm{t}({\alpha }_{\mathrm{1}}+{\beta }_{\mathrm{1}})+\mathrm{c}\mathrm{o}\mathrm{t}({\alpha }_{\mathrm{2}}+{\beta }_{\mathrm{2}})}\mathrm{。}\end{array}\right.（1）

双目视觉系统测量单点只能获取位置信息， 若想进一步得到姿态信息， 需建立跟踪模型， 如图 2 所示。

在跟踪的物体上建立虚拟的跟踪坐标系， 并将其与3个以上不在同一直线上的视觉标志点进行绑定， 得到各标志点在跟踪坐标系下的位置关系。在实际跟踪时， 双目视觉设备对视觉标志点进行测量， 并通过事先绑定得到的位置关系可间接计算出跟踪模型在双目视觉系统坐标系下的位姿信息。

由于双目视觉系统的直接测量对象为视觉标志点， 而单点测量误差在双目视觉系统视野范围内分布不一致， 当双目视觉系统从多个角度进行模型坐标系跟踪时， 如图 3 所示， 即使跟踪坐标系位于双目视觉系统视野同一位置处， 视觉标志点位置变化也会使得测量误差发生变化， 最终导致不同站位下得到的模型坐标系位姿数据存在较大差异。因此， 直接对跟踪坐标系建立误差预测模型难以满足普适性要求。

而对单个视觉标志点进行分析， 由于双目视觉系统的几何结构参数一般在出厂后都可视为固定不变， 故双目视觉系统中由相机结构参数误差所引起的单点测量误差相对稳定。同时， 由于双目视觉系统采用了固定的相机标定、 特征点提取、 模板匹配等算法， 由算法引起的单点测量误差在一定程度上也可视为稳定。因此， 通过采样和训练建立的单点误差预测模型理论上能够满足普适性要求。

2 单点测量误差预测

为实现单点测量误差预测， 需建立合适的误差回归模型。在机器学习中常用的回归模型有线性回归、 逻辑回归、 多项式回归、 支持向量回归等， 以及部分神经网络模型。由于单点测量误差由多种误差因素耦合而成， 具有高度非线性， 可采用非线性映射能力较强的支持向量回归和神经网络。考虑到测量误差需手动采样， 难以获取大数据量的样本， 因此， 小样本泛化能力较强的支持向量回归（Support Vector Regression， SVR）更适合作为预测模型。最小二乘支持向量回归（Least Squares Support Vector Regression， LSSVR）作为一种改进的SVR算法， 最早由Suykens等^［15］提出， 其采用了二次损失函数， 并将不等式约束改为等式约束， 避免了求解二次规划问题的复杂计算， 提高了优化求解效率^［16］。但传统LSSVR模型只适用于多输入单输出回归， 而单点误差为三维的笛卡尔空间位置向量， 因此， 本文将采用一种多输出最小二乘支持向量回归（Multi-Output Least Squares Support Vector Regression， MLSSVR）作为单点测量误差预测模型。

2.1 基于MLSSVR的误差预测模型

MLSSVR的核心思想是将优化问题中的回归权值向量范数、 单个输出变量拟合误差以及样本整体拟合误差同时进行最小化。

具体推导过程如下：

设多输出样本集为Z=\{({\mathit{X}}_i,{\mathit{Y}}_i)|{\mathit{X}}_i\in {\mathrm{R}}^m,{\mathit{Y}}_i\in {\mathrm{R}}^n,i=\mathrm{1,2},\cdots ,l\}， 则模型的目标函数和约束方程为

\begin{array}{c}\mathrm{m}\mathrm{i}\mathrm{n}\frac{\mathrm{1}}{\mathrm{2}}{\displaystyle \sum _{j=\mathrm{1}}^n}||{\mathit{W}}_j||+\frac{C}{\mathrm{2}}{\displaystyle \sum _{i=\mathrm{1}}^l}{\displaystyle \sum _{j=\mathrm{1}}^n}{e}_{i,j}^{\mathrm{2}}+C_{\mathrm{0}}{\displaystyle \sum _{i=\mathrm{1}}^l}{\displaystyle \sum _{j=\mathrm{1}}^n}\left|e_{i,j}\right|\\ s.t.y_{i,j}={\mathit{W}}_{\mathit{j}}^{\mathbf{T}}\varphi ({\mathit{X}}_i)+b_j+e_{i,j},\end{array}（2）

式中： W_j 为第j维输出的权重系数； C为对样本整体误差平方和的惩罚系数； C₀为对样本整体误差绝对值的惩罚系数； e_{i,j}为第i个样本的第j维输出误差。

针对式（2）， 引入Lagrange乘子{\alpha }_i， 则其Lagrange函数为

\begin{array}{c}L({\mathit{W}}_j,b_j,e_{i,j},{\alpha }_{i,j})=\frac{\mathrm{1}}{\mathrm{2}}{\displaystyle \sum _{j=\mathrm{1}}^n}||{\mathit{W}}_j|{|}^{\mathrm{2}}+\\ \frac{C}{\mathrm{2}}{\displaystyle \sum _{i=\mathrm{1}}^l}{\displaystyle \sum _{j=\mathrm{1}}^n}{e}_{i,j}^{\mathrm{2}}+C_{\mathrm{0}}{\displaystyle \sum _{i=\mathrm{1}}^l}{\displaystyle \sum _{j=\mathrm{1}}^n}\left|e_{i,j}\right|+\\ {\displaystyle \sum _{i=\mathrm{1}}^l}{\displaystyle \sum _{j=\mathrm{1}}^n}{\alpha }_{i,j}\left[y_{i,j}-{\mathit{W}}_{\mathit{j}}^{\mathbf{T}}\varphi ({\mathit{X}}_i)-b_j-e_{i,j}\right]\mathrm{。}\end{array}（3）

根据KKT条件^［17］， 对式（3）分别求关于 W_j， b_j， e_{i,j}， {\alpha }_{i,j}的偏导得

\left\{\begin{array}{l}\frac{\partial L}{\partial {\mathit{W}}_j}={\mathit{W}}_j-{\displaystyle \sum _{i=\mathrm{1}}^l}{\displaystyle \sum _{j=\mathrm{1}}^n}{\alpha }_{i,j}\varphi ({\mathit{X}}_i)=\mathrm{0},\\ \frac{\partial L}{\partial b_j}={\displaystyle \sum _{i=\mathrm{1}}^l}{\displaystyle \sum _{j=\mathrm{1}}^n}{\alpha }_{i,j}=\mathrm{0},\\ \frac{\partial L}{\partial e_{i,j}}=C{e}_{i,j}+C_{\mathrm{0}}{M}_j-{\displaystyle \sum _{i=\mathrm{1}}^l}{\displaystyle \sum _{j=\mathrm{1}}^n}{\alpha }_{i,j}=\mathrm{0},\\ \frac{\partial L}{\partial {\alpha }_{i,j}}={\displaystyle \sum _{i=\mathrm{1}}^l}{\displaystyle \sum _{j=\mathrm{1}}^n}(y_{i,j}-{\mathit{W}}_{\mathit{j}}^{\mathbf{T}}\varphi ({\mathit{X}}_i)-b_j-e_{i,j})=\mathrm{0}\mathrm{。}\end{array}\right.（4）

消去 W_j， e_{i,j}， 记{\mathit{\Omega }}_{kj}=\varphi ({\mathit{X}}_k{)}^{\mathrm{T}}\varphi ({\mathit{X}}_j)=K({\mathit{X}}_k,{\mathit{X}}_j)(k,j=\mathrm{1,2},\cdots ,l)， 并引入高斯核函数K({\mathit{X}}_k,{\mathit{X}}_j)=\mathrm{e}\mathrm{x}\mathrm{p}(-||{\mathit{X}}_k-{\mathit{X}}_j|{|}^{\mathrm{2}}/\mathrm{2}{\sigma }^{\mathrm{2}})， 得线性方程组为

\left[\begin{array}{cc}\mathrm{0}& {\mathit{I}}^{\mathrm{T}}\\ \mathit{I}& \mathit{\Omega }+C^{-\mathrm{1}}\mathit{I}\end{array}\right]\left[\begin{array}{c}{b}_j\\ {\mathit{\alpha }}_j\end{array}\right]=\left[\begin{array}{c}\mathrm{0}\\ {{\mathit{Y}}_j}^{\mathrm{*}}\end{array}\right],（5）

式中：{\mathit{\alpha }}_j=[{\alpha }_{\mathrm{1},j}{\alpha }_{\mathrm{2},j}\cdots {\alpha }_{i,j}{]}^{\mathrm{T}}，{\mathit{Y}}_j=[y_{\mathrm{1},j}{y}_{\mathrm{2},j}\cdots y_{l,j}{]}^{\mathrm{T}}， {{\mathit{Y}}_j}^{\mathrm{*}}={\mathit{Y}}_j+(C_{\mathrm{0}}/C){\mathit{M}}_j， \mathit{I}={[\mathrm{1}\mathrm{1}\cdots \mathrm{1}]}^{\mathrm{T}}， {\mathit{M}}_j为l维列向量， 其取值为

（6）

根据式（5）求解{\alpha }_j和b_j可以得到第j维的回归函数

f_j(\mathit{X})={\displaystyle \sum _{i=\mathrm{1}}^l}{\alpha }_{i,j}K(\mathit{X},{\mathit{X}}_i)+b_j\mathrm{。}（7）

将采样点在双目视觉系统下的位置坐标以及当前坐标下的位置误差分别作为输入和输出数据， 训练得到所有的回归函数， 即最终的误差预测模型。由于超参数集\mathit{\theta }=[C,C_{\mathrm{0}},\sigma ]对MLSSVR模型的预测精度有较大影响， 需要对其进行优化， 考虑到模型训练在离线状态下进行， 不需要考虑训练的时间成本， 因此， 本文将采用具有较强全局寻优能力的遗传算法对MLSSVR的超参数进行优化。

2.2 基于遗传算法的超参数优化

遗传算法（Genetic Algorithm， GA）是一种模拟遗传进化过程的计算模型， 可以根据选取的适应度函数， 通过遗传过程中的选择、 交叉、 变异操作对个体进行筛选， 最终得到优化问题的最优解。利用GA进行参数优化的步骤如下：

1） 初始化： 针对超参数集\mathit{\theta }=[C,C_{\mathrm{0}},\sigma ]分别设定3个参数的取值范围， 令C\in [C_{\mathrm{m}\mathrm{i}\mathrm{n}},C_{\mathrm{m}\mathrm{a}\mathrm{x}}]， C_{\mathrm{0}}\in [C_{\mathrm{0}\mathrm{m}\mathrm{i}\mathrm{n}},C_{\mathrm{0}\mathrm{m}\mathrm{a}\mathrm{x}}]， \sigma \in [{\sigma }_{\mathrm{m}\mathrm{i}\mathrm{n}},{\sigma }_{\mathrm{m}\mathrm{a}\mathrm{x}}]。适应度函数选择均方根误差（RMSE）。

RMSE=\sqrt[]{\frac{\mathrm{1}}{M}{\displaystyle \sum _{i=\mathrm{1}}^M}({\widehat{y}}_i-y_i{)}^{\mathrm{2}}}，（8）

式中： {\widehat{y}}_i为验证集的预测值， y_i为验证集的实际值， M为验证集样本数量。

2） 编码： 在选择、 交叉、 变异操作之前， 需先对优化参数进行编码， 常用的编码方式有二进制编码、 格雷码、 浮点数编码等。本文选择二进制编码， 以参数C的编码为例， 其编码计算如下

l_{\mathrm{1}}=\mathrm{l}\mathrm{o}{\mathrm{g}}_{\mathrm{2}}\left[(C_{\mathrm{m}\mathrm{a}\mathrm{x}}-C_{\mathrm{m}\mathrm{i}\mathrm{n}})/S\right]，（9）

式中： l₁为C的编码长度， S为搜索精度。同理可得到参数C₀和σ的编码长度l₂和l₃。

3） 选择、 交叉和变异： 选择操作通常采用轮盘赌模型， 将种群所有个体随机排序， 每次转动轮盘相当于随机取排序后一定范围内的个体， 根据式（8）计算出当前种群中所有个体的适应度， 并选择适应度最高的个体进行保留， 设种群总个体数为N， 则经过N次轮盘赌后获得的新种群即为经过选择操作后的种群。

将保留下的个体再进行交叉操作， 交叉操作常用的方式有单点交叉、 多点交叉、 顺序交叉等。本文选择单点交叉， 即随机选择2个个体a和b， 并随机取编码l_a和l_b长度范围内的交换点， 将其编码中的二进制数进行交换形成2个新的编码序列。

将交叉操作得到的新序列再进行变异操作， 变异操作需设置一个较小数m（通常小于0.1）作为变异概率， 每次进行变异操作时生成一个0~1的随机数， 若随机数小于所设定的变异概率m时， 将当前个体编码中的某一位由1变为0或由0变为1， 从而获得新的编码序列。

4） 解码： 将经过选择、 交叉、 变异操作之后的编码序列按照3个参数的编码长度进行拆分得到各自的二进制编码， 再对二进制编码进行解码操作， 可得到优化后的具体参数值， 同样以参数C的解码过程为例， 解码计算如下

x=C_{\mathrm{m}\mathrm{i}\mathrm{n}}+\frac{C_{\mathrm{m}\mathrm{a}\mathrm{x}}-C_{\mathrm{m}\mathrm{i}\mathrm{n}}}{{\mathrm{2}}^{l_{\mathrm{1}}}-\mathrm{1}}\cdot {\displaystyle \sum _{i=\mathrm{1}}^{l_{\mathrm{1}}}}{A}_i\cdot {\mathrm{2}}^{i-\mathrm{1}}，（10）

式中： A_i 为二进制编码第i位上的二进制数。

重复执行上述过程直至迭代结束， 可得到最优的超参数集\mathit{\theta }=[C,C_{\mathrm{0}},\sigma ]。

3 多点耦合测量误差补偿

通过误差预测模型可以得到双目视觉系统视野任意位置处的单点测量误差， 为最终实现跟踪坐标系的补偿， 需要通过位姿测量原理将坐标系绑定的多个视觉标志点测量误差进行耦合， 具体计算过程如下。

设双目视觉系统坐标系为F_S， 跟踪坐标系为F_E， 若有n个视觉标志点与坐标系F_E 绑定， 可将标志点在坐标系F_E 下的坐标 a_i 构成矩阵\mathit{A}=[{\mathit{a}}_{\mathrm{1}}{\mathit{a}}_{\mathrm{2}}\cdots {\mathit{a}}_n]， 在坐标系F_S 下的坐标 b_i 构成矩阵\mathit{B}=[{\mathit{b}}_{\mathrm{1}}{\mathit{b}}_{\mathrm{2}}\cdots {\mathit{b}}_n]， 将采样得到的单点测量误差 e_i 构成测量误差矩阵\mathit{E}=[{\mathit{e}}_{\mathrm{1}}{\mathit{e}}_{\mathrm{2}}\cdots {\mathit{e}}_n]， 并利用采样数据对模型进行训练， 通过训练后的模型得到预测误差矩阵为{\mathit{E}}_p=[{\mathit{e}}_{p\mathrm{1}}{\mathit{e}}_{p\mathrm{2}}\cdots {\mathit{e}}_{pn}]， 故所有视觉标志点在坐标系F_S 下补偿后的坐标值为

\begin{array}{l}{\mathit{B}}^{\text{'}}=\mathit{B}+{\mathit{E}}_p=\left[{\mathit{b}}_{\mathrm{1}}^{\text{'}}{\mathit{b}}_{\mathrm{2}}^{\text{'}}\cdot \cdot \cdot {\mathit{b}}_n^{\text{'}}\right]=\\ \left[{\mathit{b}}_{\mathrm{1}}+{\mathit{e}}_{p\mathrm{1}}{\mathit{b}}_{\mathrm{2}}+{\mathit{e}}_{p\mathrm{2}}\cdot \cdot \cdot {\mathit{b}}_n+{\mathit{e}}_{pn}\right]\mathrm{。}\end{array}（11）

视觉标志点在两个坐标系下的线性变换关系为

\mathit{R}{\mathit{a}}_i+\mathit{t}={\mathit{b}}_i^{\text{'}}，（12）

式中： R 和 t 分别为F_E 变换到F_S 的旋转矩阵和平移向量。

在实际测量时， 由于存在测量噪声和误差， 通常不存在满足所有{\mathit{a}}_i到{\mathit{b}}_i^{\text{'}}变换关系的 R 和 t。因此， 可将线性问题转化为最优估计问题， 即最小化式（13）的坐标变换误差函数。

\mathrm{m}\mathrm{i}\mathrm{n}{\displaystyle \sum _{i=\mathrm{1}}^n}{‖\mathit{R}{\mathit{a}}_i+\mathit{t}-{\mathit{b}}_i^{\text{'}}‖}^{\mathrm{2}}\mathrm{。}（13）

对式（13）中平移向量 t 求偏导可得

{\displaystyle \sum _{i=\mathrm{1}}^n}{‖\mathit{R}{\mathit{a}}_i+\mathit{t}-{\mathit{b}}_i^{\text{'}}‖}^{\mathrm{2}}=\mathrm{2}\mathit{R}{\displaystyle \sum _{i=\mathrm{1}}^n}{a}_i+\mathrm{2}nt-\mathrm{2}{\displaystyle \sum _{i=\mathrm{1}}^n}{\mathit{b}}_i^{\text{'}}\mathrm{。}（14）

继续对式（14）进行简化， 得

\left\{\begin{array}{l}{\displaystyle \sum _{i=\mathrm{1}}^n}{‖\mathit{R}{\mathit{a}}_i+\mathit{t}-{\mathit{b}}_i^{\text{'}}‖}^{\mathrm{2}}=\mathrm{2}n(\mathit{R}\overline{\mathit{a}}+\mathit{t}-{\overline{\mathit{b}}}^{\text{'}})，\\ \overline{\mathit{a}}=\frac{\mathrm{1}}{n}{\displaystyle \sum _{i=\mathrm{1}}^n}{\mathit{a}}_i，\\ {\overline{\mathit{b}}}^{\text{'}}=\frac{\mathrm{1}}{n}{\displaystyle \sum _{i=\mathrm{1}}^n}{\mathit{b}}_i^{\text{'}}，\end{array}\right.（15）

式中： \overline{\mathit{a}}和{\overline{\mathit{b}}}^{\text{'}}分别为视觉标志点的几何中心在坐标系F_E 和F_S 下的坐标。

为求取最优平移向量 t， 令式（15）中求偏导后的项为零， 可得

\mathit{t}=-\mathit{R}\overline{\mathit{a}}+{\overline{\mathit{b}}}^{\text{'}}\mathrm{。}（16）

将式（16）代入式（13）得到

\left\{\begin{array}{l}\mathrm{m}\mathrm{i}\mathrm{n}{\displaystyle \sum _{i=\mathrm{1}}^n}{‖\mathit{R}{\mathit{a}}_{ci}-{\mathit{b}}_{ci}^{\text{'}}‖}^{\mathrm{2}}，\\ \begin{array}{c}{\mathit{a}}_{ci}={\mathit{a}}_i-\overline{\mathit{a}}，\\ {\mathit{b}}_{ci}^{\text{'}}={\mathit{b}}_i^{\text{'}}-{\overline{\mathit{b}}}^{\text{'}}，\end{array}\end{array}\right.（17）

式中： {\mathit{a}}_{ci}和{\mathit{b}}_{ci}^{\text{'}}分别为在坐标系F_E 和F_S 下， 从所有标志点的几何中心到各个标志点所构成的矢量。

将式（17）中的优化目标进一步展开

\begin{array}{l}{\displaystyle \sum _{i=\mathrm{1}}^n}{‖\mathit{R}{\mathit{a}}_{ci}-{\mathit{b}}_{ci}^{\text{'}}‖}^{\mathrm{2}}=\\ {\displaystyle \sum _{i=\mathrm{1}}^n}{\mathit{a}}_{\mathit{c}\mathit{i}}^{\mathbf{T}}{\mathit{a}}_{ci}-\mathrm{2}{{\mathit{b}}_{ci}^{\text{'}}}^{\mathrm{T}}\mathit{R}{\mathit{a}}_{ci}+{{\mathit{b}}_{ci}^{\text{'}}}^{\mathrm{T}}{\mathit{b}}_{ci}^{\text{'}}\mathrm{。}\end{array}（18）

忽略式（18）中的常量项， 可将最小值优化问题转化为式（19）所示的最大值问题

\mathrm{m}\mathrm{a}\mathrm{x}{\displaystyle \sum _{i=\mathrm{1}}^n}{{\mathit{b}}_{ci}^{\text{'}}}^{\mathrm{T}}\mathit{R}{\mathit{a}}_{ci}\mathrm{。}（19）

构建矩阵{\mathit{A}}_c=[{\mathit{a}}_{c\mathrm{1}}{\mathit{a}}_{c\mathrm{2}}\cdots {\mathit{a}}_{cn}]和{\mathit{B}}_c^{\text{'}}=[{\mathit{b}}_{c\mathrm{1}}^{\text{'}}{\mathit{b}}_{c\mathrm{2}}^{\text{'}}\cdots {\mathit{b}}_{cn}^{\text{'}}]， 则式（19）转化为

\mathrm{m}\mathrm{a}\mathrm{x}{\displaystyle \sum _{i=\mathrm{1}}^n}{{\mathit{b}}_{ci}^{\text{'}}}^{\mathrm{T}}\mathit{R}{\mathit{a}}_{ci}=\mathrm{m}\mathrm{a}\mathrm{x}\mathrm{t}\mathrm{r}({{\mathit{B}}_c^{\text{'}}}^{\mathrm{T}}\mathit{R}{\mathit{A}}_c)\mathrm{。}（20）

根据矩阵迹的性质可得

\mathrm{t}\mathrm{r}({{\mathit{B}}_c^{\text{'}}}^{\mathrm{T}}\mathit{R}{\mathit{A}}_c)=\mathrm{t}\mathrm{r}(\mathit{R}{\mathit{A}}_c{{\mathit{B}}_c^{\text{'}}}^{\mathrm{T}})\mathrm{。}（21）

记\mathit{P}={\mathit{A}}_c{{\mathit{B}}_c^{\text{'}}}^{\mathrm{T}}， 结合式（20）和式（21）， 并对其进行奇异值分解

\mathrm{t}\mathrm{r}(\mathit{R}\mathit{P})=\mathrm{t}\mathrm{r}(\mathit{R}\mathit{U}\mathbf{\Sigma }{\mathit{V}}^{\mathrm{T}})=\mathrm{t}\mathrm{r}(\mathbf{\Sigma }{\mathit{V}}^{\mathrm{T}}RU)\mathrm{。}（22）

记\mathit{H}={\mathit{V}}^{\mathrm{T}}\mathit{R}\mathit{U}， 由于奇异值分解得到的矩阵 V^T， U 都为正交矩阵， R 为正交的旋转矩阵， 因此 H 也为正交矩阵。已知正交矩阵中各元素最大值不超过1， 故

\mathrm{t}\mathrm{r}(\mathbf{\Sigma }\mathit{H})=\left[\begin{array}{ccc}{\sigma }_{\mathrm{1}}& \mathrm{0}& \mathrm{0}\\ \mathrm{0}& {\sigma }_{\mathrm{2}}& \mathrm{0}\\ \mathrm{0}& \mathrm{0}& {\sigma }_{\mathrm{3}}\end{array}\right]\left[\begin{array}{ccc}{h}_{\mathrm{11}}& h_{\mathrm{12}}& h_{\mathrm{13}}\\ h_{\mathrm{21}}& h_{\mathrm{22}}& h_{\mathrm{23}}\\ h_{\mathrm{31}}& h_{\mathrm{32}}& h_{\mathrm{33}}\end{array}\right]=

{\displaystyle \sum _{i=\mathrm{1}}^{\mathrm{3}}}{\sigma }_i{h}_{ii}\le {\displaystyle \sum _{i=\mathrm{1}}^{\mathrm{3}}}{\sigma }_i\mathrm{。}（23）

若要使式（23）得到最大值， 正交矩阵 H 对角线必须都为1， 从而计算得到最优旋转矩阵 R

\mathit{R}=\mathit{V}\mathit{H}{\mathit{U}}^{\mathrm{T}}=\mathit{V}{\mathit{U}}^{\mathrm{T}}\mathrm{。}（24）

将式（24）代入式（16）， 可求解得到最优平移向量 t， 最终F_E 相对于F_S 的变换关系{}_E{}^S\mathit{T}为

{}_E{}^S\mathit{T}=\left[\begin{array}{cc}\mathit{R}& \mathit{t}\\ \mathrm{0}& \mathrm{1}\end{array}\right]\mathrm{。}（25）

最终， 从 R 和 t 中可以分别得到跟踪坐标系F_E 相对于双目视觉系统坐标系F_S 补偿后的姿态和位置。

4 实验验证

为验证本文所提双目视觉系统测量误差补偿方法的有效性， 搭建了双目视觉系统测量误差补偿实验平台， 如图 4 所示。

机器人作为移动载体带动跟踪目标在空间中移动； 双目视觉系统（Creamform公司生产）作为实验的主要对象， 通过与其配套的测量软件可建立跟踪目标坐标系， 并实时获取坐标系的位姿信息； 激光跟踪仪（型号为徕卡AT960）作为辅助测量仪器， 其跟踪精度可达0.01 mm， 且具有很高的测量稳定性， 其获取的位置数据作为参考标准， 将其与双目视觉系统测量值之间的差值作为训练预测模型的输出。实验具体步骤如下：

1） 空间采样： 为保证采样均匀， 在双目视觉系统视野内规划出一个长500 mm， 宽500 mm， 高500 mm的空间立方体作为采样区域， 立方体正对双目视觉系统的一端中心位置位于双目视觉系统视野内的最佳测量区域， 如图 5 所示。

以最佳测量区域位置所处面为最终采样面， 其对侧面为起始采样面， 将起始采样面左上顶点作为首个采样点， 按照从左到右， 从上到下， 从起始采样面到最终采样面的规则， 每间隔50 mm激光跟踪仪和双目视觉系统对同一个点进行采样， 最终采样点数量为1 331， 通过实验得到的采样点云如图 6 所示。

最终按采样顺序得到的采样点在笛卡尔空间中的位置误差分布如图 7 所示。

从图 7 可看出单点测量误差在双目视觉系统采样空间内的整体分布规律， 其中X和Y方向误差分布几乎一致， 且在0附近呈现对称分布； 而在相同距离变化下， Z轴（深度）方向误差变化更为明显， 表明双目视觉系统对于深度方向的信息不敏感。

2） 模型训练： 将采样点在双目视觉系统坐标系下的位置作为输入， 以位置误差作为输出构成样本集， 并将获取到的总共1 331个样本数据通过随机数打乱， 取打乱后前1 131个样本作为训练集， 剩下200个样本作为测试集。初始化遗传算法迭代次数为100， 超参数C、 C₀、 σ初始范围均取0~1， 搜索精度S设为0.01， 模型经过若干次训练后趋于收敛， 为测试模型拟合效果， 取训练集前200个样本的位置坐标输入模型中， 得到3个方向的训练集位置误差， 如图 8 所示。

为测试模型的预测效果， 将200个测试集样本的位置坐标输入模型中， 得到3个方向的测试集位置误差， 如图 9 所示。

由图 8 和图 9 可知， 训练集和测试集的预测值都基本位于实际值附近， 且测试集的预测值相比之下更接近实际值， 表明模型对训练集拟合效果较好。此外， 在样本中存在个别偏差较大的异常点， 分析其原因可能为采样过程中的人为误操作或环境干扰所导致。最终预测值与实际值之间的偏差统计结果如表 1 所示。

从表 1 可看出， 样本中虽存在误差较大的异常点， 但训练集和测试集的预测误差平均值都在0.037 mm以内， 表明模型对单点测量误差有较好的预测效果。

3） 跟踪测试： 为测试实际跟踪效果， 在空间中随机取30个点， 并将双目视觉系统放置于互成角度的两个站位下分别对模型目标坐标系进行跟踪。同时以其它研究提出的基于空间相似度补偿测量误差方法^［11］作为对比， 采样范围、 数量与本文方法保持一致， 并进行误差补偿。此外， 以激光跟踪仪的数据为参考标准， 最终模型目标坐标系的位姿测量误差如图 10 和图 11 所示。

由图 10 可知， 双目视觉系统测量误差补偿后， X和Z方向补偿效果显著， Y方向的测量误差有略微改善。双目视觉系统补偿前X、 Y、 Z方向的位置测量误差平均值分别为0.041、 0.031和0.108 mm， 采用其它方法补偿后分别为0.031、 0.023和0.049 mm， 采用本文方法补偿后分别为0.015、 0.017和0.027 mm。由图 11 可知， 补偿后姿态误差也有所减小， 补偿前的平均姿态测量误差分别为0.032°、 0.031°和0.032°， 采用其它方法补偿后分别为0.023°、 0.020°和0.020°， 采用本文方法补偿后分别为0.016°、 0.016°和0.015°。

由上述双目视觉系统测量误差补偿前后的结果可知， 采用其它方法的测量误差补偿效果有限， 原因是映射模型不能精准地反应跟踪模型的误差， 且未考虑双目视觉系统不同观测角度对跟踪目标坐标系测量误差的影响。而本文所提出的方法从双目视觉系统测量视觉标志点误差补偿出发， 建立了基于MLSSVR的预测模型， 实现了视觉标志点测量误差的准确预测， 然后通过最优估计计算多点耦合误差， 实现了跟踪目标坐标系的误差预测， 且误差预测精度不受双目视觉系统站位的影响。因此， 本文所提双目视觉系统测量误差补偿方法具有更高的精度补偿效果与稳定性。

5 结　论

本文针对双目视觉设备存在测量误差且误差分布不一致的问题， 提出了一种双目视觉设备测量误差补偿方法， 建立了基于MLSSVR的预测模型， 并利用遗传算法优化模型超参数集， 实现了双目视觉设备的单点测量误差预测。然后结合位姿测量原理， 将预测的多个单点误差通过最优估计计算得到多点耦合误差， 实现了跟踪目标坐标系的测量误差补偿。最后搭建实验平台， 通过空间采样、 模型训练、 跟踪测试与对比实验， 对本文所提方法进行了验证。实验结果表明， 双目视觉设备经过补偿后的跟踪目标坐标系平均位置测量误差在3个方向上分别降低63.4%， 45.2%和75.0%， 平均姿态测量误差分别降低50%， 48.4%和53.1%； 与其它方法相比， 采用本文方法双目视觉系统测量误差补偿精度更高， 且在不同观测角度下的误差补偿效果一致， 证明了本文所提双目视觉设备测量误差补偿方法的有效性与稳定性。

参考文献

[1] 周科杰. 双目视觉三维测量技术研究与应用［D］. 成都： 中国科学院大学（中国科学院光电技术研究所）， 2019.

[2] 郑联语， 付强， 樊伟， 等. 基于双目视觉和先验加工数据的大型筒件原位位姿感知方法［J］.机械工程学报， 2023， 59（11）： 129-146.

[3] ZHENG Lianyu， FU Qiang， FAN Wei， et al. A method for in-situ pose perception of large cylindrical components based on binocular vision and prior machining data［J］. Journal of Mechanical Engineering， 2023， 59（11）： 129-146. （in Chinese）

[4] 周科杰， 冯常. 基于双目视觉的三维测量技术研究［J］. 计算机测量与控制， 2019， 27（1）： 22-25.

[5] ZHOU Kejie， FENG Chang. Three-dimensional measurement technology based on binocular vision［J］. Computer Measurement & Control， 2019， 27（1）： 22-25. （in Chinese）

[6] LI B， LI Y， TIAN W， et al. Pose accuracy improvement in robotic machining by visually-guided method and experimental investigation［J］. Robotics and Autonomous Systems， 2023， 164： 104416.

[7] SHU T， GHARAATY S， XIE W， et al. Dynamic path tracking of industrial robots with high accuracy using photogrammetry sensor［J］. IEEE/ASME Transactions on Mechatronics， 2018， 23（3）： 1159-1170.

[8] 傅宇文. 基于视觉引导的机器人高精度定位与轨迹跟踪控制［D］. 南京： 南京航空航天大学， 2020.

[9] 王龙华， 曾国华， 胡华锋， 等. 精确三维数据测量的双目视觉系统优化设计［J］. 电子测量技术， 2022， 45（7）： 99-109.

[10] WANG Longhua， ZENG Guohua， HU Huafeng， et al. Optimal design of binocular vision system for accurate 3D data measurement［J］. Electronic Measurement Technology， 2022， 45（7）： 99-109. （in Chinese）

[11] 王震， 许恒硕， 廉哲. 映射关系约束下双目视觉测量误差调整仿真［J］. 计算机仿真， 2022， 39（4）： 189-192.

[12] WANG Zhen， XU Hengshuo， LIAN Zhe. Simulation of binocular vision measurement error adjustment under mapping constraints［J］. Computer Simulation， 2022， 39（4）： 189-192. （in Chinese）

[13] 孟环标. 双目视觉测量系统误差分析与控制［D］. 济南： 山东大学， 2012.

[14] JIN D， YANG Y. Sensitivity analysis of the error factors in the binocular vision measurement system［J］. Optical Engineering， 2018， 57： 104109.

[15] 崔恩坤， 滕艳青， 刘佳伟. 立体视觉测量系统标定误差补偿［J］. 应用光学， 2020， 41（6）： 1174-1180.

[16] CUI Enkun， TENG Yanqing， LIU Jiawei. Calibration error compensation technique of stereoscopic vision measurement system［J］. Journal of Applied Optics， 2020， 41（6）： 1174-1180. （in Chinese）

[17] HUO J， ZHANG G， CUI J， et al. Corrected calibration algorithm with a fixed constraint relationship and an error compensation technique for a binocular vision measurement system［J］. Applied Optics， 2018， 57（19）： 5492-5504.

[18] HONG Z， LI Z， TONG X， et al. A high-precision recognition method of circular marks based on CMNet within complex scenes［J］. IEEE Journal of Selected Topics in Applied Earth Observations and Remote Sensing， 2022， 15： 7431-7443.

[19] 卫松. 面向复材铣边加工的机器人视觉伺服控制系统［D］. 南京： 南京航空航天大学， 2022.

[20] SUYKENS J A K， VANDEWALLE J. Least squares support vector machine classifiers［J］. Neural Processing Letters， 1999， 9（3）： 293-300.

[21] 戴鹏， 周平， 梁延灼， 等. 基于多输出最小二乘支持向量回归建模的自适应非线性预测控制及应用［J］. 控制理论与应用， 2019， 36（1）： 43-52.

[22] DAI Peng， ZHOU Ping， LIANG Yanzhuo， et al. Multi-output least squares support vector regression modeling based adaptive nonlinear predictive control and its application［J］. Control Theory & Applications， 2019， 36（1）： 43-52. （in Chinese）

[23] ZHOU S. Sparse LSSVM in primal using cholesky factorization for large-scale problems［J］. IEEE Transactions on Neural Networks and Learning Systems， 2016， 27（4）： 783-795.

基金资助

国家自然科学基金资助项目(52205530)

国防基础科研计划资助项目(JCKY2021204B045)

民用航天预研资助项目(D020201)

上海市技术带头人计划资助项目(21XD1431200)