CN110726413A - 面向大规模slam的多传感器融合与数据管理机制 - Google Patents

Abstract

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

Description

面向大规模SLAM的多传感器融合与数据管理机制

技术领域

本发明涉及面向大规模SLAM(simultaneous localization and mapping，即时定位与地图构建)的多传感器融合与数据管理机制，目的是面向大规模环境下，提升SLAM的精度、鲁棒性、数据存取效率。基于上述多传感器信息融合策略与数据管理机制，采取集中式拓扑架构，由n个机器人节点和一台服务器构成协同SLAM系统，有效应对未知的大规模环境下探索任务。

背景技术

近年来，自主定位正逐渐成为机器人领域的关键技术之一。机器人要在户外独立完成被指派的具体任务，自主定位是首先要解决的问题，而小范围环境下的定位已不能满足当前越来越复杂的任务和场景。

面向大规模SLAM的应用场景下，若仅使用惯性测量单元或里程计，系统运行时间越长，累计误差就越大；而仅基于机器视觉又无法适应光照不足或出现遮挡的情况，那么，融合多种传感器的信息对于提高SLAM的精度和鲁棒性有着十分重要的意义。

闭环检测是视觉SLAM中必不可少的一个环节，用以消除机器人长时间运动过程中产生的累计误差。但该环节严重依赖建图过程中所使用的大量历史数据，且目前已有的视觉SLAM系统均采取将这些建图所需的关键帧数据存储在内存资源中的做法，并未提供对大规模环境有建图需求的应用场景的方案支撑，随着建图的规模增大，内存资源将损耗殆尽。因此，为了降低SLAM运行过程中的大量内存资源消耗，支撑SLAM闭环检测模块的实时性，提出一种高效的数据存储存取机制成为当前亟待解决的问题。

发明内容

本发明要解决的技术问题是提供一种面向大规模SLAM的多传感器融合与数据管理的机制。

本发明的基本思路是：采取集中式拓扑架构，主要由n个机器人节点和一台服务器构成，其中每个机器人都是相对独立的客户端，且独立运行基于VINS-Mono改进的SLAM框架，机器人与服务器之间进行数据交互，先由机器人将自身获取的关键帧数据发往服务器，再由服务器完成多机器人间的位姿图融合与坐标转换操作，以此实现多机器人协同SLAM。

本发明的详细技术方案如下：

第一步：每台机器人独立运行基于VINS-Mono改进的SLAM框架；

其中，改进的VINS-Mono具体步骤如下：

1.1测量处理前端，保留基于视觉的检测跟踪、IMU预积分，并新增里程计原始数据接收与缓存，增加三种传感器信息的数据对齐处理；

1.1.1接收图像，提取Harris角点，利用金字塔光流跟踪相邻帧，通过RANSAC去除异常点，进一步计算得到相机归一化平面下的特征点云，然后缓存特征点云数据；

1.1.2接收缓存IMU的数据，并进行预积分；

1.1.3接收缓存里程计数据；

1.1.4完成特征点云、IMU、里程计的数据对齐，保证相邻的特征点数据帧之间有完整的IMU数据，且每帧特征点数据帧能对应一条时间戳临近的里程计的数据，两者时间戳之差的绝对值不超过预先设定的阈值detal_t；

1.2非线性图优化，保留原有的视觉信息、IMU预积分信息，并新增里程计信息，通过三种信息联合优化求解滑窗内的状态；

1.2.1首先构建一个容量为m的滑动窗口，滑动窗口的每个单元保留原有的视觉信息、IMU预积分信息，并增加对一条步骤1.1.4所述的数据对齐的里程计信息；

1.2.2基于滑动窗口的第t-1、第t个单元中的里程计信息，计算机器人自滑窗内第t-1至第t单元对应的时间段的航偏角，记为odom_yaw，与滑窗中第t个单元的IMU预积分信息中的航偏角imu_yaw比较，若两者之差的绝对值在预先设定的阈值detal_yaw的范围内，则认为此时里程计没有打滑，数据可信度较高；

1.2.3对滑动窗口中的数据进行非线性优化，保留原有的IMU约束和视觉约束，新增里程计信息的约束，主要步骤为：遍历滑动窗口中的所有单元，计算其中相邻两个单元[t-1,t]内基于里程计信息计算的里程，记为odom_dist，其计算过程为odom_dist＝[(x_t-x_t-1)²+(y_t-y_t-1)²]^1/2，式中，x_t和y_t分别为滑动窗口中t单元中里程计记录的x轴方向位移、y方向位移，同理，x_t-1和y_t-1分别为滑动窗口中t-1单元中里程计记录的x轴方向位移、y方向位移，其中1≤t；

1.3闭环检测，改进后的具体步骤如下：

1.3.1以机器人SLAM建图起点为地图原点，构建边长为d的栅格地图；若将栅格的左下角点坐标记为(x0,y0)，则该栅格包含的地图范围为{(x,y)|x0<x<＝x0+d,y0<y<y0+d}；以每个栅格为单元，将位于其栅格范围内的关键帧数据全部编码，并添加该栅格中心点[(x0+d/2),(y0+d/2)]记为index，以一栅格一条的形式打包存入轻量级数据库，只在内存中维护闭环检测所需的栅格范围内的地图资源；

1.3.2实时检测与机器人当前位置距离最近的栅格，确定栅格中心点，并从数据库中以栅格中心点index为检索条件，取回全部地图数据进行解码，实现数据的一级检索；

1.3.3在该栅格的地图数据中，通过对机器人当前位置坐标与历史关键帧坐标进行比对，找到与机器人当前位置最邻近的所有历史关键帧，并放入闭环检测关键帧比对库供其使用，实现数据的二级检索；

第二步：每台机器人运行关键帧数据管理模块，将抽样筛选过的关键帧或更新的位姿图发送给服务器；

第三步：服务器首先为每个VINS-Mono客户端建立一个带编号的关键帧数据队列，并将接收到的数据暂时统一存储在局部位姿数据缓冲池中；同时，服务器的检测线程开启工作，实时处理缓冲池中的关键帧数据，保证数据不在内存中冗余；

第四步：服务器端使用基于特征提取与特征匹配的方法，将每个客户端关键帧与基准客户端关键帧存储池逐一进行特征匹配，若检测到超过25个匹配点则进行关键帧位姿换算，并利用获得的坐标转换关系进行局部位姿图间的坐标融合，形成全局位姿图；

第五步：服务器初次计算得出的机器人间的位置转换关系会被存储记录，以便系统对整个机器人群体的位置掌握和行为调控；

第六步：随着客户端探索环境过程的继续，会不断产生新的位姿图或更新已有的位姿转换关系，因此客户端会不断更新在服务器中的局部位姿数据缓冲列表，并对全局位姿图进行实时更新显示。

与现有技术相比，本发明具有以下技术优点：

1.本发明独创性提出了一种面向大规模SLAM的多传感器融合与数据管理的机制；

2.本发明融合多种传感器的信息，可提高SLAM的精度和鲁棒性；

3.本发明独创性提出了SLAM闭环检测环节中一种数据二级存取机制；

4.本发明基于传感器融合与数据管理的机制，提出了群体机器人的协同SLAM的一套完备流程。

附图说明

图1是本发明基于VINS-Mono改进的SLAM框架；

图2是本发明群体机器人协同SLAM结构图。

具体实施方式

图1是本发明基于VINS-Mono改进的SLAM框架，图2是本发明基于VINS-Mono改进的SLAM框架基础上，提出的群体机器人协同SLAM系统的结构图，具体流程如下：

第一步：每台机器人独立运行基于VINS-Mono改进的SLAM框架；

其中，改进的VINS-Mono具体步骤如下：

1.1测量处理前端，保留基于视觉的检测跟踪、IMU预积分，并新增里程计原始数据接收与缓存，增加三种传感器信息的数据对齐处理；

1.1.1接收图像，提取Harris角点，利用金字塔光流跟踪相邻帧，通过RANSAC去除异常点，进一步计算得到相机归一化平面下的特征点云，然后缓存特征点云数据；

1.1.2接收缓存IMU的数据，并进行预积分；

1.1.3接收缓存里程计数据；

1.1.4完成特征点云、IMU、里程计的数据对齐，保证相邻的特征点数据帧之间有完整的IMU数据，且每帧特征点数据帧能对应一条时间戳临近的里程计的数据，两者时间戳之差的绝对值不超过预先设定的阈值detal_t；

1.2非线性图优化，保留原有的视觉信息、IMU预积分信息，并新增里程计信息，通过三种信息联合优化求解滑窗内的状态；

1.2.1首先构建一个容量为m的滑动窗口，滑动窗口的每个单元保留原有的视觉信息、IMU预积分信息，并增加对一条步骤1.1.4所述的数据对齐的里程计信息；

1.2.2基于滑动窗口的第t-1、第t个单元中的里程计信息，计算机器人自滑窗内第t-1至第t单元对应的时间段的航偏角，记为odom_yaw，与滑窗中第t个单元的IMU预积分信息中的航偏角imu_yaw比较，若两者之差的绝对值在预先设定的阈值detal_yaw的范围内，则认为此时里程计没有打滑，数据可信度较高；

1.2.3对滑动窗口中的数据进行非线性优化，保留原有的IMU约束和视觉约束，新增里程计信息的约束，主要步骤为：遍历滑动窗口中的所有单元，计算其中相邻两个单元[t-1,t]内基于里程计信息计算的里程，记为odom_dist，其计算过程为odom_dist＝[(x_t-x_t-1)²+(y_t-y_t-1)²]^1/2，式中，x_t和y_t分别为滑动窗口中t单元中里程计记录的x轴方向位移、y方向位移，同理，x_t-1和y_t-1分别为滑动窗口中t-1单元中里程计记录的x轴方向位移、y方向位移，其中1≤t；

1.3闭环检测，改进后的具体步骤如下：

1.3.1以机器人SLAM建图起点为地图原点，构建边长为d的栅格地图；若将栅格的左下角点坐标记为(x0,y0)，则该栅格包含的地图范围为{(x,y)|x0<x<＝x0+d,y0<y<y0+d}；以每个栅格为单元，将位于其栅格范围内的关键帧数据全部编码，并添加该栅格中心点[(x0+d/2),(y0+d/2)]记为index，以一栅格一条的形式打包存入轻量级数据库，只在内存中维护闭环检测所需的栅格范围内的地图资源；

1.3.2实时检测与机器人当前位置距离最近的栅格，确定栅格中心点，并从数据库中以栅格中心点index为检索条件，取回全部地图数据进行解码，实现数据的一级检索；

1.3.3在该栅格的地图数据中，通过对机器人当前位置坐标与历史关键帧坐标进行比对，找到与机器人当前位置最邻近的所有历史关键帧，并放入闭环检测关键帧比对库供其使用，实现数据的二级检索；

第二步：每台机器人运行关键帧数据管理模块，将抽样筛选过的关键帧或更新的位姿图发送给服务器；

第三步：服务器首先为每个VINS-Mono客户端建立一个带编号的关键帧数据队列，并将接收到的数据暂时统一存储在局部位姿数据缓冲池中；同时，服务器的检测线程开启工作，实时处理缓冲池中的关键帧数据，保证数据不在内存中冗余；

第四步：服务器端使用基于特征提取与特征匹配的方法，将每个客户端关键帧与基准客户端关键帧存储池逐一进行特征匹配，若检测到超过25个匹配点则进行关键帧位姿换算，并利用获得的坐标转换关系进行局部位姿图间的坐标融合，形成全局位姿图；

第五步：服务器初次计算得出的机器人间的位置转换关系会被存储记录，以便系统对整个机器人群体的位置掌握和行为调控；

第六步：随着客户端探索环境过程的继续，会不断产生新的位姿图或更新已有的位姿转换关系，因此客户端会不断更新在服务器中的局部位姿数据缓冲列表，并利用三维仿真器对全局位姿图进行实时更新显示。

Claims (1)

1.面向大规模SLAM的多传感器融合与数据管理机制，其特征在于包括以下步骤：

第一步：每台机器人独立运行基于VINS-Mono改进的SLAM框架；

改进的具体步骤如下：

1.1测量处理前端，保留基于视觉的检测跟踪、IMU预积分，并新增里程计原始数据接收与缓存，增加三种传感器信息的数据对齐处理；

1.1.1接收图像，提取Harris角点，利用金字塔光流跟踪相邻帧，通过RANSAC去除异常点，进一步计算得到相机归一化平面下的特征点云，然后缓存特征点云数据；

1.1.2接收缓存IMU的数据，并进行预积分；

1.1.3接收缓存里程计数据；

1.1.4完成特征点云、IMU、里程计的数据对齐，保证相邻的特征点数据帧之间有完整的IMU数据，且每帧特征点数据帧能对应一条时间戳临近的里程计的数据，两者时间戳之差的绝对值不超过预先设定的阈值detal_t；

1.2非线性图优化，保留原有的视觉信息、IMU预积分信息，并新增里程计信息，通过三种信息联合优化求解滑窗内的状态；

1.2.1首先构建一个容量为m的滑动窗口，滑动窗口的每个单元保留原有的视觉信息、IMU预积分信息，并增加对一条步骤1.1.4所述的数据对齐的里程计信息；

1.2.2基于滑动窗口的第t-1、第t个单元中的里程计信息，计算机器人自滑窗内第t-1至第t单元对应的时间段的航偏角，记为odom_yaw，与滑窗中第t个单元的IMU预积分信息中的航偏角imu_yaw比较，若两者之差的绝对值在预先设定的阈值detal_yaw的范围内，则认为此时里程计没有打滑，数据可信度较高；

1.2.3对滑动窗口中的数据进行非线性优化，保留原有的IMU约束和视觉约束，新增里程计信息的约束，主要步骤为：遍历滑动窗口中的所有单元，计算其中相邻两个单元[t-1,t]内基于里程计信息计算的里程，记为odom_dist，其计算过程为odom_dist＝[(x_t-x_t-1)²+(y_t-y_t-1)²]^1/2，式中，x_t和y_t分别为滑动窗口中t单元中里程计记录的x轴方向位移、y方向位移，同理，x_t-1和y_t-1分别为滑动窗口中t-1单元中里程计记录的x轴方向位移、y方向位移，其中1≤t；

1.3闭环检测，改进后的具体步骤如下：

1.3.1以机器人SLAM建图起点为地图原点，构建边长为d的栅格地图；若将栅格的左下角点坐标记为(x0,y0)，则该栅格包含的地图范围为{(x,y)|x0<x<＝x0+d,y0<y<y0+d}；以每个栅格为单元，将位于其栅格范围内的关键帧数据全部编码，并添加该栅格中心点[(x0+d/2),(y0+d/2)]记为index，以一栅格一条的形式打包存入轻量级数据库，只在内存中维护闭环检测所需的栅格范围内的地图资源；

1.3.2实时检测与机器人当前位置距离最近的栅格，确定栅格中心点，并从数据库中以栅格中心点index为检索条件，取回全部地图数据进行解码，实现数据的一级检索；

1.3.3在该栅格的地图数据中，通过对机器人当前位置坐标与历史关键帧坐标进行比对，找到与机器人当前位置最邻近的所有历史关键帧，并放入闭环检测关键帧比对库供其使用，实现数据的二级检索；

第二步：每台机器人关键帧数据管理模块，将抽样筛选过的关键帧或更新的位姿图发送给服务器；

第三步：服务器首先为每个VINS-Mono客户端建立一个带编号的关键帧数据队列，并将接收到的数据暂时统一存储在局部位姿数据缓冲池中；同时，服务器的检测线程开启工作，实时处理缓冲池中的关键帧数据，保证数据不在内存中冗余；

第四步：服务器端使用基于特征提取与特征匹配的方法，将每个客户端关键帧与基准客户端关键帧存储池逐一进行特征匹配，若检测到超过25个匹配点则进行关键帧位姿换算，并利用获得的坐标转换关系进行局部位姿图间的坐标融合，形成全局位姿图；

第五步：服务器初次计算得出的机器人间的位置转换关系会被存储记录，以便系统对整个机器人群体的位置掌握和行为调控；

第六步：随着客户端探索环境过程的继续，会不断产生新的位姿图或更新已有的位姿转换关系，因此客户端会不断更新在服务器中的局部位姿数据缓冲列表，并利用三维仿真器对全局位姿图进行实时更新显示。

Images