一种面向全工况的多源融合定位自适应权重分配方法及系统与流程

本发明涉及智能驾驶与高精度定位，尤其涉及一种面向全工况的多源融合定位自适应权重分配方法及系统。

背景技术：

1、随着智能驾驶技术从l2辅助驾驶向l3有条件自动驾驶和l4高度自动驾驶演进，车辆对高精度定位的依赖日益增强。l3+智驾对定位的硬性要求包括：绝对精度≤20cm、可用率≥99.9%、更新频率≥100hz、完备性输出保护级pl以及asil-b功能安全等级。单一传感器无法满足上述全部要求：gnss在城市峡谷、隧道、地下停车场等场景信号受限；imu长时间独立工作会产生累积漂移；视觉在弱光照、弱纹理场景退化；激光雷达在空旷走廊或开阔场地因特征稀疏而退化；轮速在打滑或abs激活时失真。因此，多源传感器紧耦合融合成为高精定位的必然选择。

2、目前，主流的多源融合定位方案可分为以下几类：

3、第一类是以imu为中心的多模态因子图融合方案。例如cn118258378a公开了一种融合gnss信号的多模态高精度鲁棒位姿估计方法和系统，结合gnss、imu、激光雷达与相机多源数据，以imu为中心构建多模态融合框架，通过雷达-惯性里程计（lio）、视觉-惯性里程计（vio）与gnss、imu进行联合因子图优化。然而，该方案中因子图各源权值（噪声协方差）为经验固定取值，未对各传感器实时数据质量做连续、归一化的可信度建模；未纳入轮速里程计，缺少对车轮打滑等地面约束的利用与异常检测；缺少跨源一致性检验机制，难以发现gnss-rtk伪固定、激光退化、视觉弱纹理等单个传感器内部指标无法暴露的故障。

4、第二类是基于环境分类切换流水线的多源融合方案。例如cn121521139a公开了一种多源数据融合的车辆监控与定位数据处理方法，获取gnss、imu及里程计数据，构建含卫星数量、载噪比统计量与垂直加速度振动能量的四维特征向量，通过决策树分类器实时识别行驶环境状态，并依状态动态激活紧耦合、松耦合、航位推算或带振动补偿的增强型融合流水线。然而，该方案以离散环境类别驱动流水线硬切换，缺乏对各源连续可信度的细粒度刻画；未引入激光雷达与视觉作为独立观测源参与紧耦合；分类器依赖离线训练，对未见过工况的泛化能力有限。

5、第三类是基于故障检测与自适应噪声的因子图定位方案。例如cn115265551a公开了一种无人驾驶复杂场景下的多传感器融合定位方法和系统，构建由imu预积分、gnss与激光雷达里程计构成的因子图模型，利用故障检测函数对gnss做故障判定，并在gnss故障时仅用imu预积分与激光雷达里程计构建因子图。然而，该方案的故障检测主要面向gnss单一来源，未形成覆盖全部传感器的统一可信度模型；自适应噪声仅针对激光雷达里程计，未将轮速、视觉等源纳入统一自适应框架。

6、第四类是基于北斗多源定位数据融合与建图方案。例如cn122486640a公开了一种基于北斗的多源定位数据融合方法及系统，对北斗原始观测数据预处理与解算滤波得到优化北斗定位结果，对激光雷达点云与imu原始数据分别构建激光雷达特征点、imu预积分约束与相邻时刻轮式里程计相对位姿增量，经时空同步与坐标统一后构建因子图并非线性迭代优化。然而，该方案因子图优化中多源权值未随实时数据质量动态自适应；未建立对各源的在线可信度估计与跨源一致性检验。

7、综上，现有技术存在以下共性缺陷：缺乏对全部传感器统一、连续、归一化的可信度建模，各传感器权重或噪声多依赖经验固定值、离散环境分类或离线训练的模型；缺少跨源一致性检验，难以发现轮速打滑、rtk误固定、激光退化、视觉弱纹理等单源内部指标正常但跨源冲突的隐性故障；前端高频递推与后端全局优化常被割裂设计，二者未共用同一组自适应协方差；卫星定位失锁后缺少依据各源实时退化程度的场景自适应源提升与平滑重收敛机制。

技术实现思路

1、本部分的目的在于概述本发明的实施例的一些方面以及简要介绍一些较佳实施例，在本部分以及本技术的说明书摘要和发明名称中可能会做些简化或省略以避免使本部分、说明书摘要和发明名称的目的模糊，而这种简化或省略不能用于限制本发明的范围。

2、鉴于现有的多源融合定位方案中，传感器权重固定、缺乏跨源一致性检验、实时性与全局一致性割裂、以及卫星定位失锁后缺乏动态调整与平滑重收敛机制等问题，提出了本发明。

3、因此，本发明所要解决的问题在于如何对全部传感器进行统一、连续、归一化的可信度建模，如何通过跨源一致性检验有效识别轮速打滑、rtk误固定、激光退化、视觉弱纹理等单源内部指标无法暴露的隐性故障，如何使前端高频递推与后端全局优化共用同一组自适应协方差以保证结果一致性，以及如何在卫星定位失锁或弱信号期间依据各源实时退化程度动态调整权重并实现恢复后的平滑重收敛。

4、为解决上述技术问题，本发明提供如下技术方案：

5、第一方面，本发明实施例提供了一种面向全工况的多源融合定位自适应权重分配方法，其包括，

6、以统一的时间基准同步采集多种异构定位传感源的多源融合数据；

7、基于多源融合数据，对各传感源数据分别计算源内质量指标与跨源一致性指标，融合得到归一化信任分数；

8、将归一化信任分数通过单调递减映射函数映射为各传感源的自适应量测噪声协方差，并以前端误差状态卡尔曼滤波器与后端滑窗因子图优化器双引擎协同方式执行多源紧耦合融合；

9、在卫星定位失锁或弱信号场景下，根据各传感源的实时退化程度动态调整可用源权重，实现无缝桥接与恢复后的平滑重收敛；

10、当相关传感源的归一化信任分数持续高于标定阈值时，触发在线外参与时间偏移联合标定并回灌融合。

11、第二方面，本发明实施例提供了一种面向全工况的多源融合定位自适应权重分配系统，其包括：

12、多源数据采集模块，用于以统一时间基准同步采集多源融合数据；

13、多源可信度估计模块，包括源内质量评估子模块、跨源一致性检验子模块和信任分数融合子模块，基于多源融合数据，对各传感源数据分别计算源内质量指标与跨源一致性指标，并将二者融合为归一化信任分数；

14、自适应融合引擎模块，用于将归一化信任分数映射为各传感源的自适应量测噪声协方差，并以双引擎协同方式执行多源紧耦合融合，输出车辆位姿、速度与姿态估计结果；

15、场景自适应控制模块，用于在卫星定位失锁或弱信号场景下，根据各传感源的实时退化程度动态调整可用源的权重分配，并在卫星定位信号恢复后执行平滑重收敛。

16、第三方面，本发明实施例提供了一种计算机设备，包括存储器和处理器，存储器存储有计算机程序，其中：处理器执行计算机程序时实现上述的面向全工况的多源融合定位自适应权重分配方法的任一步骤。

17、第四方面，本发明实施例提供了一种计算机可读存储介质，其上存储有计算机程序，其中：计算机程序被处理器执行时实现上述的面向全工况的多源融合定位自适应权重分配方法的任一步骤。

18、与采用固定权重或者仅依据单个传感源内部质量进行权重配置的多源定位方法相比，本发明具有如下有益效果：

19、1、通过分别计算各异构定位传感源的源内质量分和跨源一致性分，并将二者统一转换为取值位于[0,1]区间的归一化信任分数，使不同种类定位传感源在相同尺度下具有连续的可信程度表征，定位源权重能够随实时数据质量变化而连续变化。

20、2、通过视觉里程计、激光里程计与轮速里程计之间的位移一致性检验，以及rtk固定解、激光雷达建图位置与高精地图匹配位置之间的位置一致性检验，可对轮速打滑和rtk误固定等单源内部指标难以直接反映的异常状态进行识别；通过视觉纹理和激光雷达结构特征之间的互校，可进一步反映环境退化对视觉源和激光雷达源的影响。

21、3、通过将归一化信任分数统一映射为自适应量测噪声协方差，并由前端误差状态卡尔曼滤波器和后端滑窗因子图优化器共同采用自适应量测噪声协方差，使前端实时状态更新和后端滑窗优化对于同一定位传感源采用一致的可信程度依据。

22、4、在卫星定位失锁或弱信号场景下，根据激光雷达、视觉和轮速里程计的实时退化程度进一步调整相应的归一化信任分数，使失锁期间的融合权重与剩余定位源当前可用程度相对应；卫星定位恢复后通过滑窗因子图加入恢复后的位置约束，对失锁期间的位姿轨迹进行平滑修正，降低卫星定位恢复瞬间产生的位置不连续。

23、5、在多个相关定位传感源处于较高可信状态并持续达到预设时长时，将传感源外参及时间偏移加入后端因子图进行联合估计，并将标定结果回灌至前端和后端融合过程，可对车辆振动以及温度变化等因素引起的长期外参和时间偏移变化进行修正。