本發(fā)明涉及礦井無人機(jī)車定位系統(tǒng)領(lǐng)域，具體是一種基于uwb的礦井無人機(jī)車高精度定位系統(tǒng)及方法。

背景技術(shù)：

井下大巷運(yùn)輸?shù)闹饕绞绞遣捎玫V井軌道機(jī)車運(yùn)輸，現(xiàn)階段，我國礦井機(jī)車均為人工駕駛，但礦井下復(fù)雜多變的生產(chǎn)環(huán)境給井下作業(yè)人員的生命安全造成了極大的威脅，井下作業(yè)的智能化和無人化成為今后礦業(yè)作業(yè)的必然趨勢，礦井機(jī)車無人駕駛系統(tǒng)為井下機(jī)車為解決上述問題提供了一種有效的解決方案。

但在礦井機(jī)車無人駕駛系統(tǒng)的安全可靠運(yùn)行需要準(zhǔn)確獲取井下的各類信息，其中井下機(jī)車實(shí)時(shí)位置的獲取對機(jī)車的運(yùn)行調(diào)度及安全駕駛具有非常重要的意義，但由于井下存在灰塵較多、磁場不穩(wěn)定等原因，使得目前的定位方法如基于rfid的定位方法和基于zigbee的定位方法等誤差較大，其定位精度都至少在1米以上完全不符合礦井無人駕駛系統(tǒng)的安全運(yùn)行需求嚴(yán)重影響了機(jī)車的精確調(diào)度和可靠運(yùn)行。

技術(shù)實(shí)現(xiàn)要素：

本發(fā)明的目的是提供一種基于uwb的礦井無人機(jī)車高精度定位系統(tǒng)及方法，以解決現(xiàn)有技術(shù)定位方法信道傳輸不穩(wěn)定、定位不精確的問題。

為了達(dá)到上述目的，本發(fā)明所采用的技術(shù)方案為：

一種基于uwb的礦井無人機(jī)車高精度定位系統(tǒng)，其特征在于：包括：

若干uwb標(biāo)簽節(jié)點(diǎn)tag，分別設(shè)置于礦井中作為uwb信號發(fā)送器，各個uwb標(biāo)簽節(jié)點(diǎn)tag的坐標(biāo)已知并分配有相應(yīng)的id號；

uwb錨節(jié)點(diǎn)anchor，其設(shè)置在礦井中機(jī)車上作為uwb信號接收器，所述uwb錨節(jié)點(diǎn)anchor實(shí)時(shí)接收各個uwb標(biāo)簽節(jié)點(diǎn)tag發(fā)送的脈沖信號，并記錄脈沖信號的到達(dá)時(shí)間以及脈沖信號對應(yīng)的uwb標(biāo)簽節(jié)點(diǎn)tag的坐標(biāo)數(shù)據(jù)；

ap設(shè)備，其設(shè)置在礦井中，所述uwb標(biāo)簽節(jié)點(diǎn)tag、uwb錨節(jié)點(diǎn)anchor分別與ap設(shè)備無線通信連接；

每個進(jìn)路至少一個信號機(jī)，所述信號機(jī)就是井下的路紅燈，只有綠燈時(shí)機(jī)車才能駛?cè)朐摋l進(jìn)路，信號機(jī)與井下交換機(jī)直接相連，信標(biāo)器與信號機(jī)成對存在，所述信標(biāo)器位置已知直接與井下交換機(jī)連接，可將其位置信息直接通過井下光纖環(huán)網(wǎng)傳輸至地面監(jiān)控中心。

所述的一種基于uwb的礦井無人機(jī)車高精度定位系統(tǒng)，其特征在于：uwb標(biāo)簽節(jié)點(diǎn)tag、uwb錨節(jié)點(diǎn)anchor均由主處理器以及連接在主處理器上的uwb模塊、調(diào)試接口、lcd顯示器及各類接口組成。

所述的一種基于uwb的礦井無人機(jī)車高精度定位系統(tǒng)，其特征在于：uwb標(biāo)簽節(jié)點(diǎn)tag等間距布設(shè)在礦井大巷上壁，相鄰uwb標(biāo)簽節(jié)點(diǎn)tag的間距小于其通信距離的三分之一。

所述的一種基于uwb的礦井無人機(jī)車高精度定位系統(tǒng)，其特征在于：uwb錨節(jié)點(diǎn)anchor布設(shè)在礦井中機(jī)車頂部。

一種基于uwb的礦井無人機(jī)車高精度定位方法，其特征在于：包括以下步驟：

(1)、將所有的uwb標(biāo)簽節(jié)點(diǎn)tag和uwb錨節(jié)點(diǎn)anchor維持一個相同的高精度同步時(shí)鐘；

(2)、使各個uwb標(biāo)簽節(jié)點(diǎn)tag定期向uwb錨節(jié)點(diǎn)anchor發(fā)送uwb脈沖信號及自身坐標(biāo)；

(3)、uwb錨節(jié)點(diǎn)anchor將接收到的數(shù)據(jù)提供給自身主處理器，主處理器通過toa測距算法測得各個uwb標(biāo)簽節(jié)點(diǎn)tag至錨uwb錨節(jié)點(diǎn)anchor的距離；

(4)、根據(jù)已知的各個uwb標(biāo)簽節(jié)點(diǎn)tag的坐標(biāo)及與uwb錨節(jié)點(diǎn)anchor的距離，使用最小二乘法計(jì)算給出uwb錨節(jié)點(diǎn)anchor的坐標(biāo)；

(5)、uwb錨節(jié)點(diǎn)anchor通過無線方式將數(shù)據(jù)傳輸?shù)絘p設(shè)備，ap設(shè)備再將數(shù)據(jù)經(jīng)過井下光纖環(huán)網(wǎng)傳輸?shù)降孛姹O(jiān)控中心，地面面監(jiān)控中心結(jié)合礦井機(jī)車所在信標(biāo)器位置給出機(jī)車井下的最終位置。

所述的一種基于uwb的礦井無人機(jī)車高精度定位方法，其特征在于：所述步驟(3)的具體過程為：

(3.1)、uwb錨節(jié)點(diǎn)anchor收集到的信息為uwb標(biāo)簽節(jié)點(diǎn)tagi傳輸?shù)降臅r(shí)間ti及其坐標(biāo)(xi,yi)，其中i代表標(biāo)簽節(jié)點(diǎn)tag在井下的編號，由于各個uwb標(biāo)簽節(jié)點(diǎn)tag具有等間距分布且間距小于其通信距離的三分之一的分布特點(diǎn)，uwb錨節(jié)點(diǎn)anchor接收到的uwb標(biāo)簽節(jié)點(diǎn)tag坐標(biāo)一般為3個；

(3.2)、通過toa算法，測得uwb標(biāo)簽節(jié)點(diǎn)tagi到uwb錨節(jié)點(diǎn)anchor的傳輸時(shí)間δti，然后用傳輸時(shí)間δti乘以光速c得到uwb標(biāo)簽節(jié)點(diǎn)tagi到uwb錨節(jié)點(diǎn)anchor的距離di。

所述的一種基于uwb的礦井無人機(jī)車高精度定位方法，其特征在于：所述步驟(4)的具體過程為：

(4.1)、根據(jù)上述步驟(3.2)中的距離及坐標(biāo)數(shù)據(jù)，列出方程組(1)；

(4.2)、將方程組(1)的前(n-1)個方程分別減去第n個方程，從而達(dá)到二次方程線性化的目的，得到下列方程組(2)：

(4.3)、定義：

將方程組(2)改寫成θ＝q^-1b，用最小二乘法ls求解可得θ＝(q^tq)^-1q^tb。

本發(fā)明是一種基于uwb的礦井無人機(jī)車高精度定位方法，采用uwb技術(shù)，節(jié)點(diǎn)之間通過記錄窄脈沖的到達(dá)時(shí)間來計(jì)算節(jié)點(diǎn)距離并結(jié)合最小二乘算法計(jì)算出機(jī)車最終的坐標(biāo)位置。

與已有技術(shù)相比，本發(fā)明的有益效果體現(xiàn)在：

本發(fā)明的uwb定位方法并不是直接定位機(jī)車位置，而是將定位分成三步，第一步測距，在測距方面節(jié)點(diǎn)采用的是窄脈沖信號傳輸具有很強(qiáng)抗干擾的特性，避免了井下灰塵過多及磁場紊亂等原因而造成的定位偏差，第二步是計(jì)算在進(jìn)路里的坐標(biāo)，在第一步測的多點(diǎn)間精確距離的基礎(chǔ)上利用最小二乘算法求解節(jié)點(diǎn)的坐標(biāo)，第三步監(jiān)控中心根據(jù)進(jìn)路中信標(biāo)器的位置與機(jī)車坐標(biāo)的相對位置，從而最終確定定位機(jī)車在礦井的位置，最高定位精度可達(dá)0.1m。

附圖說明

圖1基于uwb的無人機(jī)車定位系統(tǒng)結(jié)構(gòu)框圖。

圖2井下uwb的無人機(jī)車定位方法示意圖。

圖3uwb標(biāo)簽節(jié)點(diǎn)tag與錨節(jié)點(diǎn)anchor的結(jié)構(gòu)框圖。

圖4uwb測距算法示意圖。

具體實(shí)施方式

如圖1所示，一種基于uwb的礦井無人機(jī)車高精度定位系統(tǒng)，包括：

若干uwb標(biāo)簽節(jié)點(diǎn)tag，分別設(shè)置于礦井中作為uwb信號發(fā)送器，各個uwb標(biāo)簽節(jié)點(diǎn)tag的坐標(biāo)已知并分配有相應(yīng)的id號；

uwb錨節(jié)點(diǎn)anchor，其設(shè)置在礦井中機(jī)車上作為uwb信號接收器，所述uwb錨節(jié)點(diǎn)anchor實(shí)時(shí)接收各個uwb標(biāo)簽節(jié)點(diǎn)tag發(fā)送的脈沖信號，并記錄脈沖信號的到達(dá)時(shí)間以及脈沖信號對應(yīng)的uwb標(biāo)簽節(jié)點(diǎn)tag的坐標(biāo)數(shù)據(jù)；

ap設(shè)備，其設(shè)置在礦井中，所述uwb標(biāo)簽節(jié)點(diǎn)tag、uwb錨節(jié)點(diǎn)anchor分別與ap設(shè)備無線通信連接；

每個進(jìn)路至少一個信號機(jī)，所述信號機(jī)主要是顯示當(dāng)前進(jìn)路是否開放允許級車進(jìn)入，信號機(jī)與井下交換機(jī)直接相連，信標(biāo)器與信號機(jī)成對存在，所述信標(biāo)器位置已知直接與井下交換機(jī)連接，可將其位置信息直接通過井下光纖環(huán)網(wǎng)傳輸至地面監(jiān)控中心。

uwb標(biāo)簽節(jié)點(diǎn)tag、uwb錨節(jié)點(diǎn)anchor均由主處理器以及連接在主處理器上的uwb模塊、調(diào)試接口、lcd顯示器及各類接口組成。

uwb標(biāo)簽節(jié)點(diǎn)tag等間距布設(shè)在礦井大巷上壁，相鄰uwb標(biāo)簽節(jié)點(diǎn)tag的間距小于其通信距離的三分之一。

uwb錨節(jié)點(diǎn)anchor布設(shè)在礦井中機(jī)車頂部。

一種基于uwb的礦井無人機(jī)車高精度定位方法，包括以下步驟：

(1)、將所有的uwb標(biāo)簽節(jié)點(diǎn)tag和uwb錨節(jié)點(diǎn)anchor維持一個相同的高精度同步時(shí)鐘；

(2)、使各個uwb標(biāo)簽節(jié)點(diǎn)tag定期向uwb錨節(jié)點(diǎn)anchor發(fā)送uwb脈沖信號及自身坐標(biāo)；

(3)、uwb錨節(jié)點(diǎn)anchor將接收到的數(shù)據(jù)提供給自身主處理器，主處理器通過toa測距算法測得各個uwb標(biāo)簽節(jié)點(diǎn)tag至錨uwb錨節(jié)點(diǎn)anchor的距離；

(4)、根據(jù)已知的各個uwb標(biāo)簽節(jié)點(diǎn)tag的坐標(biāo)及與uwb錨節(jié)點(diǎn)anchor的距離，使用最小二乘法計(jì)算給出uwb錨節(jié)點(diǎn)anchor的坐標(biāo)；

(5)、uwb錨節(jié)點(diǎn)anchor通過無線方式將數(shù)據(jù)傳輸?shù)絘p設(shè)備，ap設(shè)備再將數(shù)據(jù)經(jīng)過井下光纖環(huán)網(wǎng)傳輸?shù)降孛姹O(jiān)控中心，地面面監(jiān)控中心結(jié)合礦井機(jī)車所在信標(biāo)器位置給出機(jī)車井下的最終位置。

步驟(3)的具體過程為：

(3.1)、uwb錨節(jié)點(diǎn)anchor收集到的信息為uwb標(biāo)簽節(jié)點(diǎn)tagi傳輸?shù)降臅r(shí)間ti及其坐標(biāo)(xi,yi)，其中i代表標(biāo)簽節(jié)點(diǎn)tag在井下的編號，由于各個uwb標(biāo)簽節(jié)點(diǎn)tag具有等間距分布且間距小于其通信距離的三分之一的分布特點(diǎn)，uwb錨節(jié)點(diǎn)anchor接收到的uwb標(biāo)簽節(jié)點(diǎn)tag坐標(biāo)一般為3個；

(3.2)、通過toa算法，測得uwb標(biāo)簽節(jié)點(diǎn)tagi到uwb錨節(jié)點(diǎn)anchor的傳輸時(shí)間δti，然后用傳輸時(shí)間δti乘以光速c得到uwb標(biāo)簽節(jié)點(diǎn)tagi到uwb錨節(jié)點(diǎn)anchor的距離di。

步驟(4)的具體過程為：

(4.1)、根據(jù)上述步驟(3.2)中的距離及坐標(biāo)數(shù)據(jù)，列出方程組(1)；

(4.2)、將方程組(1)的前(n-1)個方程分別減去第n個方程，從而達(dá)到二次方程線性化的目的，得到下列方程組(2)：

(4.3)、定義：

將方程組(2)改寫成θ＝q^-1b，用最小二乘法ls求解可得θ＝(q^tq)^-1q^tb。uwb測距方法的距離節(jié)點(diǎn)如圖2所示具備如下裝置：若干標(biāo)簽節(jié)點(diǎn)tag、錨節(jié)點(diǎn)anchor、通信ap、信標(biāo)器等及部分組成。

本發(fā)明中，各類裝置如圖2所示進(jìn)行布設(shè)，uwb標(biāo)簽節(jié)點(diǎn)tag均勻布設(shè)在井下大巷頂部并對其一一編號，uwb標(biāo)簽節(jié)點(diǎn)tag的最大通信距離為80米，每個uwb標(biāo)簽節(jié)點(diǎn)tag相隔20米，uwb錨節(jié)點(diǎn)anchor布置在機(jī)車頂部。

如圖3所示，uwb標(biāo)簽節(jié)點(diǎn)tag、uwb錨節(jié)點(diǎn)anchor都是由dw1000uwb模塊、ram/stm處理器、調(diào)試接口、lcd顯示器及各類接口組成。

所有uwb標(biāo)簽節(jié)點(diǎn)tag及uwb錨節(jié)點(diǎn)anchor都維持一個共同的同步時(shí)鐘，每隔一個時(shí)鐘周期t，標(biāo)簽節(jié)點(diǎn)tag發(fā)射一次脈沖信號。

如圖4所示：根據(jù)uwb標(biāo)簽節(jié)點(diǎn)tag的布設(shè)特點(diǎn)，uwb錨節(jié)點(diǎn)anchor隔一個周期都能收到三個uwb標(biāo)簽節(jié)點(diǎn)tag發(fā)來的占脈沖信號，并記錄其到達(dá)時(shí)間分別為t1，t2，t3及相應(yīng)坐標(biāo)如(20,15)、(30,15)、(40,15)。

uwb錨節(jié)點(diǎn)anchor通過uwb標(biāo)簽節(jié)點(diǎn)tag的發(fā)送時(shí)間t0計(jì)算節(jié)點(diǎn)傳輸?shù)臅r(shí)間差δt1，δt2，δt3，并乘以光速獲得uwb錨節(jié)點(diǎn)anchor與tag1，tag2，tag3節(jié)點(diǎn)的距離分別為25米、15米、25米。

將上述uwb錨節(jié)坐標(biāo)及距離數(shù)據(jù)帶入得到方程組(1)：

將方程組(1)的前(n-1)個方程分別減去第n個方程，從而達(dá)到把二次方程線性化的目的，得到下列方程組(2)：

定義：

解得最終利用最小二乘法θ＝(q^tq)^-1q^tb，最終解得得到錨節(jié)點(diǎn)在進(jìn)路里的坐標(biāo)位置為(40,0)。

uwb錨節(jié)點(diǎn)anchor將坐標(biāo)數(shù)據(jù)經(jīng)過ap上傳至地面監(jiān)控中心，地面監(jiān)控中心結(jié)合信標(biāo)器i的坐標(biāo)位置最終確定機(jī)車在井下巷道的最終位置。