基于TDOA定位模型的Chan井下定位算法的改進(jìn)

靳偉超 馬翠紅 楊友良

摘 要：針對(duì)因環(huán)境復(fù)雜多變，非視距及多徑效應(yīng)影響而導(dǎo)致井下定位精度低的問(wèn)題，采用帶寬寬，時(shí)間分辨率高，穿透能力強(qiáng)的UWB技術(shù)，基于到達(dá)時(shí)間差（TDOA）的定位方案，提出了一種Chan-UKF定位算法。即將Chan算法取得的位置估計(jì)值作為UKF算法的初始值，對(duì)位置初估計(jì)進(jìn)行二次估計(jì)。通過(guò)Matlab仿真平臺(tái)，在TDOA的測(cè)距誤差標(biāo)準(zhǔn)差σ≤10 cm的情況下，Chan-UKF算法能將二維平面定位的均方根誤差減小約50%。仿真結(jié)果顯示，Chan-UKF定位算法穩(wěn)定性較好，能夠有效降低定位誤差，提高定位系統(tǒng)的精度。

關(guān)鍵詞：井下定位;TDOA定位模型;Chan算法;無(wú)跡卡爾曼濾波

中圖分類(lèi)號(hào)：TP271.5;TN409文獻(xiàn)標(biāo)識(shí)碼：A文章編號(hào)：2095-1302（2019）01-00-04

0 引 言

為防止礦井事故的發(fā)生，國(guó)家安全生產(chǎn)監(jiān)督管理總局于2010年頒布了《國(guó)務(wù)院關(guān)于進(jìn)一步加強(qiáng)企業(yè)安全生產(chǎn)工作的通知》，要求各類(lèi)礦山要嚴(yán)格制定和實(shí)施生產(chǎn)技術(shù)裝備標(biāo)準(zhǔn)。其中包括裝備使用礦井人員定位系統(tǒng)[1]。因脈沖超寬帶（IR-UWB）抗信道衰減性低，頻帶資源豐富穿透能力強(qiáng)，時(shí)間分辨率高，抗多徑效應(yīng)明顯等優(yōu)勢(shì)成為了井下定位技術(shù)的研究熱點(diǎn)[2]。

礦井無(wú)線電信號(hào)傳輸受巷道分支、彎曲、傾斜、斷面面積和形狀、圍巖介質(zhì)、巷道表面粗糙度、支護(hù)、縱向?qū)w（電纜、鐵軌、水管等）、橫向?qū)w（工字鋼支護(hù)等）設(shè)備等影響，信道環(huán)境復(fù)雜多變，多徑效應(yīng)及NLOS明顯[2]，對(duì)于基于觀測(cè)信號(hào)到達(dá)角度（AOA）及接收檢測(cè)信號(hào)強(qiáng)度（RSSI）的定位方法不適用于礦井人員的精確定位。

信號(hào)到達(dá)時(shí)間（TOA）、信號(hào)到達(dá)時(shí)間差（TDOA）充分利用了超寬帶帶寬和超高時(shí)間分辨率的特點(diǎn)，定位精度與信號(hào)發(fā)射設(shè)備與接收設(shè)備之間信號(hào)傳輸時(shí)間有關(guān)，受巷道等外部環(huán)境影響小，因此適合用于礦井人員精確定位[3]。TOA原理是在移動(dòng)標(biāo)簽與固定基站及固定基站間的時(shí)鐘精確同步的前提下，測(cè)量標(biāo)簽發(fā)射信號(hào)到達(dá)基站的時(shí)間信息，乘以光速c獲得距離信息，進(jìn)而完成標(biāo)簽的位置估計(jì)。而TDOA則是通過(guò)測(cè)量移動(dòng)標(biāo)簽到兩個(gè)不同固定基站的時(shí)間差值，乘以光速c得到固定距離差值，并根據(jù)標(biāo)簽到兩個(gè)基站的距離差建立雙曲線方程，之后憑借多基站建立雙曲線方程組求解獲得標(biāo)簽的位置信息。固定基站與移動(dòng)標(biāo)簽的時(shí)間同步，降低了系統(tǒng)的測(cè)距誤差，因此被廣泛應(yīng)用于室內(nèi)定位。

1 TDOA定位模型

TDOA又稱(chēng)作雙曲線定位方案[4]。根據(jù)雙曲線的數(shù)學(xué)原理，以?xún)蓚€(gè)已知位置坐標(biāo)的固定UWB基站作為焦點(diǎn)，以?xún)烧唛g的距離差為長(zhǎng)軸做一條雙曲線，同理用另外一個(gè)固定基站也可做出雙曲線。兩曲線的交叉點(diǎn)為標(biāo)簽坐標(biāo)位置。設(shè)移動(dòng)標(biāo)簽的位置坐標(biāo)為（x，y），空間內(nèi)設(shè)N（N≥3）個(gè)固定基站，坐標(biāo)為Pi（xi，yi），i=1，2，…，N。選取1號(hào)基站為主基站，其他基站與主基站進(jìn)行相關(guān)運(yùn)算得到TDOA值。

2 Chan算法

TDOA的定位難點(diǎn)在于非線性方程組的求解轉(zhuǎn)化為非線性?xún)?yōu)化最優(yōu)估計(jì)的問(wèn)題，需要無(wú)錯(cuò)測(cè)距信息，但高斯白噪聲產(chǎn)生的測(cè)距誤差會(huì)導(dǎo)致非理想的定位估計(jì)。將Chan算法作為依據(jù)TDOA測(cè)量值的典型誤差校正方法。進(jìn)行兩次最小二乘估計(jì)代替對(duì)雙曲線方程組的求解，計(jì)算量小。在服從零均值高斯分布誤差的前提下精度高，滿(mǎn)足工程實(shí)際要求。但在NLOS環(huán)境下定位性能明顯下降[5]。

（1）只有三個(gè)固定基站接收到標(biāo)簽信號(hào)時(shí)

3 無(wú)跡卡爾曼濾波算法

UKF濾波算法是對(duì)非線性函數(shù)概率密度分布的近似估計(jì)，用一組確定樣本近似逼近狀態(tài)的后驗(yàn)概率密度分布，而非像EKF濾波算法那樣直接對(duì)非線性函數(shù)的線性化近似[6]。因此其不需要運(yùn)算Jacobian矩陣偏導(dǎo)數(shù)，計(jì)算量小。因?yàn)閁KF濾波沒(méi)有忽略高次項(xiàng)，所以對(duì)非線性分布的統(tǒng)計(jì)如服從高斯分布的TDOA測(cè)量值的非線性方程組有較高的計(jì)算精度，相比于EKF濾波算法在穩(wěn)定性和精度上都存在很大優(yōu)勢(shì)。在不同時(shí)刻k下，在添加具有高斯白噪聲V（k）的觀測(cè)值Z和高斯白噪聲w（k）的隨機(jī)變量X構(gòu)成的非線性系統(tǒng)方程描述如式（10）：

（1）獲取一組Sigma點(diǎn)集及對(duì)應(yīng)權(quán)值。

（2）計(jì)算2n+1個(gè)Sigma點(diǎn)集的一步預(yù)測(cè)，i=1，2，…，2n+1。

（3）計(jì)算系統(tǒng)狀態(tài)量X的一步預(yù)測(cè)及協(xié)方差矩陣P。

（4）根據(jù)一步預(yù)測(cè)結(jié)果再次進(jìn)行UT變換，產(chǎn)生一組新的Sigma點(diǎn)集。

（5）將步驟（4）產(chǎn)生的一組新Sigma點(diǎn)集代入觀測(cè)方程，得到預(yù)測(cè)的觀測(cè)值Z，i=1，2，…，2n+1。

（6）將步驟（5）由新Sigma點(diǎn)集得到觀測(cè)預(yù)測(cè)方程結(jié)果，通過(guò)加權(quán)求和得到系統(tǒng)預(yù)測(cè)的均值和協(xié)方差。

（7）計(jì)算增益矩陣。

（8）計(jì)算系統(tǒng)狀態(tài)更新X和協(xié)方差更新P。

為了更加直觀地顯示狀態(tài)估計(jì)的準(zhǔn)確性，將每個(gè)時(shí)刻的估計(jì)值與真實(shí)值作差，得到絕對(duì)值偏差，即：

4 Chan-UKF定位算法

Chan算法具有穩(wěn)定性高、時(shí)間復(fù)雜度低的特點(diǎn)，UKF對(duì)不確定誤差具有最佳估算能力。室內(nèi)定位中，用Chan算法的輸出作為UKF濾波的初始值，輸出得到目標(biāo)節(jié)點(diǎn)的位置，算法的整體流程如圖6所示。

算法主要分為兩大部分，即算法的初始化和目標(biāo)節(jié)點(diǎn)位置的估計(jì)。Chan算法的初始化主要依據(jù)基站的布局對(duì)式（6）中的矩陣G進(jìn)行初始化，UKF濾波器的初始化主要是對(duì)狀態(tài)及協(xié)方差陣的初始化。完成后等待接收新數(shù)據(jù)，觸發(fā)Chan算法對(duì)節(jié)點(diǎn)位置的初次估計(jì)，將輸出的估計(jì)值作為UKF濾波器的觀測(cè)狀態(tài)，對(duì)節(jié)點(diǎn)位置作二次估計(jì)，從而得到更精確的節(jié)點(diǎn)位置。

為了分析得到Chan-UKF算法的定位性能，借助 Matlab 2014b仿真平臺(tái)，仿真分析該算法的定位精度和穩(wěn)定性。

Chan-UKF算法與Chan算法位置曲線如圖7所示，可以看出，Chan-UKF算法的曲線比Chan算法更加接近真實(shí)曲線，表明Chan-UKF算法比Chan算法的定位精度高。Chan-UKF算法與Chan算法位置估計(jì)誤差曲線如圖5 結(jié) 語(yǔ)

基于UWB無(wú)線定位在井下復(fù)雜多變環(huán)境中的運(yùn)用，本文提出Chan-UKF定位算法。該算法聯(lián)合Chan算法和UKF濾波算法，用Chan算法的位置估計(jì)值作為UKF濾波的初始值，滿(mǎn)足穩(wěn)定性和實(shí)時(shí)性的同時(shí)能夠有效估計(jì)移動(dòng)標(biāo)簽位置。實(shí)驗(yàn)結(jié)果表明，Chan-UKF算法降低約50%的定位誤差，有效提高了定位精度。

參 考 文 獻(xiàn)

[1]孫繼平，楊大明，張志鈺，等.煤礦井下安全避險(xiǎn)“六大系統(tǒng)”建設(shè)指南[M].北京：煤炭工業(yè)出版社，2012.

[2]萬(wàn)群，郭賢生，陳章鑫.室內(nèi)定位理論方法和應(yīng)用[M].北京：電子工業(yè)出版社，2012.

[3]蔣恩松.礦井?dāng)U頻測(cè)距定位方法研究[D].北京：中國(guó)礦業(yè)大學(xué)（北京），2017.

[4]肖竹，王勇超，田斌，等.超寬帶定位研究與應(yīng)用：回顧和展望[J].電子學(xué)報(bào)，2011，39（1）：133-141.

[5] CHAN Y T，YAU C H，PAK-CHUNG C.Exact and approximate maximum likelihood localization algorithms[J].IEEE transactions on vehicular technology，2006，55（1）：10-16.

[6]李靜，劉琚.用卡爾曼濾波器消除TOA中NLOS誤差的三種方法[J].通信學(xué)報(bào)，2005，26（1）：130-135.

[7]孫哲星.基于時(shí)間測(cè)距的礦井人員定位方法研究[J].工礦自動(dòng)化，2018，44（4）：30-33.

[8]李仕云，鄧洪權(quán).基于EKF的雙站測(cè)向/時(shí)差聯(lián)合定位算法[J].無(wú)線電通信技術(shù)，2015，41（1）：38-40.

[9] ZHANG J，DONG F，F(xiàn)ENG G，et al. Analysis of the NLOS channel environment of TDOA multiple algorithms[C]// Sensors， IEEE，2016：1-4.

[10]唐路，李奇.基于UWB室內(nèi)定位算法研究[J].工業(yè)控制計(jì)算機(jī)，2017，30（6）：103-104.