无人驾驶车辆单目视觉里程计快速位姿估计

曾庆喜，冯玉朋,马 杉

(1.南京航空航天大学无人驾驶车辆研究中心，江苏南京 210016；2.奇瑞汽车前瞻技术研究院，安徽芜湖 241000；3.汽车仿真与控制国家重点实验室，吉林长春 130012)

1008-1542(2017)05-0438-07

10.7535/hbkd.2017yx05005

无人驾驶车辆单目视觉里程计快速位姿估计

曾庆喜1,2,3，冯玉朋1,3,马 杉1,3

(1.南京航空航天大学无人驾驶车辆研究中心，江苏南京 210016；2.奇瑞汽车前瞻技术研究院，安徽芜湖 241000；3.汽车仿真与控制国家重点实验室，吉林长春 130012)

针对传统的单目视觉里程计位姿估计算法无法满足无人驾驶车辆实时定位需求的问题，提出了一种基于基础矩阵性质改进的快速位姿估计算法。通过优化基础矩阵的计算过程来提高算法的实时性，首先得到含有8个未知参数的基础矩阵，然后利用特征匹配点对来求解基础矩阵。通过仿真实验对算法的效率和精度进行了分析，并将其与现有的算法进行了对比。实验结果表明，在不降低运动估计结果精度的前提下，优化后的位姿估计算法的运行速度可以在传统算法的基础上提高近4倍。研究对视觉里程计在无人驾驶车辆上的实时应用具有一定的参考价值。

车辆工程；单目视觉；位姿估计；5点算法；基础矩阵

无人驾驶车辆一般是通过在车上安装多种感应设备，主要包括各种车载传感器、GPS、雷达以及摄像机等，来感知周围的环境，并根据所获取的信息，自动规划路径实现车辆的自主驾驶，安全可靠地到达目的地[1]。实时准确的定位方法是实现无人驾驶技术的基础，是保障无人驾驶车辆完成自主行为的前提。传统的定位方式如全球卫星导航系统(global navigation satellite system, GNSS)、惯性导航系统(inertial navigation system, INS)以及里程计等都存在各自的局限性，在某些特殊环境下(如隧道、轮胎打滑等)会出现失误，不能确保得到精确的车辆位姿估计[2-4]。随着视觉技术不断发展，依靠视觉的定位技术已被广泛应用，比如移动机器人、无人机、卫星或水下探测器以及工厂AGV(automated guided vehicle)，同时也越来越多的被应用于无人驾驶车辆的自主定位和运动估计[5]。这主要是因为视觉定位方法的实现比较容易，仅利用摄像机拍摄的图像序列作为输入信息，获取相邻图像中的匹配特征对，然后利用摄像机成像模型实现坐标变换，通过两视角之间的对极几何约束来计算得到车辆的6自由度运动信息，包括3个方向旋转运动和3个方向的平移运动。

研制视觉里程计的最终目的是能够根据摄像机采集的图像序列来确定摄像机的空间坐标，位姿估计模块是视觉里程计的核心模块之一[6-9]。在视觉里程计位姿估计算法中，最普遍使用的是PnP(Perspective-n-Point)方法，在计算机视觉[10-11]、视觉测量[12]、机器人定位[13-14]和现实增强[15]方面有很多重要的应用。PnP方法首先是由FISCHLER等[16]提出的，随后HORAUD等[17]对该方法进行了完善。PnP问题定义为在目标坐标系中，给出一系列点的坐标以及在图像中像点的坐标，并在摄像机内参数已知的条件下，求取目标坐标系与摄像机坐标系之间的关系，包括3自由度的旋转和3个自由度的平移。对于基于PnP方法进行位姿估计的研究，主要针对P3P，P4P和P5P展开研究。因为当n<3时，该问题有无穷多解[18]。当n=3时，PnP问题最多有4个满足条件的解。当n=4时，若4个点共面，任意3点不共线，那么利用内参数已知的摄像机拍摄一幅图像，可以线性求解摄像机的外参数；若4个点共面时，则最多有5个满足条件的解。当n=5时，若5点中任意3点不共线，则P5P方法最多有2个解。并且2004年NISTÉR[19]提出了基于RANSAC的5点最小求解器来估计位姿，这成为了位姿估计问题中一个经典的算法。SCARAMUZZA等[20-21]提出一种基于RANSAC的单点算法，首次在视觉里程计位姿估计模块引入了车辆的运动学模型，通过单个摄像头来估计车辆的平面运动，并将基于RANSAC的单点估计算法与基于RANSAC的5点最小求解器算法进行了对比。基于车辆动力学模型的单目视觉里程计系统中的平面假设和忽略轮胎侧偏特性的问题，江燕华[22]提出了更适用于车载单目视觉里程计系统位姿估计的算法——MYP(model of yaw and pitch)算法，该算法解决了车体俯仰角对位姿估计的影响。纵观国内外对于视觉里程计位姿估计的研究情况，现有的算法无法同时满足高精度与实时性的需求。如5点位姿估计算法包含线性方程求解、高次多元方程求解以及奇异值分解等，对硬件的要求较高；单点算法是基于车辆运动模型平面运动假设之上的，没有考虑车辆运动中的变化，在实际应用中误差较大；MYP算法需要使用车辆本身的参数信息，需要通过其他传感器测量，且由于采用弧度表示其外参数，任何小误差都会引起较大的定位误差。

本文在传统五点位姿估计算法的基础上进行改进，根据基础矩阵的基本性质，对基础矩阵的求解过程进行优化。仿真结果表明，该方法得到的结果相较于传统的五点位姿估计算法硬件要求低，具有良好的实时性。

1 基于5点算法改进的快速位姿估计算法

1.1 5点位姿估计算法

单目视觉里程计系统位姿估计模块所涉及的是相对位姿问题。五点位姿估计算法是使用比较普遍的相对位姿估计算法。其工作原理是根据两视图之间存在的对极几何约束关系以及关联好的特征点对来求解车辆的位姿变化。对极几何约束的代数表示形式为基础矩阵F：

那么通过基础矩阵来表述对极几何约束的共面性:

空间点q在两视角下二维成像点q0和q1的归一化坐标分别为

则式(2)的对极几何约束可以变为

其中E=[t]xR，是本质矩阵。

通过叠加5对匹配特征点求解公式(5)，得到本质矩阵E，接着再对本质矩阵E进行奇异值分解得到旋转矩阵R和平移向量t。

图1 位姿估计示意图 Fig.1 Representation of pose estimation

1.2 基于基础矩阵改进的快速位姿估计算法

通过优化基础矩阵的求解过程，提出一种快速位姿估计算法，位姿估计示意图如图1所示。

算法具体步骤如下。

步骤1： 设基础矩阵F为

由于基础矩阵F在相差任意一个非零常数因子的条件下是唯一的，故可以令矩阵H中的元素fq=1，则

步骤2：利用相邻两帧得到的特征匹配集合中任意一对特征点的图像坐标的齐次坐标与式(7)的矩阵F代入式(2)中，得到式(8)：

可以将式(8)展开后重新写为如下的线性非齐次方程：

步骤3：约束方程式(9)中有8个未知参数(f1—f8)，从相邻两帧特征匹配集合中任意选取7对特征点，叠加7对特征点得到形如式(9)的方程，可以得到如下方程组：

式中，

矩阵Q7×8中左上标(i)(i=1,2,…,7)表示第i对匹配特征点。

步骤4：求解式(10)所示的非齐次线性方程组。非齐次线性方程组的解是由非齐次线性方程的一个特解与其对应的齐次方程的通解组成的。对于有7对匹配特征点形成的系数矩阵Q7×8的秩为7，所以其对应的非齐次方程的基础解系中只包含1个解向量，设为ξ，并且式(10)所示的非齐次线性方程组的解为η*，则式(10)的非齐次线性方程组的解为

步骤5：将步骤4中得到的解写成式(7)所示的矩阵形式

由基础矩阵是奇异矩阵的性质可知，矩阵F的秩为2，即其行列式det(F)=0。式(12)利用该性质可以计算得到x的值，则基础矩阵F可求出。

步骤6：由于摄像机的内参数已经提前标定得知，由式(1)可以计算本质矩阵E：

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

图2 从本质矩阵中得到的位姿矩阵的4组解Fig.2 Four sets of solutions for the position and orientation matrices obtained from the essential matrix

本质矩阵E有一个重要的性质是它的2个非零奇异值相等，那么本质矩阵E的奇异值分解(singular value decomposition, SVD)为E=Udiag(1,1,0)VT，则4组不同的旋转矩阵R和平移向量t为

从图2可以看出，只有图2 a)所示的两摄像机的位置才是视觉里程计系统所求的真实结果。摄像机的内参数已经提前标定得知，利用摄像机内参数和得到的旋转矩阵R和平移向量t，可以得到成像点在三维空间中的射线。通过特征匹配集合中任意一对特征点可以求得两条射线交点的三维坐标，然后判断该交点是否在摄像机的前方。若交点在两摄像机的前方，则得到的旋转矩阵R和平移向量t即为正确的。

2 实验结果分析

为了验证本文提出的单目视觉里程计系统算法的性能，将其与NISTÉR的5点算法、基于车辆运动学模型的位姿估计单点算法以及MYP(model of yaw and pitch)算法进行了比较。使用工业相机DFK 23G445采集图像，图像分辨率为1280×960。然后通过Matlab在PC机上进行仿真。仿真平台硬件环境为Intel Core i5，内存4 GB，软件开发环境为Windows 7操作系统，Matlab为2012b版。对4种算法的执行时间和估计误差进行了分析。

1)对4种算法的执行时间进行测试

采集图像后，首先进行特征提取，然后通过计算两幅图像各个特征点间的欧式距离得到特征匹配集合。并且固定匹配集合中外数据的比例为50%。4种算法都是基于RANSAC算法进行位姿估计的，其迭代次数NRANSAC计算公式见式(15)。

算法的迭代次数与用于生成模型假设的特征点的数量、特征匹配集合中外数据的比例以及从特征集合中选取的n个特征点都是内点的概率有关。若要保证特征集合中选取的n个特征点都是内点的概率达到99%，则NISTÉR的5点算法、基于车辆运动学模型的位姿估计单点算法、MYP算法以及本文中提出的算法各自的迭代次数如表1所示。

表1 4种算法的迭代次数

RANSAC算法是一种概率算法，具有不确定性，只是有一定的概率可以得到正确的模型假设。为了提高这个概率就需要增加迭代次数，出于算法鲁棒性的考虑，需要将其最小迭代次数放大一定的倍数。在测试中，对4种算法200次实验的执行时间的平均值、中值、最小值以及最大值进行了对比，对比结果如表2所示。

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

从表2可以看出，基于车辆运动学模型的位姿估计单点算法的运算效率最高，因为在该算法位姿解算过程中，只有横摆角一个未知量，利用一对特征点即可求出。MYP算法是通过两对匹配特征点进行解算基础矩阵的，在该算法中，需要对车辆的质量、后轮侧偏刚度以及车辆绕竖直轴转动惯量进行离线估计，其算法的运算时间约为单点算法的5倍。基于RANSAC的5点算法中涉及大量的数值计算，该算法的运行时间是MYP算法的35倍。改进算法在解算位姿过程中包括非线性方程的求解和奇异值计算，算法的运行时间是单点算法运行时间的50倍，是MYP算法的10倍，是5点算法的1/4。

2)对4种算法的定位精度进行分析

位姿估计的精度是通过计算旋转误差和平移误差进行评价的。令Rtrue和ttrue表示帧间旋转矩阵和平移向量的真实值，Rs和ts为利用位姿估计算法计算得到的旋转矩阵和平移向量。则算法的估计结果的旋转误差和平移误差分别定义为

为了对4种算法的精度进行比较，利用给图像添加高斯噪声，在不同的噪声水平下对它们进行了测试。为确保测试结果的准确性，所有列出的结果均由200次独立运行的结果求平均或取中值得到。图3给出了目标特征点数量固定为7时，从0到5的每隔0.5个像素的11个不同的噪声水平下得到的测量误差。从图3可以看出，在没有噪声时，4种算法的测量误差是没有差别的，都有较小的旋转误差与平移误差。随着噪声水平的提高，单点算法与MYP 算法的旋转误差有明显的增大趋势。这是由于单点算法是基于平面假设之上的，没有考虑车辆在行驶过程中由于车速变化或转向而引起的侧偏角和俯仰角的变化，并且俯仰角的变化会改变特征点的二维图像坐标；而MYP算法计算过程中需要用到车辆的质量、后轮侧偏刚度以及车辆绕竖直轴转动惯量，这些参数是无法测量的，还需要利用其他定位方式得到测量的真实位姿信息来对这些参数进行离线估计。并且，在以上两种算法中，位姿变换中的旋转矩阵和平移向量中都是用弧度来表示的，角度很小的差别都会引起旋转矩阵和平移向量很多的改变，故容易造成较大的误差。而5点算法与本文中的算法的测量误差也有增长，但增加的趋势不是很明显，故单点算法和MYP 算法的测量精度远不如5点算法和本文中的算法。而5点算法与本文中的算法两种算法的测量误差没有明显的差别，即两种算法的测量精度以及在噪声环境下的稳定性是相当的。本文提出的算法在保证定位精度的前提下，提高了算法的计算效率，是一种适用于车载视觉里程计的位姿估计算法。

图3 不同噪声水平下的定位误差Fig.3 Positioning error under different noise levels

3 结 语

本文提出一种基于基础矩阵性质进行改进的快速位姿估计算法，通过优化位姿计算过程，能够有效提高视觉里程计位姿估计模块的实时性。由于基础矩阵F在相差任意一个非零常数因子的条件下是唯一的，通过假设首先得到含有8个未知参数的基础矩阵，然后利用7对特征匹配点对来求解基础矩阵获取位姿信息。离线实验结果表明，本算法在兼顾定位精度的同时，具有良好的实时性，对视觉里程计在无人驾驶车辆上的实时应用具有一定的指导意义。在以后的工作中将结合车辆的运动模型，进行实时车载实验，进一步完善VO算法。

/

[1] ZHANG K, YANG A, SU H, et al. Service-oriented cooperation models and mechanisms for heterogeneous driverless vehicles at continuous static critical sections[J]. Intelligent Transportation Systems, 2017, 18(7): 1867-1881.

[2] LIU H, JIANG R, HU W. Navigational drift analysis for visual odometry[J]. Computing and Informatics, 2014, 33(3): 685-706.

[3] 张鹏娜，曾庆喜，祝雪芬，等. 卫星定位软件接受机发展综述[J]. 河北科技大学学报，2016 ,37(3):220-229.

ZHANG Pengna, ZENG Qingxi, ZHU Xuefeng, et al. Overview of development of satellite positioning software receiver[J]. Journal of Hebei University of Science and Technology, 2016 ,37(3):220-229.

[4] LIU Z, WANG L, LI K, et al. An improved rotation scheme for Dual-Axis rotational inertial navigation system[J]. Sensors, 2017, 17(13): 4189-4196.

肉制品的香气是由数百种风味各异的挥发性物质混合而成，然而只有少数的风味物质赋予食品特有的风味，这些特征性风味化合物对于产品的最终感官特性是必不可少的[31]。采用OAV筛选对板鸭总体风味最大的组分，OAV＞1表示为板鸭的关键风味化合物，对总体风味有直接影响。在一定范围内，OAV越大表示对板鸭的总体风味贡献最大。

[5] 秦永元，张洪钱，汪叔华. 卡尔曼滤波与组合导航原理[M]. 西安: 西北工业大学出版社, 2014: 227-236.

[6] SKOCZYLAS M. Vision analysis system for autonomous landing of micro drone[J]. Acta Mechanica et Automatica, 2014, 8(4): 199-203.

[7] ZHANG Yueqiang, SU Ang,LIU Haibo, et al. Pose estimation based on multiple line hypothesis and iteratively reweighted least squares[J]. Optics and Precision Engineering, 2015, 23(6): 1722-1731.

[8] YANG S, SCHERER S A, SCHAUWECKER K, et al. Autonomous landing of MAVs on an arbitrarily textured landing site using onboard monocular vision[J]. Intelligent & Robotics Systems, 2014, 74(1/2) : 27-43.

[9] XU G, QI X P, ZENG Q X, et a1. Use of land's cooperative object to estimate UAV's pose for autonomous landing[J]. Chinese Journal of Aeronautics, 2013, 26 (6): 1498-1505.

[10] LI S, XU C, XIE M. A robust O(n) solution to the perspective-n-point problem[J]. Pattern Analysis and Machine Intelligence, 2012, 34(7): 1444-1450.

[11] MIRAZEI F M, ROUMELIOTIS S I. Globally optimal pose estimation from line correspondences[C] //International Conference on Robotics and Automation(ICRA). Shanghai: IEEE, 2011: 5581-5588.

[12] ZHENG Y, KUANG Y, SUGIMOTO S, et al. Revisiting the PnP problem: A fast, general and optimal solution[J]. Computer Vision, 2013: 2344-2351.

[14] KNEIP L, SCARAMUZZA D, SIEGWARL R. A novel parametrization of the perspective-three-point problem for a direct computation of absolute camera position and orientation[J].Computer Vision and Pattern Recognition,2011,42(7):2969-2976.

[15] HESCH J A, ROUMELIOTIS S I. A direct least squares (DLS) method for PnP[C]//2011 IEEE International Conference on Computer Vision. Barcelona: IEEE, 2011: 383-390.

[16] FISCHLER M A, BOLLES R C. Random sample consensus: A paradigm for model fitting with applications to image analysis and automated cartography[J]. Communications of the ACM, 1981: 381-385.

[17] HORAUD R, CONIO B, LEBOULLEUX O. An analytic solution for the perspective 4-point problem[J]. Computer Vision Graphics Image Processing, 1989, 47(1):33-44.

[18] 吴福朝, 胡占义. PnP问题的线性求解算法[J]. 软件学报, 2003, 14(3):682-688.

WU Fuchao, HU Zhanyi. A linear method for the PnP problem[J]. Journal of Software, 2003, 14(3):682-688.

[19] NISTÉR D, NARODISKY O, Bergen J. Visual odometry[C]//2004 IEEE Computer Society Conference on Computer Vision and Pattern Recognition(CVPR). Washington: IEEE, 2004: 652-659.

[20] SCARAMUZZA D, FRAUNDORFER F, SIEGWART R. Real-time monocular visual odometry for on-road vehicles with 1-point RANSAC[C]//2009 IEEE International Conference on Robotics and Automation. Kobe: IEEE, 2009: 4293-4299.

[21] SCARAMUZZA D. 1-Point-RANSAC structure from motion for vehicle-mounted cameras by exploiting non-holonomic constraints[J]. Computer Vision, 2011, 95: 74-85.

[22] 江燕华.车辆运动特性约束的智能车辆视觉里程计系统研究[D].北京:北京理工大学,2014: 21-27.

JIANG Yanhua. Vehicle Motion Characteristics Constrained Visual Odometry System Research for Intelligent Vehicles[D]. Beijing: Beijing Institute of Technology, 2014: 21-27.

A fast monocular visual odometry pose estimation method for self-driving vehicles

ZENG Qingxi1,2,3, FENG Yupeng1,3, MA Shan1,3

(1.Self-driving Vehicle Research Center, Nanjing University of Aeronautics and Astronautics, Nanjing, Jiangsu 210016, China; 2. Prospective Technology Research Institute of Chery Automobile Company Limited, Wuhu, Anhui 241009, China;3.State Key Laboratory of Automotive Simulation and Control, Changchun, Jilin, 130012, China)

Aiming at the problem that the traditional pose estimation algorithm of monocular odometry cannot meet the needs of real-time localization of self-driving vehicles, a fast pose estimation algorithm based on the improvement of the fundamental matrix is proposed. By optimizing the calculation process of the fundamental matrix, the real-time performance of the algorithm is improved. The fundamental matrix with 8 unknown parameters is first obtained, and then the feature matching point pairs are used to solve the fundamental matrix. Through simulation experiments, the efficiency and accuracy of the algorithm are analyzed, and then it is compared with the existing algorithms. Experimental results show that the proposed algorithm can improve the speed of motion estimation by 4 times without the reduction of the accuracy of motion estimation. The study provides certain reference value to the real time application of the visual odometry of self-driving vehicles.

vehicle engineering; visual odometry; pose estimation;5 points algorithm; fundamental matrix

TP391.4

A

2017-07-21；

2017-08-21；责任编辑：冯 民

中国博士后科学基金(171980)；国家自然科学基金(51505221)；南京航空航天大学研究生创新基地(实验室)开放基金(kfjj20160216)

曾庆喜(1980—)，男，江苏溧阳人，博士，硕士生导师，主要从事无人驾驶车辆组合导航系统方面的研究。

E-mail:jslyzqx@nuaa.edu.cn

曾庆喜，冯玉朋,马 杉.无人驾驶车辆单目视觉里程计快速位姿估计[J].河北科技大学学报,2017,38(5):438-444.

ZENG Qingxi, FENG Yupeng, MA Shan.A fast monocular visual odometry pose estimation method for self-driving vehicles[J].Journal of Hebei University of Science and Technology,2017,38(5):438-444.