本發(fā)明涉及隧道安全監(jiān)測(cè)的，尤其是指一種面向隧道安全監(jiān)測(cè)的基于uwb的三維定位方法。

背景技術(shù)：

1、地鐵隧道、煤礦巷道等環(huán)境的建設(shè)施工和生產(chǎn)過(guò)程中，人員預(yù)警和交通控制都依賴人員和設(shè)備的定位信息；當(dāng)有事故發(fā)生時(shí)，指揮救援決策也依賴這些定位信息，因此，隧道中人員、生產(chǎn)設(shè)備（如盾構(gòu)機(jī)、臺(tái)車等）和運(yùn)輸設(shè)備（如小火車）的精確位置對(duì)安全生產(chǎn)非常重要。隧道內(nèi)沒(méi)有衛(wèi)星定位信號(hào)，一般也沒(méi)有公用移動(dòng)通信信號(hào)，不能采用地面的gps或者北斗等定位系統(tǒng)以及基于移動(dòng)通信的定位系統(tǒng)，室內(nèi)定位系統(tǒng)往往采用超寬帶（ultra-wideband，uwb）定位方法?；趗wb的定位方法可以達(dá)到厘米級(jí)的定位精度，常見(jiàn)的uwb測(cè)距方法是通過(guò)計(jì)算飛行時(shí)間（time?of?flight，tof）來(lái)實(shí)現(xiàn)的，其中雙邊雙向測(cè)距（doubleside?two-way?ranging，ds-twr）是一種常用的測(cè)距算法。ds-twr方法的測(cè)距發(fā)起方和響應(yīng)方互相發(fā)送各自的接收和發(fā)送時(shí)間戳，最后根據(jù)時(shí)間戳數(shù)據(jù)計(jì)算出通信幀在節(jié)點(diǎn)之間的飛行時(shí)間，最后乘以環(huán)境中修正后的光速，得到測(cè)距結(jié)果。

2、在基于測(cè)距的定位中，被定位節(jié)點(diǎn)需要與多個(gè)錨節(jié)點(diǎn)（位置已知）進(jìn)行兩兩測(cè)距才能計(jì)算出被定位節(jié)點(diǎn)的位置，這需要比較密集的錨節(jié)點(diǎn)，而且測(cè)距通信的次數(shù)較多，很難適合隧道窄長(zhǎng)型空間的應(yīng)用場(chǎng)合。除了待定位節(jié)點(diǎn)與定位錨節(jié)點(diǎn)的距離信息外，如果還能夠測(cè)量待定位節(jié)點(diǎn)與定位錨節(jié)點(diǎn)天線平面的角度，則可以大量減少錨節(jié)點(diǎn)的數(shù)量和定位所需要的通信次數(shù)。pdoa（phase-difference-of-arrival，信號(hào)到達(dá)相位差）方法是測(cè)量待定位節(jié)點(diǎn)與定位錨節(jié)點(diǎn)的相對(duì)角度信息的典型方法。具體方法是在錨節(jié)點(diǎn)上安裝兩根天線，天線的間距d小于λ/2（λ為uwb的波長(zhǎng)），由于待測(cè)節(jié)點(diǎn)與兩根天線的距離有差別，因此兩根天線接收到的信號(hào)的相位不同，通過(guò)相位差可以計(jì)算出錨節(jié)點(diǎn)與信號(hào)源的相對(duì)角度值。錨節(jié)點(diǎn)的兩根天線只能測(cè)量出待測(cè)節(jié)點(diǎn)與錨節(jié)點(diǎn)天線在同一平面（或者在該平面上投影）的角度，因此，為了在單錨節(jié)點(diǎn)的情況下獲得被測(cè)節(jié)點(diǎn)的三維定位數(shù)據(jù)，需要設(shè)置多組不平行的天線。

3、在常見(jiàn)的半波長(zhǎng)間距的三天線陣列中，測(cè)角誤差一般在3-5°。在地鐵隧道和煤礦井巷道等場(chǎng)景中，施工現(xiàn)場(chǎng)類似于狹長(zhǎng)的走廊，錨節(jié)點(diǎn)一般間隔300-500米鋪設(shè)。假設(shè)待測(cè)人員與錨節(jié)點(diǎn)相距200米，角度測(cè)量誤差為4°，在垂直于隧道的方向上，距離誤差近似為14米。誤差值已經(jīng)超過(guò)了地鐵隧道等施工現(xiàn)場(chǎng)的寬度，測(cè)量誤差太大，可用性差。在多錨節(jié)點(diǎn)的定位中，往往需要多次雙向測(cè)距，而雙向雙邊測(cè)距方法中每次節(jié)點(diǎn)收到一幀都需要等待一定時(shí)間后再發(fā)送，每增加一次測(cè)距，都會(huì)增加多次等待時(shí)間使系統(tǒng)的實(shí)時(shí)性下降，過(guò)長(zhǎng)的定位時(shí)間也減少了錨節(jié)點(diǎn)可以實(shí)時(shí)測(cè)距的最大節(jié)點(diǎn)數(shù)，這些缺點(diǎn)在需要人員設(shè)備數(shù)量較多或人員設(shè)備在高速移動(dòng)時(shí)非常明顯。為了提高隧道中三維定位的精度和定位節(jié)點(diǎn)的數(shù)量，需要提高pdoa測(cè)角的精度和減少通信次數(shù)。

技術(shù)實(shí)現(xiàn)思路

1、本發(fā)明的目的在于克服遠(yuǎn)距離場(chǎng)景下因測(cè)角誤差引起的較大位置誤差，提供了一種面向隧道安全監(jiān)測(cè)的基于uwb的三維定位方法，可有效滿足在地鐵隧道等場(chǎng)景下的精準(zhǔn)定位需求。

2、為實(shí)現(xiàn)上述目的，本發(fā)明所提供的技術(shù)方案為：面向隧道安全監(jiān)測(cè)的基于uwb的三維定位方法，包括：

3、在錨節(jié)點(diǎn)上設(shè)置由多對(duì)天線構(gòu)成的天線陣列，通過(guò)該天線陣列的位置擺放來(lái)實(shí)現(xiàn)半球面方向的高精度測(cè)角，并具有角度信息冗余；

4、只需一次ds-twr通信，在ds-twr通信中，待測(cè)節(jié)點(diǎn)亦是標(biāo)簽節(jié)點(diǎn)，其為測(cè)距發(fā)起者，按設(shè)定周期發(fā)送測(cè)距請(qǐng)求幀，即poll幀；錨節(jié)點(diǎn)收到poll幀之后，延時(shí)一段時(shí)間后，發(fā)送響應(yīng)幀，即resp幀，待測(cè)節(jié)點(diǎn)收到resp幀之后，延時(shí)一段時(shí)間后，發(fā)送一次測(cè)距的最后一幀，即final幀，錨節(jié)點(diǎn)接收到final幀之后，完成一次ds-twr通信；錨節(jié)點(diǎn)的天線陣列能夠分別獲得待測(cè)節(jié)點(diǎn)發(fā)送poll幀和final幀的到達(dá)相位差數(shù)據(jù)，即pdoa數(shù)據(jù)，錨節(jié)點(diǎn)為測(cè)距響應(yīng)者，在天線陣列中，某根最先接收到請(qǐng)求幀的天線作為響應(yīng)天線，其余天線則為非響應(yīng)天線，響應(yīng)天線和非響應(yīng)天線都與待測(cè)節(jié)點(diǎn)進(jìn)行測(cè)距；

5、錨節(jié)點(diǎn)根據(jù)待測(cè)節(jié)點(diǎn)、響應(yīng)天線和非響應(yīng)天線的接收時(shí)間戳的差值，計(jì)算出每根天線與待測(cè)節(jié)點(diǎn)的距離，再基于計(jì)算的距離和pdoa數(shù)據(jù)分別得到粗定位的待測(cè)節(jié)點(diǎn)坐標(biāo)和精定位的多個(gè)候選待測(cè)節(jié)點(diǎn)坐標(biāo)，最后，從多個(gè)候選待測(cè)節(jié)點(diǎn)坐標(biāo)中篩選出與粗定位的待測(cè)節(jié)點(diǎn)坐標(biāo)之間距離最小者作為待測(cè)節(jié)點(diǎn)的最終定位結(jié)果。

6、進(jìn)一步，所述天線陣列采用三對(duì)天線，分別是1號(hào)天線與2號(hào)天線組成一對(duì)，3號(hào)天線與4號(hào)天線組成一對(duì)，5號(hào)天線與6號(hào)天線組成一對(duì)；所述天線陣列整體呈等邊三角形擺放，包含內(nèi)外兩個(gè)等邊三角形，其中1號(hào)天線、4號(hào)天線和6號(hào)天線組成外等邊三角形，2號(hào)天線、3號(hào)天線和5號(hào)天線組成內(nèi)等邊三角形，外等邊三角形的邊長(zhǎng)為uwb的2倍波長(zhǎng)，每對(duì)天線中的兩根天線間距小于uwb的二分之一波長(zhǎng)，每對(duì)天線的連線垂直于內(nèi)外兩個(gè)等邊三角形的對(duì)邊。

7、進(jìn)一步，所述ds-twr通信的具體過(guò)程如下：

8、待測(cè)節(jié)點(diǎn)按設(shè)定周期發(fā)送測(cè)距請(qǐng)求幀，即poll幀，并在幀中加入本次poll幀的發(fā)送時(shí)間戳；

9、錨節(jié)點(diǎn)的天線陣列接收待測(cè)節(jié)點(diǎn)的poll幀，其中響應(yīng)天線產(chǎn)生poll幀接收時(shí)間戳，并主動(dòng)延時(shí)時(shí)間后發(fā)送resp幀回復(fù)待測(cè)節(jié)點(diǎn)，同時(shí)，天線陣列能夠獲得待測(cè)節(jié)點(diǎn)發(fā)送poll幀的pdoa數(shù)據(jù)；響應(yīng)天線在延時(shí)時(shí)間期間，天線陣列中剩余的非響應(yīng)天線依次接收到poll幀，每根天線都產(chǎn)生對(duì)應(yīng)的poll幀接收時(shí)間戳，記錄每根非響應(yīng)天線的poll幀接收時(shí)間戳與響應(yīng)天線的poll幀接收時(shí)間戳之差；

10、待測(cè)節(jié)點(diǎn)接收到resp幀，產(chǎn)生resp幀接收時(shí)間戳，待測(cè)節(jié)點(diǎn)主動(dòng)延時(shí)時(shí)間后發(fā)送final幀，其中final幀包含待測(cè)節(jié)點(diǎn)resp幀的接收時(shí)間戳和final幀的發(fā)送時(shí)間戳數(shù)據(jù)；

11、響應(yīng)天線接收到final幀，產(chǎn)生final幀接收時(shí)間戳，計(jì)算出、、、四個(gè)時(shí)間差數(shù)據(jù)，有如下測(cè)距公式：

12、（1）；

13、式中，是響應(yīng)天線與標(biāo)簽節(jié)點(diǎn)之間幀的飛行時(shí)間的計(jì)算值，是響應(yīng)天線與標(biāo)簽節(jié)點(diǎn)的距離的計(jì)算值，為待測(cè)節(jié)點(diǎn)發(fā)送poll幀與接收到resp幀的時(shí)間差，為錨節(jié)點(diǎn)發(fā)送resp幀與接收到final幀的時(shí)間差，為待測(cè)節(jié)點(diǎn)預(yù)設(shè)延遲時(shí)間，為錨節(jié)點(diǎn)預(yù)設(shè)延遲時(shí)間， c為空氣中的光速；

14、錨節(jié)點(diǎn)的非響應(yīng)天線隨后依次接收到final幀，產(chǎn)生對(duì)應(yīng)的接收時(shí)間戳，同時(shí)，錨節(jié)點(diǎn)的天線陣列能夠獲得待測(cè)節(jié)點(diǎn)發(fā)送final幀的pdoa數(shù)據(jù)，計(jì)算每根非響應(yīng)天線的final幀接收時(shí)間戳與響應(yīng)天線的final幀接收時(shí)間戳之差，非響應(yīng)天線根據(jù)下式（2）得到測(cè)距結(jié)果：

15、（2）；

16、式中，是非響應(yīng)天線與標(biāo)簽節(jié)點(diǎn)之間幀的飛行時(shí)間的計(jì)算值，是非響應(yīng)天線與標(biāo)簽節(jié)點(diǎn)之間的距離的計(jì)算值；根據(jù)上述公式（2）依次計(jì)算出每根非響應(yīng)天線到待測(cè)節(jié)點(diǎn)的距離，至此得到了6根天線各自到待測(cè)節(jié)點(diǎn)的距離。

17、進(jìn)一步，當(dāng)待測(cè)節(jié)點(diǎn)與錨節(jié)點(diǎn)測(cè)距過(guò)程通信結(jié)束后，即一次ds-twr通信結(jié)束，測(cè)距節(jié)點(diǎn)能夠及時(shí)進(jìn)入睡眠狀態(tài)，以節(jié)省能量；錨節(jié)點(diǎn)的6根天線都會(huì)接收到與待測(cè)節(jié)點(diǎn)在一次ds-twr通信的poll幀和final幀的uwb信號(hào)，從而每對(duì)天線得到兩個(gè)pdoa相位差，其中poll幀和final幀各一次，并取每對(duì)天線兩個(gè)pdoa相位差的平均值為該對(duì)天線本次ds-twr通信的接收相位差；

18、假設(shè)錨節(jié)點(diǎn)的天線平面置于平面，以等邊三角形重心為坐標(biāo)原點(diǎn)，三角形邊長(zhǎng)為，為uwb的波長(zhǎng)，每對(duì)天線中的兩根天線間距為a，設(shè)待測(cè)節(jié)點(diǎn)的三維坐標(biāo)為，則三對(duì)天線共有6個(gè)距離關(guān)系式：

19、（3）；

20、式中，、、、、、分別為待測(cè)節(jié)點(diǎn)到1、2、3、4、5、6號(hào)天線的距離；設(shè)為號(hào)天線，為號(hào)天線，，為每對(duì)天線中兩根天線到待測(cè)節(jié)點(diǎn)的距離平均值，為號(hào)天線與號(hào)天線接收信號(hào)的相位差，采用弧度表示，如下式（4）所示：

21、（4）；

22、由公式（3）和（4）得到粗定位的待測(cè)節(jié)點(diǎn)坐標(biāo)，表示為：

23、（5）。

24、進(jìn)一步，由1號(hào)天線、4號(hào)天線和6號(hào)天線相互之間的相位差，根據(jù)三根天線的距離公式有：

25、（6）；

26、按照前述公式（4）相同的求解方法，有：

27、（7）；

28、（8）；

29、式中，是間距為的兩根遠(yuǎn)距離天線之間的真實(shí)相位差的候選值，為兩根遠(yuǎn)距離天線之間的通過(guò)uwb芯片測(cè)得的相位差，的真實(shí)值可能與的測(cè)量值相差，，從而得到精定位中待測(cè)節(jié)點(diǎn)坐標(biāo)的三個(gè)候選值。

30、進(jìn)一步，根據(jù)粗定位和精定位的三個(gè)候選值，按照下式得到最終的精確定位坐標(biāo)為：

31、（9）；

32、式中，為向量坐標(biāo)表示，為的向量坐標(biāo)表示，即選擇粗定位和精定位之間距離最小者的坐標(biāo)為待測(cè)節(jié)點(diǎn)的最終定位結(jié)果。

33、進(jìn)一步，所述錨節(jié)點(diǎn)將待測(cè)節(jié)點(diǎn)的精確定位坐標(biāo)發(fā)送給安全監(jiān)控系統(tǒng)。

34、進(jìn)一步，所述天線陣列中每根天線都配備相同芯片，所有芯片由同一個(gè)晶振提供時(shí)鐘，晶振到每個(gè)芯片的距離相等。

35、本發(fā)明與現(xiàn)有技術(shù)相比，具有如下優(yōu)點(diǎn)與有益效果：

36、1、本發(fā)明的天線陣列具有半球面的測(cè)角能力和高測(cè)角精度，實(shí)現(xiàn)了單錨點(diǎn)的高精度三維定位，滿足了在地鐵隧道等場(chǎng)景下的精準(zhǔn)定位需求。

37、2、本發(fā)明提出的測(cè)距方法可以測(cè)量錨節(jié)點(diǎn)的每根天線與待測(cè)節(jié)點(diǎn)的距離，不需要假設(shè)待測(cè)節(jié)點(diǎn)與錨節(jié)點(diǎn)所有天線距離都近似相等，提高了近距離時(shí)的定位精度。

38、3、本發(fā)明使用的雙邊雙向測(cè)距（double?side?two-way?ranging，ds-twr）方法，錨節(jié)點(diǎn)的非響應(yīng)天線不需要發(fā)送通信幀，整個(gè)測(cè)距過(guò)程只需要進(jìn)行一次雙向雙邊測(cè)距通信，不會(huì)增加通信次數(shù)，提高了測(cè)距人員和設(shè)備的密度。

39、4、本發(fā)明使用的雙邊雙向測(cè)距數(shù)據(jù)幀針對(duì)電池供電的標(biāo)簽節(jié)點(diǎn)進(jìn)行了優(yōu)化，減少了標(biāo)簽節(jié)點(diǎn)的接收次數(shù)并降低了標(biāo)簽節(jié)點(diǎn)運(yùn)算量，延長(zhǎng)了電池供電的標(biāo)簽節(jié)點(diǎn)的可使用時(shí)間。