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

孫 偉，劉思琪

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

孫 偉，劉思琪

（遼寧工程技術大學 測繪與地理科學學院，遼寧 阜新 123000）

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

視覺導航；慣性導航；三焦張量；多狀態約束；隨機抽樣一致算法

0 引言

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

1 三焦張量建模

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

圖1 三焦張量模型

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

2 基于隨機抽樣一致算法的連續影像特征點選取

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

從特征點組中隨機挑選一組數據進行量測更新，計算出符合該組特征點關系中的所有“局內點”。最后回到特征點中找到存有最多“局內點”的特征點組來進行量測更新[11]。

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

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

3.1 狀態方程

濾波器狀態可劃分為標稱狀態和誤差狀態，根據提出的方法可知，需要三幅影像進行濾波更新，那么相應的慣導系統也要獲取三個時刻位姿信息，標稱狀態是由慣導系統的測量單元、通過運動獲得輸出信息構成的[13-14]。其標稱狀態可以表示為：

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

3.2 量測方程

3.3 基于三焦張量的MSCKF方程

根據式（9）描述的慣導系統輸出位姿信息，建立連續時間增廣誤差的狀態方程為

誤差狀態協方差矩陣預測更新為

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

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

其中，

狀態值和測量值之間的協方差矩陣為

根據卡爾曼濾波方程，狀態值與狀態協方差矩陣做量測更新，即：

4 仿真結果及分析

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

圖2 RANSAC算法優化前后的特征點匹配比較

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

圖3 移動軌跡比較

從圖3中可以看出：慣導系統的定位結果隨時間的增加，軌跡嚴重偏離理想路線并呈發散狀態；而在另外兩種方法中，基于三焦張量的方法相比傳統的MSCKF方法更接近理想路線。

圖4 均方根誤差比較

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

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

表1 三種定位方法的位置和姿態RMSE比較

仿真結果驗證了基于三焦張量的視覺輔助慣導系統的定位方法性能最好，其定位精度更高。

5 結束語

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

[1]馮國虎, 吳文啟, 曹聚亮, 等. 單目視覺里程計/慣性組合導航算法[J]. 中國慣性技術學報, 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]張紅良, 郭鵬宇, 李壯, 等. 一種視覺輔助的慣性導航系統動基座初始對準方法[J]. 中國慣性技術學報, 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]蔡迎波. 基于多狀態約束的視覺/慣性組合導航算法研究[J]. 光學與光電技術, 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]胡高歌, 高社生, 趙巖. 一種新的自適應UKF算法及其在組合導航中的應用[J]. 中國慣性技術學報, 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]. 導航定位學報,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）；遼寧省高等學校創新人才支持計劃項目（LR2018005）；遼寧省教育廳高等學校基本科研項目（LJ2017FAL005），2018年度遼寧省“百千萬人才工程”人選科技活動資助計劃項目（遼百千萬立項【2019】45號）。

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

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