晃動基座下正向-正向回溯初始對準(zhǔn)方法

徐 祥，徐大誠

(1. 蘇州大學(xué)電子信息學(xué)院,蘇州 215100；2. 蘇州大學(xué)微納傳感技術(shù)研究中心,蘇州 215100)

0 引 言

捷聯(lián)慣性導(dǎo)航系統(tǒng)具有自主性高、隱蔽性好、數(shù)據(jù)更新率高等優(yōu)點，成為艦船、車用、消費類電子等產(chǎn)品定姿、定速、定位的首選系統(tǒng)[1-4]。捷聯(lián)慣性導(dǎo)航系統(tǒng)在進入導(dǎo)航定位之前，通常需要進行初始對準(zhǔn)過程，以實現(xiàn)初始姿態(tài)角的獲取。當(dāng)前常用的初始對準(zhǔn)過程可以分為“粗對準(zhǔn)+精對準(zhǔn)”模式以及非線性濾波的大方位失準(zhǔn)角初始對準(zhǔn)模型[5-8]。文獻[9]提出了一種七階正交容積卡爾曼濾波(Cubature quadrature kalman filter,CQKF)算法，實現(xiàn)了SINS大失準(zhǔn)角初始對準(zhǔn)。然而，采用非線性濾波方法進行大失準(zhǔn)角條件下的初始對準(zhǔn)時，會導(dǎo)致計算量大、濾波不穩(wěn)定、收斂速度慢等問題，使得其在工程應(yīng)用中受到限制[10]。因此，大量研究均圍繞“粗對準(zhǔn)+精對準(zhǔn)”模式展開。

傳統(tǒng)“粗對準(zhǔn)+精對準(zhǔn)”采用兩個獨立的過程進行，即粗對準(zhǔn)過程主要完成未知姿態(tài)角的粗估計[11-12]。文獻[2]提出了一種靜基座上自對準(zhǔn)方法，文獻[7]實現(xiàn)了晃動基座上的粗對準(zhǔn)過程。但這些方法都只是討論了不同狀態(tài)下的粗對準(zhǔn)，無法實現(xiàn)姿態(tài)誤差的最小化。因此，需要通過粗對準(zhǔn)過程，建立小角度誤差的導(dǎo)航系線性誤差模型，再利用Kalman濾波實現(xiàn)姿態(tài)角的進一步估計，從而完成初始對準(zhǔn)過程[13-14]。

近年來，隨著計算機技術(shù)的不斷進步，其存儲容量和計算速度都得到了較快的發(fā)展，這也為基于回溯原理的快速初始對準(zhǔn)奠定了基礎(chǔ)[15-19]。但這些方法都只是在粗對準(zhǔn)或精對準(zhǔn)中分別進行回溯，沒有充分將兩者進行融合，使得存儲過程冗余。另外，由于計算機在讀取存儲數(shù)據(jù)時，通常采用正向數(shù)據(jù)流形式，這使得傳統(tǒng)正逆向回溯過程在實時算法設(shè)計時較為復(fù)雜。而且，正逆向?qū)Ш叫枰鎯λ袝r刻的慣性傳感器數(shù)據(jù)，這導(dǎo)致對存儲空間和計算速度要求較高，不利于快速對準(zhǔn)的實現(xiàn)。基于傳統(tǒng)正逆向?qū)Ш交厮莩跏紝?zhǔn)方法存在的問題，本文將在導(dǎo)航慣性系粗對準(zhǔn)的基礎(chǔ)上，提出一種基于導(dǎo)航慣性系誤差模型的精對準(zhǔn)方法，并利用時不變姿態(tài)誤差估計方法，設(shè)計正向-正向回溯初始對準(zhǔn)，解決傳統(tǒng)回溯對準(zhǔn)方法設(shè)計復(fù)雜度高、數(shù)據(jù)存儲量大、計算效率低的問題，實現(xiàn)快速初始對準(zhǔn)目標(biāo)。

1 常用坐標(biāo)系定義

為方便后續(xù)工作，基于正向-正向回溯原理的初始對準(zhǔn)技術(shù)涉及到以下幾個坐標(biāo)系，分別定義為[3]：

1) 地心慣性坐標(biāo)系(i系)：選取初始對準(zhǔn)起始時刻地球系作為初始地心慣性系，并且不隨地球運動而改變。

2) 導(dǎo)航坐標(biāo)系(n系)：選取“東-北-天”方向為導(dǎo)航坐標(biāo)系。

3) 導(dǎo)航慣性坐標(biāo)系(n0系)：初始對準(zhǔn)起始時刻導(dǎo)航坐標(biāo)系，并且相對慣性系不變。

4) 載體坐標(biāo)系(b系)：定義“右-前-上”為載體坐標(biāo)系。

5) 載體慣性坐標(biāo)系(b0系)：初始對準(zhǔn)起始時刻載體坐標(biāo)系，并且相對慣性系保持不變。

2 導(dǎo)航慣性系初始對準(zhǔn)原理

為實現(xiàn)正向-正向回溯初始對準(zhǔn)過程，本文將在n0系下建立系統(tǒng)誤差模型，并以此模型設(shè)計濾波算法，實現(xiàn)回溯對準(zhǔn)。初始對準(zhǔn)過程將分為粗對準(zhǔn)和精對準(zhǔn)兩部分。

2.1 導(dǎo)航慣性系粗對準(zhǔn)

(1)

式中：

(2)

由慣導(dǎo)比力方程可知：

(3)

當(dāng)載體處于晃動基座上時，可以假設(shè)載體無線速度運動，因此，式(3)可以采用矢量觀測器表示

(4)

式中：

(5)

假設(shè)陀螺儀量測和加速度計量測表示為：

(6)

采用陀螺儀量測和加速度計量測構(gòu)造矢量，可以得到

(7)

由式(2)可知：

(8)

由最優(yōu)基姿態(tài)確定算法可知[16-17]：

(9)

2.2 導(dǎo)航慣性系精對準(zhǔn)

為了得到更高的初始對準(zhǔn)精度，通常在粗對準(zhǔn)之后需要進行精對準(zhǔn)。傳統(tǒng)的初始對準(zhǔn)方法中，精對準(zhǔn)過程與粗對準(zhǔn)過程通?；ゲ幌嚓P(guān)，粗對準(zhǔn)只為精對準(zhǔn)提供一個粗略的姿態(tài)，精對準(zhǔn)過程需要繼續(xù)采樣傳感器輸出,從而實現(xiàn)姿態(tài)誤差的估計。這一過程中沒有充分利用粗對準(zhǔn)過程積累的有效數(shù)據(jù)，導(dǎo)致對準(zhǔn)過程延長。同時，現(xiàn)代計算機的處理能力已能夠在一個傳感器采樣周期內(nèi)進行多個濾波環(huán)節(jié)的運算，若需要等待傳感器采樣更新才能進行一次姿態(tài)誤差角估計更新，則大大浪費了計算機的處理速度，也延長了對準(zhǔn)時間。因此，需要將粗對準(zhǔn)與精對準(zhǔn)進行融合，充分利用粗對準(zhǔn)采集數(shù)據(jù)及計算機的處理能力，實現(xiàn)快速對準(zhǔn)目的。

為實現(xiàn)這一過程，需要將傳統(tǒng)的時變姿態(tài)誤差角估計問題轉(zhuǎn)變?yōu)闀r不變姿態(tài)誤差角估計問題。因此，本文將對傳統(tǒng)的導(dǎo)航系誤差模型進行修改，設(shè)計導(dǎo)航慣性系誤差模型，從而實現(xiàn)時不變姿態(tài)誤差角估計。假設(shè)粗對準(zhǔn)之后，方向余弦矩陣可以表示為

(10)

式中：

(11)

將式(11)代入式(10)可得

(12)

式(12)表示n0系下姿態(tài)誤差與方向余弦的關(guān)系。令

(13)

則有

(14)

(15)

因此，n0系姿態(tài)誤差角微分方程可以表示為

(16)

從式(16)可以看出，姿態(tài)誤差角的變化只與陀螺儀量測誤差有關(guān)，考慮到對準(zhǔn)過程較短，且陀螺儀量測誤差較小，可以近似認(rèn)為姿態(tài)誤差角為時不變量。由坐標(biāo)變換關(guān)系可知

(17)

由晃動基座條件可知，vn=0，因此式(17)可以表示為

(18)

將式(3)代入式(18)，并忽略n系速度項可得

(19)

因此，n0系速度誤差方程可以表示為

(20)

由式(16)和(20)可得n0系下系統(tǒng)誤差模型

(21)

(22)

(23)

至此，基于n0系的初始對準(zhǔn)模型建立完成。對上式(21)進行離散化，并采用Kalman濾波，即可實現(xiàn)時不變姿態(tài)誤差角φn 0的估計，從而實現(xiàn)精對準(zhǔn)。

3 正向-正向回溯初始對準(zhǔn)原理

圖1 正向-正向回溯對準(zhǔn)結(jié)構(gòu)圖Fig.1 Structure diagram of forward-forward backtracking alignment method.

4 試驗與結(jié)果

4.1 仿真試驗

仿真硬件環(huán)境為Intel (R) Core(TM)T9600 CPU 2.80 GHz，4G RAM，Windows 7操作系統(tǒng)。利用Matlab profile功能對算法運行時間進行統(tǒng)計，檢驗算法的收斂速度。

表1 傳感器誤差設(shè)定Table 1 Sensor errors setting

仿真過程中，慣性傳感器誤差參數(shù)設(shè)定如表1所示。設(shè)定慣性傳感器數(shù)據(jù)輸出率為200 Hz。仿真過程模擬艦船系泊狀態(tài)下初始對準(zhǔn)，設(shè)定晃動參數(shù)如表2所示。

表2 搖擺參數(shù)設(shè)定Table 2 Swinging parameters setting

在上述仿真條件下，采用最優(yōu)基方法進行300 s粗對準(zhǔn)，并對粗對準(zhǔn)數(shù)據(jù)進行記錄，采用設(shè)定回溯過程為4次，得到對準(zhǔn)誤差曲線圖如圖2～4所示。圖中點線表示第一次回溯結(jié)果、點畫線表示第二次回溯結(jié)果、虛線表示第三次回溯結(jié)果、實線表示第四次回溯結(jié)果。

粗對準(zhǔn)結(jié)束時刻航向?qū)?zhǔn)精度為0.5°，水平對準(zhǔn)誤差為0.02°左右。當(dāng)由粗對準(zhǔn)進入回溯對準(zhǔn)時，由于初始誤差協(xié)方差矩陣P0|0為經(jīng)驗值，且通常設(shè)置為較大，導(dǎo)致在回溯起始階段出現(xiàn)較大的調(diào)節(jié)現(xiàn)象。但經(jīng)過兩次回溯計算縱搖誤差角可以收斂到0.006°左右，橫搖誤差角可以收斂在0.004°以內(nèi)，航向誤差角可以收斂在0.06°左右，達到了精對準(zhǔn)的目標(biāo)。同時，由于采用正向-正向回溯過程，不需要在回溯過程中進行大量IMU數(shù)據(jù)更新，使得算法可以在1.5 s內(nèi)完成一次回溯計算，四次回溯過程計算時間為6 s。通過分析可以經(jīng)過兩次回溯之后算法即可達到極限精度，因此總的對準(zhǔn)時間為303 s，這大大提高了初始對準(zhǔn)的收斂速度，達到快速對準(zhǔn)的目的。

圖2 縱搖誤差Fig.2 Curves of the errors of pitch

圖3 橫搖誤差Fig.3 Curves of the errors of roll

圖4 航向誤差Fig.4 Curves of the errors of yaw

4.2 轉(zhuǎn)臺試驗

在仿真試驗的基礎(chǔ)上，設(shè)計轉(zhuǎn)臺試驗，檢驗算法在實時系統(tǒng)上的對準(zhǔn)性能。轉(zhuǎn)臺試驗實物圖如圖5所示。設(shè)定搖擺參數(shù)如表2所示，IMU數(shù)據(jù)更新頻率為200 Hz。對準(zhǔn)過程中，先進行300 s粗對準(zhǔn)，然后進行四次回溯對準(zhǔn)，并以轉(zhuǎn)臺輸出為姿態(tài)基準(zhǔn)，四次回溯對準(zhǔn)過程誤差曲線如圖6、7和8所示。

從圖6和7中可以看出，水平姿態(tài)角在一次回溯之后即可達到穩(wěn)定值，縱搖角誤差均值為-0.0007°，標(biāo)準(zhǔn)差為0.0156°，橫搖角誤差均值為0.0034°，標(biāo)準(zhǔn)差為0.0186°。航向角經(jīng)過兩次回溯也收斂到穩(wěn)定值，對準(zhǔn)誤差均值為-0.057°，標(biāo)準(zhǔn)差為0.0077°。其中，一次回溯過程的計算時間為1.5 s，因此可以在303 s內(nèi)完成初始對準(zhǔn)過程，加速了對準(zhǔn)的收斂速度，達到了快速對準(zhǔn)的目的。

圖5 轉(zhuǎn)臺實物圖Fig.5 IMU and turntable

圖6 縱搖誤差Fig.6 Curves of the errors of pitch

圖7 橫搖誤差Fig.7 Curves of the errors of roll

圖8 航向誤差Fig.8 Curves of the errors of yaw

在轉(zhuǎn)臺試驗中，由于轉(zhuǎn)臺數(shù)據(jù)是由慣導(dǎo)數(shù)據(jù)觸發(fā)的，這使得轉(zhuǎn)臺數(shù)據(jù)在輸出的時候相對于慣導(dǎo)數(shù)據(jù)存在延時現(xiàn)象，從而造成水平姿態(tài)誤差波動較大。

5 結(jié) 論

針對初始對準(zhǔn)收斂速度與收斂精度的矛盾問題，本文設(shè)計了一種正向-正向回溯初始對準(zhǔn)方法，利用慣性系初始對準(zhǔn)基本原理，構(gòu)造了基于n0系的系統(tǒng)誤差方程，實現(xiàn)了時變姿態(tài)估計到時不變姿態(tài)估計的轉(zhuǎn)變，為正向-正向過程奠定了基礎(chǔ)。通過設(shè)計仿真試驗與轉(zhuǎn)臺試驗，檢驗了正向-正向回溯對準(zhǔn)在晃動基座上具有收斂速度快、對準(zhǔn)精度高、計算效率高等優(yōu)點，能夠解決初始對準(zhǔn)收斂速度與收斂精度矛盾的問題。