基于UWB的密集行人三維協同定位算法

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

關鍵詞：超寬帶； 協同定位； 迭代算法； 濾波處理； K-means++； DV-Hop

中圖分類號：P228 文獻標志碼：A

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

doi：10.19734/j.issn.1001-3695.2021.08.0341

基金項目：國家自然科學基金資助項目（61872126，61772159）；河南省科技攻關項目（212102210092）；河南省高校重點研究基金資助項目（20A520015）；博士基金資助項目（60907023）

作者簡介：賀軍義（1982-），男，河南平輿人，副教授，碩導，博士，主要研究方向為慣性定位技術及應用；吳夢翔（1997-），男，河南信陽人，碩士研究生，主要研究方向為慣性定位技術及應用、協同定位算法；宋成（1980-），男（通信作者），河南信陽人，副教授，碩導，博士，主要研究方向為無線通信技術、位置隱私等（songcheng@hpu.edu.cn）；張敏（1982-），女，河南陜縣人，碩士，主要研究方向為計算機數據庫技術及應用；張俊楠（1995-），男，河南南陽人，碩士，主要研究方向為MIMO通信、協作通信和物聯網.

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 引言

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

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

1 UWB測距及協同定位原理

1.1 UWB測距原理

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

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

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

DS-TWR測量值中除NLOS誤差外，還包括受隨機因素影響產生的標準測量誤差，該誤差近似符合高斯分布，此時誤差模型如下：

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

1.2 協同定位原理

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

2 基于UWB的密集行人三維協同定位算法設計

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

2.1.1 K-means++抑制NLOS誤差

從式（3）可知，DS-TWR測量值中包括標準測量誤差和NLOS誤差，因兩種誤差產生原因不同，其相互獨立。基于這種獨立性，使用K-means++算法^[12]抑制NLOS誤差。具體做法如下：

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

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

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

d）因隨機誤差和NLOS誤差間的獨立性，將距離值集合中數據分為兩類：（a）僅包含標準測量誤差的距離值;（b）包含NLOS誤差的距離值。對此，循環執行步驟b）c），直至選出k=2個初始聚類中心。

e）計算集合D中各距離值到各初始聚類中心的距離，將各點劃分到距離其最近的初始聚類中心，形成相應的子集。

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

其中：Ck為第k個類別的數據集合。

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

h）聚類結果中兩聚類中心為C₁、C₂，若C₁lt;C₂，則將C₁對應類別中數據認為僅包含標準測量誤差的距離觀測值，并判斷（C₂－C₁）lt;30 cm，若為真，則認為行人間無NLOS誤差，否則，從集合D中剔除對應C₂類別中的數據。

2.1.2 濾波處理

假設K-means++處理后集合D中數據近似符合正態分布，使用高斯均值濾波進一步處理，以抑制標準測量誤差。D近似服從（μ，σ²）正態分布，密度函數如式（6）所示。

其中：m為K-means++處理后集合D中數據量。

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

2.2 雙層協同定位算法

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

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

a）當行人測距范圍內存在三個以上已知位置的行人或UWB基站時，采用四邊定位算法獲取初始位置。

b）當不滿足第一步條件時，因行人在室內環境中一般步速較慢，一個定位周期間行人運動距離較小，若行人在上一時刻獲取了位置估計，則采用上一時刻位置估計作為當前時刻初始位置。

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

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

當hopn=1時，說明該鄰居節點在節點n有效測距半徑上；當hopnlt;1時，說明該鄰居節點在節點n有效測距范圍內。

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

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

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

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

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

2.3 IMM濾波算法

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

在IMM-KF中行人運動方程可表示為

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

2.3.1 CV模型

當行人進行勻速運動時，可用二階常速模型表示。

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

2.3.2 CA模型

當行人進行常加速運動時，可用三階勻加速模型表示。

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

2.3.3 參數自適應調整的Singer模型

假設變加速度a（t）為一階時間相關過程，此時時間相關函數如式（27）所示，為指數形式。

其中：矩陣中各參數具體定義見文獻[17]。

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

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

b）根據當前時刻卡爾曼濾波器輸出的加速度估計值（k|k），參數λ和σ²a選取機制。若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）選取對應F（k）、Q，使用卡爾曼濾波器對式（30）濾波。

2.3.4 IMM-KF

在IMM-KF中各模型的先驗概率一般設置為[1/3 1/3 1/3]，Markov鏈轉移概率矩陣設置為

P=0.950.0250.0250.0250.950.0250.0250.0250.95（33）

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

3 實驗及分析

3.1 測距算法實驗驗證

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

從實驗結果可知，在測距過程中行人間無其他行人阻礙信號傳播時，雖然K-means++未檢測到由行人導致的NLOS誤差，但通過混合濾波有效抑制了標準測量誤差。將兩UWB設備固定在上述位置，之間無任何障礙物，對采集的數據進行濾波處理后平均誤差僅為0.023 m，而30次實驗中，在行人間無其他行人情況下，平均誤差為0.067 m。因為行人以不同角度站在固定位置時，行人攜帶的設備易不在同一側，行人自身便會導致一定NLOS誤差。當行人間有一個或多個行人阻礙信號傳播時，平均誤差為0.178 m，最大誤差為0.234 m，具有較好測距效果。其中第30組實驗的K-means++效果和濾波效果分別如圖3、4所示。從圖4可知均值—高斯混合濾波具有更好的濾波效果。

3.2 定位算法性能分析

3.2.1 仿真實驗及結果分析

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

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

圖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誤差增大，FOR、CO-EKF、CO-BEKF-IMF的平均定位誤差逐漸增大，而CO-TWO-IMMKF算法隨NLOS誤差增大平均定位誤差總體呈減小趨勢。隨NLOS誤差增大，雙層協同定位算法中選取到NLOS誤差較大的路徑概率越低，定位精度越高。

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

3.2.2 實測實驗與結果分析

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

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

1）實驗1

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

2）實驗2

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

4 結束語

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

參考文獻：

[1]裴凌，劉東輝，錢久超.室內定位技術與應用綜述[J].導航定位與授時，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].交通運輸工程學報，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]徐曉蘇，趙北辰.室內環境下基于圖優化的UWB定位方法[J].中國慣性技術學報，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]孫建強，尚俊娜，劉新華，等.氣壓計輔助的UWB室內定位方法[J]. 傳感器與微系統，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]梁玉珠，沈雪微，邱磊，等.物聯網中基于擴展卡爾曼濾波的移動群體定位[J].北京郵電大學學報，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]楊剛，朱士玲，李強，等.基于UWB的消防員室內協同定位算法[J/OL].計算機工程與應用.[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].儀器儀表學報，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].計算機科學，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]任克強，潘翠敏.融合RSSI跳數量化與誤差修正的DV-Hop改進算法[J].傳感技術學報，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]蔣俊正，李楊劍，趙海兵，等.一種大規模傳感器網絡節點分布式定位算法[J].電子與信息學報，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環境下無線傳感器網絡TOA定位算法[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]何友，修建娟，張晶煒，等.雷達數據處理及應用[M].北京： 電子工業出版社，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]魏喜慶，王社陽，李瑞康.基于自適應IMM算法的高超聲速飛行器軌跡預測[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.）