李磊磊，鐘 傲，梁 琳，呂春明，左 濤，田曉春

（1.北京理工大學 自動化學院，北京 100081；2.北京自動化控制設備研究所，北京 100074）

由于傳統(tǒng)的基于全球定位系統(tǒng)（Global Positioning System,GPS）的定位方法在拒止環(huán)境下無法正常工作，自主導航定位技術近年來成為了研究熱點。視覺慣性定位方法作為自主導航的關鍵技術，具有傳感器成本低、體積小等優(yōu)點。同時，視覺傳感器與慣性測量單元（Inertial Measurement Unit,IMU）特性互補，當視覺傳感器在快速移動時失效，IMU 數(shù)據(jù)可以為系統(tǒng)提供短時間內(nèi)的準確定位；而視覺傳感器在系統(tǒng)低速運動時精度較高，可以有效減少IMU 長時間的累積誤差。

得益于傳感器的上述優(yōu)勢，視覺慣性同時定位與地圖構建（Simultaneous Localization and Mapping,SLAM）近年來迅速發(fā)展，成為目前主流的視覺慣性融合定位方法。目前具有代表性的算法有VINS-Mono[1]、MSCKF-VIO[2]、VI-DSO[3]、StructVIO[4]、ORB-SLAM3[5]等。

視覺慣性導航系統(tǒng)對輸入圖像的質(zhì)量要求較高，而低照度圖像缺乏清晰的紋理特征，即使是上述這些最先進的方法，對照明變化的魯棒性仍然很差，實驗表明在低照度場景下容易出現(xiàn)位姿漂移甚至跟蹤失敗等問題[6]。在導航系統(tǒng)實際應用領域存在許多低照度場景，如全天候導航、井下機器人、夜間單兵導航等。而目前主流的視覺慣性定位方法由于無法提取到足夠多的特征點，難以在低照度場景下實現(xiàn)準確定位。對于低照度場景圖像的處理方法主要分為傳感器處理和算法處理兩個方向。文獻[6]提出了一種基于單目紅外相機的視覺里程計，使用紅外相機解決低照度場景下圖像紋理弱的問題，但傳感器成本顯著提高。而且與算法處理方式相比，紅外圖像紋理較弱,信噪比低，常用的特征點性能下降明顯。文獻[7]提出的LMVI-SLAM 算法在特征追蹤前，采用自適應伽馬校正和對比度限制自適應直方圖均衡化相結合的圖像增強方法對普通相機低照度圖像進行預處理，解決了傳感器成本高的問題，但該方法在黑暗場景效果較差；文獻[8]使用生成對抗網(wǎng)絡（Generative Adversarial Nets,GAN）對低照度圖像進行增強，再利用ORB-SLAM2進行定位，有效提升了黑暗場景下的定位精度，但對實驗平臺計算資源要求較高。文獻[7][8]為純視覺SLAM 方法，在相機移動過快時容易跟蹤失敗，系統(tǒng)魯棒性較差。

為解決上述問題，本文提出一種基于相機響應模型圖像增強的視覺慣性定位方法。在保證低成本、計算資源需求小的前提下，融合慣性傳感器，提升低照度場景下系統(tǒng)定位精度與魯棒性。首先對低照度圖像增強算法進行研究，設計了增強結合去噪的圖像預處理模塊。然后采用基于光流法的視覺慣性里程計實現(xiàn)低照度場景定位。最后搭建了一個視覺慣性實驗平臺，采集了多個低照度場景下的SLAM 數(shù)據(jù)集，并通過實驗驗證了本文定位方法的有效性。

1 系統(tǒng)框架

本文提出的低照度場景下視覺慣性定位算法流程如圖1 所示。視覺傳感器采集到的低照度圖像首先進入圖像增強模塊進行圖像預處理，對圖像整體進行提亮同時展現(xiàn)出更多細節(jié)，接著進行圖像去噪，便于后續(xù)的特征追蹤。但增強后的低照度圖像質(zhì)量也難以達到正常圖像水平，基于特征點匹配的特征跟蹤方法難以實現(xiàn)[9]，本文采用光流法進行特征追蹤，得到匹配點對。對視覺與慣性信息進行聯(lián)合標定校準，后端采用滑動窗口優(yōu)化方法修正關鍵幀位姿，以控制優(yōu)化變量維度，實現(xiàn)位姿實時更新。

圖1 算法流程圖Fig.1 Algorithm flowchart

2 圖像增強模塊

針對低照度圖像在增強后產(chǎn)生大量噪聲的問題，本文在Ying 等[10]提出的基于相機響應模型的低照度圖像增強算法基礎上做出改進，設計了高斯濾波器對增強后的圖像進行去噪處理，從而形成一個完整的圖像增強算法。下面對算法理論進行分析并對實際低照度圖像進行實驗。

2.1 相機響應模型

由于低照度圖像存在局部曝光不足的情況，為避免區(qū)域過曝，不能采用全局統(tǒng)一曝光率來曝光所有像素。將圖像增強過程建模為：

其中，P、P′分別為輸入低照度圖像和增強后輸出圖像；G表示相機響應模型；K表示曝光率矩陣，包含了每個像素的期望曝光率，亮區(qū)域對應較小曝光率，暗區(qū)域對應較大曝光率。

通過對同一場景不同曝光圖像直方圖的觀察，將相機響應模型確定為：

其中，p、k分別為某一像素值和該像素值對應的曝光率；g表示對每一個像素的增強函數(shù)；β表示線性放大系數(shù)；γ表示伽馬校正系數(shù)；c表示該像素對應的輻照度；a、b為相機響應模型的參數(shù)。為保證算法在不同相機上的適用性，選取可以適合大多數(shù)相機的固定相機參數(shù)(a=-0.3293,b=1.1258)[10]。相機響應函數(shù)f如式(3)所示，描述了圖像輻照度到像素值的轉換。

已知像素值，由式(3)可求得對應的輻照度c。

2.2 曝光率矩陣

曝光率圖與照明圖T成反比，首先根據(jù)文獻[11]算法求解照明圖T，將待優(yōu)化照明估計與初始估計值的差異和平滑度作為約束，建立目標優(yōu)化函數(shù)為：

其中，x表示圖像中的某一像素；I(x)表示像素x初始照明估計值；T(x)為待優(yōu)化的照明值；α為平衡兩殘差項的系數(shù)；h、v分別表示水平與豎直方向；Wd(x)表示像素x的權重，如式(5)所示；?dT(x)、?d I(x)分別表示待優(yōu)化照明值和初始照明值在d方向的一階導數(shù)；ε表示無窮小量。優(yōu)化目標函數(shù)，得到照明圖T的最優(yōu)解，對T中所有元素取倒數(shù)即得到曝光率矩陣K。

2.3 圖像去噪

低照度圖像經(jīng)算法增強后會產(chǎn)生大量高斯噪聲，圖像中的高斯噪聲與周圍像素間存在灰度值的突變，影響角點的提取，不利于后續(xù)對圖像進行特征跟蹤。為減少圖像噪聲對特征跟蹤的影響，提高系統(tǒng)定位精度，本文在上述算法基礎上加入圖像去噪模塊，進一步提升圖像質(zhì)量。

考慮整個定位系統(tǒng)的實時性能，需保證圖像去噪算法的低復雜度以及高效率，并且與上述圖像增強算法相匹配。由于上述圖像增強過程中產(chǎn)生的噪聲主要為高斯噪聲，本文選擇高斯濾波器對圖像進行去噪。

高斯濾波器取卷積核內(nèi)像素的加權均值作為中心點灰度值輸出，從而達到減小噪聲、平滑圖像的效果，高斯濾波器方程為：

其中，(x,y)表示卷積核內(nèi)某一像素點坐標；G(x,y)表示該像素點的權重；σ表示高斯分布的標準差；e為自然常數(shù)。本文設置卷積核大小為3 × 3，σ設置為1.5。

2.4 增強效果

為評估本文圖像增強方法的性能，采集了夜間室外低照度圖像數(shù)據(jù)集，并使用本文方法對原始圖像進行增強，將原始圖像與增強后的圖像分別進行光流追蹤，效果如圖2 所示。

圖2 光流追蹤效果對比圖Fig.2 Optical flow tracing effect comparison chart

圖2 中綠點表示光流成功追蹤的特征點，可見使用本文方法增強后的圖像有更多的特征點被成功提取和追蹤，在低照度場景下對光流追蹤效果提升明顯，尤其是在圖像中較暗的區(qū)域。

3 視覺慣性里程計

3.1 光流追蹤

由于低照度場景下的圖像特征及紋理不明顯，不利于特征點提取與描述子匹配，特征點法容易失效。本文采用光流法進行特征追蹤，在保證精度的同時，減少描述子匹配的耗時。

低照度場景下的視覺信息滿足亮度恒定、時間連續(xù)、空間一致假設，本文采用LK（Lucas-Kanade）光流算法進行求解。光流追蹤過程如圖3 所示，設圖像中某一點的像素坐標為(x,y)，該點在t時刻的灰度值為V(x,y,t)。由于相機運動，經(jīng)過時間Δt后，該點運動到了新位置(x+Δx,y+Δy)，此時灰度值變?yōu)閂(x+Δx,y+Δy,t+Δt)。根據(jù)亮度恒定假設，該點的灰度值保持不變，即：

圖3 光流追蹤示意圖Fig.3 Schematic diagram of optical flow tracing

對上式左端進行泰勒展開，得：

β為二階無窮小項，可忽略不計。將式(8)化簡，得到：

式(9)等式兩邊同除以dt，得到光流約束方程：

取包含該像素點的一個鄰域Ω，設鄰域Ω 中有n個像素點，每個像素點具有相同的速度矢量(u,v)，則得到如式(11)所示的超定方程組。

利用最小二乘法求出方程組(11)的最優(yōu)解(u*,v*)，從而得到經(jīng)過時間Δt后像素點的位置，實現(xiàn)低照度圖像的特征追蹤。為確保在相機快速運動時滿足要求，使用高斯金字塔[12]結合LK 光流提高準確性。

3.2 視覺/IMU 聯(lián)合標定

為保證系統(tǒng)的定位精度，將聯(lián)合視覺與IMU 信息，對陀螺儀偏置（bias）、速度（v）、重力向量（g）以及尺度（s）進行標定，標定過程如圖4 所示。

圖4 標定過程Fig.4 Calibration process

首先利用對極幾何模型求解兩幀間的旋轉與平移，用三角測量求得兩幀中觀測特征點的三維坐標，再根據(jù)這些三維特征點與其他幀觀測的二維特征點，使用PnP（Perspective-n-Point）求解滑動窗口中其他幀的位姿（平移、旋轉）以及三維特征點。

將視覺與IMU 信息融合，將IMU 預積分得到的旋轉信息作為估計值，將上述視覺初始化過程得到的旋轉信息作為觀測值，忽略測量噪聲，兩者之間的誤差只與陀螺儀偏置有關。將滑動窗口內(nèi)所有幀的旋轉信息與IMU 旋轉信息做差構建最小化誤差模型。利用最小二乘法求解得到陀螺儀偏置。同理，利用視覺與IMU 的平移、速度信息作為約束，完成對系統(tǒng)速度、重力加速度以及尺度的標定[13]。

3.3 位姿修正

為減少估計誤差，進一步提高系統(tǒng)定位精度，采用基于滑動窗口的優(yōu)化方法，對位姿進行修正。維持滑動窗口內(nèi)圖像幀的數(shù)量不變，每當新幀加入窗口內(nèi)，舊幀就會被移出，以此控制計算規(guī)模，使位姿估計可以實時更新?；皟?yōu)化過程如圖5 所示。

圖5 滑窗優(yōu)化示意圖Fig.5 Illustration of sliding window optimization

基于滑動窗口構建視覺慣性緊耦合優(yōu)化模型。將光流追蹤結果作為特征點觀測值，與三維特征點的投影位置作差，建立滑動窗口內(nèi)特征點的視覺重投影誤差；結合IMU 預積分誤差、先驗誤差作為約束，優(yōu)化目標函數(shù)，使總體誤差達到最小，從而求得最優(yōu)位姿估計。定義目標優(yōu)化函數(shù)為：

其中，χ為優(yōu)化變量，其中包含滑動窗口內(nèi)每一幀的位姿、速度、IMU 偏置、IMU 到相機的外參以及三維特征點的逆深度。

IMU 預積分約束中[14]，A表示當前滑動窗口內(nèi)所有IMU 測量的集合表示兩個連續(xù)幀bl和bl+1之間IMU 的測量值，eA表示IMU 預積分測量誤差，為IMU 預積分噪聲的協(xié)方差矩陣。

視覺重投影約束中,B表示當前滑動窗口內(nèi)至少被觀測到兩次的特征點集合表示第i個特征點在第j幀中的觀測值，eB表示視覺重投影誤差為視覺觀測噪聲的協(xié)方差矩陣，h(x)為Huber 范數(shù)。h(x)的定義如式(13)所示。

先驗約束中，ep、Hp來自移出滑動窗口的舊幀[15]。移出滑動窗口后，舊幀的位姿不再參與優(yōu)化，但舊幀與滑動窗口內(nèi)其他幀之間的約束關系作為先驗信息加入優(yōu)化函數(shù)中，在控制優(yōu)化變量維度的同時，不會減少約束信息，有利于求解精度的提高。

4 實驗驗證

為了驗證本文方法的有效性，搭建了一個車載實驗平臺，在夜晚室外停車場環(huán)境進行實驗，并與其他先進方法進行比較，驗證本文方法在低照度環(huán)境下的效果。

4.1 實驗平臺

實驗平臺如圖6 所示，分為軟件和硬件兩部分。軟件部分基于ROS 系統(tǒng)開發(fā)，使用C++實現(xiàn)本文提出的低照度視覺慣性定位算法。硬件部分為自主搭建的車載實驗平臺，相機采用 PointGrey FL3-U3-32S2C-CS，采用8 mm 焦距鏡頭，IMU 以及GPS 設備型號為MTi-G-710-GNSS/INS，實驗前已完成相機、IMU 參數(shù)標定以及聯(lián)合標定。數(shù)據(jù)采集與算法處理在Intel NUC（Intel Core i7-5850EQ，16GB RAM）上進行。

圖6 實驗裝置Fig.6 Experimental setup

4.2 夜間車載實驗

實驗開展地點為北京理工大學中關村校區(qū)，車載平臺夜間圍繞樓宇與停車場采集低照度數(shù)據(jù)集，包括低照度圖像與IMU 數(shù)據(jù)，并使用GPS 數(shù)據(jù)作為軌跡真值，實驗環(huán)境如圖7 所示。將本文方法（圖中用改進圖像增強表示）的定位結果分別與無圖像增強的方法和改進前的方法（圖中用原始圖像增強表示）相比，驗證本文方法對提升低照度場景下定位精度的有效性。

圖7 低照度實驗場景Fig.7 Low-light experimental scene

車載實驗平臺圍繞北京理工大學北區(qū)停車場以及6 號樓行駛，實驗一全長約365 m，非閉環(huán)；實驗二全長約445 m，閉環(huán)；實驗三全長約445 m，閉環(huán)。實驗結果如圖8-13 所示。

圖8 實驗一結果Fig.8 Results of experiment one

圖9 實驗一誤差圖Fig.9 Experiment one error plot

分別對沒有圖像增強的視覺慣性系統(tǒng)、使用原始圖像增強的系統(tǒng)以及使用本文改進后圖像增強的系統(tǒng)進行定位實驗，驗證本文所提出的方法以及改進的有效性。根據(jù)圖8、圖10 和圖12 可知，本文改進圖像增強算法后的定位結果在整體上最接近軌跡真值，其次是使用原始圖像增強的系統(tǒng)，最后是未使用圖像增強的系統(tǒng)，實驗結果表明本文方法以及改進是有效的。選用絕對軌跡誤差（Absolute Trajectory Error，ATE）作為定位誤差，即同一時刻真值位置與估計值位置的差，三次實驗得到的定位誤差結果如表1-3 所示。采用均方根誤差（Root Mean Square Error，RMSE）來評估算法的定位精度，結果如表4 所示。

表1 實驗一誤差對比（單位：m）Tab.1 Comparison of errors in experiment one (Unit: m)

表2 實驗二誤差對比（單位：m）Tab.2 Comparison of errors in experiment two (Unit: m)

表3 實驗三誤差對比（單位：m）Tab.3 Comparison of errors in experiment three (Unit: m)

表4 定位精度（單位：m）Tab.4 Positioning accuracy (Unit: m)

圖10 實驗二結果Fig.10 Results of experiment two

圖11 實驗二誤差圖Fig.11 Experiment two error plot

圖12 實驗三結果Fig.12 Results of experiment three

圖13 實驗三誤差圖Fig.13 Experiment three error plot

從表4 可以看出，與沒有圖像增強的視覺慣性系統(tǒng)和使用原始圖像增強的系統(tǒng)相比，本文改進圖像增強后的系統(tǒng)在低照度場景下的三次實驗中表現(xiàn)最好，平均定位精度相比前兩種方法分別提高了25.59%和6.38%。

5 結論

低照度場景下由于圖像特征提取困難，視覺慣性導航系統(tǒng)容易出現(xiàn)累積誤差、位姿漂移甚至跟蹤失敗等問題，難以提供準確的定位。為了解決上述問題，本文提出一種結合圖像增強技術的視覺慣性定位算法?；谙鄼C響應模型，對低照度圖像進行預處理，提高圖像亮度和對比度，并利用高斯濾波器對圖像進行去噪。同時采用光流法跟蹤特征，構建視覺與慣性緊耦合優(yōu)化模型實現(xiàn)位姿估計。車載實驗表明，本文方法能在低照度環(huán)境下實現(xiàn)穩(wěn)定準確的定位，定位精度與沒有圖像增強的方法相比提升明顯。引入去噪模塊后的系統(tǒng)定位精度得到進一步提升。

在極暗的環(huán)境下，圖像增強算法會引入更多的噪聲，對特征點的提取與跟蹤帶來更大的挑戰(zhàn)。下一步研究將使用微光傳感器以獲得更好的圖像質(zhì)量，使系統(tǒng)能在更加黑暗的環(huán)境下實現(xiàn)準確的定位。