-
摘要:
重力辅助惯性导航是水下潜器实现长期自主导航的重要技术，但重力异常观测与惯性导航状态参数之间存在复杂耦合关系，估计参数选取不当易导致滤波精度下降甚至发散。为明确不同导航状态参数的实时估计效能，围绕重力辅助惯性导航系统状态参数的可观测度开展建模、分析与验证研究。建立误差状态形式下的重力辅助惯性导航滤波模型，选取姿态误差、速度误差、位置误差、陀螺零偏和加速度计零偏等13维状态参数，推导重力异常观测方程及其对各状态参数的偏导关系，明确观测矩阵与导航参数估计能力之间的联系。分别构建基于协方差矩阵、可观测性矩阵和李导数的可观测度分析模型，对不同方法的适用性和一致性进行比较。基于某海域水下潜器仿真航迹，分析不同状态参数可观测度随航行时间、重力场特征和机动方式的变化规律。实验结果表明，经度、纬度、东向速度和北向速度具有较高可观测度，是重力辅助惯性导航中较优的估计状态组合；陀螺和加速度计零偏可观测度较低，不宜直接作为主要反馈校正参数。在此基础上，进一步设计固定参数组合和基于可观测度阈值的动态参数组合进行滤波验证，结果显示：位置与水平速度联合估计可使导航定位精度提升42%，动态调整状态参数组合可进一步将导航定位精度提升5%。研究结果可为重力辅助惯性导航滤波状态选取、方程结构设计和机动条件下的反馈校正策略提供依据。
Abstract:Gravity-aided inertial navigation is an important technology for enabling long-term autonomous navigation of underwater vehicles. However, the gravity anomaly observation is coupled with inertial navigation state parameters in a complex manner, and improper selection of estimated parameters may lead to reduced filtering accuracy or even filter divergence. To clarify the real-time estimation effectiveness of different navigation state parameters, this paper conducts modeling, analysis, and verification of the observabal degree of state parameters in a gravity-aided inertial navigation system. First, an error-state filtering model for gravity-aided inertial navigation is established. A 13-dimensional state vector is selected, including attitude errors, velocity errors, position errors, gyroscope biases, and accelerometer biases. The gravity anomaly observation equation and its partial derivatives with respect to each state parameter are derived, thereby clarifying the relationship between the observability matrix and the estimability of navigation parameters. Second, observabal degree analysis models based on the covariance matrix, observability matrix, and Lie derivatives are constructed, respectively, and the applicability and consistency of different methods are compared. Based on a simulated underwater vehicle trajectory in a certain sea area, the variation patterns of the observabal degree of different state parameters with navigation time, gravity field characteristics, and maneuvering conditions are analyzed. The experimental results show that longitude, latitude, eastward velocity, and northward velocity have relatively high observabal degrees and constitute a preferable combination of estimated states for gravity-aided inertial navigation. In contrast, gyroscope and accelerometer biases have relatively low observabal degrees and are not suitable to be directly used as the main feedback correction parameters. On this basis, fixed parameter combinations and a dynamic parameter combination based on an observabal degree threshold are further designed for filtering verification. The results indicate that joint estimation of position and horizontal velocity improves navigation positioning accuracy by 42%, while dynamically adjusting the state parameter combination further improves positioning accuracy by 5%. The research results provide a basis for filtering-state selection, equation-structure design, and feedback correction strategies under maneuvering conditions in gravity-aided inertial navigation.
-
图 1 仿真轨迹与DTU重力异常基准图
Figure 1. Simulated trajectory and DTU gravity anomaly reference map
图 3 基于可观测性矩阵的可观测度分析
Figure 3. Observable degree analysis based on observability matrix
图 5 水下潜器不同机动条件下的纬度、经度、东向速度和北向速度可观测度变化
Figure 5. Variations in the observable degree of latitude, longitude, eastward velocity and northward velocity of submarines under different maneuvering conditions
表 1 仿真参数设置
Table 1. Parameter settings for simulation
误差项 仪器 常值零偏 随机游走 惯导误差项 陀螺仪(x、y、z轴) 0.002 (°)/h 0.001 (°)/$ \sqrt{\mathrm{h}} $ 加速度计(x、y、z轴) 1×10−4 m/s2 5×10−5 m/s2$ \sqrt{\mathrm{h}} $ 重力仪误差项 水平加速度计(x、y轴) 5×10−4 m/s2 5×10−5 m/s2$ \sqrt{\mathrm{h}} $ 垂向加速度计(z轴) 3×10−5 m/s2 3×10−6 m/s2$ \sqrt{\mathrm{h}} $ 
下载: 导出CSV
表 2 基于协方差矩阵的可观测度统计
Table 2. Observable degree statistics based on covariance matrix
状态量 可观测度 平均值 最大值 最小值 中位数 东向姿态角$ {\phi }_{\mathrm{E}} $ 0.0049 4.9846 1.4776 ×10−40.0036 北向姿态角$ {\phi }_{\mathrm{N}} $ 0.0092 7.8214 2.7687 ×10−40.0081 天向姿态角$ {\phi }_{\mathrm{U}} $ 0.0044 5.2185 1.0591 ×10−40.0032 东向速度$ v_{\mathrm{E}}^{\mathrm{n}} $ 0.0018 6.2523 2.0848 ×10−58.0415 ×10−4北向速度$ v_{\mathrm{N}}^{\mathrm{n}} $ 1.8075 ×10−41.0000 6.5245 ×10−68.2528 ×10−4纬度$ B $ 7.4772 ×10−41.0000 5.8775 ×10−63.5263 ×10−4经度$ L $ 0.0022 2.4058 1.3126 ×10−40.0015 陀螺仪x轴零偏$ \varepsilon _{x}^{\mathrm{b}} $ 1.0596 10.5942 0.1060 0.9976 陀螺仪y轴零偏$ \varepsilon _{y}^{\mathrm{b}} $ 1.2530 16.8246 0.1442 1.1144 陀螺仪z轴零偏$ \varepsilon _{\textit{z}}^{\mathrm{b}} $ 1.1420 5.6585 0.0592 1.0954 加速度计x轴零偏$ \nabla _{x}^{\mathrm{b}} $ 1.0000 1.0001 0.9983 1.0000 加速度计y轴零偏$ \nabla _{y}^{\mathrm{b}} $ 1.0000 1.0000 0.9999 1.0000 加速度计z轴零偏$ \nabla _{\textit{z}}^{\mathrm{b}} $ 1.0053 1.0185 0.9964 1.0056
下载: 导出CSV
表 3 基于可观测性矩阵的可观测度统计
Table 3. Observable degree statistics based on observability matrix
状态量 可观测度 平均值 最大值 最小值 中位数 东向姿态角$ {\phi }_{\mathrm{E}} $ 1.2744 ×10−82.3337 ×10−76.2341 ×10−174.1144 ×10−9北向姿态角$ {\phi }_{\mathrm{N}} $ 1.1784 ×10−62.0072 ×10−54.1063 ×10−144.3412 ×10−7天向姿态角$ {\phi }_{\mathrm{U}} $ 1.1534 ×10−161.1520 ×10−158.8048 ×10−241.0979 ×10−16东向速度$ v_{\mathrm{E}}^{\mathrm{n}} $ 0.0034 0.0739 4.2204 ×10−40.0020 北向速度$ v_{\mathrm{N}}^{\mathrm{n}} $ 3.1137 ×10−58.6150 ×10−44.2506 ×10−101.2367 ×10−5纬度$ B $ 0.5921 1.0000 9.6551 ×10−80.6218 经度$ L $ 0.6746 1.0000 4.8884 ×10−70.7832 陀螺仪x轴零偏$ \varepsilon _{x}^{\mathrm{b}} $ 3.8221 ×10−81.6999 ×10−66.4954 ×10−173.7341 ×10−9陀螺仪y轴零偏$ \varepsilon _{y}^{\mathrm{b}} $ 1.6520 ×10−73.1238 ×10−52.0327 ×10−146.0601 ×10−8陀螺仪z轴零偏$ \varepsilon _{\textit{z}}^{\mathrm{b}} $ 8.4659 ×10−121.5003 ×10−94.5345 ×10−202.9569 ×10−12加速度计x轴零偏$ \nabla _{x}^{\mathrm{b}} $ 1.1639 ×10−72.0523 ×10−61.0053 ×10−153.9273 ×10−8加速度计y轴零偏$ \nabla _{y}^{\mathrm{b}} $ 6.1999 ×10−91.3457 ×10−77.2243 ×10−172.0275 ×10−9加速度计z轴零偏$ \nabla _{\textit{z}}^{\mathrm{b}} $ 5.7572 ×10−121.0471 ×10−92.6687 ×10−191.6857 ×10−12
下载: 导出CSV
表 4 基于李导数的可观测度统计
Table 4. Observability statistics based on Li derivative
统计量 可观测度 平均值 最大值 最小值 中位数 东向姿态角$ {\phi }_{\mathrm{E}} $ 1.6301 ×10−81.9703 ×10−72.5511 ×10−164.3433 ×10−9北向姿态角$ {\phi }_{\mathrm{N}} $ 1.4254 ×10−61.6758 ×10−53.5771 ×10−143.6179 ×10−7天向姿态角$ {\phi }_{\mathrm{U}} $ 7.3240 ×10−145.5146 ×10−121.4287 ×10−222.6611 ×10−15东向速度$ v_{\mathrm{E}}^{\mathrm{n}} $ 0.0040 0.0557 4.7734 ×10−40.0020 北向速度$ v_{\mathrm{N}}^{\mathrm{n}} $ 3.9321 ×10−56.2703 ×10−48.6948 ×10−101.4376 ×10−5纬度$ B $ 0.5947 1.0000 3.4716 ×10−70.6328 经度$ L $ 0.6823 1.0000 8.6636 ×10−70.7743 陀螺仪x轴零偏$ \varepsilon _{x}^{\mathrm{b}} $ 4.1122 ×10−87.6536 ×10−74.7488 ×10−164.0128 ×10−9陀螺仪y轴零偏$ \varepsilon _{y}^{\mathrm{b}} $ 1.9779 ×10−72.1788 ×10−62.2892 ×10−145.1383 ×10−8陀螺仪z轴零偏$ \varepsilon _{\textit{z}}^{\mathrm{b}} $ 1.0816 ×10−106.2871 ×10−92.0796 ×10−199.4292 ×10−12加速度计x轴零偏$ \nabla _{x}^{\mathrm{b}} $ 1.4133 ×10−71.7135 ×10−67.3289 ×10−163.4787 ×10−8加速度计y轴零偏$ \nabla _{y}^{\mathrm{b}} $ 6.9321 ×10−91.1884 ×10−73.4549 ×10−162.3215 ×10−9加速度计z轴零偏$ \nabla _{\textit{z}}^{\mathrm{b}} $ 4.8238 ×10−121.2353 ×10−95.5070 ×10−191.6667 ×10−12
下载: 导出CSV
表 5 不同状态量组合下的EKF算法精度对比
Table 5. Comparison of EKF algorithm accuracy under different combinations of state variables
导航组合量 纬度均方
误差/m经度均方
误差/m东向速度
均方误差/
(m·s−2)北向速度
均方误差/
(m·s−2)纬度绝对
平均
误差/m经度绝对
平均
误差/m东向速度绝
对平均误差/
(m·s−2)北向速度绝
对平均误差/
(m·s−2)纬度绝对
中误差/m经度绝对
中误差/m东向速度
绝对中误
差/(m·s−2)北向速度
绝对中误
差/(m·s−2)纬度
经度504.8552 340.3976 0.3607 0.2676 292.4308 229.4628 0.2899 0.2176 154.2716 146.3080 0.2516 0.2018 纬度
经度
东向速度559.3040 397.0688 0.3504 0.2834 322.4332 223.7216 0.2006 0.2296 163.3464 101.8600 0.1071 0.1939 纬度
经度
东向速度
北向速度306.3208 171.3100 0.1054 0.2115 193.1636 117.9724 0.0841 0.1679 112.0460 69.0796 0.0696 0.1369 (动态调整)
纬度
经度
东向速度
北向速度285.2080 172.2360 0.1059 0.2095 186.1260 120.5652 0.0833 0.1675 111.3052 70.3760 0.0673 0.1380
下载: 导出CSV
-
[1] 汪凤林, 蔡体菁, 王东霞. 惯性/重力匹配组合导航系统可观测性研究[J]. 安徽大学学报(自然科学版), 2008, 32(6): 27-31.Wang F L, Cai T J, Wang D X. Inertial/gravity matching integrated navigation system[J]. Journal of Anhui University (Natural Sciences), 2008, 32(6): 27-31(in Chinese). [2] 毛宁, 李安, 许江宁, 等. 惯性/重力组合导航可观测性分析及鲁棒滤波方法[J]. 武汉大学学报(信息科学版), 2024, 49(11): 2113-2121.Mao N, Li A, Xu J N, et al. Observability analysis and robust fusion algorithms of INS/gravity integrated navigation[J]. Geomatics and Information Science of Wuhan University, 2024, 49(11): 2113-2121(in Chinese). [3] 李姗姗. 水下重力辅助惯性导航的理论与方法研究[D]. 郑州: 解放军信息工程大学, 2010.Li S S. Research on the theory and method of underwater gravity-aided inertial navigation[D]. Zhengzhou: PLA Information Engineering University, 2010(in Chinese). [4] 彭富清. 海洋重力辅助导航方法及应用[D]. 郑州: 解放军信息工程大学, 2009.Peng F Q. Marine gravity aided navigation methods with their applications[D]. Zhengzhou: PLA Information Engineering University, 2009(in Chinese). [5] Ham F M, Brown R G. Observability, eigenvalues, and Kalman filtering[J]. IEEE Transactions on Aerospace and Electronic Systems, 1983, AES-19(2): 269-273. [6] 马艳红, 胡军. 基于SVD理论的可观测度分析方法的几个反例[J]. 中国惯性技术学报, 2008, 16(4): 448-452.Ma Y H, Hu J. Counterexamples for degree of observability analysis method based on SVD theory[J]. Journal of Chinese Inertial Technology, 2008, 16(4): 448-452(in Chinese). [7] 周广涛, 邵剑波, 韩少卫, 等. SVD可观测度分析方法的改进及组合导航中的应用[J]. 哈尔滨工业大学学报, 2020, 52(4): 52-57.Zhou G T, Shao J B, Han S W, et al. Improvement of observable degree analysis method based on SVD and application in integrated navigation[J]. Journal of Harbin Institute of Technology, 2020, 52(4): 52-57(in Chinese). [8] Sedoglavic A. A probabilistic algorithm to test local algebraic observability in polynomial time[J]. Journal of Symbolic Computation, 2002, 33(5): 735-755. [9] Quan Q. Introduction to multicopter design and control[M]. Berlin: Springer, 2017. [10] 孔星炜, 董景新, 吉庆昌, 等. 一种基于PWCS的惯导系统可观测度分析方法[J]. 中国惯性技术学报, 2011, 19(6): 631-636.Kong X W, Dong J X, Ji Q C, et al. INS observable degree analysis method based on PWCS[J]. Journal of Chinese Inertial Technology, 2011, 19(6): 631-636(in Chinese). [11] Du S, Sun W, Gao Y. Improving observability of an inertial system by rotary motions of an IMU[J]. Sensors, 2017, 17(4): 698. [12] Ma Y H, Fang J C, Wang W, et al. Decoupled observability analyses of error states in INS/GPS integration[J]. Journal of Navigation, 2014, 67(3): 473-494. [13] 马金艳. 基于可观测度理论的智能滤波算法研究[D]. 杭州: 杭州电子科技大学, 2017.Ma J Y. Research on smart Kalman filtering algorithm on observable degree theory[D]. Hangzhou: Hangzhou Dianzi University, 2017(in Chinese). [14] 成研, 李强, 付强文, 等. 一种改进的可观测度分析方法[J]. 航空精密制造技术, 2017, 53(6): 17-21.Cheng Y, Li Q, Fu Q W, et al. An improved method for observability analysis[J]. Aviation Precision Manufacturing Technology, 2017, 53(6): 17-21(in Chinese). [15] 马朋, 张福斌, 徐德民, 等. 基于条件数的多自主水下航行器协同定位系统可观测度分析[J]. 兵工学报, 2015, 36(1): 138-143.Ma P, Zhang F B, Xu D M, et al. Observability analysis of cooperative localization system for MAUV based on condition number[J]. Acta Armamentarii, 2015, 36(1): 138-143(in Chinese). [16] 杨晓霞, 阴玉梅. 可观测度的探讨及其在捷联惯导系统可观测性分析中的应用[J]. 中国惯性技术学报, 2012, 20(4): 405-409.Yang X X, Yin Y M. Discussions on observability and its applications in SINS[J]. Journal of Chinese Inertial Technology, 2012, 20(4): 405-409(in Chinese). [17] 严恭敏. 捷联惯导算法及车载组合导航系统研究[D]. 西安: 西北工业大学, 2004.Yan G M. Research on strapdown inertial navigation algorithm and vehicle integrated navigation system[D]. Xi’an: Northwestern Polytechnical University, 2004(in Chinese). [18] Yang Y L, Huang G Q. Aided inertial navigation with geometric features: observability analysis[C]//Proceedings of the 2018 IEEE International Conference on Robotics and Automation. Piscataway: IEEE Press, 2018: 2334-2340. [19] Ge Z Q. Exact observability and stability of stochastic implicit systems[J]. Systems & Control Letters, 2021, 157: 105029. [20] Wang Q, Wang L X, Qin W W, et al. A local observability analysis method for a time-varying nonlinear system and its application in the continuous self-calibration system[J]. Science China Information Sciences, 2020, 64(1): 11920. [21] Li J L, Wang Y, Lu Z X, et al. Instantaneous observable degree modeling based on movement measurement for airborne POS[J]. Aerospace Science and Technology, 2019, 84: 916-925. [22] Bernard P. Observer design for nonlinear systems[M]. Berlin: Springer, 2019. [23] Lu J Z, Xie L L, Zhang C X, et al. A simplified method of PWCS observability analysis theory by research of general expressions[C]//Proceedings of the 2013 International Conference on Mechatronic Sciences, Electric Engineering and Computer. Piscataway: IEEE Press, 2014: 154-162. [24] Naim M, Lahmidi F, Namir A. Controllability and observability analysis of nonlinear positive discrete systems[J]. Discrete Dynamics in Nature and Society, 2018, 2018(1): 3279290. [25] Butcher E A, Wang J W, Lovell T A. On Kalman filtering and observability in nonlinear sequential relative orbit estimation[J]. Journal of Guidance, Control, and Dynamics, 2017, 40(9): 2167-2182. [26] Rosik M, Dehnert R, Lerch S, et al. Observability analysis of nonlinear input-linear systems based on lie derivations using interval arithmetic[C]//Proceedings of the 2023 IEEE 11th International Conference on Systems and Control. Piscataway: IEEE Press, 2024: 931-937. [27] Martinelli A. Observability: a new theory based on the group of invariance[M]. Philadelphia: Society for Industrial and Applied Mathematics, 2020. [28] Kruger J, D’amico S. Observability analysis and optimization for angles-only navigation of distributed space systems[J]. Advances in Space Research, 2024, 73(11): 5464-5483. [29] Hermann R, Krener A. Nonlinear controllability and observability[J]. IEEE Transactions on Automatic Control, 1977, 22(5): 728-740. [30] Rouhani A, Abur A. Measurement selection for observability in dynamic state estimation[C]//Proceedings of the 2016 Power Systems Computation Conference. Piscataway: IEEE Press, 2016: 1-6. -
下载:
点击查看大图
计量
- 文章访问数: 368
- HTML全文浏览量: 160
- PDF下载量: 31
- 被引次数: 0
