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

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

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

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

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

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

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

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

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

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

引入幾何約束圖像模型，如圖2所示。F1和F2為相鄰兩幀圖像，p1、p2、p3為F1圖像幀的3 個(gè)特征點(diǎn)，q1、q2、q3是其在F2圖像幀上對(duì)應(yīng)的投影點(diǎn)，3 個(gè)特征點(diǎn)在F1上構(gòu)成三角形Δ p1p2p3，對(duì)應(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），則三角形對(duì)應(yīng)邊的差即為

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

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

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

2.2 RANSAC 算法

RANSAC（Random Sample Consensus，隨機(jī)抽樣一致）算法用來(lái)估算表示幀間圖像的位姿變換關(guān)系的基本矩陣H，得到一個(gè)最優(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 個(gè)數(shù)據(jù)點(diǎn)，內(nèi)點(diǎn)的比例為w，從樣本集S 中隨機(jī)選取4 個(gè)數(shù)據(jù)點(diǎn)，根據(jù)該數(shù)據(jù)點(diǎn)計(jì)算出單應(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超過(guò)了某個(gè)閾值m，就用Si重新估計(jì)單應(yīng)矩陣H 并結(jié)束，否則重新選取新的4 個(gè)數(shù)據(jù)點(diǎn)，計(jì)算單應(yīng)矩陣H，然后重復(fù)步驟2。如果迭代的次數(shù)大于T，選取最大的內(nèi)點(diǎn)集Si，用它來(lái)估計(jì)單應(yīng)矩陣H，得到最后的估計(jì)模型，然后退出。

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

2.3 IMU 預(yù)積分

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

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

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

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

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

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

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

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

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

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

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

3 實(shí)驗(yàn)結(jié)果與分析

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

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

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

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

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

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

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

4 結(jié)語(yǔ)

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