0 引言

    移動機器人定位是其自主導航中的最基本環節，也是移動機器人完成任務必須解決的問題。對定位的要求是定位精度高（亞米級精度），實時性好。目前移動機器人室內定位主要分為兩大類：(1)相對定位法，即航跡推算法^[1]。利用機器人所裝備的各種傳感器獲取機器人的運動動態信息，通過遞推累計公式獲得機器人相對初始狀態的估計位置。使用的傳感器主要是碼盤和慣性傳感器。但是他們都有一個共同的缺點：存在累積誤差，隨著行駛時間、距離的不斷增加，誤差也不斷增大，不適合長時間長距離的精確定位。(2)絕對定位法，即機器人通過獲取外界一些位置等已知的參照信息，通過計算自己與參照信息之間的相互關系解算出自己的位置。絕對定位法主要采用同步定位與建圖^[2]（Simultaneous Localization and Mapping，SLAM）、視覺定位方法以及基于信標定位等方法。其中SLAM定位法和視覺定位數據量大，價格昂貴,目前只適宜試驗研究使用。而且，這兩種方法只適用于一些結構簡單的環境，對移動機器人一般工作的復雜室內環境并不能提供很好的定位精度。因此，本系統選用基于UWB的信標定位的方法來實現復雜室內環境下的移動機器人室內定位。

    UWB信號具有超高分辨率、抗多徑效應、穿透力強以及結構簡單的優點，成為目前室內高精度定位的最佳技術^[3]。常用的UWB定位方式為基于到達時間法(Time of Arrival，TOA)以及到達時間差法(Time Difference of Arrival，TDOA)。但是，這TOA法需要標簽與基站之間時鐘同步，TDOA法需要基站與基站之間的時鐘同步，這增大了系統設計難度。

    本文基于UWB技術，采用decaWave公司生產的DWM1000模塊，應用非對稱雙邊雙向測距(Asymmetric Double Sided Two-Way Range，ADS-TWR)技術進行定位，無需基站與標簽之間和基站與基站之間時鐘同步，大大縮減系統設計難度。針對實際應用中由非視距傳播引起的測量誤差，采用卡爾曼濾波算法對測距進行優化，提高定位精度。最終實現一種高精度、高實時性的移動機器人室內定位系統。

1 系統總體設計

    移動機器人室內定位系統主要由UWB無線傳感器網絡和上位機顯示軟件兩部分組成，系統示意圖如圖1所示。系統硬件部分包括基站和安裝在移動機器人頂端的標簽。其中基站分為普通基站(基站2、基站3和基站4)和通信基站(基站1)。標簽和基站均由單片機和DWM1000組成的通信模塊構成，并由軟件配置模塊的角色(標簽或者基站)。DWM1000可以精確地測量UWB信號發送和接收的時間點，通過ADS-TWR 技術測得標簽到各個基站之間的距離，利用UWB通信功能，各基站將距離信息發送給通信基站，最終通過WiFi將距離信息傳送給上位機進行定位并顯示。通信基站的硬件結構圖如圖2所示（標簽和普通基站沒有WiFi模塊）。

2 基于ADS-TWR技術的測距及優化

    無線定位系統定位的準確性取決于測距的精度。UWB定位系統測距誤差的來源除了非視距引起的誤差外，還包括晶振的時鐘漂移引起的誤差。晶振時鐘漂移會影響對信號發送和接收時間點的測量，繼而影響測距的準確性。UWB定位系統最簡單的測距方式是單程測距（One Way Ranging，OWR），但是這對節點之間的時鐘同步要求極其嚴格，而雙程測距（Two Way Ranging，TWR）雖能消除節點之間未能完全同步的影響，但無法消除晶振時鐘漂移的影響。而對稱雙邊雙向測距（Symmetry Double Sided Two-Way Range，SDS-TWR）可以消除晶振時鐘漂移的影響，但是要求信號回復時間嚴格相等，這樣大大降低了定位的實時性^[4]。在此采用ADS-TWR測距技術。

    ADS-TWR測距過程如圖3所示，圖中poll_TX、poll_RX、answer_TX、answer_RX、final_TX、final_RX代表UWB信號離開標簽和基站天線的時間點。測距過程如下：首先標簽向基站請求幀；基站收到請求幀后啟動計時，經過延時T_reply1后向標簽發送應答幀；標簽收到應答幀后啟動計時，并將發送和接收信號時的時間點寫入終止幀，經延時T_reply2后發送給基站；基站收終止幀后表示測距結束。

    距離計算公式如式(1)和(2)所示：

其中，k_t和 k_a為標簽和基站時鐘偏移系數，二者都接近于1。對于20 ppm的時鐘(最壞規格的時鐘)，則k_t和k_a都可以是0.999 98或1.000 02。對于相對較大的測距范圍如100 m，T_tof僅為333 ns，飛行時間測量誤差為6.7 ps，換算為距離誤差僅為2.2 mm。因此，ADS-TWR測距能很好地抑制時鐘漂移的影響。

    ADS-TWR測距不要求應答時間T_reply1和T_reply2相等。因此，在標簽與多個基站進行通信時，可以通過設定各節點的應答時間來減少標簽測距的時間，保證定位系統的實時性。

    圖4為本文所采用的多基站測距機制。標簽向4個基站發送請求幀，基站接收到請求幀后按照設定的應答時間依次向標簽發送應答幀，標簽接收到應答幀后，將4個基站用于計算距離參數寫入終止幀，并發送給所有基站；各基站接收到終止幀后測距結束。基站利用式(1)和(2)計算出距離，然后通過UWB發送給通信基站。實際應用中，對于四基站定位系統，通過優化各基站應答時間，可使單輪測距時間能控制在2 ms左右，完全可以滿足移動機器人定位對實時性的要求。

3 基于卡爾曼濾波的定位算法

    移動機器人在室內活動，不可避免地受到遮擋的影響。標簽與基站之間由于存在人或座椅等遮擋物，這時UWB信號不能進行直線傳播，而是利用衍射、透射和反射的方式到達接收端，就是非視距傳播（NLOS）。這時系統對飛行時間的測量就是會出現誤差，而定位精度也就有了誤差。非視距誤差受到室內環境影響，是一個實時變化的值。由于非視距傳播增加了信號的傳播時間和傳播距離，因此非視距誤差服從一個正均值的隨機過程^[5]。

    假設t時刻標簽到基站的距離為d_i(t)，r_i(t)表示二者之間的真實距離，N_i(t)表示觀測時由環境因素引起的非視距誤差，n_i(t)為零均值高斯噪聲，則可用下式表示它們之間的關系：

    本文采用卡爾曼濾波算法對距離進行優化，對NLOS誤差值進行迭代處理，以此來減小定位誤差。首先利用狀態向量方程對距離信號進行卡爾曼濾波，估計出NLOS誤差值N_i(t)，然后從最初的測距值d_i(t)中除去非視距誤差，得到精確地距離值^[6]。系統的狀態方程和測量方程如下：

其中，Δt為采樣間隔；ω_d(t)和ω_N(t)分別為測量過程中的噪聲誤差分量；β為實驗參數；v_i(t)為測量誤差。再給出t時刻的狀態向量和估計誤差的協方差的初始值之后，就可以通過迭代運算對不同時刻的狀態向量做出估計。由于非視距誤差具有非負性，因此在迭代過程中如N_i(t)出現負值就強制置零。

    根據基站坐標和標簽到基站的距離可以得到以下方程組：

其中(x_i，y_i，z_i)為基站i的坐標，d_i為經過卡爾曼濾波優化后標簽到基站i的距離。用第i個式子減去第j個式子得：

4 系統測試

    實驗地點為12 m×8 m×6 m的南開大學微納加工實驗室，該環境下NLOS干擾現象比較嚴重。為減小人工對真實路徑測量所帶來的誤差，本文實驗結果與實驗室內QUALISYS視頻式三維運動采集系統的定位結果進行對比。通過在標簽上粘貼被動標記獲得QUALISYS系統的定位結果，其定位精度可達到亞毫米級。試驗場景照片如圖5所示，標簽安裝在移動機器人的頂端。

    系統測試分為兩部分，分別對靜態點和動態點進行定位實驗。在試驗場地內隨機選擇10個點作為定點實驗位置，分別做定位實驗 ，每個點采集500次定位數據。實驗結果如圖6所示，其中方框代表 QUALISYS系統測出的結果，小黑點為本系統定位結果。

    利用均方根誤差（RSME）對實驗數據進行分析，計算結果如表1所示。可以看出，在室內NLOS環境下，基于卡爾曼濾波的定位方法在定點定位中誤差能控制在13 cm以內。

    進一步根據移動機器人實際定位的需求，隨機選擇一個標簽以1 m/s的速度在沿著設定路線進行移動，測試結果如圖7所示，其中深色軌跡為QUALISYS系統的定位結果，淺色軌跡為所設計系統定位結果。可以看到標簽偏離目標估計的最大距離為20 cm，保證了移動機器人移動時的定位精度。

5 結語

    本文針對目前移動機器人室內定位方式靈活性差和精度不高的問題，利用UWB技術，設計了一種高精度移動機器人室內定位系統。一方面采用ADS-TWR測距技術保證定位系統的實時性和測距精度；另一方面采用卡爾曼濾波方法進行定位，濾除非視距誤差，保證系統的定位精度。實驗表明，系統具有高精度、高穩定性的特點，滿全能滿足移動機器人室內定位的需求。

參考文獻

[1] CHUNG H，OJEDA L，BORENSTEIN J.Accurate mobile robot dead-reckoning with a precision-calibrated fiber-optic gyroscope[J].Robotics & Automation IEEE Transactions on，2001，17(1)：80-84.

[2] 王煒強.基于視覺定位的地圖構建方法研究[D].浙江：浙江大學，2010.

[3] 房秉毅.基于超寬帶技術的室內定位系統[J].電子技術應用，2006，32(7)：124-127.

[4] 陶偲.基于UWB的室內SDS-TWR測距算法優化和定位算法融合的研究[D].武漢：華中師范大學，2016.

[5] 張宴龍.室內定位關鍵技術研究[D].合肥：中國科學技術大學，2014.

[6] ZHANG L，ZHANG H，CUI X R，et al.Ultra wideband indoor positioning using Kalman filters[C].Advanced Materials Research，2012，433-440：4207-4213.

作者信息:

盧靖宇1，2，余文濤1，2，趙  新1，2，孫廣毅1，2

（1.南開大學 計算機與控制工程學院，天津300350；2.天津市智能機器人技術重點實驗室，天津300350）