視覺與慣性傳感器融合的室內(nèi)導(dǎo)航算法研究

楊志芳，潘忠運(yùn)

（武漢工程大學(xué) 電氣信息學(xué)院，武漢430200）

目前，室內(nèi)移動機(jī)器人的導(dǎo)航定位算法主要采用的是SLAM（同步定位與建圖）算法[1]，SLAM 算法大致可分為兩大類，激光SLAM 和視覺SLAM[2]。激光SLAM 是通過采集不同時刻的兩片點(diǎn)云信息（一系列分散的、具有準(zhǔn)確角度和距離信息的點(diǎn)集）然后將兩片點(diǎn)云圖進(jìn)行匹配，計算出激光雷達(dá)相對運(yùn)動的距離和姿態(tài)變化，也就是對機(jī)器人的定位。激光SLAM 需要搭載成本較高的激光雷達(dá)，同時激光雷達(dá)獲得的環(huán)境信息不如視覺傳感器豐富。視覺SLAM 成本低廉、獲得的環(huán)境信息豐富，然而為了提高移動機(jī)器人定位的準(zhǔn)確度和穩(wěn)定性，靠純視覺SLAM 系統(tǒng)來對環(huán)境的感知是不夠的，因為純視覺SLAM 系統(tǒng)往往容易受到光照變化的影響，同時在低紋理和運(yùn)動速度較快導(dǎo)致圖像信息模糊的情況下定位精度很低，甚至無法使用。融合IMU[3]很好地克服了純視覺SLAM 的缺點(diǎn)，在運(yùn)動速度過快的情況下，圖像會出現(xiàn)模糊，兩幀圖像之間的特征點(diǎn)匹配極少甚至沒有，在遇到玻璃、白墻等低紋理的情況下同樣特征點(diǎn)匹配很少。在這些情況下，IMU 可以提供數(shù)據(jù)，繼續(xù)估算可靠的位姿。在動態(tài)環(huán)境中，當(dāng)環(huán)境變化時，純視覺SLAM 可能誤以為自身在動，而IMU 可以感知自身運(yùn)動，從而避免誤判。

近幾年IMU 與視覺融合的SLAM 研究越來越流行，有一些開源的VIO[4]（視覺慣導(dǎo)里程計）系統(tǒng)。如ROVIO 是基于擴(kuò)展卡爾曼濾波的緊耦合方法，由Bloesch 等人在2015年提出的一種視覺里程計[5]，具有跟蹤3D 路標(biāo)點(diǎn)圖像特征匹配的功能，但僅支持單目相機(jī)。OKVIS 也是一種基于擴(kuò)展卡爾曼濾波的緊耦合方法，它是由蘇黎世大學(xué)的Stefan Leutenegge 等人在2015年提出的[6]，在前端采用的是特征點(diǎn)法和BRISK 描述子[7]，后端使用滑動窗口模型的視覺慣導(dǎo)SLAM 系統(tǒng)，同時首次在系統(tǒng)中對IMU 進(jìn)行了預(yù)積分處理[8]，解決了IMU 與相機(jī)幀率差異問題。支持基于單目和雙目相機(jī)的滑動窗口位姿估計。VINS 是香港科技大學(xué)沈劭劼團(tuán)隊于2017年提出的通用單目視覺慣性估計的魯棒方法[9]，它的前端使用Harise 角點(diǎn)檢測，光流法來跟蹤[10]，后端是基于優(yōu)化和滑動窗口的VIO，該系統(tǒng)主要應(yīng)用于增強(qiáng)現(xiàn)實領(lǐng)域，也可以在手持式設(shè)備或無人機(jī)上運(yùn)行。

1 改進(jìn)系統(tǒng)框架

本文是在經(jīng)典的ORB-SLAM2 框架基礎(chǔ)上進(jìn)行改進(jìn)的，設(shè)計了基于非線性優(yōu)化的IMU 數(shù)據(jù)與RGB-D 圖像數(shù)據(jù)緊耦合的方式來解決純視覺SLAM所面對的問題。改進(jìn)系統(tǒng)框架如圖1所示，系統(tǒng)的前端包括圖像信息和IMU 傳感器信息，將獲得的圖像信息進(jìn)行特征提取與匹配，然后進(jìn)行動態(tài)特征點(diǎn)濾除和RANSAC 算法的誤匹配剔除，將動態(tài)特征點(diǎn)和誤匹配點(diǎn)濾除掉。IMU 數(shù)據(jù)進(jìn)行預(yù)計分處理，使其頻率與相機(jī)獲取圖像的頻率保持一致，然后系統(tǒng)進(jìn)行視覺慣導(dǎo)聯(lián)合初始化，確定載體的初始狀態(tài)（初始位置、初始速度、加速度和陀螺儀的偏差、重力向量、世界坐標(biāo)與載體坐標(biāo)系的變換關(guān)系等等），后端包括基于滑動窗口的關(guān)鍵幀篩選和緊耦合非線性優(yōu)化，最小化重投影誤差（相機(jī)的重投影誤差和IMU 誤差）求出載體的位姿信息。輸出是載體的位姿和運(yùn)動軌跡。

圖1 本文算法流程Fig.1 Algorithm flow chart of this paper

2 IMU 與RGB-D 融合算法設(shè)計

2.1 動態(tài)特征點(diǎn)濾除

動態(tài)特征點(diǎn)濾除主要是利用圖像幀間的幾何約束方法來濾除圖像中的動態(tài)特征點(diǎn)，為后面的RANSAC 算法剔除誤匹配提供更好的數(shù)據(jù)樣本。從而減少RANSAC 算法的迭代次數(shù)，提高系統(tǒng)運(yùn)行效率，同時使其在動態(tài)場景中有更高的定位精度。

引入幾何約束圖像模型，如圖2所示。F1和F2為相鄰兩幀圖像，p1、p2、p3為F1圖像幀的3 個特征點(diǎn)，q1、q2、q3是其在F2圖像幀上對應(yīng)的投影點(diǎn)，3 個特征點(diǎn)在F1上構(gòu)成三角形Δ p1p2p3，對應(yīng)的投影點(diǎn)構(gòu)成三角形Δq1q2q3，其圖像中特征點(diǎn)間的距離用三角形三邊d 表示。

圖2 幾何約束模型Fig.2 Geometric constraint model

pi和pj的距離表示為d（pi，pj），qi和qj的距離表示為d（pi，pj），則三角形對應(yīng)邊的差即為

定義幾何約束函數(shù)：

A（i，j）表示兩對應(yīng)邊的平均距離，即：

假設(shè)q1為運(yùn)動目標(biāo)上的點(diǎn)，由q1運(yùn)動到q1′的位置，則構(gòu)成新的三角形Δq1′q2q3，可知計算出的幾何約束函數(shù)q（i，j）會很大，但是由于幾何約束函數(shù)涉及的是一對特征點(diǎn)，仍然無法確定qi點(diǎn)是動態(tài)點(diǎn)，還是qj點(diǎn)是動態(tài)點(diǎn)，因此采用計分的方法來確定最終的特征點(diǎn)，即若q（i，j）大于閾值α，則將特征點(diǎn)qi和qj的得分分別加1，這樣不斷地將不同的特征點(diǎn)構(gòu)成三角形進(jìn)行計算，最終動態(tài)特征點(diǎn)會比靜止特征點(diǎn)的得分大很多，若某特征點(diǎn)得分大于閾值β，則認(rèn)為該特征點(diǎn)為動態(tài)特征點(diǎn)。

2.2 RANSAC 算法

RANSAC（Random Sample Consensus，隨機(jī)抽樣一致）算法用來估算表示幀間圖像的位姿變換關(guān)系的基本矩陣H，得到一個最優(yōu)的基本矩陣H 模型，然后根據(jù)該模型剔除掉誤匹配的點(diǎn)，使得特征匹配更加的準(zhǔn)確，從而提高系統(tǒng)的定位精度。

RANSAC 算法基本流程如圖3所示。

圖3 RANSAC 算法流程Fig.3 Flow chart of RANSAC algorithm

（1）設(shè)樣本集為S，其有N 個數(shù)據(jù)點(diǎn)，內(nèi)點(diǎn)的比例為w，從樣本集S 中隨機(jī)選取4 個數(shù)據(jù)點(diǎn)，根據(jù)該數(shù)據(jù)點(diǎn)計算出單應(yīng)矩陣H。

（2）將數(shù)據(jù)集中所有的數(shù)據(jù)按單應(yīng)矩陣H 進(jìn)行投影，得到所有數(shù)據(jù)點(diǎn)的投影誤差，將誤差小于閾值d 的數(shù)據(jù)點(diǎn)添加到內(nèi)點(diǎn)集Si中。

（3）如果內(nèi)點(diǎn)集Si超過了某個閾值m，就用Si重新估計單應(yīng)矩陣H 并結(jié)束，否則重新選取新的4 個數(shù)據(jù)點(diǎn)，計算單應(yīng)矩陣H，然后重復(fù)步驟2。如果迭代的次數(shù)大于T，選取最大的內(nèi)點(diǎn)集Si，用它來估計單應(yīng)矩陣H，得到最后的估計模型，然后退出。

由上述的計算流程可以看出，RANSAC 算法的運(yùn)行效率取決于迭代次數(shù)，進(jìn)一步取決于樣本集的內(nèi)點(diǎn)比例，因此在進(jìn)行RANSAC 之前，盡量提高樣本集的內(nèi)點(diǎn)比例，這樣可以提高RANSAC 的運(yùn)行效率。在之前進(jìn)行的動態(tài)特征點(diǎn)濾除，有效地濾除了樣本集中的部分外點(diǎn)，在很大程度上降低了RANSAC 的迭代次數(shù)，提高了RANSAC 的運(yùn)行效率。

2.3 IMU 預(yù)積分

IMU 傳感器是測量移動機(jī)器人自身的姿態(tài)角和加速度的裝置，由三軸陀螺儀和三軸加速度計組成。由于IMU 和相機(jī)的采樣頻率相差較大，在相同的時間內(nèi)IMU 采集的數(shù)據(jù)比相機(jī)更多，本文使用的IMU 采樣頻率為200 Hz，相機(jī)的采樣頻率為20 Hz。所以需要采用預(yù)積分方法來解決IMU 與相機(jī)頻率不同的問題。

在兩幀圖像之間有多幀IMU 數(shù)據(jù)，假設(shè)IMU的第i 時刻數(shù)據(jù)與圖像的第k 幀對齊，IMU 的第j時刻數(shù)據(jù)與圖像的第k+1 幀對齊，然后連續(xù)兩圖像之間的IMU 數(shù)據(jù)進(jìn)行預(yù)積分處理，如圖4所示。

圖4 IMU 預(yù)積分示意圖Fig.4 Schematic diagram of IMU pre-integration

在某一離散時刻t 采集數(shù)據(jù)，可得到t=i 和t=j兩幀之間的位姿與速度關(guān)系：

2.4 非線性優(yōu)化估計位姿

在系統(tǒng)完成初始化之后，本文采用滑動窗口的方法來控制優(yōu)化的規(guī)模，用緊耦合的方式對相機(jī)的位姿、速度以及IMU 的偏置進(jìn)行估計優(yōu)化?；瑒哟翱谑窃黾有碌淖兞窟M(jìn)入到最小二乘系統(tǒng)優(yōu)化，維持窗口關(guān)鍵幀的數(shù)量，數(shù)量達(dá)到一定個數(shù)就移除舊的關(guān)鍵幀。然后不斷地重復(fù)這一過程。在傳統(tǒng)的視覺SLAM 中，是通過最小化圖像幀中特征點(diǎn)的重投影誤差來求得相機(jī)的位姿以及三維路標(biāo)點(diǎn)。但增加IMU 傳感器之后，增加了要估計的系統(tǒng)狀態(tài)變量，需要同時考慮重投影誤差和IMU 誤差。系統(tǒng)的狀態(tài)向量表示如下：

根據(jù)相機(jī)的重投影誤差和IMU 誤差構(gòu)建目標(biāo)函數(shù)，通過高斯牛頓法求得目標(biāo)函數(shù)最小時的位姿信息，如圖5所示。

圖5 非線性優(yōu)化流程Fig.5 Flow chart of nonlinear optimization

目標(biāo)函數(shù)為

式中：V 表示滑動窗口中被觀測到兩次以上的特征點(diǎn)集；I 表示滑動窗口中IMU 預(yù)積分增量方程的集合；Ecam（k，j）為地圖點(diǎn)k 在第j 幀圖像時的重投影誤差，其誤差方程表示為

式中：xk為像素平面坐標(biāo)；π（·）表示三維空間點(diǎn)投影到像素平面的投影變換；WXk為世界坐標(biāo)系下的三維坐標(biāo)點(diǎn)；Eimu（i，i+1）為IMU 預(yù)積分中連續(xù)兩幀i和i+1 之間的增量方程誤差，其誤差方程表示為

3 實驗結(jié)果與分析

本文使用EuRoC 和TUM 數(shù)據(jù)集對本算法進(jìn)行了測試，將其測試的結(jié)果與ORB-SLAM2 算法進(jìn)行了對比和分析。EuRoC 數(shù)據(jù)集是用微型飛行器在1個工業(yè)廠房和2 個不同的房間內(nèi)采集，分為easy、medium 和difficult 3 個不同的等級。TUM 數(shù)據(jù)集中包含大量動態(tài)場景的序列，采集了辦公室中同事聊天互動、手勢和起身等場景。本數(shù)據(jù)的實驗平臺系統(tǒng)為Ubuntu18.04，CPU 為Intel i5-7200U 處理器，主頻為2.7 GHz，內(nèi)存為8 GB。

本算法的動態(tài)特征濾除效果如圖6所示，圖像為TUM 數(shù)據(jù)集freiburg3_walking 序列中的一幀，動態(tài)目標(biāo)正走向座椅。由圖6可以看出本算法對動態(tài)特征點(diǎn)濾除有很好的效果，將人身上的大部分動態(tài)點(diǎn)濾除掉了。

圖6 動態(tài)特征點(diǎn)濾除對比圖Fig.6 Comparison diagram of dynamic feature points filtering

本文在評估定位精度時，采用算法跟蹤過程中的位姿估計值與數(shù)據(jù)集提供的真實軌跡的均方根誤差（RMSE）來對算法的定位精度來進(jìn)行評估。分別比較本算法和ORB-SLAM2 算法中的雙目模式在各種環(huán)境下的均方根誤差，所有結(jié)果都為3 次實驗結(jié)果的均值。

如表1所示，本算法和ORB_SLAM2 在EuRoC數(shù)據(jù)集和TUM 數(shù)據(jù)集實驗軌跡精度比較，可以看出本文提出的慣導(dǎo)與雙目視覺融合算法在各個場景中比雙目視覺模式的ORB_SLAM2 算法的定位精度更高。

表1 在EuRoC、TUM 數(shù)據(jù)集上實驗的均方根誤差對比Tab.1 Comparison of RMS errors of experiments on EuRoC and TUM datasets

圖7為本算法和ORB_SLAM2 算法在MH_05_difficult 序列的絕對軌跡誤差，從MH_05_difficult 序列可以看出，當(dāng)相機(jī)快速運(yùn)動導(dǎo)致圖像模糊和弱紋理時，ORB_SLAM2 的定位精度有所下降，這是由于在這些序列中ORB_SLAM2 所提取的特征點(diǎn)少，且比配質(zhì)量差導(dǎo)致的。本算法在ORB_SLAM2 的基礎(chǔ)上增加了IMU 信息，在各場景中不僅僅只用視覺信息進(jìn)行定位，在提取特征點(diǎn)較少時，可以結(jié)合IMU的運(yùn)動信息進(jìn)行計算，這樣在弱紋理，快速運(yùn)動導(dǎo)致圖像模糊等場景時，本算法同樣有很好的定位精度。同時本算法還增加了動態(tài)特征點(diǎn)濾除，可以較好的濾除動態(tài)特征點(diǎn)，提高特征點(diǎn)的匹配質(zhì)量，freiburg3_sitting（低動態(tài)）和freiburg3_walking（高動態(tài)）是含有動態(tài)物體的序列，從上表可以看出在freiburg3_walking 序列中，ORB_SLAM2 算法的定位精度急劇下降，大量在動態(tài)物體上提取出來的特征點(diǎn)沒有被有效的剔除，而是參與了匹配和進(jìn)一步的計算，從而對系統(tǒng)的定位精度產(chǎn)生影響，而本算法的動態(tài)特征點(diǎn)濾除模塊能有效剔除動態(tài)特征點(diǎn)，降低動態(tài)特征點(diǎn)對系統(tǒng)的影響，所以在含有動態(tài)目標(biāo)時本算法的定位精度比ORB_SLAM2 更高。綜上本算法比ORB_SLAM2 算法對場景的適應(yīng)性更好。

4 結(jié)語

根據(jù)純視覺的SLAM 系統(tǒng)對環(huán)境的適應(yīng)性不強(qiáng)，本文在ORB-SLAM2 的基礎(chǔ)上進(jìn)行了改進(jìn)，增加了IMU 傳感器的數(shù)據(jù)，采用了基于非線性優(yōu)化的緊耦合方式，這種融合方式系統(tǒng)的運(yùn)行效率會更高。同時在特征匹配時增加了動態(tài)特征點(diǎn)濾除算法，先將動態(tài)特征點(diǎn)濾除后，再進(jìn)行RANSAC 算法誤匹配剔除，使得特征匹配的更加準(zhǔn)確，提高了系統(tǒng)的定位精度，同時使系統(tǒng)能夠更好的適應(yīng)含有動態(tài)目標(biāo)的場景。EuRoC 數(shù)據(jù)集和TUM 數(shù)據(jù)集實驗表明，本文改進(jìn)的算法在弱紋理、快速運(yùn)動、圖像模糊和含有動態(tài)目標(biāo)影響的場景中定位精度上比ORBSLAM2 表現(xiàn)更好，在快速運(yùn)動導(dǎo)致圖像模糊時也有很高的定位精度。因為融合了IMU 傳感器的數(shù)據(jù)，使其算法更為復(fù)雜，整個系統(tǒng)的運(yùn)行效率上略有降低。由于系統(tǒng)在特征點(diǎn)提取上消耗的時間較多，后續(xù)可以采用FPGA 對圖像的特征點(diǎn)提取進(jìn)行硬件加速，這樣可以提升系統(tǒng)的運(yùn)行速度，同時采用FPGA可以大幅降低功耗。