高 揚，王興奔，徐永貴，張傳璽

(長安大學(xué)汽車學(xué)院，西安 710054)

0 引言

同步定位與地圖構(gòu)建(simultaneous localization and mapping，SLAM)技術(shù)在移動機(jī)器人，自動駕駛，AR/VR等領(lǐng)域有著廣泛應(yīng)用[1-3]。單目相機(jī)無法直接獲得深度信息，估計出的尺度具有不確定性，而慣性測量單元(inertial measurement unit，IMU)可以部分提供真實尺度信息，且不需要依賴外部信息即可計算自身相對運動，與單目視覺SLAM具有很強(qiáng)的互補(bǔ)性，因此VI-SLAM技術(shù)被廣泛研究，各種算法層出不窮。

MOURIKIS等[4]提出了一種基于濾波的VI-SLAM算法，在擴(kuò)展卡爾曼濾波框架下融合視覺和IMU信息，使用IMU信息作為預(yù)測步驟，視覺信息作為更新步驟。BLOESCH等[5]提出了基于直接擴(kuò)展卡爾曼濾波的方法，使用路標(biāo)點的光度誤差作為觀測對濾波器進(jìn)行更新。LEUTENEGGER等[6]提出了一種基于關(guān)鍵幀的非線性優(yōu)化VI-SLAM算法，將視覺重投影誤差和IMU誤差項進(jìn)行聯(lián)合優(yōu)化，求解出運動位姿。MUR-ARTAL等[7]提出了一個完整的VI-SLAM，包含回環(huán)檢測、地圖重用等功能，對IMU進(jìn)行預(yù)積分，使IMU的積分結(jié)果不依賴于上一幀相機(jī)位姿。QIN等[8]提出了一種基于非線性優(yōu)化的緊耦合VI-SLAM算法，前端采用光流跟蹤，同時對特征點逆深度，慣性測量單元狀態(tài)量及偏置進(jìn)行估計，求解出運動位姿。

然而，低光照條件下的魯棒視覺SLAM研究目前仍進(jìn)展緩慢。HUANG等[9]以O(shè)RB-SLAM2[10]為框架，在跟蹤線程之前添加了一個圖像預(yù)處理步驟，以減少低光照環(huán)境的影響，同時提取ORB特征點和Brisk特征點，提高了系統(tǒng)魯棒性。WANG等[11]在RGB-D SLAM算法的基礎(chǔ)上，增加了一種改進(jìn)的Gamma自適應(yīng)校正算法，根據(jù)圖像的平均灰度值，對Gamma值進(jìn)行調(diào)整，實現(xiàn)自適應(yīng)校正。FANG等[12]提出了一種融合直方圖均衡化和ORB特征提取的算法，該算法在低對比度圖像的場景下具有較強(qiáng)的魯棒性。HAO等[13]采用Gamma校正和直方圖均衡化對圖像進(jìn)行增強(qiáng)，并建立了視覺和IMU信息緊耦合的非線性優(yōu)化模型，以獲得更精確、魯棒的位姿估計。

雖然上述低光照條件下的視覺SLAM算法對圖像進(jìn)行了預(yù)處理，但這些預(yù)處理較為簡單，在過暗環(huán)境下的效果不佳，同時上述算法沒有考慮環(huán)境光照變化的影響。因此，本文提出了一種改進(jìn)的VI-SLAM算法，在VINS-Mono框架基礎(chǔ)上，新增圖像增強(qiáng)模塊，對低光照圖像啟用圖像增強(qiáng)算法。同時，提出一種自適應(yīng)閾值FAST角點提取算法，并采用改進(jìn)的RANSAC算法對誤匹配點進(jìn)行剔除，進(jìn)而增強(qiáng)系統(tǒng)前端的魯棒性。

1 圖像增強(qiáng)算法

VI-SLAM在弱光尤其是暗光條件下，很難提取到足夠數(shù)量的角點進(jìn)行視覺跟蹤，從而導(dǎo)致軌跡漂移甚至跟蹤失敗。本文提出一種新的圖像增強(qiáng)算法，以提高低光照環(huán)境下角點提取數(shù)量，算法流程如圖1所示。

圖1 圖像增強(qiáng)算法

該圖像增強(qiáng)算法首先將輸入圖像從RGB顏色空間轉(zhuǎn)換到HSV顏色空間，使顏色信息與亮度信息分離，在增強(qiáng)過程中只對亮度分量V進(jìn)行處理，色調(diào)分量H和飽和度分量S則保持不變，減少計算量的同時避免了圖像失真。對亮度分量V，采用多尺度Retinex(multi-scale retinex,MSR)[14]算法進(jìn)行處理，在提高對比度的同時保留更多細(xì)節(jié)信息。為了更好地對亮度分量V進(jìn)行增強(qiáng)，本文分別采用自適應(yīng)Gamma校正和CLAHE方法對MSR處理后的亮度分量V進(jìn)行處理，并利用基于圖像平均灰度和熵值的融合策略進(jìn)行加權(quán)融合。最后將未處理的色調(diào)分量H，飽和度分量S和處理后的亮度分量V進(jìn)行融合，并轉(zhuǎn)換回RGB顏色空間。針對圖像增強(qiáng)過程中產(chǎn)生的噪聲，采用中值濾波方法去除噪聲，得到最終的增強(qiáng)圖像。經(jīng)實驗驗證，該算法可有效提高低光照圖像的亮度和對比度，從而提高后續(xù)角點提取的成功率。

1.1 圖像亮度判斷算法

為避免將正常光照下圖像過度增強(qiáng)，同時減少VI-SLAM算法計算量，本文提出一種圖像亮度判斷算法，對每一幀圖像進(jìn)行亮度判斷，將圖像區(qū)分為低亮度，正常亮度和高亮度三類，僅對低光照圖像進(jìn)行圖像增強(qiáng)預(yù)處理。

首先將輸入圖像img平均分成9個子塊，對每一塊單獨計算平均灰度值，然后按照平均灰度值大小進(jìn)行排序，子塊img[0]平均灰度值最小，子塊img[8]平均灰度值最大。按照圖2所示流程進(jìn)行亮度判斷。

圖2 圖像亮度判斷流程

圖中，imgL=(img[0]+img[1]+img[2])/3，imgM=(img[3]+img[4]+img[5])/3，imgR=(img[6]+img[7]+img[8])/3，t1、t2、t3分別表示灰度判斷閾值。

1.2 MSR圖像增強(qiáng)算法

本文采用基于Retinex的MSR算法對V分量進(jìn)行處理，以同時滿足圖像的細(xì)節(jié)增強(qiáng)和顏色保真。根據(jù)Retinex模型[15]，原始圖像I(x,y)可分解為反射分量R(x,y)和照度分量L(x,y)，如式(1)所示。

I(x,y)=R(x,y)×L(x,y)

(1)

由于反射分量R(x,y)反映了圖像真實的細(xì)節(jié)，而照度分量L(x,y)變化緩慢，因此可以對原圖像估計其照度分量L(x,y)，由式(1)求出反射分量R(x,y)，作為增強(qiáng)后的圖像。

單尺度Retinex(single-scale retinex,SSR)算法[16]以Retinex模型為基礎(chǔ)，采用高斯函數(shù)為卷積核對原始圖像進(jìn)行卷積運算，平滑后可估計得到照度分量L(x,y)，即：

L(x,y)=I(x,y)*F(x,y)

(2)

(3)

將式(1)轉(zhuǎn)換到對數(shù)域并將式(2)、式(3)代入，則有：

lnR(x,y)=ln[I(x,y)/L(x,y)]=lnI(x,y)-ln[I(x,y)*F(x,y)]

(4)

用式(4)的結(jié)果進(jìn)行指數(shù)運算，即可得到物體的反射分量R(x,y)。

MSR算法基于SSR算法進(jìn)行改進(jìn)，采用多個尺度進(jìn)行照度分量L(x,y)估計，可表示為：

(5)

式中，Wn為各個尺度的權(quán)重值，一般情況取1/3；Fn(x,y)為第n個尺度下的高斯環(huán)繞函數(shù)；N為尺度個數(shù)，通常為3個跨越大、中、小的尺度。

1.3 自適應(yīng)Gamma校正

采用Retinex理論估計照度分量，是基于照度分量L(x,y)變化緩慢的先驗假設(shè)，本身具有一定誤差。因此，該方法所求的反射分量與理想的增強(qiáng)圖像并不完全一致，需對照度分量進(jìn)行進(jìn)一步處理。本文提出一種自適應(yīng)Gamma校正方法，可以根據(jù)圖像亮度自動調(diào)整校正因子，從而實現(xiàn)不同亮度圖像的自適應(yīng)亮度校正，提高了算法在復(fù)雜光照變化下的魯棒性。

傳統(tǒng)的Gamma校正函數(shù)為：

(6)

式中，G(x,y)為矯正后的圖像；S(x,y)為輸入的圖像；γ為矯正因子。當(dāng)γ>1時，可以降低像素點的亮度值，當(dāng)γ<1時，能夠增加像素點的亮度值。

使用傳統(tǒng)Gamma校正對低光照圖像進(jìn)行增強(qiáng)時，γ值一般按經(jīng)驗選取固定值，因此難以適應(yīng)環(huán)境變化較為劇烈的復(fù)雜場景。本文γ值的選取根據(jù)輸入圖像的平均灰度值而定，如式(7)所示。

(7)

式中，m代表輸入圖像的平均灰度值。經(jīng)多次實驗，α1、β1分別取0.01，0.2，α2、β2分別取0.06，9。

由式(7)可知，當(dāng)輸入圖像為低亮度圖像時，γ取值小于1，圖像平均灰度值m越低時，γ取值越小，輸入圖像的對比度和亮度的增強(qiáng)效果越明顯。當(dāng)輸入圖像為正常亮度圖像時，γ值為1，不進(jìn)行Gamma校正。當(dāng)輸入圖像為高亮度圖像時，γ取值大于1，圖像平均灰度值m越高時，γ取值越大，從而降低輸入圖像的對比度和亮度。進(jìn)而實現(xiàn)根據(jù)圖像的平均灰度值自適應(yīng)進(jìn)行Gamma校正，相比于固定γ值，可以取得更好的圖像處理效果。

1.4 基于圖像平均灰度值和熵值的加權(quán)融合

經(jīng)MSR算法和自適應(yīng)Gamma校正處理后可以顯著提高輸入圖像的亮度，增強(qiáng)圖像低光照區(qū)域的細(xì)節(jié)信息，但同時易產(chǎn)生圖像泛白和對比度低的問題。因此本文采用CLAHE算法提高M(jìn)SR處理后圖像的對比度，并將處理結(jié)果與自適應(yīng)Gamma校正后的圖像進(jìn)行加權(quán)融合，以減少上述問題的影響。加權(quán)融合公式如式(8)所示。

g(x,y)=ug1(x,y)+vg2(x,y)

(8)

式中，g1(x,y)和g2(x,y)分別為自適應(yīng)Gamma校正和CLAHE的處理結(jié)果；g(x,y)為融合后的圖像；u、v為加權(quán)融合系數(shù)，定義為：

(9)

v=1-u

(10)

式中，m1和m2分別代表自適應(yīng)Gamma校正和CLAHE處理后圖像的平均灰度值；q1和q2分別表示自適應(yīng)Gamma校正和CLAHE處理后圖像的熵值，圖像熵值代表了圖像信息量的多少。

通過基于圖像平均灰度值和熵值的加權(quán)融合方法，將自適應(yīng)Gamma校正和CLAHE分別處理后的圖像進(jìn)行加權(quán)融合，在提高圖像的局部對比度的同時也改善了亮度不均的問題。

2 改進(jìn)的VI-SLAM算法

本文針對低光照環(huán)境下角點提取困難，誤匹配點較多的問題，對VINS-Mono算法進(jìn)行改進(jìn)。添加圖像預(yù)處理模塊，提高圖像亮度和對比度，采用FAST角點提取算法取代原來的Shi-Tomasi角點[17]提取算法，以提高角點提取速度，根據(jù)圖像亮度值調(diào)整角點提取閾值，以提高角點提取數(shù)量。采用改進(jìn)的RANSAC算法進(jìn)行誤匹配點剔除，提高角點匹配的正確性。

改進(jìn)后的VI-SLAM算法主要分為初始化，圖像預(yù)處理，角點提取與跟蹤，位姿優(yōu)化，回環(huán)檢測5部分，算法框架如圖3所示。

圖3 改進(jìn)的VI-SLAM算法框架

初始化主要提供一個良好的相機(jī)位姿，并對真實尺度，重力和陀螺儀偏置進(jìn)行估計。圖像預(yù)處理首先對輸入圖像進(jìn)行亮度判斷，然后對低亮度圖像進(jìn)行圖像增強(qiáng)處理。角點提取與跟蹤部分采用自適應(yīng)閾值FAST角點提取算法對圖像進(jìn)行角點提取，并通過光流法跟蹤角點在下一幀的位置，獲得匹配點對，最后采用改進(jìn)的RANSAC算法對誤匹配點進(jìn)行剔除。位姿優(yōu)化將視覺信息與IMU信息進(jìn)行聯(lián)合非線性優(yōu)化，回環(huán)檢測部分負(fù)責(zé)系統(tǒng)重定位，在檢測到回環(huán)時消除累積誤差。

2.1 自適應(yīng)閾值FAST角點提取算法

本文提出一種自適應(yīng)閾值FAST角點提取算法，可根據(jù)圖像的灰度值自動確定當(dāng)前圖像FAST角點提取閾值，從而提高角點提取的數(shù)量和成功率。

FAST角點提取算法主要對像素p周圍灰度值進(jìn)行檢測，若灰度值變化明顯則認(rèn)為該像素p是一個角點，檢測流程如下：

(1)計算像素p處灰度值Ip；

(2)選取檢測圓(p為圓心，半徑為3)上的16個像素點作為檢測點，如圖4所示，其灰度值用Ipi表示，i∈[1,16]；

圖4 FAST角點

本文提出的自適應(yīng)閾值FAST角點提取算法首先計算輸入圖像的平均灰度值，從而確定FAST角點提取閾值。

(11)

圖像較暗被判定為低亮度圖像時，為防止圖像平均灰度值過小，導(dǎo)致提取的FAST角點質(zhì)量差，F(xiàn)AST角點提取閾值th取經(jīng)驗值7和m/3的最大值。

2.2 改進(jìn)的RANSAC

角點提取與跟蹤模塊采用自適應(yīng)閾值FAST角點提取算法對輸入圖像進(jìn)行角點提取，通過LK光流跟蹤算法獲得上一幀角點在當(dāng)前幀的位置。由于LK光流跟蹤算法假設(shè)前后兩幀光度不變，且運動變化不大，因此當(dāng)圖像噪聲過大時易產(chǎn)生誤匹配點。RANSAC可以對兩幀匹配點進(jìn)行隨機(jī)抽樣，從而求出包含最多正確匹配點的模型。然而該方法沒有考慮匹配點的質(zhì)量，簡單對兩幀所有匹配點迭代求解，存在迭代時間長、求解成功率低等缺點。

本文對RANSAC算法進(jìn)行改進(jìn)，首先計算所有匹配角點的BRIEF描述子，BRIEF是一種二進(jìn)制描述子，通過計算角點周圍的像素信息對該角點進(jìn)行描述，然后計算描述子之間的漢明距離，該距離與角點之間的相似程度成正比。在漢明距離小于閾值的匹配點對中進(jìn)行RANSAC算法，從而提高模型估計成功率，減少迭代時間。改進(jìn)之后的角點提取與跟蹤模塊框架如圖5所示。

圖5 改進(jìn)的角點提取與跟蹤模塊

3 實驗驗證

為驗證算法的有效性，本文設(shè)計了三組實驗。其中第一組實驗使用New Tsukuba Stereo Dataset[18]合成數(shù)據(jù)集測試圖像增強(qiáng)算法效果，第二組實驗使用EuRoC飛行數(shù)據(jù)集[19]測試VI-SLAM定位精度，第三組實驗在低光照環(huán)境中進(jìn)行了實際場景下的對比實驗。本文的實驗環(huán)境為Lenovo G410筆記本電腦，CPU酷睿i3-4000M，主頻2.40 GHz，8 GB運行內(nèi)存，以Ubuntu16.04為實驗平臺。

3.1 角點提取實驗

為驗證提出的圖像增強(qiáng)算法對提高角點提取數(shù)量的有效性，將本文提出的圖像增強(qiáng)算法分別與常用的ALM[20]，LIME[21]，Gamma校正，CLAHE，SSR，MSR算法進(jìn)行了對比實驗。實驗基于New Tsukuba Stereo數(shù)據(jù)集中的Lamps序列進(jìn)行。Lamps序列包含1800張低光照圖像，具有良好的代表性。選取Lamps序列中的序號從01111至01120的連續(xù)10幀圖像進(jìn)行實驗。首先采用不同圖像增強(qiáng)算法對上述10幀圖像進(jìn)行增強(qiáng)處理。然后分別對上述采用不同算法增強(qiáng)后的10幀圖像進(jìn)行角點提取實驗，統(tǒng)計其角點提取數(shù)目。同時計算第01112至01120幀圖像相對于前一幀的角點匹配成功率以及經(jīng)RANSAC處理后的角點匹配成功率，對上述10幀實驗圖像的實驗結(jié)果求平均值，結(jié)果如表1所示。圖6展示了對序號為01111的經(jīng)不同圖像增強(qiáng)算法處理后的圖像進(jìn)行角點提取實驗的結(jié)果。

表1 角點提取與跟蹤對比

(a) 原始圖像 (b) ALM (c) LIME (d) Gamma校正

由圖6和表1可知，原始圖片提取角點數(shù)量很少，無法滿足VI-SLAM要求。采用ALM和Gamma校正算法處理后的圖像，角點提取數(shù)量雖有所增加，仍難以滿足要求，LIME和CLAHE算法對圖像增強(qiáng)效果不明顯，提取的角點分布不均勻，SSR和MSR算法引入了過多噪聲點，導(dǎo)致RANSAC去除誤匹配點后匹配成功率低。本文提出的圖像增強(qiáng)算法可有效提高角點提取的數(shù)量，在光流跟蹤以及RANSAC去除誤匹配點后，仍擁有較高的匹配成功率，同時算法的實時性較好，處理一幀分辨率為640×480的灰度圖，用時僅為20 ms，可以滿足SLAM實時性的要求。

3.2 軌跡定位精度對比實驗

為驗證本文算法在低光照場景下的定位精度，本文使用EuRoC數(shù)據(jù)集的MH_04_difficult，MH_05_difficult，V1_03_difficult，V2_03_difficult四個運動序列數(shù)據(jù)進(jìn)行了測試。這些數(shù)據(jù)中包含有大量低光照以及快速運動的圖像，如圖7所示。實驗中使用本文算法與VINS-Mono算法進(jìn)行了對比，對比結(jié)果如表2所示。

(a) MH_04_difficult序列中的低光照圖像 (b) V1_03_difficult序列中的低光照圖像

表2 改進(jìn)前后算法誤差對比 (m)

由表2可知，本文提出的改進(jìn)算法在4個測試序列上的軌跡跟蹤誤差均小于VINS-Mono，均方根誤差平均降低了23.98%，證明了算法的有效性和魯棒性。

圖8是本文算法與VINS-Mono算法的軌跡對比圖，圖中GroundTruth為EuRoC數(shù)據(jù)集提供的真實運動軌跡，difference為兩條軌跡的差異，代表誤差大小。由圖8可知，本文提出的改進(jìn)算法的運行軌跡更接近真實軌跡，誤差更小。

(a) VINS-Mono (b) 本文算法

3.3 實際場景試驗

為驗證算法的有效性，在實際場景下進(jìn)行實驗驗證。實驗平臺為搭載小覓相機(jī)的先鋒移動機(jī)器人，如圖9所示。為方便算法對比，保證兩種算法運行軌跡相同，本文采取在真實場景錄制數(shù)據(jù)集的方式進(jìn)行實驗對比。

圖9 先鋒機(jī)器人實驗平臺

實驗環(huán)境為僅依靠路燈照明的夜晚場景，采集的夜晚真實圖像如圖10所示，使用先鋒機(jī)器人搭載小覓相機(jī)繞行筆者所在辦公樓一周后回到出發(fā)地，總長約420 m，在衛(wèi)星地圖上的軌跡示意圖如圖11所示。

圖10 夜晚真實場景圖像 圖11 運行軌跡示意圖

圖12為在關(guān)閉回環(huán)檢測模塊的條件下，改進(jìn)前后算法在同一真實數(shù)據(jù)集運行后計算得出的軌跡，可以看到，采用本文算法的運動軌跡在繞行一周后基本回到起點，而VINS-Mono算法的運動軌跡則存在較嚴(yán)重的軌跡漂移，終點與起點之間存在較大距離。可見本文算法提高了低光照條件下VI-SLAM的精度和魯棒性。

圖12 運行軌跡對比圖

4 結(jié)束語

本文提出了一種改進(jìn)的低光照條件下的VI-SLAM算法，以提高低光照場景下的定位精度。針對低光照場景下VI-SLAM角點提取與跟蹤困難的問題，提出了一種圖像增強(qiáng)算法，對輸入圖像進(jìn)行預(yù)處理，對判定為低亮度的圖像進(jìn)行圖像增強(qiáng)，從而提高輸入圖像的對比度和亮度。提出一種自適應(yīng)閾值FAST角點提取算法，保證低亮度圖像能夠提取足夠數(shù)量的角點進(jìn)行跟蹤，同時對RANSAC算法進(jìn)行改進(jìn)，只在漢明距離小于閾值的匹配點對中進(jìn)行迭代。在公開數(shù)據(jù)集和實際場景下的實驗結(jié)果表明本文算法提高了VI-SLAM算法在低光照條件下的定位精度和魯棒性。