[发明专利]面向大规模SLAM的多传感器融合与数据管理方法

摘要

本发明公开了一种面向大规模SLAM的多传感器融合与数据管理方法。目的是面向大规模环境下，提升SLAM的精度、鲁棒性、数据存取效率。技术方案是：首先利用视觉、惯性测量单元、里程计等多种传感器信息融合的方法，形成优势互补，增强SLAM框架中的线性图优化环节；并提出一种数据管理机制，其能实现高效的数据存取机以支撑SLAM系统的实时性；基于上述多传感器信息融合策略与数据管理机制，采取集中式拓扑架构，由n个机器人节点和一台服务器构成协同SLAM系统，有效应对未知的大规模环境下探索任务。

主权项

1.面向大规模SLAM的多传感器融合与数据管理机制，其特征在于包括以下步骤：/n第一步：每台机器人独立运行基于VINS-Mono改进的SLAM框架；/n改进的具体步骤如下：/n1.1测量处理前端，保留基于视觉的检测跟踪、IMU预积分，并新增里程计原始数据接收与缓存，增加三种传感器信息的数据对齐处理；/n1.1.1接收图像，提取Harris角点，利用金字塔光流跟踪相邻帧，通过RANSAC去除异常点，进一步计算得到相机归一化平面下的特征点云，然后缓存特征点云数据；/n1.1.2接收缓存IMU的数据，并进行预积分；/n1.1.3接收缓存里程计数据；/n1.1.4完成特征点云、IMU、里程计的数据对齐，保证相邻的特征点数据帧之间有完整的IMU数据，且每帧特征点数据帧能对应一条时间戳临近的里程计的数据，两者时间戳之差的绝对值不超过预先设定的阈值detal_t；/n1.2非线性图优化，保留原有的视觉信息、IMU预积分信息，并新增里程计信息，通过三种信息联合优化求解滑窗内的状态；/n1.2.1首先构建一个容量为m的滑动窗口，滑动窗口的每个单元保留原有的视觉信息、IMU预积分信息，并增加对一条步骤1.1.4所述的数据对齐的里程计信息；/n1.2.2基于滑动窗口的第t-1、第t个单元中的里程计信息，计算机器人自滑窗内第t-1至第t单元对应的时间段的航偏角，记为odom_yaw，与滑窗中第t个单元的IMU预积分信息中的航偏角imu_yaw比较，若两者之差的绝对值在预先设定的阈值detal_yaw的范围内，则认为此时里程计没有打滑，数据可信度较高；/n1.2.3对滑动窗口中的数据进行非线性优化，保留原有的IMU约束和视觉约束，新增里程计信息的约束，主要步骤为：遍历滑动窗口中的所有单元，计算其中相邻两个单元[t-1,t]内基于里程计信息计算的里程，记为odom_dist，其计算过程为odom_dist＝[(x