孫 偉，劉思琪

基于三焦張量的視覺輔助慣性定位方法

孫 偉，劉思琪

（遼寧工程技術(shù)大學(xué) 測繪與地理科學(xué)學(xué)院，遼寧 阜新 123000）

針對單一慣導(dǎo)解算誤差易隨時間累積的問題，提出一種視覺輔助慣性定位方法：利用三張影像構(gòu)建三焦張量模型，并求解相機(jī)位姿信息作為量測模型；然后通過隨機(jī)抽樣一致算法，對連續(xù)影像中獲得的特征點進(jìn)行篩選，剔除誤匹配及運(yùn)動物體上的特征點后，通過無損變換使其滿足非線性特征；最后采用多狀態(tài)約束卡爾曼濾波滑動窗口，對兩個系統(tǒng)輸出信息進(jìn)行濾波修正。實驗結(jié)果表明，該方法能有效地解決單一慣導(dǎo)誤差漂移問題，并且其精度和均方根誤差均優(yōu)于傳統(tǒng)的多狀態(tài)約束卡爾曼濾波的視覺輔助定位方法。

視覺導(dǎo)航；慣性導(dǎo)航；三焦張量；多狀態(tài)約束；隨機(jī)抽樣一致算法

0 引言

慣導(dǎo)系統(tǒng)定位誤差隨時間累積，無法長時間獨(dú)立開展定位工作，視覺匹配導(dǎo)航方法能夠限制慣導(dǎo)系統(tǒng)解算誤差累積并實現(xiàn)對位置誤差的修正[1-3]。眾多學(xué)者圍繞相關(guān)技術(shù)開展適應(yīng)性研究并取得一定成果。其中，文獻(xiàn)[4]提出擴(kuò)展卡爾曼濾波（extended Kalman filter, EKF）測量模型，實現(xiàn)視覺輔助慣導(dǎo)的多傳感器融合，但在線性化過程中，只保留的一階或二階項，在高度非線性環(huán)境下，會產(chǎn)生誤差累積；文獻(xiàn)[5]提出對極幾何來完成動態(tài)視覺估計，但其獲取的連續(xù)兩張影像特征點的位姿會出現(xiàn)奇異；文獻(xiàn)[6]提出基于三焦點張量求解攝像機(jī)運(yùn)動參數(shù)并實現(xiàn)了車輛運(yùn)動參數(shù)的估計，但無法剔除視覺定位中誤匹配的特征點。針對上述問題，本文提出一種基于三焦張量的視覺輔助慣性定位方案，即采用多狀態(tài)約束卡爾曼濾波（multi-state constraint Kalman filter, MSCKF）滑動窗口，對視覺導(dǎo)航和慣性導(dǎo)航系統(tǒng)輸出的信息進(jìn)行融合濾波修正，以此提高系統(tǒng)定位精度。

1 三焦張量建模

與對極幾何相似[7]，三焦張量建模是指在連續(xù)三張影像中構(gòu)造幾何關(guān)系，如圖1所示。連續(xù)影像的選擇，需要滿足在三張影像中，包含一定數(shù)量相同的特征點，確定三焦張量后，即可從中提取相機(jī)在三個位置的姿態(tài)矩陣，根據(jù)三張影像中的相同特征點位置變化獲得相機(jī)的位姿信息。

圖1 三焦張量模型

三焦張量不僅能滿足空間中某一點在三張影像中投影得到相機(jī)的位姿信息，還可以通過空間中某一線段獲取相應(yīng)位姿信息。同理推導(dǎo)出三焦張量的“點-點-線”關(guān)系式為

2 基于隨機(jī)抽樣一致算法的連續(xù)影像特征點選取

通過三焦張量模型得到的特征點中存在誤匹配點，將不同影像中相同特征點進(jìn)行有效匹配是視覺導(dǎo)航的核心。本文采用隨機(jī)抽樣一致算法（random sample consensus, RANSAC）[8-10]來剔除匹配錯誤或移動物體上的特征點并留下“局內(nèi)點”。首先，利用RANSAC來選取“局內(nèi)點”，其表達(dá)式為

從特征點組中隨機(jī)挑選一組數(shù)據(jù)進(jìn)行量測更新，計算出符合該組特征點關(guān)系中的所有“局內(nèi)點”。最后回到特征點中找到存有最多“局內(nèi)點”的特征點組來進(jìn)行量測更新[11]。

3 基于三焦張量的濾波融合算法

使用視覺輔助慣導(dǎo)系統(tǒng)進(jìn)行定位時，需要利用相機(jī)得到的特征點的位姿信息、以及連續(xù)影像的相對位姿信息作為濾波更新中的量測信息，兩個系統(tǒng)相互約束可提高定位精度，與常規(guī)視覺導(dǎo)航不同，量測信息是由三焦張量模型獲得的，用來輔助修正慣導(dǎo)信息[12]。

3.1 狀態(tài)方程

濾波器狀態(tài)可劃分為標(biāo)稱狀態(tài)和誤差狀態(tài)，根據(jù)提出的方法可知，需要三幅影像進(jìn)行濾波更新，那么相應(yīng)的慣導(dǎo)系統(tǒng)也要獲取三個時刻位姿信息，標(biāo)稱狀態(tài)是由慣導(dǎo)系統(tǒng)的測量單元、通過運(yùn)動獲得輸出信息構(gòu)成的[13-14]。其標(biāo)稱狀態(tài)可以表示為：

預(yù)測過程中的前一時刻位姿，在濾波器中不隨時間變化，不參與狀態(tài)方程更新，運(yùn)動模型可定義為：

3.2 量測方程

3.3 基于三焦張量的MSCKF方程

根據(jù)式（9）描述的慣導(dǎo)系統(tǒng)輸出位姿信息，建立連續(xù)時間增廣誤差的狀態(tài)方程為

誤差狀態(tài)協(xié)方差矩陣預(yù)測更新為

為提高量測模型非線性測量精度，采用無損變換（unscented transform, UT），選取西格馬（Sigma）點[15]后，基于MSCKF濾波器更新。選取Sigma點的計算方法為

將得到的Sigma點帶入量測模型，計算出測量值為

其中，

狀態(tài)值和測量值之間的協(xié)方差矩陣為

根據(jù)卡爾曼濾波方程，狀態(tài)值與狀態(tài)協(xié)方差矩陣做量測更新，即：

4 仿真結(jié)果及分析

1）RANSAC算法仿真結(jié)果及分析。如圖2所示，在圖2（a）中，傳統(tǒng)特征點匹配方法共提取32對特征點，其中存在較多的誤匹配點；在圖2（b）中，RANSAC算法剔除了這些誤匹配點。RANSAC算法具有較高的誤匹配點剔除能力，獲得最大“局內(nèi)點”集合并求得最佳模型，依賴RANSAC算法篩掉誤匹配特征點，可提高視覺導(dǎo)航精度。

圖2 RANSAC算法優(yōu)化前后的特征點匹配比較

2）基于三焦張量的視覺輔助慣性定位方法的仿真結(jié)果及分析。采用由德國卡爾斯魯厄理工學(xué)院和豐田美國技術(shù)研究院聯(lián)合創(chuàng)辦的公開數(shù)據(jù)集，獲取不同路徑下記錄的傳感器同步數(shù)據(jù)，將全球定位系統(tǒng)（global positioning system, GPS）及慣性測量系統(tǒng)（inertial measurement unit, IMU）組合定位結(jié)果作及慣性組合作為參考基準(zhǔn)，通過軌跡圖、均方根誤差（root mean square error, RMSE）圖以及RMSE，從姿態(tài)和位置兩個方面，對三種定位方法進(jìn)行對比，以驗證基于三焦張量的方法用于定位的可行性，比較結(jié)果如圖3、圖4所示。

圖3 移動軌跡比較

從圖3中可以看出：慣導(dǎo)系統(tǒng)的定位結(jié)果隨時間的增加，軌跡嚴(yán)重偏離理想路線并呈發(fā)散狀態(tài)；而在另外兩種方法中，基于三焦張量的方法相比傳統(tǒng)的MSCKF方法更接近理想路線。

圖4 均方根誤差比較

從圖4可以看出：隨時間的增加，慣導(dǎo)系統(tǒng)的測量值與真值偏差越來越大，且傳統(tǒng)的MSCKF方算法相比于基于三焦張量的方法具有更大的誤差，因此，基于三焦張量的方法與傳統(tǒng)的MSCKF方法相比，定位精度有一定的提高。三種定位方法的RMSE比較見表1。

從表1可以看出，慣導(dǎo)系統(tǒng)的位置均方根誤差達(dá)到了420.15 m，其定位結(jié)果不具可用性。傳統(tǒng)的MSCKF方法的位置RMSE為9.76 m，而基于三焦張量方法的位置RMSE只有傳統(tǒng)MSCKF方法的一半。同時，基于三焦張量的方法的姿態(tài)RMSE與傳統(tǒng)的MSCKF方法和慣導(dǎo)系統(tǒng)的姿態(tài)RMSE雖然相差不大，但其姿態(tài)RMSE是最小的。值得注意的是，慣導(dǎo)系統(tǒng)的姿態(tài)RMSE與另外兩種方法的RMSE相差不大，這是因為慣導(dǎo)系統(tǒng)中的陀螺儀獲得的角度信息只需要進(jìn)行一次積分，所以相對于位置信息，慣導(dǎo)系統(tǒng)的姿態(tài)信息更加可靠。

表1 三種定位方法的位置和姿態(tài)RMSE比較

仿真結(jié)果驗證了基于三焦張量的視覺輔助慣導(dǎo)系統(tǒng)的定位方法性能最好，其定位精度更高。

5 結(jié)束語

本文提出了基于三焦張量的視覺輔助慣性方法，只需利用三張影像構(gòu)建三焦張量模型并求解相機(jī)位姿信息，作為量測模型用于量測更新，通過隨機(jī)抽樣一致算法，對連續(xù)影像中獲得特征點進(jìn)行篩選，剔除誤匹配及運(yùn)動物體上的特征點后，通過UT變換使其滿足非線性特征。與慣導(dǎo)解算結(jié)果對比可看出，該方法均方根誤差較低且定位精度更有優(yōu)勢。

[1]馮國虎, 吳文啟, 曹聚亮, 等. 單目視覺里程計/慣性組合導(dǎo)航算法[J]. 中國慣性技術(shù)學(xué)報, 2011, 19(3): 302-306.

[2]?ZASLAN T, LOIANNO G, KELLER J, et al. Autonomous navigation and mapping for inspection of penstocks and tunnels with MAVs[C]//The Institute of Electrical and Electronic Engineers (IEEE). Proceedings of Robotics and Automation Letters. United States: IEEE, 2017: 1740-1747[2020-06-24].

[3]HESCH J A, KOTTAS D G, BOWMAN S L, et al. Consistency analysis and improvement of vision-aided inertial navigation[C]//The Institute of Electrical and Electronic Engineers (IEEE). Proceedings of Transactions on Robotics. United States: IEEE, 2014: 158-176[2020-06-24].

[4]張紅良, 郭鵬宇, 李壯, 等. 一種視覺輔助的慣性導(dǎo)航系統(tǒng)動基座初始對準(zhǔn)方法[J]. 中國慣性技術(shù)學(xué)報, 2014, 22(4): 469-473.

[5]STEFANO S, RUGGERO F, PIETRO P. Motion estimation via dynamic vision[C]//The Institute of Electrical and Electronic Engineers (IEEE). Proceedings of Transactions on Automatic Control. Jinan: IEEE, 1996: 393-413[2020-06-24].

[6]KITT B, GEIGER A, LATEGAHN H. Visual odometry based on stereo image sequences with RANSAC-based outlier rejection scheme[C]//The Institute of Electrical and Electronic Engineers (IEEE). Proceedings of Intelligent Vehicles Symposium. San Diego: IEEE, 2010: 486-492[2020-06-24].

[7]ZACHARIAH D, JANSSON M. Camera-aided inertial navigation using epipolar points[C]//The Institute of Electrical and Electronic Engineers (IEEE). Proceedings of Position Location & Navigation Symposium. Indian Wells: IEEE, 2010: 303-309[2020-06-24].

[8]WROBEL P B. Multiple view geometry in computer vision[J]. Kybernetes, 2001, 30(9/10): 1333-1341.

[9]FANG Qiang, ZHANG Daibing, HU Tianjiang. Vision-aided localization and navigation based on trifocal tensor geometry[J]. International Journal of Micro Air Vehicles, 2017, 9(4): 306-317.

[10]INDELMAN V, GURFIL P, RIVLIN E, et al. Real-time vision-aided localization and navigation based on three-view geometry[C]//The Institute of Electrical and Electronic Engineers (IEEE). Proceedings of Transactions on Aerospace & Electronic Systems. United States: IEEE, 2012: 2239-2259[2020-06-24].

[11]CIVERA J, GRASA O G, DAVISON A J, et al. 1-point RANSAC for EKF-based structure from motion[C]//The Institute of Electrical and Electronic Engineers (IEEE). Proceedings of International Conference on Intelligent Robots and Systems. Saint Louis: IEEE, 2009: 3498-3504[2020-06-24].

[12]蔡迎波. 基于多狀態(tài)約束的視覺/慣性組合導(dǎo)航算法研究[J]. 光學(xué)與光電技術(shù), 2015, 13(6): 62-66.

[13]CLEMENT L E， PERETROUKHIN V， LAMBERT J, et al. The battle for filter supremacy: a comparative study of the multi-state constraint Kalman filter and the sliding window filter[C]//The Institute of Electrical and Electronic Engineers (IEEE). Proceedings of Conference on Computer & Robot Vision. Halifax: IEEE, 2015: 23-30[2020-07-24].

[14]LI Mingyang, MOURIKIS A I. Improving the accuracy of EKF-based visual-inertial odometry[C]//The Institute of Electrical and Electronic Engineers (IEEE). Proceedings of International Conference on Robotics & Automation. Saint Paul: IEEE, 2012: 828-835[2020-06-24].

[15]胡高歌, 高社生, 趙巖. 一種新的自適應(yīng)UKF算法及其在組合導(dǎo)航中的應(yīng)用[J]. 中國慣性技術(shù)學(xué)報, 2014, 22(3): 357-361.

Visual assisted inertial positioning method based on trifocal tensor

SUN Wei, LIU Siqi

（School of Geomatics, Liaoning Technical University, Fuxin, Liaoning 123000, China）

Aiming at the problem that it is easy to accumulate over time for the calculation error of single inertial navigation, the paper proposed a visual assisted inertial positioning method: a three focus tensor model based on three images was established, and the camera pose information was solved as the measurement model; then the feature points obtained from continuous images were screened by the random sampling consistent algorithm, and after the mismatching and feature points on moving objects were eliminated, the points were made to meet the nonlinear characteristics through unscented transformation; finally the sliding window of multi state constrained Kalman filter was used to filter and modify the output information of the two systems. Experimental result showed that the proposed method could effectively solve the error drift problem of single inertial navigation, and its accuracy and root mean square error would be both better than those of the traditional visual aided positioning method based on multi-state constrained Kalman filter.

visual navigation; inertial navigation; trifocal tensor; multi-state constraint; random sampling consistent algorithm

P228

A

2095-4999(2021)02-0006-06

孫偉，劉思琪.基于三焦張量的視覺輔助慣性定位方法[J]. 導(dǎo)航定位學(xué)報,2021, 9(2): 6-11.（SUN Wei, LIU Siqi.Visual assisted inertial positioning method based on trifocal tensor[J]. Journal of Navigation and Positioning,2021,9(2): 6-11.）

10.16547/j.cnki.10-1096.20210202.

2020-07-09

2019遼寧省“興遼英才計劃”青年拔尖人才項目（XLYC1907064）；2019年遼寧省自然基金資助項目（2019-MS-157）；遼寧省高等學(xué)校創(chuàng)新人才支持計劃項目（LR2018005）；遼寧省教育廳高等學(xué)?；究蒲许椖浚↙J2017FAL005），2018年度遼寧省“百千萬人才工程”人選科技活動資助計劃項目（遼百千萬立項【2019】45號）。

孫偉（1984—），男，黑龍江鶴崗人，博士，教授，博士生導(dǎo)師，研究方向為多源信息融合導(dǎo)航、自適應(yīng)導(dǎo)航。

劉思琪（1996—），女，遼寧鞍山人，碩士研究生，研究方向為視覺導(dǎo)航與慣性導(dǎo)航。