激光点云线性KNN 算法FPGA 实现及加速

陈小宇，阳梦雪，李常对，赵鹏程

1.华中师范大学物理科学与技术学院，湖北武汉430079

2.武汉大学遥感信息工程学院，湖北武汉430079

迭代最近点（iterative closest point,ICP）是一种常用的激光点云匹配算法，ICP 算法通常需要大量的计算时间。文献[1-3] 的研究表明，减少ICP 计算时间的方法包括减少迭代次数、减少同名点数量和加速K最近邻（K-nearest neighbor,KNN）搜索3 种，基于ICP 方法的基准显示，KNN 搜索花费的时间占ICP 算法总计算时间的75% 左右，其效率是影响ICP效率的主要因素。

目前常用的KNN 搜索大都是基于中央处理器（central processing unit,CPU）、图形处理器（graphics processing unit,GPU）、专用集成电路（application specific integrated circuit,ASIC）、现场可编程门阵列（field programmable gate array,FPGA）平台。文献[4] 利用统一计算设备架构（compute unified device architecture,CUDA）对KNN 算法问题进行加速，并且对比了KNN 距离计算和排序在CPU、GPU 上的运行时间，结果显示基于GPU 平台进行KNN 距离计算和排序的耗时分别为79.81 ms 和0.53 ms，而基于CPU 平台处理耗时为5 647.45 ms 和15.32 ms。文献[5-8] 对CPU、ASIC、FPGA 平台上KNN 搜索的时间进行了对比，耗时分别为8 423 ms、1 169 ms、720 ms，因此基于FPGA 平台进行KNN 搜索可减少搜索耗时。文献[9-10] 基于FPGA 异构计算平台的特点，设计了KNN 算法的异构实现方案KB-KNN，并且对并行方案CU-KNN 和KB-KNN 在CPU、GPU、FPGA 平台上的性能进行了对比，结果表明KB-KNN 方案与移植到FPGA 平台上的CU-KNN 方案相比获得了1.7 倍的加速比，与原始CU-KNN 方案相比获得了1.5 倍的能效比。因此合理的优化方法将使FPGA 作为一种新型异构计算加速设备具有更高的能量效率。

综上可知，基于FPGA 平台进行的三维激光点云KNN 搜索具有功耗低、耗时短的优势。本文提出了一种利用MPSoC FPGA 实现三维激光点云线性KNN 快速搜索的方法，在保证邻近点搜索精度的前提下，发挥FPGA 高速并行的优势，通过存储管理模块、距离计算模块、K最邻近比较模块、K邻近特征结果写回模块以及调度管控模块相互协调配合，大大减少了三维激光点云K最近邻搜索耗时。

1 KNN 算法

KNN 算法是一种理论完善、简单有效的惰性学习算法[11-12]，1968 年由Cover 和Hart提出。K最近邻是指最近的K个邻居，每个样本都可以用其最接近的K个邻居来代表[13]。在深度学习领域中，可以用于分类的方法有决策树、神经网络、遗传算法、支持向量机、KNN分类等，其中KNN 作为一种较为成熟的算法，是向量空间模型（vector space model,VSM）下最好的分类算法之一[14]。

KNN 算法的基本思想是把每个样本假设为空间中对应的各个点，同时也把新样本假设成空间中的一个点，当输入需要分类的新样本时，通过距离函数计算这些新样本点与空间中各个点之间的距离并排序，选择K个距离最小的样本，再将需要进行分类的未知样本归类到K个距离值中出现频率最高的类[15]。将KNN 算法简要描述为如下流程图[16]。

2 总体设计框架

激光雷达可以从环境中获取被测目标的精确距离及激光回波强度信息，被广泛应用于地图匹配定位[17]。利用三维激光点云进行地图匹配定位时，需要在分别在两帧激光点云和激光点云地图中进行KNN 搜索，总体框架如图2 所示。

图1 KNN 算法流程图Figure 1 Flow chart of KNN algorithm

图2 总体框架图Figure 2 Overall frame diagram

帧间搜索：从双数据率同步动态随机存储器（double data rate synchronous dynamic random access memory,DDR）中实时加载当前帧三维激光点云数据，依次与已经部署在嵌入式块存储器（block random access memory,BRAM）中的上一帧激光点云数据进行K最近邻搜索，并且将搜索结果写回DDR 中。

地图搜索：从DDR 中加载一帧位姿变换后的激光点云数据，与已经从DDR 中加载并存储在BRAM 中的地图特征点数据进行KNN 搜索，并且将搜索结果写回DDR 中。

3 KNN 算法的模块设计

本文利用MPSoC FPGA 实现激光点云KNN 搜索，利用处理系统（processing system,PS）进行任务调度，可编程逻辑（programmable logic,PL）进行KNN 加速，采用多个模块并行计算，以减少KNN 搜索耗时，如图3，包括存储管理模块、距离计算模块、K最邻近比较模块、K邻近特征结果写回模块以及调度管控模块。为了提高激光点云匹配的精度，分别利用角特征点和面特征点进行帧间KNN 搜索和地图KNN 搜索，帧间KNN 搜索和地图KNN搜索实现过程相似，本文将以地图面特征点的KNN 搜索为例进行说明。

图3 KNN 算法的并行模块Figure 3 Parallel module of KNN algorithm

3.1 存储管理模块

在地图面特征点的KNN 搜索中，需要将位姿变换后的一帧面点数据与地图面点数据进行KNN 搜索。为减少KNN 搜索耗时，首先需要将位姿变换后的面点数据与地图面点数据分别部署到BRAM 中，如图4 所示，位姿变换后的面点数据被部署到BRAM163 中，地图面点数据被部署到BRAM1,BRAM2,···,BRAM96 中，使位姿变换后的面点数据同时与96 个地图面点数据进行欧氏距离计算，从而提高KNN 搜索效率。

图4 地图面点搜索数据存储图Figure 4 Map surface point search data storage diagram

3.2 距离计算模块

在存储管理模块的每个BRAM 后跟随一个距离计算模块Cal_PE。将存储管理模块中部署在BRAM 里的数据按照行列方向依次取出，每一列在前一列的时间上延迟一个时钟节拍，形成pipeline 流水结构，这样可以避免特征点的数据路径太长或扇出太大导致的时序违规。X、Y、Z方向上分别延一拍读出特征点的X、Y、Z坐标，然后进行欧氏距离计算。如图5 为地图面点搜索距离计算图。

图5 地图面点搜索距离计算图Figure 5 Map surface point search distance calculation diagram

3.3 K 最邻近比较模块

从经过位姿变换的点云数据中每提取一个特征点，都需要在地图的特征点中找出与其最邻近的K个特征点，因此经过欧氏距离计算的特征点，都需要输入到K最邻近比较模块中，找出K个最邻近的坐标值。

在K最邻近比较模块中，将比较模块Com_PE 中较小结果和对应的地址addr1 以及所在列序号num1 向流水线的右侧进行传输，将每个比较模块Com_PE 中较大结果和对应的地址addr2 以及所在列序号num2 向流水线的下侧进行传输。由于K最邻近比较模块的并行度与距离计算模块一样，因此在减少欧氏距离计算耗时的同时，可以节省从欧氏距离计算结果中找出最小值所用的时间。图6 为地图面点搜索最邻近比较图，其中K取5。

3.4 K 邻近特征结果写回模块

依次根据K最邻近比较模块输出的地址和所在列序号，将对应地址中特征点的3 个维度坐标同时读出进行数据位拼接，再将K个最邻近点的三维坐标写回DDR 中，即可完成一次KNN 搜索。

3.5 调度管控模块

如图7 为调度管控模块处理图，调度管控模块类似一个状态机，配合并行计算架构和调度管控等模块协调处理数据流之间的时序关系。在程序启动时，调度管控模块控制从DDR 中读取激光点云数据，并且控制将搜索结果写回DDR 中，实现三维激光点云数据的连续读取、计算和写回。

4 系统仿真与验证

整个工程在vivado 2019.2 环境中设计开发，结合自带的仿真软件进行仿真。工作时钟为210 MHz 经过测试系统功能实现正常。图8 为欧氏距离计算仿真波形图，图中的result 就是两个点进行欧氏距离计算的结果，例如，result_1 是bram_douta_163 和douta_1 的三维坐标进行欧氏距离计算的结果。

图8 欧氏距离计算仿真图Figure 8 Simulation diagram of Euclidean distance calculation

图9 是利用激光雷达连续采集得到的三维激光点云数据进行帧间和地图的KNN 搜索中的一帧地图面点KNN 搜索结果图，其中图9(a) 是基于ARM 进行地图面点的KNN 搜索，而图9(b) 是基于MPSoC FPGA 进行地图面点的KNN 搜索，图中红色的点是位姿变换后的一帧面点数据与地图面点数据进行KNN 搜索得到的K个最邻近点，白色点是地图面点。对比两幅图的红色点可以看出基于MPSoC FPGA 和ARM 进行地图的KNN 搜索结果一致。

图9 基于ARM 和MPSoC FPGA 的地图面点KNN 搜索结果对比Figure 9 Comparison of map surface point KNN search results based on ARM and MPSoC FPGA

图10 办公室实景图Figure 10 Real picture of the office

图11 点云实时处理结果图Figure 11 Real-time processing results of point cloud

将激光雷达置于办公室环境中，并且使激光雷达按照画8 的轨迹运动，实时采集当前帧和地图点云，进行帧间和地图的KNN 搜索，最后将搜索后的激光雷达运动轨迹实时显示。如点云实时处理结果图中，白色的点是当前帧点，蓝色的是地图点云，红色的点是激光雷达的运动轨迹，从点云实时处理结果图中可以看出激光雷达的运动轨迹与实际轨迹相符，说明基于MPSoC FPGA 进行地图KNN 搜索在实际的运动场景中搜索精度高。

根据Vivado2019.2 综合实现后的报告给出MZU15A 硬件消耗情况可以看出，如表1 所示，本工程中共使用207 382.0 个LUT，236 904.0 个触发器，580.5 个BRAM 和2 880.0 个DSP，充分利用了MZU15A 开发板的硬件资源。通过将距离计算模块的欧氏距离计算结果直接输入到K最邻近比较模块中比较大小，节省了BRAM 的资源消耗，位宽为96 位的114 688.0 个点云数据进行KNN 搜索只需要580.5 个BRAM，大大减少了BRAM 资源的占用。

表1 资源消耗对比Table 1 Comparison of resource consumption

ARM 性能的提升，使得它在激光雷达数据处理中得到了广泛的应用。本文采用和ARM对比的方式来验证基于MPSoC FPGA 平台实现三维激光点云KNN 算法的加速效果。ARM的型号为Qual-core ARM Cortex-A53 @1 333 MHz，MPSoC FPGA 采用米联客MZU15A 开发板，工作时钟为210 MHz。测试ARM 和MPSoC FPGA 的耗时情况如表2 所示，可以看出，基于MPSoC FPGA 的帧间KNN 搜索中1 024×1 024 个角点搜索耗时为0.67 ms，相比ARM 加速了8.22 倍，3 072×3 072 个面点搜索耗时为2.01 ms，相比ARM 加速了24.65倍；地图KNN 搜索中1 024×20 480 个角点搜索耗时为4.66 ms，相比ARM 加速了23.62倍，3 072×81 920 个面点搜索耗时为13.98 ms，相比ARM 加速了94.74 倍。可见，基于MPSoC FPGA 平台实现三维激光点云KNN 算法耗时更短。由于开发板的BRAM 资源有限，地图面点的KNN 搜索选择了96 倍的并行度。若开发板BRAM 资源充足，基于MPSoC FPGA 平台实现三维激光点云KNN 搜索算法耗时将会更短，加速效果更好。

表2 耗时对比Table 2 Comparison of time consuming

5 结语

本文利用FPGA 高速并行的优势，设计了存储管理模块、距离计算模块、K最邻近比较模块、K邻近特征结果写回模块以及调度管控模块，通过各模块的协调合作，实现了三维激光点云K最近邻快速搜索及FPGA 加速。在真实场景中，利用载有MZU15A 开发板和激光雷达的小车对本文的方法进行了测试验证，结果表明本文提出的基于MPSoC FPGA 的线性KNN 搜索算法具有明显的加速效果，同时兼具精度高、功耗较低的优点。此外，该搜索方法采用了参数化的设计方案，可灵活配置激光点云特征点数、维度、邻近特征点个数和数据位宽等参数，以便根据不同的FPGA 资源进行适当调整部署，大大方便了后期的使用。