符 彥，王劍輝，韓 菲

地固坐標(biāo)系下捷聯(lián)慣導(dǎo)系統(tǒng)初始精對(duì)準(zhǔn)

符 彥1，王劍輝1，韓 菲2

（1. 廣東省地質(zhì)測(cè)繪院，廣州 510800；2. 長(zhǎng)安大學(xué) 地質(zhì)工程與測(cè)繪學(xué)院，西安 710054）

為了使捷聯(lián)慣性導(dǎo)航系統(tǒng)（SINS）在初始工作時(shí)更加準(zhǔn)確地輸出導(dǎo)航參數(shù)，提出一種地固系下的初始精對(duì)準(zhǔn)方法：在全球衛(wèi)星導(dǎo)航系統(tǒng)（GNSS）與SINS組合導(dǎo)航中，以地固系力學(xué)編排方案所對(duì)應(yīng)的誤差狀態(tài)方程為基礎(chǔ)，用卡爾曼濾波估計(jì)SINS載體坐標(biāo)系到地固坐標(biāo)系的平臺(tái)“失準(zhǔn)角”，直接輸出載體在地固坐標(biāo)系下的位置與速度，從而實(shí)現(xiàn)SINS的初始精對(duì)準(zhǔn)。實(shí)驗(yàn)結(jié)果表明，航向角誤差可小于0.1°，水平角誤差可小于0.01°，證明了基于當(dāng)?shù)厮阶鴺?biāo)系與地固坐標(biāo)系下的卡爾曼濾波初始精對(duì)準(zhǔn)方法的有效性。

地固坐標(biāo)系；捷聯(lián)慣導(dǎo)系統(tǒng)；卡爾曼濾波；精對(duì)準(zhǔn)；平臺(tái)“失準(zhǔn)角”

0 引言

捷聯(lián)慣性導(dǎo)航系統(tǒng)（strapdown inertial navigation system, SINS）是通過(guò)固定在載體上的陀螺和加速度計(jì)為載體提供姿態(tài)、航向、位置、速度及加速度、角速率等信息的慣性導(dǎo)航系統(tǒng)。全球衛(wèi)星導(dǎo)航系統(tǒng)（global navigation satellite system, GNSS）與捷聯(lián)慣性導(dǎo)航系統(tǒng)結(jié)合的組合導(dǎo)航中，SINS力學(xué)編排必須提供初始位置、速度與姿態(tài)矩陣，保證SINS起始工作時(shí)準(zhǔn)確地輸出導(dǎo)航參數(shù)[1]。獲取載體的起始位置與速度比較簡(jiǎn)單，而初始對(duì)準(zhǔn)相對(duì)復(fù)雜?？柭鼮V波是SINS初始對(duì)準(zhǔn)的一種常用手段，很多學(xué)者對(duì)此問(wèn)題進(jìn)行了研究，但大多數(shù)研究都是在當(dāng)?shù)厮阶鴺?biāo)系下進(jìn)行的。在地固坐標(biāo)系下進(jìn)行GNSS與SINS的組合導(dǎo)航卻在當(dāng)?shù)厮阶鴺?biāo)系下進(jìn)行初始精對(duì)準(zhǔn)將大大增加科研工作者在理論推導(dǎo)與編程上的工作量。因此，用地固坐標(biāo)系下代替當(dāng)?shù)厮阶鴺?biāo)系進(jìn)行卡爾曼濾波初始對(duì)準(zhǔn)是更好的方法。盡管已有文獻(xiàn)對(duì)地固系下SINS初始精對(duì)準(zhǔn)進(jìn)行了研究[2]，但僅限于理論分析與數(shù)據(jù)仿真實(shí)驗(yàn)，對(duì)此方法的實(shí)際應(yīng)用仍然鮮有討論。

本文對(duì)地固系下卡爾曼濾波精對(duì)準(zhǔn)進(jìn)行了以下工作：在分析平臺(tái)“失準(zhǔn)角”的幾何意義的基礎(chǔ)上，指出在地固坐標(biāo)系下初始平臺(tái)“失準(zhǔn)角”方差的配置與在當(dāng)?shù)厮阶鴺?biāo)下不同；用實(shí)測(cè)數(shù)據(jù)分別在當(dāng)?shù)厮阶鴺?biāo)系與地固坐標(biāo)系下進(jìn)行初始對(duì)準(zhǔn)，并將其結(jié)果與IE軟件對(duì)比，驗(yàn)證地固坐標(biāo)系下SINS初始對(duì)準(zhǔn)的有效性。

1 函數(shù)模型與系統(tǒng)設(shè)計(jì)

捷聯(lián)慣導(dǎo)系統(tǒng)初始對(duì)準(zhǔn)分為粗對(duì)準(zhǔn)與精對(duì)準(zhǔn)2個(gè)階段：在粗對(duì)準(zhǔn)階段，一般采用解析法[3-4]；在精對(duì)準(zhǔn)階段用卡爾曼濾波法。SINS精對(duì)準(zhǔn)的基本原理是將載體處于靜態(tài)時(shí)速度為零作為外部觀測(cè)信息，用卡爾曼濾波估計(jì)出慣導(dǎo)系統(tǒng)的速度誤差、平臺(tái)“失準(zhǔn)角”與慣性導(dǎo)航元件誤差，并利用估計(jì)出的導(dǎo)航系統(tǒng)誤差校正慣導(dǎo)系統(tǒng)的姿態(tài)、得到精對(duì)準(zhǔn)所需的航向角、傾斜角與俯仰角。若采用反饋校正模式，需將估計(jì)誤差反饋到慣導(dǎo)系統(tǒng)的力學(xué)編排中，具體流程見(jiàn)圖1。圖中：INS(inertial navigation system)為慣性導(dǎo)航系統(tǒng)；卡爾曼濾波的誤差狀態(tài)方程是對(duì)SINS導(dǎo)航方程進(jìn)行擾動(dòng)得到，觀測(cè)方程由零速更新提供。

圖1 卡爾曼濾波SINS初始精對(duì)準(zhǔn)

1.1 捷聯(lián)慣導(dǎo)導(dǎo)航方程

在地固坐標(biāo)系下，慣航系統(tǒng)的導(dǎo)航方程[6]為

1.2 誤差狀態(tài)方程

對(duì)SINS導(dǎo)航方程擾動(dòng)得到速度及姿態(tài)角誤差狀態(tài)方程，并用一階馬爾科夫過(guò)程描述SINS慣性導(dǎo)航元件的誤差模型，可得SINS誤差狀態(tài)方程[7]為

在時(shí)刻第歷元慣導(dǎo)系統(tǒng)精對(duì)準(zhǔn)的卡爾曼濾波誤差狀態(tài)方程即式（2）可簡(jiǎn)寫(xiě)為

將上式（3)離散化，可得

該誤差矩陣在文獻(xiàn)[8-9]中提出的基于姿態(tài)矩陣分解和速度方程重構(gòu)的姿態(tài)估計(jì)方法基礎(chǔ)上，從卡爾曼濾波入手，設(shè)計(jì)了多狀態(tài)的矩陣方程。

1.3 零速更新

卡爾曼濾波的量測(cè)方程由速度“真值”（速度為零）與SINS力學(xué)編排的速度之差得到

2 卡爾曼濾波及慣導(dǎo)初始精對(duì)準(zhǔn)的濾波參數(shù)設(shè)置

卡爾曼濾波過(guò)程可分為一步預(yù)測(cè)與一步修正2個(gè)過(guò)程，每個(gè)過(guò)程又可分為濾波狀態(tài)的更新與描述濾波狀態(tài)的統(tǒng)計(jì)性質(zhì)的方差協(xié)方差陣的更新[10]。

卡爾曼濾波的一步預(yù)測(cè)方程為

卡爾曼濾波的一步更新方程為

可得出以下結(jié)論：

3 平臺(tái)“失準(zhǔn)角”幾何含義及其初始方差陣的配置

卡爾曼濾波估計(jì)出慣導(dǎo)系統(tǒng)的狀態(tài)誤差后，利用平臺(tái)“失準(zhǔn)角”這一參數(shù)校正慣導(dǎo)系統(tǒng)的姿態(tài)誤差，即可輸出卡爾曼濾波精對(duì)準(zhǔn)結(jié)果。

3.1 平臺(tái)“失準(zhǔn)角”的幾何意義

將式（12）代入式（11），可得

3.2 初始對(duì)準(zhǔn)精度分析

捷聯(lián)慣導(dǎo)系統(tǒng)的航向角在0~360°內(nèi)變化，而俯仰角與傾斜角相對(duì)較小，因而捷聯(lián)慣導(dǎo)系統(tǒng)水平角“失準(zhǔn)角”誤差由東、北向加速度的精度共同決定，方位角的對(duì)準(zhǔn)由東、北向陀螺儀的精度共同決定。一般而言，加速度計(jì)的精度相對(duì)于重力加速度（參考式（17）及式（18））而言高出許多個(gè)數(shù)量級(jí)，水平失準(zhǔn)角的精度較高；而陀螺儀的精度相對(duì)于地球自轉(zhuǎn)角速度（參考式（19））仍難以滿(mǎn)足要求，因此方位失準(zhǔn)角精度相對(duì)較低。

3.3 平臺(tái)“失準(zhǔn)角”的初始方差陣的配置

相應(yīng)地，用平臺(tái)“失準(zhǔn)角”修正后的姿態(tài)矩陣分別為：

4 實(shí)驗(yàn)與結(jié)果分析

為驗(yàn)證本文所提出方法有效性，采集了2組IMU數(shù)據(jù)，在3種方案按實(shí)現(xiàn)初始對(duì)準(zhǔn)：①當(dāng)?shù)厮阶鴺?biāo)系；②地固坐標(biāo)系，考慮平臺(tái)“失準(zhǔn)角”的權(quán)陣（陣）按本文所述方法配置；③地固坐標(biāo)系，平臺(tái)“失準(zhǔn)角”的權(quán)陣（陣）為等權(quán)模型。

其中，卡爾曼濾波的參數(shù)是根據(jù)廠商提供的數(shù)據(jù)（詳見(jiàn)表1）與經(jīng)驗(yàn)設(shè)定的（詳見(jiàn)表2、表3），初始位置為全球定位系統(tǒng)（global positioning system, GPS）定位的結(jié)果，校正方式為開(kāi)環(huán)校正。

表1 IMU數(shù)據(jù)來(lái)源與性能

表2 初始狀態(tài)方差陣

表3 IMU誤差模型與參數(shù)

表4 方案①、方案②及IE姿態(tài)結(jié)果

若不考慮地固坐標(biāo)與當(dāng)?shù)厮阶鴺?biāo)系下平臺(tái)“失準(zhǔn)角”之間的轉(zhuǎn)換關(guān)系（方案③）（如圖2、圖3所示），航向角將不收斂或收斂過(guò)慢，導(dǎo)致卡爾曼濾波失效。表4給分別給出方案①、方案②以及NovATel公司的IE（inertial explorer）軟件的濾波結(jié)果，3種方案在航向角相差小于0.1°，水平角相差小于0.01°，表明基于當(dāng)?shù)厮阶鴺?biāo)系與地固坐標(biāo)系下的卡爾曼濾波初始精對(duì)準(zhǔn)是都是有效的，且?guī)缀醯刃А?/p>

圖2 車(chē)載數(shù)據(jù)濾波結(jié)果

圖3 機(jī)載數(shù)據(jù)濾波結(jié)果

5 結(jié)束語(yǔ)

通過(guò)本文實(shí)驗(yàn)和分析，得出結(jié)論如下：地固坐標(biāo)系下平臺(tái)“失準(zhǔn)角”的方差陣的配置問(wèn)題不但是地固系下進(jìn)行初始精對(duì)準(zhǔn)所必須考慮的問(wèn)題，對(duì)于地固系下GNSS/SINS組合導(dǎo)航的初始方差陣的設(shè)置問(wèn)題同樣適用。用地固坐標(biāo)系代替當(dāng)?shù)厮阶鴺?biāo)系進(jìn)行卡爾曼濾波初始精對(duì)準(zhǔn)將大大降低在地固下GNSS/ SINS組合導(dǎo)航的科研工作者在理論推導(dǎo)與編程上的工作量。

[1] 秦永元, 張洪鉞, 汪叔華. 卡爾曼濾波與組合導(dǎo)航原理[M]. 西安: 西北工業(yè)大學(xué)出版社, 1998: 4-12.

[2] 柴華, 王勇, 許大欣, 等. 地固系下四元數(shù)和卡爾曼濾波方法的慣導(dǎo)初始精對(duì)準(zhǔn)研究[J]. 武漢大學(xué)學(xué)報(bào)（信息科學(xué)版）, 2012, 37（10）: 665-669.

[3] 萬(wàn)德鈞, 房建成. 慣性導(dǎo)航初始對(duì)準(zhǔn)[M]. 南京: 東南大學(xué)出版社. 1998: 40-41.

[4] JIANG Y F. Error analysis of analytic coarse alignment methods[J]. IEEE Transactions on Aerospace and Electronic Systems, 1998, 34(1): 334-337.

[5] JIANG Y F, LIN Y P. Error estimation of INS ground alignment through observability analysis[J]. IEEE Transactions on Aerospace and Electronic Systems, 1992, 28(1): 92-97.

[6] 董緒榮, 張守信, 華仲春. GPS/INS組合導(dǎo)航定位及其應(yīng)用[M]. 長(zhǎng)沙: 國(guó)防科技大學(xué)出版社, 1998: 102-104.

[7] ANGRISANO A, PETOVELLO M, PUGLIANO G. Benefits of combined GPS/GLONASS with low-cost MEMS IMUs for vehicular urban navigation[J]. Sensors, 2012, 12(12): 5134-5158.

[8] 劉海洋, 徐軍輝, 甄占昌, 等. 基于姿態(tài)估計(jì)的里程計(jì)輔助捷聯(lián)慣導(dǎo)系統(tǒng)動(dòng)基座初始對(duì)準(zhǔn)[J]. 航天控制, 2018(5).

[9] 趙政, 劉冰. 一種提高捷聯(lián)慣導(dǎo)系統(tǒng)動(dòng)基座初始對(duì)準(zhǔn)精度的方法[J]. 導(dǎo)航與控制, 2018, 17(5): 66-71.

[10] 王磊, 汪洲, 任元, 等. 捷聯(lián)慣導(dǎo)系統(tǒng)中卡爾曼濾波的應(yīng)用研究[J]. 計(jì)算機(jī)測(cè)量與控制, 2017, 25(6): 139-141.

[11] 亓洪標(biāo), 吳苗, 郭士犖. 捷聯(lián)慣導(dǎo)系統(tǒng)初始對(duì)準(zhǔn)精度評(píng)估算法[J]. 計(jì)算機(jī)仿真, 2017, 34(2): 16-19.

Initial fine alignment method of SINS under ECEF frame

FU Yan1, WANG Jianhui1, HAN Fei2

（1. Geology Surveying and Mapping Institute of Guangdong, Guangzhou 510800, China;2. College of Geology Engineering and Geomatics, Chang’ an University, Xi’an 710054, China）

In order to more accurately output the navigation parameters for strapdown inertial navigation system (SINS) in the initial work, the paper proposed an initial fine alignment method under ECEF frame: in the combination navigation of GNSS/SINS, the attitude misalignment angle from SINS carrier coordinate system to earth-centered, earth-fixed frame (ECEF) was estimated by Kalman filter based on the error equation of state corresponding to the mechanical arrangement scheme of ECEF, and the position and velocity of the carrier under ECEF were output directly, finally the initial fine alignment of SINS was realized. Experimental result showed that the deviation of heading angle (yaw) and horizontal angle (roll and pitch) error would be under 0.1 degree and 0.01 degree respectively, which verified the feasibility of the proposed method.

earth-centered, earth-fixed (ECEF); strapdown inertial navigation system (SINS); Kalman filter; fine alignment; attitude misalignment

P228

A

2095-4999(2019)04-0050-06

符彥，王劍輝，韓菲.地固坐標(biāo)系下捷聯(lián)慣導(dǎo)系統(tǒng)初始精對(duì)準(zhǔn)[J].導(dǎo)航定位學(xué)報(bào),2019,7(4): 50-55.（FU Yan，WANG Jianhui，HAN Fei.Initial fine alignment method of SINS under ECEF frame[J].Journal of Navigation and Positioning,2019,7(4): 50-55.）

10.16547/j.cnki.10-1096.20190409.

2019-01-10

符彥（1973—），男，海南東方人，本科，高級(jí)工程師，研究方向?yàn)樾l(wèi)星導(dǎo)航與定位及攝影測(cè)量。

韓菲（1981—），男，河南滎陽(yáng)人，博士，高級(jí)工程師，研究方向?yàn)閯?dòng)態(tài)坐標(biāo)框架與衛(wèi)星導(dǎo)航定位。