基于緊耦合的視覺慣性定位方法

盧佳偉，許哲

(上海海洋大學工程學院，上海 201306)

0 引言

結合視覺和慣性測量進行同步定位和地圖構建，在智能機器人和計算機視覺領域已經引起廣泛關注.通過引入慣性測量單元（IMU）的信息可以很好地彌補單目視覺即時定位與地圖構建（SLAM）算法本身存在的一些缺陷.一方面將IMU和相機估計的位姿序列對齊可以得到相機軌跡的真實尺度，而且IMU可以很好地預測出圖像幀的位姿，提高特征跟蹤算法的匹配速度和應對快速旋轉的算法魯棒性.另一方面IMU中加速度計提供的重力向量可以將估計的位姿轉化為實際導航需要的世界坐標系中.

根據相機和IMU數據融合框架的不同，視覺慣性里程計（VIO）可分為緊耦合和松耦合[1].松耦合直接將VIO和慣性導航的輸出結果進行融合.典型的方法是S.Weiss等[2-3]提出的基于擴展卡爾曼濾波的SSF、MSF算法，將視覺里程計（VO）處理完的數據與IMU數據進行融合.這兩種算法雖然能實時獲得航行器的位姿，但是無法糾正視覺測量引入的漂移，所以定位精度沒有緊耦合框架的高.而在緊耦合中是使用兩個傳感器的原始數據共同估計一組變量，傳感器的噪聲也是相互影響的.雖然緊耦合的算法比較復雜，但充分利用了傳感器數據，是目前研究的重點.Mourikis等[4]于2007年提出了一種基于濾波的VIO算法模型多狀態約束卡爾曼濾波（MSCKF）.MSCKF利用一個特征點同時約束多個相機位姿，進行卡爾曼濾波更新，但是在跟蹤情況下沒有使用IMU的先驗數據，且無法用到全局的信息進行全局優化，因此系統極易不收斂，導致定位精度較差.Stefan Leutenegge等[5]于2015年提出基于雙目相機和IMU的OKVIS算法.該算法通過IMU測量值對當前狀態做預測，根據預測進行特征提取與匹配，并將優化中的重投影和IMU的測量誤差放在一起做優化.但是由于OKVIS不支持重定位，也沒有閉環檢測和校正方案，在機器人劇烈運動時容易跟蹤失敗.香港科技大學沈劭劼團隊[6]于2017年開源了一套Visual-Inertial融合定位算法VINS-MOMO，該算法以KTL光流跟蹤作為前端，而且具有比OKVIS更加完善和魯棒的初始化以及閉環檢測過程.但是該算法不容易構建全局地圖，且視覺約束只靠滑動窗口里的關鍵幀.

針對上述問題，本文采用將IMU與視覺SLAM兩種定位方案在基于非線性的緊耦合結構下相互融合，構建具有較高定位精度的定位系統[7-8].采用VINS-MONO算法，使用單目相機作為視覺傳感器與IMU進行組合定位與建圖.但是在前端中選用快速特征點提取和描述的算法（ORB）[9]特征，保證了特征子具有旋轉，尺度不變性的同時，提取特征點的速度與匹配效率明顯提升，從而能夠及時的擴展場景，構建全局地圖.

1 視覺里程計

在經典的視覺SLAM框架中，VO是通過相鄰圖像之間匹配的特征點估算出相機的運動并構建局部場景的地圖.VO又稱前端（Front End），它通過匹配好的特征點計算出相鄰圖像之間的運動，得到機器人的運動軌跡，從而解決了定位問題.另一方面，根據每個時刻的相機位置，計算出各像素對應空間點的位置，就得到了地圖.VO的算法流程如圖1所示.

圖1 VO算法流程圖

1.1 圖像特征點提取與匹配

在圖像特征中，尺度不變特征轉換(SIFT)[10]是考慮得比較完善的一種.它能夠適應機器人運動過程中出現的光照及旋轉等變化，但是會增加大量的計算.另一些特征，考慮適當地降低精度和魯棒性，提升計算速度.例如FAST關鍵點屬于計算特別快的一種特征點.而ORB特征可以用來對圖像中的關鍵點快速創建特征向量，這些特征向量可以用來識別圖像中的對象.它在FAST檢測子不具有方向性的問題上做了改進，并采用速度極快的二進制描述子二元魯棒獨立基本特征（BRIEF），使整個圖像特征提取環節大大加速.提取ORB特征分為兩個步驟：

1）查找圖像中的關鍵點，并使用FAST算法對關鍵點進行檢測；

2）使用BRIEF算法將關鍵點變成特征向量.

特征匹配是通過計算特征向量之間的距離來確定路標點前后時間的對應關系.當特征點數量較小時，可使用暴力匹配算法，它計算某一個特征點的特征向量與其他所有特征點的特征向量之間的距離，然后取向量距離最小的一個作為匹配點.但特征點多時，暴力匹配的運算量變大，會影響算法的實時性要求.因此，選用快速近似最近鄰（FLANN）算法[11]更加適合于匹配點數量極多的情況.該算法理論已集成到OpenCV，使用時可直接從中調用.

1.2 單目相機運動估計

通過特征提取與匹配，可從兩張圖像中得到一對匹配好的特征點，如圖2所示.相鄰的圖像存在很多對匹配點，可通過它們之間的對應關系得到相機的運動.

圖2 對極幾何約束

在第一幀的坐標系下，設P點的空間位置為：

由針孔相機模型推出兩個像素點p1,p2的像素位置為：

式中：K為相機內參矩陣；R、t為兩個坐標系下相機的旋轉與平移運動.

兩個像素點在歸一化平面上的坐標x1,x2為：

對式（3）進行一系列變換并帶入p1,p2得：

可將上式（4）中間部分記作兩個矩陣：基礎矩陣F和本質矩陣E，帶入（4）式得：

相機的位置估計問題變為以下兩步：

1）根據配對點的像素位置，求出E或者F；

2）根據E或者F，求出R、t.

2 IMU傳感器模型構建

一般而言，IMU內部包含加速度計和陀螺儀，分別測量機器人的加速度信息和角速度信息.

忽略尺度因子的影響，只考慮白噪聲和隨機游走偏差，VIO中IMU模型為：

式中：上標b為IMU坐標系，w為世界坐標系world；上標a為加速度計，g為陀螺儀；ωb和ab分別為IMU輸出的角速度和加速度；為IMU真實的角速度和加速度數據；mg和ma為陀螺儀和加速度計的零偏；ng和na為陀螺儀和加速度計的高斯白噪聲；gw為世界坐標系下的重力加速度；qbw為世界坐標系向IMU坐標系下的旋轉矩陣.

IMU的位移,速度,旋轉對時間的導數為：

根據式（10）、（11）、（12）導數關系，使用中值法得到運動模型的離散積分，即兩個相鄰時刻k到k+1的位姿用兩個時刻的測量值ω，a的平均值來計算：

其中：

3 VIO緊耦合優化器設計

融合IMU是為了克服純視覺SLAM的一些缺點.視覺受光線條件影響和限制，且在紋理豐富的場景中可以正常工作，然而遇到玻璃、白墻等特征稀少的場景時就無法正常工作.純視覺還難以處理動態場景，環境變化時，會誤認為自己在運動，而IMU能夠感受到自己的運動，避免運動誤判.純視覺也難以處理運動過快的場景，兩幀圖像之間的重疊區域太少甚至沒有，造成無法基于兩幀圖像中相同點和不同點測算運動，而IMU可以補充發揮作用，提供可靠的位姿估算.

VINS系統主要包括3部分.如圖3所示：前端進行數據預處理，包括特征提取匹配和IMU預積分；初始化是對系統初始狀態變量（重力方向、速度、尺度等等）進行處理；后端通過滑動窗口算法得到最優狀態估計.

圖3 VINS系統框架

3.1 測量數據預處理

特征提取與跟蹤在視覺里程計部分已進行詳細討論.接下來主要是對IMU數據進行預積分.可對IMU數據進行積分得到從i時刻到j時刻的位移、旋轉和速度.但是每次優化更新后，都需要重新進行積分，導致運算量較大.此時，可用一個簡單的公式轉換，將積分模型轉化為預積分模型：

通過式（18），位移、速度、旋轉積分公式中的積分項則變成相對于第i時刻的姿態，而不是相對于世界坐標系的姿態.此時，位移、速度、旋轉積分公式為：

式(19)、(20)、(21)中的積分項僅跟IMU測量值有關，它將一段時間內的IMU數據直接積分起來就得到了預積分量：

3.2 VINS系統初始化

單目緊耦合VIO是一個非線性的系統.VIO無法直接觀測到尺度，如果沒有良好的初始值，很難直接將相機和IMU的數據融合在一起.可以假設一個靜止的初始條件來啟動單目VINS估計器.然而，這種假設是不合適的，因為在實際應用中經常會遇到運動下的初始化.當IMU測量結果被大偏置破壞時，情況就變得更加復雜了.事實上，初始化通常是單目VINS最脆弱的步驟.需要一個魯棒的初始化過程以確保系統的適用性.

初始化過程中，首先要建立視覺坐標系和IMU坐標系的聯系.假設相機和IMU之間有粗略測量的外部參數：旋轉qbc、位移pbc，我們可以將姿態從IMU坐標系轉換到相機坐標系：

由于IMU采集數據的頻率大于相機的，所以初始化過程中需將IMU預積分數據與相機的位姿數據對齊.具體過程如下：

1)旋轉外參數qbc未知,則先估計旋轉外參數.

2)利用旋轉約束估計陀螺儀偏離.

3)利用平移約束估計重力方向、速度、以及尺度初始值.

5)求解相機坐標系與世界坐標系之間的旋轉矩陣，并將軌跡對齊到世界坐標系.

3.3 滑動窗口優化

初始化完成后，VINS系統采用基于滑動窗口的緊耦合單目VIO進行高精度和魯棒的狀態估計.

VINS系統優化的狀態變量為：

式中：xk表示第k幀圖像時的IMU的位姿；n是關鍵幀的總數；m是滑動窗口中的特征總數；λi為第一次觀測到第i個特征的逆深度.

通過最小化滑動窗口中的殘差項來估計系統的狀態變量：

式中：rB和rC分別是IMU和視覺測量的殘差；B是所有IMU數據的集合；C是在當前滑動窗口中至少觀測到兩次的一組特征；（rP，JP）是來自邊緣化的先驗信息；ρ為Huber范數.

4 實驗驗證與分析

本文中實驗所用電腦為Inter Core i5-4210記本，內存12 GB，系統為Ubuntu16.04，ROS版本為kinetic.

實驗1的算法驗證是基于圖3所示的VINS系統框架（無回環檢測）.通過數據文本構建仿真場景，如圖4所示該場景是由三個長方體堆疊組成，長、寬、高分別為：10×10×3、7×7×3、4×4×4.然后由仿真場景生成視覺特征的仿真數據集，采樣頻率為30 Hz，其中特征點的3D坐標如圖4所示為長方體的頂點和附近的點，共48個.IMU的仿真數據集是由在x、y平面內做橢圓運動，z軸做正弦運動的曲線生成，采樣頻率為180Hz.最后通過配置不同陀螺儀和加速度計噪聲的大小，驗證該算法的有效性，其中圖像的像素誤差給定為1.5.

圖4 仿真場景

將已經準備好的仿真數據集（視覺特征，IMU數據）接入VINS代碼，并運行出軌跡結果.具體實驗步驟為：

1)IMU仿真數據集無噪聲；

2)IMU仿真數據集有噪聲,具體噪聲參數的設置如表1所示.表中g_bias、a_bias分別表示陀螺儀和加速度計的隨機游走偏差，g_noise、a_noise分別表示陀螺儀和加速度計的高斯白噪聲.

表1 仿真數據集噪聲參數

當IMU仿真數據集無噪聲，如圖5所示，優化軌跡imu_int和帶有噪聲的軌跡imu_int_noise重合，但由于存在像素誤差，它們與真實軌跡ground_truth并未重合.當IMU仿真數據集有噪聲時，由表1所示，第三組數據各項參數大概是第二組的10倍，但如圖7所示，盡管imu_int_noise軌跡變化很大，最終優化軌跡imu_int都能像圖6有有效的跟蹤真實軌跡ground_truth.

圖5 第1組實驗結果

實驗2采用的是EuRoC提供的V1_01_easy數據集，將本文方案與ORB-SLAM 2單目作比較.圖8和圖9分別為兩種算法的V1_01_easy軌跡平面圖，圖中的虛線表示真實軌跡，彩色線表示算法的定位軌跡，從這兩幅幅圖中，可以看出VINS算法定位軌跡在V1_01_easy數據集下與真實軌跡更為穩合.圖10表示兩種算法在V1_01_easy數據集下的軌跡誤差，圖中可以明顯看出VINS_result_no_loop_V1.csv的定位誤差在整體上要優于ORB-SLAM 2_1.txt單目的定位誤差.表2顯示了定位誤差的狀態對比，VINS-MONO定位誤差的最大值、最小值、標準差、均方根誤差均小于ORB-SLAM 2單目的定位誤差.圖11是定位誤差的小提琴圖，主要統計和分析數據的離散程度和分布密度，兩圖中左邊表示VINS_result_no_loop_V1.csv，右邊表示ORB-SLAM 2_1.txt.圖中可以看出VINS_result_no_loop_V1.csv誤差的分布集中在0.2m附近，而ORB-SLAM 2_1.txt分布集中在1m附近且分布范圍較廣；圖中VINS_result_no_loop_V1.csv誤差的中位數和四分位數都優于ORBSLAM 2_1.txt單目；VINS_result_no_loop_V1.csv誤差的上側和下側的離散情況也要優于ORBSLAM 2_1.txt單目.

圖6 第2組實驗結果

圖7 第3組實驗結果

圖8 VINS算法軌跡

圖9 單目ORB-SLAM算法軌跡

圖10 兩種算法軌跡誤差對比

表2 兩種算法在V1_01_easy數據集下誤差狀態對比 m

圖11 兩種算法定位誤差對比的小提琴圖

5 結束語

本文針對慣性傳感器積分時位姿容易發散，且機器人劇烈運動時，VO定位容易失效等問題，提出了一種基于單目相機與IMU的機器人定位算法.該算法主要是利用VINS-MONO框架，但在前端進行特征提取與匹配的時候，采用ORB特征，提高了新增關鍵點的特征匹配效率，更好地構建場景地圖.在處理IMU數據時，利用中值法得到運動模型的離散積分，能夠更加精確地定位機器人的位置.后端仍采用滑動窗口的非線性優化得到機器人運動的最優狀態估計.

實驗一在IMU仿真數據中添加噪聲進行試驗驗證，該算法能夠有效地減小慣性傳感器的累積誤差.實驗二利用V1_01_easy數據集驗證了該算法優于單目ORB-SLAM算法.但本文算法沒有閉環檢測模塊，在機器人長時間、大范圍的導航和定位中，單純依靠VINS系統的狀態估計會出現嚴重偏差.且該算法處理的數據量很大，實時性問題還存在問題，現只適用于室內小范圍場景.因此下一步會將閉環檢測添加到該算法框架，考慮進一度簡化該算法框架，達到實時性的要求.