一种基于多传感器融合的SLAM位姿估计系统及方法

本技术属于机器人定位与三维感知，具体涉及一种基于多传感器融合的slam位姿估计系统及方法。

背景技术：

1、随着无人系统、移动机器人及边缘智能设备的快速发展，复杂环境下的高精度定位与建图需求日益增长。然而，单一传感器在实际应用中均存在明显局限性：激光雷达虽具有较高几何测量精度，但对语义与纹理信息缺乏感知能力，且在结构退化场景下易出现匹配不稳定问题；单目视觉传感器能够提供丰富的环境纹理信息，但存在尺度不可观测及对光照、运动模糊敏感的问题；imu能够提供高频运动信息，但由于积分误差累积，长期运行易产生显著漂移。因此，如何实现多源异构传感器之间的有效融合成为提升slam系统鲁棒性的关键问题。现有方法中，如fast-lio2等激光惯性紧耦合框架虽可实现高频稳定定位，但缺乏全局视觉约束能力；视觉slam方法（如orb-slam、pyslam等）虽具备较强的全局一致性表达能力，但在弱纹理或动态场景中易发生跟踪失败。因此，现有技术难以在复杂环境中同时兼顾全局一致性与局部稳定性，亟需一种能够基于多源观测质量自适应调节融合权重的统一slam位姿估计方法与系统。

技术实现思路

1、本技术针对现有slam系统在复杂动态环境中存在的多源观测时序一致性不足、激光雷达与视觉传感器在退化场景下可靠性变化明显、融合权重难以随观测质量自适应调整，以及边缘计算设备算力和存储资源受限等问题，提出一种基于多传感器融合slam位姿估计方法及系统。该方法通过激光雷达惯性里程计、单目视觉里程计、深度学习视觉里程计以及两级扩展卡尔曼滤波融合结构，实现连续、鲁棒且具有公制尺度的位姿估计输出。

2、一种基于多传感器融合的slam位姿估计系统，包括多传感器数据采集模块m1、激光雷达惯性里程计模块m2、视觉里程计估计模块m3、fast3r深度学习视觉里程计模块m4、第一阶段位姿融合模块m5、最终融合模块m6；

3、多传感器数据采集模块：获取第帧激光雷达点云、imu观测和单目图像；

4、激光雷达惯性里程计模块m2：根据和估计，输出激光雷达惯性里程计lio匹配质量信息，并引入的冻结恢复机制；

5、视觉里程计估计模块m3：根据单目视觉里程计或视觉slam模型获得单目图像估计单目视觉位姿，并根据视觉跟踪状态生成视觉位姿协方差和视觉状态码；

6、fast3r深度学习视觉里程计模块m4：根据相邻或间隔图像帧进行跨帧多视图几何推理，估计fast3r帧间相对位姿，累计得到fast3r视觉里程计输出位姿，并输出fast3r综合置信度、位姿协方差和状态码；

7、第一阶段位姿融合模块m5：对、和进行时间同步、单目尺度恢复、观测视觉综合置信度评估和异常状态判断，得到第一阶段融合位姿；

8、最终融合模块m6：对和进行时间同步、fast3r尺度恢复、增量一致性筛选和ekf辅助融合，得到最终位姿。

9、优选的，在正常状态下保存上一可信lio状态和上一可信点云签名，当满足雷达输入超时、有效点数量低于阈值、点云平均距离低于阈值、匹配残差超过阈值、协方差尺度超过阈值或者点云签名突变中的至少一种条件时，停止使用当前点云更新状态和局部地图，并持续输出上一可信位姿；

10、冻结期间，记录冻结开始时的加速度参考值，并在冻结期间持续计算加速度变化量、加速度变化积分、角速度积分、峰值加速度、imu加速度噪声阈值、imu原始加速度矢量、imu角速度矢量、相邻两个imu采样点之间的时间间隔：

11、；

12、；

13、；

14、；

15、若满足、或中的任一条件，则认为冻结期间发生了真实运动，并记为；否则记为；其中为平移或姿态变化阈值、为旋转阈值、为瞬间运动阈值。

16、优选的，在冻结状态下，激光雷达惯性里程计模块m2持续检查新到来的雷达点云是否恢复正常：恢复判断条件包括当前点云有效点、平均距离满足阈值、点云签名回到合理范围，并且连续满足一定数量的恢复帧：

17、；

18、；

19、；

20、；

21、其中为当前点云签名；为上一帧可信点云签名；为上述两帧点云签名的差异；、、、分别为点云有效点恢复阈值、平均距离阈值、点云签名差异最小允许范围、有效恢复帧数量阈值；

22、当检测到lio恢复后，激光雷达惯性里程计模块m2将为0，并在中输出冻结期间是否发生运动的标志；

23、第一阶段位姿融合模块m5据此采取不同处理策略：若=0，则继续使用冻结前的上一可信位姿并恢复正常融合；若=1且视觉连续帧可信，则对冻结期间缓存的视觉位姿进行回放，补偿冻结段运动；若=1但视觉帧不可信，则不强行使用视觉轨迹，而是在lio恢复后将lio坐标重新锚定到当前融合轨迹。

24、优选的，视觉里程计估计模块m3基于单目图像序列进行帧间位姿估计，并支持vo模式与slam模式；在vo模式下，视觉里程计估计模块m3采用轻量级特征提取与匹配方法对连续图像帧进行处理，通过构建帧间特征对应关系，并利用本质矩阵分解或pnp方法求解相机相对运动，从而快速输出视觉位姿变化；在slam模式下，视觉里程计估计模块m3在vo基础上引入关键帧管理、局部地图跟踪、回环检测或全局图优化机制；

25、视觉里程计估计模块m3输出的视觉位姿消息中包含位姿协方差和视觉状态码，其中视觉状态码用于区分完整slam可信帧、视觉前端fallback帧和失败帧。

26、优选的，fast3r深度学习视觉里程计模块m4接收第k帧图像，先将ros2图像消息转换为opencv图像，再根据相机畸变模型进行去畸变处理，得到；若为第一帧，则m4仅保存该图像作为参考帧，不输出有效帧间运动；对于后续图像，m4根据预设计算间隔m进行跳帧控制；若当前帧未满足计算间隔，则m4不进行深度推理，而是发布保持当前fast3r位姿的低优先级里程计消息，并在状态码中标记为跳帧状态；

27、当满足计算间隔时，m4将上一帧去畸变图像与当前去畸变图像构成图像对，并将该图像对、相机内参k以及图像真实尺寸信息输入fast3r模型，以获得跨帧多视图几何预测结果，预测结果至少包括每一视图在另一视图坐标系下的三维点预测以及对应的置信度图；根据三维点预测和对应像素坐标，利用pnp算法求解相机帧间相对位姿，并累计得到。

28、优选的，m4将模型置信度和pnp位姿质量组合为fast3r综合置信度：

29、；

30、；

31、；

32、；

33、为置信度统计量，为预设置信度参考值，表示将数值限制在给定区间内，为高置信像素比例，为有效像素比例，为中值重投影误差，为重投影误差参考值，和分别为内点比例和内点数量参考值，为pnp内点比例，为内点数量；

34、根据两帧pnp得到的相机位姿和，m4计算fast3r帧间相对位姿：

35、；

36、若中的数值均为有限值，并且综合置信度大于fast3r最低置信度阈值，或者允许低置信帧积分，则m4将全局fast3r位姿更新为：

37、。

38、优选的，第一阶段融合时，对和进行时间戳对齐，对于单目视觉尺度不确定的情形，利用lio公制轨迹与单目视觉轨迹之间的同步位姿对计算尺度候选，并通过范围约束、中位数估计和异常值剔除得到单目尺度因子，将单目视觉平移恢复为公制尺度；然后根据视觉位姿协方差和视觉状态码判断单目视觉观测是否可信；

39、根据判断激光雷达惯性里程计是否冻结、退化或失效；系统构建点云签名，通过当前点云签名与参考点云签名之间的归一化加权差异判断点云结构是否发生突变；在冻结期间，根据imu加速度变化积分、角速度积分和峰值加速度判断设备是否发生真实运动，并在雷达观测恢复后根据该判断结果选择保持冻结位姿、视觉补偿或者重新锚定lio轨迹；

40、当激光雷达惯性里程计和单目视觉观测均可信时，将作为主观测、将尺度恢复后的作为辅助观测进行扩展卡尔曼滤波融合，得到第一阶段融合位姿；当单目视觉观测不可信时，退化为激光雷达惯性里程计单源更新；当激光雷达惯性里程计失效且单目视觉观测可信时，利用单目视觉观测进行桥接或保持上一可信融合状态。

41、优选的，最终融合时，不直接融合和的绝对位姿，而是分别计算第一阶段融合位姿增量和fast3r位姿增量；利用与的平移距离比值估计fast3r尺度因子，并对中的平移部分进行尺度恢复，得到尺度恢复后的fast3r位姿增量；当所述尺度恢复后的fast3r位姿增量满足置信度、尺度范围、平移跳变、旋转跳变以及与的一致性约束时，将其确定为可参与融合的fast3r辅助增量观测，否则拒绝该fast3r辅助增量观测。

42、优选的，最终融合采用扩展卡尔曼滤波融合方式进行状态更新；最终融合模块维护包含三维位置和四元数姿态的最终融合状态，以上一时刻最终融合位姿为基准，分别根据生成第一阶段融合候选位姿，并根据尺度恢复后的fast3r位姿增量生成fast3r候选位姿；所述最终融合模块先以第一阶段融合候选位姿作为主观测进行扩展卡尔曼滤波更新，当fast3r辅助增量观测通过筛选时，再以fast3r候选位姿作为辅助观测进行二次扩展卡尔曼滤波更新；当fast3r辅助增量观测未通过筛选时，仅使用第一阶段融合候选位姿更新最终融合状态。

43、一种基于多传感器融合的slam位姿估计方法，包括以下步骤：

44、（1）多传感器数据采集；采集激光雷达点云、imu观测和单目图像；

45、（2）激光雷达惯性里程计估计：利用激光雷达惯性里程计输出位姿，并输出lio匹配质量信息；并引入的冻结恢复机制；

46、（3）视觉里程计估计：根据单目视觉里程计或视觉slam模型获得单目图像估计单目视觉位姿，并根据视觉跟踪状态生成视觉位姿协方差和视觉状态码；

47、（4）深度学习视觉里程计估计：根据相邻或间隔图像帧进行跨帧多视图几何推理，估计fast3r帧间相对位姿，累计得到fast3r视觉里程计输出位姿，并输出fast3r置信度、位姿协方差和状态码；

48、（5）第一阶段位姿融合步骤：对、和进行时间同步、单目尺度恢复、观测置信度评估和异常状态判断，得到第一阶段融合位姿；

49、（6）最终位姿融合：对和进行时间同步、fast3r尺度恢复、增量一致性筛选和扩展卡尔曼滤波辅助融合，得到最终融合位姿。

50、与现有技术相比，本技术有益效果如下：

51、本技术首先通过多源异构传感器同步采集模块获取激光雷达点云、imu观测和单目图像，并基于时间戳和ros2通信机制对不同频率的数据流进行统一管理。各处理环节仅订阅自身所需数据，并通过标准化消息输出位姿、协方差、状态码和质量指标，从而形成解耦式异步运行结构。通过时间同步队列和近似同步策略，系统能够为后续激光雷达惯性里程计、视觉里程计、fast3r视觉里程计以及融合过程提供时序一致的输入。

52、在激光雷达惯性里程计方面，系统可基于现有fast-lio2框架实现点云预处理、imu辅助点云去畸变、局部地图维护、点到平面残差构建以及迭代状态估计，从而获得高频、低漂移的激光雷达惯性里程计位姿。本技术并不以fast-lio2基础框架本身作为改进点，而是在该框架基础上增加lio匹配质量信息输出机制。所述匹配质量信息包括匹配残差、有效点数量、点云平均距离、协方差尺度、冻结状态、点云签名差异以及imu运动统计量等，用于后续退化判断、冻结恢复和融合权重调节。

53、为提高激光观测异常情况下的鲁棒性，本技术进一步提出基于观测质量的lio冻结与恢复机制。系统在正常运行时持续保存上一可信lio状态和上一可信点云签名；当检测到雷达输入超时、点云为空或有效点过少、点云平均距离异常、匹配残差过大、协方差尺度异常或者点云签名突变时，暂停异常点云对状态和局部地图的更新，并持续输出上一可信位姿，避免错误激光观测污染轨迹和地图。在冻结期间，系统利用imu加速度变化积分、角速度积分和峰值加速度判断设备是否发生真实运动；当雷达观测恢复后，根据冻结期间运动判断结果选择保持冻结位姿、利用视觉轨迹补偿冻结段运动，或者将恢复后的lio轨迹重新锚定到当前融合轨迹附近，从而降低恢复瞬间的轨迹跳变

54、在单目视觉里程计方面，系统可基于现有pyslam封装的视觉里程计或视觉slam算法实现特征提取、特征匹配、相对位姿估计、关键帧跟踪或局部优化。本技术在该视觉算法输出的基础上进一步生成视觉位姿协方差和视觉状态码，用于区分可信帧、fallback帧和失败帧。针对单目视觉天然存在的尺度不确定问题，系统利用lio公制轨迹与单目视觉轨迹之间的同步位姿对计算尺度候选，并通过范围约束、中位数估计和异常值剔除得到单目尺度因子，使单目视觉平移恢复到公制尺度后参与第一阶段融合。

55、系统引入基于现有fast3r模型的深度学习视觉里程计处理。该处理按照预设计算间隔选取上一去畸变图像和当前去畸变图像组成图像对，并将图像对、相机内参和图像尺寸信息输入fast3r模型，获得跨帧三维点预测和置信度图。本技术在fast3r模型输出基础上进行工程化位姿构建：对置信度图进行对数归一化和鲁棒统计，得到模型置信度；根据三维点预测和对应像素坐标构造pnp问题，统计内点比例、内点数量和重投影误差，得到pnp位姿质量；再将模型置信度和pnp位姿质量组合为fast3r综合置信度，并据此构造fast3r位姿协方差和状态码。通过上述方式，fast3r输出不直接作为不可区分可靠性的位姿结果，而是作为具有置信度和诊断信息的视觉几何约束。

56、在第一阶段融合中，系统将激光雷达惯性里程计输出位姿和尺度恢复后的单目视觉位姿进行时间同步和置信度筛选，并采用扩展卡尔曼滤波维护第一阶段融合状态。融合时，以激光雷达惯性里程计观测作为主约束，以单目视觉观测作为辅助约束；当视觉观测处于fallback状态时，增大其观测噪声使其仅作为弱约束参与融合；当视觉观测失败时，系统退化为lio-only更新；当lio冻结或退化而视觉连续可信时，系统利用视觉观测进行短时桥接或保持上一可信融合状态。由此获得第一阶段融合位姿，为后续fast3r最终融合提供稳定的公制参考。

57、在最终融合中，系统不直接融合第一阶段融合位姿和fast3r位姿的绝对值，而是分别计算二者在相邻同步时刻之间的位姿增量。系统利用第一阶段融合增量的公制平移距离对fast3r增量进行尺度恢复，并对尺度恢复后的fast3r增量进行置信度、尺度范围、平移跳变、旋转跳变以及与第一阶段融合增量的一致性筛选。通过筛选的fast3r增量作为辅助观测参与最终扩展卡尔曼滤波更新；未通过筛选时，系统仅根据第一阶段融合增量更新最终状态。该两级融合结构使传统视觉里程计、激光惯性里程计和fast3r深度视觉几何约束能够分层协同，避免低置信深度模型输出直接影响主定位链路。

58、在边缘设备实现方面，本技术采用轻量化、解耦式架构，各功能环节通过ros2话题进行异步交互，可根据设备算力选择cpu或gpu执行fast3r推理，并通过图像缓存、跳帧推理、低置信降权和断流回退机制降低深度模型对实时主链路的影响。当fast3r模型未启用、加载失败、推理延迟过大或图像流异常时，系统仍可由第一阶段融合链路持续提供最终位姿输出，从而保证系统在资源受限平台上的连续运行能力。

59、在轨迹输出方面，系统将最终融合结果发布为ros2标准位姿、里程计和路径消息，并可将轨迹保存为标准轨迹文件，用于在线导航、可视化显示和离线精度评估。系统还可以根据路径下采样率和输入空闲状态控制轨迹发布与文件刷新，以适应长时间运行和批量实验场景。

60、综上，本技术在现有fast-lio2、pyslam和fast3r等基础框架或模型之上，重点提出了lio匹配质量输出与冻结恢复、单目视觉尺度恢复、fast3r置信度与pnp质量评价、两级置信度驱动扩展卡尔曼滤波融合、fast3r增量尺度恢复和断流回退等机制。通过上述技术方案，系统能够在雷达退化、视觉退化、动态干扰、深度模型低置信以及边缘设备算力受限等情况下保持连续、鲁棒且具有公制尺度的slam位姿估计结果。