郁露，唐超禮，黃友銳，韓濤，徐善永，付家豪

（安徽理工大學(xué) 電氣與信息工程學(xué)院，安徽 淮南 232001）

0 引言

煤礦井下工作環(huán)境惡劣，對井下工作人員的人身安全造成極大威脅。煤礦無人開采或者少人開采日漸成為研究熱點(diǎn)，引入煤礦機(jī)器人代替井下工作人員完成危險(xiǎn)、繁重的井下工作，將為解決煤礦安全生產(chǎn)問題發(fā)揮重要作用[1]。

煤礦機(jī)器人可以應(yīng)用于掘進(jìn)、運(yùn)輸及救援等方面，精準(zhǔn)定位是其智能化實(shí)現(xiàn)的基礎(chǔ)。為解決煤礦機(jī)器人的精準(zhǔn)定位問題，目前常見的定位方法包括基于射頻識(shí)別定位技術(shù)、基于超聲波定位技術(shù)、基于超寬帶（Ultra Wide Band,UWB）定位技術(shù)、基于慣導(dǎo)定位技術(shù)等方法。張曉莉等[2]提出使用擴(kuò)展卡爾曼濾波對射頻識(shí)別信息和慣導(dǎo)解算信息進(jìn)行融合的方法，實(shí)現(xiàn)了煤礦機(jī)器人實(shí)時(shí)高精度的定位。譚玉新等[3]提出了基于無損卡爾曼濾波的超聲網(wǎng)絡(luò)定位算法，使用無損卡爾曼濾波將超聲網(wǎng)絡(luò)定位和電子羅盤及光電碼盤定位所得到的航向角度信息及位置坐標(biāo)信息進(jìn)行組合，達(dá)到了降低定位誤差的目的。陳美蓉等[4]提出了一種基于超寬帶的煤礦井下定位混合解算方法，使用頭腦風(fēng)暴優(yōu)化和泰勒級數(shù)展開的混合定位方法對人?機(jī)?物位置進(jìn)行解算，解決了使用泰勒級數(shù)展開需要較好初值的問題。馬宏偉等[5]提出了基于捷聯(lián)慣導(dǎo)和里程計(jì)融合的煤礦機(jī)器人定位方法，首先通過卡爾曼濾波校準(zhǔn)捷聯(lián)慣導(dǎo)，再將捷聯(lián)慣導(dǎo)解算出的煤礦機(jī)器人位置信息與里程計(jì)解算的位置信息通過自適應(yīng)卡爾曼濾波進(jìn)行校正，得到組合定位結(jié)果，定位精度較高。楊金衡等[6]提出了基于自適應(yīng)卡爾曼濾波的雙慣導(dǎo)定位方法，使用2套慣導(dǎo)系統(tǒng)所獲得的加速度信息及角速度信息建立了雙慣導(dǎo)模型。但由于井下環(huán)境復(fù)雜，使得許多定位技術(shù)在井下使用受限。射頻技術(shù)定位不能對煤礦機(jī)器人進(jìn)行實(shí)時(shí)跟蹤，超聲波技術(shù)定位會(huì)受到多普勒效應(yīng)等影響，定位精度低，且成本高。UWB作為一種新興技術(shù)，有較高的時(shí)間分辨率[7]，將其應(yīng)用在室內(nèi)環(huán)境定位中，相比于其他定位技術(shù)有更高的穩(wěn)定性及定位精度。但UWB技術(shù)在煤礦機(jī)器人定位中單獨(dú)使用時(shí)，受UWB基站布置、多徑效應(yīng)、非視距誤差等對定位精度的影響，其定位結(jié)果具有一定的波動(dòng)性。慣性測量單元（Inertial Measurement Unit，IMU）工作時(shí)，環(huán)境因素對其干擾較小，更新速率高，在短時(shí)間內(nèi)定位精度高，故可適應(yīng)較為復(fù)雜的井下環(huán)境。但單獨(dú)使用IMU定位的主要缺點(diǎn)是其定位誤差會(huì)隨著時(shí)間累計(jì)，導(dǎo)致長時(shí)間定位精度較低。為實(shí)現(xiàn)煤礦機(jī)器人精準(zhǔn)定位，火元亨[8]將UWB測距信息與IMU定位信息通過擴(kuò)展卡爾曼濾波進(jìn)行松組合定位，定位精度有一定提高，但UWB測距過程中由于非視距誤差等因素的影響，使定位結(jié)果仍存在一定的誤差。

為了減小UWB測距的非視距誤差，實(shí)現(xiàn)煤礦機(jī)器人的準(zhǔn)確定位，本文提出了一種基于UWB和IMU的煤礦機(jī)器人緊組合定位方法。引入最小二乘支持向量機(jī)（Least Square Support Vector Machine，LSSVM）對UWB測距信息進(jìn)行修正，以減小井下定位的非視距誤差；利用誤差狀態(tài)卡爾曼濾波（Error-State Kalman Filter,ESKF）將修正后的UWB測距信息及通過慣性導(dǎo)航解算出的距離信息緊組合，更新煤礦機(jī)器人的位置信息，實(shí)現(xiàn)精確定位。使用UWB模塊及IMU獲得的煤礦機(jī)器人的實(shí)驗(yàn)數(shù)據(jù)，在Matlab中對緊組合定位方法進(jìn)行靜態(tài)和動(dòng)態(tài)定位仿真實(shí)驗(yàn)，結(jié)果證明了該方法的可靠性。

1 緊組合定位方法

基于UWB和IMU的煤礦機(jī)器人緊組合定位方法原理如圖1所示。利用UWB測距模塊得到煤礦機(jī)器人與UWB基站之間的距離實(shí)測值，使用煤礦機(jī)器人與UWB基站之間的距離真實(shí)值和實(shí)測值訓(xùn)練LSSVM模型，得到LSSVM修正模型，并對煤礦機(jī)器人定位過程中UWB測距信息進(jìn)行修正；利用IMU采集煤礦機(jī)器人的加速度及角速度信息[9]，使用慣性導(dǎo)航解算出煤礦機(jī)器人的狀態(tài)信息。將經(jīng)過LSSVM修正后的測距信息作為ESKF的量測輸入，與慣性導(dǎo)航解算出的位置信息構(gòu)成其量測方程，完成狀態(tài)更新，得到更為精確的煤礦機(jī)器人位置信息，實(shí)現(xiàn)對煤礦機(jī)器人的精確定位。

圖1 基于UWB和IMU的煤礦機(jī)器人緊組合定位方法原理Fig.1 Principle of tightly combined positioning method of coal mine robot based on UWB and IMU

1.1 UWB測距信息修正

UWB測距信息是非視距環(huán)境下所采集得到的[10]，嚴(yán)重的遮擋等因素導(dǎo)致UWB信號不能夠直接傳遞，這將使信號在介質(zhì)中傳播的時(shí)間加長，測距信息不準(zhǔn)確，最終導(dǎo)致定位效果較差。為了減小由環(huán)境帶來的定位誤差，需要對非視距環(huán)境下的UWB測距值進(jìn)行處理。LSSVM模型適用于非線性估計(jì)，當(dāng)樣本數(shù)據(jù)較小時(shí)，也有很好的性能。本文使用LSSVM模型對測距誤差進(jìn)行修正。

LSSVM模型回歸原理[11]可以描述如下。設(shè)有訓(xùn)練集{(xi,yi)}li=1， xi為UWB測距模塊獲得的實(shí)測值，作為LSSVM模型的輸入值，yi為煤礦機(jī)器人與UWB基站之間的距離真實(shí)值，作為LSSVM模型的期望輸出值，l為訓(xùn)練集樣本數(shù)量。將樣本中的數(shù)據(jù)映射到高維空間，可以得到回歸方程：

式中：w 為權(quán)向量；?(xi)為建立的輸入數(shù)據(jù)和高維特征空間的映射，將非線性的樣本變成線性可分的數(shù)據(jù)[12]；b為偏差。

LSSVM模型將SVM優(yōu)化問題的非等式約束用等式約束替換。具有等式約束的LSSVM模型可以描述為

式中：η為正則化參數(shù)；ξi為隨機(jī)擬合誤差。

引入拉格朗日法來解決式（2）中的優(yōu)化問題，構(gòu)造函數(shù)為

式中：L 為拉格朗日函數(shù)；αi為拉格朗日乘子。

對式（3）求偏導(dǎo)，可得

決策函數(shù)定義為

式中：yj為UWB測距信息修正值；H為核函數(shù)，xj為煤礦機(jī)器人定位過程中UWB模塊獲得的實(shí)測值。

LSSVM模型選擇高斯徑向基函數(shù)（Radial Basis Function，RBF）作為核函數(shù)，核函數(shù)寬度為σ。

選擇UWB測距實(shí)測值d′作為LSSVM模型的輸入，選擇UWB基站與煤礦機(jī)器人之間的真實(shí)值d作為輸出，訓(xùn)練LSSVM模型，得到LSSVM修正模型。

將煤礦機(jī)器人在定位過程中UWB模塊測得的實(shí)測值作為LSSVM修正模型的輸入，通過LSSVM修正模型對UWB實(shí)測值進(jìn)行修正，得到較為準(zhǔn)確的距離信息。

1.2 IMU定位原理

導(dǎo)航坐標(biāo)系（N系）采取UWB所在坐標(biāo)系，載體坐標(biāo)系（B系）使用左前上坐標(biāo)系。從B系轉(zhuǎn)換到N系需要通過旋轉(zhuǎn)矩陣

式中：θ為航向角；θk為 k 時(shí)刻的航向角；Ts為采樣周期；為k?1時(shí)刻在N系下的角速度。

IMU定位原理的數(shù)學(xué)模型[13]可以表示為

式中：pk為k時(shí)刻位置；vk為k時(shí)刻速度；qk為k時(shí)刻姿態(tài)；ak為k時(shí)刻加速度；g為 重力加速度；?k為k時(shí)刻四元數(shù)更新矩陣。

1.3 UWB和IMU緊組合定位

煤礦機(jī)器人搭載的UWB/IMU標(biāo)簽中內(nèi)置IMU芯片，可以輸出煤礦機(jī)器人的三軸加速度和三軸角速度信息；UWB模塊可以測得煤礦機(jī)器人距離每個(gè)UWB基站的歐氏距離。緊組合將UWB和IMU作為一個(gè)測量傳感器，利用IMU獲取的煤礦機(jī)器人位置信息估算煤礦機(jī)器人與UWB基站之間的距離，與UWB測得的煤礦機(jī)器人與UWB基站之間的距離信息組合。UWB和IMU緊組合定位方法主要由UWB測距值修正部分及數(shù)據(jù)融合部分組成。

1.3.1 UWB測距值修正

UWB測距值修正具體實(shí)現(xiàn)步驟如下：

（1）在實(shí)驗(yàn)場景中，使用UWB測距模塊對不同真實(shí)距離（1，2，…，18 m）測量300次并取其平均值作為不同真實(shí)距離的實(shí)測值。

（2）初始化LSSVM模型參數(shù)，并將18個(gè)真實(shí)值及其對應(yīng)的18個(gè)實(shí)測值輸入LSSVM模型中進(jìn)行訓(xùn)練，得到LSSVM修正模型。

（3）將煤礦機(jī)器人運(yùn)動(dòng)過程中UWB測距實(shí)測值輸入訓(xùn)練好的LSSVM修正模型中進(jìn)行預(yù)測，得到較準(zhǔn)確的UWB測距信息，用于ESKF數(shù)據(jù)融合。

1.3.2 數(shù)據(jù)融合

使用ESKF將UWB和IMU緊組合。這里設(shè)計(jì)煤礦機(jī)器人的位置誤差 δpN（三維）、目標(biāo)點(diǎn)的速度誤差δvN（三維）、失準(zhǔn)角φ（三維）、加速度零偏誤差δba（ 三維）和陀螺零偏誤差δbg（三維）為狀態(tài)向量，得到狀態(tài)方程:

式中：δX=[δpNδvNφ δbgδba]；F（t）為動(dòng)態(tài)矩陣；G（t）為噪聲輸入系數(shù)矩陣[13]； s(t)為噪聲。

由于IMU通常是高速率采樣數(shù)據(jù)，所以需要將連續(xù)時(shí)間系統(tǒng)方程轉(zhuǎn)換成相應(yīng)的離散系統(tǒng)方程：

式中：ψk?1為狀態(tài)轉(zhuǎn)移矩陣，具體推導(dǎo)過程參考文獻(xiàn)[14]；sk?1為 k?1時(shí)刻的噪聲；I為單位矩陣。

利用IMU獲取的煤礦機(jī)器人位置信息估算煤礦機(jī)器人與UWB基站之間的距離，與UWB測距信息的差值構(gòu)造量測方程。ESKF的量測輸入為經(jīng)過LSSVM模型修正后的煤礦機(jī)器人距離每一個(gè)UWB基站的UWB測距修正值dk，卡爾曼狀態(tài)更新方程[15-17]為

式中：Xk為 k時(shí)刻更新后狀態(tài)；為k 時(shí)刻的先驗(yàn)狀態(tài)估計(jì)值；K為卡爾曼增益；h為利用IMU獲取的煤礦機(jī)器人位置信息估算的煤礦機(jī)器人與UWB基站之間的距離，其輸入為慣性導(dǎo)航系統(tǒng)解算的煤礦機(jī)器人位置pk，輸出為煤礦機(jī)器人與基站的距離。

式中c為UWB基站的位置。

2 實(shí)驗(yàn)分析

為了驗(yàn)證基于UWB和IMU的煤礦機(jī)器人緊組合定位方法的定位精度，在模擬井下巷道中進(jìn)行了實(shí)驗(yàn)。在實(shí)驗(yàn)中，首先對基站的不同布置方式進(jìn)行靜態(tài)定位實(shí)驗(yàn)，分析UWB基站所在位置對定位精度的影響，確定基站布置的最佳方案；然后使用LSSVM修正模型修正前后的UWB測距信息分別進(jìn)行UWB和IMU緊組合定位實(shí)驗(yàn)，比較2種方式的定位誤差；最后進(jìn)行UWB和IMU緊組合定位動(dòng)態(tài)實(shí)驗(yàn)，比較經(jīng)過LSSVM修正模型修正前后融合的煤礦機(jī)器人運(yùn)動(dòng)軌跡。

2.1 實(shí)驗(yàn)器材及環(huán)境

本次實(shí)驗(yàn)所涉及的實(shí)驗(yàn)器材包括4個(gè)LD150型UWB基站、1個(gè)LD150?I型UWB/IMU標(biāo)簽（其中IMU型號為ICM?426005）、1臺(tái)便攜式計(jì)算機(jī)、1臺(tái)Turtlebot2機(jī)器人、若干數(shù)據(jù)線及1個(gè)三腳架等。其中UWB/IMU標(biāo)簽采樣間隔為10 ms。實(shí)驗(yàn)場景模擬井下巷道，如圖2所示。4個(gè)UWB基站固定在墻上，UWB/IMU標(biāo)簽固定在煤礦機(jī)器人上。

圖2 實(shí)驗(yàn)場景Fig.2 Experimental scenario

實(shí)驗(yàn)中使用的軟件環(huán)境：Intel i5?8250處理器，Windows10操作系統(tǒng)，軟件為Matlab2018a，串口調(diào)試助手為ATK XCOM V2.0版本。

2.2 UWB定位靜態(tài)實(shí)驗(yàn)分析

針對煤礦機(jī)器人定位的實(shí)際情況，實(shí)驗(yàn)設(shè)置了3種UWB基站布置方案。方案1：4個(gè)基站等高對稱分布[18]；方案2：將方案1中基站2和基站3的高度移動(dòng)至1 m；方案3：將方案1中基站2和基站3無規(guī)律放置。使用均方根誤差作為測試指標(biāo)，分析不同布置方案及使用LSSVM修正模型對測距數(shù)據(jù)進(jìn)行處理對定位精度的影響。不同UWB基站位置坐標(biāo)見表1。

表1 UWB基站位置坐標(biāo)Table 1 UWB base station layout coordinates m

實(shí)驗(yàn)中，對搭載UWB/IMU標(biāo)簽的煤礦機(jī)器人在同一位置與1個(gè)UWB基站之間的距離進(jìn)行多次測量，取1 500次UWB測距結(jié)果進(jìn)行仿真實(shí)驗(yàn)。搭載標(biāo)簽的煤礦機(jī)器人與UWB基站之間的真實(shí)歐氏距離為9.6 m，仿真結(jié)果如圖3所示?？煽闯鼋?jīng)過LSSVM修正模型修正后的UWB測距值比實(shí)測值小，更接近煤礦機(jī)器人與UWB基站之間的真實(shí)距離，減小了UWB測距的非視距誤差。

圖3 UWB測距仿真結(jié)果Fig.3 UWB ranging simulation results

對LSSVM修正模型處理前后的數(shù)據(jù)進(jìn)行靜態(tài)定位仿真實(shí)驗(yàn)。實(shí)驗(yàn)中搭載UWB/IMU標(biāo)簽的煤礦機(jī)器人的真實(shí)位置為（?1.8 m，7.2 m）。在表1中的3種方案中分別進(jìn)行定位解算，3種基站布置方案的定位結(jié)果如圖4所示。經(jīng)過計(jì)算實(shí)際位置與使用UWB信息解算位置之間的均方根誤差，得到3種基站布置方案的計(jì)算結(jié)果，見表2。

根據(jù)圖4和表2可看出：當(dāng)UWB基站等高對稱布置時(shí)，定位結(jié)果更接近真實(shí)位置，其均方根誤差最小，定位精度最高；當(dāng)搭載UWB/IMU標(biāo)簽的煤礦機(jī)器人在基站布置的范圍外（方案3），其定位精度最低。對比LSSVM修正模型修正前后的定位結(jié)果，可看出經(jīng)過LSSVM修正模型對UWB測距信息修正后，在3種基站布置方案下，UWB和IMU緊組合定位均方根誤差均有所減小。

表2 3種基站布置方案的實(shí)驗(yàn)均方根誤差Table 2 Experimental root mean squareerror of three basestations layout schemes m

圖4 3種基站布置方案的定位結(jié)果Fig.4 Positioning results of threebase stationslayout schemes

2.3 UWB/IMU緊組合定位動(dòng)態(tài)實(shí)驗(yàn)分析

動(dòng)態(tài)實(shí)驗(yàn)過程中，煤礦機(jī)器人沿L型路線行駛，使用Matlab對獲取的實(shí)驗(yàn)數(shù)據(jù)進(jìn)行定位仿真，實(shí)驗(yàn)結(jié)果如圖5所示?？煽闯鍪褂肔SSVM修正模型對UWB測距值進(jìn)行修正后的融合定位軌跡相較于未修正的融合定位軌跡更接近于實(shí)際軌跡，UWB測距修正后定位均方根誤差比修正前降低了4.5%；將LSSVM修正模型修正后的UWB測距信息用于緊組合定位解算，效果更優(yōu)，更適用于煤礦機(jī)器人定位。

圖5 UWB/IMU緊組合定位結(jié)果Fig.5 UWB/IMU tightly combined positioning results

3 結(jié)論

（1）提出了一種基于UWB和IMU的煤礦機(jī)器人緊組合定位方法，使用LSSVM修正模型對所測得的UWB測距信息進(jìn)行修正，減小了非視距誤差對UWB測距的影響；使用ESKF對UWB和IMU緊組合，達(dá)到了提高定位精度的目的。

（2）研究了UWB基站不同布置方案下，經(jīng)過LSSVM修正模型處理的UWB測距信息對煤礦機(jī)器人定位精度的影響。實(shí)驗(yàn)結(jié)果表明：當(dāng)UWB基站在應(yīng)用場景中等高對稱布置時(shí)，相比于其他布置方式，其定位均方根誤差較小，故在實(shí)際場景中，應(yīng)使UWB基站盡可能等高對稱布置；對利用LSSVM修正模型修正前后的UWB測距信息分別進(jìn)行UWB和IMU緊組合實(shí)驗(yàn)，結(jié)果表明：經(jīng)過LSSVM修正模型修正后的組合定位均方根誤差有所減小，測距信息修正后融合定位軌跡相較于未修正的融合定位軌跡更接近煤礦機(jī)器人運(yùn)動(dòng)的真實(shí)軌跡。LSSVM修正模型對UWB測距信息修正后組合定位精度更高，更適用于煤礦機(jī)器人定位。