曹 鈺，高學(xué)澤

（酒泉職業(yè)技術(shù)學(xué)院新能源工程學(xué)院，甘肅 酒泉 735000）

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

慣性導(dǎo)航技術(shù)擁有抗干擾能力強(qiáng)的優(yōu)點(diǎn)，且隨著現(xiàn)代電子技術(shù)的發(fā)展，慣性元件的精度已有很大的提高，在小型旋翼機(jī)中已得到大量的應(yīng)用，能夠滿足礦井下車輛定位精度的需求。因而本文提出一種基于慣性導(dǎo)航技術(shù)和RFID 技術(shù)相結(jié)合的礦井下車輛定位系統(tǒng)，本系統(tǒng)在慣性導(dǎo)航技術(shù)的基礎(chǔ)上，利用RFID 技術(shù)把車輛的行駛路線進(jìn)行分段標(biāo)記，分段處的位置信息作為每段姿態(tài)解算的初始位置，以減少慣性元件的累計(jì)誤差，提高定位精度。

1 系統(tǒng)設(shè)計(jì)

系統(tǒng)由RFID 標(biāo)簽、車載慣性姿態(tài)解算單元、數(shù)據(jù)傳輸模塊和管理控制模塊等四部分構(gòu)成，其總體結(jié)構(gòu)如圖1 所示。

圖1 系統(tǒng)總體結(jié)構(gòu)圖

系統(tǒng)中選用遠(yuǎn)距離無源標(biāo)簽（以下簡(jiǎn)稱標(biāo)簽），在車輛行駛線路沿線每隔一定距離固定一個(gè)，該標(biāo)簽中含有固定處的位置信息，在車輛經(jīng)過時(shí)，讀取標(biāo)簽信息，作為姿態(tài)解算的初始位置信息，采用分段姿態(tài)解算的方法減小累計(jì)誤差，提高定位精度。RFID 閱讀器（以下簡(jiǎn)稱閱讀器）、慣性測(cè)量單元和無線發(fā)射器件組成姿態(tài)解算模塊。其中閱讀器用來讀取每段路程的初始位置信息，在解算單元得出位置信息后將其通過無線發(fā)射器件、串口服務(wù)器傳輸?shù)骄峦ㄐ啪€路中，最終將車輛運(yùn)行位置信息在控制中心進(jìn)行顯示。

2 系統(tǒng)硬件設(shè)計(jì)

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

3 軟件設(shè)計(jì)

整個(gè)系統(tǒng)軟件由節(jié)點(diǎn)部分和上位機(jī)部分組成，節(jié)點(diǎn)軟件主要實(shí)現(xiàn)車輛位置的實(shí)時(shí)解算和數(shù)據(jù)的傳輸，上位機(jī)部分則主要是顯示姿態(tài)參數(shù)與車輛運(yùn)動(dòng)軌跡。

3.1 姿態(tài)解算

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

3.1.1 井下車輛運(yùn)動(dòng)的四元數(shù)描述

對(duì)于井下車輛，設(shè)其本體坐標(biāo)系為O-XbYbZb，坐標(biāo)原點(diǎn)為姿態(tài)解算模塊的質(zhì)心，Xb軸與車輛的速度方向一致，Yb則沿車輛橫軸垂直于速度方向，Zb垂直向上；運(yùn)動(dòng)的參考坐標(biāo)跟導(dǎo)航坐標(biāo)系都為地理坐標(biāo)系O'-XnYnZn，其中坐標(biāo)原點(diǎn)跟O 重合，Xn、Yn、Zn分別指向東向，北向，天向。設(shè)初始時(shí)刻b 系和n系重合。

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

圖2 剛體旋轉(zhuǎn)示意圖

由文獻(xiàn)[3]可得，四元數(shù)可確定出由b 系到R 系（同時(shí)也為n 系）的坐標(biāo)變換矩陣（即姿態(tài)陣）為：

3.1.2 姿態(tài)解算

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

由姿態(tài)轉(zhuǎn)換矩陣，得到車輛的三軸姿態(tài)角為：

3.1.3 車輛位置解算

根據(jù)捷聯(lián)慣性導(dǎo)航中位置解算的模型可得礦井下車輛的速度的微分方程為[5]：

車輛位置的解算方程為：

式中：L、λ、h 分別是車輛位置的緯度、經(jīng)度和高度，vx、vy、vz則是車速在導(dǎo)航坐標(biāo)系各方向的分量，R 和RN則分別是當(dāng)?shù)氐淖游缛Π霃胶兔先Φ陌霃絒6]。

3.1.4 參數(shù)求解及數(shù)據(jù)融合

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

圖3 車輛定位原理圖示

3.2 實(shí)驗(yàn)及仿真驗(yàn)證

本文采用半實(shí)物仿真和軟件仿真相結(jié)合的方式來驗(yàn)證礦井下車輛運(yùn)動(dòng)過程中本定位系統(tǒng)的可行性和有效性。

3.2.1 對(duì)姿態(tài)解算和數(shù)據(jù)發(fā)送的實(shí)驗(yàn)驗(yàn)證

在驗(yàn)證實(shí)驗(yàn)中車輛模型選用飛思卡爾比賽中的模型車，該模型車的車體采用雙等臂式單懸架結(jié)構(gòu)，具有良好的機(jī)械性能，測(cè)試中所用硬件實(shí)物如圖4 所示。

圖4 硬件測(cè)試平臺(tái)

測(cè)試分靜態(tài)和動(dòng)態(tài)兩部分，測(cè)試中所采集的數(shù)據(jù)為模型的姿態(tài)參數(shù)，即俯仰角、滾轉(zhuǎn)角和偏航角。在靜態(tài)測(cè)試中，給定模型車的初始姿態(tài)為：俯仰角ξ=-0.6°，滾轉(zhuǎn)角γ=-0.75°偏航角ψ=4.94°；實(shí)際測(cè)試數(shù)據(jù)如圖5 所示。由計(jì)算可得，俯仰、滾轉(zhuǎn)和偏航的最大瞬時(shí)誤差均為0.01°。由此說明該系統(tǒng)中傳感器數(shù)據(jù)采集、姿態(tài)解算和數(shù)據(jù)發(fā)送均可靠、可行。

圖5 靜態(tài)數(shù)據(jù)測(cè)試

為測(cè)試車輛在運(yùn)動(dòng)情況下該系統(tǒng)核心模塊的可靠性，設(shè)置動(dòng)態(tài)測(cè)試部分。在測(cè)試中，模型平臺(tái)在普通瀝青路面上以0～1.0m/s 的速度運(yùn)動(dòng)，所采集到的姿態(tài)參數(shù)如圖6 所示。測(cè)試數(shù)據(jù)表明姿態(tài)解算模塊精度足夠，穩(wěn)定性好。

圖6 動(dòng)態(tài)姿態(tài)測(cè)試

3.2.2 Matlab 仿真驗(yàn)證

考慮到實(shí)際測(cè)試中平臺(tái)的局限性，采用軟件仿真作為補(bǔ)充，以進(jìn)一步說明該系統(tǒng)的可行性。在仿真中提供.xls 格式的運(yùn)動(dòng)數(shù)據(jù)文件。該文件包含運(yùn)動(dòng)時(shí)間，車輛在b 系中角增量和速度增量。在此數(shù)據(jù)基礎(chǔ)上，通過Matlab 解算出其對(duì)應(yīng)的運(yùn)行軌跡如圖7 所示。

圖7 車輛運(yùn)行軌跡仿真圖

4 結(jié)語

本文基于RFID 技術(shù)和慣性導(dǎo)航技術(shù)設(shè)計(jì)了一套礦井下車輛定位系統(tǒng)，利用慣性元件的抗干擾能力強(qiáng)的優(yōu)點(diǎn)，提高了定位系統(tǒng)的可靠性；同時(shí)通過RFID 技術(shù)進(jìn)行分段導(dǎo)航，減小系統(tǒng)的積累誤差。通過實(shí)驗(yàn)驗(yàn)證和仿真表明，系統(tǒng)具有良好的穩(wěn)定性和足夠的精度，能夠提高車輛運(yùn)行的安全性。