用于礦山等極端環境中的高精度定位算法

佟海濱 陳騰峰 鄧慶緒 蘇憲利

（東北大學計算機科學與工程學院，遼寧沈陽110169）

定位技術根據應用場景的不同，大體可以為兩種，一種是基于衛星信號的室外定位技術，另一種為室內定位技術［1-3］。基于衛星信號的室外定位技術，例如北斗導航系統，由于衛星信號的穿透性較弱而無法被應用于室內環境。室內定位技術一般需要鋪設已知坐標的基站作為信標節點，待定位目標（如工人、車輛等）佩戴電子標簽后，電子標簽與信標節點之間的無線通信會提供某種特征信息，例如時間、信號強度等，這些特征信息可以反映出標簽與信標的相對坐標，在此基礎上，通過一些定位算法（例如三邊定位）實現對電子標簽的定位［4-6］。

在金屬礦山、煤礦這類環境中，災害伴隨著整個開采過程。在該類環境中，人的安全無疑是最重要的，因而針對礦井作業人員位置的監控與管理，是礦井重大災害防控的重要工作。例如當檢測到災害將要發生時，迅速定位附近的工作人員并警告其離開；當災害發生時，尋找災害波及的員工及其位置。該類功能實現的技術基礎是室內技術，對于確保井下作業的安全性具有重要作用［7-8］。

但是在該類環境中實現精確定位需要面對很多極端情況，例如濃度極高的粉塵或灰塵、超過90%的空氣濕度、大型電力設備的電磁干擾、金屬礦脈對地磁環境的影響以及大型礦車、彎曲巷道或設備對信號的阻擋。當前定位技術在該類環境中難以實現高精度定位。該類環境對于定位精度的干擾方式主要可以分為3 類［9］：首先是多路徑折射，它是指無線信號被發送出后，可能會由于遮擋、反射等因素，導致信號通過多個路徑到達接收端，這對基于信號強度的室內定位算法造成的干擾極大［10］，超寬帶通信已經對此類干擾有了成熟的解決方案；其次，障礙物遮擋導致的非視線情況（NLoS）的通信，信號經過NLoS傳輸，會導致信號衰減、增加時間延遲，甚至直接導致信號中斷，并且在一定程度上還會干擾超寬帶通信對多路徑的識別［11-13］；最后是電磁干擾，它會導致信號強度異常或信號中斷。面對上述挑戰，不少研究者嘗試了各種方式來提高在極端環境中的定位精度［11-12］。超寬帶通信由于能識別多徑信號，并且有很高的時間精度，近年來吸引了領域內學者們的關注，不少研究成果已經得到了一定程度的商業應用。但由于超寬帶通信極易受到障礙物（NLoS 通信）干擾，會導致基于超寬帶的定位系統在極端環境中的定位精度大幅下降。識別和消除NLoS通信帶來的干擾是將基于超寬帶的定位算法應用于極端環境所面臨的核心問題，對此，很多研究者提出了相應的算法或解決方案［13-15］。如文獻［13］通過試驗分析，比較全面地描述了NLoS 傳輸對超寬帶信號的影響，進而提出了識別與消除的算法；文獻［14］使用支持向量機的機器學習模型精確地識別與消除了超寬帶NLoS通信對定位的影響。

由于超寬帶的工作頻率很高，導致其對障礙物的穿透性較弱。在實際應用中，礦山這類極端環境中存在很多大型設備、礦車等障礙物，并且很多巷道并非規范的寬敞直通巷道，存在轉彎和不規范分支路徑情況，在很多情況下，超寬帶信號根本無法穿透巷道壁這類障礙物，導致識別與消除NLoS 通信干擾變得沒有意義。當前定位算法最基本的原理為三邊定位［16-17］，即測量待測目標與3 個已知坐標的信標節點的距離，而后組成方程組來計算目標坐標。也就是說，計算待測目標的坐標需要至少與3個信標節點通信，但NLoS可能會造成只有2個或1個信標節點能夠與目標通信的情況，在這種情況下，現階段尚無成熟的解決方案實現高精度定位。雖然可以通過增加信標節點密度或無線信號的功率來緩解該問題，但無法從根本上解決這一問題。更重要的是，增加信標節點密度就意味著成本的增加，增加無線信號的功率則會極大縮減電子標簽的續航時間，這些因素都會限制室內定位技術在極端環境中的應用。

針對超寬帶定位系統中存在的問題，本研究提出了一種基于目標行為分析的定位算法，該算法能夠在短時間內僅剩余1個信標節點與標簽通信時，也可實現高精度定位。本研究所提定位算法的核心思想是，使用TOF 算法測距，而后根據超寬帶通信情況，將定位情形分為兩種：一種是在正常情況下的定位，此時目標節點能夠與3 個及以上的信標節點通信，可使用最小二乘法實現定位，同時根據定位結果，結合卡爾曼濾波算法分析與計算目標的移動速度與方向；第二種情況為信標節點缺失情況，是指能夠與目標節點通信并完成測距的信標節點的數量少于3 個，此時傳統的三邊定位算法無法完成定位，本研究算法根據目標的移動速度與方向，再結合少量的超寬帶測距信息，計算環境中的路徑，實現高精度定位。通過將該算法部署在一個基于超寬帶通信的定位系統中，并在一個礦山模擬環境中進行測試，討論該算法是否能夠大幅度提高超寬帶定位系統在極端環境中的定位精度。

1 研究框架

本研究算法基于超寬帶的定位系統，一般至少包含兩種硬件設備，即電子標簽與基站。其中，電子標簽由待定位目標佩戴，即為目標節點，定位系統則對電子標簽進行定位；基站為信標節點，布置在需要定位的環境中，其坐標已知。標簽與信標節點通信時，使用3 條消息［18-19］完成無線信號飛行時間的計算，進而獲得標簽與信標節點之間的距離。具體測距過程如圖1 所示，標簽發送初始消息，信標節點接收到消息后發送響應消息，標簽接收到響應消息后，向信標節點發送最終消息。此過程中會產生4 個重要的時間段：①標簽發送初始消息與接收響應消息的時間差Tround1；②標簽接收響應消息與發送最終消息的時間差Treplay2；③信標節點接收初始消息與發送響應消息之間的時差Treply1；④信標節點發送響應消息與接收消息的時間差Tround2。根據以上4個時間段，標簽與信標節點之間的無線信號的平均飛行時間Pprop及通信距離（標簽與信標節點之間的距離）d可進行如下計算：

其中，C為光速，即無線信號的傳播速度。

在定位系統中，一般標簽數量較大，但本研究為了便于分析，假設定位系統中只有一個標簽。標簽以1 Hz 的頻率與信標節點通信，同時，要求在定位環境中，標簽在任意時刻至少能夠與一個信標節點通信并完成測距。在真實的定位環境中一般存在固定的行走路徑，定位目標在絕大多數情況下應行走在固定的路徑上。本研究設計的定位算法要求當標簽只能夠與一個信標節點通信并完成測距時，在其連續的覆蓋范圍內，標簽行走的路徑僅存在一個轉彎。

2 基于超寬帶的最小二乘算法定位

根據上述分析，標簽可以計算與每個信標節點的距離。在理想情況下，可以直接使用三邊定位算法完成標簽坐標計算。如圖2所示，但是在實際情況下，距離的計算存在誤差，而且可能存在超過3 個信標節點接收到標簽消息的情況，會大幅降低位精度，因此，本研究使用最小二乘法［20-21］提高定位精度。

根據計算得到的距離與信標節點的坐標，可以得到下式：

式中，( x,y )為待測標簽的坐標；( xi,yi)為第i 個信標節點的坐標；di為標簽與第i個信標節點之間的距離。

式（2）表示存在n 個方程，將前n-1 個方程分別減去第n 個方程，可以得到由n-1 個方程組成的方程組，用矩陣形式可表示為

上式可以簡化為

式（4）中，當A為非奇異矩陣時，表示標簽至少能夠與3個信標節點同行并完成測距，則標簽的定位結果可表示為

3 基于目標行為分析的定位

根據式（5）中矩陣A，可以確定當前定位狀態，上一節給出了當A為非奇異矩陣時的定位狀態與算法。此外，還有兩種情況，無法通過式（5）求得標簽坐標：①A 為零矩陣時，表示標簽只能與一個信標節點通信并完成測距；②A 為非零奇異矩陣，此時表示能夠與標簽通信并完成測距的信標節點超過一個，但它們的坐標在一條直線上。這兩種狀態下，無法進行直接定位，本研究基于目標行為分析，輔助完成定位。

3.1 目標行為

目標能夠影響定位的行為主要包括兩個方面，一個是速度，另一個為移動方向，本研究使用一個二維向量表示目標在二維地圖中的速度與方向。可以在超寬帶定位效果較好情況獲取該狀態值，即當式（5）中A為非奇異矩陣時，使用最小二乘法計算得到目標坐標。根據坐標差與時間，可以計算該過程的平均速度，本研究將該速度視為當前速度的觀測值Zt：

其中，Xt、Xt-1分別為t、t-1 時刻通過超寬帶計算得到的目標二維坐標向量。

為提高預估速度向量的精度，本研究采用卡爾曼濾波器對其進行實時濾波。對于速度向量的卡爾曼濾波器，首先需要明確其預測與測量過程。預測過程是根據目標上一時刻狀態，結合控制量來預測當前狀態的過程。但在定位系統中，目標的控制量主要是加速和轉向的過程，但大多數定位系統無法獲取該變量。本研究將對目標速度向量進行卡爾曼濾波的系統預測控制量設置為0，主要基于：①目標的移動在大多數情況下為勻速直線運動；②為系統預測過程加入高斯白噪聲模型，使得當目標的速度和方向發生較小范圍變化時，卡爾曼濾波器能夠輸出正確的濾波結果；③卡爾曼濾波器同樣重視測量值，當目標發生較大轉彎或變速時，測量值和預測噪聲的設置能夠使濾波器輸出相對正確的結果。帶有噪聲的系統預測過程可表示為

帶有噪聲的系統觀測方程可表示為

根據以上設定，可以通過以下步驟迭代完成卡爾曼濾波。首先是預測過程：

式中，Pt-1為t-1 時刻的后驗估計協方差矩陣；是根據t-1 時刻的協方差矩陣估計的t 時刻協方差矩陣是根據t-1 時刻狀態，通過系統方程預估的t時刻狀態，屬于濾波過程的中間變量。

而后是更新過程：

式中，I 為單位矩陣；Kt為卡爾曼增益矩陣，屬于濾波過程的中間變量，為卡爾曼濾波器提供初始狀態與協方差矩陣，即V0和P0。通過式（9）、式（10）即可完成卡爾曼濾波過程。

3.2 目標行為輔助定位算法

當外部環境存在干擾導致信標節點與標簽之間能夠實現測距的數量少于3個時，可以通過目標行為進行輔助定位，本研究將這種情況細分為兩類。

3.2.1 當A為非0奇異矩陣

當A 為非0 奇異矩陣時，表示能夠與標簽通信并完成測距的信標節點數量至少有兩個，且他們分布在同一條直線上。由于超寬帶測距精度較高，超寬帶定位依舊能夠使用并提供較高的定位精度。首先通過上述計算的速度向量預測標簽當前位置二維坐標Xt，

式中，Δt為時刻t-1到t的時間差。

而后選擇距離該位置最近的兩個信標節點進行定位，可得如下方程組：

式中，( x1,y1)和( x2,y2)分別為兩個信標節點的坐標。

求解式（12）能夠得到兩個結果，可選擇距離Xt最近的坐標值作為計算結果。需要注意的是：當標簽與相應的信標節點處于一條直線或近似一條直線上時（情形一），該算法可能導致較大誤差；反之（情形二），該算法定位效果較好（圖3），因而需要將信標節點盡量布置在定位環境外側。

3.2.2 當A為0矩陣時

當A 為0 矩陣時表示只有一個信標節點能夠實現與標簽的通信與測距，此時超寬帶定位算法無法提供可參考的定位結果，即使考慮目標的移動速度與方向，所給出的坐標位置范圍也很大。此時的定位算法包括兩個方面，一是路徑探測，二是定位，在系統實現時，表現為兩個獨立運行的線程。

首先，關于路徑探測，該功能是在目標進入僅有一個信標節點能夠與標簽完成測距的區域（以下簡稱“單信標區域”）啟動，走出單信標區域時結束。此時，依靠出入單信標區域時的兩個坐標和目標的速度向量來探測該區域的路徑。由于事先約定在所有單信標區域只存在一個路徑的拐點，因而路徑探測問題可以歸結為求解該拐點的坐標（圖4）。

假設該拐點坐標為( px,py)，可得到如下方程組：

式（13）中，第一個方程表示拐點與出入點之間的距離等于行走的總路徑長度，第二個、第三個方程表示拐點位于標簽進入時的方向上。

由于超寬帶定位與速度估計都存在誤差，導致單次的路徑探測存在較大誤差。因此，本研究在定位過程中會持續收集出入口坐標與拐點坐標，將它們的均值作為最終的路徑，可見，隨著定位系統運行時間越長，路徑探測精準度越高。

在定位方面，由于有了路徑，所以速度向量的方向因素可以忽略。以信標節點為圓心，距離為半徑，與路徑的交點或最近的位置為目標坐標。符合該要求的坐標可能有多個，根據速度矢量與路徑，并基于上一時刻的位置來迭代預測當前時刻的位置，選擇與該位置最近的坐標為最終結果。

上述多種情況的定位結果都可以使用卡爾曼濾波器進一步提高精度，即以計算結果作為觀測值，控制狀態為0的系統迭代過程。

4 試驗分析

本研究將該算法部署在一個基于超寬帶的室內定位系統上對其性能進行驗證。該定位系統采用dw1000 芯片實現超寬帶通信，使用信號放大器可以實現超過230 m 的視距通信距離，標簽以1 Hz的頻率發送定位信息，并使用基于信號飛行時間的算法完成與信標節點的測距。試驗環境為作者所在大學內的一處公園，為模擬礦山等極端環境對通信造成的各種干擾，試驗環境中存在樓宇與大型模型，它們類似礦山環境中的大型工程設備或車輛，會完全阻斷超寬帶信號的傳輸。環境中存在較多的樹木，會對信號造成衰減并干擾定位精度，但信號未被完全阻斷，信標節點與標簽依舊能夠完成測距。

在該環境中部署了3 個信標節點，如圖5 所示。由于上所述干擾源的存在，使得定位環境中存在很多未能被信標節點覆蓋的區域，該類區域無法滿足傳統超寬帶定位算法中最少需要3 個信標節點通信的要求。其中，信號覆蓋情況可以分為3 類：①標簽能夠與3個及以上信標節點完成通信與測距；②標簽能夠與2個信標節點完成通信與測距；③標簽僅能夠與1 個信標節點完成通信與測距。本研究布置的3個信標節點中，由于障礙物的遮擋，每個信標節點都存在信號覆蓋不到的區域，圖中使用不同顏色的陰影表示不同基站無法覆蓋到的區域。即，如果某個區域沒有陰影覆蓋，則表示該區域內的標簽能夠與三個信標節點通信，如果有一種顏色的陰影覆蓋，則表示該區域內的標簽能夠與兩個信標節點通信，如果有兩個顏色的陰影同時覆蓋，則表示該區域的標簽僅能夠與一個信標節點通信。

由于本研究算法需要實時數據作為訓練數據，故以相同軌跡行走了5 次。定位結果如圖6 所示，分析可知：傳統的超寬帶定位算法中，當能夠與標簽完成測距的信標節點數量少于3 個時，無法完成定位；從第2 次行走開始，由于第1 次行走收集了訓練數據，并完成了路徑計算與速度預估，因而能夠計算得到定位結果，而且隨著行走次數越多，定位精度越高，經過統計，當行走次數達到5次時，且定位標簽在單信標節點區域不回頭的情況下，基于目標行為分析的定位結果在單信標節點區域的平均誤差為0.93 m，最大誤差為1.41 m，在2 個信標節點測距的區域，平均誤差為0.64 m，最大誤差為1.2 m。

5 結 語

傳統的基于超寬帶通信的高精度室內定位算法，在解決非視線傳輸帶來的信號衰減、延遲等干擾時，主要通過識別與消除這種干擾的方法來提高精度。但是，超寬帶通信的頻率非常高，導致其信號穿透性較差，因此，在大多數情況下，障礙物造成非視線傳輸時，信標節點與標簽之間根本無法實現通信。這意味著在該類情況下，識別與消除非視線傳輸帶來的干擾變得不可行。當信標節點出現缺失時，即能夠與標簽通信并完成測距的信標節點數量少于3個時，傳統的三邊定位算法無法實現定位。本研究所提出的定位算法，能夠預估目標在信標節點缺失區域的行走路徑、速度、拐點等信息，再結合少量的超寬帶測距信息，來計算該類區域中的目標坐標。該定位算法基于超寬帶通信，使用信號飛行時間計算標簽與信標節點之間的距離。將該算法部署在了一套超寬帶系統上，并在一個模擬礦山環境中進行了試驗。結果證明：本研究所提算法在信標節點缺失的區域中，當路徑中僅存在一個拐點時，能夠實現較高精度的定位，當目標在該區域返回時會導致定位精度下降，此外，該算法也無法處理區域內路徑存在多個拐點或多路徑的問題。因此，后續將進一步開展研究，實現多路徑、多拐點情形下的路徑探測與高精度定位。