摘 要：為了在光照不足和低紋理場(chǎng)景中實(shí)現(xiàn)移動(dòng)機(jī)器人的高精度實(shí)時(shí)定位和建圖，提出了一種基于視覺點(diǎn)線特征以及IMU特征融合的實(shí)時(shí)SLAM算法。首先通過跳躍路由策略和自適應(yīng)閾值策略改進(jìn)了EDlines算法，提高了線特征提取的質(zhì)量，從而提高了特征跟蹤的有效性。然后將視覺慣性特征緊耦合建立約束，通過滑動(dòng)窗口和邊緣化模型進(jìn)行非線性優(yōu)化，實(shí)現(xiàn)了高精度高實(shí)時(shí)性的狀態(tài)估計(jì)。實(shí)驗(yàn)證明，所提算法在線特征提取的有效性方面優(yōu)于傳統(tǒng)的線段提取算法，同時(shí)SLAM系統(tǒng)的定位精度和魯棒性均得到有效提升。

關(guān)鍵詞：視覺同步定位與建圖；特征提?。灰曈X慣性緊耦合；滑動(dòng)窗口

中圖分類號(hào)：TP242 文獻(xiàn)標(biāo)志碼：A 文章編號(hào)：1001-3695（2024）10-018-3008-07

doi：10.19734/j.issn.1001-3695.2023.12.0636

Real-time visual-inertial SLAM algorithm based on point-line feature fusion

Wang Leia，Chen Shuaikuna，Qi Junyanb，Yuan Ruifuc

（a.School of Computer Science & Technology，b.School of Software，c.School of Energy Science & Engineering，Henan Polytechnic University，Jiaozuo Henan 454003，China）

Abstract：This paper introduced a real-time SLAM algorithm designed for mobile robots operating in environments characte-rized by low illumination and low texture.The algorithm leveraged the fusion of visual point-line features and IMU characteristics.It enhanced the EDlines algorithm by incorporating a leapfrog routing strategy and an adaptive threshold strategy，thereby improving the quality of line feature extraction and consequently enhancing feature tracking effectiveness.It established tight coupling constraints between visual and inertial features and employed a sliding window，along with a marginalization model，for nonlinear optimization.This enabled high-precision and real-time state estimation.Experimental results show that the proposed algorithm is superior effectiveness in online feature extraction when compared to traditional line segment extraction methods.Concurrently，the SLAM system achieves enhanced localization accuracy and robustness.

Key words：visual simultaneous localization and mapping（VSLAM）;feature extraction;visual-inertial tight coupling;sliding window

0 引言

同步定位與建圖（simultaneous localization and mapping，SLAM）技術(shù)是機(jī)器人領(lǐng)域具有重要意義的技術(shù)之一。目前，SLAM方法可分為視覺和激光兩大領(lǐng)域。雖然激光SLAM在理論、技術(shù)和實(shí)際應(yīng)用上都比較成熟，但是二維激光SLAM只能獲取環(huán)境中的單個(gè)平面信息，不適合在大型復(fù)雜場(chǎng)景中應(yīng)用。而通過增加雷達(dá)線路數(shù)量來獲取更多信息的方法會(huì)導(dǎo)致硬件成本的大幅增加。視覺傳感器與激光雷達(dá)相比具有輕便、便宜的優(yōu)點(diǎn)，且視覺傳感器所構(gòu)建的地圖包含更多的信息。低功耗和低重量也使得在負(fù)載有限的移動(dòng)平臺(tái)上部署視覺傳感器成為可能。

隨著相關(guān)技術(shù)的快速發(fā)展，許多優(yōu)秀的視覺SLAM（VSLAM）算法涌現(xiàn)出來。文獻(xiàn)［1］提出了MonoSLAM，使用圖像Shi-Tomasi角點(diǎn)，再利用EKF算法進(jìn)行狀態(tài)估計(jì)。其作為首個(gè)使用單目視覺傳感器的實(shí)時(shí)SLAM方法，取得了不錯(cuò)的效果，使得VSLAM的研究流行起來。后來，基于關(guān)鍵幀的并行跟蹤與建圖方法（parallel tracking and mapping）［2］的提出，為VSLAM打開了基于非線性優(yōu)化方向的突破，自此VSLAM的研究開始有了區(qū)分前端和后端的概念。該算法實(shí)現(xiàn)了并行處理任務(wù)，能夠更好地進(jìn)行全局優(yōu)化，并且使得跟蹤更加穩(wěn)定。VSLAM根據(jù)前端對(duì)圖像的處理方法，可分為特征點(diǎn)法和直接法。特征點(diǎn)法即通過提取關(guān)鍵子和計(jì)算描述子兩步來確定圖像特征。直接法通過最小化光度誤差來直接確定點(diǎn)與點(diǎn)之間的關(guān)系，通過圖像的像素灰度信息來計(jì)算相機(jī)運(yùn)動(dòng)［3］。但是直接法由于其原理特性，也存在算法非凸性及魯棒性差等問題。因此在VSLAM前端部分，采用特征點(diǎn)法是研究工作的主流。特征點(diǎn)提取算法包括SIFT、SURF、Harris角點(diǎn)、ORB［4～7］等。在這些特征提取方法中，都只提取圖像中的重要特征點(diǎn)，同時(shí)為了提高算法的實(shí)時(shí)性，特征點(diǎn)的分布是稀疏的。這使得VSLAM對(duì)光照更加敏感，在光照劇烈變化或相機(jī)高速運(yùn)動(dòng)的情況下，算法將無法有效提取到圖像中的特征信息。例如，ORB-SLAM2［8］和DSO［9］均采用視覺特征點(diǎn)方法進(jìn)行定位，在環(huán)境光照條件良好的情況下，能夠達(dá)到良好的定位精度，然而在低紋理和光照條件差的情況下，這些系統(tǒng)表現(xiàn)較差甚至發(fā)生崩潰。

為了豐富SLAM技術(shù)的應(yīng)用場(chǎng)景，研究者意圖通過融合相機(jī)、激光雷達(dá)、慣性測(cè)量單元（IMU）和GPS等不同傳感器以提高系統(tǒng)的精度和魯棒性。其中，視覺傳感器和IMU傳感器的融合是十分有效的。視慣融合優(yōu)秀的算法如ORB-SLAM3［10］，利用長(zhǎng)期的數(shù)據(jù)相關(guān)性實(shí)現(xiàn)大場(chǎng)景的高精度定位。其快速準(zhǔn)確的IMU初始化和多地圖拼接技術(shù)使其能夠在各種場(chǎng)景中實(shí)現(xiàn)魯棒的實(shí)時(shí)操作，但這也需要較高的計(jì)算成本。2018年Qin等人［11］提出了VINS-Mono，采用視覺慣性緊耦合的方法建立視覺特征約束和IMU預(yù)積分約束，在前端部分采用Shi-Tomasi角點(diǎn)和LK光流法進(jìn)行特征點(diǎn)提取。與傳統(tǒng)的視覺SLAM算法相比，其定位精度得到了顯著提升，但由于其前端采用光流法進(jìn)行跟蹤匹配，不提取特征描述符，當(dāng)圖像幀劇烈運(yùn)動(dòng)且模糊時(shí)，特征點(diǎn)會(huì)丟失，系統(tǒng)也極易受到光照影響。

為了解決上述問題，研究者關(guān)注到可通過多特征融合，包括提取點(diǎn)、線、灰度值等特征，獲得多個(gè)特征映射元素，進(jìn)而提高單一基于特征點(diǎn)的SLAM系統(tǒng)的準(zhǔn)確性和魯棒性。線特征具有良好的光照不變性和旋轉(zhuǎn)不變性，可以對(duì)點(diǎn)特征進(jìn)行更高結(jié)構(gòu)層次的補(bǔ)充，對(duì)場(chǎng)景結(jié)構(gòu)提供額外的約束條件。如Pumarola等人［12］提出的PL-SLAM，在ORB特征點(diǎn)的基礎(chǔ)上添加了采用LSD線段提取算法［13］的線特征，提高了系統(tǒng)的精度和魯棒性。2018年He等人［14］提出了PL-VIO，實(shí)現(xiàn)了點(diǎn)線特征融合以及視覺慣性融合。后來，F(xiàn)u等人［15］在PL-VIO的基礎(chǔ)上進(jìn)行改進(jìn)，將原先的線特征提取算法LSD進(jìn)行了優(yōu)化，調(diào)整一些隱藏參數(shù)并引進(jìn)長(zhǎng)度抑制策略，使得LSD的運(yùn)行速度達(dá)到原來的三倍。趙偉博等人［16］提出了一種點(diǎn)線結(jié)合的視慣SLAM算法，在前端引入了改進(jìn)的LSD線段提取算法來增加線特征，取得了不錯(cuò)的效果。此外，研究者還注意到基于深度學(xué)習(xí)的視覺慣性里程計(jì)具有明顯的優(yōu)勢(shì)。與傳統(tǒng)方法相比，神經(jīng)網(wǎng)絡(luò)完全依靠數(shù)據(jù)驅(qū)動(dòng)訓(xùn)練和提取特征，能夠?qū)?shù)據(jù)自身蘊(yùn)涵的信息最大程度地挖掘并進(jìn)行深度預(yù)測(cè)。2017年，Clark等人［17］首次提出融合了IMU信息的VINet，它利用CNN從相鄰幀圖像中提取出視覺特征，并使用LSTM網(wǎng)絡(luò)來訓(xùn)練慣導(dǎo)特征模型。既減少了系統(tǒng)對(duì)手動(dòng)同步和校準(zhǔn)的依賴，同時(shí)也表現(xiàn)出了更強(qiáng)的魯棒性。2021年，Zuo等人［18］提出了CodeVIO，做到了網(wǎng)絡(luò)在實(shí)時(shí)單線程運(yùn)行的同時(shí)，具有較強(qiáng)的泛化能力以及更高的計(jì)算效率。但是，限于深度學(xué)習(xí)的特性，多數(shù)系統(tǒng)無法很好地兼顧效率和性能，并且多數(shù)基于深度學(xué)習(xí)的視慣SLAM算法仍僅利用特征點(diǎn)作為前端輸入，造成系統(tǒng)的魯棒性較低。

目前，主流SLAM算法對(duì)于線特征處理的方法多為L(zhǎng)SD算法。而LSD算法計(jì)算運(yùn)行速度較慢，難以滿足實(shí)時(shí)性要求。雖然EDLines［19］線段提取算法提取速度是LSD算法的近十倍，但對(duì)光照變化較為敏感，提取的線段存在較多不連續(xù)的分割短線，難以提供精確的有效線特征，加大了線特征匹配任務(wù)的難度，導(dǎo)致SLAM系統(tǒng)魯棒性較差。針對(duì)以上問題，本文在VINS-Mono框架的基礎(chǔ)上提出了一種具有實(shí)時(shí)性的基于點(diǎn)線特征的單目視覺慣性緊融合SLAM算法。算法在前端部分加入了改進(jìn)的EDLines提取匹配算法來解決線段檢測(cè)算法在復(fù)雜環(huán)境下線特征提取質(zhì)量低的問題，提高了系統(tǒng)的實(shí)時(shí)性和魯棒性。然后在后端利用滑動(dòng)窗口將點(diǎn)、線特征和IMU特征進(jìn)行緊耦合，并通過邊緣化模型優(yōu)化位姿，聯(lián)合最小化多差目標(biāo)函數(shù)，實(shí)現(xiàn)精準(zhǔn)的狀態(tài)估計(jì)和環(huán)境建圖。

1 系統(tǒng)架構(gòu)

本文系統(tǒng)采用點(diǎn)線視覺特征與IMU信息聯(lián)合對(duì)位姿進(jìn)行非線性優(yōu)化的方法，實(shí)現(xiàn)了前端、后端優(yōu)化和回環(huán)檢測(cè)與全局優(yōu)化三個(gè)線程。系統(tǒng)的整體結(jié)構(gòu)如圖1所示。

在前端視覺里程計(jì)部分，系統(tǒng)對(duì)相機(jī)和IMU采集的數(shù)據(jù)作預(yù)處理。其中包含并行處理原始圖像，對(duì)其進(jìn)行點(diǎn)特征和線特征的提取以及跟蹤匹配。其中點(diǎn)特征的提取沿用VINS-Mono的Harris角點(diǎn)提取算法，利用KLT光流法跟蹤特征點(diǎn)，并使用RANSAC算法剔除異常值。線特征的提取是基于改進(jìn)的EDLines算法，通過跳躍路由策略和自適應(yīng)閾值策略提高線特征提取算法在復(fù)雜環(huán)境中的有效性。然后利用LBD描述子［20］計(jì)算跟蹤線特征，進(jìn)行線段匹配。最后用幾何約束算法剔除誤匹配。此外，在前端部分的系統(tǒng)初始化過程中，主要是利用點(diǎn)、線以及IMU預(yù)積分［21］處理之后的信息優(yōu)化求取系統(tǒng)的初始化參數(shù)。

之后，在后端對(duì)位姿進(jìn)行非線性優(yōu)化，將所有約束項(xiàng)構(gòu)建為目標(biāo)函數(shù)。主要工作包括通過對(duì)二維幀間的點(diǎn)、線特征對(duì)應(yīng)進(jìn)行三角化，重投影空間點(diǎn)和線?？臻g點(diǎn)和空間線分別由逆深度參數(shù)化和普呂克坐標(biāo)參數(shù)化［22］。然后，基于滑動(dòng)窗口，聯(lián)合最小化殘差函數(shù)，得到位姿、速度、空間點(diǎn)和空間線以及加速度和陀螺儀bias在內(nèi)的最優(yōu)狀態(tài)向量。為了保持系統(tǒng)持續(xù)運(yùn)行的精確性和實(shí)時(shí)性，當(dāng)系統(tǒng)輸入新的圖像幀時(shí)，通過本文的邊緣化模型滾動(dòng)窗口中的最后一幀來固定保持窗口大小不變。

在后端進(jìn)行局部?jī)?yōu)化之后，根據(jù)當(dāng)前幀與最后幀之間的視差是否大于設(shè)定閾值，或根據(jù)跟蹤的特征數(shù)量是否低于設(shè)定閾值來判斷當(dāng)前幀是否為關(guān)鍵幀。若確定為關(guān)鍵幀則會(huì)激活回環(huán)線程，根據(jù)文獻(xiàn)［11］的理論所述，利用DBoW詞袋模型來搜索并決定是否已經(jīng)發(fā)生了回環(huán)。當(dāng)檢測(cè)到發(fā)生回環(huán)，系統(tǒng)將進(jìn)行重定位，對(duì)所有位姿進(jìn)行全局優(yōu)化，以達(dá)到更高的精度。

2 前端視覺里程計(jì)

2.1 線特征提取算法

作為一種基于局部梯度方向的算法，EDLines算法生成的是一組干凈的、連續(xù)的像素鏈［23］。本文基于EDLines所改進(jìn)的線特征提取算法流程如圖2所示。

算法的改進(jìn)策略可分為以下兩部分概括。

1）跳躍路由策略

EDLines算法作為一種局部分割檢測(cè)方法，其局限性之一是梯度不連續(xù)會(huì)產(chǎn)生許多小的分割，在如陰影、遮擋等情況下會(huì)產(chǎn)生許多斷裂的短線。在線段繪制的過程中，一條線段可能會(huì)被幾個(gè)邊緣的不連續(xù)所中斷。這些間隔是圖像中梯度方向改變的區(qū)域或者梯度幅度趨于零的區(qū)域。為了實(shí)現(xiàn)相同方向上存在像素?cái)嗔训亩叹€段能夠合并為長(zhǎng)線段，獲得更有效的線段檢測(cè)結(jié)果，提出此策略。

根據(jù)文獻(xiàn)［16］可知，算法在連接錨點(diǎn)的過程中，從任一錨點(diǎn)開始，利用相鄰像素的梯度信息，通過梯度極值連接到下一個(gè)錨點(diǎn)。當(dāng)像素延伸方向有變化時(shí)，仍優(yōu)先按照當(dāng)前方向進(jìn)行延伸，同時(shí)將改變的方向存儲(chǔ)在一個(gè)堆棧中。當(dāng)像素延伸至圖像邊緣時(shí)，回溯堆棧中最后一次改變的方向并再次沿此方向延伸，如此遞歸直到堆棧中存儲(chǔ)的方向都提取完成。算法流程如圖3所示。

在延伸過程中，每當(dāng)檢測(cè)至像素?cái)嗔训奈恢脮r(shí)，即沒有邊緣像素或邊緣方向與擬合線段不對(duì)齊區(qū)域，通過跳躍這段像素間隔的方法來判斷是否需要保持像素當(dāng)前的延伸方向。這一策略的前提有以下必要條件：

a）從錨點(diǎn)a開始，算法能夠沿著邊緣方向繪制至少J像素。將這組J像素稱為擴(kuò)展像素。由于算法使用5×5高斯平滑核降噪，任何1像素的不連續(xù)都會(huì)對(duì)至少5像素大小的鄰域產(chǎn)生影響，因此可將擴(kuò)展像素的初始參數(shù)設(shè)置為J∈［5，7，9］。

b）與線段L對(duì)齊的錨點(diǎn)像素a，距離當(dāng)前像素為J像素，且在圖像內(nèi)部滿足G［a］>0（即不是弱邊緣像素）。

c）Llength>Jpixels，即繪制線段的總長(zhǎng)大于擴(kuò)展像素J。最后，通過計(jì)算像素梯度的自相關(guān)矩陣M來驗(yàn)證擴(kuò)展像素與當(dāng)前線段的方向是否對(duì)齊。M為擴(kuò)展像素鄰域的每側(cè)方向各取1像素并對(duì)其梯度信息進(jìn)行加權(quán)平均計(jì)算得出。若滿足

λ1λ2≥TEigenExt，∠（v1，n）≤TAngleExt（1）

則可證明擴(kuò)展像素沿當(dāng)前線段方向連續(xù)延伸。其中：λ1和λ2（λ1>λ2）是M的特征值；∠（·，·）是角距離；v1是M的第一個(gè)特征向量；n是與線段正交的向量。當(dāng)TEigenExt設(shè)為10，TAngleExt為10°時(shí)，則可認(rèn)為擴(kuò)展像素的延伸方向與線段結(jié)構(gòu)的主導(dǎo)方向足夠一致。

由此，算法不僅保證了像素延伸過程的連續(xù)性，還可解決圖像中存在像素?cái)嗔训膯栴}，從而保證一條有像素?cái)嗔训拈L(zhǎng)線段不會(huì)被判定為多個(gè)短線。該策略做到了在即使有陰影、遮擋等復(fù)雜環(huán)境中，算法也能保證有效性和準(zhǔn)確性。

2）自適應(yīng)閾值策略

由于錨點(diǎn)像素大部分都是邊緣元素，進(jìn)而可形成邊緣像素鏈，完成邊緣特征檢測(cè)。圖像梯度計(jì)算公式如下：

gx（x，y）=I（x+1，y）-I（x，y）+I（x+1，y+1）-I（x，y+1）2（2）

gy（x，y）=I（x，y+1）-I（x，y）+I（x+1，y+1）-I（x+1，y）2（3）

g（x，y）=gx（x，y）2+gy（x，y）2angle（x，y）=arctangx（x，y）-gy（x，y）（4）

其中：I（x，y）表示輸入圖像于像素（x，y）處的強(qiáng)度;g（x，y）為梯度幅值;angle（x，y）為像素點(diǎn)方向的水平線夾角。

EDLines算法在選取錨點(diǎn)前，首先需要對(duì)梯度小于閾值ρ的像素進(jìn)行篩除，但是將閾值設(shè)置為固定參數(shù)會(huì)降低算法在復(fù)雜場(chǎng)景的魯棒性。因此，本文對(duì)EDLines算法的梯度閾值ρ進(jìn)行自適應(yīng)改進(jìn)，通過自適應(yīng)調(diào)整矩陣來適應(yīng)像素點(diǎn)上的水平線、垂直線和斜線的梯度變化，從而使梯度閾值對(duì)不同方向上的梯度變化更敏感，提高邊緣檢測(cè)的準(zhǔn)確性，矩陣如式（5）～（7）所示。

P1x，y=ρx，y+ρx，y+1+ρx，y-1+ρx-1，y+ρx+1，y+z5（5）

P2x，y=ρx，y+ρx+1，y+1+ρx-1，y-1+ρx-1，y+1+ρx+1，y-1+z5（6）

Px，y=min{P1x，y，P2x，y}（7）

其中：ρx，y是點(diǎn)（x，y）的像素梯度;z為噪聲抑制常數(shù)，本文將其設(shè)定為錨點(diǎn)閾值的一半。

最終，選擇兩者中的較小值作為梯度閾值。完成這一步后，算法對(duì)提取的線段數(shù)量進(jìn)行計(jì)數(shù)。若提取的線段數(shù)量低于自適應(yīng)閾值的設(shè)定值，則會(huì)通過降低梯度閾值參數(shù)，再次進(jìn)行線特征提取，以獲得足夠的有效線特征。這種自適應(yīng)閾值提取方法能夠更好地應(yīng)對(duì)如光照變化明顯、不足或弱紋理等環(huán)境情況，提高了線特征提取算法在復(fù)雜條件下的魯棒性。

2.2 線特征描述及匹配

在改進(jìn)的提取線特征的基礎(chǔ)上，通過LBD描述子來對(duì)圖像相鄰幀中對(duì)應(yīng)的線特征進(jìn)行幾何一致性評(píng)估。LBD描述子通過高斯下采樣建立多尺度空間金字塔，對(duì)原始圖像進(jìn)行N次高斯降采樣。尺度金字塔從底層向上分層依次是原始圖像和經(jīng)過高斯模糊算法降低分辨率的圖像。使用本文算法對(duì)高斯分層采樣的圖像中線特征進(jìn)行提取，得到一組線段序列。尺度空間中的所有線特征都有唯一標(biāo)識(shí)，將其進(jìn)行重組后，相關(guān)或相同的線段存儲(chǔ)到同一個(gè)LineVec向量中。如此將整個(gè)金字塔提取完成，得到一組LineVec向量。

LBD描述子對(duì)每條線段進(jìn)行標(biāo)識(shí)，以線段為中心周圍所建立一個(gè)矩陣的線支持域LSR。它以一組有相同寬度的頻帶Bj表示。計(jì)算每個(gè)頻帶的特征向量BDj，所有頻帶的特征向量構(gòu)成初始LBD描述子：（BDT1，BDT2，…，BDTm）T。分別計(jì)算頻帶Bj及其相鄰頻帶Bj-1和Bj+1的局部梯度g′=（g′d⊥，g′dL），然后求和。第K行像素的梯度分為四個(gè)部分，如下：

v1kj=λ∑g′d⊥>0g′d⊥，v2kj=λ∑g′d⊥<0g′d⊥v3kj=λ∑g′dL>0g′dL，v4kj=λ∑g′dL<0g′dL（8）

其中：高斯權(quán)重λ=fg（k）fl（k）。把所有行的梯度匯總，構(gòu)成特征向量BDj對(duì)應(yīng)的特征描述矩陣BDMj如下：

BDMj=v11jv12j…v1njv21jv22j…v2njv31jv32j…v3njv41jv42j…v4nj∈Euclid ExtraaBp4×n，n=2wj=1 or m3welse（9）

計(jì)算出BDMj的均值方差MTj與均值向量STj，得到特征向量BDj=（MTj，STj）T∈Euclid ExtraaBp8。LBD特征為

LBD=（MT1，ST1，MT2，ST2，…，MTm，STm）∈Euclid ExtraaBp8m（10）

得到LBD描述子后，檢查L(zhǎng)ineVecs的一元幾何屬性和局部相似性以消除異常值。最后使用幾何約束算法剔除誤匹配線特征，提高匹配準(zhǔn)確率。

2.3 線特征重投影殘差模型

特征線的重投影誤差可表示為歸一化圖像坐標(biāo)系下線段端點(diǎn)到投影直線的距離。對(duì)于空間線段與其在世界坐標(biāo)系下的正交表示：Euclid Math OneLAp=［n，v］T，通過式（1）將其轉(zhuǎn)換到相機(jī)坐標(biāo)系下后，再投影到成像平面上得到lcil。

l=l1l2l3=Euclid Math OneKApnc=fy000fx0-fycx-fxcyfxfync（11）

則線特征的重投影誤差定義如式（12）所示。

rl（zciLl，Euclid Math OneXAp）=d（scil，lcil）d（ecil，lcil）（12）

其中：d（s，l）計(jì)算方法如式（13），特征線重投影誤差示意如圖4所示。

d（s，l）=sTll21+l22（13）

3 后端優(yōu)化與回環(huán)

3.1 基于滑動(dòng)窗口的非線性優(yōu)化

本文系統(tǒng)為了在減少計(jì)算量的同時(shí)保證優(yōu)化效果，在后端部分采用基于滑動(dòng)窗口的非線性優(yōu)化模型對(duì)位姿進(jìn)行優(yōu)化。定義滑動(dòng)窗口的狀態(tài)變量為

X=［xn，xn+1，…，xn+N，λm，λm+1，…，λm+M，Euclid Math OneOApo，Euclid Math OneOApo+1，…，Euclid Math OneOApo+O］T

xi=［pwbi，qwbi，vwi，bbia，bbig］T，i∈［n，n+N］（14）

其中：xi為攝像頭在滑動(dòng)窗口中的狀態(tài);pwbi為世界坐標(biāo)系下的IMU的位置;qwbi是IMU的方向信息;vwi是IMU的速度;bbia是IMU載體坐標(biāo)系下的加速度計(jì)偏置;bbig是陀螺儀偏置;λk為空間特征點(diǎn)的逆深度;Euclid Math OneOApl是世界坐標(biāo)系下線特征的正交表達(dá)式。式（15）表示為將構(gòu)建的目標(biāo)函數(shù)最小化，以此來優(yōu)化滑動(dòng)窗口內(nèi)的全部狀態(tài)變量：

minEuclid Math OneXApρk（‖rp－JpEuclid Math OneXAp‖2Σp）+∑i∈Bρk（‖rb（zbibi+1，Euclid Math OneXAp）‖2Σbibi+1）+∑（i，j）∈Fρh（‖rf（zcifj，Euclid Math OneXAp）‖2Σcifj）+∑（i，l）∈Lρh（‖rl（zciEuclid Math OneLApi，Euclid Math OneXAp）‖2Σcifi）（15）

其中：rb（zbibi+1，Euclid Math OneXAp）是狀態(tài)xi與xi+1之間的IMU測(cè)量殘差數(shù)據(jù)；B是滑動(dòng)窗口內(nèi)所有IMU預(yù)積分值的集合；rf（zcifj，Euclid Math OneXAp）與rl（zciEuclid Math OneLApi，Euclid Math OneXAp）分別表示點(diǎn)和線特征的重投影殘差；rp和Jp是被邊緣化一幀之后的先驗(yàn)信息；Jp是優(yōu)化后得到的海信矩陣的先驗(yàn)雅可比矩陣；ρk為優(yōu)化后的柯西魯棒核函數(shù)；ρh為Huber核函數(shù)［24］；F和L分別是觀測(cè)到的點(diǎn)、線特征集合。

3.2 邊緣化模型

主流基于優(yōu)化方法的VIO系統(tǒng)都會(huì)使用滑窗與邊緣化的方式減少優(yōu)化參數(shù)，降低系統(tǒng)的整體復(fù)雜度，提高狀態(tài)估計(jì)的效率和精確性。因此，邊緣化的過程即為將聯(lián)合概率分布分解為邊緣概率分布和條件概率分布的過程。本文所提邊緣化模型可以保持后端滑動(dòng)窗口的大小，在滑動(dòng)窗口前，進(jìn)行邊緣化操作，提高系統(tǒng)運(yùn)行的實(shí)時(shí)性和魯棒性。通過邊緣化去除掉滑動(dòng)窗口內(nèi)的最舊幀或者次新幀，在以約束項(xiàng)的形式保留此幀和窗口內(nèi)其余幀的約束關(guān)系的同時(shí)，不再冗余計(jì)算該幀的位姿與相關(guān)路標(biāo)點(diǎn)，以達(dá)到對(duì)狀態(tài)變量的優(yōu)化效果。這樣可以應(yīng)對(duì)當(dāng)移動(dòng)機(jī)器人進(jìn)行退化運(yùn)動(dòng)時(shí)，既擁有相關(guān)的歷史約束信息求解，又可達(dá)到全局約束優(yōu)化的目標(biāo)。本文的邊緣化模型將對(duì)系統(tǒng)中的每一圖像幀都進(jìn)行優(yōu)化以確保系統(tǒng)的精度和性能。

將邊緣化的狀態(tài)變量設(shè)為Euclid Math OneXApa，保留變量設(shè)為Euclid Math OneXApb，可以將HδX=b表示為

ΛaΛbΛTbΛcδEuclid Math OneXApaδEuclid Math OneXApb=babb（16）

利用Schur補(bǔ)方法推導(dǎo)出式（17），即可計(jì)算得出變量δEuclid Math OneXApb的先驗(yàn)信息。其中ΛTbΛ-1aΛb為Λa在Λb中的Schur項(xiàng)。則被保留下來的變量δEuclid Math OneXApb的先驗(yàn)信息如式（18）所示。

ΛaΛb0Λc-ΛTbΛ-1aΛbδEuclid Math OneXApaδEuclid Math OneXApb=babb-ΛTbΛ-1aba（17）

（Λc-ΛTbΛ-1aΛb）δEuclid Math OneXApb=bb-ΛTbΛ-1aba（18）

在邊緣化過程中，相機(jī)和路標(biāo)點(diǎn)之間對(duì)應(yīng)的關(guān)系模型如圖5所示。當(dāng)Xp1被邊緣化后，此時(shí)被邊緣化的變量式為δEuclid Math OneXApa=δx6×1P1。在此之后，Xm1、Xm2和Xm3之間將建立起新的約束，Xm1與Xp2也將建立起新約束。此時(shí)H矩陣對(duì)應(yīng)于Λc-ΛTbΛ-1aΛb。接下來以相似的過程繼續(xù)邊緣化Xm1。

3.3 回環(huán)檢測(cè)和全局優(yōu)化

回環(huán)檢測(cè)與全局優(yōu)化是為了系統(tǒng)能為了得到全局一致的位姿估計(jì)。回環(huán)檢測(cè)能夠給除了相鄰圖像幀以外的更加久遠(yuǎn)的約束，它提供了當(dāng)前數(shù)據(jù)與所有歷史數(shù)據(jù)的關(guān)聯(lián)，能夠提高系統(tǒng)長(zhǎng)時(shí)間運(yùn)行的精確性和魯棒性。

本文對(duì)完成后端優(yōu)化后的關(guān)鍵幀進(jìn)行回環(huán)檢測(cè)：首先從圖像上提取一些的新的角點(diǎn)進(jìn)行描述；接著從關(guān)鍵幀數(shù)據(jù)庫中檢測(cè)當(dāng)前幀對(duì)應(yīng)的閉環(huán)幀，在檢測(cè)到閉環(huán)后，當(dāng)前幀會(huì)產(chǎn)生一個(gè)閉環(huán)約束，將此約束項(xiàng)構(gòu)建到目標(biāo)函數(shù)中，非線性優(yōu)化求解得到相對(duì)位姿關(guān)系；然后對(duì)滑窗內(nèi)的所有幀進(jìn)行調(diào)整，完成快速重定位。在重定位之后，閉環(huán)優(yōu)化使得過去的位姿信息能夠被加載到全局地圖中。

4 實(shí)驗(yàn)與評(píng)估

本文實(shí)驗(yàn)平臺(tái)為64位Linux操作系統(tǒng)，CPU為Intel Core i5-8300H；核心數(shù)為4，內(nèi)存為8 GB。為驗(yàn)證改進(jìn)算法以及系統(tǒng)的有效性，實(shí)驗(yàn)分為兩部分。首先將本文改進(jìn)的線特征提取匹配算法與LSD算法和EDLines算法進(jìn)行對(duì)比來評(píng)估其性能。然后在公開數(shù)據(jù)集上評(píng)估本文算法和VINS-Mono、ORB-SLAM3、PL-VIO以及PL-VINS算法的性能。其中，VINS-MONO和ORB-SLAM3是目前最先進(jìn)的視覺慣性算法，在前端僅使用點(diǎn)特征進(jìn)行跟蹤。PL-VIO算法是一種基于點(diǎn)-線特征的視覺慣性里程計(jì)，其前端部分是利用改進(jìn)的LSD算法提取線特征，能夠達(dá)到很好的效果。而PL-VINS算法是在VINS-mono和PL-VIO的基礎(chǔ)上改進(jìn)的優(yōu)秀視慣SLAM算法，在前端利用改進(jìn)的LSD線特征提取算法，提升了系統(tǒng)實(shí)時(shí)性和定位精度。通過與這兩種算法的比較，不僅證明了本文線特征對(duì)全局軌跡估計(jì)結(jié)果的性能提升，也證明了本文所提出的單目點(diǎn)線視慣SLAM算法的精確性和魯棒性。

4.1 線特征提取實(shí)驗(yàn)

首先在EuRoC數(shù)據(jù)集中的MH_04_difficult和MH_05_difficult序列上實(shí)驗(yàn)本文改進(jìn)的線特征提取算法。該序列MAV運(yùn)動(dòng)快速且場(chǎng)景黑暗，紋理效果也有好有壞，能夠很好地檢測(cè)線段提取算法的有效性和魯棒性。由圖6、7可知，在場(chǎng)景黑暗，紋理不足的環(huán)境下，本文算法與LSD算法相比提取的特征線數(shù)目基本相當(dāng)，比EDLines算法提取數(shù)目多，而提取線段的平均長(zhǎng)度（平均線長(zhǎng)=線段長(zhǎng)度總和/線段總數(shù)目）比這兩種算法都要大，這說明本文算法在光照和紋理不足的場(chǎng)景中仍能提取到足夠數(shù)量的有效長(zhǎng)線段用于約束場(chǎng)景結(jié)構(gòu)，從而進(jìn)行更好的位姿估計(jì)。這對(duì)系統(tǒng)的魯棒性有很大提升。

本節(jié)實(shí)驗(yàn)結(jié)果證明，本文算法在提取出的線特征數(shù)量上以及線特征的長(zhǎng)度上均有所進(jìn)步，實(shí)驗(yàn)對(duì)比如圖8、9所示。因此，本文的線特征提取算法在黑暗、弱紋理的復(fù)雜環(huán)境中所提取的線段質(zhì)量?jī)?yōu)于傳統(tǒng)的線段提取算法，對(duì)系統(tǒng)的后端優(yōu)化以及數(shù)據(jù)融合均有所幫助。

4.2 系統(tǒng)精度評(píng)估

在EuRoC公開數(shù)據(jù)集上進(jìn)行測(cè)試，驗(yàn)證本文系統(tǒng)的有效性和精度。數(shù)據(jù)集序列提供了軌跡的真實(shí)值用以量化軌跡誤差，且根據(jù)微型飛行器和手持相機(jī)移動(dòng)的復(fù)雜程度以及環(huán)境光照、紋理等條件，序列分為簡(jiǎn)單、中等和困難三個(gè)等級(jí)。本節(jié)實(shí)驗(yàn)所選序列對(duì)于系統(tǒng)的前端特征的提取和后端優(yōu)化模塊的魯棒性都具有很大的考驗(yàn)。尤其在光照變化劇烈或紋理信息差的場(chǎng)景中，線特征的有效提取將會(huì)給系統(tǒng)的精度提升提供較大幫助。表1給出了以APE（absolute pose error）為指標(biāo)進(jìn)行定量分析的結(jié)果，并將不同SLAM系統(tǒng)進(jìn)行對(duì)比，可以直觀地展示三種算法的軌跡精度。

由表1可知，在這些算法中，本文算法均保持了最好的性能。與VINS-mono和ORB-SLAM3相比，本文算法的RMSE在所選的MH序列上精度分別平均提升了47.8%和37.7%，在VR序列上與PL-VINS性能較為相近。因?yàn)镸H序列是飛行器在光照條件較差且低紋理場(chǎng)景中采集，故證明了改進(jìn)的線特征提取匹配算法在此類復(fù)雜場(chǎng)景中的有效性。VR序列為手持相機(jī)在室內(nèi)采集，點(diǎn)、線特征較繁雜，且含有大量的無規(guī)則高速晃動(dòng)，證明了本文系統(tǒng)的精度和魯棒性都有所提升。從表中不難得出結(jié)論，本文算法對(duì)于這些場(chǎng)景精度都很高，在所選VR序列中，比同樣基于點(diǎn)線特征的PL-VINS平均精度提升了259%，表現(xiàn)更好，魯棒性更高。

圖10更為直觀地顯示了所選序列的地真軌跡（虛線）和本文算法獲得的軌跡（實(shí)線）的對(duì)比。它們分別是MH_03_medium、MH_05_difficult、V1_02_medium和V2_03_difficult的絕對(duì)軌跡誤差結(jié)果。圖中誤差大小的變化情況由小到大，以深藍(lán)色到紅色的漸變色帶為代表。通過結(jié)果圖可以看出，本文算法的穩(wěn)定性較強(qiáng)，整體軌跡誤差持續(xù)控制在較低范圍。且當(dāng)相機(jī)傳感器運(yùn)動(dòng)方向突然加速或轉(zhuǎn)向，或光線變化較大區(qū)域時(shí)，系統(tǒng)誤差相對(duì)來說會(huì)增大，但仍能穩(wěn)定控制在較小范圍。系統(tǒng)整體可以較好地完成位姿精準(zhǔn)估計(jì)的任務(wù)。

如圖11所示，以V2_02_medium序列的軌跡結(jié)果為例可以看出，本文算法的估計(jì)軌跡與真實(shí)軌跡的貼合度在幾個(gè)算法中最好。在相機(jī)傳感器遭遇大幅度快速變向，運(yùn)動(dòng)模糊以至紋理效果很差的情況時(shí)，也能較好地貼合真實(shí)軌跡。對(duì)比來說VINS-mono與PL-VINS算法與真實(shí)軌跡的整體貼合程度不如本文算法。

圖12為不同算法分別在x、y和z軸的軌跡誤差對(duì)比以及x、y、z軸的細(xì)節(jié)?？梢钥闯觯m然整體的軌跡誤差幾種算法的效果都相差較小，但是本文算法是誤差控制以及與軌跡真值的貼合度是最好的，且能夠做到在算法初始化階段就將軌跡誤差控制在最低范圍并持續(xù)平穩(wěn)保持直到系統(tǒng)運(yùn)行結(jié)束。

圖13和14展示了系統(tǒng)運(yùn)行過程中的實(shí)時(shí)APE和絕對(duì)位姿誤差分布范圍情況。可以看出，本文算法的精度整體上優(yōu)于兩種對(duì)比算法。在系統(tǒng)運(yùn)行的整個(gè)過程中，本文算法比其余兩種對(duì)比算法的誤差控制更低，分布更為集中，波動(dòng)更小。VINS-mono由于僅使用特征點(diǎn)而造成在運(yùn)動(dòng)模糊、紋理低且光照變化大的復(fù)雜環(huán)境中時(shí)精度會(huì)產(chǎn)生較大波動(dòng)。而PL-VINS和本文算法添加了線特征，因此整體波動(dòng)穩(wěn)定且更低。得益于本文的改進(jìn)的線特征提取匹配算法，所提取的線特征效果更好，特征有效性更強(qiáng)。由實(shí)驗(yàn)結(jié)果可知，由于本文的后端非線性優(yōu)化模塊和邊緣化模型，本文算法比PL-VINS的誤差控制效果更加穩(wěn)定，精度更高。系統(tǒng)運(yùn)行的穩(wěn)定性相對(duì)于兩種對(duì)比算法而言有了較大的提升，做到了精度和魯棒性的同步提高。

5 結(jié)束語

本文提出了一種基于VINS-mono所改進(jìn)的，融合IMU數(shù)據(jù)和點(diǎn)、線特征的視覺慣性實(shí)時(shí)SLAM算法。得益于改進(jìn)線特征提取算法以及邊緣化模型，本文算法達(dá)到了更優(yōu)秀的系統(tǒng)位姿優(yōu)化效果，提高了SLAM算法整體的精度和魯棒性。在系統(tǒng)前端，本文采用基于LBD描述子的改進(jìn)的EDLines線特征匹配算法，通過引入跳躍路由策略和自適應(yīng)閾值策略提高了線特征提取的質(zhì)量與精確性，為系統(tǒng)提供了更有效的圖像特征，提高了后端優(yōu)化的精度和魯棒性。通過公開數(shù)據(jù)集表明，在光照條件差、弱紋理的復(fù)雜環(huán)境下，本文算法與VINS-mono和PL-VINS等SLAM算法相比，定位精度更高，魯棒性更強(qiáng)。在后續(xù)的工作中，計(jì)劃進(jìn)一步簡(jiǎn)化線特征提取和匹配算法以達(dá)到更好的效果，繼續(xù)優(yōu)化后端模塊，提高算法的性能，并引入特征加權(quán)策略以達(dá)到更好的后端優(yōu)化效果，進(jìn)而將其應(yīng)用到機(jī)器人路徑規(guī)劃與導(dǎo)航中。

參考文獻(xiàn)：

［1］Andrew J D，Ian D R，Nicholas D M，et al.MonoSLAM：real-time single camera SLAM［J］.IEEE Trans on Pattern Analysis and Machine Intelligence，2007，29（6）：1052-1067.

［2］Klein G，Murray D.Parallel tracking and mapping for small AR workspaces［C］//Proc of the 6th IEEE and ACM International Symposium on Mixed and Augmented Reality.Piscataway，NJ：IEEE Press，2007：225-234.

［3］Forster C，Zhang Zichao，Gassner M，et al.SVO：semidirect visual odometry for monocular and multicamera systems［J］.IEEE Trans on Robotics，2017，33（2）：249-265.

［4］Lowe D G.Distinctive image features from scale-invariant keypoints［J］.International Journal of Computer Vision，2004，60（2）：91-110.

［5］Bay H，Tuytelaars T，Van Gool L.SURF：speeded up robust features［J］.Computer Vision and Image Understanding，2008，110（3）：346-359.

［6］Harris C，Stephens M.A combined corner and edge detector［C］//Proc of the 4th Alvey Vision Conference.Piscataway，NJ：IEEE Press，1988：147-152.

［7］Rublee E，Rabaud V，Konolige K，et al.ORB：an efficient alternative to SIFT or SURF［C］//Proc of International Conference on Computer Vision.Piscataway，NJ：IEEE Press，2011：2564-2571.

［8］Mur-Artal R，Tardós J D.ORB-SLAM2：an open-source SLAM system for monocular，stereo and RGB-D cameras［J］.IEEE Trans on Robotics，2017，33（5）：1255-1262.

［9］Engel J，Koltun V，Cremers D.Direct sparse odometry［J］.IEEE Trans on Pattern Analysis and Machine Intelligence，2018，40（3）：611-625.

［10］Carlos C，Richard E，Juan J G R，et al.ORB-SLAM3：an accurate open-source library for visual，visual-Inertial，and multimap SLAM［J］.IEEE Trans on Robotics，2021，37（6）：1874-1890.

［11］Qin Tong，Li Peiliang，Shen Shaojie.VINS-mono：a robust and versatile monocular visual-inertial state estimator［J］.IEEE Trans on Robotics，2018，34（4）：1004-1020.

［12］Pumarola A，Vakhitov A，Agudo A，et al.PL-SLAM：real-time mono-cular visual SLAM with points and lines［C］//Proc of IEEE International Conference on Robotics and Automation.Piscataway，NJ：IEEE Press，2017：4503-4508.

［13］Gioi R G V，Jakubowicz J，Morel J W，et al.LSD：a fast line segment detector with a false detection control［J］.IEE Trans on Pattern Analysis and Machine Intelligence，2010，32（4）：722-732.

［14］He Yijia，Zhao Ji，Guo Yue，et al.PL-VIO：tightly-coupled monocular visual-inertial odometry using point and line features［J］.Sensors，2018，18（4）：1159.

［15］Fu Qiang，Wang Jialong，Yu Hongshan，et al.PL-VINS：real-time monocular visual-inertial slam with point and line features［EB/OL］.（2022-04-15）.https：//arxiv.org/abs/2009.07462.

［16］趙偉博，田軍委，王沁，等.基于點(diǎn)線特征融合的視覺慣性SLAM算法［J］.計(jì)算機(jī)應(yīng)用研究，2023，40（2）：445-449.（Zhao Weibo，Tian Junwei，Wang Qin，et al.Visual inertial SLAM algorithm based on point-line feature fusion［J］.Application Research of Compu-ters，2023，40（2）：445-449.）

［17］Clark R，Wang Sen，Wen Hongkai，et al.VINET：visual-inertial odo-metry as a sequence-to-sequence learning problem［C］//Proc of AAAI Conference on Artificial Intelligence.Palo Alto，CA：AAAI Press，2017：3995-4001.

［18］Zuo Xing Xing，Merrill N，Li Wei，et al.CodeVIO：visual-inertial odometry with learned optimizable dense depth［C］//Proc of IEEE International Conference on Robotics and Automation.Piscataway，NJ：IEEE Press，2021：14382-14388.

［19］Cuneyt A，Cihan T.EDLines：a real-time line segment detector with a false detection control［J］.Pattern Recognition Letters，2011，23（13）：1633-1642.

［20］Zhang Lilian，Koch R.An efficient and robust line segment matching approach based on LBD descriptor and pairwise geometric consistency［J］.Journal of Visual Communication and Image Representation，2013，24（7）：794-805.

［21］Elaziz M A，Heidari A A，F(xiàn)ujita H，et al.A competitive chain-based Harris hawks optimizer for global optimization and multi-level image thresholding problems［J］.Applied Soft Computing，2020，95：106347.

［22］Bartoli A，Sturm P.The 3D line motion matrix and alignment of line reconstructions［J］.International Journal of Computer Vision，2004，57（3）：159-178.

［23］付煜，鄭爽，別桐，等.融合點(diǎn)線特征的視覺慣性SLAM算法［J］.計(jì)算機(jī)應(yīng)用研究，2022，39（2）：349-355.（Fu Yu，Zheng Shuang，Bie Tong，et al.Visual inertial SLAM algorithm fusing point and line feature［J］.Application Research of Computers，2022，39（2）：349-355.）

［24］Rainer K，Giorgio G，Hauke S，et al.g2o：a general framework for graph optimization［C］//Proc of IEEE International Conference on Robotics and Automation.Piscataway，NJ：IEEE Press，2011：3607-3613.