基于卡爾曼濾波的主子慣組匹配標(biāo)定方法設(shè)計(jì)

李維剛，張鵬，李孟委,2,王樹(shù)森

(1.中北大學(xué) 南通智能光機(jī)電研究院，江蘇 南通 226000；2.中北大學(xué) 電子測(cè)試技術(shù)國(guó)家重點(diǎn)實(shí)驗(yàn)室，山西 太原 030051)

0 引 言

捷聯(lián)慣性導(dǎo)航系統(tǒng)(strapdown inertial navigation system， SINS)將慣性測(cè)量組件(inertial measurement unit,IMU)與載體直接固連，僅利用IMU所感知到的載體比力及角速率信息就可為載體提供高精度的姿態(tài)信息，進(jìn)而解算速度位置等導(dǎo)航參數(shù)[1]。該工作過(guò)程不受天氣、磁力等外界干擾，也無(wú)需依靠衛(wèi)星、中繼等外界信息輸入，具有自主性、無(wú)源性、體積小、抗干擾等優(yōu)勢(shì),越來(lái)越廣泛地應(yīng)用于艦船、導(dǎo)彈、飛機(jī)等各種裝備上，在民用和軍事領(lǐng)域都發(fā)揮著愈發(fā)重要的作用[2]。

慣性測(cè)量組件作為測(cè)量載體姿態(tài)、位置、速度的敏感部件，是SINS的核心信息源，是慣性裝備導(dǎo)航定位、精確制導(dǎo)、高效打擊等關(guān)鍵技術(shù)的核心[3]。IMU關(guān)鍵指標(biāo)的精確與否在很大程度上決定了SINS的導(dǎo)航精度。

對(duì)于安裝在武器裝備上的慣性測(cè)量組件，傳統(tǒng)的周期性標(biāo)定方法是拆卸以后運(yùn)送到滿(mǎn)足條件的實(shí)驗(yàn)室或倉(cāng)庫(kù)，利用高精度三軸轉(zhuǎn)臺(tái)進(jìn)行分立式標(biāo)定，對(duì)IMU的誤差進(jìn)行辨識(shí)。這樣做對(duì)作戰(zhàn)裝備的機(jī)動(dòng)性和實(shí)時(shí)性造成了比較大的掣肘，為解決這一問(wèn)題，研究者提出引入外觀測(cè)信息代替?zhèn)鹘y(tǒng)測(cè)試設(shè)備，實(shí)現(xiàn)外場(chǎng)標(biāo)定。

楊曉霞等[4-6]在測(cè)試過(guò)程中引入了GPS、里程計(jì)和主慣組等外觀測(cè)信息，無(wú)需測(cè)試轉(zhuǎn)臺(tái),只要輔以車(chē)輛的不同機(jī)動(dòng)方式就可利用Kalman濾波實(shí)現(xiàn)誤差參數(shù)的標(biāo)定；王樂(lè)[7]在僅引入主慣組信息的前提下設(shè)計(jì)了29維Kalman濾波器，通過(guò)編排導(dǎo)彈發(fā)射車(chē)的轉(zhuǎn)彎、起豎和側(cè)傾等機(jī)動(dòng)，進(jìn)行被測(cè)慣組的誤差激勵(lì)以實(shí)現(xiàn)免拆標(biāo)定；王海亮等[8]利用炮車(chē)的常規(guī)機(jī)動(dòng)方式在野戰(zhàn)環(huán)境下實(shí)現(xiàn)了彈載IMU的簡(jiǎn)易標(biāo)定。

以上研究者所做工作都實(shí)現(xiàn)了預(yù)期標(biāo)定，但是存在濾波器維數(shù)過(guò)高、計(jì)算量大和載體運(yùn)動(dòng)路徑過(guò)于理想化等問(wèn)題。本文引入主慣組信息，設(shè)計(jì)12維卡爾曼濾波器，可在一定程度上補(bǔ)償小型化可機(jī)動(dòng)測(cè)試設(shè)備(350 mm×400 mm×500 mm，50 kg)的低控制精度，實(shí)現(xiàn)在低精度轉(zhuǎn)臺(tái)上的標(biāo)定，具有一定的實(shí)用價(jià)值。

1 主子慣組匹配標(biāo)定算法

目前，慣性測(cè)量組件的標(biāo)定根據(jù)不同的輸入基準(zhǔn)可分為分立式標(biāo)定和系統(tǒng)級(jí)標(biāo)定。分立式標(biāo)定以轉(zhuǎn)臺(tái)輸入的角速率作為陀螺儀的基準(zhǔn)，以角位置產(chǎn)生的重力加速度分量作為加速度計(jì)的基準(zhǔn)，主要通過(guò)最小二乘法擬合輸入矩陣與采樣輸出矩陣計(jì)算標(biāo)定參數(shù)，其標(biāo)定精度嚴(yán)重依賴(lài)于測(cè)試環(huán)境和測(cè)試設(shè)備精度；系統(tǒng)級(jí)標(biāo)定從導(dǎo)航誤差中推導(dǎo)IMU的標(biāo)定誤差模型，設(shè)計(jì)合適的轉(zhuǎn)停方案激勵(lì)被測(cè)件誤差，再通過(guò)濾波算法逐步迭代校正標(biāo)定模型參數(shù)[9]。系統(tǒng)級(jí)標(biāo)定方法擺脫了對(duì)測(cè)試環(huán)境和設(shè)備的過(guò)度依賴(lài)，簡(jiǎn)化了測(cè)試流程[10]。

主子慣組匹配標(biāo)定算法屬于系統(tǒng)級(jí)標(biāo)定方法，特別之處在于本方法以基準(zhǔn)慣組和被測(cè)慣組導(dǎo)航參數(shù)之差作為觀測(cè)量，通過(guò)卡爾曼濾波算法估計(jì)IMU誤差項(xiàng)，然后將主慣組輸出的加速度和角速率看作實(shí)際測(cè)量值，利用最小二乘法分離出子慣組中其余誤差項(xiàng)。該方案的特點(diǎn)是：(1)擺脫了傳統(tǒng)標(biāo)定方法對(duì)測(cè)試環(huán)境和測(cè)試設(shè)備的苛刻要求；(2)測(cè)試過(guò)程中對(duì)外界環(huán)境干擾和測(cè)量噪聲不敏感，從而提升了測(cè)量精度；(3)標(biāo)定流程簡(jiǎn)單，易于實(shí)現(xiàn)。該方法適用于外場(chǎng)作戰(zhàn)環(huán)境下對(duì)慣性裝備中的捷聯(lián)慣性測(cè)量裝置進(jìn)行標(biāo)定和檢測(cè)，同時(shí)也滿(mǎn)足庫(kù)存條件下慣性裝備的定期維護(hù)需求。算法流程如圖1所示。

1.1 誤差模型建立

構(gòu)建合理的誤差模型是Kalman濾波器建立的先決條件，對(duì)陀螺儀和加速度計(jì)的誤差產(chǎn)生機(jī)理進(jìn)行分析并建立誤差傳遞方程如下。

陀螺儀忽略隨機(jī)干擾，誤差模型為[11]

(1)

圖1 主子慣組匹配標(biāo)定算法流程圖

加速度計(jì)忽略二次項(xiàng)誤差，誤差模型為

(2)

緯度誤差

(3)

高度誤差

(4)

經(jīng)度誤差

(5)

姿態(tài)誤差模型為

速度誤差模型為

(7)

誤差模型中各參數(shù)的涵義及其單位如表1所示，表1中δφE，δφN，δφU指子慣組東-北-天向姿態(tài)角相較于主慣組的誤差。

表1 誤差參數(shù)明細(xì)

1.2 卡爾曼濾波器構(gòu)建

卡爾曼濾波中,觀測(cè)量為輸入量，狀態(tài)量為輸出量。濾波器根據(jù)觀測(cè)量修正狀態(tài)量，進(jìn)行迭代從而達(dá)到最優(yōu)估計(jì)。卡爾曼濾波器是主子慣組匹配標(biāo)定算法中最為重要的一部分，濾波器對(duì)采集到的IMU離散信號(hào)進(jìn)行線性最優(yōu)回歸數(shù)據(jù)處理，通過(guò)觀測(cè)量的不斷修正，將誤差模型參數(shù)迭代遞推至最接近真實(shí)值的狀態(tài)量[12]。

算法原理如圖2所示。

圖2 卡爾曼濾波原理圖

1.3 系統(tǒng)狀態(tài)量選取

系統(tǒng)狀態(tài)方程選用12階卡爾曼濾波模型，包括三軸速度誤差、姿態(tài)誤差、陀螺誤差和加速度計(jì)誤差。其狀態(tài)方程變量定義為

(8)

式中：

XT=[δvEδvNδvUδφEδφNδφUεx

εyεzXYZ]；

(9)

W=[WgxWgyWgzWaxWayWaz

0 0 0 0 0 0]T；

(10)

式中，V為量測(cè)噪聲。

1.4 系統(tǒng)觀測(cè)量選取

一般情況下，增加觀測(cè)量可以加強(qiáng)系統(tǒng)的可觀測(cè)性，提高系統(tǒng)的標(biāo)定精度，捷聯(lián)慣性測(cè)量單元測(cè)量的速度、姿態(tài)角、角速率、加速度、姿態(tài)矩陣等外部信息都可以利用[13]。但是加速度值受動(dòng)態(tài)環(huán)境影響產(chǎn)生高頻擾動(dòng)，角速率積分會(huì)產(chǎn)生積分誤差，這里都不予考慮。高動(dòng)態(tài)環(huán)境下主慣導(dǎo)姿態(tài)是否可觀測(cè)還有待研究；位置信息由速度信息積分得到，可觀測(cè)性下降[14]。如何選取系統(tǒng)觀測(cè)量，決定了標(biāo)定系統(tǒng)的性能。

在傳統(tǒng)的系統(tǒng)級(jí)標(biāo)定算法中，整個(gè)標(biāo)定過(guò)程不存在線運(yùn)動(dòng)，只存在角運(yùn)動(dòng)，若慣性器件為理想器件，則可以認(rèn)為導(dǎo)航解算所得的速度為0，位置不變。因此,導(dǎo)航解算得到的速度與位置信息可以看作速度誤差和位置誤差[7]。在主子慣導(dǎo)匹配標(biāo)定算法中，以基準(zhǔn)慣組和被測(cè)慣組導(dǎo)航參數(shù)之差作為觀測(cè)量。這樣不僅可以提高觀測(cè)量精度，而且能滿(mǎn)足機(jī)動(dòng)標(biāo)定需求，還能降低動(dòng)態(tài)干擾對(duì)標(biāo)定結(jié)果的影響，滿(mǎn)足外場(chǎng)標(biāo)定的需求。

選用速度+姿態(tài)+位置匹配作為觀測(cè)量時(shí)，由于主從慣組固連，軸向重合，二者的狀態(tài)轉(zhuǎn)移矩陣無(wú)限趨近于單位矩陣，因此可以認(rèn)為二者在轉(zhuǎn)臺(tái)運(yùn)動(dòng)過(guò)程中的姿態(tài)角、速度、位置應(yīng)完全一致。將主慣組的速度、姿態(tài)、位置作為基準(zhǔn)，與從慣組的導(dǎo)航參數(shù)相減可看作更加準(zhǔn)確的速度誤差、姿態(tài)誤差以及位置誤差。將三軸9個(gè)誤差項(xiàng)作為觀測(cè)量，可得觀測(cè)向量和觀測(cè)方程為

Z=[δvEδvNδvUδφEδφNδφUδLδλδh]，

Z9×1=H9×12X12×1+V9×1，

(14)

式中,觀測(cè)矩陣H為

其中，Ta為基準(zhǔn)慣組測(cè)得的方向余弦矩陣。

選用速度+姿態(tài)匹配作為觀測(cè)量時(shí)，觀測(cè)向量與觀測(cè)方程為

H=[δvEδvNδvUδφEδφNδφU]T

Z6×1=H6×12x12×1+v6×1，

(17)

觀測(cè)矩陣為

(18)

只選用速度作為觀測(cè)量時(shí)，觀測(cè)向量和觀測(cè)方程為

Z=[δvEδvNδvU]T

Z3×1=H3×12X12×1+V3×1，

(19)

相應(yīng)觀測(cè)矩陣為

H=[O3×3I3O3×6]。

(20)

2 標(biāo)定方法

根據(jù)上述系統(tǒng)級(jí)標(biāo)定原理，設(shè)計(jì)標(biāo)定方案。標(biāo)定流程如下。

(1)將基準(zhǔn)捷聯(lián)慣性測(cè)量裝置與待測(cè)慣組固連在三軸轉(zhuǎn)臺(tái)臺(tái)面中心，軸向重合。

(2)以基準(zhǔn)慣組的自身坐標(biāo)系為參考坐標(biāo)系，對(duì)被測(cè)慣組進(jìn)行初始對(duì)準(zhǔn)，保證其失準(zhǔn)角非常小，即基準(zhǔn)慣組與被測(cè)慣組之間的狀態(tài)轉(zhuǎn)移矩陣無(wú)限趨近于單位陣[15-16]。

(3)連接測(cè)試設(shè)備，給慣組供電并且預(yù)熱，同時(shí)采集主、從慣組陀螺和加速度計(jì)的輸出，等待測(cè)試設(shè)備與主、從慣組完成初始化。

(4)按照表2標(biāo)定路徑對(duì)三軸轉(zhuǎn)臺(tái)進(jìn)行規(guī)定路徑編排[11,17]。

表2 標(biāo)定路徑

(5)在三軸轉(zhuǎn)臺(tái)運(yùn)動(dòng)的全過(guò)程中采集IMU輸出數(shù)據(jù)，進(jìn)行包括導(dǎo)航解算在內(nèi)的離線處理[18]。

(6)通過(guò)卡爾曼濾波估計(jì)出陀螺儀誤差和加速度計(jì)誤差，然后將主捷聯(lián)慣性測(cè)量裝置的加速度計(jì)輸出看作是實(shí)際的加速度，陀螺儀輸出看作是實(shí)際的角速率，通過(guò)最小二乘法分離標(biāo)度因數(shù)和安裝誤差參數(shù)[19]。

3 測(cè)試與實(shí)驗(yàn)

以IMU-500型慣性測(cè)量單元作為基準(zhǔn)慣組，以MIN-900型慣性測(cè)量單元作為被測(cè)慣組，二者固連于UOO型慣性測(cè)試標(biāo)定設(shè)備進(jìn)行測(cè)試。分別以速度+姿態(tài)+位置匹配、速度+位置匹配、速度匹配作觀測(cè)量，設(shè)置3組對(duì)照組，對(duì)比3種傳感器布控方法對(duì)于標(biāo)定結(jié)果和標(biāo)定效率的影響。

3.1 測(cè)試條件

被測(cè)慣組和基準(zhǔn)慣組都經(jīng)過(guò)高精度三軸轉(zhuǎn)臺(tái)標(biāo)校，其主要性能參數(shù)如表3所示。

表3 慣組性能指標(biāo)

實(shí)驗(yàn)采用UOO型慣性測(cè)量單元測(cè)試標(biāo)定系統(tǒng)進(jìn)行IMU的標(biāo)定。測(cè)試設(shè)備的實(shí)物如圖3所示。

該設(shè)備能提供速率、位置、搖擺等運(yùn)動(dòng)控制，并且能夠?qū)崟r(shí)采集被測(cè)慣性測(cè)量單元的原始數(shù)據(jù)信息，導(dǎo)航解算之后進(jìn)行存儲(chǔ)以便后端處理。測(cè)試標(biāo)定系統(tǒng)為MEMS慣組的靜態(tài)標(biāo)定測(cè)試提供恒定轉(zhuǎn)速，做為慣組測(cè)試信號(hào)源，通過(guò)上位機(jī)控制轉(zhuǎn)臺(tái)內(nèi)框、中框、外框以固定步長(zhǎng)旋轉(zhuǎn)和固定時(shí)間。

相較于大多數(shù)高精度大型三軸轉(zhuǎn)臺(tái)，該測(cè)試設(shè)備具有小型化、易于外場(chǎng)機(jī)動(dòng)的特點(diǎn)，但是需要配備隔振基座，角速率控制精度和角位置控制精度也遠(yuǎn)低于傳統(tǒng)設(shè)備。

圖3 慣性測(cè)量單元測(cè)試標(biāo)定系統(tǒng)

根據(jù)表4，該設(shè)備屬于低精度測(cè)試轉(zhuǎn)臺(tái)。測(cè)試標(biāo)定系統(tǒng)的具體指標(biāo)如表4所示。

表4 轉(zhuǎn)臺(tái)精度分類(lèi)

表5 慣性測(cè)量單元測(cè)試標(biāo)定系統(tǒng)性能指標(biāo)

3.2 實(shí)驗(yàn)結(jié)果

測(cè)試中，以高精度轉(zhuǎn)臺(tái)標(biāo)校后的參數(shù)，即表3中主子慣組的性能指標(biāo)作為基準(zhǔn)，共設(shè)置3組對(duì)照組,分別是速度+位置+姿態(tài)匹配、速度+姿態(tài)匹配以及速度匹配。在對(duì)卡爾曼濾波器設(shè)置不同觀測(cè)量的情況下，觀察IMU標(biāo)定的精度以及濾波器收斂的速率,如表6和圖4～5所示。

表6 陀螺儀X軸參數(shù)標(biāo)定誤差

圖4 不同觀測(cè)量標(biāo)定結(jié)果對(duì)照?qǐng)D

圖5 慣性測(cè)量單元安裝誤差估計(jì)

通過(guò)對(duì)表6與圖4～5分析可以得出：對(duì)于大多數(shù)誤差參數(shù)，速度+姿態(tài)匹配方法標(biāo)定的標(biāo)定誤差最小，速度+姿態(tài)+位置匹配方法次之。但是速度+姿態(tài)+位置匹配方法在卡爾曼濾波器中的收斂速度遠(yuǎn)慢于其他2種方法，基本上要推遲>250 s之后才能收斂至準(zhǔn)確數(shù)值。因此，速度+姿態(tài)匹配的方法標(biāo)定誤差最小，收斂速度最快，運(yùn)算復(fù)雜程度適中，在3種匹配方法中最優(yōu)。

通過(guò)圖6和圖7對(duì)誤差補(bǔ)償前后慣組的一系列測(cè)試，證明本文提出的主子慣組匹配標(biāo)定方法補(bǔ)償效果更優(yōu)，而且本方法能夠在一定程度上補(bǔ)償?shù)途绒D(zhuǎn)臺(tái)各項(xiàng)誤差造成的標(biāo)定精度降低的問(wèn)題。

圖7 動(dòng)態(tài)姿態(tài)角補(bǔ)償測(cè)試曲線

4 結(jié) 語(yǔ)

基于卡爾曼濾波的主子慣組匹配標(biāo)定算法可以在低精度三軸轉(zhuǎn)臺(tái)上實(shí)現(xiàn)IMU的快速標(biāo)定，標(biāo)定精度在可接受范圍內(nèi)。將速度+姿態(tài)匹配作卡爾曼濾波器觀測(cè)量時(shí)，系統(tǒng)標(biāo)定精度最高，濾波器收斂最快，計(jì)算量適中，適合實(shí)際外場(chǎng)標(biāo)定時(shí)的工程應(yīng)用。