基于OPA的时空约束单目视觉位姿估计算法

齐乃新，张胜修，曹立佳，杨小冈，陈世伟

（第二炮兵工程大学，a．304室；b．303室，西安710025）

摘要：针对单目视觉系统中载体纯旋转运动引起的估计退化问题，结合运动参数约束条件，提出了一种基于OPA的时空约束单目视觉位姿估计方法。该方法利用OPA算法在估计本质矩阵和平移方向之前判断载体的运动形式，检测修正估计退化问题，并结合摄像机在运动过程中的时间相关约束及图像特征点在空间中的相关约束，实现单目视觉系统的位姿估计。两组对比实验结果分析表明，新算法相比于传统的估计算法具有更高的精度和稳定性。

关键词：视觉导航；单目视觉；OPA；位姿估计；时空约束

O引言

    视觉导航是根据摄像机采集的图像序列恢复摄像机的运动信息，实现定位定姿的过程。位姿估计是指采用特定的算法求解不同参考系之间的相对位姿关系（包括3个旋转角和1个三维平移向量），是视觉导航过程中的重要环节。就目前位姿估计方法的研究发展而言，单目视觉算法较为成熟，然而单目视觉位姿估计的过程是求解非线性问题，实现起来较为复杂。因此，寻求稳定高效的位姿估计方法仍然是国内外研究的热点问题。

    早期的位姿估计算法通过找到3个或4个不共线的对应点，采用最小二乘或直接矩阵求逆运算，得到视觉系统唯一的位姿参数解。这种算法是根据世界坐标中的点和图像上的特征点的对应关系得到视觉系统的运动参数，但位姿估计结果精度不高。随后，位姿估计的方法产生了非线性迭代算法和基于摄像机先验运动模型的滤波算法两个方向。非线性迭代算法的典型代表是文献[4]中基于Longuet-Higgins准则的非线性优化方法，具有较高的精度，但针对非凸问题易陷入局部最优的困境。基于摄像机先验运动模型的滤波方法由于有先验运动模型不准的缺陷，精度不高，且可能带来维数灾难问题。

    针对各类位姿估计方法的不足，国内外学者做了大量的研究和改进，取得了较多的研究成果。例如，文献[5]针对深度相机，提出了估计位姿变换精度的闭式解算法；文献[6]提出了一种已标定和未标定摄像机同时适用的高精度快速算法，用于解决对应点个数大于等于6的摄像机位姿估计问题；文献[7]针对单目视觉系统的自运动估计问题，提出了滚动时域位姿估计算法。

    上述改进算法在提高位姿估计的精度方面都取得了一定的成效，但是上述算法在估计载体的位姿参数时都没有考虑到运动过程中载体的纯旋转运动，忽视了图像序列间本质矩阵以及不同时刻摄像机平移方向的估计退化问题，影响了算法的计算效率和计算精度。针对这个问题，结合摄像机在运动过程中的时间相关约束及图像特征点在空间中的相关约束，提出了基于OPA的时空约束位姿估计算法，解决载体运动过程中的纯旋转运动估计退化问题。

1  摄像机模型及其运动估计算法

1.1针孔成像模型

计算机视觉研究中，三维空问中的物体到像平面的投影关系即为成像模型，理想的投影成像模型是光学中的中心投影，也称为针孔模型，如图1所示。

由光的直线传播条件可知，物体表面的反射光尸经过一个针孔0投影到像平面上，对应地形成一个虚像平面，P1为光线与虚像平面的交点。针孔O即为光心，过投影中心O且垂直于摄像机平面的射线称为光轴或主轴，主轴与虚像平面的交点称为摄像机的主点，光心与主点之间的距离定义为摄像机的焦距fo由此可得到摄像机模型的描述为

1.2摄像机运动估计算法

现阶段，视觉导航大都是采用基于图像序列二维特征点匹配的位姿估计算法，总结起来有基于先验运动模型的滤波算法，基于两幅图像之间极线方程的本质矩阵特征值分解法、最小二乘算法以及非线性迭代算法。两幅图像的极线几何关系如图2所示，对应的极线方程可以写为

式中：E=txR是两幅图像对应的本质矩阵；分别是第一幅图像和第二幅图像上同一个三维特征点在空间中的归一化三维坐标。

特征值分解方法和最小二乘法计算过程简单、实时性好，但是精度不高。非线性迭代方法是以式(2)为基础，通过构造式(3)所示的I．onguet-Higgins准则直接估计出摄像机的未知运动参数，即

式中，匹配特征点个数用n表示。线性迭代方法能够很大程度上提高位姿估计的精度，但是容易陷入局部最优。基于摄像机运动模型的滤波估计方法能够解决最优解陷入局部最优问题，但是该方法位姿估计的精度不高，可能带来维数灾难的问题。

2  基于OPA的时空约束位姿估计算法

2.1  估计退化检测修正

    载体的运动过程中会出现单纯平移和单纯旋转的情况，需要单独分析计算。在纯平移运动的情况下，第二幅图像中的点与第一幅图像中的点视场方向匹配，得到的3D点都移向（或远离）汇集点。此时所产生的本质矩阵E在理想情况下是反对称的，可以直接估计出。在纯旋转运动的情况下，第二幅图像和第一幅图像之间不存在平移，导致本质矩阵E和平移方向f估计退化，最终导致位姿估计结果不准确，影响导航精度。同时，退化问题会使得目标函数在迭代过程中可能并不改进，一旦前面出现的基在迭代过程中又重新出现，则后面的迭代过程可能会在几个基上面兜圈子，出现“基的循环”现象，以致最终不能得到最优解。因此，在计算整个本质矩阵之前，检测载体的纯旋转运动，解决估计退化问题是十分必要的。

    解决两幅图像之间的本质矩阵及平移方向估计退化问题的一个关键环节就是载体的纯旋转运动检测，当检测出载体的纯旋转运动后就可以通过纯旋转运动的本质矩阵求解方法来估计两幅图像之问的本质矩阵，从而避免出现估计退化的问题。下文结合载体纯旋转运动的检测给出了估计退化检测修正的步骤。

    1)旋转矩阵估计。

旋转矩阵估计的一个常用方法是正交Procrustes算法( OPA)，它计算3x3相关矩阵的SVD分解

由式(7)可以得到

先用向量R．历，与式(8)叉乘，再用平移向量f和式(8)点乘，得到

式中，向量t，在理想的情况下是共面的，式(2)和式(9)是代数等价的。

    2)时间相关约束。

    不同时刻图像之间存在一定的时间相关约束，此约束即可作为约束条件用于摄像机运动参数的估计。

如图3所示，载体的运动过程是连续的，摄像机在不同时刻的位姿参数具有密切的联系关系而非独立的，其思想类似于环闭检测，称为虚拟闭环，不需要摄像机进行真实环闭运动，累积误差通过不相邻时刻场景特征的相似性进行消减。

由以上约束条件可以得到摄像机在整个运动过程中，用于位姿参数最优估计的非线性优化函数为

    在载体的整个运动过程中，摄像机位姿的最优估计结果满足式(10)和式(13)建立的约束条件。将此约束条件与传统的极线约束条件、图像特征点空间相关约束条件综合用于非线性迭代的优化目标函数，能够精确地估计载体的运动参数。

3  实验及结果分析

    实验设计了原地旋转实验和行驶导航实验，其中原地旋转实验专门用于评估载体姿态测量的精度，行驶导航实验通过与其他几种位姿估计算法相比，综合评估载体在实际运动中的姿态测量精度。

本实验中所用的实验平台是根据需要自主改装的由DrRobot公司生产的X80-H型WiFi控制智能小车，如图4所示。该智能实验平台包含视觉传感器和通信控制模块，能够通过编程预定行驶轨迹模拟平台的运动真值，并且能够通过WiFi实时传输图像序列，用于估计平台的位姿参数。实验选在实验室环境，摄像机的视场中有各种实验器材、办公用品等静态的景物特征，并且还含有人员走动等动态元素。在此复杂的环境下，本文进行了原地旋转和L形行走转弯两组实验。

    实验1  原地旋转实验。

在原地旋转实验中，单目视觉系统连续转动90。，摄像机以20帧／s的频率采集图像序列，分别用本质矩阵特征值分解法、非线性迭代算法以及基于OPA的时空约束导航算法估计平台的位姿参数，进行对比评估。实验对应的位姿计算结果如图5所示。

    从图5可以看出，在原地旋转90。的实验条件下，3种导航位姿估计算法都能估计出载体的姿态角信息，但是相比于传统的本质矩阵特征值分解算法和非线性迭代算法，本文提出的基于OPA的时空约束导航算法具有明显的优势，姿态测量精度更高。

    实验2  L形行走转弯实验。

L形行走转弯实验的实验过程由东向西行驶，90。转弯，向南行驶3部分组成。实验中分别采用本质矩阵特征值分解法、非线性迭代算法以及基于OPA的时空约束导航算法进行实验分析，实验结果如图6所示。

    从图6可以看出，在未知环境导航的实验条件下，相比于传统的视觉导航位姿估计算法，基于OPA的时空约束导航算法具有更高的精度，偏航姿态角误差能够控制在5。以内，俯仰姿态角误差能够控制在3。以内，滚动姿态角误差能够控制在4。以内。

    通过两组对比实验，可以得出基于OPA的时空约束导航算法在纯旋转运动时能够解决估计退化的问题，较大程度上提高了姿态估计的精度，能够较好地应用于未知环境下载体的姿态估计。

4结论

    针对单目视觉导航算法中的位姿估计问题，介绍了摄像机的成像模型，在描述单目视觉导航基本原理和具体过程的前提下，采用OPA算法解决了估计退化问题，通过建立摄像机运动的时间域和空间域的约束条件，提出了基于OPA的时空约束单目视觉位姿估计算法。最后通过两组实验，验证了提出的视觉导航算法相比于传统的算法具有更高的精度和稳定性。