双目视觉测量系统位姿解算方法研究

李勤文，王志乾，杨文昌，沈铖武

(1.中国科学院长春光学精密机械与物理研究所，吉林 长春 130033；2. 中国科学院大学，北京 10049)

1 引言

工业发展自动化、数字化水平的提高促进了自动对准技术的发展[1，2]，姿态测量技术作为自动对准系统的关键技术，主要用于定位被对准部件或直接测量和定位各种工艺设备，是自动调姿对准系统的重要组成部分[3]。基于点特征的单目视觉测量方法是三维姿态测量的基本方法，它通过n个满足透视投影关系的三维特征点和它们的二维投影图像点来计算位姿，又称为“透视n点问题”(PnP问题)[4，5]。目前针对单目视觉的位姿解算方法已比较成熟，但在实际应用中直接采用单目相机往往会由于视场及拍摄角度的限制而导致获取的信息量少。近年来随着相机价格的降低和计算机性能的提高，利用多个相机来扩大视场范围和提高测量精度的方法得到了广泛的研究应用，如大型体育赛事，自动生产装配和国防军事领域等[6]。在利用多个相机拍摄的特征点信息对物体在空间中的位姿进行解算的过程中，由于目标的三维特征点和其对应的图像点之间不再满足透视投影规则，传统的PnP求解方法不再适用，因此要考虑多个相机拍摄信息的转换问题[7，8]，将不同相机拍摄的信息转换到统一的坐标系下进行表示，然后利用位姿解算算法的思想进行求解。

本文针对大部件双目自动对准系统的位姿解算问题，利用单目视觉OI迭代算法[9]思想最小化四个共面特征点的目标空间共线性误差求解旋转矩阵和平移矩阵，针对算法在特征点数目较少时迭代误差函数陷入局部极小值的问题，利用各坐标系之间的转换关系构造相机成像方程，求解迭代初始值进行迭代，并通过仿真验证了本文方法的有效性。

2 自动对准测量系统概述

本文主要针对如图1所示的自动对准系统布置双目相机进行位姿测量，利用位姿解算方法计算固定部件在空间中的位姿，测量对准偏差，调整移动部件直到完成对准。在固定部件和移动部件上分别布设两个合作靶标和两个光学测量相机AA、CB，两个靶标共面且平行于固定部件对接面，每个靶标上设置两个合作特征点，为保证在最终对接阶段两相机依然能观测到每个靶标上的全部特征点，两个靶标安装在固定部件对接面后方100mm处。两个光学相机的位置关系及内参数已标定。开始时人工粗对准至光学可测范围内，然后切换至自动控制。因为要讨论近距离测量对准的情况，所以设定固定部件和移动部件之间仅有小范围的对准偏差，即两部件之间的旋转角α′，β′，γ′∈[-5°，5°]，平移距离ΔX′，ΔY′∈[-200mm，200mm]，ΔZ′∈[0，200mm]。

由图1，移动部件与固定部件之间的位姿偏差可以通过对准坐标系O′-X′Y′Z′到目标坐标系O-XYZ之间的转换关系进行表示，因此测量的主要目的便是求解两坐标系之间的转换矩阵。通过将两相机拍到的点转换到基准相机CA坐标系进行位姿解算，可以求得CA坐标系与目标坐标系O-XYZ之间的转换关系，再利用已知的相机CA坐标系到对准坐标系O′-X′Y′Z′之间的转换关系可得到两部件之间的位姿偏差。其中解算基准相机CA坐标系与目标坐标系O-XYZ之间的转换关系是求解的关键，所以本文主要研究如何求得相机CA坐标系到目标坐标系O-XYZ之间的转换关系，即两坐标系之间的旋转角α，β，γ和平移距离ΔX，ΔY，ΔZ。最后，利用相机CA坐标系到对准坐标系O′-X′Y′Z′之间的转换关系便可得到两部件之间的旋转角偏差α′，β′，γ′和平移偏差ΔX′，ΔY′，ΔZ′。

图1 自动对准双目位姿测量系统

3 双目位姿解算的迭代算法

为了便于分析，将双目视觉测量系统简化为图2形式，选取相机CA为基准相机，P1i=(X1i，Y1i，Z1i)T，P2i=(X2i，Y2i，Z2i)T(i=1，2)分别表示相机CA、CB视场范围内的点在目标坐标系下的坐标，ATO表示要求的从目标坐标系O-XYZ到基准相机CA坐标系的转换矩阵，它是由两坐标系之间的旋转矩阵R和平移矩阵t构成的四阶方阵。ATB表示已标定的从相机CB到相机CA坐标系的转换矩阵，令p1i=(u1i，v1i，1)T，p2i=(u2i，v2i，1)T表示点的齐次图像坐标，将两个相机上点的图像坐标转换到它们的归一化像平面上，得

图2 自动对准双目位姿测量系统示意图

(1)

由于本文采用的是双目相机，点的图像坐标与目标坐标系下的坐标不再满足单相机透视投影规则，因此不能直接采用单目视觉位姿解算的OI迭代算法，需要将相机坐标系下的点通过向量坐标变换转换到基准相机坐标系下，如图3所示，然后再套用OI算法进行解算。

图3 相机CB到基准相机CA坐标变换示意图

已知目标坐标系下的点P2i，将其转换到相机CA坐标系下表示，得

Q2i=RP2i+t

(2)

=RP2i+t-AtB

(3)

(4)

(5)

RP2i+t-AtB=V2i(RP2i+t-AtB)

(6)

故对相机CB来说，目标空间共线性误差为：

eB=(I-V2i)(RP2i+t-AtB)

(7)

由于选择相机CA为基准相机，相机CA视场范围内的点满足透视投影规则，可直接套用OI算法，为了与相机CB的相应计算公式保持一致，将相机CA视场范围内点的视线投影矩阵及目标空间共线性误差重写为：

(8)

eA=(I-V1i)(RP1i+t-AtA)

(9)

因此，算法的目标函数为：

(10)

对于给定的R，平移矩阵t关于R的函数可以表示为

(11)

将式(11)代入式(10)并化简，得

(12)

这是一个一元函数的最小化问题，给定旋转矩阵R的初值R0，假设R的第k次估计值为Rk，则R的下一次估计值Rk+1为

(13)

(14)

(15)

Mk的SVD分解为(Uk，Σk，Vk)，为了保证求得的旋转矩阵R不会出现det(Rk+1)=-1的情况，需要对传统的SVD求解方法进行校正，校正后的解为[10]

Rk+1=Vkdiag(1，1，det(Vk(Uk)T))(Uk)T

(16)

得到Rk+1后，平移向量t的下一次估计值tk+1可以由式(11)求得，然后继续进行迭代直至误差E收敛，可以得到目标坐标系O-XYZ和相机CA坐标系之间的旋转矩阵R和平移矩阵t，进而利用已知的相机CA坐标系和对准坐标系O′-X′Y′Z′之间的转换关系求得固定部件与移动部件之间的对准偏差。

一般情况下，非线性优化算法都不能保证误差函数E一定收敛，取决于初值的选取。优化算法有可能最终收敛到局部极小值，或者在有限的时间内根本不收敛。本文介绍的OI迭代算法虽然在单目视觉解算时可保证全局收敛性，但应用于双目视觉解算时，在特征点数目较少时会出现迭代误差E收敛到局部极小值的情况。为了解决这个问题，要考虑迭代初始值的选取问题，通过数值解算方法求出初始旋转矩阵作为迭代初值进行迭代，保证解的准确性。

4 迭代初始值的求解

令P=(X，Y，Z，1)T表示目标坐标系下点的齐次坐标，Q=(x，y，z，1)T表示点在各自相机坐标系下的齐次坐标，p=(u，v，1)T表示点的齐次图像坐标。各坐标之间的联系可以表示为一系列的几何变换，如图4所示，其中KA，KB为已知的两相机内参数矩阵，BTA表示从相机CA到相机CB坐标系的转换矩阵，且BTA=(ATB)-1。

图4 坐标变换过程

基准相机CA视场范围内的点P11，P12在目标坐标系下的坐标与它们的图像坐标满足单相机透视成像规则，即

(17)

其中z1i为标量系数，i=1，2。为了建立两相机成像之间的联系，将相机CB视场范围内的点P21，P22先转换到基准相机CA坐标系下，再利用已标定的两相机之间的坐标转换关系在CB坐标系下进行成像，最后利用已知的相机CB的内参数矩阵KB建立点在相机坐标系下的坐标与图像坐标之间的转换，代数表达式为

(18)

要求的变换矩阵

(19)

共有六个未知数(α，β，γ，ΔX，ΔY，ΔZ)，式(17)，式(18)可分别提供三个等式方程，方程中含有未知的标量系数zji(j=1，2)，从Pji中任选三个点(如P11，P12，P21)列写成像方程，得

(20)

其中BRA，BtA表示已标定的相机CA坐标系到相机CB坐标系的旋转矩阵和平移矩阵。将式(20)展开可得由九个未知数(α，β，γ，ΔX，ΔY，ΔZ，z11，z12，z21)表示的九个方程，利用MATLAB求解方程，并利用测量范围约束去掉不符合条件的值，可以求得旋转矩阵R和平移矩阵t。由于三点位姿解算的解不唯一，可能出现1，2，3或4个解[11]，若在测量范围内出现多个解，选取令迭代误差函数E最小的R，t作为迭代初值进行迭代，求得相机CA坐标系和目标坐标系之间的转换关系ATO，进而利用已知的相机CA坐标系和对准坐标系之间的转换关系O′TA求得固定部件与移动部件之间的对准偏差O′TO，O′TO可转换为两部件之间的旋转角α′，β′，γ′和平移距离ΔX′，ΔY′，ΔZ′。

5 仿真研究

为了验证算法的正确性，建立MATLAB仿真对自动对准系统的双目姿态估计过程进行了仿真。仿真相机采用Nikon D700相机的相关参数，相机焦距为35mm， 分辨率为4256×2832，传感器尺寸为36×23.9mm。两相机之间的位置关系已知，选取相机CA为基准相机，目标坐标系和基准相机坐标系之间的实际旋转角度和平移距离在各自的测量范围内随机产生。

为了验证算法的正确性，本文对比分析了理想情况下直接使用OI迭代算法思想对四个共面特征点进行位姿求解的结果和使用本文初始化后的算法进行位姿解算结果，如图5、图6所示。其中图5为固定平移距离取值的情况下，在旋转角α′，β′，γ′的取值范围[-5。，5。]内以1。为步长遍历所有点得到的平移距离解算误差；图6为固定旋转角取值的情况下，在平移距离ΔX′，ΔY′，ΔZ′的取值范围内以50mm为步长遍历所有点得到的平移距离解算误差。可以看出，直接使用迭代算法在某些点处会由于迭代误差函数陷入局部极小值而使得平移距离解算误差达到103mm，使用本文初始化后的算法求得的平移距离解算误差小于10-9mm，证明了本文方法的有效性。

图5 角度变化时的平移距离解算误差

图6 位移变化时的平移距离解算误差

6 结论

通过上述仿真证明，本文针对自动对准系统所采用的双目视觉位姿测量解算方法是有效的。该方法利用单目视觉位姿解算的OI迭代算法思想进行位姿解算，针对算法在特征点数目较少时迭代误差陷入局部极小值的问题，利用各坐标系之间的转换关系求解迭代初值，保证解的正确性。总之，本文针对大部件自动对准系统近距离观测所采用的双目视觉位姿测量解算方法可以有效解决大部件的空间定位问题，具有很强的应用前景和实用价值。