無人駕駛車輛單目視覺里程計快速位姿估計

曾慶喜，馮玉朋,馬 杉

(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點最小求解器算法進行了對比?；谲囕v動力學模型的單目視覺里程計系統中的平面假設和忽略輪胎側偏特性的問題，江燕華[22]提出了更適用于車載單目視覺里程計系統位姿估計的算法——MYP(model of yaw and pitch)算法，該算法解決了車體俯仰角對位姿估計的影響?？v觀國內外對于視覺里程計位姿估計的研究情況，現有的算法無法同時滿足高精度與實時性的需求。如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.