申炳琦，張志明，舒少龍

（1.同濟(jì)大學(xué) 電子與信息工程學(xué)院，上海 200092；2.浙江大學(xué) 控制科學(xué)與工程學(xué)院，杭州 310027）

0 引言

室外環(huán)境中的移動(dòng)機(jī)器人定位主要依賴(lài)于全球?qū)Ш叫l(wèi)星系統(tǒng)（Global Navigation Satellite System，GNSS）進(jìn)行定位，但在室內(nèi)環(huán)境中，由于GNSS 的信號(hào)被建筑阻隔，無(wú)法實(shí)現(xiàn)機(jī)器人的準(zhǔn)確定位，故而需要借助其他技術(shù)如無(wú)線傳感器網(wǎng)絡(luò)進(jìn)行定位。

為解決GNSS 信號(hào)缺失環(huán)境下的定位問(wèn)題，同時(shí)定位與地圖構(gòu)建（Simultaneous Localization And Mapping，SLAM）近年來(lái)取得了巨大的發(fā)展［1-2］。目前，根據(jù)機(jī)器人所搭載的傳感器不同，可將SLAM 技術(shù)分為傳統(tǒng)的SLAM 技術(shù)與視覺(jué)SLAM（Visual SLAM，VSLAM）技術(shù)。近十幾年來(lái)，隨著計(jì)算機(jī)視覺(jué)理論技術(shù)的發(fā)展，視覺(jué)SLAM 憑借硬件成本低廉、輕便、高精度等優(yōu)勢(shì)獲得了學(xué)術(shù)界及工業(yè)界的青睞［3］。在此基礎(chǔ)上引入慣性測(cè)量元件（Inertial Measurement Unit，IMU），克服純視覺(jué)導(dǎo)航系統(tǒng)在尺度不確定和純旋轉(zhuǎn)三角化算法退化等場(chǎng)景的缺陷，彌補(bǔ)IMU 測(cè)量誤差隨時(shí)間推移而積累的弊端，使得兩種方式特性互補(bǔ)［4］，提供一種魯棒性更強(qiáng)的室內(nèi)移動(dòng)機(jī)器人定位策略，即視覺(jué)慣性里程SLAM（Visual-Inertial-Odometry SLAM，VIO-SLAM）。近年來(lái)，學(xué)術(shù)界在VIO-SLAM 領(lǐng)域取得了許多突破：Mourikis 等［5］提出一種基于緊耦合濾波的方法，該方法解決傳統(tǒng)VIO 方法維數(shù)過(guò)多的問(wèn)題；Leutenegger 等［6］提出了一種視覺(jué)慣性融合方法OKVIS（Open Keyframe-based Visual-Inertial SLAM），該方法使用了關(guān)鍵幀與非線性?xún)?yōu)化估計(jì)位姿，其建圖效果較好，但未能加上回環(huán)檢測(cè)實(shí)現(xiàn)對(duì)全局的優(yōu)化，所以定位誤差將隨著時(shí)間有所累積；Qin 等［7］提 出VINS-Mono（Visual-Inertial Navigation System Mono）算 法，其前端使用GFTT（Good Features To Track）特征點(diǎn)提取加上多層KLT（Kanade-Lucas-Tomasi）光流跟蹤，后端采用關(guān)鍵幀和滑動(dòng)窗口的優(yōu)化手段，加入回環(huán)檢測(cè)，具有較好的全局一致性。

與此同時(shí)，以無(wú)線信號(hào)強(qiáng)度作為主要參數(shù)的無(wú)線定位技術(shù)也得到快速發(fā)展，包括無(wú)線局域網(wǎng)（Wireless Local Area Network，WLAN）、射頻識(shí)別（Radio Frequency IDentification，RFID）、藍(lán)牙、ZigBee 等；但其易受外界干擾而較不穩(wěn)定，難以滿足室內(nèi)移動(dòng)機(jī)器人魯棒定位的需求［8］。近年來(lái)在室內(nèi)無(wú)線定位研究中，超寬帶（Ultra Wide-Band，UWB）技術(shù)因其能獲得相對(duì)高的定位精度而在學(xué)術(shù)界與工業(yè)界引起關(guān)注。UWB 技術(shù)依靠高頻非正弦窄脈沖傳輸數(shù)據(jù)，占據(jù)很寬的頻帶，是一種具有低功耗高數(shù)據(jù)速率的無(wú)載波通信技術(shù)。Prorok 等［9］首次提 出了一 種UWB 到達(dá)時(shí)間差（Time Difference Of Arrival，TDOA）的測(cè)量模型，得到UWB 定位誤差在空間上的多模態(tài)統(tǒng)計(jì)特征；Hanssens 等［10］首先測(cè)量了UWB 信號(hào)傳播過(guò)程中的幾何特征，如信號(hào)到達(dá)時(shí)延、信號(hào)離去角與到達(dá)角以及上述參數(shù)方差等。

UWB 屬于有源定位方法，由于多徑效應(yīng)、非視距（Non-Line Of Sight，NLOS）等因素的影響，單靠UWB 系統(tǒng)無(wú)法為室內(nèi)移動(dòng)機(jī)器人提供穩(wěn)定、可靠的定位信息；VIO-SLAM 屬于無(wú)源定位方法，其定位誤差會(huì)隨著時(shí)間的推移而累積，無(wú)法為室內(nèi)移動(dòng)機(jī)器人提供長(zhǎng)期可靠的定位。為克服上述兩種定位方法的不足，文獻(xiàn)［11-12］中構(gòu)建了一套基于優(yōu)化的緊耦合框架，將VIO 測(cè)量數(shù)據(jù)與UWB 測(cè)量數(shù)據(jù)融合，以進(jìn)行機(jī)器人的相對(duì)位姿估計(jì)；同樣，文獻(xiàn)［13］中構(gòu)建了一套基于優(yōu)化的松耦合框架，通過(guò)實(shí)驗(yàn)證明UWB 測(cè)量可以克服純視覺(jué)定位產(chǎn)生的漂移，一定程度上提高了系統(tǒng)的魯棒性。受到上述文獻(xiàn)的啟發(fā)，不同于其構(gòu)建基于優(yōu)化的框架，本文在松耦合數(shù)據(jù)融合框架中，將UWB 與VIO 的定位信息與濾波相結(jié)合，為移動(dòng)機(jī)器人在復(fù)雜條件的室內(nèi)環(huán)境中提供長(zhǎng)期、準(zhǔn)確、可靠的實(shí)時(shí)定位策略。融合后的UWB 數(shù)據(jù)可以對(duì)視覺(jué)慣性里程計(jì)（Visual Inertial Odometry，VIO）產(chǎn)生的累積誤差進(jìn)行校正，保證定位的精度；而VIO 可以對(duì)UWB 測(cè)量數(shù)據(jù)進(jìn)行平滑處理，補(bǔ)償多徑引起的缺陷，降低非視距誤差的影響。

1 UWB與VIO組合定位算法原理

針對(duì)組合定位問(wèn)題，首先將對(duì)所采用的UWB 定位算法以及VIO 定位算法原理進(jìn)行分析介紹，然后采用濾波算法對(duì)二者定位結(jié)果進(jìn)行數(shù)據(jù)融合，以得到各時(shí)刻下移動(dòng)機(jī)器人位置的最優(yōu)估計(jì)值。

1.1 UWB三邊定位算法

對(duì)于二維情況下的定位問(wèn)題，基于UWB 三邊定位算法原理，即在已知3 個(gè)UWB 基站坐標(biāo)的前提下，根據(jù)目標(biāo)物體（移動(dòng)機(jī)器人）到3 個(gè)UWB 基站的距離，對(duì)所在坐標(biāo)進(jìn)行解算。系統(tǒng)設(shè)計(jì)如圖1 所示，UWB 基站與標(biāo)簽之間的測(cè)距信息匯總至基站A 中，并通過(guò)串口上傳至上位機(jī)平臺(tái)，進(jìn)行實(shí)時(shí)的位置計(jì)算及軌跡繪制等任務(wù)。

圖1 UWB定位系統(tǒng)Fig.1 UWB positioning system

UWB 定位包括測(cè)距及坐標(biāo)解算兩部分，其中測(cè)距采用雙邊雙向測(cè)距（Double-Sided Two-Way Ranging，DS-TWR）算法［14］，該算法的原理如圖2（a）中所示：設(shè)備A 主動(dòng)發(fā)送TX數(shù)據(jù)，同時(shí)記錄發(fā)送時(shí)間戳；設(shè)備B 在接收到數(shù)據(jù)的同時(shí)記錄接收時(shí)間戳；延時(shí)Treply之后，設(shè)備B 發(fā)送數(shù)據(jù)，同時(shí)記錄發(fā)送時(shí)間戳；設(shè)備A 收到數(shù)據(jù)后，同時(shí)記錄接收時(shí)間戳，并在此基礎(chǔ)上再增加一次傳輸，以此來(lái)計(jì)算設(shè)備A 與設(shè)備B 之間UWB 信號(hào)的飛行時(shí)間Tprop，之后乘以光速c 即可得到設(shè)備A、B 之間的距離d。二維情況下可通過(guò)三邊定位法解決，其原理如圖2（b）中所示：在已知3 個(gè)UWB 基站BS1、BS2、BS3 坐標(biāo)的前提下，可根據(jù)目標(biāo)物體到3 個(gè)UWB 基站的距離d1、d2、d3，列寫(xiě)方程組，對(duì)目標(biāo)物體的坐標(biāo)進(jìn)行解算。

圖2 UWB定位算法原理Fig.2 Principles of UWB positioning algorithms

1.2 VIO定位算法原理

目前，緊耦合框架下的兩種主流VIO 算法分別為多狀態(tài)約束下的Kalman 濾波器（Multi-State Constraint Kalman Filter，MSCKF）算法與視覺(jué)慣性導(dǎo)航系統(tǒng)（Visual-Inertial Navigation System，VINS）算法。MSCKF 在EKF 框架下融合IMU 和視覺(jué)信息，相較于單純的視覺(jué)里程計(jì)（Visual Odometry，VO）算法，MSCKF 可以適應(yīng)更劇烈的運(yùn)動(dòng)、并在一定時(shí)間的紋理缺失情況下魯棒性更強(qiáng)；而根據(jù)Jeon 等［15］與Ma 等［16］的實(shí)驗(yàn)驗(yàn)證，與VINS 等基于優(yōu)化的VIO 算法相比，MSCKF 精度略遜但速度更快，適合在計(jì)算資源有限的嵌入式平臺(tái)運(yùn)行。故而在本文設(shè)計(jì)中，采用MSCKF 算法的雙目版本——立體視覺(jué)多狀態(tài)約束下的Kalman 濾波器（Stereo-Multi-State Constraint Kalman Filter，S-MSCKF）算法［17］原理進(jìn)行基于VIO 的室內(nèi)定位算法設(shè)計(jì)。

與多數(shù)視覺(jué)SLAM 算法類(lèi)似，S-MSCKF 算法可分為前端視覺(jué)與后端濾波兩部分。前端視覺(jué)主要用于檢測(cè)圖像中的特征點(diǎn)，并向后端發(fā)布特征點(diǎn)在相機(jī)坐標(biāo)系的坐標(biāo)等數(shù)據(jù)，在此過(guò)程中需對(duì)提取到的特征點(diǎn)進(jìn)行追蹤并剔除雙目誤匹配點(diǎn)及前后幀誤匹配點(diǎn)。前端視覺(jué)的算法流程如圖3（a）所示。

圖3 VIO定位模塊S-MSCKF算法流程Fig.3 Flow of VIO positioning module S-MSCKF algorithm

算法流程主要包括以下關(guān)鍵環(huán)節(jié)。

1）初始化第一幀。雙目相機(jī)在獲取第一幀圖片后，首先檢測(cè)左目圖片中的加速分割測(cè)試特征（Features from Accelerated Segment Test，F(xiàn)AST）特征點(diǎn)，根據(jù)左右目相機(jī)之間的內(nèi)參將左目特征點(diǎn)投影至右目中進(jìn)行雙目特征點(diǎn)匹配；然后通過(guò)圖像金字塔進(jìn)行LK（Lucas-Kanade）光流追蹤、對(duì)極幾何約束等處理，剔除追蹤到圖像外的特征點(diǎn)；最后將特征點(diǎn)進(jìn)行網(wǎng)格劃分并輸出FAST 特征點(diǎn)信息。

2）跟蹤特征點(diǎn)。在相機(jī)移動(dòng)的過(guò)程中會(huì)陸續(xù)接收到不同幀圖像，需在下一幀圖像中對(duì)上一幀圖像提取到的特征點(diǎn)進(jìn)行跟蹤，并提取新的特征點(diǎn)。該階段同樣利用對(duì)極幾何約束、LK 光流追蹤以及Two-points RANSAC（RANdom SAmple Consensus）法不斷剔除左右目誤匹配特征點(diǎn)以及超出圖像邊界的特征點(diǎn)。

3）添加新特征點(diǎn)。隨著時(shí)間流逝，視頻圖像中的某些特征會(huì)消失，某些特征可能也會(huì)存在累計(jì)誤差，所以在持續(xù)追蹤過(guò)程中勢(shì)必要添加一些新的特征來(lái)保證程序能夠一直運(yùn)行。

S-MSCKF 后端的主要功能在于根據(jù)視覺(jué)前端傳來(lái)的特征點(diǎn)坐標(biāo)，與IMU 狀態(tài)進(jìn)行擴(kuò)展卡爾曼濾波，從而得到相機(jī)當(dāng)前位姿的最優(yōu)估計(jì)值。首先通過(guò)訂閱相關(guān)話題接收前端圖像特征點(diǎn)信息，再初始化重力及偏差等相關(guān)量；然后處理IMU 數(shù)據(jù)以構(gòu)建微分方程組中的狀態(tài)矩陣以及輸入矩陣，并求得狀態(tài)轉(zhuǎn)移矩陣；之后依次進(jìn)行狀態(tài)預(yù)測(cè)、狀態(tài)擴(kuò)增以及狀態(tài)更新，移除歷史相機(jī)狀態(tài)數(shù)據(jù)，得到當(dāng)前相機(jī)狀態(tài)；并最終發(fā)布相機(jī)的位姿信息話題。

1.3 UWB-VIO組合定位算法

為得到長(zhǎng)期運(yùn)行后仍準(zhǔn)確可靠的定位數(shù)據(jù)，需對(duì)UWB 與VIO 分別輸出的定位數(shù)據(jù)進(jìn)行融合解算。傳感器信息融合方法主要包括加權(quán)平均法、卡爾曼濾波法、貝葉斯推理法、神經(jīng)網(wǎng)絡(luò)算法等。其中加權(quán)平均法簡(jiǎn)單直觀，但很難得到最優(yōu)加權(quán)平均，且計(jì)算最優(yōu)加權(quán)平均值需要大量時(shí)間；貝葉斯推理信息被描述為概率分布，需要先驗(yàn)概率和似然函數(shù)，分析計(jì)算復(fù)雜；神經(jīng)網(wǎng)絡(luò)算法根據(jù)輸入數(shù)據(jù)樣本訓(xùn)練和調(diào)整網(wǎng)絡(luò)的權(quán)重，但訓(xùn)練階段需要大量數(shù)據(jù)，實(shí)時(shí)性較差［18］?？紤]到本文搭建的UWB-VIO 組合定位系統(tǒng)為非線性系統(tǒng)，同時(shí)為避免可能的協(xié)方差矩陣奇異性及萬(wàn)向鎖等問(wèn)題［19］，選用處理非線性問(wèn)題的誤差狀態(tài)擴(kuò)展卡爾曼濾波（Error State-Extended Kalman Filter，ES-EKF）算法［20］進(jìn)行傳感器的信息融合。

UWB-VIO 組合定位算法中，以UWB 定位過(guò)程中所使用到的獨(dú)立坐標(biāo)系作為VIO 定位的世界坐標(biāo)系。然后以移動(dòng)機(jī)器人的位置與速度作為系統(tǒng)的狀態(tài)向量，假設(shè)UWB 定位觀測(cè)值與VIO 定位觀測(cè)值在k時(shí)刻進(jìn)行時(shí)間戳對(duì)齊，則k時(shí)刻的狀態(tài)向量即，其中(xk，yk)代表第k時(shí)刻組合系統(tǒng)在二維平面上的位置坐標(biāo)；和分別代表第k時(shí)刻組合系統(tǒng)在x方向和y方向上的速度。假設(shè)k時(shí)刻的狀態(tài)向量的標(biāo)稱(chēng)值為，誤差值為δXk，Xk=+δXk。則該系統(tǒng)的誤差狀態(tài)模型可表示為式（1）：

其中：t為組合系統(tǒng)的采樣間隔時(shí)間；分別代表第k-1 時(shí)刻組合系統(tǒng)在x方向和y方向上的加速度。ES-EKF 算法流程如圖4 所示。

圖4 ES-EKF算法流程Fig.4 Flow of ES-EKF algorithm

以接收到組合系統(tǒng)的VIO 定位數(shù)據(jù)為狀態(tài)向量標(biāo)稱(chēng)值，之后進(jìn)行誤差狀態(tài)更新。對(duì)式（1）進(jìn)行整理，則對(duì)k時(shí)刻誤差狀態(tài)及協(xié)方差的預(yù)測(cè)如式（2）所示：

其中：F為系統(tǒng)的狀態(tài)矩陣，Wk-1為系統(tǒng)的噪聲向量，分別定義為：為k-1時(shí)刻誤差狀態(tài)協(xié)方差的最優(yōu)估計(jì)；Qk-1為k-1 時(shí)刻的噪聲協(xié)方差?？紤]到組合系統(tǒng)的狀態(tài)向量?jī)H用于追蹤系統(tǒng)的位置與速度，故本文將加速度作為系統(tǒng)的隨機(jī)噪聲。

以UWB 與VIO 得出的定位信息差值作為該系統(tǒng)的觀測(cè)向量Yk，則該組合系統(tǒng)的觀測(cè)方程為：

之后可進(jìn)行誤差狀態(tài)及協(xié)方差矩陣的更新，包括計(jì)算卡爾曼增益Kk，更新誤差狀態(tài)δXk|k以及更新協(xié)方差矩陣Pk|k。具體方程如式（4）所示：

其中：Rk為k時(shí)刻的觀測(cè)協(xié)方差。而后根據(jù)Xk=+δXk得到k時(shí)刻真實(shí)狀態(tài)的最優(yōu)估計(jì)值結(jié)果。該算法的偽代碼原理如下所示。

過(guò)程：

以VIO 定位數(shù)據(jù)作為標(biāo)稱(chēng)值；

令k時(shí)刻狀態(tài)向量誤差值δXk=0；

計(jì)算時(shí)間間隔：dt=tk-tk-1；

根據(jù)式（1）（2）對(duì)k時(shí)刻狀態(tài)向量的誤差值及協(xié)方差矩陣進(jìn)行預(yù)測(cè)，計(jì)算得到δXk|k-1及Pk|k-1；

根據(jù)式（3）得到系統(tǒng)在k時(shí)刻的觀測(cè)向量Yk；

根據(jù)式（4）對(duì)k時(shí)刻狀態(tài)向量的誤差值及協(xié)方差矩陣進(jìn)行更新，計(jì)算得到δXk|k及Pk|k；

令δXk=δXk|k根據(jù)Xk=+δXk，計(jì)算得到k時(shí)刻狀態(tài)向量的最優(yōu)估計(jì)；

輸出k時(shí)刻系統(tǒng)的最優(yōu)狀態(tài)估計(jì)Xk

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

搭建實(shí)驗(yàn)平臺(tái)，選擇實(shí)驗(yàn)場(chǎng)地進(jìn)行實(shí)驗(yàn)，獲取定位結(jié)果，并根據(jù)結(jié)果分析造成誤差的原因，得出結(jié)論并驗(yàn)證算法的有效性。

2.1 UWB-VIO組合定位實(shí)驗(yàn)平臺(tái)搭建

以UGV 無(wú)人地面小車(chē)為平臺(tái)，操作系統(tǒng)采用Ubuntu16.04+ROS Kinetic 進(jìn)行軟件設(shè)計(jì)；UWB 模塊采用廣州聯(lián)網(wǎng)科技有限公司設(shè)計(jì)的D-DWG-PG2.5 UWB 標(biāo)簽和UWB基站模塊，采用棒狀3.1～4.9 Hz 全向高頻天線，通信距離可達(dá)130 m，測(cè)距精度約10 cm，定位精度約30 cm；VIO 模塊為Intel T265，包括一組雙目魚(yú)眼鏡頭（OV9282）；同時(shí)集成硬件同步的200 Hz 陀螺儀和62.5 Hz 加速度計(jì)，根據(jù)S-MSCKF 算法計(jì)算模塊在3D空間中的方向與位置，從而實(shí)現(xiàn)六自由度追蹤。該UWB-VIO組合定位系統(tǒng)的整體架構(gòu)如圖5所示。

圖5 UWB-VIO組合定位系統(tǒng)整體架構(gòu)Fig.5 Overall architecture of UWB-VIO integrated positioning system

2.2 室內(nèi)定位實(shí)驗(yàn)場(chǎng)地選擇

分別在無(wú)障礙小場(chǎng)地與有障礙大場(chǎng)地進(jìn)行定位實(shí)驗(yàn)。無(wú)障礙小場(chǎng)地選在同濟(jì)大學(xué)嘉定校區(qū)華樓一樓大廳，有障礙大場(chǎng)地選在華樓二樓大廳，其中大場(chǎng)地在定位過(guò)程中有室內(nèi)支柱作為障礙物干擾，而小場(chǎng)地?zé)o障礙。實(shí)驗(yàn)時(shí)間均選擇在14：00 左右，此時(shí)光照環(huán)境良好，利于實(shí)驗(yàn)的開(kāi)展。本次實(shí)驗(yàn)采取4 個(gè)UWB 模塊，包括3 個(gè)坐標(biāo)已知的基站以及1 個(gè)待定位的標(biāo)簽。3 個(gè)UWB 基站擺放位置盡量?jī)蓛傻染啵源_保UWB 定位的精確性。室內(nèi)移動(dòng)機(jī)器人以接近勻速運(yùn)動(dòng)的方式沿著已知的固定軌跡進(jìn)行運(yùn)動(dòng)。實(shí)驗(yàn)現(xiàn)場(chǎng)以及機(jī)器人行駛軌跡如圖6 所示。

圖6 實(shí)驗(yàn)現(xiàn)場(chǎng)場(chǎng)景Fig.6 Scenes of experiment sites

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

對(duì)兩種定位方法得到的數(shù)據(jù)進(jìn)行時(shí)間同步等一系列數(shù)據(jù)處理操作后，得到基于UWB 與VIO 的單一定位以及組合定位的定位結(jié)果對(duì)比結(jié)果。

2.3.1 無(wú)障礙小場(chǎng)地定位實(shí)驗(yàn)

在無(wú)障礙小場(chǎng)地定位實(shí)驗(yàn)中，機(jī)器人的移動(dòng)過(guò)程并未受到室內(nèi)支柱等障礙物的干擾，3 種定位方法的結(jié)果對(duì)比如圖7 所示。定位誤差指標(biāo)對(duì)比如表1 所示。

表1 無(wú)障礙小場(chǎng)地下三種定位算法結(jié)果誤差對(duì)比Tab.1 Error comparison for results of different positioning algorithms in obstacle-free small site

圖7 無(wú)障礙小場(chǎng)地下的定位效果對(duì)比Fig.7 Positioning effect comparison in obstacle-free small site

在這種室內(nèi)較為空曠的情況下，僅靠UWB 的室內(nèi)定位方法便可達(dá)到厘米級(jí)的定位精度；而VIO 定位方法在初期階段定位結(jié)果令人滿意，但隨著定位過(guò)程的推進(jìn)，因累計(jì)誤差的存在使得其在軌跡的后半段定位結(jié)果發(fā)生漂移，致使定位結(jié)果誤差過(guò)大。此時(shí)組合定位結(jié)果精度劣于UWB 單一定位方法，但優(yōu)于VIO 單一定位方法。

2.3.2 有障礙大場(chǎng)地定位實(shí)驗(yàn)

在有障礙大場(chǎng)地定位實(shí)驗(yàn)中，機(jī)器人的運(yùn)行軌跡包含室內(nèi)支柱這一障礙物，這將會(huì)對(duì)UWB 定位造成一定干擾。在該情況下，三種定位方法的定位結(jié)果對(duì)比如圖8 所示。

圖8 有障礙大場(chǎng)地下的定位效果對(duì)比Fig.8 Positioning effect comparison in large site with obstacles

在測(cè)量UWB 信號(hào)時(shí)，視距傳播必不可少，當(dāng)兩點(diǎn)之間不存在直接傳播路徑時(shí)，只能通過(guò)反射和衍射效應(yīng)由接收機(jī)接收到信號(hào)，如果以第一個(gè)脈沖的到達(dá)時(shí)間（Time Of Arrival，TOA）作為到達(dá)時(shí)間的真實(shí)值，將會(huì)帶來(lái)很大的誤差。與無(wú)障礙場(chǎng)地實(shí)驗(yàn)對(duì)比發(fā)現(xiàn)：正常情況下，UWB 能夠達(dá)到厘米級(jí)定位效果，當(dāng)實(shí)驗(yàn)場(chǎng)景中出現(xiàn)遮擋情況而產(chǎn)生非視距誤差時(shí)，UWB 的定位精度出現(xiàn)較大的跳變和誤差；而VIO 定位結(jié)果整體較為平滑，在定位初期效果令人滿意，但因累計(jì)誤差的存在使得其在軌跡的后半段定位結(jié)果發(fā)生漂移，且在拐角處因相機(jī)場(chǎng)景變化較大導(dǎo)致定位結(jié)果不甚理想。在這種情況下，經(jīng)過(guò)ES-EKF 融合后的定位結(jié)果相比以上兩種單一傳感器定位方法的定位精度更高、可靠性更強(qiáng)，定位結(jié)果更接近實(shí)際軌跡。在有障礙物的室內(nèi)環(huán)境下，定位結(jié)果與實(shí)際軌跡之間的最大誤差與均方根誤差均得到改善，如表2 中所示，與單一UWB 定位方法相比，所提算法的總體定位的最大誤差減小約4.4%，均方誤差減小約6.3%；與單一VIO 定位方法相比，所提算法的總體定位的最大誤差減小約31.5%，均方誤差減小約60.3%。

表2 有障礙大場(chǎng)地下三種定位算法結(jié)果誤差對(duì)比Tab.2 Error comparison for results of different positioning algorithms in large site with obstacles

3 結(jié)語(yǔ)

相較而言，UWB 技術(shù)在定位精度上可取得不錯(cuò)的效果，但非視距誤差等因素的存在致使其長(zhǎng)期穩(wěn)定性還無(wú)法滿足需求；視覺(jué)慣性SLAM 技術(shù)在定位魯棒性方面優(yōu)勢(shì)明顯，但其在精確性方面仍顯不足。本文采用ES-EKF 算法對(duì)UWB定位數(shù)據(jù)以及VIO 定位數(shù)據(jù)進(jìn)行融合，并以地面移動(dòng)機(jī)器人為測(cè)試對(duì)象，基于ROS 移動(dòng)機(jī)器人平臺(tái)構(gòu)建UWB-VIO 混合定位系統(tǒng)。通過(guò)對(duì)比實(shí)驗(yàn)，表明了本文算法能夠在含有障礙物的室內(nèi)環(huán)境中，提升移動(dòng)機(jī)器人定位的精確性和魯棒性。

考慮到本文所提出的基于濾波的方法能夠提供實(shí)時(shí)定位結(jié)果，但在精確性與基于優(yōu)化的方法相比尚有所不足；故在未來(lái)的工作中可考慮如何同時(shí)兼顧定位的實(shí)時(shí)性和準(zhǔn)確性。此外，鑒于本文的定位工作主要在二維平面上進(jìn)行，隨著未來(lái)空間技術(shù)的發(fā)展，無(wú)人機(jī)（Unmanned Aerial Vehicle，UAV）平臺(tái)或?qū)⒊蔀槭覂?nèi)移動(dòng)機(jī)器人的主體，用于隧道三維重建等場(chǎng)景；故在未來(lái)的研究中，可考慮以無(wú)人機(jī)等作為室內(nèi)移動(dòng)機(jī)器人平臺(tái)，實(shí)現(xiàn)三維立體定位。同樣的，可考慮將本文算法移植到室外移動(dòng)機(jī)器人中，助力自動(dòng)駕駛技術(shù)以及增強(qiáng)現(xiàn) 實(shí)（Augmented Reality，AR）和虛擬現(xiàn)實(shí)（Virtual Reality，VR）領(lǐng)域的發(fā)展。