李雪強，李建勝，王安成，羅豪龍，楊子迪，李凱林

（1.信息工程大學 地理空間信息學院，鄭州 450000；2.61618 部隊，北京 100080）

室外定位通常是依靠全球?qū)Ш叫l(wèi)星系統(tǒng)（Global Navigation Satellite System,GNSS）來實現(xiàn)，然而在衛(wèi)星拒止的室內(nèi)環(huán)境中，GNSS 信號往往失效。近年來，同時定位與建圖（Simultaneous Localization and Mapping,SLAM）技術(shù)的發(fā)展很大程度上解決了GNSS信號缺失情況下的定位問題。

視覺SLAM 憑借成本低、體積小、精度高等優(yōu)勢被廣泛應(yīng)用于各類室內(nèi)移動導航平臺，但是在純旋轉(zhuǎn)、尺度不確定等退化場景下存在缺陷。因此，在實際應(yīng)用中需引入慣性測量單元（Inertial Measurement Unit,IMU）進行組合，使得兩種傳感器特性互補，組合后的視覺慣性里程計[1,2]（Visual-inertial Odometry,VIO）可以實現(xiàn)可靠、實時的位姿估計。VIO 本質(zhì)上是一種局部定位算法，在沒有其他傳感器信息輔助和回環(huán)檢測的情況下，系統(tǒng)定位誤差隨時間的推移而累積。對于零加速度和恒加速的特定運動，VIO 尺度變得不可觀[3]，因此，目前許多學者設(shè)計全局的路標標志或利用其他全局信息[4,5]進行輔助，從而提高VIO 長時間運行的適用性。

在射頻識別（Radio Frequency Identification,RFID）、ZigBee、藍牙等能提供全局位置信息的室內(nèi)無線定位技術(shù)研究中，超寬帶（Ultra-wideband,UWB）技術(shù)因其定位精度高且不易受干擾而受到廣泛關(guān)注，理論研究發(fā)展較為成熟，但由于多徑效應(yīng)、非視距（Non Line of Sight,NLOS）等因素的影響，導致部分距離測量精度較低，進而影響定位結(jié)果[6]。

鑒于VIO 和UWB 各自定位存在的問題，引入UWB 全局定位信息可以校正VIO 長時間運行的累積誤差，而VIO 可以減小NLOS 對UWB 定位結(jié)果的影響，兩者具有良好的互補性。對于多傳感器融合，現(xiàn)仍有大量方法采用卡爾曼濾波（Kalman Filter,KF）及其變體[7,8]。文獻[9][10]提出基于濾波松耦合的VIO/UWB 多傳感器融合方法，并依賴于組合系統(tǒng)的狀態(tài)方程。然而對于移動機器人，狀態(tài)方程是高度非線性的，并且在一些復雜情況下難以建立，將系統(tǒng)狀態(tài)方程簡化為勻速或勻加速運動，不可避免帶來估計誤差以及對UWB 異常值較為敏感。不同于傳統(tǒng)的濾波方法，文獻[11][12]提出基于圖優(yōu)化的松耦合全局位姿估計框架。文獻[11]沒有考慮UWB 非視距情況；文獻[12]則是對上一時刻估計位置與基站的距離和當前測距值進行比較，通過設(shè)定閾值來判斷NLOS 異常值。

抗差估計多用于濾波方法中抵御粗差的影響[13,14]。在上述文獻研究成果的基礎(chǔ)上，不同于以往將抗差用于濾波，本文將抗差估計應(yīng)用于非線性優(yōu)化中，提出了一種基于抗差的單目VIO-UWB 多傳感融合定位算法，提高室內(nèi)復雜條件下定位的精度和魯棒性。主要創(chuàng)新點為：1）構(gòu)建了UWB 與VIO 圖優(yōu)化框架，利用UWB 全局定位結(jié)果對VIO 定位結(jié)果進行優(yōu)化，并在仿真數(shù)據(jù)集和真實場景下對算法進行驗證；2）將抗差估計加入后端優(yōu)化過程，進行重優(yōu)化以實時調(diào)整傳感器之間的權(quán)值，提高算法在UWB 非視距情況下的定位精度，進而提升組合系統(tǒng)的穩(wěn)定性和魯棒性。

1 方法與原理

1.1 UWB 定位算法

通常，UWB 系統(tǒng)通過飛行時間（Τime of Flight,ΤoF）測量UWB 基站（Anchor）與標簽（Τag）的距離，采用基于距離的方法進行定位。對于二維平面定位來說，需要三個及以上基站到標簽之間的距離才能確定位置，而對于三維空間定位，則需要四個及以上基站到標簽之間的距離[15]。

三維空間標簽的位置計算公式為：

其中，(x j,y j,z j)(j=1,2…N)為第j個UWB 基站位置；(x,y,z) 為標簽的真實位置；d j為第j個基站到標簽的真實距離值；為第j個基站到標簽距離的測量值；ηj為第j個基站到標簽測量距離的噪聲。

采用最小二乘估計法[16]求解標簽坐標估計值。

1.2 VIO 定位算法

使用基于滑動窗口的緊耦合單目VIO 優(yōu)化框架，如圖1 所示，將視覺特征信息與IMU 預積分結(jié)果共同輸入非線性優(yōu)化的滑動窗口內(nèi)，以獲得最大后驗估計，得出VIO 局部位姿估計結(jié)果。

滑動窗口中的完整狀態(tài)向量定義為：

通過滑動窗口因子圖模型構(gòu)建非線性優(yōu)化總體目標代價函數(shù)：

1.3 VIO/UWB 組合定位算法

為減少VIO 算法的累積漂移，長期運行后仍可得到準確可靠的定位數(shù)據(jù)，對UWB 與VIO 輸出的定位數(shù)據(jù)進行非線性松耦合融合解算，如圖2 所示。在VIO/UWB 組合定位算法中，以IMU 中心為原點，UWB 定位過程中所使用到的獨立坐標系三個軸方向建立全局坐標系。

圖2 UWB/VIO 組合算法后端優(yōu)化Fig.2 Back-end optimization of VIO/UWB positioning algorithm

完整全局變量為：

其中，W代表全局坐標系標志；s表示滑動窗口內(nèi)待優(yōu)化的狀態(tài)向量個數(shù)；待優(yōu)化狀態(tài)向量包括全局坐標系下第i幀平移向量和旋轉(zhuǎn)四元數(shù)。

松耦合優(yōu)化方程為：

1.3.1 VIO 殘差

由于局部估計在較小范圍內(nèi)是準確的，可以利用兩幀之間的相對位姿來構(gòu)建局部因子，即VIO 兩幀之間的變換與全局兩幀之間的變換差：

根據(jù)基于相鄰兩幀之間相對姿態(tài)構(gòu)建的局部因子殘差公式，對待優(yōu)化變量求偏導數(shù)，得到相應(yīng)的Jacobian 矩陣。

1.3.2 UWB 殘差

由于UWB 只能得到位置信息，因此可以將當前幀的UWB 解算位置轉(zhuǎn)到全局坐標系下，再與待估位置參數(shù)之差來構(gòu)建UWB 因子，如式(10)所示。

根據(jù)全局UWB 位置約束因子的殘差公式，對待優(yōu)化變量求偏導數(shù)，得到相應(yīng)的Jacobian 矩陣。

1.4 基于抗差估計的VIO/UWB 自適應(yīng)組合定位算法

在非視距環(huán)境下，UWB 測距值含有較大誤差，難以保證定位精度。如果優(yōu)化過程中仍采用權(quán)值初值，就會造成組合系統(tǒng)位姿精度的損失。為了減少觀測值粗差對算法的影響，采用抗差估計的方法減弱粗差對位姿精度的影響。算法流程框架如圖3 所示，在第一次非線性優(yōu)化后進行抗差估計，自適應(yīng)調(diào)節(jié)UWB 定位權(quán)重因子，然后用更新的權(quán)值進行重優(yōu)化。

圖3 基于抗差估計的VIO/UWB 組合算法框架Fig.3 Structure of VIO/UWB integrated algorithm framework based on robust estimation

抗差估計采用IGGⅢ權(quán)函數(shù)[18]：

2 實驗驗證與分析

2.1 開源數(shù)據(jù)集仿真實驗

由于目前公開的開源視覺、慣性和UWB 組合數(shù)據(jù)集較少，因此本文在采用公開的視覺慣性SLAM 權(quán)威數(shù)據(jù)集EuRoc[19]基礎(chǔ)上，根據(jù)數(shù)據(jù)集軌跡真值數(shù)據(jù)，仿真出UWB 數(shù)據(jù)，從而進行視覺/慣性/UWB 組合系統(tǒng)實驗。

在仿真數(shù)據(jù)中，將EuRoc 數(shù)據(jù)集提供的載體位置真值坐標系作為世界坐標系，并根據(jù)運行軌跡設(shè)置四個UWB 基站，使載體軌跡包含在基站所構(gòu)成的空間內(nèi)。文獻[20]對UWB 測距誤差進行了深入研究，發(fā)現(xiàn)在LOS 和NLOS 條件下分別服從高斯和伽馬分布，并采用高斯和伽馬分布結(jié)合對LOS 和NLOS 混合環(huán)境測距誤差模型進行準確建模。測距誤差分布的概率密度函數(shù)f(d) 如式(13)所示。

其中，d為測距誤差；u和σd為高斯分布對應(yīng)的均值和標準差；λ和k為伽馬分布對應(yīng)的參數(shù)；c0為固定誤差。

數(shù)據(jù)集實驗以及后續(xù)對比實驗所用的計算機處理器是Intel(R) Xeon(R) Silver 4110 CPU @ 2.10GHz×16，操作系統(tǒng)為Ubuntu 18.04.6 LΤS，都是在機器人操作系統(tǒng)（Robot Operating System,ROS）下運行。

2.1.1 組合定位性能分析

為了驗證基于松耦合優(yōu)化VIO/UWB 組合定位算法的可行性，使用EuRoc 數(shù)據(jù)集全序列仿真LOS 條件下UWB 測距數(shù)據(jù)，并與VINS_Mono、UWB 定位算法進行比較。

圖4 顯示了UWB、VINS_Mono 和VIO/UWB 組合算法在MH04 困難序列上的軌跡和絕對位姿誤差（Absolute Pose Error,APE）。圖4(a)中Refernce 為軌跡參考真值，圖4(b)中數(shù)據(jù)記錄時間為ROS 時間戳，故橫坐標時間為刻度值與右下角用科學計數(shù)法表示的數(shù)值之和。從圖4(a)可以看出，與UWB、VINS_Mono相比，VIO/UWB 組合算法估計出的軌跡與真實軌跡有更好的擬合度。圖4(b)顯示了VIO/UWB 組合算法的大部分定位估計誤差在0.1 m 以內(nèi)，表明UWB 傳感器能夠提供穩(wěn)定的約束，進而使組合算法的定位結(jié)果保持較高精度。

圖4 MH04 序列：算法軌跡與定位精度比較Fig.4 MH04 sequence: comparison of trajectories and positioning accuracy of the algorithms

表1 給出不同傳感器組合在EuRoc 數(shù)據(jù)集上的位置均方根誤差（Root-mean-squared Error,RMSE）。從表1 可以看出，UWB 定位算法的定位誤差大約為0.17 m，即UWB 在良好視距環(huán)境下定位精度相對穩(wěn)定。相比于VINS_Mono，VIO/UWB 組合算法在全序列下定位精度提升27.94%～85.19%不等，平均提升66.72%；相比于UWB 在全序列下定位精度提升55.84%～82.62%不等，平均提升69.27%。表明組合系統(tǒng)引入UWB 傳感器可以提供全局的位置約束，進而有效提高系統(tǒng)性能。

表1 不同傳感器組合定位精度對比Tab.1 Comparison of positioning accuracy of different integrated sensors

2.1.2 遮擋下組合定位分析

為了研究UWB 在短時間遮擋情況下對組合算法的影響，以及驗證基于抗差估計的VIO/UWB 組合算法在NLOS 環(huán)境下的有效性，本實驗使用MH 全序列和V102序列模擬UWB在NLOS情況下的測距數(shù)據(jù)。

圖5(a)顯示了V102 序列上兩個時間段內(nèi)模擬非視距場景下的UWB 定位誤差，圖5(b)顯示了該序列重優(yōu)化后的UWB 權(quán)重值變化。從圖中可以看出，在視距階段存在個別突變誤差，UWB 權(quán)值進行了相應(yīng)降權(quán)調(diào)整，而在非視距階段因其誤差太大，導致調(diào)整后的權(quán)值趨于0，說明抗差模型可以根據(jù)誤差大小進行權(quán)值調(diào)整。

圖5 V102 序列：UWB 定位輸出和權(quán)重變化Fig.5 V102 sequence: UWB positioning output and weight change

為了更加具體比較VIO/UWB 組合算法與基于抗差估計的VIO/UWB 組合算法的差異性，圖6 給出了不同算法在V102 序列上的軌跡和定位誤差結(jié)果，圖中Proposed 代表基于抗差估計的VIO/UWB 組合算法。從圖6(a)可以看出，基于抗差估計的VIO/UWB 組合算法的定位軌跡與真實軌跡的貼合程度優(yōu)于不加抗差估計的VIO/UWB 組合算法，尤其是在拐角轉(zhuǎn)彎處和非視距環(huán)境下。從圖6(b)可以看出，不加抗差估計的VIO/UWB 組合算法的大部分定位估計誤差在0.25 m以內(nèi)，但是在非視距環(huán)境下其定位估計誤差不穩(wěn)定，最高可達到將近0.5 m。改進后的基于抗差估計的VIO/UWB 組合算法的定位誤差絕大部分在0.2 m 以內(nèi)，可有效抑制非視距誤差帶來的影響?？芍?，即使UWB 在短暫遮擋下，基于抗差估計改進后的組合算法依舊能輸出較高精度的定位結(jié)果，對非視距環(huán)境具有良好魯棒性。

圖6 V102 序列：算法軌跡與定位精度比較Fig.6 V102 sequence: comparison of trajectories and positioning accuracy of the algorithms

表2 給出遮擋條件下不同組合算法在EuRoc 數(shù)據(jù)集上MH 全序列的位置RMSE。UWB 受非視距因素影響定位偏差較大，不加抗差估計的VIO/UWB 組合算法在一定程度上對VIO 的累積誤差和UWB 的非視距誤差進行了抑制，但是其平均定位誤差仍為0.2 m左右；而基于抗差估計的VIO/UWB 組合算法定位精度更高，其平均定位誤差為0.07 m 左右。相比于VINS_Mono，基于抗差估計的VIO/UWB 組合算法在MH 全序列下定位精度提升49.15%～80.74%不等，平均提升72.07%；相比于不加抗差估計的VIO/UWB 組合算法，定位精度提升58.33%～73.85%不等，平均提升66.11%。

表2 遮擋條件下定位精度對比Tab.2 Comparison of positioning accuracy under occlusion

2.2 真實場景實驗

為了驗證算法的真實有效性，本文在某高校室內(nèi)實驗場進行測試。圖7(a)為實驗場景圖，其中紅色方框標注的為四個UWB 基站放置的位置，實驗場地范圍大約為8 m×8 m。圖7(b)為實驗數(shù)據(jù)采集設(shè)備，以UGV 無人地面小車為平臺，UWB 模塊采用Nooploop設(shè)計的LinkΤrack P 型號標簽和基站，測距精度約為10 cm；VIO 模塊采用Intel RealSense d455，包括一組雙目鏡頭以及集成硬件同步的IMU 模塊，實驗中選取左目相機與IMU 數(shù)據(jù)進行算法驗證。軌跡運動位置姿態(tài)真值由Vicon 光學運動捕捉系統(tǒng)進行記錄，精度為毫米級。

圖7 實驗場景與采集設(shè)備圖Fig.7 Experiment scene and acquisition equipment

實驗中傳感器性能指標如表3 所示。

表3 傳感器性能參數(shù)Tab.3 Sensor performance parameters

實驗場景分為UWB 信號無遮擋和有遮擋兩種，分別記為場景1 和場景2。無遮擋場景是在通視良好、場地無障礙物情況下進行；有遮擋場景主要是通過在某個時間段對基站進行遮擋實現(xiàn)。

2.2.1 非遮擋條件下組合定位性能分析

圖8 給出 VIO/UWB 組合算法與 UWB、VINS_Mono 算法在場景1 上的軌跡和絕對位姿誤差對比。從圖8(a)可以看出，相比于其它兩種算法，VIO/UWB組合算法估計出的軌跡更貼近于真實軌跡，且基本與真實軌跡重合。圖8(b)顯示VIO/UWB 組合算法的定位誤差大多在0.1 m 以內(nèi)，表明UWB 傳感器提供的約束能夠提高組合算法的定位精度。

表4 給出不同傳感器組合定位精度對比。從表中可以看出，VIO/UWB 組合算法的位置RMSE 相比于VINS_Mono、UWB 分別提升75.05%和56.03%，驗證了組合算法具有更加準確可靠的定位性能。

表4 不同傳感器組合定位精度對比Tab.4 Comparison of positioning accuracy of different integrated sensors

2.2.2 遮擋條件下組合定位分析

圖9 顯示了基于抗差估計的VIO/UWB 組合算法與VINS_Mono、不加抗差估計的VIO/UWB 組合算法在場景2 下的軌跡和絕對位姿誤差對比。圖9(a)顯示不加抗差估計的VIO/UWB 組合算法在非視距時間段內(nèi)與軌跡真值偏差較大，而加入抗差后的組合算法的定位軌跡更貼近于真實軌跡。從圖9(b)可以看出，在UWB 非視距階段，不加抗差估計的VIO/UWB 組合算法定位誤差最高達到0.65 m 左右，而加入抗差估計后降低到大約0.2 m 左右，表明加入抗差估計的組合算法能夠有效抑制UWB 非視距誤差，提供穩(wěn)定的高精度定位結(jié)果。

圖9 場景2：算法軌跡與定位精度比較Fig.9 Scene 2: comparison of trajectories and positioning accuracy of the algorithms

從表5 可以看出，基于抗差估計的VIO/UWB 組合算法的位置RMSE 相比于VINS_Mono、不加抗差估計的 VIO/UWB 組合算法分別提升 33.53%和37.53%，驗證了基于抗差估計的VIO/UWB 組合算法具有更加準確可靠的定位性能。

表5 遮擋條件下定位精度對比Tab.5 Comparison of positioning accuracy under occlusion

3 結(jié)論

本文提出了一種基于抗差估計的UWB 與VIO 圖優(yōu)化模型自適應(yīng)定位算法，可有效解決VIO 無回環(huán)情況下產(chǎn)生偏移誤差累積的問題，且能抑制UWB 定位過程中非視距誤差的影響。真實場景下的實驗結(jié)果表明：在良好視距環(huán)境下，VIO/UWB 組合算法相對于VINS_Mono 和UWB 定位精度分別提高了75.05%和56.03%；在非視距環(huán)境下，加入抗差估計的VIO/UWB組合算法相比于正常的VIO/UWB 組合算法定位精度提高了37.53%，驗證了基于抗差估計的VIO/UWB 自適應(yīng)組合算法具有較高的定位精度和良好的魯棒性。

盡管本文算法在不同的場景下取得了較好的定位效果，但是仍需要四個以上UWB 基站，極大限制了場景的應(yīng)用性。此外，本文算法是與UWB 定位結(jié)果進行松組合，沒有充分利用其距離信息，后續(xù)需要與UWB 測距值進行緊組合以減少基站布設(shè)的復雜度和提高組合系統(tǒng)的定位穩(wěn)定性。