移動機器人超寬帶與視覺慣性里程計組合的室內定位算法

申炳琦，張志明，舒少龍

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

0 引言

室外環境中的移動機器人定位主要依賴于全球導航衛星系統（Global Navigation Satellite System，GNSS）進行定位，但在室內環境中，由于GNSS 的信號被建筑阻隔，無法實現機器人的準確定位，故而需要借助其他技術如無線傳感器網絡進行定位。

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

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

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

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

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

1.1 UWB三邊定位算法

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

圖1 UWB定位系統Fig.1 UWB positioning system

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

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

1.2 VIO定位算法原理

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

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

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

算法流程主要包括以下關鍵環節。

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

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

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

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

1.3 UWB-VIO組合定位算法

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

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

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

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

以接收到組合系統的VIO 定位數據為狀態向量標稱值，之后進行誤差狀態更新。對式（1）進行整理，則對k時刻誤差狀態及協方差的預測如式（2）所示：

其中：F為系統的狀態矩陣，Wk-1為系統的噪聲向量，分別定義為：為k-1時刻誤差狀態協方差的最優估計；Qk-1為k-1 時刻的噪聲協方差。考慮到組合系統的狀態向量僅用于追蹤系統的位置與速度，故本文將加速度作為系統的隨機噪聲。

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

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

其中：Rk為k時刻的觀測協方差。而后根據Xk=+δXk得到k時刻真實狀態的最優估計值結果。該算法的偽代碼原理如下所示。

過程：

以VIO 定位數據作為標稱值；

令k時刻狀態向量誤差值δXk=0；

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

根據式（1）（2）對k時刻狀態向量的誤差值及協方差矩陣進行預測，計算得到δXk|k-1及Pk|k-1；

根據式（3）得到系統在k時刻的觀測向量Yk；

根據式（4）對k時刻狀態向量的誤差值及協方差矩陣進行更新，計算得到δXk|k及Pk|k；

令δXk=δXk|k根據Xk=+δXk，計算得到k時刻狀態向量的最優估計；

輸出k時刻系統的最優狀態估計Xk

2 實驗與結果分析

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

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

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

圖5 UWB-VIO組合定位系統整體架構Fig.5 Overall architecture of UWB-VIO integrated positioning system

2.2 室內定位實驗場地選擇

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

圖6 實驗現場場景Fig.6 Scenes of experiment sites

2.3 實驗結果分析

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

2.3.1 無障礙小場地定位實驗

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

表1 無障礙小場地下三種定位算法結果誤差對比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

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

2.3.2 有障礙大場地定位實驗

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

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

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

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

3 結語

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

考慮到本文所提出的基于濾波的方法能夠提供實時定位結果，但在精確性與基于優化的方法相比尚有所不足；故在未來的工作中可考慮如何同時兼顧定位的實時性和準確性。此外，鑒于本文的定位工作主要在二維平面上進行，隨著未來空間技術的發展，無人機（Unmanned Aerial Vehicle，UAV）平臺或將成為室內移動機器人的主體，用于隧道三維重建等場景；故在未來的研究中，可考慮以無人機等作為室內移動機器人平臺，實現三維立體定位。同樣的，可考慮將本文算法移植到室外移動機器人中，助力自動駕駛技術以及增強現 實（Augmented Reality，AR）和虛擬現實（Virtual Reality，VR）領域的發展。