李 輝, 王金剛, 張小俊

(河北工業(yè)大學(xué)機(jī)械工程學(xué)院，天津 300130)

在地下車庫環(huán)境中進(jìn)行定位無法像在地面等室外環(huán)境一樣使用全球定位系統(tǒng)(global positioning system，GPS)等衛(wèi)星信號(hào)，只能依靠車輛自身攜帶的傳感器實(shí)現(xiàn)定位[1]。傳感器定位一般分為使用多傳感器融合定位的方法[2-3]和使用單獨(dú)傳感器的定位方法。由于目前市面上的傳感器在精度方面都有一定的誤差，無法確保定位精度，所以多傳感器融合定位成為重點(diǎn)研究方向。

即時(shí)定位與建圖(simultaneous localization and mapping,SLAM)技術(shù)最先由Smith等[4-5]提出，目前已成為解決智能汽車在地下車庫環(huán)境下導(dǎo)航與定位的有效方法之一，依據(jù)采用傳感器種類不同主要分為激光SLAM和視覺SLAM。其中，激光SLAM[6]在理論和工程應(yīng)用中發(fā)展較為成熟，但受限于高昂成本，很難在量產(chǎn)車上大規(guī)模使用。而視覺SLAM因?yàn)槠渚薮蟮牡统杀緝?yōu)勢(shì)正逐漸被業(yè)界所青睞，可預(yù)見在未來將有著廣闊的應(yīng)用前景。自21世紀(jì)以來，研究者們使用運(yùn)動(dòng)恢復(fù)結(jié)構(gòu)(structure from motion, SFM)中的方式[7]，基于優(yōu)化問題來求解SLAM問題，該方法以位姿圖的形式描述物體在不同時(shí)刻的狀態(tài)，又稱為基于圖優(yōu)化的SLAM，在視覺定位領(lǐng)域中取得了主導(dǎo)地位[8-9]。2015年，ORB-SLAM[10]算法被提出，該算法整合當(dāng)前各種主流的方式，創(chuàng)新地使用實(shí)時(shí)跟蹤特征點(diǎn)線程、局部束優(yōu)化線程以及全局位姿圖的回環(huán)檢測(cè)與優(yōu)化線程三線程完成SLAM，能夠確保地圖與軌跡的全局一致性[11]。然而該算法的建圖部分只含有稀疏的地圖點(diǎn)，只能滿足定位需求，不能用于導(dǎo)航。另外，整個(gè)系統(tǒng)都采用特征點(diǎn)進(jìn)行計(jì)算，對(duì)于每張圖都要計(jì)算ORB特征，這使得ORB-SLAM系統(tǒng)的實(shí)時(shí)性難以滿足即時(shí)定位的需求。

特征點(diǎn)法在視覺SLAM中依舊占著主流地位，但特征點(diǎn)的提取與描述子的計(jì)算非常耗時(shí)，且使用特征點(diǎn)時(shí)，只用到特征點(diǎn)信息，其他像素點(diǎn)則被忽略。針對(duì)這些缺點(diǎn)，出現(xiàn)了直接法視覺SLAM，該法基于灰度不變假設(shè)，根據(jù)像素來直接計(jì)算相機(jī)運(yùn)動(dòng)。直接法是從光流法[12]演變而來，光流法是描述像素在圖像中的運(yùn)動(dòng)，而直接法附帶一個(gè)相機(jī)運(yùn)動(dòng)模型，算法中空間點(diǎn)成像的灰度值被認(rèn)為是恒定的，直接根據(jù)像素亮度信息，估計(jì)相機(jī)的運(yùn)動(dòng)。直接法完全舍去了特征點(diǎn)提取和描述子計(jì)算，不僅避開了特征點(diǎn)法耗時(shí)的缺點(diǎn)，而且保留了所有特征信息。

Qin等[13]提出了ROVIO(robust visual inertial odometry)算法，這是一個(gè)基于EKF濾波的視覺慣導(dǎo)融合定位(visual inertial odometry，VIO)系統(tǒng)。ROVIO系統(tǒng)新穎的地方在于，基于當(dāng)前相機(jī)坐標(biāo)系的方向矢量和逆深度來參數(shù)化路標(biāo)點(diǎn)。VINS-Mono[14]是香港科技大學(xué)于2017年開源的一種基于非線性優(yōu)化的緊耦合VIO算法，采用單目和慣性測(cè)量單元(inertial measurement unit，IMU)信息恢復(fù)尺度，其中使用光流法進(jìn)行特征點(diǎn)匹配，大大縮短了前端所耗時(shí)間。ROVIO的濾波方法相比于VINS-mono的非線性優(yōu)化方法有一個(gè)明顯的缺點(diǎn)就是存儲(chǔ)量和狀態(tài)量是平方增長關(guān)系，因此不適合大型場(chǎng)景。而現(xiàn)在基于視覺的SLAM方案，路標(biāo)點(diǎn)數(shù)據(jù)很大，此時(shí)濾波的方法效率非常低。VINS-Mono是目前在VIO方面較為先進(jìn)的算法，但在特殊的弱特征紋理環(huán)境下，如地下車庫車位旁的墻面、地面或支撐立柱區(qū)域，通過對(duì)算法試驗(yàn)，無法準(zhǔn)確提取出有效的關(guān)鍵點(diǎn)，導(dǎo)致位姿估計(jì)失敗。

針對(duì)智能汽車在車庫弱紋理環(huán)境下實(shí)現(xiàn)SLAM存在的問題，提出了一種基于VINS-Mono框架的改進(jìn)視覺慣導(dǎo)融合定位算法。在前端通過提取灰度梯度變化明顯的像素點(diǎn)來代替Harris特征點(diǎn)的提取，并進(jìn)行深度估計(jì)，采用非線性優(yōu)化方法來初步估計(jì)相機(jī)位姿，通過這種改進(jìn)使得初始化精度有了提升，SLAM效果大大提高。

1 基于非線性優(yōu)化的融合算法框架

改進(jìn)的整個(gè)算法流程圖如圖1所示。在前端像素點(diǎn)提取階段改進(jìn)為采用提取像素值陡變點(diǎn)用于像素追蹤和視覺定位[15]，同時(shí)對(duì)IMU做預(yù)積分運(yùn)算，輸出兩幀圖像之間的IMU積分結(jié)果，并保證IMU和圖像數(shù)據(jù)的同步；初始化階段，使用非線性優(yōu)化建立視覺代價(jià)誤差函數(shù)估計(jì)相機(jī)運(yùn)動(dòng)，經(jīng)過深度濾波，使得視覺方面得到一個(gè)相對(duì)運(yùn)動(dòng)的情況，結(jié)合IMU與積分得到的相對(duì)運(yùn)動(dòng)情況，兩者做視覺慣導(dǎo)松耦合初始化，從而標(biāo)定出尺度、重力加速度、IMU速度和IMU偏差；后端階段，把滑動(dòng)窗口中相鄰幀間的IMU產(chǎn)生的殘差部分、視覺代價(jià)誤差函數(shù)部分以及從滑動(dòng)窗口中去掉的位姿和像素點(diǎn)的約束即邊緣化殘差部分進(jìn)行聯(lián)合BA優(yōu)化，優(yōu)化完之后得到各個(gè)時(shí)刻相機(jī)的位姿,實(shí)現(xiàn)VIO；利用Harris關(guān)鍵點(diǎn)計(jì)算描述子進(jìn)行回環(huán)檢測(cè)；在重定位后，為了確?；谥囟ㄎ唤Y(jié)果對(duì)過去的位姿進(jìn)行全局一致配置。

圖1 算法框架

1.1 算法前端

1.1.1 改進(jìn)的灰度值陡變像素點(diǎn)提取

現(xiàn)階段用于前端提取像素點(diǎn)的方法主要分為直接法中的灰度值陡變像素點(diǎn)提取法和特征角點(diǎn)提取法。

目前主要的檢測(cè)特征點(diǎn)的方式有提取SIFT關(guān)鍵點(diǎn)、FAST關(guān)鍵點(diǎn)、Harris關(guān)鍵點(diǎn)等。VINS-mono算法中在關(guān)鍵點(diǎn)檢測(cè)階段采用了Harris角點(diǎn)提取算法。它的基本原理是計(jì)算圖像中每一點(diǎn)與周圍點(diǎn)變化率的平均值，在圖像追蹤精準(zhǔn)度方面要更優(yōu)于其他角點(diǎn)提取的算法，但對(duì)于弱紋理區(qū)域依然不能提取足夠的特征點(diǎn)來匹配計(jì)算位姿，如圖2所示，在這種弱紋理區(qū)域提取的特征點(diǎn)個(gè)數(shù)約為30，遠(yuǎn)不能滿足位姿計(jì)算的需求。

圖2 特征點(diǎn)提取與匹配

直接法中提取的是梯度發(fā)生明顯變化的像素，相機(jī)有時(shí)候會(huì)拍攝到紋理特征不太明顯或缺失特征的區(qū)域圖像，如地下車庫的墻面或其中支撐柱體的白色表面，利用其邊緣或邊縫直接法依然可以較好地估計(jì)出位姿[16]，本文算法在像素提取階段改用提取灰度值陡變像素點(diǎn)的方案改進(jìn)算法中的灰度梯度閾值可以根據(jù)具體的環(huán)境有所調(diào)整，運(yùn)行起始階段設(shè)定閾值threshold大小為70，當(dāng)提取的像素點(diǎn)過少時(shí)，閾值threshold將降低10個(gè)灰度值以保證提取到更多的像素點(diǎn)；當(dāng)像素點(diǎn)提取過多時(shí)閾值將提高5個(gè)灰度值用于減少計(jì)算的時(shí)間代價(jià)，保證實(shí)時(shí)性，灰度值陡變像素點(diǎn)提取如圖3所示，提取的可用于位姿計(jì)算的像素點(diǎn)個(gè)數(shù)在200以上，可以保證環(huán)境信息的充分利用性。

圖3 灰度值陡變關(guān)鍵點(diǎn)提取

當(dāng)前相機(jī)運(yùn)動(dòng)到參考幀的距離達(dá)到一個(gè)閾值后，則創(chuàng)建當(dāng)前幀為關(guān)鍵幀[17]，距離函數(shù)為

(1)

式(1)中：W為權(quán)重矩陣；ξ為兩幀之間的變換矩陣，并且距離閾值根據(jù)當(dāng)前場(chǎng)景的平均逆深度來確定。

根據(jù)Horn-Schunck光流進(jìn)行像素的追蹤。在前一步創(chuàng)建的關(guān)鍵幀上提取一定數(shù)量Harris關(guān)鍵點(diǎn)，計(jì)算描述子用于回環(huán)檢測(cè)，由于在地下車庫自主泊車中對(duì)回環(huán)的需求并不明顯，同時(shí)為了盡可能的保證實(shí)時(shí)性，故提取該關(guān)鍵點(diǎn)數(shù)量的閾值較小。

1.1.2 IMU預(yù)積分

慣性測(cè)量單元IMU一般由3個(gè)單軸加速度計(jì)和3個(gè)單軸陀螺儀構(gòu)成，加速度計(jì)檢測(cè)物體在載體坐標(biāo)系統(tǒng)獨(dú)立三軸的加速度信號(hào)，而陀螺儀檢測(cè)載體相對(duì)于導(dǎo)航坐標(biāo)系的角速度信號(hào)，對(duì)這些信號(hào)進(jìn)行處理之后，便可解算出物體的姿態(tài)。

對(duì)于圖像幀k和k+1，體坐標(biāo)系對(duì)應(yīng)為bk和bk+1，位置、速度和方向狀態(tài)值p、v、q可以根據(jù)[tk,tk+1]時(shí)間間隔內(nèi)的IMU測(cè)量值，在世界坐標(biāo)系下進(jìn)行傳遞的。

(2)

(3)

(4)

經(jīng)過變換可得

(5)

(6)

(7)

式中：α、β、γ為bk+1對(duì)bk的相機(jī)相對(duì)運(yùn)動(dòng)量，分別對(duì)應(yīng)量綱為位移，速度和四元數(shù)。此時(shí)使用一階泰勒展開來獲取近似值。

1.2 初始化-視覺追蹤

由于在像素提取階段創(chuàng)新性地采用了提取陡變像素點(diǎn)的方案，為了保證魯棒性和可行性，在像素點(diǎn)追蹤方面摒棄了原有的運(yùn)動(dòng)恢復(fù)結(jié)構(gòu)(SFM)方法，轉(zhuǎn)而使用非線性優(yōu)化的直接法來估計(jì)出兩幀之間的位姿[18]，其中空間點(diǎn)的深度結(jié)合深度濾波器進(jìn)行估計(jì)，所構(gòu)建的誤差函數(shù)Ep可表示為

(8)

rp(P,ξji)=Ii(P)-Ij{V[P,Di(P),ξji]}

(9)

(10)

(11)

(12)

式中：Px、Py分別為匹配點(diǎn)P在世界坐標(biāo)系下的x坐標(biāo)和y坐標(biāo)；D為深度的集合；ξji為新幀與關(guān)鍵幀的變換關(guān)系矩陣；rp為圖像像素灰度值誤差；Ii、Ij分別表示第i幀和第j幀的灰度值；P為匹配點(diǎn)；V為從圖像向關(guān)鍵幀的一個(gè)映射；x′、y′、z′分別表示匹配點(diǎn)的像素坐標(biāo)；d表示對(duì)應(yīng)點(diǎn)的深度值；δ為誤差的衡量標(biāo)準(zhǔn)。

使用ξ作為優(yōu)化變量，最小化誤差的二范數(shù)，得到優(yōu)化問題，可表示為

(13)

式(13)中：N表示匹配點(diǎn)個(gè)數(shù)，使用李代數(shù)下的擾動(dòng)模型，求誤差關(guān)于相機(jī)運(yùn)動(dòng)的導(dǎo)數(shù)，最終得到一階導(dǎo)數(shù)為

(14)

式(14)中：q為空間點(diǎn)在第2個(gè)相機(jī)坐標(biāo)系下的坐標(biāo)；u為它的像素坐標(biāo)，得到雅克比矩陣為

(15)

最后使用高斯-牛頓方法計(jì)算增量，迭代求解。

1.3 后端優(yōu)化

(16)

優(yōu)化中的誤差狀態(tài)量為

(17)

結(jié)合特征點(diǎn)深度、IMU誤差、關(guān)鍵幀姿態(tài)三部分，得到優(yōu)化的殘差函數(shù)為

(18)

rC=Ii-Ij

(19)

式(19)中：Ii、Ij為相鄰兩關(guān)鍵幀中同一像素點(diǎn)的灰度值。

IMU約束部分：通過對(duì)前面IMU預(yù)積分公式的變換，得到IMU殘差為

(20)

邊緣化約束部分：將滑出窗外的幀與滑窗內(nèi)的幀的約束使用邊緣化的形式保存為先驗(yàn)誤差進(jìn)行后續(xù)非線性優(yōu)化，以保留約束信息。其中根據(jù)次新幀是否為關(guān)鍵幀，分為兩種邊緣化策略，通過對(duì)比次新幀和次次新幀的視差量，來決定邊緣化掉次新幀或者最老幀，當(dāng)次新幀為關(guān)鍵幀時(shí)，將邊緣化掉最老幀，及其看到的路標(biāo)點(diǎn)和相關(guān)聯(lián)的IMU數(shù)據(jù)，將其轉(zhuǎn)化為先驗(yàn)信息加到整體的目標(biāo)函數(shù)中；否則，邊緣化掉次新幀[20]。

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

2.1 數(shù)據(jù)集測(cè)試

改進(jìn)算法是在VINS-Mono的框架下進(jìn)行的改進(jìn)，旨在應(yīng)對(duì)地下車庫紋理特征不明顯的情境時(shí)能夠有較好的位姿估計(jì)。使用EuRoC數(shù)據(jù)集對(duì)改進(jìn)算法和VINS-Mono算法的定位精度進(jìn)行對(duì)比評(píng)估，其中，該數(shù)據(jù)集采用Skybotix VI傳感器提供一系列的圖片，以及與其對(duì)應(yīng)的IMU加速度和角速度測(cè)量值，通過硬件計(jì)算給科研人員提供了真實(shí)軌跡?，F(xiàn)分別選取其中的工廠場(chǎng)景(MH_02)、房間1場(chǎng)景(V1_01)和房間2場(chǎng)景(V2_01)3個(gè)數(shù)據(jù)包，分別來對(duì)代碼進(jìn)行實(shí)驗(yàn)測(cè)試，圖4為3個(gè)數(shù)據(jù)集的測(cè)試效果。

實(shí)線表示改進(jìn)算法的測(cè)試軌跡圖；虛線表示真實(shí)軌跡

圖4中，不同區(qū)域的顏色表示此處測(cè)試軌跡和真實(shí)軌跡的距離誤差，顏色越靠近右側(cè)色帶條上方的紅色表示誤差越大，越靠近下方的藍(lán)色表示誤差越小。根據(jù)圖4繪制出表1。

表1是兩個(gè)軌跡的整體均方根誤差，可知改進(jìn)算法在定位精度方面比原算法有了一定的提升，在相比特征紋理不豐富的V1_01和V2_01的場(chǎng)景里更為明顯。

表1 不同數(shù)據(jù)集上絕對(duì)軌跡的均方根誤差

2.2 地下車庫弱紋理場(chǎng)景實(shí)驗(yàn)

試驗(yàn)設(shè)備及平臺(tái)：小覓深度相機(jī)，ubuntu16.04操作系統(tǒng)，Intel Core i5處理器，ROS操作平臺(tái)；試驗(yàn)車輛：奇瑞小螞蟻；試驗(yàn)場(chǎng)景：地下車庫中墻面或立柱集中的區(qū)域；場(chǎng)景大小：35 m×35 m，如圖5所示。

圖5 試驗(yàn)設(shè)備及試驗(yàn)環(huán)境

實(shí)驗(yàn)方法為：首先，標(biāo)記車輛起始位置，在車輛起點(diǎn)處的地面上貼上用于標(biāo)記的白色膠帶。在車輛跑完整個(gè)回環(huán)時(shí)使車輛停到起始的標(biāo)記位置，以此作為標(biāo)準(zhǔn)來估計(jì)運(yùn)行軌跡在終點(diǎn)和起始點(diǎn)的距離誤差。環(huán)形軌跡如圖6所示。

圖6 環(huán)形軌跡

以下為原VINS-Mono算法在該實(shí)驗(yàn)環(huán)境下的軌跡運(yùn)行結(jié)果，如圖7所示。改進(jìn)算法在該實(shí)驗(yàn)環(huán)境下的軌跡運(yùn)行結(jié)果，如圖8所示。

由圖7和圖8可知，在車庫拐角的弱紋理明顯的區(qū)域，VINS-Mono算法的定位效果圖上出現(xiàn)了顯而易見的波動(dòng)，對(duì)比VINS-Mono算法，改進(jìn)的算法形成了較好的運(yùn)動(dòng)軌跡，在弱紋理明顯的區(qū)域曲線過渡更為平緩。改進(jìn)算法在回環(huán)軌跡閉合點(diǎn)處的xy平面內(nèi)的距離誤差約為1 m；原VINS-Mono算法在閉合點(diǎn)處xy平面內(nèi)的誤差為3～4 m；改進(jìn)算法在z軸上的誤差比原算法減小了50%左右。

圖7 VINS-Mono實(shí)驗(yàn)結(jié)果

圖8 改進(jìn)算法實(shí)驗(yàn)結(jié)果

通過在地下車庫弱紋理環(huán)境中算法的對(duì)比實(shí)驗(yàn)，改進(jìn)算法的軌跡精度比原算法有了提高，其魯棒性和精確性都得到了驗(yàn)證。

3 結(jié)論

提出了一種改進(jìn)的視覺和IMU融合的算法來針對(duì)地下車庫中光線不充足、且特征紋理較弱的定位問題，得到如下結(jié)論。

(1)改進(jìn)算法在前端采用提取灰度值陡變的像素點(diǎn)來代替特征點(diǎn)進(jìn)行位姿估計(jì)，使得在弱紋理區(qū)域能夠提取到足夠的特征信息用于滿足算法的定位需求，始終保持200個(gè)以上用于定位估計(jì)的關(guān)鍵點(diǎn)。在后端優(yōu)化過程中加入了灰度值不變?cè)硭鶚?gòu)建的視覺殘差，使變量的優(yōu)化過程更適應(yīng)地下車庫環(huán)境，進(jìn)一步提升了算法的魯棒性。

(2)通過實(shí)驗(yàn)驗(yàn)證了改進(jìn)算法在定位精度方面優(yōu)于原VINS-Mono算法。相比原算法雖然在回環(huán)檢測(cè)模塊提取了較少的Harris特征點(diǎn)，在閉環(huán)精度上有一定的犧牲，但在整體的定位準(zhǔn)確度方面并未有明顯差別。