張軒軒, 楊誠(chéng)

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

0 引 言

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

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

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

1.1 坐標(biāo)系定義

SINS姿態(tài)對(duì)準(zhǔn)的目的是為了獲取載體坐標(biāo)系與參考坐標(biāo)系之間的關(guān)系. 本文的載體坐標(biāo)系(b)為右前上坐標(biāo)系, 其原點(diǎn)位于慣性測(cè)量裝置(IMU)測(cè)量中心,x軸指向載體的右方,y軸指向載體運(yùn)動(dòng)的方向,z軸指向載體上方,構(gòu)成右手坐標(biāo)系. 為了討論問(wèn)題方便, 一般令導(dǎo)航坐標(biāo)系(n)與當(dāng)?shù)厮阶鴺?biāo)系(l)重合; 另一個(gè)關(guān)注的坐標(biāo)系是當(dāng)?shù)厮阶鴺?biāo)系,該坐標(biāo)系為東北天坐標(biāo)系,其原點(diǎn)為運(yùn)載體的中心,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)誤差角為小角度. 因此,誤差方程可以通過(guò)在經(jīng)典慣性導(dǎo)航力學(xué)編排的基礎(chǔ)上施加小的誤差擾動(dòng)獲得[9],如式(2)～(4)所示.

(2)

(3)

(4)

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

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

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

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

(5)

式中，δ(·)代表狀態(tài)的誤差,本文以δvE、δvN為觀測(cè)量.因此,卡爾曼濾波的離散化狀態(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時(shí)刻的卡爾曼濾波的狀態(tài)矢量;Xk為k時(shí)刻的卡爾曼濾波的狀態(tài)矢量;Wk為系統(tǒng)過(guò)程的噪聲向量;Hk為卡爾曼濾波觀測(cè)方程的系數(shù)矩陣;Vk為速度噪聲觀測(cè)向量;L為當(dāng)?shù)鼐暥?Ω為地球自轉(zhuǎn)角速率;R0為地球半徑.

1.3 小波降噪

圖1 小波分解示意圖

2 仿真與實(shí)驗(yàn)測(cè)試

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

表1 傳感器參數(shù)

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

(12)

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

(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軸角速度降噪前后變化圖

對(duì)3個(gè)慣性器件的實(shí)測(cè)數(shù)據(jù)與MPU-6700仿真數(shù)據(jù)進(jìn)行功率譜分析,由于陀螺儀的精度對(duì)慣性導(dǎo)航的導(dǎo)航精度影響較大,本文選取各個(gè)慣性器件陀螺儀的z軸輸出角速度進(jìn)行研究,各個(gè)數(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可知,各個(gè)慣性器件在小波降噪前的高頻噪聲比較大,但經(jīng)過(guò)小波分析處理,其高頻噪聲得到了有效抑制,保留了低頻有效信息.

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

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

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

最后利用卡爾曼濾波精對(duì)準(zhǔn)的方法進(jìn)行姿態(tài)的解算,得到三種慣性器件姿態(tài)角隨時(shí)間的變化情況.進(jìn)行卡爾曼濾波精對(duì)準(zhǔn)時(shí)三種慣性器件的濾波初始狀態(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),動(dòng)力學(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)；

動(dòng)力學(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)；

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

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

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

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

(a) ATLANS航偏角變化圖

(b) ATLANS俯仰角變化圖

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

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

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

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

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

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

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

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

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

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

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

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

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

綜合圖10、11、12,由圖10(a),圖11(a),圖12(a)可以清楚地看到ATLANS的航偏角很快收斂到了一個(gè)固定角度的附近,3DM-GX5-25的航偏角則用了相對(duì)較長(zhǎng)的時(shí)間才收斂,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é)束語(yǔ)

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

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

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

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

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