本發(fā)明屬于定位，具體涉及一種地下密閉空間內(nèi)nlos場(chǎng)景下pdr輔助uwb協(xié)同定位方法。

背景技術(shù)：

1、在地下工程中，精準(zhǔn)定位對(duì)于保障人員安全和提高作業(yè)效率至關(guān)重要。當(dāng)前，有多種定位技術(shù)被應(yīng)用于地下人員及設(shè)備定位，其中包括rfid定位、zigbee定位、wi-fi定位、藍(lán)牙定位和uwb定位等。rfid難以集成到移動(dòng)設(shè)備中且作用距離短；zigbee技術(shù)信號(hào)不穩(wěn)定，定位范圍小，安全性差；wi-fi定位技術(shù)易受環(huán)境的干擾，并且當(dāng)訪問點(diǎn)多時(shí)，定位效果較差。

2、例如公開號(hào)為cn117061991a的中國(guó)專利文件，提供一種基于uwb的井下人員定位系統(tǒng)及定位方法，系統(tǒng)包括：井下人員定位卡、井下車輛、井下車輛定位卡、定位裝置、特定藍(lán)牙信標(biāo)、定位基站和定位服務(wù)器；定位裝置和特定藍(lán)牙信標(biāo)設(shè)置在井下車輛的內(nèi)部，特定藍(lán)牙信標(biāo)與井下車輛綁定；井下人員定位卡用于井下人員隨身攜帶，與定位裝置通信連接；井下車輛定位卡安裝于井下車輛外部，與定位裝置通過串口通訊連接；定位基站設(shè)置在地面上與定位服務(wù)器、井下人員定位卡以及井下車輛定位卡通信連接，定位服務(wù)器注冊(cè)有綁定的特定藍(lán)牙信標(biāo)和井下車輛。

3、該現(xiàn)有技術(shù)中井下人員定位極大的依賴于藍(lán)牙信標(biāo)，然而，類似井下這種地下密閉空間可能存在各種電磁干擾源，這些干擾可能會(huì)影響藍(lán)牙信號(hào)的傳輸，從而降低定位精度，相比之下，uwb定位技術(shù)因具備出色的多徑分辨能力、強(qiáng)大的抗干擾能力和低功耗特性，成為了首選的地下密閉空間定位解決方案，但地下密閉空間環(huán)境復(fù)雜，多徑效應(yīng)和物體阻擋問題普遍，導(dǎo)致uwb的測(cè)距性能受nlos傳播的顯著影響。因此如何識(shí)別、抑制nlos誤差成為提高地下密閉空間定位精度必須解決的問題。

4、再例如公開號(hào)為cn113038377a的中國(guó)專利文件，公開了一種基于uwb技術(shù)c-twr的井下一維精確定位方法，包括以下步驟：在井下布設(shè)uwb定位基站、采用了c-twr過程得到兩個(gè)測(cè)量值(識(shí)別卡到達(dá)uwb定位器a、uwb定位器b距離)、雙程數(shù)據(jù)的誤差初步修正、判斷識(shí)別卡的一維線性方向、采用一維卡爾曼濾波得到最終的距離值，該發(fā)明通過可以實(shí)現(xiàn)更高的井下定位精度，并具有很好的定位實(shí)時(shí)性，能實(shí)現(xiàn)無盲區(qū)定位，滿足煤礦井下應(yīng)用技術(shù)要求。

5、然而，上述現(xiàn)有技術(shù)僅能實(shí)現(xiàn)井下密閉空間一維線性方向上的定位，位置精確度較低，因此需要一種定位方法，能夠有效識(shí)別地下密閉空間nlos路徑、抑制nlos誤差，且能夠提高人員定位的可靠性和準(zhǔn)確性。

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

1、為解決上述問題，本發(fā)明提供了一種地下密閉空間內(nèi)nlos場(chǎng)景下pdr輔助uwb協(xié)同定位方法，以解決現(xiàn)有技術(shù)中的問題。

2、為了達(dá)到上述的發(fā)明目的，本發(fā)明提出一種地下密閉空間內(nèi)nlos場(chǎng)景下pdr輔助uwb協(xié)同定位方法，包括：

3、s1：建立四元狀態(tài)假設(shè)模型，所述四元狀態(tài)假設(shè)模型包括固定基站的los狀態(tài)和nlos狀態(tài)、移動(dòng)基站的los狀態(tài)和nlos狀態(tài)；

4、s2：基于cmd行列式識(shí)別每個(gè)所述固定基站或所述移動(dòng)基站是否處于nlos狀態(tài)，將處于所述los狀態(tài)的所述固定基站和所述移動(dòng)基站分別定義為第一固定基站和第一移動(dòng)基站；

5、s3：基于pdr技術(shù)獲取待定位標(biāo)簽的位置信息，基于所述位置信息融合uwb定位技術(shù)獲取所述第一固定基站和所述第一移動(dòng)基站到所述待定位標(biāo)簽的第一距離信息，基于所述第一距離信息建立擴(kuò)展卡爾曼濾波的狀態(tài)方程和量測(cè)方程；

6、s4：將定位產(chǎn)生的誤差作為系統(tǒng)狀態(tài)向量，基于所述狀態(tài)方程和所述量測(cè)方程對(duì)所述系統(tǒng)狀態(tài)向量進(jìn)行預(yù)測(cè)和更新，獲得后驗(yàn)估計(jì)信息，基于所述后驗(yàn)估計(jì)信息對(duì)所述待定位標(biāo)簽位置進(jìn)行實(shí)時(shí)矯正并輸出。

7、進(jìn)一步地，建立四元狀態(tài)假設(shè)模型包括以下步驟：

8、基于第一公式計(jì)算移動(dòng)基站對(duì)待定位標(biāo)簽的權(quán)值wi，所述第一公式為：其中，de為測(cè)距誤差，re為相對(duì)定位誤差，獲取固定基站或所述移動(dòng)基站到所述待定位標(biāo)簽之間的距離信息，對(duì)所述距離信息進(jìn)行建模生成四元狀態(tài)假設(shè)模型，所述四元狀態(tài)假設(shè)模型包括所述固定基站的los狀態(tài)假設(shè)h1和nlos狀態(tài)假設(shè)h2，以及所述移動(dòng)基站的los狀態(tài)假設(shè)h3和nlos狀態(tài)假設(shè)h4；

9、基于第二公式獲取在四種狀態(tài)假設(shè)下的距離信息di，所述第二公式為：其中，ri為基于toa定位算法獲取的第i個(gè)固定基站或移動(dòng)基站到所述待定位標(biāo)簽的測(cè)量距離，ei為系統(tǒng)噪聲，滿足高斯分布，ei～n(0,σ2)，ni為nlos傳播引起的正向誤差，e為所述移動(dòng)基站自身的誤差，ni＞＞ei。

10、進(jìn)一步地，計(jì)算測(cè)距誤差和相對(duì)定位誤差包括以下步驟：

11、基于第三公式計(jì)算所述測(cè)距誤差de，所述第三公式為：其中，ra和rb分別表示所述移動(dòng)基站和所述待定位標(biāo)簽之間的真實(shí)距離和估計(jì)距離，r為通信半徑，基于第四公式計(jì)算所述相對(duì)定位誤差re，所述第四公式為：其中，(xi,yi)為第i個(gè)移動(dòng)基站的實(shí)際坐標(biāo)，為第i個(gè)待定位標(biāo)簽的估計(jì)坐標(biāo)，n為所述移動(dòng)基站數(shù)量。

12、進(jìn)一步地，基于cmd行列式識(shí)別每個(gè)所述固定基站或所述移動(dòng)基站是否處于nlos狀態(tài)包括以下步驟：

13、確定cmd矩陣，所述cmd矩陣為：其中，a和b為所述固定基站，t1為所述移動(dòng)基站，t2為所述待定位標(biāo)簽，d2(a,b)＝d2(b,a)，且均表示所述固定基站a和所述固定基站b之間的歐式距離的平方，d2(a,t1)＝d2(t1,a)，且均表示所述固定基站a和所述移動(dòng)基站t1距離的平方，d2(a,t2)＝d2(t2,a)，且均表示所述固定基站a和所述待定位標(biāo)簽t2距離的平方，d2(b,t1)＝d2(t1,b)，且均表示所述固定基站b和所述移動(dòng)基站t1距離的平方，d2(b,t2)＝d2(t2,b)，且均表示所述固定基站b和所述待定位標(biāo)簽t2距離的平方，d2(t1,t2)＝d2(t2,t1)，且均表示所述移動(dòng)基站t1和所述待定位標(biāo)簽t2距離的平方；

14、基于第五公式確定所述cmd矩陣的行列式d(a,b,t1,t2)，所述第五公式為：det為行列式計(jì)算公式，獲取所述行列式d(a,b,t1,t2)的秩，若所述行列式為0且所述秩小于第一閾值，則表示所述固定基站或所述移動(dòng)基站處于nlos狀態(tài)，否則處于los狀態(tài)。

15、進(jìn)一步地，基于pdr技術(shù)獲取待定位標(biāo)簽的位置信息包括以下步驟：

16、獲取所述待定位標(biāo)簽在k時(shí)刻的位置信息，所述位置信息包括所述待定位標(biāo)簽所在的行人當(dāng)前的橫坐標(biāo)和縱坐標(biāo)、步長(zhǎng)l和方向角θ，基于第六公式計(jì)算第k時(shí)刻的所述橫坐標(biāo)xk和所述縱坐標(biāo)yk，所述第六公式為：其中，(xk-1,yk-1)為所述行人在k-1時(shí)刻的位置坐標(biāo)，基于第七公式計(jì)算所述步長(zhǎng)l，所述第七公式為：其中，h為所述行人的身高，p為所述行人的步頻，a、b和c分別為第一參數(shù)、第二參數(shù)和第三參數(shù)。

17、進(jìn)一步地，建立狀態(tài)方程包括以下步驟：

18、基于第八公式獲取所述系統(tǒng)狀態(tài)向量xt，所述第八公式為：xt＝[ex(t),ey(t),el(t),eθ(t)]t，其中，ex(t)、ey(t)、el(t)和eθ(t)分別為x軸和y軸上的位置誤差、步長(zhǎng)誤差和方向角誤差，基于第九公式建立狀態(tài)方程，所述第九公式為：xt+1＝ftxt+wt，其中，xt+1為t+1時(shí)刻的所述系統(tǒng)狀態(tài)向量，ft為系統(tǒng)狀態(tài)轉(zhuǎn)移矩陣，θt為t時(shí)刻所述行人的方向角，wt為系統(tǒng)動(dòng)態(tài)噪聲，包括x軸和y軸上的位置噪聲wx和wy、步長(zhǎng)噪聲wl和方向角噪聲wθ，其中，wx～n(0,2)，wy～n(0,2)，wl～n(0,1)，wθ～n(0,4)。

19、進(jìn)一步地，建立量測(cè)方程之前包括以下步驟：

20、基于第十公式建立初始量測(cè)方程，所述第十公式為：z＝htxt+vt，其中，z為系統(tǒng)量測(cè)信息，ht為量測(cè)系數(shù)矩陣，vt為測(cè)量噪聲，其中，vt～n(0,rt)，rt為量測(cè)噪聲協(xié)方差矩陣，所述量測(cè)系數(shù)矩陣為：其中，為t時(shí)刻所述第一固定基站的位置信息，為所述待定位標(biāo)簽的位置信息，為t時(shí)刻基于所述pdr技術(shù)推算出來的虛擬基站的位置信息，為第i個(gè)所述第一固定基站或所述第一移動(dòng)基站在t時(shí)刻基于pdr算法推算的距離信息，所述量測(cè)噪聲協(xié)方差矩陣為：

21、基于第十一公式對(duì)所述四元狀態(tài)假設(shè)模型下的狀態(tài)假設(shè)進(jìn)行二值化處理，所述第十一公式為：ηi,t為t時(shí)刻第i個(gè)所述固定基站或所述移動(dòng)基站的所處的狀態(tài)，基于二值化處理結(jié)果生成第一矩陣，所述第一矩陣為基于所述第一矩陣識(shí)別處于所述nlos狀態(tài)下的基站并進(jìn)行剔除，獲得過渡量測(cè)方程，所述過渡量測(cè)方程為：zt＝ηtz，其中，zt為t時(shí)刻的量測(cè)信息。

22、進(jìn)一步地，建立量測(cè)方程包括以下步驟：

23、對(duì)所述過渡量測(cè)方程進(jìn)行維度簡(jiǎn)化獲得新的所述量測(cè)方程，所述量測(cè)方程為：其中，為基于所述uwb定位技術(shù)獲得的第i個(gè)所述第一固定基站與所述待定位標(biāo)簽之間的測(cè)距信息，為基于所述uwb定位技術(shù)獲得的所述第一移動(dòng)基站與所述待定位標(biāo)簽之間的測(cè)距信息；

24、基于第十二公式計(jì)算所述所述第十二公式為：基于第十三公式計(jì)算所述所述第十三公式為：基于第十四公式計(jì)算所述所述第十四公式為：其中，為t時(shí)刻所述第一移動(dòng)基站的位置信息。

25、進(jìn)一步地，預(yù)測(cè)階段包括以下步驟：

26、基于第十五公式計(jì)算t-1時(shí)刻對(duì)t時(shí)刻預(yù)測(cè)的系統(tǒng)狀態(tài)向量所述第十五公式為：其中，為在t-1時(shí)刻的預(yù)測(cè)系統(tǒng)狀態(tài)向量，ft/t-1為所述系統(tǒng)狀態(tài)轉(zhuǎn)移矩陣，基于第十六公式計(jì)算t-1時(shí)刻對(duì)t時(shí)刻預(yù)測(cè)的均方誤差矩陣pt/t-1，所述第十六公式為：其中，所述為所述系統(tǒng)狀態(tài)轉(zhuǎn)移矩陣的轉(zhuǎn)置，qt-1為t-1時(shí)刻的系統(tǒng)噪聲，pt-1為t-1時(shí)刻預(yù)測(cè)的均方誤差矩陣。

27、進(jìn)一步地，更新階段包括以下步驟：

28、基于第十七公式獲得所述擴(kuò)展卡爾曼濾波的濾波增益kt，所述第十七公式為：其中，為所述量測(cè)系數(shù)矩陣的轉(zhuǎn)置，基于第十八公式計(jì)算量測(cè)殘差et，所述第十八公式為：基于第十九公式對(duì)預(yù)測(cè)的所述系統(tǒng)狀態(tài)向量進(jìn)行矯正獲得狀態(tài)矯正矩陣所述第十九公式為：基于第二十公式對(duì)預(yù)測(cè)的所述均方誤差矩陣pt/t-1進(jìn)行更新，獲得更新后的所述均方誤差矩陣所述第二十公式為：將更新后的所述狀態(tài)矯正矩陣和所述均方誤差矩陣作為后驗(yàn)估計(jì)信息。

29、與現(xiàn)有技術(shù)相比，本發(fā)明的有益效果至少如下所述：

30、本發(fā)明提出了基于cmd行列式統(tǒng)計(jì)假設(shè)的nlos算法，通過仿真驗(yàn)證該方法能夠有效的識(shí)別出處于nlos環(huán)境下的固定基站或移動(dòng)基站；基于此識(shí)別效果，舍棄掉nlos情況下的固定基站和誤差大的移動(dòng)基站，挑選出los情況下的固定基站和誤差較小的移動(dòng)基站；利用改進(jìn)ekf算法，融合來自固定基站和移動(dòng)基站的測(cè)距數(shù)據(jù)，以及通過pdr算法推導(dǎo)出的距離信息，為待定位標(biāo)簽提供更加精確的定位信息。