基于加速度补偿的惯性行人导航非零速区间姿态估计CKF算法

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

摘要/Abstract

摘要：

姿态估计是导航解算的基础, 在基于足绑式惯性测量单元的行人导航系统中, 由于足部运动加速度变化频繁且剧烈, 使得常见的姿态融合算法精度下降。为了减小运动加速度对姿态解算的影响, 通过数据分析定义了可以进行加速度补偿的拟合区间, 在零速检测的基础上给出了拟合区间的判定方法, 提出了对加速度计的输出进行一阶拟合补偿的方案, 并设计了能完成后续行人导航姿态估计任务的容积卡尔曼滤波(cubature Kalman filter, CKF)算法, 在非拟合区间则采用三子样旋转矢量法进行姿态更新。在数值仿真中，将所提算法与纯三子样旋转矢量法进行了对比分析, 对算法精度进行了测试, 在行人导航试验中验证了算法的有效性。试验结果表明, 在行走过程中及出现较大运动加速度的情况下, 加入加速度补偿的CKF姿态估计精度平均提高了35.3%。在矩形闭合路径试验中, 起终点水平误差降低了56.3%, 起终点高度误差降低了20.3%。

关键词: 惯性导航, 姿态估计, 行人导航, 容积卡尔曼滤波, 加速度补偿

Abstract:

Attitude estimation is the foundation of the navigation computation. In pedestrian navigation system based on foot-bound inertial measurement unit, due to the frequent and violent changes of foot motion acceleration, the accuracy of common attitude fusion algorithms is reduced. In order to reduce the impact of motion acceleration on attitude calculation, the fitting interval that can be used for acceleration compensation is defined through data analysis, the determination method of fitting interval based on zero-speed detection is given, and a first-order fitting compensation scheme is proposed for accelerometer output.The cubature Kalman filter (CKF) algorithm which can complete the subsequent pedestrian navigation attitude estimation task is designed, and the three-sample rotation vector method is used to update the attitude in the non-fitting interval. In the numerical simulation, the proposed algorithm is compared with the pure three-sample rotation vector method, and the accuracy of the algorithm is tested. The effectiveness of the algorithm is verified in the pedestrian navigation experiment. The experiment test results show that the accuracy of CKF attitude estimation with acceleration compensation increases by 35.3% on average in the case of large motion acceleration during walking. In the rectangular closed path test, the horizontal error of starting and ending points decreased by 56.3%, and the height error of starting and ending points decreased by 20.3%.

Key words: inertial navigation, attitude estimation, pedestrian navigation, cubature Kalman filter (CKF), acceleration compensation

中图分类号:

U666.1

王希彬, 戴洪德, 全闻捷, 王瑞, 贾临生. 基于加速度补偿的惯性行人导航非零速区间姿态估计CKF算法[J]. 系统工程与电子技术, 2023, 45(9): 2894-2901.

Xibin WANG, Hongde DAI, Wenjie QUAN, Rui WANG, Linsheng JIA. Nonzero velocity interval attitude estimation CKF algorithm based on acceleration compensation for inertial pedestrian navigation[J]. Systems Engineering and Electronics, 2023, 45(9): 2894-2901.

图/表 16

图1

图2

图3

图4

图5

表1

图6

图7

图8

图9

表2

表3

图10

表4

图11

表5

参考文献 41

1	NAGY S B ARNE J T , FOSSEN T I. , et al. Attitude estimation by multiplicative exogenous Kalman filter[J]. Automatica, 2018, 95 (1): 347- 355.
2	SHUSTER M D , OH S D . Three-axis attitude determination from vector observations[J]. Guidance Control Dynam, 1981, 4 (1): 70- 77. doi: 10.2514/3.19717
3	CRASSIDIS J L , MARKLEY F L , CHENG Y . Survey of nonlinear attitude estimation methods[J]. Journal of Guidance Control & Dynamics, 2007, 30 (1): 12- 28.
4	WU J , ZHOU Z B , FOURATI H , et al. Generalized linear quaternion complementary filter for attitude estimation from multi-sensor observations: an optimization approach[J]. IEEE Trans.on Automation Science and Engineering, 2019, 16 (3): 1330- 1343. doi: 10.1109/TASE.2018.2888908
5	FISCHER C , SUKUMAR P T , MIKE H . Tutorial: implementing a pedestrian tracker using inertial sensors[J]. IEEE Pervasive Computing, 2013, 12 (2): 17- 27. doi: 10.1109/MPRV.2012.16
6	TITTERTON D H , WESTON J L . Strapdown inertial navigation technology[M]. 2nd ed. USA: American Institude of Aeronautics & Astronautics lncorporation, 2004.
7	CHOUKROUN D , BARITZHACK I Y , OSHMAN Y . Novel quaternion Kalman filter[J]. IEEE Trans.on Aerospace & Electronic Systems, 2006, 42 (1): 174- 190.
8	张煌军, 徐雪松, 罗伟, 等. 基于平方根容积卡尔曼滤波的四旋翼无人机的姿态解算[J]. 科学技术与工程, 2019, 19 (12): 248- 253. doi: 10.3969/j.issn.1671-1815.2019.12.036
	ZHANG H J , XU X S , LUO W , et al. Attitude calculation of four rotor UAV based on square root volume Kalman filter[J]. Science, Technology and Engineering, 2019, 19 (12): 248- 253. doi: 10.3969/j.issn.1671-1815.2019.12.036
9	PSIAKI M L . Backward-smoothing extended Kalman filter[J]. Journal of Guidance Control & Dynamics, 2005, 28 (5): 885- 894.
10	JULIER S J, UHLMANN J K. A new extension of the Kalman filter to nonlinear systems[C]//Proc. of the International Symposium on Aerospace/Defense Sensing, Simulation and Controls, 1997: 182-193.
11	ARASARATNAM I , HAYKIN S . Cubature Kalman filters[J]. IEEE Trans.on Automatic Control, 2009, 54 (6): 1254- 1269. doi: 10.1109/TAC.2009.2019800
12	ELWELL J. Inertial navigation for the urban warrior[C]//Proc. of the SPIE International Society for Optical Engineering, 1999, 3709: 196-204.
13	戴文战, 黄晓姣, 沈忱. 带遗忘因子的自适应迭代容积卡尔曼滤波算法[J]. 科技通报, 2019, 35 (1): 181- 185. doi: 10.13774/j.cnki.kjtb.2019.01.036
	DAI W Z , HUANG X J , SHEN C . Adaptive iterative cubature Kalman filter with forgetting factor[J]. Science and Technology Bulletin, 2019, 35 (1): 181- 185. doi: 10.13774/j.cnki.kjtb.2019.01.036
14	刘华, 缪晨, 吴文. 平方根嵌入式容积卡尔曼粒子滤波算法[J]. 南京理工大学学报, 2015, 39 (4): 471- 476. doi: 10.14177/j.cnki.32-1397n.2015.39.04.015
	LIU H , MIAO C , WU W . Square root embedded cubature Kalman particle filter algorithm[J]. Journal of Nanjing University of Technology, 2015, 39 (4): 471- 476. doi: 10.14177/j.cnki.32-1397n.2015.39.04.015
15	徐树生, 林孝工, 李新飞. 强跟踪自适应平方根容积卡尔曼滤波算法[J]. 电子学报, 2014, 42 (12): 2394- 2400. doi: 10.3969/j.issn.0372-2112.2014.12.009
	XU S S , LIN X G , LI X F . Strong tracking adaptive square root cubature Kalman filter algorithm[J]. Journal of Electronics, 2014, 42 (12): 2394- 2400. doi: 10.3969/j.issn.0372-2112.2014.12.009
16	徐树生, 林孝工, 赵大威, 等. 强跟踪SRCKF及其在船舶动力定位中的应用[J]. 仪器仪表学报, 2013, 34 (6): 1266- 1272. doi: 10.3969/j.issn.0254-3087.2013.06.010
	XU S S , LIN X G , ZHAO D W , et al. Strong tracking SRCKF and its application in ship dynamic positioning[J]. Journal of Instrumentation, 2013, 34 (6): 1266- 1272. doi: 10.3969/j.issn.0254-3087.2013.06.010
17	巫春玲, 李永萍, 谢美美, 等. 迭代自适应容积卡尔曼滤波算法[J]. 电子测量技术, 2019, 42 (17): 65- 70. doi: 10.19651/j.cnki.emt.1902809
	WU C L , LI Y P , XIE M M , et al. Iterative adaptive volume Kalman filter algorithm[J]. Electronic Measurement Technology, 2019, 42 (17): 65- 70. doi: 10.19651/j.cnki.emt.1902809
18	WEI X Q , SONG S M . Cubature Kalman filter based satellite attitude estimation[J]. Journal of Astronautics, 2013, 34 (2): 193- 200.
19	ZANETTI R, BISHOP R. Quaternion estimation and norm constrained kalman filtering[C]//Proc. of the AIAA/AAS Astrodynamics Specialist Conference and Exhibit, 2013.
20	WANG X L, YAN S J, CAO L Z. Research on pedestrian navigation system based on multi-sensor zero speed correction[C]// Proc. of the IEEE 4th Information Technology and Mechatronics Engineering Conference, 2018: 1786-1790.
21	田晓春, 陈家斌, 韩勇强, 等. 多条件约束的行人导航零速区间检测算法[J]. 中国惯性技术学报, 2016, 24 (1): 1- 5.
	TIAN X C , CHEN J B , HAN Y Q , et al. Zero speed interval detection algorithm for pedestrian navigation with multiple constraints[J]. Chinese Journal of Inertial Technology, 2016, 24 (1): 1- 5.
22	苑宝贞, 苏中, 李擎, 等. 基于贝叶斯网络的强鲁棒性零速检测方法[J]. 计算机测量与控制, 2016, 24 (3): 200- 207.
	YUAN B Z , SU Z , LI Q , et al. Strong robustness zero speed detection method based on Bayesian network[J]. Computer Measurement and Control, 2016, 24 (3): 200- 207.
23	陈国良, 杨洲. 基于加速度量测幅值零速检测的计步算法研究[J]. 武汉大学学报(信息科学版), 2017, 42 (6): 726- 731.
	CHEN G L , YANG Z . Research on step counting algorithm based on zero velocity detection of acceleration measurement amplitude[J]. Geomatics and Information Science of Wuhan University, 2017, 42 (6): 726- 731.
24	SKOG I, NILSSON J O, HNDEL P. Evaluation of zero-velocity detectors for foot-mounted inertial navigation system[C]//Proc. of the International Conference Indoor Positioning and Indoor Navigation, 2010.
25	SKOG I , HNDEL P , NILSSON J O , et al. Zero-velocity detection-an algorithm evaluation[J]. IEEE Trans.on Biomedical Engineering, 2010, 57 (11): 2657- 2666.
26	WANG Z L , ZHAO H Y , QIU S , et al. Stance-phase detection for ZUPT-aided foot-mounted pedestrian navigation system[J]. IEEE/ASME Trans.on Mechatronics, 2015, 20 (6): 3170- 3181.
27	LI X F, MAO Y L, XIE L, et al. Applications of zero-velocity detector and Kalman filter in zero velocity update for inertial navigation system[C]//Proc. of the IEEE Chinese Guidance, Navigation and Control Conference, 2014: 1760-1763.
28	徐昊, 郑婷婷, 宋天威, 等. 自适应零速修正辅助的微惯性定位系统研究[J]. 南京师范大学学报(工程技术版), 2017, 17 (4): 14- 19.
	XU H , ZHENG T T , SONG T W , et al. Research on micro inertial positioning system assisted by adaptive zero speed correction[J]. Journal of Nanjing Normal University (Engineering Technology Edition), 2017, 17 (4): 14- 19.
29	时伟, 王阳. 基于不等式约束卡尔曼滤波的双MIMU导航位置校正方法[J]. 中国惯性技术学报, 2017, 25 (1): 11- 16.
	SHI W , WANG Y . Dual MIMU navigation position correction method based on inequality constrained Kalman filter[J]. Chinese Journal of Inertial Technology, 2017, 25 (1): 11- 16.
30	NILSSON J O, SKOG I, HNDEL P, et al. Foot-mounted INS for everybody-an open source embedded implementation[C]//Proc. of the IEEE/ION Position Location and Navigation Symposium, 2012.
31	OA L, BORENSTEIN J. Non-GPS navigation with the personal dead-reckoning system[C]//Proc. of the SPIE Defense and Security Conference, 2007: 130-140.
32	KRACH B, ROBERSTON P. Cascaded estimation architecture for integration of foot-mounted inertial sensors[C]//Proc. of the IEEE/ION Position Location and Navigation Symposium, 2008: 112-119.
33	JIMENEZ A, SECO F, PRIETO J C, et al. Indoor pedestrian navigation using an INS/EKF framework for yaw drift reduction and a foot-mounted IMU[C]//Proc. of the 7th Workshop on Positioning Navigation and Communication, 2010: 135-143.
34	CASTANEDA N, LAMY-PERBAL S. An improved shoe-mounted inertial navigation system[C]//Proc. of the IEEE International Conference on Indoor Positioning and Indoor Navigation, 2010.
35	MAKNI A , KIBANGOU A Y , FOURATI H . Data fusion-based descriptor approach for attitude estimation underaccele-rated maneuvers[J]. Asian Journal of Control, 2019, 21 (4): 1433- 1442.
36	AILNENI S , KASHYAP S K , KUMAR N S . INS/GPS fusion architectures for unmanned aerial vehicles[J]. International Journal of Intelligent Unmanned Systems, 2014, 2 (3): 154- 167.
37	EUSTON M, COOTE P, MAHONY R, et al. A complementary filter for attitude estimation of a fixed-wing UAV[C]//Proc. of the IEEE/RSJ International Conference on Intelligent Robots and Systems, 2008: 340-345.
38	全闻捷, 周绍磊, 戴洪德, 等. 基于高斯-牛顿四元数描述符滤波器的姿态估计算法[J]. 传感技术学报, 2020, 33 (11): 1627- 1636.
	QUAN W J , ZHOU S L , DAI H D , et al. Attitude estimation algorithm based on Gaussian Newton quaternion descriptor filter[J]. Journal of Sensing Technology, 2020, 33 (11): 1627- 1636.
39	全闻捷, 周绍磊, 姜旭, 等. 基于加速度和速度建模的行人导航位置估计算法研究[J]. 仪表技术, 2020, (11): 29- 35.
	QUAN W J , ZHOU S L , JIANG X , et al. Research on pedes trian navigation position estimation algorithm based on accele- ration and velocity modeling[J]. Instrument Technology, 2020, (11): 29- 35.
40	戴洪德, 李松林, 周绍磊, 等. 基于伪标准差和N-P准则的行人导航零速检测[J]. 中国惯性技术学报, 2018, 26 (6): 701- 707.
	DAI H D , LI S L , ZHOU S L , et al. Zero speed detection of pedestrian navigation based on pseudo standard deviation and N-P criterion[J]. Chinese Journal of Inertial Technology, 2018, 26 (6): 701- 707.
41	李松林, 周绍磊, 戴洪德. 基于MEMS传感器的惯性行人导航算法研究[D]. 烟台: 海军航空大学, 2018.
	LI S L, ZHOU S L, DAI H D. Research on inertial pedestrian navigation algorithm based on MEMS sensor[D]. Yantai: Naval Aviation University, 2018.

参数	x轴拟合误差	y轴拟合误差	z轴拟合误差
均值/(m/s²)	-0.004 4	0.001 3	0.018 7
方差(m²/s⁴)	0.016 2	0.016 3	0.021 7

角速度/(rad/s)	0 < t≤2 s
ω_{x, t}	5cos(1.5t)
ω_{y, t}	2sin t
ω_{z, t}	1cos(2t)

时间/s	加速度/(m/s²)
0.3≤t≤0.8	3sin(5t)
1.3<t≤1.8	5sin(2t)

算法	俯仰角/(°)	滚转角/(°)	航向角/(°)
CKF	0.071	0.086	0.044
三子样旋转矢量法	0.116	0.143	0.062

算法	水平误差/m	高度误差/m
加入CKF算法	0.105	0.160
未加入CKF算法	0.243	0.201