CN109857702A - 一种基于机器人的激光雷达数据读写控制系统及芯片 - Google Patents

Abstract

本发明涉及一种基于机器人的激光雷达数据读写控制系统及芯片，该激光雷达数据读写控制系统包括CPU、AHB总线、预配置器、AHB接口控制器、AHB读写控制层、地址排序模块、地图匹配处理模块以及片上地图存储介质；AHB接口控制器连接片上地图存储介质，同时通过地址排序模块连接片上地图存储介质；AHB读写控制层分别连接AHB接口控制器和地图匹配处理模块；CPU同时与地图匹配处理模块和片上地图存储介质的相应信号端连接，预配置器与CPU连接存在电性连接；解决栅格地图数据不能被突发传输的问题，加速所述激光雷达数据和所述参考栅格数据的匹配运算速度，满足大批量所述激光雷达数据的处理场景。

Description

一种基于机器人的激光雷达数据读写控制系统及芯片

技术领域

本发明涉及数字电路领域，具体涉及一种基于机器人的激光雷达数据读写控制系统及芯片。

背景技术

现有技术中，机器人在采用360度的激光雷达扫描方式定位建图，其优点是建图成功率较高，能根据激光雷达数据实时矫正预先构建的地图。其中，激光雷达内置动作旋转机构能支持激光雷达360度旋转，机器人可能跟随定位目标物体在不断运动的，所以激光雷达每旋转一圈就得到一批激光点云数据。

机器人内置的激光导航模组在执行激光导航定位算法的过程中，其软件流程包括：激光导航模组中的CPU（处理器）发出访问指令，控制通信协议总线查找存储器所在的空间，再对存储器进行访问。访问成功则存储器会对通信协议总线返回成功响应信号，同时送出其他CPU希望得到的数据，通信协议总线将这些信号返回给CPU以完成一次指令的操作。由于CPU的工作频率往往高于存储器的读写频率，以至于CPU每发出一条指令对存储器进行操作时都要等待好几个时钟周期才能获得存储器的响应，大大降低了整个激光导航模组的运行效率。特别是构建的栅格地图对应的栅格数据是存储在离散地址单元上，容易造成CPU的内存访问50%以上是空操作，只作地址变化，而没有进行实际的读取操作。这些都浪费了大量的CPU时间。

所以，上述激光导航模组使用CPU对大批量的激光雷达数据（本领域技术人员可认为是激光点云数据）反复读写、比对是非常消耗软件资源的，并抢占了其他系统模块的CPU工作时间，影响其他模块的工作速度，尤其是使用大量的激光点云数据构建栅格地图的时候会大大降低CPU的工作效率，严重影响整个激光导航模组的性能，在大批量的激光点云数据传输的环境下，如何克服栅格地图数据不能被突发传输，以提高激光雷达数据的读写速度，是现有技术需解决的问题。

发明内容

为了克服上述技术缺陷，本发明提供一种基于机器人的激光点云数据读写控制系统，控制存储介质中的激光点云数据自动进出所述激光点云数据读写控制系统以减少CPU读写访问的次数，本发明的技术方案如下：

一种基于机器人的激光雷达数据读写控制系统，该激光雷达数据读写控制系统包括CPU和AHB总线，还包括AHB接口控制器、AHB读写控制层、地址排序模块、地图匹配处理模块以及片上地图存储介质；AHB接口控制器的一个信号输入端直接连接片上地图存储介质，用于基于AHB总线突发传输片上地图存储介质内存储的激光雷达数据；AHB接口控制器的另一个信号输入端通过地址排序模块连接片上地图存储介质，用于基于AHB总线突发传输经地址排序模块缓存排序的参考栅格数据；AHB读写控制层的信号端口分别连接AHB接口控制器和地图匹配处理模块的对应信号端，用于通过AHB接口控制器将片上地图存储介质内存储的激光雷达数据和参考栅格数据直接读取入地图匹配处理模块，还用于通过AHB接口控制器将地图匹配处理模块运算得到的定位数据直接写回片上地图存储介质；CPU同时与地图匹配处理模块和片上地图存储介质的相应信号端连接，用于在地图匹配处理模块完成预设采样数量的定位路标所采集的激光雷达数据的匹配运算后，从地图存储介质中直接提取地图匹配处理模块写入的定位数据送往软件应用层处理得到定位目标物体的位置信息；其中，参考栅格数据是根据预先构建的栅格地图配置在片上地图存储介质内的数组，栅格地图是根据激光雷达数据的坐标信息构建的，激光雷达数据是激光雷达采集的激光点云数据，激光点云数据是二维坐标数据；定位路标是根据所述机器人周边环境特征设定的；预设采样数量是激光雷达数据读写控制系统为实现定位目标物体定位而设定的定位路标采样数量。相比于现有技术，本发明通过在CPU与存储器之间增加读写控制电路，所述AHB接口控制器和所述地址排序模块的加入，解决栅格地图数据不能被突发传输的问题，加快所述AHB读写控制层读写效率，进而加速所述激光雷达数据和所述参考栅格数据的匹配运算速度，满足大批量所述激光雷达数据的处理场景；既能保证系统的稳定性，又保证AHB接口目标的灵活性，进而减少CPU访问频数，优化各个模块的读写时序。

进一步地，所述激光雷达数据读写控制系统还包括预配置器，预配置器与所述CPU连接存在电性连接，用于在所述激光雷达数据读写控制系统启动读写操作前，直接告知所述CPU配置所述片上地图存储介质的读写配置信息。预配置器辅助所述AHB接口控制器做出预取址的判断和操作，省去CPU对存储器发出取址指令，可以实现直接响应无需等待的效果，减缓CPU资源占用紧张的情况。

进一步地，所述片上地图存储介质包括激光点云数据存储阵列、栅格数据存储阵列和定位数据存储阵列；激光点云数据存储阵列，用于存储所述激光雷达采集的所述激光点云数据，并存储在连续的地址存储单元中；栅格数据存储阵列，用于存储所述参考栅格数据，并且存储在离散的地址存储单元中；所述地址排序模块连接栅格数据存储阵列，用于将栅格数据存储阵列输出的所述参考栅格数据按顺序缓存到连续地址的存储单元中；定位数据存储阵列，用于存储所述地图匹配处理模块运算得到的所述定位数据。相比于现有技术，本技术方案通过在栅格地图数据存储器和AHB接口控制器之间引入地址排序模块，将读取的栅格数据保存至连续地址的存储空间中，从而减少所述AHB接口控制器或者所述AHB读写控制层的读写访问次数，加快后续控制AHB总线突发读取栅格数据进行匹配性判断的速度，削减内存访问开销。

进一步地，所述AHB读写控制层包括第一突发读取模块、第二突发读取模块和单次写模块；第一突发读取模块，用于在所述CPU输出所述读写配置信息后，从所述激光点云数据存储阵列突发读取一个突发长度的所述激光点云数据，输出至所述地图匹配处理模块；第二突发读取模块，用于第一突发读取模块每完成一次突发读操作时，从所述地址排序模块突发读取一个突发长度的所述参考栅格数据，输出至所述地图匹配处理模块；单次写模块，用于当所述地图匹配处理模块每完成一个所述定位路标所对应采集的激光雷达数据的匹配运算后，控制所述地图匹配处理模块运算得到的所述定位数据写入所述地图存储介质；其中，第一突发读取模块、第二突发读取模块和单次写模块都是由硬件语言实现的状态机，该状态机是由状态寄存器和组合逻辑电路构成，第一突发读取模块和第二突发读取模块支持突发读操作，突发读操作是直接从存储阵列连续读取一个突发长度的连续地址单元上的数据；突发长度表示一次突发读操作下传输到所述AHB总线的数据的个数。本技术方案使得所述激光雷达数据读写控制系统通过AHB总线执行两次突发读操作来读写外部存储器件，加速所述激光雷达数据和所述参考栅格数据的匹配运算速度，减缓CPU资源占用紧张的情况。

进一步地，所述地图匹配处理模块中，所述匹配运算的过程包括：按照所述地址排序模块的存储单元对应的地址顺序，将输入所述地图匹配处理模块的所述突发长度的所述参考栅格数据进行累加，然后取平均值，得到参考栅格数据平均值；判断参考栅格数据平均值是否与所述第一突发读取模块输入的所述激光点云数据匹配，是则将参考栅格数据平均值保留在所述地图匹配处理模块内，等待下一次的匹配性判断再进行累加得到预处理值；否则加一运算作为预处理值；当判断完一个所述定位路标所对应采集的的所述激光点云数据与所述参考栅格数据的匹配情况后，计算预处理值的平均值，即所述定位数据，并将所述定位数据输出至所述片上地图存储介质；然后将预处理值清零再进行下一个所述定位路标的匹配运算，同时向所述CPU发送中断信号；其中，针对一个所述定位路标，所述激光点云数据与所述参考栅格数据的匹配性判断次数为，基于一个所述定位路标所述激光雷达采集的所述激光点云数据的两倍个数与所述突发长度的比值，也是预处理值求平均值所对应的频数。该技术方案利用预处理的参考栅格数据来矫正所述激光点云数据，提高建图的精准性，有利于提高所述激光雷达定位的准确性。

进一步地，所述地址排序模块包括一块FIFO存储器，包括读端口和写端口，读端口连接所述栅格数据存储阵列的输出端，用于在所述第一突发读取模块每完成一次所述突发读操作时，直接从所述栅格数据存储阵列读取所述突发长度的所述参考栅格数据，并依次写入FIFO存储器内部的连续地址空间内；写端口连接所述地图匹配处理模块的一个输入端，用于在所述第二突发读取模块执行所述突发读操作的过程中，按照所述栅格数据存储阵列写入FIFO存储器的顺序，从所述地址排序模块突发读取出所述突发长度的所述参考栅格数据并写入所述地图匹配处理模块。该技术方案为所述AHB读写控制层提供连续地址单元上的所述参考栅格数据，以便于执行所述突发读操作；所述FIFO存储器中，连续的数据流读写可避免频繁的总线操作，减轻CPU的负担。

进一步地，所述AHB接口控制器用于控制转换所述第一突发读取模块、所述第二突发读取模块和所述单次写模块所对应的所述状态机的工作状态，并反馈至所述AHB总线，进而控制所述AHB总线上的数据突发传输。该技术方案能够及时切换所述AHB读写控制层对AHB总线使用权，保证所述激光雷达数据读写控制系统效率的最大化。

进一步地，所述激光点云数据存储阵列、所述栅格数据存储阵列和所述定位数据存储阵列都采用DDR4颗粒。提高数据传输的带宽。

一种芯片，该芯片包括所述激光雷达数据读写控制系统。该芯片借助AHB接口及相关缓冲模块读写经过整理的来自片上存储器件的数据，进而加速所述激光雷达数据和所述参考栅格数据的匹配运算速度，满足大批量所述激光雷达数据的处理场景，既保证数据的传输速度，又保证芯片的性能。

附图说明

图1为本发明实施例提供的一种基于机器人的激光雷达数据读写控制系统的系统框架图。

具体实施方式

本发明实施方式中所涉及到的各模块均为基于verilog硬件编程语言在EDA工具下映射生成的逻辑电路模块，在实际应用中，一个逻辑电路模块可以是一个逻辑电路单元，也可以是一个逻辑电路单元的一部分，还可以以多个逻辑电路单元的组合实现。此外，为了突出本发明的创新部分，本发明实施方式中并没有将与解决本发明所提出的技术问题关系不太密切的逻辑电路模块引入，但这并不表明本发明实施方式中不存在其它的模块。下面结合附图对本发明的具体实施方式作进一步说明。

机器人采用360度的激光雷达扫描方式定位建图，其优点是建图成功率较高，能根据采集的激光雷达数据实时矫正预先构建的地图。具体地，激光雷达内置动作旋转机构能支持激光雷达360度旋转，同时控制机器人跟随定位目标物体运动，所以激光雷达每旋转一圈就得到一批激光点云数据，即所述激光雷达数据。本发明实施例中，采用逻辑电路模块控制所述激光点云数据连续与预先构建并存储的地图进行匹配，从而得到矫正后的定位目标物体的位置信息。这里的定位目标物体可以包括静态的物体和动态的物体。

本发明实施例提供一种基于机器人的激光雷达数据读写控制系统，该激光雷达数据读写控制系统包括CPU和AHB总线，CPU与AHB总线之间存在取址指令的信号联系。该激光雷达数据读写控制系统还包括用于配置取址指令的预配置器、选择FIFO作为数据传输通道的地址排序模块、内置地图匹配算法的逻辑架构的地图匹配处理模块、用于存储所述激光雷达数据和所述参考栅格数据的片上地图存储介质、用于控制AHB总线的数据解析传输的AHB接口控制器和用于按照时序访问AHB总线的AHB读写控制层。如图1所示，AHB接口控制器的一个信号输入端直接连接片上地图存储介质，用于基于AHB总线突发传输片上地图存储介质内存储的激光雷达数据；AHB接口控制器的另一个信号输入端通过地址排序模块连接片上地图存储介质，用于基于AHB总线突发传输经地址排序模块缓存排序的参考栅格数据，AHB接口控制器仅通过读取地址排序模块的数据缓存状态，控制地址排序模块实现进入的数据存储地址重排序，并按照写入顺序输出至所述AHB读写控制层，被所述AHB读写控制层突发读取连续地址单元上的数据，实现数据的先入先出，简化AHB总线上挂载的工作模块的接口设计。进而解决栅格地图数据不能被突发传输的问题，提高所述AHB读写控制层读写效率。其中，地址排序模块刚开始从片上地图存储介质读取所述参考栅格数据时，地址排序模块为空；当地址排序模块被读入的所述参考栅格数据填满后，所述AHB接口控制器可以从地址排序模块突发传输一个突发长度的所述参考栅格数据，并结合所述AHB读写控制层的控制作用，将这个突发长度的所述参考栅格数据突发传输进入所述地图匹配处理模块参与匹配运算，从而加快所述AHB读写控制层读写效率，进而加速所述激光雷达数据和所述参考栅格数据的匹配运算速度。因此，本发明实施例满足大批量所述激光雷达数据的处理场景。

AHB读写控制层的信号端口分别连接AHB接口控制器的对应信号端和地图匹配处理模块的对应信号端，用于通过AHB接口控制器将片上地图存储介质内存储的激光雷达数据和参考栅格数据直接读取入地图匹配处理模块，地图匹配处理模块中专门设有缓冲寄存器组中，等待缓冲AHB读写控制层内的相应状态机控制传输入所述激光雷达数据和所述参考栅格数据；AHB读写控制层还用于通过AHB接口控制器将地图匹配处理模块运算得到的定位数据直接写回片上地图存储介质。AHB接口控制器能跟踪AHB突发传输状态的比特计数器，AHB读写控制层包括对AHB总线载入控制信号的有限状态机（FSM）控制器，AHB读写控制层及AHB接口控制器相当于CPU和片上地图存储介质之间的AHB桥，用于解析总线协议指令，将总线传送的所述激光雷达数据和所述参考栅格数据对应的数据信号和地址信号进行转化，然后按照总线地址信号将总线上的从AHB接口控制器送入AHB读写控制层；本发明实施中使用的总线协议是AHB总线协议，但不排除按照判断匹配和运算要求使用其他总线协议来完成数据的传输。

CPU同时与地图匹配处理模块和片上地图存储介质的相应信号端连接，用于在地图匹配处理模块完成预设采样数量的定位路标所采集的激光雷达数据的匹配运算后，从地图存储介质中直接提取地图匹配处理模块输出的定位数据送往软件应用层进行换算，得到定位目标物体的位置信息；地图匹配处理模块将接收/发送中断信号IRQ传送给CPU并报告结束信号；地图匹配处理模块还用于接受所述AHB读写控制层的工作状态信息和CPU的配置信息，并发出中断通知CPU去处理所述定位数据。其中，软件应用层是区别于激光雷达数据读写控制系统的顶层应用程序，用于根据地图匹配处理模块输出的定位数据进行坐标换算得到定位目标物体的位置信息。所述预设采样数量是激光雷达数据读写控制系统为实现定位目标物体定位而设定的定位路标采样数量。

优选地，预配置器与CPU连接存在电性连接，用于在所述激光雷达数据读写控制系统启动读写操作前，直接告知CPU配置片上地图存储介质的读写配置信息。在所述地图匹配处理模块完成所述预设采样数量的定位路标所采集的激光雷达数据的匹配运算后，预配置器辅助所述AHB接口控制器做出预取址的判断和操作，预配置器通过所述AHB接口控制器配置针对同一个定位路标所采集的激光雷达数据的个数、所述预设采样数量、所述栅格数据存储阵列的读基地址、所述激光点云数据存储阵列的读基地址和所述定位数据存储阵列的写基地址，从而使得AHB接口作为一个高速接口读取所述激光雷达数据和所述参考栅格数据的数据流，简化了接口设计和总线通信时序，进一步省去CPU对存储器发出的取址指令，可以实现直接响应无需等待的效果，减缓CPU资源占用紧张的情况。所述预配置器存储有的读写配置信息包括针对同一个定位路标所采集的激光雷达数据的个数、所述预设采样数量、所述栅格数据存储阵列的读基地址、所述激光点云数据存储阵列的读基地址和所述定位数据存储阵列的写基地址。

相比于现有技术，本发明实施例在所述CPU与所述片上地图存储介质之间，加入所述AHB接口控制器和所述地址排序模块，拉近系统中的存储器件和运算控制器的信号联系，使得所述片上地图存储介质及其外部与AHB总线相关的电路模块形成一个整体，加快所述AHB读写控制层读写效率，进而加速所述激光雷达数据和所述参考栅格数据的匹配运算速度。因此，本发明实施例满足大批量所述激光雷达数据的处理场景；既能保证系统的稳定性，又保证AHB接口目标的灵活性，进而减少CPU访问频数，优化各个模块的读写时序。

需要说明的是，所述参考栅格数据是根据预先构建的栅格地图配置在所述片上地图存储介质内的二维数组，栅格地图是根据所述激光雷达数据的坐标信息构建的，所述激光雷达数据是所述激光雷达采集的所述激光点云数据，所述激光点云数据也是二维坐标数据。

优选地，所述片上地图存储介质包括激光点云数据存储阵列、栅格数据存储阵列和定位数据存储阵列；激光点云数据存储阵列，用于存储所述激光雷达采集的所述激光点云数据，即所述激光雷达360度旋转过程中连续采集的所述激光点云数据的集合，并存储在连续的地址存储单元中，具体地，本发明实施中，所述激光雷达数据读写控制系统在本发明实施例中定位采样9个激光路标，所述预设采样数量设置为9，针对每一个所述定位路标采集的320个激光点云数据（2的倍数），对应320个二维坐标点，每个坐标点包括x轴方向的16位数据和y轴方向的16位数据，即总共640个数据。栅格数据存储阵列，用于存储所述参考栅格数据，并且存储在离散的地址存储单元中；所述栅格数据存储阵列的位宽定义为16位，并以map[MAP_H][MAP_W]的二维数组的形式保存在每个存储单元中，使得所述栅格数据存储阵列中具体的存储单元有相对应的索引号，方便外部的访问，其中变量MAP_H和变量MAP_W分别代表所述栅格数据存储阵列的长和宽，它们的数值大小都设置为801。定位数据存储阵列，用于存储所述地图匹配处理模块运算得到的所述定位数据。

需要说明的是，在所述片上地图存储介质内部是一个存储阵列，将数据“填”进去，和表格的检索原理一样，先指定一个行，再指定一个列，就准确地找到所需要的单元格，这就是内存芯片寻址的基本原理。对于内存，这个单元格可称为存储单元,那么这个表格（存储阵列）就是逻辑 Bank（Logical Bank，下面简称Bank）。本发明实施在所述片上地图存储介质中，使用3个Bank分别作为所述激光点云数据存储阵列、所述栅格数据存储阵列和所述定位数据存储阵列。在所述AHB接口控制器与所述片上地图存储介质进行数据交互的过程中，发出的地址都是对齐的，所述对齐地址是对所述片上地图存储介质的存储空间（逻辑Bank）的一种划分，在外部访问突发读取数据时，要以这种划分为前提进行，对齐地址是由每拍突发传输的数据的宽度来决定的。

所述地址排序模块连接栅格数据存储阵列，用于将栅格数据存储阵列输出的所述参考栅格数据按顺序缓存到连续地址的存储单元中；本发明实施例中，所述地址排序模块的位宽定义为32位，存储深度设置为16。由于所述参考栅格数据是离散的量化栅格值，所述栅格数据存储阵列中所述参考栅格数据对应的存储地址不是连续的，所以所述参考栅格数据不支持所述AHB接口控制器直接突发传输，所以本发明实施例在所述栅格数据存储阵列的输出端引入了地址排序模块，通过控制所述参考栅格数据进入地址排序模块后依次缓冲到连续的地址单元中，使得缓冲到所述地址排序模块中的数据存储地址都是连续，相邻的两个所述参考栅格数据对应的存储地址也是相邻的，然后等待所述AHB接口控制器突发读取所述地址排序模块的缓冲排序的所述参考栅格数据。相比于现有技术，本技术方案通过在栅格数据存储阵列和AHB接口控制器之间引入地址排序模块，将读取的栅格数据保存至连续地址的存储空间中，从而减少所述AHB接口控制器或者所述AHB读写控制层的读写访问次数，加快后续控制AHB总线突发读取栅格数据进行匹配性判断的速度，削减内存访问开销。

所述AHB读写控制层包括第一突发读取模块、第二突发读取模块和单次写模块，第一突发读取模块、第二突发读取模块和单次写模块都是由硬件语言编写的状态机实现的，该状态机是由状态寄存器和组合逻辑电路构成。所述AHB接口控制器用于控制转换所述第一突发读取模块、所述第二突发读取模块和所述单次写模块所对应的所述状态机的工作状态，并反馈至所述AHB总线，进而控制所述AHB总线上的数据突发传输。需要说明的是，根据AHB的时序规定，所述AHB接口控制器提供的地址与控制信号要先于数据发送，地址和数据传输发生在不同的周期，与这笔传输的地址段传输的同时发生的是上一笔传输的数据段传输。这种地址与数据的交叠是AHB总线流水线的特征，允许高速传输，并为所述片上地图存储介质响应一笔传输留出充裕的时间。AHB是采用流水线技术，分为地址阶段和数据阶段。所以，当控制信号和地址信号切换的时候，数据信号并不能切换，而是要保证最后一个控制信号要读或者写的数据完成才可以。它会监测所述AHB读写控制层和所述片上地图存储介质的情况，及时切换所述AHB读写控制层对AHB总线使用权，保证所述激光雷达数据读写控制系统效率的最大化。

作为本发明的一种实施方式，CPU发出中断使能指令，开始标志位置1，所述AHB读写控制层开始从初始状态跳至突发读工作状态，交由所述第一突发读取模块进行数据处理工作。在本发明实施中，所述激光雷达基于第一个定位路标采集了320个所述激光点云数据（2的倍数），对应320个二维坐标点，每个坐标点包括x轴方向的16位数据和y轴方向的16位数据，即总共640个数据。

所述第一突发读取模块，用于在所述CPU输出所述读写配置信息后，在所述AHB接口控制器的控制下，从所述激光点云数据存储阵列中已配置的读基地址开始，突发读取一个突发长度的所述激光点云数据，在本实施例中，所述突发长度设置为16个，在一次操作中连续读取所述激光点云数据存储阵列内当前一行中框定的16个地址单元，并且将读取的所述激光点云数据通过映射关系换算后变为待匹配的激光点云数据的集合，输出至所述地图匹配处理模块，用来在后续参与突发读取的所述参考栅格数据的匹配判断。其中，所述读写配置信息包括所述栅格数据存储阵列的读基地址、所述激光点云数据存储阵列的读基地址、所述定位数据存储阵列的写基地址、针对同一个定位路标所采集的激光雷达数据的个数和所述预设采样数量；突发长度表示一次突发读操作下所述激光点云数据传输到所述AHB总线的个数。

所述第二突发读取模块，用于所述第一突发读取模块每完成一次突发读操作时，从所述地址排序模块的首地址开始突发读取一个突发长度的所述参考栅格数据，在本实施例中，所述突发长度设置为16个，所述地址排序模块的存储深度设置为16个，在一次操作中连续读取所述地址排序模块中16相邻地址单元上的所述参考栅格数据，然后输出16个待匹配的参考栅格数据至所述地图匹配处理模块，其中，突发长度表示一次突发读操作下所述参考栅格数据传输到所述AHB总线的个数。

需要说明的是，所述第一突发读取模块和所述第二突发读取模块支持突发读操作，突发读操作是直接从存储阵列连续读取一个突发长度的连续地址单元上的数据。使得所述激光雷达数据读写控制系统通过AHB总线执行两次突发读操作来读写外部存储器件，加速所述激光雷达数据和所述参考栅格数据的匹配运算速度，减缓CPU资源占用紧张的情况。

当所述第二突发读取模块完成一次突发读操作后，所述地图匹配处理模块开始执行所述匹配运算，具体过程包括：所述地图匹配处理模块按照所述地址排序模块的存储单元对应的地址顺序，将输入所述地图匹配处理模块的16个所述参考栅格数据进行累加，对x轴的栅格坐标数据累加取平均值，同时对y轴的栅格坐标数据累加取平均值，得到栅格坐标的平均值，即作为参考栅格数据平均值；然后判断所述参考栅格数据平均值是否与所述第一突发读取模块输入的16个所述激光点云数据的其中一个相匹配，是则将所述参考栅格数据平均值保留在所述地图匹配处理模块的结果缓冲区中，等待下一次的匹配性判断再进行累加得到预处理值；否则加一运算作为预处理值。与现有技术相比，所述第二突发读取模块可以一次性处理16个所述参考栅格数据，从而减少所述地图匹配处理模块的匹配判断的次数。因为现有技术中使用单次读的方式获取所述栅格数据存储阵列中的所述参考栅格数据，每次只能读取2个数据（x轴数据和y轴数据），所以基于16个所述激光点云数据需要进行8次匹配判断，而本发明实施例一次性读取16个所述参考栅格数据并取平均值再进行一次匹配判断即可，既满足栅格地图数据的相似性匹配要求，又提高总线读写效率，进而提高栅格数据的匹配速度。需要说明的是，前述的匹配是所述激光雷达在局部环境采集的激光点云数据匹配到已经建立的参考栅格地图位置上，所采用的主要算法包括迭代最近点、图优化计算等本领域技术人员常用的算法，故不再赘述。

当所述地图匹配处理模块根据所述匹配运算获取一个所述预处理值时，所述AHB接口控制器继续控制所述第一突发读取模块继续突发读取所述激光点云数据存储阵列的下一行内框定的16个连续的地址单元，并且将读取的所述激光点云数据通过映射关系换算后变为待匹配的激光点云数据的集合，输出至所述地图匹配处理模块；接着，在所述AHB接口控制器的工作状态转换作用下，所述第二突发读取模块继续从所述地址排序模块的首地址开始突发读取一个突发长度的所述参考栅格数据，输出至所述地图匹配处理模块；所述地图匹配处理模块又开始执行所述匹配运算以累加所述预处理值，从而更新所述预处理值。其目的是为了获得新的定位数据以拼接到已经建立的参考栅格地图中完成地图融合更新，提高机器人激光定位的准确性。

如此重复循环所述预处理值获取过程40次，直到处理完所述激光点云数据存储阵列上的640个数据（对应320个所述激光点云数据）以及相匹配的所述参考栅格数据的累加运算，当判断完一个所述定位路标所对应采集的的所述激光点云数据与所述参考栅格数据的匹配情况后，计算预处理值的平均值，即所述定位数据，并在单次写模块的控制作用下将所述定位数据输出至所述片上地图存储介质。其中，单次写模块，用于当所述地图匹配处理模块每完成一个所述定位路标所对应采集的激光雷达数据的匹配运算后，输出制所述地图匹配处理模块运算得到的所述定位数据，并控制这些待处理的定位数据通过所述AHB接口控制器写入所述地图存储介质，具体从所述定位数据存储阵列的写基地址开始写入所述地图存储介质，使得所述激光雷达数据读写控制系统通过AHB总线执行两次突发读操作来读写外部存储器件，加速所述激光雷达数据和所述参考栅格数据的匹配运算速度，减缓CPU资源占用紧张的情况。

然后将所述预处理值清零再进行下一个所述定位路标的匹配运算，同时向所述CPU发送中断信号；其中，针对一个所述定位路标，所述激光点云数据与所述参考栅格数据的匹配性判断次数为，一个所述定位路标对应采集的所述激光点云数据的两倍个数与所述突发长度的比值，也是预处理值求平均值所对应的频数。在本实施例中，一个所述定位路标对应采集的所述激光点云数据的两倍个数为640，所述突发长度为16，则所述激光点云数据与所述参考栅格数据的匹配性判断次数为40。所述地图匹配处理模块处理的所述定位路标为9个，当处理完9个所述定位路标对应的所述激光点云数据后，得到9个所述定位数据。并且，每处理完1个所述定位路标的同时，将运算得到的所述定位数据写入所述定位数据存储阵列，每写入所述定位数据存储阵列9个所述定位数据，则所述地图匹配处理模块发出的相应中断通知给CPU，便于软件应用层进行下一步的操作，不需要CPU重复查询等待每一次状态转换的结果，节省软件指令开销，减小整个过程的耗时。

为了实现定位目标物体定位，所述CPU从所述地图存储介质中直接提取地图匹配处理模块写入的9个所述定位数据送往软件应用层进行处理，具体为：根据所述定位数据及三角几何关系计算出各个所述定位路标与机器的距离，再结合离线理论值进行比较，从而在考虑数量的基础上进行位置信息的修正，最终换算出激光雷达的位置信息，又由于所述机器人被控制为与定位目标物体的位置保持预设距离，所述预设距离是设定保存在所述地图匹配处理模块中。软件应用层是区别于激光雷达数据读写控制系统的顶层应用程序。本发明实施例利用预处理的参考栅格数据来矫正所述激光点云数据，提高建图的精准性，有利于提高所述激光雷达定位的准确性。

优选地，所述地址排序模块包括一块FIFO存储器，FIFO存储器是一个先入先出的双口缓冲器，即第一个进入其内的数据第一个被移出，其中一个存储器的输入口，本发明实施例定义为读端口，另一个口是存储器的输出口，本发明实施例定义为写端口，读端口连接所述栅格数据存储阵列的输出端，用于在所述第一突发读取模块每完成一次所述突发读操作时，直接从所述栅格数据存储阵列读取所述突发长度的所述参考栅格数据，并依次写入FIFO存储器内部的连续地址空间中，所述参考栅格数据的存储地址值由离散变为连续，起到重排序的作用，为后续的突发读操作提供必要的传输条件。写端口连接所述地图匹配处理模块的一个输入端，用于在所述第二突发读取模块执行所述突发读操作的过程中，按照所述栅格数据存储阵列写入FIFO存储器的顺序，从所述地址排序模块突发读取出所述突发长度的所述参考栅格数据并写入所述地图匹配处理模块，从而转换为满状态，此时，所述地址排序模块被读取走的所述突发长度的所述参考栅格数据之后，FIFO存储器内部腾出16个空的连续地址空间，从而转换为空状态，使得读端口可接受所述栅格数据存储阵列输入的所述突发长度的新的所述参考栅格数据。本发明实施例使用一个FIFO，而不是一个读FIFO和一个写FIFO组合成一个缓冲模块，必须要等待写FIFO满再进入读FIFO，然后再从读FIFO输出至所述地图匹配处理模块，减小逻辑上的开销，简化控制接口的设计，提高数据流的传输速率。从而为所述AHB读写控制层提供连续地址单元上的所述参考栅格数据，以便于执行所述突发读操作；所述FIFO存储器中，连续的数据流读写可避免频繁的总线操作，减轻CPU的负担。

优选地，所述激光点云数据存储阵列、所述栅格数据存储阵列和所述定位数据存储阵列都采用DDR4颗粒。提高数据传输的带宽。在一个实施例中，所述片上地图存储介质可以包括3组存储阵列。每一组所述存储阵列可以包括多个DDR4颗粒(芯片)。在一个实施例中，所述存储阵列可以包括32位DDR4颗粒(芯片)，32bit用于传输数据。可以理解，当每一组所述存储阵列中采用DDR4-3200颗粒时，数据传输的理论带宽可达到25600MB/s。

一种芯片，该芯片包括所述激光雷达数据读写控制系统。该芯片借助AHB接口及相关缓冲模块读写经过整理的来自片上存储器件的数据，进而加速所述激光雷达数据和所述参考栅格数据的匹配运算速度，满足大批量所述激光雷达数据的处理场景，既保证数据的传输速度，又保证芯片的性能。

在本申请所提供的实施例中，应该理解到，所揭露的系统、装置和方法，可以通过其它的方式实现。例如，以上所描述的装置实施例仅仅是示意性的，例如，所述单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些接口，装置或单元的间接耦合或通信连接，可以是电性，机械或其它的形式。所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目。

Claims (9)

1.一种基于机器人的激光雷达数据读写控制系统，该激光雷达数据读写控制系统包括CPU和AHB总线，其特征在于，还包括AHB接口控制器、AHB读写控制层、地址排序模块、地图匹配处理模块以及片上地图存储介质；

AHB接口控制器的一个信号输入端直接连接片上地图存储介质，用于基于AHB总线突发传输片上地图存储介质内存储的激光雷达数据；AHB接口控制器的另一个信号输入端通过地址排序模块连接片上地图存储介质，用于基于AHB总线突发传输经地址排序模块缓存排序的参考栅格数据；

AHB读写控制层的信号端口分别连接AHB接口控制器和地图匹配处理模块的对应信号端，用于通过AHB接口控制器将片上地图存储介质内存储的激光雷达数据和参考栅格数据直接读取入地图匹配处理模块，还用于通过AHB接口控制器将地图匹配处理模块运算得到的定位数据直接写回片上地图存储介质；

CPU同时与地图匹配处理模块和片上地图存储介质的相应信号端连接，用于在地图匹配处理模块完成预设采样数量的定位路标所采集的激光雷达数据的匹配运算后，从地图存储介质中直接提取地图匹配处理模块写入的定位数据送往软件应用层处理得到定位目标物体的位置信息；

其中，参考栅格数据是根据预先构建的栅格地图配置在片上地图存储介质内的数组，栅格地图是根据激光雷达数据的坐标信息构建的，激光雷达数据是激光雷达采集的激光点云数据，激光点云数据是二维坐标数据；

定位路标是根据所述机器人周边环境特征设定的；预设采样数量是激光雷达数据读写控制系统为实现定位目标物体定位而设定的定位路标采样数量。

2.根据权利要求1所述激光雷达数据读写控制系统，其特征在于，所述激光雷达数据读写控制系统还包括预配置器，预配置器与所述CPU连接存在电性连接，用于在所述激光雷达数据读写控制系统启动读写操作前，直接告知所述CPU配置所述片上地图存储介质的读写配置信息。

3.根据权利要求2所述激光雷达数据读写控制系统，其特征在于，所述片上地图存储介质包括激光点云数据存储阵列、栅格数据存储阵列和定位数据存储阵列；

激光点云数据存储阵列，用于存储所述激光雷达采集的所述激光点云数据，并存储在连续的地址存储单元中；

栅格数据存储阵列，用于存储所述参考栅格数据，并且存储在离散的地址存储单元中；所述地址排序模块连接栅格数据存储阵列，用于将栅格数据存储阵列输出的所述参考栅格数据按顺序缓存到连续地址的存储单元中；

定位数据存储阵列，用于存储所述地图匹配处理模块运算得到的所述定位数据。

4.根据权利要求3所述激光雷达数据读写控制系统，其特征在于，所述AHB读写控制层包括第一突发读取模块、第二突发读取模块和单次写模块；

第一突发读取模块，用于在所述CPU输出所述读写配置信息后，从所述激光点云数据存储阵列突发读取一个突发长度的所述激光点云数据，输出至所述地图匹配处理模块；

第二突发读取模块，用于第一突发读取模块每完成一次突发读操作时，从所述地址排序模块突发读取一个突发长度的所述参考栅格数据，输出至所述地图匹配处理模块；

单次写模块，用于当所述地图匹配处理模块每完成一个所述定位路标所对应采集的激光雷达数据的匹配运算后，控制所述地图匹配处理模块运算得到的所述定位数据写入所述地图存储介质；

其中，第一突发读取模块、第二突发读取模块和单次写模块都是由硬件语言实现的状态机，该状态机是由状态寄存器和组合逻辑电路构成，第一突发读取模块和第二突发读取模块支持突发读操作，突发读操作是直接从存储阵列连续读取一个突发长度的连续地址单元上的数据；突发长度表示一次突发读操作下传输到所述AHB总线的数据的个数。

5.根据权利要求4所述激光雷达数据读写控制系统，其特征在于，所述地图匹配处理模块中，所述匹配运算的过程包括：

按照所述地址排序模块的存储单元对应的地址顺序，将输入所述地图匹配处理模块的所述突发长度的所述参考栅格数据进行累加，然后取平均值，得到参考栅格数据平均值；

判断参考栅格数据平均值是否与所述第一突发读取模块输入的所述激光点云数据匹配，是则将参考栅格数据平均值保留在所述地图匹配处理模块内，等待下一次的匹配性判断再进行累加得到预处理值；否则加一运算作为预处理值；

当判断完一个所述定位路标所对应采集的的所述激光点云数据与所述参考栅格数据的匹配情况后，计算预处理值的平均值，即所述定位数据，并将所述定位数据输出至所述片上地图存储介质；

然后将预处理值清零再进行下一个所述定位路标的匹配运算，同时向所述CPU发送中断信号；

其中，针对一个所述定位路标，所述激光点云数据与所述参考栅格数据的匹配性判断次数为，基于一个所述定位路标所述激光雷达采集的所述激光点云数据的两倍个数与所述突发长度的比值，也是预处理值求平均值所对应的频数。

6.根据权利要求5所述激光雷达数据读写控制系统，其特征在于，所述地址排序模块包括一块FIFO存储器，包括读端口和写端口，

读端口连接所述栅格数据存储阵列的输出端，用于在所述第一突发读取模块每完成一次所述突发读操作时，直接从所述栅格数据存储阵列读取所述突发长度的所述参考栅格数据，并依次写入FIFO存储器内部的连续地址空间内；

写端口连接所述地图匹配处理模块的一个输入端，用于在所述第二突发读取模块执行所述突发读操作的过程中，按照所述栅格数据存储阵列写入FIFO存储器的顺序，从所述地址排序模块突发读取出所述突发长度的所述参考栅格数据并写入所述地图匹配处理模块。

7.根据权利要求4所述激光雷达数据读写控制系统，其特征在于，所述AHB接口控制器用于控制转换所述第一突发读取模块、所述第二突发读取模块和所述单次写模块所对应的所述状态机的工作状态，并反馈至所述AHB总线，进而控制所述AHB总线上的数据突发传输。

8.根据权利要求3所述激光雷达数据读写控制系统，其特征在于，所述激光点云数据存储阵列、所述栅格数据存储阵列和所述定位数据存储阵列都采用DDR4颗粒。

9.一种芯片，其特征在于，该芯片包括权利要求1至8任一项所述激光雷达数据读写控制系统。