一种基于GPU加速的多传感器融合的SLAM方法

一种基于gpu加速的多传感器融合的slam方法
技术领域
1.本发明涉及机器人即时定位与地图构建技术领域，更具体的，一种基于gpu加速的多传感器融合的slam方法。


背景技术：

2.即时定位与地图构建技术（slam：simultaneous localization and mapping）是机器人实现自主导航的关键性技术，近年来，多传感器融合的slam方法被持续的关注和研究，并且得到了广泛的应用。虽然多传感器融合提高了slam技术的稳定性、精确性和鲁棒性，但也意味着随着数据量的增加，要求更强大的计算机算力，然而，事实是传感器功能与系统的实际信息处理功能之间存在脱节。近年来，图形图像处理技术的进一步发展，对运算速度有了更高的要求，图形处理器gpu凭借强大的计算能力逐渐引起各种技术领域的关注。slam技术的实时性和庞大的数据量之间的矛盾亟待解决，gpu的发展为此提供了契合点。
3.slam技术非常依赖状态估计的准确性和高效性，所以前端数据匹配至关重要，如果在这部分能够准确高效的完成，对于地图的精确性以及整体的实时性提供了重要的保障。除了前端，后端的位姿优化和回环检测也起到非常重要的作用，尤其是在特征不明显或者高度重复的场景，不仅对传感器捕获信息完整丰富性有很高的要求，而且回环检测的准确高效性也值得进一步提高，能够及时捕获回环信息，更新准确的状态信息对于slam具有重要意义。现有的技术只能实现某一方面的加速，未能将两部分同时加速，并且融合方式也有不足之处，基于因子图优化的融合方法具有更精确的机器人状态，是目前多传感器融合的研究趋势。所以，如何加速基于因子图优化的多传感器融合的slam技术这两方面的运行速度，以满足更高层次的应用需求，是一个值得研究的方向。


技术实现要素：

4.为了使得slam技术准确实时的完成，本发明提供了一种基于gpu加速的多传感器融合slam方法，其特征在于，包括基于gpu加速的激光雷达/imu紧耦合前端里程计模块、基于因子图的后端优化模块、点云地图创建模块、基于gpu加速的视觉回环检测模块：所述的基于gpu加速的激光雷达/imu紧耦合前端里程计模块，其中gpu加速的点云匹配步骤如下：步骤1：降采样：主机端（cpu）读取激光雷达数据，对雷达点云数据进行体素网格下采样操作，将所有点云划分在不同的体素中，将距离网格较远和明显错误的点剔除；步骤2：运动补偿：主机端（cpu）使用imu运动预测对雷达帧内运动产生的点云失真进行运动补偿，将每个点云投影到激光雷达帧的起始时刻；步骤3：体素化目标点：主机端（cpu）设置上一帧点云数据为目标点云并存储在容器中，使用kd-tree寻找目标点半径为r范围内的点云存储在容器中，将目标点云从主机端(cpu)复制到设备端(gpu)，每个目标点云对应一个体素索引，初始化均值和协方差，（非初始化跳过此步骤）；
步骤4：体素化源点：主机端（cpu）设置当前帧点云数据为源点云并存储在容器中，使用kd-tree寻找目标点半径r范围内的点云存储在容器中，将源点云从主机端（cpu）复制到设备端（gpu），每个源点云对应一个体素索引，初始化均值和协方差；步骤5：计算均值和协方差：主机端（cpu）根据设备条件和点云数量设计核函数，设定内核宽度，线程数量等于体素数量，驱动设备端（gpu）根据索引将点云建模成高斯分布，计算源点云和目标点云的均值和协方差；步骤6：计算相对变换：设备端（gpu）计算体素之间的对应关系，采用最大似然估计求得最大后验概率，使用高斯牛顿法迭代优化，使用体素中的点数对误差项进行加权，根据体素的协方差矩阵得到信息矩阵，迭代直到目标代价函数收敛，得到相应的变换矩阵t，将变换矩阵t传输到主机端（cpu）；步骤7：更新点云：将当前帧点云设置为目标点云，等待下一次计算。
5.所述的基于因子图的后端优化模块，构建包括imu预积分因子，激光雷达匹配成本因子、视觉回环因子的滑动窗口。当新的传感器状态添加进来时，使用固定滞后平滑方法优化因子图并将更新的位姿状态反馈到基于gpu加速的激光雷达/imu紧耦合前端里程计模块用于修正，边缘化的关键帧用来构建点云地图。
6.所述的基于gpu加速的视觉回环检测模块，首先进行基于gpu加速的fast角点特征提取，随后进行字典查询、相似性评分、组匹配、时间合理性校验、结构一致性校验。当确定回环后，将回环因子加入基于因子图的后端优化模块执行一次全局优化。
7.可选的，所述基于因子图的后端优化模块中，构建的滑动窗口因子图中的传感器状态节点大小固定为20个。优化因子之间的约束为：被边缘化的关键帧与最新帧之间的约束为一元边、其他因子如imu预积分因子、激光雷达匹配成本因子、视觉回环因子为二元边。可选的，所述基于gpu加速的视觉回环检测模块，其中gpu加速的 fast角点特征提取的步骤如下：步骤a：获取图像金字塔：主机端（cpu）对相机输入图像进行二次采样获取图像金字塔；步骤b：分割成矩形单元：主机端（cpu）对每种分辨率图像划分成固定宽度和高度的矩形单元，并拷贝到设备端（gpu），每个矩形块对应一个线程块；步骤c：确定感兴趣区域：设备端（gpu）在warp中计算fast角点，将选中的像素点与其周围领域内足够多的像素点进行亮度比较，以此来确定感兴趣区域（roi）；步骤d：角响应函数评估：对于每个矩形块中感兴趣区域（roi）的每个像素，设备端（gpu）先进行粗角响应函数（ccrf）评估，然后进行角响应函数（crf）评估；步骤e：非极大值抑制：设备端（gpu）将角点响应图划分为规则的单元格网格，每个单元分配一个线程块，使用多个warp处理一个单元， 并进行非极大值抑制；步骤f：cpu读取fast角点：线程块中所有warp将最大结果（crf分数和像素位置）写入指定的共享内存中，块中的第一个线程为每个单元选择最大结果并将其写入全局内存，即完成整张图的fast角点检测，将结果传输到主机端（cpu）。
8.可选的，所述基于gpu加速的视觉回环检测模块中基于gpu加速的 fast角点特征提取步骤c在设备端（gpu）的warp中计算fast角点，首先选择该像素点半径为3 的16个像素点，然后将16个像素点比较的结果存储为一个位数组，作为查找表的索引。计算所有可能的
结果为种且都为二进制，存储在位（8kb）中，使用4字节整数存储这些结果，每个整数存储32个组合。
9.可选的，所述基于gpu加速的视觉回环检测模块中基于gpu加速的 fast角点特征提取步骤e的使用多个warp进行非极大值抑制，其步骤如下：步骤(1)： warp读取单元格的第一行，每个线程获取1个像素的响应；步骤(2)：整个warp开始摩尔邻域抑制，每个线程验证其响应是否在摩尔邻域中达到最大值，非最大值线程将抑制其响应；步骤(3)：warp移动到下一行，重复步骤1、2的操作，处理完整个单元格网格。块中的第一个线程为该单元选择最大结果并将其写入全局内存，完成该单元的处理。
10.总体而言，通过本发明所构思的以上技术方案与现有技术相比，能够取得下列有益效果（1）本发明提出一种基于gpu加速的3d激光雷达、imu、视觉多传感器融合的slam方法，激光雷达和imu在前后端实现紧耦合，在回环检测部分采用了视觉传感器实现，视觉图像纹理更为丰富，实现回环的方式相比于激光雷达更为简单、快速，而且对于特征不明显或者特征重复的场景，回环检测能力更强。将这三种传感器有效的结合，既避免了三种传感器数据紧耦合的复杂度，又达到了相当的效果；（2）采用了基于体素的gicp（vgicp）匹配成本因子，与常用的基于特征的线和平面点匹配方式相比，该方法使用了更多的点来计算配准误差，使得匹配精度更高，并且利用gpu的并行结构加速实现，提高了点云匹配的实时性；（3）使用视觉传感器做回环检测，特征检测部分部署在gpu上进行加速实现，认定为回环后，由激光雷达关键帧再次确认，达到了准确且快速的回环检测需求。
附图说明
11.图1是本发明一种基于gpu加速的多传感器融合的slam方法的总体框图。
12.图2是本发明一种基于gpu加速的多传感器融合的slam方法的激光雷达点云匹配加速策略图。
13.图3是本发明一种基于gpu加速的多传感器融合的slam方法的滑动窗口因子图。
14.图4是本发明一种基于gpu加速的多传感器融合的slam方法的视觉回环检测加速策略图。
15.图5是本发明一种基于gpu加速的多传感器融合的slam方法的视觉回环检测流程图。
具体实施方式
16.为使本发明的目的、优点以及技术方案更加清晰具体，下面结合本发明具体的实施例和相应的附图做进一步详细说明。
17.图1是本发明一种基于gpu加速的多传感器融合的slam方法的一个实施例的总体框图，所述传感器包括3d激光雷达、imu、单目相机，所述方法包括基于gpu加速的激光雷达/imu紧耦合的前端里程计模块、基于因子图的后端优化模块、创建点云地图模块以及基于gpu加速的视觉回环检测模块。
18.一种基于gpu加速的多传感器融合的slam方法，以imu为body坐标系，定义传感器状态为如下形式：（1）其中和是imu线加速度和角加速度噪声。
19.图2是本发明一种基于gpu加速的多传感器融合的slam方法的激光雷达点云匹配加速策略图， 包含以下步骤：步骤1：降采样：读取激光雷达数据，对雷达点云数据进行体素网格下采样操作，具体为设定固定大小的体素容器（本实施例中选取：0.1m*0.1m*0.1m），将所有点云划分在不同的体素中，将距离网格较远和明显错误的点剔除；步骤2：运动补偿：使用imu运动预测对雷达帧内运动产生的点云失真进行运动补偿，首先积分一个激光帧扫描起始和结束时刻之间的imu测量，然后利用每个激光测量点的时间戳对imu积分结果进行线性插值，从而将每个点投影到激光帧的起始时刻，完成运动补偿，imu在未来时刻的速度、位置、旋转计算如下：（2）其中和是imu测量中的白噪声imu预积分获取雷达两个时间戳之间的相对运动，得到第i时刻与第j时刻之间的速度测量、位置测量和旋转测量计算如下：（3）其中，和都是预积分过程中的白噪声步骤3：体素化目标点：主机端（cpu）设置上一帧点云数据为目标点云并存储在容器中，使用kd-tree寻找目标点半径为r范围内的点云存储在容器中，将目标点云从主机端(cpu)复制到设备端(gpu)，每个目标点云对应一个体素索引，初始化均值和协方差，（非初始化跳过此步骤）；步骤4：体素化源点：主机端（cpu）设置当前帧点云数据为源点云并存储在容器中，使用kd-tree寻找目标点半径r范围内的点云存储在容器中，将源点云从主机端（cpu）复制到设备端（gpu），每个源点云对应一个体素索引，初始化均值和协方差；步骤5：计算均值和协方差：主机端（cpu）根据设备条件和点云数量设计核函数，设定内核宽度，线程数量等于体素数量，驱动设备端（gpu）根据索引将点云建模成高斯分布，对每个输入点云建模成高斯分布，并计算均值和协方差，其中协方差由该点云半径r范围内的k个相邻点计算得出；
步骤6：计算相对变换：设备端（gpu）计算体素之间的对应关系，计算源点与多个目标点半径r范围内邻点之间的距离，距离残差如式（4）所示，采用最大似然估计求得最大后验概率，使用高斯牛顿法迭代优化，使用体素中的点数对误差项进行加权，根据体素的协方差矩阵得到信息矩阵，迭代直到目标代价函数收敛，找到置信度最高的变换矩阵，其代价函数如式（5）所示，最后将变换矩阵传输到主机端（cpu）；（4）其中s表示原点，t表示目标点，j表示多个目标点的数量（5）其中i表示邻近点的数量。
20.图 3是本发明一种基于gpu加速的多传感器融合的slam方法的后端滑动窗口因子图，包括imu预积分因子、激光雷达匹配成本因子、视觉回环因子。imu预积分因子通过（3）式给出，激光雷达匹配成本因子通过（5）式给出，视觉回环因子通过（6）式给出。具体地，构建固定窗口大小为20个状态节点的因子图，优化因子之间的约束为：被边缘化的关键帧与最新帧之间的约束为一元边、其他因子如imu预积分因子、激光雷达匹配成本因子、视觉回环因子为二元边。当新的传感器状态添加进来时，使用固定滞后平滑方法优化因子图并边缘化旧帧，通过关键帧机制管理边缘化帧。关键帧的空间分布良好并且与最新帧有足够的重叠。设置重叠率阈值来管理关键帧，重叠率小于阈值（本实施例中选取90%），则设为关键帧。否则丢弃该帧。因子图边缘化的关键帧，作为传感器状态的初始估计送到点云地图创建模块，点云地图创建模块不断接受边缘化的关键帧，并且得到关键帧之间的变换关系，拼接为全局一致的点云地图。
21.图4是一种基于gpu加速的多传感器融合的slam方法的视觉回环检测加速策略图。基于gpu加速的视觉回环检测模块，加速部分为fast角点特征检测，具体步骤为：步骤a：获取图像金字塔：主机端（cpu）对相机输入图像进行二次采样获取图像金字塔；步骤b：分割成矩形单元：主机端（cpu）对每种分辨率图像划分成固定宽度和高度的矩形单元，并拷贝到设备端（gpu），每个矩形块对应一个线程块；步骤c：确定感兴趣区域：设备端（gpu）在warp中计算fast角点，将选中的像素点与其周围半径为3 的16个像素点进行亮度比较，然后将16个像素点亮度比较结果存储为一个位数组，作为查找表的索引。计算所有可能的结果为种且都为二进制，将其存储在位
（8kb）中，使用4字节整数存储这些结果，每个整数存储32个组合。该索引命中在缓存表中，以此来确定感兴趣区域（roi）；步骤d：角响应函数评估：对于每个矩形块中感兴趣区域（roi）的每个像素，设备端（gpu）先进行粗角响应函数（ccrf）评估，然后进行角响应函数（crf）评估；步骤e：非极大值抑制：设备端（gpu）将角点响应图划分为规则的单元格网格，每个单元分配一个线程块，使用多个warp处理一个单元， 进行非极大值抑制，首先warp读取单元格的第一行，每个线程获取1个像素的响应，然后整个warp开始摩尔邻域抑制，每个线程验证其响应是否在摩尔邻域中达到最大值，非最大值线程将抑制其响应，接着warp移动到下一行，重复步骤1、2的操作，处理完整个单元格网格；步骤f：cpu读取fast角点：线程块中所有warp将最大结果（crf分数和像素位置）写入指定的共享内存中，块中的第一个线程为每个单元选择最大结果并将其写入全局内存，即完成整张图的fast角点检测，将结果传输到主机端（cpu）。
22.图5是一种基于gpu加速的多传感器融合的slam方法的视觉回环检测流程图。实现视觉回环检测首先构建视觉里程计，视觉里程计通过特征点法估计变换矩阵，尺度通过激光雷达每隔20帧矫正一次。回环检测模块由视觉基于词袋模式进行检测。基于gpu加速的视觉回环检测的具体步骤为：步骤1：特征提取：特征提取如图4一种基于gpu加速的多传感器融合的slam方法的视觉回环检测加速策略步骤所述，提取到fast特征后将结果传输到主机端（cpu），计算特征点的brief 描述子转换为单词（词袋字典树已创建完成），确定当前帧的单词，形成当前帧词袋向量；步骤2：字典查询：找出与当前帧相同单词的关键帧；步骤3：相似性评分：比较词袋向量之间的差异，计算公式如式（6）所示，两帧词袋向量之间的差异是否小于阈值（本实施例中选取90%），如果小于阈值，则跳转到步骤7；（6）其中，表示当前帧（源）词袋向量，表示目标词袋向量，n表示特征数量步骤4：组匹配：将时间相近的帧合成一个组别，匹配时只对其中一个进行匹配。如果不通过组匹配，则跳转到步骤7；步骤5：时间合理性校验：对于检测到的一个组匹配，计算在此之前连续k个组匹配都成立，将评分最高的组匹配视作回环。如果不通过时间合理性校验，则跳转到步骤7；步骤6：结构性一致性校验：对于词袋向量相同的当前帧和回环帧，进一步确认词袋向量在结构顺序上的一致性。如果不通过结构一致性校验，则跳转到步骤7；步骤7：结束回环，若检测到回环，输出回环帧，否则，结束回环；检测到回环后，再由相同时间戳的激光雷达关键帧进一步确认。确认为回环后，将回环因子添加到后端因子图执行一次全局优化，同时优化传感器位姿状态和点云地图。