复杂环境下基于加权因子图的全源导航算法

All-Source Navigation Algorithm Based on Weighted Factor Graph in Complex Environment

针对无人机在复杂环境中传感器性能变化和测量异常的情况,为了提高全源导航系统的鲁棒性能,提出一种基于加权因子图模型的多传感器融合算法。根据故障信息的渐变特点,建立自适应权重函数,实时动态地调整不同因子的权重,并构造因子图优化模型和加权非线性代价函数,抑制异常值对状态估计的干扰。最后搭建一个融合多种传感器的全源导航系统,对所提出的方法进行软故障和混合故障仿真测试。结果表明,与Huber鲁棒核和噪声修正方法相比,所提出方法的位置误差降低了34%以上,具备更好的鲁棒性和可靠性,同时不会增加计算成本。

In order to improve the robustness of all-source navigation system,a multi-sensor fusion algorithm based on weighted factor graph model is proposed to solve the problem of sensor performance changes and measurement anomalies of UAV in complex environment.According to the gradient characteristics of fault information,an adaptive weight function is established to dynamically adjust the weights of different factors in real time,and a factor graph optimization model and a weighted nonlinear cost function are constructed to suppress the interference of outliers on state estimation.Finally,an all-source navigation system integrating multiple sensors is built,and the soft fault and mixed fault simulation tests are carried out for the proposed method.The results show that,compared with Huber robust kernel and noise modification methods,the proposed method reduces the position error by more than 34%,has better robustness and reliability,and does not increase the calculation cost.