摘要：針對(duì)目前單目視覺SLAM易受光照、環(huán)境、紋理影響等問題，文章提出一種基于點(diǎn)線特征的單目視覺慣性SLAM（Simultaneous Localization and Mapping）方法。首先，在視覺慣性系統(tǒng)VINS-Mono的基礎(chǔ)上，提取EDLines線特征，并采用LBD描述子匹配線特征；其次，在線特征擬合階段，采用RANSAC方法剔除外點(diǎn)，提高擬合精度；最后，根據(jù)所提取的點(diǎn)線特征數(shù)量，提出特征篩選策略，保證系統(tǒng)的實(shí)時(shí)性和定位精度。通過在數(shù)據(jù)集上的實(shí)驗(yàn)分析表明，該算法相較VINS-Mono算法的魯棒性及在定位精度上均有一定的提高，其在實(shí)際應(yīng)用中具有廣闊的潛力。

關(guān)鍵詞：?jiǎn)文恳曈X慣性；點(diǎn)線特征；特征篩選

中圖分類號(hào)：TP391.41 """""""文獻(xiàn)標(biāo)識(shí)碼：A"""""""""文章編號(hào)：1672-4437（2024）04-0048-06

0 引言

移動(dòng)機(jī)器人技術(shù)近年來備受關(guān)注，其應(yīng)用場(chǎng)景也逐漸復(fù)雜多樣。在人工智能興起之后，移動(dòng)機(jī)器人逐漸向自主化和智能化進(jìn)步。面對(duì)復(fù)雜環(huán)境，移動(dòng)機(jī)器人如何根據(jù)搭載的傳感器獲取周圍環(huán)境信息實(shí)現(xiàn)準(zhǔn)確定位是該領(lǐng)域的核心問題，其定位的精度、實(shí)時(shí)性及穩(wěn)定性均會(huì)對(duì)移動(dòng)機(jī)器人應(yīng)用產(chǎn)生重要影響。

同步定位與建圖技術(shù)的發(fā)展可以追溯到1986年，當(dāng)時(shí)在無人機(jī)、自主車輛和機(jī)器人等領(lǐng)域出現(xiàn)了對(duì)自主導(dǎo)航技術(shù)的迫切需求。Smith等^[1]首次提出了空間不確定性估計(jì)的概念，為后續(xù)SLAM技術(shù)的研究奠定了基礎(chǔ)。同步定位與建圖^[2]方法根據(jù)所使用傳感器種類的不同，可以將SLAM技術(shù)分為視覺SLAM和激光SLAM。其中視覺慣性SLAM因價(jià)格便宜、定位效果好備受社會(huì)關(guān)注：Leutenegger等于2015年提出了OKVIS方法^[3]，利用Harris角點(diǎn)^[4]和BRISK描述子^[5]進(jìn)行前端匹配跟蹤，后端采用滑動(dòng)窗口進(jìn)行優(yōu)化，同時(shí)處理視覺和IMU信息。為了降低計(jì)算量和消除重復(fù)積分，F(xiàn)oster等在2016年提出了IMU預(yù)積分理論^[6]，融入SVO框架^[7]，實(shí)現(xiàn)視覺和慣性信息緊密耦合。2017年，Mur-Artal等人結(jié)合ORB-SLAM^[8]和IMU預(yù)積分理論提出了VIORB-SLAM^[9]，通過閉環(huán)檢測(cè)減少累積誤差。同年，香港科技大學(xué)Shen S J團(tuán)隊(duì)提出了VINS-Mono^[10]，基于滑動(dòng)窗口優(yōu)化的單目視覺慣性SLAM系統(tǒng)，通過視覺慣性對(duì)齊進(jìn)行初始化和回環(huán)檢測(cè)，優(yōu)化全局位姿圖消除整體漂移，在室內(nèi)和室外場(chǎng)景驗(yàn)證了高定位精度。2019年，該團(tuán)隊(duì)基于VINS-Mono提出了VINS-Fusion^[11]，將VINS-Mono拓展為雙目與IMU、GPS融合。

移動(dòng)機(jī)器人能在正常環(huán)境中提取特征點(diǎn)并進(jìn)行跟蹤，確定自身定位，但其在某些復(fù)雜環(huán)境下，如光照較弱的弱紋理場(chǎng)所，往往提取不到足夠的特征點(diǎn)，因此僅僅依靠點(diǎn)特征可能無法滿足移動(dòng)機(jī)器人的定位需求，進(jìn)而導(dǎo)致定位和建圖的準(zhǔn)確性下降甚至系統(tǒng)無法運(yùn)行。

針對(duì)上述問題，本研究基于VINS-Mono系統(tǒng)框架，在視覺前端部分加入EDLines線特征^[12]，并在線段擬合階段改用RANSAC方法剔除外點(diǎn)，然后利用LBD描述子進(jìn)行線特征匹配；對(duì)獲取到的點(diǎn)線特征采取特征篩選策略，設(shè)定數(shù)量閾值，解決復(fù)雜環(huán)境下特征提取不足的問題。根據(jù)上述改進(jìn)方法，構(gòu)建基于點(diǎn)線特征的視覺慣性SLAM系統(tǒng)。實(shí)驗(yàn)表明，該方法能有效提高系統(tǒng)定位精度。

1 系統(tǒng)框架

系統(tǒng)采用點(diǎn)線融合和非線性優(yōu)化方法，實(shí)現(xiàn)移動(dòng)機(jī)器人的自身定位，系統(tǒng)框架如圖1所示。首先，在圖像幀中提取和跟蹤點(diǎn)線特征，并將提取到的特征進(jìn)行篩選；其次，獲取IMU數(shù)據(jù)進(jìn)行IMU預(yù)積分，通過初始化獲取關(guān)鍵參數(shù)；最后，利用點(diǎn)線視覺重投影殘差和IMU預(yù)積分殘差構(gòu)建非線性優(yōu)化函數(shù)，通過最小化殘差獲取精確的自身定位信息。

2 視覺慣性系統(tǒng)模型

2.1 單目視覺模型

單目相機(jī)所使用的模型即針孔相機(jī)模型，如圖2所示：為相機(jī)坐標(biāo)系，根據(jù)右手定則，O為相機(jī)的光心即針孔模型中的針孔、軸指向相機(jī)前方、軸向右、軸向下；為物理成像平面坐標(biāo)系；點(diǎn)和點(diǎn)為對(duì)應(yīng)的空間點(diǎn)和成像點(diǎn)；?為焦距即物理成像平面到光心的距離；為世界坐標(biāo)系。

圖2 針孔相機(jī)模型

隨后將點(diǎn)從物理成像平面轉(zhuǎn)到像素平面：像素坐標(biāo)系原點(diǎn)位于圖像左上角，軸向右與軸平行，軸與軸平行，即像素坐標(biāo)系與成像平面之間相差了一個(gè)縮放和原點(diǎn)得平移。假設(shè)像素坐標(biāo)在軸上縮放了倍，在軸上縮放了倍，原點(diǎn)平移了。那么，的坐標(biāo)與像素坐標(biāo)的關(guān)系為：

將其寫成齊次坐標(biāo)并換成矩陣形式：

其中把由參數(shù)組成的矩陣稱為相機(jī)的內(nèi)參數(shù)矩陣；為外參矩陣，由相機(jī)系到世界系之間的旋轉(zhuǎn)和位移組成。

2.2 慣性模型

IMU作為慣性傳感器，通過加速度計(jì)和陀螺儀測(cè)得線性加速度和旋轉(zhuǎn)角速度，以及在測(cè)量時(shí)產(chǎn)生的隨機(jī)游走誤差和白噪聲，具體模型如下：

其中為IMU坐標(biāo)系到世界坐標(biāo)系的轉(zhuǎn)換，表示從IMU坐標(biāo)系到世界坐標(biāo)系的旋轉(zhuǎn)，為隨時(shí)間緩慢變化的隨機(jī)游走噪聲，為服從高斯分布的白噪聲，為重力加速度在世界坐標(biāo)系下的表示。且該模型假設(shè)運(yùn)行區(qū)域是個(gè)平面，不考慮地球自轉(zhuǎn)，的方向固定且大小固定，即認(rèn)為世界坐標(biāo)系W是慣性系。

根據(jù)運(yùn)動(dòng)學(xué)方程可得到從時(shí)間到時(shí)間的狀態(tài)：

其中為位移、為速度、為旋轉(zhuǎn)量，噪聲項(xiàng)為的離散形式。

由于VINS采用了BA優(yōu)化^[13]的思想，每次優(yōu)化都會(huì)導(dǎo)致IMU重新積分，這一過程會(huì)增加系統(tǒng)的計(jì)算時(shí)間，進(jìn)而推出IMU預(yù)積分理論。利用IMU預(yù)積分公式，分離出測(cè)量值中的噪聲項(xiàng)，根據(jù)測(cè)量值和真實(shí)值計(jì)算出IMU預(yù)積分殘差，構(gòu)建IMU約束并優(yōu)化狀態(tài)量。

2.3 點(diǎn)特征參數(shù)化

環(huán)境中的路標(biāo)點(diǎn)一般是基于世界坐標(biāo)系使用三個(gè)參數(shù)進(jìn)行參數(shù)化，但這種參數(shù)化方式在后端優(yōu)化中增加了計(jì)算量，并且隨著特征點(diǎn)的深度增加，優(yōu)化的誤差也會(huì)更大。因此本研究采用逆深度對(duì)3D點(diǎn)進(jìn)行參數(shù)化，即使用一個(gè)參數(shù)實(shí)現(xiàn)點(diǎn)特征的參數(shù)表示。

當(dāng)相機(jī)得到一幀圖像后，可以得到特征點(diǎn)在相機(jī)坐標(biāo)系下的坐標(biāo)為、相機(jī)內(nèi)參矩陣為，則可以得到特征點(diǎn)的齊次像素坐標(biāo)：

其中為特征點(diǎn)的逆深度、相機(jī)坐標(biāo)可通過觀測(cè)得到。通過式（5）即可用一個(gè)參數(shù)表示一個(gè)路標(biāo)點(diǎn)，將三個(gè)優(yōu)化變量減少為一個(gè)優(yōu)化變量，并且使用逆深度也可以減小在深度過大時(shí)的誤差。

3 視覺慣性系統(tǒng)模型的改進(jìn)

3.1 EDLines線特征的提取及優(yōu)化

在弱紋理環(huán)境下，單純依靠點(diǎn)特征容易導(dǎo)致定位失敗，因此本系統(tǒng)在視覺前端融入線特征，以點(diǎn)線融合的方式保證在復(fù)雜環(huán)境下的穩(wěn)定定位。本研究使用EDLines算法提取圖像中的線特征，并對(duì)其進(jìn)行改進(jìn)。

EDLines是一種無參數(shù)的線段檢測(cè)器，具備高效、快速特點(diǎn)。其原理如下：首先，對(duì)輸入的灰度圖像進(jìn)行濾波處理，得到平滑的圖像；然后，計(jì)算圖像中每個(gè)像素的梯度方向和梯度值；接著，根據(jù)梯度圖中梯度值的峰值點(diǎn)確定錨點(diǎn)；最后，通過連接這些錨點(diǎn)，并將它們繪制成線段，完成邊緣的提取。

定義線特征提取公式為：

其中A為一條線段，n為A的線段長(zhǎng)度，k為A上與A方向一致的點(diǎn)的數(shù)目，為N×N圖像幀中所有可能的線段數(shù)量，方向一致定義為線段方向與像素梯度方向處于22.5°之內(nèi)，即p取1/8。若NFAlt;1，則保留線段，否則丟棄。此方法同時(shí)能篩選掉短線段。

在線段擬合階段，EDLines算法采用的是最小二乘法^[14]，該方法利用局內(nèi)和局外所有點(diǎn)計(jì)算模型。由于提取的錨點(diǎn)存在較多局外點(diǎn)，進(jìn)而會(huì)影響直線擬合的準(zhǔn)確性，所以本研究采用RANSAC^[15]算法，僅利用局內(nèi)點(diǎn)的數(shù)據(jù)擬合線段，如圖3所示，擬合出的直線精確度更高。

將本系統(tǒng)在EuRoC數(shù)據(jù)集中運(yùn)行，可得到如圖4所示的效果圖。其中（a）采用最小二乘法擬合圖中線特征，（b）采用RANSAC法擬合圖中線特征，可以看出，改進(jìn)后的EDLines算法剔除了較多的不穩(wěn)定線段，保留了較長(zhǎng)的穩(wěn)定線段。

3.2 特征篩選策略

由于系統(tǒng)加入了線特征，增加了特征提取的計(jì)算量，為了減少引入線特征后增加的計(jì)算量，本研究提出一種針對(duì)點(diǎn)特征和線特征的數(shù)據(jù)選擇策略。該策略通過設(shè)置線特征數(shù)量的閾值，將點(diǎn)特征作為主要參考，而線特征作為輔助。設(shè)定閾值時(shí)，考慮系統(tǒng)的實(shí)時(shí)性能和定位精度之間的平衡。較高的線特征閾值雖然可以提高特征匹配的準(zhǔn)確性，但會(huì)增加計(jì)算復(fù)雜度，降低實(shí)時(shí)性；而較低的線特征閾值則相反。通過動(dòng)態(tài)調(diào)節(jié)閾值數(shù)量，適應(yīng)不同環(huán)境下的特征數(shù)量變化。例如，在點(diǎn)特征豐富的場(chǎng)景中，減少線特征閾值以降低計(jì)算復(fù)雜度；在點(diǎn)特征稀少的環(huán)境中，提高線特征閾值以確保足夠的特征用于匹配。

將本系統(tǒng)在COCO數(shù)據(jù)集多個(gè)場(chǎng)景（如室內(nèi)、室外、特征豐富、特征稀少等）中進(jìn)行多次實(shí)驗(yàn)，在不同點(diǎn)特征數(shù)量下選取不同線特征閾值，系統(tǒng)地調(diào)整閾值并記錄實(shí)時(shí)性能與特征數(shù)量，尋找最優(yōu)閾值范圍（如圖5）。

最終對(duì)比不同數(shù)據(jù)集序列的特征處理時(shí)間與特征數(shù)量，得出特征篩選策略（見表1），根據(jù)圖像幀中提取出的點(diǎn)特征數(shù)目來決定后續(xù)提取的線特征數(shù)量。

如圖6所示，（a）（b）是點(diǎn)特征較多的場(chǎng)景，其中（a）圖提取了較多線特征，會(huì)影響系統(tǒng)實(shí)時(shí)性，因此采用特征篩選策略減少圖中線特征的數(shù)量，改進(jìn)后如圖（b）所示；（c）（d）是點(diǎn)特征較少的弱紋理場(chǎng)景，其中（c）圖提取了較少的點(diǎn)特征，會(huì)影響系統(tǒng)精度，因此采用特征篩選策略增加圖中線特征的數(shù)量，改進(jìn)后如圖（d）所示。

根據(jù)圖6可統(tǒng)計(jì)出具體的特征提取數(shù)量，如表2所示，紋理較強(qiáng)的場(chǎng)景中，線特征的提取數(shù)量減少了一倍多；紋理較弱的場(chǎng)景中，線特征的提取數(shù)量增長(zhǎng)了一倍多。由此可以看出，采用該特征篩選策略，能盡量均衡系統(tǒng)提取到的特征，進(jìn)而保證實(shí)時(shí)性和定位精度。

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

為驗(yàn)證本算法的精度，采用公開的EuRoC數(shù)據(jù)集^[16]進(jìn)行相關(guān)性能分析。EuRoC數(shù)據(jù)集包括在微型飛行器上收集到的11個(gè)飛行序列數(shù)據(jù)。該數(shù)據(jù)集的圖像采集頻率為20Hz，IMU的采集頻率為200Hz，并且該數(shù)據(jù)集提供真實(shí)軌跡。

本研究采用EuRoC數(shù)據(jù)集中Machine Hall場(chǎng)景下的MH_01_easy、MH_03_medium、MH_05_difficult來驗(yàn)證算法的定位精度，采用無人車實(shí)驗(yàn)平臺(tái)記錄的序列驗(yàn)證算法的定位精度。

為測(cè)試本研究的定位精度，將系統(tǒng)與經(jīng)典算法VINS-Mono進(jìn)行對(duì)比，在EuRoC數(shù)據(jù)集上測(cè)試兩者的絕對(duì)軌跡誤差（Absolute Trajectory Error，ATE）。所選取的三個(gè)序列場(chǎng)景從簡(jiǎn)單到困難，周圍環(huán)境的紋理依次變?nèi)酰茌^合適地驗(yàn)證本算法改進(jìn)視覺特征提取的有效性。圖7為在三個(gè)序列下VINS-Mono與本研究算法的軌跡誤差對(duì)比，其中（a）（b）、（c）（d）、（e）（f）分別為VINS-Mono算法和本研究算法在EuRoC數(shù)據(jù)集MH_01_easy、MH_03_medium和MH_05_difficult序列下的絕對(duì)軌跡誤差，可以看出本研究算法的定位誤差要低于VINS-Mono算法（見表3）。從表3可以看出，與VINS-Mono算法相比，本研究算法的絕對(duì)軌跡誤差均有不同程度的減少，其中均方根誤差（RMSE）平均減少了約35%。

5 結(jié)論

本研究提出了一種基于點(diǎn)線特征融合的單目視覺慣性SLAM方法，該方法結(jié)合了點(diǎn)線視覺特征，通過改進(jìn)線特征提取方式以及特征篩選策略，提高了移動(dòng)機(jī)器人的定位精度。其中在所選EuRoC數(shù)據(jù)集中均方根誤差平均減少了35%。

然而，這種方法仍然存在一些局限性。當(dāng)周圍環(huán)境存在較多運(yùn)動(dòng)物體時(shí)，由于提取了較多的點(diǎn)線特征，將這些特征用于移動(dòng)機(jī)器人定位，反而會(huì)降低定位精度。因此，如何在高動(dòng)態(tài)環(huán)境下，保證移動(dòng)機(jī)器人高精度的自主定位是下一步要開展的工作。

參考文獻(xiàn)：

[1]SMITH R C， CHEESEMAN P.On the representation and estimation of spatial uncertainty[J].The international journal of Robotics Research，1986， 5（4）：56-68.

[2]田野，陳宏巍，王法勝，等.室內(nèi)移動(dòng)機(jī)器人的SLAM算法綜述[J].計(jì)算機(jī)科學(xué)，2021，48（09）：223-234.

[3]LEUTENEGGER S， LYNEN S， BOSSE M， et al. Keyframe-based visual-inertial odometry using nonlinear optimization[J].International Journal of Robotics Research，2015，34（3）：314334.

[4]GLOVER A， DINALE A， ROSA L D S， et al.luvharris： A practical corner detector for event-cameras[J]. IEEE Transactions on Pattern Analysis and Machine Intelligence，2021，44（12）：10087-10098.

[5]LEUTENEGGER S， CHLI M， SIEGWART R Y.BRISK：Binary robust invariant scalable keypoints[C]//2011 International Conference on Computer Vision.IEEE， 2011.

[6]CHANG L， NIU X， LIU T.GNSS/IMU/ODO/LiDAR-SLAM integrated navigation system using IMU/ODO pre-integration[J].Sensors，2020，20（17）：4702.

[7]FORSTER C， PIZZOLI M， SCARAMUZZA D.SVO：Fast semi-direct monocular visual odometry[C]//2014 IEEE International Conference on Robotics and Automation.IEEE，2014.

[8]MUR-ARTAL R， MONTIEL J M M， TARDOS J D.ORB-SLAM： a versatile and accurate monocular SLAM system[J]. IEEE Transactions on Robotics，2015，31（5）： 1147-1163.

[9]MUR-ARTAL R， TARDOS J D.Visual-inertial monocular SLAM with map reuse[J].IEEE Robotics and Automation Letters，2017，2（2）：796-803.

[10]QIN T， LI P， SHEN S.Vins-mono： A robust and versatile monocular visual-inertial state estimator[J].IEEE Transactions on Robotics，2018， 34（4）：1004-1020.

[11]QIN T， PAN J， CAO S， et al.A general optimization-based framework for local odometry estimation with multiple sensors[J/OL].（2019-01-11）[2024-7-20].https：//arxiv.org/abs/1901.03638.

[12]ZHAO S， ZHANG T， WEI H.Fast visual inertial odometry with point–line features using adaptive EDLines algorithm[J].Measurement Science and Technology，2022，33（10）：105401.

[13]明德琦.基于關(guān)鍵幀的單目視覺慣性SLAM[D].鎮(zhèn)江：江蘇科技大學(xué)，2023.

[14]陸健.最小二乘法及其應(yīng)用[J].中國(guó)西部科技，2007（12）：19-21.

[15]CHUM O， MATAS J， KITTLER J.Locally optimized RANSAC[C]//Pattern Recognition： 25th DAGM Symposium， Magdeburg， Germany， September 10-12， 2003， Proceedings.Springer Berlin Heidelberg， 2003：236-243.

[16]BURRI M， NIKOLIC J， GOHL P， et al. The EuRoC micro aerial vehicle datasets[J].The International Journal of Robotics Research，2016，35（10）： 1157-1163.