基于點轉移矯正的視覺慣性里程計

張夢龍，張凱杰，劉昌林，周 琦，李京波

（華南師范大學 半導體科學與技術學院，廣州 528225）

視覺慣性里程計[1（]Visual-inertial Odometry,VIO）是一種利用視覺和慣性測量單元（Inertial Measurement Unit,IMU）進行位置和姿態估計的技術，在自動駕駛、無人機[2]、增強現實等領域有廣泛的應用需求。視覺信息能夠減少IMU 長時間積分帶來的累積誤差，同時IMU 能夠提供尺度信息。在面對圖像模糊、圖像像素信息不準確時，IMU 信息作為補償能夠提高算法的魯棒性和準確性。VIO 主要分為基于濾波框架[3]、基于優化框架[4-7]和基于深度學習框架[8]三類原理。基于優化的VIO 采用關鍵幀策略，本質上是一個非線性的最小二乘問題；基于深度學習的VIO 采用神經網絡，因此十分依賴硬件的性能；基于濾波的VIO因其對計算資源的要求更低而受到廣泛關注。

卷簾相機因其成本更低在嵌入式設備中的應用比全局快門相機更為廣泛，但由于卷簾相機是依次讀取圖像上的每一行像素，所以圖像各行像素不是在同一時間獲取。卷簾效應如圖1 所示，左圖表示手機在快速運動下拍攝的圖像，相較于靜止狀態下手機拍攝的照片（右圖）有明顯的失真。因此，當系統處于較快的運動時，卷簾效應導致像素信息不準確，從而嚴重降低系統的定位精度。文獻[9][10]基于勻速假設對運動軌跡進行建模，估計每個關鍵幀的速度并對連續關鍵幀之間的位姿進行線性插值，有效避免了姿態解算錯誤或無法解算等情況。雖然基于勻速假設的運動模型可以節約計算資源，但定位精度較低。文獻[11]～[13]采用B 樣條曲線參數化運動軌跡以提高定位精度，文獻[4]采用神經網絡估計兩幀之間的矯正場，將卷簾圖像恢復成全局圖像。但是，這些方法在資源受限的移動端上實時運行起來較為困難。

圖1 卷簾效應Fig.1 Rolling shutter effect

綜上所述，低維的運動模型會降低位姿估計的精度[4]，但是高維模型會引入更多需要優化的變量，導致計算量大大增加。考慮到采用運動模型優化軌跡的方法具有一定的局限性，本文提出了一種基于三焦點張量的點轉移方法。在不引入更多變量的情況下，系統融合視覺和IMU 信息，并利用三焦點張量的點轉移方法提供嚴格的幾何約束信息，將卷簾圖像上的部分像素點（Rolling-shutter Point,RS 點）矯正成全局相機下的像素點（Global-shutter Point,GS 點）。采用擴展卡爾曼濾波（Extended Kalman Filter,EKF）和滑動窗口的方法，提高視覺慣性里程計的定位精度并降低計算量。

1 系統框架

基于點轉移矯正的VIO系統濾波算法包括狀態向量、IMU 運動學模型和相機觀測模型三部分，其整體框架如圖2 所示。

圖2 基于點轉移矯正的視覺慣性里程計Fig.2 VIO based on point transfer correction

1.1 狀態向量

k時刻的狀態向量xk是由當前時刻IMU 狀態xI、相關參數xW、n幀IMU 歷史位姿xC和SLAM點特征f組成：

其中，相關參數xW包括相機到IMU 的外參：IMU 和相機之間的旋轉四元數和平移向量CpI、相機和IMU 之間的時間差td、卷簾相機的讀取時間tr和相機的內參K。

定義x為真實值，為估計值，為誤差值。

1.2 IMU 運動學模型

IMU 運動學模型的輸入為加速度計和陀螺儀的測量值am和ωm：

其中，a和ω為真實的局部加速度和角速度；重力加速度矢量Gg≈ [0 0 9.81]T；為IMU 在世界坐標系下的旋轉矩陣；na和ng為零均值高斯噪聲。

利用非線性IMU 運動學模型Ε(?)將狀態向量估計值和協方差矩陣Pk|k從tk時刻傳播到tk+1時刻，得到tk+1時刻的狀態估計量為k+1|k和協方差矩陣為Pk+1|k：

其中，Qd為離散測量噪聲協方差矩陣；Gk為噪聲雅可比矩陣；Φ(tk+1,tk)為系統雅克比矩陣。

1.3 相機觀測模型

當系統接收新的一幀圖像時，系統增廣狀態向量和協方差矩陣。狀態向量加入新的IMU 位姿，該位姿對應的時間戳被設為圖像中間行的時間戳，其中t是圖像第一行對應的時間戳，是卷簾圖像的讀取時間。由于IMU 和相機之間存在時間差，因此使用時刻的IMU 測量值，并用該估計值增廣狀態向量。

EKF 協方差矩陣被增廣為：

當系統檢測和跟蹤一個3D 點特征時，對應k時刻圖像的像素坐標zk為：

其中，h(?)為透視投影方程；nk為測量噪聲向量，并且該噪聲服從零均值白高斯分布；為相對于相機幀坐標系（C系）的點特征位置。

將對應于單個點特征的所有測量值堆疊起來，并通過零空間操作對點特征位置進行線性邊緣化，最終得到EKF 的實際值和估計值之間的殘差：

其中，Hk為測量的雅克比矩陣；殘差用來更新狀態向量和協方差矩陣。

2 卷簾相機模型

2.1 融合IMU 信息求取RS 位姿

VIO 系統中的卷簾相機模型如圖3 所示。模型的實質是點特征矯正，將卷簾圖像的像素坐標（RS 點）矯正為真實的像素坐標（GS 點），GS 點是在卷簾圖像讀出時間tr的中間時刻（圖3 中紅色虛線）全局相機記錄的坐標點。

圖3 卷簾相機模型Fig.3 Rolling shutter camera model

滑動窗口內會存放多幀IMU 歷史位姿（GS 位姿），對應的時間戳是卷簾圖像讀出時間的中間時刻，即全局圖像上GS 點對應的時間戳。而系統接受的卷簾圖像時間戳對應的是卷簾圖像第一行的時間t。由于卷簾效應，圖像的每行像素對應的時間戳是不一致的，導致同一幀卷簾圖像上提取的RS 點對應不同時刻，即不同RS 點對應不同的相機位姿（RS 位姿）。如圖3 所示，通過融合高頻的IMU 信息和滑窗內歷史已優化的GS位姿求解圖像第n行tn時刻對應的RS 位姿

如圖3 所示，Δti是連續兩幀IMU 之間的時間差。卷簾圖像的中間行（紫色箭頭）對應的時刻和第n行（紅色箭頭）對應的時刻之間有k-1幀IMU 位姿。Δtk是卷簾圖像第n行（紅色箭頭）對應時刻與其最近的IMU 位姿（藍色箭頭）對應時刻之間的時間差。IMU的角速度為全局坐標系下的加速度為全局坐標系下的速度為通過上述方法，最終得到圖像第n行tn時刻對應的RS 位姿。

2.2 點轉移

當系統接收到新圖像時，存在多個3D 點特征X被當前卷簾相機幀IR1和滑動窗口內的歷史幀IR2同時觀察到，如圖4 所示。當前卷簾相機幀IR1對應的全局相機幀為IG1，當前卷簾相機幀的RS 點像素坐標為x1，與x1匹配的滑窗內歷史幀的RS 點像素坐標為x2。將通過點x2的直線l2選在歷史圖像幀上，基于三焦點張量的點轉移的方法，將當前幀IR1上的RS 點x1轉移到全局圖像IG1上，最終得到矯正后的GS 點xG-1：

圖4 采用三焦點張量的點轉移Fig.4 Point transfer using trifocal tensors

若l2=F21x1是對應于x1的對極線，則即無法計算出xG-1，所以應該盡量避免選擇到對極線。一種方法是利用x2的兩條或者三條直線，計算每條直線對應的xG-1并保留最大范數的一個；另一種方法是總選取垂直于對極線的直線。后者與前者相比，計算量更少。如圖4 所示，在相機幀IR1上x1對應的匹配點為相機幀IR2上的歸一化坐標點x2=(y1,y2,1)T，然后過點x2且垂直于直線（綠線）得到x1的對極線，即（紅線）。

已知基礎矩陣F21、F31和F32，由于第三個相機矩陣P3必須滿足條件是反對稱矩陣，所以這些矩陣中的每一個矩陣都能產生10 個關于P3元素的線性方程，一共可以得到關于P3的12 個元素的20 個方程，從而線性求出P3。最終得到的圖像幀相機矩陣對表示為：

根據相機矩陣對獲取張量：

其中，矩陣P2的(i,j)元記為；矩陣P3的(i,j)元記為。

至此，得到了當前卷簾相機幀IR1對應的全局相機幀IG1下的GS 點xG-1。隨著點特征追蹤長度的增加，滑窗內不同的歷史幀對應在全局圖像上矯正后的多個GS 點，采用最小二乘法計算出最優的GS 點的像素坐標。

2.3 靜態檢測和異常檢測

當系統處于靜止狀態時，圖像上不同行像素對應的RS 位姿都是一致的，相當于不同行的像素是在同一時刻獲取。因此，卷簾圖像可以被當作全局圖像，即可將圖像直接輸入VIO 系統，無須使用卷簾相機模型。將基于IMU 信息的檢測方法引入靜態檢測中，有：

檢測過程需要采用合適的卡方閾值χ2，并對慣性測量噪聲R放大α（α? [50,100]）倍，檢測系統是否處于零速狀態。

其中，H和P分別為關于ba、bg和的雅克比矩陣和協方差矩陣。

當檢測到系統處于零速狀態時，系統將關閉卷簾相機模塊，防止無效的點轉移影響系統的定位精度和穩定性。當系統進行點特征匹配時，點特征匹配對可能是錯誤的，錯誤的匹配對導致矯正后的GS 點偏離真實值。因此，需采用隨機采樣一致性（Random Sample Consensus,RANSAC）算法[14]剔除偏離點，從而保證輸入到VIO 系統的GS 點坐標值的準確性。

3 實驗驗證

為了驗證本文算法的性能，采用公開的TUM-RSVI 數據集[15]進行實驗，并與其他算法進行對比。同時，使用智能手機收集室內外數據，對各種算法進行測試，進一步驗證本文算法的適用性和魯棒性。

3.1 TUM-RSVI 數據集

TUM-RSVI 數據集由慕尼黑工業大學發布，是首個同時包含IMU、全局相機和卷簾相機的VIO 評估公開數據集。該數據集的卷簾相機和全局相機同步記錄圖像信息，并且IMU 和運動捕捉系統的數據在時間上是對齊的。數據集包含10 個室內數據，卷簾相機和全局相機均以20 Hz 的頻率和1280×1024 的分辨率同步記錄圖像，IMU 的頻率為200 Hz，運動捕捉系統以120 Hz 的頻率記錄真實運動，同時數據集給矯正后的卷簾圖像提供了同一時刻的全局圖像作為參照系。為了更好地驗證卷簾模型的有效性，分別采用全局圖像數據測試未帶卷簾相機模型的VIO（OpenVINS[3]、VINS-Mono[4]）和使用卷簾圖像測試帶卷簾相機模型的VIO（RS-VINS-Mono[4]、文獻[9]算法、本文算法（圖、表中用OUR 表示））。

本文算法的狀態向量里最多可存放75個“SLAM”特征，滑動窗口大小為11，每幀最大跟蹤的特征數為150。在公共TUM-RSVI 數據集中，所有算法都是在配備Intel Core i7CPU、3.2 GHz 和32 GB RAM 的電腦上運行。

圖5 和圖6 展示了不同算法在TUM-RSVI 數據集中第三個和第四個序列上的軌跡。VINS-Mono 是基于優化的方法，RS-VINS-Mono 是VINS-Mono 的帶卷簾相機模型的VIO 算法，其處理卷簾效應的方法類似于文獻[16]。文獻[9]和本文算法都有卷簾相機模型，并且都是基于濾波的VIO 系統，但文獻[9]是基于恒速運動假設的線性模型，而本文算法則是基于非線性模型。OpenVINS 作為基于濾波的基線方法為本文算法提供參照。

圖5 不同算法在TUM-RSVI 數據集中第三個序列的軌跡Fig.5 Trajectories of the different algorithm for the third sequence in TUM-RSVI dataset

圖6 不同算法在TUM-RSVI 數據集中第四個序列的軌跡Fig.6 Trajectories of the different algorithm for the fourth sequence in TUM-RSVI dataset

計算各種算法在每個序列上絕對軌跡誤差的均方根誤差，每個序列進行六次實驗，取平均值作為最終結果。實驗結果如表1 所示。

表1 不同算法在TUM-RSVI 數據集上的絕對軌跡均方根誤差（單位：米）Tab.1 Root mean square error of absolute trajectory for different algorithms on TUM-RSVI dataset (Unit: m)

表1 中圖像數據類型包括全局圖像（GS）和卷簾圖像（RS）。從表1 可以看出，未帶卷簾相機模型的VIO 算法（OpenVINS、VINS-Mono）在全局圖像上保持不錯的定位精度，但在卷簾圖像上，尤其是在平均角速度特別大的序列9 和序列10 上，其絕對軌跡誤差值較大，說明卷簾效應對系統定位精度的影響十分明顯。通過對比表 1 中基線方法（OpenVINS、VINS-Mono）在卷簾圖像上的誤差，帶卷簾相機模型的VIO 算法（RS-VINS-Mono、文獻[9]算法、本文算法）在處理卷簾圖像時能有效降低卷簾效應對系統定位精度的影響，并與未帶卷簾相機模型的VIO 在全局圖像上精度相當。如表1 所示，未帶卷簾相機模型的基準算法（OpenVINS）在卷簾圖像數據集上的平均絕對軌跡誤差為47.352 m，本文算法在卷簾圖像數據集上的平均絕對軌跡誤差為0.076 m，定位精度提高了99%；與帶有卷簾相機模型的RS-VINS-Mono 方法相比，本文算法的定位精度提高了27%；與文獻[9]算法相比，本文算法的定位精度提升了 47%。RS-VINS-Mono 和文獻[9]算法都是基于恒速運動模型的方法，在面對大的加速度場景時會出現較大的定位誤差。相比之下，本文算法利用高頻IMU 信息對RS點進行校正，不受場景限制和恒速運動假設的影響，實驗結果也驗證了本文算法在定位精度方面表現更好。

3.2 現實世界數據集

為了進一步評估本文算法在更具挑戰性的現實場景下的性能，將系統集成到一個安卓手機應用程序中，并在一臺華為手機上進行測試。卷簾相機以每秒25幀左右的頻率和640×480 的分辨率記錄圖像，IMU 以每秒250 次的頻率讀取。如圖7 所示，應用程序的界面左側實時顯示預覽畫面，左下角是啟動VIO 按鈕，右側顯示手機的運動軌跡（紅色線）和3D 點云（白色點），fps 代表華為手機處理卷簾圖像的幀率，一般每秒處理圖像幀數保持在15 幀～35 幀之間。

圖7 手機應用的運行界面Fig.7 Mobile application interface

打開安卓應用，并手持華為手機在室內環境移動，采用Nokov 運動捕捉系統記錄定位真值，室內環境如圖8 所示。

圖8 Nokov 運動捕捉系統Fig.8 Nokov motion capture system

為了更好地驗證本文算法在不同場景下的定位精度，采集了更多具有挑戰性的數據，并制作了一個真實世界數據集。該數據集包含四個序列，每個序列都基于不同的運動模式，其中包括加速度變化顯著的運動（A-S）、加速度穩定的運動（A-L）、帶有部分靜態狀態的運動（V-C）和全程無靜止狀態的運動（V-N）。表2 給出四種帶卷簾相機模型的VIO 系統在這些數據集上的性能。

表2 不同算法在現實世界數據集上的絕對軌跡均方根誤差（單位：米）Tab.2 Root mean square error of absolute trajectory for different algorithms in real-world dataset (Unit: m)

在不同運動模式下，相較于RS-VINS-Mono，本文算法的定位精度提升了49%；相較于文獻[9]算法，本文算法的定位精度提升了84%。另外，OURS-Z 是本文算法去除了零速檢測模塊，對比這兩種算法在V-N 和V-C 兩個序列上的實驗結果可以看出，對于全程無靜止狀態的數據V-N，OURS-Z 和OURS 的精度幾乎相當，但是對于帶有部分靜止狀態的數據V-C，去除零速檢測模塊的OURS-Z 的性能相較于OURS 明顯下降，進一步驗證了零速檢測模塊的有效性。

3 結論

針對卷簾快門相機由于卷簾效應引起圖像扭曲進而影響系統定位精度的問題，本文提出了一種融合IMU 信息的點轉移矯正VIO 方法。通過基于三焦點張量的點轉移的方法修正卷簾圖像，相當于輸入系統的是全局圖像，提高了系統定位的準確性。本文算法是在基于濾波的框架下實現的，保證了整體算法在嵌入式設備上的實時性。并且，本文算法引入了靜態檢測和異常檢測來提高算法的魯棒性。在公共數據集上的實驗結果表明，與帶有卷簾相機模型的RS-VINS-Mono 方法相比，本文算法的定位精度提升了27%；與文獻[9]算法相比，本文算法的定位精度提升了47%。本文算法與輸入全局圖像的基線方法（OpenVINS）的精度相當，進一步說明本文算法能有效降低卷簾效應帶來誤差，提升系統的定位精度。在更具挑戰性的室內數據集上的實驗結果表明，相比于其他算法，本文算法的定位精度提升了49%以上。此外，本文算法被嵌入到手機應用程序中，在手機上以25 Hz 左右的幀率實時處理卷簾圖像，對解決基于卷簾快門相機的VIO在未知環境中的定位問題具有一定指導意義。