張軒軒, 楊誠

(中國地質(zhì)大學(xué)(北京) 土地科學(xué)技術(shù)學(xué)院,北京 100083)

0 引 言

捷聯(lián)式慣性導(dǎo)航系統(tǒng)(SINS)相對于平臺式慣性導(dǎo)航系統(tǒng)具有體積小、成本低等特點,已經(jīng)廣泛應(yīng)用于各種軍民導(dǎo)航領(lǐng)域. SINS通過加速度計和陀螺儀等慣性量測器件,測量載體的加速度和角速度[1]. 系統(tǒng)通過對載體加速度的積分獲得速度信息,再次對速度進行積分,獲得載體的位置增量信息. 然而,慣性導(dǎo)航系統(tǒng)需要事先獲取載體的初始狀態(tài)信息,包括載體的位置,速度和姿態(tài),初始狀態(tài)的任何誤差都將無條件帶入后續(xù)的動態(tài)導(dǎo)航定位中. 初始狀態(tài)一般可以通過慣性導(dǎo)航的對準(zhǔn)來實現(xiàn). 慣性導(dǎo)航初始對準(zhǔn)中的姿態(tài)對準(zhǔn)是為了估計載體坐標(biāo)系相對于參考坐標(biāo)系的指向,并獲得載體坐標(biāo)系到參考坐標(biāo)系的旋轉(zhuǎn)矩陣. 該矩陣在導(dǎo)航過程中,將慣性傳感器量測到的比力和角速度從載體系投影到參考坐標(biāo)系中. 因此,對準(zhǔn)速度和精度決定了慣性導(dǎo)航系統(tǒng)后續(xù)導(dǎo)航精度.

捷聯(lián)式慣性導(dǎo)航傳感器的對準(zhǔn)過程可以分為粗對準(zhǔn)和精對準(zhǔn)兩個部分. 粗對準(zhǔn)一般采用解析的方法獲取姿態(tài)角的初始值,然后使用精對準(zhǔn)對姿態(tài)角進行估計[2-4]. 由于慣性導(dǎo)航系統(tǒng)不依賴外界條件,可以通過自主量測的數(shù)據(jù)實現(xiàn)自主導(dǎo)航的系統(tǒng),因此,對于高精度的慣性傳感器,可以通過自身量測數(shù)據(jù)感知地球重力與地球自轉(zhuǎn),再通過卡爾曼濾波進行精對準(zhǔn),此過程稱為自對準(zhǔn)[5]. 然而,對于中低精度的慣性傳感器,由于自身的噪聲較大,無法高精度地量測地球自轉(zhuǎn),需要通過輔助的觀測信息進行精對準(zhǔn). El-Sheimy[6]提出利用多層小波分解法削弱傳感器量測數(shù)據(jù)的高頻噪聲. 該方法有效提高了對準(zhǔn)精度和效率. 目前,精對準(zhǔn)一般是基于慣性傳感器的小失準(zhǔn)角線性誤差方程,通過卡爾曼濾波和慣性傳感器的小失準(zhǔn)角線性誤差方程實現(xiàn)的[7]. 本文以高、中、低三種精度的捷聯(lián)式慣性傳感器為研究對象,利用卡爾曼濾波分析比較三種精度的慣性傳感器的精對準(zhǔn)性能.

1 精對準(zhǔn)及小波降噪原理

1.1 坐標(biāo)系定義

SINS姿態(tài)對準(zhǔn)的目的是為了獲取載體坐標(biāo)系與參考坐標(biāo)系之間的關(guān)系. 本文的載體坐標(biāo)系(b)為右前上坐標(biāo)系, 其原點位于慣性測量裝置(IMU)測量中心,x軸指向載體的右方,y軸指向載體運動的方向,z軸指向載體上方,構(gòu)成右手坐標(biāo)系. 為了討論問題方便, 一般令導(dǎo)航坐標(biāo)系(n)與當(dāng)?shù)厮阶鴺?biāo)系(l)重合; 另一個關(guān)注的坐標(biāo)系是當(dāng)?shù)厮阶鴺?biāo)系,該坐標(biāo)系為東北天坐標(biāo)系,其原點為運載體的中心,z軸向上指向天頂,x軸指向地理東向,y軸垂直于x軸與z軸構(gòu)成的平面成右手坐標(biāo)系.

(1)

式中，φ、θ、ψ分別表示橫滾角、俯仰角和航向角.

1.2 小失準(zhǔn)角誤差模型

小失準(zhǔn)角誤差模型是慣性導(dǎo)航系統(tǒng)的經(jīng)典誤差模型,其核心是假設(shè)姿態(tài)誤差角為小角度. 因此,誤差方程可以通過在經(jīng)典慣性導(dǎo)航力學(xué)編排的基礎(chǔ)上施加小的誤差擾動獲得[9],如式(2)～(4)所示.

(2)

(3)

(4)

式中，D-1為位置誤差方程計算矩陣.

靜基座條件下,由于傳感器在靜止?fàn)顟B(tài)下,νn=[0,0,0]T,理論上,位置分量誤差為零,因此式(4)可以省略.

因此,取導(dǎo)航坐標(biāo)系為東北天坐標(biāo)系時,慣性系統(tǒng)的狀態(tài)向量為[10]

Xk-1=[φE,φN,φU,δvE,δvN]T,

(5)

式中，δ(·)代表狀態(tài)的誤差,本文以δvE、δvN為觀測量.因此,卡爾曼濾波的離散化狀態(tài)模型為

(6)

其中：

(7)

wN=ΩcosL+vE/R0,

(8)

wE=vN/R0,

(9)

wD=ΩsinL-vEtanL/R0,

(10)

(11)

式中:φk,k-1為卡爾曼濾波的系數(shù)轉(zhuǎn)移矩陣;Xk-1為k-1時刻的卡爾曼濾波的狀態(tài)矢量;Xk為k時刻的卡爾曼濾波的狀態(tài)矢量;Wk為系統(tǒng)過程的噪聲向量;Hk為卡爾曼濾波觀測方程的系數(shù)矩陣;Vk為速度噪聲觀測向量;L為當(dāng)?shù)鼐暥?Ω為地球自轉(zhuǎn)角速率;R0為地球半徑.

1.3 小波降噪

圖1 小波分解示意圖

2 仿真與實驗測試

為了對比分析高中低三種精度的慣性傳感器的對準(zhǔn)精度,同時考慮到低精度慣性導(dǎo)航器件受外部噪聲影響較大,本文采用實測數(shù)據(jù)和低精度慣性器件仿真數(shù)據(jù)進行對準(zhǔn)性能分析.實測數(shù)據(jù)由ATLANS、3DM-GX5-25和MPU-6700三種慣性傳感器采集,數(shù)據(jù)采樣頻率均為100 Hz. 三種傳感器的性能參數(shù)如表1所示. 同時使用相同參數(shù)生成MPU-6700仿真數(shù)據(jù)進行對比分析.

表1 傳感器參數(shù)

對于數(shù)據(jù)采樣時間通常是由對準(zhǔn)所需的時間來決定的,而對準(zhǔn)時間與對準(zhǔn)所需達到的精度存在以下關(guān)系:

(12)

式中:ARW表示角度隨機游走;we為地球自轉(zhuǎn)角速度;φ為運載體所在的緯度;δA是方位對準(zhǔn)誤差;Ta是對準(zhǔn)時間. 本文所需要達到的方位對準(zhǔn)誤差為0.01°/h,通過計算可知,ATLANS對準(zhǔn)時間需要4 s,3DM-GX5-25對準(zhǔn)時間需要10 903 s,MPU-6700對準(zhǔn)時間需要24 532 s. 因此,數(shù)據(jù)采樣時長比對準(zhǔn)時間長即可,本文采樣時長分別為:10 000 s,11 000 s, 500 000 s.對高頻噪聲,本文采用文獻[11]AB中的函數(shù)命令wden,選用db4小波基,分解層數(shù)為5. 經(jīng)小波分析處理后,各個慣性器件的有效輸出信號均明顯降低,如圖2～5所示. 可以發(fā)現(xiàn),MPU-6700去噪后的信號在0～500000 s間有明顯的非線性特征,這是仿真數(shù)據(jù)和另外兩種傳感器輸出的數(shù)據(jù)所沒有的.

(a) ATLANS z軸角速度降噪前變化圖

(b) ATLANS z軸角速度降噪前變化圖圖2 ATLANS z軸角速度降噪前后變化圖

(a) 3DM-GX5-25 z軸角速度降噪前變化圖

(b) 3DM-GX5-25 z軸角速度降噪后變化圖圖3 3DM-GX5-25 z軸角速度降噪前后變化圖

(a) MPU-6700 z軸角速度降噪前變化圖

(b) MPU-6700仿真z軸角速度降噪后變化圖圖5 MPU-6700仿真z軸角速度降噪前后變化圖

對3個慣性器件的實測數(shù)據(jù)與MPU-6700仿真數(shù)據(jù)進行功率譜分析,由于陀螺儀的精度對慣性導(dǎo)航的導(dǎo)航精度影響較大,本文選取各個慣性器件陀螺儀的z軸輸出角速度進行研究,各個數(shù)據(jù)的功率譜分析如圖6～9所示.

(a) ATLANS z軸角速度降噪前頻譜分析圖

(b) ATLANS z軸角速度降噪后頻譜分析圖圖6 ATLANS z軸角速度降噪前后頻譜分析圖

(a) 3DM-GX5-25 z軸角速度降噪前頻譜分析圖

(b) 3DM-GX5-25 z軸角速度降噪后頻譜分析圖圖7 3DM-GX5-25 z軸角速度降噪前后頻譜分析圖

(a) MPU-6700 z軸角速度降噪前頻譜分析圖

(b) MPU-6700 z軸角速度降噪后頻譜分析圖圖8 MPU-6700 z軸角速度降噪前后頻譜分析圖

(a) MPU-6700仿真z軸角速度降噪前頻譜分析圖

(b) MPU-6700仿真z角速度降噪后頻譜分析圖圖9 MPU-6700仿真z軸角速度降噪前后頻譜分析圖

由圖6～9可知,各個慣性器件在小波降噪前的高頻噪聲比較大,但經(jīng)過小波分析處理,其高頻噪聲得到了有效抑制,保留了低頻有效信息.

數(shù)據(jù)經(jīng)小波降噪后,通過粗對準(zhǔn)獲取姿態(tài)角的初值,如表2所示.

一個完善的制度體系可以給管理者們起到良好的輔助作用，更可以讓員工約束其自身，促進工程進度，打消員工消極怠工的狀態(tài)，增強團隊的管理體系，完善制度從以下幾方面入手：

表2 粗對準(zhǔn)姿態(tài)角

最后利用卡爾曼濾波精對準(zhǔn)的方法進行姿態(tài)的解算,得到三種慣性器件姿態(tài)角隨時間的變化情況.進行卡爾曼濾波精對準(zhǔn)時三種慣性器件的濾波初始狀態(tài)均為X=[10°,10°,10°,0°,0°]T,慣性器件ATLANS的狀態(tài)協(xié)方差矩陣為

P=diag((0.00625°/h)2,(0.00625°/h)2,(0.00625°/h)2,(100 μg)2,(100 μg)2),動力學(xué)模型噪聲矩陣為

Q=diag((0.009899°/h)2,(0.009899°/h)2,(0.009899°/h)2,(50 μg)2,(50 μg)2);

3DM-GX5-25狀態(tài)協(xié)方差矩陣為

P=diag((8°/h)2,(8°/h)2,(8°/h)2,(200 μg)2,(200 μg)2)；

動力學(xué)模型噪聲矩陣為

Q=diag((1.2°/h)2,(1.2°/h)2,(1.2°/h)2,(180 μg)2,(180 μg)2);

慣性器件MPU-6700狀態(tài)協(xié)方差矩陣為

P=diag((10.33°/h)2,(10.33°/h)2,(10.33°/h)2,(3300 μg)2,(3300 μg)2)；

動力學(xué)模型噪聲矩陣為

Q=diag((1.8°/h)2,(1.8°/h)2,(1.8°/h)2,(2880 μg)2,(2880 μg)2).

上述矩陣P和Q中前3項分別為各慣性器件陀螺儀的零偏和隨機誤差,后兩項為加速度計的零偏和隨機誤差.

仿真數(shù)據(jù)及實測數(shù)據(jù)的對準(zhǔn)結(jié)果如圖10～13所示.

(a) ATLANS航偏角變化圖

(b) ATLANS俯仰角變化圖

(c) ATLANS翻滾角變化圖圖10 ATLANS實測數(shù)據(jù)精對準(zhǔn)結(jié)果圖

圖10示出的是ATLANS實測數(shù)據(jù)進行卡爾曼濾波精對準(zhǔn)的姿態(tài)結(jié)果.由圖10(a)、(b)、(c)可知,ATLANS慣性器件的姿態(tài)角在最開始的時候是發(fā)散的,但是經(jīng)過一段時間后開始收斂到固定的角度或固定角度附近,收斂的一致性比較好.

(a) 3DM-GX5-25航偏角變化圖

(b) 3DM-GX5-25俯仰角變化圖

(c) 3DM-GX5-25翻滾角變化圖圖11 3DM-GX5-25實測數(shù)據(jù)精對準(zhǔn)結(jié)果圖

圖11示出的是3DM-GX5-25實測數(shù)據(jù)和精對準(zhǔn)的姿態(tài)結(jié)果,由圖11(a)、(b)、(c)可知,3DM-GX5-25慣性器件的姿態(tài)角最后均收斂到了一個固定角度或其附近,但可以明顯看到,其收斂速度慢,同時其翻滾角收斂時仍存在較ATLANS的翻滾角大的震動.

(a) MPU-6700航偏角變化圖

(b) MPU-6700俯仰角變化圖

(c) MPU-6700翻滾角變化圖圖12 MPU-6700實測數(shù)據(jù)精對準(zhǔn)結(jié)果圖

(a) MPU-6700仿真數(shù)據(jù)航偏角變化圖

(b) MPU-6700仿真數(shù)據(jù)俯仰角變化圖

(c) MPU-6700仿真數(shù)據(jù)翻滾角變化圖圖13 MPU-6700仿真數(shù)據(jù)精對準(zhǔn)結(jié)果圖

圖12和圖13示出的是MPU-6700實測數(shù)據(jù)和仿真數(shù)據(jù)的精對準(zhǔn)結(jié)果圖,由圖12(a)、(b)、(c)可以直觀看出,MPU-6700慣性器件的三個姿態(tài)角隨時間的變化在不斷振蕩,最終無法收斂,而由圖13可以看出仿真數(shù)據(jù)能夠最終收斂于固定的角度附近.造成該現(xiàn)象的原因是低精度傳感器不僅僅受到白噪聲的影響,而且受其他復(fù)雜特性誤差的影響(如非線性誤差),其數(shù)據(jù)的低頻分量有著明顯的非線性特性. 而仿真數(shù)據(jù)受到的干擾單一,能夠快速收斂.

綜合圖10、11、12,由圖10(a),圖11(a),圖12(a)可以清楚地看到ATLANS的航偏角很快收斂到了一個固定角度的附近,3DM-GX5-25的航偏角則用了相對較長的時間才收斂,MPU-6700的航偏角則完全振蕩、發(fā)散;由圖10(b),圖11(b),圖12(b)可以看出ATLANS和3DM-GX5-25的俯仰角均較快收斂到了各自固定角度附近,而MPU-6700的航偏角則依舊振蕩、發(fā)散;由圖10(c),圖11(c),圖12(c)可以看出ATLANS和3DM-GX5-25的翻滾角均收斂到了各自固定角度附近,但3DM-GX5-25翻滾角的振蕩較ATLANS的翻滾角劇烈,MPU-6700的翻滾角則依舊振蕩、發(fā)散.

3 結(jié)束語

論文通過卡爾曼濾波對低、中、高三種不同性能的慣性導(dǎo)航器件進行基于以小波分析的精對準(zhǔn)性能分析,最后對精對準(zhǔn)結(jié)果的三個姿態(tài)角進行對比分析,得到如下結(jié)論:

1)小波分析方法可以有效抑制噪聲信號影響,提取出對精對準(zhǔn)有用的信號,去噪效果明顯.

2)性能較好的慣性導(dǎo)航器件其精對準(zhǔn)的速度較快,性能較差的慣性導(dǎo)航器件精對準(zhǔn)的速度較慢.

3)性能高的慣性傳感器有著較好的對準(zhǔn)精度;而低精度的傳感器,由于噪聲及誤差的原因,對準(zhǔn)精度較低,甚至出現(xiàn)濾波發(fā)散情況.

4)性能較低的慣性傳感器由于受復(fù)雜特性誤差的影響無法對準(zhǔn),而采用相同噪聲參數(shù)仿真的數(shù)據(jù)可以實現(xiàn)較好的對準(zhǔn).