楊瑞 張敏 張潤(rùn)豪

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

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

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

0 引言

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

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

定位過(guò)程：首先，根據(jù)錨點(diǎn)（Anchor）得出第i顆錨點(diǎn)的真實(shí)位置坐標(biāo)（xi，yi，zi）；其次，根據(jù)偽距方法，計(jì)算出靶點(diǎn)（Tag）與第i顆錨點(diǎn)之間的距離di，對(duì)方程組求解得出靶點(diǎn)的三維坐標(biāo)（x，y，z），根據(jù)t時(shí)刻待測(cè)用戶(hù)處的基站所接收到的信號(hào)、錨點(diǎn)信號(hào)到達(dá)的時(shí)間等數(shù)據(jù)可以確定以下4個(gè)方程：［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個(gè)方程中待測(cè)靶點(diǎn)的坐標(biāo)（x，y，z）和靶點(diǎn)與錨點(diǎn)時(shí)鐘之間的誤差時(shí)鐘Δt為未知數(shù)，di為錨點(diǎn)i到靶點(diǎn)的距離，（xi，yi，zi）為錨點(diǎn)i在t時(shí)刻的位置坐標(biāo)，只要測(cè)量得到4顆錨點(diǎn)到接收機(jī)的偽距，就可以得到待測(cè)靶點(diǎn)的三維位置（x，y，z）和靶點(diǎn)與錨點(diǎn)時(shí)鐘之間的誤差時(shí)鐘Δt，GPS室外定位系統(tǒng)具有全天候、全覆蓋，高精度、高效率的特點(diǎn)。

1 基于線(xiàn)性卡爾曼濾波的定位模型

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

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

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

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個(gè)錨點(diǎn)坐標(biāo)信息（i=1，2，3，4）。對(duì)4個(gè)距離點(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ù)卡爾曼濾波原理對(duì)解算位置的濾波處理

預(yù)測(cè)過(guò)程為：

假設(shè)已知運(yùn)動(dòng)物體的初始狀態(tài)概率密度，在k-1時(shí)刻的狀態(tài)概率密度為P（xk-1|zk-1），由一階馬爾科夫過(guò)程得出k時(shí)刻的狀態(tài)，這個(gè)狀態(tài)只與k-1時(shí)刻狀態(tài)有關(guān)，與其他時(shí)刻狀態(tài)無(wú)關(guān)。因此，預(yù)測(cè)過(guò)程是由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）

更新的過(guò)程為：利用運(yùn)動(dòng)物體最新的觀測(cè)值z(mì)k，對(duì)先驗(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)動(dòng)過(guò)程中加速度均值會(huì)收斂于零，假設(shè)在ti時(shí)刻加速度值為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)的過(guò)程噪聲，并且假設(shè)橫軸、縱軸的噪聲是獨(dú)立的，則方差矩陣可以定義為：

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

0Qy（ti-1）（11）

針對(duì)上述運(yùn)動(dòng)目標(biāo)的機(jī)動(dòng)模型，首先，對(duì)采樣的數(shù)據(jù)進(jìn)行計(jì)算；其次，設(shè)置系統(tǒng)跟蹤的模型參數(shù)，設(shè)定靶點(diǎn)的初始值；再次，用自適應(yīng)的參數(shù)機(jī)動(dòng)目標(biāo)模型對(duì)目標(biāo)的運(yùn)動(dòng)軌跡進(jìn)行估計(jì)，設(shè)第一個(gè)點(diǎn)為其初始位置，沿逆時(shí)針?lè)较蛞来螁螕簦蛔詈?，得到其運(yùn)動(dòng)軌跡。

3 結(jié)語(yǔ)

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

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

參考文獻(xiàn)

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

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

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

［4］姚健，劉送永，崔玉明，等.基于KF/PF測(cè)距濾波的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