基于改進(jìn)關(guān)鍵幀選取策略的快速PL-SLAM算法

陳孟元，丁陵梅，張玉坤

1 引言

近年來(lái)，同時(shí)定位與地圖構(gòu)建（Simultaneous Local?ization And Mapping，SLAM）技術(shù)是計(jì)算機(jī)視覺(jué)和機(jī)器人領(lǐng)域的一大研究熱點(diǎn)，其核心在于提高機(jī)器人認(rèn)知自身位姿以及周圍環(huán)境幾何結(jié)構(gòu)的準(zhǔn)確性［1~3］. 根據(jù)機(jī)器人使用傳感器的不同，SLAM 主要分為激光SLAM 和視覺(jué)SLAM（visual SLAM，vSLAM）兩大類. 激光發(fā)生器在成本、功耗、體積和質(zhì)量等方面的劣勢(shì)限制了激光SLAM的應(yīng)用，使得性價(jià)比較高的vSLAM 逐漸成為自主導(dǎo)航與感知的主流［4，5］.

目前較為成熟的vSLAM 系統(tǒng)有ORB-SLAM（Ori?ented FAST and Rotated BRIEF SLAM）［6］和LSD-SLAM（Large Scale Direct SLAM）［7］等，前者利用角點(diǎn)附近領(lǐng)域信息計(jì)算特征描述子，完成特征匹配，繼而基于特征點(diǎn)的數(shù)據(jù)關(guān)聯(lián)構(gòu)造優(yōu)化問(wèn)題求解狀態(tài)變量. 后者主要基于光度不變假設(shè)，使用光流法實(shí)現(xiàn)相機(jī)位姿跟蹤. 基于特征點(diǎn)的vSLAM 系統(tǒng)為視覺(jué)SLAM 算法研究提供了較為完整的實(shí)現(xiàn)框架，但其易受環(huán)境紋理特征稀疏、運(yùn)動(dòng)過(guò)快或曲線運(yùn)動(dòng)等情況的影響，導(dǎo)致定位精度較低，位姿估計(jì)困難，甚至跟蹤丟失［8］.Neira 等人［9］首次將直線特征融合到基于擴(kuò)展卡爾曼濾波的單目SLAM 算法中，通過(guò)對(duì)直線特征的端點(diǎn)進(jìn)行參數(shù)化表示，進(jìn)而構(gòu)建2D地圖，但該系統(tǒng)的定位性能受制于線特征提取精度不高等因素. 為提高線特征的提取效果，Von 等人［10］提出了LSD（Line Segment Detector）線段檢測(cè)器算法，該方法利用區(qū)域增長(zhǎng)算法在形成的像素梯度場(chǎng)中獲得線段支持域，通過(guò)錯(cuò)誤報(bào)警數(shù)更新矩形線段的支持域，輸出了亞像素級(jí)別的線特征.Zhang等人［11］針對(duì)線特征端點(diǎn)位置具有不確定性，導(dǎo)致線特征匹配準(zhǔn)確率較低的問(wèn)題，提出了二進(jìn)制的LBD（Line Band Descriptor）線特征描述符，該算法將線特征支持域劃分為若干條帶，并利用劃分的條帶描述提取的線特征. 文獻(xiàn)［12］提出了第一個(gè)較為完整的基于線特征的雙目SLAM 系統(tǒng)，該系統(tǒng)框架基于3D 線特征，可完成地圖構(gòu)建及閉環(huán)檢測(cè)任務(wù)；Zuo 等人［13］組合使用LSD 和LBD 對(duì)ORB-SLAM 的雙目結(jié)構(gòu)進(jìn)行了改進(jìn)，提出一種魯棒的點(diǎn)線特征結(jié)合的雙目SLAM 系統(tǒng)，該系統(tǒng)繼承了文獻(xiàn)［12］中線特征的3D表示方法，并引入普呂克坐標(biāo)進(jìn)行位姿優(yōu)化，但是該系統(tǒng)在構(gòu)造誤差函數(shù)時(shí)，將點(diǎn)、線特征的檢測(cè)結(jié)果簡(jiǎn)單相加，直接將線特征匹配的高誤差引入系統(tǒng)，降低了數(shù)據(jù)關(guān)聯(lián)的準(zhǔn)確性. Gomez-Ojeda 等人［14］開(kāi)源了PL-SLAM算法，該系統(tǒng)廣泛適用于不同的場(chǎng)景，尤其是點(diǎn)特征稀疏或圖像特征分布不均的場(chǎng)景. 但該算法在所有線程（位姿跟蹤、局部建圖、閉環(huán)檢測(cè)）中同時(shí)使用點(diǎn)線特征，存在數(shù)據(jù)冗余的同時(shí)增大了特征誤匹配的風(fēng)險(xiǎn)，且該算法在曲線運(yùn)動(dòng)尤其是拐彎時(shí)未關(guān)注關(guān)鍵幀的漏選問(wèn)題，存在定位精度不高，閉環(huán)準(zhǔn)確率較低等問(wèn)題.Mur-Artal等人［15］為提高特殊場(chǎng)景下相機(jī)位姿跟蹤的魯棒性，采取盡可能快的插入新的關(guān)鍵幀的策略. 該方法雖通過(guò)增大關(guān)鍵幀的選取密度提高曲線運(yùn)動(dòng)的定位精度，但大量冗余關(guān)鍵幀將增大后端優(yōu)化的計(jì)算負(fù)擔(dān).

綜上所述，現(xiàn)有的PL-SLAM 算法缺乏對(duì)場(chǎng)景紋理稠密程度的識(shí)別能力，在SLAM 各線程中同時(shí)使用點(diǎn)線特征造成特征計(jì)算冗余，實(shí)時(shí)性不足，以及曲線運(yùn)動(dòng)時(shí)存在漏選關(guān)鍵幀等問(wèn)題，本文提出一種基于改進(jìn)關(guān)鍵幀選取策略的快速PL-SLAM 算法（Improved key?frame extraction strategy-based Fast PL-SLAM algorithm，IFPL-SLAM），解決了現(xiàn)有PL-SLAM 算法在特殊場(chǎng)景環(huán)境下運(yùn)行的不足. 在公開(kāi)的KITTI 數(shù)據(jù)集和TUM 數(shù)據(jù)集中對(duì)本文算法進(jìn)行測(cè)試. 測(cè)試結(jié)果表明IFPL-SLAM算法與ORB-SLAM2 算法、PL-SLAM 算法相比，在實(shí)時(shí)性，定位精度等方面有較大優(yōu)勢(shì)，表現(xiàn)出了良好的構(gòu)圖能力.

2 系統(tǒng)整體框架

本文提出的IFPL-SLAM 算法在PL-SLAM 算法的基礎(chǔ)上引入了基于信息熵引導(dǎo)的位姿跟蹤決策和逆向索引關(guān)鍵幀選取策略，其系統(tǒng)整體框架如圖1所示.

圖1 IFPL-SLAM算法框架

位姿跟蹤階段，IFPL-SLAM 算法為解決紋理稠密場(chǎng)景下各線程同時(shí)使用點(diǎn)線特征造成特征計(jì)算冗余，實(shí)時(shí)性不足的問(wèn)題，優(yōu)先對(duì)系統(tǒng)輸入的圖像序列提取點(diǎn)特征，而后通過(guò)基于信息熵引導(dǎo)的位姿跟蹤決策判定是否調(diào)用線特征，使得算法對(duì)場(chǎng)景紋理稠密情況具有識(shí)別能力，提高了系統(tǒng)的實(shí)時(shí)性. 關(guān)鍵幀選取與閉環(huán)檢測(cè)階段，使用逆向索引關(guān)鍵幀選取策略，補(bǔ)充在曲線運(yùn)動(dòng)中漏選的關(guān)鍵幀，提高系統(tǒng)的定位精度和閉環(huán)檢測(cè)的準(zhǔn)確率.

3 點(diǎn)線特征結(jié)合的位姿估計(jì)策略

IFPL-SLAM 算法分別采用ORB（Oriented FAST and Rotated BRIEF）算法［16］和LSD 算法對(duì)場(chǎng)景中點(diǎn)特征、線特征進(jìn)行提取，為避免在紋理稠密場(chǎng)景下點(diǎn)線特征同時(shí)使用造成特征計(jì)算冗余，本文提出一種基于信息熵引導(dǎo)的位姿跟蹤決策，使用信息熵評(píng)價(jià)優(yōu)先提取的特征點(diǎn)，并依據(jù)評(píng)價(jià)結(jié)果決策點(diǎn)線特征的融合使用方式，提高系統(tǒng)的實(shí)時(shí)性. 同時(shí)，為克服LSD 線特征提取時(shí)存在的長(zhǎng)線特征被過(guò)度分割為短線特征，為后續(xù)特征匹配造成誤匹配干擾等問(wèn)題，本文對(duì)LSD 算法提取出的線特征進(jìn)行條件篩選和拼接，提高特征匹配的準(zhǔn)確率.

3.1 基于條件篩選的線特征提取與匹配

LSD作為一種優(yōu)良的局部直線特征提取算法，其優(yōu)點(diǎn)是能在線性時(shí)間內(nèi)得出亞像素級(jí)別精度的線特征檢測(cè)結(jié)果．但缺點(diǎn)也很明顯，如計(jì)算耗時(shí). 因此，本文對(duì)LSD算法提取的線特征進(jìn)行條件篩選，包括拼接被過(guò)度分割的線特征和去除冗余短線特征，減少冗余匹配的計(jì)算耗時(shí)和提高線特征匹配的準(zhǔn)確率. 其具體步驟如下.

步驟1 機(jī)器人在實(shí)驗(yàn)場(chǎng)景中探索環(huán)境，采集當(dāng)前位置的圖像信息，計(jì)算圖像像素梯度，包括方向與幅值，計(jì)算方式如式（1）~（3）所示.

其中，gx(x，y)、gy(x，y) 為像素點(diǎn)在x軸和y軸方向上的梯度，i(x，y)表示圖像中(x，y)位置處像素點(diǎn)的灰度值. 在形成的像素梯度場(chǎng)中利用區(qū)域增長(zhǎng)算法獲得線段支持域，隨后對(duì)該支持域進(jìn)行矩形估計(jì)，得到備選的線特征.

步驟2 依據(jù)錯(cuò)誤報(bào)警數(shù)（NFA）更新矩形線段支持域，輸出符合標(biāo)準(zhǔn)的LSD線特征.

其中，閾值ε為矩形中像素點(diǎn)密度的評(píng)估量. 使用LBD描述算子對(duì)輸出的線特征進(jìn)行描述，并將提取的線特征存入集合中，集合表示第i幀圖像中共有n條線特征，表示第i幀圖像中的第n條線特征.

①計(jì)算當(dāng)前幀圖像fc上線段長(zhǎng)度大于長(zhǎng)度閾值λ的線特征數(shù)目，記為Nλ，記集合

②當(dāng)集合Nλ中的線特征數(shù)目大于δ（δ適應(yīng)于場(chǎng)景紋理的整體稀疏程度）時(shí)，保留集合中前δ條線特征，刪除中其他冗余線特征.

③若集合Nλ中的線特征數(shù)目小于δ，則通過(guò)斷線拼接方法拼接被過(guò)度分割的長(zhǎng)線特征并同步更新和［17，18］. 當(dāng)Nλ≥δ時(shí)停止，返回步驟3②. 否則，說(shuō)明圖像fc中場(chǎng)景紋理較為稀疏，可能相機(jī)正處于紋理稀疏的區(qū)域或相機(jī)抖動(dòng)導(dǎo)致圖像質(zhì)量不佳，此時(shí)保留中的所有線特征并輸出. 其流程圖如圖2所示.

圖2 線特征按條件篩選流程圖

步驟4 本文將冗余的線特征剔除后，利用直線特征的外觀和幾何約束進(jìn)行有效的直線匹配求解相機(jī)位姿.

圖3（a）、3（c）為L(zhǎng)SD 線特征提取效果圖，由于圖像中物體本身的特性和光線等原因，存在較多短線特征和少數(shù)被過(guò)度分割的長(zhǎng)線特征. 圖3（b）、3（d）為條件篩選的線特征提取效果圖，圖中可以看出通過(guò)本文提出的條件篩選方法剔除了圖像中冗余短線特征和部分重復(fù)提取的線特征，使得提取的線特征在圖像中分布較為清晰，為后續(xù)線特征匹配提供了良好的基礎(chǔ).

圖3 線特征提取效果對(duì)比圖

3.2 基于信息熵引導(dǎo)的位姿跟蹤決策

機(jī)器人獲取視覺(jué)場(chǎng)景圖像序列后，為避免圖像紋理稠密場(chǎng)景下，同時(shí)提取點(diǎn)線特征進(jìn)行位姿估計(jì)造成特征冗余，本文提出一種基于信息熵引導(dǎo)的位姿跟蹤決策. 該跟蹤決策優(yōu)先提取圖像中的點(diǎn)特征，并通過(guò)信息熵計(jì)算點(diǎn)特征的信息量，當(dāng)點(diǎn)特征信息量不足時(shí)，調(diào)用圖像中的線特征進(jìn)行輔助位姿估計(jì)，基于信息熵引導(dǎo)的位姿跟蹤決策框架圖如圖4所示.

圖4 基于信息熵引導(dǎo)的位姿跟蹤決策框架圖

信息熵是對(duì)隨機(jī)離散信息的一種量化度量，能夠直接表示圖像中含有的信息量［19，20］. 由于圖像中提取的特征點(diǎn)分布具有不確定性，因此本文通過(guò)信息熵來(lái)計(jì)算優(yōu)先提取的點(diǎn)特征含有的信息量. 其計(jì)算規(guī)則如式（5）所示.其中，P(xi，yi)表示第i個(gè)特征點(diǎn)在圖像中(xi，yi)處出現(xiàn)的概率.n為圖像中特征點(diǎn)的個(gè)數(shù).HX為圖像中特征點(diǎn)的信息熵. 信息熵值越大，圖像中提取的特征點(diǎn)攜帶的信息量越大，反之，說(shuō)明圖像中特征點(diǎn)攜帶的信息量較少.

當(dāng)點(diǎn)特征的信息熵HX高于設(shè)定閾值ξ時(shí)（ξ為經(jīng)驗(yàn)值，由多次實(shí)驗(yàn)估計(jì)而來(lái)），表明該圖像上的點(diǎn)特征可滿足算法進(jìn)行位姿估計(jì)和跟蹤，即此時(shí)不需調(diào)用環(huán)境中的線特征. 而當(dāng)圖像中的場(chǎng)景紋理特征變稀疏時(shí)，點(diǎn)特征提取難度增大，數(shù)量減少或分布不均，此時(shí)信息熵HX低于設(shè)定閾值ξ，為保證有足夠的環(huán)境信息參與到位姿估計(jì)中選擇調(diào)用圖像中的線特征輔助位姿估計(jì)，避免位姿跟蹤丟失. 此外，有選擇地加入線特征位姿估計(jì)相對(duì)于傳統(tǒng)的點(diǎn)線融合視覺(jué)里程計(jì)算法，從根源上避免了圖像特征的冗余提取和計(jì)算，緩解了計(jì)算資源的緊張和因大量計(jì)算帶來(lái)的實(shí)時(shí)性不足等問(wèn)題. 算法1所示為基于信息熵引導(dǎo)的位姿跟蹤決策算法.

4 關(guān)鍵幀選取與閉環(huán)檢測(cè)

4.1 逆向索引關(guān)鍵幀選取策略

關(guān)鍵幀選取對(duì)SLAM 算法的定位精度和建圖效率有著重要作用，后端優(yōu)化和回環(huán)檢測(cè)都是基于選取的關(guān)鍵幀進(jìn)行的. 傳統(tǒng)的關(guān)鍵幀選取方法在曲線運(yùn)動(dòng)拐點(diǎn)附近易出現(xiàn)漏選關(guān)鍵幀等問(wèn)題. 使得整體算法缺乏克服機(jī)器人曲線運(yùn)動(dòng)條件下累計(jì)誤差較大，準(zhǔn)確定位困難，以及回環(huán)誤差修正不完全的能力. 因此本文提出一種逆向索引關(guān)鍵幀選取策略.

逆向索引關(guān)鍵幀選取策略示意圖如圖5所示，機(jī)器人沿指示方向行駛，獲取連續(xù)的圖像幀數(shù)據(jù)，首先確定第一幀圖像為第一個(gè)關(guān)鍵幀并將其位置定為世界坐標(biāo)系的起點(diǎn). 通過(guò)對(duì)極幾何方法估計(jì)幀間的位姿變化.機(jī)器人直線運(yùn)動(dòng)階段旋轉(zhuǎn)變化量Δθ變化較小，運(yùn)動(dòng)變化主要體現(xiàn)在位移變化量Δp上，因此機(jī)器人直線運(yùn)動(dòng)階段依照位移閾值選取關(guān)鍵幀，即當(dāng)Δp超過(guò)位移閾值τ時(shí)選取關(guān)鍵幀，對(duì)應(yīng)于圖5 中的順向索引. 當(dāng)機(jī)器人曲線運(yùn)動(dòng)，尤其是發(fā)生轉(zhuǎn)彎時(shí)，機(jī)器人可在短時(shí)間、短距離范圍內(nèi)發(fā)生較大的行駛角度變化，若未將關(guān)鍵位置處的圖像幀設(shè)為關(guān)鍵幀，造成關(guān)鍵位置關(guān)鍵幀遺漏，將直接影響后端的回環(huán)檢測(cè)及累計(jì)誤差修正，繼而影響全局一致性軌跡圖的構(gòu)建，因此，機(jī)器人曲線運(yùn)動(dòng)時(shí)，結(jié)合角度閾值進(jìn)行關(guān)鍵幀選取. 具體過(guò)程為：計(jì)算當(dāng)前幀fc與上一關(guān)鍵幀kfj之間的旋轉(zhuǎn)變化量Δθjc，當(dāng)Δθjc大于順向角度閾值γ1時(shí)，記當(dāng)前關(guān)鍵幀為新的關(guān)鍵幀，記為kfc，同時(shí)以當(dāng)前關(guān)鍵幀為起點(diǎn)雙向索引，其中逆向索引是指，估計(jì)kfc與介于kfc與kfc-1之間所有的普通幀之間的逆向旋轉(zhuǎn)變化量Δθci，當(dāng)Δθci大于逆向角度閾值γ2時(shí)，補(bǔ)充對(duì)應(yīng)的普通幀fi為關(guān)鍵幀，插入關(guān)鍵幀庫(kù)，該過(guò)程不中斷關(guān)鍵幀的順向索引，即繼續(xù)幀間位姿估計(jì)，依據(jù)位移閾值τ和角度閾值γ1選取新的關(guān)鍵幀.在實(shí)際操作中γ1和γ2的設(shè)定受環(huán)境紋理稀疏程度的影響，同時(shí)，為了避免關(guān)鍵幀的冗余，一般設(shè)置γ1>γ2.

圖5 逆向索引關(guān)鍵幀選取策略示意圖

Δpjc和Δθjc表示當(dāng)前幀fc相對(duì)于上一關(guān)鍵幀kfj的位移和角度變化量. 幀間位姿變化矩陣用對(duì)極幾何表示為式（6）. 圖像中的點(diǎn)、線特征經(jīng)過(guò)提取與匹配之后，得到如圖6 所示的點(diǎn)特征對(duì)極幾何約束和線特征的三角化算法.

圖6 點(diǎn)特征對(duì)極幾何約束和線特征的三角化算法

以點(diǎn)特征為例，空間一點(diǎn)P=[X，Y，Z]T在兩幀圖像中的投影為p1和p2，由針孔相機(jī)模型可得像素點(diǎn)p1和p2的像素位置為：

其中，s1、s2為尺度因子，K為相機(jī)內(nèi)參矩陣，R21和t21分別為I2相對(duì)于I1的旋轉(zhuǎn)和平移矩陣. 可由基礎(chǔ)矩陣或本質(zhì)矩陣求解恢復(fù)相機(jī)運(yùn)動(dòng)得到. 旋轉(zhuǎn)和平移矩陣得出之后將三維向量投影到二維平面，繼而可求得本算法所需的任意兩幀之間的位移變化量Δp和旋轉(zhuǎn)變化量Δθ.

新關(guān)鍵幀選取方法的數(shù)學(xué)表達(dá)式如式（7）所示. 兩類閾值是判別關(guān)鍵幀的主要依據(jù)，當(dāng)新關(guān)鍵幀是依據(jù)旋轉(zhuǎn)變化量確定的時(shí)，啟動(dòng)逆向索引關(guān)鍵幀選取策略，避免關(guān)鍵位置遺漏. 逆向索引關(guān)鍵幀選取策略流程如算法2所示.

4.2 融合點(diǎn)線特征的閉環(huán)檢測(cè)

當(dāng)插入新的關(guān)鍵幀之后，利用詞袋模型［21］中的詞包向量在地圖中搜索與之具有相同詞匯的所有關(guān)鍵幀，并通過(guò)點(diǎn)線特征匹配確定閉環(huán). 在計(jì)算相似性得分時(shí)，按照閉環(huán)候選幀中的點(diǎn)線比例進(jìn)行加權(quán)：

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

本文實(shí)驗(yàn)所用的電腦軟硬件配置：CPU 為Inter Core i5-9400F，6 核處理器，主頻2.9 GHz，內(nèi)存8 GB，系統(tǒng)為Ubuntu18.04. 為了驗(yàn)證本文算法的有效性，分別使用KITTI 公開(kāi)數(shù)據(jù)集［22］和TUM［23］公開(kāi)數(shù)據(jù)集對(duì)本文所提策略進(jìn)行驗(yàn)證.

長(zhǎng)期以來(lái)由于投入資金不足，大多數(shù)縣鄉(xiāng)河道，特別是河道沿岸的縣城、重要集鎮(zhèn)和糧食生產(chǎn)基地的已建防洪排澇設(shè)施不足，遇到常遇洪水就可能造成較大洪澇災(zāi)害。一些河道因水土流失嚴(yán)重、侵占河道的問(wèn)題，致使河道萎縮，行洪排澇能力降低，對(duì)所在地區(qū)防洪排澇安全構(gòu)成威脅。

5.1 點(diǎn)線特征提取實(shí)驗(yàn)

圖7 所示為ORB-SLAM2、PL-SLAM 和IFPL-SLAM 3種算法在TUM 數(shù)據(jù)集上的特征提取效果對(duì)比圖. 表1所示為對(duì)應(yīng)3 種算法的特征提取時(shí)間與匹配準(zhǔn)確率對(duì)比結(jié)果. 其中，圖7（a）~7（f）所示場(chǎng)景紋理信息豐富，點(diǎn)線特征稠密. 該6 幅圖的特征提取對(duì)比可知，稠密場(chǎng)景下3種算法均能提取到豐富的特征信息，而IFPL-SLAM算法與PL-SLAM 算法相比，由于IFPL-SLAM 算法采用了基于信息熵引導(dǎo)的位姿跟蹤決策，通過(guò)計(jì)算點(diǎn)特征的信息熵得出該圖像中提取的點(diǎn)特征已能滿足相機(jī)位姿求解，因此，本文算法在該圖像幀未進(jìn)行線特征提取，節(jié)省了線特征的處理時(shí)間. 使得本文算法的單幀處理時(shí)間與PL-SLAM 算法相比減少了16.0%. 圖7（h）~7（j）所示場(chǎng)景的紋理較為稀疏，僅提取點(diǎn)特征的ORBSLAM2 算法由于缺乏數(shù)目充足且均勻分布的點(diǎn)特征進(jìn)行特征匹配，導(dǎo)致匹配準(zhǔn)確率較低.IFPL-SLAM 算法克服了稀疏場(chǎng)景中點(diǎn)特征不足的弊端，針對(duì)稀疏場(chǎng)景點(diǎn)特征不足的情況及時(shí)調(diào)用環(huán)境中的線特征，并對(duì)提取的線特征進(jìn)行條件篩選，剔除了冗余線特征，整體提升了該算法的特征匹配準(zhǔn)確率. 使得本文算法的匹配準(zhǔn)確率與ORB-SLAM2相比提高了7.8%.

表1 TUM數(shù)據(jù)集下3種算法特征匹配準(zhǔn)確率和特征提取時(shí)間對(duì)比

5.2 KITTI數(shù)據(jù)集評(píng)估

本文選取KITTI 數(shù)據(jù)集進(jìn)行評(píng)估，如圖8（a）~8（d）分別為ORB-SLAM2、PL-SLAM 和IFPL-SLAM 3 種算法在KITTI 4 個(gè)序列下獲取的軌跡圖對(duì)比. 表2 為對(duì)應(yīng)的3種算法在4個(gè)序列中運(yùn)行時(shí)獲取的對(duì)比數(shù)據(jù).01序列和10序列較為簡(jiǎn)單，3種算法均實(shí)現(xiàn)了不同精準(zhǔn)程度的跟蹤，其中IFPL-SLAM 算法的軌跡更加接近真實(shí)軌跡.圖8（c）所示為3 種算法在08 序列中獲取的軌跡圖，該序列在真實(shí)軌跡坐標(biāo)為（375，400）位置附近時(shí)，場(chǎng)景紋理信息較為稀疏.ORB-SLAM2 算法由于在稀疏場(chǎng)景中提取的點(diǎn)特征較少導(dǎo)致跟蹤丟失. 而IFPL-SLAM 和PL-SLAM 算法憑借線特征的補(bǔ)充跟蹤完成了對(duì)整個(gè)環(huán)境的建圖. 圖8（d）所示02序列是一個(gè)帶閉環(huán)的圖像序列，對(duì)比的3 種算法均通過(guò)閉環(huán)檢測(cè)修正累計(jì)誤差. 從圖8（c）和8（d）中的局部放大圖中可以看出，在拐彎和曲線運(yùn)動(dòng)過(guò)程中，由于IFPL-SLAM 算法提出的逆向索引關(guān)鍵幀選取策略補(bǔ)充了更多關(guān)鍵位置的關(guān)鍵幀，提高了算法的定位精度，使得IFPL-SLAM 的絕對(duì)軌跡誤差相比于PL-SLAM 算法縮小了23.4%，相比于ORBSLAM2 縮小了43.0%. 可見(jiàn)本文算法在定位精度方面與ORB-SLAM2和PL-SLAM算法相比有較大優(yōu)勢(shì).

圖7 3種算法對(duì)不同稠密程度場(chǎng)景的特征提取對(duì)比

表2 3種算法在KITTI數(shù)據(jù)集4個(gè)序列下的結(jié)果對(duì)比

5.3 TUM數(shù)據(jù)集評(píng)估

本文進(jìn)一步使用TUM 數(shù)據(jù)集進(jìn)行評(píng)估，TUM 數(shù)據(jù)集主要源于室內(nèi)環(huán)境，實(shí)驗(yàn)中選取紋理較為稀疏的fr3/large/cabinet 序列、旋轉(zhuǎn)度較大的fr2/360-hemisphere 序列、具有閉環(huán)場(chǎng)景的fr1/plant 與fr2/xyz 序列對(duì)本文算法進(jìn)行驗(yàn)證. 圖9 分別為ORB-SLAM2、PL-SLAM 和IFPLSLAM 3種算法在不同序列下構(gòu)建的軌跡圖.

圖8 KITTI數(shù)據(jù)集序列下的軌跡圖

圖9 TUM數(shù)據(jù)集序列下的軌跡圖

從圖9（a）可以看出，由于場(chǎng)景為稀疏環(huán)境，ORBSLAM2 算法的跟蹤效果較差，且在運(yùn)行時(shí)由于提取的點(diǎn)特征無(wú)法滿足匹配要求導(dǎo)致跟蹤丟失. 而IFPLSLAM 算法和PL-SLAM 算法通過(guò)點(diǎn)線特征結(jié)合進(jìn)行跟蹤，能夠完成全局一致性建圖. 從圖9（b）中可以看出機(jī)器人在藍(lán)色矩形區(qū)域內(nèi)出現(xiàn)了多次劇烈的拐彎導(dǎo)致ORB-SLAM2 和PL-SLAM 最終在（1.344，0）附近發(fā)生跟蹤丟失，而IFPL-SLAM 算法的逆向索引關(guān)鍵幀選取策略補(bǔ)充了拐彎處漏選的關(guān)鍵幀，因此能夠較好地完成彎道軌跡的跟蹤并提升整個(gè)環(huán)境的建圖效果. 圖9（c）和9（d）是對(duì)閉環(huán)場(chǎng)景軌跡的跟蹤效果，2 種場(chǎng)景下的3種算法均因具有閉環(huán)檢測(cè)環(huán)節(jié)完成了全局地圖的構(gòu)建，由于IFPL-SLAM 算法對(duì)提取的線特征進(jìn)行了條件篩選且引入了逆向索引關(guān)鍵幀選取策略使得本文算法獲取的軌跡圖更加接近真實(shí)軌跡.

表3 為3 種算法在不同序列下的平移誤差和旋轉(zhuǎn)誤差對(duì)比. 從表中數(shù)據(jù)可知，IFPL-SLAM 算法的平移誤差與PL-SLAM 相比減少了21.3%，與ORB-SLAM2 相比減少了43.1%. 同時(shí)，IFPL-SLAM算法的旋轉(zhuǎn)誤差與PLSLAM 相比減少了17.2%，與ORB-SLAM2 相比減少了39.3%.

表3 3種算法在TUM數(shù)據(jù)集下的平移誤差和旋轉(zhuǎn)誤差對(duì)比

圖10 TUM數(shù)據(jù)下的準(zhǔn)確率－召回率對(duì)比圖

圖11所示為3種算法在TUM 數(shù)據(jù)集下的運(yùn)行時(shí)間對(duì)比圖，從圖中可以看出本文算法的運(yùn)行時(shí)間與PLSLAM 相比減少了22.1%，與ORB-SLAM2 算法相比，由于本文算法增加了對(duì)場(chǎng)景紋理稠密程度的識(shí)別能力、線特征的提取與篩選占用了一部分運(yùn)行時(shí)間，使得本文算法運(yùn)行時(shí)間較ORB-SLAM2 有所增加，但仍可滿足實(shí)時(shí)性需求.

圖11 3種算法在TUM數(shù)據(jù)集下的運(yùn)行時(shí)間對(duì)比圖

6 結(jié)論

本文提出了一種IFPL-SLAM 算法，該算法具有以下優(yōu)點(diǎn).（1）提出一種基于信息熵引導(dǎo)的位姿跟蹤決策，解決了PL-SLAM 在稠密場(chǎng)景下同時(shí)使用點(diǎn)線特征（進(jìn)行位姿跟蹤）造成特征計(jì)算冗余的問(wèn)題，提高了系統(tǒng)的實(shí)時(shí)性.（2）針對(duì)LSD 線特征提取時(shí)存在的長(zhǎng)線特征被過(guò)度分割為短線特征，對(duì)LSD 算法提取出的線特征進(jìn)行按條件篩選和拼接，剔除冗余短線特征，提高了特征匹配的準(zhǔn)確率.（3）通過(guò)引入逆向索引關(guān)鍵幀選取策略解決了在非直線運(yùn)動(dòng)條件下漏選關(guān)鍵幀的問(wèn)題，提高了算法的閉環(huán)準(zhǔn)確率和定位精度. 所提算法在公開(kāi)的KITTI 數(shù)據(jù)集和TUM 數(shù)據(jù)集上進(jìn)行測(cè)試，結(jié)果表明，所提算法與ORB-SLAM2 和PL-SLAM 算法相比，在實(shí)時(shí)性和定位精度方面具有一定的優(yōu)勢(shì)，體現(xiàn)出了良好的構(gòu)圖能力. 然而本文算法受動(dòng)態(tài)障礙物影響較大，下一步將在本文的研究基礎(chǔ)上，融合IMU、激光雷達(dá)等較精確的傳感器提高機(jī)器人遇到動(dòng)態(tài)障礙物的魯棒性，使得本文所提方法的位姿估計(jì)能力更加精確.