胡 杰, 朱倚嫻, 石瀟竹

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

0 引 言

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

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

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

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

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

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

(1)

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

圖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中,陀螺和加速度計(jì)輸出值為

(2)

(3)

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

(4)

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

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

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

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

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

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

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

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

然后按照上述順序循環(huán)運(yùn)動(dòng),且在每個(gè)位置處停留時(shí)間為5 min。

2 方位陀螺漂移估計(jì)原理

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

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

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

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

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

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

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

圖4 估計(jì)流程圖Fig.4 Estimation process chart

3 IMU誤差估計(jì)Kalman濾波模型

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

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

(5)

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

(6)

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

H=[I2×202×13]

(7)

式中,I表示單位矩陣。

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

(8)

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

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

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

步驟1時(shí)間更新

(9)

(10)

步驟2量測(cè)更新

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

4 驗(yàn)證實(shí)驗(yàn)

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

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

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

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

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

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

圖6 車載實(shí)驗(yàn)路線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 陀螺標(biāo)度因數(shù)誤差估計(jì)值

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

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

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

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

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

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

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

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

5 結(jié) 論

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

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