ac2SLAM: FPGA加速高精度SLAM与Heapsort和并行关键点提取器

ac2SLAM: FPGA加速高精度SLAM与Heapsort和并行关键点提取器

发表于FPT CCF C，

摘要

摘要-为了满足应用层的丰富功能，鲁棒和准确的同步定位和映射(SLAM)技术对机器人非常关键。然而，由于缺乏足够的计算能力和存储能力，在嵌入式设备中高效地欺骗高精度SLAM是具有挑战性的。在这项工作中，我们提出了一种基于ORB-SLAM2算法的完整加速方案ac2SLAM，包括前端和后端，并在FPGA平台上实现它。具体来说，提出的ac2SLAM特征:1)可扩展并行ORB提取器，提取足够的关键点和分数，吞吐量匹配误差为4%;2) PingPong堆排序组件(PP - HEATAPSORT)选择重要的关键点，可以实现单周期启动间隔，减少加速器与主机CPU之间的数据传输量，3)后端优化的潜在并行加速策略。与在ARM处理器上运行ORBSLAM2相比，ac2SLAM在TUM和KITTI数据集上实现了2.1倍和2.7倍的速度，同时保持了SOTA eSLAM的10%误差。此外，FPGA加速前端比eSLAM和ARM快4.55倍和40倍。ac2SLAM 在 https://github.com/SLAMHardware/acSLAM 完全开源。

介绍

同时定位和映射(SLAM)是构建地图并保持跟踪机器人在未知环境中的定位的算法，这对于作为支持高级应用的必要底层组件至关重要，如导航和多智能体协作。在自主移动机器人上部署时，受严格的功率预算和物理空间的限制，嵌入式CPU的计算能力不足以实时服务于SLAM算法。因此，专用加速器的设计引起了学术界和工业界的广泛关注。

ORB-SLAM是一种广泛应用的鲁棒基于特征的SLAM方法，它通过匹配提取的ORB特征[1]来估计机器人的姿态。ORB-SLAM的结构如图1所示。ORB特征是通过加速段测试(oFAST)[2]特征检测和二进制鲁棒独立基本(BRIEF)[3]描述符计算的定向特征提取的。由于特征提取是前端最耗时的阶段，目前的SLAM加速器主要关注前端加速，同时以牺牲精度[4]-[8]来简化后端，在大型开放环境中渲染性能较差，尤其是具有多个循环的场景。因此，移动机器人需要一个完整的SLAM系统，优化精度不足，以支持其在大型实时部署场景。

前端可以提取每个图像的数千个关键点。然而，只需要选择重要的关键点进行进一步的处理，因此需要一种高效的基于硬件的关键点提取和排序架构来加速执行并过滤关键点，以节省CPU和加速器之间的时间和传输带宽。此外，提取的关键点数量随输入帧的不同而不同，动态变化的关键点集使许多排序策略[9]-[13]。由于大多数硬件排序电路处理固定数量的输入，一旦不满足条件，将导致硬件资源的浪费或不足。这可能会导致不同帧之间的关键点对数量不足，降低SLAM的性能。

可以选择任何数据大小的前 K 个元素的硬件堆排序 [14] 适合解决这个问题，但它仍然受到多周期启动间隔 (II) 的影响，因为 Block-RAM (BRAM) 不可避免的写读延迟要求新写入的数据只能在至少两个循环之后使用。特别是当同时执行数十个前端ORB提取模块时，这种情况将进一步加剧，并使未优化的写读延迟成为一个新的瓶颈。或者，简单地用查找表 (LUT) 替换 BRAM 将消耗嵌入式 FPGA 可以提供的禁止资源。

要求一个有效的硬件排序架构满足II=1合理的资源，可以解决输入数据的动态大小
为了实现上述目标，我们做出了以下贡献：

1、一种基于开源fpga的高精度ORBSLAM2[15]方案，支持大场景下的单目、立体和RGB-D相机。

2、一种可扩展的ORB提取器硬件，可以并行计算每个周期4n个像素(n=1，2，3，4)。通过调整并行性可以很容易地实现模块之间的吞吐量匹配

3、基于FIFO的堆间协调PP-HEAPSORT可以避免BRAM的写读延迟限制，实现单周期启动间隔。它只消耗16%的LUT和20%的FF资源在没有BRAM的情况下实现。此外，吞吐量为 2n 的堆排序 (n = 1, 2,.)-data-percycle 可以通过 PP-HEAPSORT s 的树结构构建。

4、我们分析了矩阵算子级别上后端的瓶颈，并通过在ZYNQ FPGA上并行化矩阵计算，提出了一种加速操作的策略，我们相信这也可以激发进一步的加速工作。

系统概述

ac2SLAM的整体架构如图2所示。我们将ORB提取器和堆排序实现为专门的硬件模块，并与CPU结合，实现ORB-SLAM2异构端到端系统。系统的总体工作流程如下:

1 2 3：ARM处理器接收来自相机或数据集的帧，然后将它们存储在片外存储器中然后通过直接内存访问 (DMA) 将图像读取和传输到 FPGA (PL-side) 上的可编程逻辑。

4：用于关键点检测的尺度不变性[16]要求输入图像应该在调整大小模块中迭代缩放，以获得8层图像金字塔。每次迭代的比例比为 1.2。因此，从最顶层到金字塔底层的比例因子约为 3.5。

5：FAST是一种角点检测方法，可以检测图像中关键点的坐标位置，并使用高斯滤波器来降低噪声。该模块从每个输入图像中提取 FAST 关键点并对其进行评分，同时使用高斯滤波器对图像进行模糊处理。

6：RS-BRIEF计算模块计算关键点的角度和RS-BRIEF[4]描述符。然后，根据角度移动RS-BRIEF，并将关键点的坐标、角度、分数和RS-BRIEF发送给下游过程。

7：堆排序模块过滤关键点来存储 2n -2 (n = 1, 2, 3。) 分数最高的元素。这一部分的挑战是，对于不同的图像，关键点的数量是不同的，这需要一个有效的硬件设计来支持对不同数量的输入进行排序。

8：通过DMA将具有相应坐标、角度和分数的选定关键点转换回CPU。

9：ARM处理器从片外存储器读取步骤표7的结果，并执行特征跟踪来估计相机的姿势。然后，将启动后端以减少估计姿态的累积误差和噪声。

三、前端实现

本节介绍所提出的可扩展 ORB 提取器的架构，它可以并行计算每个周期 4n 个像素（n = 1, 2, 3,。）。ORB提取器中的所有部分都有不同 n 的 1 周期起始间隔。所有三个模块，包括调整大小、FAST检测和RS-BRIEF计算共享相同的图像补丁。并行策略如图3所示。本节将首先介绍整体架构，然后讨论每个模块的详细实现。

A orb 特征提取

a）线缓冲区：具有列号 Nc 的输入图像将需要一个深度为 2푡log(Nc /(4n)))푡 的 BRAM。此外，并行度 4n 将需要宽度为 4n × 9 位的 BRAM。在我们对KITTI数据集的实现中，每条线缓冲区包含18K-BRAM，以36位×512的方式组织，可以存储输入图像的一行像素。

b) 寄存器库:我们实现了一个大型寄存器库，如图3所示。k × (k + 4n−1)寄存器库被划分为4n k × k图像补丁。每个图像补丁在调整大小模块中执行单个算子，即双线性插值算子、FAST检测模块中的关键点检测算子或RS-BRIEF计算模块中RS-BRIEF算子。在不同的图像补丁上并行执行多个操作符。整个寄存器库以与数据输入行缓冲区相同的方向滑动。寄存器库在每个时钟周期滑动4n个像素。

c)操作细节：由于关键点在图像中分布稀疏，即关键点的数量通常不超过图像中所有像素数量的 1%，我们使用 4n 个补丁并行确定关键点的存在。为了找到关键点，计算寄存器库内的滑动寄存器库停止和4n个图像补丁。具体地说，在图2中，可以并行计算4和5中的多个图像块，而6按顺序计算图像块，因为并行展开将消耗太多的资源。相反，如果没有遇到关键点，该模块简单地跳过计算过程并将寄存器库滑动到下一个位置以节省时间。我们在第 VI 节中分析了对 ORB 提取器延迟的影响。

B 前端模块FPGA实现

a) Resizeing:我们使用双线性插值将输入图像的大小调整为不同大小的，这是图像金字塔的层。为了确保最大比例因子 3.5 下的 4n 像素吞吐量，即单个有效输出出现在最大比例因子下的每 3.5 个图像块，调整大小需要 3.5×4n ≈ 16n 个图像块。因此，我们在双线性插值中实现了 2 × (2 + 16n − 1) 像素寄存器库。

b) FAST检测和高斯滤波:为了检测像素是否为关键点，需要在非最大抑制(NMS)上实现FAST检测9×9图像补丁中心像素。将像素寄存器库从9 × 9扩展到9 ×(9 + 4n−1)，可以并行检测4n个像素。

c)RS-BRIEF计算:我们通过结合[3]中生成模式的方法和[4]中RS-BRIEF的规则，随机生成大量的RS模式。然后选择TUM[17]和KITTI[18]数据集中精度最高的数据集。此外，从BRIEF到RSBRIEF的迁移会降低DBoW2模型[19]的准确性，该模型用于重新定位、重新初始化和闭环。因此，我们使用 TUM 和 KITTI 数据集的 RS-BRIEF 模式训练一个新的词汇表，可以适应室内和室外环境。单个RS-BRIEF模式需要29×29图像补丁，这需要系统中的29×(29+4n−1)像素寄存器库在理论上实现4n并行。29 × 29图像补丁的计算逻辑要求如此之高，以至于只构造一个计算逻辑来按顺序处理寄存器库的所有图像补丁。考虑到关键点的稀疏性，在大多数情况下，顺序处理不会降低性能。

PP-HEAPSORT 实现

动态输入大小使一些经典的硬件排序架构失效，如位码合并排序等。另一方面，由于BRAM的写读延迟限制，现有的排序硬件可以适应动态输入大小，如堆排序，要么需要令人望而却步的LUT资源，要么包含多个周期启动间隔(II)。为了消除上述问题，本节提出了一种具有 II=1 的低资源开销 PP-HEAPSORT 实现。

A. 典型堆排序的功能

典型的子堆由一堆排序节点 (SN) 和数据存储 [14] 构成，如图 2 的 7 中的子堆所示。 n 级堆排序主要执行两个功能：（1）在连续输入数据序列中选择最大的 2n - 1 值。最大的 2n - 1 值称为 top2n - 1 个最大数据。(2) 从顶部到底部升序维护堆排序中的所有数据，即一个小的顶部堆结构。因此，最上面的数据始终是整个堆的最小值。对于大小为 2n - 1 的典型堆排序，新的到达数据（如果大于最顶层的数据）将启动顶部数据并发生它。一旦顶部数据被替换，堆排序就会调整自己以将子堆的新最小数据放在顶部。该过程使 subheap 能够始终维护不同大小的输入数据序列的前 2n-1 个值。请注意，硬件堆排序的功能与堆排序数据结构不相似，堆排序的详细功能可以在[14]中找到。

B.为什么乒乓和如何乒乓

我们使用 BRAM 来实现堆排序，因为 BRAM 至少有两轮写入读取延迟，基于 BRAM 的硬件堆排序至少需要两个启动间隔周期。考虑到比较的最坏情况，两个连续的比较使用相同的数据。在第一个循环中的第一个比较中，数据移动到另一个位置，只能在两个循环之后使用。在第二次循环中进行第二次比较时，数据没有准备好，导致到数据危害。我们没有消除双周期写读延迟，而是通过 Ping-Pong 策略规避它。具体来说，通过利用大小为 2(n-1)-1 的两个子堆并以 Ping-Pong 方式切换它们，如图 4 所示，我们确保每个子堆每两个循环接收数据以避免数据危害。此外，作为一个整体工作的两个乒乓子堆可以达到一个周期的起始间隔。我们将在以下小节中说明所提出的PP - HEATPSORT实现的三个特征，大小为2n−2。

c.堆间协调FIFO

PP - HEATPSORT架构实现了单周期启动间隔。但两个子堆分别执行排序。两个子堆中的每一个都单独维护其最大的 2(n-1)-1 数据。请注意，从 sub-heap A 丢弃的数据可能小于 subheap A 中的所有数据，但它可能大于 sub-heap B 中的一些数据，反之亦然。此类数据称为协调数据，应保留而不是丢弃。为了避免丢弃协调数据，需要设计合理的协调机制。具体来说，我们为每个 sub-heap 附加一个 Inter Subheaps Coordination FIFO (ISC-FIFO)，用于存储从相应的 sub-heap 排除的数据，以及一个额外的维护过程，将 PP-HEAPSORT 的过程分为以下四个部分。

1. 外部输入阶段：新的输入数据将通过 Ping-Pong 分配转发到子堆之一（下面的子堆 A），如 1 所示。在与亚堆 A 的顶部值进行比较时，将存储较大的一个，而较小的一个被放电（下面的 X）。同时，sub-heap B 的顶级数据 Y，它始终是 sub-heap B 的最小值。然后，我们需要确保 X 存储在 ISC-FIFO B 中，如果它是协调数据，否则被丢弃。具体来说，如果 X > Y , X 是一个协调的数据，应该进入亚堆 B。否则，X 可以直接丢弃。考虑到 sub-heap B 只能每 2 个循环获取数据，X 不能直接输入 sub-heap B。相反，我们将 X 存储到 ISC-FIFO A 中，并且需要相应的逻辑来决定是将 X 重新进入子堆 B 还是丢弃它。

Y将变得更大，因为亚堆 B 获取更多的外部数据，因此 ISC-FIFO A 中的数据可能小于 Y，输入数据不再需要重新进入亚堆 B。因此，我们将ISC-FIFO A中的前数据Z与Y进行比较，如图4的下图所示。当Z < Y, Z直接丢弃时，ISC-FIFO B使用相同的方法。这个过程一直持续到所有外部数据都被输入到 PP - HEATPSORT 中。

ISC-FIFO自放电阶段：在所有外部数据都被输入堆排序后，ISCFIFO 之一中的所有数据都将被丢弃，因为它们都小于两个堆中的所有数据。这个过程被称为ISC-FIFO自放电，如图4中的휃所示。例如，ISC-FIFO A只存储来自亚堆A的放电数据，使得ISC-FIFO A中的所有数据在外部输入阶段后都不会大于X。如果 Y > X，ISC-FIFO A 中的所有数据都不会大于次堆 B 中的任何数据，因此 ISC-FIFO 中的数据不需要输入亚堆 B 并且可以完全放电。（在 X > Y 时删除 ISC-FIFO B 中的所有数据。) 同时，剩余的带有数据的 ISC-FIFO B 将开始将其数据馈送到子堆 A 以保持子堆间数据协调。当剩余的 ISC-FIFO B 发送所有数据时，此阶段将完成。

3)子堆调整:当ISC-FIFO自放电完成时，我们仍然需要等待子堆的几个循环来调整内部数据，以保持子堆顶部的新最小值。该过程的延迟最多需要单个子堆级别的两倍，如图 4 所示。

4）输出有效数据：一旦 Sub-heap Adjustment 阶段完成，两个子堆都将以 Ping-Pong 方式开始输出内部值，如图 4 中的 4 所示。

D. 块RAM重用
在每个周期中，在子堆的每一层中最多会访问具有不同地址的两个数据，这使我们能够对每个级别使用单个排序节点和两个 BRAM [14]。最重要的是，sub-heap 的水平 k 只需要存储 2(k-1), k = 1, 2,。… 数据的数量。当 k < 5 时，要存储的数据量不超过 20，这将浪费大量 BRAM 存储。因此，我们将下层的BRAM替换为LUT，以增加BRAM的整体利用率。

E.扩展到2n吞吐量
在本节中，我们说明了具有单数据周期吞吐量的 PP-HEAPSORT。此外，通过将两个 PP - HEATPSORT s 与两个额外的 ISC-FIFO 并行排列以保持它们之间的数据协调，可以构建具有 2 个数据每个周期吞吐量的堆排序。具有 2n (n = 1, 2,.)-每周期吞吐量可以通过在共享相同父节点的每个 PP-HEAPSORT s 之间建立具有 ISCFIFO 的树结构中的 PP-HEAPSORT s 来构建。

5 后端分析

稳健且高精度的后端是 SLAM 系统中的另一个瓶颈。在本节中，我们将首先深入研究 G2O、后端使用的优化工具、操作的类别模式，然后提出一种并行加速器方法来减少多核 CPU 上的处理延迟。最后，所提出的并行加速将激发潜在的加速器设计。
A. G2O在后端
G2O 是一个基于图的非线性误差函数优化工具。G2O 的图由用户定义的顶点和边组成。在ORB-SLAM2中，顶点包括摄像机的姿态(vc)，通常是一个由三维旋转向量和三维平移向量组成的六维向量，以及地标的位置(vl)，一个三维坐标。连接vc和vl的边(e)表示摄像机(vc)像素坐标系统中地标(vl)坐标的重投影误差。优化的目标是减少所有边的重投影误差之和∑e2

如图 5 所示，Building 系统和 Solve 方程解释了基于 G2O 的后端大部分整体延迟。通过对图中列出的每个模块所需的计算复杂度进行详细分析，将在以下小节中 V-B 分析原因。

B. 基于G2O的后端流程
典型的G2O优化是运行多次迭代的优化流程，以逐渐降低误差。优化流程的一次迭代如下所示：
1)构建结构（BS）：该模块将图结构（包括 vc、vl 和 e）转换为解决增量方程所需的数据结构：

其中 H 是一个 ncl-by-ncl 块稀疏矩阵。所有矩阵块都用粗体格式表示，如 H。J 是一个 ne-by-nclparse 矩阵，等于 ∂e/∂v。λ 在开始时定义并在每次迭代中更新。在这个模块中，只有内存空间被分配给 H 和 b，无需任何计算。此外，该模块仅在第一次迭代上执行一次，从而导致整体后端过程中的延迟可以忽略不计。

2. 计算当前鲁棒误差(CCRE):该阶段对e应用鲁棒核，然后计算∑e2得到当前鲁棒误差。所有边都应该一起累积，这使得这一步成为ne积累的计算开销。

3. 构建系统 (BSys)：此阶段计算 H 的值。H通常通过顶点的关系分为四个部分，如式2所示。
   
   只有边e连接摄像机顶点vc和地标顶点vl，不存在摄像机内边或地标内边。因此，B、C 和 E 分别是 nc-by-nc、nlby-nl 和 nc-by-nl 块对角矩阵。尽管 B、C、E 都是维度较大的矩阵，但图 6 所示的稀疏本质使 G2O 只能计算非零元素。因此 G2O 只需要迭代边的总数，而不是矩阵 H 中的元素数量。在第 i 次迭代中，计算∂ei∂vm cand ∂ei∂vn l，B、C、E 将计算如式 3 所示：
   
   上述符号的含义如表中所示

4. 求解循环(SL):求解循环模块由求解方程、更新数据、计算优化鲁棒误差和更新λ组成。
   求解方程(SE):这一步将增量方程(公式1)简化为chur增量方程(公式4)。Schur 消除将增量方程的规模从 ncl 减少到 nc：
   
   u 是向量 b 的前 6nc 个元素，而 w 是等式 1 中向量 b 的最后一个 3nl 个元素。矩阵 C 必不可少的块对角线将反转简化为 nl3-by-3 矩阵反转的数量。尽管等式 4 中 E 的维度很大，但它将被分成多个小块，计算如下：

5. 选择第 n 列，并选择 En 和 C−1 nn。n 将迭代地标 nl 的数量。

6. 此选定列 En 中的所有行，其中数据块非零，将在此步骤中迭代。
   选择第 m 行。从而选择一个小的密集矩阵块Emn。然后计算 Emn · C−1
   选择第 k 行，并选择一个小的密集矩阵 blockEkn。然后计算
   …

四、在本节中，我们将提出的硬件设计部署在FPGA上，并分析整个系统和每个提议的模块。

延迟资源比分析:我们在Zynq®UltraScale+™MPSoC XCZU7EV设备上实现ac2SLAM，集成了四核ARM®Cortex™-A53处理系统(PS)和双核ARM Cortex-R5实时处理器。PS的CPU频率为1.5GHz，加速模块的时钟频率为100 MHz。我们展示了具有 4 个并行性的关键点提取器的部署，即配置图 2 中 n = 1 的建议硬件。 ac2SLAM 和 eSLAM [4] 之间的资源利用率比较如表 III 所示。

SLAM加速器系统在TUM和KITTI数据集上进行评估。用于TUM的4级图像金字塔和KITTI的8级图像金字塔特征提取延迟如表IV所示。与 eSLAM 相比，ac2SLAM 仅使用 2.6 倍 LUT、1.1 倍 FF、1.56 倍 DSP 和 0.78 倍 BRAM 来实现 4.5 倍的加速。与ARM相比，TUM和KITTI的速度提高了40.1×和39.88倍。

精度分析：我们将我们的 ORB 提取器计算的特征点位置与 KITTI 数据集上的 OpenCV 中的 FAST 提取器进行比较。总的来说，从FPGA加速FAST提取器中提取的96%特征点位置与OpenCV在CPU处提取的位置相匹配。特征点提取的高精度有助于整个SLAM系统的整体端到端精度。

为了比较 ac2SLAM 与 eSLAM 的准确性，TUM 中的 fr1/xyz、fr1/desk、fr1/room、fr2/xyz 序列用于评估。

如图7所示，得益于ORB-SLAM2的鲁棒后端，ac2SLAM的绝对轨迹误差为fr1/xyz和fr1/desk序列上的eSLAM的50%。在fr2/xyz序列上，ac2SLAM甚至将绝对轨迹误差降低到eSLAM的10%。

表 V 比较了 ac2SLAM 与 ARM 处理器的帧速率。在 TUM 和 KITTI 数据集上，ac2SLAM 分别实现了 2.1 倍和 2.7 倍的帧速率改进。

如表VI所示，图像中有200个关键点，延迟减少大致与并行度成线性关系。当图像有数千个关键点时，RS-BRIEF 中的顺序计算将成为瓶颈，以便延迟不能通过更高的并行性进一步减少。因此，对于每张图像数千个关键点，RS-BRIEF 中应采用图像块的并行处理而不是顺序处理，以在并行度大于 4 时实现吞吐量匹配。

B.图 8 显示了所提出的 PP - HEATPSORT 的延迟改进。所有数据都是随机生成的，并输入到测试的设计中。

最后的一部分感觉看不懂，也稍微有点水，全部都是于eslam比较的。这篇文章没有介绍他的算法是如何改进的但是这篇算法应该是对ORB slam2进行了轻量化。否则根据ORB SLAM2的性能分析特征提取大概占比

这是在ubuntu虚拟机上对ORB SLAM2的性能分析，在单目下，特征提取大概占整个ORB SLAM中百分之十几，也可能是分析的不对。如果单纯的加速特征提取性能提升应该不会那么大。即便是特征提取占百分之50的运行时间，加速到最快也要整个系统的一半呀。像eslam删减了ORB SLAM中的很多模块。