融合先验与抗差估计的因子图多源融合定位方法及系统与流程

本发明涉及多传感器融合定位与导航，具体地说，涉及融合先验与抗差估计的因子图多源融合定位方法及系统。

背景技术：

1、随着无人车、无人船、无人机等自主移动平台向复杂作业场景不断拓展，在卫星信号遮挡严重、多径效应显著、环境扰动强烈的工况下，多源异构传感器融合定位成为保障高精度持续定位的核心技术路径。因子图优化作为多源信息融合的主流框架，凭借灵活的多约束建模能力与稀疏求解特性，在组合导航、同步定位与地图构建等领域得到广泛应用。

2、现有技术中，如发明专利cn202611034689.9公开了一种基于异构先验的因子图抗差边缘化定位方法，首先，在因子图滑动窗口推进期间，提取待边缘化节点在窗口驻留期的均方残差，获得历史综合观测置信度；其次，对待边缘化节点的故障状态进行判定，构建包含高斯分布先验或重尾分布先验的异构先验信息矩阵；然后，将异构先验信息矩阵作为后端优化求解的输入；最后，结合自适应因子完成系统全局状态的迭代解算，实现载体位姿的抗差更新并输出最终的定位结果。又如发明专利cn202411198252.x公开了一种基于因子图优化北斗/ins数据分步组合导航方法和装置，首先利用ins辅助北斗双差模糊度解算，得到整周双差模糊度固定解，利用无整周模糊度载波相位和多普勒频移观测值，与ins航位推算预测的“伪距”和“伪距率”观测值进行融合；基于滑动窗口因子图优化，构建北斗载波相位观测值残差因子、多普勒频移观测值残差因子和imu预积分，实现组合导航全局最优解。

3、综合上述两类方案可见，二者均以滑动窗口因子图优化为基础框架实现多源观测融合定位，前者聚焦边缘化过程的故障传递问题，通过异构先验信息抑制缓变故障的时序传递；后者聚焦卫星与惯性导航的融合精度问题，通过分步解算与多约束因子建模提升组合导航性能。但从整体技术路线来看，两类方案仍存在两方面核心技术局限：其一，抗差调整均依赖已产生的残差或观测结果进行后验被动修正，缺乏对环境干扰风险的前置预判与主动调控，在强干扰突发工况下存在抗差响应滞后的局限，且权重分配均未形成传感器先验质量、环境风险与后验残差的多维度协同调控体系，抗差精度与自适应能力不足；其二，滑动窗口边缘化过程中未针对异质传感器观测的量纲差异设置权重尺度一致性约束，长航时工况下定位误差易随边缘化进程累积放大，同时现有方案均未建立定位系统健康状态的量化评估与预警机制，难以直观反映定位结果的可靠性水平。基于此，如何提升因子图融合定位抗差机制的主动性与多维度协同调控精度，以及如何保障异质传感器边缘化过程的信息传递稳定性并实现定位可靠性的量化评估，是当前该领域亟待解决的两项技术问题。

技术实现思路

1、本发明的目的在于提供融合先验与抗差估计的因子图多源融合定位方法及系统，以解决上述背景技术中提出的因子图融合定位抗差机制被动滞后、缺乏多维度协同调控，以及异质传感器滑动窗口边缘化权重尺度失配、缺少定位可靠性量化预警的问题。

2、为实现上述技术问题的解决，本发明的目的之一在于，提供了融合先验与抗差估计的因子图多源融合定位方法，包括如下步骤：

3、s1、多源数据时空校准与前端质量预处理：采集自主平台多类异构传感器的观测数据，完成时间戳对齐与空间坐标统一，在因子图优化器外部完成观测质量增强处理，输出标准化观测数据、先验置信度与物理先验特征；

4、s2、因子图多约束观测建模：基于标准化观测数据，将待估计的系统状态量定义为因子图状态节点，将各类传感器观测抽象为连接对应节点的约束因子，构建多类型约束的因子图模型，确立加权残差代价最小化的优化目标；

5、s3、前馈后验协同的动态权重迭代求解：迭代前基于物理先验特征完成环境风险前馈预判并生成抗差参数基准；迭代中基于前馈预判结果与后验残差分布的一致性程度对抗差参数进行动态调整，结合多层级权重协同分配机制得到最终观测权重，通过迭代重加权最小二乘求解最优状态；

6、s4、尺度一致性滑动窗口边缘化：设定固定长度的滑动窗口以控制计算复杂度；对移出窗口的状态节点，基于最终观测权重施加权重尺度一致性约束，再通过舒尔补构建边缘化先验因子；

7、s5、系统健康度评估与融合状态输出：基于因子图信息矩阵计算节点平移脆弱性与系统健康度，监测系统健康度时序变化并触发定位可靠性预警，输出最优定位结果与系统健康状态。

8、作为本技术方案的进一步改进，所述s1中，观测质量增强处理具体包括：

9、通过特征提取网络提取观测数据中的特征点，并在特征提取网络输出层设置不确定性估计头，逐点输出各特征点对应的观测不确定性度量；

10、通过语义分割网络对观测数据进行像素级语义识别并生成动态干扰掩膜，基于所述动态干扰掩膜对提取的特征点进行筛选，剔除动态干扰掩膜覆盖区域内的动态特征并保留静态特征；

11、基于信号可用性指标、数据连续性指标与噪声特性指标评估各传感器的性能退化等级，输出取值区间为0至1的退化度评分，并基于退化度评分加权融合生成先验置信度；

12、提取传感器信号质量时序统计量与历史残差时序统计量作为物理先验特征；

13、对传感器历史残差进行概率分布拟合，建立残差幅值与协方差缩放系数的映射关系；所述观测不确定性度量与所述映射关系分别从逐点层面与分布层面对观测协方差进行调整。

14、作为本技术方案的进一步改进，所述s2中，基于标准化观测数据构建多类型约束因子并组成因子图模型，确立加权残差代价最小化的优化目标，包括以下步骤：

15、s2.1、将待估计的系统状态量定义为因子图状态节点，将观测质量增强处理后保留的静态特征位置定义为环境特征节点，两类节点共同构成因子图模型的节点集合；

16、s2.2、分别基于标准化观测数据中的惯性测量值、卫星定位观测值、点云配准结果，构建惯性预积分约束因子、绝对定位约束因子、相对观测约束因子，对应连接相邻时刻、单时刻、相邻或跨时刻的因子图状态节点，形成运动递推约束、全局位置约束与相对位姿约束；

17、s2.3、基于静态特征构建视觉重投影约束因子，连接因子图状态节点与环境特征节点，形成位姿与环境特征的重投影误差约束；

18、s2.4、根据标准化观测数据中的不确定性信息与先验置信度确定各约束因子的观测协方差，将全部约束因子的残差以对应观测协方差的逆矩阵为权重进行加权求和，构成加权残差代价最小化的优化目标。

19、作为本技术方案的进一步改进，所述s3中，前馈后验协同的动态权重迭代求解，包括以下步骤：

20、s3.1、迭代求解前，基于物理先验特征完成环境风险前馈预判，生成各约束因子的环境风险评分，并基于环境风险评分确定各约束因子的抗差参数基准；

21、s3.2、迭代求解过程中，基于当前迭代计算的后验残差分布，计算前馈预判结果与后验残差分布的一致性程度，基于一致性程度对抗差参数基准进行动态调整，得到融合抗差参数；

22、s3.3、结合先验置信度与融合抗差参数，通过多层级权重协同分配机制得到最终观测权重；

23、s3.4、基于最终观测权重，通过迭代重加权最小二乘更新系统状态量，迭代执行s3.2至s3.4直至满足收敛条件，输出最优状态。

24、作为本技术方案的进一步改进，所述s3.2中，基于前馈预判结果与后验残差分布的一致性程度对抗差参数基准进行动态调整，包括以下步骤：

25、s3.21、计算当前迭代中各约束因子的白化残差相对于当前窗口同类型约束因子白化残差中位数的偏离度，其中白化残差为各约束因子残差经其观测协方差归一化后的无量纲残差，将所述偏离度通过映射函数转换至0至1区间，偏离度越大则后验异常概率越高，得到各约束因子的后验异常概率；

26、s3.22、获取环境风险前馈预判输出的各约束因子环境风险评分，计算环境风险评分与后验异常概率的差值绝对值，将所述差值绝对值通过指数衰减函数转换为取值区间为0至1的一致性程度，环境风险评分与后验异常概率越接近则一致性程度越高；

27、s3.23、基于当前迭代白化后验残差分布的中位数绝对偏差，计算各约束因子对应的后验抗差参数，以一致性程度为权重对各约束因子的抗差参数基准与后验抗差参数进行加权融合得到融合抗差参数；一致性程度越高则融合抗差参数越趋近抗差参数基准，一致性程度越低则融合抗差参数越趋近后验抗差参数。

28、作为本技术方案的进一步改进，所述s3.3中，结合先验置信度与所述融合抗差参数通过多层级权重协同分配机制得到最终观测权重，包括以下步骤：

29、s3.31、以先验置信度作为第一层权重，将所述融合抗差参数通过负相关映射得到抗差映射权重作为第二层权重，融合抗差参数越大则抗差映射权重越低，基于抗差代价函数与当前迭代中各约束因子的残差计算后验等效权重作为第三层权重；

30、s3.32、将第一层权重、第二层权重与第三层权重分别归一化至0至1的统一权重空间；

31、s3.33、计算归一化后三层权重之间的离散度，基于所述离散度对三层权重进行共识合成得到共识权重：

32、离散度越小则三层权重判断越一致，共识权重趋近三层权重的均值；

33、离散度越大则三层权重判断越冲突，共识权重趋近三层权重中的最小值；

34、s3.34、将当前迭代的共识权重与上一轮迭代的最终观测权重进行动量平滑得到当前迭代的最终观测权重，首轮迭代时将上一轮迭代的最终观测权重初始化为1。

35、作为本技术方案的进一步改进，所述s3中，还包括抗差参数基准动态修正与最终观测权重保护，具体如下：

36、逐轮记录各约束因子的一致性程度并计算其在连续多轮迭代中的滑动均值，当滑动均值低于预设一致性阈值时，判定抗差参数基准存在系统性偏差并触发动态修正，根据前馈预判结果指示的风险程度与后验残差分布指示的异常程度确定修正方向：前馈预判结果指示的风险程度低于后验残差分布指示的异常程度时增大抗差参数基准，前馈预判结果指示的风险程度高于后验残差分布指示的异常程度时减小抗差参数基准，滑动均值越低则修正幅度越大，修正后的抗差参数基准用于后续迭代的动态调整；

37、通过多层级权重协同分配机制得到最终观测权重后，对最终观测权重设置权重下限，当最终观测权重低于所述权重下限时，将最终观测权重截断为权重下限。

38、作为本技术方案的进一步改进，所述s4中，对移出窗口的状态节点基于最终观测权重施加权重尺度一致性约束并通过舒尔补构建边缘化先验因子，包括以下步骤：

39、s4.1、设定固定长度的滑动窗口，仅保留滑动窗口内的因子图状态节点参与实时优化，当新的状态节点加入导致滑动窗口超出固定长度时，确定移出滑动窗口的状态节点；

40、s4.2、对移出滑动窗口的状态节点所关联的约束因子，基于最终观测权重以平方根形式对残差雅可比矩阵进行尺度缩放，使多类异构传感器约束因子的权重尺度保持一致；

41、s4.3、基于尺度缩放后的残差雅可比矩阵构建海森矩阵，并结合残差向量构建信息向量，对所述海森矩阵与信息向量进行舒尔补运算，得到边缘化先验因子的信息矩阵与残差向量；

42、s4.4、将所述边缘化先验因子作为先验约束加入因子图，等价保留移出滑动窗口的状态节点所承载的历史观测信息。

43、作为本技术方案的进一步改进，所述s5中，基于因子图信息矩阵计算节点平移脆弱性与系统健康度并进行融合状态输出，包括以下步骤：

44、s5.1、基于因子图信息矩阵的稀疏结构对不确定性传播进行显式建模，得到各因子图状态节点的位置协方差；

45、s5.2、根据所述位置协方差计算各因子图状态节点的节点平移脆弱性，所述节点平移脆弱性为该状态节点的位置协方差椭球体相对于预设基准协方差椭球体的相对膨胀率，用于表征节点定位可靠性的退化程度；

46、s5.3、对各因子图状态节点的节点平移脆弱性进行加权聚合后施加指数衰减映射得到系统健康度，节点平移脆弱性越高则系统健康度越低；

47、s5.4、逐滑动窗口记录系统健康度并监测其时序变化，当系统健康度持续下降时触发定位可靠性预警；

48、s5.5、输出最优定位结果与系统健康状态。

49、本发明的目的之二在于，提供了融合先验与抗差估计的因子图多源融合定位系统，用于执行上述的融合先验与抗差估计的因子图多源融合定位方法的步骤，包括：

50、时空校准与质量预处理单元，所述时空校准与质量预处理单元接收自主平台多类异构传感器的观测数据，并对观测数据进行时间戳对齐与空间坐标统一，在因子图优化器外部完成观测质量增强处理，得到标准化观测数据、先验置信度与物理先验特征；

51、约束建模与因子图构建单元，所述约束建模与因子图构建单元基于标准化观测数据，将待估计的系统状态量定义为因子图状态节点并将各类传感器观测抽象为连接对应节点的约束因子，构建多类型约束的因子图模型，并确立加权残差代价最小化的优化目标；

52、前馈预判与权重迭代单元，所述前馈预判与权重迭代单元迭代前基于物理先验特征完成环境风险前馈预判并生成抗差参数基准，并在迭代中基于前馈预判结果与后验残差分布的一致性程度对抗差参数进行动态调整，结合多层级权重协同分配机制得到最终观测权重，通过迭代重加权最小二乘求解最优状态；

53、窗口管理与边缘化构建单元，所述窗口管理与边缘化构建单元设定固定长度的滑动窗口以控制计算复杂度，并对移出窗口的状态节点基于最终观测权重施加权重尺度一致性约束，通过舒尔补构建边缘化先验因子；

54、健康度评估与状态输出单元，所述健康度评估与状态输出单元基于因子图信息矩阵计算节点平移脆弱性与系统健康度，并监测系统健康度时序变化以触发定位可靠性预警，输出最优定位结果与系统健康状态。

55、与现有技术相比，本发明的有益效果：

56、1.本发明通过基于物理先验特征完成环境风险前馈预判生成抗差参数基准，结合前馈后验一致性动态调整抗差参数与三层权重共识合成机制，解决了后验被动抗差响应滞后、权重调控维度单一的问题，实现了强干扰工况下主动式抗差与多维度精准权重调控；

57、2.本发明通过以最终观测权重平方根形式对残差雅可比矩阵进行尺度缩放，再经舒尔补构建边缘化先验因子，结合信息矩阵的节点平移脆弱性与系统健康度建模，解决了异质传感器边缘化尺度失配、定位可靠性无法量化预警的问题，实现了长航时定位误差抑制与定位状态可监测预警。