基于UWB和IMU的煤礦機器人緊組合定位方法研究

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

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

0 引言

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

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

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

1 緊組合定位方法

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

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

1.1 UWB測距信息修正

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

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

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

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

式中：η為正則化參數；ξi為隨機擬合誤差。

引入拉格朗日法來解決式（2）中的優化問題，構造函數為

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

對式（3）求偏導，可得

決策函數定義為

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

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

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

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

1.2 IMU定位原理

導航坐標系（N系）采取UWB所在坐標系，載體坐標系（B系）使用左前上坐標系。從B系轉換到N系需要通過旋轉矩陣

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

IMU定位原理的數學模型[13]可以表示為

式中：pk為k時刻位置；vk為k時刻速度；qk為k時刻姿態；ak為k時刻加速度；g為 重力加速度；?k為k時刻四元數更新矩陣。

1.3 UWB和IMU緊組合定位

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

1.3.1 UWB測距值修正

UWB測距值修正具體實現步驟如下：

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

（2）初始化LSSVM模型參數，并將18個真實值及其對應的18個實測值輸入LSSVM模型中進行訓練，得到LSSVM修正模型。

（3）將煤礦機器人運動過程中UWB測距實測值輸入訓練好的LSSVM修正模型中進行預測，得到較準確的UWB測距信息，用于ESKF數據融合。

1.3.2 數據融合

使用ESKF將UWB和IMU緊組合。這里設計煤礦機器人的位置誤差 δpN（三維）、目標點的速度誤差δvN（三維）、失準角φ（三維）、加速度零偏誤差δba（ 三維）和陀螺零偏誤差δbg（三維）為狀態向量，得到狀態方程:

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

由于IMU通常是高速率采樣數據，所以需要將連續時間系統方程轉換成相應的離散系統方程：

式中：ψk?1為狀態轉移矩陣，具體推導過程參考文獻[14]；sk?1為 k?1時刻的噪聲；I為單位矩陣。

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

式中：Xk為 k時刻更新后狀態；為k 時刻的先驗狀態估計值；K為卡爾曼增益；h為利用IMU獲取的煤礦機器人位置信息估算的煤礦機器人與UWB基站之間的距離，其輸入為慣性導航系統解算的煤礦機器人位置pk，輸出為煤礦機器人與基站的距離。

式中c為UWB基站的位置。

2 實驗分析

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

2.1 實驗器材及環境

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

圖2 實驗場景Fig.2 Experimental scenario

實驗中使用的軟件環境：Intel i5?8250處理器，Windows10操作系統，軟件為Matlab2018a，串口調試助手為ATK XCOM V2.0版本。

2.2 UWB定位靜態實驗分析

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

表1 UWB基站位置坐標Table 1 UWB base station layout coordinates m

實驗中，對搭載UWB/IMU標簽的煤礦機器人在同一位置與1個UWB基站之間的距離進行多次測量，取1 500次UWB測距結果進行仿真實驗。搭載標簽的煤礦機器人與UWB基站之間的真實歐氏距離為9.6 m，仿真結果如圖3所示。可看出經過LSSVM修正模型修正后的UWB測距值比實測值小，更接近煤礦機器人與UWB基站之間的真實距離，減小了UWB測距的非視距誤差。

圖3 UWB測距仿真結果Fig.3 UWB ranging simulation results

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

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

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

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

2.3 UWB/IMU緊組合定位動態實驗分析

動態實驗過程中，煤礦機器人沿L型路線行駛，使用Matlab對獲取的實驗數據進行定位仿真，實驗結果如圖5所示。可看出使用LSSVM修正模型對UWB測距值進行修正后的融合定位軌跡相較于未修正的融合定位軌跡更接近于實際軌跡，UWB測距修正后定位均方根誤差比修正前降低了4.5%；將LSSVM修正模型修正后的UWB測距信息用于緊組合定位解算，效果更優，更適用于煤礦機器人定位。

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

3 結論

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

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