劉永昌 龔元明

摘 ?要：利用超寬帶定位技術(shù)，改進(jìn)了基于三邊定位技術(shù)的超寬帶室內(nèi)定位技術(shù)應(yīng)用模型。該模型基于TW-TOF（雙向飛行時(shí)間法）采集定位數(shù)據(jù)，利用三邊定位原理解算坐標(biāo)位置，結(jié)合卡爾曼濾波算法，解決了定位模型中由于設(shè)備自身誤差帶來(lái)的定位誤差大和定位點(diǎn)漂移等問(wèn)題，進(jìn)一步提高了定位精度。經(jīng)現(xiàn)場(chǎng)實(shí)驗(yàn)檢驗(yàn)，定位模型的定位坐標(biāo)精度可達(dá)厘米級(jí)，誤差低于12 cm，濾波后定位均方差減少了38%以上。該模型具有定位算法簡(jiǎn)單可靠、定位結(jié)果誤差小等優(yōu)勢(shì)，為室內(nèi)定位技術(shù)的應(yīng)用提供了參考。

關(guān)鍵詞：室內(nèi)定位;超寬帶;卡爾曼濾波;應(yīng)用模型

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

Application of Ultra-wideband Positioning Technology based on Kalman Filter

LIU Yongchang， GONG Yuanming

（College of Automotive Engineering， Shanghai University of Engineering Science， Shanghai 201620， China）

yongchangsd@163.com; gongyuanming@tsinghua.org.cn

Abstract： This paper proposes to use ultra-wideband （UWB） positioning technology to improve the application model of ultra-wideband indoor positioning technology based on trilateral positioning technology. In this model， positioning data is collected based on TW-TOF （two-way time-of-flight method） and the trilateral positioning principle is used to calculate the coordinate position. Combined with Kalman filter algorithm， it solves the problems of large positioning error and positioning point drift caused by device's errors in the positioning model， further improving the positioning accuracy. On-site experiment shows that positioning accuracy of the positioning model can reach centimeter level， the error is less than 12 cm， and the mean square error of positioning after filtering is reduced by more than 38%. The proposed model has the advantages of simple and reliable positioning algorithm and small positioning error. This model provides a reference for application of indoor positioning technology.

Keywords： indoor positioning; ultra-wideband; Kalman filter; application model

1 ? 引言（Introduction）

我國(guó)各個(gè)行業(yè)產(chǎn)業(yè)信息化、智能化等技術(shù)飛速發(fā)展，高精度室內(nèi)定位技術(shù)的應(yīng)用擁有廣闊的發(fā)展前景。隨著物聯(lián)網(wǎng)滲透到人們生活的方方面面，日常使用的定位技術(shù)中，特別是室內(nèi)定位技術(shù)在人們的生活中扮演著越來(lái)越重要的角色[1]，但是相對(duì)成熟的應(yīng)用技術(shù)還很少。比如，在大型室內(nèi)停車場(chǎng)，由于室內(nèi)信號(hào)的屏蔽作用導(dǎo)致衛(wèi)星定位技術(shù)無(wú)法施展，車輛識(shí)別定位系統(tǒng)還沒(méi)有得到有效地開(kāi)發(fā)。又如，由于山體遮擋，對(duì)于現(xiàn)代交通道路中的長(zhǎng)隧道道路，目前還沒(méi)有有效地解決車輛定位的辦法;大型立交橋立體空間的車輛定位也是亟待解決的問(wèn)題。我國(guó)大力發(fā)展智慧產(chǎn)業(yè)等技術(shù)[2]，智慧農(nóng)業(yè)、智慧交通、智慧物流、智慧礦山等都需要高精度的定位技術(shù)來(lái)支撐智能設(shè)備在現(xiàn)場(chǎng)的應(yīng)用。比如，對(duì)隧道里礦工定位，可以對(duì)施工工人進(jìn)行實(shí)時(shí)自動(dòng)位置跟蹤，全面監(jiān)測(cè)隧道里人員的活動(dòng)軌跡和位置分布。建立雙向報(bào)警系統(tǒng)機(jī)制，在緊急情況下雙向報(bào)警。當(dāng)隧道施工或者礦井開(kāi)采遇到突發(fā)事故時(shí)可以迅速找到被困人員的位置，最大可能保障生命安全。與此同時(shí)，也會(huì)在企業(yè)管理方面提高生產(chǎn)施工效率等。

目前，對(duì)比市場(chǎng)上各種定位技術(shù)，超寬帶已逐漸成為無(wú)線通信領(lǐng)域研究與開(kāi)發(fā)的熱點(diǎn)[1]。首先，近幾年超寬帶技術(shù)的發(fā)展使得設(shè)備成本得以降低，為科研發(fā)展提供了前提條件。同時(shí)，融合和結(jié)合其他定位技術(shù)，超寬帶定位技術(shù)在算法上有了很多突破，比如，超寬帶定位的三種常用定位算法，包括到達(dá)時(shí)間差（Time Difference Of Arrival， TDOA）定位算法[3]、飛行時(shí)間差（Time Of Flight， TOF）定位算法、到達(dá)時(shí)間（Time Of Arrival， TOA）定位算法[4]等。另外，超寬帶定位技術(shù)相比較其他室內(nèi)定位技術(shù)具有定位范圍廣、定位速度快，定位精度高、同時(shí)容納的定位設(shè)備數(shù)量多等優(yōu)勢(shì)。

目前，國(guó)內(nèi)外對(duì)超寬帶定位技術(shù)的研究還處于初級(jí)階段，大規(guī)模的超寬帶應(yīng)用還沒(méi)有成熟的案例。在理論研究方面，國(guó)內(nèi)各個(gè)高校對(duì)超寬帶定位技術(shù)及其各種優(yōu)化算法有比較深入的研究。例如，哈爾濱工程大學(xué)的楊俊杰用神經(jīng)網(wǎng)絡(luò)算法來(lái)補(bǔ)償一些定位中的不確定因素造成的測(cè)距偏差，并利用系統(tǒng)輸出的距離數(shù)據(jù)和系統(tǒng)的測(cè)距偏差數(shù)據(jù)訓(xùn)練了神經(jīng)網(wǎng)絡(luò)參數(shù)[5]。阿壩師范學(xué)院電子信息與自動(dòng)化學(xué)院的莫建麟提出，利用RSSI中的Shadowing傳播模型來(lái)估算待定位節(jié)點(diǎn)與發(fā)送節(jié)點(diǎn)之間的距離，建立關(guān)于待定位節(jié)點(diǎn)和發(fā)送節(jié)點(diǎn)距離的目標(biāo)方程，通過(guò)最小二乘算法來(lái)估計(jì)方程中的系數(shù)[6]等。

2 ? 超寬帶定位原理（UWB positioning principle）

2.1 ? 幾何定位原理

在常見(jiàn)的定位系統(tǒng)中，測(cè)量相對(duì)位置通常有三種方法：測(cè)量角度、測(cè)量距離和測(cè)量距離差[3-4]。其本質(zhì)是測(cè)量基站和標(biāo)簽相對(duì)位置，通過(guò)幾何位置和算法確定相對(duì)位置。本文應(yīng)用的超寬帶定位模組采用的是雙向飛行時(shí)間法（TW-TOF）。雙向飛行時(shí)間法屬于常用定位系統(tǒng)中的到達(dá)時(shí)間法，這種算法可以測(cè)量出標(biāo)簽到基站間的距離。

如圖1所示，表示該基站從發(fā)送信號(hào)時(shí)刻到重新接收到信號(hào)時(shí)刻的時(shí)間間隔，表示標(biāo)簽從接收到信號(hào)的時(shí)刻到發(fā)送信號(hào)時(shí)刻的時(shí)間間隔。假設(shè)基站發(fā)出一組信號(hào)，經(jīng)過(guò)時(shí)間后標(biāo)簽收到信號(hào)，同樣地標(biāo)簽也發(fā)送一組信號(hào)，經(jīng)過(guò)時(shí)間后基站收到信號(hào)。因?yàn)閮纱涡畔⒔粨Q標(biāo)簽移動(dòng)的距離很短或者沒(méi)有移動(dòng)，所以兩次互傳信息的時(shí)間間隔相同。

在基站或者標(biāo)簽系統(tǒng)中，為電磁波波速，和可以根據(jù)模組自身時(shí)鐘系統(tǒng)計(jì)算時(shí)間差得到，即模組已經(jīng)知道自己連續(xù)發(fā)送兩次的時(shí)間間隔。在TW-TOF定位系統(tǒng)中降低了對(duì)基站的時(shí)鐘同步要求，減小定位誤差。

2.2 ? 三邊測(cè)量法的原理與計(jì)算方法

根據(jù)以上已經(jīng)測(cè)量出來(lái)的基站到基站和標(biāo)簽到基站的數(shù)據(jù)，把基站和標(biāo)簽組成的點(diǎn)連接成三角形，運(yùn)用三角學(xué)原理推算標(biāo)簽的坐標(biāo)。三邊測(cè)量法的原理如圖2所示，以三個(gè)節(jié)點(diǎn)、、為圓心作圓，、、即為基站位置，、、與交點(diǎn)的距離分別為、、，假設(shè)交點(diǎn)的坐標(biāo)為。

坐標(biāo)分別為、、，這三個(gè)圓周相交于一點(diǎn)，交點(diǎn)即為移動(dòng)標(biāo)簽：

（4）

由式（4）可以得到標(biāo)簽的坐標(biāo)為：

（5）

根據(jù)上一節(jié)的論述可以計(jì)算出標(biāo)簽坐標(biāo)的具體范圍，但是由于系統(tǒng)硬件誤差和測(cè)量的精度問(wèn)題，測(cè)出的位置坐標(biāo)范圍其實(shí)是一個(gè)服從正態(tài)分布的坐標(biāo)區(qū)域。從圖3可以看出，三邊定位算法組成的定位系統(tǒng)存在系統(tǒng)誤差，誤差范圍較大，定位坐標(biāo)不斷漂移，致使定位精度下降。其中該正態(tài)分布數(shù)學(xué)期望為，方差為，可以看出單純依靠三邊定位算法計(jì)算出來(lái)的數(shù)據(jù)，靜態(tài)定位坐標(biāo)落于±12 cm范圍內(nèi)，定位坐標(biāo)漂移較大，數(shù)據(jù)不能在高精度場(chǎng)所應(yīng)用。

3 ? 建立定位模型（Establishing positioning model）

3.1 ? 卡爾曼濾波算法

卡爾曼濾波算法[7-8]是目前應(yīng)用最為廣泛的濾波算法，在導(dǎo)航、制導(dǎo)、通信等領(lǐng)域得到了較好的應(yīng)用?？柭鼮V波的核心思想就是：根據(jù)當(dāng)前儀器的測(cè)量值以及上一時(shí)刻的預(yù)測(cè)值和誤差，計(jì)算得到當(dāng)前最優(yōu)量，再預(yù)測(cè)下一時(shí)刻的數(shù)值?？柭鼮V波算法具體步驟如下：

（1）先驗(yàn)估計(jì)

由現(xiàn)代控制理論知識(shí)，假設(shè)某一狀態(tài)方程如下：

（6）

其中，為先驗(yàn)估計(jì)，是根據(jù)狀態(tài)方程計(jì)算出的數(shù)值，為狀態(tài)矩陣;為控制矩陣。

（2）先驗(yàn)誤差協(xié)方差矩陣

（7）

其中，

（8）

值得注意的是，在先驗(yàn)估計(jì)過(guò)程中和測(cè)量過(guò)程中都存在誤差和噪聲，而式（7）就是將先驗(yàn)估計(jì)中的過(guò)程誤差考慮進(jìn)來(lái)。

（3）卡爾曼增益

（9）

式（9）表明，將測(cè)量誤差和先驗(yàn)誤差進(jìn)行融合，轉(zhuǎn)換為卡爾曼增益。當(dāng)測(cè)量誤差的方差較大且較小時(shí)，卡爾曼增益越小;反之，當(dāng)測(cè)量誤差的方差較小且較大時(shí)，卡爾曼增益越大。

（4）后驗(yàn)估計(jì)

（10）

其中，為觀測(cè)值，是測(cè)量出來(lái)的數(shù)值，其中，為觀測(cè)矩陣;為測(cè)量噪聲。后驗(yàn)估計(jì)值由卡爾曼增益權(quán)衡，越小，后驗(yàn)估計(jì)值越接近先驗(yàn)估計(jì);反之越大，后驗(yàn)估計(jì)值越接近測(cè)量值。

（5）更新誤差協(xié)方差矩陣

（11）

3.2 ? 建立基于卡爾曼濾波的定位模型

根據(jù)式（6）—式（11）卡爾曼濾波的基本公式，建立以Python為基礎(chǔ)的濾波應(yīng)用模型，實(shí)現(xiàn)函數(shù)代碼如下：

def KalmanFilter（Z_measure）：

global x_last，start_flag， Kk， p_last #定義過(guò)程參數(shù)

if start_flag == 1： ? ? ?#初始化先驗(yàn)估計(jì)值

x_last = Z_measure

start_flag=2

x_mid=x_last ? ? ? ? ? ? #先驗(yàn)估計(jì)

p_mid=np.add（p_last，Q） ? ?#計(jì)算先驗(yàn)誤差協(xié)方差矩陣

Kk1=np.add（p_mid，R）

Kk=np.divide（p_mid，Kk1） ? #計(jì)算卡爾曼增益

Sm1=np.subtract（Z_measure，x_mid）

Sm1=np.multiply（Kk，Sm1）

Sm=np.add（x_mid，Sm1） ? ? ? #計(jì)算后驗(yàn)估計(jì)

Kk2=np.subtract（[1，1]，Kk）

p_now=np.multiply（Kk2，p_mid） #更新誤差協(xié)方差矩陣

p_last=p_now

x_last=Sm

return Sm

需要指出的是，建立卡爾曼濾波模型之前要對(duì)測(cè)量誤差的方差矩陣和過(guò)程誤差方差矩陣賦初值。該模型在初始階段對(duì)先驗(yàn)估計(jì)進(jìn)行賦值，減少了模型從零更新到可靠數(shù)值的時(shí)間，使得模型更加具有實(shí)用性。測(cè)量誤差的方差矩陣由系統(tǒng)決定，對(duì)于不同的設(shè)備和測(cè)量環(huán)境測(cè)量誤差有所不同，后期精確測(cè)試實(shí)驗(yàn)要求現(xiàn)場(chǎng)采集，以獲得更加精確的定位數(shù)據(jù)。過(guò)程誤差方差矩陣可以現(xiàn)場(chǎng)調(diào)試。

4 ? 實(shí)驗(yàn)測(cè)試與結(jié)果（Test and results）

4.1 ? 超寬帶定位模組與環(huán)境搭建

該模塊采用STM32F103T8U6單片機(jī)為主控芯片，外圍電路包括DW1000芯片、天線模組電源轉(zhuǎn)換模塊、LED燈指示模塊、復(fù)位電路等。模塊通過(guò)串口與電腦通信，通信距離可達(dá)300 m。基站和標(biāo)簽實(shí)物如圖4所示。

本次測(cè)試在室內(nèi)進(jìn)行，測(cè)試基站布置如圖5所示，完全模擬現(xiàn)實(shí)中室內(nèi)環(huán)境。測(cè)試使用的UWB定位模組采用三基站的組網(wǎng)方式。其中，基站標(biāo)記為A0、A1、A2，移動(dòng)標(biāo)簽標(biāo)記為T0，實(shí)驗(yàn)時(shí)將標(biāo)簽安裝在無(wú)人智能小車上，為了實(shí)驗(yàn)方便簡(jiǎn)潔，只采用一個(gè)標(biāo)簽驗(yàn)證實(shí)驗(yàn)結(jié)果。采集的定位數(shù)據(jù)由基站A0通過(guò)串口通信方式發(fā)送到計(jì)算機(jī)上。計(jì)算機(jī)對(duì)接收到的數(shù)據(jù)進(jìn)行解析和解算，顯示相應(yīng)的坐標(biāo)位置。

4.2 ? 測(cè)試結(jié)果與分析

測(cè)試分為靜態(tài)數(shù)據(jù)測(cè)試和動(dòng)態(tài)數(shù)據(jù)測(cè)試。

4.2.1 ? 靜態(tài)數(shù)據(jù)測(cè)試

在靜態(tài)測(cè)試環(huán)境下，智能小車保持不動(dòng)，持續(xù)采集定位數(shù)據(jù)，采樣間隔為100 ms。對(duì)采集的定位數(shù)據(jù)進(jìn)行標(biāo)記，圖形化顯示如圖6所示，并且對(duì)定位數(shù)據(jù)進(jìn)行計(jì)算，得出未經(jīng)過(guò)卡爾曼濾波的定位數(shù)據(jù)期望及均方差和經(jīng)過(guò)卡爾曼濾波的定位數(shù)據(jù)期望及均方差，如表1所示。分別對(duì)橫坐標(biāo)和縱坐標(biāo)方差進(jìn)行分析，卡爾曼濾波處理后的定位數(shù)據(jù)方差明顯減小，其中X軸均方差降低47%，Y軸均方差降低38%，數(shù)據(jù)離散程度降低，明顯優(yōu)于未經(jīng)濾波的原始定位數(shù)據(jù)。

4.2.2 ? 動(dòng)態(tài)數(shù)據(jù)測(cè)試

動(dòng)態(tài)數(shù)據(jù)采集中，為了數(shù)據(jù)分析方便，現(xiàn)將小車攜帶標(biāo)簽按照設(shè)定軌跡直線行駛，持續(xù)觀測(cè)定位數(shù)據(jù)，采樣間隔為100 ms。圖7藍(lán)色曲線是原始定位數(shù)據(jù)，紅色曲線是濾波后的定位數(shù)據(jù)，很明顯濾波后的行駛軌跡更加規(guī)則，其定位精度高于5 cm，沒(méi)有經(jīng)過(guò)卡爾曼濾波的定位數(shù)據(jù)定位精度高于20 cm。對(duì)采集到的數(shù)據(jù)進(jìn)行統(tǒng)計(jì)得到表2，可以看出濾波前定位數(shù)據(jù)方差為，而濾波后的定位數(shù)據(jù)離散程度的定位方差為，方差減少38%以上。卡爾曼濾波明顯減小了數(shù)據(jù)離散程度，提高了定位的精度。

5 ? 結(jié)論（Conclusion）

基于超寬帶的室內(nèi)定位技術(shù)能很好地定位室內(nèi)標(biāo)簽，定位精度在厘米級(jí)別，對(duì)室內(nèi)定位技術(shù)的應(yīng)用有借鑒意義。本文提出的結(jié)合三邊定位算法的卡爾曼濾波定位模型定位可靠，在去除噪聲和優(yōu)化定位精度上效果明顯。在定位初期將初始值賦給先驗(yàn)估計(jì)，減少了初始濾波時(shí)間，提高了效率。在該模型中，現(xiàn)場(chǎng)測(cè)量的誤差協(xié)方差大小會(huì)影響濾波的響應(yīng)時(shí)間，如果要提高濾波響應(yīng)時(shí)間就要以降低濾波精度為代價(jià)。值得一提的是，室內(nèi)定位技術(shù)的應(yīng)用還應(yīng)考慮更加復(fù)雜的環(huán)境，比如遮擋物帶來(lái)的影響，必要時(shí)要進(jìn)行趨勢(shì)分析和補(bǔ)償，進(jìn)一步提高該模型應(yīng)用的魯棒性。

參考文獻(xiàn)（References）

[1] 吉華芳.超寬帶無(wú)線通信系統(tǒng)中同步技術(shù)的研究[D].南京：東南大學(xué)，2005.

[2] 鄧世燕，郭承軍.智慧城市建設(shè)中的室內(nèi)定位現(xiàn)狀與發(fā)展趨勢(shì)分析[C]//中科北斗匯（北京）科技有限公司.第十一屆中國(guó)衛(wèi)星導(dǎo)航年會(huì)論文集——S02導(dǎo)航與位置服務(wù).成都：第十一屆中國(guó)衛(wèi)星導(dǎo)航年會(huì)，2020：131-137.

[3] 余央.NLOS條件下TOA/TDOA定位優(yōu)化方法[D].成都：電子科技大學(xué)，2018.

[4] PORCINOD H W. Ultra-wideband radio technology： Potential and challenges ahead[J]. IEEE Communications Magazine， 2003， 41（7）：66-74.

[5] 楊俊杰.基于UWB的室內(nèi)定位系統(tǒng)設(shè)計(jì)[D].哈爾濱：哈爾濱工程大學(xué)，2018.

[6] 莫建麟.基于卡爾曼濾波的室內(nèi)定位方法設(shè)計(jì)[J].長(zhǎng)春師范大學(xué)學(xué)報(bào)，2019，38（04）：43-47.

[7] 岳崇倫，曾苑，郭云開(kāi).提高衛(wèi)星導(dǎo)航精度的卡爾曼濾波算法應(yīng)用研究[J].測(cè)繪工程，2021，30（02）：60-64，71.

[8] 蔡明明，王運(yùn)鋒.基于擴(kuò)展卡爾曼濾波的時(shí)差定位算法[J].現(xiàn)代雷達(dá)，2020，42（04）：50-54.

作者簡(jiǎn)介：

劉永昌（1993-），男，碩士生.研究領(lǐng)域：汽車電子.

龔元明（1964-），男，博士，教授.研究領(lǐng)域：汽車電子與控制，總線與儀表，物聯(lián)網(wǎng)和工業(yè)自動(dòng)化.