基于双欧拉角的UKF组合导航滤波算法

doi:10.12305/j.issn.1001-506X.2021.07.23

摘要/Abstract

摘要：

针对非线性滤波组合导航中四元数无迹估计器(unscented quaternion estimator, USQUE)规范性约束导致的算法计算量大、实时性差等问题, 提出一种基于双欧拉角姿态表示的无迹卡尔曼滤波(dual-Euler unscented Kalman filter, DEUKF)算法。通过正、反欧拉角相互切换, 在正、反欧拉微分方程精华区进行姿态滤波更新, 避免了奇异性, 确保了滤波精度。捷联惯性导航系统/全球定位系统直接式组合导航仿真试验与车载实验结果表明, 相比较于USQUE算法, DEUKF算法计算量小、实时性好, 水平姿态角估计精度与USQUE相当。

关键词: 组合导航, 姿态估计, 非线性滤波, 双欧法, 四元数

Abstract:

Aiming at the problem of huge computation load and poor real-time performance caused by the normative constraints of the unscented quaternion estimator (USQUE) in the nonlinear filter for an integrated navigation system, a dual-Euler unscented Kalman filter (DEUKF) based on the dual-Euler attitude representation is proposed. The proposed filter updates attitude in the positive and reverse essence region of the positive and reverse Euler differential equations by switching Euler angle to avoid singularity and ensure filtering accuracy. In the direct integrated navigation of a strapdown inertial navigation system/global positioning system, both the simulation and vehicle test results show that, compared with USQUE algorithm, the computation load of the proposed DEUKF is reduced significantly so that it has good real-time performance while it achieves the equivalent pitch and roll estimation accuracy with USQUE algorithm.

Key words: integrated navigation, attitude estimation, nonlinear filtering, dual-Euler method, quaternion

中图分类号:

U666.11

赵仁杰, 胡柏青, 吕旭, 田佳玉. 基于双欧拉角的UKF组合导航滤波算法[J]. 系统工程与电子技术, 2021, 43(7): 1912-1920.

Renjie ZHAO, Baiqing HU, Xu LYU, Jiayu TIAN. Filtering algorithm of UKF integrated navigation based on dual-Euler angles[J]. Systems Engineering and Electronics, 2021, 43(7): 1912-1920.

图/表 18

图1

图2

图3

图4

图5

图6

图7

图8

表1

图9

图10

表2

图11

图12

图13

图14

图15

表3

参考文献 27

1	严恭敏. 捷联惯导算法与组合导航原理[M]. 西安: 西北工业大学出版社, 2019.
	YAN G M . Strapdown inertial navigation system algorithm and integrated navigation principle[M]. Xi'an: Northwestern Polytechnical University Press, 2019.
2	ALI J , MIRZA M R . Performance comparison among some nonlinear filters for a low cost SINS/GPS integrated solution[J]. Nonlinear Dynamics, 2010, 61 (3): 491- 502. doi: 10.1007/s11071-010-9665-y
3	ZHANG T , XU X S . A new method of seamless land navigation for GPS/INS integrated system[J]. Measurement, 2012, 45 (4): 691- 701. doi: 10.1016/j.measurement.2011.12.021
4	MA H B , YAN L P , XIA Y Q , et al. Kalman filtering and information fusion[M]. Beijing: Science Press, 2020.
5	杨波, 秦永元, 柴艳. UKF在INS/GPS直接法卡尔曼滤波中的应用[J]. 传感技术学报, 2007, (4): 842- 846. doi: 10.3969/j.issn.1004-1699.2007.04.030
	YANG B , QIN Y Y , CHAI Y . Application of UKF in direct method of Kalman filter for INS/GPS[J]. Chinese Journal of Sensors and Actuators, 2007, (4): 842- 846. doi: 10.3969/j.issn.1004-1699.2007.04.030
6	LI K L , HU B Q , GAO J D , et al. Direct navigation mode and indirect navigation mode for integrated SINS/GPS[J]. Applied Mechanics and Materials, 2014, 568 (2): 958- 963.
7	ZHOU J , KNEDLIK S , LOFFELD O . INS/GPS tightly-coupled integration using adaptive unscented particle filter[J]. Journal of Navigation, 2010, 63 (3): 491- 511. doi: 10.1017/S0373463310000068
8	CHANG L B , HU B Q , LI A , et al. Strapdown inertial navigation system alignment based on marginalised unscented Kalman filter[J]. Iet Science Measurement & Technology, 2013, 7 (2): 128- 138.
9	ZHOU W D, QIAO X W, MENG F B, et al. Study on SINS/GPS tightly integrated navigation based on adaptive extended Kalman filter[C]//Proc. of the International Conference on Information and Automation, 2010: 2344-2347.
10	CASEY R T, KARPENKO M, CURRY R, et al. Attitude representation and kinematic propagation for low-cost UAVs[C]//Proc. of the Aiaa Guidance, Navigation, & Control, 2013.
11	JUNKINS J L . How to avoid singularity when using Euler angles?[J]. Advances in the Astronautical Sciences, 2005, 119, 1409- 1426.
12	乔相伟, 周卫东, 吉宇人. 用四元数状态切换无迹卡尔曼滤波器估计的飞行器姿态[J]. 控制理论与应用, 2012, 29 (1): 97- 103.
	QIAO X W , ZHOU W D , JI Y R . Aircraft attitude estimation based on quaternion state-switching unscented Kalman filter[J]. Control Theory & Applications, 2012, 29 (1): 97- 103.
13	CRASSIDIS J L , MARKLEY F L . Unscented filtering for spacecraft attitude estimation[J]. Journal of Guidance Control and Dynamics, 2003, 26 (4): 536- 542. doi: 10.2514/2.5102
14	CRASSIDIS J L . Sigma-point Kalman filtering for integrated GPS and inertial navigation[J]. IEEE Trans.on Aerospace and Electronic Systems, 2006, 42 (2): 750- 756. doi: 10.1109/TAES.2006.1642588
15	李开龙, 胡柏青, 常路宾. 改进四元数无味卡尔曼滤波算法[J]. 系统工程与电子技术, 2016, 38 (6): 1399- 1404.
	LI K L , HU B Q , CHANG L B . Modified quaternion unscented Kalman filter[J]. Systems Engineering and Electronics, 2016, 38 (6): 1399- 1404.
16	KARLGAARD C D , SCHAUB H . Nonsingular attitude filtering using modified rodrigues parameters[J]. Journal of The Astronautical Sciences, 2009, 57 (4): 777- 791. doi: 10.1007/BF03321529
17	LI K L , CHANG L B , HU B Q . Unscented attitude estimator based on dual attitude representations[J]. IEEE Trans.on Instrumentation and Measurement, 2015, 64 (12): 3564- 3576. doi: 10.1109/TIM.2015.2454631
18	陈永冰, 李文魁, 周岗, 等. 基于双姿态的高阶罗德里格斯参数姿态估计[J]. 华中科技大学学报(自然科学版), 2017, 45 (1): 82- 86.
	CHEN Y B , LI W K , ZHOU G , et al. Dual attitude representations based attitude estimation using high order Rodrigues parameter[J]. Huazhong University of Science & Technology. (Natural Science Edition), 2017, 45 (1): 82- 86.
19	黄雪樵. 克服欧拉方程奇异性的双欧法[J]. 飞行力学, 1994, (4): 28- 37.
	HUANG X Q . The dual-Euler method for overcoming the singularity of Euler equation[J]. Flight Dynamics, 1994, (4): 28- 37.
20	陈廷楠, 张登成. 双欧法与四元数法的应用比较[J]. 飞行力学, 1996, (4): 59- 64.
	CHEN T N , ZHAN D C . Application comparision of dual Euler method and quaternion method[J]. Flight Dynamics, 1996, (4): 59- 64.
21	SHIN S , LEE J , YOON S . A comparison study of real-time solution to all-attitude angles of an aircraft[J]. Journal of Mechanical Science and Technology, 2006, 20 (3): 376- 381. doi: 10.1007/BF02917520
22	李跃军, 阎超. 飞行器姿态角解算的全角度双欧法[J]. 北京航空航天大学学报, 2007, 33 (5): 505- 508. doi: 10.3969/j.issn.1001-5965.2007.05.001
	LI Y J , YAN C . Improvement of dual-Euler method for full scale Eulerian angles solution of aircraft[J]. Journal of Beijing University of Aeronautics and Astronautics, 2007, 33 (5): 505- 508. doi: 10.3969/j.issn.1001-5965.2007.05.001
23	RAN C Y , CHENG X H . A direct and non-singular UKF approach using Euler angle kinematics for integrated navigation systems[J]. Sensors, 2016, 16 (9): 1415. doi: 10.3390/s16091415
24	吕维维, 程向红, 邱伟. 基于弹载捷联惯性导航系统精确导航的双欧拉全姿态方法[J]. 上海航天, 2019, 36 (1): 34- 42.
	LYU W W , CHENG X H , QIU W . Dual-Euler whole attitude solving method based on accurate navigation of missile-borne strapdown inertial navigation system[J]. Aerospace Shanghai, 2019, 36 (1): 34- 42.
25	YOON S. A study on optimal switching angles in dual-Euler method[C]//Proc. of the Aiaa Modeling & Simulation Technologies Conference & Exhibit, 2002.
26	吕旭, 胡柏青, 戴永彬, 等. 基于PSO的USQUE在组合导航姿态估计中的应用[J]. 系统工程与电子技术, 2020, 42 (6): 1366- 1371.
	LYU X , HU B Q , DAI Y B , et al. Application of USQUE based on PSO in attitude estimation of integrated navigation[J]. Systems Engineering and Electronics, 2020, 42 (6): 1366- 1371.
27	CHANG L B , HU B Q , CHANG G B . Modified unscented quaternion estimator based on quaternion averaging[J]. Journal of Guidance, Control, and Dynamics, 2013, 37 (1): 305- 309.

算法	均方根误差/(′)			时间/s
算法	俯仰角	横滚角	航向角	时间/s
USQUE	0.629	0.672	1.72	394
DEUKF	0.651	0.728	5.83	160

性能指标	陀螺仪	加速度计
动态范围	±150°/s	±10 g
零偏稳定性	≤0.02°/s	≤0.001 g
零偏重复性	≤0.02°/s	≤0.001 g
更新频率	100 Hz	100 Hz

算法	均方根误差/(°)			时间/s
算法	俯仰角	横滚角	航向角	时间/s
USQUE	0.241	0.245	3.334	97
DEUKF	0.221	0.250	3.426	56

[1]	姜颖颖, 潘树国, 叶飞, 高旺, 马春, 王浩. 基于抗差估计和改进AIME的缓变故障检测方法[J]. 系统工程与电子技术, 2022, 44(9): 2894-2902.
[2]	董一平, 刘宁, 苏中, 王靖骁, 白宏阳. 基于AEKF的高速自旋飞行体组合导航方法[J]. 系统工程与电子技术, 2022, 44(6): 1977-1983.
[3]	李文华, 汪立新, 沈强, 李灿, 吴宗收. 基于鲁棒EKF的MEMS-INS/GNSS/VO组合导航方法[J]. 系统工程与电子技术, 2022, 44(6): 1994-2000.
[4]	张平安, 汪伟, 高敏, 王毅. SR-CH∞KF用于弹丸飞行姿态测量研究[J]. 系统工程与电子技术, 2022, 44(1): 262-269.
[5]	孙淑光, 温启新. 基于高程异常补偿的飞机终端区组合导航高度优化算法[J]. 系统工程与电子技术, 2021, 43(9): 2612-2619.
[6]	文者, 卞鸿巍, 马恒, 臧涛. 中低纬度下惯导极区性能模拟测试方法[J]. 系统工程与电子技术, 2021, 43(9): 2620-2627.
[7]	蔡保杰, 邵雷. 三段判别域与最小二乘拟合的抗差滤波算法[J]. 系统工程与电子技术, 2021, 43(5): 1346-1353.
[8]	苏敬, 何华锋, 何耀民, 王依繁, 韩晓斐. 考虑SAR量测特性的弹载SINS/SAR组合导航滤波算法[J]. 系统工程与电子技术, 2021, 43(4): 1044-1049.
[9]	赵仁杰, 李开龙, 胡柏青, 田佳玉. 基于改进四元数阻尼误差模型的SINS初始对准算法[J]. 系统工程与电子技术, 2021, 43(11): 3330-3337.
[10]	吕旭, 胡柏青, 戴永彬, 赵仁杰. 基于PSO的USQUE在组合导航姿态估计中的应用[J]. 系统工程与电子技术, 2020, 42(6): 1366-1371.
[11]	罗凯鑫, 吴美平, 范颖. 基于最大熵方法的鲁棒自适应滤波及其应用[J]. 系统工程与电子技术, 2020, 42(3): 667-673.
[12]	何康辉, 董朝阳. 基于正交变换的五阶容积卡尔曼滤波导航算法[J]. 系统工程与电子技术, 2020, 42(3): 680-685.
[13]	张闯, 郭晨, 张大恒. 船舶组合导航自适应迭代粒子滤波方法及应用[J]. 系统工程与电子技术, 2019, 41(4): 883-888.
[14]	葛宝爽, 张海, 晋燕琼. 基于冗余量测的GNSS多路径误差抑制算法[J]. 系统工程与电子技术, 2019, 41(11): 2581-2587.
[15]	程建华, 范世龙, 李亮, 董萍. 基于量测修正的组合导航系统时间同步方法[J]. 系统工程与电子技术, 2019, 41(10): 2328-2333.