李小燕

(1.中國(guó)煤炭科工集團(tuán)太原研究院有限公司，太原 030006；2.煤礦采掘機(jī)械裝備國(guó)家工程實(shí)驗(yàn)室，太原 030032；3.山西天地煤機(jī)裝備有限公司，太原 030032)

對(duì)于姿態(tài)角獲取，中外學(xué)者提出了不同類(lèi)型的計(jì)算方法。文獻(xiàn)[1]所述基于雙目視覺(jué)的物體識(shí)別定位方法，其方法為利用匹配點(diǎn)求取物體母線斜率，再隨機(jī)取斜率等于母線斜率的兩點(diǎn)，通過(guò)兩點(diǎn)的世界坐標(biāo)求出目標(biāo)物體的姿態(tài)。文獻(xiàn)[2]提出速度匹配加機(jī)動(dòng)輔助的滾轉(zhuǎn)角空中對(duì)準(zhǔn)法，機(jī)動(dòng)輔助采取縱向比例導(dǎo)引加重力補(bǔ)償制導(dǎo)律形式，結(jié)合卡爾曼濾波完成滾轉(zhuǎn)角的空中對(duì)準(zhǔn)。文獻(xiàn)[3]提出的姿態(tài)估計(jì)方法，利用地磁強(qiáng)度和方向可以用位置函數(shù)求解姿態(tài)的特性以及捷聯(lián)慣導(dǎo)系統(tǒng)的姿態(tài)角信息進(jìn)行融合獲取姿態(tài)估計(jì)。

而對(duì)于特殊環(huán)境的姿態(tài)角獲取，視覺(jué)方法對(duì)于物體變化狀態(tài)難以實(shí)時(shí)捕捉，全球定位系統(tǒng)(global positioning system，GPS)信號(hào)存在失鎖的情況，地磁信息極易受到強(qiáng)磁干擾等，而捷聯(lián)慣導(dǎo)具有自主性強(qiáng)、不受外界干擾、實(shí)時(shí)性好的優(yōu)點(diǎn)，對(duì)于捷聯(lián)慣導(dǎo)系統(tǒng)姿態(tài)解算，文獻(xiàn)[4]采用四元數(shù)解算姿態(tài)角，利用 Mahony 算法進(jìn)行誤差修正，實(shí)現(xiàn)多維度參數(shù)獲取運(yùn)動(dòng)過(guò)程目標(biāo)的姿態(tài)，能夠有效實(shí)現(xiàn)被測(cè)對(duì)象的姿態(tài)檢測(cè)。文獻(xiàn)[5]利用慣性導(dǎo)航原理，采用四元數(shù)法進(jìn)行坐標(biāo)轉(zhuǎn)換，并用低通濾波算法對(duì)采集信號(hào)進(jìn)行處理，對(duì)濾波后的信號(hào)值進(jìn)行解算獲取相關(guān)導(dǎo)航參數(shù)。文獻(xiàn)[6]通過(guò)四元數(shù)卡爾曼濾波算法對(duì)傳感器數(shù)據(jù)進(jìn)行解算、融合，得到桿塔的傾斜角。

而捷聯(lián)慣導(dǎo)系統(tǒng)存在累積誤差，長(zhǎng)時(shí)間導(dǎo)航將失去實(shí)際參考意義的問(wèn)題，所以對(duì)于抑制捷聯(lián)慣導(dǎo)系統(tǒng)的累積誤差，主要有以下幾種濾波方法。文獻(xiàn)[7]針對(duì)傳統(tǒng)四元數(shù)無(wú)味卡爾曼濾波(unscented quaternion Kalman filter, USQUE)算法的量測(cè)噪聲統(tǒng)計(jì)未知及時(shí)變引起濾波發(fā)散精度降低等問(wèn)題，提出一種變分貝葉斯自適應(yīng)四元數(shù)無(wú)味卡爾曼濾波算法(variational Bayesian-based adaptive USQUE, VBAUSQUE)。通過(guò)變分貝葉斯高斯迭代近似估計(jì)，獲取近似的量測(cè)噪聲協(xié)方差矩陣濾波先驗(yàn)條件。文獻(xiàn)[8]提出改進(jìn)的卡爾曼濾波算法，即高階線性互補(bǔ)濾波與擴(kuò)展卡爾曼濾波(extended Kalman filter, EKF)相結(jié)合的融合算法，將互補(bǔ)濾波的姿態(tài)角數(shù)據(jù)作為該系統(tǒng)模型的觀測(cè)值，利用EKF算法對(duì)加速度計(jì)、陀螺儀、磁力計(jì)進(jìn)行數(shù)據(jù)融合。文獻(xiàn)[9]提出了一種基于參考天線位置修正的擴(kuò)展卡爾曼濾波(extended Kalman filter, EKF)算法。根據(jù)載體移動(dòng)速度和接收機(jī)之間的時(shí)鐘差對(duì)每一歷元載體參考天線位置矯正，提高基線解算的精度，從而提高姿態(tài)角精度。文獻(xiàn)[10]提出采用魯棒濾波的方法對(duì)姿態(tài)角進(jìn)行估計(jì)，能夠有效地提高整個(gè)濾波系統(tǒng)的健壯性，實(shí)現(xiàn)快速估計(jì)。

針對(duì)特殊的應(yīng)用環(huán)境，捷聯(lián)慣導(dǎo)系統(tǒng)需要不依靠外界自主獲取精確的滾轉(zhuǎn)角信息，而基于傳統(tǒng)的四元數(shù)捷聯(lián)慣導(dǎo)解算過(guò)程不可避免存在累積誤差，現(xiàn)提出融合雙軸加速度計(jì)和單軸陀螺儀的滾轉(zhuǎn)角測(cè)試方法抑制累積誤差，同時(shí)為提高運(yùn)算實(shí)時(shí)性，避免復(fù)雜的濾波方法導(dǎo)致運(yùn)算速率降低，采用卡爾曼濾波作為最優(yōu)估計(jì)算法。利用雙軸加速度計(jì)通過(guò)重力投影測(cè)量滾轉(zhuǎn)角，單軸陀螺儀通過(guò)角速率積分得到滾轉(zhuǎn)角，將加速度計(jì)測(cè)量得到的滾轉(zhuǎn)角作為觀測(cè)量，通過(guò)卡爾曼濾波抑制單獨(dú)使用陀螺儀測(cè)量滾轉(zhuǎn)角帶來(lái)的累積誤差，從而提高滾轉(zhuǎn)角測(cè)量精度。

1 滾轉(zhuǎn)角測(cè)量原理

1.1 雙軸加速度計(jì)與單軸陀螺儀安裝方式

雙軸加速度計(jì)與單軸陀螺儀在高旋載體內(nèi)的安裝示意圖如圖1所示，γ為滾轉(zhuǎn)角，Xb、Yb、Zb分別為載體坐標(biāo)系的三個(gè)軸向，單軸陀螺儀Gx安裝在縱軸上，加速度計(jì)Ay安裝在垂直于縱向橫截面上，加速度計(jì)Az的安裝與Gx、Ay構(gòu)成右手坐標(biāo)系的方向上[11]。

圖1 傳感器在高旋載體內(nèi)的安裝方式

1.2 加速度計(jì)輸出

載體坐標(biāo)系與慣性坐標(biāo)系的位置關(guān)系如圖2所示，OiXiYiZi為慣性坐標(biāo)系，原點(diǎn)Oi在地心處，OiZi軸與地球自轉(zhuǎn)軸重合指向北極方向，OiXi軸指向春分點(diǎn)，OiYi軸與OiXi軸、OiZi軸構(gòu)成右手坐標(biāo)系；ObXbYbZb為載體坐標(biāo)系，原點(diǎn)Ob選在載體質(zhì)心處，ObXb軸與載體縱軸重合指向載體頭部，ObYb軸在載體縱向?qū)ΨQ(chēng)面內(nèi)垂直于ObXb軸指向上方，ObYb軸與ObXb軸、ObZb軸構(gòu)成右手坐標(biāo)系。

在載體上任意找一點(diǎn)P，在慣性坐標(biāo)系和載體坐標(biāo)系中的分布情況如圖2所示。由圖2可知：

R0為地心Oi到P點(diǎn)的位置矢量；R1為地心Oi到載體質(zhì)心Ob的位置矢量；R2為載體質(zhì)心Ob到P點(diǎn)的位置矢量

R0=R1+R2

(1)

要求取P點(diǎn)處的加速度表達(dá)式，需對(duì)式(1)求二階導(dǎo)數(shù)，為方便表達(dá)，將R0、R1投影到慣性坐標(biāo)系中，R2投影到載體系中，表達(dá)式為

(2)

(3)

(4)

(5)

將式(5)兩邊再次求導(dǎo)并整理得

(6)

f=a-g

(7)

式(7)中：a為P點(diǎn)相對(duì)于慣性系的絕對(duì)加速度矢量；g為地球引力加速度矢量。將式(6)代入式(7)并投影到慣性系中，整理得

(8)

(9)

(10)

(11)

雙軸加速度計(jì)分別安裝在載體坐標(biāo)系的橫軸和豎軸上，豎軸上安裝的加速度計(jì)用Ay表示，橫軸上安裝的加速度計(jì)用Az表示，由式(11)并分別展開(kāi)到載體坐標(biāo)系Xb、Yb、Yb軸上可得

(12)

(13)

(14)

(15)

載體在運(yùn)動(dòng)過(guò)程中，俯仰、滾轉(zhuǎn)會(huì)影響加速度計(jì)輸出，俯仰角用θ表示，滾轉(zhuǎn)角用γ表示，XbYbZb為無(wú)運(yùn)動(dòng)時(shí)的坐標(biāo)系，XbYbZb經(jīng)過(guò)俯仰θ得到X′bY′bZ′b，X′bY′bZ′b經(jīng)過(guò)滾轉(zhuǎn)γ得到X″bY″bZ″b，三軸加速度計(jì)隨俯仰、滾轉(zhuǎn)變化如圖3所示。

圖3 三軸加速度計(jì)隨俯仰、滾轉(zhuǎn)變化示意圖

(16)

圖4 提取重力加速度投影過(guò)程

(17)

(18)

由式(18)可以解得滾轉(zhuǎn)角，雙軸加速度計(jì)提取滾轉(zhuǎn)角為

(19)

1.3 陀螺儀輸出

陀螺儀為角速率傳感器，通過(guò)時(shí)間積分后得到角度。陀螺儀隨機(jī)漂移模型ε為

ε=εb+εn+wg

(20)

式(20)中：εb為逐次啟動(dòng)漂移屬于隨機(jī)常值；εn為一階馬爾科夫過(guò)程屬于慢變漂移；wg為白噪聲屬于快變漂移。

微機(jī)電系統(tǒng)(micro-electro-mechanical system, MEMS)陀螺儀累積誤差主要由白噪聲和一階馬爾科夫過(guò)程引起，誤差表達(dá)式為

(21)

(22)

式中：wn為驅(qū)動(dòng)白噪聲；τg為馬爾科夫過(guò)程驅(qū)動(dòng)相時(shí)間。

2 卡爾曼濾波融合

由于加速度計(jì)存在隨機(jī)噪聲，單獨(dú)使用兩軸加速度計(jì)測(cè)量滾轉(zhuǎn)角影響滾轉(zhuǎn)角測(cè)量精度，但是誤差不會(huì)隨時(shí)間累積；單獨(dú)使用陀螺儀測(cè)量滾轉(zhuǎn)角短時(shí)測(cè)量精度高，但隨著時(shí)間加長(zhǎng)，存在累積誤差。通過(guò)卡爾曼濾波將兩軸加速度計(jì)測(cè)量得到的滾轉(zhuǎn)角與陀螺測(cè)量得到的滾轉(zhuǎn)角融合，進(jìn)行優(yōu)勢(shì)互補(bǔ)，獲得高精度的滾轉(zhuǎn)角，融合過(guò)程如圖5所示。

圖5 卡爾曼濾波融合過(guò)程

觀測(cè)量Z為

Z=[γ1-γ2]=HX+V

(23)

用I表示單位矩陣，其中觀測(cè)方程系數(shù)H為

H=[0I01×13]

(24)

狀態(tài)變量X為

(25)

在此卡爾曼濾波融合算法中，將預(yù)測(cè)值與狀態(tài)量通過(guò)量測(cè)更新更新?tīng)顟B(tài)量，同時(shí)計(jì)算陀螺累積誤差，最后將得到的累積誤差進(jìn)行對(duì)應(yīng)補(bǔ)償，得到精確的滾轉(zhuǎn)角信息。

3 算法驗(yàn)證

3.1 仿真驗(yàn)證

為驗(yàn)證算法，進(jìn)行了仿真實(shí)驗(yàn)，設(shè)置初始俯仰角5°，航向北偏西30°，初始轉(zhuǎn)速為8 r/s，目標(biāo)點(diǎn)坐標(biāo)為(2 504，7.227，0.999 1)，仿真軌跡如圖6所示。通過(guò)反演信號(hào)方法獲取傳感器的原始值[12]，仿真過(guò)程中需加入傳感器噪聲，加速度計(jì)選取MS9000加速度計(jì)的噪聲，陀螺儀選取ADXRS649單軸陀螺儀的噪聲。

圖6 仿真軌跡

圖7 y軸陀螺角速率輸出

圖8 比力輸出

圖9 y、z軸加速度輸出低通濾波結(jié)果

實(shí)驗(yàn)結(jié)果分別用加速度計(jì)測(cè)量滾轉(zhuǎn)角，陀螺加速率積分測(cè)量滾轉(zhuǎn)角，融合算法滾轉(zhuǎn)角測(cè)量進(jìn)行滾轉(zhuǎn)角解算，為了對(duì)比方便，初始滾轉(zhuǎn)角設(shè)置為0，實(shí)驗(yàn)結(jié)果如圖10所示。

圖10 三種滾轉(zhuǎn)角測(cè)量結(jié)果

三種滾轉(zhuǎn)角解算結(jié)果與基準(zhǔn)滾轉(zhuǎn)角作對(duì)比，誤差曲線如圖11所示，可以看出加速度計(jì)測(cè)量由于隨機(jī)噪聲及低通濾波不能完全濾除高頻信號(hào)，導(dǎo)致滾轉(zhuǎn)角測(cè)量有較大的誤差，陀螺測(cè)量滾轉(zhuǎn)角誤差隨時(shí)間增大。

圖11 兩種測(cè)量方法誤差

從圖12中可以看出，融合算法可以很快地將誤差收斂在0.5°～1.2°范圍內(nèi)，而單獨(dú)采用陀螺進(jìn)行滾轉(zhuǎn)角測(cè)量具有累積誤差，在實(shí)驗(yàn)結(jié)束時(shí)誤差達(dá)到-4.51°，可以看出融合算法有效地抑制了陀螺漂移帶來(lái)的誤差。

圖12 融合算法誤差

從RMSE誤差、誤差平均值、最大誤差，分析三種測(cè)量方法，從表1可以看出，卡爾曼濾波融合算法均比加計(jì)測(cè)量或者陀螺測(cè)量方法有所提高。

表1 滾轉(zhuǎn)角測(cè)量實(shí)驗(yàn)誤差值

3.2 半物理仿真驗(yàn)證

為驗(yàn)證算法的實(shí)際工程應(yīng)用可行性，進(jìn)行了地面樣機(jī)跑車(chē)實(shí)驗(yàn)。圖13為車(chē)載試驗(yàn)平臺(tái)，該試驗(yàn)平臺(tái)搭載了高精度車(chē)載轉(zhuǎn)臺(tái)，在地面可以模擬高速旋轉(zhuǎn)實(shí)時(shí)高動(dòng)態(tài)環(huán)境運(yùn)動(dòng)過(guò)程,同時(shí)車(chē)載試驗(yàn)平臺(tái)搭載了加拿大 NovAtel 公司的高精度定位定向系統(tǒng) span-lci 提供實(shí)時(shí)導(dǎo)航參數(shù)對(duì)比基準(zhǔn)。

圖13 車(chē)載試驗(yàn)平臺(tái)

實(shí)驗(yàn)分別用加速度計(jì)測(cè)量滾轉(zhuǎn)角，陀螺角速率積分測(cè)量滾轉(zhuǎn)角，滾轉(zhuǎn)角空中初對(duì)準(zhǔn)方法測(cè)量進(jìn)行滾轉(zhuǎn)角解算，為對(duì)比明確，將三種方法測(cè)量所得滾轉(zhuǎn)角減去轉(zhuǎn)臺(tái)反饋，將最后結(jié)果與車(chē)載參考系統(tǒng)輸出進(jìn)行對(duì)比，實(shí)驗(yàn)結(jié)果如圖14所示。三種滾轉(zhuǎn)角解算結(jié)果與基準(zhǔn)滾轉(zhuǎn)角作對(duì)比，誤差曲線如圖15所示，可以看出加速度計(jì)測(cè)量滾轉(zhuǎn)角算法存在有較大的誤差，而單獨(dú)采用陀螺進(jìn)行滾轉(zhuǎn)角測(cè)量具有累積誤差，而且誤差隨時(shí)間累積越來(lái)越大，在實(shí)驗(yàn)結(jié)束時(shí)誤差達(dá)到 7.6°，而融合算法地將誤差收斂在 ±1° 范圍內(nèi)，可以看出融合算法有效地抑制了陀螺帶來(lái)的累積誤差，并且也比其他兩種滾轉(zhuǎn)角測(cè)量方法精度高。

圖14 三種算法滾轉(zhuǎn)角測(cè)量結(jié)果

根據(jù)圖15所示的模擬車(chē)載試驗(yàn)數(shù)據(jù)進(jìn)行結(jié)果分析，滾轉(zhuǎn)角測(cè)量實(shí)驗(yàn)誤差總結(jié)如表2所示，分別以平均值、最大值、RMSE值(1σ標(biāo)準(zhǔn))誤差三個(gè)方面分析三種測(cè)量方法誤差，融合算法均比加計(jì)測(cè)量、滾轉(zhuǎn)角測(cè)量精度高，證明在實(shí)際工程應(yīng)用上，滾轉(zhuǎn)角空中初對(duì)準(zhǔn)方法具有可行性。

表2 滾轉(zhuǎn)角測(cè)量實(shí)驗(yàn)誤差值

圖15 三種測(cè)量方法誤差

4 結(jié)論

通過(guò)建立卡爾曼濾波模型融合加速度計(jì)和陀螺儀輸出信息，利用加速度計(jì)的輸出信息作為觀測(cè)量，抑制陀螺測(cè)量滾轉(zhuǎn)角造成的累積誤差，并將誤差收斂在一定范圍內(nèi)，有效地提高了滾轉(zhuǎn)角測(cè)量精度。并通過(guò)仿真實(shí)驗(yàn)與半物理仿真實(shí)驗(yàn)驗(yàn)證了該算法，實(shí)驗(yàn)表明卡爾曼融合算法精度比單獨(dú)使用陀螺或者加計(jì)精度均有提高，經(jīng)仿真證明了理論模型的正確性和工程實(shí)現(xiàn)的可行性。