基于卡爾曼濾波的超寬帶定位技術(shù)應(yīng)用

劉永昌 龔元明

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

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

中圖分類號：TP391 ? ? 文獻標識碼：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）

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

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

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

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

2.1 ? 幾何定位原理

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

如圖1所示，表示該基站從發(fā)送信號時刻到重新接收到信號時刻的時間間隔，表示標簽從接收到信號的時刻到發(fā)送信號時刻的時間間隔。假設(shè)基站發(fā)出一組信號，經(jīng)過時間后標簽收到信號，同樣地標簽也發(fā)送一組信號，經(jīng)過時間后基站收到信號。因為兩次信息交換標簽移動的距離很短或者沒有移動，所以兩次互傳信息的時間間隔相同。

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

2.2 ? 三邊測量法的原理與計算方法

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

坐標分別為、、，這三個圓周相交于一點，交點即為移動標簽：

（4）

由式（4）可以得到標簽的坐標為：

（5）

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

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

3.1 ? 卡爾曼濾波算法

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

（1）先驗估計

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

（6）

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

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

（7）

其中，

（8）

值得注意的是，在先驗估計過程中和測量過程中都存在誤差和噪聲，而式（7）就是將先驗估計中的過程誤差考慮進來。

（3）卡爾曼增益

（9）

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

（4）后驗估計

（10）

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

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

（11）

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

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

def KalmanFilter（Z_measure）：

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

if start_flag == 1： ? ? ?#初始化先驗估計值

x_last = Z_measure

start_flag=2

x_mid=x_last ? ? ? ? ? ? #先驗估計

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

Kk1=np.add（p_mid，R）

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

Sm1=np.subtract（Z_measure，x_mid）

Sm1=np.multiply（Kk，Sm1）

Sm=np.add（x_mid，Sm1） ? ? ? #計算后驗估計

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

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

p_last=p_now

x_last=Sm

return Sm

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

4 ? 實驗測試與結(jié)果（Test and results）

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

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

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

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

測試分為靜態(tài)數(shù)據(jù)測試和動態(tài)數(shù)據(jù)測試。

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

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

4.2.2 ? 動態(tài)數(shù)據(jù)測試

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

5 ? 結(jié)論（Conclusion）

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

參考文獻（References）

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

[2] 鄧世燕，郭承軍.智慧城市建設(shè)中的室內(nèi)定位現(xiàn)狀與發(fā)展趨勢分析[C]//中科北斗匯（北京）科技有限公司.第十一屆中國衛(wèi)星導(dǎo)航年會論文集——S02導(dǎo)航與位置服務(wù).成都：第十一屆中國衛(wèi)星導(dǎo)航年會，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è)計[D].哈爾濱：哈爾濱工程大學(xué)，2018.

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

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

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

作者簡介：

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

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