基于TSOA定位原理混合算法的掘進機位姿檢測方法

劉 超，符世琛，成 龍，劉 丹，沈 陽，吳 淼

(中國礦業大學(北京) 機電與信息工程學院，北京 100083)

智能化開采成為煤炭安全高效開采的發展方向與必然趨勢[1]。無人采掘裝備是當前國際煤炭領域的發展前沿[2]。在地下巷道掘進過程中，傳統掘進方法效率低，精度有限，而且井下礦工長期處于高溫、高濕、高濃度粉塵環境，健康嚴重受損，井下環境復雜，事故頻發，增加了礦工的工作強度及危險性，因此我國亟待研制無人采掘裝備[3]。要實現綜掘工作面的無人化，綜掘裝備自主導控是其關鍵，掘進機的位姿檢測方法是實現綜掘裝備自主導控的核心，具有重要的研究意義。

智能化開采成為煤炭安全高效開采的發展方向與必然趨勢[1]。無人采掘裝備是當前國際煤炭領域的發展前沿[2]。在地下巷道掘進過程中，傳統掘進方法效率低，精度有限，而且井下礦工長期處于高溫、高濕、高濃度粉塵環境，健康嚴重受損，井下環境復雜，事故頻發，增加了礦工的工作強度及危險性，因此我國亟待研制無人采掘裝備[3]。要實現綜掘工作面的無人化，綜掘裝備自主導控是其關鍵，掘進機的位姿檢測方法是實現綜掘裝備自主導控的核心，具有重要的研究意義。

自20世紀90年代以來，國外主要產煤大國多以露天礦為主，在井下實際運用掘進機位姿檢測技術的報道較少，多數技術集中在盾構掘進機中[4]。日本GYRO系統[5]使用陀螺儀檢測盾構掘進機的姿態角，環境溫度的變化對陀螺儀的機械有較大影響，該系統實用性不佳，只能作為人工測量的輔助參考。德國艾柯夫公司[6]的掘進機具有方向控制和自動成型以及對故障監控的功能，但位姿檢測精度不高，受環境影響較大，在礦井中未見成熟應用案例。

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

筆者基于UWB測距，根據TSOA定位原理，提出了一種面向掘進機的混合算法的位姿檢測方法，建立了觀測方程組，詳細地推導了混合算法的計算過程;基于MATLAB仿真對比了間接法和混合算法的定位點空間分布、三軸誤差及均方根誤差，分析了混合算法的姿態角精度，在模擬巷道進行了實驗驗證，代入混合算法得到三維坐標，與標定值進行對比，得到了誤差隨距離變化的規律。

1 面向掘進機的超寬帶位姿檢測系統

1.1 UWB定位技術

UWB測距[19]是利用納秒甚至皮秒級的極窄脈沖來實現信息的傳輸，UWB信號的相對帶寬必須大于0.2，絕對帶寬大于500 MHz，具有恒定的波達時間分辨率，可解算出準確的測距信息。它具有抗干擾能力強、多徑分辨能力強、系統容量大、傳輸速率高、安全性高、成本低和功耗低等優點。

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

圖1 UWB定位結構Fig.1 Ultra-wideband positioning structure

1.2 TSOA定位原理

測距和定位(Time Summation of Arrival，TSOA)原理[20]是基于波達時間和，通過信號同步網絡實現在時域、頻域、空域上的嚴格同步，各站將測得的目標數據通過數據傳遞網進行傳遞，發射站只發射信號，接收站通過接收待測目標反射的發射站信號，測量從發射站到待測目標以及待測目標到接收站的距離和。在一個特定空間，保持與兩個指定基準點距離和相等的點的軌跡是以接收站、發射站為焦點的橢球面，該面稱為橢圓球位置面。在測量沒有誤差的情況下，UWB測距模塊一旦得到接收站和發射站的距離和P，那么待測目標一定在由P所確定的橢圓球位置面上，其定位原理如圖2所示。

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

(1)

式中，

2 基于間接法與Taylor級數展開法的混合算法

位置坐標的解算方法有很多種，但是無論采用哪種方法均需求解非線性方程。方程(1)就是非線性方程組，然而在工程實現中，求解非線性方程并不是一件容易的事，同時，UWB測距存在一定的誤差，測距誤差的引入會導致橢圓球位置面不相交于一點或者根本沒有交點。綜上所述，對掘進機機身的定位問題就由原來的非線性方程精確求解問題轉換成非線性優化的最優估計問題。

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

2.1 間接法

在間接法中，首先把rA看成一個已知量，從而解得XE,YE,ZE是rA的線性函數，求出機身定位點E的坐標估計值，具體過程如下:

通過對方程組(1)進行移項，平方，整理化簡，得

(2)

式中,

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

AX=b

(3)

式中

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

X=(ATA)-1ATb

(4)

可得機身定位點E的坐標估計值為XE,YE,ZE。

2.2 混合定位算法

Taylor級數展開法是一種基于Taylor級數展開的加權最小二乘估計迭代算法，其核心思想為:首先在目標位置的初始估計點利用Taylor級數展開，并忽略二次及以上項，將非線性方程變為線性方程，采用最小二乘法對三軸偏移量進行估計;然后利用估計的偏移量對初始坐標進行修正，不斷迭代，使估計的目標位置逼近真實位置，從而得到對目標位置的最優估計，混合算法程序流程圖如圖3所示。算法步驟主要如下:

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

(5)

(2)將式(1)在(XE,YE,ZE)點用Taylor級數展開，并忽略二次及以上項，則有

(6)

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

Αδ=Ζ

(7)

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

可得到δ的最小二乘解為

δ=(ATA)-1ATZ

(8)

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

(9)

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

(10)

3 混合算法精度分析

3.1 定位點分布

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

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

圖4 15 m定位點分布Fig.4 15 m positioning point distribution

對15 m這個定位點進行定位精度分析，在MATLAB中利用2種算法測量100次，得到如圖5所示的三軸誤差仿真曲線。在間接法中，最大誤差在1 cm以內，Y軸誤差較小，X軸、Z軸次之;在混合算法中，三軸的誤差在毫米級以上，X軸誤差較小，Y軸、Z軸無較大差異，該算法精度高于間接法。

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

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

圖6 90 m定位點分布Fig.6 90 m positioning point distribution

在90 m，用2種算法在MATLAB中對定位點測量100次，與真值比較，得到如圖7所示的三軸誤差曲線。由圖7可知，間接法的誤差基本維持在4 cm以內，Y軸誤差最小，X軸、Z軸誤差無明顯差異，而混合算法三軸的誤差達毫米級以上，Y軸誤差最小，混合算法的精度有較大提高。

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

3.2 均方根誤差分析

在定位過程中，無論何種定位方法，由于受測量設備和傳播環境的影響，測量的定位參量都會存在誤差，從而導致定位誤差的存在，并且采用不同的定位算法，定位誤差的大小也不一樣。均方根誤差[23]是評價定位精度的一個十分重要的指標，均方根誤差越小，定位精度越高，因此此處采用均方根誤差作為定位算法精度的評判指標。

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

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

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

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

圖10 掘進機機身定位點布置Fig.10 Layout of the roadheader fuselage locating point

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

3.3 姿態角精度分析

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

4個基站的坐標與3.1節中相同，基站對機身的3個定位點E，F，G進行測距，利用混合算法解算出定位點的坐標，代入位姿解算公式[16]，可得到掘進機的航向角α、俯仰角β、橫滾角γ。

(11)

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

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

4 實驗驗證

煤礦的巷道可近似為狹長封閉空間，實驗選擇在與巷道相似的狹長封閉的樓道中進行，二者在其空間變化上是相似的，重點研究該位姿檢測系統及其算法的精度，以及空間結構對UWB模塊的影響。采用的UWB測距模塊是美國Time domain公司生產的P440模塊。準備工作如下:

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

(2)將模塊1，2，3，4(端口ID:101，102，103，104)連接至PC端，運行軟件并調至ALOHA組網模式，將4個模塊設置為基站。

(3)將模塊5，6，7(端口ID:105，106，107)連接至電腦，運行軟件，將軟件調至ALOHA組網模式，將3個模塊設置為動點，斷開連接。

(4)將1，2，3，4模塊用數據線接入USB分線器，連接至PC端。

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

表1 模塊坐標Table 1 Coordinates of modules

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

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

實驗步驟:

(1)用激光測距儀測量模塊1，2，3，4的三維坐標，以及模塊5，6，7到模塊1，2，3，4天線基準點之間的距離并記錄。

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

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

(4)返回測距窗口，點擊“send”按鈕啟動模塊發送電波開始測距，數據以Excel文件形式自動保存在剛才選擇的文件夾中。

(5)將模塊5，6，7向前移動，保持3個模塊之間的相對距離不動，間隔一定距離再次重復以上步驟并記錄。實驗實物如圖13所示。

圖13 定位實驗實物Fig.13 Photograph of the experiment for positioning

在3～94 m內，根據UWB模塊所測模塊5，6，7到模塊1，2，3，4的測距信息，利用混合算法計算模塊5，6，7的位置坐標，相關數據見表2，3，4。

表2 模塊5測量數據Table 2 Measured data of module 5

表3 模塊6測量數據Table 3 Measured data of module 6

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

表4 模塊7測量數據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 結 論

(1)基于UWB測距，根據 TSOA定位原理，提出了一種面向掘進機的混合算法的位姿檢測方法，建立了觀測方程組，系統地推導了混合算法的計算過程，將間接法計算得到的初始值代入Taylor級數展開法，循環迭代，消除誤差，最終得到了掘進機機身的三維坐標。

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

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