本技術(shù)屬于車輛定位，尤其涉及一種基于協(xié)作定位的礦井自動駕駛車輛定位方法。

背景技術(shù)：

1、隨著智慧礦山的建設(shè)，越來越多的5g基站部署到國內(nèi)大型礦井中，尤其是在輔助運輸巷道及其他重要工作地點，這對實現(xiàn)礦山輔助運輸系統(tǒng)少人化、自動化提供了強有力的技術(shù)支持。目前井下車輛常用的定位技術(shù)主要有5g定位、激光雷達(laser?radar)、超寬帶(ultra?wide?band,uwb)、慣性導航(inertial?measurement?unit,imu)等?；趗wb和5g的定位技術(shù)在室內(nèi)環(huán)境中定位精度已經(jīng)可以達到厘米級，但是在井下巷道狹窄的空間內(nèi)信號容易被遮擋，導致定位精度嚴重下降。基于激光雷達的定位技術(shù)容易受到惡劣環(huán)境及光線的影響，而慣性導航的定位誤差會隨著時間而累計，無法單獨使用。5g網(wǎng)絡(luò)的高分辨率、低時延和較高的可靠性等可以使無線定位技術(shù)可以應用到井下自動駕駛車輛的定位過程中，基于無線信號的定位技術(shù)可以利用井下部署的基站對車輛進行協(xié)作定位，不受惡劣環(huán)境及光線粉塵等條件的限制。無線信號在狹小的巷道中的多徑傳播會給信號的估計帶來誤差，目前很多無線定位技術(shù)多傾向于減低多徑效應的影響來提升定位精度，但多徑分量中還包含有大量車輛和周圍環(huán)境的位置信息，所以可以利用多徑信號來輔助車輛進行定位，化劣勢為優(yōu)勢。

2、guan提出了一種削弱多徑效應的方法，該方法利用加權(quán)算法來緩解無線網(wǎng)絡(luò)中的非視距誤差。wielandt在室內(nèi)環(huán)境中利用反射面建立虛擬發(fā)射點，通過共享虛擬發(fā)射點實現(xiàn)對被測目標的精確定位，但該方法在復雜的環(huán)境中效果較差。leitinger提出了一種bt-slam的方法，該方法利用地面車輛的多徑信號進行基于特征的slam建圖與定位，但在多車場景下計算復雜、延時較高。savic等提出了基于移動節(jié)點的協(xié)作定位算法，利用非參數(shù)的置信度進行信息的傳遞，該算法根據(jù)網(wǎng)絡(luò)中各個節(jié)點的信息，對置信度進行更新，然后通過因子圖迭代使置信度收斂，解決了在重復性采樣過程中后驗密度函數(shù)無法充分利用信息量的問題。meng等設(shè)計了一種利用v2v輔助通信的協(xié)作定位算法，該算法建立了基于slam同步定位與建圖的貝葉斯模型，根據(jù)車車之間互相發(fā)送位置信息以及周圍環(huán)境中障礙物的信息，在貝葉斯因子圖上采用協(xié)作通信的聯(lián)合傳播算法來對目標車輛進行定位。nam等人提出了一種新的協(xié)作定位算法，該算法根據(jù)車輛自身攜帶的傳感器來感知周圍環(huán)境中其他車輛的信息，然后利用粒子濾波對這些信息進行融合，輔助計算目標車輛的位置信息，但是該算法含有累積誤差，容易陷入局部優(yōu)化。huang提出了一種回歸算法，解決了復雜網(wǎng)絡(luò)的協(xié)作定位問題，該算法常采用反向傳播來聚合網(wǎng)絡(luò)節(jié)點的位置特征，并利用卷積計算優(yōu)化網(wǎng)絡(luò)模型的節(jié)點參數(shù)，但該算法在多徑傳播環(huán)境下的定位精度較差。okugi提出了基于v2v的協(xié)同定位，該方法利用地圖融合對車輛進行定位，利用圖卷積算法來對地圖中各個車輛的位置信息進行融合，適合處理復雜環(huán)境中的避障問題，但是該算法容易陷入局部優(yōu)化。以上研究大多需要大量的先驗知識、復雜的地圖或已知的位置信息，很少利用多徑分量中的信息對車輛進行輔助定位。

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

1、針對現(xiàn)有技術(shù)中的缺陷，本發(fā)明提供一種基于協(xié)作定位的礦井自動駕駛車輛定位方法，基于車車協(xié)作的置信傳播算法，利用車車之間的多徑信號及車輛的位置信息，來提高井下自動駕駛車輛的定位精度。

2、為解決上述技術(shù)問題，根據(jù)本發(fā)明的一個方面，本發(fā)明提供了如下技術(shù)方案：

3、一種基于協(xié)作定位的礦井自動駕駛車輛定位方法，采用單個基站進行定位，基于單基站的定位系統(tǒng)，利用多徑信號中的幾何信息建立虛擬發(fā)射點來補充基站數(shù)量，然后將當前測量值與多個虛擬發(fā)射點進行匹配，對車輛進行輔助定位。

4、作為本發(fā)明所述的一種基于協(xié)作定位的礦井自動駕駛車輛定位方法的優(yōu)選方案，其中：根據(jù)系統(tǒng)模型建立貝葉斯模型，在貝葉斯模型的因子圖上利用置信傳播算法計算各個信息傳播節(jié)點的邊緣分布，推導網(wǎng)絡(luò)節(jié)點的后驗密度函數(shù)，經(jīng)過不斷迭代，直到各個節(jié)點的概率分布收斂于一個穩(wěn)態(tài)；信息在因子圖上傳遞，并對各個車輛以及虛擬發(fā)射點的位置進行估算，可聯(lián)合周圍車輛的信息和虛擬發(fā)射點的位置對目標車輛進行輔助定位。

5、作為本發(fā)明所述的一種基于協(xié)作定位的礦井自動駕駛車輛定位方法的優(yōu)選方案，其中：所述定位方法是車輛之間的通信和車車之間的多徑傳播，網(wǎng)絡(luò)中的節(jié)點發(fā)送自身的位置信息并接收來自其他節(jié)點的信號，在時刻t，目標節(jié)點接收到每個多徑分量的傳播時間及到達角，其測量值表示為：

6、

7、基站的預編碼器為：

8、y(t)＝[y1(t),y2(t),…yn(t)]????????????????(2)

9、編碼后目標節(jié)點的發(fā)送信號為：

10、

11、所述定位方法只考慮井下環(huán)境中的單反射多徑分量，不考慮路徑損耗較大、方向性不明確的多次反射，所以在時刻t，節(jié)點的接收信號為：

12、

13、式中，r(t)為時刻t的接收信號，λp(t)為第p條多徑信道的信道系數(shù)，εr和εt為發(fā)射端和接收端的響應矩陣，θp(t)為多徑p的到達角度，為多徑p的發(fā)射角度，τp(t)為多徑p的到達時間，z(t)為多徑p的信道噪聲，信道隨機噪聲服從高斯分布。

14、作為本發(fā)明所述的一種基于協(xié)作定位的礦井自動駕駛車輛定位方法的優(yōu)選方案，其中：在井下環(huán)境中隨著車輛的不斷移動，每一個車輛都會接收來自多個虛擬發(fā)射點發(fā)出的信號，但是考慮井下空間狹小、道路單一的特點，所以互相協(xié)作通信的車輛會接收到來自相同的虛擬發(fā)射點的信號，這個虛擬發(fā)射點稱為共享虛擬發(fā)射點。

15、作為本發(fā)明所述的一種基于協(xié)作定位的礦井自動駕駛車輛定位方法的優(yōu)選方案，其中：不同車輛可共享同一個虛擬發(fā)射點，考慮車車之間和人車之間的通信，經(jīng)過幾何推導，可得出共虛擬發(fā)射點的位置，計算公式為：

16、

17、式中，為發(fā)射給vm的信號在反射面上的反射位置，tm為車輛vm的系統(tǒng)參數(shù)，為車輛vm在第p條多徑下的方向算子，滿足

18、作為本發(fā)明所述的一種基于協(xié)作定位的礦井自動駕駛車輛定位方法的優(yōu)選方案，其中：共享虛擬發(fā)射點與測量值的似然函數(shù)可推導為如下公式：

19、

20、式中，表示為t時刻下虛擬發(fā)射點的數(shù)據(jù)關(guān)聯(lián)變量，表示為第m輛車在t+1時刻的狀態(tài)，則表示在t-1時刻第s個虛擬發(fā)射點的位置信息；為測量誤差的高斯分布，dm為服從的集合，表示第m輛車可以檢測到的共享虛擬發(fā)射點；nm為服從的集合，表示第m輛車檢測到的新的共享虛擬發(fā)射點；

21、由于測量值及其數(shù)量是固定的，上式可簡化為如下形式：

22、

23、作為本發(fā)明所述的一種基于協(xié)作定位的礦井自動駕駛車輛定位方法的優(yōu)選方案，其中：系統(tǒng)中的多徑成分主要包括aoa測量值和toa測量值，可以通過多徑分離和相位解相的方法進行計算，在時刻t，第m條多徑信號的測量值可以用如下公式表示：

24、

25、式中，為aoa測量值的估計，和分別為方向角和俯仰角，為toa測量值的估計；

26、所以，由多徑信號構(gòu)建的多徑路徑計算公式如下：

27、

28、式中，fm表示目標車輛上接收信號裝置的高度，表示目標車輛的方向算子，

29、作為本發(fā)明所述的一種基于協(xié)作定位的礦井自動駕駛車輛定位方法的優(yōu)選方案，其中：置信傳播算法可以將基站的觀測值和其他車輛的相對位置{ri}進行融合，得到網(wǎng)絡(luò)中各個基站的位置；通過節(jié)點互相傳遞信息，將節(jié)點的狀態(tài)估計值傳遞給相鄰節(jié)點，從而計算各個節(jié)點觀測值的條件概率密度。

30、作為本發(fā)明所述的一種基于協(xié)作定位的礦井自動駕駛車輛定位方法的優(yōu)選方案，其中：在置信傳播算法的第n次迭代中，各個節(jié)點u對其他節(jié)點傳遞的信息的計算公式為：

31、

32、式中，為節(jié)點第n次信息迭代值，為網(wǎng)絡(luò)節(jié)點的方向向量，αu(xu,xv)為觀測值的似然函數(shù)估計值；

33、由此可推出第n次的迭代估計值為：

34、

35、式中，μ為節(jié)點參數(shù)。

36、作為本發(fā)明所述的一種基于協(xié)作定位的礦井自動駕駛車輛定位方法的優(yōu)選方案，其中：在井下環(huán)境中，由于基站與目標節(jié)點之間的距離及角度在估計過程中有誤差，對直射信號來說，觀測錨點的到達角qa與觀測錨點離開角qd之間不會重合，由于角度和位置估計的偏差造成了網(wǎng)絡(luò)節(jié)點之間的位置誤差，因此需要將直射信號進行分離，判別公式為：

37、

38、式中，σ為臨界常數(shù)，用于區(qū)分直射偏移量；

39、對于直射信號來說，qa與qd在實際應用中是不重合的，在時刻t節(jié)點i到節(jié)點j的直射信號傳遞的位置信息為：

40、

41、對于非直射信號來說，其定位信號為多徑線段hp，與分別hp的兩個端點，所以在時刻t，節(jié)點i到節(jié)點j的非直射定位信息為：

42、

43、作為本發(fā)明所述的一種基于協(xié)作定位的礦井自動駕駛車輛定位方法的優(yōu)選方案，其中：因子圖上的信息傳遞時，節(jié)點i到節(jié)點j相對位置矢量為：

44、

45、式中，ψp(rj←i)為直射信號的直線距離，若節(jié)點i與j之間存在直射信號，則

46、用表示非直射徑hp的方向向量，qa與qd分別為觀測錨點的到達角和離開角，則ψp(rj←i)為：

47、

48、式中，

49、所以，結(jié)合式(13)、(15)中的非直射成分為：

50、

51、式中，

52、信號中的直射成分為：

53、

54、所以，節(jié)點i到節(jié)點j的相對位置矢量rj←i為：

55、rj←i＝argmax{(ej←irj←i-dj←i)t(ej←irj←i-dj←i)}??????(19)。

56、作為本發(fā)明所述的一種基于協(xié)作定位的礦井自動駕駛車輛定位方法的優(yōu)選方案，其中：當直射信號和非直射信號都存在時，則：

57、

58、當只有非直射信號時，則：

59、

60、當只有直射信號時，則：

61、

62、可得到相對位置的矢量為：

63、

64、對于系統(tǒng)中第i個節(jié)點，其相對位置的測量節(jié)點集合為：

65、ω(i)＝{j|rj←i≠φ}????????????????(24)

66、最后可以計算出目標車輛的相對位置矢量：

67、vi＝{rj←i|i∈ω(i)}??????????????????(25)。

68、作為本發(fā)明所述的一種基于協(xié)作定位的礦井自動駕駛車輛定位方法的優(yōu)選方案，其中：基于網(wǎng)絡(luò)節(jié)點觀測到的數(shù)據(jù)、假設(shè)的先驗概率，用貝葉斯模型估算出目標節(jié)點信息的邊緣后驗密度函數(shù)；貝葉斯模型可表示為：

69、

70、式中，p(e|h)為后驗概率，p(h|e)為目標的似然函數(shù)，p(h)為先驗概率。

71、作為本發(fā)明所述的一種基于協(xié)作定位的礦井自動駕駛車輛定位方法的優(yōu)選方案，其中：在井下巷道環(huán)境中反射面一般有多個，每個目標車輛都同時接收多個虛擬發(fā)射點的信息；考慮到井下巷道狹長筆直的特點，將目標節(jié)點的似然函數(shù)擴展為如下形式：

72、

73、式中，為誤差測量的分布；

74、假設(shè)虛擬發(fā)射點的狀態(tài)轉(zhuǎn)移和車輛的狀態(tài)，經(jīng)過一階馬爾科夫推導，節(jié)點的轉(zhuǎn)移函數(shù)可表示為：

75、

76、在式中計算出了網(wǎng)絡(luò)節(jié)點中的信息傳遞，就可推導出各個節(jié)點的置信度，各個節(jié)點的置信度滿足對應節(jié)點的邊緣后驗分布，利用該分布可以計算出車輛之間的位置信息及各個虛擬發(fā)射點的位置估計。

77、作為本發(fā)明所述的一種基于協(xié)作定位的礦井自動駕駛車輛定位方法的優(yōu)選方案，其中：車輛的狀態(tài)預測包括當前時刻車輛和虛擬發(fā)射點的狀態(tài)預測，首先計算上一時刻車輛的置信度，然后根據(jù)車輛的狀態(tài)轉(zhuǎn)移函數(shù)和多徑傳播計算虛擬發(fā)射點的置信度，最后將置信度帶入因子圖，從而可以預測當前時刻車輛和虛擬發(fā)射點的狀態(tài)，公式如下：

78、

79、將計算得到的車輛和虛擬發(fā)射點的狀態(tài)預測值代入網(wǎng)絡(luò)節(jié)點的觀測值的似然函數(shù)中，可得到從網(wǎng)絡(luò)節(jié)點i傳遞到網(wǎng)絡(luò)節(jié)點j的測量信息估計值，其計算公式如下：

80、

81、網(wǎng)絡(luò)中每個節(jié)點的置信度可以看作各個節(jié)點近似的后驗分布，網(wǎng)絡(luò)中各個節(jié)點的置信度可以通過上述公式計算得到，車輛之間的測量值結(jié)合后驗密度函數(shù)可以得到車輛的置信度計算公式為：

82、

83、結(jié)合虛擬發(fā)射點的后驗密度函數(shù)，原有的虛擬發(fā)射點的置信度公式為：

84、

85、新出現(xiàn)的虛擬發(fā)射點的置信度公式為：

86、

87、綜上所述，利用后驗密度函數(shù)和置信度通過最小均方誤差法來計算被測車輛的狀態(tài)估計，其狀態(tài)公式為：

88、

89、新得到的虛擬發(fā)射點的位置計算公式為：

90、

91、本發(fā)明的有益效果如下：

92、本發(fā)明提出一種基于協(xié)作定位的礦井自動駕駛車輛定位方法，通過提取多徑信號中的有用信息和利用置信度傳播算法計算網(wǎng)絡(luò)中各個節(jié)點的相對位置，融合部分基站覆蓋范圍之內(nèi)的定位信息，可以計算出目標車輛的位置。最后通過對比仿真，與傳統(tǒng)的bp算法相比，辦發(fā)明方法提出的算法魯棒性更強，軌跡跟蹤更加平穩(wěn)，定位精度提高了10.6％，為井下車輛的定位技術(shù)提供了一些理論依據(jù)，有利于智慧礦山的發(fā)展。