單軸旋轉(zhuǎn)SINS方位陀螺漂移精確估計方法

胡 杰, 朱倚嫻, 石瀟竹

(1. 中國電子科技集團公司第二十八研究所, 江蘇 南京 210007;2. 空中交通管理系統(tǒng)與技術(shù)國家重點實驗室, 江蘇 南京 210007;3. 東南大學微慣性儀表與先進導航技術(shù)教育部重點實驗室, 江蘇 南京 210096)

0 引 言

慣性導航系統(tǒng)的北向陀螺漂移和方位陀螺漂移會引起位置的積累性誤差,可以通過提高慣性測量單元(inertial measurement unit, IMU)的精度或者采用系統(tǒng)級補償方法提高定位精度[1-3]。系統(tǒng)級補償方法具有見效快、實現(xiàn)簡單等優(yōu)點,目前有兩種常用方法[4-6]:①采用旋轉(zhuǎn)調(diào)制技術(shù)消除器件誤差影響;②利用Kalman濾波器估計IMU誤差并在線補償。旋轉(zhuǎn)式(strapdown inertial navigation system,SINS)周期性轉(zhuǎn)動IMU抵消了與轉(zhuǎn)軸垂直方向上的器件誤差,提高了系統(tǒng)導航精度。國外激光陀螺旋轉(zhuǎn)式SINS已經(jīng)應(yīng)用于水面艦船或潛艇,如美國的MK39Mod3C單軸旋轉(zhuǎn)SINS,MK49雙軸旋轉(zhuǎn)SINS等[7-9]。國內(nèi)相關(guān)單位也進行了旋轉(zhuǎn)式SINS的研究工作,如國防科學技術(shù)大學、哈爾濱工程大學、航天科工33所以及航天科技13所[4, 10-12]等。

單軸旋轉(zhuǎn)SINS的方位陀螺性能,特別是常值漂移和陀螺標度因數(shù)誤差,決定了導航系統(tǒng)的性能指標,如方位陀螺漂移為0.01 °/h,地理緯度為45°時,24h后其位置誤差約為10n mile[13]。為了提高單軸旋轉(zhuǎn)SINS長時間導航精度,許多學者對此進行了研究。孫楓[14]提出傾斜放置旋轉(zhuǎn)軸以盡可能抵消3個轉(zhuǎn)軸方向上的IMU誤差,該方法所采用的轉(zhuǎn)位方式實際中難以實現(xiàn)。于旭東[15]提出利用經(jīng)緯度誤差辨識方位陀螺漂移,辨識時間長且需要載體靜止不動。陸志東[16]對雙軸旋轉(zhuǎn)SINS誤差進行了分析,并提出了一種IMU誤差系統(tǒng)級標校方法,通過周期性轉(zhuǎn)動IMU能夠有效估計出IMU誤差。Gao[17]研究了旋轉(zhuǎn)式SINS初始對準與自標校技術(shù),慣導系統(tǒng)啟動后進行初始對準的同時對IMU誤差進行自校準,根據(jù)仿真結(jié)果可以看出,方位陀螺漂移無法進行有效估計。孫偉[18]同樣研究了雙軸旋轉(zhuǎn)式慣導系統(tǒng)IMU在線標定方法,根據(jù)實驗結(jié)果可以看出,初始姿態(tài)和航向角精度影響方位陀螺漂移的估計精度。綜上文獻可知,目前單軸旋轉(zhuǎn)SINS方位陀螺漂移標校方法主要有最小二乘辨識與Kalman濾波估計兩種方法,其中基于經(jīng)緯誤差的最小二乘辨識方法耗時長,且需要載體保持靜止不動,實際應(yīng)用并不廣泛,而Kalman濾波估計法目前較多的應(yīng)用在雙軸旋轉(zhuǎn)SINS中,因此需要結(jié)合單軸旋轉(zhuǎn)SINS誤差傳播特性,研究一種適用于單軸旋轉(zhuǎn)SINS的方位陀螺漂移估計方法。

本文對慣導系統(tǒng)誤差傳播規(guī)律進行了分析,提出了一種單軸旋轉(zhuǎn)SINS方位陀螺漂移估計方法。首先分析了單軸旋轉(zhuǎn)SINS方位陀螺漂移估計原理,給出了方位陀螺漂移估計流程,然后建立了Kalman濾波模型,給出了一種優(yōu)化后的Kalman濾波方法。進行了系統(tǒng)驗證實驗,結(jié)果表明該方法具有可行性,能夠為中等精度單軸旋轉(zhuǎn)SINS的研制提供參考。

1 單軸旋轉(zhuǎn)誤差補償原理

1.1 坐標系定義及單軸旋轉(zhuǎn)SINS系統(tǒng)構(gòu)成

單軸旋轉(zhuǎn)SINS中,IMU放置在旋轉(zhuǎn)機構(gòu)上,可以繞轉(zhuǎn)動軸作往返運動。定義旋轉(zhuǎn)坐標系為s系(oxsyszs),其余坐標系與常規(guī)SINS定義一致,分別有載體坐標系(b系,oxbybzb),慣性坐標系(i系,oxiyizi)以及導航坐標系(n系,oxnynzn)[19]。初始時刻旋轉(zhuǎn)系與載體系重合,系統(tǒng)通電后電機驅(qū)動IMU以角速度Ω繞轉(zhuǎn)動軸旋轉(zhuǎn),則當時間為t時其互相之間的轉(zhuǎn)換矩陣為

(1)

圖1為單軸旋轉(zhuǎn)SINS結(jié)構(gòu)圖,系統(tǒng)主要由旋轉(zhuǎn)機構(gòu)、IMU以及導航計算機組成。IMU包括3個光纖陀螺和3個石英撓性加速度計。力矩電機能夠驅(qū)動IMU作正反旋轉(zhuǎn)運動,角度編碼器實時給出旋轉(zhuǎn)系相對載體系轉(zhuǎn)過的角度。

圖1 單軸旋轉(zhuǎn)SINS結(jié)構(gòu)圖Fig.1 Single-axis rotational SINS structure

1.2 旋轉(zhuǎn)調(diào)制原理分析[20-21]

在單軸旋轉(zhuǎn)SINS中,陀螺和加速度計輸出值為

(2)

(3)

將式(2)代入到式(3)中得:

(4)

由式(4)知,水平方向上的陀螺漂移和加速度計偏置經(jīng)過旋轉(zhuǎn)調(diào)制后其在一個周期內(nèi)積分為零,而轉(zhuǎn)軸方向上慣性量漂移無法被抵消,其對導航精度的影響依然存在。

1.3 單軸旋轉(zhuǎn)方案

單軸旋轉(zhuǎn)主要有單向連續(xù)旋轉(zhuǎn)、大于360°兩位置正反旋轉(zhuǎn)以及小于360°四位置正反旋轉(zhuǎn)3種方案。為了減小IMU標度因數(shù)誤差與轉(zhuǎn)軸運動耦合而帶來的系統(tǒng)級誤差,采用正反旋轉(zhuǎn)方案[22]。同時小于360°旋轉(zhuǎn)方式可靠性最高,因此本文采用四位置正反旋轉(zhuǎn)方案,如圖2所示。

圖2 單軸旋轉(zhuǎn)方案Fig.2 Single-axis rotational scheme

轉(zhuǎn)位1IMU由A點逆時針轉(zhuǎn)動180°到達C點,然后停止Ts;

轉(zhuǎn)位2IMU由C點逆時針轉(zhuǎn)動90°到達D點,然后停止Ts;

轉(zhuǎn)位3IMU由D點順時針轉(zhuǎn)動180°到達B點,然后停止Ts;

轉(zhuǎn)位4IMU由B點順時針轉(zhuǎn)動90°到達A點,然后停止Ts。

然后按照上述順序循環(huán)運動,且在每個位置處停留時間為5 min。

2 方位陀螺漂移估計原理

單軸旋轉(zhuǎn)SINS方位陀螺漂移可觀測性較差,為了減小系統(tǒng)誤差對方位陀螺漂移估計精度影響,需要對SINS誤差方程進行分析[23-24]。圖3為SINS誤差傳遞過程。

圖3 SINS誤差傳播路徑Fig.3 Errors propagation path of SINS

圖3中,εE、εU為等效東向陀螺漂移和方位陀螺漂移;δvN為SINS北向速度誤差;δL為SINS緯度誤差;φx、φz分別表示東向失準角和方位失準角。由圖3可知,方位陀螺漂移激發(fā)方位失準角,而方位失準角會進一步引起東向失準角,同時東向陀螺漂移也會引起對應(yīng)的東向失準角,可以看出,東向陀螺漂移與方位陀螺漂移具有相同的誤差傳播路徑。因此,當采用“速度匹配”或者“位置匹配”誤差方程估計方位陀螺漂移時,對應(yīng)的東向陀螺漂移、初始方位失準角等都會影響方位陀螺漂移估計精度。

在單軸旋轉(zhuǎn)SINS中,水平方向上的IMU誤差已經(jīng)被調(diào)制,因此可以忽略東向陀螺漂移對方位陀螺漂移估計精度影響,方位失準角精度最終決定了方位陀螺漂移估計效果。同時通過SINS旋轉(zhuǎn)式精對準可以得到較為準確的方位角,本文利用該特點設(shè)計了一種適用于單軸旋轉(zhuǎn)SINS的方位陀螺漂移估計流程,共3步,如圖4所示。

步驟1單軸旋轉(zhuǎn)SINS通電完成自檢后首先進行“慣性系抗晃動基座粗對準”[25-26],可以得到SINS初始姿態(tài)和航向角,設(shè)定時間為T1;

步驟2啟動轉(zhuǎn)位機構(gòu)進行四位置精對準,利用Kalman濾波器估計3個失準角,精對準結(jié)束后反饋修正SINS速度、位置以及失準角,設(shè)定時間為T2;

步驟3單軸旋轉(zhuǎn)SINS繼續(xù)進行四位置轉(zhuǎn)停運動,利用Kalman濾波器在線估計IMU誤差,設(shè)定時間為T3,然后修正SINS速度、位置、失準角和IMU誤差,至此SINS啟動前誤差標校流程結(jié)束,切換系統(tǒng)至純慣性導航工作模式。

圖4 估計流程圖Fig.4 Estimation process chart

3 IMU誤差估計Kalman濾波模型

3.1 狀態(tài)方程與量測方程建立

狀態(tài)向量包括SINS位置和速度誤差、3個失準角、陀螺漂移和加速度計偏置以及陀螺標度因數(shù)誤差,具體可以寫成:

(5)

根據(jù)捷聯(lián)慣導系統(tǒng)誤差方程建立狀態(tài)空間模型,可得慣導系統(tǒng)狀態(tài)方程和量測方程為

(6)

式中,A為系統(tǒng)狀態(tài)轉(zhuǎn)移矩陣,可以由SINS誤差方程推導得到;W為系統(tǒng)噪聲向量;Z為量測向量;H為量測矩陣;V表示系統(tǒng)量測噪聲向量。本文利用δL和δλ作為觀測量,因此H矩陣可以寫成如下形式:

H=[I2×202×13]

(7)

式中,I表示單位矩陣。

式(6)為連續(xù)性誤差方程,對其進一步離散化可以得到離散化后的狀態(tài)方程和量測方程:

(8)

3.2 優(yōu)化的Kalman濾波方法

在單軸旋轉(zhuǎn)SINS中,由于加速度計尺寸效應(yīng)、陀螺標度因數(shù)誤差以及安裝誤差等因素的存在,IMU轉(zhuǎn)動過程中會引起速度誤差(也稱為鋸齒形速度誤差),即Kalman濾波方程中量測新息的突變,從而會帶來濾波器較大估計誤差。本文對傳統(tǒng)Kalman濾波器進行優(yōu)化,以抑制旋轉(zhuǎn)式慣導系統(tǒng)中鋸齒形速度誤差引起的濾波發(fā)散。

當IMU從一個位置轉(zhuǎn)動到另一個位置的旋轉(zhuǎn)過程中,慣導系統(tǒng)只進行導航解算和Kalman濾波時間更新,而不進行濾波量測更新,以隔離鋸齒速度誤差對濾波精度影響,當轉(zhuǎn)動完成后,恢復Kalman濾波正常計算,以下為算法具體計算流程。

步驟1時間更新

(9)

(10)

步驟2量測更新

(11)

(12)

Pm,k+1=Pk+1/k-Kk+1Hk+1Pk+1/k

(13)

步驟3綜合判斷

(14)

Pk+1=β·Pm,k+1+(1-β)·Pk+1/k

(15)

如果轉(zhuǎn)位機構(gòu)轉(zhuǎn)動則置β=0,此時Kalman濾波器輸出值為一步預測值,隔離了鋸齒形速度誤差對系統(tǒng)精度影響;當轉(zhuǎn)位機構(gòu)停止轉(zhuǎn)動時β=1,此時利用量測新息值對濾波器狀態(tài)量進行估計,循環(huán)判斷直至整個濾波過程結(jié)束,轉(zhuǎn)位機構(gòu)的運動狀態(tài)可以由角度編碼器讀取。

4 驗證實驗

為了對本文所提出的方位陀螺漂移估計方法進行充分驗證,利用實驗室研制的單軸旋轉(zhuǎn)SINS分別進行了轉(zhuǎn)臺搖擺和車載行進間驗證實驗,單軸旋轉(zhuǎn)SINS的IMU由3個光纖陀螺和3個石英撓性加速度計組成,主要技術(shù)參數(shù)見表1,系統(tǒng)驗證實驗中控制IMU做單軸四位置正反轉(zhuǎn)停旋轉(zhuǎn)運動,設(shè)計了如下兩種實驗方案。

表1 單軸旋轉(zhuǎn)SINS技術(shù)參數(shù)

方案1轉(zhuǎn)臺搖擺實驗,將單軸旋轉(zhuǎn)SINS放置在三軸模擬轉(zhuǎn)臺上,如圖5(a)所示,控制轉(zhuǎn)臺做三軸搖擺運動,其中航向角搖擺幅度為5°,頻率為0.2 Hz,俯仰角搖擺幅度為5°,頻率為0.2 Hz,橫搖角搖擺幅度為2°,頻率為0.8 Hz,然后啟動單軸旋轉(zhuǎn)SINS開始進行實驗;

方案2車載行進間實驗,將單軸旋轉(zhuǎn)SINS放置在跑車實驗系統(tǒng)上,如圖5(b)所示,車載實驗中基準位置由GPS提供,其位置誤差小于10 m,車載實驗地點為貴陽市內(nèi)某條道路,如圖6所示,圖中A點為實驗起始點,行駛至B點掉頭,然后繼續(xù)行駛至C點停止,整個實驗過程中控制車輛行駛速度為10 km/h。

圖5 驗證實驗環(huán)境Fig.5 Verified experiment environment

標定流程中設(shè)定時間T1=1 min,T2=15 min,T3=2 h,車載實驗標定結(jié)束后繼續(xù)進行12 h的導航驗證實驗。表2、表3對轉(zhuǎn)臺搖擺實驗和車載行進間實驗進行了統(tǒng)計,圖7～圖9給出了車載實驗過程中IMU誤差估計曲線。

圖6 車載實驗路線Fig.6 Vehicle experiment route

參數(shù)Δx/μgΔy/μgεx/((°)/h)εy/((°)/h)εz/((°)/h)搖擺69.02-32.750.006 9-0.003 10.009 1車載75.63-28.620.007 4-0.002 90.008 8

表3 陀螺標度因數(shù)誤差估計值

由表2和表3可看出,搖擺與車載兩種實驗環(huán)境下,IMU誤差估計值相當,同時該估計值與本實驗所使用的IMU精度相當,說明本文所給出的方法具有可行性。

圖7 加速度計偏置估計曲線Fig.7 Estimation curves of the accelerometer biases

圖8 陀螺漂移估計曲線Fig.8 Estimation curves of the gyro drifts

由圖7～圖9可以看出,在單軸旋轉(zhuǎn)SINS中,轉(zhuǎn)動IMU提高了系統(tǒng)的可觀測性,將SINS中加速度計偏置與東向陀螺漂移由不可觀測變?yōu)榭捎^測,同時利用本文所給出的方法有效估計得到方位軸陀螺漂移以及標度因數(shù)誤差。

圖9 陀螺標度因數(shù)誤差估計曲線Fig.9 Estimation curves of the gyro scale factor errors

為了進一步驗證結(jié)果的準確性,車載驗證實驗在完成IMU誤差估計補償后進行了12 h導航實驗,圖10為有無補償IMU誤差前后位置誤差對比曲線,圖中紅色點畫線是系統(tǒng)實際解算得到的12 h位置誤差曲線,藍線為利用保存的數(shù)據(jù)離線計算得到的未補償IMU誤差時的12 h位置誤差曲線。

圖10 位置誤差對比曲線Fig.10 Position error comparison curves

由圖10可以看出,IMU誤差經(jīng)過標校補償后,慣導系統(tǒng)12 h位置誤差由5.13n mile減小為2.12n mile,系統(tǒng)定位精度提高了58.7%。

5 結(jié) 論

本文首先給出了單軸旋轉(zhuǎn)SINS誤差調(diào)制原理,分析了影響方位陀螺漂移估計精度的主要誤差因素,指出東向陀螺漂移和方位失準角是其中影響最大的兩個誤差源。由于陀螺標度因數(shù)誤差在單軸旋轉(zhuǎn)SINS中無法被調(diào)制,因此將此項誤差也加入到標校模型中。對傳統(tǒng)Kalman濾波器進行了優(yōu)化,設(shè)計了一種合理的估計流程以較為全面地對系統(tǒng)中的各項誤差進行估計。

利用單軸旋轉(zhuǎn)SINS進行了轉(zhuǎn)臺搖擺和車載行進間驗證實驗,由誤差估計曲線可以看出,經(jīng)過一段時間后IMU誤差能夠得到有效估計。車載驗證實驗完成IMU誤差估計后轉(zhuǎn)入到純慣性導航,系統(tǒng)定位精度提高了58.7%,由此驗證了該方法的有效性和可行性。