Relative navigation method based on modified likelihood filtering for unmanned aerial vehicle formation
-
摘要:
针对无人机编队相对导航系统中视觉导航传感器量测数据存在随机时延问题，提出一种能够处理多步随机延迟量测的修正似然容积卡尔曼滤波（ML-CKF）算法。用多个伯努利随机变量对量测模型进行修正以描述随机延迟；通过边缘化延迟变量来计算滤波的似然函数以从延迟量测中提取准确的信息；采用三阶球面-径向容积准则计算高斯加权积分以解决系统的非线性。滤波中的加权因子根据接收量测的特性进行调整，因此，所提修正似然滤波具有自适应卡尔曼滤波属性。利用罗德里格斯参数表示姿态误差，设计了基于修正似然容积卡尔曼滤波的相对导航滤波器。仿真结果表明：所提算法可以准确地估计出长机和僚机之间的相对位置、速度和姿态，且估计精度高于容积卡尔曼滤波和传统随机时延滤波。
Abstract:A modified likelihood cubature Kalman filtering (ML-CKF) is proposed to solve the problem that the measurements of vision-based relative navigation sensor for unmanned aerial vehicle formation are randomly delayed by multiple steps. The measurement model is modified by the Bernoulli random variables to describe the random delay. The likelihood function of the filtering is calculated by marginalizing out the delay variable to extract accurate information from the delayed measurements. The third-degree spherical-radial rule is utilized to compute the Gaussian-weighted integrals for the nonlinear system. The proposed modified likelihood filtering has the property of adaptive filtering because the weighting factors of the filtering are tuned based on the characteristics of the received measurements. By utilizing the Rodrigues parameters to denote the attitude errors, the relative navigation filter of unmanned aerial vehicle formation is designed based on the ML-CKF. Simulation results indicate that the proposed filtering algorithm could accurately estimate the relative position, velocity and attitude between the leader and follower. Moreover, the estimation accuracy of ML-CKF is superior to cubature Kalman filtering and conventional randomly delayed filtering.
-
图 1 修正似然容积卡尔曼滤波流程
Figure 1. Flowchart of modified likelihood cubature Kalman filtering
表 1 惯性和视觉传感器偏差参数
Table 1. Deviation parameter of inertial and visual sensors
参数 数值 加速度计初始漂移/$ {\rm{mg}} $ $0.2\;$ 加速度计随机游走/(${\rm{mg}}\cdot{{\rm{s}}^{-1/2} }$) $ 0.002\; $ 加速度计噪声/($ \;{\rm{mg}} \cdot {{\rm{s}}^{1/2}} $) $ 0.02 $ 陀螺仪初始漂移/($(^\circ) \cdot {\rm{h} }^{-1}$) $0.1\;$ 陀螺仪随机游走/($( ^\circ) \cdot { {\rm{h} }^{-3/2} }$) $0.06\;$ 陀螺仪噪声/($(^\circ) \cdot{ {\rm{h} }^{-1/2} }$) $0.01$ 视觉传感器噪声/$ \text{µ} {\rm{rad}} $ $ 80\; $ 
下载: 导出CSV
表 2 特征光点位置列表
Table 2. List of beacon locations
特征光点标号 $ {X_j} $/m $ {Y_j} $/m $ {Z_j} $/m 1 1.5 0 0 2 −2.5 0 0 3 0 2.5 0 4 0 −2.5 0 5 0 3.5 −0.5 6 0 −3.5 0.5
下载: 导出CSV
表 3 不同滤波算法的计算耗时
Table 3. Single computation time of different filtering algorithms
滤波算法 计算耗时/ms CKF 0.60 ORD-CKF 1.88 MRD-CKF 12.72 ML-CKF 5.49
下载: 导出CSV
-
[1] ZHU Y F, SUN Y R, ZHAO W, et al. A novel relative navigation algorithm for formation flight[J]. Journal of Aerospace Engineering, 2020, 234(2): 308-318. [2] ELLINGSON G, BRINK K, MCLAIN T. Relative navigation of fixed-wing aircraft in GPS-denied environments[J]. Navigation, 2020, 67: 255-273. doi: 10.1002/navi.364 [3] 万九卿, 布少聪, 钟丽萍. 基于混合动态信念传播的多无人机协同定位算法[J]. 北京航空航天大学学报, 2016, 42(5): 934-944. doi: 10.13700/j.bh.1001-5965.2015.0321WAN J Q, BU S C, ZHONG L P. Cooperative localization algorithm of multi-UAVs based on dynamics hybrid belief propagation[J]. Journal of Beijing University of Aeronautics and Astronautics, 2016, 42(5): 934-944(in Chinese). doi: 10.13700/j.bh.1001-5965.2015.0321 [4] FOSBURY A M, CRASSIDIS J L. Relative navigation of air vehicles[J]. Journal of Guidance, Control and Dynamics, 2008, 31(4): 824-834. doi: 10.2514/1.33698 [5] JEONG J, KIM S, SUK J. Parametric study of sensor placement for vision-based relative navigation system of multiple spacecraft[J]. Acta Astronautica, 2017, 141: 36-49. doi: 10.1016/j.actaastro.2017.09.020 [6] XU Z, QI N, CHEN Y. Parameter estimation of a three-axis spacecraft simulator using recursive least-squares approach with tracking differentiator and extended Kalman filter[J]. Acta Astronautica, 2015, 117: 254-262. doi: 10.1016/j.actaastro.2015.08.010 [7] JULIER S J, UHLMANN J K, DURRANT-WHYTE H F. A new method for the nonlinear transformation of means and covariances in filters and estimators[J]. IEEE Transaction Automatica Control, 2000, 45(3): 77-482. [8] 卢道华, 付怀达, 王佳, 等. 基于IMU与UKF的船舶升沉运动信息测量方法[J]. 北京航空航天大学学报, 2021, 47(7): 1323-1331. doi: 10.13700/j.bh.1001-5965.2020.0223LU D H, FU H D, WANG J, et al. Measurement of ship’s heave motion information based on IMU and UKF algorithm[J]. Journal of Beijing University of Aeronautics and Astronautics, 2021, 47(7): 1323-1331(in Chinese). doi: 10.13700/j.bh.1001-5965.2020.0223 [9] ARASARATNAM I, HAYKIN S. Cubature Kalman filters[J]. IEEE Transaction Automatica Control, 2009, 54(6): 1254-1269. doi: 10.1109/TAC.2009.2019800 [10] 李兆铭, 杨文革, 丁丹, 等. 多星对合作目标的分布式协同导航滤波算法[J]. 北京航空航天大学学报, 2018, 44(3): 462-469. doi: 10.13700/j.bh.1001-5965.2017.0150LI Z M, YANG W G, DING D, et al. Distributed coordinated navigation filtering algorithm for cooperative target by multi-satellite[J]. Journal of Beijing University of Aeronautics and Astronautics, 2018, 44(3): 462-469(in Chinese). doi: 10.13700/j.bh.1001-5965.2017.0150 [11] 崔乃刚, 王小刚, 郭继峰. 基于Sigma-point卡尔曼滤波的INS/Vision相对导航方法研究[J]. 宇航学报, 2009, 30(6): 2220-2225. doi: 10.3873/j.issn.1000-1328.2009.06.028CUI N G, WANG X G, GUO J F. Reserch on relative navigation method based on INS/Vision using Sigma-point Kalman filter[J]. Journal of Astronautics, 2009, 30(6): 2220-2225(in Chinese). doi: 10.3873/j.issn.1000-1328.2009.06.028 [12] PLETT G L, ZARZHITSKY D, PACK D J. Out-of-order sigma-point Kalman filtering for target localization using cooperating unmanned aerial vehicles[M]. Berlin: Advances in Cooperative Control and Optimization, 2007: 21-43. [13] KIM Y, HONG K, BANG H. Utilizing out-of-sequence measurement for ambiguous update in particle filtering[J]. IEEE Transactions on Aerospace and Electronic Systems, 2018, 54(1): 493-501. doi: 10.1109/TAES.2017.2741878 [14] HERMOSO-CARAZO A, LINARES-PEREZ J. Extended and unscented filtering algorithms using one-step randomly delayed observations[J]. Applied Mathematics and Computation, 2007, 190(2): 1375-1393. doi: 10.1016/j.amc.2007.02.016 [15] HERMOSO-CARAZO A, LINARES-PEREZ J. Unscented filtering algorithm using two-step randomly delayed observations in nonlinear systems[J]. Applied Mathematical Modelling, 2009, 33: 3705-3717. doi: 10.1016/j.apm.2008.12.008 [16] WANG X X, LIANG Y, PAN Q, et al. Gaussian filter for nonlinear systems with one-step randomly delayed measurement[J]. Automatica, 2013, 49: 976-986. doi: 10.1016/j.automatica.2013.01.012 [17] 张勇刚, 黄玉龙, 赵琳. 一种带多步随机延迟量测高斯滤波器的一般框架解[J]. 自动化学报, 2015, 41(1): 122-135. doi: 10.16383/j.aas.2015.c140293ZHANG Y G, HUANG Y L, ZHAO L. A general framework solution to Gaussian filter with multiple-step randomly-delayed measurements[J]. Acta Automatica Sinica, 2015, 41(1): 122-135(in Chinese). doi: 10.16383/j.aas.2015.c140293 [18] ESMZAD R, ESFANJANI M E. Modified likelihood Kalman filter for systems with incomplete, delayed and lost measurements[J]. System and Control Letters, 2018, 120: 23-28. doi: 10.1016/j.sysconle.2018.08.001 [19] ESMZAD R, ESFANJANI M E. Bayesian filter for nonlinear systems with randomly delayed and lost measurements[J]. Automatica, 2019, 107: 36-42. doi: 10.1016/j.automatica.2019.05.025 [20] 张旭. 基于鲁棒自适应滤波的无人机编队相对导航方法研究[D]. 哈尔滨: 哈尔滨工业大学, 2017: 56-58.ZHANG X. Research on relative navigation method of UAV formation based on robust adaptive filtering [D]. Harbin: Harbin Institute of Technology, 2017: 56-58(in Chinese). -
下载:
点击查看大图
计量
- 文章访问数: 522
- HTML全文浏览量: 81
- PDF下载量: 32
- 被引次数: 0
