陳孟元，丁陵梅，張玉坤

1 引言

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

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

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

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

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

圖1 IFPL-SLAM算法框架

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

3 點線特征結合的位姿估計策略

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

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

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

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

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

步驟2 依據(jù)錯誤報警數(shù)（NFA）更新矩形線段支持域，輸出符合標準的LSD線特征.

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

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

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

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

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

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

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

圖3 線特征提取效果對比圖

3.2 基于信息熵引導的位姿跟蹤決策

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

圖4 基于信息熵引導的位姿跟蹤決策框架圖

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

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

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

4.1 逆向索引關鍵幀選取策略

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

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

圖5 逆向索引關鍵幀選取策略示意圖

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

圖6 點特征對極幾何約束和線特征的三角化算法

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

其中，s1、s2為尺度因子，K為相機內參矩陣，R21和t21分別為I2相對于I1的旋轉和平移矩陣. 可由基礎矩陣或本質矩陣求解恢復相機運動得到. 旋轉和平移矩陣得出之后將三維向量投影到二維平面，繼而可求得本算法所需的任意兩幀之間的位移變化量Δp和旋轉變化量Δθ.

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

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

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

5 實驗結果與分析

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

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

5.1 點線特征提取實驗

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

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

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

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

圖7 3種算法對不同稠密程度場景的特征提取對比

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

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

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

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

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

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

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

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

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

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

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

6 結論

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