楊瑞 張敏 張潤豪

摘要：文章主要解決在強(qiáng)干擾下的室內(nèi)精確定位的問題。根據(jù)UWB測距原理得出靶點(diǎn)坐標(biāo)與錨點(diǎn)距離的關(guān)系，求解出靶點(diǎn)的三維坐標(biāo)，利用卡爾曼濾波方法進(jìn)一步提升定位坐標(biāo)的精度，從而得到靶點(diǎn)的精確位置，并對采集的數(shù)據(jù)進(jìn)行計(jì)算。另外，設(shè)定靶點(diǎn)的初始值，建立自適應(yīng)參數(shù)目標(biāo)模型對目標(biāo)的運(yùn)動軌跡進(jìn)行估計(jì)。結(jié)果顯示，根據(jù)卡爾曼濾波方法對其進(jìn)行預(yù)測和定位，預(yù)測結(jié)果與真實(shí)結(jié)果基本吻合，并最終得到動態(tài)靶點(diǎn)的運(yùn)動軌跡圖。隨著UWB技術(shù)的不斷發(fā)展，相信未來室內(nèi)的定位技術(shù)可以得到更快的發(fā)展。

關(guān)鍵詞：線性卡爾曼濾波；誤差估計(jì)；精確定位

中圖分類號：TP391? 文獻(xiàn)標(biāo)志碼：A

0 引言

用于戶外精準(zhǔn)定位和位置導(dǎo)航的技術(shù)已經(jīng)成熟，人們習(xí)慣在室外使用GPS獲得位置導(dǎo)航。UWB超寬帶定位技術(shù)是近年來發(fā)展起來的一種定位方式。它是一種具有短距離高速無線通信的技術(shù)，能夠組建個人的高速無線局域網(wǎng)，使其與互聯(lián)網(wǎng)互聯(lián)互通。它的信號定位精度高，對信號的衰落表現(xiàn)得不明顯，且安全性高、復(fù)雜程度低，可應(yīng)用于各個領(lǐng)域。隨著技術(shù)的不斷發(fā)展，UWB已經(jīng)成了短距離無線通信的熱點(diǎn)，供廣大學(xué)者科研人員研究。

UWB的定位功能與衛(wèi)星定位原理類似，即通過4個已知坐標(biāo)點(diǎn)的基站信息，攜帶定位標(biāo)簽，通過發(fā)射一定頻率的脈沖波，對已知的4個位置的基站進(jìn)行實(shí)時測距，最終通過算法定位出標(biāo)簽的位置［1］。對于衛(wèi)星GPS定位原理進(jìn)行分析，可以大致得到UWB的定位原理如圖1所示。分析如下：已知錨點(diǎn)坐標(biāo)信息，通過在靶點(diǎn)附近安裝基站用于接收標(biāo)簽傳來的一定頻率脈沖波，就可以計(jì)算出靶點(diǎn)高精度的瞬時位置［2］。UWB室內(nèi)定位基站的三維定位布置如圖1所示。

定位過程：首先，根據(jù)錨點(diǎn)（Anchor）得出第i顆錨點(diǎn)的真實(shí)位置坐標(biāo)（xi，yi，zi）；其次，根據(jù)偽距方法，計(jì)算出靶點(diǎn)（Tag）與第i顆錨點(diǎn)之間的距離di，對方程組求解得出靶點(diǎn)的三維坐標(biāo)（x，y，z），根據(jù)t時刻待測用戶處的基站所接收到的信號、錨點(diǎn)信號到達(dá)的時間等數(shù)據(jù)可以確定以下4個方程：［3］

d1=（x1-x）2+（y1-y）2+（z1-z）2+c·Δt

d2=（x2-x）2+（y2-y）2+（z2-z）2+c·Δt

d3=（x3-x）2+（y3-y）2+（z3-z）2+c·Δt

d4=（x4-x）2+（y4-y）2+（z4-z）2+c·Δt（1）

上述4個方程中待測靶點(diǎn)的坐標(biāo)（x，y，z）和靶點(diǎn)與錨點(diǎn)時鐘之間的誤差時鐘Δt為未知數(shù)，di為錨點(diǎn)i到靶點(diǎn)的距離，（xi，yi，zi）為錨點(diǎn)i在t時刻的位置坐標(biāo)，只要測量得到4顆錨點(diǎn)到接收機(jī)的偽距，就可以得到待測靶點(diǎn)的三維位置（x，y，z）和靶點(diǎn)與錨點(diǎn)時鐘之間的誤差時鐘Δt，GPS室外定位系統(tǒng)具有全天候、全覆蓋，高精度、高效率的特點(diǎn)。

1 基于線性卡爾曼濾波的定位模型

卡爾曼濾波（Kalman）目前已經(jīng)廣泛地應(yīng)用于各領(lǐng)域，通過使用距離測量結(jié)果來對移動節(jié)點(diǎn)進(jìn)行定位，輔助定位的節(jié)點(diǎn)可以是靜止的也可以是移動的［4］。在使用到達(dá)時間差和到達(dá)角測量結(jié)果的情況下，可以采用卡爾曼濾波方式，并假定錨點(diǎn)和移動節(jié)點(diǎn)之間的信號傳播在視距范圍內(nèi)，對位置進(jìn)行精確定位［5］。

1.1 計(jì)算出靶點(diǎn)的三維坐標(biāo)

由UWB定位原理可以得到靶點(diǎn)和錨點(diǎn)坐標(biāo)之間的4個距離為：

d02=（x1-x）2+（y1-y）2+（z1-z）2

d12=（x2-x）2+（y2-y）2+（z2-z）2

d22=（x3-x）2+（y3-y）2+（z3-z）2

d32=（x4-x）2+（y4-y）2+（z4-z）2（2）

其中：di為靶點(diǎn)到錨點(diǎn)的距離（i=1，2，3，4）；x，y，z分別代表靶點(diǎn)的坐標(biāo)，記為X（x，y，z）；xi，yi，zi分別代表錨點(diǎn)位置的坐標(biāo)；Zi（xi，yi，zi）記為4個錨點(diǎn)坐標(biāo)信息（i=1，2，3，4）。對4個距離點(diǎn)分別求差，可以得到：

2（x1-x2）x+2（y1-y2）y+2（z1-z2）z=d12-d02+x12-x22+y12-y22+z12-z22

2（x1-x3）x+2（y1-y3）y+2（z1-z3）z=d22-d02+x12-x32+y12-y32+z12-z32

2（x1-x4）x+2（y1-y4）y+2（z1-z4）z=d32-d02+x12-x42+y12-y42+z12-z42（3）

將其化為矩陣形式：

AX=B（4）

綜上，可以計(jì)算出靶點(diǎn)的三維坐標(biāo)：

X=A-1B（5）

1.2 根據(jù)卡爾曼濾波原理對解算位置的濾波處理

預(yù)測過程為：

假設(shè)已知運(yùn)動物體的初始狀態(tài)概率密度，在k-1時刻的狀態(tài)概率密度為P（xk-1|zk-1），由一階馬爾科夫過程得出k時刻的狀態(tài)，這個狀態(tài)只與k-1時刻狀態(tài)有關(guān)，與其他時刻狀態(tài)無關(guān)。因此，預(yù)測過程是由P（xk-1|z1：k-1）得到P（xk|z1：k-1），即：

P（xk，xk-1|Zk-1）=p（xk|xk-1，Z1：k-1）p（xk-1|Z1：k-1）（6）

更新的過程為：利用運(yùn)動物體最新的觀測值zk，對先驗(yàn)概率密度p（xk-1|Z1：k-1）進(jìn)行修正，實(shí)現(xiàn)后檢驗(yàn)概率密度p（xk-1|Z1：k）的遞推：

P（xk|Z1：k）=p（zk|Z1：k-1）p（xk|Z1：k-1）p（zk|Z1：k-1）（7）

假設(shè)zk只由xk決定，可得：

P（zk|xk，Z1：k-1）=p（zk|xk）（8）

其中，p（zk|Z1：k-1）為歸一化常量，因此：

P（xk|Z1：k）=p（zk|xk）p（xk|Z1：k-1）∫p（zk|xk）p（xk|Z1：k-1）dxk（9）

2 不規(guī)則采樣間隔下的系統(tǒng)模型

由于室內(nèi)目標(biāo)在運(yùn)動過程中加速度均值會收斂于零，假設(shè)在ti時刻加速度值為0，本文使用如下不規(guī)則采樣間隔下的系統(tǒng)模型：

X（ti）=A（ti-1）X（ti-1）+w（ti-1）（10）

其中，A（ti-1）=Ax（ti-1）00Ay（ti-1）為狀態(tài)轉(zhuǎn)移矩陣，w（ti-1）=［wx（ti-1）wy（ti-1）］T代表橫縱坐標(biāo)的過程噪聲，并且假設(shè)橫軸、縱軸的噪聲是獨(dú)立的，則方差矩陣可以定義為：

Q（ti-1）=Qx（ti-1）0

0Qy（ti-1）（11）

針對上述運(yùn)動目標(biāo)的機(jī)動模型，首先，對采樣的數(shù)據(jù)進(jìn)行計(jì)算；其次，設(shè)置系統(tǒng)跟蹤的模型參數(shù)，設(shè)定靶點(diǎn)的初始值；再次，用自適應(yīng)的參數(shù)機(jī)動目標(biāo)模型對目標(biāo)的運(yùn)動軌跡進(jìn)行估計(jì)，設(shè)第一個點(diǎn)為其初始位置，沿逆時針方向依次單擊；最后，得到其運(yùn)動軌跡。

3 結(jié)語

本文通過對有無信號干擾條件下的超帶寬精確定位，建立了基于卡爾曼濾波位置的數(shù)學(xué)模型，通過有效性檢驗(yàn)與誤差估計(jì)，給出了不同的靜態(tài)和動態(tài)定位的目標(biāo)物精確定位。

對實(shí)際場景實(shí)測，采集到一定數(shù)量的錨點(diǎn)與靶點(diǎn)之間的距離信息，通過UWB定位原理和超寬帶測距方法（TOF），來解決目標(biāo)物在是否有信號干擾條件下的超寬帶精確定位的問題。本文建立的Kalman位置定位模型，經(jīng)過數(shù)據(jù)處理之后帶入模型中，坐標(biāo)數(shù)據(jù)明顯更加精確，坐標(biāo)位置更加接近實(shí)際值。所建立的Kalman位置定位模型有效性強(qiáng)，可以進(jìn)行精確定位，非常可靠。建立的位置信號分類模型可以對不同的實(shí)驗(yàn)場景和信號有無干擾條件下進(jìn)行精準(zhǔn)分類，得到最終的運(yùn)動軌跡。

參考文獻(xiàn)

［1］楊洲.基于UWB/MEMS的高精度室內(nèi)定位技術(shù)研究［D］.徐州：中國礦業(yè)大學(xué)，2015.

［2］楊玉林.基于卡爾曼濾波和粒子濾波的目標(biāo)跟蹤性能對比［J］.佳木斯大學(xué)學(xué)報（自然科學(xué)版），2021（3）：72-75，78.

［3］陳燕.基于UWB的高精度室內(nèi)三維定位技術(shù)研究［D］.成都：電子科技大學(xué)，2018.

［4］姚健，劉送永，崔玉明，等.基于KF/PF測距濾波的UWB精確定位試驗(yàn)研究［J］.煤炭工程，2021（9）：122-126.

［5］SAN MARTN J，CORTS A，ZAMORA-CADENAS L，et al. Precise positioning of autonomous vehicles combining UWB ranging estimations with on-board sensors［J］.Electronics，2020（8）：1238.

（編輯 姚 鑫）

Research on UWB precise positioning technology in complex environment

Yang? Rui， Zhang? Min， Zhang? Runhao

（Xijing University， Xian 710123， China）

Abstract：? This paper mainly solves the problem of indoor precise positioning under strong interference. According to the UWB ranging principle， the relationship between the target coordinates and the anchor distance is obtained， and the three-dimensional coordinates of the target are solved.The accuracy of the positioning coordinates is further improved by using the Kalman filter method， so as to obtain the accurate position of the target and calculate the collected data. In addition， the initial value of the target is set， and an adaptive parameter target model is established to estimate the motion trajectory of the target.The results show that the prediction and positioning are carried out according to the Kalman filter method， and the prediction results are basically consistent with the real results. Finally， the motion trajectory map of the dynamic target is obtained. With the continuous development of UWB technology， it is believed that indoor positioning technology can be developed faster in the future.

Key words： linear Kalman filter; error estimation; precise positioning