移動機(jī)器人超寬帶與視覺慣性里程計組合的室內(nèi)定位算法

申炳琦，張志明，舒少龍

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

0 引言

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

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

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

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

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

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

1.1 UWB三邊定位算法

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

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

UWB 定位包括測距及坐標(biāo)解算兩部分，其中測距采用雙邊雙向測距（Double-Sided Two-Way Ranging，DS-TWR）算法［14］，該算法的原理如圖2（a）中所示：設(shè)備A 主動發(fā)送TX數(shù)據(jù)，同時記錄發(fā)送時間戳；設(shè)備B 在接收到數(shù)據(jù)的同時記錄接收時間戳；延時Treply之后，設(shè)備B 發(fā)送數(shù)據(jù)，同時記錄發(fā)送時間戳；設(shè)備A 收到數(shù)據(jù)后，同時記錄接收時間戳，并在此基礎(chǔ)上再增加一次傳輸，以此來計算設(shè)備A 與設(shè)備B 之間UWB 信號的飛行時間Tprop，之后乘以光速c 即可得到設(shè)備A、B 之間的距離d。二維情況下可通過三邊定位法解決，其原理如圖2（b）中所示：在已知3 個UWB 基站BS1、BS2、BS3 坐標(biāo)的前提下，可根據(jù)目標(biāo)物體到3 個UWB 基站的距離d1、d2、d3，列寫方程組，對目標(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）算法與視覺慣性導(dǎo)航系統(tǒng)（Visual-Inertial Navigation System，VINS）算法。MSCKF 在EKF 框架下融合IMU 和視覺信息，相較于單純的視覺里程計（Visual Odometry，VO）算法，MSCKF 可以適應(yīng)更劇烈的運動、并在一定時間的紋理缺失情況下魯棒性更強(qiáng)；而根據(jù)Jeon 等［15］與Ma 等［16］的實驗驗證，與VINS 等基于優(yōu)化的VIO 算法相比，MSCKF 精度略遜但速度更快，適合在計算資源有限的嵌入式平臺運行。故而在本文設(shè)計中，采用MSCKF 算法的雙目版本——立體視覺多狀態(tài)約束下的Kalman 濾波器（Stereo-Multi-State Constraint Kalman Filter，S-MSCKF）算法［17］原理進(jìn)行基于VIO 的室內(nèi)定位算法設(shè)計。

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

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

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

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

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

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

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

1.3 UWB-VIO組合定位算法

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

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

其中：t為組合系統(tǒng)的采樣間隔時間；分別代表第k-1 時刻組合系統(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)稱值，之后進(jìn)行誤差狀態(tài)更新。對式（1）進(jìn)行整理，則對k時刻誤差狀態(tài)及協(xié)方差的預(yù)測如式（2）所示：

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

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

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

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

過程：

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

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

計算時間間隔：dt=tk-tk-1；

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

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

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

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

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

2 實驗與結(jié)果分析

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

2.1 UWB-VIO組合定位實驗平臺搭建

以UGV 無人地面小車為平臺，操作系統(tǒng)采用Ubuntu16.04+ROS Kinetic 進(jìn)行軟件設(shè)計；UWB 模塊采用廣州聯(lián)網(wǎng)科技有限公司設(shè)計的D-DWG-PG2.5 UWB 標(biāo)簽和UWB基站模塊，采用棒狀3.1～4.9 Hz 全向高頻天線，通信距離可達(dá)130 m，測距精度約10 cm，定位精度約30 cm；VIO 模塊為Intel T265，包括一組雙目魚眼鏡頭（OV9282）；同時集成硬件同步的200 Hz 陀螺儀和62.5 Hz 加速度計，根據(jù)S-MSCKF 算法計算模塊在3D空間中的方向與位置，從而實現(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)定位實驗場地選擇

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

圖6 實驗現(xiàn)場場景Fig.6 Scenes of experiment sites

2.3 實驗結(jié)果分析

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

2.3.1 無障礙小場地定位實驗

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

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

圖7 無障礙小場地下的定位效果對比Fig.7 Positioning effect comparison in obstacle-free small site

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

2.3.2 有障礙大場地定位實驗

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

圖8 有障礙大場地下的定位效果對比Fig.8 Positioning effect comparison in large site with obstacles

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

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

3 結(jié)語

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

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