基于捷聯(lián)慣導(dǎo)與差速里程計(jì)的掘進(jìn)機(jī)組合定位方法

王浩然，王宏偉，3，4，李正龍，付翔

（1. 太原理工大學(xué) 安全與應(yīng)急管理工程學(xué)院， 山西 太原 030024；2. 太原理工大學(xué) 山西省煤礦智能裝備工程研究中心， 山西 太原 030024；3. 太原理工大學(xué) 機(jī)械與運(yùn)載工程學(xué)院， 山西 太原 030024；4. 山西焦煤集團(tuán)有限責(zé)任公司 博士后工作站， 山西 太原 030024；5. 太原理工大學(xué) 礦業(yè)工程學(xué)院， 山西 太原 030024）

0 引言

煤炭目前仍是我國(guó)的主導(dǎo)能源，占能源結(jié)構(gòu)的57%[1-2]。掘進(jìn)作業(yè)環(huán)境惡劣，危險(xiǎn)系數(shù)高，對(duì)一線工人的安全與健康產(chǎn)生了較多不利影響，同時(shí)采掘不平衡也制約著煤炭生產(chǎn)，因此實(shí)現(xiàn)掘進(jìn)工作面的智能化乃至無(wú)人化對(duì)煤礦安全高效生產(chǎn)有重要意義。掘進(jìn)機(jī)自主精確定位是實(shí)現(xiàn)掘進(jìn)工作面智能化乃至無(wú)人化的關(guān)鍵。

捷聯(lián)慣導(dǎo)技術(shù)是一種不依賴外部信息的自主導(dǎo)航技術(shù)，其無(wú)源特性符合井下要求[3]，但利用捷聯(lián)慣導(dǎo)技術(shù)進(jìn)行掘進(jìn)機(jī)定位，其測(cè)量誤差會(huì)隨時(shí)間累計(jì)，影響定位精度，應(yīng)考慮與其他測(cè)量技術(shù)進(jìn)行組合來(lái)減小誤差[4]。里程計(jì)具有精度高、自主性強(qiáng)等優(yōu)點(diǎn)，因此捷聯(lián)慣導(dǎo)與里程計(jì)組合是一種較為理想的掘進(jìn)機(jī)定位方案。吳淼等[5]將二維里程計(jì)、捷聯(lián)慣導(dǎo)與激光偏距感知系統(tǒng)結(jié)合，在巷道坐標(biāo)下描述了掘進(jìn)機(jī)偏距與偏角，實(shí)現(xiàn)了掘進(jìn)機(jī)相對(duì)巷道軸向偏距與偏角的精確感知。沈陽(yáng)等[6]對(duì)文獻(xiàn)[5]中二維里程計(jì)與捷聯(lián)慣導(dǎo)的數(shù)據(jù)融合算法進(jìn)行了改進(jìn)，提高了掘進(jìn)機(jī)自主導(dǎo)航系統(tǒng)誤差估計(jì)的精度。劉豪[7]將捷聯(lián)慣導(dǎo)與里程計(jì)組合定位用于掘進(jìn)機(jī)，并針對(duì)掘進(jìn)機(jī)打滑導(dǎo)致的里程計(jì)失效問(wèn)題，設(shè)計(jì)了基于BP神經(jīng)網(wǎng)絡(luò)的打滑識(shí)別與補(bǔ)償方法，實(shí)現(xiàn)了封閉巷道內(nèi)掘進(jìn)機(jī)的長(zhǎng)時(shí)導(dǎo)航定位。

上述研究解決了捷聯(lián)慣導(dǎo)誤差隨時(shí)間累計(jì)的問(wèn)題，但單里程計(jì)僅能測(cè)得掘進(jìn)機(jī)單條履帶速度，不能真實(shí)反映掘進(jìn)機(jī)車體中心的實(shí)際運(yùn)行狀態(tài)[8]，且存在安裝位置帶來(lái)的誤差[9]。因此，筆者提出了一種基于捷聯(lián)慣導(dǎo)與差速里程計(jì)的掘進(jìn)機(jī)組合定位方法，采用卡爾曼濾波將捷聯(lián)慣導(dǎo)與差速里程計(jì)測(cè)量的掘進(jìn)機(jī)位姿數(shù)據(jù)進(jìn)行融合，對(duì)捷聯(lián)慣導(dǎo)誤差進(jìn)行補(bǔ)償校正，以減小捷聯(lián)慣導(dǎo)累計(jì)誤差對(duì)掘進(jìn)機(jī)定位的影響，提高掘進(jìn)機(jī)定位精度。

1 掘進(jìn)機(jī)組合定位原理

基于捷聯(lián)慣導(dǎo)與差速里程計(jì)的掘進(jìn)機(jī)組合定位方法采用以捷聯(lián)慣導(dǎo)為主、差速里程計(jì)為輔的定位方式，其原理如圖1所示。

圖1 基于捷聯(lián)慣導(dǎo)與差速里程計(jì)的掘進(jìn)機(jī)組合定位原理Fig. 1 Combined positioning principle of roadheader based on strapdown inertial navigation and differential odometer

該方法的實(shí)現(xiàn)由3個(gè)部分組成：基于捷聯(lián)慣導(dǎo)的位姿感知、基于差速里程計(jì)的航位推算、基于卡爾曼濾波的數(shù)據(jù)融合。① 捷聯(lián)慣導(dǎo)對(duì)準(zhǔn)后得到初始的姿態(tài)轉(zhuǎn)換矩陣，隨著掘進(jìn)機(jī)移動(dòng)，更新姿態(tài)轉(zhuǎn)換矩陣，將加速度與角加速度積分，得到載體坐標(biāo)系下的速度與角速度，進(jìn)一步得到導(dǎo)航坐標(biāo)系下掘進(jìn)機(jī)參考姿態(tài)角與參考位移。② 2個(gè)里程計(jì)分別測(cè)量掘進(jìn)機(jī)左右履帶速度，在已知前一時(shí)刻掘進(jìn)機(jī)位姿的前提下，推算出當(dāng)前時(shí)刻掘進(jìn)機(jī)位姿，并通過(guò)姿態(tài)轉(zhuǎn)換矩陣投影至導(dǎo)航坐標(biāo)系下。③ 將捷聯(lián)慣導(dǎo)與差速里程計(jì)測(cè)量的位姿數(shù)據(jù)之差作為誤差觀測(cè)值輸入卡爾曼濾波器，以卡爾曼濾波器輸出值對(duì)捷聯(lián)慣導(dǎo)數(shù)據(jù)進(jìn)行校正與補(bǔ)償。

2 掘進(jìn)機(jī)位姿數(shù)據(jù)測(cè)量

2.1 坐標(biāo)系及掘進(jìn)機(jī)姿態(tài)角描述

為描述掘進(jìn)機(jī)位姿，本文用到如下坐標(biāo)系。

（1） 地心坐標(biāo)系——oixiyizi。地心坐標(biāo)系以地心為坐標(biāo)原點(diǎn)，oixi軸 指向春分點(diǎn)，oizi軸沿地球自轉(zhuǎn)軸指向北極，oiyi軸與其余兩軸構(gòu)成右手坐標(biāo)系。在捷聯(lián)慣導(dǎo)系統(tǒng)中，陀螺儀與加速度計(jì)的輸出均以此為參考系。

（2） 地球坐標(biāo)系——oexeyeze。地球坐標(biāo)系以地球中心為坐標(biāo)原點(diǎn)，oexe與oeye均在地球赤道平面內(nèi)，其中oexe軸 指向本初子午線與赤道的交點(diǎn)，oeye軸沿地球自轉(zhuǎn)軸指向北極，oeze軸與其余兩軸構(gòu)成右手坐標(biāo)系。

（3） 導(dǎo)航坐標(biāo)系——onxnynzn。導(dǎo)航坐標(biāo)系以掘進(jìn)機(jī)重心為坐標(biāo)原點(diǎn)，onxn軸 指向地理東向，onyn軸指向地理北向，onzn軸與其余兩軸構(gòu)成右手坐標(biāo)系。導(dǎo)航坐標(biāo)系用來(lái)表示組合定位結(jié)果。導(dǎo)航坐標(biāo)系相對(duì)于地球坐標(biāo)系的位置可用掘進(jìn)機(jī)的地理位置（緯度L、經(jīng)度λ、高度h）表示。

（4） 載體坐標(biāo)系——obxbybzb。載體坐標(biāo)系以掘進(jìn)機(jī)重心為坐標(biāo)原點(diǎn)，obxb軸與obyb軸分別指向載體的右方與前方，obzb指向天并與其余兩軸構(gòu)成右手坐標(biāo)系。

在三維空間中，掘進(jìn)機(jī)姿態(tài)的變化為三自由度的剛體定軸旋轉(zhuǎn)，可用一組姿態(tài)角（航向角ψ、橫滾角γ 、俯仰角θ）描述掘進(jìn)機(jī)具體空間指向[10]，如圖2所示。掘進(jìn)機(jī)轉(zhuǎn)動(dòng)角度由載體坐標(biāo)系與導(dǎo)航坐標(biāo)系之間的角位置關(guān)系決定。為簡(jiǎn)化計(jì)算模型，本文主要探討對(duì)航向角ψ的感知精度，忽略橫滾角γ與俯仰角 θ的變化。

圖2 掘進(jìn)機(jī)姿態(tài)角Fig. 2 Attitude angle of roadheader

2.2 捷聯(lián)慣導(dǎo)

2.2.1 姿態(tài)更新

捷聯(lián)慣導(dǎo)與差速里程計(jì)的初始數(shù)據(jù)為載體坐標(biāo)系下掘進(jìn)機(jī)的位姿信息，組合定位需要確定導(dǎo)航坐標(biāo)系下的掘進(jìn)機(jī)狀態(tài)，因此需將載體坐標(biāo)系下的數(shù)據(jù)通過(guò)姿態(tài)轉(zhuǎn)換矩陣投影至導(dǎo)航坐標(biāo)系。掘進(jìn)機(jī)在運(yùn)動(dòng)過(guò)程中，姿態(tài)不斷變化，姿態(tài)信息同樣不斷更新。

姿態(tài)更新主要有方向余弦法、四元數(shù)法、歐拉角法，其中四元數(shù)法算法簡(jiǎn)單、易于操作、實(shí)用性強(qiáng)[11]。規(guī)范化四元數(shù)Q=[q0q1q2q3]T（q0-q3為實(shí)數(shù)）能夠描述剛體繞定軸旋轉(zhuǎn)運(yùn)動(dòng)的過(guò)程。載體坐標(biāo)系至導(dǎo)航坐標(biāo)系某一時(shí)刻的姿態(tài)轉(zhuǎn)換矩陣可用一組四元數(shù)表示：

式中：Tij（i,j=1,2,3）為姿態(tài)轉(zhuǎn)換矩陣Cnb第i行第j列的元素。

姿態(tài)轉(zhuǎn)換矩陣的更新實(shí)質(zhì)為四元數(shù)的更新。文獻(xiàn)[10-12]對(duì)四元數(shù)更新已有詳盡描述，本文采用等效旋轉(zhuǎn)矢量法[12]對(duì)四元數(shù)更新。

式中：Q(k+1)，Q(k)分別為k+1和k時(shí)刻的姿態(tài)四元數(shù)；?為四元數(shù)乘法；Q(h)為由k至k+1時(shí)刻的姿態(tài)變化四元數(shù)。

取得姿態(tài)轉(zhuǎn)換矩陣后，捷聯(lián)慣導(dǎo)可求解得到導(dǎo)航坐標(biāo)系下掘進(jìn)機(jī)姿態(tài)角：

2.2.2 速度與位置更新

通過(guò)將掘進(jìn)機(jī)加速度投影至導(dǎo)航坐標(biāo)系下，得到掘進(jìn)機(jī)速度微分方程：

式中：為捷聯(lián)慣導(dǎo)得到的導(dǎo)航坐標(biāo)系下掘進(jìn)機(jī)速度，VIn=[VEVNVU]T，VE，VN，VU分別為VIn在東向、北向、天向的分量；fb為載體坐標(biāo)系下加速度計(jì)測(cè)得的比力；為地球坐標(biāo)系相對(duì)于地心坐標(biāo)系的旋轉(zhuǎn)角速度；為導(dǎo)航坐標(biāo)系相對(duì)于地球坐標(biāo)系的旋轉(zhuǎn)角速度，分別為掘進(jìn)機(jī)所在地理位置的子午圈與卯酉圈曲率半徑；gn為導(dǎo)航坐標(biāo)系下重力加速度。

式（4）表明，只有在加速度計(jì)輸出中去除有害加速度a=(2ωnie+ωnen)×VIn-gn，才能得到掘進(jìn)機(jī)運(yùn)動(dòng)的加速度。

掘進(jìn)機(jī)靜止時(shí)，=0，捷聯(lián)慣導(dǎo)據(jù)此初對(duì)準(zhǔn)，得到初始的姿態(tài)轉(zhuǎn)換矩陣?；谒俣确匠?，捷聯(lián)慣導(dǎo)的位置微分方程為

式中：為捷聯(lián)慣導(dǎo)得到的導(dǎo)航坐標(biāo)系下掘進(jìn)機(jī)地理坐標(biāo)；LI，λI，hI分別為捷聯(lián)慣導(dǎo)得到的掘進(jìn)機(jī)緯度、經(jīng)度、高度。

2.3 差速里程計(jì)

參考文獻(xiàn)[13]的方法，對(duì)掘進(jìn)機(jī)進(jìn)行建模，掘進(jìn)機(jī)行走部簡(jiǎn)化模型如圖3所示。掘進(jìn)運(yùn)動(dòng)可看作在xbobyb平面繞瞬時(shí)轉(zhuǎn)動(dòng)中心c旋轉(zhuǎn)，隨著掘進(jìn)機(jī)的行走，c的位置不斷變化。

圖3 掘進(jìn)機(jī)行走部簡(jiǎn)化模型Fig. 3 Simplified model of roadheader walking section

基于簡(jiǎn)化模型可得

掘進(jìn)機(jī)兩側(cè)各有1個(gè)里程計(jì)對(duì)履帶測(cè)速，左右里程計(jì)輸出分別為根據(jù)航位推算原理，掘進(jìn)機(jī)線速度與角速度可由履帶速度推得：

式中I23為初等行變化矩陣。

差速里程計(jì)得到的導(dǎo)航坐標(biāo)系下掘進(jìn)機(jī)速度VDn與角速度ωnD為

由速度VDn可 得掘進(jìn)機(jī)位置的微分方程：

式中：PnD為 差速里程計(jì)得到的導(dǎo)航坐標(biāo)系下掘進(jìn)機(jī)地理坐標(biāo)；LD，λD，hD分別為差速里程計(jì)得到的掘進(jìn)機(jī)緯度、經(jīng)度、高度；

由ωnD可得掘進(jìn)機(jī)的姿態(tài)角微分方程：

3 基于卡爾曼濾波的掘進(jìn)機(jī)位姿數(shù)據(jù)融合

對(duì)于線性系統(tǒng)，卡爾曼濾波可以準(zhǔn)確地更新系統(tǒng)的均方誤差，對(duì)系統(tǒng)狀態(tài)做出最優(yōu)估計(jì)[14-15]。根據(jù)卡爾曼濾波器原理，將捷聯(lián)慣導(dǎo)與差速里程計(jì)位姿數(shù)據(jù)的差值作為卡爾曼濾波器的觀測(cè)值，以卡爾曼濾波器的輸出值作為捷聯(lián)慣導(dǎo)誤差的補(bǔ)償值，對(duì)速度進(jìn)行補(bǔ)償后得到最終的掘進(jìn)機(jī)定位結(jié)果。

3.1 捷聯(lián)慣導(dǎo)誤差方程

捷聯(lián)慣導(dǎo)速度誤差微分方程為

式中： ΔVIn為速度誤差；為姿態(tài)誤差；fn為導(dǎo)航坐標(biāo)系下加速度計(jì)測(cè)得的比力；ΔKA，ΔαA分別為加速度計(jì)的刻度系數(shù)誤差和安裝誤差角；δn為加速度計(jì)零偏誤差。

由速度誤差可得位置誤差微分方程：

根據(jù)四元數(shù)的微分方程可得姿態(tài)誤差微分方程[10]：

3.2 差速里程計(jì)誤差方程

設(shè)里程計(jì)工作的坐標(biāo)系為m系，其x，y，z軸分別指向履帶的右方、前方、上方。里程計(jì)安裝時(shí)應(yīng)使得m系與b系（載體坐標(biāo)系）重合[16]，且里程計(jì)只能得到履帶y軸方向的速度（即履帶線速度）。但由于里程計(jì)安裝誤差角 α=[αθαγαψ]T（ αθ,αγ,αψ分別為俯仰誤差角、橫滾誤差角、航向誤差角）的存在，m系與b系會(huì)存在旋轉(zhuǎn)角[17]，兩坐標(biāo)系的姿態(tài)轉(zhuǎn)換矩陣為

式中I為單位矩陣。

考慮到里程計(jì)的刻度系數(shù)誤差ΔK與安裝誤差角α，里程計(jì)得到的履帶實(shí)際速度為

單里程計(jì)擴(kuò)展為差速里程計(jì)后，在導(dǎo)航坐標(biāo)系下掘進(jìn)機(jī)速度誤差可表示為

其中

式中：ΔKl，ΔKr分別為左右里程計(jì)刻度系數(shù)誤差；αθl，αθr分 別為左右里程計(jì)的俯仰誤差角；αψl，αψr分別為左右里程計(jì)的航向誤差角。

由式（17）可知，里程計(jì)的橫滾誤差角并不影響差速里程計(jì)的精度。

3.3 卡爾曼濾波器設(shè)計(jì)

將捷聯(lián)慣導(dǎo)和差速里程計(jì)的誤差相關(guān)聯(lián)，選取與掘進(jìn)機(jī)位姿相關(guān)的18維向量X作為狀態(tài)量：

以捷聯(lián)慣導(dǎo)與差速里程計(jì)解算的速度與位姿之差作為卡爾曼濾波器的觀測(cè)值：

建立卡爾曼濾波狀態(tài)方程：

式中：Xk為k時(shí)刻的狀態(tài)量；A為狀態(tài)轉(zhuǎn)移矩陣；wk-1為k-1時(shí) 刻系統(tǒng)過(guò)程噪聲；Zk為k時(shí)刻的觀測(cè)值；H為量測(cè)矩陣；Dk為k時(shí)刻的量測(cè)白噪聲。

將狀態(tài)方程離散化并通過(guò)以下步驟進(jìn)行濾波[18]。

（1） 計(jì)算狀態(tài)量的先驗(yàn)估計(jì)值：

（2） 計(jì)算均方誤差的先驗(yàn)估計(jì)值：

式中：Sk-1為k-1時(shí) 刻的均方誤差；Nk-1為k-1時(shí)刻的系統(tǒng)噪聲協(xié)方差矩陣。

（3） 更新卡爾曼濾波增益：

式中Jk為k時(shí)刻的量測(cè)噪聲協(xié)方差矩陣。

（4） 計(jì)算狀態(tài)量的后驗(yàn)估計(jì)值：

（5） 更新均方誤差：

將后驗(yàn)估計(jì)值作為狀態(tài)量的最優(yōu)估計(jì)值。捷聯(lián)慣導(dǎo)速度消去誤差最優(yōu)估計(jì)值得到所需速度，積分后便可得到掘進(jìn)機(jī)位移。

3.4 里程計(jì)輸出有效性判斷

掘進(jìn)機(jī)可能出現(xiàn)打滑狀況，此時(shí)的里程計(jì)輸出為無(wú)效輸出，因此需判斷里程計(jì)輸出的有效性。里程計(jì)輸出為有效輸出時(shí)，卡爾曼濾波的新息rk=滿足以下正態(tài)分布[19]：

記mk=HS-k HT+Jk， 可得自由度為1的χ2分布：

式中：rk(u)為rk的第u個(gè)元素；mk(u,u)為mk對(duì)角線上的第u個(gè)元素。

綜合掘進(jìn)機(jī)的實(shí)際運(yùn)行環(huán)境和對(duì)誤檢率與漏檢率的相關(guān)要求，選取閾值 ξ對(duì)新息進(jìn)行檢驗(yàn)。當(dāng)滿足下式時(shí)，判定新息為異常，里程計(jì)輸出無(wú)效，反之判定新息正常，里程計(jì)輸出有效。

隔離里程計(jì)無(wú)效輸出的方法：對(duì)于異常的新息元素rk(u)，將其對(duì)應(yīng)的量測(cè)噪聲的元素設(shè)置為無(wú)窮大，從而使得卡爾曼濾波增益對(duì)應(yīng)元素為0，在狀態(tài)估計(jì)中便可依靠其他可信的觀測(cè)量進(jìn)行狀態(tài)更新，隔離里程計(jì)的無(wú)效輸出。

4 實(shí)驗(yàn)驗(yàn)證

4.1 實(shí)驗(yàn)平臺(tái)搭建

基于捷聯(lián)慣導(dǎo)與差速里程計(jì)的掘進(jìn)機(jī)組合定位實(shí)驗(yàn)平臺(tái)設(shè)備主要包括EBZ160M-2掘錨機(jī)、FSON II捷聯(lián)慣導(dǎo)、TS60全站儀、差速里程計(jì)、CH110捷聯(lián)慣導(dǎo)。實(shí)驗(yàn)場(chǎng)地為封閉式模擬巷道，地面為鋼制地板。模擬巷道可伸展，全部展開長(zhǎng)度達(dá)32 m，寬度為4 m，高度為5.5 m。掘進(jìn)機(jī)整機(jī)高度為2.5 m，長(zhǎng)度為10.4 m，履帶間距為1.1 m。全站儀得到的掘進(jìn)機(jī)位置與FSON II捷聯(lián)慣導(dǎo)得到的掘進(jìn)機(jī)姿態(tài)角作為掘進(jìn)機(jī)實(shí)際位姿數(shù)據(jù)，用于驗(yàn)證CH110捷聯(lián)慣導(dǎo)與差速里程計(jì)組合定位的精度。捷聯(lián)慣導(dǎo)參數(shù)見表1。

表1 捷聯(lián)慣導(dǎo)參數(shù)Table 1 Strapdown inertial navigation parameters

設(shè)備安裝如圖4所示。以掘進(jìn)機(jī)重心坐標(biāo)作為整機(jī)位置，將CH110捷聯(lián)慣導(dǎo)安裝于掘進(jìn)機(jī)上側(cè)靠近中心的位置，以減小掘進(jìn)機(jī)轉(zhuǎn)動(dòng)引起的速度誤差。棱鏡安裝于CH110捷聯(lián)慣導(dǎo)旁，以減小安裝位置帶來(lái)的誤差，左右履帶輸出軸處各有1個(gè)里程計(jì)通過(guò)齒圈相連。全站儀觀測(cè)裝置部署于掘進(jìn)機(jī)后方10 m處。

圖4 設(shè)備安裝Fig. 4 Equipment installation

一般情況下里程計(jì)安裝誤差角為小角度，若不滿足小角度的條件，只需要首次標(biāo)定即可使得安裝誤差角為小角度[20]。標(biāo)定后的里程計(jì)參數(shù)見表2。

表2 里程計(jì)參數(shù)Table 2 Odometer parameters

若捷聯(lián)慣導(dǎo)安裝誤差角并非小角度，可借助FSON II捷聯(lián)慣導(dǎo)對(duì)CH110捷聯(lián)慣導(dǎo)進(jìn)行標(biāo)定。以CH110捷聯(lián)慣導(dǎo)幾何中心為原點(diǎn)建立坐標(biāo)系（ch系），其x，y，z軸分別指向CH110捷聯(lián)慣導(dǎo)的右方、前方、上方。掘進(jìn)機(jī)靜止時(shí)CH110捷聯(lián)慣導(dǎo)相對(duì)于FSON II捷聯(lián)慣導(dǎo)的姿態(tài)角差值為安裝誤差角αch=[αθchαγchαψch]T，調(diào)整捷聯(lián)慣導(dǎo)安裝角度，使安裝誤差角為小角度。ch系相對(duì)于b系的姿態(tài)轉(zhuǎn)換矩陣為

通過(guò)坐標(biāo)轉(zhuǎn)換矩陣可將CH110捷聯(lián)慣導(dǎo)的位姿數(shù)據(jù)轉(zhuǎn)換至b系，以消除安裝誤差角對(duì)定位的影響。

4.2 實(shí)驗(yàn)過(guò)程及結(jié)果分析

在相同路況下實(shí)驗(yàn)多次，并選取某一組實(shí)驗(yàn)數(shù)據(jù)進(jìn)行分析。捷聯(lián)慣導(dǎo)根據(jù)全站儀給出的起始點(diǎn)經(jīng)緯度信息進(jìn)行15 min的粗對(duì)準(zhǔn)與精對(duì)準(zhǔn)。對(duì)準(zhǔn)后，捷聯(lián)慣導(dǎo)計(jì)算得到初始的姿態(tài)轉(zhuǎn)換矩陣。掘進(jìn)機(jī)移動(dòng)，捷聯(lián)慣導(dǎo)開始對(duì)姿態(tài)轉(zhuǎn)換矩陣及速度進(jìn)行更新。同時(shí)，差速里程計(jì)獲得掘進(jìn)機(jī)左右履帶速度，解算得到掘進(jìn)機(jī)速度與航向角?？柭鼮V波器對(duì)兩者數(shù)據(jù)融合，并對(duì)捷聯(lián)慣導(dǎo)誤差進(jìn)行校正與補(bǔ)償。掘進(jìn)機(jī)初始位姿數(shù)據(jù)見表3。

表3 掘進(jìn)機(jī)初始位姿數(shù)據(jù)Table 3 Initial position and posture data of roadheader

操控掘進(jìn)機(jī)從起點(diǎn)位置沿著目標(biāo)路線行進(jìn)，并控制掘進(jìn)機(jī)在模擬巷道內(nèi)完成前進(jìn)、轉(zhuǎn)向等動(dòng)作。在掘進(jìn)機(jī)前進(jìn)一定距離后對(duì)其位姿進(jìn)行調(diào)整并繼續(xù)前進(jìn)，最終操控掘進(jìn)機(jī)行走至模擬煤壁處。實(shí)驗(yàn)持續(xù)250 s，掘進(jìn)機(jī)行進(jìn)東向位移9.51 m，北向位移1.72 m，航向角變化23.78°。

不同定位方法解算的掘進(jìn)機(jī)航向角如圖5所示。可看出隨著時(shí)間增加，CH110捷聯(lián)慣導(dǎo)得到的航向角與FSON II捷聯(lián)慣導(dǎo)得到的實(shí)際航向角偏離度逐漸增加，而組合定位方法得到的航向角與實(shí)際值更吻合；組合定位方法得到的航向角誤差最大為0.587 7°，平均為0.140 0°。

圖5 掘進(jìn)機(jī)航向角Fig. 5 Heading angle of roadheader

將掘進(jìn)機(jī)的經(jīng)緯度坐標(biāo)轉(zhuǎn)換為高斯坐標(biāo)系下坐標(biāo)并與初始坐標(biāo)對(duì)比，得到掘進(jìn)機(jī)東向位移與北向位移，如圖6所示。將掘進(jìn)機(jī)的東向位移與北向位移組合得到掘進(jìn)機(jī)軌跡，如圖7所示。

圖6 掘進(jìn)機(jī)位移Fig. 6 Displacement of roadheader

圖7 掘進(jìn)機(jī)軌跡Fig. 7 Track of roadheader

從圖6、圖7可看出，隨著時(shí)間增加，CH110捷聯(lián)慣導(dǎo)得到的掘進(jìn)機(jī)位移誤差逐漸增大，而組合定位方法得到的位移更接近全站儀得到的實(shí)際位移；組合定位方法得到的東向位移誤差最大為0.182 1 m，平均為0.145 8 m，北向位移誤差最大為0.110 8 m，平均為0.088 5 m。

5 結(jié)論

（1） 綜合利用捷聯(lián)慣導(dǎo)無(wú)源性和里程計(jì)自主性的優(yōu)點(diǎn)，提出了一種基于捷聯(lián)慣導(dǎo)與差速里程計(jì)的掘進(jìn)機(jī)組合定位方法。

（2） 根據(jù)捷聯(lián)慣導(dǎo)工作原理，推導(dǎo)了捷聯(lián)慣導(dǎo)的位姿更新方程與誤差方程。根據(jù)航位推算原理，簡(jiǎn)化掘進(jìn)機(jī)行走部模型，通過(guò)運(yùn)動(dòng)學(xué)分析，得到差速里程計(jì)的位姿更新方程與誤差方程。基于捷聯(lián)慣導(dǎo)和差速里程計(jì)的誤差方程，設(shè)計(jì)了具有18維狀態(tài)向量的卡爾曼濾波器，以捷聯(lián)慣導(dǎo)與差速里程計(jì)得到的掘進(jìn)機(jī)位姿數(shù)據(jù)之差作為卡爾曼濾波器的觀測(cè)量，以卡爾曼濾波器輸出值對(duì)捷聯(lián)慣導(dǎo)數(shù)據(jù)進(jìn)行校正與補(bǔ)償。將里程計(jì)數(shù)據(jù)有效性判斷融入組合定位方法中，避免了履帶打滑對(duì)定位精度的影響。

（3） 在模擬巷道中進(jìn)行了掘進(jìn)機(jī)定位實(shí)驗(yàn)，結(jié)果表明：組合定位方法測(cè)得的掘進(jìn)機(jī)航向角誤差能夠控制在0.6°以內(nèi)，位置誤差能夠控制在0.19 m以內(nèi)，可減小捷聯(lián)慣導(dǎo)累計(jì)誤差對(duì)掘進(jìn)機(jī)定位的影響，具有較高的定位精度。