點(diǎn)線特征自適應(yīng)融合室內(nèi)SLAM算法

劉少哲,劉作軍,胡超芳,陳海永

1(河北工業(yè)大學(xué) 人工智能與數(shù)據(jù)科學(xué)學(xué)院,天津 300130) 2(天津大學(xué) 電氣自動(dòng)化與信息工程學(xué)院,天津 300072)

1 引 言

近年來(lái),無(wú)人機(jī)逐漸發(fā)展成為一種可以替代人類對(duì)復(fù)雜未知環(huán)境進(jìn)行探索的新型特種機(jī)器人平臺(tái).人們希望無(wú)人機(jī)在室內(nèi)未知環(huán)境下能夠完全自主地進(jìn)行探索或監(jiān)控任務(wù),但其始終存在著由于信號(hào)不佳而無(wú)法進(jìn)行準(zhǔn)確定位的問(wèn)題.同步定位與地圖構(gòu)建(Simultaneous Localization and Mapping,SLAM)是無(wú)人機(jī)平臺(tái)自主導(dǎo)航的關(guān)鍵技術(shù)[1].過(guò)去20年來(lái),它一直是計(jì)算機(jī)視覺(jué)和機(jī)器人學(xué)領(lǐng)域的研究熱點(diǎn).

隨著計(jì)算機(jī)技術(shù)的飛速發(fā)展,視覺(jué)SLAM以其獨(dú)特的優(yōu)勢(shì)成為了熱門的研究領(lǐng)域.視覺(jué)傳感器可以提供豐富的顏色和紋理等環(huán)境信息,來(lái)實(shí)現(xiàn)準(zhǔn)確的位姿估計(jì)[2].對(duì)于單目相機(jī),像素點(diǎn)的真實(shí)深度需要通過(guò)三角測(cè)量來(lái)求解,不僅增加了計(jì)算時(shí)間,還會(huì)面臨尺度漂移問(wèn)題.雙目相機(jī)可以通過(guò)求解圖像視差來(lái)獲取三維信息,但在低紋理場(chǎng)景中效果不佳.與單目和雙目相機(jī)相比,RGB-D相機(jī)可以通過(guò)物理方法直接獲取像素點(diǎn)深度,節(jié)省了大量的計(jì)算資源.雖然感知距離有限且易受光照影響,但在室內(nèi)環(huán)境中仍具有很大的優(yōu)勢(shì).視覺(jué)里程計(jì)用于估計(jì)相鄰幀的位姿變換,是視覺(jué)SLAM中關(guān)鍵部分.在直接法中,視覺(jué)里程計(jì)利用大量的像素點(diǎn),通過(guò)最小化亮度誤差來(lái)求解相機(jī)的運(yùn)動(dòng)變化.然而直接法依賴于灰度不變假設(shè),在復(fù)雜光照環(huán)境下效果不佳且存在計(jì)算量大的問(wèn)題,限制了其在實(shí)際環(huán)境中的廣泛應(yīng)用.

作為視覺(jué)里程計(jì)另一個(gè)重要分支,特征點(diǎn)法提取圖像中的關(guān)鍵點(diǎn),并計(jì)算關(guān)鍵點(diǎn)之間的匹配關(guān)系[3],具有運(yùn)行穩(wěn)定,對(duì)光照不敏感等優(yōu)點(diǎn).早期特征點(diǎn)法的一個(gè)經(jīng)典代表是PTAM算法[4],它通過(guò)兩個(gè)并行的線程來(lái)求解位姿估計(jì)并建圖,是第一個(gè)實(shí)現(xiàn)實(shí)時(shí)魯棒位姿估計(jì)的特征點(diǎn)法.ORB(Oriented FAST and Rotated BRIEF)特征[5]是最具代表性的實(shí)時(shí)圖像特征之一,具有尺度和旋轉(zhuǎn)不變性,它采用速度極快的二進(jìn)制描述子BRIEF[6,7]來(lái)優(yōu)化提取效率.ORB-SLAM2[8]是由Mur-Artal和Tardos提出的開(kāi)源SLAM算法,該算法以其卓越的速度和精度成為比較SLAM算法時(shí)常用基準(zhǔn)之一[9].在過(guò)去幾年中,一些令人印象深刻的特征點(diǎn)法取得了出色的表現(xiàn),但其實(shí)質(zhì)性問(wèn)題仍未解決.在低紋理和光線變化的室內(nèi)環(huán)境中,這些方法面臨著嚴(yán)峻挑戰(zhàn).

與點(diǎn)特征相比,線特征在以結(jié)構(gòu)化為主室內(nèi)場(chǎng)景中的數(shù)量十分豐富且有更好的魯棒性.在視覺(jué)SLAM發(fā)展早期線特征就引起了學(xué)者們的關(guān)注,但提取效果不穩(wěn)定限制了其在特征法中的應(yīng)用[10].隨著線段檢測(cè)器(line segment detector,LSD)[11],Hough變換[12],EDLines[13]等線段檢測(cè)方法和線帶描述符(line band descriptor,LBD)[14]等線段描述方法的出現(xiàn),基于線特征的SLAM算法應(yīng)用更加廣泛.然而,這些算法的實(shí)際表現(xiàn)并不理想.在特征提取和匹配時(shí),存在端點(diǎn)位置不準(zhǔn)確、幾何約束不明確等問(wèn)題.過(guò)多的誤匹配反而給系統(tǒng)帶來(lái)更多的累積誤差.因此,僅用線特征求解的位姿估計(jì)不如點(diǎn)特征可靠[15].

為了獲得魯棒的SLAM性能,研究人員提出了一些基于點(diǎn)-線特征的SLAM算法.PL-SLAM是第一個(gè)使用點(diǎn)線特征的開(kāi)源SLAM系統(tǒng)[16].該系統(tǒng)利用兩個(gè)特征的組合約束能力,將特征組合擴(kuò)展到BOW(Bag-of-Words)模型以增強(qiáng)閉環(huán)過(guò)程.文獻(xiàn)[17]和文獻(xiàn)[18]將雙目相機(jī)和慣性測(cè)量單元(IMU)緊密結(jié)合,對(duì)誤差函數(shù)進(jìn)行優(yōu)化,這些算法的精度均優(yōu)于點(diǎn)特征法.針對(duì)相機(jī)轉(zhuǎn)彎過(guò)快時(shí)魯棒性差的問(wèn)題,Wang[19]在特征提取和重投影之間增加了角度約束,設(shè)計(jì)了自適應(yīng)加權(quán)模型來(lái)改善位姿優(yōu)化結(jié)果,但并未擴(kuò)展到BA(Bundle Adjustment)模型.Gomez-Ojeda[20]提出了一種新穎的基于點(diǎn)線組合的雙目視覺(jué)里程計(jì).它考慮了參數(shù)提取過(guò)程中的不確定性,使用點(diǎn)線特征對(duì)應(yīng)協(xié)方差矩陣的逆對(duì)其進(jìn)行加權(quán),提高了位姿估計(jì)的精度.此外,研究者還提出了基于RGB-D相機(jī)的點(diǎn)線特征SLAM算法.在分析了深度信息的不確定性后,Yu和Song[21]應(yīng)用RANSAC算法[22]濾除了錯(cuò)誤的線特征,基于點(diǎn)線特征實(shí)現(xiàn)魯棒的視覺(jué)里程計(jì).Zhang[23]利用RGB-D相機(jī)獲取深度信息,構(gòu)建了包含2D和3D信息的點(diǎn)線幾何約束誤差函數(shù),并將基于粒子群優(yōu)化的位姿求解模型擴(kuò)展到BA模塊.為了提取更高質(zhì)量的特征,Li[24]利用RGBD圖像的深度信息,提出了一種SLD線段提取算法和一種線段匹配算法DBM來(lái)消除微小線段.同時(shí)將點(diǎn)線特征加入到閉環(huán)檢測(cè),有效地解決了優(yōu)化中的不穩(wěn)定問(wèn)題.Zou[25]提出了一種室內(nèi)點(diǎn)線特征的RGB-D SLAM算法.它使用Pluecker坐標(biāo)表示空間線特征.通過(guò)分析點(diǎn)和線測(cè)量的不確定度,采用最大似然估計(jì)法計(jì)算相機(jī)運(yùn)動(dòng).

總的來(lái)說(shuō),現(xiàn)有的SLAM算法將點(diǎn)線特征的提取和匹配完全分開(kāi),大大增加了系統(tǒng)的運(yùn)行成本.而且在優(yōu)化誤差函數(shù)時(shí),點(diǎn)線特征的權(quán)重往往是由經(jīng)驗(yàn)設(shè)定的,沒(méi)有實(shí)現(xiàn)真正意義上的“無(wú)縫融合”.針對(duì)上述問(wèn)題,本文提出了一種基于無(wú)人機(jī)飛行特點(diǎn)的點(diǎn)線雙模態(tài)特征自適應(yīng)融合SLAM算法(APL-SLAM).該算法充分發(fā)揮了點(diǎn)特征和線特征的優(yōu)點(diǎn),提高了SLAM的精度和穩(wěn)定性,并滿足無(wú)人機(jī)飛行的實(shí)時(shí)性要求.具體開(kāi)展的工作包括:1)設(shè)計(jì)一種基于特征豐富度的點(diǎn)線特征提取策略,提高了算法在低紋理和模糊場(chǎng)景下的魯棒性;2)利用點(diǎn)線特征關(guān)聯(lián)圖顯著提高了線特征匹配的精度,并有效地節(jié)省了系統(tǒng)進(jìn)行線特征匹配的計(jì)算資源;3)根據(jù)線特征在位姿空間的約束強(qiáng)度,對(duì)點(diǎn)特征進(jìn)行自適應(yīng)加權(quán),建立誤差函數(shù)的自適應(yīng)模型進(jìn)一步提高了算法的精度.

論文的結(jié)構(gòu)安排如下:第2部分介紹了APL-SLAM總體框架,描述了它的3個(gè)線程.第3部分提出了點(diǎn)線雙模態(tài)特征自適應(yīng)融合算法.接著,第4部分驗(yàn)證了本文的SLAM算法,并展示了實(shí)驗(yàn)結(jié)果.最后,第5部分對(duì)本文進(jìn)行總結(jié).

2 APL-SLAM算法的總體框架

本文提出的APL-SLAM算法融合了點(diǎn)線特征,使算法在低紋理環(huán)境下仍能夠穩(wěn)定運(yùn)行.APL-SLAM算法分為3個(gè)主線程:跟蹤、局部建圖和閉環(huán)檢測(cè),其整體框架如圖1所示.

圖1 APL-SLAM系統(tǒng)的整體框架Fig.1 Overall framework of APL-SLAM

跟蹤線程對(duì)每幀圖像進(jìn)行跟蹤和計(jì)算,是SLAM系統(tǒng)的基本步驟.它的主要任務(wù)是在當(dāng)前幀和局部地圖之間尋找盡可能多的對(duì)應(yīng)關(guān)系,以優(yōu)化當(dāng)前幀的位姿.初始化后,SLAM系統(tǒng)對(duì)相機(jī)新采集到的圖像進(jìn)行特征提取處理.在該階段中,點(diǎn)特征使用ORB特征,而線特征則應(yīng)用LSD算法和LBD描述符進(jìn)行提取.針對(duì)低紋理和模糊區(qū)域,本文提出了一種基于區(qū)域特征豐富度的點(diǎn)線特征提取策略,充分利用環(huán)境中的結(jié)構(gòu)信息,保證了跟蹤穩(wěn)定運(yùn)行.將點(diǎn)線特征通過(guò)三角化映射到局部地圖中,并對(duì)其進(jìn)行匹配.本算法通過(guò)建立點(diǎn)線特征關(guān)聯(lián)圖來(lái)優(yōu)化匹配結(jié)果.根據(jù)匹配關(guān)系,通過(guò)投影在參考幀中找到當(dāng)前幀的匹配特征,優(yōu)化各特征的投影誤差即可得到位姿估計(jì)結(jié)果.本文建立了一個(gè)點(diǎn)線特征無(wú)縫融合模型,根據(jù)線特征在位姿空間各方向上的約束強(qiáng)度對(duì)點(diǎn)特征進(jìn)行自適應(yīng)加權(quán).構(gòu)造的目標(biāo)函數(shù)使位姿估計(jì)模型能夠適應(yīng)相機(jī)的運(yùn)動(dòng)狀態(tài).最后,根據(jù)關(guān)鍵幀確定規(guī)則,將當(dāng)前幀添加進(jìn)關(guān)鍵幀集合中.

局部建圖線程主要完成局部地圖的構(gòu)建.局部地圖由關(guān)鍵幀和關(guān)鍵幀所對(duì)應(yīng)的地圖點(diǎn)線構(gòu)成.在該線程中,將跟蹤線程中的新關(guān)鍵幀插入局部地圖,篩選并更新生成的地圖點(diǎn)線.采用光束法平差進(jìn)行局部位姿優(yōu)化,光束法平差(Bundle Adjustment,BA)作為一個(gè)優(yōu)化模型,其本質(zhì)是為了最小化重投影誤差.最后,對(duì)插入的關(guān)鍵幀進(jìn)行篩選,剔除冗余關(guān)鍵幀.

隨著相機(jī)的運(yùn)動(dòng),即使采用局部或全局BA進(jìn)行優(yōu)化,計(jì)算得到的相機(jī)位姿和三角化得到的點(diǎn)云位置仍存在累計(jì)誤差,尋找閉環(huán)并利用閉環(huán)對(duì)所有結(jié)果進(jìn)行優(yōu)化是消除誤差的有效途徑.閉環(huán)檢測(cè)線程主要分為兩個(gè)過(guò)程,即閉環(huán)檢測(cè)和閉環(huán)校正.在閉環(huán)檢測(cè)階段,使用詞袋模型檢測(cè)并構(gòu)建關(guān)鍵幀數(shù)據(jù)庫(kù),詞袋模型(Bag-of-Words,BoW)的作用在于利用特征集合描述圖像,進(jìn)而通過(guò)比較描述的相似程度來(lái)檢測(cè)閉環(huán).接著,利用Sim3算法計(jì)算相似變換,進(jìn)一步篩選相似關(guān)鍵幀.閉環(huán)校正階段主要包括閉環(huán)融合和圖優(yōu)化.由于閉環(huán)檢測(cè)線程對(duì)特征的密度、光照和尺度不變性有較高要求,因此本文在閉環(huán)檢測(cè)線程中只使用點(diǎn)特征進(jìn)行篩選,保證了算法的實(shí)時(shí)性.

3 點(diǎn)-線雙模態(tài)特征的自適應(yīng)融合

3.1 點(diǎn)線雙模態(tài)特征的提取策略

在點(diǎn)特征提取算法中,考慮提取速度和穩(wěn)定性等因素,本文選取具有旋轉(zhuǎn)、尺度不變性的ORB算法進(jìn)行點(diǎn)特征的提取和描述.對(duì)于線特征,采用可以在室內(nèi)低紋理場(chǎng)景中穩(wěn)定提取的LSD算法,基線描述符LBD用來(lái)描述所提取線段的特征信息,可以獲取端點(diǎn)的三維坐標(biāo)或2D坐標(biāo).

由于機(jī)載相機(jī)角度的限制,無(wú)人機(jī)在室內(nèi)飛行過(guò)程中會(huì)攝入大量地面和白墻區(qū)域,給點(diǎn)特征的提取帶來(lái)困難.此外,無(wú)人機(jī)還極易出現(xiàn)抖動(dòng)或大角度轉(zhuǎn)向,這也使點(diǎn)特征的表現(xiàn)不佳,進(jìn)而對(duì)定位精度產(chǎn)生不利影響.相反,線特征受低紋理環(huán)境或模糊的影響較小,且在環(huán)境中數(shù)量足夠豐富,有效準(zhǔn)確的線特征匹配可以保證SLAM算法的穩(wěn)定性.實(shí)驗(yàn)結(jié)果表明,當(dāng)相機(jī)轉(zhuǎn)動(dòng)時(shí),線特征起到重要作用[26].然而,線特征的實(shí)際匹配精度較差,誤匹配的線特征反而給SLAM系統(tǒng)帶來(lái)了更多的誤差,在視覺(jué)紋理信息豐富的環(huán)境中表現(xiàn)得比點(diǎn)特征差.

基于以上分析,本小節(jié)使用特征豐富度表示單幀圖像內(nèi)特征數(shù)量的大小,設(shè)計(jì)了一種基于室內(nèi)場(chǎng)景特征豐富度的點(diǎn)線雙模態(tài)特征提取策略.針對(duì)不同區(qū)域采取對(duì)應(yīng)策略的過(guò)程如圖2所示.

圖2 特征提取策略Fig.2 Feature extraction strategy

為了節(jié)省計(jì)算資源,保證系統(tǒng)的實(shí)時(shí)性,在點(diǎn)特征豐富度較高的區(qū)域只需要提取點(diǎn)特征.但當(dāng)區(qū)域內(nèi)點(diǎn)特征的豐富度較低時(shí),點(diǎn)特征提取的數(shù)量不能滿足位姿優(yōu)化的最低要求.此時(shí),添加線特征可以幫助系統(tǒng)有效克服算法崩潰的問(wèn)題,如圖3所示.此外,當(dāng)機(jī)載相機(jī)處于不穩(wěn)定運(yùn)動(dòng)狀態(tài)時(shí),即相機(jī)捕獲到大量的模糊和旋轉(zhuǎn)圖像序列,線特征可以提供有關(guān)環(huán)境的重要結(jié)構(gòu)信息,以彌補(bǔ)區(qū)域特征豐富度的不足.點(diǎn)線雙模態(tài)特征提取策略充分利用點(diǎn)線特征各自優(yōu)勢(shì),在保證運(yùn)算速度的前提下,提高了系統(tǒng)的魯棒性.

圖3 低紋理場(chǎng)景下點(diǎn)線特征提取效果Fig.3 Point-line feature strategy extraction in low texture scene

3.2 點(diǎn)線雙模態(tài)特征的關(guān)聯(lián)匹配算法

文獻(xiàn)[27]中提出了一種針對(duì)平面特征匹配的關(guān)聯(lián)圖結(jié)構(gòu),其結(jié)點(diǎn)表示平面特征,連接結(jié)點(diǎn)的邊代表特征之間的幾何關(guān)系.本小節(jié)對(duì)其進(jìn)行擴(kuò)展和改進(jìn),針對(duì)線特征匹配效果不佳的問(wèn)題,提出了點(diǎn)線特征關(guān)聯(lián)圖(Point-Line Features Association Graph,PLFAG),同時(shí)參考了線特征之間的相似關(guān)系和點(diǎn)特征與線特征之間的幾何關(guān)系,并建立線特征與線特征、線特征與點(diǎn)特征之間的關(guān)聯(lián)關(guān)系,從而優(yōu)化線特征匹配結(jié)果.

通過(guò)特征提取算法得到的線特征集合{Lj}j=1,…,NL與點(diǎn)特征集合{Pi}i=1,…,NP,建立的PLFAG可表示為G=(H,E).其中H表示特征結(jié)點(diǎn)集合,包括兩種類型:線段特征結(jié)點(diǎn)HL,j=Lj和點(diǎn)特征結(jié)點(diǎn)HP,i=Pi.E表示連接邊集合,也包括兩種類型:線特征結(jié)點(diǎn)之間的邊(LLE)ELL,jl、線特征結(jié)點(diǎn)和點(diǎn)特征結(jié)點(diǎn)之間的邊(PLE)EPL,ij.由于點(diǎn)特征的匹配算法已經(jīng)相對(duì)完善且準(zhǔn)確率較高,處理過(guò)程可參考文獻(xiàn)[3],故本文不再計(jì)算兩個(gè)點(diǎn)特征結(jié)點(diǎn)之間的匹配關(guān)系,因而在PLFAG中點(diǎn)特征結(jié)點(diǎn)之間不進(jìn)行連接.

那么,ELL,jl可定義為:

ELL,jl=(θLL,jl,dLL,jl),j,l∈1,…,NL,j≠l

(1)

其中θLL,jl為線特征方向向量vj和vl之間的夾角,如式(2)所示:

(2)

dLL,jl定義為線特征Lj和Ll的中點(diǎn)Mj和Ml的距離,如式(3)所示:

dLL,jl=|MjMl|

(3)

同樣地,PLE的EPL,ij定義為:

EPL,ij=(θPL,ij,dPL,ij),i∈1,…,NP,j∈1,…,NL

(4)

其中θPL,ij為線特征Lj的方向向量vj和點(diǎn)特征Pi與坐標(biāo)原點(diǎn)連線的方向向量ni之間的夾角,如式(5)所示:

(5)

dPL,ij定義為:

(6)

其中P0為線段Lj上一點(diǎn).至此,該圖像幀的PLFAG建立完成.

假設(shè)從相鄰兩幀圖像中提取到的兩種特征集合表示為{Pci,Lcj}i=1,…,NPc,j=1,…,NLc和{Prm,Lrn}m=1,…,NPr,n=1,…,NLr.對(duì)該相鄰幀分別建立點(diǎn)線特征關(guān)聯(lián)圖Gc=(Hc,Ec)和Gr=(Hr,Er),并通過(guò)計(jì)算Gc和Gr中線特征結(jié)點(diǎn)之間的相似程度,構(gòu)建兩幀線特征之間的匹配關(guān)系.

首先,定義線特征結(jié)點(diǎn)之間的相似程度.相鄰兩幀的線特征結(jié)點(diǎn)HLc,j∈Hc和HLr,n,Hr之間的結(jié)點(diǎn)相似程度SL(HLc,j,HLr,n)由兩部分組成,即兩個(gè)線特征的LBD描述符相似程度和幾何相似程度.

(7)

其中LBD描述符相似程度由該對(duì)線特征描述符的漢明距離

(8)

式中η接近1,κ充分小.δh為1.

接著,為了計(jì)算線特征結(jié)點(diǎn)的幾何相似程度,需要先定義相鄰兩幀LLE和PLE之間的關(guān)系,如下式所示:

(9)

滿足上式關(guān)系的LLE或PLE被稱為一對(duì)相似邊,其中閾值δd取0.1m.該對(duì)相似邊表示其所連接的兩個(gè)特征結(jié)點(diǎn)之間具有相似的幾何關(guān)系.考慮到具有匹配關(guān)系的線特征與其所在幀中其它關(guān)聯(lián)特征之間應(yīng)具有相同的幾何關(guān)系,基于此,通過(guò)以相似邊連接的點(diǎn)特征結(jié)點(diǎn)或線段特征結(jié)點(diǎn)去計(jì)算該對(duì)線特征結(jié)點(diǎn)的幾何相似程度SGEO(HLc,j,HLr,n).由于點(diǎn)特征的ORB信息具有較高的準(zhǔn)確性,因此,通過(guò)與點(diǎn)特征結(jié)點(diǎn)的連接關(guān)系計(jì)算得到的線結(jié)點(diǎn)相似程度也具有較高的準(zhǔn)確性.

SGEO(HLc,j,HLr,n)=

(10)

(11)

其中SORB(HPc,i,HPr,m)表示兩個(gè)點(diǎn)特征結(jié)點(diǎn)之間ORB描述符相似程度,由點(diǎn)特征描述符的漢明距離求得,如式(11)所示.IPL,jn表示索引對(duì)的集合,索引到Hc和Hr中分別與結(jié)點(diǎn)HLc,j和HLr,n以相似邊連接的點(diǎn)特征結(jié)點(diǎn).|IPL,jn|則表示IPL,jn集合中元素的個(gè)數(shù).SLBD(HLc,l,HLr,k)表示兩個(gè)線特征結(jié)點(diǎn)之間LBD描述符相似程度.ILL,jn表示索引對(duì)的集合,索引到Hc和Hr中分別與結(jié)點(diǎn)HLc,j和HLr,n以相似邊連接的線特征結(jié)點(diǎn).|ILL,jn|則表示ILL,jn集合中元素的個(gè)數(shù).

通過(guò)PLFAG進(jìn)行線特征匹配時(shí),根據(jù)式(7)對(duì)來(lái)自當(dāng)前幀和參考幀的每一對(duì)線段特征結(jié)點(diǎn)之間的相似程度進(jìn)行計(jì)算,通過(guò)比較結(jié)點(diǎn)相似程度的大小來(lái)尋找最佳匹配關(guān)系.本文以室內(nèi)場(chǎng)景內(nèi)某特征分布情況為例,繪制了PLFAG結(jié)構(gòu)的示意圖,如圖4所示.其中虛線和實(shí)現(xiàn)分別表示線特征結(jié)點(diǎn)和點(diǎn)特征結(jié)點(diǎn).對(duì)于圖4中點(diǎn)線特征,假設(shè)有如下關(guān)系:

SORB(HPc,1,HPr,1)=SORB(HPc,3,HPr,3)=SORB(HPc,3,HPr,3)=η

SORB(HPc,1,HPr,2)=SORB(HPc,1,HPr,3)=κ

SLBD(HLc,1,HLr,1)=SLBD(HLc,2,HLr,2)=η

SLBD(HLc,1,HLr,2)=κ

(12)

以節(jié)點(diǎn)HLc,1和HLr,1為例,假設(shè)圖4中特征之間的集合位置關(guān)系滿足下式:

(13)

則HLc,1和HLr,1所對(duì)應(yīng)的索引集合為:

(14)

根據(jù)式(7)可知:

(15)

圖4 PLFAG結(jié)構(gòu)示意圖Fig.4 PLFAG structure schematic diagram

本文設(shè)置了閾值δs對(duì)誤匹配進(jìn)行剔除.由于匹配線特征與誤匹配線特征所對(duì)應(yīng)結(jié)點(diǎn)相似程度數(shù)值相差較大,因此閾值δs的取值對(duì)算法結(jié)果的影響非常小.本文實(shí)驗(yàn)中δs取為0.99.則根據(jù)式(15)結(jié)果可知{Lc1,Lr1}為一對(duì)匹配的線特征,{Lc1,Lr2}則為誤匹配特征.基于PLFAG的特征匹配算法充分利用特征之間的幾何關(guān)系,與PAG相比,PLFAG擴(kuò)展了特征結(jié)點(diǎn)的類型,在圖結(jié)構(gòu)中加入了點(diǎn)線特征,并考慮了點(diǎn)線特征之間的約束關(guān)系,提升了算法的魯棒性和適用性.

3.3 點(diǎn)線雙模態(tài)特征對(duì)位姿求解的約束分析及自適應(yīng)融合算法

由于點(diǎn)線兩種特征的誤差向量定義在不同空間,應(yīng)該在總誤差函數(shù)中區(qū)別對(duì)待.因此,本節(jié)提出了一種基于運(yùn)動(dòng)求解約束分析的點(diǎn)線特征自適應(yīng)融合算法,首先分析線特征在位姿空間中各方向上的約束情況,并對(duì)線特征所提供的約束強(qiáng)度進(jìn)行定量描述.接著,根據(jù)線特征的約束分析結(jié)果,定義并計(jì)算點(diǎn)特征的自適應(yīng)權(quán)值和成本函數(shù),完成點(diǎn)線特征的無(wú)縫融合.最后根據(jù)權(quán)值對(duì)點(diǎn)特征進(jìn)行篩選,進(jìn)一步提高算法的準(zhǔn)確性和有效性.

相機(jī)運(yùn)動(dòng)求解的目標(biāo)函數(shù)定義為:

(16)

其中,eLj和ePi表示線特征和點(diǎn)特征的誤差向量,wPi為點(diǎn)特征的自適應(yīng)權(quán)值.

3.3.1 線特征對(duì)位姿求解的約束分析

假設(shè)相鄰兩幀之間相機(jī)運(yùn)動(dòng)表示為ξ=[tT,ωT]T∈6,對(duì)應(yīng)的旋轉(zhuǎn)矩陣R∈SO(3)是反對(duì)稱矩陣[ω]x∈SD(3)的指數(shù)映射,向量t∈3表示平移運(yùn)動(dòng).

假設(shè)多對(duì)線特征{Lcj,Lrj}j=1,…,NL的參數(shù)為L(zhǎng)=[uT,vT]T,其中向量u∈3的方向?yàn)樵摼€段和坐標(biāo)系原點(diǎn)構(gòu)成平面的法向量方向,其大小為坐標(biāo)系原點(diǎn)到該線段的垂直距離,v∈S2是線段的單位方向向量.誤差向量定義為:

eLj=Lcj-T(Lrj,ξ)

(17)

其中,

(18)

接著,計(jì)算eLj對(duì)ξ的雅可比矩陣為:

(19)

相機(jī)運(yùn)動(dòng)的微分量dξ引起的誤差向量eLj變化可以表示為:

deLj=JLjdξ

(20)

對(duì)于具有匹配關(guān)系的線特征集合{Lcj,Lrj}j=1,…,NL,有:

(21)

令:

(22)

矩陣ψL包含關(guān)于所有線特征所對(duì)應(yīng)的雅可比矩陣分布情況的信息,對(duì)其進(jìn)行特征值分解得到:

(23)

其中λLz(z=1,…,6)表示矩陣ψL的特征值,qLz(z=1,…,6)表示對(duì)應(yīng)的特征向量.向量qLz構(gòu)成了六維空間的一組正交基底,而λLz可以表示其對(duì)應(yīng)qLz方向上的約束強(qiáng)度.那么,定義線特征約束向量λL為:

λL=[λL1,λL2,λL3,λL4,λL5,λL6]T

(24)

線特征約束向量λL是以qLz(z=1,…,6)為基底的六維位姿空間中的一個(gè)向量,λL在每個(gè)基底方向上的分量,即為線特征在該方向上所能提供的約束強(qiáng)度.具體來(lái)說(shuō),如果在qL1方向,即最大特征值λL1所對(duì)應(yīng)的特征向量方向上施加一個(gè)運(yùn)動(dòng)的微分量,則會(huì)在誤差向量上引起最大的變化量,而在最小特征值λL6對(duì)應(yīng)方向qL6上的運(yùn)動(dòng)則會(huì)引起最小的變化量.

3.3.2 點(diǎn)-線雙模態(tài)特征的自適應(yīng)融合

由上節(jié)可知,通過(guò)計(jì)算矩陣ψL的特征值分解,可以得到線特征對(duì)相機(jī)運(yùn)動(dòng)估計(jì)的約束分析結(jié)果.對(duì)于點(diǎn)特征,定義優(yōu)化式(16)中目標(biāo)函數(shù)的誤差向量ePi定義為:

ePi=pci-T(pri,ξ)

(25)

其中,

T(pi,ξ)=Rpi+t

(26)

計(jì)算ePi對(duì)ξ的雅可比矩陣為:

(27)

定義矩陣ψPi為:

(28)

計(jì)算{pci,pri}在qLz方向上所能提供的約束強(qiáng)度為:

(29)

定義點(diǎn)特征約束向量λPi為:

λPi=[λPi1,λPi2,λPi3,λPi4,λPi5,λPi6]T

(30)

λPi在每個(gè)基底方向上的分量,代表該對(duì)點(diǎn)特征{pci,pri}在該方向上所能提供的約束強(qiáng)度.那么,式(16)中點(diǎn)特征的自適應(yīng)權(quán)值wPi定義為:

(31)

如式(31)所示,點(diǎn)特征在目標(biāo)函數(shù)中的權(quán)值wPi并非人為設(shè)定,而是基于線特征在六維位姿空間各個(gè)維度上所提供的約束進(jìn)行自適應(yīng)計(jì)算得到的.對(duì)于任意z∈{1,…,6},點(diǎn)特征{pci,pri}在qLz方向上所能提供的約束強(qiáng)度與線特征在該方向所提供的約束強(qiáng)度相似,換言之,對(duì)于線特征無(wú)法提供約束的方向,點(diǎn)特征{pci,pri}也不能提供約束信息.此時(shí),wPi的值會(huì)比較小,這表明點(diǎn)特征{pci,pri}在目標(biāo)函數(shù)中的貢獻(xiàn)也將比較小.而如果存在z∈{1,…,6},使得λLz較小的同時(shí)λPiz較大,即點(diǎn)線特征在對(duì)位姿求解的約束能力上存在互補(bǔ)性.此時(shí),wPi會(huì)比較大,那么點(diǎn)特征{pci,pri}在目標(biāo)函數(shù)中的貢獻(xiàn)也會(huì)比較大,補(bǔ)充了線特征無(wú)法約束的位姿自由度,從而實(shí)現(xiàn)了點(diǎn)線特征的“無(wú)縫融合”,增強(qiáng)了位姿求解問(wèn)題的適應(yīng)性,進(jìn)一步提高整體算法的精度和魯棒性.

此外,wPi還用于對(duì)點(diǎn)特征進(jìn)行篩選.既可以有效地減少特征的數(shù)量,又保留了對(duì)優(yōu)化求解貢獻(xiàn)較大的特征.進(jìn)一步提升了算法的實(shí)時(shí)性.

4 實(shí)驗(yàn)與分析

本節(jié)使用TUM[28]以及ICL-NUIM[29]公開(kāi)數(shù)據(jù)集對(duì)本文提出的點(diǎn)線雙模態(tài)自適應(yīng)融合SLAM算法進(jìn)行實(shí)驗(yàn)驗(yàn)證.實(shí)驗(yàn)所用處理器為Intel Core i5(1.8GHz),運(yùn)行內(nèi)存為12G.使用的軟件包括Ubuntu 20.04、OpenCV 3.2.0、Eigen 3.0等.

使用絕對(duì)軌跡誤差(absolute trajectory error,ATE)的均方根誤差(Root Mean Square Errors,RMSE)對(duì)APL-SLAM的性能進(jìn)行測(cè)評(píng),然后將其與6種先進(jìn)SLAM算法進(jìn)行對(duì)比分析.RMSE值由式(32)求得:

(32)

其中Xi為絕對(duì)軌跡誤差向量,n為幀數(shù).本節(jié)還在室內(nèi)環(huán)境下對(duì)APL-SLAM進(jìn)行了真實(shí)評(píng)測(cè),進(jìn)一步驗(yàn)證算法的性能.

4.1 TUM和ICL-NUIM數(shù)據(jù)集SLAM結(jié)果與分析

實(shí)驗(yàn)選擇的10組TUM數(shù)據(jù)集全部來(lái)自真實(shí)室內(nèi)環(huán)境,包括低紋理、運(yùn)動(dòng)模糊、旋轉(zhuǎn)等典型狀況,圖像采集頻率為30Hz,可以滿足不同的測(cè)試要求.該數(shù)據(jù)集還包括由運(yùn)動(dòng)捕捉系統(tǒng)獲取的相機(jī)位姿基準(zhǔn)值,用于對(duì)SLAM算法得到的相機(jī)位姿的準(zhǔn)確性進(jìn)行評(píng)測(cè).本節(jié)將APL-SLAM算法與6種先進(jìn)的SLAM系統(tǒng)進(jìn)行性能對(duì)比:ORB-SLAM2、RGBD-SLAM[30]、ORB-SLAM3[31]、PL-SLAM、edge-SLAM[32]以及PL-SVO[33].其中,ORB-SLAM2、RGBD-SLAM和ORB-SLAM3算法為影響廣泛的基于點(diǎn)特征的SLAM系統(tǒng),PL-SLAM、Edge-SLAM和PL-SVO則是基于點(diǎn)線特征的SLAM算法.表1列出了10組TUM數(shù)據(jù)集下各個(gè)SLAM算法ATE的均方根誤差結(jié)果.

實(shí)驗(yàn)中提取的點(diǎn)線特征數(shù)量分別設(shè)置為1000和300.通過(guò)ATE來(lái)評(píng)價(jià)各算法的運(yùn)行效果,實(shí)驗(yàn)結(jié)果如表1所示.“failed”表示算法崩潰.從表1可以看出,在10個(gè)圖像序列中的6個(gè),APL-SLAM的精度要高于另外6種SLAM算法.雖然APL-SLAM在另外4個(gè)圖像序列上的精度略低于其他算法,但差距不大.

表1 TUM數(shù)據(jù)集下各SLAM算法的ATE均方根誤差結(jié)果對(duì)比Table 1 Comparison of ATE RMSE[m]results of SLAM algorithms on TUM datasets

圖5給出了APL-SLAM和ORB-SLAM2算法在部分圖像序列上的ATE誤差結(jié)果對(duì)比.從圖中可以更直觀地看出APL-SLAM算法精度取得了顯著的提高.本文方法的ATE比ORB-SLAM2平均降低了27%.在低紋理和模糊場(chǎng)景(fr1/floor、fr3/str_ntex_near)中,本文方法性能提升平均為49%.

圖5 TUM數(shù)據(jù)集上APL-SLAM(左)與ORB-SLAM2(右)的ATE結(jié)果對(duì)比Fig.5 Comparison of ATE results on APL-SLAM(left) and ORB-SLAM2(right) on TUM datasets

此外,實(shí)時(shí)性也是SLAM算法的重要評(píng)價(jià)指標(biāo).表2為APL-SLAM算法與ORB-SLAM2和PL-SLAM在TUM數(shù)據(jù)集上平均運(yùn)行時(shí)間對(duì)比結(jié)果,包括了跟蹤線程中的特征提取匹配部分、姿態(tài)估計(jì)部分、跟蹤局部地圖部分和局部建圖線程的平均運(yùn)行時(shí)間.從表中可以看到,在本文的運(yùn)行平臺(tái)上APL-SLAM算法每幀處理特征所需的運(yùn)行時(shí)間為29.85ms,占跟蹤線程的46%.由于APL-SLAM算法增加了線特征,特征處理時(shí)間超過(guò)了ORB-SLAM2的2倍,但其優(yōu)化了點(diǎn)線雙模態(tài)特征的匹配和融合,因此比同為基于點(diǎn)線特征的PL-SLAM算法所需時(shí)間短.由表5可知,3種算法在位姿估計(jì)部分上差別不大,但在跟蹤局部地圖部分和局部建圖線程中,由于地圖線特征的加入,APL-SLAM和PL-SLAM運(yùn)行時(shí)間明顯增加.

表2 TUM數(shù)據(jù)集下SLAM算法的運(yùn)行時(shí)間對(duì)比Table 2 Comparison of running time[ms]results of SLAM algorithms on TUM datasets

圖6給出了APL-SLAM算法每幀平均運(yùn)行時(shí)間的箱線圖.從圖中可以看出,運(yùn)行時(shí)間與環(huán)境結(jié)構(gòu)密切相關(guān).APL-SLAM算法的平均跟蹤運(yùn)行時(shí)間約為64.23ms,雖然高于ORB-SLAM2算法,但仍然滿足實(shí)時(shí)性要求.

圖6 APL-SLAM算法每幀平均運(yùn)行時(shí)間的統(tǒng)計(jì)箱線圖Fig.6 Statistical boxplot of average running time per frame of APL-SLAM algorithm

本文還使用ICL-NUIM數(shù)據(jù)集對(duì)APL-SLAM進(jìn)行評(píng)估.ICL-NUIM數(shù)據(jù)集包括了起居室場(chǎng)景和辦公室場(chǎng)景,可用于評(píng)測(cè)算法的精度.使用ATE的均方根誤差來(lái)比較APL-SLAM和ORB-SLAM2的性能,結(jié)果如表3所示.本文算法的ATE比ORB-SLAM2算法平均降低了54%,在其中的5組數(shù)據(jù)中結(jié)果優(yōu)于ORB-SLAM2算法.

表3 ICL-NUIM數(shù)據(jù)集下SLAM算法的ATE RMSE結(jié)果對(duì)比Table 3 Comparison of ATE RMSE[m]results of SLAM algorithms on ICL-NUIM datasets

4.2 真實(shí)室內(nèi)環(huán)境數(shù)據(jù)集SLAM結(jié)果與分析

與視覺(jué)點(diǎn)特征相比,線特征對(duì)低紋理環(huán)境和光照變化場(chǎng)景具有更強(qiáng)的魯棒性.在APL-SLAM中,由于加入了線特征,系統(tǒng)對(duì)各類場(chǎng)景都具有更好的適應(yīng)性.本小節(jié)通過(guò)在真實(shí)室內(nèi)環(huán)境下的SLAM實(shí)驗(yàn)驗(yàn)證了本文算法的性能.

選用Intel RealSense D435i作為實(shí)驗(yàn)相機(jī),實(shí)驗(yàn)數(shù)據(jù)集是一個(gè)包含了運(yùn)動(dòng)模糊、旋轉(zhuǎn)和低紋理等室內(nèi)場(chǎng)景的照片序列.該數(shù)據(jù)集模擬了無(wú)人機(jī)室內(nèi)飛行的各種典型情況,如機(jī)載相機(jī)拍攝的大量低紋理場(chǎng)景以及無(wú)人機(jī)不規(guī)則抖動(dòng)或大角度轉(zhuǎn)向?qū)е碌哪：龍D像.圖7展示了數(shù)據(jù)集序列的部分示例,它是一個(gè)具有挑戰(zhàn)性的圖像序列,會(huì)極大地影響SLAM算法的精度.

在本實(shí)驗(yàn)平臺(tái)上ORB-SLAM2算法在該數(shù)據(jù)集上的表現(xiàn)并不理想,系統(tǒng)崩潰時(shí)有發(fā)生.而APL-SLAM算法能夠克服低紋理場(chǎng)景帶來(lái)的不利影響,有效延長(zhǎng)系統(tǒng)跟蹤時(shí)間.跟蹤效果也優(yōu)于ORB-SLAM2,其建立的室內(nèi)稠密點(diǎn)云地圖如圖8所示.該稠密點(diǎn)云圖較完整地重建了室內(nèi)環(huán)境,特別在地板、白墻和窗戶等低紋理場(chǎng)景上有明顯改善.然而,環(huán)境中強(qiáng)光照帶來(lái)的不良影響仍無(wú)法消除,部分區(qū)域的APL-SLAM建圖精

圖7 D435i相機(jī)在室內(nèi)房間拍攝的圖像實(shí)例Fig.7 Example of images captured by D435i in an indoor room

度仍不理想,其原因在于,APL-SLAM算法雖然使用了點(diǎn)線特征,但在強(qiáng)曝光的圖像幀中,提取有效特征十分困難,導(dǎo)致里程計(jì)無(wú)法實(shí)時(shí)跟蹤,進(jìn)而影響了定位和建圖效果.但是,上述不足對(duì)整體地圖重建效果的影響有限,APL-SLAM算法仍可以達(dá)到室內(nèi)困難環(huán)境下運(yùn)行的基本要求.

圖8 APL-SLAM算法的整體實(shí)驗(yàn)結(jié)果Fig.8 Overall experimental result of APL-SLAM algorithm

表4總結(jié)了各SLAM算法在不同環(huán)境下整體表現(xiàn),可以看到,本文算法可以在室內(nèi)環(huán)境、機(jī)身抖動(dòng)和昏暗環(huán)境等條件下穩(wěn)定運(yùn)行,解決了當(dāng)前方法在困難環(huán)境下性能不佳的問(wèn)題,而且運(yùn)行速度得到提高.但在遇到強(qiáng)烈光照時(shí),這4種基于特征的SLAM算法均表現(xiàn)不佳,原因在于強(qiáng)光照導(dǎo)致特征無(wú)法提取,直接影響SLAM的運(yùn)行,因此SLAM算法未來(lái)仍有提高的空間.

表4 各SLAM算法在不同環(huán)境下的表現(xiàn)Table 4 Performance of SLAM algorithms in different environments

5 結(jié) 論

本文提出了一種室內(nèi)點(diǎn)線特征自適應(yīng)融合SLAM算法.本文算法主要有如下創(chuàng)新點(diǎn):1)提出了一種基于特征豐富度的特征提取策略.針對(duì)低紋理、模糊和旋轉(zhuǎn)區(qū)域采用點(diǎn)線特征提取策略,不僅顯著降低了系統(tǒng)運(yùn)行時(shí)間,而且充分保證了各特征的魯棒性;2)采用PLFAG解決線特征誤匹配問(wèn)題.PLFAG不僅考慮了線特征之間的匹配關(guān)系,而且利用點(diǎn)與線特征之間的幾何關(guān)系消除了誤匹配.雖然增加了匹配計(jì)算,但位姿估計(jì)精度明顯提高;3)根據(jù)線特征約束分析結(jié)果,建立了成本函數(shù)點(diǎn)線自適應(yīng)加權(quán)模型.根據(jù)線特征的約束強(qiáng)度計(jì)算點(diǎn)特征的自適應(yīng)權(quán)重,完成點(diǎn)與線特征的無(wú)縫融合.實(shí)驗(yàn)部分中,在TUM和ICL-NUIM公共數(shù)據(jù)集上對(duì)APL-SLAM算法進(jìn)行評(píng)估,并與其他先進(jìn)算法進(jìn)行比較.實(shí)驗(yàn)結(jié)果表明,APL-SLAM算法平均精度提高了27%,在低紋理和模糊數(shù)據(jù)集中平均提升49%.此外,本文還在真實(shí)的室內(nèi)場(chǎng)景中進(jìn)行了實(shí)驗(yàn).APL-SLAM在運(yùn)動(dòng)模糊和低紋理區(qū)域取得了更好的魯棒性和準(zhǔn)確性.在未來(lái)的研究中,將APL-SLAM應(yīng)用于真實(shí)無(wú)人機(jī)平臺(tái),驗(yàn)證算法的可行性.