劉貴濤，張 雷，徐 方

(1.中國(guó)科學(xué)院沈陽(yáng)自動(dòng)化研究所機(jī)器人學(xué)國(guó)家重點(diǎn)實(shí)驗(yàn)室，沈陽(yáng) 110016；2.中國(guó)科學(xué)院機(jī)器人與智能制造創(chuàng)新研究院，沈陽(yáng) 110169;3.中國(guó)科學(xué)院大學(xué)，北京 100049;4.沈陽(yáng)新松機(jī)器人自動(dòng)化股份有限公司，沈陽(yáng) 110168)

0 引言

雙目視覺(jué)SLAM(simultaneous localization and mapping)系統(tǒng)通過(guò)處理圖像序列，可以實(shí)時(shí)地估計(jì)雙目相機(jī)的位置和姿態(tài)。因?yàn)殡p目相機(jī)具有成本低廉，安裝方便，同時(shí)適應(yīng)室內(nèi)和室外環(huán)境，獲取信息豐富等優(yōu)點(diǎn)，因而被廣泛地應(yīng)用到移動(dòng)機(jī)器人自主導(dǎo)航中。通過(guò)將雙目視覺(jué)SLAM系統(tǒng)前后幀數(shù)據(jù)關(guān)聯(lián)，可以實(shí)時(shí)地向機(jī)器人輸出雙目相機(jī)的位姿。但是由于噪聲的干擾，隨著時(shí)間增加，雙目視覺(jué)SLAM系統(tǒng)估計(jì)的相機(jī)位姿累積誤差會(huì)越來(lái)越大[1],可能最終導(dǎo)致SLAM精度下降甚至定位失敗。

針對(duì)此問(wèn)題，目前主流的方法是依靠回環(huán)檢測(cè)及回環(huán)校正，消除雙目視覺(jué)SLAM系統(tǒng)的累積誤差。ORB-SLAM2系統(tǒng)[2-4]引入了DBOW2庫(kù)[5]，用于回環(huán)檢測(cè)，消除系統(tǒng)的累積誤差。雖然該方法回環(huán)的準(zhǔn)確性很高，但是回環(huán)率很低，即經(jīng)常在發(fā)生回環(huán)的地方，檢測(cè)不到回環(huán)。同樣是利用DBOW2，PL-SLAM[6]同時(shí)對(duì)點(diǎn)特征描述和線特征描述進(jìn)行聚類訓(xùn)練，得到基于點(diǎn)線特征的詞袋模型。應(yīng)用點(diǎn)線特征的模型，雖然定位精度更可靠，但是回環(huán)率依然很低。單純依靠人工特征AprilTag的TagSLAM系統(tǒng)[7]的回環(huán)檢測(cè)方法成功率和準(zhǔn)確率很高，可以很好地進(jìn)行累計(jì)誤差消除。UcoSLAM[8-9]是一種融合自然特征和人工特征的單目視覺(jué)SLAM方法。在存在人工特征的地方，回環(huán)檢測(cè)的成功率和準(zhǔn)確率都很高，可以很好地消除視覺(jué)SLAM系統(tǒng)的累積誤差。雖然回環(huán)校正的方法可以有效地消除SLAM系統(tǒng)的累積誤差。但是，實(shí)際機(jī)器人工作場(chǎng)景中存在許多無(wú)法回環(huán)的情況。在無(wú)回環(huán)場(chǎng)景下，消除雙目視覺(jué)SLAM系統(tǒng)的累積誤差，有著重要的實(shí)際應(yīng)用價(jià)值。因?yàn)樗粌H會(huì)極大地?cái)U(kuò)展移動(dòng)機(jī)器人的應(yīng)用場(chǎng)景，還可以大大提高移動(dòng)機(jī)器人的定位精度。

為了解決上述問(wèn)題，本文提出了一套利用ArUco[10]碼先驗(yàn)信息輔助消除累積誤差的算法。該算法由下面三個(gè)步驟組成，首先，進(jìn)行ArUco碼檢測(cè)，得到ArUco碼在檢測(cè)圖像中的位置；然后，結(jié)合ArUco碼先驗(yàn)信息和ArUco碼在圖像中的位置，估計(jì)相機(jī)在世界坐標(biāo)系下沒(méi)有累積誤差的位姿；最后，構(gòu)建圖模型，優(yōu)化全局地圖，消除雙目SLAM系統(tǒng)的累積誤差，得到精確的環(huán)境模型。

1 SLAM系統(tǒng)

本文提出的算法以O(shè)RB-SLAM2系統(tǒng)為基礎(chǔ)框架，新增ArUco碼檢測(cè)模塊、相機(jī)位姿估計(jì)模塊和累積誤差消除模塊。整體系統(tǒng)架構(gòu)如圖1所示，虛線框部分為本文所提方法的貢獻(xiàn)。

圖1 SLAM系統(tǒng)

整個(gè)算法包含前端跟蹤、后端建圖、校正累積誤差三個(gè)線程,下面分別進(jìn)行介紹。

1.1 前端跟蹤線程

前端跟蹤線程接收雙目圖像數(shù)據(jù)流。首先，按照參考幀模型或運(yùn)動(dòng)模型跟蹤3D地圖特征點(diǎn)位置，粗略估計(jì)相機(jī)位姿。之后，跟蹤局部地圖的所有3D地圖特征點(diǎn)，精確估計(jì)相機(jī)位姿。當(dāng)圖像中跟蹤的3D地圖特征點(diǎn)數(shù)目不足時(shí)，建立新的關(guān)鍵幀，生成新的3D地圖特征點(diǎn)。

在構(gòu)造新的關(guān)鍵幀時(shí)，系統(tǒng)還會(huì)檢測(cè)該關(guān)鍵幀圖像中是否存在ArUco碼。在檢測(cè)到ArUco碼后，采用EPNP[11]算法計(jì)算相機(jī)相對(duì)該ArUco碼坐標(biāo)系的位姿變換。之后，結(jié)合該位姿變換和ArUco碼先驗(yàn)信息計(jì)算該關(guān)鍵幀處相機(jī)消除累積誤差后的位姿，并將其保存。最后，系統(tǒng)將該關(guān)鍵幀傳入后端建圖線程。

1.2 后端建圖線程

后端建圖線程接收到新的關(guān)鍵幀后，對(duì)該關(guān)鍵幀新建立的3D地圖特征點(diǎn)按照一定的策略進(jìn)行篩選，只保留質(zhì)量高的點(diǎn)。在此進(jìn)程中，還會(huì)進(jìn)行三角化左右目圖像2D特征點(diǎn)、融合地圖、刪除冗余關(guān)鍵幀以及局部地圖集束調(diào)整(bundle ajustment,BA)等工作。最后，后端建圖線程將該關(guān)鍵幀傳入全局校正線程。

1.3 校正累積誤差線程

校正累積誤差線程接收到新的關(guān)鍵幀后，首先判斷該關(guān)鍵幀的圖像中是否可以檢測(cè)到ArUco碼。如果沒(méi)檢測(cè)到，則跳過(guò)此關(guān)鍵幀。如果該關(guān)鍵幀檢測(cè)到ArUco碼，則根據(jù)ArUco碼的解碼ID是否正確判斷是否進(jìn)行累積誤差消除的工作。當(dāng)ArUco碼的解碼ID正確時(shí)，執(zhí)行位姿校正和全局地圖優(yōu)化，優(yōu)化算法采用levenberg-marquardt[14](LM)算法。

2 累積誤差消除

2.1 ArUco碼檢測(cè)

在建立新的關(guān)鍵幀時(shí)，前端跟蹤線程會(huì)檢測(cè)該關(guān)鍵幀的圖像中是否存在ArUco碼。檢測(cè)過(guò)程如下:首先,對(duì)圖像進(jìn)行閾值化處理，檢測(cè)出當(dāng)前幀圖像中所有具有凸性的候選四邊形；然后，按照四邊形是否相似、是否靠近邊界、是否與其他四邊形有包含關(guān)系等條件篩除不合適的四邊形；之后，對(duì)檢測(cè)到的候選四邊形進(jìn)行解碼，辨識(shí)其ID；最后，亞像素優(yōu)化ArUco碼的4個(gè)頂點(diǎn)位置，并按照順時(shí)針順序?qū)?個(gè)頂點(diǎn)坐標(biāo)保存記錄在系統(tǒng)中。

2.2 相機(jī)位姿估計(jì)

首先對(duì)ArUco碼的坐標(biāo)系進(jìn)行定義，如圖2所示。ArUco碼的坐標(biāo)系Oa-XaYaZa，坐標(biāo)原點(diǎn)為ArUco碼的中心點(diǎn)，X、Y軸分別為沿ArUco碼的橫縱線，Z軸為垂直于ArUco碼表面并穿過(guò)坐標(biāo)原點(diǎn)的直線。

本文算法的ArUco碼先驗(yàn)信息包括兩種：第一種是環(huán)境中各個(gè)ArUco碼在世界坐標(biāo)系的位姿Taw，第二種是ArUco碼的4個(gè)3D頂點(diǎn)在該ArUco碼坐標(biāo)系中的相對(duì)位置。其中，各個(gè)ArUco碼坐標(biāo)系的位姿由以下方法確定：如圖2所示，默認(rèn)ID為0的ArUco碼坐標(biāo)系為世界坐標(biāo)系Ow-XwYwZw，其他ArUco碼坐標(biāo)系的位姿即相對(duì)于該世界坐標(biāo)系的位姿變換Taw。兩種先驗(yàn)信息由人工預(yù)先標(biāo)定。

圖2 相機(jī)位姿估計(jì)

將第二種先驗(yàn)信息和2.1節(jié)中檢測(cè)到的ArUco碼4個(gè)頂點(diǎn)在關(guān)鍵幀圖像中的亞像素位置對(duì)應(yīng)，建立式(1)。

λu=KTcaP

(1)

式中，Tca是相機(jī)在ArUco碼坐標(biāo)系下的位姿；P是ArUco碼的頂點(diǎn)在ArUco碼坐標(biāo)系下的三維坐標(biāo)；u是ArUco碼的頂點(diǎn)在像素坐標(biāo)系下的對(duì)應(yīng)二維坐標(biāo)；K是相機(jī)的內(nèi)參；λ是在相機(jī)坐標(biāo)系下的深度。

4個(gè)頂點(diǎn)可得到4個(gè)式(1)，利用EPNP算法可求解得到相機(jī)在ArUco碼坐標(biāo)系的位姿Tca。如圖2所示，Tca為從ArUco碼坐標(biāo)系Oa-XaYaZa到相機(jī)坐標(biāo)系Oc-XcYcZc的位姿變換。

本文中的SLAM系統(tǒng)根據(jù)第一種先驗(yàn)信息Taw及相機(jī)在ArUco碼坐標(biāo)系下的位姿Tca可解算出當(dāng)前幀相機(jī)的位姿Tcw。解算公式如式(2)所示。

Tcw=TcaTaw

(2)

2.3 累積誤差消除

校正累積誤差線程會(huì)逐個(gè)關(guān)鍵幀地檢查該幀是否檢測(cè)到ArUco碼。如果該關(guān)鍵幀沒(méi)有檢測(cè)到ArUco碼，則跳過(guò)該幀處理下一幀；如果該關(guān)鍵幀檢測(cè)到ArUco碼時(shí)，則對(duì)其建立圖模型，更新地圖中關(guān)鍵幀的位姿信息和3D地圖特征點(diǎn)的位置信息，消除SLAM系統(tǒng)的累積誤差。

為了敘述方便，將檢測(cè)到ArUco碼的關(guān)鍵幀統(tǒng)一簡(jiǎn)潔命名為Ar幀。

2.3.1 地圖預(yù)處理

在建立圖模型之前首先對(duì)地圖進(jìn)行預(yù)處理，為圖模型優(yōu)化提供良好的初值，使其更快迭代收斂到最優(yōu)值。

地圖預(yù)處理的工作包括兩部分，更新局部關(guān)鍵幀的位姿信息和更新局部3D地圖特征點(diǎn)的位置信息。其中，局部關(guān)鍵幀是指與Ar幀有足夠共視點(diǎn)的所有關(guān)鍵幀，局部地圖特征點(diǎn)是指局部關(guān)鍵幀觀測(cè)到的所有地圖特征點(diǎn)。

利用2.2節(jié)得到的Ar幀消除累積誤差后的位姿，更新局部關(guān)鍵幀的位姿信息和更新局部3D地圖特征點(diǎn)的位置信息。

(3)

(4)

2.3.2 位姿圖圖模型

本文SLAM系統(tǒng)位姿圖圖模型的頂點(diǎn)由地圖中各個(gè)關(guān)鍵幀的位姿構(gòu)成，邊由共視關(guān)鍵幀間的相對(duì)位姿關(guān)系構(gòu)成。

各個(gè)頂點(diǎn)的初值按照上節(jié)關(guān)鍵幀被校正更新后的位姿取值。若該關(guān)鍵幀的位姿未在上節(jié)被更新，則按原始的位姿取值。

位姿示意圖如圖3所示，圓和五角星代表頂點(diǎn)，直線和虛線代表邊。五角星頂點(diǎn)代表Ar幀的位姿，因其累積誤差已被消除，所以令該頂點(diǎn)固定不變；圓頂點(diǎn)代表普通關(guān)鍵幀的位姿，為優(yōu)化變量。

本文SLAM系統(tǒng)位姿圖中的邊，按照是否有Ar幀參與，可分為一元邊和二元邊。如圖3所示，直線代表二元邊，連接兩個(gè)普通關(guān)鍵幀，構(gòu)造同ORB-SLAM2中本質(zhì)圖的二元邊一樣，優(yōu)化變量為兩個(gè)普通關(guān)鍵幀的位姿；虛線邊代表一元邊，連接一個(gè)普通關(guān)鍵幀和一個(gè)Ar幀，優(yōu)化變量?jī)H有普通關(guān)鍵幀的位姿。滿足以下條件的關(guān)鍵幀，其位姿相應(yīng)的頂點(diǎn)被一元邊約束：①和Ar幀是父子幀關(guān)系；②和Ar幀有足夠多的共視點(diǎn)。

圖3 位姿圖示意圖

位姿圖中一元邊的誤差函數(shù)ei1如式(5)所示。其中，邊的測(cè)量Tij取為局部關(guān)鍵幀位姿未更新前相機(jī)從Ar幀向關(guān)鍵幀Ki的位姿變換，Ti是Ki的位姿，Tj是Ar幀的位姿。

(5)

位姿圖的誤差函數(shù)Epose由所有邊的誤差函數(shù)相加組成，如式(6)所示。對(duì)Epose采用LM算法迭代優(yōu)化，使其最小化，得到位姿圖中所有頂點(diǎn)的最優(yōu)值。

(6)

2.3.3 全局重投影誤差圖圖模型

全局重投影誤差圖圖模型的頂點(diǎn)是地圖中所有關(guān)鍵幀的位姿和所有3D地圖特征點(diǎn)的位置，邊是3D地圖特征點(diǎn)在關(guān)鍵幀圖像上投影的誤差。

重投影誤差圖模型示意圖如圖4所示，圓三角形和五角星代表頂點(diǎn)，直線和虛線代表邊。五角星頂點(diǎn)代表Ar幀的位姿，因其累積誤差已被消除，所以令該頂點(diǎn)固定不變；圓頂點(diǎn)代表普通關(guān)鍵幀的位姿，為優(yōu)化變量。三角形頂點(diǎn)代表3D地圖特征點(diǎn)位置，為優(yōu)化變量。

在全局重投影誤差圖圖模型中，邊按照是否有Ar幀參與，分為一元邊和二元邊。如圖4所示，直線邊代表二元邊，連接一個(gè)普通關(guān)鍵幀和一個(gè)3D地圖特征點(diǎn)，其構(gòu)造同ORB-SLAM2中本質(zhì)圖的二元邊一樣，優(yōu)化變量為兩個(gè)普通關(guān)鍵幀的位姿；虛線邊代表一元邊，連接一個(gè)Ar幀，和一個(gè)3D地圖特征點(diǎn),因?yàn)锳r幀位姿的累積誤差已被消除，所以優(yōu)化變量?jī)H有3D地圖特征點(diǎn)的位置。

圖4 全局重投影誤差圖示意圖

一元邊對(duì)應(yīng)的誤差函數(shù)如式(7)所示。

(7)

式中，K是相機(jī)內(nèi)參；ξj是Ar幀位姿Tj的李代數(shù)表示；Pi是3D地圖特征點(diǎn)的世界坐標(biāo)系表示；ui是該3D地圖特征點(diǎn)在關(guān)鍵幀圖像上匹配的二維特征點(diǎn)的坐標(biāo)。

全局重投影誤差圖的誤差函數(shù)Ereproj由所有邊的誤差函數(shù)相加組成，如式(8)所示。對(duì)Ereproj采用LM算法迭代優(yōu)化，使其最小化，得到全局重投影誤差圖中所有頂點(diǎn)的最優(yōu)值。

(8)

3 實(shí)驗(yàn)

3.1 實(shí)驗(yàn)條件

為驗(yàn)證提出的算法是否能夠消除無(wú)回環(huán)場(chǎng)景下雙目視覺(jué)SLAM產(chǎn)生的累積誤差，在沈陽(yáng)某公司的兩種無(wú)回環(huán)場(chǎng)景下總共錄制了8個(gè)數(shù)據(jù)集進(jìn)行實(shí)驗(yàn)，分別將各個(gè)數(shù)據(jù)集命名為Dataset1-8。

兩種實(shí)驗(yàn)場(chǎng)景布局圖如圖5所示，圖5a和圖5b分別對(duì)應(yīng)圖1中的不閉合場(chǎng)景和長(zhǎng)廊場(chǎng)景。直線代表相機(jī)在每次錄制數(shù)據(jù)時(shí)行進(jìn)的大致軌跡，灰色矩形塊代表該公司辦公區(qū)域，為機(jī)器人的不可越過(guò)障礙物，矩形黑白色塊代表ArUco碼。

(a) 不閉合場(chǎng)景示意圖 (b) 長(zhǎng)廊場(chǎng)景示意圖圖5 實(shí)驗(yàn)場(chǎng)景布局圖

以ID為0的ArUco碼坐標(biāo)系為世界坐標(biāo)系，用米尺測(cè)量每一個(gè)ArUco碼中心點(diǎn)在世界坐標(biāo)系的X、Y軸坐標(biāo)，完成對(duì)所有ArUco碼坐標(biāo)系位姿的預(yù)標(biāo)定。米尺的最小長(zhǎng)度單位為mm。

采集數(shù)據(jù)集所用雙目相機(jī)為小覓雙目相機(jī)標(biāo)準(zhǔn)版，左右目像素都是752×480像素，曝光模式采用自動(dòng)曝光。實(shí)驗(yàn)處理算法在一臺(tái)筆記本電腦上完成，電腦配置如下：CPU為Intel-i7-9750H，主頻最高2.60 GHz，8 G內(nèi)存，電腦操作系統(tǒng)軟件采用Ubuntu18.04。

3.2 累積誤差消除

為了檢驗(yàn)本文系統(tǒng)的累積誤差消除效果，分別在8個(gè)實(shí)驗(yàn)數(shù)據(jù)集下進(jìn)行實(shí)驗(yàn)。因?yàn)楦鱾€(gè)實(shí)驗(yàn)數(shù)據(jù)集中ArUco碼個(gè)數(shù)不同，為保證實(shí)驗(yàn)一致性，各個(gè)實(shí)驗(yàn)場(chǎng)景中都只用4個(gè)ArUco碼消除SLAM系統(tǒng)的累積誤差。在實(shí)驗(yàn)中，終點(diǎn)位置誤差由米尺測(cè)量得到，最小單位為mm。為對(duì)比本文算法消除雙目SLAM系統(tǒng)累積誤差的有效性，進(jìn)行了和其他優(yōu)秀雙目SLAM系統(tǒng)的對(duì)比實(shí)驗(yàn)。但由于數(shù)據(jù)集為自制數(shù)據(jù)集，所以選擇了和典型的開(kāi)源算法ORB-SLAM2(特征法SLAM主流代表)、stereo-DSO[14](直接法SLAM主流代表)和VINS_FUSION[15](光流法SLAM主流代表)進(jìn)行對(duì)比實(shí)驗(yàn)。

在8個(gè)數(shù)據(jù)集下，分別采用ORB-SLAM2、VINS_FUSION(雙目模式)、stereo-DSO和本文系統(tǒng)計(jì)算終點(diǎn)位置累積誤差。統(tǒng)計(jì)4個(gè)SLAM系統(tǒng)在8個(gè)數(shù)據(jù)集下的累積誤差，如表1所示。

表1 累積誤差 (m)

由表1可見(jiàn)，在8個(gè)數(shù)據(jù)集中，相比其它杰出的雙目SLAM系統(tǒng)，本文雙目SLAM系統(tǒng)的累積誤差都更小。

在數(shù)據(jù)集Dataset5中，其他雙目SLAM系統(tǒng)均存在一定的累積誤差，而本文雙目SLAM系統(tǒng)的累積誤差僅有0.012 m。這是因?yàn)樵诮K點(diǎn)處剛剛進(jìn)行了累積誤差消除，使得誤差值接近于0。該實(shí)驗(yàn)直接說(shuō)明了本文累積誤差消除算法的有效性。

在其余數(shù)據(jù)集中，本文SLAM系統(tǒng)和其他SLAM系統(tǒng)均存在一定的累積誤差，但本文SLAM系統(tǒng)的累積誤差遠(yuǎn)小于其他SLAM系統(tǒng)的累積誤差。如在數(shù)據(jù)集Dataset1中，雙目ORB-SLAM2系統(tǒng)的累積誤差約為1.602 m，STEREO-DSO約為1.854 m，VINS-FUSION約為2.262 m，而本文系統(tǒng)的累積誤差只有0.138 m。究其原因，本文SLAM系統(tǒng)累積誤差較小，是由于采用圖優(yōu)化技術(shù)(graph optimization)對(duì)檢測(cè)位姿和特征點(diǎn)的位置進(jìn)行了優(yōu)化的結(jié)果。這從側(cè)面驗(yàn)證了本文累積誤差消除算法的有效性。

3.3 系統(tǒng)實(shí)時(shí)性

為了檢驗(yàn)本文系統(tǒng)的實(shí)時(shí)性，在8個(gè)數(shù)據(jù)集下分別進(jìn)行了和雙目ORB-SLAM2的耗時(shí)對(duì)比實(shí)驗(yàn)。在每個(gè)數(shù)據(jù)集中，記錄ORB-SLAM2系統(tǒng)和本文系統(tǒng)處理每幀圖像的用時(shí)。數(shù)據(jù)集Dataset1中的用時(shí)對(duì)比如圖6所示。在用時(shí)圖中，橫軸為圖像幀的序列號(hào)，縱軸為相應(yīng)圖像幀的花費(fèi)時(shí)間。統(tǒng)計(jì)8個(gè)數(shù)據(jù)集下本文系統(tǒng)和雙目ORB-SLAM2系統(tǒng)平均用時(shí)，如表2所示。

(a) ORB-SLAM2用時(shí) (b) 本文算法用時(shí)圖6 用時(shí)對(duì)比圖

表2 單幀圖片平均耗費(fèi)時(shí)間 (ms)

由圖6可見(jiàn)，本文系統(tǒng)和雙目ORB-SLAM2系統(tǒng)的共同點(diǎn)是每幀數(shù)據(jù)處理用時(shí)都集中地穩(wěn)定在30～40 ms之間，不同點(diǎn)在于本文系統(tǒng)算法處理時(shí)間的峰值比雙目ORB-SLAM2系統(tǒng)大很多。按照對(duì)應(yīng)幀數(shù)查詢，發(fā)現(xiàn)它們的對(duì)應(yīng)圖像中存在ArUco碼，即ArUco碼檢測(cè)和相機(jī)位姿估計(jì)需要多花費(fèi)一部分時(shí)間。

由表2知，本文系統(tǒng)的平均耗時(shí)約為34.85 ms，雙目ORB-SLAM2的平均耗時(shí)約為33.98 ms，本文SLAM系統(tǒng)平均耗時(shí)僅比ORB-SLAM2系統(tǒng)多0.87 ms。雖然本文增加了ArUco碼檢測(cè)部分，但由于其只在關(guān)鍵幀處檢測(cè)，因而對(duì)平均耗時(shí)影響甚微。本文系統(tǒng)處理每幀圖像用時(shí)34.85 ms，約為每秒處理28幀圖像，可以滿足雙目SLAM系統(tǒng)實(shí)時(shí)性要求。

4 結(jié)束語(yǔ)

本文提出了一套利用ArUco碼先驗(yàn)信息消除雙目SLAM系統(tǒng)累積誤差的創(chuàng)新算法。該算法由3部分組成，ArUco碼檢測(cè)、相機(jī)位姿估計(jì)和累積誤差消除。經(jīng)過(guò)在8個(gè)實(shí)驗(yàn)數(shù)據(jù)集下實(shí)驗(yàn)測(cè)試，證明本文提出的算法，可以穩(wěn)定可靠地消除雙目SLAM系統(tǒng)的累積誤差，同時(shí)滿足機(jī)器人定位的實(shí)時(shí)性要求。

在實(shí)際應(yīng)用本文提出的SLAM系統(tǒng)時(shí)，發(fā)現(xiàn)雖然可以解決累積誤差問(wèn)題，但視覺(jué)SLAM系統(tǒng)有時(shí)仍然不能正常工作。究其原因，是實(shí)驗(yàn)場(chǎng)景中的特征在一些局部區(qū)域內(nèi)非常少，如柱面、墻面等，導(dǎo)致系統(tǒng)無(wú)法提取到足夠的特征，不足以估計(jì)相機(jī)位姿和建立新的地圖特征點(diǎn)。因此，本文的下一步工作將會(huì)向融合點(diǎn)線面特征等方向發(fā)展，用以解決弱紋理區(qū)域定位困難的問(wèn)題。