一种基于先验地图模糊帧匹配和退化场景结构化误差参数的多传感器可信融合定位方法

本发明公开一种基于先验地图模糊帧匹配和退化场景结构化误差参数的多传感器可信融合定位方法，属于多传感器融合定位与场景感知。

背景技术：

1、多传感器融合定位通过综合利用视觉、惯性、卫星导航、轮速、磁力和气压等多源观测信息，能够在多数场景下获得较单一传感器更高精度和更高鲁棒性的定位结果，因此已广泛应用于移动机器人、无人系统、智能车辆和复杂环境导航等领域。

2、然而，在实际应用中，多传感器融合定位的性能高度依赖于各传感器观测质量、误差模型以及场景条件。当系统进入隧道、地下通道、弱纹理区域、重复纹理区域、光照剧烈变化区域、强动态模糊区域、卫星信号受阻区域或磁场异常区域等退化场景时，不同传感器的误差特性会发生显著变化。例如，视觉观测可能因纹理不足、重复纹理、图像模糊或光照突变而匹配不稳定；卫星导航可能因遮挡或多路径效应而精度下降甚至不可用；磁力计可能因局部电磁干扰而出现异常； 轮速计可能因打滑而误差增大。若仍采用固定不变的误差模型、固定噪声矩阵以及固定传感器权重进行融合，容易导致定位结果精度下降、稳定性变差，甚至出现明显失真。

3、现有一类方法依赖高精度先验地图，通过地图帧识别、特征匹配、重定位和全局因子图优化实现精确的融合定位。但在退化场景中，地图帧识别和特征匹配往往不稳定，可靠的重定位难以触发。此外，现有先验地图通常仅作为位置参考，尚缺乏一种将先验地图作为结构化误差信息载体，并于在线阶段通过可靠的退化场景识别和误差信息调用以驱动可信融合的技术方案。

4、因此，如何在不依赖精确单帧重定位的条件下，通过先验地图鲁棒识别当前退化场景，并利用与该退化场景对应的结构化误差参数动态调节多传感器融合中的误差模型、噪声矩阵、可用性和置信度，从而实现退化场景下的可信融合定位，是本发明要解决的技术问题。

技术实现思路

1、本发明的目的在于提供一种基于先验地图模糊帧匹配和退化场景结构化误差参数的多传感器可信融合定位方法，以解决现有技术中，现有多传感器融合定位方法在退化场景中难以可靠识别特定地图帧并进行重定位，从而导致融合定位结果误差漂移的问题。

2、一种基于先验地图模糊帧匹配和退化场景结构化误差参数的多传感器可信融合定位方法，包括：

3、s1、获取先验地图，将先验地图划分为正常场景段和退化场景段，对正常场景段绑定正常场景标签，对退化场景段绑定退化场景标签，为每个地图帧绑定场景段索引；

4、s2、对每一种退化场景标签构建对应的结构化误差参数集合；

5、s3、利用不同退化场景标签对应的结构化误差参数，构建场景参数表；

6、s4、在线运行多传感器定位系统，输出不同传感器的观测信息和图像帧；利用当前连续若干在线帧与先验地图中的地图帧进行匹配，根据匹配到的地图帧绑定的退化场景标签识别当前所处退化场景；

7、s5、根据识别出的退化场景标签获取对应的结构化误差参数，不同场景段切换时，将当前采用的配置参数切换为与当前场景对应的配置参数；

8、s6、利用结构化误差参数，调节多传感器融合定位过程中的各项配置参数，执行多传感器融合定位；

9、s7、输出可信融合最优估计、对应的可信度指标和各传感器可用性状态。

10、s1包括，先验地图包括若干地图帧、特征点集、特征描述符号。

11、s2包括，对每一种退化场景标签构建对应的结构化误差参数集合：

12、；

13、式中，标签为的退化场景对应的结构化误差参数集合，里程计校正因子，系统误差模型参数，过程噪声矩阵或其调节参数，观测噪声矩阵或其调节参数，传感器可用性标记或状态向量，置信度权重参数，场景复杂程度或融合结果可靠性参数。

14、s3包括，场景参数表中的每一项对应一种退化场景标签或一个退化场景段索引，并存储一份对应的。

15、s4包括，取当前连续若干在线图像帧构成在线窗口，对在线窗口的图像提取特征描述信息，并与先验地图中的地图帧进行相似性计算，获得与各在线帧对应的候选地图帧，并读取各候选地图帧绑定的退化场景标签，相似性计算采用词袋模型；

16、若连续个在线帧对应的候选地图帧绑定同一退化场景标签，且退化场景标签一致性满足预设的连续性时长或帧数阈值，判定当前场景为退化场景标签对应的场景。

17、s5包括，若连续个在线帧对应的候选地图帧绑定同一退化场景标签，读取退化场景标签绑定的；

18、由正常场景切换至某一退化场景，由一种退化场景切换至另一种退化场景或由退化场景返回正常场景时，将当前采用的参数切换为与当前场景对应的参数。

19、s6包括，利用更新过程噪声矩阵，利用更新观测噪声矩阵，根据控制各观测的参与状态，依据和设置各因子在目标函数中的权重和协方差。

20、s7包括，输出可信融合最优估计：

21、；

22、式中，为待估计状态，为第类传感器观测或第个融合因子的残差项，为对应残差项的协方差矩阵，为欧几里得范数，为因子数；

23、输出与对应的可信度指标和各传感器可用性状态。

24、相对比现有技术，本发明具有以下有益效果：本发明通过先验地图模糊帧匹配识别在线帧对应的退化场景标签，并通过连续多个在线帧标签的一致性判定当前所处场景，从而避免了退化环境中单帧精确匹配不稳定、难以依赖可靠重定位抑制误差漂移的问题，提高了退化场景定位的鲁棒性和精度；在先验地图中为退化场景路段内的每一地图帧绑定退化场景标签，使在线模糊帧匹配结果能够直接转化为场景标签识别结果，有利于通过连续帧统计实现稳定的场景级识别；将结构化误差参数与退化场景标签或场景段索引建立绑定关系，并按标签或场景段统一存储一份，而非在每一地图帧中重复存储，从而降低了数据冗余，提高了参数一致性，便于后续更新和维护；通过结构化误差参数统一组织多定位源或传感器的校正因子、误差模型、噪声矩阵、可用性、置信度和状态信息，使退化场景下的多源误差特性能够以结构化方式表达，并直接服务于融合定位过程；不采用固定误差模型和固定权重，而是根据识别出的退化场景标签动态调用对应结构化误差参数，进而调节多传感器融合中的误差模型、协方差、传感器权重和启用状态，从而提升融合结果的可信性和稳定性；不仅适用于扩展卡尔曼滤波、误差状态卡尔曼滤波等滤波框架，也适用于因子图优化、滑动窗口优化等优化框架，具有较好的通用性和工程适用性。

1.一种基于先验地图模糊帧匹配和退化场景结构化误差参数的多传感器可信融合定位方法，其特征在于，包括：

2.根据权利要求1所述的一种基于先验地图模糊帧匹配和退化场景结构化误差参数的多传感器可信融合定位方法，其特征在于，s1包括，先验地图包括若干地图帧、特征点集、特征描述符号。

3.根据权利要求2所述的一种基于先验地图模糊帧匹配和退化场景结构化误差参数的多传感器可信融合定位方法，其特征在于，s2包括，对每一种退化场景标签构建对应的结构化误差参数集合：

4.根据权利要求3所述的一种基于先验地图模糊帧匹配和退化场景结构化误差参数的多传感器可信融合定位方法，其特征在于，s3包括，场景参数表中的每一项对应一种退化场景标签或一个退化场景段索引，并存储一份对应的。

5.根据权利要求4所述的一种基于先验地图模糊帧匹配和退化场景结构化误差参数的多传感器可信融合定位方法，其特征在于，s4包括，取当前连续若干在线图像帧构成在线窗口，对在线窗口的图像提取特征描述信息，并与先验地图中的地图帧进行相似性计算，获得与各在线帧对应的候选地图帧，并读取各候选地图帧绑定的退化场景标签，相似性计算采用词袋模型；

6.根据权利要求5所述的一种基于先验地图模糊帧匹配和退化场景结构化误差参数的多传感器可信融合定位方法，其特征在于，s5包括，若连续个在线帧对应的候选地图帧绑定同一退化场景标签，读取退化场景标签绑定的；

7.根据权利要求6所述的一种基于先验地图模糊帧匹配和退化场景结构化误差参数的多传感器可信融合定位方法，其特征在于，s6包括，利用更新过程噪声矩阵，利用更新观测噪声矩阵，根据控制各观测的参与状态，依据和设置各因子在目标函数中的权重和协方差。

8.根据权利要求7所述的一种基于先验地图模糊帧匹配和退化场景结构化误差参数的多传感器可信融合定位方法，其特征在于，s7包括，输出可信融合最优估计：