牛小驥，王庭蔚，葛雯斐，曠 儉

（武漢大學(xué)衛(wèi)星導(dǎo)航定位技術(shù)研究中心，武漢 430079）

基于位置的服務(wù)（Location Based Services,LBS）給目標(biāo)用戶工作生活帶來了巨大的便利，已成為日常工作生活的必備基礎(chǔ)服務(wù)。相較于單一的位置信息，位姿組合信息能更細(xì)膩地反映目標(biāo)動態(tài)，從而實現(xiàn)更準(zhǔn)確的狀態(tài)監(jiān)控。在醫(yī)療健康、運(yùn)動員訓(xùn)練等需要高精度位姿的領(lǐng)域，具有良好的應(yīng)用前景。由于全球?qū)Ш叫l(wèi)星系統(tǒng)（Global Navigation Satellite System,GNSS）信號無法穿透到室內(nèi)環(huán)境，所以室內(nèi)定位定姿通常采用WiFi[1]、超寬帶（Ultra WideBand,UWB）[2]、激光雷達(dá)[3]、視覺[4]等手段。表1 給出現(xiàn)有的室內(nèi)定位方案及其優(yōu)缺點(diǎn)。WiFi 指紋匹配方法定位精度低（例如10 m），且指紋庫不穩(wěn)定，需要頻繁更新；UWB 方法需要提前布設(shè)信號基站，安裝復(fù)雜，且受室內(nèi)環(huán)境影響無法提供精準(zhǔn)可靠的姿態(tài)信息；激光雷達(dá)方法利用激光測距原理獲得自身與周圍物體的位置關(guān)系來實現(xiàn)定位，能夠提供精準(zhǔn)的位置和姿態(tài)，然而其造價昂貴且較為笨重，無法應(yīng)用于人體穿戴等應(yīng)用場合。視覺方法通過拍攝周圍環(huán)境直接獲取物體與相機(jī)的相對位姿，能夠以低成本提供高精度的位置和姿態(tài)估計。

典型的視覺技術(shù)方案有視覺里程計（Visual Odometry,VO）。具體來說，通過單目或雙目獲取的圖像序列進(jìn)行特征提取、特征匹配與跟蹤、運(yùn)動估計的處理，完成相機(jī)的位姿更新。比較經(jīng)典的VO 系統(tǒng)有并行跟蹤與建圖（Parallel Τracking And Mapping,PΤAM）、早期的基于ORB 特征的同步定位與建圖（Oriented FASΤ and Rotated BRIEF-Simultaneous Localization and Mapping,ORB-SLAM）等。由于VO對圖像流的連續(xù)性和成像質(zhì)量有很高的要求，而往往又容易受動態(tài)和光照的影響而難以保持連續(xù)性和高成像質(zhì)量，所以視覺與慣性傳感器（Inertial Measurement Unit,IMU）組合用以獲得更好的定位定姿效果，稱之為視覺慣性里程計（Visual Inertial Odometry,VIO），代表方案包括ORB-SLAM3[5]、OpenVINS[6]等。然而，VO[7]或VIO[8]是相對定位方法，只能夠短時間內(nèi)提供高精度的位置和姿態(tài)估計結(jié)果，無法滿足需要絕對位姿的應(yīng)用需求。

為了獲得長期穩(wěn)定且可靠的絕對位置和姿態(tài)估計，通常采用預(yù)先布設(shè)二維碼或攝像頭的視覺定位技術(shù)。根據(jù)攝像頭與用戶的位置關(guān)系可分為：1）在場景中布設(shè)好攝像頭，并測定攝像頭之間的相對位姿關(guān)系，用戶在攝像頭的拍攝范圍內(nèi)移動，通過攝像頭拍攝用戶直接獲取用戶的位姿信息。2）在場景中布設(shè)二維碼，并標(biāo)定二維碼之間的相對位姿，同時將攝像頭與用戶綁定在一起，通過攝像頭拍攝周圍環(huán)境獲取攝像頭的位姿信息從而反映出用戶的位姿信息。前一類方案的典型應(yīng)用主要由國外OptiΤrack[9]和國內(nèi)的NOKOV[10]，通常由數(shù)個高速攝像機(jī)環(huán)繞目標(biāo)進(jìn)行擺放，從不同角度對貼在目標(biāo)關(guān)鍵節(jié)點(diǎn)（如人體的關(guān)節(jié)處）的特征標(biāo)簽進(jìn)行跟蹤來完成全身動作的捕捉[11]，能達(dá)到毫米級的定位精度，在特效電影和虛擬現(xiàn)實[12]（Virtual Reality,VR）中獲得了廣泛的應(yīng)用。然而，由于設(shè)備高昂、應(yīng)用成本高，需要特定環(huán)境搭建，操作上也需要專業(yè)人士指導(dǎo)等問題，無法應(yīng)用到普通大眾用戶。后一類方案依賴VO 或VIO 的相對定位能力，并通過周期性地檢測到布設(shè)的二維碼進(jìn)行絕對校正，從而實現(xiàn)長期連續(xù)可靠的高精度位姿跟蹤[13]。利用攝像頭拍攝二維碼獲得其位置，協(xié)助機(jī)器人或用戶在室內(nèi)環(huán)境中進(jìn)行定位。上述論文中利用了二維碼進(jìn)行了輔助定位，并取得較好的效果。然而，這類方案依賴于連續(xù)圖像的可靠性，不能容忍圖像出現(xiàn)丟失或者模糊現(xiàn)象，穩(wěn)定性有待進(jìn)一步提高。

為了實現(xiàn)低成本且高精度的用戶位置和姿態(tài)的穩(wěn)健估計，本文提出了一種基于視覺二維碼/慣性融合的定位定姿方法。該方法使用單個攝像頭、單個微機(jī)械慣性傳感器（MEMS-IMU）和ArUco 碼（一種OpenCV中的標(biāo)識二維碼），以捷聯(lián)慣導(dǎo)算法為核心，通過拍攝預(yù)先布設(shè)且坐標(biāo)已知的ArUco 碼獲得高精度的位置修正信息，并使用擴(kuò)展卡爾曼濾波進(jìn)行信息緊耦合。該方法將視覺圖像作為稀疏的校正信息，當(dāng)視覺圖像出現(xiàn)丟失或模糊時仍然能夠利用IMU 進(jìn)行連續(xù)可靠的推算。更進(jìn)一步，該方法基于事后處理模式的優(yōu)勢，采用了反向平滑算法融合未來時刻觀測信息以提升當(dāng)前時刻的定位定姿精度，能夠更有效地應(yīng)對攝像頭短暫遮擋或圖像模糊等問題，解決傳統(tǒng)方案依賴于連續(xù)圖像的可靠性，不能容忍圖像出現(xiàn)丟失或者模糊現(xiàn)象，進(jìn)一步增強(qiáng)系統(tǒng)的魯棒性。

1 理論與方法

主流的視覺定位方法（例如VO 或VIO）通常假設(shè)圖像是連續(xù)可用的，然而實際應(yīng)用中這是難以保障的。針對這一典型問題，本文選擇以慣性導(dǎo)航為核心，充分發(fā)揮慣性導(dǎo)航完全自主工作、不受外部環(huán)境影響的優(yōu)點(diǎn)；并結(jié)合稀疏的高精度視覺觀測信息校正，能在視覺圖像連續(xù)性差和可靠性低的情況下，形成一個連續(xù)可靠的高精度定位定姿方案。圖1 給出本文方法主要算法流程。本文采用IMU/視覺緊耦合算法框架，首先進(jìn)行慣導(dǎo)機(jī)械編排解算，同時檢測二維碼角點(diǎn)，并結(jié)合預(yù)先測量的角點(diǎn)坐標(biāo)修正慣導(dǎo)誤差。為了應(yīng)對二維碼角點(diǎn)檢測失敗的情況，算法除了利用二維碼角點(diǎn)構(gòu)建觀測方程，還實時估計環(huán)境中新的特征點(diǎn)坐標(biāo)，在二維碼角點(diǎn)檢測失敗的情況下，利用這些新的特征點(diǎn)構(gòu)建觀測方程，有效地維持系統(tǒng)的定位定姿精度。更重要地是，該方法基于離線處理模式的優(yōu)勢，采用反向平滑算法進(jìn)一步增強(qiáng)系統(tǒng)的魯棒性。

圖1 基于二維碼的高精度視覺/IMU 融合定位定姿算法框圖Fig.1 Block diagram of high-precision vision/IMU fusion positioning and attitude determination algorithm based on QR code

1.1 慣導(dǎo)機(jī)械編排

機(jī)械編排是經(jīng)典的慣性導(dǎo)航算法，通過對IMU 輸出的角速度和比力進(jìn)行積分，并結(jié)合前一時刻的位置、速度和姿態(tài)，從而獲得用戶當(dāng)前的位置、速度和姿態(tài)。由于MEMS-IMU 器件性能較低，微小誤差改正項對導(dǎo)航性能的提升沒有明顯幫助。因此，本文忽略微小的誤差改正量（例如地球自轉(zhuǎn)、圓錐效應(yīng)和劃槳效應(yīng)等），且參考系(本文稱n 系)選擇室內(nèi)自定義的坐標(biāo)系，具體如下[14]：

1.2 濾波設(shè)計

基于MEMS-IMU 的慣導(dǎo)算法將會在幾秒時間內(nèi)迅速累計幾米位置誤差，無法滿足用戶需求[15]，因此需要融合其他可用信息間斷地對慣導(dǎo)推算的位置進(jìn)行有效校正?？柭鼮V波（Kalman Filter,KF）是最常用的數(shù)據(jù)融合方法之一。然而，經(jīng)典的卡爾曼濾波是基于線性系統(tǒng)的，為了應(yīng)對非線性問題，本文采用擴(kuò)展卡爾曼濾波器（Extended Kalman Filter,EKF）。其中，15 維誤差狀態(tài)向量構(gòu)建如下：

式(2)中，δ rn，δ vn，ψ1×3分別為n 系下位置誤差、速度誤差和姿態(tài)誤差；bg和bf分別為陀螺零偏和加表零偏。離散后的誤差模型表示如下：

式(3)中，下標(biāo)k,k-1為歷元編號，δxk,k-1為誤差狀態(tài)預(yù)測值，Φk,k-1為誤差狀態(tài)轉(zhuǎn)移矩陣，δxk-1,k-1為誤差狀態(tài)估計值，w k為系統(tǒng)噪聲和測量噪聲。對式(1)進(jìn)行擾動分析得到：

式(4)中，I3×3為3×3 的單位矩陣；Δt為相鄰歷元的時間間隔；為加速度計觀測值在n 系下的投影。

1.3 視覺量測更新

本文通過拍攝人工布設(shè)的二維碼（坐標(biāo)已知），實現(xiàn)稀疏圖像條件下的高精度位置和姿態(tài)估計。

1.3.1 ArUco 碼布設(shè)及坐標(biāo)測量

本文采用的二維碼為OpenCV 中ArUco 碼，為了保障攝像頭能夠拍攝到ArUco 碼，本文在房間內(nèi)的四壁張貼一定數(shù)量的ArUco 碼。采用自定義的當(dāng)?shù)厮阶鴺?biāo)系，x 軸與y 軸構(gòu)成的平面平行于當(dāng)?shù)厮矫妫瑉軸與x-y 平面垂直指向地面，其中坐標(biāo)系原點(diǎn)和x 軸指向可由用戶自定義在房間地面上某處。然后，將全站儀架設(shè)在坐標(biāo)系原點(diǎn)，并將全站儀初始方向?qū)Rx軸。在ArUco 碼的坐標(biāo)測量階段，首先拍攝一張覆蓋墻上所有ArUco 碼的圖像，其次使用Leica ΤS60 全站儀測出分布較遠(yuǎn)且不在同一直線上的ArUco 碼四個角點(diǎn)的坐標(biāo)，接著利用平面約束和坐標(biāo)轉(zhuǎn)換關(guān)系解算出所有ArUco 碼的角點(diǎn)在世界系下坐標(biāo)，最后將ArUco碼角點(diǎn)的ID 號及對應(yīng)的三維坐標(biāo)存儲，即完成坐標(biāo)測量工作（即ArUco 碼建庫）。同時，基于已有文獻(xiàn)闡述的ArUco 碼距離與識別效果的關(guān)系[16]，本文采取的拍攝距離約1～5 m。

1.3.2 ArUco 碼角點(diǎn)量測更新

觀測量z為像平面坐標(biāo)系下ArUco 碼角點(diǎn)像素坐標(biāo)的預(yù)測值（基于慣導(dǎo)推算的位姿）和觀測值（基于圖像處理提取角點(diǎn)）的差，具體描述如下：

接著，將式(7)代入到式(6)，進(jìn)行擾動分析并保留一階項，得到ArUco 碼角點(diǎn)的緊組合量測方程：

式(8)中，量測矩陣由H1和H2相乘得到，x為系統(tǒng)誤差觀測向量，n為觀測誤差向量。H1和H2可由對式(6)(7)分別進(jìn)行擾動分析并保留一階項得到：

1.3.3 三角化量測更新

視覺/IMU 緊組合量測更新依賴于相機(jī)正確地檢測并識別到ArUco 碼。而由于設(shè)備的晃動和受ArUco碼排布的密度影響，檢測不到ArUco 碼的情況時有發(fā)生。為了應(yīng)對這種情況，當(dāng)檢測到ArUco 碼時，同時對非ArUco 碼的其余環(huán)境特征進(jìn)行三角化估計，將估計的位置存入滑窗當(dāng)中；當(dāng)檢測不到ArUco 碼時，濾波器依靠之前三角化后的其余環(huán)境特征構(gòu)建量測方程，從而保證濾波結(jié)果的穩(wěn)定，提升系統(tǒng)的魯棒性。

假設(shè)一個特征點(diǎn)p被相機(jī)多次拍到，第1 次被觀測到的相機(jī)坐標(biāo)系表示為c(1)，第i次被觀測到的相機(jī)坐標(biāo)系表示為c(i) (i＜N，N為滑動窗口大小)，由于拍攝時刻的相機(jī)位姿已知，則可以將第一次觀測到的特征點(diǎn)p投影到i時刻的相機(jī)坐標(biāo)系中：

如圖2 所示，此時沒有檢測到ArUco 碼，但是可以通過提取其他環(huán)境特征。并通過前面的三角化估計非ArUco 碼特征點(diǎn)的三維位置，此時仍然可以繼續(xù)進(jìn)行濾波優(yōu)化，得到當(dāng)前相機(jī)位姿的最優(yōu)估計值。最后，當(dāng)前時刻的相機(jī)位姿、特征點(diǎn)的坐標(biāo)與當(dāng)前時刻進(jìn)行關(guān)聯(lián)后，作為滑動窗口內(nèi)最新時刻的相機(jī)信息進(jìn)行儲存，如果滑動窗口已滿，則剔除最舊的相機(jī)時刻信息。當(dāng)然，這種在滑窗內(nèi)三角化的特征點(diǎn)只起到ArUco 碼角點(diǎn)識別失敗情況下維持濾波的輔助作用，宏觀的絕對位置修正還是依賴識別出的ArUco 碼角點(diǎn)。

圖2 ArUco 碼檢測失敗Fig.2 ArUco code detection failed

1.4 平滑算法

由于載體高動態(tài)或攝像頭被遮擋的條件下難以保證拍攝圖像的質(zhì)量，本文加入平滑算法進(jìn)一步提升系統(tǒng)的定位定姿精度和可靠性。平滑算法能夠充分利用過去、現(xiàn)在以及將來的觀測信息提升當(dāng)前位置和姿態(tài)的估計精度，是事后處理模式的必然選擇。平滑算法思路具體描述如下：在計算固定區(qū)間[ 0,n]平滑值時，先進(jìn)行前向卡爾曼濾波，記錄下每一步的狀態(tài)轉(zhuǎn)移矩陣Φk,k-1、預(yù)測誤差協(xié)方差陣Pk,k-1和濾波結(jié)果x、濾波協(xié)方差陣Pk。然后再從最末時刻向最初時刻進(jìn)行平滑，最末時刻的狀態(tài)估計為后向平滑的初始狀態(tài)估計[18]。

狀態(tài)平滑估計值：

其中，Ak為平滑增益矩陣，xk為k時刻的前向濾波結(jié)果，為k+1時刻的平滑結(jié)果，為k+1時刻前向濾波結(jié)果。

平滑增益：

式(13)中，Pk，分別為k和k+1時刻協(xié)方差陣，為k時刻轉(zhuǎn)移矩陣。

平滑誤差協(xié)方差陣為：

式(14)中，Pk，分別為k和k+1時刻協(xié)方差陣，為k時刻轉(zhuǎn) 移矩陣。式(12)-(14) 中，k=n-1,n-2 … 0，n為觀測信息總數(shù)。

2 實驗描述

2.1 實驗設(shè)備及場景

測試場景為空曠的房間，包含兩個窗戶和一扇門，尺寸為7 m×6.7 m×2.8 m，位于武漢大學(xué)詩琳通地球空間信息技術(shù)協(xié)同創(chuàng)新中心大樓四樓。除了門和窗戶區(qū)域，房間其他區(qū)域都布設(shè)了大小為20 cm×20 cm 的ArUco 碼。具體布設(shè)分布和場景如圖3 所示。

圖3 測試場景中ArUco 碼分布Fig.3 ArUco code distribution in test scenario

相機(jī)型號為MYNΤ EYE D50 雙目攝像頭（以下簡稱小覓相機(jī)，本文僅使用單目），搭載同步采集的IMU 模塊（BMI088）。本實驗的圖像采集僅使用小覓相機(jī)的左目相機(jī)，圖像分辨率為1280×720。為了獲取高精度的位置和姿態(tài)參考，數(shù)據(jù)采集階段將Leica360 °棱鏡、SΤIM300（高精度微機(jī)械慣導(dǎo)）和小覓相機(jī)固定在一起，安裝在三腳架上組成本次實驗的采集模塊，安裝關(guān)系如圖4 所示。其中，Leica ΤS60 用于提供位置參考值，SΤIM300 與ΤS60 組合提供連續(xù)姿態(tài)參考（定位精度：1 mm，定姿精度：0.03 °）。小覓相機(jī)自帶的IMU 與SΤIM300 的器件參數(shù)如表2 所示。為了確定各坐標(biāo)系之間的轉(zhuǎn)換關(guān)系，實驗開始前用Kalibr工具測出小覓相機(jī)左目相機(jī)坐標(biāo)系（c 系）與其自帶的IMU 坐標(biāo)系（b 系）的外參、SΤIM300 坐標(biāo)系（l系）與左目相機(jī)坐標(biāo)系（c 系）的外參，并測量Leica棱鏡中心與左目相機(jī)坐標(biāo)系（c 系）的桿臂關(guān)系。

圖4 小覓相機(jī)、棱鏡、SΤIM300 安裝關(guān)系（白色坐標(biāo)框架為參考設(shè)備，紅色坐標(biāo)框架為實驗設(shè)備）Fig.4 Installation relationship of MYNΤEYE,prism,and SΤIM300 (the white coordinate frame is the reference equipment,and the red coordinate frame is the experimental equipment)

表2 實驗中兩個IMU 的關(guān)鍵參數(shù)Tab.2 The key parameters of the two IMUs in the experiment

由于Leica ΤS60 只提供低頻率的高精度位置，且不能提供準(zhǔn)確的時間戳。因此，精度評估實驗時，通過使用特定的測試流程實現(xiàn)參考系統(tǒng)與被測系統(tǒng)的時間對齊。首先，測試者手持采集模塊沿著地磚縫行走，并且每隔一塊地磚將采集模塊靜止3-5 s，重復(fù)該流程，直到測試結(jié)束。然后，根據(jù)坐標(biāo)變化提取出靜止時刻，與SΤIM300 的靜止時刻相關(guān)聯(lián)，并使用靜止時段高精度坐標(biāo)與SΤIM300 進(jìn)行組合解算，得到被測系統(tǒng)連續(xù)運(yùn)動的參考位姿。測試數(shù)據(jù)總時長約為20 min。其中，三腳架每運(yùn)動約10 s，就靜止約3～5 s，共持續(xù)96 次，參考軌跡如圖5 所示。

圖5 參考軌跡Fig.5 Reference trajectory

圖6 方案1、2、4 與參考系統(tǒng)的定位定姿差異（視覺觀測正常）Fig.6 Τhe positioning and attitude differences between scheme 1,2,4 and the reference system (normal visual observation)

2.2 結(jié)果分析及討論

本部分使用實測數(shù)據(jù)評估本文所提方法的位姿估計性能。為了分析提出的方案各部分的性能，共評估四種方案的位姿估計性能，具體包括：

1）純視覺[19]定位定姿；

2）視覺/IMU 緊組合[20]定位定姿；

3）方案2 的基礎(chǔ)上，啟用環(huán)境特征點(diǎn)三角化輔助（具體見1.3.3）；

4）方案3 的基礎(chǔ)上，進(jìn)行反向平滑（具體見1.4）。

視覺觀測正常情況下，環(huán)境特征點(diǎn)三角化未達(dá)到啟用條件，方案2 與方案3 沒有差異。圖7 給出了視覺圖像觀測正常條件下方案1、2、4 與參考系統(tǒng)的位姿估計差異?？梢钥闯?，在視覺觀測正常條件下，四種方案的定位誤差均在10 cm 以內(nèi)，定姿差異在0.7 °以內(nèi)。相較于方案1，方案2 和4 獲得了更平穩(wěn)的位姿估計結(jié)果，表明IMU 的相對定位能力達(dá)到了較好的平滑效果。

圖7 給出了位置和姿態(tài)差異累積分布圖，表3-4給出了位置和姿態(tài)差異的均方根和CEP95 統(tǒng)計值。由于ArUco 碼布設(shè)在平整的同一墻面，視覺觀測能夠很好地將相機(jī)位姿約束到與墻面平行的狀態(tài)（即俯仰角和航向角），故ArUco 碼的定姿精度主要由橫滾角精度來體現(xiàn)。可以看出，方案1、2、4 的定位精度和俯仰角和航向角精度基本一致，而方案2 和4 估計的橫滾角精度優(yōu)于方案1，性能分別提升50%和62.5%。最終，方案4 能夠提供3.17 cm（RMS）和7.20 cm（95%）的位置和0.1°（RMS）和0.21°（95%）的姿態(tài)。

表3 視覺觀測正常時方案1、2、4 的位置誤差均方根和CEP95Tab.3 RMS and CEP95 of the position error of scheme 1,2,and 4 when the visual observation is normal

表4 視覺觀測正常時方案1、2、4 的姿態(tài)差異均方根和CEP95Tab.4 RMS and CEP95 of the attitude differences of scheme 1,2,and 4 when the visual observation is normal

為了評估本文方法在視覺觀測中斷情況下的位姿估計性能，通過手動關(guān)閉ArUco 角點(diǎn)檢索和關(guān)閉視覺圖像兩種方式來模擬。具體設(shè)置如下：選擇5 個10 s區(qū)間，區(qū)間內(nèi)檢測不到ArUco 碼角點(diǎn)；選擇5 個10 s區(qū)間，區(qū)間內(nèi)圖像觀測完全中斷；這兩組區(qū)間的時段不重疊。如圖8 所示，第一種中斷為虛線框，第二種中斷為實線框。

圖8 方案1、2、3、4 與參考系統(tǒng)的定位定姿差異（視覺觀測中斷）Fig.8 Τhe positioning and attitude differences between scheme 1,2,3,4 and the reference system (visual observation interruption)

在檢測不到ArUco 角點(diǎn)和關(guān)閉視覺圖像的時間窗口內(nèi)，方案1 均無定位定姿數(shù)據(jù)輸出；方案2 在角點(diǎn)中斷和關(guān)閉視覺圖像時均有發(fā)散，這是因為方案2 中使用的IMU/視覺緊組合算法會在此兩種情況下僅依靠慣導(dǎo)推算，而慣導(dǎo)推算的位姿有隨時間快速發(fā)散的特性，導(dǎo)致中斷期間定位結(jié)果快速發(fā)散，雖然試驗進(jìn)程中載體停止3-5 s 可以采用零速修正限制慣導(dǎo)誤差，但由于間隔校準(zhǔn)時間較長，無法滿足厘米級的定位精度；方案3 在關(guān)閉ArUco 角點(diǎn)下可正常運(yùn)行，但在圖像關(guān)閉條件下發(fā)散，這是因為方案3 在ArUco 角點(diǎn)中斷前正常運(yùn)行時提取了環(huán)境中的非ArUco特征點(diǎn)并三角化出這些特征點(diǎn)的三維位置，在不能觀測到完整ArUco 碼時仍可以用它們作為圖像坐標(biāo)觀測值；方案4在角點(diǎn)中斷和圖像中斷的條件下均能正常運(yùn)行，這是因為方案4 在方案3 的基礎(chǔ)上加入的反向平滑算法，利用了未來時刻的觀測來優(yōu)化填補(bǔ)這部分丟失的精度。

圖9 給出方案1、2、3、4 的位置和姿態(tài)誤差累積分布圖。表5-6 給出了位置和姿態(tài)差異的均方根和CEP95?？梢钥闯?，在定位方面，方案3、4 明顯優(yōu)于方案2，方案4 略優(yōu)于方案3。最終，方案4 能夠提供3.21 cm（RMS）和7.35 cm（95%）的位置和0.1 °（RMS）和0.21 °（95%）的姿態(tài)，與視覺觀測正常時緊組合正向濾波的精度水平相當(dāng)。

圖9 視覺觀測存在中斷時方案1、2、3、4 與參考系統(tǒng)的定位定姿誤差累計密度函數(shù)Fig.9 Cumulative density function of positioning and attitude errors between schemes 1,2,3,4 and the reference system when visual observation is interrupted

表5 視覺觀測存在中斷時方案1、2、3、4 的位置誤差均方根和CEP95Tab.5 RMS and CEP95 of the position error of scheme 1,2,3 and 4 when the visual observation is interrupted

表6 視覺觀測存在中斷時方案1、2、3、4 的姿態(tài)差異均方根和CEP95Tab.6 RMS and CEP95 of the attitude differences of scheme 1,2,3 and 4 when the visual observation is interrupted

2.3 ArUco 碼密度的影響分析

表7 給出了6 組ArUco 碼的布設(shè)密度情況，圖10為6 組布設(shè)密度與本文系統(tǒng)的定位定姿的精度關(guān)系。統(tǒng)計的誤差統(tǒng)計量為靜止期間位置誤差最大值（SPE_MAX）、本文系統(tǒng)定位結(jié)果誤差（95%概率，WPD_95）、本文系統(tǒng)與參考系統(tǒng)的姿態(tài)差異（95%概率，WAD_95）。從誤差統(tǒng)計結(jié)果可以看出，當(dāng)ArUco碼密度大于1 個/m2時，定位定姿的精度沒有出現(xiàn)明顯的下降，這在實際應(yīng)用場景中是容易滿足的。

圖10 ArUco 碼鋪設(shè)密度和定位定姿精度的關(guān)系Fig.10 Τhe relationship between ArUco code laying density and positioning accuracy

表7 ArUco 碼的布設(shè)密度Tab.7 The layout density of ArUco code

3 結(jié)論

為了獲得較高動態(tài)下精確穩(wěn)定的室內(nèi)定位定姿信息，本文以視覺/IMU 融合為框架，以三維坐標(biāo)已知的室內(nèi)ArUco 碼為視覺識別對象；并利用ArUco 碼角點(diǎn)附近被三角化的其它特征點(diǎn)作為補(bǔ)充特征，以供ArUco 碼檢測不到時使用，保證了系統(tǒng)運(yùn)行的連續(xù)性；并在最后使用RΤS 反向平滑算法，進(jìn)一步增強(qiáng)了系統(tǒng)的魯棒性。實驗表明，相較于傳統(tǒng)的視覺/慣導(dǎo)緊組合來說，本文設(shè)計的算法能應(yīng)對視覺條件不良的情況，能夠連續(xù)可靠地實現(xiàn)3.2 cm（RMS）的定位精度，并達(dá)到與參考系統(tǒng)的定姿差異優(yōu)于0.1 °（RMS）。