非視距環境下的IMU和UWB融合定位系統

彭俊杰，李英華，王春琦，周小安

(1.深圳大學 電子與信息工程學院，廣東 深圳 518060；2.國家無線電監測中心，北京 102609)

0 引言

自組織網絡系統內沒有中央控制實體，節點之間可以直接共享資源和服務，具有完全的自主性和平等的地位，使得自組織網絡具有靈活、無中心、自組織、可擴展性強、負載平衡和很好的抗毀性等特點，越來越受到學術界和工業界的關注。自組織的對等計算模式已給計算機和通信網絡領域帶來了重大的變革，在文件共享、內容分發、協同計算和即時通信等方面，以及軍事、傳感器網絡、緊急場合、動態臨時場合和商業上都有廣泛的應用。隨著物聯網技術的不斷發展，基于位置的服務(Location Based Service，LBS)越來越受到人們的重視。室外場景下的定位應用已經十分成熟，但是面對更為復雜的室內環境，室外定位技術不能滿足人們的日常生活需求。在數字化的交互應用場景下，高精度的室內定位系統有著巨大的市場潛力。例如智慧家居、智慧園區和智慧商超的建設，都需要高精度的室內定位系統作為硬件基礎。目前主流的室內定位技術包括超寬帶、藍牙、WiFi、ZigBee、紅外線、LED和超聲波等方式。超寬帶(Ultra-Wide Band，UWB)相較于其他傳感器具有更低的能耗和更高的定位精度，但是基于信號傳輸的傳感器都會受到非視距(None-Line-of-Sight，NLOS)的干擾，從而影響到定位的精度[1]。UWB的通信機制主要包括基于時間、到達角度和信號強度的方式。在基于時間的測距方式中包括基于信號的到達時間(Time of Arrival，TOA)[2]、到達時間差(Time Difference of Arrival，TDOA)[3]、時分多址(Time Division Multiple Access，TDMA)和雙向測距(Two-way Ranging，TWR)。其中TWR和TDOA技術的測距精度較高，也更易于實現，且對基站和標簽之間的時間同步要求不高?；诘竭_角度的定位方式中主要包括信號到達角度(Angle of Arrival，AOA)和到達相位差(Phase Difference of Arrival，PDOA)。慣性測量單元(Inertial Measurement Unit，IMU)不受NLOS的影響，可以結合IMU的定位信息進行融合定位。

1 定位系統描述

基于自組織的分布式網絡管理模型如圖1所示，主要由3部分組成：網管服務器、分布式網絡代理(Distributed Network Agent，DNA)和被管設備。

圖1 自組織的分布式網絡管理模型Fig.1 Self-organization distributed network management model

IMU傳感器中的陀螺儀通過姿態矩陣進行姿態解算，加速度計通過二次積分獲得位置信息。UWB通過基站與標簽之間的通信解算出位置信息[4]。通過當前位置和UWB測得的距離信息采用支持向量機(Support Vector Machine，SVM)[5-6]進行NLOS信號的識別分類。卡爾曼濾波(Kalman Filter，KF)通常用于室內定位的算法融合[7-9]，也有基于KF提出的2步KF算法定位[10-11]，在視距(Line-of-Sight，LOS)場景下利用UWB基于最小二乘(Least Squares，LS)[12]算法定位，提出了采用LS和EKF融合的定位算法[13-14]。

1.1 IMU定位系統

IMU主要元件有陀螺儀、加速度計和磁力計。其中，陀螺儀可以得到各個軸的加速度，加速度計能得到x，y，z方向的加速度，而磁力計能獲得周圍磁場的信息。IMU器件主要的工作是將3個傳感器的數據融合得到較為準確的姿態信息。陀螺儀測量角速度，通過一次積分得到角度值。加速度計測量加速度，通過一次積分得到速度。通常來說陀螺儀的溫漂是比較嚴重的，溫漂的數據既與溫度相關又與時間相關，在不同的溫度和上電時間的情況下，會產生不同的溫漂值。

慣性導航中基于牛頓力學定律的基礎，采用慣性導航坐標系。載體進行導航的過程中實時地確定其導航參數，如載體的姿態、位置和速度等。載體的導航通過各個坐標系之間的關系確定，慣性導航坐標系下解算的坐標信息需要經過坐標系轉換，才能應用到實際的場景下，慣性導航坐標系的轉換如圖2所示。

圖2 慣性導航坐標系的轉換Fig.2 Transformation of inertial navigation coordinate system

圖2中，ψ，θ，γ分別表示載體的航向角、俯仰角和橫滾角，不同顏色的坐標軸表示每一次旋轉后的坐標系狀態，經過3次坐標軸的旋轉就能實現從載體坐標系變換到導航坐標系。第1次繞垂直于z0軸的平面旋轉ψ角，從坐標系Ox0y0z0轉換到坐標系Ox1y1z0，第2次繞垂直于x1軸的平面旋轉θ角，從坐標系Ox1y1z0轉換到坐標系Ox1yz1，第3次繞垂直于y軸的平面旋轉γ角，從坐標系Ox1yz1轉換到坐標系Oxyz。通過3次旋轉可以得到方向余弦矩陣：

(1)

當ψ，θ，γ都比較小時，略去二階小量，得：

(2)

式(2)即為實際定位中采取的旋轉矩陣?；谛D矩陣，可以將IMU在導航坐標系下解算的位置信息轉換到地理坐標系中。

1.2 UWB定位系統

UWB是一種基于極窄脈沖無載波的無線技術，傳輸速率高，發射功率較低，穿透能力較強，能在短距離內高速傳輸數據。UWB不采用正弦載波，而是利用ns級的非正弦波窄脈沖傳輸數據，因此其所占的頻譜范圍很寬，適用于高速、近距離的無線個人通信。UWB、寬帶和窄帶的信號比較模型如圖3所示。

圖3 超寬帶/寬帶/窄帶信號比較模型Fig.3 UWB/wideband/narrowband signal comparison model

UWB信號相較于寬帶和窄帶信號擁有更豐富的頻譜資源。UWB信號不需要依靠載頻傳輸，在信號的發射端不需要混頻器和振蕩器將載波變換至所需頻率波段，而在接收端則省略掉了載波恢復的步驟。相較于應用于室內定位中的其他技術，UWB室內定位系統結構更加簡單且易于部署。

UWB的定位算法中，采用TWR算法無需時鐘同步，測距精度較高，抗干擾能力強，且易于實現。TWR原理圖如圖4所示。

標簽向基站發送一個輪詢信號poll，經過Treply1的反應時間處理后，發送給標簽一個回應信號response。標簽從發送到接收信號的一個輪回時間是Tround1。接收到信號后，經過Treply1的處理時間標簽再次向基站發送一個最終信號final?；緩陌l送到接收的一個輪回時間是Tround2，結束一次信號飛行時間的測量，從而得到信號的飛行時間:

(3)

然后，基于信號的傳輸時間解算出距離信息。

圖4 TWR原理Fig.4 Schematic diagram of TWR

1.3 NLOS信號的檢測

雖然UWB技術具有較強的抗干擾能力和抗多徑衰弱能力，但是在NLOS情況下，UWB信號的測距定位能力將大大降低[15]。因此在進行UWB定位時，要進行NLOS的識別。在TWR的解算過程中，脈沖信號一共傳遞了3次。當出現障礙物遮擋時，信號的傳輸會多次受到NLOS的影響，相比于前一時刻的信號傳播時間會出現較大的跳變。提出使用機器學習[16-17]的方式，利用SVM對實際場景中的NLOS和LOS情況下的距離測量值進行訓練，得到符合對應應用場景的分辨閾值。由于移動障礙物可能會對UWB系統的NLOS測距結果產生規律性的影響，因此通過設置障礙物的位置模擬NLOS場景，并研究障礙物位置與NLOS檢測之間的關系[18-19]。NLOS/LOS樣點采集模型如圖5所示。

圖5 NLOS/LOS樣點采集模型Fig.5 NLOS/LOS sample collection model

在NLOS情況下的測量誤差曲線圖如圖6所示，其中橫坐標表示測量范圍，縱坐標表示測量值與實際值的差值，可以看出，在NLOS情況下會存在正誤差，同時可以看出障礙物所在不同位置對測距精度的影響。

圖6 NLOS環境下的測量誤差Fig.6 Measurement error in NLOS environment

對應的測量值與實際值之間的比值情況曲線圖如圖7所示，縱坐標表示測量值與實際值的比值情況，且比值不低于1。比值曲線圖相較于誤差曲線圖線條更加平滑，曲線的變化情況基本一致。

圖7 NLOS測量值與實際值的比值Fig.7 Ratio of NLOS measured value to actual value

圖8是在LOS情況下的測量誤差，橫坐標表示標簽距離基站的距離，縱坐標表示測量值與實際值的差值，可以看出實驗中的LOS情況下會存在負誤差[20]。

圖8 LOS環境下的測量誤差Fig.8 Measurement error in LOS environment

對應的測量值與實際值之間的比值情況曲線圖如圖9所示，縱坐標表示測量值與實際值的比值，且比值不超過1。誤差曲線圖的數據抖動存在一定的范圍。測量值與實際值之間的比值比誤差值更加平滑，更加符合SVM的閾值進行線性的劃分。因此,選取測量值與實際值的比值作為SVM的訓練樣本。

圖9 LOS測量值與實際值的比值Fig.9 Ratio of LOS measured value to actual value

支持向量機的訓練結果如圖10所示。

圖10 支持向量機的分類結果Fig.10 Classification results of SVM

圖中黑色方塊為NLOS樣本點，藍色加號為LOS樣本點，綠色線條為訓練得到的閾值。分類結果顯示，產生的閾值線條很好地區分了上半區域的NLOS樣本點和下半區域的LOS樣本點。

2 IMU和UWB融合定位策略

2.1 LS定位算法

基于UWB測得的基站到標簽的3邊距離，通過LS可以解算出標簽的坐標。LS的具體算法如圖11所示。

圖11 最小二乘定位算法Fig.11 Least square positioning algorithm

LS的具體算法如下所示：

設在二維平面內目標Tag(x,y)和觀測站BS(xn,yn)，通過計算可以得到二者之間的距離dn：

(4)

本文提出的新型分布式動態自組織網絡的管理體系，基于自組織的DNA網絡實現分布式的自組織網絡和復雜動態網絡的管理，從上往下均與最后一列相減后，用矩陣的形式可表示為：

(5)

(6)

(7)

從而可以得到標簽的坐標為：

X=(ATA)-1ATb。

(8)

2.2 LS與EKF融合定位

① EKF算法流程如下：

設置狀態方程和觀測方程分別為：

X(k+1)=ΦX(k)+ΓW(k)，

(9)

Y(k)=HX(k)+V(k),

(10)

式中，在離散時間k下的狀態為X(k)；Φ為狀態轉移矩陣；Γ為噪聲驅動矩陣；W(k)為白噪聲。

得到的協方差預測和增益矩陣分別為：

P(k+1|k)=ΦP(k|k)ΦT+ΓQΓT，

(11)

K(k+1)=P(k+1|k)HT[HP(k+1|k)HT+R]-1。

(12)

狀態更新和協方差更新分別為：

(13)

(14)

P(k+1|k+1)=[In-K(k+1)H]P(k+1|k)。

(15)

② LS-EKF算法流程如下：

將3個基站觀測得到的距離信息d1，d2和d3作為觀測向量，將IMU解算得到的水平和垂直方向的位移和速度作為狀態向量X(k)=[xx(k)，xy(k)，vx(k)，vy(k)]T，其中基于速度的狀態方程為：

(16)

表示成矩陣的形式：

X(k+1)=FX(k)+GW(k)，

(17)

(18)

設置標簽到3個基站的距離信息的觀測值作為觀測向量，且表示為：

(19)

(20)

3 仿真設計和結果分析

為了驗證所提出的算法的性能，通過仿真實驗模擬場地的大小為6 m×6 m，假設運動是線性且勻加速的，標簽初始化位置設置為參考坐標(0,0)。最初的水平和垂直方向的速度設置0.15 m/s，加速度設置為0.002 m/s2。在運行期間設置過程噪聲的方差大小為Q=10-8，用來模擬真實場景下的標簽運動軌跡。在使用傳感器觀測時，會存在一定的觀測噪聲，設置觀測噪聲的方差系數和2種情況。設置信號采樣周期為t=1 s，并且總模擬時間為50 s，即采樣次數為N=50。

在不同觀測噪聲下，通過LS和LS-EKF算法的定位軌跡如圖12(a)所示。在同等觀測噪聲下,UWB和IMU經過EKF濾波過后的定位軌跡要比只經過LS定位算法的定位軌跡數據抖動更小，軌跡更加平滑。且觀測噪聲也會對定位結果有較大影響。圖12(b)顯示了2種算法的誤差變化趨勢，在觀測噪聲R=0.1時，經過LS算法定位的軌跡誤差在0.1～1.4 m抖動，經過EKF濾波后定位誤差下降并逐漸保持穩定在0.2～0.4 m。在觀測噪聲R=0.01時，LS-EKF算法的誤差明顯要比LS算法平滑得多，且定位誤差更小。圖12(c)是2種算法對應的誤差累計分布函數(Cumulative Distribution Function，CDF)圖，在觀測噪聲R=0.1時，LS和LS-EKF算法在仿真的數據范圍內的最大誤差分別為1.3 m和0.85 m；在觀測噪聲R=0.01時，LS和LS-EKF算法在仿真的數據范圍內的最大誤差分別為0.3 m和0.2 m。

(a) LS和LS-EKF定位軌跡

(b) 定位誤差

(c) 累計誤差分布函數圖12 LS和LS-EKF定位算法Fig.12 LS and LS-EKF positioning algorithms

LS和LS-EKF算法平均誤差和標準差如表1所示，在觀測噪聲R=0.1時，LS和LS-EKF算法平均誤差為0.403 2 m和0.104 8 m，誤差的標準差大小為0.074 4 m和0.028 5 m。在觀測噪聲R=0.01時，LS和LS-EKF算法平均誤差為0.028 5 m和0.014 7 m，誤差的標準差大小為0.007 7 m和0.001 8 m。在觀測噪聲分別為R=0.1和R=0.01時，相較于LS算法，LS-EKF算法的定位精度分別提高了74%和48%。

表1 LS和LS-EKF算法平均誤差和標準差

4 結束語

本文基于對IMU和UWB傳感器的研究，結合了UWB的高精度定位性能和IMU不受NLOS影響的特性，提出了一種基于傳感器融合的室內導航定位系統。為了解決室內環境下的NLOS對定位的干擾，需要先進行NLOS的識別。通過對NLOS和LOS情況下的測距研究，基于測量值與實際值的不同比值作為樣本點，應用SVM的機器學習方式提出了一種適用于特定實驗場景下的NLOS的識別方法，能夠有效地識別NLOS信號。隨后在LOS場景下基于LS和EKF算法提出了一種LS-EKF算法，實驗仿真結果表明,提出的LS-EKF算法相較于LS算法定位精度提高了48%～74%。