周韋，孫憲坤，吳飛

(上海工程技術(shù)大學 電子電氣工程學院,上海 201620)

0 引言

隨著傳感器技術(shù)、計算機視覺、人工智能等技術(shù)的快速發(fā)展，許多智能移動機器人開始進入我們的生活[1].在這種情況下，移動機器人的自主定位顯得十分重要，因為它是實現(xiàn)移動機器人路徑規(guī)劃、自主避障等功能的前提[2].由于移動機器人在室內(nèi)導航時缺乏可靠的全球衛(wèi)星導航系統(tǒng)(GNSS)[3-4]信號，因此相對于室外導航有著更多的挑戰(zhàn).

目前，室內(nèi)移動機器人的定位主要通過慣性導航系統(tǒng)(INS)、激光雷達(LR)、視覺定位等來實現(xiàn).但是這些定位方式由于其傳感器感知能力的局限性并不能解決所有的定位問題.INS 在運行一段時間后會出現(xiàn)漂移，導致定位發(fā)散.LR 定位不適用于變化的環(huán)境，否則會造成激光掃描信息與地圖不匹配導致定位失敗，并且其價格較為昂貴.視覺定位[5]是通過即時定位與地圖構(gòu)建(SLAM)算法利用連續(xù)圖像幀來估計相機的旋轉(zhuǎn)與平移，因每次估計均存在誤差，長時間或長距離運行后，會造成誤差累計.綜上所述，單一的室內(nèi)定位技術(shù)無法實現(xiàn)移動機器人室內(nèi)自主導航功能.為此許多專家學者將目光轉(zhuǎn)向于多傳感器融合[6-7]定位方向.

視覺SLAM 具有低成本、獲取環(huán)境信息豐富等特點，不僅能夠提供定位信息，而且能夠構(gòu)建相應(yīng)的點云地圖.由于誤差累計的問題，需要一種能夠提供絕對定位信息的定位方式對此進行修正，而超寬帶(UWB)定位系統(tǒng)具有高帶寬、穿透能力強等特點，可以提供絕對的位置服務(wù)，適合室內(nèi)定位應(yīng)用.但是在復雜的室內(nèi)環(huán)境下，UWB 容易受到反射、多徑效應(yīng)、非視距(NLOS)等因素的影響，導致定位精度下降.針對這些問題，文獻[8]提出一種基于單目視覺與UWB相結(jié)合室內(nèi)定位方法，能夠有效克制單目視覺ORBSLAM 跟蹤失敗和抑制UWB 非視距誤差的影響，其定位精度可達到亞分米級，然而該算法并沒有對視覺SLAM 失效的情況做處理，因此缺乏一定的穩(wěn)定性；文獻[9]提出一種基于UWB、里程計和RGB-D融合的室內(nèi)定位方法，可以將定位誤差抑制在10 cm以內(nèi)，偏轉(zhuǎn)角誤差抑制在1°以內(nèi)，徹底解決RGB-D攝像頭SLAM 時跟蹤失敗問題，但是該方法沒有考慮到UWB 的非視距誤差的影響；文獻[10]提出通過SLAM 提高UWB 定位增強無人機室內(nèi)導航，能將90%分位數(shù)誤差為20.5 cm 的UWB 定位精度提高到13.9 cm，實現(xiàn)無人機在環(huán)境中的有效導航，然而該方法使用權(quán)重的方式進行融合，并不能應(yīng)對復雜的室內(nèi)環(huán)境，以及長時間運行視覺SLAM 造成的累積誤差的問題.

為了更好地解決移動機器人室內(nèi)定位問題，本文采用擴展卡爾曼濾波(EKF)算法[11]將UWB 的全局絕對坐標和視覺SLAM 的相對位移增量進行融合，利用UWB 提供的全局定位信息補償視覺SLAM 的累計誤差.接著考慮到EKF 雖然能夠處理大多數(shù)復雜且非線性的系統(tǒng)，但它要求測量噪聲符合零均值的高斯分布.現(xiàn)實情況下，UWB 易受多徑效應(yīng)、非視距等因素影響，視覺定位易受圖像紋理和清晰度的影響，其測量噪聲并不完全符合高斯分布.因此在傳統(tǒng)EKF 算法的基礎(chǔ)上添加Sage-Husa 測量噪聲估計器來減弱非高斯噪聲對濾波器性能的影響，并添加閾值判斷來識別和剔除測量中的異常值.通過實驗表明：本文算法在有障礙物、小范圍場景和大范圍場景下都能良好運行，并且比傳統(tǒng)EKF 算法擁有更高的定位精度和更好的魯棒性.

1 UWB 與SLAM 定位算法

1.1 UWB 定位算法

UWB 定位系統(tǒng)采用基于信號達到時間差(TDOA)的算法，此算法通過標簽向多個基站發(fā)射信號，根據(jù)標簽信號到達每個基站的時間差，得到標簽到各個基站的距離差，最后通過雙曲線模型求出標簽坐標[12].與到達時間(TOA)算法相比，此算法無需基站和標簽之間實現(xiàn)時鐘同步，只需基準進行時鐘同步，因此具有通信次數(shù)少，定位精度高的優(yōu)點.

其定位原理如圖1 所示，UWB基站安裝在固定位置且坐標已知，坐標分別為BSi(x1,y1)(i=1,2,3)，待求取的標簽MS坐標為(x,y)，標簽到基站的距離為di(i=1,2,3).

圖1 TDOA 算法原理圖

當所有基站實現(xiàn)時間同步后，標簽發(fā)出定位信號.定義標簽信號到達基站的時間為ti(i=1,2,3)，光速為c，可利用式(1)求出標簽坐標.

1.2 視覺SLAM 定位算法

視覺定位采用ORB-SLAM2 算法，這是一套基于單目、雙目和RGB-D 相機的開源SLAM 算法.它將經(jīng)典的視覺SLAM 算法劃分為三個線程：位置跟蹤(Tracking)、建圖(Local Mapping)和回環(huán)檢測(Loop closing)，并通過System 線程完成定位與建圖功能.文獻[13]完整詳細地介紹了該算法.算法的流程框圖如圖2 所示.

圖2 ORB-SLAM2 流程框圖

本文選用RGB-D 相機作為圖像輸入源，相比單目和雙目相機，RGB-D 相機能夠同時采集彩色圖片和對應(yīng)的深度圖.既能解決單目視覺中的尺度不確定性的問題，也能省去雙目視覺中計算左右相機視差的繁瑣步驟，減少計算機運算量，保證實時性要求.

2 SLAM/UWB 融合定位算法

通過對UWB 和視覺SLAM 定位技術(shù)的介紹，可以得知單一的室內(nèi)定位方式無法有效的保證其定位的精度和穩(wěn)定性.因此本文對上述兩類定位技術(shù)進行數(shù)據(jù)融合，形成優(yōu)勢互補，實現(xiàn)移動機器人在室內(nèi)環(huán)境下高精度、高穩(wěn)定性的自主定位.

2.1 時間同步與坐標系統(tǒng)一

如圖3 所示，在進行數(shù)據(jù)融合之前，必須考慮到兩種定位方式的時間同步與坐標系統(tǒng)一，因此在實驗之前設(shè)計一個預處理階段，此階段分為兩個部分：第一部分將兩種定位方式的解算設(shè)備進行時間同步，并對SLAM 與UWB 的輸出數(shù)據(jù)添加時間標簽；第二部分通過坐標系轉(zhuǎn)化的方式將ORB-SLAM2 的相機坐標系統(tǒng)一到UWB 坐標下.

圖3 預處理結(jié)構(gòu)框圖

為求取ORB-SLAM2 的相機坐標系到UWB 坐標系的旋轉(zhuǎn)和偏移矩陣，在時間同步之后需采集同一軌跡下一定樣本數(shù)量的UWB 和SLAM 數(shù)據(jù).記此軌跡下UWB 的坐標數(shù)據(jù)集為U，SLAM 的坐標數(shù)據(jù)集為S，使用迭代法求取兩個數(shù)據(jù)集時間臨近的點集，得到新的數(shù)據(jù)集U′={u1,u2,u3···,un}和S′={s1,s2,s3···,sn}，設(shè)相機坐標系到UWB 坐標系的旋轉(zhuǎn)矩陣和平移矩陣為R和T.

則有公式

將新的數(shù)據(jù)集去質(zhì)心化，得：

2.2 基于EKF 的數(shù)據(jù)融合算法

經(jīng)過時間同步與坐標系統(tǒng)一后，運用EKF 濾波算法對SLAM 數(shù)據(jù)和UWB 數(shù)據(jù)進行融合，得到更加準確的定位信息.基于EKF 的狀態(tài)方程和量測方程見式(7)～(8).

狀態(tài)方程為

量測方程為

式中：Xk指狀態(tài)向量；Yk指觀測向量；wk-1表示過程噪聲；δk表示觀測噪聲；f(·) 表示狀態(tài)函數(shù)；h(·)表示觀測函數(shù).

則融合定位系統(tǒng)的狀態(tài)方程為

F為狀態(tài)轉(zhuǎn)移矩陣，Δt為數(shù)據(jù)采用間隔，則有

由于視覺SLAM 定位輸出頻率遠低于UWB 定位，無法實現(xiàn)同步輸入觀測值.于是本文將兩種定位方式的觀測值分別代入EKF 濾波器中，即EKF 中存在兩種量測方程.

則SLAM 的量測方程可表示為

UWB 的量測方程可表示為

EKF 算法包括狀態(tài)更新和量測更新，具體過程如下：

狀態(tài)更新：

式中：Pk/k為第k時刻誤差協(xié)方差預測；Pk為誤差協(xié)方差矩陣；Ak為狀態(tài)轉(zhuǎn)移矩陣，是狀態(tài)函數(shù)f(·)雅克比矩陣；Qk為過程噪聲的協(xié)方差矩陣.

量測更新：

式中：Kk為卡爾曼增益；Rk為過程噪聲協(xié)方差矩陣；Hk為量測轉(zhuǎn)移矩陣，是量測函數(shù)h(·)的雅克比矩陣,Hk根據(jù)輸入數(shù)據(jù)的定位類型選擇或者.

2.3 測量噪聲估計與閾值判斷

傳統(tǒng)的 EKF 在迭代過程中要求噪聲服從零均值的高斯分布，即Qk～N(0,Q)，Rk～N(0,R).其中Qk主要與系統(tǒng)預測模型有關(guān)，Rk主要與傳感器測量數(shù)據(jù)有關(guān).考慮到SLAM 和UWB 定位時均易受環(huán)境影響，其測量噪聲Rk并不完全符合高斯分布，這將導致EKF 的估計誤差增大甚至發(fā)散.為此本文在EKF基礎(chǔ)上添加Sage-Husa 噪聲估計器和閾值判斷.一方面，噪聲估計器可以不斷調(diào)整測量噪聲強度大??；另一方面，通過添加閾值判斷機制，刪除嚴重偏移實際位置的測量數(shù)據(jù)，實現(xiàn)對濾波器的調(diào)優(yōu).

當固定Qk的情況下，使用Sage-Husa 算法對系統(tǒng)測量噪聲的協(xié)方差[15]估計為：

式中，b表示遺忘因子，取值范圍為 0.95～0.99，具體數(shù)值可通過實驗獲得.

為剔除異常的測量數(shù)據(jù)，在融合之前首先比較測量值與狀態(tài)向量的估計值，將二者的差值與提前設(shè)置的閾值進行相減，超過閾值將使用上一次的狀態(tài)估計代替.

結(jié)合閾值判斷后，EKF 中的式(15)被修改為

式中，thresh為提前選定的閾值.最終的室內(nèi)融合定位算法主要步驟如圖4 所示.

圖4 融合定位算法框架圖

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

3.1 實驗說明

為了驗證SLAM/UWB 的融合定位算法的效果，搭建室內(nèi)測試場景，選用智能移動小車作為室內(nèi)移動機器人.該車搭載了因特爾NUC 迷你電腦，并運行ROS 操作系統(tǒng).UWB 定位傳感器使用上海物聯(lián)網(wǎng)公司的硬件設(shè)備，其輸出頻率可達50 Hz；視覺傳感器選用樂視LeTMC-520 型號的RGB-D 相機，支持每秒30 幀的RGB 和深度圖像輸出，其深度測距每米誤差小于3 mm.將UWB 標簽和RGB-D 相機固定在智能小車上，保持標簽與相機相對靜止.

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

在實驗樓中分別進行小場地定位實驗和大場地定位實驗.按照圖5 所示搭建室內(nèi)測試場景，小場地實驗在實驗室內(nèi)進行，實驗室的長寬分別為14 m 和8 m.根據(jù)實驗室場地大小，首先將4 個UWB基站架設(shè)在場地的4 個角落，然后在內(nèi)部選擇矩形ABCD作為智能小車移動的軌跡，最后在BC和CD段放置3 個障礙物，模擬UWB 的NLOS 路段.其中小車行進的固定軌跡作為參考值與融合算法的輸出軌跡進行對比.

圖5 實驗場景

對實驗過程中采集到的UWB 數(shù)據(jù)進行解算，并與參考軌跡進行對比，對比結(jié)果如圖6 所示.可以得出在BC和CD段，由于小車行進途中存在障礙物的遮擋，引起NLOS 誤差，導致UWB 定位精度下降且出現(xiàn)明顯的抖動，定位誤差最大為0.79 m，平均定位精度為0.21 m.而AB段和DA段受NLOS 誤差影響較小，其定位軌跡接近參考軌跡，最大定位誤差為0.51 m，平均定位精度為0.06 m.

圖6 UWB 軌跡與參考軌跡對比

圖7 為了RGB-D 相機通過ORB-SLAM2 算法解算的軌跡.如圖7 所示，在AB段和BC段前半部分視覺SLAM 解算的軌跡接近真值.但是由于累計誤差的存在，BC后半段解算的軌跡開始偏離真實軌跡.在經(jīng)過點C后，解算軌跡與真實軌跡形成一定的夾角，定位誤差也越來越大，最大定位誤差為1.65 m.

圖7 SLAM 軌跡與參考軌跡對比

使用本文提出的SLAM/UWB 融合定位算法進行解算，其結(jié)果如圖8 所示.可以得出其解算軌跡無論是在有障礙的BC和CD段，還是無障礙的AB和DA段都接近真值.其中最大定位誤差為0.34 m，平均定位精度為0.10 m.

圖8 SLAM/UWB 融合軌跡與參考軌跡對比

表1 給出了UWB、SLAM 以及SLAM/UWB 融合定位在每段路徑中的誤差統(tǒng)計.經(jīng)過分析可知，在無遮擋的室內(nèi)環(huán)境下，UWB 定位精度可達厘米級，在有遮擋的情況下，也能達到分米級定位.視覺SLAM 在初始階段具有厘米級的定位精度，但隨著誤差累計，定位結(jié)果逐漸偏離實際軌跡.SLAM/UWB融合定位算法在全路徑中定位精度最高，即使在有遮擋的復雜環(huán)境下，也能達到0.14 m 的定位精度.

表1 單一定位與本文融合定位誤差對比 m

使用傳統(tǒng)EKF 算法將視覺SLAM 與UWB 定位數(shù)據(jù)進行融合，其融合結(jié)果如圖9 所示.結(jié)果表明，在觀測良好的環(huán)境下，EKF 算法解算的結(jié)果能夠很好地接近真實軌跡.但在UWB 存在NLOS 誤差的情況下，定位誤差變大.傳統(tǒng)EKF 算法與本文算法解算的軌跡和真實軌跡誤差曲線如圖10 所示，在前期AB段和BC段兩種融合算法效果相差不大，傳統(tǒng)EKF 算法部分優(yōu)于本文算法，但是在后期CD段，傳統(tǒng)EKF 算法誤差峰值遠遠大于本文算法，同時本文算法軌跡也更加平滑.兩種融合算法的定位精度如表2 所示，本文算法平均定位精度相比傳統(tǒng)EKF 算法提高的0.03 m，最大定位誤差減少0.25 m.

圖9 EKF、本文算法與真實軌跡對比

圖10 EKF、本文算法與真實軌跡誤差曲線

表2 小場地下兩種融合定位算法誤差對比 m

大場地定位試驗在實驗室外的走廊進行，走廊的長和寬分別為32 m、8 m，其形狀呈現(xiàn)“冂”字形.實驗布置與小場地試驗類似，在四個角架設(shè)UWB基站，并在智能小車行進的軌跡旁放置障礙物.圖11 為大場地下UWB 定位、SLAM 定位與參考軌跡對比，可以看出UWB 定位在BC段的兩端，由于NLOS 誤差，定位結(jié)果出現(xiàn)較大跳變和抖動，最大誤差可達1.08 m，其平均定位精度為0.21 m.SLAM 定位在B點轉(zhuǎn)彎處由于圖像變化較快，導致誤差累計，其結(jié)果逐漸偏離參考軌跡，最大定位誤差為1.96 m，平均定位精度為0.82 m.

圖11 大場地單一定位方式實驗結(jié)果

圖12 為大場地下比較本文融合算法和傳統(tǒng)EKF融合算法，從圖中可以看出本文融合算法比傳統(tǒng)EKF融合的軌跡更加平滑，尤其是在BC段兩端UWB 出現(xiàn)NLOS 誤差的情況下，定位結(jié)果也更接近參考軌跡.表3 給出了兩種融合算法的誤差統(tǒng)計，本文算法的定位精度能夠達到0.15 m，相比于傳統(tǒng)EKF 算法擁有更高的定位精度和更好的魯棒性.

圖12 大場地兩種融合定位實驗結(jié)果

表3 大場地下兩種融合定位算法誤差對比 m

4 結(jié)束語

本文利用視覺SLAM技術(shù)和UWB技術(shù)，針對目前室內(nèi)移動機器人單一定位方式精度低、穩(wěn)定性差的問題，提出一種SLAM/UWB 融合定位算法.該算法在傳統(tǒng)EKF 濾波基礎(chǔ)上添加測量噪聲估計器和閾值檢測，提升了融合定位系統(tǒng)在復雜環(huán)境下的穩(wěn)定性.實驗結(jié)果表明：其定位精度優(yōu)于單一視覺SLAM或UWB 定位；當UWB 出現(xiàn)NLOS 影響時，該算法比傳統(tǒng)EKF 算法擁有更穩(wěn)定的定位效果.綜上，本文融合定位算法能夠同時滿足室內(nèi)移動機器人對定位精度和穩(wěn)定的要求.