基于頻域分離算子的采煤機(jī)動(dòng)基座SINS初始對(duì)準(zhǔn)算法

陳宇鳴，李 威，辛改芳，王禹橋，司卓印

(1.中國(guó)礦業(yè)大學(xué) 機(jī)電工程學(xué)院，江蘇 徐州 221116; 2.常州信息職業(yè)技術(shù)學(xué)院 智能裝備學(xué)院，江蘇 常州 213164; 3.河海大學(xué) 物聯(lián)網(wǎng)工程學(xué)院，江蘇 常州 213022; 4.中國(guó)礦業(yè)大學(xué)徐海學(xué)院，江蘇 徐州 221116)

煤炭作為世界上分布最廣、最豐富的資源，已成為世界能源結(jié)構(gòu)的主導(dǎo)能源之一。然而，煤礦工作環(huán)境的復(fù)雜性給煤礦開采帶來(lái)了很大的困難，煤礦的安全問(wèn)題近年來(lái)受到越來(lái)越廣泛的關(guān)注[1]。為解決煤礦生產(chǎn)事故和人員傷亡問(wèn)題，同時(shí)實(shí)現(xiàn)井下安全生產(chǎn)自動(dòng)化，實(shí)施無(wú)人工作面智能化采煤具有重要意義，其中采煤機(jī)位置和姿態(tài)的確定是實(shí)現(xiàn)井下自動(dòng)化的關(guān)鍵技術(shù)之一。鑒于井下煤礦環(huán)境的特殊性，無(wú)線定位[2]、藍(lán)牙定位[3]、衛(wèi)星定位[4]、天文定位[5]等方式都依賴外部可靠的信息源，同時(shí)這些定位方式都無(wú)法測(cè)量采煤機(jī)的實(shí)時(shí)姿態(tài)，因此其在井下的應(yīng)用都受到了極大的限制。基于慣性導(dǎo)航系統(tǒng)(SINS)的采煤機(jī)定位定姿技術(shù)是一種既不依賴于外部信息源又不向外輻射能量的自主導(dǎo)航系統(tǒng)[6-7]。SINS是指將陀螺儀和加速度計(jì)捷聯(lián)組成的慣性測(cè)量單元直接固定在運(yùn)載體上，利用陀螺儀和加速度計(jì)實(shí)時(shí)測(cè)量采煤機(jī)的角速度和加速度，并基于牛頓三大定律計(jì)算采煤機(jī)的姿態(tài)、速度、位置。高自主性及安全性的特點(diǎn)使得慣性導(dǎo)航系統(tǒng)能夠滿足于井下煤礦環(huán)境的采煤機(jī)定位，并受到了廣泛的關(guān)注[8-9]。

采煤機(jī)初始姿態(tài)角的確定作為導(dǎo)航定位的前提，后續(xù)導(dǎo)航的性能很大程度上取決于初始姿態(tài)矩陣的精確程度[10-11]。采煤機(jī)初始對(duì)準(zhǔn)主要包括2個(gè)連續(xù)的階段:粗對(duì)準(zhǔn)和精對(duì)準(zhǔn)。所謂粗對(duì)準(zhǔn)，顧名思義，即在沒有任何先驗(yàn)知識(shí)的情況下短時(shí)間內(nèi)獲得粗略的初始姿態(tài)矩陣。經(jīng)短時(shí)間的粗對(duì)準(zhǔn)后，初始姿態(tài)誤差角可以限制在幾度以內(nèi)[12-13]。相對(duì)于粗對(duì)準(zhǔn)，精對(duì)準(zhǔn)是在粗對(duì)準(zhǔn)的基礎(chǔ)上，通過(guò)構(gòu)建線性失準(zhǔn)角誤差方程，并利用卡爾曼濾波技術(shù)不斷修正粗對(duì)準(zhǔn)獲得的初始姿態(tài)矩陣，進(jìn)而獲得更加精確的初始姿態(tài)角。因此，粗對(duì)準(zhǔn)作為精對(duì)準(zhǔn)的前提，高性能的粗對(duì)準(zhǔn)可以有效地提高精對(duì)準(zhǔn)的精度和收斂速度[14-15]。

慣性導(dǎo)航粗對(duì)準(zhǔn)主要包括解析法和優(yōu)化法。解析法是在運(yùn)載體對(duì)地靜止環(huán)境下執(zhí)行，即運(yùn)載體相對(duì)地面沒有明顯的角運(yùn)動(dòng)和線運(yùn)動(dòng)，同時(shí)初始對(duì)準(zhǔn)的所在地理位置精確已知，利用陀螺儀和加速度計(jì)敏感載體系下的地球自轉(zhuǎn)角速度和重力加速度，同時(shí)結(jié)合導(dǎo)航系下的地球自轉(zhuǎn)角速度和重力加速度，最終確定出初始姿態(tài)旋轉(zhuǎn)矩陣。然而解析法需要載體長(zhǎng)時(shí)間保持靜止?fàn)顟B(tài)，同時(shí)這種方法收斂速度慢且精度低，因此限制了其在井下采煤機(jī)初始對(duì)準(zhǔn)的應(yīng)用[16-17]。近年來(lái)，基于優(yōu)化的動(dòng)基座粗對(duì)準(zhǔn)方法(OBA)得到了廣泛的關(guān)注，OBA方法利用姿態(tài)分解技術(shù)，將采煤機(jī)的實(shí)時(shí)姿態(tài)矩陣分解為2個(gè)時(shí)變矩陣和1個(gè)常值矩陣，利用陀螺儀和加速度計(jì)獲得多個(gè)時(shí)刻且不共線的觀測(cè)矢量，最終采用優(yōu)化算法確定出采煤機(jī)的初始姿態(tài)矩陣[18-20]。OBA算法不僅可以在晃動(dòng)的載體上進(jìn)行初始對(duì)準(zhǔn)，而且該方法具有一定的抗干擾能力，因此OBA方法可以運(yùn)用于采煤機(jī)截割煤壁環(huán)境下的初始對(duì)準(zhǔn)。然而，由于采煤機(jī)的工況環(huán)境比較惡劣，干擾較多，采煤機(jī)截割煤壁時(shí)會(huì)產(chǎn)生劇烈的震動(dòng)，固聯(lián)在采煤機(jī)機(jī)身的SINS容易受到震動(dòng)的影響，震動(dòng)會(huì)使得加速度計(jì)敏感到高頻噪聲值，這些高頻噪聲會(huì)累積到OBA算法中，最終造成OBA算法的性能嚴(yán)重下降。雖然設(shè)計(jì)低通濾波器可以有效地抑制加速度計(jì)敏感到的高頻噪聲，但是對(duì)于不同的機(jī)動(dòng)條件及環(huán)境，低通濾波器的參數(shù)選擇是一個(gè)繁瑣的過(guò)程，過(guò)低的截止頻率有可能濾除有用的信息，而過(guò)高的截止頻率又會(huì)降低對(duì)準(zhǔn)性能，因此合理的低通濾波器參數(shù)是提高井下采煤機(jī)初始對(duì)準(zhǔn)的一項(xiàng)關(guān)鍵指標(biāo)。針對(duì)上述問(wèn)題，筆者建立了采煤機(jī)動(dòng)力學(xué)模型，通過(guò)仿真分析的手段獲得采煤機(jī)截割煤壁時(shí)機(jī)身運(yùn)動(dòng)信息，并運(yùn)用傅里葉變換將時(shí)域運(yùn)動(dòng)信號(hào)轉(zhuǎn)換到頻域，從而獲得低通濾波器的截止頻率。之后，采用等紋波逼近法構(gòu)建低通濾波器模型，并將所建立的低通濾波器引入到OBA方法中，從而有效地抑制震動(dòng)所產(chǎn)生的高頻噪聲。最后搭建采煤機(jī)截割煤壁模擬實(shí)驗(yàn)臺(tái)驗(yàn)證所提出方法的有效性。

1 采煤機(jī)OBA算法原理

1.1 采煤機(jī)動(dòng)態(tài)對(duì)準(zhǔn)原理

(1)

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

(2)

(3)

1.2 數(shù)值解算算法

(4)

(5)

(6)

將式(6)代入式(5)并整理得

(7)

對(duì)式(4)，(7)進(jìn)行離散化，得

(8)

2 基于采煤機(jī)動(dòng)力學(xué)模型的頻域去噪技術(shù)

2.1 采煤機(jī)動(dòng)力學(xué)模型

井下采煤機(jī)工作條件惡劣，工況復(fù)雜，在建立截割狀態(tài)的采煤機(jī)動(dòng)力學(xué)模型時(shí)，對(duì)采煤機(jī)的運(yùn)行工況和結(jié)構(gòu)作適當(dāng)?shù)暮?jiǎn)化[22]。

(1)采煤機(jī)的滾筒和機(jī)身質(zhì)量分布均勻，忽略其彈性并轉(zhuǎn)化為集中質(zhì)量。

(2)忽略采煤機(jī)下面刮板輸送機(jī)的彈性阻尼，并視搖臂、滑靴的阻尼為黏性阻尼。

(3)作用于滾筒上的載荷按文獻(xiàn)[23]所給的曲線變化。

根據(jù)采煤機(jī)的結(jié)構(gòu)特點(diǎn)，采煤機(jī)的運(yùn)動(dòng)可以認(rèn)為是具有阻尼的多自由度受迫振動(dòng)。采用質(zhì)量塊法，將滾筒采煤機(jī)簡(jiǎn)化為如下5個(gè)部分:前后滾筒、前后搖臂與采煤機(jī)機(jī)身，同時(shí)將滾筒和機(jī)身視為剛體，采煤機(jī)滑靴與搖臂建模為彈簧阻尼系統(tǒng)首先考慮垂直平面Z方向的運(yùn)動(dòng)情況，對(duì)圖2(a)進(jìn)行動(dòng)力學(xué)建模，取左右滾筒和采煤機(jī)機(jī)身的平衡位置起的位移Z1，Z2，Z3作為廣義坐標(biāo)，利用牛頓-拉格朗日方程獲得采煤機(jī)在機(jī)體垂直平面Z軸方向的系統(tǒng)動(dòng)能、系統(tǒng)彈性勢(shì)能、系統(tǒng)的耗散能。

(9)

(10)

(11)

式中，T為系統(tǒng)動(dòng)能;U為系統(tǒng)彈性勢(shì)能;D為系統(tǒng)耗散能;Z1，Z2，Z3分別為采煤機(jī)前后滾筒和機(jī)身的位移;m1，m2，m3分別為采煤機(jī)前后滾筒和機(jī)身的質(zhì)量;k1，k2為前后搖臂的連接剛度;c1，c2為前后搖臂的連接阻尼;k3，k4為前后導(dǎo)向滑靴的連接剛度;c3，c4為前后導(dǎo)向滑靴的連接阻尼。

圖2 采煤機(jī)動(dòng)力學(xué)模型

將式(9)～(11)代入拉格朗日動(dòng)力學(xué)方程(12)，由此得到采煤機(jī)沿著機(jī)體Z軸的動(dòng)力學(xué)模型(13)，即

(12)

式中，F(xiàn)i為受到的力;xi為位移。

(13)

為了方便MATLAB求解，將式(13)改寫為狀態(tài)方程的形式，可得

(14)

式中，M為質(zhì)量矩陣;C為阻尼矩陣;K為彈性系數(shù)矩陣;F為由Fi組成的矢量。

X和Y方向的動(dòng)力學(xué)方程可以通過(guò)同樣的手段獲得，因此將這兩個(gè)方向的建模過(guò)程省略。圖2中，F(xiàn)′Z，F(xiàn)Z，F(xiàn)′X，F(xiàn)X，F(xiàn)′Y，F(xiàn)Y分別為采煤機(jī)左滾筒和右滾筒的受力沿著Z，X，Y軸的投影;Y1，Y2為左右滾筒的Y軸方向的位移;c5～c12為采煤機(jī)的等效阻尼;k5～k12為采煤機(jī)的等效剛度。

2.2 采煤機(jī)動(dòng)力學(xué)模型求解

為了獲得采煤機(jī)截割煤壁時(shí)機(jī)身的運(yùn)動(dòng)狀態(tài)，需要對(duì)非線性微分方程(14)進(jìn)行求解，然而在方程(14)中，廣義力F是一個(gè)時(shí)變函數(shù)，無(wú)法獲得方程(14)的解析解。為了獲得非線性微分方程(14)的解，采用降階的方式，將方程(14)轉(zhuǎn)化為6個(gè)一階微分方程，同時(shí)利用MATLAB提供的基于Runge-Kutta變步長(zhǎng)算法的Ode45函數(shù)求解非線性微分方程(14)，將MG150/345-WDK型采煤機(jī)的有關(guān)參數(shù)代入微分方程，該類型采煤機(jī)的具體參數(shù)如下:采煤機(jī)機(jī)身質(zhì)量m3為15 300 kg，前滾筒質(zhì)量m1和后滾筒質(zhì)量m2為2 430 kg，采煤機(jī)前搖臂和后搖臂在Z方向的阻尼c1,c2分別為690.7 N/(m/s),采煤機(jī)前搖臂和后搖臂在Z方向的剛度k1,k2分別為149 000 N/(m/s)，采煤機(jī)前后導(dǎo)向滑靴在Z方向的連接阻尼c3，c4為90 N/(m/s),采煤機(jī)前后導(dǎo)向滑靴在Z方向的連接剛度k3，k4為300 000 N/m。通過(guò)數(shù)值模擬的方式，分析采煤機(jī)截割煤壁時(shí)的動(dòng)力學(xué)特征，圖3，4為采煤機(jī)截割煤壁時(shí)機(jī)身的運(yùn)動(dòng)加速度幅值曲線和頻率曲線。

圖3 采煤機(jī)機(jī)身加速度時(shí)域曲線

圖4 采煤機(jī)機(jī)身加速度頻域曲線

由圖3可知，采煤機(jī)機(jī)身的加速度呈現(xiàn)周期性的波動(dòng)，采煤機(jī)機(jī)身X，Y，Z軸的加速度最大幅值分別為0.20，0.27，0.49 m/s2，采用傅里葉變換將時(shí)域加速度信號(hào)轉(zhuǎn)換到頻域后，得到了采煤機(jī)截割煤壁時(shí)機(jī)身運(yùn)動(dòng)頻譜圖，從圖4可以看出采煤機(jī)在截割煤壁時(shí)機(jī)身的加速度信號(hào)主要位于0～2 Hz的低頻段，屬于低頻信息。然而當(dāng)采煤機(jī)截割煤壁時(shí)，劇烈的震動(dòng)會(huì)導(dǎo)致加速度計(jì)敏感到高頻噪聲。因此如果設(shè)計(jì)一個(gè)理想的低通濾波器，能夠抑制由于采煤機(jī)震動(dòng)引發(fā)的高頻噪聲，并且保留有用的低頻信息，那么這種做法可以提高OBA對(duì)準(zhǔn)的精度和對(duì)準(zhǔn)時(shí)間。2.2節(jié)通過(guò)動(dòng)力學(xué)建模的方式確定出采煤機(jī)截割煤壁時(shí)機(jī)身振動(dòng)頻率位于0～2 Hz的低頻段，在實(shí)際中，采煤機(jī)的動(dòng)力學(xué)參數(shù)和仿真所采用的參數(shù)存在一定的差異，因此在設(shè)計(jì)數(shù)字濾波器時(shí)將截止頻率設(shè)置為5 Hz。

2.3 基于采煤機(jī)振動(dòng)頻率的濾波器設(shè)計(jì)

工程中常用的數(shù)字濾波器分為2種，一種是有限長(zhǎng)單位沖激響應(yīng)(FIR)濾波器，另一種是無(wú)限沖激響應(yīng)(IIR)濾波器。FIR濾波器雖然計(jì)算量略大一些，但是其具備線性相位及穩(wěn)定性的優(yōu)點(diǎn)在數(shù)字信號(hào)處理領(lǐng)域發(fā)揮著巨大的作用。常用的FIR濾波器設(shè)計(jì)手段包括窗函數(shù)法，頻率采樣法，最佳逼近法等。其中等紋波最佳逼近法是一種優(yōu)化設(shè)計(jì)方法，所設(shè)計(jì)的FIR濾波器的幅頻響應(yīng)在通帶和阻帶是等波紋的，這種方法相較于窗函數(shù)法和頻率采樣法，濾波器階數(shù)相同時(shí)，該方法設(shè)計(jì)的濾波器最大逼近誤差最小，濾波器性能最高，因此筆者采用等紋波最佳逼近法設(shè)計(jì)FIR濾波器。

FIR濾波器具有線性相位的條件是濾波器系數(shù)h(n)為實(shí)序列，并且序列h(n)關(guān)于(N-1)/2偶對(duì)稱，若要設(shè)計(jì)線性相位濾波器，定義加權(quán)誤差函數(shù)E(ω)為

E(ω)=W(ω)[Hd(ω)-Hg(ω)]

(15)

其中，W(ω)為誤差加權(quán)函數(shù);Hd(ω)為理想的幅度特性函數(shù);Hg(ω)為實(shí)際設(shè)計(jì)的濾波器幅度特性函數(shù)。等紋波最佳逼近基于切比雪夫逼近，在通帶和阻帶內(nèi)滿足誤差極大值最小化準(zhǔn)則，采用Remez多重交換迭代算法求解濾波器的系數(shù)h(n)，W(ω)的取值越大，那么相應(yīng)的頻率段逼近精度越高，在Remez多重交換迭代算法中W(ω)是已知的函數(shù)。

采用最佳逼近法并利用Remez多重交換迭代算法設(shè)計(jì)所需要的FIR濾波器，F(xiàn)IR濾波器指標(biāo)如下:采樣頻率為100 Hz，通帶截止頻率為5 Hz，阻帶起始頻率為10 Hz，通帶最大衰減為1 dB，阻帶最小衰減為40 dB，濾波器的階數(shù)為31階，則所設(shè)計(jì)的幅度曲線如圖5所示。

圖5 FIR濾波器幅度特性曲線

3 實(shí) 驗(yàn)

為驗(yàn)證所提出的方法的有效性和優(yōu)越性，搭建了采煤機(jī)截割煤壁SINS初始對(duì)準(zhǔn)實(shí)驗(yàn)，實(shí)驗(yàn)平臺(tái)如圖6所示。實(shí)驗(yàn)系統(tǒng)包括光纖捷聯(lián)慣導(dǎo)，MG150/345-WDK型號(hào)采煤機(jī)，導(dǎo)航計(jì)算機(jī)以及供電裝置。光纖捷聯(lián)慣導(dǎo)固定在采煤機(jī)機(jī)身中央并采用專用的防爆電源供電，光纖捷聯(lián)慣導(dǎo)包括三軸加速度計(jì)和三軸陀螺儀，其中加速度計(jì)和陀螺儀以100 Hz的采樣頻率采集采煤機(jī)的角速度和加速度信號(hào)，陀螺儀和加速度計(jì)的零偏分別為0.01°/h和10-4g。

圖6 采煤機(jī)初始對(duì)準(zhǔn)實(shí)驗(yàn)平臺(tái)

光纖捷聯(lián)慣導(dǎo)固定在采煤機(jī)上預(yù)熱10 min左右，待慣導(dǎo)啟動(dòng)穩(wěn)定后，首先采煤機(jī)保持靜止10 min左右，之后操作人員啟動(dòng)采煤機(jī)進(jìn)行280 s的截割煤壁實(shí)驗(yàn)，將上述總計(jì)880 s的陀螺儀和加速度計(jì)數(shù)據(jù)保存進(jìn)行事后分析。首先采用傳統(tǒng)的粗對(duì)準(zhǔn)外加卡爾曼濾波精對(duì)準(zhǔn)對(duì)前10 min的數(shù)據(jù)進(jìn)行預(yù)處理，所得到的初始姿態(tài)角作為初始對(duì)準(zhǔn)的參考值。接著利用后280 s的數(shù)據(jù)進(jìn)行姿態(tài)跟蹤，所得到的實(shí)時(shí)姿態(tài)曲線如圖7所示，圖7反應(yīng)出了采煤機(jī)截割煤壁時(shí)機(jī)身的晃動(dòng)情況，將此姿態(tài)跟蹤的結(jié)果作為評(píng)估運(yùn)動(dòng)對(duì)準(zhǔn)性能的指標(biāo)。

圖7 采煤機(jī)機(jī)身實(shí)時(shí)姿態(tài)角

圖8 采煤機(jī)機(jī)身加速度頻域曲線

首先將采煤機(jī)截割煤壁時(shí)機(jī)身的加速度信號(hào)進(jìn)行傅里葉變換，變換后的頻域曲線如圖8所示。對(duì)比圖4，8，在采煤機(jī)動(dòng)力學(xué)建模的過(guò)程中，對(duì)采煤機(jī)的運(yùn)行工況及結(jié)構(gòu)做了適當(dāng)?shù)暮?jiǎn)化，因此圖4，8在數(shù)值上存在著一些差異，但是從圖8中可以明顯的看出采煤機(jī)機(jī)身的晃動(dòng)頻率位于低頻段，這和動(dòng)力學(xué)建模所得到的結(jié)論一致。此外，從圖8中可以看出，采煤機(jī)截割煤壁時(shí)，機(jī)身的晃動(dòng)會(huì)造成加速度傳感器敏感到高頻噪聲值，這些高頻噪聲值雖然數(shù)值不大，但是會(huì)累加到動(dòng)態(tài)OBA對(duì)準(zhǔn)算法中，從而造成OBA算法精度以及收斂速度下降。因此，接下來(lái)對(duì)傳統(tǒng)的動(dòng)基座對(duì)準(zhǔn)算法和所提出的基于頻域分離算子的采煤機(jī)動(dòng)基座SINS初始對(duì)準(zhǔn)算法進(jìn)行對(duì)比，從而驗(yàn)證所提出算法的有效性和合理性。

圖9，10是傳統(tǒng)的OBA算法和經(jīng)過(guò)濾波器補(bǔ)償后橫滾角和俯仰角的實(shí)時(shí)誤差跟蹤曲線，由于俯仰角和橫滾角的可觀測(cè)性較強(qiáng)，因此經(jīng)數(shù)字濾波器補(bǔ)償后和未補(bǔ)償?shù)腛BA算法都取得了較好的效果，實(shí)時(shí)跟蹤誤差均小于1°，但是所提出的算法跟蹤精度更高，穩(wěn)態(tài)后誤差小于0.1°，而傳統(tǒng)的OBA算法的俯仰角和橫滾角的誤差會(huì)隨著時(shí)間發(fā)散。

圖9 俯仰角誤差曲線

圖10 橫滾角誤差曲線

圖11 航向角誤差曲線

圖11是航向角的誤差跟蹤曲線，由于航向角的可觀測(cè)性較差，因此相對(duì)于橫滾角和俯仰角而言，兩種算法的收斂速度和精度都有所下降。從圖11可以看出，所提出的算法獲得的對(duì)準(zhǔn)結(jié)果不僅精度更高，而且收斂速度更快，其原因是數(shù)字濾波器過(guò)濾了采煤機(jī)截割煤壁時(shí)的高頻振動(dòng)噪聲，這些高頻噪聲會(huì)累積在傳統(tǒng)的OBA算法中，由此導(dǎo)致傳統(tǒng)的OBA算法收斂速度慢以及精度低等問(wèn)題，因此所提出的算法適用于井下采煤機(jī)快速對(duì)準(zhǔn)。

4 結(jié) 論

(1)研究了采煤機(jī)截割煤壁時(shí)SINS運(yùn)動(dòng)對(duì)準(zhǔn)問(wèn)題。針對(duì)采煤機(jī)動(dòng)基座初始對(duì)準(zhǔn)收斂速度慢及精度低等問(wèn)題，提出了1種基于頻域分離算子的采煤機(jī)動(dòng)基座SINS初始對(duì)準(zhǔn)算法，該方法可以有效地抑制高頻震動(dòng)干擾。

(2)所提出的算法具有更快的收斂速度以及更高的精度，俯仰角和橫滾角的穩(wěn)態(tài)誤差小于0.1°，航向角穩(wěn)態(tài)誤差小于0.7°，滿足后續(xù)精對(duì)準(zhǔn)的要求，因此所提出的算法對(duì)于井下采煤機(jī)SINS初始對(duì)準(zhǔn)問(wèn)題具有廣泛的應(yīng)用前景。