采煤機(jī)捷聯(lián)慣導(dǎo)定位方法研究

張金堯，　李威，　楊海，　司卓印，　應(yīng)葆華

(中國(guó)礦業(yè)大學(xué) 機(jī)電工程學(xué)院， 江蘇 徐州　221116)

?

采煤機(jī)捷聯(lián)慣導(dǎo)定位方法研究

張金堯，李威，楊海，司卓印，應(yīng)葆華

(中國(guó)礦業(yè)大學(xué) 機(jī)電工程學(xué)院， 江蘇 徐州221116)

摘要：針對(duì)綜采工作面采煤機(jī)定位精度較低的問題，提出了一種基于Rodrigues參數(shù)法的采煤機(jī)捷聯(lián)慣導(dǎo)定位方法。該方法運(yùn)用捷聯(lián)慣導(dǎo)的三軸加速度計(jì)和三軸陀螺儀輸出采煤機(jī)的加速度和角速度信息，采用無冗余度的Rodrigues參數(shù)法對(duì)其進(jìn)行位姿解算。仿真結(jié)果表明，在采煤機(jī)斜切進(jìn)刀過程中，采煤機(jī)最大定位誤差為0.291 4 m，姿態(tài)誤差最大值為0.602 2 °，滿足采煤機(jī)的定位精度要求。

關(guān)鍵詞：采煤機(jī)定位； 捷聯(lián)慣導(dǎo)； Rodrigues參數(shù)法

0引言

煤炭是我國(guó)的主要能源，其需求量高，但事故頻發(fā)，實(shí)現(xiàn)綜采工作面的無人化和少人化是提高煤炭開采效率和安全性的必要保障。其中，實(shí)現(xiàn)綜采工作面“三機(jī)”自動(dòng)化、智能化是關(guān)鍵技術(shù)之一，而采煤機(jī)定位技術(shù)又是首先需要解決的問題[1]。現(xiàn)有的采煤機(jī)定位技術(shù)有齒輪計(jì)數(shù)法、紅外對(duì)射法以及超聲波反射法和無線傳感器網(wǎng)絡(luò)法等[2]。齒輪計(jì)數(shù)法只能確定沿工作面方向行走的距離，而無法確定垂直于工作面的位移，且存在累積誤差。紅外對(duì)射法是把紅外接收裝置安裝在液壓支架上，根據(jù)采煤機(jī)發(fā)來的廣角信號(hào)的強(qiáng)度來判斷采煤機(jī)的位置，其只能定性，不能定量。超聲波反射法和無線傳感器網(wǎng)絡(luò)法都存在信號(hào)在井下衰減太快的問題，具體實(shí)施困難。上述4種方法定位精度都較低，且易受環(huán)境影響。捷聯(lián)慣導(dǎo)直接安裝在采煤機(jī)上，不需要接收外部信息，直接采集采煤機(jī)的三軸加速度和三軸角速度信息，通過解算算法求解采煤機(jī)的速度、位置和姿態(tài)[3]。捷聯(lián)慣導(dǎo)不依賴于外部環(huán)境，適用于煤礦井下的復(fù)雜環(huán)境。

傳統(tǒng)的捷聯(lián)慣導(dǎo)解算算法有歐拉角法、方向余弦矩陣法和四元數(shù)法[4]。歐拉角法涉及到大量的三角運(yùn)算，計(jì)算量大，不適用于實(shí)時(shí)性要求高的場(chǎng)合。方向余弦矩陣法的約束條件比較多，其計(jì)算效率難以保證。而四元數(shù)法存在一個(gè)冗余約束，為非最小實(shí)現(xiàn)。經(jīng)典Rodrigues參數(shù)法由于存在等效旋轉(zhuǎn)角為180 °時(shí)出現(xiàn)奇異值的現(xiàn)象，故不適用于飛行器、航天器、船舶等的姿態(tài)解算。但采煤機(jī)在運(yùn)行時(shí)，其航向角、俯仰角、橫滾角均遠(yuǎn)小于180 °，不會(huì)出現(xiàn)奇異值的問題，且Rodrigues參數(shù)法計(jì)算更為簡(jiǎn)單，沒有冗余度，占用內(nèi)存和儲(chǔ)存空間少，具有較好的實(shí)用價(jià)值。故本文采用Rodrigues參數(shù)法進(jìn)行采煤機(jī)姿態(tài)解算。

1采煤機(jī)姿態(tài)解算模型

1.1采煤機(jī)的Rodrigues參數(shù)描述

剛體繞單位矢量a旋轉(zhuǎn)α角，則定義Φ為描述其轉(zhuǎn)動(dòng)的Rodrigues參數(shù)[4]：

(1)

對(duì)于采煤機(jī)，導(dǎo)航坐標(biāo)系選取地理坐標(biāo)系O-XnYnZn，其中Xn，Yn，Zn三軸分別指向東向、北向、天向。載體坐標(biāo)系O′-XbYbZb原點(diǎn)位于采煤機(jī)的質(zhì)心，Xb軸指向采煤機(jī)的前進(jìn)方向，Yb軸指向采煤機(jī)的垂直推進(jìn)方向，Zb軸指向上，如圖1所示。

圖1　采煤機(jī)定位坐標(biāo)系

設(shè)采煤機(jī)的姿態(tài)角為航向角ψ、俯仰角θ、橫滾角γ。初始時(shí)刻，載體坐標(biāo)系與導(dǎo)航坐標(biāo)系重合，經(jīng)3次旋轉(zhuǎn)后，分別繞Z軸旋轉(zhuǎn)ψ，繞X軸旋轉(zhuǎn)γ，繞Y軸旋轉(zhuǎn)θ，如圖2所示。

載體坐標(biāo)系繞3軸旋轉(zhuǎn)的Rodrigues參數(shù)為

(2)

將3次旋轉(zhuǎn)的等效旋轉(zhuǎn)矢量合成，形成1次旋

圖2　采煤機(jī)姿態(tài)角

轉(zhuǎn)的Rodrigues參數(shù)：

(3)

式中：φ1，φ2，φ3分別為Φ的3個(gè)元素。

設(shè)采煤機(jī)的角速度為ω，則其剛體姿態(tài)運(yùn)動(dòng)學(xué)微分方程的Rodrigues參數(shù)描述為

(4)

根據(jù)Rodrigues參數(shù)微分方程，對(duì)其積分，實(shí)現(xiàn)其姿態(tài)更新。設(shè)t時(shí)刻采煤機(jī)的姿態(tài)為Φ(t)，則t+h時(shí)刻的姿態(tài)為

(5)

式中：p為[t,t+h]時(shí)段內(nèi)捷聯(lián)慣導(dǎo)的輸出角增量，p為p的模；*表示Rodrigues參數(shù)乘法。

1.2采煤機(jī)姿態(tài)解算

Rodrigues參數(shù)Φ包含采煤機(jī)的3個(gè)姿態(tài)角信息，根據(jù)Φ得出采煤機(jī)載體坐標(biāo)系(b系)到導(dǎo)航坐標(biāo)系(n系)之間的姿態(tài)轉(zhuǎn)換矩陣Cnb為[5]

(6)

式中φ=ΦTΦ，為Φ模的平方。

由采煤機(jī)的姿態(tài)轉(zhuǎn)換矩陣Cnb，得到采煤機(jī)的三軸姿態(tài)角：

(7)

1.3采煤機(jī)位置解算

根據(jù)捷聯(lián)慣導(dǎo)解算模型[6]，得到采煤機(jī)速度更新的微分方程為

(8)

采煤機(jī)位置更新的微分方程為

(9)

式中：L，λ，h分別為采煤機(jī)所在位置的緯度、經(jīng)度和高度；vx，vy，vz分別為采煤機(jī)在導(dǎo)航坐標(biāo)系下各方向的速度分量；RM，RN分別為當(dāng)?shù)刈游缛Π霃胶兔先Π霃健?/p>

對(duì)式( 9) 積分即可得到采煤機(jī)所在位置的緯度、經(jīng)度和高度。

2采煤機(jī)斜切進(jìn)刀仿真

2.1采煤機(jī)斜切軌跡描述

采煤機(jī)割煤時(shí)通常采用斜切進(jìn)刀法，斜切進(jìn)刀法又分為端部斜切式進(jìn)刀法和中部斜切式進(jìn)刀法，本文以中部斜切式進(jìn)刀法為例進(jìn)行仿真。整個(gè)仿真過程分為空載和割煤2個(gè)階段。設(shè)空載行走速度為1 m/s，割煤行走速度為0.5 m/s；空載直行，在1 s內(nèi)，速度從0加速到1 m/s，勻速行走6 s；從第7 s開始減速，準(zhǔn)備斜切進(jìn)刀割煤，8 s時(shí)速度降為0.5 m/s，開始斜切進(jìn)刀沿x方向前進(jìn)3 m，y方向前進(jìn)0.6 m，14 s后直行割煤。斜切進(jìn)刀時(shí)航向角為11.309 9 °。

由于捷聯(lián)慣導(dǎo)在高度通道具有發(fā)散性，所以仿真時(shí)根據(jù)采煤機(jī)的運(yùn)動(dòng)特性，限定采煤機(jī)在高度方向的速度最大值為0.05 m/s。

2.2仿真結(jié)果與分析

在仿真過程中模擬加速度計(jì)和陀螺儀的數(shù)據(jù)輸出，并疊加上相應(yīng)的噪聲，采樣時(shí)間為0.01 s。圖3和圖4 分別為采煤機(jī)的捷聯(lián)慣導(dǎo)定位軌跡和定位誤差。從圖3可看到，采煤機(jī)捷聯(lián)慣導(dǎo)定位軌跡能夠較好地跟蹤基準(zhǔn)軌跡。從圖4可知，x，y，z方向最大定位誤差分別為0.250 8，0.041 7，0.015 4 m，總體定位誤差最大值為0.291 4 m。

圖3　采煤機(jī)捷聯(lián)慣導(dǎo)定位軌跡

(a) x方向位置誤差

(b) y方向位置誤差

(c) z方向位置誤差

采煤機(jī)姿態(tài)角誤差如圖5所示。從圖5可看出，采煤機(jī)姿態(tài)角在航向角、俯仰角、橫滾角3個(gè)方向的誤差最大值分別為0.602 2，0.031 9，0.154 2°，采煤機(jī)的姿態(tài)角具有很穩(wěn)定的定位精度。綜合采煤機(jī)的位置誤差和姿態(tài)角誤差可知，基于Rodrigues算法的采煤機(jī)捷聯(lián)慣導(dǎo)定位能夠滿足井下采煤機(jī)定位精度要求。

(a) 航向角誤差

(b) 俯仰角誤差

(c) 橫滾角誤差

3結(jié)語

針對(duì)井下采煤機(jī)自主定位的問題，提出了一種基于Rodrigues算法的采煤機(jī)捷聯(lián)慣導(dǎo)定位方法。該方法利用Rodrigues參數(shù)法對(duì)采煤機(jī)斜切進(jìn)刀工作過程進(jìn)行定位仿真，其仿真軌跡能夠較好地跟蹤基準(zhǔn)軌跡，姿態(tài)角誤差最大值為0.602 2 °，位置定位誤差最大值為0.291 4 m，定位精度能夠滿足井下采煤機(jī)定位要求。

參考文獻(xiàn)：

[1]張世洪. 我國(guó)綜采采煤機(jī)技術(shù)的創(chuàng)新研究[J].煤炭學(xué)報(bào),2010,35(11):1898-1902.

[2]楊海, 李威, 羅成名, 等. 基于捷聯(lián)慣導(dǎo)的采煤機(jī)定位定姿技術(shù)實(shí)驗(yàn)研究[J]. 煤炭學(xué)報(bào), 2014,39(12): 2550-2556.

[3]樊啟高, 李威, 王禹橋, 等. 一種采用捷聯(lián)慣導(dǎo)的采煤機(jī)動(dòng)態(tài)定位方法[J].煤炭學(xué)報(bào),2011,36(10): 1758-1761.

[4]周江華, 苗育紅, 王明海. 姿態(tài)運(yùn)動(dòng)的Rodrigues參數(shù)描述[J]. 宇航學(xué)報(bào), 2004,25(5): 514-519.

[5]王勇軍, 秦永元, 楊波. 四元數(shù)、Rodrigues參數(shù)在衛(wèi)星姿態(tài)解算上的對(duì)比研究[J]. 中國(guó)空間科學(xué)技術(shù), 2007,27(3): 18-23.

[6]秦永元. 慣性導(dǎo)航[M]. 北京: 科學(xué)出版社, 2014.

Study of shearer positioning method using strapdown inertial navigation system

ZHANG Jinyao，LI Wei，YANG Hai，SI Zhuoyin，YING Baohua

(School of Mechatronic Engineering, China University of Mining and Technology, Xuzhou 221116, China)

Abstract：For problem that shearer on fully mechanized coal face has lower positioning accuracy, a shearer positioning method using strapdown inertial navigation system based on Rodrigues parametric method was proposed. The tri-axial accelerometer and suspended gyroscope in the SINS were used to output information of acceleration and angular velocity of the shearer, and Rodrigues parametric method with no-redundancy was applied to calculate the position and attitude data. The simulation result shows that in the process of beveling feed, the maximum positioning error is 0.291 4 m and the maximum error of attitude angle is 0.602 2 °, it meets the requirements of positioning accuracy of shearer.

Key words:shearer positioning; strapdown inertial navigation system; Rodrigues parametric method

中圖分類號(hào)：TD421.6

文獻(xiàn)標(biāo)志碼：A網(wǎng)絡(luò)出版時(shí)間：2016-03-07 15:20

作者簡(jiǎn)介：張金堯(1992-)，男，四川廣元人，碩士研究生，研究方向?yàn)椴擅簷C(jī)定位和慣性導(dǎo)航，E-mail：502300523@qq.com。通信作者：李威(1964-)，男,江蘇徐州人，教授，博士研究生導(dǎo)師，研究方向?yàn)闄C(jī)電系統(tǒng)智能控制及檢測(cè)應(yīng)用，E-mail：cmeecumt512@yahoo.com。

基金項(xiàng)目：國(guó)家高技術(shù)研究發(fā)展計(jì)劃(863計(jì)劃)資助項(xiàng)目(2013AA06A411)；江蘇省“333工程”科研資助項(xiàng)目(BRA2015300)；江蘇省研究生培養(yǎng)創(chuàng)新工程(KYLX_1374)；江蘇省高校優(yōu)勢(shì)學(xué)科建設(shè)工程資助項(xiàng)目。

收稿日期：2015-11-23；修回日期：2016-01-06；責(zé)任編輯：胡嫻。

文章編號(hào)：1671-251X(2016)03-0052-04

DOI:10.13272/j.issn.1671-251x.2016.03.012

網(wǎng)絡(luò)出版地址：http://www.cnki.net/kcms/detail/32.1627.TP.20160307.1520.012.html

張金堯，李威，楊海，等.采煤機(jī)捷聯(lián)慣導(dǎo)定位方法研究[J].工礦自動(dòng)化，2016,42(3)：52-55.