基于FPGA的点云特征匹配方法、系统和路径规划系统

技术领域

本发明涉及数据处理领域，具体地，涉及一种基于FPGA的点云特征匹配方法、系统和路径规划系统。

背景技术

随着人工智能、自动驾驶等新兴技术的快速发展，激光雷达被广泛用于人工智能、智能机器人的感知、导航、定位等领域，并且在中低速的移动机器人特种作业场景下取得了不错的效果。对于应用系统的性能、体积、功耗、成本的要求也越来越高。目前在移动机器人领域，激光雷达在环境感知和定位、避障等方面发挥着巨大的优势，然而激光雷达的的数据处理流程几乎都是在传统的工控机和CPU或者GPU上进行处理，包括点云数据获取、特征提取、特征匹配、位姿估计、距离优化求解以及地图特征匹配等过程。

对于智能机器人或者移动终端产品，随着产品功能的多样化，用户体验需求的提升，对于产品的低延时、高帧率的性能要求也是越来越严格导致算法的数据量、复杂程度也越来越大。在传统的CPU处理器或者硬件平台上处理海量的数据和复杂业务逻辑时，耗时较大，效率低下，几乎不能满足业务的实时性需求。

专利文献CN103345382A公开了一种CPU+GPU群核超算系统及SIFT特征匹配并行计算方法，通过并行性分析，将许多计算分割在CPU和GPU之间分别计算，发挥了各自的计算优势，使SIFT特征匹配GPU并行算法较CPU串行算法速度提高了近30倍，大幅缩短数据处理的时间，实时性提高，实现遥感影像特征点提取和匹配。

工控机+CPU的数据处理过程，因为CPU是串行执行过程，即使是多核多线程的CPU，在处理海量的数据和业务逻辑时，耗时较大，效率低下，不能满足业务的实时性需求。并且即将面临失效的摩尔定律的CPU难以继续支撑复杂数据的处理能效，工控机体积大，功耗大，成本高也是不容忽视的。而GPU的SIMD指令架构在处理不规则，不规整的数据结构时，加速性能也不能很好的发挥出来。

发明内容

针对现有技术中的缺陷，本发明的目的是提供一种基于FPGA的点云特征匹配方法、系统和路径规划系统。

根据本发明提供的一种基于FPGA的点云特征匹配系统，包括：处理系统部分和可编程逻辑部分；

所述处理系统部分进行周期性点云数据采集，采集的点云数据写入存储器的指定地址空间中，并通过寄存器通道与所述可编程逻辑部分进行通信，所述可编程逻辑部分从所述指定地址空间中按帧获取点云数据，进行逻辑处理。

优选地，所述可编程逻辑部分包括：

第一功能模块：从所述存储器中加载一帧点云数据，进行特征提取处理得到特征点，并将特征点写入所述存储器；

第二功能模块：从所述存储器中加载上一帧点云数据进行缓存管理，与所述第一功能模块得到的特征点进行逐一匹配计算和结果输出，输出的结果包括所述特征点的多个最邻近特征点，将所述结果进行缓存，同时启动突发将结果写回所述存储器中；

第三功能模块：从所述存储器中加载特征点进行缓存管理，与所述第一功能模块得到的特征点进行逐一匹配计算和结果输出，输出的结果包括所述特征点的多个最邻近特征点，将所述结果进行缓存，同时启动突发将结果写回所述存储器中。

优选地，所述处理系统部分包括：

获取当前帧的特征点和2帧多个最邻近特征点的数据，进行构建线、面，求点到线、面的距离，对距离项进行最优化求解。

优选地，所述存储器包括DDR。

根据本发明提供的一种基于FPGA的点云特征匹配方法，包括：处理系统部分和可编程逻辑部分；

所述处理系统部分进行周期性点云数据采集，采集的点云数据写入存储器的指定地址空间中，并通过寄存器通道与所述可编程逻辑部分进行通信，所述可编程逻辑部分从所述指定地址空间中按帧获取点云数据，进行逻辑处理。

优选地，所述可编程逻辑部分包括：

从所述存储器中加载一帧点云数据，进行特征提取处理得到特征点，并将特征点写入所述存储器；

从所述存储器中加载上一帧点云数据进行缓存管理，与所述第一功能模块得到的特征点进行逐一匹配计算和结果输出，输出的结果包括所述特征点的多个最邻近特征点，将所述结果进行缓存，同时启动突发将结果写回所述存储器中；

从所述存储器中加载特征点进行缓存管理，与所述第一功能模块得到的特征点进行逐一匹配计算和结果输出，输出的结果包括所述特征点的多个最邻近特征点，将所述结果进行缓存，同时启动突发将结果写回所述存储器中。

优选地，所述处理系统部分包括：

获取当前帧的特征点和2帧多个最邻近特征点的数据，进行构建线、面，求点到线、面的距离，对距离项进行最优化求解。

优选地，所述存储器包括DDR。

根据本发明提供的一种路径规划系统，包括上述的基于FPGA的点云特征匹配系统。

优选地，所述点云数据包括静态hmap地图文件。

与现有技术相比，本发明具有如下的有益效果：

1.基于FPGA的点云特征匹配方法避免和简化了传统处理器CPU上的处理地图特征点云匹配的KNN过程，比如KDtree或者图节点构造过程中复杂的方差计算、排序、分割以及大量数据多次存储的过程以及KDtree或者图节点搜索过程中的距离计算、多级索引查找、多级存储等复杂的数据结构的处理过程。

2.提供了一种对于点云特征计算过程中，复杂的匹配计算、距离迭代求解、位姿优化等过程中任务分配的方法，结合CPU、FPGA各处理器的特性，将各功能步骤或数据流部署在各自适合计算和操作的处理器中，有利于整个任务执行效率的提升。

3.可参数化局部地图的特征点数N、特征点的维度M、临近特征点个数K以及特征点的数据位宽B等、可灵活配置，更易于扩展，可根据不同硬件资源的芯片进行适当调整部署。

4.该种并行匹配计算方法，消耗的硬件资源少，并行加速计算效率高。对于上述局部地图特征点N＝32768,M＝3,K＝3，B＝48bit等参数，消耗的36Kb BRAM个数＝48个，计算并行度为3*8＝24，可同时处理8个特征点以及每个特征点3个维度上的数据处理过程，设计PL逻辑侧主处理时钟为250M,对于关键帧提取的每个特征点在局部地图帧的特征点集合中查找K＝3个临近特征点所消耗的时间t＝(N/8+8+K)/250MHZ＝(32768/8+8+3)/250MHZ＝4107/250MHZ＝0.0164ms，并且从计算公式中可知K的大小对于消耗时间的影响不大。那么对于当前关键帧提取的5000个特征点在局部地图帧的特征点集合中查找每个点对应K＝3个临近特征点所消耗的总时间T＝t*5000＝0.0164ms*5000＝82ms。相比传统的CPU处理时间计算效率提升了10倍以上。如果在硬件资源比较充沛的情况下，可部署列方向并行度＝16或者更多，行方向依据特征点的维度空间进行扩展，整体并行计算效率将再成倍增加，对于激光雷达扫描周期为10HZ的速率,可达到实时性的性能要求。那么不需要每隔10帧取一帧关键帧来进行运动估计了，可以实时在移动机器人运动过程中，每帧进行地图匹配和精确定位，关键帧间的位姿累积计算过程也可以删除，进一步减少硬件资源，在PS侧得到的初始位姿也更为准确，在求解距离优化迭代次数也会减少，位姿解算时间也大大减小，整体上地图匹配和位姿解算效率都会大大提升，可以控制移动机器人或者小车的速度大大提升，用户体验也会更好。

5.相比传统工控机上CPU处理，功耗低、成本低，能耗高，更适合部署在移动终端产品上。

附图说明

通过阅读参照以下附图对非限制性实施例所作的详细描述，本发明的其它特征、目的和优点将会变得更明显：

图1为本发明的框图；

图2为帧间位姿变化示意图；

图3为关键帧间位姿变化示意图；

图4为定位与建图过程示意图；

图5为运动路径示意图；

图6为特征点数固定过程示意图；

图7为地图存储模块示意图；

图8为距离计算模块示意图；

图9为Cal_PE计算流程示意图；

图10为K最临近比较模块示意图；

图11为K临近特征结果写回示意图。

具体实施方式

下面结合具体实施例对本发明进行详细说明。以下实施例将有助于本领域的技术人员进一步理解本发明，但不以任何形式限制本发明。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变化和改进。这些都属于本发明的保护范围。

图1是系统的硬件实现框图，其处理平台是内嵌ARM核的FPGA芯片,总体设计框图分为PS系统和PL子系统，其中PS内嵌有ARM处理器核和内存控制器。根据业务逻辑，将激光点云的数据处理过程分解如下。从上述框图中可知，硬件上电时，将存储在SD卡或EMMC芯片上的静态hmap地图文件导入到DDR指定地址中。激光点云数据是网络接口输出，在PS端进行周期采集，写入DDR的指定地址空间，并通过寄存器通道与PL进行通信，PL从对应的地址空间获取点云帧，进行逻辑处理。

PL逻辑处理的过程主要分为3个功能模块：

第1个功能模块PL侧从DDR中加载一帧点云，进行特征提取处理，包括地面点分割、角点提取等，并将最终的分类特征点结果写入DDR。

第2个功能模块PL侧从DDR中加载上一帧特征点进行缓存管理，与第一个功能模块输出的分类特征点进行逐一匹配计算和结果输出，该结果保存了的特征点的K个最邻近特征点信息。并将K个最邻近点的结果进行缓存，同时启动写突发将结果数据写回DDR中。

第3个功能模块PL侧从DDR中加载地图帧特征点进行缓存管理，与第一个功能模块输出的特征点进行逐一匹配计算和结果输出，该结果保存了的特征点的K个最邻近特征点信息。并将K个最邻近特征点结果进行缓存，同时启动写突发将结果数据写回DDR中。

PS处理过程：

获取当前帧的分类特征点数据和2帧K个最邻近特征点结果数据，进行构建线、面，求点到线、面距离，对距离项进行最优化求解，位姿估计计算以及路径规划设计等。

在本发明中以16线激光雷达为例，垂直方向角分辨率为2°，垂直视角为±15°,水平方位角分辨率为0.2°水平视角为360°，激光雷达帧率为10HZ,每一帧输出的原始点云数据量为16x360°/0.2°＝28800个点。激光雷达是对三维环境的感知，每个点包含x,y,z 3个维度的信息，每个维度上的数值代表该轴方向上目标点离激光雷达中心的距离。在PS端通过网络接口对激光雷达输出的点云数据网络UDP包进行接收和解包处理，并对原始的点云数据进行极坐标转化为笛卡尔坐标系下的xyz坐标，由于激光雷达的测距能力最大为200米左右，xyz各坐标取值范围为8bit的无符号数取值范围，但是为了提高测量数据的精度和浮点转定点的需要，在坐标转换过程中，将xyz每个维度坐标的数值放大256倍，取值范围设定为16bit。所以原始的点云数据量最大为28800x16x16x16bit，但在激光雷达探测过程中，没有目标或者目标点没有返回时，点云的数据量将会变少，每帧周期转换后的点云数据写入DDR中，PL端通过AXI4总线每帧周期性读取激光点云数据进行特征提取，包括地面点分割、平面点提取、角点提取等过程。从图1中可知，激光点云数据经过角点特征提取模块之后，生成一定数量定义为N个角点特征点，每一帧都会生成N个角点特征点，并同时将当前帧的N个特征点写入DDR中，并在下一帧时读取出来和当前帧的N个特征点进行匹配。匹配就是对当前帧中的每个特征点，在上一帧的特征点中找出K个临近特征点，在PS端用于构建当前帧的特征点到上一帧中临近特征点组成的线或者面的距离，对距离项进行优化求解并求出激光雷达的相邻两帧间的位姿估计，如图2所示，每一帧都可以求得一个相对于前一帧的位姿R、T，但是相邻两帧间的位姿估计R、T只是局部的的位姿，并且是带有累积误差。如图1所示，将相邻两帧间的位姿估计R、T作为地图匹配过程中输入的初始位姿，在地图匹配过程中进行误差消除，精确定位。

激光雷达扫描周期一般为10HZ，帧与帧之间匹配求解的位姿R、T也为10HZ周期，但是在与地图匹配过程中，由于地图数据量大，在CPU处理器计算过程中匹配计算耗时大通常达到秒级的缘故，需要间隔T帧，通常T＝10帧抽取一帧进行一次地图匹配。这里每隔T帧取一帧定义为关键帧key进行计算,如图3所示，每两个关键帧之间的总的位姿变化是每两帧间的小的位姿变化的累积效应，累积就是把旋转乘积，平移累加，得到当前关键帧相对于前一个关键帧的位姿变化量P。将当前关键帧的特征点云与P乘积转化为世界坐标系中，再与地图进行匹配。如图4为定位建图示意图，激光雷达每个关键帧周期输出的特征点云为QK+1,通过累积计算相邻两关键帧间的位姿为TK，建图过程是为了把QK+1乘以TK转化为世界坐标系下的特征点云再和全局地图或者局部地图进行匹配，且得到当前关键帧相对于世界坐标系下的精确位姿TK+1。然后进入全局规划模块，因为移动机器人的运动路径是提前规划与设定好的，运动路径是在某一个应用场景中期望移动机器人行驶的一条路径的3维坐标点集合，在运动路径中把计算得到的位姿TK+1乘以运动路径上当前点坐标，预测下一个目标点的坐标。如图5所示为一条简单运动路径，路径上有ABCD4个点集，运动轨迹是A->B->C->D。如果在行驶过程中，预测有偏差，假如给定初始位置为A点，乘以TK+1后预测的下一个目标点是B`,而不是B，在局部规划模块中再进行位置修正，调整移动机器人的位姿。最后把满足匹配的当前特征点填充进地图。地图特征匹配可以预测移动机器人运动的轨迹，匹配的速度影响预测的精度和效率。当前关键帧提取到的环境的目标特征点云与局部地图点云匹配，在PL侧其实就是将目标特征点云的每个点找出在局部地图点云中最邻近的K个点，找出K个点后将数据写会DDR中，在PS侧读出DDR中当前的目标点云和其对应的K个临近点进行距离优化迭代求解位姿。

CPU在计算过程中，取得当前关键帧与全局或者较大的一个局部地图进行匹配，匹配计算耗时大，所以需要每隔T＝10帧取一帧来进行地图特征点云的匹配，因为移动机器人是运动作业的，每帧提取的目标点云都是变化的，如果计算耗时较大，间隔的帧数较大，导致取得的局部地图的范围较大才能匹配准确。局部地图范围的数据量太大，也会进一步增加计算的耗时。所以在该种串行的处理器上执行海量数据的搜索查找效率低下，导致定位建图过程中位姿解算慢，移动小车的导航速度无法提升。

全局地图是描述移动机器人在当前应用场景下运动轨迹周围环境的特征信息，通常应用场景的面积越大，全局地图的特征信息数据量越大，地图文件也越大。如果将整个全局地图的特征点数据读出来，进行匹配，特征点数太大，无效的特征点计算和消耗巨大。所以需要考虑获取当前激光雷达所处运动轨迹上具体某个点的周围局部地图上的特征点信息。局部地图的特征点区域大小是可以设定的，比如设定在运动轨迹上具体某个点附近(1m×1m×1m)的立方体邻域内搜索。但是这样一个立方体中的特征点数并不是固定的，对于目标点比较密集的场景，同样大小的立方体中的特征点数比较多，稀疏的场景中，立方体中的点数比较少。而在FPGA加速并行运算结构中，需要提前设计好固定大小的存储空间和计算单元来并行完成固定特征点数的存储和计算，所以对于固定立方体尺寸不固定特征点数并不能适配这种并行计算任务，需要提前固定特征点数。通过在立方体中进行分格处理，以固定特征点数，如图6所示分格处理示意图，假定设计存储局部地图的BRAM空间最大为M＝32768个点，按长宽高三个方向均等分格，32768＝32*32*32,设定长宽高三个方向均为32格子，根据空间上坐标位置判断当前的格子中是否有特征点，每个格子里最多保留一个特征点，如果方格中有多个特征点存在，保留在空间位置上离每个格子中心坐标最近的一个特征点，其余的特征点丢弃。分格处理完后整个立方体中里的特征点数最多保留为M＝32768个点,每个点有XYZ 3个维度，每个维度为16bit，如图7为地图存储模块示意图在列并行度＝8和行并行度＝3的计算过程中，需要提前将DDR中地图点云数据装载到XYZ 3个维度下的各8个片上缓存空间BRAM1,BRAM2,BRAM3…BRAM8。每个关键帧都如此操作将当前关键帧的局部地图固化为M＝32768个点后，写入XYZ 3个维度下的各8个片上缓存空间BRAM1,BRAM2,BRAM3…BRAM8中。

针对每个关键帧在局部地图上查找K个临近特征点，如图8为距离计算模块示意图，图中3个维度下8个BRAM对应于存储管理模块里的8个BRAM存储空间，并且每个BRAM后面跟随一个Cal_PE的距离计算子过程，配合对应的1个时延节拍(小三角符号表示)，形成了一个脉动阵列，行列方向同时进行曼哈顿距离计算,并且列并行度＝8，行并行度＝3。关键帧上特征点提取后输入该脉动阵列，依次启动每个行列方向的BRAM读取操作，每列先后依次读出来的xyz 3个坐标值与输入特征点对应的xyz 3个坐标依次求差值的绝对值累加求和。每一列在前一列的时间上延迟了一个时钟节拍，形成pipeline流水结构，同时可以避免特征点的数据路径太长，扇出太大导致时序违规。在行方向读出时第一节拍读出BRAM1空间的X1数据，第2拍读出BRAM1空间的X9、BRAM2空间的X2,第3拍读出BRAM1空间的X17、BRAM2空间的X10、BRAM3空间的X3,依次循环的读出BRAM1、BRAM2、BRAM3…BRAM8空间里的所有数据，Y维度的BRAM1、BRAM2、BRAM3…BRAM8空间里的所有数据整体上相比X维度延后一节拍，而Z维度的BRAM1、BRAM2、BRAM3…BRAM8空间里的所有数据整体上相比Y维度也是延后一节拍。同理在列方向读出时，第一节拍读出X1数据，第二节拍读出Y1数据和X2数据，第三节拍读出Z1数据、Y2数据和X3数据，依次遍历完成XYZ 3个维度上的各BRAM空间里数据的读取操作。在读出3行8列上的每个BRAM空间的数据，和上述提取特征点的对应坐标数据同时传递到Cal_PE计算模块进行距离的计算。如图9所示为Cal_PE计算流程示意图，对应于每一列3个维度的计算过程，列如XYZ为当前帧角点提取模块提取的XYZ坐标值，X`Y`Z`为BRAM空间读出的上一帧某一特征点的对应的XYZ坐标值，先在X维度判断2个坐标值大小后输出X维度的绝对差值，再判断Y维度上2个坐标值大小后输出Y维度的绝对差值并累加，最后再判断Z维度上2个坐标值大小后输出Z维度的绝对差值再累加，依此过程求出关键帧特征点提取模块提取的每个特征点与地图帧特征点之间的曼哈顿距离。这种距离计算方法相比欧式距离可避免在FPGA芯片里计算平方以及开平方根的复杂运算过程，同时消耗更少的硬件资源。

如图10所示为K最临近比较模块示意图。在关键帧上每提取一个特征角点后，需要在局部地图帧的特征点集合里找出与其最临近的K个特征点的集合，所以经过上述每个特征点对应维度距离累加，将最终曼哈顿距离结果传递到K最临近比较模块，求出K个最临近点的坐标值。本发明中K＝3，需要3级流水结构来完成3个最临近点的查找，在每一级流水结构中只能找到当前这一级流水中的最小距离以及最小距离对应的临近点坐标。图中求取第1临近特征点过程对应第1级流水，共有8个com_PE比较模块，前后首尾相连接，对应依次接收计算模块输出的result1、result2、result3…result8结果、依次进行比较，将比较后的较小结果和对应当前BRAM地址addr1以及所在列序号num1输出，在数据流水中向右进行传输，直至最右边的com_PE模块循环比较完成后得到第1临近特征点。同时将每个com_PE比较模块的较大结果和对应当前BRAM地址addr2以及所在列序号num2输出，在数据流水中向下进行传输。特别注意，在每级流水中第一个com_PE比较模块中需要区别首次的比较和非首次比较，可以设计标记信号区分。因为在第1至第8个节拍时result1只有一个结果输入，此时可以传递一个较大的常量值与其比较，设计较大的常量比所有的距离都要大，避免错误结果的传递。而从第9个节拍开始，此时输入的result1需要与第8个com_PE模块输出的反馈结果进行比较。与第一级流水类似，而在第2级流水中每个com_PE模块接收上一级流水com_PE比较后较大的结果作为输入，而输出是两个较大的结果中比较的较小结果，以及对应当前BRAM地址addr1以及所在列序号num1输出，在数据流水中向右进行传输，直至最右边的com_PE模块循环比较完成后得到第2临近特征点。同时将每个com_PE比较模块的较大结果和对应当前BRAM地址addr2以及所在列序号num2输出，在数据流水中向下进行传输。第3级流水同理可求出第3临近特征点以及对应当前BRAM地址addr1以及所在列序号num1。因为没有第4级流水，第3级流水中com_PE模块比较的较大结果可忽略，不用向下一级流水输出。所以在3级流水的com_PE阵列中列方向向右传递的结果是越来越小的，而行方向向下传递的结果是越来越大的变化趋势。并且当前帧每提取一个特征角点后，只需要在上一帧的特征点集合逐个读出比较一轮就能完成K＝3个临近点的获取，行列方向的所有com_PE是同时进行比较工作的，与上述距离计算模块的数据流速率完全同步，每个点同步计算和同步比较，高效的并行化计算加速了KNN的实现过程。在比较流程过程中，模块输入2组变量，输出2组变量，其中输出端口{addr1,com_result1,num1}为对应比较的较小值，输出端口{addr2,com_result2,num2}为对应比较的较大值。

经过K最临近比较模块一轮比较之后，输出了K＝3个临近特征点距离和3个特征点所在BRAM空间的地址和所在的列序号，依次根据BRAM地址和所在列序号，将对应BRAM地址中的特征点的3个维度的坐标同时读出进行数据位拼接{x,y,z}16bit+16bit+16bit＝48bit,所以需要将每个特征点的输出结果48bit*3数据写回DDR中，方便后端PS端业务逻辑的处理。如图11所示为K临近特征结果写回模块。因为要适配DDR的突发特性，在写回模块中设计了FIFO用于临时存储K临近特征结果的3维坐标值，FIFO的位宽设计64bit，深度为32，在存储了一次突发长度的数据量时，突发长度lenth＝128Byte，启动DDR的突发写操作，并实时监控FIFO中数据量的变化，当FIFO中数据够一次突发时，就启动DDR的突发写操作，类似DMA的数据传输流程，将KNN过程中每输入一个特征点，查找到的K个最邻近特征点的坐标依次获取并写回到DDR中。然后在PS端周期获取当前帧的特征点信息和2帧K邻近的特征点的坐标结果信息后，进行构建线、面，求点到线、面距离，对距离项进行最优化求解，位姿估计计算等。

本领域技术人员知道，除了以纯计算机可读程序代码方式实现本发明提供的系统及其各个装置、模块、单元以外，完全可以通过将方法步骤进行逻辑编程来使得本发明提供的系统及其各个装置、模块、单元以逻辑门、开关、专用集成电路、可编程逻辑控制器以及嵌入式微控制器等的形式来实现相同功能。所以，本发明提供的系统及其各项装置、模块、单元可以被认为是一种硬件部件，而对其内包括的用于实现各种功能的装置、模块、单元也可以视为硬件部件内的结构；也可以将用于实现各种功能的装置、模块、单元视为既可以是实现方法的软件模块又可以是硬件部件内的结构。

在本申请的描述中，需要理解的是，术语“上”、“下”、“前”、“后”、“左”、“右”、“竖直”、“水平”、“顶”、“底”、“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本申请和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本申请的限制。

以上对本发明的具体实施例进行了描述。需要理解的是，本发明并不局限于上述特定实施方式，本领域技术人员可以在权利要求的范围内做出各种变化或修改，这并不影响本发明的实质内容。在不冲突的情况下，本申请的实施例和实施例中的特征可以任意相互组合。