基于RFID 和慣性導航技術的礦井下車輛定位系統的設計*

曹 鈺，高學澤

（酒泉職業技術學院新能源工程學院，甘肅 酒泉 735000）

目前已有的礦井下車輛位置檢測系統中，通過WiFi 技術或RFID 技術定位的方法只能得到車輛的大致位置[1]；而采用ZigBee 技術定位的系統則存在無線電信號易受環境干擾的不足[2]，不能夠可靠地得到車輛運行狀態的信息。

慣性導航技術擁有抗干擾能力強的優點，且隨著現代電子技術的發展，慣性元件的精度已有很大的提高，在小型旋翼機中已得到大量的應用，能夠滿足礦井下車輛定位精度的需求。因而本文提出一種基于慣性導航技術和RFID 技術相結合的礦井下車輛定位系統，本系統在慣性導航技術的基礎上，利用RFID 技術把車輛的行駛路線進行分段標記，分段處的位置信息作為每段姿態解算的初始位置，以減少慣性元件的累計誤差，提高定位精度。

1 系統設計

系統由RFID 標簽、車載慣性姿態解算單元、數據傳輸模塊和管理控制模塊等四部分構成，其總體結構如圖1 所示。

圖1 系統總體結構圖

系統中選用遠距離無源標簽（以下簡稱標簽），在車輛行駛線路沿線每隔一定距離固定一個，該標簽中含有固定處的位置信息，在車輛經過時，讀取標簽信息，作為姿態解算的初始位置信息，采用分段姿態解算的方法減小累計誤差，提高定位精度。RFID 閱讀器（以下簡稱閱讀器）、慣性測量單元和無線發射器件組成姿態解算模塊。其中閱讀器用來讀取每段路程的初始位置信息，在解算單元得出位置信息后將其通過無線發射器件、串口服務器傳輸到井下通信線路中，最終將車輛運行位置信息在控制中心進行顯示。

2 系統硬件設計

為降低系統的維護工作量和提高其可靠性選用無源電子標簽，同時為避免在讀取標簽信息時對車輛速度造成影響，選用工作頻率為13.56MHz 的Mifare S50 射頻卡。主控板的MCU 采用STM32103F，該單片機是32 位低功耗處理器，慣性元件則采用同時集成三軸陀螺儀和三軸加速度計的MPU6050，無線發射功能由GPRS 模塊來實現。

3 軟件設計

整個系統軟件由節點部分和上位機部分組成，節點軟件主要實現車輛位置的實時解算和數據的傳輸，上位機部分則主要是顯示姿態參數與車輛運動軌跡。

3.1 姿態解算

常用的慣性導航解算方法有歐拉角法、方向余弦法和四元數法。其中歐拉角法雖然概念直觀，容易理解，但是其存在鎖定的弊端，因此適用范圍受到限制；方向余弦法則需要對含有9 個位置變量的姿態矩陣微分方程進行求解，這就產生實時計算困難的弊端；而對于四元數法，只需要求解4 個未知量的線性微分方程組，計算量不大，且算法簡單實用。所以本文采用四元數法對井下車輛進行姿態解算[2]。

3.1.1 井下車輛運動的四元數描述

對于井下車輛，設其本體坐標系為O-XbYbZb，坐標原點為姿態解算模塊的質心，Xb軸與車輛的速度方向一致，Yb則沿車輛橫軸垂直于速度方向，Zb垂直向上；運動的參考坐標跟導航坐標系都為地理坐標系O'-XnYnZn，其中坐標原點跟O 重合，Xn、Yn、Zn分別指向東向，北向，天向。設初始時刻b 系和n系重合。

如圖2 所示，在三維空間中，剛體繞定軸旋轉角度后到的位置，其中，描述該旋轉的四元數為。

圖2 剛體旋轉示意圖

由文獻[3]可得，四元數可確定出由b 系到R 系（同時也為n 系）的坐標變換矩陣（即姿態陣）為：

3.1.2 姿態解算

四元數法中的參數包含了運載體完整的姿態信息，設運載體的航向角為ψ，俯仰角為ξ，滾轉角為γ，則從b 系到n 系的姿態轉換矩陣為[4]：

由姿態轉換矩陣，得到車輛的三軸姿態角為：

3.1.3 車輛位置解算

根據捷聯慣性導航中位置解算的模型可得礦井下車輛的速度的微分方程為[5]：

車輛位置的解算方程為：

式中：L、λ、h 分別是車輛位置的緯度、經度和高度，vx、vy、vz則是車速在導航坐標系各方向的分量，R 和RN則分別是當地的子午圈半徑和卯酉圈的半徑[6]。

3.1.4 參數求解及數據融合

四元數中的參數q0、q1、q2和q3通過一階龍格庫塔法來求解，其中所需的物理量為這個計算周期中的角速度gx、gy、gz以及周期時間T。其中角速度值由陀螺儀采集得到，但由于陀螺儀的積分作用，會增大角速度的偏差值，導致電路飽和，從而無法得到正確的信號；為此，通過加速度傳感器獲得的角度信息來進行校正。整個姿態解算流程圖如3 所示。

圖3 車輛定位原理圖示

3.2 實驗及仿真驗證

本文采用半實物仿真和軟件仿真相結合的方式來驗證礦井下車輛運動過程中本定位系統的可行性和有效性。

3.2.1 對姿態解算和數據發送的實驗驗證

在驗證實驗中車輛模型選用飛思卡爾比賽中的模型車，該模型車的車體采用雙等臂式單懸架結構，具有良好的機械性能，測試中所用硬件實物如圖4 所示。

圖4 硬件測試平臺

測試分靜態和動態兩部分，測試中所采集的數據為模型的姿態參數，即俯仰角、滾轉角和偏航角。在靜態測試中，給定模型車的初始姿態為：俯仰角ξ=-0.6°，滾轉角γ=-0.75°偏航角ψ=4.94°；實際測試數據如圖5 所示。由計算可得，俯仰、滾轉和偏航的最大瞬時誤差均為0.01°。由此說明該系統中傳感器數據采集、姿態解算和數據發送均可靠、可行。

圖5 靜態數據測試

為測試車輛在運動情況下該系統核心模塊的可靠性，設置動態測試部分。在測試中，模型平臺在普通瀝青路面上以0～1.0m/s 的速度運動，所采集到的姿態參數如圖6 所示。測試數據表明姿態解算模塊精度足夠，穩定性好。

圖6 動態姿態測試

3.2.2 Matlab 仿真驗證

考慮到實際測試中平臺的局限性，采用軟件仿真作為補充，以進一步說明該系統的可行性。在仿真中提供.xls 格式的運動數據文件。該文件包含運動時間，車輛在b 系中角增量和速度增量。在此數據基礎上，通過Matlab 解算出其對應的運行軌跡如圖7 所示。

圖7 車輛運行軌跡仿真圖

4 結語

本文基于RFID 技術和慣性導航技術設計了一套礦井下車輛定位系統，利用慣性元件的抗干擾能力強的優點，提高了定位系統的可靠性；同時通過RFID 技術進行分段導航，減小系統的積累誤差。通過實驗驗證和仿真表明，系統具有良好的穩定性和足夠的精度，能夠提高車輛運行的安全性。