李小燕

(1.中國煤炭科工集團太原研究院有限公司，太原 030006；2.煤礦采掘機械裝備國家工程實驗室，太原 030032；3.山西天地煤機裝備有限公司，太原 030032)

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

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

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

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

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

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

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

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

1.2 加速度計輸出

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

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

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

R0=R1+R2

(1)

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

(2)

(3)

(4)

(5)

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

(6)

f=a-g

(7)

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

(8)

(9)

(10)

(11)

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

(12)

(13)

(14)

(15)

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

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

(16)

圖4 提取重力加速度投影過程

(17)

(18)

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

(19)

1.3 陀螺儀輸出

陀螺儀為角速率傳感器，通過時間積分后得到角度。陀螺儀隨機漂移模型ε為

ε=εb+εn+wg

(20)

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

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

(21)

(22)

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

2 卡爾曼濾波融合

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

圖5 卡爾曼濾波融合過程

觀測量Z為

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

(23)

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

H=[0I01×13]

(24)

狀態(tài)變量X為

(25)

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

3 算法驗證

3.1 仿真驗證

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

圖6 仿真軌跡

圖7 y軸陀螺角速率輸出

圖8 比力輸出

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

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

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

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

圖11 兩種測量方法誤差

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

圖12 融合算法誤差

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

表1 滾轉(zhuǎn)角測量實驗誤差值

3.2 半物理仿真驗證

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

圖13 車載試驗平臺

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

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

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

表2 滾轉(zhuǎn)角測量實驗誤差值

圖15 三種測量方法誤差

4 結(jié)論

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