高過(guò)載慣導(dǎo)誤差建模與補(bǔ)償方法

白鉦皓, 張 琨, 王國(guó)棟, 程 玉, 陳 帥

（1. 南京理工大學(xué), 南京 210094; 2. 北京航天控制儀器研究所, 北京 100039）

0 引言

因自主性強(qiáng)、尺寸小、成本低等特點(diǎn), 微慣性測(cè)量單元（Miniature Inertial Measurement Unit,MIMU）被廣泛應(yīng)用于現(xiàn)代軍事領(lǐng)域。 在彈載沖擊環(huán)境下, 慣性器件會(huì)受到非常嚴(yán)重的影響, 過(guò)載前后的性能（零偏、標(biāo)度因數(shù)等） 很難保持一致［1］,由此產(chǎn)生的誤差在慣性導(dǎo)航解算中會(huì)迅速積累。因此, 研究過(guò)載沖擊對(duì)慣性器件的影響十分必要?，F(xiàn)有的標(biāo)定模型大多工作在低動(dòng)態(tài)、低過(guò)載環(huán)境下［2］, 有學(xué)者在此基礎(chǔ)上建立了導(dǎo)彈過(guò)載段的多源誤差模型［3］, 主要針對(duì)過(guò)載段的系統(tǒng)誤差模型進(jìn)行分析。

慣性導(dǎo)航系統(tǒng)的誤差與初始參數(shù)和器件誤差的標(biāo)定精度直接相關(guān), 需對(duì)器件誤差進(jìn)行標(biāo)定以保證慣性導(dǎo)航系統(tǒng)的定位精度［4］。 區(qū)別于傳統(tǒng)標(biāo)定方法, 借助于武器平臺(tái)的高精度主慣導(dǎo)系統(tǒng)提供的信息, 利用傳遞對(duì)準(zhǔn)可實(shí)現(xiàn)精確的初始對(duì)準(zhǔn)和子慣導(dǎo)系統(tǒng)誤差的估計(jì)與補(bǔ)償［5］。 傳遞對(duì)準(zhǔn)常用的匹配方式有速度匹配、速度+姿態(tài)匹配等, 在引入姿態(tài)作為觀測(cè)量后可以極大縮短對(duì)準(zhǔn)時(shí)間［6］。 由于GPS 信號(hào)易受到干擾, 不能完全依賴GPS 進(jìn)行組合導(dǎo)航, 有學(xué)者針對(duì)此問(wèn)題提出了速度約束和高程約束兩種方法, 提高無(wú)衛(wèi)星導(dǎo)航情況下的導(dǎo)航系統(tǒng)能力［7］。 非完整性約束利用載體的側(cè)向和天向速度為零的約束條件對(duì)慣導(dǎo)導(dǎo)航誤差進(jìn)行修正,可實(shí)現(xiàn)無(wú)外部傳感器條件下的自主導(dǎo)航［8］。 目前,純慣性導(dǎo)航的研究都采用較高精度的慣導(dǎo)和自對(duì)準(zhǔn)的方式進(jìn)行驗(yàn)證, 存在慣導(dǎo)成本高昂、自對(duì)準(zhǔn)精度低且機(jī)動(dòng)性差等問(wèn)題。 因此, 針對(duì)低精度慣導(dǎo), 有必要研究一種動(dòng)基座精確對(duì)準(zhǔn)的方法以提高慣導(dǎo)的純慣性導(dǎo)航定位精度。

本文分析了過(guò)載沖擊對(duì)慣性器件的影響, 建立了沖擊誤差補(bǔ)償模型以實(shí)現(xiàn)對(duì)慣性器件誤差的補(bǔ)償。 提出了一種傳遞對(duì)準(zhǔn)與運(yùn)動(dòng)約束輔助的導(dǎo)航定位方法, 針對(duì)該方法的兩種算法建立了統(tǒng)一的系統(tǒng)模型和不同的觀測(cè)模型。 通過(guò)半實(shí)物仿真驗(yàn)證該方法的可行性, 實(shí)現(xiàn)了慣性傳感器誤差的標(biāo)定與校正, 提高了無(wú)衛(wèi)星導(dǎo)航情況下的自主導(dǎo)航定位精度。

1 過(guò)載沖擊下的MIMU 誤差建模

1.1 MIMU 誤差分析

目前, 慣性器件一般采用剛體外框架和灌膠封裝實(shí)現(xiàn)抗高過(guò)載, 當(dāng)慣導(dǎo)受到過(guò)載沖擊時(shí)可以保證其完好, 但不可避免地會(huì)發(fā)生性能參數(shù)（如零偏、標(biāo)度因數(shù)、穩(wěn)定性等）變化［9］, 導(dǎo)致慣導(dǎo)的零偏與標(biāo)度因數(shù)相較于沖擊前會(huì)產(chǎn)生一定變化, 而靜態(tài)標(biāo)定模型未對(duì)其進(jìn)行修正, 造成測(cè)量結(jié)果的誤差。

陀螺儀的輸入輸出模型為

式（1）中,F為陀螺儀的輸出,Kg為陀螺儀的標(biāo)度因數(shù),Ωr為陀螺儀的輸入,Kg0為陀螺儀的零偏,v為擬合誤差。 假設(shè)此時(shí)標(biāo)度因數(shù)和零偏因陀螺儀受到?jīng)_擊而存在誤差, 則沖擊后的陀螺儀測(cè)得的輸出為

式（2）中,Fc為陀螺儀沖擊后的輸出,δKg為沖擊引起陀螺儀的標(biāo)度因數(shù)變化量,δKg0為沖擊后陀螺儀的零偏變化量,vc為擬合誤差。 根據(jù)式（1）所示的陀螺儀輸出模型計(jì)算角速度, 并將式（2）帶入式（1）得到陀螺儀的角速度

式（3）中,v1為擬合誤差。 令, 可得陀螺儀受到?jīng)_擊后的輸出誤差模型

式（4）中,δΩc為陀螺儀角速度誤差,δg0為陀螺儀沖擊后的擬合零位變化量。

同理, 加速度計(jì)受到?jīng)_擊后的輸出誤差模型為

式（5）中,δfc為加速度計(jì)比力誤差,Kf為加速度計(jì)的標(biāo)度因數(shù),f為加速度計(jì)輸入,δKf、δf0分別為沖擊引起加速度計(jì)的標(biāo)度因數(shù)與擬合零位的變化量,v2為擬合誤差。 由式（4）和式（5）可知, 當(dāng)載體受到高過(guò)載沖擊時(shí), 標(biāo)度因數(shù)會(huì)增大比力和角速度誤差, 使定位精度快速發(fā)散。

1.2 MIMU 誤差補(bǔ)償模型

考慮到MIMU 的零偏誤差、標(biāo)度因數(shù)誤差與安裝誤差, 根據(jù)器件物理特性建立陀螺儀與加速度計(jì)的誤差模型［10］。 根據(jù)式（4）和式（5）, MIMU 受到過(guò)載沖擊后, 標(biāo)度因數(shù)與零偏會(huì)發(fā)生變化, 使已有的誤差模型不再完全適用。 對(duì)慣性器件沿x軸方向進(jìn)行過(guò)載沖擊, 對(duì)MIMU 的x軸在不同沖擊下產(chǎn)生的沖擊誤差進(jìn)行建模分析, 慣性器件沿沖擊方向的軸向（x軸）受沖擊作用影響明顯。 利用標(biāo)定模型計(jì)算每次沖擊后的各項(xiàng)參數(shù), 利用基于最小二乘法的多項(xiàng)式回歸模型擬合參數(shù)的變化, 多項(xiàng)式如下

式（6）中,s為MIMU 受到的沖擊量級(jí),ai（i=1, 2, …,n）為多項(xiàng)式系數(shù)。 在原有的標(biāo)定模型上, 利用擬合的變化曲線代替原有模型中的部分常值, 構(gòu)建基于沖擊量級(jí)的動(dòng)態(tài)補(bǔ)償模型, 其補(bǔ)償原理如圖1 所示。

圖1 慣性傳感器沖擊補(bǔ)償模型Fig.1 Shock compensation model of inertial sensor

根據(jù)上述補(bǔ)償原理可得新的MIMU 陀螺儀和加速度計(jì)的誤差模型, 此處以陀螺儀為例

式（7） 中,W= ［WxWyWz］T為MIMU 陀螺儀的實(shí)際測(cè)量值,fωx（s）、fx（s）為陀螺儀x軸的零偏和標(biāo)度因數(shù)為沖擊量級(jí)的函數(shù),ωy0、ωz0為其余兩個(gè)軸的零偏,Kωi、Kωij（i=x,y,z;j=x,y,z）分別為陀螺儀的標(biāo)度因數(shù)與安裝誤差系數(shù),ω=［ωxωyωz］T為陀螺儀實(shí)際的角速度。

2 MIMU 在線誤差補(bǔ)償與約束

由于微慣性傳感器會(huì)存在各種固有誤差, 包括陀螺零偏、加速度計(jì)零偏、安裝誤差角等, 導(dǎo)致純慣性導(dǎo)航定位誤差隨時(shí)間累積迅速發(fā)散。 傳統(tǒng)的分立式標(biāo)定方法需要對(duì)MIMU 進(jìn)行轉(zhuǎn)臺(tái)實(shí)驗(yàn),這既增加了設(shè)備成本, 也不利于現(xiàn)場(chǎng)標(biāo)定的應(yīng)用需求。 通常, 系統(tǒng)模型采用固定誤差會(huì)存在較大的剩余誤差, 需要對(duì)MIMU 進(jìn)行實(shí)時(shí)修正。 考慮到系統(tǒng)無(wú)外部輔助信息的定位需求, 直接采用慣性導(dǎo)航算法無(wú)法有效地抑制系統(tǒng)累積誤差。 針對(duì)上述問(wèn)題, 采用一種傳遞對(duì)準(zhǔn)與運(yùn)動(dòng)學(xué)約束輔助的方法, 既可以實(shí)現(xiàn)精確對(duì)準(zhǔn)和實(shí)時(shí)誤差修正, 又能提高無(wú)衛(wèi)星導(dǎo)航情況下的純慣性導(dǎo)航定位精度。

2.1 傳遞對(duì)準(zhǔn)系統(tǒng)模型

傳遞對(duì)準(zhǔn)分為粗對(duì)準(zhǔn)和精對(duì)準(zhǔn)兩個(gè)階段, 粗對(duì)準(zhǔn)是將主慣導(dǎo)精確的初始姿態(tài)、位置、速度信息傳遞給子慣導(dǎo), 精對(duì)準(zhǔn)是利用Kalman 濾波在線估計(jì)子慣導(dǎo)的零偏誤差, 同時(shí)對(duì)主子慣導(dǎo)桿臂和安裝誤差角進(jìn)行實(shí)時(shí)估計(jì)［11］, 選取系統(tǒng)狀態(tài)量為

式（8）中,φ為子慣導(dǎo)姿態(tài)誤差,δvn為速度誤差,εb為子慣導(dǎo)陀螺零偏,Δb為加速度計(jì)零偏,μb、lb分別為主子慣導(dǎo)安裝角誤差和桿臂誤差。 本文使用基于地理系（東北天坐標(biāo)系）的慣導(dǎo)系統(tǒng)誤差模型, 結(jié)合捷聯(lián)慣導(dǎo)誤差方程可得系統(tǒng)方程［12］為

建立Kalman 濾波器狀態(tài)方程

式（10） 中,Φ為狀態(tài)轉(zhuǎn)移矩陣,W為系統(tǒng)噪聲矩陣。

根據(jù)文獻(xiàn)［13］構(gòu)建系統(tǒng)量測(cè)方程

式（11） 中,V1為量測(cè)噪聲,H1為量測(cè)矩陣。H1滿足

2.2 運(yùn)動(dòng)學(xué)約束

定義載體直角坐標(biāo)系m系,y軸指向載體前進(jìn)方向,x軸沿橫軸指向右側(cè),z軸指向載體上方。載體運(yùn)動(dòng)學(xué)約束條件是指: 載體在運(yùn)動(dòng)過(guò)程中僅有前向動(dòng)力, 不發(fā)生側(cè)滑和跳躍, 則載體在x軸和z軸的速度分量為零, 即

載體運(yùn)動(dòng)速度可表示為

對(duì)式（14）進(jìn)行全微分, 得到

由式（13）和式（16）可以構(gòu)造運(yùn)動(dòng)學(xué)約束的量測(cè)方程

式（17） 中,V2為量測(cè)噪聲,H2為量測(cè)矩陣。H2滿足

3 實(shí)驗(yàn)與仿真分析

本次實(shí)驗(yàn)使用了MEA200 高過(guò)載慣導(dǎo)作為子慣導(dǎo), 對(duì)上文提出的MIMU 沖擊補(bǔ)償模型和傳遞對(duì)準(zhǔn)與運(yùn)動(dòng)學(xué)約束輔助進(jìn)行驗(yàn)證: 首先, 對(duì)子慣導(dǎo)進(jìn)行不同量級(jí)的沖擊實(shí)驗(yàn), 采集慣導(dǎo)數(shù)據(jù)建立沖擊補(bǔ)償模型; 再將高精度組合導(dǎo)航系統(tǒng)安裝在實(shí)驗(yàn)車內(nèi), 用于模擬武器平臺(tái)的導(dǎo)航系統(tǒng), 為子慣導(dǎo)提供必要信息; 將子慣導(dǎo)與高精度導(dǎo)航系統(tǒng)安裝在支架上并置于車廂內(nèi), 進(jìn)行跑車實(shí)驗(yàn)并實(shí)時(shí)采集子慣導(dǎo)和組合導(dǎo)航系統(tǒng)數(shù)據(jù), 利用沖擊補(bǔ)償模型修正子慣導(dǎo)的數(shù)據(jù)信息; 最后, 驗(yàn)證傳遞對(duì)準(zhǔn)與運(yùn)動(dòng)學(xué)約束輔助方法的有效性。 MIMU 過(guò)載沖擊實(shí)驗(yàn)如圖2 所示, 跑車實(shí)驗(yàn)現(xiàn)場(chǎng)裝車如圖3 所示。

圖2 過(guò)載沖擊安裝實(shí)驗(yàn)Fig.2 Diagram of overload shock installation experiment

圖3 跑車實(shí)驗(yàn)現(xiàn)場(chǎng)裝車圖Fig.3 Diagram of the vehicle loading in the experiment equipment

3.1 沖擊誤差補(bǔ)償實(shí)驗(yàn)

實(shí)驗(yàn)采用過(guò)載沖擊設(shè)備對(duì)固定在落臺(tái)上的高過(guò)載慣導(dǎo)撞擊造成過(guò)載沖擊, 沖擊軸向?yàn)橥勇輧x和加速度計(jì)的x軸。 實(shí)驗(yàn)對(duì)慣導(dǎo)依次進(jìn)行了2000g～15000g的過(guò)載沖擊, 該設(shè)備對(duì)慣導(dǎo)施加半正弦波的沖擊, 沖擊整體時(shí)間為0.5ms 左右, 每次沖擊前后各采集3 組數(shù)據(jù)并對(duì)慣導(dǎo)進(jìn)行性能測(cè)試與標(biāo)定。

通過(guò)分析不同沖擊量級(jí)的慣導(dǎo)數(shù)據(jù), 慣性器件沿沖擊方向的軸向（x軸） 受沖擊作用影響明顯,選取其一組數(shù)據(jù)對(duì)陀螺儀和加速度計(jì)x軸的零偏與標(biāo)度因數(shù)進(jìn)行多項(xiàng)式擬合。 綜合考慮模型的計(jì)算量和準(zhǔn)確性, 采用3 階多項(xiàng)式模型對(duì)不同沖擊量級(jí)造成的影響進(jìn)行誤差補(bǔ)償。

為驗(yàn)證沖擊誤差補(bǔ)償模型的效果, 以沖擊后慣導(dǎo)的標(biāo)定結(jié)果為基準(zhǔn), 選擇另外一組數(shù)據(jù)比較慣導(dǎo)受過(guò)載沖擊后的前1000 幀輸出數(shù)據(jù)。 在10000g沖擊下, 加速度計(jì)x軸沖擊誤差補(bǔ)償曲線如圖4 所示。 不同沖擊量級(jí)下, 沖擊誤差補(bǔ)償模型在x軸的補(bǔ)償效果如表1 和表2 所示。 實(shí)驗(yàn)結(jié)果表明, MIMU 在受到10000g、15000g沖擊情況下,相較于未補(bǔ)償情況, 使用沖擊誤差模型補(bǔ)償加速度計(jì)和陀螺儀, 其誤差明顯降低。

圖4 加速度計(jì)x 軸沖擊誤差補(bǔ)償曲線Fig.4 Diagram of accelerometer x-axis shock error compensation curves

表1 加速度計(jì)x 軸沖擊誤差補(bǔ)償對(duì)比Table 1 Comparison of accelerometer x-axis shock error compensation

3.2 跑車實(shí)驗(yàn)

跑車實(shí)驗(yàn)地點(diǎn)選在南京市麒麟公園空曠路段,行駛時(shí)間約為13min, 前6min 繞圈行駛進(jìn)行主子慣導(dǎo)傳遞對(duì)準(zhǔn), 最后在500s 時(shí)行駛到指定位置, 停止傳遞對(duì)準(zhǔn), 子慣導(dǎo)切換為運(yùn)動(dòng)學(xué)約束導(dǎo)航行駛3min, 主慣導(dǎo)作為導(dǎo)航基準(zhǔn)進(jìn)行比較, 包含車輛直線行駛的所有狀態(tài), 跑車實(shí)驗(yàn)路線如圖5 所示。

圖5 跑車實(shí)驗(yàn)路線圖Fig.5 Diagram of vehicle experiment route

實(shí)驗(yàn)采用MEA200 作為子慣導(dǎo), 輸出頻率為50Hz, 陀螺儀三軸平均零偏穩(wěn)定性實(shí)測(cè)優(yōu)于11（°） /h,加速度計(jì)零偏穩(wěn)定性為0.1mg。 主慣導(dǎo)采用KVH1750 高精度激光慣導(dǎo), 陀螺儀零偏穩(wěn)定性為0.05（°） /h。 選用NovAtel 公司的PwrPak7 接收機(jī)和KVH1750 慣導(dǎo)作為高精度組合導(dǎo)航系統(tǒng), 其主要性能指標(biāo)如表3 所示。 同時(shí), 采集MIMU 和高精度基準(zhǔn)數(shù)據(jù), 離線進(jìn)行導(dǎo)航算法驗(yàn)證。

表3 高精度組合導(dǎo)航系統(tǒng)主要性能指標(biāo)Table 3 Main performance indexes of high-precision integrated navigation system

跑車實(shí)驗(yàn)行駛路徑如圖6 所示, 姿態(tài)誤差曲線如圖7 所示。 可以看出, 在行駛過(guò)程中主慣導(dǎo)姿態(tài)誤差會(huì)影響子慣導(dǎo)誤差估計(jì)。 在前6min 內(nèi)繞圈行駛進(jìn)行傳遞對(duì)準(zhǔn), 主慣導(dǎo)姿態(tài)存在波動(dòng), 因此子慣導(dǎo)姿態(tài)也存在較大波動(dòng), 姿態(tài)誤差在0.8°以內(nèi);當(dāng)傳遞對(duì)準(zhǔn)完成后, 三個(gè)姿態(tài)角全部收斂, 姿態(tài)誤差穩(wěn)定在0.05°以內(nèi)。 圖8 為約束算法速度誤差曲線, 速度誤差在載體運(yùn)動(dòng)時(shí)存在波動(dòng), 主要是因?yàn)樽藨B(tài)不準(zhǔn)確造成的。 從500s 開(kāi)始停止傳遞對(duì)準(zhǔn), 之后進(jìn)行運(yùn)動(dòng)學(xué)約束。 可以看出, 速度誤差逐漸增大且存在一定幅度的波動(dòng), 原因是由于載體的前向通道沒(méi)有約束。 圖9 為純慣性導(dǎo)航速度誤差曲線, 前500s 與圖8 一致, 存在小范圍波動(dòng),之后停止傳遞對(duì)準(zhǔn)開(kāi)始純慣性導(dǎo)航, 則速度誤差相較于圖8 明顯發(fā)散。

圖6 跑車實(shí)驗(yàn)行駛路徑Fig.6 Driving path of vehicle experiment

圖7 姿態(tài)誤差曲線Fig.7 Curves of attitude error

圖8 約束算法速度誤差曲線Fig.8 Speed error curves of constraint algorithm

圖9 純慣性導(dǎo)航速度誤差曲線Fig.9 Speed error curves of pure inertial navigation

圖10 為運(yùn)動(dòng)學(xué)約束輔助下的慣性導(dǎo)航定位誤差, 單向位置誤差小于50m, 由于直線行駛期間偏航角未發(fā)生改變, 前向通道無(wú)法約束造成某個(gè)方向位置誤差呈現(xiàn)一定累計(jì)趨勢(shì)。 圖11 給出了純慣性導(dǎo)航定位誤差, 可以看到位置誤差快速積累, 單向位置誤差在350m 以內(nèi), 由于主慣導(dǎo)通過(guò)傳遞對(duì)準(zhǔn)為子慣導(dǎo)提供了相對(duì)準(zhǔn)確的初始姿態(tài), 也使得純慣性導(dǎo)航定位精度得到一定程度的優(yōu)化。 比較圖10 和圖11可知, 采用運(yùn)動(dòng)學(xué)約束輔助后, 導(dǎo)航定位精度提升了85%, 高度通道的發(fā)散趨勢(shì)明顯得到抑制。

圖10 約束輔助下的慣性導(dǎo)航定位誤差Fig.10 Diagram of inertial navigation positioning error assisted by constraints

圖11 純慣性導(dǎo)航定位誤差Fig.11 Diagram of pure inertial navigation positioning error

4 結(jié)論

針對(duì)高過(guò)載慣導(dǎo)受到?jīng)_擊后零偏與標(biāo)度因數(shù)產(chǎn)生一定變化的問(wèn)題, 本文構(gòu)建了基于沖擊量級(jí)的動(dòng)態(tài)補(bǔ)償模型, 利用擬合參數(shù)動(dòng)態(tài)調(diào)整MIMU 的誤差模型, 從而減小了加速度計(jì)和陀螺儀的誤差。同時(shí), 本文研究了一種傳遞對(duì)準(zhǔn)與運(yùn)動(dòng)學(xué)約束輔助的方法, 可對(duì)MIMU 進(jìn)行誤差在線標(biāo)定與約束。仿真結(jié)果表明, 該方法可有效提高系統(tǒng)對(duì)準(zhǔn)精度,實(shí)時(shí)修正系統(tǒng)誤差, 可提高無(wú)衛(wèi)星導(dǎo)航情況下的自主導(dǎo)航定位精度。