李醒飛，李婧嫻，郭鳳志

(天津大學(xué) 精密測(cè)試技術(shù)及儀器國(guó)家重點(diǎn)實(shí)驗(yàn)室，天津 300072)

近年來(lái)，SLAM 技術(shù)由于其環(huán)境感知和自我定位能力，成為了自主導(dǎo)航領(lǐng)域的研究熱點(diǎn)[1]。降低估計(jì)誤差、提升系統(tǒng)穩(wěn)定性一直是SLAM技術(shù)發(fā)展的方向。目前，SLAM 技術(shù)已從基于概率和統(tǒng)計(jì)方法[2]，發(fā)展至基于非線性優(yōu)化的圖優(yōu)化方法。在苛刻條件下，SLAM 系統(tǒng)的累計(jì)誤差成為了限制定位和建圖精度的關(guān)鍵因素，嚴(yán)重影響了SLAM 系統(tǒng)的穩(wěn)定性。盡管視覺(jué)已經(jīng)擺脫了輪速計(jì)較為嚴(yán)重的漂移問(wèn)題[3]，但機(jī)器人在長(zhǎng)期運(yùn)行過(guò)程中，視覺(jué)位姿和地圖的不確定度增大，誤差逐步累積依然會(huì)影響系統(tǒng)的穩(wěn)定性。

對(duì)此，研究學(xué)者傾向于采用多傳感融合的方案加以解決。如2018 年香港科技大學(xué)秦通等人實(shí)現(xiàn)了視覺(jué)與IMU 的緊耦合位姿估計(jì)[4]，夏琳琳等人則在融合IMU 基礎(chǔ)上又結(jié)合了光流-線特征方法[5]，另外張福斌等人避免了IMU 二次積分的距離誤差，轉(zhuǎn)而融合里程計(jì)與陀螺儀降低累積誤差[6]，王立玲等人則額外融合了IMU 和關(guān)節(jié)編碼器實(shí)現(xiàn)機(jī)器人的自主定位[7]。P.Gakne 等人設(shè)計(jì)了基于GNSS 與空天相機(jī)緊耦合的位姿估計(jì)算法，定位精度有明顯提升[8]。此外，還有學(xué)者利用地磁傳感器獲得實(shí)時(shí)測(cè)量的地磁序列修正SLAM 的姿態(tài)漂移[9]。但這些方法受制于使用時(shí)間或者環(huán)境，無(wú)法在特殊場(chǎng)景下校正累積誤差。

為在嚴(yán)苛條件下能夠降低累積誤差，SLAM 系統(tǒng)需要找到準(zhǔn)確可靠且不產(chǎn)生累計(jì)誤差的傳感器或傳感系統(tǒng)。在導(dǎo)航和定位領(lǐng)域，尋北儀作為直接準(zhǔn)確測(cè)量地理真實(shí)姿態(tài)的設(shè)備，已被廣泛應(yīng)用于導(dǎo)彈發(fā)射等軍事領(lǐng)域以及采礦、測(cè)繪和天文學(xué)等民用領(lǐng)域[10]。故在本文中使用自研捷聯(lián)式尋北儀與靶標(biāo)構(gòu)成外部測(cè)姿系統(tǒng)，為SLAM 系統(tǒng)在嚴(yán)苛條件下提供地理真實(shí)姿態(tài)信息作為姿態(tài)約束，從而有效降低SLAM 系統(tǒng)的累積誤差。

本文針對(duì)地下、室內(nèi)等特殊應(yīng)用場(chǎng)景，提出了一種融合外部姿態(tài)信息的SLAM 算法。相機(jī)在采集過(guò)程中通過(guò)標(biāo)定靶獲得外部姿態(tài)信息，在解算圖像幀的相對(duì)位姿不確定度后，通過(guò)稀疏的外部姿態(tài)信息和密集的圖像幀建立了融合姿態(tài)信息的姿態(tài)圖和全局優(yōu)化模型，在特殊應(yīng)用場(chǎng)景下依然能夠有效降低SLAM 系統(tǒng)的累計(jì)誤差，提高SLAM 系統(tǒng)的穩(wěn)定性，對(duì)機(jī)器人定位和建圖具有重要意義。

1 框架搭建

為降低SLAM 系統(tǒng)的累計(jì)誤差，在原有框架中加入外部姿態(tài)信息，以連續(xù)三幀相機(jī)位姿和兩個(gè)路標(biāo)點(diǎn)為例，可構(gòu)建如圖1 所示的貝葉斯網(wǎng)絡(luò)。由圖1 可知，除原本的起始點(diǎn)和相機(jī)約束外，測(cè)姿系統(tǒng)的觀測(cè)量z5和z6為相機(jī)位姿添加了額外約束關(guān)系，通過(guò)約束關(guān)系中的姿態(tài)信息可對(duì)相機(jī)位姿和路標(biāo)點(diǎn)位置進(jìn)行優(yōu)化，以減小姿態(tài)和位置的漂移。

圖1 貝葉斯網(wǎng)絡(luò)Fig.1 Bayesian network

在實(shí)際應(yīng)用場(chǎng)景下，測(cè)姿系統(tǒng)的觀測(cè)量較圖像觀測(cè)量顯著稀疏。此外，為保證姿態(tài)的測(cè)量與相機(jī)運(yùn)動(dòng)過(guò)程不沖突，測(cè)姿系統(tǒng)和相機(jī)相互獨(dú)立。由于測(cè)姿系統(tǒng)能便捷地獲取地理真實(shí)姿態(tài)信息，相機(jī)可在需要校正時(shí)拍攝測(cè)姿系統(tǒng)的靶標(biāo)，通過(guò)視覺(jué)進(jìn)行位姿傳遞，獲取相機(jī)姿態(tài)。x 為相機(jī)位姿，p 為路標(biāo)點(diǎn)位置，z 為觀測(cè)值，其中z0為固定的起始點(diǎn)，z1～z4為相機(jī)觀測(cè)量，z5和z6為測(cè)姿系統(tǒng)觀測(cè)量。

2 融合外部姿態(tài)信息的SLAM 算法

2.1 協(xié)方差估計(jì)算法

相對(duì)姿態(tài)的協(xié)方差矩陣描述了相對(duì)姿態(tài)的不確定度大小，外部姿態(tài)信息相比于圖像關(guān)鍵幀十分稀疏，兩者比值可達(dá)到兩個(gè)數(shù)量級(jí)，但前者給出的是世界坐標(biāo)系下不含累積誤差的三維姿態(tài)，后者則是通過(guò)相對(duì)位姿的迭代獲取世界坐標(biāo)系下六維姿態(tài)。由于前者不含累積誤差，得到的相機(jī)相對(duì)姿態(tài)的協(xié)方差行列式遠(yuǎn)小于后者姿態(tài)的協(xié)方差行列式，這為優(yōu)化方程的參數(shù)設(shè)計(jì)提供了參考。另一方面，對(duì)于后者不同關(guān)鍵幀間的相對(duì)姿態(tài)的協(xié)方差矩陣也各不相同，因此，為能夠根據(jù)不確定度進(jìn)行準(zhǔn)確的姿態(tài)圖優(yōu)化，需要估計(jì)姿態(tài)圖優(yōu)化中各個(gè)邊的協(xié)方差。

1）相機(jī)連續(xù)幀的協(xié)方差估計(jì)

在前段的跟蹤環(huán)節(jié)，非線性優(yōu)化問(wèn)題的量測(cè)方程可寫(xiě)為含有高斯噪聲的形式：

式中，x 是相機(jī)位姿， f （x ）是量測(cè)方程，y 是被觀測(cè)量， N （0,Σ ）表示高斯噪聲， y 具有無(wú)偏高斯噪聲，噪聲協(xié)方差矩陣為半正定矩陣Σ 。因此，在觀測(cè)量為y 的條件下，x 的最大似然估計(jì)為：

式(2)實(shí)際是Mahalanobis 距離的平方[11]。當(dāng)f （x）在 x*處的雅克比矩陣 J（ x*）是列滿秩時(shí)，相機(jī)位姿的協(xié)方差矩陣為：

2）組合位姿協(xié)方差估計(jì)

在關(guān)鍵幀非線性優(yōu)化模型中，需要考慮兩關(guān)鍵幀之間的相對(duì)位姿的不確定度，進(jìn)而獲得用于姿態(tài)圖優(yōu)化的信息矩陣。本文通過(guò)遞推關(guān)鍵幀間的相機(jī)位姿來(lái)計(jì)算得到關(guān)鍵幀間的相對(duì)位姿不確定度估計(jì)。首先，給出連續(xù)兩幀相機(jī)姿態(tài)，分別為：

因此，當(dāng)3D-2D 匹配點(diǎn)數(shù)目較多時(shí)，協(xié)方差矩陣Σ1和Σ2的行列式較小，對(duì)于連續(xù)兩幀的組合相機(jī)位姿，可以直接保留二階項(xiàng)用于估計(jì)組合相機(jī)位姿的協(xié)方差矩陣：

對(duì)于連續(xù)多次的相機(jī)位姿組合問(wèn)題，推導(dǎo)可得邊（m, n ）的相對(duì)位姿不確定度：

2.2 姿態(tài)圖算法

1）姿態(tài)圖優(yōu)化

考慮到常用的位姿圖優(yōu)化需要六自由度的信息，由于地理姿態(tài)信息不含對(duì)相機(jī)位置的約束，因此推導(dǎo)融合地理真實(shí)姿態(tài)的姿態(tài)圖優(yōu)化算法。首先，定義相對(duì)姿態(tài)李群表達(dá)形式如下：

將其表達(dá)為李代數(shù)形式：

誤差表達(dá)式為：

對(duì)式(12)添加左擾動(dòng)，可得：

根據(jù)恒等式：

繼續(xù)整理可得：

式中：

式(16)和式(17)即為姿態(tài)圖優(yōu)化的雅克比矩陣子塊，將其帶入高斯牛頓方程可迭代求解相機(jī)的姿態(tài)。在獲得用于姿態(tài)優(yōu)化的雅克比矩陣子塊后，給出誤差方程表達(dá)式：

式中，雅克比矩陣 J（ x ）為：

式中，M 為圖像共視關(guān)鍵幀所組成的邊的個(gè)數(shù)；邊的集合為MΘ ，m 為序號(hào)下標(biāo)；N 為測(cè)姿幀所組成的邊的個(gè)數(shù)；邊的集合為NΘ ，n 為序號(hào)下標(biāo)。且滿足：

式中，（ i ,j ）∈ΘM以及（ p ,q ）∈ΘN，下面給出誤差項(xiàng)的協(xié)方差矩陣：

式中，oΣ 為對(duì)角分塊矩陣。通過(guò)非線性優(yōu)化方法可迭代求解關(guān)鍵幀姿態(tài)。

通過(guò)融合地理真實(shí)姿態(tài)的姿態(tài)圖優(yōu)化算法，不僅能夠在運(yùn)算中避免計(jì)算大量路標(biāo)點(diǎn)，提升優(yōu)化效率，還能夠有效避免路標(biāo)點(diǎn)本身所具有的累積誤差對(duì)于相機(jī)位姿估計(jì)和路標(biāo)點(diǎn)位置的影響，從而正確利用地理真實(shí)姿態(tài)準(zhǔn)確校正各個(gè)關(guān)鍵幀的相機(jī)位姿和路標(biāo)點(diǎn)位置，避免直接進(jìn)行全局優(yōu)化而導(dǎo)致相機(jī)位姿和路標(biāo)點(diǎn)位置收斂于局部最小值。此外，進(jìn)一步考慮到姿態(tài)圖優(yōu)化后的相機(jī)位姿和路標(biāo)點(diǎn)的位置還存在一定的偏差，需要在下一步進(jìn)行全局優(yōu)化以獲得更加準(zhǔn)確的估計(jì)結(jié)果。

2）全局優(yōu)化

在全局優(yōu)化中，相機(jī)位姿和路標(biāo)點(diǎn)位置同時(shí)作為優(yōu)化變量，路標(biāo)點(diǎn)重投影誤差和測(cè)姿幀誤差表達(dá)式分別為：

由此，可以將姿態(tài)圖優(yōu)化后的相機(jī)位姿和路標(biāo)點(diǎn)位置作為初始值，利用雅克比矩陣求解高斯牛頓方程，實(shí)現(xiàn)相機(jī)位姿和路標(biāo)點(diǎn)位置的全局優(yōu)化。

3仿真分析及試驗(yàn)結(jié)果

3.1 仿真分析

為驗(yàn)證算法的有效性，本文利用標(biāo)準(zhǔn)數(shù)據(jù)集KITTI中編號(hào)為00的數(shù)據(jù)集中部分?jǐn)?shù)據(jù)進(jìn)行仿真分析，該數(shù)據(jù)集提供了真實(shí)位姿，仿真中提取部分關(guān)鍵幀的真實(shí)姿態(tài)作為尋北系統(tǒng)所檢測(cè)到的地理真實(shí)姿態(tài)信息。

圖2中，X 軸為數(shù)據(jù)集序列號(hào)，Y 軸為融合前后的絕對(duì)軌跡誤差（AbsolutePosition Error,APE）?？梢钥闯?，當(dāng)相機(jī)運(yùn)動(dòng)到拐彎處，即關(guān)鍵幀序號(hào)為35幀之后時(shí)，誤差變化劇烈，盡管在(40,50)階段APE在減少，實(shí)際上是由于姿態(tài)發(fā)生了嚴(yán)重的漂移而導(dǎo)致相機(jī)估計(jì)軌跡與實(shí)際軌跡在三維空間形成了較大的相交角度，因此在接近區(qū)域誤差減小，但此后誤差逐漸增大；在融合優(yōu)化后，如藍(lán)色曲線所示，相機(jī)的位置誤差顯著減少。

圖2 融合優(yōu)化前后的絕對(duì)位置誤差Fig.2 Absolute position error beforeand after fusion optimization

此外，相對(duì)位置誤差能夠有效體現(xiàn)相鄰關(guān)鍵幀位置估計(jì)誤差的變化情況，圖3給出了相同序列上關(guān)鍵幀相對(duì)位置誤差和相對(duì)位姿協(xié)方差行列式對(duì)數(shù)值。其中，藍(lán)色圓圈標(biāo)記了相對(duì)位置誤差較大的點(diǎn)，綠色圓圈標(biāo)記了相對(duì)位置協(xié)方差較大的曲線段，可以看出兩者基本呈負(fù)相關(guān)，即協(xié)方差大的位置，相對(duì)位姿誤差較大，證明了協(xié)方差估計(jì)的合理性。通過(guò)在姿態(tài)圖中融入?yún)f(xié)方差進(jìn)行優(yōu)化，能夠正確設(shè)置圖優(yōu)化中邊的權(quán)重，實(shí)現(xiàn)累積誤差的有效消除。

圖3 關(guān)鍵幀相對(duì)位置誤差和相對(duì)位姿協(xié)方差行列式對(duì)數(shù)值Fig.3 Relative position error of key frameand relative pose covariancedeterminant log value

仿真分析表明，在沒(méi)有回環(huán)檢測(cè)環(huán)節(jié)參與的情況下，經(jīng)過(guò)融合優(yōu)化后相機(jī)位姿的絕對(duì)軌跡誤差下降至優(yōu)化前的30%以下。此外，根據(jù)實(shí)際測(cè)量需求可以降低閾值，以獲得更好的優(yōu)化效果。在下一節(jié)中，將該算法應(yīng)用于經(jīng)典框架ORB-SLAM2以分析算法性能。

3.2 試驗(yàn)結(jié)果

3.2.1 室內(nèi)試驗(yàn)

在室內(nèi)試驗(yàn)中，將本文算法搭載于ORB-SLAM2，外部測(cè)姿系統(tǒng)如圖4所示，由轉(zhuǎn)臺(tái)、光纖陀螺儀和加速度計(jì)構(gòu)成，航向精度在30s內(nèi)達(dá)到0.3°，水平精度在30s內(nèi)達(dá)到0.05°。通過(guò)靶標(biāo)進(jìn)行姿態(tài)傳遞，相機(jī)能夠較為準(zhǔn)確的獲得地理真實(shí)姿態(tài)。

圖4 測(cè)姿系統(tǒng)Fig.4 Attitudemeasurement syste m

本試驗(yàn)中，具有尋北信息的圖像幀共有5幀?？紤]到相機(jī)絕對(duì)位置在當(dāng)前實(shí)驗(yàn)條件下不易獲得，本文利用實(shí)驗(yàn)場(chǎng)地處于空間內(nèi)一個(gè)平面這一限制條件分析算法的效果。融合優(yōu)化前后相機(jī)在y 軸方向上的軌跡誤差值如表1所示。

從表1可知，融合后各項(xiàng)誤差均顯著降低，其中均值誤差僅為融合前的67.6%，最大值為融合前的68.2%。最后，將y 軸誤差呈現(xiàn)在相機(jī)運(yùn)動(dòng)的xz 平面內(nèi)，如圖5所示。

表1 融合前后相機(jī)在y軸上的軌跡誤差（單位：米）Tab.1 The trajectory error of thecamera onthe y-axis b efore andafter the fusion(Unit:m)

圖5中，右側(cè)色帶從藍(lán)色過(guò)渡到紅色表示軌跡在y 軸的誤差逐漸變大，在靠近起點(diǎn)同時(shí)也是回環(huán)處的誤差相對(duì)較小，軌跡顯示為藍(lán)色，這是由于開(kāi)始運(yùn)動(dòng)時(shí)累積誤差較小，同時(shí)回環(huán)檢測(cè)可以有效降低誤差。同時(shí)，從兩幅曲線圖也可以發(fā)現(xiàn)，在距離起點(diǎn)較遠(yuǎn)的位置處，融合前的軌跡誤差達(dá)到了整個(gè)估計(jì)值誤差中的最大值，這是由于回環(huán)檢測(cè)對(duì)距離回環(huán)距離較遠(yuǎn)的位置校正能力十分有限。在通過(guò)融合算法后，距離校正點(diǎn)最遠(yuǎn)的位置曲線顏色從紅色變?yōu)榱它S綠色，同時(shí)整個(gè)曲線的顏色向藍(lán)色偏移，即全局誤差整體顯著降低，在一定程度上驗(yàn)證了算法對(duì)于減小累積誤差的有效性。為進(jìn)一步準(zhǔn)確地評(píng)估融合優(yōu)化前后的誤差情況，下一節(jié)將融合視覺(jué)和地理真實(shí)姿態(tài)的ORB-SLAM2 算法應(yīng)用在標(biāo)準(zhǔn)數(shù)據(jù)集KITTI 上進(jìn)行實(shí)驗(yàn)。

圖5 運(yùn)動(dòng)軌跡xz 平面的y軸誤差Fig.5 The y-axiserror of the xz planeof the trajectory

3.2.2 基于標(biāo)準(zhǔn)數(shù)據(jù)集KITTI的試驗(yàn)

本節(jié)利用KITTI 數(shù)據(jù)集進(jìn)行實(shí)驗(yàn)，該數(shù)據(jù)集是國(guó)際上最大的自動(dòng)駕駛場(chǎng)景下計(jì)算機(jī)視覺(jué)算法評(píng)測(cè)數(shù)據(jù)集，部分?jǐn)?shù)據(jù)集還提供了相機(jī)的真實(shí)位姿，本實(shí)驗(yàn)所需要的地理真實(shí)姿態(tài)信息來(lái)自數(shù)據(jù)集所提供的真實(shí)位姿。為體現(xiàn)在稀疏尋北幀下的優(yōu)化效果并保證均勻性，每200個(gè)圖像關(guān)鍵幀添加一次尋北關(guān)鍵幀，以進(jìn)行融合優(yōu)化。此外，為驗(yàn)證算法在不同情況下的性能，本實(shí)驗(yàn)選取KITTI 數(shù)據(jù)集中具有多次回環(huán)的00號(hào)數(shù)據(jù)集和不存在回環(huán)的01號(hào)數(shù)據(jù)集。

1）具有多次回環(huán)的00號(hào)數(shù)據(jù)集實(shí)驗(yàn)

在本實(shí)驗(yàn)中，共添加了6幀尋北關(guān)鍵幀，利用軌跡真實(shí)值對(duì)融合前后軌跡的估計(jì)值進(jìn)行評(píng)定，融合前后誤差值如表2所示。

表2 融合前后誤差Tab.2 Error before andafter fusion

根據(jù)表2，融合后的軌跡誤差顯著下降，均值下降為融合前的62.9%，標(biāo)準(zhǔn)差下降為53.6%；融合后旋轉(zhuǎn)角誤差同樣顯著下降，均值下降為融合前的55.9%，除標(biāo)準(zhǔn)差微弱上升外，其余各項(xiàng)誤差均顯著下降。此外，將相機(jī)的軌跡誤差呈現(xiàn)在運(yùn)動(dòng)軌跡的xz平面中，如圖6所示，彩色線為估計(jì)軌跡，虛線為真實(shí)軌跡。

如圖6(a)所示，在距離回環(huán)檢測(cè)位置較遠(yuǎn)的位置，回環(huán)檢測(cè)的校正能力有限，導(dǎo)致誤差較大，因此呈現(xiàn)紅色；如圖6(b)所示，在經(jīng)過(guò)融合優(yōu)化后，全局的軌跡誤差明顯降低，誤差最嚴(yán)重的區(qū)域也從紅色變?yōu)榱它S綠色。綜上，對(duì)于00號(hào)數(shù)據(jù)集，融合后軌跡誤差和旋轉(zhuǎn)角誤差明顯降低，證明了優(yōu)化算法的有效性。

圖6 運(yùn)動(dòng)軌跡xz 平面的軌跡誤差Fig.6 The y-axiserror of the xz planeof the trajectory

2）無(wú)回環(huán)的01號(hào)數(shù)據(jù)集實(shí)驗(yàn)

上述實(shí)驗(yàn)所利用的KITTI 的00號(hào)數(shù)據(jù)集具有多次回環(huán)，為進(jìn)一步驗(yàn)證算法的綜合性能，對(duì)KITTI 的01號(hào)數(shù)據(jù)集進(jìn)行測(cè)試，該數(shù)據(jù)集并未存在回環(huán)，因此可以很好檢測(cè)在沒(méi)有回環(huán)情況下融合算法的性能，其中尋北關(guān)鍵幀共有7幀，融合前后誤差值如表3所示。

表3 融合前后誤差Tab.3 Error beforeand after fusion

根據(jù)表3，融合后的軌跡誤差有所下降，最大值下降為融合前的79.7%，標(biāo)準(zhǔn)差下降為80.9%；融合后旋轉(zhuǎn)角誤差顯著下降，均值僅為融合前的8.8%，標(biāo)準(zhǔn)差僅為16.7%?？紤]到旋轉(zhuǎn)角誤差下降明顯，進(jìn)一步分析融合算法對(duì)姿態(tài)角誤差的校正情況，姿態(tài)角變化情況如圖7所示，X軸為數(shù)據(jù)集序列號(hào)，綠線為融合前角度，藍(lán)線為融合后角度，虛線為真實(shí)角度。

圖7 姿態(tài)角Fig.7 Attitude angle

從整體和局部姿態(tài)角曲線可知，藍(lán)色曲線與虛線基本重合，而它們與綠色曲線有一定偏離，這代表融合后的俯仰角、翻滾角和偏航角與它們的真實(shí)值基本相同，而融合前角度則與真實(shí)值有一定差別。因此，融合優(yōu)化算法顯著提升了相機(jī)旋轉(zhuǎn)角的估計(jì)精度，能夠有效降低相機(jī)的姿態(tài)誤差。此外，將相機(jī)的軌跡誤差呈現(xiàn)在運(yùn)動(dòng)軌跡的xz 平面中，如圖8所示，彩色線為估計(jì)軌跡，虛線為真實(shí)軌跡。

圖8 運(yùn)動(dòng)軌跡xz平面的軌跡誤差圖Fig.8 The trajectory error of the xz planeof the motion trajectory

如圖8(a)所示，在融合前，軌跡末端（右下角所示）明顯偏離于軌跡真實(shí)值，而如圖8(b)所示，在經(jīng)過(guò)融合優(yōu)化后，軌跡曲線中代表誤差最大的深紅色變?yōu)榱顺壬?，全局的軌跡誤差有效降低。綜上，對(duì)于不存在回環(huán)的01 號(hào)數(shù)據(jù)集，融合后軌跡誤差在一定程度上被減小，姿態(tài)角誤差幾乎被消除，證明了融合優(yōu)化算法在數(shù)據(jù)集不存在回環(huán)情況下對(duì)于減小累積誤差的有效性。

4 結(jié) 論

本文設(shè)計(jì)了融合外部姿態(tài)信息的SLAM 算法，以消除系統(tǒng)的累積誤差。為有效利用真實(shí)位姿信號(hào)減小累積誤差，設(shè)計(jì)了融合視覺(jué)和地理真實(shí)姿態(tài)的SLAM算法架構(gòu)。通過(guò)標(biāo)定靶傳遞外部姿態(tài)信息，推導(dǎo)了姿態(tài)組合后的不確定度表達(dá)式，并提出了姿態(tài)圖和全局優(yōu)化的算法，獲得了校正累積誤差后的全局最優(yōu)軌跡和地圖。實(shí)驗(yàn)驗(yàn)證了融合視覺(jué)和地理真實(shí)姿態(tài)對(duì)于累計(jì)誤差消除的有效性。

考慮在室內(nèi)建模、地下救援、災(zāi)后重建等場(chǎng)景下，常規(guī)傳感器易受到環(huán)境的制約，無(wú)法穩(wěn)定的消除SLAM 系統(tǒng)中的累積誤差，導(dǎo)致系統(tǒng)在長(zhǎng)時(shí)間工作過(guò)程中定位和建圖精度嚴(yán)重下降，甚至失效。本文提出的視覺(jué)和外部姿態(tài)測(cè)量系統(tǒng)融合方案，則能在這些嚴(yán)苛環(huán)境下依然為SLAM系統(tǒng)提供真實(shí)準(zhǔn)確的外部姿態(tài)信息，同時(shí)通過(guò)算法解決了外部姿態(tài)信息相比于圖像幀過(guò)于稀疏而帶來(lái)的優(yōu)化難題，對(duì)SLAM 的應(yīng)用領(lǐng)域拓展具有重要意義。

此外，本文所使用的測(cè)姿系統(tǒng)體積較大，不能靈活移動(dòng)，會(huì)在一定程度上限制SLAM 系統(tǒng)的應(yīng)用，且室內(nèi)實(shí)驗(yàn)未能提供用于評(píng)估誤差的完整的相機(jī)運(yùn)動(dòng)軌跡。未來(lái)計(jì)劃選型其他更加便捷的測(cè)姿系統(tǒng)，并利用激光跟蹤儀等設(shè)備獲取相機(jī)的真實(shí)軌跡，開(kāi)展進(jìn)一步驗(yàn)證工作。