CN113643326B - 一种基于SoC的KNN计算装置和路径规划系统 - Google Patents

Abstract

本发明提供一种基于SoC的KNN计算装置和路径规划系统，所述装置包括：数据交互模块、内存管理模块和KNN计算模块，内存管理模块控制数据交互模块的读写区域，数据交互模块负责读取待处理数据和回写处理后的数据，KNN计算模块对待处理数据执行KNN计算。本方案基于SoC平台构建，相比于大多数的基于纯ARM架构或云端架构，SoC可以充分发挥ARM和FPGA的优势，数据交互模块可以在存储器中读取大量数据，KNN计算模块可以对数据进行并行处理和快速计算，此架构兼备FPGA的并行计算能力与ARM的多任务调度能力，提升了数据处理速率和效率，解决了采用市场上大多数CPU或硬件平台进行点云数据处理时存在的延时过高，能耗过高，发热严重，体积过大等问题。

Description

一种基于SoC的KNN计算装置和路径规划系统

技术领域

本申请涉及智能机器人领域，尤其涉及一种基于SoC的KNN计算装置和路径规划系统。

背景技术

近年来无人化、自动化的服务机器人备受关注，比如无人驾驶机器人，无人驾驶机器人融合了路径导航、环境分析、视觉识别和运动控制等多种技术，无人驾驶机器人在进行路径导航时通常采用激光雷达进行定位。

无人驾驶机器人在运行过程中，通常会产生巨大的数据量，这就要求CPU可以实时处理并计算激光雷达传回的大量数据，又要求调度不同的任务模块，多线程执行任务，有些场景中甚至要求CPU运行操作系统显示用户界面。

目前市场上主要的智能机器人系统，通常基于云端架构进行数据处理，或基于纯ARM架构进行数据处理，但是基于云端架构进行数据处理需要将现场数据传至后端管理平台进行处理，所以其缺点是需要有强大的无线通信模块，这使得在一些比较复杂的环境比如隧道、井下等场地使用时，会有较大的延时，甚至不能正常工作，而纯ARM架构其本身逻辑资源有限，导致其算力不高，使得计算的精度和实时性都不高。

发明内容

为解决在目前数据处理存在的时延高、算力低等技术问题，本申请提供了一种基于SoC的KNN计算装置和路径规划系统。

第一方面，本申请提供了一种基于SoC的KNN计算装置，包括：

内存管理模块、数据交互模块和KNN计算模块；

所述内存管理模块用于对存储器进行区域划分，并将与所述数据交互模块对应的读区域和写区域的地址发送至所述数据交互模块；

所述数据交互模块用于从所述读区域中读取待处理数据，并将所述待处理数据传输至所述KNN计算模块，所述待处理数据包括目标特征点、多个第一特征点和多个第二特征点，所述目标特征点为当前帧激光点云数据的任一特征点，所述第一特征点为上一帧激光点云数据的特征点，所述第二特征点为地图点云数据的特征点；

所述KNN计算模块包括第一计算子模块和第二计算子模块；

所述第一计算子模块用于将所述目标特征点与多个所述第一特征点进行匹配计算，输出所述目标特征点的K个第一邻近特征点，其中K为预设值；

所述第二计算子模块用于对所述目标特征点进行RT位姿变换，并将RT位姿变换后的所述目标特征点与多个所述第二特征点进行匹配计算，输出所述目标特征点的K个第二邻近特征点；

所述数据交互模块还用于将所述目标特征点、所述第一邻近特征点和所述第二邻近特征点写入所述写区域。

作为一种可能的实现方式，所述数据交互模块包括读写命令子模块和读写数据子模块；

所述读写命令子模块用于接收读取命令或写入命令，并将所述读取命令或写入命令传输给所述读写数据子模块；

所述读写数据子模块用于在接收到所述读取命令后，从所述读区域读取所述待处理数据，并将所述待处理数据传输至所述KNN计算模块；

所述读写数据子模块还用于在接收到所述写入命令后，将所述目标特征点、所述第一邻近特征点和所述第二邻近特征点写入所述写区域。

作为一种可能的实现方式，所述读写数据子模块包括先进先出存储器；

所述读写数据子模块在将所述目标特征点、所述第一邻近特征点和所述第二邻近特征点写入所述写区域之前，将所述目标特征点、所述第一邻近特征点和所述第二邻近特征点存储至所述先进先出存储器中，并在确定所述先进先出存储器中存储的数据量满足预设要求之后，将所述先进先出存储器中存储的所述目标特征点、所述第一邻近特征点和所述第二邻近特征点写入所述写区域。

作为一种可能的实现方式，所述读写数据子模块将所述目标特征点、所述第一邻近特征点和所述第二邻近特征点存储至所述先进先出存储器时，分别统计所述第一邻近特征点的个数和所述第二邻近特征点的个数，并根据所述第一邻近特征点的个数和所述第二邻近特征点的个数确定所述先进先出存储器中存储的数据量是否满足预设要求。

作为一种可能的实现方式，所述根据所述第一邻近特征点的个数和所述第二邻近特征点的个数确定所述先进先出存储器中存储的数据量是否满足预设要求，包括：

计算所述第一邻近特征点的个数和所述第二邻近特征点的个数之和，若计算得到的和值不小于第一阈值，则确定所述先进先出存储器中存储的数据量满足预设要求；

或

判断所述第一邻近特征点的个数是否小于第二阈值，和判断所述第二邻近特征点的个数是否小于第三阈值，若所述第一邻近特征点的个数不小于第二阈值，且所述第二邻近特征点的个数不小于第三阈值，则确定所述先进先出存储器中存储的数据量满足预设要求。

作为一种可能的实现方式，所述第一计算子模块包括第一距离计算单元和第一距离比较单元，所述第一距离计算单元包括多个并行运行的第一运算子单元；

所述第一运算子单元用于计算所述目标特征点和所述第一特征点之间的距离，输出第一距离值；

所述第一距离比较单元用于对所述第一计算子模块输出的多个所述第一距离值进行比较，输出所述目标特征点的K个第一邻近特征点；

所述第二计算子模块包括第二距离计算单元和第二距离比较单元，所述第二距离计算单元包括多个并行运行的第二运算子单元；

所述第二运算子单元用于计算RT位姿变换后的所述目标特征点和所述第二特征点之间的距离，输出第二距离值。

所述第二距离比较单元用于对所述第二计算子模块输出的多个所述第二距离值进行比较，输出所述目标特征点的K个第二邻近特征点。

作为一种可能的实现方式，所述第一距离比较单元为K级流水结构，其中每一级流水结构用于查找当前级流水中的最小第一距离值以及该最小第一距离值对应的第一特征点，并将该第一特征点作为所述目标特征点的第一邻近特征点；

所述第二距离比较子单元为K级流水结构，其中每一级流水结构用于查找当前级流水中的最小第二距离值以及该最小第二距离值对应的第二特征点，并将该第二特征点作为所述目标特征点的第二邻近特征点。

第二方面，本申请实施例还提供了一种路径规划系统，包括PS子系统、PL子系统和存储器；

所述PL子系统包括内存管理模块、数据交互模块和KNN计算模块；

所述PS子系统用于采集激光点云数据和地图点云数据；

所述PS子系统还包括特征提取模块，所述特征提取模块用于对所述激光点云数据和地图点云数据分别进行特征提取，得到所述激光点云数据的特征点和所述地图点云数据的特征点，并将所述激光点云数据的特征点和所述地图点云数据的特征点写入所述存储器中与所述数据交互模块对应的读区域；

所述PS子系统还用于从所述数据交互模块对应的写区域中读取目标特征点、目标特征点对应的第一邻近特征点和第二邻近特征点。

作为一种可能的实现方式，所述PS子系统还用于根据所述目标特征点、目标特征点对应的第一邻近特征点和第二邻近特征点确定所述激光点云数据对应的激光雷达的位姿偏移量。

作为一种可能的实现方式，所述PS子系统根据所述目标特征点、目标特征点对应的第一邻近特征点和第二邻近特征点确定所述激光点云数据对应的激光雷达的位姿偏移量，包括：

根据所述目标特征点和所述第一邻近特征点构建线和面，计算所述目标特征点到线和面的距离，得到第一距离项，由所述第一距离项对RT求偏导，构建雅克比矩阵，求解线性方程组得到的该特征点对应的实际偏移量；

根据所述目标特征点和所述第二邻近特征点构建线和面，计算所述目标特征点到线和面的距离，得到第二距离项，由所述第二距离项对RT求偏导，构建雅克比矩阵，求解线性方程组得到的对应的地图偏移量。

本申请实施例提供的上述技术方案与传统技术相比具有如下优点：

本申请实施例提供的一种基于SoC的KNN计算装置，包括数据交互模块、内存管理模块和KNN计算模块，内存管理模块控制数据交互模块的读写区域，数据交互模块负责读取待处理数据和回写处理后的数据，KNN计算模块对待处理数据执行KNN计算。本方案基于SoC平台构建，相比于大多数的基于纯ARM架构或云端架构，SoC可以充分发挥ARM和FPGA的优势，数据交互模块可以在存储器中读取大量待处理数据，KNN计算模块可以对数据进行并行处理和快速计算，此架构兼备FPGA的并行计算能力与ARM的多任务调度能力，提升了数据处理速率和效率，解决了采用市场上大多数CPU或硬件平台进行点云数据处理时存在的延时过高，能耗过高，发热严重，体积过大等问题。更适宜安装在小型移动终端和智能机器人平台上。

附图说明

此处的附图被并入说明书中并构成本说明书的一部分，示出了符合本发明的实施例，并与说明书一起用于解释本发明的原理。

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，对于本领域普通技术人员而言，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。

图1为本申请实施例提供的一种系统的架构示意图；

图2为本申请实施例提供的一种基于SoC的KNN计算装置的示意图；

图3为本申请实施例提供的一种数据交互模块的示意图；

图4为本申请实施例提供的一种路径规划系统示意图；

图5为本申请实施例提供的一种路径规划方法的流程图；

图6为本申请实施例提供的另一种路径规划方法的示意图；

图7为本申请实施例提供的一种数据交互模块缓存待处理数据的示意图；

图8为本申请实施例提供的一种距离计算单元示意图；

图9为本申请实施例提供的一种数据从数据交互模块中读出的顺序示意图；

图10为本申请实施例提供的一种运算子单元的计算流程示意图；

图11为本申请实施例提供的一种距离比较单元示意图；

图12为本申请实施例提供的一种比较子单元的比较流程示意图；

图13为本申请实施例提供的一种数据交互模块将目标特征点的邻近特征点写回DDR的示意图；

图14为本申请实施例提供的一种调度管控模块工作示意图。

具体实施方式

为使本申请实施例的目的、技术方案和优点更加清楚，下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本申请的一部分实施例，而不是全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都属于本申请保护的范围。

参见图1，为本申请实施例示出的一种系统的架构示意图，该系统的中央处理器为同时拥有ARM架构和FPGA架构的SoC芯片。

如图1所示，该系统包括PS(Processing System)子系统和PL(ProgrammableLogic)子系统，PS子系统中有ARM内核和存储控制器，PL子系统中有资源丰富的RAM和高速接口，其中，PL子系统主要基于SoC芯片的FPGA实现，PS子系统和PL子系统有多种连接总线和通信接口。

该系统还可以包括存储器，存储器主要负责存储PS子系统采集的数据，以及PL子系统处理后写回的数据。其中，PS采集的数据可以包括激光雷达的点云数据和/或IMU(惯性导航单元)位姿数据。

作为一个实施例，上述存储器可以是与PS子系统和PL子系统相连的外设存储器，比如DDR，即双倍速率同步动态随机存储器，该存储器并不是SoC芯片的片内资源，PS端和PL端均有挂载。

作为另一个实施例，上述存储器还可以是PL子系统中的BRAM(Block Memory)即块存储器，用于实现PS子系统和PL子系统之间数据的交换。本申请实施例对存储器不做具体限定。

在本申请实施例中，PL子系统可以利用DMA(Direct Memory Access)控制器访问存储器，利用高速总线与PS子系统通信。

PL子系统的任务主要为从存储器中读取接收PS子系统发送过来的待处理数据，对待处理数据进行处理，然后将处理过后的数据或结果发送回PS子系统和/或存储器。

具体的，PL子系统主要实现下述几个功能：

第一个功能：读取和接收PS子系统采集的待处理数据，具体的，可以通过DMA控制器，从存储器中间接地获取待处理数据，或是直接通过高速总线接收，从PS子系统中发过来的待处理数据。

第二个功能：将读取到的待处理数据进行缓冲存储，并根据实际需求输入到不同的模块中进行处理。在本申请实施例中，将读取的待处理数据按照读取顺序，分别放入不同的BRAM中，以便可以对多个数据并行处理。在得到处理结果后，PL子系统再将结果缓冲存储，以便后续第三个功能的实现。

第三个功能：将PL子系统得到的处理结果，进行分割、判断等处理后，有步骤地、快速地回写到PS子系统和/或存储器，并且向PS子系统写入命令，以告知PS子系统可以读取回写的数据。第二个功能中，PL子系统将处理结果缓冲存储，直到缓冲到一定数量或者全部缓冲存储完毕后，PL子系统即开始向PS子系统和/或存储器快速地、大量地回写数据，回写完毕后，PL子系统立即写入命令，使PS子系统读取回写的结果数据。

基于上述系统，本申请实施例提供了一种基于SoC的KNN计算装置，该装置可以应用于激光雷达定位场景，用于对激光点云数据进行KNN计算处理，该装置可以设置于图1所示的系统中。下面结合附图对本申请实施例提供的一种基于SoC的KNN计算装置进行说明。

参见图2，为本申请实施例提供的一种基于SoC的KNN计算装置的示意图，如图2所示，该装置可以包括：内存管理模块201、数据交互模块202和KNN计算模块203。

内存管理模块201用于对存储器进行区域划分，并将与所述数据交互模块对应的读区域和写区域的地址发送至所述数据交互模块。

所述数据交互模块202用于从所述读区域中读取待处理数据，并将所述待处理数据传输至所述KNN计算模块203，所述待处理数据包括目标特征点、多个第一特征点和多个第二特征点，所述目标特征点为当前帧激光点云数据的任一特征点，所述第一特征点为上一帧激光点云数据的特征点，所述第二特征点为地图点云数据的特征点。

其中，当前帧激光点云数据即为当前待处理的激光点云数据，上一帧激光点云数据则为当前帧激光点云数据的上一帧激光点云数据。

所述KNN计算模块203包括第一计算子模块和第二计算子模块。

所述第一计算子模块用于将所述目标特征点与多个所述第一特征点进行匹配计算，输出所述目标特征点的K个第一邻近特征点。

所述第二计算子模块用于对所述目标特征点进行RT位姿变换，并将RT位姿变换后的所述目标特征点与多个所述第二特征点进行匹配计算，输出所述目标特征点的K个第二邻近特征点。

其中，K为预设值，例如K可以为3。

所述数据交互模块202还用于将所述目标特征点、所述第一邻近特征点和所述第二邻近特征点写入所述写区域。

本实施例提供的基于SoC的KNN计算装置，基于SoC平台构建，相比于大多数的基于纯ARM架构或云端架构，SoC可以充分发挥ARM和FPGA的优势，ARM对系统的各个功能模块进行统一调度，将传感器传来的大量数据传输给FPGA，FPGA对数据进行并行处理和快速计算后再回传给ARM，此架构兼备FPGA的并行计算能力与ARM的多任务调度能力，具有能耗低、价格低的优点，更适宜安装在小型移动终端和智能机器人平台上，此外，本方案中的大多数模块都采用了并行结构，提升了处理速度和效率，解决了采用市场上大多数CPU或硬件平台进行点云数据处理时存在的延时过高，能耗过高，发热严重，体积过大等问题，故本方案更能满足实时性和小型化、智能化要求。

进一步的，在本发明实施例中，通过设置数据交互模块可以让FPGA并行地读取或写入数据，即对存储器上不同区域上的数据可以同时进行读取或写入的操作。同时数据交互模块也可以让ARM和FPGA对数据交互的次序和逻辑等进行控制，即ARM可以“控制”什么时候写进去数据，什么时候读出来数据，同样地，FPGA也可以“控制”什么时候写进去数据，什么时候读出来数据。其中提到的“控制”，也是通过数据交互模块来实现的。ARM和FPGA根据数据交互模块读取到的指令来进行“控制”。同时，ARM和FPGA也是通过数据交互模块传递指令的。

作为一个实施例，内存管理模块201可以在系统上电时把存储器上面的区域划分好，确定出PL子系统和PS子系统运行时各自访问的区域，以免发生冲突。具体的，内存管理模块201划分的区域有如下特点：允许PS子系统写入的区域只允许PL子系统读取，允许PL子系统写入的区域只允许PS子系统读取。这样就保证了PS子系统和PL子系统在进行读写时不会出错，防止双方的读写区域发生混乱和冲突。

在实际应用中，数据交互模块202设置于PL子系统，因此可以将PL子系统可以访问的区域划分为数据交互模块202对应的读区域和写区域，然后将读区域和写区域对应地址发送至数据交互模块202，以便数据交互模块可以与存储器之间进行数据读写操作。

进一步的，读区域中可以存储有激光点云数据的特征点和地图点云的特征点。

进一步的，内存管理模块201除了可以对存储器进行区域划分之外，还可以负责初始化和复位等操作，比如系统运行了一段时间，存储器上面的数据有很多，而且经过了很多次覆盖写入，此时若想重新从头开始运行程序，则可以利用内存管理模块201对存储器进行复位。

再进一步的，内存管理模块201还可以对数据进行缓冲管理。比如不同类型的数据存放在不同的区域，比如，激光点云数据的特征点分为角点和面点，他们是分开进行KNN计算的，所以存放时要存放在不同的区域。再比如，如果PS子系统还未读取完对上一帧激光点云数据进行处理得到的结果，此时PL子系统已经计算好了下一帧激光点云数据的结果，则此时就要由内存管理模块201进行缓冲存储，直到PS子系统读取完了上一帧激光点云数据的结果，再来读下一帧激光点云数据的结果，否则会引起不同帧处理结果之间的混乱。

作为一个实施例，如图3所示，数据交互模块202可以包括：读写命令子模块和读写数据子模块。其中，读写命令子模块，用于读取和写入命令，读写数据子模块，用于数据的接收和发送，是数据交互模块202的核心部分，数据交互的关键动作由此子模块完成。

具体的，其他单元、模块或装置发出信号或命令之后，读写命令子模块对命令进行读取，然后判断此命令是否为读写数据子模块所需要的命令，若是的话，则将命令传输给读写数据子模块，读写数据子模块根据读写命令子模块传递过来的命令，读取其他单元或模块发出的原始的、待处理的数据，再按照一定的数据结构和时序要求进行一些处理，如将一整块的数据进行分割或对零散的数据进行整合，并且判断识别数据，比如判断数据的格式是否正确，数据的时间信息是否正确等，做完这些操作后，读写数据子模块将数据发送给其他模块进行计算和处理，比如将待处理数据发送给KNN计算模块进行KNN计算。进一步的，读写数据子模块在完成读写操作后，还会传递给读写命令子模块信号，表示完成数据交互的动作，根据信号，读写命令子模块会向外发出命令，以控制其他单元、模块或装置继续运行。

进一步的，等到KNN计算模块结果出来后，并且读写命令子模块发出相应的写入命令后，则读写数据子模块再将KNN计算模块输出的结果收取回来，并且同样进行分割或判断等操作，比如打上时间戳等信息，对不同类型的数据进行分类并且统计其个数，这些操作完成后，则向读写命令子模块发出信号，并且同时也向外部发出信号，将结果数据发送出去。

具体的，KNN计算模块输出目标特征点的第一邻近特征点和第二邻近特征点后，读写命令子模块可以向读写数据子模块发送写入命令，读写数据子模块接收到写入命令之后，获取目标特征点、第一邻近特征点和第二邻近特征点，并将目标特征点、第一邻近特征点和第二邻近特征点写入存储器中与数据交互模块对应的写区域。

作为一个实施例，存储器可以是DDR，为了适配DDR的突发特性，读写数据子模块还可以包括先进先出存储器(也即FIFO)，读写数据子模块在将目标特征点、第一邻近特征点和所述第二邻近特征点写入所述写区域之前，可以先将目标特征点、第一邻近特征点和第二邻近特征点存储至FIFO中，并在确定FIFO中存储的数据量满足预设要求之后，将FIFO中存储的目标特征点、第一邻近特征点和第二邻近特征点写入写区域。

作为一个实施例，读写数据子模块将所述目标特征点、所述第一邻近特征点和所述第二邻近特征点存储至所述先进先出存储器时，可以分别统计第一邻近特征点的个数和所述第二邻近特征点的个数，并根据第一邻近特征点的个数和所述第二邻近特征点的个数确定所述先进先出存储器中存储的数据量是否满足预设要求。

作为一个实施例，可以计算第一邻近特征点的个数和所述第二邻近特征点的个数之和，若计算得到的和值不小于第一阈值，则确定先进先出存储器中存储的数据量满足预设要求，其中第一阈值可以根据实际需求设定，例如可以为2K。

作为另一个实施例，可以判断第一邻近特征点的个数是否小于第二阈值，和判断第二邻近特征点的个数是否小于第三阈值，若第一邻近特征点的个数不小于第二阈值，且第二邻近特征点的个数不小于第三阈值，则确定先进先出存储器中存储的数据量满足预设要求，其中第二阈值和第三阈值也可以根据实际需求设定，例如第二阈值和第三阈值都可以为K。

作为一个实施例，第一计算子模块可以包括第一距离计算单元和第一距离比较单元。其中，第一距离计算单元包括多个并行运行的第一运算子单元，第一运算子单元用于计算所述目标特征点和所述第一特征点之间的距离，输出第一距离值，第一距离比较单元用于对第一计算子模块输出的多个所述第一距离值进行比较，输出所述目标特征点的K个第一邻近特征点；

作为一个实施例，第二计算子模块包括第二距离计算单元和第二距离比较单元。其中，第二距离计算单元包括多个并行运行的第二运算子单元，第二运算子单元用于计算RT位姿变换后的目标特征点和第二特征点之间的距离，输出第二距离值，第二距离比较单元用于对第二计算子模块输出的多个第二距离值进行比较，输出目标特征点的K个第二邻近特征点。

作为一个实施例，第一运算子单元和第二运算子单元都可以采用曼哈顿距离计算函数计算距离值，因为曼哈顿距离中的距离计算公式比欧氏距离的计算公式更加简洁，只需要把两个特征点各维度坐标相减取绝对值，再累加即可，可避免在PS子系统里计算平方以及开平方根的复杂运算过程，消耗更少的硬件资源，缩短计算时间，效率更高。

作为一个实施例，第一距离比较单元可以为K级流水结构，其中每一级流水结构用于查找当前级流水中的最小第一距离值以及该最小第一距离值对应的第一特征点，并将该第一特征点作为所述目标特征点的第一邻近特征点。

作为一个实施例，第二距离比较子单元可以为K级流水结构，其中每一级流水结构用于查找当前级流水中的最小第二距离值以及该最小第二距离值对应的第二特征点，并将该第二特征点作为所述目标特征点的第二邻近特征点。

参见图4为本发明实施例提供的一种路径规划系统的示意图，如图4所示，该系统可以包括：PS子系统、PL子系统和存储器；

其中，PL子系统包括内存管理模块、数据交互模块和KNN计算模块。

其中，数据交互模块、KNN计算模块和内存管理模块的描述可参见上述实施例的相关描述，此处不再赘述。

PS子系统用于采集激光点云数据和地图点云数据。

作为一个实施例，PS子系统可以在系统上电时，导入存储在SD卡或EMMC芯片上的静态hmap地图点云数据，并通过网络接口周期采集激光点云数据，将地图点云数据和激光点云数据写入存储器的指定地址空间中，比如写入存储器中划分出的PS子系统可以访问的区域中。

所述PS子系统还包括：特征提取模块，特征提取模块用于对激光点云数据和地图点云数据分别进行特征提取，得到所述激光点云数据的特征点和所述地图点云数据的特征点，并将所述激光点云数据的特征点和所述地图点云数据的特征点写入所述存储器中与数据交互模块对应的读区域。

作为一个实施例，特征提取模块可以从存储器中获取激光点云数据和地图点云数据，对激光点云数据和地图点云数据分别执行特征提取操作，其中特征提取操作可以包括：地面点分割，然后进行角点提取和平面点提取，将提取到的角点和平面点作为特征点，其中提取的特征点的数量为N，因为特征提取方式不涉及本实施例的改进点，因此此处不再过多描述。

PS子系统还用于从数据交互模块对应的写区域中读取目标特征点、目标特征点对应的第一邻近特征点和第二邻近特征点。

本申请实施例提供的路径规划系统，基于SoC平台构建，相比于大多数的基于纯ARM架构或云端架构，SoC可以充分发挥ARM和FPGA的优势，ARM对系统的各个功能模块进行统一调度，将传感器传来的大量数据传输给FPGA，FPGA对数据进行并行处理和快速计算后再回传给ARM，此架构兼备FPGA的并行计算能力与ARM的多任务调度能力，具有能耗低、价格低的优点，更适宜安装在小型移动终端和智能机器人平台上，此外，本发明中的大多数模块都采用了并行结构，提升了处理速度和效率，解决了采用市场上大多数CPU或硬件平台进行点云数据处理时存在的延时过高，能耗过高，发热严重，体积过大等问题，故本发明更能满足实时性和小型化、智能化要求。

作为一个实施例，PS子系统还可以用于根据目标特征点、目标特征点对应的第一邻近特征点和第二邻近特征点确定所述激光点云数据对应的激光雷达的位姿偏移量。

作为一个实施例，激光雷达的位姿偏移量可以包括实际偏移量和地图偏移量，PS子系统根据目标特征点、目标特征点对应的第一邻近特征点和第二邻近特征点确定所述激光点云数据对应的激光雷达的位姿偏移量，可以包括：

根据所述目标特征点和所述第一邻近特征点构建线和面，计算所述目标特征点到线和面的距离，得到第一距离项，由所述第一距离项对RT求偏导，构建雅克比矩阵，求解线性方程组得到的该特征点对应的实际偏移量。

根据所述目标特征点和所述第二邻近特征点构建线和面，计算所述目标特征点到线和面的距离，得到第二距离项，由所述第二距离项对RT求偏导，构建雅克比矩阵，求解线性方程组得到的对应的地图偏移量。

其中，RT即为R向量和T向量，其中R向量为偏移向量，T向量为初始向量。

作为一个实施例，PS子系统中还可以包括路径规划模块，用于根据激光雷达的位姿偏移量进行路径规划设计、导航、定位等。

作为一个实施例，路径规划系统中还可以包括调度管控模块，调度管控模块用于配合路径规划系统中的各模块和单元，进行协调处理数据流之间的时序关系。

参见图5，为本发明实施例提供的一种路径规划方法流程图，该方法应用于上述路径规划系统，如图5所示，该方法可以包括如下步骤：

S51.特征提取模块从存储器中获取当前帧激光点云数据，对当前帧激光点云数据进行特征提取，得到当前帧激光点云数据对应的特征点，并将特征点存储至存储器中与数据交互模块对应的读区域。

S52.数据交互模块从读区域读取中目标特征点、多个第一特征点和多个第二特征点，并传递给KNN计算模块。

其中，目标特征点为当前帧激光点云数据的任一特征点，第一特征点为上一帧激光点云数据的特征点，第二特征点为所述地图点云数据的特征点。

作为一个实施例，上一帧激光点云数据的特征点和地图点云数据的特征点可以为之前利用特征提取模块提取后存储在存储器中的。

S53.KNN计算模块中的第一计算子模块将目标特征点与多个第一特征点进行匹配计算，输出所述目标特征点的K个第一邻近特征点；KNN计算模块中的第二计算子模块对目标特征点进行RT位姿变换，并将RT位姿变换后的目标特征点与多个第二特征点进行匹配计算，输出所述目标特征点的K个第二邻近特征点。

作为一个实施例，第一计算子模块将目标特征点与多个第一特征点进行匹配计算包括：第一计算子模块可以利用第一距离计算单元中多个并行运行的第一运算子单元同时计算目标特征点与多个第一特征点的距离，得到多个第一距离值，然后利用第一距离比较单元对多个第一距离值进行比较，得到目标特征点的K个第一邻近特征点。

作为一个实施例，第二计算子模块将目标特征点与多个第二特征点进行匹配计算包括：第二计算子模块可以利用第二距离计算单元中多个并行运行的第二运算子单元同时计算目标特征点与多个第二特征点的距离，得到多个第二距离值，然后利用第二距离比较单元对多个第二距离值进行比较，得到目标特征点的K个第二邻近特征点。

S54.数据交互模块将目标特征点、K个第一邻近特征点和K个第二邻近特征点写入存储器中与数据交互模块对应的写区域。

S55.PS子系统从存储器中读取目标特征点、K个第一邻近特征点和K个第二邻近特征点，并根据目标特征点、K个第一邻近特征点和K个第二邻近特征点确定激光雷达的位姿偏移量。

本实施例提供的路径规划方法，通过在KNN计算模块实现并行计算，提高了点云数据处理的效率，减少了耗时。为便于理解，下面结合图6，以采用图4所示的路径规划系统对16线激光雷达采集的激光点云数据进行处理进行路径规划为例，对路径规划系统的运行流程进行说明。

在本例子中，存储器为DDR，16线激光雷达的垂直方向角分辨率为2°，垂直视角为±15°，水平方位角分辨率为0.2°水平视角为360°，激光雷达帧率为10Hz，每一帧输出的原始点云数据量为16x360°/0.2°＝28800个点。激光雷达通过对三维环境进行感知，采集点云数据，点云数据中的每个点云帧包含x，y，z 3个维度的信息，每个维度上的数值代表该轴方向上目标点离激光雷达中心的距离。

参见图6，系统在运行时，PS子系统从SD卡或EMMC芯片中导入地图点云数据，并存储至DDR中，PS子系统还通过网络接口获取激光雷达输出的激光点云数据，并将激光点云数据存储至DDR中。进一步的PS子系统在获取激光雷达输出的激光点云数据时，对获取的激光点云数据网络UDP包进行接收和解包处理，得到原始点云数据，然后对原始的点云数据进行极坐标转换，转换为笛卡尔坐标系下的x，y，z坐标。由于激光雷达的测距能力最大为200米左右，x，y，z各坐标的取值范围为8bit的无符号数取值范围，但是为了提高测量数据的精度和浮点转定点的需要，可以在坐标转换过程中，将x，y，z每个维度坐标的数值放大256倍，取值范围设定为16bit。所以点云帧的数据量最大为28800×16×16×16bit，但在激光雷达探测过程中，没有目标或者目标点没有返回时，激光点云数据的数据量将会变少。每帧激光点云数据进行转换后，PS子系统将坐标转换后的激光点云帧存储至DDR中。

在进行路径规划时，PS子系统中的特征提取模块从DDR中读取待处理的点云数据，作为当前帧激光点云数据，对读取的当前帧激光点云数据进行地面点分割，然后进行角点提取，从而得到当前帧激光点云数据的N个特征点，其中N的值是预先根据需求设定的。将N个特征点写入DDR中。

然后，针对当前帧激光点云数据的每个特征点，与上一帧激光点云数据的特征点逐一进行KNN计算得到K个第一邻近特征点，与地图点云数据的特征点逐一进行匹配得到K个第二邻近特征点。至于是如何进行KNN计算的在下文中结合附图7-附图12进行说明，此处先不详述。

将得到的邻近特征点写入到DDR中，至于如何写入，在下文中结合附图13进行说明，此处先不详述。

PS子系统从DDR中获取当前帧点云的N个特征点和每个特征点对应的第一邻近特征点和第二邻近特征点，针对每个特征点执行下述操作：根据特征点和第一邻近特征点构建线和面，然后计算特征点到线和面的距离，得到第一距离项，对第一距离项进行最优化求解，得到特征点对应的实际偏移量；根据特征点和第二邻近特征点构建线和面，然后计算特征点到线和面的距离，得到第二距离项，对第二距离项进行最优化求解，得到特征点对应的地图偏移量。其中对距离项进行最优化求解包括：将距离项对RT求偏导，得到雅可比矩阵，对雅克比矩阵求解线性方程组，从而得到最优解。将N个特征点对应的实际偏移量组成的矩阵作为激光雷达的实际偏移量，将N个特征点对应的地图偏移量组成的矩阵作为激光雷达的地图偏移量。

然后根据激光雷达的实际偏移量和地图偏移量进行路径规划。

计算第一邻近特征点和计算第二邻近特征点的方式一致，下面结合附图7-12，以计算第一邻近特征点为例，对基于SoC的KNN计算装置的KNN计算过程进行说明。

下面以基于SoC的KNN计算装置在进行KNN计算时，列并行度Parallelism＝8，行并行度Parallelism＝3为例，进行说明，其中列并行度8代表8个特征点，行并行度3表示X，Y，Z3个维度，即每个时钟节拍下同时进行着每个特征点3个维度的计算过程和8个特征点之间距离的计算和比较过程。

在进行KNN计算时，数据交互模块从DDR中读取当前帧激光点云数据的一个特征点(为便于描述下称目标特征点)和上一帧激光点云数据的多个特征点(为便于描述下称第一特征点)，并对多个第一特征点进行缓存。

图7为数据交互模块缓存待处理数据的示意图，其中数据交互模块中包括多个BRAM。

原始的激光点云的数据量为28800bit，特征点的数据量更多，例如特征点数量为32768，若每个特征点分为X，Y，Z 3个维度(也可以为其他维度，对此不作具体限定)，每个维度为16bit(也可以为其他值，对此不作具体限定)，则一个特征点的数据位宽就是16+16+16＝48bit。特征点的数据量就是32768×48bit。

为了兼容扩展性，特征点的数量、每个特征点的维度和每个特征点的数据位宽均可参数化、可灵活配置，更易于扩展，可适配不同硬件资源的芯片进行部署。

在列并行度＝8和行并行度＝3的计算过程中，需要提前将第一特征点装载到数据交互模块中XYZ 3个维度下的各8个片上缓存空间BRAM1，BRAM2，BRAM3，…，BRAM8。具体实现过程为，从DDR中读取第1个特征点48bit，将低16bit对应的z值z1写入Z维度的BRAM1的0号地址空间，同时将中间16bit对应的y值y1写入Y维度的BRAM1的0号地址空间，将高16bit对应的x值x1写入X维度的BRAM1的0号空间。当读出第2个特征点48bit时，将低16bit对应的z值z2写入Z维度的BRAM2的0号地址空间，同时将中间16bit对应的y值y2写入Y维度的BRAM2的0号地址空间，高16bit对应的x值x2写入X维度的BRAM2的0号空间。当读出第3个特征点48bit，将低16bit对应的z值z3写入Z维度的BRAM3的0号地址空间，同时将中间16bit对应的y值y3写入Y维度的BRAM3的0号地址空间，高16bit对应的x值x3写入X维度的BRAM3的0号空间。以此循环，写完BRAM8空间后，再从BRAM1开始遍历，例如从DDR中读取第9个特征点48bit，将低16bit对应的z值z9写入Z维度的BRAM1的0号地址空间，同时将中间16bit对应的y值y9写入Y维度的BRAM1的0号地址空间，高16bit对应的x值x9写入X维度的BRAM1的0号空间，依次类推，将所有的特征点全部部署在3个维度上8个BRAM空间里。由于特征点数量为32768，所以每个BRAM的深度D＝32768/8＝5096，位宽为每个维度的位宽16bit，每个BRAM消耗2个36Kb block ram硬件存储块，共计消耗2×24＝48个36Kb block ram硬件存储块，这种存储方式对距离计算单元的数据调度和获取是非常有效的。

图8为距离计算单元的示意图，图中3个维度下，8个BRAM对应于数据交互模块里的8个BRAM存储空间，并且每个BRAM后面跟随一个Cal_PE即运算子单元，配合对应的1个时延节拍(即图6中的小三角符号表示)，形成了一个脉动阵列。行列方向同时进行曼哈顿距离计算，并且列并行度＝8，行并行度＝3。具体的，在提取出当前帧激光点云数据的目标特征点后，将提取的目标特征点输入上述脉动阵列，依次启动每个行列方向的BRAM读取操作，每列先后依次读出来的xyz 3个坐标值与目标特征点对应的xyz 3个坐标依次求曼哈顿距离。每一列在前一列的时间上延迟了一个时钟节拍，形成pipeline流水结构，同时可以避免特征点的数据路径太长，扇出太大导致时序违规。

为了简化计算，运算子单元计算的是两两特征点之间的曼哈顿距离。曼哈顿距离的距离计算公式比欧氏距离的计算公式要简洁很多，只需要把两个点各维度坐标相减取绝对值，再累加，因此计算曼哈顿距离相比于计算欧氏距离更简单，消耗的硬件资源更少，计算效率更高。

图9为计算过程中第一特征点从数据交互模块中读出的顺序示意图，左边为X维度上行方向读出顺序示意图，第一节拍读出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维度也是延后一节拍。图9右边为列方向读出顺序示意图，同理，第一节拍读出X1数据，第二节拍读出Y1数据和X2数据，第三节拍读出Z1数据、Y2数据和X3数据，依次遍历完成XYZ 3个维度上的各BRAM空间里数据的读取操作。在读出3行8列上的每个BRAM空间的数据，和上述提取特征点的对应坐标数据，同时传递到Cal_PE即距离计算单元进行距离的计算。

图10为距离计算单元中各运算子单元的计算流程示意图，对应于每一列3个维度的计算过程，例如X、Y、Z对应目标特征点的x、y、z坐标值，X`、Y`、Z`为BRAM空间读出的第一特征点的对应的x、y、z坐标值。如图10所示，先在X维度上判断2个坐标值大小后输出X维度的绝对差值，再判断Y维度上2个坐标值大小后输出Y维度的绝对差值并累加，最后再判断Z维度上2个坐标值大小后输出Z维度的绝对差值再累加，依此过程求出目标特征点与第一特征点之间的曼哈顿距离。这种距离计算方法相比欧式距离可避免在SoC的FPGA芯片里计算平方以及开平方根的复杂运算过程，同时消耗更少的硬件资源。

图11为距离比较单元示意图。针对目标特征点，需要在多个第一特征点里找出与该目标特征点最邻近的K个特征点，所以经过上述距离计算单元计算得到的曼哈顿距离后，传递到距离比较单元，求出K个最邻近点的特征点。

本申请实施例中，距离比较单元为K级流水结构，K＝3，需要3级流水结构来完成3个最邻近点的查找，在每一级流水结构中只能找到当前这一级流水中的最小距离以及最小距离对应的邻近点特征点。每一级流水结构分别包含多个首位相连的比较子单元(即图11中的com_PE)，每个比较子单元包含两路输入和两路输出，其中两路输入用于获取两个距离值，比较子单元用于对所述两路输入的两个距离值进行比较，一路输出用于输出较大的距离值，另一路输出用于输出较小的距离值，其中较小的距离值传入同一级流水中的下一比较子单元，较大的距离值则传入下一级流水中。如图11所示，求取第1个邻近特征点过程对应第1级流水，共有8个com_PE即比较单元，前后首尾相连接，对应依次接收距离计算单元输出的result1、result2、result3…result8，通过com_PE的连接顺序从左到右依次进行比较。具体的，每个com_PE对输入该com_PE的数据进行比较，将比较后的较小结果以及该结果对应的BRAM地址以及所在列序号输出，在数据流水中向右进行传输，输入到下一个com_PE中，直至最右边的com_PE循环比较完成后得到第1个邻近特征点。同时将每个com_PE比较模块的较大结果和对应的BRAM地址以及所在列序号输出，在数据流水中向下进行传输。特别注意，在每级流水中第一个com_PE比较模块中需要区别首次的比较和非首次比较，可以设计标记信号区分。因为在第1至第8个节拍时result1只有一个结果输入，此时可以传递一个较大的常量值与其比较，设计较大的常量比所有的距离都要大，避免错误结果的传递。而从第9个节拍开始，此时输入的result1需要与第8个com_PE输出的反馈结果进行比较。与第一级流水类似，而在第2级流水中每个com_PE接收上一级流水com_PE比较后较大的结果作为输入，而输出是两个较大的结果中比较的较小结果，以及较小结果对应的BRAM地址以及所在列序号输出，在数据流水中向右进行传输，直至最右边的com_PE循环比较完成后得到第2邻近特征点。同时将每个com_PE比较模块的较大结果和对应的BRAM地址以及所在列序号输出，在数据流水中向下进行传输。第3级流水同理可求出第3邻近特征点以及对应的BRAM地址以及所在列序号。因为没有第4级流水，第3级流水中com_PE比较的较大结果可忽略，不用向下一级流水输出。所以在3级流水的com_PE阵列中列方向向右传递的结果是越来越小的，而行方向向下传递的结果是越来越大的变化趋势。并且当前帧激光点云数据每提取一个特征点后，只需要在第一特征点集合中逐个读出比较一轮就能完成K＝3个邻近点的获取。在本申请实施例中，行列方向的所有com_PE是同时进行比较工作的，与上述距离计算单元的数据流速率完全同步，每个特征点同步计算和同步比较，高效的并行化计算加速了点数数据处理的实现过程。

图12为com_PE即比较子单元的示意图，如图12所示，比较子单元包含两个输入端口和两个输出端口，两个输入端口result和result`用于输入两个待比较的值，其中输出端口{com_result1}为对应比较的较小值及较小值对应的BRAM地址以及所在列序号，输出端口{com_result2}为对应比较的较大值以及较大值对应的BRAM地址以及所在列序号。

经过距离比较单元一轮比较之后，输出了K＝3个邻近特征点和3个特征点所在的BRAM空间的地址和所在的列序号。为了方便后端PS子系统的业务逻辑处理，需要将当前帧激光点云数据的每个特征点对应的3个邻近特征点数据写回DDR中，因此可以根据距离比较单元输出的BRAM地址和所在列序号，将对应BRAM地址中的特征点的3个维度的坐标同时读出，并进行数据位拼接{x,y,z}16bit+16bit+16bit＝48bit，然后将拼接后的数据写回DDR中。

图13为数据交互模块将目标特征点的邻近特征点写回DDR的示意图，因为要适配DDR的突发特性，在读写数据子模块中设计了FIFO即先进先出存储器，用于临时存储K个邻近特征点的3维坐标值。FIFO的位宽设计64bit，深度为32，在存储了一次突发长度的数据量时，突发长度lenth＝128Byte，启动DDR的突发写操作，并实时监控FIFO中数据量的变化，当FIFO中数据够一次突发时，就启动DDR的突发写操作，类似DMA的数据传输流程，将在对当前帧激光点云数据的处理过程中，每输入一个特征点，将查找到的对应的K个邻近特征点的坐标依次获取，并写回到DDR中，便于后续PS子系统进行逻辑处理时获取。

图14为调度管控模块处理示意图，调度管控模块类似一个状态机功能，路径规划系统中的各模块，进行协调处理数据流之间的时序关系。在程序启动时，调度管控模块触发tready1信号，启动特征提取模块从DDR中读取点云数据stream1，并将提取到的点云数据的特征点以数据流stream2写入DDR中，完成一帧数据量后触发tfinish1信号反馈给调度管控模块，调度管控模块检测到tfinish1后，触发tready2信号启动数据交互模块读取第一特征点数据流stream3，并部署在数据交互模块的BRAM阵列中，完成一帧数据量后触发tfinish2信号反馈给调度管控模块，调度管控模块检测到tfinish2信号后再次触发tready1信号，启动特征提取装置从DDR中读取点云数据流stream1，并将提取到的特征点以数据流stream2写入DDR中，同时将特征点坐标以数据流stream4传输到KNN计算模块中，进行计算，计算产生的K邻近特征点结果以数据流stream5写回DDR中，完成一帧数据量后触发tfinish3信号反馈给调度管控模块，调度管控模块检测到tfinish3信号后再次启动数据交互模块。依次周期循环，连续的进行数据读取、并行计算、结果回写。整个过程在调度管控模块的指挥下高效并发的执行。

本申请实施例提供的上述基于SoC的KNN计算装置，基于SoC平台构建，避免和简化了传统处理器CPU上的查找K邻近特征点的过程，比如现有的KDtree或者图节点构造过程中复杂的方差计算、排序、分割以及大量数据多次存储的过程以及KDtree或者图节点搜索过程中的距离计算、多级索引查找、多级存储等复杂的数据结构的处理过程，在本方案中均不需要。

进一步的，采用并行计算架构，减少了消耗的硬件资源，并行加速提高了计算效率高。

以上述利用基于SoC的KNN计算装置对包含32768个特征点的当前帧激光点云数据进行处理为例，每个特征点的数据宽度为48bit，维度为3维，为每个特征点查找3个邻近特征点，消耗的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的大小对于消耗时间的影响不大。那么对于当前帧激光点云数据提取的32768个特征点在第一特征点集合中查找每个特征点对应K＝3个邻近特征点所消耗的总时间T＝t×32768＝0.0164ms×32768＝537ms。相比传统的CPU处理时间为7711ms，计算效率提升了14倍。如果在硬件资源比较充沛的情况下，可部署列方向并行度＝16或者更多，行方向依据特征点的维度空间进行扩展，整体并行计算效率将成倍增加。在实际应用场景中，比如移动机器人定位和位姿估算过程中，实际经过角点、平面点等类型分类特征点提取后，特征点的个数并不是太多，在本方案中约为5000，在本发明的并行计算架构中执行消耗的时间T`＝(N/8+8+K)/250MHZ×N＝(5000/8+8+3)/250MHZ×5000＝12.7ms，可达到实时性的性能要求。

需要说明的是，在本文中，诸如“第一”和“第二”等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要速度过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要速度过程、方法、物品或者设备中还存在另外的相同要素。

以上所述仅是本发明的具体实施方式，使本领域技术人员能够理解或实现本发明。对这些实施例的多种修改对本领域的技术人员来说将是显而易见的，本文中所定义的一般原理可以在不脱离本发明的精神或范围的情况下，在其它实施例中实现。因此，本发明将不会被限制于本文所示的这些实施例，而是要符合与本文所申请的原理和新颖特点相一致的最宽的范围。

Claims (9)

1.一种基于SoC的KNN计算装置，其特征在于，包括：内存管理模块、数据交互模块和KNN计算模块；

所述内存管理模块用于对存储器进行区域划分，并将与所述数据交互模块对应的读区域和写区域的地址发送至所述数据交互模块；

所述数据交互模块用于从所述读区域中读取待处理数据，并将所述待处理数据传输至所述KNN计算模块，所述待处理数据包括目标特征点、多个第一特征点和多个第二特征点，所述目标特征点为当前帧激光点云数据的任一特征点，所述第一特征点为上一帧激光点云数据的特征点，所述第二特征点为地图点云数据的特征点；

所述KNN计算模块包括第一计算子模块和第二计算子模块；

所述第一计算子模块用于将所述目标特征点与多个所述第一特征点进行匹配计算，输出所述目标特征点的K个第一邻近特征点，其中K为预设值；

所述第二计算子模块用于对所述目标特征点进行RT位姿变换，并将RT位姿变换后的所述目标特征点与多个所述第二特征点进行匹配计算，输出所述目标特征点的K个第二邻近特征点；

所述数据交互模块还用于将所述目标特征点、所述第一邻近特征点和所述第二邻近特征点写入所述写区域；

所述数据交互模块包括读写命令子模块和读写数据子模块；

所述读写命令子模块用于接收读取命令或写入命令，并将所述读取命令或写入命令传输给所述读写数据子模块；

所述读写数据子模块用于在接收到所述读取命令后，从所述读区域读取所述待处理数据，并将所述待处理数据传输至所述KNN计算模块；

所述读写数据子模块还用于在接收到所述写入命令后，将所述目标特征点、所述第一邻近特征点和所述第二邻近特征点写入所述写区域。

2.根据权利要求1所述的装置，其特征在于，所述读写数据子模块包括先进先出存储器；

所述读写数据子模块在将所述目标特征点、所述第一邻近特征点和所述第二邻近特征点写入所述写区域之前，将所述目标特征点、所述第一邻近特征点和所述第二邻近特征点存储至所述先进先出存储器中，并在确定所述先进先出存储器中存储的数据量满足预设要求之后，将所述先进先出存储器中存储的所述目标特征点、所述第一邻近特征点和所述第二邻近特征点写入所述写区域。

3.根据权利要求2所述的装置，其特征在于，所述读写数据子模块将所述目标特征点、所述第一邻近特征点和所述第二邻近特征点存储至所述先进先出存储器时，分别统计所述第一邻近特征点的个数和所述第二邻近特征点的个数，并根据所述第一邻近特征点的个数和所述第二邻近特征点的个数确定所述先进先出存储器中存储的数据量是否满足预设要求。

4.根据权利要求3所述的装置，其特征在于，所述根据所述第一邻近特征点的个数和所述第二邻近特征点的个数确定所述先进先出存储器中存储的数据量是否满足预设要求，包括：

计算所述第一邻近特征点的个数和所述第二邻近特征点的个数之和，若计算得到的和值不小于第一阈值，则确定所述先进先出存储器中存储的数据量满足预设要求；

或

判断所述第一邻近特征点的个数是否小于第二阈值，和判断所述第二邻近特征点的个数是否小于第三阈值，若所述第一邻近特征点的个数不小于第二阈值，且所述第二邻近特征点的个数不小于第三阈值，则确定所述先进先出存储器中存储的数据量满足预设要求。

5.根据权利要求1所述的装置，其特征在于，

所述第一计算子模块包括第一距离计算单元和第一距离比较单元，所述第一距离计算单元包括多个并行运行的第一运算子单元；

所述第一运算子单元用于计算所述目标特征点和所述第一特征点之间的距离，输出第一距离值；

所述第一距离比较单元用于对所述第一计算子模块输出的多个所述第一距离值进行比较，输出所述目标特征点的K个第一邻近特征点；

所述第二计算子模块包括第二距离计算单元和第二距离比较单元，所述第二距离计算单元包括多个并行运行的第二运算子单元；

所述第二运算子单元用于计算RT位姿变换后的所述目标特征点和所述第二特征点之间的距离，输出第二距离值；

所述第二距离比较单元用于对所述第二计算子模块输出的多个所述第二距离值进行比较，输出所述目标特征点的K个第二邻近特征点。

6.根据权利要求5所述的装置，其特征在于，所述第一距离比较单元为K级流水结构，其中每一级流水结构用于查找当前级流水中的最小第一距离值以及该最小第一距离值对应的第一特征点，并将该第一特征点作为所述目标特征点的第一邻近特征点；

所述第二距离比较单元为K级流水结构，其中每一级流水结构用于查找当前级流水中的最小第二距离值以及该最小第二距离值对应的第二特征点，并将该第二特征点作为所述目标特征点的第二邻近特征点。

7.一种路径规划系统，其特征在于，包括PS子系统、PL子系统和存储器；

所述PL子系统包括如权利要求1-6任一项所述的基于SoC的KNN计算装置，所述基于SoC的KNN计算装置包括：内存管理模块、数据交互模块和KNN计算模块；

所述PS子系统用于采集激光点云数据和地图点云数据；

所述PS子系统中还包括特征提取模块，所述特征提取模块用于对所述激光点云数据和地图点云数据分别进行特征提取，得到所述激光点云数据的特征点和所述地图点云数据的特征点，并将所述激光点云数据的特征点和所述地图点云数据的特征点写入所述存储器中与所述数据交互模块对应的读区域；

所述PS子系统还用于从所述数据交互模块对应的写区域中读取目标特征点、目标特征点对应的第一邻近特征点和第二邻近特征点。

8.根据权利要求7所述的系统，其特征在于，所述PS子系统还用于根据所述目标特征点、目标特征点对应的第一邻近特征点和第二邻近特征点确定所述激光点云数据对应的激光雷达的位姿偏移量。

9.根据权利要求8所述的系统，其特征在于，所述PS子系统根据所述目标特征点、目标特征点对应的第一邻近特征点和第二邻近特征点确定所述激光点云数据对应的激光雷达的位姿偏移量，包括：

根据所述目标特征点和所述第一邻近特征点构建线和面，计算所述目标特征点到线和面的距离，得到第一距离项，由所述第一距离项对RT求偏导，构建雅克比矩阵，求解线性方程组得到的该特征点对应的实际偏移量；

根据所述目标特征点和所述第二邻近特征点构建线和面，计算所述目标特征点到线和面的距离，得到第二距离项，由所述第二距离项对RT求偏导，构建雅克比矩阵，求解线性方程组得到的对应的地图偏移量。

Images