引入陀螺觀測量的捷聯(lián)快速雙位置對準方法

李　逸，楊孟興，徐兵華

（中國航天科技集團第16研究所，西安710100）

引入陀螺觀測量的捷聯(lián)快速雙位置對準方法

李逸，楊孟興，徐兵華

（中國航天科技集團第16研究所，西安710100）

為了解決初始對準中速度與精度相矛盾的問題，提出了靜基座下引入陀螺測量誤差信息的捷聯(lián)快速雙位置對準方法。在常規(guī)對準方法基礎(chǔ)上，增加陀螺測量角速度信息為觀測量，提高系統(tǒng)可觀測度，提升濾波器的收斂速度。針對雙位置單濾波器方位失準角估計慢的問題，提出雙位置雙濾波器的對準方案。仿真驗證表明，該方案方位失準角的估計精度與水平失準角相當。該方法切實可行，對提高慣導(dǎo)系統(tǒng)的可觀測性和縮短對準時間，有重要的參考價值。

捷聯(lián)慣導(dǎo)；卡爾曼濾波；雙位置對準；可觀測度

0　引言

在初始對準中，常用的方法是卡爾曼濾波，該方法對不可觀測狀態(tài)很難進行有效估計，從而制約了武器系統(tǒng)的精度與速度［1］。為了提高對準速度，文獻［2］、文獻［3］中將等效陀螺引入到對準過程中，加強對陀螺信息的利用，這個方法能加快對準速度，卻不能提高精度，不利于提高武器命中率。為了提高對準精度，文獻［4］、文獻［5］中分別提出了多位置對準方法和連續(xù)旋轉(zhuǎn)對準方法。目前捷聯(lián)慣導(dǎo)系統(tǒng)采用傳統(tǒng)的雙位置對準方法，以速度誤差作為觀測量，整個對準過程采用一個卡爾曼濾波器［6］。由于轉(zhuǎn)位運動、慣性器件誤差的影響，濾波器收斂速度較慢，導(dǎo)致對準時間變長。

為了同時提高初始對準的速度與精度，本文提出了一種快速雙位置對準方法，在兩個位置分別采用兩個卡爾曼濾波器進行狀態(tài)估計，通過兩個位置之間的空間關(guān)系列寫方程組并求解各項誤差參數(shù)，且將每段濾波過程中加入的等效陀螺儀測量信息作為新觀測量，提高每個位置濾波器收斂速度。這樣既能提高系統(tǒng)可觀測性，又能保證系統(tǒng)快速性，最后通過仿真驗證這種方法的可行性。

1　傳統(tǒng)精對準誤差模型建立

1.1捷聯(lián)慣導(dǎo)系統(tǒng)誤差模型

選取東北天坐標系為導(dǎo)航坐標系n系，右前上為機體坐標系b系，在不考慮垂直通道的加速度計偏置和速度誤差時，建立靜基座的誤差模型如下［7］：

其中，δVE、δVN分別表示東、北方向的速度誤差；表示東、北方向的等效加速度計常值漂移；φE、φN、 φU表示三個姿態(tài)失準角；εE、εN、 εU表示東、北、天向的等效陀螺常值漂移；ωie為地球自轉(zhuǎn)角速度，g為當?shù)刂亓铀俣龋琇為當?shù)鼐暥取?/p>

1.2狀態(tài)方程建立

根據(jù)捷聯(lián)系統(tǒng)誤差模型，在粗對準獲得載體的初始姿態(tài)矩陣后，可以使用卡爾曼濾波法進行精對準。首先建立系統(tǒng)的狀態(tài)方程：

式中，

其中，I表示單位矩陣，W表示系統(tǒng)噪聲矩陣。

1.3常規(guī)觀測方程

在靜基座條件下，傳統(tǒng)的卡爾曼濾波常采用慣導(dǎo)解算的速度誤差作為觀測量。取東向和北向速度誤差為觀測量，設(shè)觀測噪聲為V，得到觀測方程為：

由狀態(tài)方程（式（2））和觀測方程（式（3））組成常規(guī)對準系統(tǒng)方程：

將系統(tǒng)方程（式（4））離散化后，使用離散卡爾曼濾波算法對狀態(tài)X進行估計，完成常規(guī)的精對準過程。

2　引入陀螺信息的快速對準方法

靜基座下，常規(guī)對準方法采用速度誤差為觀測量，通過卡爾曼濾波方法完成精對準。該算法對φU估計時間長，這是由于其是利用水平用加速度計測量方位羅經(jīng)效應(yīng)引起的水平傾斜，然后通過積分得到速度誤差進行估計的，陀螺信息沒有直接參與初始對準。為了加快φU的估計速度，文獻［2］、文獻［3］將等效陀螺測量信息加入到觀測方程中，加強對陀螺信息的利用。

2.1建立等效陀螺觀測方程

在理想靜基座條件下，假設(shè)導(dǎo)航坐標各軸系指向真東北天，陀螺儀測量沒有誤差，那么陀螺的輸出應(yīng)該為地球自轉(zhuǎn)速度ωie在機體坐標系各軸上的投影，即然而實際情況中，由于慣性器件本身的誤差和各種外界隨機干擾，其輸出必然帶有誤差，其中一部分是由于計算坐標系c系和導(dǎo)航坐標系n系的偏差角φ引起的，另一部分是由于陀螺儀自身的漂移引起的。

將式（6）展開，并把εb投影到n系中，得到方程組：

其中，

由式（2）和式（8）構(gòu)成了快速對準系統(tǒng)方程：

2.2可觀測性與可觀測度分析

從系統(tǒng)可觀測性和可觀測度出發(fā)，比較常規(guī)對準系統(tǒng)（式（4））和快速對準系統(tǒng)（式（9））的對準效果。設(shè)式（4）和式（9）的可觀測矩陣分別為Q和Q1，則有：

計算rank（Q）=rank（Q1）=7，說明兩個系統(tǒng)均有三個不可觀狀態(tài)，一般認為是▽E、▽N和εE，這說明引入等效陀螺測量信息不能增加系統(tǒng)的可觀測狀態(tài)個數(shù)。由于初始對準的極限精度與這三個不可觀測狀態(tài)有關(guān)，故這兩個系統(tǒng)的對準精度相當。

將Q和Q1進行奇異值分解Q=UΣVT，，得到：

可以看出，Σ與Σ1的區(qū)別就是εU對應(yīng)的可觀測度由常規(guī)方法的0.0006變成新方法的1，即可觀測度顯著提高，短時間內(nèi)由近似不可觀變?yōu)榭捎^測。

3　快速雙位置對準方案設(shè)計

傳統(tǒng)單位置初始對準方法中，系統(tǒng)不完全可觀，而且對準的極限精度與系統(tǒng)的不可觀測狀態(tài)有關(guān)。雙位置對準法通過改變IMU位置，使系統(tǒng)變?yōu)橥耆捎^測系統(tǒng)，估計出全部的狀態(tài)變量，從而有效消除不可觀測狀態(tài)的影響，提高系統(tǒng)精度。文獻［4］中指出，雙位置對準采用航向角變化的效果最好，且當航向角轉(zhuǎn)動180°時，所得到的第二位置具有最小航向角誤差估計和最好的位置變化靈敏度。

3.1常規(guī)雙位置對準方法

常規(guī)雙位置對準方法采用系統(tǒng)方程完成精對準，并且兩個位置使用一個卡爾曼濾波器。首先在第一位置進行對準，待方位失準角收斂后引入第二位置，繼續(xù)進行對準，待方位失準角再次收斂后，IMU轉(zhuǎn)回正常位置，對準結(jié)束。

該方法對準精度高，時間長。方位失準角φU和等效北向陀螺漂移εN估計速度慢，兩者需要約300s才能穩(wěn)定。為了使雙位置對準達到最優(yōu)精度，必須等到狀態(tài)估計收斂后，才能引入第二位置，而且每一位置的對準時間均需300s，總共需要600s，大大制約了初始對準時間。同時由于對準過程中轉(zhuǎn)位機構(gòu)、慣性器件誤差等因素影響，若濾波器參數(shù)設(shè)計不好會導(dǎo)致大的超調(diào)過程，致使對準時間變長。

3.2快速雙位置對準流程

針對以上缺點，本文提出一種分段式濾波的雙位置對準法，在兩個位置分別采用兩個卡爾曼濾波器進行狀態(tài)估計，建立兩個位置之間誤差參數(shù)的數(shù)學(xué)關(guān)系，通過求解方程組獲得各項誤差參數(shù)，最后對系統(tǒng)進行一次修正，完成兩位置精對準過程。同時在每個濾波器的觀測方程中引入陀螺測量信息加快濾波器的收斂速度，達到快速對準的目的。

根據(jù)單位置對準的仿真結(jié)果分析，水平失準角的收斂速度很快，在十幾秒內(nèi)就可以達到穩(wěn)定，而且精度很高，提升的空間不大。故在每段濾波完成后先進行水平精對準，然后進行方位失準角計算和修正。

根據(jù)分段濾波的思想，設(shè)計具體對準方案：

2）在位置1進行精對準，濾波器穩(wěn)定后記錄失準角φE、φN、φU和等效北向漂移εN的估計值，對進行水平修正但不修正方位角，然后進入導(dǎo)航狀態(tài)。

3）按照轉(zhuǎn)位要求，在一定時間內(nèi)進行轉(zhuǎn)位。為保證可觀測度，轉(zhuǎn)位角度要求在90°～270°之間。

4）在位置2重新進行精對準，記錄第二次對準的估計值φE、φN、φU、εN。

通過兩個位置記錄數(shù)據(jù)進行誤差參數(shù)計算，并對系統(tǒng)進行修正。

整個對準過程的基本流程如圖1所示。

圖1　雙位置對準流程圖Fig.1　Flow chart of two-position alignment

在本方案中，在當φU的估計精度達到要求后，才能對姿態(tài)更新矩陣作修正。修正前，φU以粗對準確定的姿態(tài)誤差為初值進行傳播，并未受到任何抑制，所以采用開環(huán)卡爾曼濾波，有利于誤差信息的積累。

4　誤差參數(shù)計算公式

4.1數(shù)據(jù)記錄

快速雙位置對準的本質(zhì)是通過兩個位置的精對準過程所獲得的數(shù)據(jù)求解各個誤差參數(shù)，包括姿態(tài)誤差角、等效陀螺常值漂移等。

第一個位置精對準結(jié)束后，記錄如下數(shù)據(jù)：

φE1、φN1、φU1——位置1東北天三個方向的失準角；

εN1——位置1等效北向陀螺漂移。第二個位置精對準結(jié)束后，記錄如下數(shù)據(jù)：

φE2、φN2、φU2——位置2東北天三個方向的失準角；

εN2——位置2等效北向陀螺漂移。

4.2誤差參數(shù)計算公式推導(dǎo)

在第一次精對準過程中，濾波器達到穩(wěn)定后，位置1的狀態(tài)估計值有以下關(guān)系：

其中，εE1為位置1的不可觀測狀態(tài)。

為了保證對準過程中方位失準角φU變化不大，在第一次精對準結(jié)束后進行水平修正。設(shè)粗對準完成后獲得初始姿態(tài)矩陣為水平修正后的姿態(tài)矩陣為其中，

同理，位置2的狀態(tài)估計值有以下關(guān)系：

接下來計算方位失準角φU，引入IMU坐標系b1、b2，在位置1時，坐標系b1的xyz軸分別與初始機體坐標系b右前上重合，在繞方位軸旋轉(zhuǎn)θ角后到位置2的坐標系為b2，轉(zhuǎn)換矩陣為：

分別計算兩個位置的等效陀螺漂移：

為了保證計算精度，Cnb需經(jīng)過誤差修正：

根據(jù)仿真條件，采用分段卡爾曼濾波算法對本文中提出的雙位置快速對準法進行10min的數(shù)學(xué)仿真。盡管在引入陀螺測量信息后只需要50s狀態(tài)

當載體接近水平時，由于IMU按照方位軸進行旋轉(zhuǎn)，天向通道誤差參數(shù)對水平誤差參數(shù)的計算基本沒有影響，根據(jù)轉(zhuǎn)換矩陣，可以得到誤差參數(shù)有以下關(guān)系：

式中，εx、εy表示IMU上固連的陀螺儀常值漂移。

將式（10）和式（11）、式（12）、式（13）對應(yīng)相減并整理，得到等效水平漂移的變化量為：

聯(lián)立式（16）和式（17），得到：

其中，

解式（18）得水平陀螺儀誤差參數(shù)：

在計算εx、εy后，可以根據(jù)式（16）計算兩個位置的不可觀測狀態(tài)εE。

在獲得εE后，根據(jù)式（12）計算φU，得：

最后對系統(tǒng)進行一次方位修正，整個對準過程結(jié)束。

5　仿真分析

5.1仿真條件

為了驗證該算法的正確性，進行計算機仿真，設(shè)置仿真條件如下：

陀螺儀漂移：0.02（°）/h；

加速度計零偏：50μg；

粗對準后方位失準角：1°；

載體姿態(tài)角：均為0°；

轉(zhuǎn)位速度：10（°）/s；

轉(zhuǎn)位角度：180°。

5.2仿真結(jié)果與分析

首先在單位置條件下，進行常規(guī)對準系統(tǒng)和快速對準系統(tǒng)的對準效果比較仿真，結(jié)果如圖2所示?？梢钥闯觯谝胪勇轀y量信息后，方位失準角φU，等效北向漂移εN和等效天向漂移εU在50s內(nèi)可以快速收斂，方位失準角的估計精度與常規(guī)算法相當。量可以快速收斂。但為了減小噪聲和外界干擾，適當延長每一段精對準時間到100s，方位失準角估計結(jié)果如圖3所示。

圖2　單位置對準常規(guī)方法與快速算法對比圖Fig.2　Comparison chart of conventional method and fast algorithm in one-position alignment

圖3　雙位置快速對準方位角誤差估計曲線Fig.3　Estimated curve of azimuth error under fast twoposition Kalman filter

作為對比，當采用連續(xù)卡爾曼濾波，即兩個位置用一個濾波器時，在第250s進行轉(zhuǎn)位引入第二位置，方位角誤差估計曲線如圖4所示。

圖4　連續(xù)卡爾曼濾波方位角誤差估計曲線Fig.4　Estimated curve of azimuth error under continuous Kalman filter

通過新的雙位置分段卡爾曼濾波對準方法得到的結(jié)果，利用式（12）、式（16）、式（19）和式（20）可計算得到陀螺儀等效漂移、方位失準角等參數(shù)，計算結(jié)果如表1所示。

綜上可以得出以下結(jié)果：

表1　快速雙位置對準估計結(jié)果Table 1　Result of fast two-position alignment

1）根據(jù)圖2，當引入陀螺測量信息后，φU與εN的收斂速度比常規(guī)方法明顯提升，εU也由近似不可觀變?yōu)榭捎^測。

2）比較圖3和圖4，受載體轉(zhuǎn)位運動和慣導(dǎo)內(nèi)部誤差影響，采用一個濾波器的雙位置對準算法在第二個位置的估計過程中超調(diào)較大且收斂緩慢，對準過程需要10min。采用雙濾波器結(jié)構(gòu)不存在這一問題，在每個位置很快收斂。而且在引入陀螺測量信息為觀測量后，系統(tǒng)收斂速度明顯提升，整個對準過程可以在4min內(nèi)完成。

3）根據(jù)表1的計算結(jié)果，采用雙位置快速對準法，可以準確地估計出方位失準角和陀螺儀漂移。而且經(jīng)過計算修正以后，方位對準的精度達到30''，實現(xiàn)快速、準確的對準。

6　結(jié)論

本文針對傳統(tǒng)雙位置對準中方位失準角收斂速度慢的問題，提出了引入陀螺測量信息的快速雙位置對準算法。該方法充分利用了陀螺輸出信息，改善了系統(tǒng)的可觀測性和可觀測度，并且采用雙位置雙濾波器的結(jié)構(gòu)，避免了旋轉(zhuǎn)過程中由于系統(tǒng)超調(diào)而引起的收斂速度過慢。對準結(jié)束后，根據(jù)記錄數(shù)據(jù)與數(shù)據(jù)之間的物理關(guān)系用求解方程組的方法求解各項誤差參數(shù)。通過計算機仿真驗證，對準過程可在4min內(nèi)完成，采用常值漂移為0.02（°）/h的陀螺，方位姿態(tài)角估計誤差在1'之內(nèi)。該方法切實可行，有一定的應(yīng)用參考價值。

［1］曹通.光纖陀螺捷聯(lián)慣導(dǎo)系統(tǒng)在線對準及標定技術(shù)研究［D］.哈爾濱工程大學(xué)，2012. CAO Tong.On-line alignment and calibration techniqueof fiber optic gyroscope SINS［D］.Harbin Engineering University，2012.

［2］高偉熙，繆玲娟，倪茂林.一種引入陀螺角速度信息的快速對準方法［J］.宇航學(xué)報，2010，31（6）∶1597-1601. GAO Wei-xi，MIAO Ling-juan，NI Mao-lin.A fast alignment method based on gyro palstance information［J］. Journal ofAstronautics，2010，31（6）∶1597-1601.

［3］黃湘遠，湯霞清，郭理彬.基于等效陀螺的捷聯(lián)慣導(dǎo)快速兩位置對準仿真［J］.計算機仿真，2012，29（3）∶68-71. HUANG Xiang-yuan，TANG Xia-qing，GUO Li-bin. Simulation of fast two-position alignment of SINS based on equivalent gyro［J］.Computer Simulation，2012，29 （3）∶68-71.

［4］錢偉行，劉建業(yè)，趙偉，趙文芳.基于轉(zhuǎn)動基座的SINS初始對準方法研究［J］.宇航學(xué)報，2008，29（3）∶928-932. QIAN Wei-xing，LIU Jian-ye，ZHAO Wei，ZHAO Wenfang.Study of SINS initial alignment based on rotating susceptor［J］.Journal of Astronautics，2008，29（3）∶928-932.

［5］周紹磊，吳修振，徐海剛，等.一種新的捷聯(lián)慣導(dǎo)快速雙位置對準方法［J］.中國慣性技術(shù)學(xué)報，2012，20（5）∶525-529. ZHOU Shao-lei，WU Xiu-zhen，XU Hai-gang，et al.New rapid two-position alignment method of SINS［J］.Journal of Chinese Inertial Technology，2012，20（5）∶525-529.

［6］高偉熙，繆玲娟，沈軍.一種分段快速捷聯(lián)慣導(dǎo)靜基座自對準新方法［C］.第29屆中國控制會議，2010（7）∶3307-3310. GAO Wei-xi，MIAO Ling-juan，SHEN Jun.A nes piecewise fast alignment methoh for SINS stationary selfalignment［C］.Proceedings of the 29thChinese Control Conference，2010（7）∶3307-3310.

［7］秦永元.慣性導(dǎo)航［M］.北京∶科學(xué)出版社，2006. QIN Yong-yuan.Inertial navigation［M］.Beijing∶Science Press，2006.

［8］ Zhang Ludong，Liang Junxiang，Wu Meiping，et al.Research on auto compensation technique of strapdown inertial navigation systems［C］.2009 International Asia Conference on Informatics in Control，Automation and Robotics，2009∶350-359.

［9］劉志琴，王新龍.捷聯(lián)慣導(dǎo)系統(tǒng)最優(yōu)多位置對準的確定與分析［J］.北京航空航天大學(xué)學(xué)報，2013，39（3）∶330-334. LIU Zhi-qin，WANG Xin-long.Confirmation and analysis on the optimal multipostion alignment of strapdown inertial navigation system［J］.Journal of Beijing University of Aeronautics and Astronautics，2013，39（3）∶330-334.

［10］Li An，Chang Guobin，Qin Fangjun，et al.Improved precision of strapdown inertial navigation system brought by dual-axis continuous rotation of inertial measurement unit［C］.2010 2ndInternational Asia Conference on Informatics in Control，Automation and Robotics，2010∶284-287.

［11］Gebre-Egziabher D，Elkaim G H.MAV attitude determination by vector matching［J］.IEEE Transactions on Aerosapce and Electronic Systems，2008，44（3）∶1011-1028.

［12］ Garry A，John T，David C.Riccati equation and EM alogrithm convergence for inertial navigation alignment ［J］.IEEE Transactions on Signal Processing，2009，57 （1）∶370-375.

Fast Two-positionAlignment of SINS Based on Gyro Observation Information

LI Yi，YANG Meng-xing，XU Bing-hua
（The 16thInstitute，ChinaAerospace Science and Technology Corporation，Xi’an 710100）

To solve the contradiction between rapid response and high precision of SINS，a rapid two-position alignment scheme with gyro error observation information on the static base is presented.By analyzing conventional alignment method，the observation with gyro error information was increased to improve the system observability and the convergence speed of Kalman filter.To solve the problem of slow estimation of Tazimuth misalignment angle in traditional two-position alignment，the alignment method of two-position and two-filter is presented.The simulation results show that，the horizontal and azimuth errors are approximately equal.These show that the method is feasible and very consultation to improve the system observability and reduce the alignment time greatly.

SINS；Kalman filter；two-position alignment；observability

U666.1

A

1674-5558（2016）01-01065

10.3969/j.issn.1674-5558.2016.02.006

2015-01-19

李逸，男，精密儀器與機械專業(yè)，碩士，研究方向為高精度捷聯(lián)慣性導(dǎo)航研究。