一种基于增量式TSDF与激光雷达SLAM融合的硬件加速系统

一种基于增量式tsdf与激光雷达slam融合的硬件加速系统
技术领域
1.本发明涉及定位导航技术领域，具体涉及一种基于增量式tsdf与激光雷达slam融合的硬件加速系统。


背景技术：

2.tsdf算法(基于截断的带符号距离函数)，是一种常见的在3d重建中计算隐势面的方法。它是用一个大的空间(称之为volume)来作为我们要建立的三维模型，这个空间可以完全包括我们的模型，其中volume又由许多个小的体素(称之为voxel，就是小立方体)组成。通过求每个体素的值，来构建整个空间。
3.slam(同时定位与地图构建)是自主定位导航领域中最关键的技术之一，也是最基本的问题之一，它广泛应用于自动驾驶汽车、自主导航机器人、虚拟现实以及增强现实等领域。在三维重建中，主流的方式有视觉和激光雷达两种。视觉技术的测距方法是基于三角的测距法，并不适用于较大的空间，且这种方法受光线的影响很大。相比之下，激光雷达可以用于的场景更为广泛。
4.多年来，已经提出了许多基于lidar传感器或深度相机的6d姿态和3d地图来解决此问题的方法。但这些解决方案的主要缺点之一是所需的计算能力和相应的能耗太大。而现场可编程逻辑门阵列(field programmable gate array，fpga)可以对算法进行软硬件协同设计，充分利用算法和硬件的优势使得两者之间优劣互补，一方面弥补了只有软件算法时，嵌入式设备运行的实时性以及稳定性，另一方面也具备了和软件算法一样的定位精度。同时，与基于cpu、gpu和嵌入式arm系统的经典方法相比，功耗大大降低，得益于fpga具有高能效的优势。


技术实现要素：

5.针对上述存在的技术不足，本发明提供的一种基于增量式tsdf与激光雷达slam融合的硬件加速系统。
6.为解决上述技术问题，本发明采用如下技术方案：一种基于增量式tsdf与激光雷达slam融合的硬件加速系统，该硬件加速系统包含传感器输入输出模块、基于tsdfslam的fpga硬件加速模块和主机处理器模块；所述传感器模块包含激光雷达传感器、imu惯性测量单元，所述激光雷达传感器用于获取环境中物体的精确位置和距离信息，所述惯性测量单元(imu)可以获取设备的姿态和加速度等信息驱动；所述基于tsdf slam的fpga硬件加速模块包含预处理模块、加速配准内核模块以及tsdf地图更新模块，所述预处理模块用于将原始的输入点云经过环形中值过滤器和缩减过滤器来优化数据，同时获得初识旋转估计，所述加速配准内核模块用于表示扫描的环境，采用一种允许系统映射大型环境的方法，即根据传感器数据计算和插值新的tsdf距离值和权重，并将结果集成到tsdf地图中，所述tsdf地图更新模块用于根据各个体素tsdf值来将局部地图片段拼接为全局地图；所述加速配准内核模块包含映射模块和本地化存储局部地图模块，所述映射模块用于根据当前位姿将局
部地图部署到正确位置并根据输入扫描数据计算tsdf网格值以及根据新计算的条目更新局部tsdf卷积，所述本地化存储局部地图模块用于将当前扫描配准并存储到局部地图，所述硬件加速系统可称作集成了独立传感器的slam盒子，由一个带有电池，soc和头戴式激光扫描仪的小型计算机机箱组成，所述独立的slam盒子带有可选的ros接口，可以直接在soc系统上输入来自不同lidar的扫描，所述的基于tsdf slam的fpga硬件加速模块在集成的片上系统(soc)上实现，从而在高分辨率lidar下创建大规模tsdf地图,该方法既不依赖于测程法也不依赖于gps，并且能显示出低漂移，所述基于tsdf slam的fpga硬件加速模块能够实时处理lidar的点云，并使用已知的点到tsdf地图的配准方法，具有较高的精确度。但在cpu上的计算成本很高，并且硬件成本高和能耗大，而该完全集成的硬件加速模块具有fpga硬件加速功能，能够克服这些缺点，所述主机处理器模块中含有ros系统。
7.所述tsdf是一种基于体素的数据结构，其中3d网格中的每个像元存储到最近表面的近似距离。对于任何空间中的每个体素，其距离值计算为其坐标与最近测量表面点的坐标之间的深度差。
8.其中，数据可以是实时的，也可以来自ros系统文件预先记录的数据流，可以将它们输入到系统中进行参数调整和基准测试。
9.其中，所述soc是可重新配置的，为相关的硬件和软件分区提供了广泛的机会来加速应用。集成了基于sata的ssd用于存储全局地图，ddr4内存用于局部地图。
10.其中，所述硬件平台提供基本的通信基础设施，包括以太网(用于lidar和外部通信)和usb(用于imu)。
11.其中，各种任务已在嵌入式cpu和fpga之间划分，以实现最大吞吐量，所述fpga实现，使用高级综合方法，在开发时间和算法性能之间提供很好的折衷。
12.所述基于tsdf slam的fpga硬件加速模块采用流水线的方式进行，大大提高了系统的数据吞吐量，每个处理步骤都在自己的线程中执行，以保证在新的扫描可用时立即对其进行处理，而不必等待前一个扫描完全处理。
13.所述传感器输入输出模块可以在两种不同的模式下运行：在soc本身上提供实时传感器数据或从ros实时接收并记录传感器数据。并且其使用了软件堆栈中用于在可重新配置的soc和安装了ros的基于ubuntu的主机之间进行双向、非阻塞通信的桥，所述桥是一个基于zeromq的消息传递库，可以无缝集成到ros生态系统中。激光雷达、imu、估计姿势和其他调试数据从soc发送到主机，并转换为与ros兼容的消息格式，通过这种转换后，使用ros bag文件记录数据可以供后续使用和评估。此外，主机可以将任何数据从ros发送到soc。桥的中心是将各个系统的正确时间戳转换为相应的系统，用于配准过程中的准确初始轮换估计。虽然ros使用以秒和纳秒为单位的时间戳，但本发明系统使用c++std：：chrono库来使用基于unix时间的时间戳进行同步。本发明可以不使用网桥，姿势的历史记录和生成的映射将本地保存在连接到soc的ssd上，能够使系统作为一个独立的单元运行，所述全局映射采用hdf5分层数据格式存储。其允许在自定义架构中组织数据，还可以将所有数据存储在单个文件中。在hdf5文件中，局部tsdf地图存储为单个数据集。全局映射中本地块的索引编码在数据集的元数据字段中。hdf5快速访问当前位置周围的所需数据，根据集成的独立传感器系统的位置变化更新局部地图。
14.进一步地，所述传感器输入输出模块用于采集图像数据信息，所述基于tsdfslam
的fpga硬件加速模块用于接收来自传感器模块记录的数据，并处理这些数据以估计当前姿势，接着生成保存到内部ssd的地图，并传送位姿信息与tsdf地图信息到主机处理器模块用于进一步的任务，用于实时跟踪3d lidar的位置，同时维护全局tsdf地图，还可进行三维重建。
15.本发明的有益效果在于：
16.1、基于tsdf slam的fpga硬件加速系统是针对室内大型场景三维重建的tsdf slam实施。通过在可重新配置的soc上高效利用嵌入式arm处理器和基于fpga的硬件加速器，本发明实现了实时性能，与最先进的pc相比，每帧能耗降低了18倍。集成的ros桥不仅可以在线检查映射过程，还可以对参考数据集进行离线评估。
17.2、在所有场景中，基于tsdf slam的fpga硬件模块都与已建立的slam算法相得益彰，以最小的漂移提供准确的姿态估计。由于其模块化架构，该实施可以很容易地适应新的传感器或算法修改。
18.3、运行时利用硬件重新配置，使系统能够自动适应不断变化的环境条件。
附图说明
19.为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
20.图1为本发明的连接架构图。
21.图2为本发明的整体系统架构图；
22.图3为本发明的基于tsdf slam的fpga硬件加速模块的fpga内核及其内部结构；
23.图4为本发明的基于tsdf slam的fpga硬件加速模块数据流水线示意图；
24.图5为本发明的迭代点到tsdf地图配准过程的流程图。
具体实施方式
25.下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
26.实施例
27.本发明提供了本发明实施例的一种基于增量式tsdf与激光雷达slam融合的硬件加速系统，具体涉及自主定位导航领域，包括传感器输入输出模块、基于tsdf slam的fpga硬件加速模块、主机处理器模块，如图1所示。
28.首先传感器模块收集图像数据信息，通过以太网或者usb接口传送给硬件加速模块，slam box接收来自传感器模块记录的数据，并处理这些数据以估计当前姿势，接着生成保存到内部ssd的地图和传送位姿信息与tsdf地图信息到主机处理器模块，主机处理器模块可以实时处理跟踪3d lidar的位置，同时维护全局tsdf地图。
29.所述传感器输入输出模块包含以下步骤：
30.(1)激光雷达传感器获取环境中物体的精确位置和距离信息，惯性测量单元(imu)获取设备的姿态和加速度等信息驱动，由此得到输入图像的特征图。
31.(2)将数据及特征信息通过以太网或者usb接口传送给硬件加速模块进行特征点匹配。
32.所述的基于tsdf slam的fpga硬件加速模块主要由预处理模块、加速配准内核模块和tsdf地图更新模块这三个步骤组成。
33.s1：预处理模块包含以下步骤：
34.不同的过滤器应用于原始输入点云。首先，应用环形中值滤波器去除异常值。其次，应用缩减过滤器来显著减少点云中的点数，以提高系统的整体运行时间，同时保留足够的信息以在配准步骤中获得良好的结果。最后累积上次扫描和当前扫描之间发布的imu数据，以获得初始旋转估计。
35.s2：加速配准内核模块包含以下步骤：
36.(1)映射模块
37.首先，每个扫描点遍历相应的光线来生成表示lidar数据的tsdf空间。其次，在与体素的每个交点处到扫描点的截断距离将作为新条目。然后将新计算的tsdf值通过运行平均机制集成到局部地图中，与此同时并存储每个地图条目的权重。接着根据到扫描点的距离计算权重。最后，只有表示到扫描表面的最小距离的地图条目用于地图更新。此外，扫描点按水平扫描线排序，每个扫描点以不同的打开角度记录。上述过程此可以描述为以下公式中所示的最小化问题。
[0038][0039]
ξ＝[ω1 ω2 ω3 v1 v2 v3]
t
ꢀꢀꢀ
(2)
[0040]
式中，p表示当前扫描点集，而d包含参考地图的每个点的有符号距离值。
[0041]
如公式(2)所示，ξ是一个六维向量，将两帧之间的运动表示为旋转速度ω和线速度v，对于每个轴和t(ξ)，返回基于此表示形式的转换矩阵。由于配准误差可以视为扫描点到实际扫描表面的距离，因此偏差也可以计算为点本身的tsdf值之和。
[0042]
此优化问题的求解策略是沿距离值递减的方向迭代移动所有扫描点，直到到达表面，同时将imu数据用作初始旋转估计。在每次迭代中，根据每个扫描点确定中间变换，该转换用于沿表面方向进一步移动点，直到它们足够接近表面单元或达到最大迭代次数。
[0043]
公式(3)描述了构建中间变换，公式(4)、(5)描述了矩阵的构建方式h和g矢量可以根据所有接收到的扫描点确定。
[0044]
ξ
*
＝-h-1
·gꢀꢀꢀ
(3)
[0045][0046][0047]
采用雅可比矩阵解决此问题，即根据每个点周围的tsdf邻域来计算，如公式(6)所示。
[0048][0049]
根据最小化问题，矩阵表示距离场相对于ξ并且可以通过应用链式规则来确定，其中右侧的tsdf梯度指向表面的方向，并且可以使用中心微分商计算离散化tsdf图。
[0050]
此外，变化率涉及阻尼系数，该系数随着每次迭代而线性增加，同时可以加权每个扫描点的影响。两个连续扫描帧之间的总转换是中间扫描帧的组合。使用的soc的可重构硬件架构使映射和本地化过程都显示出巨大的并行化能力。
[0051]
(2)本地化存储局部地图过程
[0052]
首先使用当前姿态周围的本地地图进行配准。其次，当检测到的姿势偏移超过给定的最大值，则按需访问这些块以替换局部地图。这些区块根据其在全局体素格网中的位置进行空间索引，并作为序列化数组存储在hdf5文件的专用数据集中。每个数据集的名称表示区块在全球地图中的位置。最后，应用程序还在主内存中的本地姿势周围缓存一组块，以加快加载相关数据的速度。
[0053]
其中tsdf投影和插值以流水线方式并行。对于每个扫描点，光线行进在tsdf投影步骤中执行，并沿当前位置和扫描点之间的线选择tsdf网格的下一个单元格。对于此像元，将计算相应的插值区域和tsdf值并将其插入到临时局部地图中。投影、插值过程都将在fpga上多次实例化，因此内核将针对不同的fpga大小和存储器配置进行扩展。
[0054]
s3：tsdf地图更新模块包含以下步骤：
[0055]
首先使用当前扫描数据触发新的tsdf地图更新之前进行配准扫描，然后同步地图，接着分离各阶段并提高扫描处理的总吞吐量。根据系统的平均移动速度，地图更新在预定义的扫描次数后触发。当位置发生重大变化后地图更新会再次触发，以确保更改环境中新识别的体素进行正确配准。在上次地图更新和配准计算的当前位置之间测量位置变化，可以配置阈值，当环境仅包含稀疏体素，阈值对应降低，以防止某些体素可能会在扫描中消失，而其他体素可能会重新出现。最后，将更新好的各个片段局部地图在fpga上多次实例化合并为全局地图。
[0056]
所述主机处理器部署了ros系统，可用于实时跟踪3d lidar的位置，同时维护全局tsdf地图，还可以将tsdf地图用于三维网格重建，实现slam自主导航定位。
[0057]
显然，本领域的技术人员可以对本发明进行各种改动和变型而不脱离本发明的精神和范围。这样，倘若本发明的这些修改和变型属于本发明权利要求及其等同技术的范围之内，则本发明也意图包含这些改动和变型在内。

技术特征：
1.一种基于增量式tsdf与激光雷达slam融合的硬件加速系统，其特征在于：该硬件加速系统包含传感器输入输出模块、基于tsdfslam的fpga硬件加速模块和主机处理器模块；所述传感器模块包含激光雷达传感器、imu惯性测量单元，所述激光雷达传感器用于获取环境中物体的精确位置和距离信息，所述惯性测量单元(imu)可以获取设备的姿态和加速度等信息驱动；所述基于tsdf slam的fpga硬件加速模块包含预处理模块、加速配准内核模块以及tsdf地图更新模块，所述预处理模块用于将原始的输入点云经过环形中值过滤器和缩减过滤器来优化数据，同时获得初识旋转估计，所述加速配准内核模块用于表示扫描的环境，采用一种允许系统映射大型环境的方法，即根据传感器数据计算和插值新的tsdf距离值和权重，并将结果集成到tsdf地图中，所述tsdf地图更新模块用于根据各个体素tsdf值来将局部地图片段拼接为全局地图；所述加速配准内核模块包含映射模块和本地化存储局部地图模块，所述映射模块用于根据当前位姿将局部地图部署到正确位置并根据输入扫描数据计算tsdf网格值以及根据新计算的条目更新局部tsdf卷积，所述本地化存储局部地图模块用于将当前扫描配准并存储到局部地图，所述硬件加速系统可称作集成了独立传感器的slam盒子，由一个带有电池，soc和头戴式激光扫描仪的小型计算机机箱组成，所述独立的slam盒子带有可选的ros接口，可以直接在soc系统上输入来自不同lidar的扫描，所述的基于tsdfslam的fpga硬件加速模块在集成的片上系统(soc)上实现，从而在高分辨率lidar下创建大规模tsdf地图,该方法既不依赖于测程法也不依赖于gps，并且能显示出低漂移，所述基于tsdfslam的fpga硬件加速模块能够实时处理lidar的点云，并使用已知的点到tsdf地图的配准方法，具有较高的精确度。但在cpu上的计算成本很高，并且硬件成本高和能耗大，而该完全集成的硬件加速模块具有fpga硬件加速功能，能够克服这些缺点，所述主机处理器模块中含有ros系统。2.根据权利要求1所述的一种基于增量式tsdf与激光雷达slam融合的硬件加速系统，其特征在于：所述tsdf是一种基于体素的数据结构，其中3d网格中的每个像元存储到最近表面的近似距离。对于任何空间中的每个体素，其距离值计算为其坐标与最近测量表面点的坐标之间的深度差。3.根据权利要求2所述的一种基于增量式tsdf与激光雷达slam融合的硬件加速系统，其特征在于：所述数据可以是实时的，也可以来自ros系统文件预先记录的数据流，可以将它们输入到系统中进行参数调整和基准测试。4.根据权利要求3所述的一种基于增量式tsdf与激光雷达slam融合的硬件加速系统，其特征在于：所述soc是可重新配置的，为相关的硬件和软件分区提供了广泛的机会来加速应用。集成了基于sata的ssd用于存储全局地图，ddr4内存用于局部地图。5.根据权利要求4所述的一种基于增量式tsdf与激光雷达slam融合的硬件加速系统，其特征在于：所述硬件平台提供基本的通信基础设施，包括以太网(用于lidar和外部通信)和usb(用于imu)。6.根据权利要求5所述的一种基于增量式tsdf与激光雷达slam融合的硬件加速系统，其特征在于：各种任务已在嵌入式cpu和fpga之间划分，以实现最大吞吐量，所述fpga实现，使用高级综合方法，在开发时间和算法性能之间提供很好的折衷。7.根据权利要求6所述的一种基于增量式tsdf与激光雷达slam融合的硬件加速系统，其特征在于：所述基于tsdfslam的fpga硬件加速模块采用流水线的方式进行，大大提高了
系统的数据吞吐量，每个处理步骤都在自己的线程中执行，以保证在新的扫描可用时立即对其进行处理，而不必等待前一个扫描完全处理。8.根据权利要求7所述的一种基于增量式tsdf与激光雷达slam融合的硬件加速系统，其特征在于：所述传感器输入输出模块可以在两种不同的模式下运行：在soc本身上提供实时传感器数据或从ros实时接收并记录传感器数据。并且其使用了软件堆栈中用于在可重新配置的soc和安装了ros的基于ubuntu的主机之间进行双向、非阻塞通信的桥，所述桥是一个基于zeromq的消息传递库，可以无缝集成到ros生态系统中。激光雷达、imu、估计姿势和其他调试数据从soc发送到主机，并转换为与ros兼容的消息格式，通过这种转换后，使用rosbag文件记录数据可以供后续使用和评估。此外，主机可以将任何数据从ros发送到soc。桥的中心是将各个系统的正确时间戳转换为相应的系统，用于配准过程中的准确初始轮换估计。虽然ros使用以秒和纳秒为单位的时间戳，但本发明系统使用c++std：：chrono库来使用基于unix时间的时间戳进行同步。本发明可以不使用网桥，姿势的历史记录和生成的映射将本地保存在连接到soc的ssd上，能够使系统作为一个独立的单元运行，所述全局映射采用hdf5分层数据格式存储。其允许在自定义架构中组织数据，还可以将所有数据存储在单个文件中。在hdf5文件中，局部tsdf地图存储为单个数据集。全局映射中本地块的索引编码在数据集的元数据字段中。hdf5快速访问当前位置周围的所需数据，根据集成的独立传感器系统的位置变化更新局部地图。9.根据权利要求8所述的一种基于增量式tsdf与激光雷达slam融合的硬件加速系统，其特征在于：所述传感器输入输出模块用于采集图像数据信息，所述基于tsdfslam的fpga硬件加速模块用于接收来自传感器模块记录的数据，并处理这些数据以估计当前姿势，接着生成保存到内部ssd的地图，并传送位姿信息与tsdf地图信息到主机处理器模块用于进一步的任务，用于实时跟踪3dlidar的位置，同时维护全局tsdf地图，还可进行三维重建。

技术总结
本发明公开了一种基于增量式TSDF与激光雷达SLAM融合的硬件加速系统，包括传感器输入输出模块、基于TSDFSLAM的FPGA硬件加速模块和主机处理器模块；所述传感器模块包含激光雷达传感器、IMU惯性测量单元，所述基于TSDF SLAM的FPGA硬件加速模块包含预处理模块、加速配准内核模块以及TSDF地图更新模块，主机处理器模块部署了ROS系统。本发明涉及定位导航技术领域，具体为一种基于增量式TSDF与激光雷达SLAM融合的硬件加速系统。融合的硬件加速系统。融合的硬件加速系统。


技术研发人员：贺建伟 段章领 杨建文 梁好 宋乐怡 高刘轩祺 王子昂
受保护的技术使用者：安徽大学
技术研发日：2023.07.12
技术公布日：2023/9/14