[發明專利]基于異構傳感器信息融合的位姿跟蹤系統及方法

在審

• 申請號: 202111295309.4
• 申請日: 2021-11-03
• 公開（公告）號: CN114022553A
• 公開（公告）日: 2022-02-08
• 發明（設計）人: 王景川;趙文韜;茅天陽
• 申請（專利權）人: 上海交通大學
• 主分類號: G06T7/73
• 分類號: G06T7/73;G06K9/62;G06F17/18
• 代理公司: 上海漢聲知識產權代理有限公司 31236
• 代理人: 胡晶

本發明提供了一種基于異構傳感器信息融合的位姿跟蹤方法及系統，包括：步驟S1：基于內部傳感器提取運動數據建立六自由度運動模型，基于六自由度運動模型進行位姿狀態預測；步驟S2：基于外部傳感器提取環境特征信息建立觀測模型，基于觀測模型進行觀測更新位姿狀態預測；步驟S3：針對異構傳感器位姿狀態進行非線性狀態空間建模，得到非線性狀態空間模型；步驟S4：利用PaRIS算法對非線性狀態空間模型進行在線正切濾波估計，并利用隨機梯度下降算法實時更新多傳感器標定外參，同時獲得高精度位姿估計。本發明所采用的融合位姿跟蹤算法可以有效地提升定位的準確性與魯棒性。

技術領域

本發明涉及位姿跟蹤系統數據集采集以及多傳感器融合等技術領域，具體地，涉及基于異構傳感器信息融合的位姿跟蹤系統及方法，更為具體地，涉及一種位姿跟蹤系統在長期工作或者數據采集過程中的多傳感器的在線聯合標定及位姿估計方案。

背景技術

多傳感器融合技術是現階段移動機器人、智能駕駛等領域研究的熱點。相比于單一傳感器受限于自身工作環境的約束：惡劣天氣會造成激光雷達測量精度下降甚至失效；早晚間陽光的斜射可能會在相機視野中形成光斑、夜間曝光不足而給機器視覺帶來了極大的困難。多傳感器融合定位具有更高精度和更好的魯棒性[1]。

在位姿跟蹤系統的長時間工作或者數據集的長期工作中，除了收到外界因素的影響外，傳感器的退化、傳感器內部時鐘的不同步、結構不穩導致的震動、偏差同樣會對融合定位算法帶來新的挑戰。如何確定傳感器之間的相對時延、相對位姿，從而克服運動過程中的震動、結構形變等因素，是為長期數據集提供可靠的真值基準的首要條件。

國內外主流方案有基于深度學習、基于優化和基于濾波三種。[2]提出了一種用于單目視覺慣性里程計的自監督深度學習網絡DeepVIO，通過直接結合2D光流特征和IMU數據來提供絕對軌跡估計。但是基于深度學習的方法十分依賴訓練數據的質量，并且不具備可解釋性，較難落地應用；[3]提出了VINS-Mono：一種具有魯棒性和通用性的單目視覺慣性狀態估計器。該方法從用于估計器初始化和故障恢復的魯棒的程序開始。采用一種基于緊耦合、非線性優化的方法，通過融合預積分后的IMU測量值和特征觀測值，獲得高精度的視覺慣性里程計。基于優化的方法計算復雜度偏高，對位姿跟蹤計算性能要求較高；[4]提出了MSCKF2.0算法，基于擴展卡爾曼濾波，且能夠實現一致估計，確保了建立的線性系統模型正確的觀測性，并能對相機-IMU的外參進行在線估計校正。基于濾波的方法具有計算簡單的特點通常可以做到實時運行，但精度較低。

本發明采用一種基于粒子濾波的方法實現位姿估計。與現有濾波方案不同，本發明在位姿估計中采用了一種高效率的前向濾波與反向平滑相結合的方案，在不降低太多計算效率的情況下，進一步提升了算法準確性。

基于前人的工作，本發明創新地提出了一種可用于數據長期采集過程中的多傳感器融合定位算法，該方法可以動態調整各個傳感器之間的相對位姿，可用于位姿跟蹤系統長時工作以及作為長期數據采集的真值方案。

[1]Nagla K S,Uddin M,Singh D.Multisensor data fusion and integrationfor mobile robots:A review[J].IAES International Journal of Robotics andAutomation,2014,3(2):131.

[2]Han L,Lin Y,Du G,et al.Deepvio:Self-supervised deep learning ofmonocular visual inertial odometry using 3d geometric constraints[C]//2019IEEE/RSJ International Conference on Intelligent Robots and Systems(IROS).IEEE,2019:6906-6913.