一種適用于MEMS慣組的低維傳遞對(duì)準(zhǔn)方法

馬 昆，戴居峰，聶曉慧，胡 雋

(1.北京控制工程研究所，北京100094;2.北京軒宇空間科技有限公司，北京100190)

0 引言

使用微機(jī)電系統(tǒng)(Micro Electro Mechanical Systems,MEMS)慣組作為敏感器是實(shí)現(xiàn)低成本、輕量化捷聯(lián)慣導(dǎo)系統(tǒng)的主要方案。為解決MEMS陀螺和加速度計(jì)常漂大、重復(fù)性差、與載體安裝誤差難以標(biāo)定的問題,需利用高精度主慣導(dǎo)信息通過傳遞對(duì)準(zhǔn)實(shí)現(xiàn)對(duì)MEMS慣導(dǎo)誤差的估計(jì),從而提高導(dǎo)航精度。傳遞對(duì)準(zhǔn)方法的評(píng)價(jià)準(zhǔn)則為誤差估計(jì)的收斂速度及準(zhǔn)確度。

影響上述估計(jì)效果的主要因素為誤差模型的構(gòu)造、匹配量的選取、濾波算法的設(shè)計(jì)。誤差模型中包含有慣導(dǎo)系統(tǒng)誤差和慣性器件誤差,其中系統(tǒng)誤差主要來源于主子慣導(dǎo)的安裝誤差、撓性形變、高頻震顫、臂桿效應(yīng)；器件誤差則大多考慮陀螺和加速度計(jì)的常漂、刻度系數(shù)非線性等因素[1]。匹配量的選擇會(huì)影響傳遞對(duì)準(zhǔn)的速度、精度和可觀測(cè)性,常用的匹配量包括姿態(tài)、速度和位置。Kain等[2]首次使用了 “速度+姿態(tài)”的組合匹配量將估計(jì)收斂速度降為10s,而后國(guó)內(nèi)外學(xué)者結(jié)合傳遞對(duì)準(zhǔn)期間載體的機(jī)動(dòng)特性,對(duì)多種組合匹配量的估計(jì)效果進(jìn)行了研究,文獻(xiàn)[3]對(duì)各種組合匹配量的優(yōu)缺點(diǎn)及適用場(chǎng)景進(jìn)行了總結(jié)。濾波算法設(shè)計(jì)方面,Baziw等[4-5]提出了最小二乘法,Schneider[6]則首次使用了Kalman濾波公式。目前,傳遞對(duì)準(zhǔn)方法主要使用Kalman濾波算法對(duì)組合匹配量進(jìn)行估計(jì)。

本文選取 “速度+姿態(tài)陣”進(jìn)行Kalman濾波估計(jì)。為提高收斂速度,相較于傳統(tǒng)傳遞對(duì)準(zhǔn)方法,本文對(duì)誤差模型進(jìn)行了適當(dāng)簡(jiǎn)化,只選擇作為誤差主要來源的陀螺、加速度計(jì)常漂及安裝誤差角進(jìn)行估計(jì),從而實(shí)現(xiàn)了誤差估計(jì)的快速、準(zhǔn)確收斂。本文簡(jiǎn)要介紹了MEMS慣組捷聯(lián)慣導(dǎo)系統(tǒng)的組成,對(duì)傳遞對(duì)準(zhǔn)方法進(jìn)行了詳細(xì)推導(dǎo),開展了數(shù)學(xué)仿真與跑車試驗(yàn)驗(yàn)證,并對(duì)仿真估計(jì)誤差來源進(jìn)行了分析,同時(shí)對(duì)跑車試驗(yàn)中方法的估計(jì)效果進(jìn)行了評(píng)價(jià)。

1 系統(tǒng)組成

如圖1所示,慣導(dǎo)系統(tǒng)的硬件包括MEMS慣組、信號(hào)接口電路及導(dǎo)航計(jì)算機(jī)。其中,MEMS慣組采集載體角速度及加速度,信號(hào)接口電路完成慣組數(shù)據(jù)的接收,導(dǎo)航計(jì)算機(jī)根據(jù)載體角速度及加速度完成導(dǎo)航計(jì)算并進(jìn)行傳遞對(duì)準(zhǔn)計(jì)算。

圖1 慣導(dǎo)系統(tǒng)硬件組成Fig.1 Hardware composition of inertial navigation system

導(dǎo)航計(jì)算機(jī)、MEMS慣組、信號(hào)接口電路、電源轉(zhuǎn)換電路安裝在三塊電路板上,電路板間通過撓性板連接,三塊電路板嵌入安裝在圓柱形筒體內(nèi),其外部結(jié)構(gòu)如圖2所示。

圖2 慣導(dǎo)系統(tǒng)外結(jié)構(gòu)Fig.2 External structure of inertial navigation system

導(dǎo)航計(jì)算機(jī)處理器選用TI公司的DSP產(chǎn)品TMS320C6727,其最高主頻為350MHz,內(nèi)置256KB RAM,BGA256封裝(17.2mm×17.2mm),3.3V/1.2V供電,具備完成導(dǎo)航及傳遞對(duì)準(zhǔn)計(jì)算的能力。接口電路使用FPGA實(shí)現(xiàn)SPI串行外設(shè)接口,完成慣性測(cè)量單元與導(dǎo)航計(jì)算機(jī)的數(shù)據(jù)交互。MEMS慣性測(cè)量單元使用AD公司生產(chǎn)的ADIS16488,該款產(chǎn)品包含一個(gè)三軸陀螺、一個(gè)三軸加速度計(jì)、一個(gè)三軸磁強(qiáng)計(jì)和一個(gè)壓力傳感器,使用SPI串行外設(shè)接口與外部進(jìn)行通訊,該慣組的指標(biāo)如表1所示。

表1 MEMS慣組指標(biāo)Table 1 Indexes of MEMS inertial units

2 傳遞對(duì)準(zhǔn)方法設(shè)計(jì)

傳遞對(duì)準(zhǔn)方法包括誤差模型構(gòu)建、匹配量選取以及濾波算法設(shè)計(jì)三個(gè)方面。在誤差模型構(gòu)建方面,由于MEMS組件零偏重復(fù)性較差,無法直接使用地面標(biāo)定結(jié)果,故陀螺常漂Δω、加速度計(jì)常漂Δf可以看作導(dǎo)航誤差的主要來源[7]。而載體相對(duì)主慣導(dǎo)安裝誤差δμ以及MEMS慣組相對(duì)載體的安裝誤差δA因無法進(jìn)行準(zhǔn)確測(cè)量并補(bǔ)償,成為產(chǎn)生導(dǎo)航誤差的另一來源[8]。

確定誤差模型后,需要匹配量完成誤差模型的表達(dá)與觀測(cè)。在捷聯(lián)慣導(dǎo)計(jì)算中,常用的匹配量有 “速度”匹配、“角速度”匹配、“姿態(tài)角”匹配、“姿態(tài)陣”匹配,各匹配方案特性如表2所示[9]。本文使用MEMS慣組進(jìn)行傳遞對(duì)準(zhǔn)計(jì)算的主要應(yīng)用場(chǎng)景為平動(dòng)狀態(tài),結(jié)合表2的匹配方案特性分析,為實(shí)現(xiàn)平動(dòng)狀態(tài)下誤差的快速估計(jì)[10],選取速度及姿態(tài)陣作為匹配量。

表2 匹配量特征Table 2 Features of matching data

本文使用Kalman濾波算法對(duì)誤差進(jìn)行估計(jì),為提高估計(jì)速度,需在構(gòu)建準(zhǔn)確誤差模型的前提下降低誤差估計(jì)向量維度、減少濾波算法計(jì)算量。相對(duì)傳統(tǒng)傳遞對(duì)準(zhǔn)方法,本文在誤差模型構(gòu)建時(shí)略去了陀螺、加速度計(jì)的刻度系數(shù)誤差及撓曲變形角誤差,最大限度地降低了誤差估計(jì)向量維度。在使用簡(jiǎn)化模型進(jìn)行估計(jì)時(shí),刻度系數(shù)誤差將計(jì)入安裝誤差,不會(huì)影響估計(jì)精度。而撓曲變形角誤差在實(shí)際物理環(huán)境下為一變化量,由撓曲變形及高頻顫振產(chǎn)生,但是本文的主子慣導(dǎo)為剛性安裝,不存在上述問題。因此,在建模時(shí)忽略撓曲變形角,對(duì)估計(jì)精度影響有限。低維度、適當(dāng)精度估計(jì)誤差模型的建立,確保了Kalman濾波算法的快速、準(zhǔn)確收斂,誤差建模及濾波算法迭代計(jì)算過程如下。

構(gòu)建X軸、Y軸、Z軸分別為地理北、天、東三個(gè)方向的導(dǎo)航直角坐標(biāo)系n,構(gòu)建主慣導(dǎo)測(cè)量坐標(biāo)系bm、子慣導(dǎo)測(cè)量坐標(biāo)系bs。根據(jù)上述匹配量選取分析,匹配向量Zc為

依據(jù)捷聯(lián)慣導(dǎo)導(dǎo)航算法,主子慣導(dǎo)的速度、角速度更新公式如下

主子慣導(dǎo)進(jìn)行導(dǎo)航計(jì)算產(chǎn)生的不一致性主要原因有子慣導(dǎo)測(cè)量加速度計(jì)常漂、陀螺常漂、子慣導(dǎo)本體系相對(duì)載體系安裝誤差δA以及載體相對(duì)主慣導(dǎo)安裝誤差δμ, 構(gòu)造測(cè)量誤差模型有

可得匹配量誤差模型為(略去二階小量)

依據(jù)誤差模型,可得估計(jì)矩陣A為

根據(jù)觀測(cè)模型,得到觀測(cè)矩陣H為

根據(jù)Kalman濾波算法,按圖3所示的方式進(jìn)行遞推計(jì)算。

圖3 Kalman濾波算法計(jì)算過程Fig.3 Calculation process of Kalman filter algorithm

3 仿真及試驗(yàn)驗(yàn)證

3.1 仿真條件及仿真結(jié)果

仿真生成的運(yùn)動(dòng)軌跡為平動(dòng)的載體三軸角速度與加速度,以此作為主慣導(dǎo)信息。以主慣導(dǎo)信息為基礎(chǔ),附加安裝誤差δA和δμ、 陀螺常漂加速度計(jì)常漂生成子慣導(dǎo)的陀螺角速度、加速度。主子慣導(dǎo)使用相同的導(dǎo)航算法進(jìn)行導(dǎo)航解算,并按照第2章所述傳遞對(duì)準(zhǔn)方法對(duì)子慣導(dǎo)的安裝誤差、陀螺常漂、加速度計(jì)常漂進(jìn)行估計(jì)。

仿真初始滾動(dòng)角、俯仰角、偏航角分別為0°、0°、45°,初始北、天、東地理速度分別為45m/s、0m/s、 0m/s, 陀螺常漂為 0.574(°)/s、 -0.574(°)/s、0.2(°)/s,加速度計(jì)常漂為 0.4g、0.3g、-0.2g,載體安裝誤差δμ為-0.5°、 1.5°、 1.0°, 子慣導(dǎo)安裝誤差δA為-0.5°、 0.5°、 0.5°, 主慣導(dǎo)到子慣導(dǎo)的總安裝誤差為-1.0°、 2.0°、1.5°。 傳遞對(duì)準(zhǔn)方法的誤差估計(jì)曲線如圖4、圖5所示。圖中的ux、uy、uz為載體安裝誤差向量δμ的元素,Ax、Ay、Az為子慣導(dǎo)安裝誤差向量δA的元素,為加速度計(jì)常漂向量的元素,為陀螺常漂向量的元素。

圖4 載體和子慣導(dǎo)安裝誤差的仿真估計(jì)收斂曲線Fig.4 Convergence curves of simulation estimation for installation error of carrier and sub-INS

圖5 加速度計(jì)常漂和陀螺常漂的仿真估計(jì)收斂曲線Fig.5 Convergence curves of simulation estimation for constant drift of accelerometer and gyroscope

從誤差估計(jì)曲線可以看出,誤差估計(jì)均在10s內(nèi)實(shí)現(xiàn)收斂,估計(jì)得出的陀螺三軸常漂為0.53862(°)/s、-0.53289(°)/s、 0.18336(°)/s, 加速度計(jì)三軸常漂為0.3786g、0.2829g、-0.2089g,載體三 軸 安裝 誤差 為 -0.52716°、 0.95691°、0.75063°,子慣導(dǎo)三軸安裝誤差為-0.52716°、0.95691°、0.75063°,主慣導(dǎo)到子慣導(dǎo)的總安裝誤差為-1.0543°、 1.9138°、 1.5012°。 上述誤差估計(jì)結(jié)果與仿真設(shè)置誤差值基本一致,說明該傳遞對(duì)準(zhǔn)方法能夠?qū)崿F(xiàn)快速、準(zhǔn)確的估計(jì)。

載體安裝誤差δμ與子慣導(dǎo)安裝誤差δA兩者估計(jì)結(jié)果相同,估計(jì)結(jié)果與仿真設(shè)置值不一致,但是兩者相加的總誤差與仿真設(shè)置值一致。這是由于誤差模型中兩者估計(jì)矩陣一致,因此估計(jì)結(jié)果也一致。該誤差模型不能實(shí)現(xiàn)δμ與δA的分別估計(jì),只能完成δμ+δA總誤差的估計(jì)。

陀螺常漂、加速度計(jì)常漂、總安裝誤差估計(jì)結(jié)果雖與仿真設(shè)置值基本一致,但仍有差別,這主要是由于誤差模型中對(duì)子慣導(dǎo)相對(duì)于主慣導(dǎo)的誤差姿態(tài)陣?yán)硐牖癁?I-δφ×)Cnbs=Cnbm。而在實(shí)際應(yīng)用或仿真中,CnbmCbsn并非對(duì)角陣(為I的理想矩陣),且上述偏差隨時(shí)間會(huì)持續(xù)累積增大。

3.2 跑車試驗(yàn)驗(yàn)證

為驗(yàn)證傳遞對(duì)準(zhǔn)方法在實(shí)際應(yīng)用中的效果,本文開展了跑車試驗(yàn)。試驗(yàn)中,高精度主慣導(dǎo)及子慣導(dǎo)載體均安裝在汽車上。在汽車勻速行駛階段進(jìn)行40s的傳遞對(duì)準(zhǔn)計(jì)算,估計(jì)子慣導(dǎo)MEMS慣組的加速度計(jì)常漂陀螺常漂、安裝誤差δA和δμ。傳遞對(duì)準(zhǔn)完成后,將估計(jì)出的誤差對(duì)MEMS慣組測(cè)量值進(jìn)行補(bǔ)償,并開始自主導(dǎo)航。汽車上安裝高動(dòng)態(tài)GPS接收機(jī)作為基準(zhǔn),對(duì)導(dǎo)航結(jié)果進(jìn)行評(píng)估,同時(shí)與MEMS慣組未進(jìn)行誤差補(bǔ)償?shù)膶?dǎo)航結(jié)果進(jìn)行比較。

設(shè)置初始安裝誤差δA和δμ均為5°、加速度計(jì)常漂為0.1g、陀螺常漂為8(°)/s,經(jīng)傳遞對(duì)準(zhǔn)計(jì)算40s的誤差估計(jì)收斂曲線如圖6、圖7所示。

圖6 載體和子慣導(dǎo)安裝誤差的估計(jì)收斂曲線Fig.6 Estimation convergence curves for installation error of carrier and sub-INS

圖7 加速度計(jì)常漂和陀螺常漂的估計(jì)收斂曲線Fig.7 Estimation convergence curves for constant drift of accelerometer and gyroscope

經(jīng)傳遞對(duì)準(zhǔn)方法估計(jì)出的載體安裝誤差δμ與子慣導(dǎo)安裝誤差δA相同,均為 0.22°、0.05°、-0.18°；加速度計(jì)三軸常漂分別為-0.0240g、0.0005g、-0.0310g,陀螺三軸常漂分別為0.0032(°)/s、0.0071(°)/s、 -0.0084(°)/s。

傳遞對(duì)準(zhǔn)完成后,使用上述估計(jì)結(jié)果對(duì)子慣導(dǎo)初始姿態(tài)、加速度計(jì)常漂、陀螺常漂進(jìn)行補(bǔ)償并開始導(dǎo)航計(jì)算,將補(bǔ)償后的導(dǎo)航結(jié)果與未進(jìn)行補(bǔ)償?shù)膶?dǎo)航結(jié)果進(jìn)行比較,補(bǔ)償前后導(dǎo)航20s內(nèi)導(dǎo)航系(北、天、東)三個(gè)方向的導(dǎo)航誤差如圖8所示。

圖8 補(bǔ)償前后導(dǎo)航誤差曲線對(duì)比結(jié)果Fig.8 Comparison of navigation error curves before and after compensation

由圖8可以看出,北、天、東三個(gè)方向的導(dǎo)航20s后的誤差由補(bǔ)償前的 16.05m、98.94m、62.45m降為補(bǔ)償后的12.34m、28.35m、33.29m。可見經(jīng)補(bǔ)償后,導(dǎo)航精度得到較大提升。

4 結(jié)論

本文設(shè)計(jì)了一種同時(shí)使用速度以及姿態(tài)陣作為匹配量的Kalman濾波傳遞對(duì)準(zhǔn)方法,實(shí)現(xiàn)了高精度主慣導(dǎo)對(duì)MEMS慣組加速度計(jì)常漂、陀螺常漂、安裝誤差的正確估計(jì)。通過數(shù)學(xué)仿真及跑車試驗(yàn)驗(yàn)證,該傳遞對(duì)準(zhǔn)方法估計(jì)誤差快速、準(zhǔn)確,可以較大幅度提升MEMS慣組的導(dǎo)航精度。