基于卡尔曼滤波器的数字式捷联航姿系统算法设计

Fig.5 Map of vehicle-bore data
字式捷联航姿系统的关键，经半实物仿真证明，该算法合理、有效，为研发新型航姿系统替代老式模拟航姿器奠定了基 础。由于民用飞机的飞行非常平稳，几乎没有大的机动，载机进行加速或者大角速度转弯的时间很短，因此以上的航姿 算法应用在民用飞机等低动态载体上非常适合；同时，民用飞机上还有诸如大气数据等分系统，未来研究中将会进一步 将大气数据纳入航姿滤波器中进行综合，进一步提高航姿系统的动态适应性和测量精度。 参考文献(References)： [1] 曾庆化，刘建业，赖际舟，杜亚玲. DSP硬件算法在捷联惯性AHRS系统中的实现[J]. 中国惯性技术学报，2006，14(6)：1-4. ZENG Qing-hua, LIU Jian-ye, LAI Ji-zhou, DU Ya-ling. Realization of DSP hardware algorithms for strapdown inertial AHRS[J]. Journal of Chinese Inertial Technology, 2006, 14(6): 1-4. [2] Gebre-Egziabher D, Hayward R C, Powell J D. A low-cost GPS/inertial attitude heading reference system (AHRS) for general aviation applications[C]//IEEE1998 Position, Location and Navigation Symposium, April 1998: 518-525. [3] 王宇，王庆，吴峻，万德钧. 光纤捷联航姿基准系统的设计与实现[J]., 中国惯性技术学报，2007，15(2)：128-131. WANG Yu, WANG Qing, WU Jun, WAN De-jun. Design and realization on strapdown attitude and heading reference system based on FOG[J]. Journal of Chinese Inertial Technology, 2007, 15(2): 128-131. [4] 陈小凤，关政军. 船用光纤陀螺捷联航姿基准系统[J]. 大连海事大学学报，2005，31（1）：26-28. CHEN Xiao-feng, GUAN Zheng-jun. Fiber optic gyro strapdown attitude and heading reference system for ship[J]. Journal of Dalian Maritime University, 2005, 31(1): 26-28. [5] Rios J A, white E. Low cost solid tate GPS/INS package[EB/OL]. [2006-11-08]. . （上接第 207 页） [3] 黄谟涛，翟国君，管诤，等. 海洋重力场测定及其应用[M]. 北京：测绘出版社，2004：69-70. HUANG Mo-tao, ZHAI Guo-jun, GUAN Zheng, et al. The measurement and application for sea gravity measurements[M]. Beijing: Publishing Company of Mapping, 2004: 69-70. [4] 吴太棋，黄谟涛，陆秀平，金迹航. 重力场匹配导航的重力图生成技术[J]. 中国惯性技术学报，2007，15(4): 438-441. WU Tai-qi, HUANG Mo-Tao, LU Xiu-ping, JIN Hang-ji. Gravity map creating technology in gravity matching navigation[J]. Journal of Chinese inertial technology, 2007, 15(4): 438-441. [5] Jacques Hinderer, David Crossley, Olivia Jensen. A search for the slichter triplet in superconducting gravimeter data[J]. Physics of The Earth and Planetary Interiors, 1995, 90(3-4): 183-195. [6] Cheinway Hwang, Yu-Shen Hsiao, Hsuan-Chang Shih. Data reduction in scalar airborne gravimetry: Theory, software and case study in Taiwan[J]. Computers & Geosciences, 2006, 32(10): 1573-1584. [7] Pálinkáš V. Precise tidal measurements by spring gravimeters at the station Pecný[J]. Journal of Geodynamics, 2006, 41(1-3): 14-22. [8] Guo J Y, Dierks O, Neumeyer J, Shum C K. Weighting algorithms to stack superconducting gravimeter data for the potential detection of the slichter modes[J]. Journal of Geodynamics, 2006, 41(1-3): 326-333. [9] Merriam J B. The response method applied to the analysis of superconducting gravimeter data[J]. Physics of The Earth and Planetary Interiors, 2000, 121(3-4): 289-299.
第 16 卷第 2 期 2008 年 4 月 文章编号：1005-6734(2008)02-0208-04
中国惯性技术学报 Journal of Chinese Inertial Technology
Vol.16 No.2 Apr. 2008
基于卡尔曼滤波器的数字式捷联航姿系统算法设计
汪 芳，朱少华，雷宏杰
1×10－4 g，磁航向传感器精度约为1 (°)/h。
根据跑车试验中系统实时输出的补偿过的角速度和加速度进行了仿真计算。 由图 3 可以看出， 根据前面提出的算法， 在停车状态下（汽车发动机不熄火），在 30 s 时估计的陀螺漂移精度可以达到 1 (°)/h；100 s 时估计的陀螺漂移精度优于 0.5 (°)/h。在随后进行的约 30 min 中的车载试验中，仿真计算的姿态误差结果见表 1 和图 4，跑车路线如图 5 所示。
0 ⎤ ⎡ 0 ⎡q ⎢q ⎥ ⎢ω ⎢ 1⎥ = ⎢ x ⎢q 2 ⎥ ⎢ω y ⎢ ⎥ ⎢ 3 ⎦ ⎣ ⎢ω z ⎣q
建立捷联航姿系统的连续型非线性方程如式（2） ：
−ω x 0 −ω z
−ω y
ωz
0 −ωx
ωy
−ω z ⎤ ⎡ q0 ⎤ ⎢ ⎥ −ω y ⎥ ⎥ ⎢ q1 ⎥ −ω x ⎥ ⎢ q2 ⎥ ⎥⎢ ⎥ 0 ⎦ ⎥ ⎣ q3 ⎦
33.95 33.85
φ/(deg)
211
表1 仿真计算的姿态误差(1σ) Tab.1 Errors of emulational attitude/head(1σ) 误差 跑车全 过程中 组合滤波 过程中 俯仰角 误差(°) 0.76 0.34 横滚角 误差(°) 0.64 0.18 航向角 误差(°) 1.09 0.57
X K , K −1 = ΦK ⋅ X K −1 ，
其中， ΦK 为F 阵的离散化展开式。
PK , K −1 = ΦK ⋅ PK −1 ⋅ΦKT + Q0
（4）
210 滤波增益计算如式(5)所示：
中国惯性技术学报
第 16 卷
−1 T T KK = ⎡ ⎣ PK −1 ⋅ H ⋅ ( H ⋅ PK −1 ⋅ H + R) ⎤ ⎦
计、磁传感器和信息处理电路组成，如图 1 所示。 收稿日期：2007-12-09；修回日期：2008-01-24 基金项目：国防科技重点预研项目（51309060403）
作者简介：汪芳（1973—），女，高级工程师，从事惯性导航系统技术研究。 E-mail：shzhu_2002@
第2期
（1）
= F ( X ,ω) + W ， X 1
其中，状态变量 X = ⎡ ⎣ q0 q1 q2
Z = H ( X ) + V1
f yb
（2） f zb ψ mag ⎤ ⎦，
q3 ε x 0 ε y 0 ε z 0 ⎤ ⎦ ，表示四元数向量和陀螺漂移；输出变量 Z = ⎡ ⎣ f xb
表示机体系的加速度和磁航向；W1表示状态噪声；V1表示量测噪声。 由式（1）可以得到式（2）中的离散型状态方程如（3）所示：
0
ωy
X K -1 (4) ⎤ − X K -1 (3) ⎥ ⎥ X K -1 (2) ⎥ ⎥ − X K -1 (2) − X K -1 (1) ⎥ ⎥ ⎦ X K -1 (3) X K -1 (4) − X K -1 (1)
（3）
考虑到系统在民机上的实际使用过程中，飞行过程非常平稳，航向的变化缓慢，因而在算法的设计上可以将式(2)中 的观测量相应的分为两部分：四元数和航向。由此可以设计两个量测方程H1和H2，以不同的计算周期分别进行卡尔曼滤 波器计算；同时也可以降低信息处理计算机的运算量。按以下方程进行离散型卡尔曼滤波方程迭代计算。 一步预测计算如式(4)所示：