[發明專利]頭戴式增強現實系統的視覺和慣性混合位姿跟蹤方法

在審

• 申請號: 202210055655.3
• 申請日: 2022-01-18
• 公開（公告）號: CN114485637A
• 公開（公告）日: 2022-05-13
• 發明（設計）人: 何寧;晁建剛;許振瑛;胡福超;黃冉;佘佳宏;郭俊鵬;陳煒
• 申請（專利權）人: 中國人民解放軍63919部隊
• 主分類號: G01C21/16
• 分類號: G01C21/16;G01C21/18;G06T7/70;G06T7/20;G06T17/00;G06K9/62;G06F17/13;G06F17/16
• 代理公司: 北京豐浩知識產權代理事務所(普通合伙) 11781
• 代理人: 李學康

1.一種頭戴式增強現實系統的視覺和慣性混合位姿跟蹤方法，其特征在于，首先進行設備初始化，包括對視覺傳感器和慣性測量設備進行測量預處理，以及對視覺傳感器和慣性測量設備進行聯合標定，再利用視覺傳感器和慣性測量設備二者所采集的測量數據對頭戴式增強現實系統進行融合定位，對定位結果進行回環檢測和全局位姿圖優化，得到最終的位姿估計結果。

2.如權利要求1所述的頭戴式增強現實系統的視覺和慣性混合位姿跟蹤方法，其特征在于，所述的測量預處理過程，包括將視覺傳感器采集的幀圖像進行處理，同時對兩個連續幀圖像對應的慣性測量結果進行預積分。

3.如權利要求1所述的頭戴式增強現實系統的視覺和慣性混合位姿跟蹤方法，其特征在于，所述的融合定位過程，融合了預積分的慣性測量結果和視覺圖像特征觀測結果，實現對頭戴式增強現實系統的融合定位。

4.如權利要求1所述的頭戴式增強現實系統的視覺和慣性混合位姿跟蹤方法，其特征在于，所述的全局位姿圖優化，利用重定位結果進行全局優化以消除漂移。

5.如權利要求1所述的頭戴式增強現實系統的視覺和慣性混合位姿跟蹤方法，其特征在于，所述的設備初始化過程，首先基于滑動窗口的視覺SFM算法進行初始化，再計算頭戴式增強現實系統的位姿變化，通過將IMU預積分結果與視覺SFM算法結果進行匹配，恢復出尺度、重力、速度與偏差，用以引導基于非線性優化的VIO。

6.如權利要求5所述的頭戴式增強現實系統的視覺和慣性混合位姿跟蹤方法，其特征在于，所述的基于滑動窗口的視覺SFM算法進行初始化，包括特征點跟蹤、相機位姿估計、優化，對于特征點跟蹤，首先檢查頭戴式增強現實系統采集圖像的最新幀圖像與滑動窗口中所有幀圖像之間的特征對應關系，如果最新幀圖像和滑動窗口中任何幀圖像存在超過一定數量的跟蹤特征和旋轉補償像素，對最新幀圖像利用該跟蹤特征和旋轉補償像素進行相對旋轉和尺度平移，對這兩個幀中的所有特征進行三角化，若不存在，將最新幀圖像保存在滑動窗口中，并等待新的幀圖像；對于相機位姿估計，利用三角化后的特征，采用PnP方法來估計滑動窗口中所有幀圖像的姿態，進一步得到相機姿態；所述的優化，利用BA優化方法來最小化所有特征觀測的總重投影誤差，從而完成對相機姿態的優化估計；P為左目、右目相機分別獲取的左、右幀圖像的特征點對應實物在真實空間中的位置坐標，將P稱為特征位置，特征點p₁、p₂分別是該實物在左目、右目相機上的投影，O1、O2分別是左目、右目相機的中心；P的表示式為P＝[X,Y,Z]^T，依據針孔相機模型，則特征點p₁、p₂的位置坐標滿足關系系：

s₁p₁＝K₁P

s₂p₂＝K₂(RP+t)，

其中，K₁、K₂分別為左目、右目相機的內參矩陣，R、t分別為左目和右目兩個相機間的剛體轉換矩陣和偏置矩陣，s₁、s₂分別為左目相機和右目相機的比例系數，求得P的最小二乘解為：

從而得到特征位置估計結果；根據特征點匹配信息及特征位置估計，利用SFM算法獲得當前特征點的三維空間位置以及其在相機上的投影位置，再直接利用PnP算法估計圖像姿態。

7.如權利要求1所述的頭戴式增強現實系統的視覺和慣性混合位姿跟蹤方法，其特征在于，所述的對視覺傳感器和慣性測量設備進行聯合標定，對慣性測量設備進行標定，得到目標姿態、速度、重力矢量、陀螺儀偏置和三維特征位置的初始估計結果，用于引導基于非線性優化的視覺-慣性里程計VIO，計算出慣性測量設備對視覺傳感器的絕對尺度比例因子、陀螺儀偏置、加速度偏置、重力加速度和每個測量時刻的相對速度。