陸 博，周俊武，趙建軍

（北京礦冶研究總院礦冶過程自動(dòng)控制技術(shù)北京市重點(diǎn)實(shí)驗(yàn)室，北京102628）

基于UWB系統(tǒng)的井下車輛高精度定位研究

陸 博，周俊武，趙建軍

（北京礦冶研究總院礦冶過程自動(dòng)控制技術(shù)北京市重點(diǎn)實(shí)驗(yàn)室，北京102628）

隨著無線通信技術(shù)的進(jìn)步，超寬帶（UWB）技術(shù)從縮減硬件成本及定位精度的提升都有了大幅的進(jìn)步，因此其在定位系統(tǒng)中的使用越來越多。通過研究適用于在井下巷道環(huán)境的高精度定位方法，研制出了一套適用于井下環(huán)境的超寬帶無線車輛定位系統(tǒng)。試驗(yàn)結(jié)果顯示，該系統(tǒng)定位精度高，對(duì)進(jìn)一步實(shí)現(xiàn)井下的車輛無人自主導(dǎo)航具有非常大的支撐作用。

井下定位；UWB測(cè)距；傳感器網(wǎng)絡(luò)；三邊定位法；車輛導(dǎo)航

礦山井下安全生產(chǎn)的重要性已經(jīng)毋庸置疑，井下安全監(jiān)測(cè)的手段也隨著技術(shù)發(fā)展而不斷前進(jìn)。一方面，我國(guó)近年來大力推進(jìn)井下安全生產(chǎn)的“六大系統(tǒng)”，另外一方面，我國(guó)也重點(diǎn)開發(fā)新一代智能化、自主化、無人化的新型井下裝備。因此，地下設(shè)備的精確定位和自主導(dǎo)航是開發(fā)智能化設(shè)備的必要條件之一。

金屬、非金屬礦山井下移動(dòng)裝備種類繁多，主要以井下各類無軌類車輛為主。這類車輛一般行駛速度較快，作業(yè)工作面經(jīng)常發(fā)生改變，實(shí)現(xiàn)其在巷道這類狹窄區(qū)域中的自主導(dǎo)航最關(guān)鍵是要解決車輛精確定位的問題。對(duì)于井下裝備精確定位目前有兩種研究方向：第一是獲取車輛與巷道內(nèi)障礙物的相對(duì)距離，通過避障及岔路決策等方法實(shí)現(xiàn)車輛無障礙行駛；第二是實(shí)時(shí)提供世界坐標(biāo)系下的絕對(duì)位置信息，通過預(yù)先設(shè)計(jì)好的電子地圖來完成車輛按規(guī)劃路徑自主行駛。無疑，第二種方法的研究難度更高，也更具有普適意義，本文的研究?jī)?nèi)容就是針對(duì)世界坐標(biāo)系下的實(shí)時(shí)定位而展開的［1］。

1 無線定位方法的研究

針對(duì)井下的特殊環(huán)境，傳統(tǒng)意義的室內(nèi)定位在井下往往都不適用，具體體現(xiàn)在三個(gè)方面：第一，井下無GPS信號(hào)；第二，狹長(zhǎng)封閉的巷道對(duì)各類信號(hào)的傳輸都是一個(gè)非常復(fù)雜的傳輸模型，多徑效應(yīng)對(duì)定位結(jié)果產(chǎn)生的干擾不容忽視；第三，井下特殊的環(huán)境（如潮濕、多塵、地磁場(chǎng)變化等）造成了很多種定位方法都不適用［2］。因此找到一種適合于井下的高精度實(shí)時(shí)無線定位系統(tǒng)是我們必須解決的問題。目前，基于非接觸式定位的方法有很多，按照其定位手段來分大致有三種：電磁波定位、光通訊定位和超聲波定位。每種定位方法在實(shí)際的井下環(huán)境中都存在著缺陷，綜合各方面的考慮，基于UWB方式的超寬帶電磁波定位技術(shù)很適合井下裝備的移動(dòng)精確定位，其具體特點(diǎn)有：

1）其定位精度及刷新速度較高。10cm以內(nèi)的定位精度及高刷新速度可以滿足車輛這類較快移動(dòng)的設(shè)備的實(shí)時(shí)定位的需求。

2）抗多徑效應(yīng)好，抗井下粉塵及潮濕環(huán)境好。

3）設(shè)備成本適中，施工難度較低。

根據(jù)這些特點(diǎn)，研制適合于井下車輛定位的UWB定位方法是完成井下定位的最核心的技術(shù)方法。

2 UWB測(cè)距技術(shù)

UWB技術(shù)的中文意義為超寬帶，是一種采用非載波技術(shù)，利用納秒級(jí)的脈沖通信，進(jìn)行的一種高速數(shù)據(jù)傳輸?shù)男滦屯ㄐ偶夹g(shù)。由于UWB系統(tǒng)脈沖持續(xù)的時(shí)間極短，具有很強(qiáng)的時(shí)間及空間分辨率，可以有效地對(duì)抗因多徑效應(yīng)而產(chǎn)生的多徑衰落，因此在無線定位領(lǐng)域有著獨(dú)特的優(yōu)勢(shì)。UWB的相關(guān)產(chǎn)品可以實(shí)現(xiàn)非常高的定位測(cè)距精度。采用無線方式研制的井下車輛精確定位系統(tǒng)一般由兩部分組成：一部分是固定在巷道壁上的固定的“錨節(jié)點(diǎn)”，這類節(jié)點(diǎn)在定位系統(tǒng)中會(huì)預(yù)先設(shè)置好絕對(duì)坐標(biāo)系下的物理坐標(biāo)，進(jìn)行定位計(jì)算；另外一部分是系統(tǒng)中的移動(dòng)節(jié)點(diǎn)，安裝在移動(dòng)設(shè)備之上，通過無線通訊的各種手段來確定動(dòng)節(jié)點(diǎn)的信息。

基于UWB系統(tǒng)的定位測(cè)距方法主要有信號(hào)角度測(cè)量法（AOA）、到達(dá)時(shí)間定位法（TOA）、時(shí)間差定位法（TDOA）。在這三種主要方法的原理基礎(chǔ)上，又拓展衍生了多種定位方法，本文的研究主要集中在基于TOA方法為基礎(chǔ)的擴(kuò)展TW-TOF方法。該方法是通過電磁波在介質(zhì)中來回傳播時(shí)間來計(jì)算定位距離的，如圖1所示。

當(dāng)發(fā)射點(diǎn)在T0時(shí)刻發(fā)射信號(hào)，接收端在T1時(shí)刻接收到信號(hào)，并在T2時(shí)刻返回信號(hào)，信號(hào)到達(dá)發(fā)射點(diǎn)的時(shí)間為T3。這樣，發(fā)射端與接收端的距離為：

式中：V0—電磁波在介質(zhì)中傳播的速度，一般計(jì)算時(shí)我們都以真空中光速c來代替。這樣，我們可以通過動(dòng)節(jié)點(diǎn)發(fā)射有規(guī)律的定位請(qǐng)求信號(hào)來計(jì)算與各個(gè)錨節(jié)點(diǎn)之間的距離。然后再通過定位算法來計(jì)算動(dòng)節(jié)點(diǎn)的位置信息［3］。

圖1 TW-TOF定位法示意圖Fig.1 Diagram of TW-TOF positioning method

3 定位算法研究

通過固定錨點(diǎn)定位的算法大致有三種類型：幾何定位法、迭代定位法和最優(yōu)遞推估計(jì)法。這三種方法在實(shí)際的定位系統(tǒng)搭建過程中互有優(yōu)劣，算法的選取主要與根據(jù)運(yùn)動(dòng)系統(tǒng)的慣性特性有關(guān)。針對(duì)井下車輛這種移動(dòng)速度較快、要求的刷新速度較高的定位系統(tǒng)而言，各種定位算法的混合使用會(huì)使系統(tǒng)測(cè)量誤差有效的降低。

3.1 位置估計(jì)算法

幾何定位法是測(cè)距定位的基本原理，所有定位算法都是位置估計(jì)算法，按照錨節(jié)點(diǎn)與動(dòng)節(jié)點(diǎn)的空間幾何關(guān)系，見公式：

式（1）是二維空間（平面）定位法，如果確定該方程有唯一解，那么i至少應(yīng)該大于3；而對(duì)應(yīng)的式（2）是三維空間定位法，確定一個(gè)三維位置則至少需要由四個(gè)錨節(jié)點(diǎn)來進(jìn)行定位。井下車輛一般都是在已知空間地圖拓展平面上行駛的，也就是說，我們定位系統(tǒng)導(dǎo)航的目標(biāo)可以延伸為一個(gè)平面上繪制的若干個(gè)航跡點(diǎn)（盡管這些航跡點(diǎn)有時(shí)并不在一個(gè)幾何平面上），因此，我們只針對(duì)二維平面的導(dǎo)航系統(tǒng)進(jìn)行研究。

假設(shè)我們的測(cè)距系統(tǒng)測(cè)量結(jié)果足夠精確，那么移動(dòng)物體M與空間固定的三個(gè)坐標(biāo)ABC之間的距離分別為A、B、C，由于固定的三個(gè)位置的空間坐標(biāo)是已知的，可以得出公式（3）［4-5］。

化簡(jiǎn)該方程組，得

圖2 三邊定位計(jì)算示意圖Fig.2 Diagram of trilateration positioning estimation

上述方程若有唯一的解析解，必須滿足如下條件：第一，行列式值不為零，也即三點(diǎn)不在同一直線上；第二，滿足如下等式Xa2＋Xb2＋Xc2＋Ya2＋Yb2＋Yc2-2（XaXb＋XaXc＋XbXc＋YaYb＋YaYc＋YbYc）＝2（A2＋B2＋C2）也即測(cè)距的ABC值的平方和的2倍與固定的三角形周長(zhǎng)相等。這樣我們可以獲得空間平面上的唯一解，即三邊交匯的點(diǎn)M坐標(biāo)。

簡(jiǎn)單地說，滿足上述條件是十分苛刻的，必須保證足夠的測(cè)量精度才可以有確定的、唯一的解析解，但實(shí)際情況往往并非如此。

或者是這兩種情況的計(jì)算方法都一樣，可以采用兩圓相交的內(nèi)側(cè)焦點(diǎn)的質(zhì)心作為定位的原點(diǎn)，估算出實(shí)際的M的位置。

圖3 幾何測(cè)距點(diǎn)過交情況Fig.3 Over crossing of the geometric distance estimation

圖4 幾何測(cè)距點(diǎn)欠交情況Fig.4 Lack crossing of the geometric distance estimation

這樣根據(jù)方程組（3），我們可以得到不計(jì)誤差的二維平面解：

通過上述計(jì)算，我們可以得到一個(gè)三錨點(diǎn)平面定位系統(tǒng)的位置估計(jì)值。這個(gè)定位值在視距緩動(dòng)的情況下，精度會(huì)比較高，常常被用作各種精確估計(jì)算法的初始值尋找。特別是井下車輛在慢速移動(dòng)情況下，各種估計(jì)算法收斂后的誤差已經(jīng)遠(yuǎn)遠(yuǎn)低于系統(tǒng)的白噪聲。因此，我們?cè)谶@些情況下均采用幾何定位法提供質(zhì)心坐標(biāo)，作為動(dòng)節(jié)點(diǎn)的最終位置估計(jì)［6］。

3.2 位置修正算法

由于多徑效應(yīng)和動(dòng)節(jié)點(diǎn)移動(dòng)速度的影響，在偶發(fā)的非視距（NLOS）傳播條件下，我們需要采用其他的位置估計(jì)算法來進(jìn)行位置修正，最常用的三種方法為加權(quán)最小二乘法（Weighted Least-Square，WLS）、最大似然估計(jì)法（MLE）、卡爾曼濾波器法（Kalman Filter）［7］。

加權(quán)最小二乘法是線性最小二乘法的延伸，其核心思想是將加權(quán)的誤差平方和最小化。對(duì)定位錨節(jié)點(diǎn)的加權(quán)權(quán)重選取目前采用的測(cè)距波動(dòng)及距離估算，當(dāng)動(dòng)節(jié)點(diǎn)的周圍存在＞3個(gè)錨節(jié)點(diǎn)的測(cè)量值時(shí)，我們根據(jù)如下公式計(jì)算第i個(gè)錨節(jié)點(diǎn)的定位權(quán)重值：

這里參數(shù)k為區(qū)間比例系數(shù)，它取決于巷道環(huán)境與錨節(jié)點(diǎn)密度，根據(jù)經(jīng)驗(yàn)關(guān)系，在直巷道取1，彎曲巷道取0.5。然后將計(jì)算出權(quán)重值集合Ci進(jìn)行降序排序，選取三個(gè)權(quán)重較大的作為加權(quán)錨點(diǎn)。然后再進(jìn)行回歸計(jì)算。求線性最小二乘的方法是采用高斯-牛頓法迭代，利用泰勒級(jí)數(shù)展開來替代非線性回歸模型。

上述為位置描述方程，根據(jù)高斯牛頓迭代法，k＋1級(jí)的收斂結(jié)果如下：

對(duì)于一般意義而言，高斯迭代具有普遍的快速收斂特性，因此對(duì)于現(xiàn)代計(jì)算機(jī)技術(shù)來講具有很快的執(zhí)行效率，但是，其也有一些缺點(diǎn)，主要有：

1）需要有一個(gè)初始的種子進(jìn)行迭代計(jì)算，如果選取不當(dāng)，收斂速度和效果會(huì)大打折扣。

2）由于其是非線性的描述，在某些情況下收斂的精度會(huì)比較差。

3）迭代次數(shù)的不確定導(dǎo)致算法運(yùn)算速度不確定。

因此，我們?cè)趫?zhí)行加權(quán)最小二乘法時(shí)，一般是先設(shè)定一個(gè)收斂次數(shù)，對(duì)于我們研究的UWB系統(tǒng)，我們的收斂次數(shù)k設(shè)為4，這樣既可以保證精度，又不影響運(yùn)算處理速度。在車輛控制的過程中，當(dāng)車輛處于可預(yù)測(cè)的單一方向且速度加速度均勻變化時(shí)，利用該方法估計(jì)的定位結(jié)果與實(shí)際值的偏差非常小。另外，預(yù)測(cè)結(jié)果的閾值與車輛本身固有運(yùn)動(dòng)特性的慣性模型有關(guān)，這里受限于篇幅我們不做展開。

極大似然估計(jì)的精度取決于錨節(jié)點(diǎn)的個(gè)數(shù)，考慮到井下系統(tǒng)錨節(jié)點(diǎn)分布會(huì)比較稀疏，且視距（LOS）情況下的節(jié)點(diǎn)有限，因此極大似然估計(jì)法在井下定位系統(tǒng)中并不實(shí)用［8］。

正如上文所述，當(dāng)車輛處于頻繁的加減速及連續(xù)的彎道行駛時(shí)，非線性的描述會(huì)大大影響迭代過程的收斂速度，固定收斂次數(shù)的定位精度會(huì)受到較大的影響。因此，我們采用搭建最優(yōu)濾波器的方法來解決車輛在處于連續(xù)變向等慣性特性復(fù)雜描述等情況。

當(dāng)處理的系統(tǒng)是線性微分系統(tǒng)時(shí)，且過程和測(cè)量都是高斯白噪聲時(shí)，卡爾曼濾波算法是最優(yōu)的信息處理器［9］。這種技術(shù)把一個(gè)簡(jiǎn)單的目標(biāo)運(yùn)動(dòng)模型和測(cè)距以及參照位置相結(jié)合來產(chǎn)生一種根據(jù)觀察和模型估算把加速度誤差和測(cè)距誤差適當(dāng)混合的最優(yōu)算法。由于歐幾里得距離公式的非線性特質(zhì)，卡爾曼濾波器需要在此基礎(chǔ)上進(jìn)行擴(kuò)展?？柭鼮V波器的一般形式為：

上述預(yù)測(cè)公式的變量更新規(guī)則為：

這種方法有很多優(yōu)點(diǎn)，主要有：

a）遞歸性質(zhì)使其可以使用之前的結(jié)果從而暗中消除了偶然的外圍測(cè)距誤差。

b）它允許把測(cè)量后單個(gè)的測(cè)距結(jié)果引入計(jì)算，不需要分組和三邊。

c）當(dāng)無法測(cè)距時(shí)，運(yùn)動(dòng)模型可以更新估算解，從而擴(kuò)大了覆蓋范圍，或需要較少的參照節(jié)點(diǎn)［10-11］。

d）其他定位傳感器可以和超寬帶測(cè)距進(jìn)行優(yōu)化組合，從而提供最終的導(dǎo)航解決方案。

通過上述兩種位置修正的估計(jì)方法，本文找到了一種適用于井下特殊環(huán)境的車輛位置定位方法，即根據(jù)車輛運(yùn)行狀態(tài)，通過組合方式來估計(jì)車輛位置的修正值，從而使定位系統(tǒng)應(yīng)用于車輛導(dǎo)航。

4 系統(tǒng)精度驗(yàn)證

我們?cè)趯?shí)驗(yàn)室通過小車在已知路徑的滑動(dòng)導(dǎo)軌上運(yùn)動(dòng)來模擬井下車輛運(yùn)動(dòng)情況。測(cè)試系統(tǒng)是利用敷設(shè)在地面上的標(biāo)尺，通過車載攝像頭的機(jī)器視覺信息來高速獲取車輛實(shí)時(shí)軌跡，進(jìn)而與定位系統(tǒng)的結(jié)果實(shí)時(shí)比較而進(jìn)行精度驗(yàn)證［12］。

在實(shí)驗(yàn)過程中，我們分別針對(duì)平巷、平坡、斜巷等各種井下環(huán)境進(jìn)行勻速、加速、高速等模擬實(shí)驗(yàn)，實(shí)驗(yàn)的結(jié)論見表1。

其中，表1中的移動(dòng)速度是根據(jù)定位系統(tǒng)定位結(jié)果推算得出的；平均偏差的意義為UWB系統(tǒng)定位點(diǎn)與視覺系統(tǒng)定位點(diǎn)距離的算術(shù)平均值，上述測(cè)量結(jié)果的測(cè)量間隔為100ms，每組測(cè)量樣本數(shù)量不少于300個(gè)。從試驗(yàn)結(jié)果來看，總體定位精度已經(jīng)可以達(dá)到預(yù)期目標(biāo)，滿足井下車輛的需要。

表1 UWB系統(tǒng)定位精度驗(yàn)證表Table 1 The positioning accuracy of UWB system testing table

圖5 實(shí)驗(yàn)室模擬測(cè)試平臺(tái)Fig.5 The laboratory simulation test platform

5 結(jié)論

本文重點(diǎn)研究了無線定位系統(tǒng)在井下環(huán)境的高精度定位，研制了一套適用于井下車輛的位置定位系統(tǒng)。綜合實(shí)驗(yàn)室試驗(yàn)和場(chǎng)地車輛試驗(yàn)的定位數(shù)據(jù)與車輛控制的預(yù)期軌跡來看，當(dāng)試驗(yàn)用鏟運(yùn)機(jī)以小于5km/h的時(shí)速在模擬井下的復(fù)雜環(huán)境中試驗(yàn)時(shí)，其在定位信號(hào)完全覆蓋的區(qū)域定位標(biāo)準(zhǔn)差小于5cm；最大定位偏差小于17cm；連續(xù)10次測(cè)量的定位軌跡最大測(cè)量偏差（垂直于移動(dòng)方向）均小于10cm。從目前的試驗(yàn)情況來看，該系統(tǒng)具有定位精度高、實(shí)時(shí)性好、抗多徑效應(yīng)能力強(qiáng)等特點(diǎn)，非常適用于井下車輛這種慢速、連續(xù)對(duì)精度要求高的封閉環(huán)境。這種車輛定位方法達(dá)到了課題預(yù)期設(shè)計(jì)目標(biāo)，具有較高的經(jīng)濟(jì)價(jià)值及市場(chǎng)應(yīng)用前景。

［1］Hongpeng Chi，Kai Zhan，Boqiang Shi.Automatic guidance of underground mining vehicles using laser sensors［J］.Tunnelling and Underground Space Technology，2012，27：142-148.

［2］Abdellah Chehri，Paul Fortier，Pierre Martin Tardif.UWB-based sensor networks for localization in mining environments［J］.Ad Hoc Networks，2009（7）：987-1000.

［3］葛文濤，陳俊杰.基于三邊定位的WSN錨節(jié)點(diǎn)加權(quán)補(bǔ)償算法［J］.測(cè)控技術(shù)，2011，29（9）：92-95.

［4］尚俊龍，胡建華，鄧紅衛(wèi).基于加權(quán)三邊測(cè)算的井下人員精確定位研究［J］.中國(guó)安全科學(xué)學(xué)報(bào)，2011，21（2）：126-130.

［5］田明明，朱宗玖，張 蕊.低功耗煤礦井下人員定位系統(tǒng)［J］.煤礦機(jī)械，2012，33（4）：238-240.

［6］Thuraiappah Sathyan，David Humphrey，Mark Hedley.WASP-A System and Algorithms for Accurate Radio Localization Using Low-Cost Hardware［C］//IEEE TRANSACTIONS ON SYSTEMS，MAN，AND CYBERNETICS，2011，41（2）：211-222.

［7］劉向舉，劉麗娜.基于物聯(lián)網(wǎng)井下跟蹤定位系統(tǒng)［J］.長(zhǎng)春工業(yè)大學(xué)學(xué)報(bào)：自然科學(xué)版，2012，33（1）：82-86.

［8］周小艷，何為，胡國(guó)輝.基于ZigBee無線傳感器網(wǎng)絡(luò)的變電站人員定位的改進(jìn)算法研究［J］.電力系統(tǒng)保護(hù)與控制，2013，41（17）：56-62.

［9］Wang Ke，Wang Qianping，Jiang Dong，et al.A routing and positioning algorithm based on a K-barrier for use in an underground wireless sensor network［J］.Mining Science and Technology（China），2011，21：773-779.

［10］樊家琛，吳曉莉，李宇翔，等.基于三頻數(shù)據(jù)的北斗衛(wèi)星導(dǎo)航系統(tǒng)DCB參數(shù)精度評(píng)估方法［J］.中國(guó)空間科學(xué)技術(shù)，2013（4）：62-70.

［11］武瑾媛，房建成，楊照華.基于擴(kuò)維卡爾曼濾波的火星探測(cè)器脈沖星相對(duì)導(dǎo)航方法［J］.儀器儀表學(xué)報(bào)，2013，34（8）：1711-1716.

［12］Baiden G，Bissiri Y，Luoma S，et al.Mapping utility infrastructure via underground GPS positioning with autonomous telerobotics［J］.Tunnelling and Underground Space Technology，2013（3）：1-9.

Study on high precision positioning system of underground vehicle based on UWB system

LU Bo，ZHOU Junwu，ZHAO Jianjun
（Beijing Key Laboratory of Automation of Mining ＆Metallurgy Process，Beijing General Research Institute of Mining ＆Metallurgy，Beijing 102628，China）

Depending on the development of wireless communication technology，Ultra Wide Band（UWB）technology has been significantly progressed in the reduction of hardware cost and the improvement of the positioning accuracy，so it is increasingly used in positioning systems.The research in high precision positioning method of underground tunnel is undertaken.A wireless vehicle positioning system is developed based on ultra wideband technology.The experimental results show that the system positioning localization is very accurate，which will take a large supporting role to realize the autonomous navigation in underground unmanned vehicle.

underground localization；UWB ranging；sensor network；trilateration positioning estimation；vehicle navigation

TD529

Α

1671-4172（2015）06-0057-05

國(guó)家高技術(shù)研究發(fā)展計(jì)劃 （863 計(jì)劃）項(xiàng)目（2011AA060408）

陸 博（1981-），男，高級(jí)工程師，碩士，自動(dòng)化儀表專業(yè)，主要從事礦冶自動(dòng)化、信息化領(lǐng)域的研究。

10.3969/j.issn.1671-4172.2015.06.012