一种单目VO快速位姿估计方法与流程

本发明涉及一种单目VO快速位姿估计方法，属于视觉导航技术领域。

背景技术：

无人驾驶车辆一般是通过在车上安装多种感应设备，主要包括各种车载传感器、GPS、雷达以及摄像机等，来感知周围的环境，并根据所获取的信息，自动规划路径实现车辆的自主驾驶，安全可靠地到达目的地。实时准确的定位方法是实现无人驾驶技术的基础，是保障无人驾驶车辆完成自主行为的前提。随着视觉技术不断发展，视觉里程计技术已被广泛应用，比如移动机器人、无人机、卫星或水下探测器以及工厂AGV(Automated Guided Vehicle)，同时也越来越多的被应用于无人驾驶车辆的自主定位和运动估计。

技术实现要素：

为解决现有技术的不足，本发明的目的在于提供一种单目VO快速位姿估计方法，在保证定位精度的前提下，可以有效的提高位姿估计算法的运行速度。

为了实现上述目标，本发明采用如下的技术方案：

一种单目VO快速位姿估计方法，其特征是，包括如下步骤：

1)设置基础矩阵

2)利用相邻两帧得到的特征匹配集合中任意一对特征点的图像坐标的齐次坐标与上式的矩阵F，得到

3)从相邻两帧特征匹配集合中任意选取7对特征点，叠加7对特征点得到如下方程组：其中，矩阵Q7×8中左上标(i),i＝1,2，…，7表示第i对匹配特征点，

4)求解步骤3)得到的非齐次线性方程组的解η^*，且有ξ为非齐次方程的基础解系中的解向量；

5)将步骤4)中得到的解写成步骤1)中所示的矩阵形式有由基础矩阵是奇异矩阵的性质可知，矩阵F 的秩为2，即其行列式det(F)＝0，对其求解可以计算得到x的值以及基础矩阵F，

6)采集摄像机的内参数，由步骤1)中所示的矩阵计算本质矩阵 E＝Min^T·F·Min；

7)利用奇异值分解从本质矩阵E中得到旋转矩阵R和平移向量t。

前述的一种单目VO快速位姿估计方法，其特征是，所述步骤2)中将等式展开后重写为如下的线性非齐次方程为：其中，

前述的一种单目VO快速位姿估计方法，其特征是，所述步骤7)中利用本质矩阵E的两个非零奇异值相等的性质进行奇异值分解，并对得到的若干组旋转矩阵R和平移向量t进行判定是否是真实。

前述的一种单目VO快速位姿估计方法，其特征是，所述本质矩阵E的奇异值分解为E＝Udiag(1,1,0)V^T，得到四组不同的旋转矩阵R和平移向量t：其中，U3表示矩阵U的第三行。

前述的一种单目VO快速位姿估计方法，其特征是，所述步骤7)中判定准则是：判断该交点是否在摄像机的前方，若交点在两摄像机的前方，则得到的旋转矩阵R和平移向量t即为正确的。

本发明所达到的有益效果：本方法通过对基础矩阵的优化，在保证位姿估计精度的前提下，有效的提高算法的运行速度。

附图说明

图1是本发明的位姿估计示意图；

图2(a)(b)(c)(d)是步骤7)中本质矩阵中得到的位姿矩阵的四组解的示意图；

图3(a)是本方法与现有算法的旋转误差均值对比图；

图3(b)是本方法与现有算法的平移误差均值对比图；

图3(c)是本方法与现有算法的旋转误差中值对比图；

图3(d)是本方法与现有算法的平移误差中值对比图。

具体实施方式

下面结合附图对本发明作进一步描述。以下实施例仅用于更加清楚地说明本发明的技术方案，而不能以此来限制本发明的保护范围。

本发明通过优化视觉里程计位姿估计模块基础矩阵的求解过程，在保证定位精度的前提下，可以有效的提高位姿估计算法的运行速度。

如图1所示，具体的步骤如下：

1)设置基础矩阵由于基础矩阵F在相差任意一个非零常数因子的条件下是唯一的，故可以令矩阵H中的元素f9＝1，则变形有

2)利用相邻两帧得到的特征匹配集合中任意一对特征点的图像坐标的齐次坐标与上式的矩阵F，得到将等式展开后重写为如下的线性非齐次方程为：其中，

其中，特征集合是两幅图像提取特征后进行特征关联得到的，有跟踪和匹配两种方式，特征匹配集合采用的就是匹配方式。特征匹配是在每帧图像中进行特征点提取，然后利用合适的相似度度量在两帧图像的所有特征点中进行比较，找到最佳匹配特征点对。

3)约束方程式中有8个未知参数(f1～f8)，从相邻两帧特征匹配集合中任意选取7对特征点，叠加7对特征点得到如下方程组：其中，矩阵Q7×8中左上标(i),i＝1,2，…，7表示第i对匹配特征点。

4)对于有7对匹配特征点形成的系数矩阵Q7×8的秩的7，所以其对应的非齐次方程的基础解系中只包含一个解向量，设为ξ，求解步骤3)得到的非齐次线性方程组的解η^*，且有

5)将步骤4)中得到的解写成步骤1)中所示的矩阵形式有由基础矩阵是奇异矩阵的性质可知，矩阵F 的秩为2，即其行列式det(F)＝0，对其求解可以计算得到x的值以及基础矩阵F；

6)采集摄像机的内参数，由步骤1)中所示的矩阵计算本质矩阵 E＝Min^T·F·Min；

7)利用奇异值分解从本质矩阵E中得到旋转矩阵R和平移向量t。但是，从矩阵E中得到的旋转矩阵R和平移向量t都存在投影歧义，故将从一个本质矩阵中得到四组不同的旋转矩阵R和平移向量t。如图2所示。

利用本质矩阵E的两个非零奇异值相等的性质进行奇异值分解，分解为E＝U diag(1,1,0)V^T，得到四组不同的旋转矩阵R和平移向量t：其中，U3表示矩阵U的第三行，并对得到的若干组旋转矩阵R和平移向量t进行判定是否是真实。摄像机的内参数已经提前标定得知，利用摄像机内参数和得到的旋转矩阵R和平移向量t，可以得到成像点在三维空间中的射线。通过特征匹配集合中任意一对特征点可以求得两条射线交点的三维坐标，然后判断该交点是否在摄像机的前方。若交点在两摄像机的前方，则得到的旋转矩阵R和平移向量t即为正确的。从图2中可以看出，只有图2 (a)所示的两摄像机的位置才是视觉里程计系统所求的真实结果。

下面通过将现有的集中算法与本发明的方法进行比较，以突显本发明的进步性：

Nistér的5点算法、基于车辆运动学模型的位姿估计单点算法、MYP算法以及本文中提出算法分别的迭代次数如表1所示。

表1各种算法的迭代次数

由于RANSAC算法是一种概率算法，具有不确定性，它只是有一定的概率可以得到正确的模型假设。为了提高这个概率就需要增加迭代次数。

所以，处于算法鲁棒性的考虑，需要将其最小迭代次数放大一定的倍数，如表1所示。

表2四种单目视觉位姿估计算法运行时间比较

通过表1和表2的内容，能够看出本方法通过对基础矩阵的优化，在保证位姿估计精度的前提下，有效的提高算法的运行速度，与现有的算法相比，其运行速度的提升是巨大的，而且本方法在同样的条件下，迭代次数更高，精度更精确。

以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明技术原理的前提下，还可以做出若干改进和变形，这些改进和变形也应视为本发明的保护范围。