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

2024年4月第2 期现代导航91复杂环境下基于加权因子图的全源导航算法庄凯杰，刘锦旺，黄嘉铖（广东工业大学自动化学院，广州510 0 0 0）摘要：针对无人机在复杂环境中传感器性能变化和测量异常的情况，为了提高全源导航系统的鲁棒性能，提出一种基于加权因子图模型的多传感器融合算法。根据故障信息的渐变特点，建立自适应权重函数，实时动态地调整不同因子的权重，并构造因子图优化模型和加权非线性代价函数，抑制异常值对状态估计的干扰。最后搭建一个融合多种传感器的全源导航系统，对所提出的方法进行软故障和混合故障仿真测试。结果表明，与Huber鲁棒核和噪声修正方法相比，所提出方法的位置误差降低了34%以上，具备更好的鲁棒性和可靠性，同时不会增加计算成本。关键词：全源导航；因子图；复杂环境；故障检测中图分类号：TP212文献标志码：A文章编号：16 7 4-7 9 7 6-(2 0 2 4)-0 2-0 9 1-0 7All-Source Navigation Algorithm Based on Weighted Factor Graphin Complex EnvironmentZHUANG Kaijie,LIU Jinwang,HUANG JiachengAbstract:In order to improve the robustness of all-source navigation system,a multi-sensor fusion algorithm based onweighted factor graph model is proposed to solve the problem of sensor performance changes and measurement anomalies of UAV incomplex environment.According to the gradient characteristics of fault information,an adaptive weight function is established todynamically adjust the weights of different factors in real time,and a factor graph optimization model and a weighted nonlinear costfunction are constructed to suppress the interference of outliers on state estimation.Finally,an all-source navigation systemintegrating multiple sensors is built,and the soft fault and mixed fault simulation tests are carried out for the proposed method.Theresults show that,compared with Huber robust kernel and noise modification methods,the proposed method reduces the positionerror by more than 34%,has better robustness and reliability,and does not increase the calculation cost.Key words:All-Source Navigation;Factor Graph;Complex Environment;Fault Detection决策的先决条件，是飞行控制和任务实施的关键。0引言无人机在现代军事上被广泛应用于侦查巡逻和制导突袭等领域，而定位导航作为无人系统智能收稿日期：2 0 2 4-0 1-2 7。庄凯杰（19 9 9.0 5一），广东汕头人，硕士研究生，主要研究方向为组合导航与多传感器融合。传统的组合导航已经得到深入的研究，Gao将基于稀疏网格正交滤波器的分布式状态融合应用在惯性/天文/卫星导航多源融合中l，M u 提出一种基于自适应联邦滤波的多传感器组合导航以缓解多普勒数据不可靠的问题2。复杂的外部环境使传统导航形式难以满足任务要求，需要依靠多种测量方式构建全源定位导航，文献3 中构建了基于惯性/无92线电/天文/地磁的高超飞行器全源导航框架。因子图方法能充分将历史测量信息进行多次迭代，且具备良好的扩展性，满足全源导航系统即插即用的要求。因子图在理论体系发展方面日渐成熟，ChristianForster等使用因子图搭建视觉惯性导航系统并提出预积分理论4，而Kaess 将抽象的矩阵分解转化为贝叶斯树模型5 并提出新的增量平滑映射算法（Incremental Smoothing and Mapping,iSAM2）应用在稀疏非线性优化问题中；在无人自主系统方面也得到广泛应用，Zeng将因子图融合算法应用在实际微型无人机的定位导航中6 ，Sina 等采用惯性测量单元（Inertial Measurement Unit，I M U)预积分和边缘化方法降低信息融合计算成本7，与传统扩展卡尔曼滤波（ExtendedKalmanFilter，EK F）相比定位精度得到显著提高。文献8 证明了以因子图为基础的全源融合框架的可行性。无人机在复杂环境中需要人为地根据地形、时段和气候确定传感器优化方案，但磁干扰、信号遮挡和温度漂移等因素无法预判，需要搭建有效的故障检测及隔离机制，解决传感器面临的异常干扰问题。针对传感器性能变化的影响，不少研究方法提出通过因子图的灵活性来降低外界干扰对导航系统的影响。基于因子图的鲁棒核估计和通过噪声矩阵调整可信度等检测方法被应用在越来越多的融合情景，例如解决测量丢失、离群值和异常噪声等问题9-10 ，文献11 使用M估计方法为代价函数封装上一层鲁棒核模型，提高对异常值的抗干扰能力；Ge等针对不同场景需求建立基于因子图和时变噪声估计的全源融合系统12 ，通过改变噪声协方差矩阵降低异常值的融合权重。常见的故障信息有硬故障和软故障，后者由于其缓慢变化的特点很难受到检测和抑制。一方面，Wei和Zhang分别在车辆和室内机器人测试中调整因子权重，提出加权传感器因子的方法来提高导航系统的鲁棒性能13-14，但在克服软故障方面缺乏深入研究，对渐变故障的敏感度不高；另一方面，文献15 提出改进的残差卡方检验应对软故障问题，但状态推算需要充分依赖导航预测值的精度。现有因子图算法的不足主要体现在故障检测机制中各个传感器测量值的权重分布，初始分配的测量权重不再适用于复杂环境下的状态估计，需要做出动态调整以提高模型的可靠性。加权因子能为每个融合时刻的因子计算权重大小，能直接调整传感器对融现代导航合的贡献，降低异常传感器的可信度，提高全源导航算法的环境自适应性能。为了更好地解决传感器性能变化和测量异常等问题，本文提出基于加权因子图模型的多传感器融合算法，提高复杂动态场景下全源导航系统的抗干扰能力和导航精度。先建立多传感器融合的因子图优化模型，然后提出基于加权因子图的故障检测与隔离算法，并通过两个仿真实验与鲁棒核和噪声修正算法进行对比和分析。1全源导航信息融合算法1.1因子图优化模型因子图由一个二分图F=(U,V,E)来表示，边e,EE连接因子节点f,eU和状态节点x,eV。高斯噪声假设下，因子具有如式（1）所示形式(1)2式中：h(x)为传感器测量函数；P,为测量值z,的高斯噪声协方差矩阵；II哆表示马氏距离的平方。对于任意一个因子图，最大后验估计都能转化为最大化所有因子的乘积。为确定导航状态x的最优解，将最大后验估计转化为如式（2）所示最小二乘问题xMAP=arg max II f(x,)=arg min EIl h(x,)-z,Il,Xi考虑载体的角速度R3和平移速度vR,定义载体在时间间隔t内的位姿增量如式（3）所示5T=o,vAt设传感器融合过程所有位姿增量的集合三=5)，得到因子图优化模型的代价函数如式（4)所示=arg min(三+ZZm(,z)/keMieN2024年(2)(3)(H,=-b,)+Z1(,t,)i.jelm(4)第2 期式中：H，和b,为先验因子的信息矩阵和残差向量；r(,)和rm(,z)分别表示IMU因子和其他传感器因子的残差。对所有传感器的测量函数进行一阶泰勒展开并线性化求和，再将噪声协方差矩阵白化，转化为如式（5）所示最小二乘问题进行优化=arg min|H(3)=-b(x)l)(5)式中：H(3)为大型稀疏雅克比矩阵；b(x)为包含所有测量信息的残差向量。1.2基于加权因子图的故障检测算法多传感器融合中的残差指传感器测量值z与状态预测值的系统输出之差，记为r(x,z)=z-h(x)庄凯杰等：复杂环境下基于加权因子图的全源导航算法1,bu式中：(r)=0,1为因子权重，取值越小表示传感器可信度越低；系数与传感器特性有关，取值越大，因子权重对残差变化就越敏感；b,和b,为残差上限和下限。代入式（1），得到加权因子图模型的传感器因子的一般形式如式（10）所示f(x,)=exp/I1lp(r)x(h(x,)-z,/,2=exp甲(r)(h(x,)-z,)plT(r)(h(x,)-2=exp(h(x,)-z.)P=lY(r)(h(x,)-z.)2(6)93b,|rlb,(9)(H,=-b,)+Z(,t)故障检测机制将残差作为观测信息与状态预测的偏离程度。在后文中将残差向量的模记为。在导航系统预测可靠的前提下，残差越小，传感器的可信度越高，反之越低。现有研究中提出的动态权重函数13 能有效抑制部分故障信息，但可能产生累积误差并在故障结束瞬间迫使系统对正常测量值做出误判。针对软故障的渐变特点，在给不同因子分配权重的同时，在i时刻基于残差序列做一步时间间隔为T的滑动平均计算，得到残差均值如式（7）所示(7)j=i-T+1定义i时刻的放大系数如式（8）所示1,R(i,w)bw(i)=)w,R(i,w)bw若均值超过容忍阈值bw，则将滑动窗口的尺度w作为当前因子权重的放大系数，增强权重函数对故障信息的敏感度，实现对软故障的检测与隔离。在复杂环境下，为减小传感器性能变化的影响，定义当前时刻传感器因子的自适应权重函数如式（9)所示(10)式中，(r)为所有元素为(r)的对角矩阵。将式(9)和式（10）代入式（4），得到多传感器融合非线性优化的加权代价函数如式（11）所示*=arg min(三keMieN=arg min三keMieN式中，()表示代价函数中的先验部分和IMU因子部分。式（11）即加权因子算法（WeightedFactorGraphOptimization，W-FG O),其流程如图1所示，在一次优化中同时完成权重调整、故障隔离和状态估计，同时不会明显增加计算成本。(8)预测值测量值i,jel自适应残差权重函数放大系数测量因子图1故障检测算法流程图P剔除故障分配权重加权信息因子融合(11)942仿真验证为验证本文所提出算法的有效性，设计如图2所示的飞行轨迹，无人机先后经过匀速前行、加速、转弯、爬升和俯冲等阶段。80070060050040030015001 000500纬度/m图2 无人机真实飞行轨迹图本文考虑 IMU、全球定位系统（Global PositionSystem,GPS）、气压高度计（Barometric Altimeter,BAR）、三轴磁力计（Magnetometer，M A G）、天文导航系统（Celestial Navigation System，C NS）、合成孔径雷达（Synthetic ApertureRadar，SA R）和超声波传感器（Ultrasonic Sensor，U LT），搭建无人机全源导航系统。为验证本文所提出算法的优越性，将基于鲁棒核估计和噪声协方差矩阵修正的两种因子图故障检测方法作为对比实验。Huber 算法16 是常用的鲁棒核模型，其损失函数为p(r)根据文献12 和文献17 ，修正后的噪声协方差矩阵可定义为Z=(1-)2,+?Zh现代导航式中，Z,和Z,为噪声协方差矩阵的上下限，式（13)和式（14）构成噪声修正（Modify）算法。2.1参数设置三轴陀螺仪和加速度计的输出由PSINS工具箱的轨迹发生器产生，参数如表1所示。GPS、C NS和SAR的测量误差分别设为10 m、0.0 1和12 m，频率为1Hz；BA R、M A G 和ULT的测量误差分别设为2.5m、0.1和0.3m，频率为5Hz。表1IMU仿真参数参数数值高斯白噪声10/h陀螺仪随机游走0高斯白噪声-1000-50002.000-1500经度/m-k?,rlkhl-k=k,/rlkhk-kio,lk,0.50一0(14)70图3GPS 权重函数值与测量残差的变化关系0.58090100时间/s110120130第2 期采用三种故障检测算法和传统因子图方法计算得到的无人机三维位置误差曲线如图4所示。在125s和36 5s处，由于GPS和SAR的测量偏移达到最大值，Huber算法出现接近50 m的大误差，其中GPS高度误差超过10 m；在2 0 5s和2 8 5s左右，CNS和MAG的累积误差较大降低了定位精度，导致Modify算法的纬度误差超过30 m。100FGOU/-10050100100U/-1005010/翼-1050100150图4三维位置误差曲线图同理，三维姿态误差和速度误差曲线如图5和图6 所示。传统因子图和Huber算法在2 8 5s处的滚转角和俯仰角误差超过13，且速度误差曲线的峰值都相对较高，出现次数较多；而Modify算法在2 50 s附近的姿态角误差比W-FGO略大，尽管前者垂直方向的速度误差与后者相近，但水平方向的误差幅度相对更大。从整体上来看，W-FGO算法对斜坡偏移的检测和抑制能力最显著，验证了本文所提方法的鲁棒性能。FGOHuber（。转滋200205010020020-40501006040200-2050100图5三维姿态误差曲线图庄凯杰等：复杂环境下基于加权因子图的全源导航算法Huber10(s/m)/回业-105010010(s/ml)/回-1050HuberModify150200100150150200150200150200时间/s95FGOModify150200100150200W-FGO2(s/u)/?2L250300200250200250300350400时间/sModify250300250300250300350W-FGO250300350250300350350400300350W-FGO35040035040040040040045050100W图6 三维速度误差曲线图为了定量地分析三种算法的导航精度，定义导400450450450450450450450150200时间/s航状态的平均方根误差（RootMean Square Error，RMSE）如式（15）所示RMSE=nk=l式中：表示无人机在k时刻的位置、姿态角和速度估计值；x表示真实值。三种故障检测算法得到的RMSE如表2 所示。所提出的W-FGO算法与Huber算法相比，位置误差分别降低34.9%、33.1%和30.2%，俯仰角误差降低39.4%，而速度误差均降低13%以上；与Modify算法相比，位置误差分别降低18.1%、18.8%和13.5%，滚转角误差降低38.3%，而速度误差均降低18%以上。验证本文所提方法对传感器软故障更加敏感，导航精度更高，且不会增加计算成本。表2 导导航状态误差（软故障）导航状态Huber经度14.089位置/m纬度12.730高度2.105俯仰角7.239姿态角）滚转角3.431偏航角8.232东向3.535速度北向3.290/(m/s)天向0.631运行时长/s40.8952）混合故障测试250之(&-x)ModifyW-FGO11.2039.17310.4938.5211.6991.4704.7314.3903.7492.3127.0747.2853.5562.9003.4752.7980.7030.54537.94538.103300350400(15)45096对导航系统添加服从均匀分布的随机离群值、高斯大噪声和常值偏移，故障参数如表3所示。表3混合故障的仿真参数传感器故障类型GPS斜坡偏移CNS随机离群值XU(-1.0,1.0)MAG高斯大噪声SAR常值偏移对于位置、姿态角和速度误差，本文讨论的三种算法和传统因子图方法得到的三维平均方根误差之和如图7 所示。W-FGO算法将位置误差控制在2 0 m以下，与Huber和Modify算法相比分别降低了42.7%和34.3%，并且姿态角和速度误差均最低，显著提高了全源导航系统在复杂动态环境中的鲁棒性能。4035302520151050图7 导航状态误差对比图（混合故障）经过仿真测试和对比，鲁棒核作用于整个代价函数，能夯实优化器的韧性但难以检测到软故障的存在；噪声修正作用于噪声矩阵以调整传感器的可信度，能较好跟踪除软故障以外的异常信息；加权因子则相当于调整因子图的拓扑结构，能实时分配权重信息和抑制多种传感器故障信息的影响。3结语本文提出一种基于加权因子图模型的多传感器融合算法，建立自适应权重函数，通过软故障和混合故障两组仿真测试，与Huber鲁棒核和噪声修正算法进行对比，得到的位置、姿态角和速度误差均最低。与传统的故障检测方法相比，本文提出的现代导航加权因子算法能动态调整传感器测量值的权重信息，提升无人机在复杂环境中的适应能力和精确可靠的定位能力。在后续的研究中，将考虑具体的物理实验测试和更全面的故障隔离机制。数值时间段/s2,2,0.5 m/s80125160205增强50 倍240285100,80 m320365FGOHuberModifyW-FGO位置/姿态角/）2024年参考文献：1 Gao B,Hu G,Zhong Y,et al.Distributed state fusionusing sparse-grid quadrature filter with application toINS/CNS/GNSS integrationJ.IEEE Sensors Journal,2021,22(4):3430-3441.2 Mu X,He B,Wu S,et al.A practical INS/GPS/DVL/PSintegrated navigation algorithm and its application onAutonomous Underwater VehicleJ.Applied OceanResearch,2021,106:102441.3赵岩，吴建峰，高育鹏基于多智能体导航的高超飞行器信息融合方法.系统工程与电子技术，2 0 2 0,42（2)：405-413.4 Forster C,Carlone L,Dellaert F,et al.IMU preintegrationonvisual-inertialmaximum-a-posteriori estimationC/Robotics:Scienceand Systems XI.2015.5 Kaess M,Johannsson H,Roberts R,et al.iSAM2:Incremental smoothing and mapping using the BayestreeJ.The International Journal of Robotics Research,2012,31(2):216-235.6 Zeng Q,Chen W,Liu J,et al.An improved multi-sensorfusion navigation algorithm based on the factor graphJ.速度/(m/s)Sensors，2 0 17,17(3):6 41.7 Taghizadeh S,Nezhadshahbodaghi M,Safabakhsh R,et al.A low-cost integrated navigation system based onfactor graph nonlinear optimization for autonomousflightJJ.GPS Solutions，2 0 2 2,2 6(3):7 8.8朱晓晗，陈帅，蒋长辉，等。基于因子图的组合导航方法及其可行性研究J电光与控制，2 0 19,2 6(4)：6 6-7 0.9 Li Y,Hou L,Yang Y,et al.Hubers M-estimation-basedcubature Kalman filter for an INS/DVL integratedsystemJ.Mathematical Problems in Engineering,2020:1-12.10徐晓苏，仲灵通一种基于M估计的抗差自适应多模型组合导航算法J.中国惯性技术学报，2 0 2 1，2 9（4)：482-490.(下转第10 1页）manifoldforefficient第2 期500-50-100-150ZH/李源-200-250-300-350-400-450300350400450500550600元6 50 7 0 0仿真次数21.510.500.5-1.524结语研究表明，载波环路采用FLL与PLL相结合的方式，可以使得环路能够快速地锁定信号，同时也具有较好的跟踪精度。因此，本文首先分析了FLL和 PLL的原理，确定了FLL采用二阶环，PLL采用三阶环路。随后设计了一种根据阈值动态调整的二阶FLL辅助三阶PLL的载波环路跟踪方法。通过Matlab仿真可以看出，本文提出的载波环路跟踪方法可以快速地对信号进行跟踪，可以作为一种高动态环境下的信号跟踪解决方案。王宇琦等：基于锁频环辅助锁相环的载波跟踪方法研究7508008509009501.0002004006008001 00012001.40016001.8002.000仿真次数图9载波跟踪环路频差、相差:101:参考文献：1王兰芳，吴长奇，高秀英.基于FLL与PLL级联的高动态载波跟踪技术 电子测量技术，2 0 0 9，34（3：25-27.2沈锋，徐定杰，薛冰.基于FLL_PLL相结合的载波跟踪在导航接收机中的应用J中国航海，2 0 0 4，59（2)：51-53.3FloydM.Gardner.锁相环技术M。姚剑清译，北京：人民邮电出版社，2 0 0 7.4Roland E.Best.锁相环设计仿真与应用M李永明译,北京：清华大学出版社，2 0 0 7.5 Jun Ma,Mike Li,Mark Marlett.A new measurement andanalysis for a third orderphase locked loop(PLL)transferfunctionCJ/International Test Conference,2005:1-10.6 赵婉璐.BDS接收机捕获与跟踪方法的研究D.南京：南京理工大学，2 0 14.7】王熙新体制导航信号的高动态捕获和跟踪算法设计D.成都：电子科技大学，2 0 17.8王俊，李加琪，吴嗣亮。锁频环辅助下锁相环的跟踪误差分析.北京理工大学学报，2 0 11，7：8 38-8 43.9】谢钢.GPS原理与接收机设计M.北京：电子工业出版社，2 0 0 8.10郑兴平，寇艳红，高动态GPS接收机跟踪环路设计与实现 测控遥感与导航定位，2 0 10，40（1)：6-2 7.(上接第9 6 页)11 Watson R M,Gross J N.Robust navigation in GNSSdegraded environment using graph optimizationCJ/Pro-ceedings of the 30th international technical meeting of thesatellite division of the institute of navigation.2017:2906-2918.12 Ge Y,Wang X.The Factor Graph Information FusionAlgorithm With Time-varying Noise Estimate forAll-Source Navigation SystemCJ/2021 IEEE/AIAA 40thDigital Avionics Systems Conference（D A SC).IEEE,2021:1-6.13 Wei X,Li J,Zhang D,et al.An improved integratednavigation method with enhanced robustness based onfactor graphJ.Mechanical SystemsandSignalProcessing，2 0 2 1,155:10 7 56 5.14 Zhang L,Wu X,Gao R,et al.A multi-sensor fusionpositioning approach for indoor mobile robot using factorgraphJ.Measurement,2023,216:112926.15王慧哲.基于多信息融合的无人机全源导航关键技术研究D.南京：南京航空航天大学，2 0 17.16 Qin H,Wang X,Wang G,et al.A novel INS/USBL/DVLintegrated navigation scheme against complex underwaterenvironmentJ.Ocean Engineering,2023,286:115485.17张兴学.基于因子图的多传感器信息融合导航算法研究D哈尔滨：哈尔滨工业大学，2 0 18.