摘 要：針對復(fù)雜室內(nèi)環(huán)境中密集行人定位精度低、超寬帶（UWB）基站密度要求高的問題，提出一種基于UWB的密集行人三維協(xié)同定位算法。首先使用聚類算法抑制測距數(shù)據(jù)中較大非視距（NLOS）誤差，并使用高斯均值混合濾波抑制標(biāo)準(zhǔn)測量誤差；然后提出雙層協(xié)同定位算法，建立協(xié)同定位數(shù)學(xué)模型，并結(jié)合迭代初始值獲取策略進(jìn)行初步定位，降低了基站數(shù)量要求，在篩選出NLOS誤差較小的測距數(shù)據(jù)并修正后，進(jìn)行二次定位；最后考慮行人高機(jī)動性，設(shè)計(jì)一種交互多模型卡爾曼濾波算法，緩解了定位結(jié)果跳變問題。實(shí)驗(yàn)結(jié)果表明，所提算法在弱NLOS環(huán)境和強(qiáng)NLOS環(huán)境下定位精度分別達(dá)到0.11 m、0.17 m，相比其他算法，具有較高定位精度，進(jìn)一步降低了對UWB基站密度要求。

關(guān)鍵詞：超寬帶； 協(xié)同定位； 迭代算法； 濾波處理； K-means++； DV-Hop

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

文章編號：1001-3695（2022）03-025-0790-07

doi：10.19734/j.issn.1001-3695.2021.08.0341

基金項(xiàng)目：國家自然科學(xué)基金資助項(xiàng)目（61872126，61772159）；河南省科技攻關(guān)項(xiàng)目（212102210092）；河南省高校重點(diǎn)研究基金資助項(xiàng)目（20A520015）；博士基金資助項(xiàng)目（60907023）

作者簡介：賀軍義（1982-），男，河南平輿人，副教授，碩導(dǎo)，博士，主要研究方向?yàn)閼T性定位技術(shù)及應(yīng)用；吳夢翔（1997-），男，河南信陽人，碩士研究生，主要研究方向?yàn)閼T性定位技術(shù)及應(yīng)用、協(xié)同定位算法；宋成（1980-），男（通信作者），河南信陽人，副教授，碩導(dǎo)，博士，主要研究方向?yàn)闊o線通信技術(shù)、位置隱私等（songcheng@hpu.edu.cn）；張敏（1982-），女，河南陜縣人，碩士，主要研究方向?yàn)橛?jì)算機(jī)數(shù)據(jù)庫技術(shù)及應(yīng)用；張俊楠（1995-），男，河南南陽人，碩士，主要研究方向?yàn)镸IMO通信、協(xié)作通信和物聯(lián)網(wǎng).

Three-dimensional cooperative location algorithm of dense pedestrians based on UWB

He Junyi， Wu Mengxiang， Song Cheng?， Zhang Min， Zhang Junnan

（College of Computer Science amp; Technology， Henan Polytechnic University， Jiaozuo Henan 454002， China）

Abstract：In view of the problem that low positioning accuracy of dense pedestrians and high density requirement of UWB base stations in complex indoor environment， this paper developed a three-dimensional cooperative location algorithm for dense pedestrians based on UWB. Firstly， it used clustering algorithm to suppress the large non-line-of-sight （NLOS） error in the ranging data， and suppressed the standard measurement error with the Gaussian mean mixture filtering. Then， it proposed a two-layer cooperative positioning algorithm， established a co-localization mathematical model and combined iterative initial value acquisition strategies for initial localization， which reduced the requirement for number of base stations. After screening the ranging data with smaller NLOS errors and correcting them， it performed secondary positioning. Finally， considering the high mobility of pedestrians， it designed an interactive multi-model Kalman filter algorithm to alleviate the problem of jumps in positioning results. Experimental results show that the positioning accuracy of the proposed algorithm is 0.11 m in weak NLOS environment and 0.17 m in strong NLOS environment respectively， which has a higher positioning accuracy compared with other algorithms and further reduces the requirement for UWB base station density.

Key words：ultra-wideband （UWB）; collaborative positioning; iterative algorithm; filter processing; K-means++; DV-Hop

0 引言

如何在現(xiàn)代大型建筑復(fù)雜室內(nèi)環(huán)境中快速準(zhǔn)確定位，滿足緊急場景偵查救助、商業(yè)場景引導(dǎo)、物流場景跟蹤等位置服務(wù)需求，室內(nèi)定位技術(shù)日益成為研究熱點(diǎn)并具有重要的研究及應(yīng)用價(jià)值^[1]。UWB定位定向技術(shù)較藍(lán)牙、Wi-Fi、RFID、Zigbee等技術(shù)具有多徑分辨能力強(qiáng)、抗干擾性強(qiáng)、定位精度高等特點(diǎn)^[2]。隨著設(shè)備成本逐漸降低，目前眾多智能設(shè)備逐漸配備UWB模塊，并逐漸走進(jìn)日常生產(chǎn)生活。在大型公共建筑物內(nèi)，使用UWB設(shè)備實(shí)現(xiàn)密集行人高精度三維定位是本文研究的重點(diǎn)。文獻(xiàn)[3]提出一種基于圖優(yōu)化的UWB三維定位方法，相較于改進(jìn)的卡爾曼濾波算法具有更高的定位精度。文獻(xiàn)[4]提出一種在UWB基礎(chǔ)上結(jié)合氣壓計(jì)實(shí)現(xiàn)三維定位的方法，通過較少的UWB基站實(shí)現(xiàn)三維定位，但利用氣壓計(jì)獲取標(biāo)簽在定位坐標(biāo)系中的高度，需為其設(shè)置高度初值。以上方法雖然簡單，但未充分利用標(biāo)簽間測距數(shù)據(jù)，且未考慮UWB信號標(biāo)準(zhǔn)測量誤差和受障礙物影響產(chǎn)生的NLOS誤差，導(dǎo)致在實(shí)際應(yīng)用中定位精度較低。對此，部分學(xué)者提出了將UWB結(jié)合其他定位技術(shù)的組合定位方法。文獻(xiàn)[5]中采用UWB和慣性測量單元（IMU）組合定位，通過最大似然估計(jì)法抑制NLOS誤差，并采用擴(kuò)展卡爾曼濾波（EKF）融合傳感器數(shù)據(jù)。文獻(xiàn)[6]中采用UWB、Wi-Fi、IMU進(jìn)行組合定位，采用粒子濾波融合各傳感器數(shù)據(jù)，估算移動目標(biāo)位置。以上方法有效提高了定位精度，但采用的定位設(shè)備更多、成本更高，不易實(shí)現(xiàn)。對此，在室內(nèi)密集目標(biāo)場景中，為解決上述問題，減少UWB基站數(shù)量，充分利用定位目標(biāo)間的測距數(shù)據(jù)，文獻(xiàn)[7]提出一種基于半定規(guī)劃的協(xié)同定位方法，抑制了弱NLOS誤差，但在強(qiáng)NLOS環(huán)境下性能改善不明顯。文獻(xiàn)[8]提出一種基于EKF的移動群體定位算法，利用群體間距離提升定位性能，但其未考慮NLOS誤差。文獻(xiàn)[9]提出一種基于UWB的消防員室內(nèi)協(xié)同定位算法，通過偏移擴(kuò)展卡爾曼濾波抑制NLOS誤差，并采用改進(jìn)的均值濾波進(jìn)一步優(yōu)化定位結(jié)果。文獻(xiàn)[8，9]的協(xié)同定位算法要求參與協(xié)同定位的目標(biāo)至少與三個(gè)以上已知位置的基站或目標(biāo)進(jìn)行通信測距，而在大型建筑物內(nèi)基站數(shù)量較少的情況下，部分目標(biāo)無法滿足要求。

針對上述問題，本文提出一種基于UWB的密集行人三維協(xié)同定位算法。因室內(nèi)部署大量固定基站存在部署成本高、布線難度大等缺點(diǎn)，在待定位行人間建立通信連接以降低UWB基站部署數(shù)量，最大程度利用整個(gè)協(xié)同網(wǎng)絡(luò)的資源獲取更多定位參考數(shù)據(jù)，同時(shí)抑制測距誤差。首先，采用K-means++和高斯均值混合濾波處理采集的距離測量數(shù)據(jù)，抑制部分NLOS和標(biāo)準(zhǔn)測量誤差；其次，提出一種雙層協(xié)同定位算法，在算法中首先建立了協(xié)同定位數(shù)學(xué)模型，并設(shè)置定位初始值獲取策略，使攜帶UWB設(shè)備的行人附近只要有三個(gè)以上基站或位置未知的行人，即可隨時(shí)加入?yún)f(xié)同定位網(wǎng)絡(luò)，然后在獲取初步位置估計(jì)后篩選出NLOS誤差較小的測距數(shù)據(jù)并對其修正，進(jìn)行二次定位；最后，考慮行人高機(jī)動性，設(shè)計(jì)一種交互多模型卡爾曼濾波（IMM-KF）算法進(jìn)一步處理雙層協(xié)同定位算法定位結(jié)果，抑制定位坐標(biāo)跳變，提高定位精度。

1 UWB測距及協(xié)同定位原理

1.1 UWB測距原理

通過UWB設(shè)備獲取可靠的測距數(shù)據(jù)是為目標(biāo)提供高精度定位的前提^[9]。圖1為三消息雙邊雙向測距法（DS-TWR）的原理圖，其中rmarker為發(fā)送/接收時(shí)間戳，用于記錄UWB設(shè)備每次發(fā)送/接收時(shí)間戳。設(shè)備A為測距發(fā)起者在TA1時(shí)刻首次發(fā)送（TX）一個(gè)poll消息后，設(shè)備B在TB1時(shí)刻接收到此消息，并經(jīng)過Treply1后在TB2時(shí)刻發(fā)送一個(gè)response消息以響應(yīng)設(shè)備A發(fā)送的poll消息，設(shè)備A在TA2時(shí)刻接收到該response消息，并在過Treply2后的TA3時(shí)刻發(fā)送一個(gè)final消息，請求結(jié)束此次測距，設(shè)備B在TB3時(shí)刻接收到final消息后，此次測距結(jié)束，最終獲取四個(gè)時(shí)間差數(shù)據(jù)Tround1、Tround2、Treply1、Treply2。

根據(jù)四個(gè)時(shí)間差可計(jì)算UWB信號的飛行時(shí)間，如式（1）所示。在獲取飛行時(shí)間后可獲取設(shè)備A到B的距離，如式（2）所示，其中c為光速。

DS-TWR測量值中一般包括NLOS誤差，而NLOS誤差分布受信號傳播路徑上障礙物分布影響，具有隨機(jī)性。在高機(jī)動密集行人場景中，行人間存在其他行人阻礙信號傳播，由于人體的表面具有平滑且彎曲的特點(diǎn)，無線信號在人體表面進(jìn)行傳播時(shí)會形成體表爬行波^[10]。即使人體對直射路徑信號產(chǎn)生阻礙，但爬行波會使部分被遮擋的信號到達(dá)接收端，且爬行波會導(dǎo)致接收端接收到信號的路徑條數(shù)和信號到達(dá)時(shí)間都產(chǎn)生變化從而影響DS-TWR測距精度。行人通常將移動設(shè)備放在兩側(cè)褲袋中，因行人作為高機(jī)動目標(biāo)行走過程中身體易產(chǎn)生幅動，導(dǎo)致在短時(shí)間內(nèi)兩目標(biāo)間產(chǎn)生LOS路徑或爬行波爬行路徑較小的信號?？紤]定位算法復(fù)雜度、單位時(shí)間內(nèi)計(jì)算量等因素，通常將定位周期設(shè)置為1 s。目前UWB模塊測距頻率一般在200～1 000 Hz，對此，本文在每次定位時(shí)采集0.2 s測距數(shù)據(jù)，同時(shí)為降低行人在0.2 s內(nèi)移動對測距精度的影響，設(shè)置每次定位測距時(shí)間段為[k－0.1，k+0.1]，其中k為定位時(shí)刻。NLOS誤差是信號在傳播中因障礙物產(chǎn)生超量延遲所致，所以NLOS誤差總是正值，消除NLOS誤差也就是在一定程度上消除測量數(shù)據(jù)中的過大值。

DS-TWR測量值中除NLOS誤差外，還包括受隨機(jī)因素影響產(chǎn)生的標(biāo)準(zhǔn)測量誤差，該誤差近似符合高斯分布，此時(shí)誤差模型如下：

其中：dAB（k）為k時(shí)刻設(shè)備A和B間距離觀測數(shù)據(jù)；RAB（k）為設(shè)備A和B間實(shí)際距離；nAB（k）為零均值高斯變量；NAB（k）為正隨機(jī)變量，一般認(rèn)為其服從均勻或Delta分布模型、時(shí)變模型^[11]。

1.2 協(xié)同定位原理

在定位區(qū)域較大時(shí)，為了保證定位準(zhǔn)確性、覆蓋率，需部署大量UWB基站，導(dǎo)致成本和部署復(fù)雜度急劇上升。與傳統(tǒng)定位方式相比，協(xié)同定位充分利用各目標(biāo)間測距信息，在定位過程中基站位置固定，行人作為移動目標(biāo)，使用目標(biāo)到基站和目標(biāo)到目標(biāo)間測距數(shù)據(jù)來估計(jì)目標(biāo)位置。協(xié)同定位如圖2所示，圖中A、B、C、D、E、F代表UWB基站，虛線圓為基站有效測距范圍，M1、M2、M3為待定位目標(biāo)。UWB三維定位算法中四邊定位法、加權(quán)質(zhì)心法等方法的前提都為：獲取目標(biāo)和至少四個(gè)已知位置基站間測距數(shù)據(jù)。故圖2中三個(gè)目標(biāo)均無法得到高精度位置數(shù)據(jù)，但目前已知目標(biāo)到基站、目標(biāo)到目標(biāo)間測距數(shù)據(jù)，可使各基站和目標(biāo)構(gòu)成一個(gè)大型約束關(guān)系，此時(shí)采用優(yōu)化方法求解即可獲取使整體測距誤差最小的各目標(biāo)位置估計(jì)。

2 基于UWB的密集行人三維協(xié)同定位算法設(shè)計(jì)

2.1 基于K-means++和混合濾波的測距算法

2.1.1 K-means++抑制NLOS誤差

從式（3）可知，DS-TWR測量值中包括標(biāo)準(zhǔn)測量誤差和NLOS誤差，因兩種誤差產(chǎn)生原因不同，其相互獨(dú)立?；谶@種獨(dú)立性，使用K-means++算法^[12]抑制NLOS誤差。具體做法如下：

a）假設(shè)設(shè)備間多次測量的距離值集合為D={d₁，d₂，…，di，…，dn}，從集合中隨機(jī)選取一個(gè)距離觀測值作為第一個(gè)聚類中心C₁。

b）計(jì)算集合中各距離值與當(dāng)前已有聚類中心的最小距離，獲取各值成為下個(gè)聚類中心的概率Pi，如式（4）所示。

c）生成一個(gè)隨機(jī)數(shù)R∈[0，1]，用R依次減去P₁，P₂，…，Pn，取第一個(gè)差值≤0時(shí)的距離值作為下一個(gè)聚類中心。

d）因隨機(jī)誤差和NLOS誤差間的獨(dú)立性，將距離值集合中數(shù)據(jù)分為兩類：（a）僅包含標(biāo)準(zhǔn)測量誤差的距離值;（b）包含NLOS誤差的距離值。對此，循環(huán)執(zhí)行步驟b）c），直至選出k=2個(gè)初始聚類中心。

e）計(jì)算集合D中各距離值到各初始聚類中心的距離，將各點(diǎn)劃分到距離其最近的初始聚類中心，形成相應(yīng)的子集。

f）使用各子集的均值uk更新各聚類中心，并將式（5）計(jì)算的誤差平方和作為聚類中心的變化。

其中：Ck為第k個(gè)類別的數(shù)據(jù)集合。

g）重復(fù)步驟e）f），直至各聚類中心變化收斂。

h）聚類結(jié)果中兩聚類中心為C₁、C₂，若C₁lt;C₂，則將C₁對應(yīng)類別中數(shù)據(jù)認(rèn)為僅包含標(biāo)準(zhǔn)測量誤差的距離觀測值，并判斷（C₂－C₁）lt;30 cm，若為真，則認(rèn)為行人間無NLOS誤差，否則，從集合D中剔除對應(yīng)C₂類別中的數(shù)據(jù)。

2.1.2 濾波處理

假設(shè)K-means++處理后集合D中數(shù)據(jù)近似符合正態(tài)分布，使用高斯均值濾波進(jìn)一步處理，以抑制標(biāo)準(zhǔn)測量誤差。D近似服從（μ，σ²）正態(tài)分布，密度函數(shù)如式（6）所示。

其中：m為K-means++處理后集合D中數(shù)據(jù)量。

使用高斯濾波剔除集合D中落在（μ－σ，μ+σ）外的距離測量值^[13]，并利用均值濾波對集合D中的數(shù)據(jù)作進(jìn)一步處理，最后得到高斯均值混合濾波輸出值dfilter。

2.2 雙層協(xié)同定位算法

為降低計(jì)算量，在獲取行人A位置時(shí)，選取有效測量距離dvalid內(nèi)的行人和UWB基站，與其有效測量范圍內(nèi)行人有效測距范圍內(nèi)的UWB基站構(gòu)建協(xié)同定位模型。為便于表示，在k時(shí)刻，行人A有效測距范圍內(nèi)所有行人包括行人A位置p^Ak，分別為p¹k，p²k，…，p^mk，其中pⁱk=（xⁱk，yⁱk，zⁱk）；行人A和其有效測量范圍內(nèi)各行人有效測距范圍內(nèi)的基站坐標(biāo)為a₁，a₂，…，ao，…，an，其中ao=（xo，yo，zo）；用d^ijk表示行人間帶誤差的觀測距離；d^iok表示各行人與其有效測距范圍內(nèi)UWB基站間帶誤差的觀測距離；（i，j）∈U₁表示各行人間可測距離的集合；（i，o）∈U₂表示各行人與其有效測距范圍內(nèi)UWB基站可測距離的集合。此時(shí)，使用帶誤差的測距信息將定位問題轉(zhuǎn)換為無約束的高度非線性非凸的優(yōu)化問題：

通過四邊定位法獲取各個(gè)行人初始位置后，可使用Levenberg-Marquardt算法^[3]求解式（10），但實(shí)際應(yīng)用中部分行人附近無三個(gè)以上已知位置的行人或UWB基站。對此，各行人按以下優(yōu)先級獲取初始位置：

a）當(dāng)行人測距范圍內(nèi)存在三個(gè)以上已知位置的行人或UWB基站時(shí)，采用四邊定位算法獲取初始位置。

b）當(dāng)不滿足第一步條件時(shí)，因行人在室內(nèi)環(huán)境中一般步速較慢，一個(gè)定位周期間行人運(yùn)動距離較小，若行人在上一時(shí)刻獲取了位置估計(jì)，則采用上一時(shí)刻位置估計(jì)作為當(dāng)前時(shí)刻初始位置。

c）當(dāng)不滿足第一步條件且行人在上一時(shí)刻未獲取位置估計(jì)時(shí)，引入無線傳感器網(wǎng)絡(luò)中非測距定位技術(shù)。其中DV-Hop算法復(fù)雜度低，在節(jié)點(diǎn)數(shù)為200時(shí)，算法執(zhí)行時(shí)間不足0.1 s，且UWB設(shè)備通信時(shí)延低，具有較高的實(shí)時(shí)性。由于已知各行人間距離觀測和行人與UWB基站間距離觀測，為提高初始值精度，進(jìn)而提高求解式（10）效率和獲取全局最優(yōu)解概率，本文采用改進(jìn)的DV-Hop算法。在改進(jìn)算法中將UWB基站和在步驟a）中獲取初始位置的行人作為錨節(jié)點(diǎn)，將尚未獲取初始位置的行人作為未知節(jié)點(diǎn)，具體流程如下：

（a）基于UWB測距值的跳數(shù)量化。為了使跳數(shù)連續(xù)化，更好反映節(jié)點(diǎn)間相對距離，使節(jié)點(diǎn)間距離估計(jì)更加準(zhǔn)確，根據(jù)文獻(xiàn)[14]中基于RSSI的跳數(shù)量化方法，使用UWB測距值對跳數(shù)量化，具體做法如下：UWB有效測量距離為dvalid，假設(shè)節(jié)點(diǎn)n與某鄰居節(jié)點(diǎn)間距離觀測數(shù)據(jù)為dn，則節(jié)點(diǎn)n到該鄰居節(jié)點(diǎn)的量化跳數(shù)為

當(dāng)hopn=1時(shí)，說明該鄰居節(jié)點(diǎn)在節(jié)點(diǎn)n有效測距半徑上；當(dāng)hopnlt;1時(shí)，說明該鄰居節(jié)點(diǎn)在節(jié)點(diǎn)n有效測距范圍內(nèi)。

（b）距離估計(jì)。節(jié)點(diǎn)間互相通信，在記錄所有未知節(jié)點(diǎn)到錨節(jié)點(diǎn)間最小量化跳數(shù)和錨節(jié)點(diǎn)坐標(biāo)后，傳統(tǒng)DV-Hop算法首先計(jì)算錨節(jié)點(diǎn)平均跳距并廣播平均跳距，然后未知節(jié)點(diǎn)將最先獲取的錨節(jié)點(diǎn)平均跳距作為自身跳距，但其因節(jié)點(diǎn)分布不均勻、單個(gè)錨節(jié)點(diǎn)并不能體現(xiàn)整個(gè)網(wǎng)絡(luò)等因素，跳段距離估計(jì)精度較低。為了提高跳段距離估計(jì)精度，在基于UWB測距值的跳數(shù)量化基礎(chǔ)上，首先根據(jù)文獻(xiàn)[14]的式（12）～（16）獲取修正后的錨節(jié)點(diǎn)平均跳距；其次根據(jù)文獻(xiàn)[14]的式（17）（18）獲取修正后的未知節(jié)點(diǎn)平均跳距；最后利用未知節(jié)點(diǎn)平均跳距和其到各錨節(jié)點(diǎn)的最小量化跳數(shù)計(jì)算未知節(jié)點(diǎn)到各錨節(jié)點(diǎn)間跳段距離。

（c）未知節(jié)點(diǎn)位置估計(jì)。在通過距離估計(jì)獲取未知節(jié)點(diǎn)與各錨節(jié)點(diǎn)間跳段距離后，錨節(jié)點(diǎn)中虛擬錨節(jié)點(diǎn)為通過步驟a）獲取初始位置的行人，其位置存在亞米級誤差，其誤差會傳遞到當(dāng)前未知節(jié)點(diǎn)位置估計(jì)。因UWB測距精度較高，若未知節(jié)點(diǎn)有效測距范圍內(nèi)存在錨節(jié)點(diǎn)，則將其與錨節(jié)點(diǎn)間UWB測距距離替代跳段距離。此時(shí)，分別將未知節(jié)點(diǎn)到其有效測距范圍內(nèi)錨節(jié)點(diǎn)（非虛擬錨節(jié)點(diǎn)）的距離權(quán)重設(shè)為1，到虛擬錨節(jié)點(diǎn)的距離權(quán)重設(shè)為0.75，并分別將未知節(jié)點(diǎn)到其有效測距范圍外錨節(jié)點(diǎn)（非虛擬錨節(jié)點(diǎn)）的距離權(quán)重設(shè)為0.5，到虛擬錨節(jié)點(diǎn)的距離權(quán)重設(shè)為0.25。根據(jù)設(shè)置的權(quán)重構(gòu)建對稱正定權(quán)值矩陣后，利用加權(quán)最小二乘法獲取未知節(jié)點(diǎn)初始位置。

通過上述方法獲取的初始位置誤差較大，為降低迭代次數(shù)，使用文獻(xiàn)[15]中基于單位步長的改進(jìn)牛頓法快速收斂到局部最優(yōu)解，獲取行人A和其dvalid內(nèi)行人的初步位置估計(jì)。因2.1節(jié)中方法無法有效抑制由墻、柜子等嚴(yán)重阻礙行人間信號傳播的障礙物導(dǎo)致的NLOS誤差，為了進(jìn)一步抑制NLOS誤差，提高定位精度，進(jìn)行二次定位估計(jì)。首先依據(jù)初步定位結(jié)果，計(jì)算集合U₁中行人間的距離、集合U₂中行人和UWB基站間的距離：

依據(jù)文獻(xiàn)[16]中抑制NLOS的思想，為每個(gè)行人選取四個(gè)和ρ均較小的DS-TWR測量值。這些距離值中含有較小的NLOS誤差，但由于初步定位結(jié)果不準(zhǔn)確，導(dǎo)致存在d^re-estk大于包含NLOS誤差的DS-TWR測量值d^rek，所以對選取的DS-TWR測量值進(jìn)行如下數(shù)值處理：

利用所有修正后的DS-TWR測量值和初步定位中行人A、行人A有效測距范圍內(nèi)行人、各行人有效測距范圍內(nèi)的UWB基站，按上述流程再次得到式（10）后，為降低迭代次數(shù)，使用初步定位結(jié)果作為各行人初始位置，使用Levenberg-Marquardt算法^[3]求解行人A的位置估計(jì)。

2.3 IMM濾波算法

在使用雙層協(xié)同定位算法獲取行人位置估計(jì)后，需對行人進(jìn)行跟蹤，但行人運(yùn)動具有高機(jī)動性，而卡爾曼濾波、粒子濾波等僅使用單一模型來描述行人運(yùn)動，效果不佳。對此，本文使用交互多模型卡爾曼濾波算法（IMM-KF）進(jìn)行追蹤，其中運(yùn)動模型集為勻速（CV）模型、勻加速（CA）模型、參數(shù)自適應(yīng)調(diào)整的Singer模型。同時(shí)在濾波后，可進(jìn)一步緩解定位結(jié)果跳變問題，使定位結(jié)果更加逼近真實(shí)值。

在IMM-KF中行人運(yùn)動方程可表示為

其中：xt為行人在定位坐標(biāo)系中X、Y、Z軸方向的位置、速度、加速度，如式（22）所示；A為模型描述矩陣；w（t）為過程噪聲。

2.3.1 CV模型

當(dāng)行人進(jìn)行勻速運(yùn)動時(shí)，可用二階常速模型表示。

其中：x、分別為行人的位置和速度分量；w（t）為均值為0、方差為σ²的高斯白噪聲。CV模型用于行人無機(jī)動運(yùn)動情況。

2.3.2 CA模型

當(dāng)行人進(jìn)行常加速運(yùn)動時(shí)，可用三階勻加速模型表示。

其中：為行人加速度分量。CA模型在變加速機(jī)動情況下效果較差。

2.3.3 參數(shù)自適應(yīng)調(diào)整的Singer模型

假設(shè)變加速度a（t）為一階時(shí)間相關(guān)過程，此時(shí)時(shí)間相關(guān)函數(shù)如式（27）所示，為指數(shù)形式。

其中：矩陣中各參數(shù)具體定義見文獻(xiàn)[17]。

模型中參數(shù)λ和σ²a一般依賴于先驗(yàn)知識設(shè)置，且一旦確定，在濾波過程中不再變化。而λ和σ²a若與實(shí)際情況差距較大，濾波精度較低，但若在IMM模型集中添加多個(gè)不同λ、σ²a的Singer模型會導(dǎo)致計(jì)算量較大，因此，為在保證濾波精度的同時(shí)降低計(jì)算量，采用模型參數(shù)自適應(yīng)調(diào)整策略，具體做法如下：

a）依據(jù)機(jī)動等級，設(shè)定三組參數(shù)λ和σ²a，分別為（1/20，（1×1.4 m/s²）²）、（1/40，（1×0.6 m/s²）²）、（1/60，（1×0.2 m/s²）²），由式（30）（31）分別計(jì)算出相應(yīng)狀態(tài)矩陣F（k）、過程噪聲的協(xié)方差Q。

b）根據(jù)當(dāng)前時(shí)刻卡爾曼濾波器輸出的加速度估計(jì)值（k|k），參數(shù)λ和σ²a選取機(jī)制。若ai－1，maxlt;（k|k）≤ai，max，選擇λ=λi，σ²a=σ²ai；若（k|k）≤a₁，max，選擇λ=λ₁，σ²a=σ²a1；若（k|k）gt;aN，max，選擇λ=λN，σ²a=σ²aN。

c）選取對應(yīng)F（k）、Q，使用卡爾曼濾波器對式（30）濾波。

2.3.4 IMM-KF

在IMM-KF中各模型的先驗(yàn)概率一般設(shè)置為[1/3 1/3 1/3]，Markov鏈轉(zhuǎn)移概率矩陣設(shè)置為

P=0.950.0250.0250.0250.950.0250.0250.0250.95（33）

設(shè)置各模型在采樣初始時(shí)刻的概率分別為μ₁=0.8、μ₂=0.1、μ₃=0.1。IMM-KF中混合概率計(jì)算、輸入融合、模型條件濾波、模型概率更新、濾波估計(jì)融合同文獻(xiàn)[18]，本文不再贅述。

3 實(shí)驗(yàn)及分析

3.1 測距算法實(shí)驗(yàn)驗(yàn)證

因河南理工大學(xué)三號教學(xué)樓大廳門口處人流量大，在大廳門口處設(shè)置兩處固定位置，兩位置間距20 m，中間無固定障礙物。在實(shí)驗(yàn)中采用型號為D-DWM-PG2.5的UWB模塊，該模塊在6.8 m速率下通信距離為50 m，自帶改進(jìn)的測距算法有DS-TWR+和HDS-TWR，DS-TWR+算法在一對一模式下測距耗時(shí)約3 ms，HDS-TWR算法在一個(gè)標(biāo)簽與3～8個(gè)標(biāo)簽同時(shí)測距情況下耗時(shí)約7～12 ms。兩行人將UWB設(shè)備攜帶在左側(cè)褲袋中，采用DS-TWR+算法測距，將測距頻率設(shè)置為140 Hz，在其他行人密集出入教學(xué)樓的時(shí)間段，以不同角度站在固定位置并啟動測距。一共進(jìn)行30次實(shí)驗(yàn)，實(shí)驗(yàn)結(jié)果如表1所示。

從實(shí)驗(yàn)結(jié)果可知，在測距過程中行人間無其他行人阻礙信號傳播時(shí)，雖然K-means++未檢測到由行人導(dǎo)致的NLOS誤差，但通過混合濾波有效抑制了標(biāo)準(zhǔn)測量誤差。將兩UWB設(shè)備固定在上述位置，之間無任何障礙物，對采集的數(shù)據(jù)進(jìn)行濾波處理后平均誤差僅為0.023 m，而30次實(shí)驗(yàn)中，在行人間無其他行人情況下，平均誤差為0.067 m。因?yàn)樾腥艘圆煌嵌日驹诠潭ㄎ恢脮r(shí)，行人攜帶的設(shè)備易不在同一側(cè)，行人自身便會導(dǎo)致一定NLOS誤差。當(dāng)行人間有一個(gè)或多個(gè)行人阻礙信號傳播時(shí)，平均誤差為0.178 m，最大誤差為0.234 m，具有較好測距效果。其中第30組實(shí)驗(yàn)的K-means++效果和濾波效果分別如圖3、4所示。從圖4可知均值—高斯混合濾波具有更好的濾波效果。

3.2 定位算法性能分析

3.2.1 仿真實(shí)驗(yàn)及結(jié)果分析

在仿真實(shí)驗(yàn)中，定位區(qū)域?yàn)?2 m×28 m×2.5 m的長方體區(qū)域，行人行走區(qū)域是定位區(qū)域中大小為20 m×26 m×2.5 m的長方體區(qū)域，四個(gè)UWB基站U1、U2、U3、U4，其坐標(biāo)分別為（0，0，2）、（22，28，2）、（22，0，2）、（0，28，2）?；竞蜆?biāo)簽的有效測距范圍均為35 m。5個(gè)標(biāo)簽T0～T4中，4個(gè)標(biāo)簽作為輔助標(biāo)簽T0定位的行人，為便于仿真，將T1～T4的起始坐標(biāo)分別設(shè)置為（1，1，1.1）、（20，26，1）、（20，0，1.2）、（0，26，0.9），T1～T4均按照圖5中黑色軌跡進(jìn)行逆時(shí)針運(yùn)動，T0在行人行走區(qū)域進(jìn)行機(jī)動運(yùn)動。假設(shè)標(biāo)簽到標(biāo)簽、基站到標(biāo)簽間均連通，與標(biāo)簽T0的通信路徑有8條，全局可連通路徑有30條，其中10條為NLOS路徑。仿真實(shí)驗(yàn)中使用的UWB距離測量值是在真實(shí)距離上添加噪聲得到的，其中LOS下的標(biāo)準(zhǔn)測量誤差是在真實(shí)值上添加了均值為0、標(biāo)準(zhǔn)差為0.2的高斯白噪聲，NLOS下距離測量值除在真實(shí)值上添加高斯白噪聲外，還添加了0.5～1.5 m的隨機(jī)噪聲，采樣周期T為1 s，總采樣點(diǎn)數(shù)為106。

在仿真實(shí)驗(yàn)中，為驗(yàn)證本文算法（CO-TWO-IMMKF）性能，將本文算法與四邊定位算法（FOR）、文獻(xiàn)[8]中基于擴(kuò)展卡爾曼濾波的協(xié)同定位算法（CO-EKF）和文獻(xiàn)[9]中CO-BEKF-IMF算法進(jìn)行對比。圖6（a）～（d）為T0標(biāo)簽在四種算法下的定位軌跡，黑線表示真實(shí)軌跡，藍(lán)線表示各算法定位軌跡（見電子版）。由圖6可知，F(xiàn)OR算法定位效果最差，因其易受誤差影響；CO-EKF將標(biāo)簽T0～T4作為一個(gè)整體，充分利用了標(biāo)簽和標(biāo)簽間的測距信息，使定位參考數(shù)據(jù)增多，有效提高了定位精度，但因未有效抑制NLOS誤差，定位效果較差；CO-BEKF-IMF使用偏移擴(kuò)展卡爾曼濾波抑制NLOS誤差，并用改進(jìn)的均值濾波進(jìn)一步抑制定位結(jié)果波動，但其未抑制標(biāo)準(zhǔn)測量誤差，定位效果一般；CO-TWO-IMMKF算法首先通過K-means++和混合濾波有效抑制了標(biāo)準(zhǔn)測量誤差和部分NLOS誤差，然后使用雙層協(xié)同定位算法充分利用了基站和標(biāo)簽、標(biāo)簽和標(biāo)簽間距離數(shù)據(jù)，有效抑制NLOS誤差，最后通過IMM-KF算法進(jìn)一步抑制高機(jī)動目標(biāo)定位結(jié)果波動，雖然篩選出NLOS誤差較小、可能性較大的路徑時(shí)可能存在失誤，但總體定位效果最好。

圖6中各算法平均定位誤差分別為0.76 m、0.52 m、0.33 m、0.18 m，CO-TWO-IMMKF算法相較FOR、CO-EKF、CO-BEKF-IMF分別降低了76.3%、65.4%、45.5%。圖7為各算法在不同NLOS誤差下的平均定位誤差變化情況。從圖中可看出，隨NLOS誤差增大，F(xiàn)OR、CO-EKF、CO-BEKF-IMF的平均定位誤差逐漸增大，而CO-TWO-IMMKF算法隨NLOS誤差增大平均定位誤差總體呈減小趨勢。隨NLOS誤差增大，雙層協(xié)同定位算法中選取到NLOS誤差較大的路徑概率越低，定位精度越高。

為進(jìn)一步驗(yàn)證NLOS誤差對CO-TWO-IMMKF算法定位性能的影響，圖8展示了各算法平均定位誤差隨NLOS路徑增加的變化情況。從圖中可知，F(xiàn)OR算法平均定位誤差最大，誤差呈逐漸上升趨勢，但NLOS路徑數(shù)量達(dá)到20條后，增加標(biāo)簽和標(biāo)簽間的NLOS路徑對其無影響；CO-EKF因利用了基站與標(biāo)簽、標(biāo)簽與標(biāo)簽間測距數(shù)據(jù)，隨NLOS路徑數(shù)量增加誤差逐漸上升；CO-BEKF-IMF算法因抑制了NLOS誤差，誤差增長幅度較?。籆O-TWO-IMMKF算法相比于其他三種算法誤差較低，因其通過K-means++和混合濾波有效抑制了標(biāo)準(zhǔn)測量誤差和部分NLOS誤差，并在使用雙層協(xié)同定位算法獲取位置估計(jì)后，利用IMM-KF進(jìn)一步抑制定位結(jié)果波動性。

3.2.2 實(shí)測實(shí)驗(yàn)與結(jié)果分析

為驗(yàn)證CO-TWO-IMMKF算法在實(shí)際環(huán)境中的可行性，使用13個(gè)型號為D-DWM-PG2.5的UWB模塊，測距算法采用其自有的HDS-TWR算法，并利用其配套的上位機(jī)、下位機(jī)代碼實(shí)現(xiàn)基站與行人、行人和行人間的通信測距功能。在實(shí)驗(yàn)中，測距頻率設(shè)置為100 Hz，實(shí)驗(yàn)場景為一間11 m×19 m×4 m的非階梯大教室和3.8 m×19 m×4 m走廊中，各模塊有效測距范圍設(shè)置為12 m，具體實(shí)驗(yàn)場景如圖9所示。

圖9中，紅色矩形為UWB基站，藍(lán)色圓點(diǎn)為攜帶UWB設(shè)備的行人，黃色矩形為對應(yīng)行人活動范圍（見電子版），虛線為對應(yīng)基站測距范圍。因?qū)崿F(xiàn)行人T1三維定位一般需要四個(gè)及以上UWB設(shè)備輔助定位，圖9中只有T1標(biāo)簽所在區(qū)域滿足該條件，對此，在T1活動區(qū)域外FOR算法無法定位，而CO-EKF、CO-BEKF-IMF算法對行人定位至少需要四個(gè)已知位置的UWB基站或攜帶UWB設(shè)備的行人，但其在T1活動區(qū)域外可通過行人T1～T8輔助定位。在實(shí)際實(shí)驗(yàn)中采用CO-EKF、CO-BEKF-IMF、CO-TWO-IMMKF算法對行人T0定位。實(shí)驗(yàn)一共分為兩組，一組定位區(qū)域在教室內(nèi)，將基站U3、U4放置在圖9中綠色矩形處，并讓行人T2～T4在教室中行走;另一組定位區(qū)域包括教室和走廊，基站和輔助行人的安排，如圖9所示（見電子版）。在兩組實(shí)驗(yàn)中，還安排10個(gè)行人在定位區(qū)域內(nèi)隨機(jī)運(yùn)動，此時(shí)，行人T0在定位區(qū)域行走時(shí)，信號除受講臺、墻壁等影響，還會受到人體阻礙。第1組實(shí)驗(yàn)屬于弱NLOS環(huán)境，第2組為強(qiáng)NLOS環(huán)境。

1）實(shí)驗(yàn)1

圖10中黑線為固定軌跡，藍(lán)線為各算法解算出的定位軌跡（見電子版）。行人T0在行走時(shí)，因UWB信號受行人、講臺等阻擋，產(chǎn)生弱NLOS誤差。與仿真結(jié)果一致，CO-TWO-IMMKF算法定位精度較高，平均定位誤差達(dá)到0.11 m，相較CO-EKF算法的平均定位誤差0.33 m、CO-BEKF-IMF的平均定位誤差0.18 m，分別降低了66.6%、38.9%。

2）實(shí)驗(yàn)2

行人T0在行走時(shí)，因UWB信號受墻壁、行人等阻擋，產(chǎn)生強(qiáng)NLOS誤差。如圖11所示，與仿真結(jié)果一致，CO-TWO-IMMKF算法定位精度較高，平均定位誤差達(dá)到0.17 m，相較CO-EKF算法的平均定位誤差0.54 m、CO-BEKF-IMF的平均定位誤差0.31 m，分別降低了68.5%、45.2%。

4 結(jié)束語

為了準(zhǔn)確獲取密集行人在復(fù)雜室內(nèi)環(huán)境中的位置，提出一種基于UWB的密集行人協(xié)同定位算法。通過使用K-means++對采集的測距數(shù)據(jù)進(jìn)行分類抑制部分NLOS誤差，并采用高斯中值混合濾波進(jìn)一步處理數(shù)據(jù)，抑制了標(biāo)準(zhǔn)測量誤差；通過雙層協(xié)同定位算法，充分利用了行人間距離數(shù)據(jù)，使用迭代初始值設(shè)置策略可使長時(shí)間未獲取位置數(shù)據(jù)或新加入?yún)f(xié)同定位的行人在附近有四個(gè)及以上基站或行人（無須已獲取位置）的情況下獲取位置估計(jì)，同時(shí)相較其他算法進(jìn)一步降低了對UWB部署密度的要求；通過以CV模型、CA模型、自適應(yīng)參數(shù)的Singer模型為模型集的IMM-KF算法，抑制雙層協(xié)同定位算法定位結(jié)果的跳變，進(jìn)一步提高定位精度。仿真和實(shí)際實(shí)驗(yàn)一致驗(yàn)證了本文算法的可行性，其定位精度明顯優(yōu)于其他算法。雖然本文算法具有較高定位精度，但相較其他算法復(fù)雜性較高，適用于對行人定位實(shí)時(shí)性要求較低的場景。

參考文獻(xiàn)：

[1]裴凌，劉東輝，錢久超.室內(nèi)定位技術(shù)與應(yīng)用綜述[J].導(dǎo)航定位與授時(shí)，2017，4

（3）：1-10.（Pei Ling， Liu Donghui， Qian Jiuchao. A survey of indoor positioning technology and application[J].Navigation Positioning amp; Timing，2017，4（3）：1-10.）

[2]朱冰，陶曉文，趙健，等.智能汽車兩階段UWB定位算法[J].交通運(yùn)輸工程學(xué)報(bào)，2021，21（2）：256-266.（Zhu Bing， Tao Xiaowen， Zhao Jian， et al. Two-stage UWB positioning algorithm of intelligent vehicle[J].Journal of Traffic and Transportation Engineering，2021，21（2）：256-266.）

[3]徐曉蘇，趙北辰.室內(nèi)環(huán)境下基于圖優(yōu)化的UWB定位方法[J].中國慣性技術(shù)學(xué)報(bào)，2019，27（3）：334-339.（Xu Xiaosu， Zhao Beichen. Ultra wide-band positioning method based on general graph optimization in indoor environment[J].Journal of Chinese Inertial Technology，2019，27（3）：334-339.）

[4]孫建強(qiáng)，尚俊娜，劉新華，等.氣壓計(jì)輔助的UWB室內(nèi)定位方法[J]. 傳感器與微系統(tǒng)，2021，40（6）：30-33.（Sun Jianqiang， Shang Junna， Liu Xinhua， et al. Barometer-assisted UWB indoor positioning method[J].Transducer and Microsystem Technologies，2021，40（6）：30-33.）

[5]Li Beibei， Hao Zhanjun， Dang Xiaochao. An indoor location algorithm based on Kalman filter fusion of ultra-wide band and inertial measurement unit[J].AIP Advances，2019，9（8）：085210.

[6]Liu Ran， Yuen C， Do T， et al. Cooperative positioning for emergency responders using self IMU and peer-to-peer radios measurements[J].Information Fusion，2020，56：93-102.

[7]Vaghefi R M， Buehrer R M. Cooperative localization in NLOS environ-ments using semidefinite programming[J].IEEE Communications Letters，2015，19（8）：1382-1385.

[8]梁玉珠，沈雪微，邱磊，等.物聯(lián)網(wǎng)中基于擴(kuò)展卡爾曼濾波的移動群體定位[J].北京郵電大學(xué)學(xué)報(bào)，2019，42（2）：95-100.（Liang Yuzhu， Shen Xuewei， Qiu Lei， et al. Extended Kalman filter for mobile groups users localization in Internet of Things[J].Journal of Beijing University of Posts and Telecommunications，2019，42（2）：95-100.）

[9]楊剛，朱士玲，李強(qiáng)，等.基于UWB的消防員室內(nèi)協(xié)同定位算法[J/OL].計(jì)算機(jī)工程與應(yīng)用.[2021-09-29].http：//kns.cnki.net/kcms/detail/11.2127.TP.20210127.1621.020.html.（Yang Gang， Zhu Shiling， Li Qiang， et al. UWB based indoor collaborative positio-ning algorithm for firefighters[J].Computer Engineering and Applications.[2021-09-29].http：//kns.cnki.net/kcms/detail/11.2127.TP.20210127.1621.020.html.）

[10]Smith D B， Miniutti D， Lamahewa T A， et al. Propagation models for body-area networks： a survey and new outlook[J].IEEE Antennas and Propagation Magazine，2013，55（5）：97-117.

[11]Wylie M P， Wang S S. Robust range estimation in the presence of the non-line-of-sight error[C]//Proc of the 54th IEEE Vehicular Technology Conference.Piscataway，NJ：IEEE Press，2003.

[12]鄧聰穎，葉波，苗建國，等.基于K-means+聚類與概率神經(jīng)網(wǎng)絡(luò)的數(shù)控機(jī)床變位姿動態(tài)特性模糊評估[J].儀器儀表學(xué)報(bào)，2020，41（12）：227-235.（Deng Congying， Ye Bo， Miao Jianguo， et al. Fuzzy evaluation of machine tool dynamic characteristics for changing machining position based on K-means+clustering and probabilistic neural network[J].Chinese Journal of Scientific Instrument，2020，41（12）：227-235.）

[13]倪曉軍，高雁，李凌鋒.基于RSSI的混合濾波算法[J].計(jì)算機(jī)科學(xué)，2019，46（8）：133-137.（Ni Xiaojun， Gao Yan， Li Lingfeng.Hybrid filtering algorithm based on RSSI[J].Computer Science，2019，46（8）：133-137.）

[14]任克強(qiáng)，潘翠敏.融合RSSI跳數(shù)量化與誤差修正的DV-Hop改進(jìn)算法[J].傳感技術(shù)學(xué)報(bào)，2020，33（5）：718-724.（Ren Keqiang， Pan Cuimin. Improved DV-Hop algorithm based on RSSI hop quantization and error correction[J].Chinese Journal of Sensors and Actuators，2020，33（5）：718-724.）

[15]蔣俊正，李楊劍，趙海兵，等.一種大規(guī)模傳感器網(wǎng)絡(luò)節(jié)點(diǎn)分布式定位算法[J].電子與信息學(xué)報(bào)，2019，41（12）：3022-3028.（Jiang Junzheng， Li Yangjian， Zhao Haibing， et al. A distributed node localization algorithm for large scale sensor networks[J].Journal of Electronics amp; Information Technology，2019，41（12）：3022-3028.）

[16]徐彤陽.NLOS環(huán)境下無線傳感器網(wǎng)絡(luò)TOA定位算法[J].計(jì)算機(jī)工程，2013，39（12）：93-96.（Xu Tongyang. TOA location algorithm in wireless sensor network under NLOS environment[J].Computer Engineering，2013，39（12）：93-96.）

[17]何友，修建娟，張晶煒，等.雷達(dá)數(shù)據(jù)處理及應(yīng)用[M].北京： 電子工業(yè)出版社，2009：184-187.（He You， Xiu Jianjuan， Zhang Jingwei， et al. Radar data processing and application[M].Beijing：Publishing House of Electronics Industry，2009：184-187.）

[18]魏喜慶，王社陽，李瑞康.基于自適應(yīng)IMM算法的高超聲速飛行器軌跡預(yù)測[J].上海航天，2016，33（2）：27-31.（Wei Xiqing， Wang Sheyang， Li Ruikang. Trajectory predicting of hypersonic vehicle using adaptive interactive multiple model filter[J].Aerospace Shanghai，2016，33（2）：27-31.）