基于TSOA定位原理混合算法的掘進(jìn)機(jī)位姿檢測(cè)方法

劉 超，符世琛，成 龍，劉 丹，沈 陽(yáng)，吳 淼

(中國(guó)礦業(yè)大學(xué)(北京) 機(jī)電與信息工程學(xué)院，北京 100083)

智能化開(kāi)采成為煤炭安全高效開(kāi)采的發(fā)展方向與必然趨勢(shì)[1]。無(wú)人采掘裝備是當(dāng)前國(guó)際煤炭領(lǐng)域的發(fā)展前沿[2]。在地下巷道掘進(jìn)過(guò)程中，傳統(tǒng)掘進(jìn)方法效率低，精度有限，而且井下礦工長(zhǎng)期處于高溫、高濕、高濃度粉塵環(huán)境，健康嚴(yán)重受損，井下環(huán)境復(fù)雜，事故頻發(fā)，增加了礦工的工作強(qiáng)度及危險(xiǎn)性，因此我國(guó)亟待研制無(wú)人采掘裝備[3]。要實(shí)現(xiàn)綜掘工作面的無(wú)人化，綜掘裝備自主導(dǎo)控是其關(guān)鍵，掘進(jìn)機(jī)的位姿檢測(cè)方法是實(shí)現(xiàn)綜掘裝備自主導(dǎo)控的核心，具有重要的研究意義。

智能化開(kāi)采成為煤炭安全高效開(kāi)采的發(fā)展方向與必然趨勢(shì)[1]。無(wú)人采掘裝備是當(dāng)前國(guó)際煤炭領(lǐng)域的發(fā)展前沿[2]。在地下巷道掘進(jìn)過(guò)程中，傳統(tǒng)掘進(jìn)方法效率低，精度有限，而且井下礦工長(zhǎng)期處于高溫、高濕、高濃度粉塵環(huán)境，健康嚴(yán)重受損，井下環(huán)境復(fù)雜，事故頻發(fā)，增加了礦工的工作強(qiáng)度及危險(xiǎn)性，因此我國(guó)亟待研制無(wú)人采掘裝備[3]。要實(shí)現(xiàn)綜掘工作面的無(wú)人化，綜掘裝備自主導(dǎo)控是其關(guān)鍵，掘進(jìn)機(jī)的位姿檢測(cè)方法是實(shí)現(xiàn)綜掘裝備自主導(dǎo)控的核心，具有重要的研究意義。

自20世紀(jì)90年代以來(lái)，國(guó)外主要產(chǎn)煤大國(guó)多以露天礦為主，在井下實(shí)際運(yùn)用掘進(jìn)機(jī)位姿檢測(cè)技術(shù)的報(bào)道較少，多數(shù)技術(shù)集中在盾構(gòu)掘進(jìn)機(jī)中[4]。日本GYRO系統(tǒng)[5]使用陀螺儀檢測(cè)盾構(gòu)掘進(jìn)機(jī)的姿態(tài)角，環(huán)境溫度的變化對(duì)陀螺儀的機(jī)械有較大影響，該系統(tǒng)實(shí)用性不佳，只能作為人工測(cè)量的輔助參考。德國(guó)艾柯夫公司[6]的掘進(jìn)機(jī)具有方向控制和自動(dòng)成型以及對(duì)故障監(jiān)控的功能，但位姿檢測(cè)精度不高，受環(huán)境影響較大，在礦井中未見(jiàn)成熟應(yīng)用案例。

近年來(lái)，國(guó)內(nèi)學(xué)者對(duì)掘進(jìn)機(jī)的位姿檢測(cè)方法進(jìn)行了許多研究。陶云飛[7]提出了一種基于激光導(dǎo)向系統(tǒng)的位姿測(cè)量方法、周玲玲等[8]提出了一種基于雙激光標(biāo)靶圖像識(shí)別的位姿檢測(cè)方法、童敏明等[9]提出了一種多傳感器的定位系統(tǒng)、杜雨馨等[10]提出了一種基于機(jī)器視覺(jué)的位姿檢測(cè)系統(tǒng)、賈文浩等[11]提出了一種基于iGPS的掘進(jìn)機(jī)定位系統(tǒng)、吳淼等[12]提出了一種基于空間交匯測(cè)量技術(shù)的位姿自主測(cè)量方法、黃東等[13]提出了一種基于視覺(jué)/慣導(dǎo)的位姿測(cè)量方法。從上述研究可以發(fā)現(xiàn)，目前掘進(jìn)機(jī)的位姿檢測(cè)方法主要有:慣性導(dǎo)航、機(jī)器視覺(jué)、激光、全站儀、無(wú)線電等。不同的定位方法定位精度也各不相同，慣性導(dǎo)航[14]由于導(dǎo)航信息經(jīng)過(guò)積分產(chǎn)生，所以定位誤差隨時(shí)間增大，精度差;基于激光、機(jī)器視覺(jué)的定位環(huán)境適應(yīng)性較差，巷道中粉塵較大，會(huì)干擾可見(jiàn)光及紅外線的傳播，對(duì)成像會(huì)產(chǎn)生嚴(yán)重的影響;基于全站儀的定位需要專(zhuān)業(yè)測(cè)量人員參與，巷道環(huán)境復(fù)雜，增加了全站儀的工作難度;無(wú)線電定位主要包括WiFi、Bluetooth等，但上述方法精度不高，而超寬帶(Ultra-wideband,UWB)測(cè)量范圍廣、系統(tǒng)組成簡(jiǎn)單、抗干擾能力強(qiáng)，局域范圍內(nèi)具有高精度測(cè)距功能，在掘進(jìn)機(jī)靜止或者移動(dòng)過(guò)程中均能提供較高的定位精度?；赨WB測(cè)距，符世琛等[15-16]提出了一種基于TOA定位原理的掘進(jìn)機(jī)自主定位定向方法，但該原理需要保證基站和終端的時(shí)基必須是同步的，同時(shí)該原理存在位置模糊解[17]，影響精度。

筆者基于UWB測(cè)距，根據(jù)TSOA定位原理，提出了一種面向掘進(jìn)機(jī)的混合算法的位姿檢測(cè)方法，建立了觀測(cè)方程組，詳細(xì)地推導(dǎo)了混合算法的計(jì)算過(guò)程;基于MATLAB仿真對(duì)比了間接法和混合算法的定位點(diǎn)空間分布、三軸誤差及均方根誤差，分析了混合算法的姿態(tài)角精度，在模擬巷道進(jìn)行了實(shí)驗(yàn)驗(yàn)證，代入混合算法得到三維坐標(biāo)，與標(biāo)定值進(jìn)行對(duì)比，得到了誤差隨距離變化的規(guī)律。

1 面向掘進(jìn)機(jī)的超寬帶位姿檢測(cè)系統(tǒng)

1.1 UWB定位技術(shù)

UWB測(cè)距[19]是利用納秒甚至皮秒級(jí)的極窄脈沖來(lái)實(shí)現(xiàn)信息的傳輸，UWB信號(hào)的相對(duì)帶寬必須大于0.2，絕對(duì)帶寬大于500 MHz，具有恒定的波達(dá)時(shí)間分辨率，可解算出準(zhǔn)確的測(cè)距信息。它具有抗干擾能力強(qiáng)、多徑分辨能力強(qiáng)、系統(tǒng)容量大、傳輸速率高、安全性高、成本低和功耗低等優(yōu)點(diǎn)。

通過(guò)UWB信號(hào)無(wú)線電波[20]，測(cè)量目標(biāo)的方向、距離、距離差等定位參量，實(shí)現(xiàn)位置坐標(biāo)求解，定位過(guò)程如圖1所示。利用UWB測(cè)距進(jìn)行掘進(jìn)機(jī)的位姿檢測(cè)，關(guān)鍵是獲得掘進(jìn)機(jī)機(jī)身定位點(diǎn)的坐標(biāo)。首先由一個(gè)或多個(gè)位置已知的基站發(fā)射無(wú)線電信號(hào)，無(wú)線電信號(hào)的電參量(如振幅、頻率、相位、時(shí)間等)中的一個(gè)或多個(gè)攜帶著定位參量信息，經(jīng)電波傳播到PC端;其次PC端接收并處理該無(wú)線電信號(hào)，并根據(jù)電波傳播特性，由電參量得到定位參量(如來(lái)波方向、距離、距離差、高度等);再次根據(jù)得到的定位參量及位置已知的基站，獲得多個(gè)相對(duì)于各基站的位置面;最后由多個(gè)位置面根據(jù)相應(yīng)的定位算法，得到掘進(jìn)機(jī)機(jī)定位點(diǎn)的坐標(biāo)。

圖1 UWB定位結(jié)構(gòu)Fig.1 Ultra-wideband positioning structure

1.2 TSOA定位原理

測(cè)距和定位(Time Summation of Arrival，TSOA)原理[20]是基于波達(dá)時(shí)間和，通過(guò)信號(hào)同步網(wǎng)絡(luò)實(shí)現(xiàn)在時(shí)域、頻域、空域上的嚴(yán)格同步，各站將測(cè)得的目標(biāo)數(shù)據(jù)通過(guò)數(shù)據(jù)傳遞網(wǎng)進(jìn)行傳遞，發(fā)射站只發(fā)射信號(hào)，接收站通過(guò)接收待測(cè)目標(biāo)反射的發(fā)射站信號(hào)，測(cè)量從發(fā)射站到待測(cè)目標(biāo)以及待測(cè)目標(biāo)到接收站的距離和。在一個(gè)特定空間，保持與兩個(gè)指定基準(zhǔn)點(diǎn)距離和相等的點(diǎn)的軌跡是以接收站、發(fā)射站為焦點(diǎn)的橢球面，該面稱(chēng)為橢圓球位置面。在測(cè)量沒(méi)有誤差的情況下，UWB測(cè)距模塊一旦得到接收站和發(fā)射站的距離和P，那么待測(cè)目標(biāo)一定在由P所確定的橢圓球位置面上，其定位原理如圖2所示。

設(shè)定4個(gè)定位基站A，B，C，D，基站A為發(fā)射站，只發(fā)射信號(hào)，其位置坐標(biāo)為(XA,YA,ZA)，基站B，C，D為接收站，只接收信號(hào)，其基站群位置坐標(biāo)為(Xi,Yi,Zi)，其中i=B,C,D。接收站通過(guò)接收目標(biāo)反射的發(fā)射站信號(hào)，測(cè)量從發(fā)射站到目標(biāo)的距離rA，以及目標(biāo)到接收站的距離ri。掘進(jìn)機(jī)的機(jī)身節(jié)點(diǎn)E的位置坐標(biāo)為(XE,YE,ZE);第i個(gè)接收站獲得的觀測(cè)量為Pi，解算出3個(gè)橢圓球的交點(diǎn)，得出目標(biāo)點(diǎn)的坐標(biāo)值，則機(jī)身定位點(diǎn)位置坐標(biāo)的TSOA觀測(cè)方程組為

(1)

式中，

2 基于間接法與Taylor級(jí)數(shù)展開(kāi)法的混合算法

位置坐標(biāo)的解算方法有很多種，但是無(wú)論采用哪種方法均需求解非線性方程。方程(1)就是非線性方程組，然而在工程實(shí)現(xiàn)中，求解非線性方程并不是一件容易的事，同時(shí)，UWB測(cè)距存在一定的誤差，測(cè)距誤差的引入會(huì)導(dǎo)致橢圓球位置面不相交于一點(diǎn)或者根本沒(méi)有交點(diǎn)。綜上所述，對(duì)掘進(jìn)機(jī)機(jī)身的定位問(wèn)題就由原來(lái)的非線性方程精確求解問(wèn)題轉(zhuǎn)換成非線性?xún)?yōu)化的最優(yōu)估計(jì)問(wèn)題。

間接法與Taylor級(jí)數(shù)展開(kāi)法各有優(yōu)缺點(diǎn)，二者都是將非線性問(wèn)題轉(zhuǎn)化為線性問(wèn)題。間接法可以通過(guò)直接計(jì)算得到一個(gè)初始點(diǎn)坐標(biāo)，性能穩(wěn)定。Taylor級(jí)數(shù)展開(kāi)法為了防止收斂到局部最優(yōu)點(diǎn)以及減少迭代次數(shù)，初始點(diǎn)的估計(jì)值需要靠近真實(shí)值，但在實(shí)際中不易實(shí)現(xiàn)，雖然一般都能收斂到真實(shí)點(diǎn)，但迭代過(guò)程的收斂性無(wú)法保證，同時(shí)每一次迭代都需要對(duì)矩陣求逆，運(yùn)算量大。而由間接法得到的定位點(diǎn)估計(jì)值可以解決初始點(diǎn)這一問(wèn)題，滿(mǎn)足Taylor算法對(duì)初始點(diǎn)的要求，文獻(xiàn)[18]表明Taylor級(jí)數(shù)算法的精度高于間接法。因此，在本文中提出了將間接法與Taylor級(jí)數(shù)展開(kāi)法進(jìn)行混合，通過(guò)間接法得到初始點(diǎn)坐標(biāo)值后，代入Taylor級(jí)數(shù)展開(kāi)法進(jìn)行循環(huán)迭代消除誤差，從而可以得到更精確的掘進(jìn)機(jī)機(jī)身位姿。

2.1 間接法

在間接法中，首先把rA看成一個(gè)已知量，從而解得XE,YE,ZE是rA的線性函數(shù)，求出機(jī)身定位點(diǎn)E的坐標(biāo)估計(jì)值，具體過(guò)程如下:

通過(guò)對(duì)方程組(1)進(jìn)行移項(xiàng)，平方，整理化簡(jiǎn)，得

(2)

式中,

將式(2)轉(zhuǎn)化為矩陣形式:

AX=b

(3)

式中

使rank(A)=3，由此可得最小二乘解為

X=(ATA)-1ATb

(4)

可得機(jī)身定位點(diǎn)E的坐標(biāo)估計(jì)值為XE,YE,ZE。

2.2 混合定位算法

Taylor級(jí)數(shù)展開(kāi)法是一種基于Taylor級(jí)數(shù)展開(kāi)的加權(quán)最小二乘估計(jì)迭代算法，其核心思想為:首先在目標(biāo)位置的初始估計(jì)點(diǎn)利用Taylor級(jí)數(shù)展開(kāi)，并忽略二次及以上項(xiàng)，將非線性方程變?yōu)榫€性方程，采用最小二乘法對(duì)三軸偏移量進(jìn)行估計(jì);然后利用估計(jì)的偏移量對(duì)初始坐標(biāo)進(jìn)行修正，不斷迭代，使估計(jì)的目標(biāo)位置逼近真實(shí)位置，從而得到對(duì)目標(biāo)位置的最優(yōu)估計(jì)，混合算法程序流程圖如圖3所示。算法步驟主要如下:

(1)將式(4)中計(jì)算得到的(XE,YE,ZE)作為E點(diǎn)的初始估計(jì)坐標(biāo)值，與真值(XE0,YE0,ZE0)的誤差設(shè)為(δX,δY,δZ)，則有

(5)

(2)將式(1)在(XE,YE,ZE)點(diǎn)用Taylor級(jí)數(shù)展開(kāi)，并忽略二次及以上項(xiàng)，則有

(6)

將式(6)轉(zhuǎn)化為矩陣形式，則有

Αδ=Ζ

(7)

圖3 混合算法程序流程Fig.3 Hybrid algorithm program flow chart

可得到δ的最小二乘解為

δ=(ATA)-1ATZ

(8)

(3)判斷選擇的判據(jù)ε是否小于給定的門(mén)限ε0，其中ε=δX+δY+δZ，設(shè)定ε0=0.1，若ε大于門(mén)限，則用式(9)代替對(duì)目標(biāo)的上次估計(jì)值，并重復(fù)步驟(2)，(3)。

(9)

(4)若ε小于門(mén)限，則停止迭代，定位點(diǎn)E的最終估計(jì)位置為XE1,YE1,ZE1。

(10)

3 混合算法精度分析

3.1 定位點(diǎn)分布

為了探究定位點(diǎn)的分布，設(shè)定基站坐標(biāo)A(0，10，0)，B(2，5，0)，C(-2，5，0)，D(2，0，5)，E點(diǎn)為掘進(jìn)機(jī)機(jī)身定位點(diǎn)，坐標(biāo)為(0，n，0)。采用美國(guó)Time domain公司生產(chǎn)的P440 UWB測(cè)距模塊，寬帶為3.1～5.3 GHz，中心頻率為4.3 GHz，經(jīng)過(guò)課題組的大量實(shí)驗(yàn)驗(yàn)證[21-22]，該模塊在100 m狹長(zhǎng)封閉空間的誤差基本都小于0.02 m，在視距傳播的條件下的測(cè)距參量一般呈現(xiàn)高斯分布，因此將均值為0、標(biāo)準(zhǔn)差為0.02 m的測(cè)距誤差代入間接法和混合算法，得到定位點(diǎn)的三維坐標(biāo)，并進(jìn)行1 000次定位仿真，將仿真結(jié)果所得到的均值作為最終定位點(diǎn)的坐標(biāo)值，n分別取15 m和90 m。

當(dāng)n為15 m時(shí)，得到2種算法的定位點(diǎn)空間分布如圖4所示。在間接法中，定位點(diǎn)在空間呈橢球狀分布，定位點(diǎn)比較密集，三軸的誤差可以達(dá)到厘米級(jí)，而在混合算法中，定位點(diǎn)在空間呈發(fā)射狀分布，定位點(diǎn)發(fā)散，三軸的誤差可以達(dá)到毫米級(jí)以上，相比間接法，精度有較為明顯的提高。

圖4 15 m定位點(diǎn)分布Fig.4 15 m positioning point distribution

對(duì)15 m這個(gè)定位點(diǎn)進(jìn)行定位精度分析，在MATLAB中利用2種算法測(cè)量100次，得到如圖5所示的三軸誤差仿真曲線。在間接法中，最大誤差在1 cm以?xún)?nèi)，Y軸誤差較小，X軸、Z軸次之;在混合算法中，三軸的誤差在毫米級(jí)以上，X軸誤差較小，Y軸、Z軸無(wú)較大差異，該算法精度高于間接法。

當(dāng)n為90 m，2種算法的定位點(diǎn)分布如圖6所示。二者在空間中均呈橢球狀，定位點(diǎn)比較密集，但混合算法的精度高于間接法，可達(dá)毫米級(jí)。

圖5 15 m三軸誤差仿真曲線Fig.5 15 m triaxial error curves

圖6 90 m定位點(diǎn)分布Fig.6 90 m positioning point distribution

在90 m，用2種算法在MATLAB中對(duì)定位點(diǎn)測(cè)量100次，與真值比較，得到如圖7所示的三軸誤差曲線。由圖7可知，間接法的誤差基本維持在4 cm以?xún)?nèi)，Y軸誤差最小，X軸、Z軸誤差無(wú)明顯差異，而混合算法三軸的誤差達(dá)毫米級(jí)以上，Y軸誤差最小，混合算法的精度有較大提高。

圖7 90 m三軸誤差仿真曲線Fig.7 90 meter triaxial error curves

3.2 均方根誤差分析

在定位過(guò)程中，無(wú)論何種定位方法，由于受測(cè)量設(shè)備和傳播環(huán)境的影響，測(cè)量的定位參量都會(huì)存在誤差，從而導(dǎo)致定位誤差的存在，并且采用不同的定位算法，定位誤差的大小也不一樣。均方根誤差[23]是評(píng)價(jià)定位精度的一個(gè)十分重要的指標(biāo)，均方根誤差越小，定位精度越高，因此此處采用均方根誤差作為定位算法精度的評(píng)判指標(biāo)。

選取3.1節(jié)中的基站坐標(biāo)，E為定位點(diǎn)，坐標(biāo)為(0，n，0)，n從10 m處開(kāi)始，每隔1 m移動(dòng)一次，直到移動(dòng)到100 m處，在MATLAB中根據(jù)2種算法對(duì)其進(jìn)行仿真，得到2種算法的均方根誤差隨距離的變化曲線如圖8所示。由圖8可知，在10～100 m內(nèi)，間接法的均方根誤差控制在4.5 cm 以?xún)?nèi)，隨著距離的增大均方根誤差也在增大，而在混合算法中，均方根誤差達(dá)到毫米級(jí)以上，相比間接法，精度明顯更高。

圖8 均方根誤差隨距離變化曲線Fig.8 RMSE error curves with distance

為了探究三軸的定位精度隨定位距離的變化關(guān)系，在MATLAB中進(jìn)行仿真模擬，得到三軸的均方根誤差隨定位距離的變化曲線如圖9所示。由圖9可知，10～100 m內(nèi)，在間接法中，三軸均方根誤差隨測(cè)量距離增大而增大，Y軸的均方根誤差較小，在5 mm以下，X軸和Z軸次之，沒(méi)有明顯差異;在混合算法中，三軸的均方根誤差達(dá)到毫米級(jí)以下，X軸和Y軸均方根誤差較小。

圖9 三軸均方根誤差隨距離變化曲線Fig.9 Triaxial RMSE error curves with distance

圖10 掘進(jìn)機(jī)機(jī)身定位點(diǎn)布置Fig.10 Layout of the roadheader fuselage locating point

由2種算法的定位點(diǎn)均方根誤差曲線和三軸均方根誤差曲線可知，混合算法的均方根誤差可達(dá)到毫米級(jí)以上，精度明顯高于間接法。

3.3 姿態(tài)角精度分析

圖10為掘進(jìn)機(jī)機(jī)身定位點(diǎn)布置示意圖，3個(gè)定位點(diǎn)布置在同一水平面上，形成一個(gè)等腰三角形，掘進(jìn)機(jī)機(jī)身視為一個(gè)剛體。結(jié)合EBZ160懸臂式掘進(jìn)機(jī)的尺寸，圖中L1為定位點(diǎn)E到定位點(diǎn)F的距離，L2為定位點(diǎn)E到掘進(jìn)機(jī)中軸線的距離，定位點(diǎn)E到定位點(diǎn)G之間的距離為2 m，定位點(diǎn)F到L2之間的距離為2 m。

4個(gè)基站的坐標(biāo)與3.1節(jié)中相同，基站對(duì)機(jī)身的3個(gè)定位點(diǎn)E，F(xiàn)，G進(jìn)行測(cè)距，利用混合算法解算出定位點(diǎn)的坐標(biāo)，代入位姿解算公式[16]，可得到掘進(jìn)機(jī)的航向角α、俯仰角β、橫滾角γ。

(11)

分別選取15 m和90 m進(jìn)行位姿參數(shù)精度分析。在15 m中，E(1,13,0)，F(xiàn)(0,15,0)，G(-1,13,0)，90 m中，E(1,88,0)，F(xiàn)(0,90,0);G(-1,88,0)，可得到如圖11所示的位姿參數(shù)精度分布曲線。由圖可知，在15 m處，掘進(jìn)機(jī)的航向角、俯仰角、橫滾角的精度可達(dá)到0.005°左右，其中航向角精度最高;在90 m處，各姿態(tài)角的精度可控制在0.008°以下。隨著定位距離的增大，定位誤差也隨之增大，同樣，位姿誤差也隨著定位距離的增大而增大。

圖11 位姿解算精度曲線Fig.11 Accuracy curves of pose

4 實(shí)驗(yàn)驗(yàn)證

煤礦的巷道可近似為狹長(zhǎng)封閉空間，實(shí)驗(yàn)選擇在與巷道相似的狹長(zhǎng)封閉的樓道中進(jìn)行，二者在其空間變化上是相似的，重點(diǎn)研究該位姿檢測(cè)系統(tǒng)及其算法的精度，以及空間結(jié)構(gòu)對(duì)UWB模塊的影響。采用的UWB測(cè)距模塊是美國(guó)Time domain公司生產(chǎn)的P440模塊。準(zhǔn)備工作如下:

(1)在電腦上安裝P440測(cè)距軟件。

(2)將模塊1，2，3，4(端口ID:101，102，103，104)連接至PC端，運(yùn)行軟件并調(diào)至ALOHA組網(wǎng)模式，將4個(gè)模塊設(shè)置為基站。

(3)將模塊5，6，7(端口ID:105，106，107)連接至電腦，運(yùn)行軟件，將軟件調(diào)至ALOHA組網(wǎng)模式，將3個(gè)模塊設(shè)置為動(dòng)點(diǎn)，斷開(kāi)連接。

(4)將1，2，3，4模塊用數(shù)據(jù)線接入U(xiǎn)SB分線器，連接至PC端。

(5)放置三腳架，將模塊1，2，3，4分別固定在4個(gè)三角架的云臺(tái)上，擺放好位置，用重錘線標(biāo)定天線相位中心位置，便于確定坐標(biāo)位置，用激光測(cè)距儀測(cè)量4個(gè)模塊的三維坐標(biāo)，坐標(biāo)見(jiàn)表1。

表1 模塊坐標(biāo)Table 1 Coordinates of modules

(6)在前方放置一張桌子，模塊5，6，7安裝在手機(jī)支架上，將手機(jī)支架固定在桌子上的3個(gè)角，形成一個(gè)直角三角形，確保高度一致。模塊位置示意如圖12所示。

(7)給模塊接入移動(dòng)電源，確保模塊供電。

實(shí)驗(yàn)步驟:

(1)用激光測(cè)距儀測(cè)量模塊1，2，3，4的三維坐標(biāo)，以及模塊5，6，7到模塊1，2，3，4天線基準(zhǔn)點(diǎn)之間的距離并記錄。

(2)在電腦上運(yùn)行P440測(cè)距軟件，模塊1，2，3，4設(shè)置為廣播模式，時(shí)間間隔設(shè)置為100 ms。

(3)在logging窗口選擇保存數(shù)據(jù)的文件夾。

(4)返回測(cè)距窗口，點(diǎn)擊“send”按鈕啟動(dòng)模塊發(fā)送電波開(kāi)始測(cè)距，數(shù)據(jù)以Excel文件形式自動(dòng)保存在剛才選擇的文件夾中。

(5)將模塊5，6，7向前移動(dòng)，保持3個(gè)模塊之間的相對(duì)距離不動(dòng)，間隔一定距離再次重復(fù)以上步驟并記錄。實(shí)驗(yàn)實(shí)物如圖13所示。

圖13 定位實(shí)驗(yàn)實(shí)物Fig.13 Photograph of the experiment for positioning

在3～94 m內(nèi)，根據(jù)UWB模塊所測(cè)模塊5，6，7到模塊1，2，3，4的測(cè)距信息，利用混合算法計(jì)算模塊5，6，7的位置坐標(biāo)，相關(guān)數(shù)據(jù)見(jiàn)表2，3，4。

表2 模塊5測(cè)量數(shù)據(jù)Table 2 Measured data of module 5

表3 模塊6測(cè)量數(shù)據(jù)Table 3 Measured data of module 6

將定位模塊5，6，7的標(biāo)定坐標(biāo)與計(jì)算坐標(biāo)相減得到絕對(duì)誤差，對(duì)三軸誤差在 MATLAB中進(jìn)行擬合得到誤差隨距離變化的誤差曲線，如圖14所示。由圖14分析可知，基于混合算法在3～94 m的測(cè)量范圍內(nèi)，X軸和Y軸在30～50 m處出現(xiàn)了峰值，這是由于該位置樓道環(huán)境發(fā)生了變化，成為空曠非封閉環(huán)境(圖12)，導(dǎo)致UWB的傳播路徑發(fā)生變化，從而出現(xiàn)了多徑效應(yīng)[21]，因此誤差較大。其中Y軸精度較高，在測(cè)量范圍內(nèi)可達(dá)到毫米級(jí)，且不隨測(cè)量距離的增大而增大，X軸方向誤差基本控制在4 cm以?xún)?nèi)，Z軸方向誤差隨測(cè)量距離的增大而增大，在94 m附近誤差高達(dá)6.763 m。由此可知，在3～94 m測(cè)量范圍內(nèi)，X軸和Y軸可以滿(mǎn)足掘進(jìn)機(jī)的定位要求。

表4 模塊7測(cè)量數(shù)據(jù)Table 4 Measured data of module 7

圖14 模塊5，6，7的X,Y,Z軸誤差曲線Fig.14 X,Y,Z xial error curves of module 5,6 and 7

5 結(jié) 論

(1)基于UWB測(cè)距，根據(jù) TSOA定位原理，提出了一種面向掘進(jìn)機(jī)的混合算法的位姿檢測(cè)方法，建立了觀測(cè)方程組，系統(tǒng)地推導(dǎo)了混合算法的計(jì)算過(guò)程，將間接法計(jì)算得到的初始值代入Taylor級(jí)數(shù)展開(kāi)法，循環(huán)迭代，消除誤差，最終得到了掘進(jìn)機(jī)機(jī)身的三維坐標(biāo)。

(2)基于MATLAB對(duì)間接法和混合算法進(jìn)行了定位仿真實(shí)驗(yàn)，仿真對(duì)比了2種算法的定位點(diǎn)空間分布、三軸誤差、均方根誤差以及姿態(tài)角，由仿真結(jié)果可知:混合算法的精度優(yōu)于間接算法，在10～100 m的測(cè)量范圍內(nèi)，間接法的各軸精度可達(dá)到厘米級(jí)，Y軸精度較高，均方根誤差可控制在4.5 cm以下，而混合算法的各軸精度可以達(dá)到毫米級(jí)以上，均方根誤差在毫米級(jí)以上，姿態(tài)角的精度可以控制在0.008°以下。

(3)搭建了掘進(jìn)機(jī)位姿檢測(cè)實(shí)驗(yàn)平臺(tái)，開(kāi)展了模擬巷道的UWB測(cè)距實(shí)驗(yàn)，完整地采集了實(shí)驗(yàn)的相關(guān)數(shù)據(jù)，將所測(cè)數(shù)據(jù)代入混合算法得到掘進(jìn)機(jī)的三維坐標(biāo)，與標(biāo)定坐標(biāo)比較，得到了誤差隨距離變化的規(guī)律。實(shí)驗(yàn)表明:在3～94 m的測(cè)量范圍內(nèi)，X軸誤差基本控制在4 cm以?xún)?nèi)，Y軸誤差可達(dá)毫米級(jí)以上，Z軸誤差較大，隨測(cè)量距離的增大而增大。在三維空間中提高Z軸精度的方法值得繼續(xù)研究。