CN114443264A - 一种基于硬件加速的激光重定位系统及芯片 - Google Patents

Abstract

本发明涉及一种基于硬件加速的激光重定位系统及芯片，该激光重定位系统采用硬件计算电路模块对读取过来的大批量的激光点云数据进行分块处理，并在此基础上按照AHB总线读写时序变化，对即时扫描获得的激光点云数据进行栅格位置概率进行分批量的遍历累加运算，再按照AHB总线读写时序变化自动写入专门的存储介质中以供CPU软件选择处理完成定位，代替CPU软件反复迭代处理数据，对CPU的主频要求不高，减少对外部存储介质的访问读写次数，比如不占用大量DDR带宽，还能兼顾激光探头旋转或激光重定位系统移动过程中产生的误差，整个激光重定位系统不在静止状态都保持较高的系统性能，提高客户的模组开发体验度。

Description

一种基于硬件加速的激光重定位系统及芯片

技术领域

本发明涉及硬件电路加速的技术领域，具体涉及一种基于硬件加速的激光重定位系统及芯片。

背景技术

目前，针对扫地机利用激光地图进行重定位，业界多是采用的软件算法实现，进行大量迭代计算处理激光点云，从而求出在地图上的目标位置信息。这种方法会占用大量的硬件资源，而且对CPU的主频要求很高，并且会占用大量的DDR带宽，挤占系统中其他应用程序对于DDR的带宽需求，从而造成系统性能下降，客户体验变差。

比如，中国发明专利申请2019111761598公开一种定位恢复方法、装置、机器人及存储介质，结合设定时间内所处区域的分辨率和定位栅格的概率进行定位恢复操作，这种方法增加扫地机在导航行走过程中的计算量，并且会占用较多的DDR带宽，从而造成系统重定位的速度下降。而且机器人需要保持在静止状态下才能进行，具有一定的局限性。

发明内容

为了解决上述技术问题，本发明技术方案采用硬件电路的方式，利用专用存储模块保存激光扫描出的分块地图，然后将重定位软件算法硬件化，利用硬件电路的方式完成对激光点云的换算，然后进行遍历求概率和，再将概率和写入专门的存储器件中，实现由软件仅仅通过概率和比较就得出目标点在地图上的位置信息。具体的技术方案如下：

本发明公开一种基于硬件加速的激光重定位系统，该激光重定位系统的外围连接有地图信息存储介质与概率和值存储介质，该激光重定位系统的外围接口装配有激光探头；该激光重定位系统包括读写状态控制器、AHB总线解析模块、激光点云寄存处理模块、横纵坐标计算模块以及栅格信息遍历累加模块；读写状态控制器与横纵坐标计算模块存在电性连接关系，读写状态控制器用于在AHB总线解析模块的传输控制作用下，将地图信息存储介质内部存储的激光点云数据突发读取入该读写状态控制器内部的缓存区，再传输给横纵坐标计算模块；其中，突发读取到的激光点云数据携带有采样信息；横纵坐标计算模块，用于在读写状态控制器每完成一次突发读取操作后，根据突发读取的一个突发传输长度的激光点云数据的采样信息和角度偏移信息换算出这批激光点云数据在预配置的栅格地图上发生角度偏移的参考坐标；激光点云寄存处理模块与横纵坐标计算模块存在电性连接关系，激光点云寄存处理模块用于以存储阵列的形式缓存横纵坐标计算模块换算出的参考坐标；读写状态控制器与激光点云寄存处理模块存在电性连接关系，读写状态控制器，用于在AHB总线解析模块的传输控制作用下，单次读取激光点云寄存处理模块缓存的参考坐标，并将其传输给栅格信息遍历累加模块；栅格信息遍历累加模块与读写状态控制器存在电性连接关系，栅格信息遍历累加模块还与横纵坐标计算模块存在电性连接关系，用于在读写状态控制器将激光探头旋转采样到的预设待测数量的激光点云数据全部突发读取进来、并经过横纵坐标计算模块换算处理、激光点云寄存处理模块缓存处理后，在读写状态控制器的单次读取操作作用下，遍历激光点云寄存处理模块内缓存的所有参考坐标，再将每个参考坐标在预配置的栅格地图上的定位概率累加处理，然后分别控制这些参考坐标沿着坐标轴方向偏移，使得这些参考坐标在所述横纵坐标计算模块的角度偏移信息的触发作用下，每完成一次偏移处理则执行累加以得到一个定位概率和值；读写状态控制器分别与概率和值存储介质和地图信息存储介质都存在电性连接，读写状态控制器，还用于在AHB总线解析模块的传输控制作用下，每当栅格信息遍历累加模块获得的定位概率和值的个数达到一个突发传输长度时，将这些定位概率和值突发写入概率和值存储介质；其中，预设待测数量的激光点云数据全部被处理为对应偏移位置处的定位概率和值并都写入概率和值存储介质后，外部CPU最终选择定位概率和值最大的一个对应的栅格位置作为所述激光重定位系统在预配置的栅格地图上定位恢复的位置。

与现有技术相比，本技术方案采用硬件计算电路模块对读取过来的大批量的激光点云数据进行分块处理，并在此基础上按照AHB总线读写时序变化，对即时扫描获得的激光点云数据进行栅格位置概率进行分批量的累加运算，再按照AHB总线读写时序变化自动写入专门的存储介质中以供CPU软件选择处理完成定位，减少对外部存储介质的访问读写次数。

因此，本技术方案采用硬件电路读写的方式来代替CPU软件反复迭代处理数据，对CPU的主频要求不高，在使用这个激光重定位系统去读写外部的DDR存储器（概率和值存储介质是DDR）时，不会占用大量DDR带宽，还能兼顾激光探头旋转或激光重定位系统移动过程中产生的误差，整个激光重定位系统不在静止状态都保持较高的系统性能，提高模组开发体验度。

进一步地，所述读写状态控制器包括突发读取模块，突发读取模块内部设置第一FIFO模块，所述突发读取模块在所述AHB总线解析模块的每一次突发传输控制作用下，将一个突发传输长度的激光点云数据突发读取入第一FIFO模块，再交由第一FIFO模块以先入先出的存储方式将读取到的激光点云数据传输给所述横纵坐标计算模块。本技术方案通过增加基于AHB总线协议的突发读取模块替代CPU完成激光点云数据的读取操作，整个读取传输过程不需CPU重复查询等待每一次状态转换的结果，且能够一次性获取多个激光点云数据，进而加速所述激光点云数据通过第一FIFO模块突发传输进入横纵坐标计算模块，加快坐标换算速度。

进一步地，所述横纵坐标计算模块，用于根据所述角度偏移信息和所述激光点云数据携带的采样信息建立三角函数运算，以获得所述激光点云数据在预配置的栅格地图上完成角度偏移的参考坐标；其中，所述激光点云数据携带有采样信息包括初始栅格坐标、所述预设待测数量、激光探头的扫描半径以及所述激光点云数据对应位置的偏转角度，扫描半径包括所述激光探头与最近目标点的距离；其中，角度偏移信息表示：所述激光点云数据对应的坐标位置绕着预配置的栅格地图的坐标系原点位置旋转的预设旋转步长角度和预设旋转次数。该技术方案能够使用扫描半径与所述激光探头的旋转角度作三角函数运算以获得坐标变化量，再结合初始栅格坐标可获得在预配置的栅格地图上的参考坐标，方便后续获取相应坐标位置处的概率值和在小范围内进行坐标偏移计算。

进一步地，所述读写状态控制器包括单次读取模块，单次读取模块内部设置第二FIFO模块，所述单次读取模块在所述AHB总线解析模块的每一次单次传输控制作用下，将所述激光点云寄存处理模块内部存储的相邻两个地址上的参考坐标单次读取入第二FIFO模块，再交由第二FIFO模块以先入先出的方式将填满的参考坐标传输给栅格信息遍历累加模块；其中，所述激光点云寄存处理模块内部存储的参考坐标包括X轴坐标和Y轴坐标。本技术方案通过增加基于AHB总线协议的突发读取模块替代CPU完成逐个地图坐标的读取操作，整个读取坐标存储模块的传输过程不需CPU重复查询等待，进而加速第二FIFO模块从坐标存储模块中查找数据。

进一步地，所述读写状态控制器包括突发写模块，突发写模块内部设置第三FIFO模块，第三FIFO模块用于以先入先出的存储方式接收所述栅格信息遍历累加模块输出的定位概率和值，当第三FIFO模块填充的定位概率和值的个数达到一个突发传输长度时，所述突发写模块在所述AHB总线解析模块的每一次突发传输控制作用下，将一个突发传输长度的定位概率和值突发写入所述概率和值存储介质。与现有技术相比，本技术方案通过在CPU与存储模块之间增加基于AHB总线协议的突发写模块替代CPU完成数据的写出，整个写转换过程不需CPU重复查询等待每一次状态转换的结果，进而加速所述定位概率和值的软件判断速度，加快整个系统的重定位处理速度。

进一步地，所述栅格信息遍历累加模块，用于每当通过所述单次读取模块从所述激光点云寄存处理模块读取出所述参考坐标时，控制所述参考坐标在预配置的栅格地图上的定位概率参与累加运算，直到遍历读取出的参考坐标的数目达到所述预设待测数量时，将这些参考坐标对应的定位概率的累加结果确定为定位概率和值并传输给所述第三FIFO模块；然后，所述栅格信息遍历累加模块还用于控制这些参考坐标先后沿着预配置的栅格地图的X轴方向及其Y轴方向进行坐标偏移，所述栅格信息遍历累加模块每触发一次坐标偏移则控制这些坐标偏移后的参考坐标在预配置的栅格地图上的定位概率进行累加以获得新的定位概率和值，再将这些新的定位概率和值传输给所述第三FIFO模块；其中，所述栅格信息遍历累加模块还配置有：所述参考坐标在预配置的栅格地图的X轴方向上的最大配置偏移次数及Y轴方向上的最大配置偏移次数、所述参考坐标在预配置的栅格地图的X轴方向上的坐标偏移步长及Y轴方向上的坐标偏移步长。与现有技术相比，所述栅格信息遍历累加模块在控制计算定位概率的和值时，依次针对激光点云寄存处理模块内保存的栅格地图的X轴方向偏移量、栅格地图的Y轴方向偏移量以及激光探头的旋转角度变化步长进行遍历累加，每发生一次偏移，遍历累加出的概率和值依次进入第三FIFO模块缓存，充分考虑到整个系统因移动而产生的坐标定位误差问题，为软件的最后重定位提高足够多的坐标偏移误差。以适应当前激光扫描构建的地图环境。

进一步地，所述概率和值存储介质与所述激光重定位系统外部的CPU存在电性连接，所述概率和值存储介质存储有用于定位一个目标点所需的所有定位概率和值时，外部CPU从所述概率和值存储介质内选择出最大的定位概率和值，并将其对应的栅格位置作为这个目标点在预配置的栅格地图上定位恢复的位置；其中，所述概率和值存储介质是DDR。与现有技术相比，所述激光重定位系统减少对所述概率和值存储介质的读取和回写操作。

进一步地，所述地图信息存储介质包括概率和值地址存储模块和扫描信息存储模块；扫描信息存储模块与所述突发读取模块存在电性连接关系，扫描信息存储模块用于存储所述激光探头旋转过程中实时扫描出的激光点云数据，以使得所述突发读取模块在所述AHB总线解析模块的每一次突发传输控制作用下，根据所述点云遍历控制信息从扫描信息存储模块突发读取一个突发传输长度的激光点云数据进入所述第一FIFO模块；栅格信息遍历累加模块与地图信息存储介质存在电性连接关系，栅格信息遍历累加模块用于将每次遍历读取累加得到的定位概率和值的存放地址写入概率和值地址存储模块，使得该激光重定位系统的外围的激光探头扫描构建的栅格地图的定位信息得到保存，以便下次定位恢复时直接读取调用；其中，所述地图信息存储介质是SRAM。该技术方案在减少CPU参与的前提下，基于AHB总线的状态机机制，与现有技术软件CPU处理同样大小的点云数据相比，对于频率要求大大降低，对DDR带宽的要求也降低。

进一步地，所述激光点云寄存处理模块是所述激光重定位系统上设置的RAM，用于以存储阵列的形式保存所述参考坐标。方便外部寻址访问或者写入地图数据。

一种芯片，该芯片包括所述的基于硬件加速的激光重定位系统。该芯片满足在不同的处理场景中的大批量激光点云数据处理，对于CPU主频没有要求，只是对于算法硬件化电路频率有要求，但相对于CPU频率来说还是要低得多的，简化各个模块的读写时序，减缓CPU资源占用紧张的情况。提高激光建图重定位的准确性。

附图说明

图1为本发明公开的一种基于硬件加速的激光重定位系统的结构示意图。

具体实施方式

下面结合附图对本发明的具体实施方式作进一步说明。以下实施方式中所涉及到的各模块均为逻辑电路单元，一个逻辑电路单元可以是一个物理单元、也可以是由多个逻辑器件按照一定的读写时序和信号逻辑变化组合而成的状态机，也可以是一个物理单元的一部分，还可以以多个物理单元的组合实现。此外，为了突出本发明的创新部分，本发明实施方式中并没有将与解决本发明所提出的技术问题关系不太密切的单元引入，但这并不表明本发明实施方式中不存在其它的单元。

机器人采用360度的激光雷达扫描方式定位建图，其优点是建图成功率较高，能根据采集的激光点云数据求出目标点在地图中的位置，使得机器人能够利用激光地图进行重定位。具体地，激光雷达内置激光探头和动作旋转机构，动作旋转机构能支持激光探头360度旋转，同时控制机器人保持运动，所以激光探头每旋转一圈就得到一批激光点云数据。本发明实施例中，本发明技术方案采用逻辑电路模块加速处理数据的方式，利用专用存储模块保存激光扫描出的分块地图，然后将重定位软件算法硬件化，利用硬件电路的方式完成对激光点云的换算，然后进行遍历求概率和，再将概率和写入专门的存储器件中，实现由软件仅仅通过概率和比较就得出目标点在地图上的位置信息。更加适应当前的环境特征，充分考虑到不同工作环境所带来的重定位误差因素。这里的目标点可以包括静态的物体和动态的物体。

如图1所示，本发明实施例公开一种基于硬件加速的激光重定位系统，该激光重定位系统的外围连接有地图信息存储介质与概率和值存储介质，在本实施例中，该激光重定位系统作为主机设备电路，而地图信息存储介质与概率和值存储介质则都作为从机设备电路；该激光重定位系统的外围接口装配有激光探头；在具体的重定位场景下，该激光重定位系统连同外围接口的激光探头、地图信息存储介质与概率和值存储介质都装配在移动机器人上；该激光重定位系统包括读写状态控制器、AHB总线解析模块、激光点云寄存处理模块、横纵坐标计算模块以及栅格信息遍历累加模块。所述AHB总线解析模块，用于解析总线协议指令，将总线传送的数据信号和地址信号进行转化，在CPU发出取址指令后按照总线地址信号访问所述地图信息存储介质；本发明实施中使用的总线协议是AHB总线协议，但不排除按照重定位算法的要求或栅格位置的概率要求使用其他总线协议来完成数据的传输。

在所述激光重定位系统中，读写状态控制器与横纵坐标计算模块存在电性连接关系，读写状态控制器用于在AHB总线解析模块的传输控制作用下，将地图信息存储介质内部存储的激光点云数据突发读取入该读写状态控制器内部的缓存区，再传输给横纵坐标计算模块，其中，突发读取到的激光点云数据携带有采样信息，这些激光点云数据是所述激光探头旋转过程中实时采集的。横纵坐标计算模块，用于在读写状态控制器每完成一次突发读取操作后，根据突发读取的一个突发传输长度的激光点云数据的采样信息和角度偏移信息换算出这批激光点云数据在预配置的栅格地图上发生角度偏转后的参考坐标，以适应于在移动机器人重定位过程中出现左右偏转的现象。激光点云寄存处理模块与横纵坐标计算模块存在电性连接关系，激光点云寄存处理模块用于以存储阵列的形式缓存横纵坐标计算模块换算出的参考坐标，优选地，所述激光点云寄存处理模块是所述激光重定位系统上设置的RAM，用于以存储阵列的形式保存所述参考坐标，方便外部寻址访问或者写入地图数据，从而在所述激光重定位系统内及时构建激光栅格地图。需要说明的是，从所述地图信息存储介质往所述读写状态控制器传输的数据都是先暂存在所述激光重定位系统专门开设的缓冲区中，等待所述读写状态控制器在AHB总线解析模块的控制下调度使用。读写状态控制器与激光点云寄存处理模块存在电性连接关系，读写状态控制器，用于在AHB总线解析模块的传输控制作用下，单次读取激光点云寄存处理模块缓存的参考坐标，并将其传输给栅格信息遍历累加模块，其中，所述激光点云数据是二维坐标数据，包括栅格坐标系X轴和Y轴方向上的数据；参考坐标也包括X轴坐标和Y轴坐标；其中，一次突发读取的数据个数与一次单次读取的数据个数的比值是预设整数值，使得读写状态控制器经过预设整数值对激光点云寄存处理模块的单次读取操作，才能将读写状态控制器一次突发读取操作获取的激光点云数据处理出来的坐标信息传输给栅格信息遍历累加模块，避免传输数据的速度快于栅格信息遍历累加模块的计算速度，造成AHB总线数据堵塞。

在本实施例中，栅格信息遍历累加模块与读写状态控制器存在电性连接关系，栅格信息遍历累加模块还与横纵坐标计算模块存在电性连接关系，在读写状态控制器将激光探头在旋转过程中实时采样到的预设待测数量的激光点云数据全部突发读取进来、并经过横纵坐标计算模块换算处理、激光点云寄存处理模块缓存处理后，栅格信息遍历累加模块在读写状态控制器的单次读取操作作用下，遍历激光点云寄存处理模块内缓存的所有参考坐标，这里的参考坐标是激光点云数据经过横纵坐标计算模块换算得到的在预配置的栅格地图上发生角度偏移的栅格坐标数据；栅格信息遍历累加模块再将每个参考坐标在预配置的栅格地图上的定位概率累加处理，然后分别控制这些参考坐标沿着预配置的栅格地图的坐标轴方向（包括X轴方向和Y轴方向）偏移，在栅格信息遍历累加模块中，这些参考坐标在所述横纵坐标计算模块的角度偏移信息的触发作用下，每进行一次偏移处理（X轴方向上的偏移或Y轴方向上的偏移或同一偏转方向下的角度偏移）则执行累加以得到一个定位概率和值，具体可以是：激光点云寄存处理模块缓存到读写状态控制器的参考坐标先执行X轴方向上的偏移再执行累加以得到新的定位概率和值，然后执行Y轴方向上的偏移再执行累加以得到新的定位概率和值，当完成X轴方向和Y轴方向上的偏移后，栅格信息遍历累加模块触发所述横纵坐标计算模块继续对同一批激光点云数据执行同一偏转方向下的角度偏移以换算获得新的参考坐标，再交由栅格信息遍历累加模块重复执行X轴方向上的偏移和Y轴方向上的偏移。其中，定位概率用于表示所述激光重定位系统所属的装置落入栅格地图的一个栅格位置的概率。

读写状态控制器分别与概率和值存储介质和地图信息存储介质都存在电性连接，读写状态控制器，还用于在AHB总线解析模块的传输控制作用下，每当栅格信息遍历累加模块获得的定位概率和值的个数达到一个突发传输长度时，根据点云遍历控制信息将这些定位概率和值突发写入概率和值存储介质；其中，激光探头旋转过程中实时采样到的激光点云数据全部被处理为对应偏移位置（不仅是一个偏移位置）处的定位概率和值并都写入概率和值存储介质后，外部CPU通过软件算法最终选择定位概率和值最大的一个对应的栅格位置作为所述激光重定位系统在预配置的栅格地图上定位恢复的位置。

与现有技术相比，与现有技术相比，本实施例采用硬件计算电路模块对读取过来的大批量的激光点云数据进行分块处理，并在此基础上按照AHB总线读写时序变化，对即时扫描获得的激光点云数据进行栅格位置概率进行分批量的累加运算，再按照AHB总线读写时序变化自动写入专门的存储介质中以供CPU软件选择处理完成定位，减少对外部存储介质的访问读写次数。因此，本实施例采用硬件电路读写的方式来代替CPU软件反复迭代处理数据，对CPU的主频要求不高，在使用这个激光重定位系统去读写外部的DDR存储器（概率和值存储介质是DDR）时，不会占用大量DDR带宽，还能兼顾激光探头旋转或激光重定位系统移动过程中产生的误差，整个激光重定位系统不在静止状态都保持较高的系统性能，提高客户体验度。

需要说明的是，实施例以部分环境地图为例，即以装配所述激光重定位系统的机器人的当前位姿为原点，在设定搜索范围内生成位姿图。考虑到激光探头所发射的激光束具有一定的射程，即只能扫描到一定范围内的区域，为此，在确定搜索范围时，可以以激光束的射程为依据，可以在机器人前后左右5米射程的范围内生成位姿图。可以理解的是，机器人出现定位丢失时，当前位姿存在一定的偏差，为此，实施例在当前位姿的基础上按照设定的角度偏移量进行偏移，得到偏移后的位姿图；机器人在导航移动时，当前位姿存在一定的漂移误差，为此，实施例在当前位姿的基础上按照设定的横轴方向和纵轴方向的偏移量进行栅格位置偏移，得到偏移后的位姿图。本发明实施例中，配置的定位概率和值用于反映了机器人的准确定位在该位姿图可能性的大小，定位概率和值越大，表示机器人的准确定位在该位姿图可能性越大。可选的，在确定环境地图中区域的定位概率时，可以对环境地图进行处理，得到对应的栅格地图。栅格地图中每一个栅格代表一定的坐标，环境地图中包含障碍物和空白区域；栅格地图确定之后，可以进行保存，以便在下次进行定位恢复时可以直接使用。在此基础上，根据机器人的当前位姿，可以确定机器人在栅格地图中的位置，然后根据激光探头所扫描到各个对应的栅格点的定位概率的和值，得到定位概率和值。优选地，栅格地图中的障碍物所对应的定位概率可以服从正态分布。反正是从概率和值存储介质存储的所有定位概率和值中挑出最大的作为重定位的结果，然后随着机器运动，这个最大的定位概率和值会随之更新，这里配置的定位概率与现有技术中公开的定位算法相关，但与解决本发明所提出的硬件加速的技术问题关系不太密切，故在此不进行详细说明。

如图1所示，所述读写状态控制器包括突发读取模块，突发读取模块内部设置第一FIFO模块，所述突发读取模块在所述AHB总线解析模块的每一次突发传输控制作用下，将一个突发传输长度的激光点云数据突发读取入第一FIFO模块FIFO1，再交由第一FIFO模块FIFO1以先入先出的存储方式将读取到的激光点云数据传输给所述横纵坐标计算模块。具体地，所述突发读取模块被激活进入突发读工作模式时，按照CPU指定的地图信息存储介质接收实时扫描得到的激光点云数据的基地址与突发长度，将一个突发传输长度的激光点云数据连续读取入第一FIFO模块FIFO1，再由第一FIFO模块FIFO1传输这个突发传输长度的激光点云数据给所述横纵坐标计算模块，这时所述突发读取模块停止当前一次突发读取操作，然后激活单次读取操作。本实施例通过增加基于AHB总线协议的突发读取模块替代CPU完成激光点云数据的读取操作，整个读取传输过程不需CPU重复查询等待每一次状态转换的结果，且能够一次性获取多个激光点云数据，进而加速所述激光点云数据通过第一FIFO模块突发传输进入横纵坐标计算模块，加快硬件电路转换处理条件下的坐标换算速度。其中，一个突发传输长度的激光点云数据是一个突发传输长度的连续地址上的激光点云数据。

如图1所示，所述横纵坐标计算模块，用于根据所述角度偏移信息和所述激光点云数据携带有采样信息进行三角函数运算，以获得所述激光点云数据在预配置的栅格地图上发生角度偏移的参考坐标，是已发生角度偏移的参考坐标，可以是根据内部配置的角度最大旋转次数执行角度偏移的参考坐标，使得所述横纵坐标计算模块换算出的参考坐标是根据内部计数器计数值执行多次角度偏移变换而获得的，其中角度偏移变换的最大次数是内部配置的角度最大旋转次数；所述横纵坐标计算模块通过所述突发读取模块执行多次突发读操作，以实现将实时扫描获得的激光点云数据全部处理为参考坐标。其中，所述激光点云数据携带有采样信息包括初始栅格坐标、所述预设待测数量、激光探头的扫描半径以及所述激光探头的起始旋转角度以及所述激光点云数据对应位置的偏转角度，这个偏转角度可以是所述激光点云数据对应位置偏离X轴正方向的角度，扫描半径包括所述激光探头与最近目标点的距离；其中，角度偏移信息表示：所述激光点云数据对应的坐标位置绕着预配置的栅格地图的坐标系原点位置旋转一个预设步长角度。每当所述激光点云数据对应的坐标位置绕着坐标系原点位置旋转一个预设步长角度，即控制所述参考坐标发生一次角度偏移，可以是自身触发的，也可以是所述栅格信息遍历累加模块完成坐标轴的偏移后触发的。本实施例能够使用预设步长角度与旋转次数的乘积与所述激光点云数据对应位置的偏转角度的角度和值、扫描半径和初始栅格坐标（初始偏移坐标）建立三角函数关系式，以获得预配置的栅格地图上的坐标变化量及参考坐标，这个参考坐标也可以转换为用栅格数目表示，方便后续获取相应坐标位置处的概率值和在小范围内进行坐标偏移计算。其中，扫描半径按比例收缩为使用栅格数目表示。

如图1所示，所述读写状态控制器包括单次读取模块，单次读取模块内部设置第二FIFO模块FIFO2，所述单次读取模块在所述AHB总线解析模块的每一次单次传输控制作用下，将所述激光点云寄存处理模块内部存储的相邻两个地址上的参考坐标单次读取入第二FIFO模块FIFO2，再交由第二FIFO模块FIFO2以先入先出的方式将填满的参考坐标传输给栅格信息遍历累加模块；其中，所述激光点云寄存处理模块内部存储的参考坐标包括X轴坐标和Y轴坐标，都是发生过角度偏移后的坐标值。具体地，在所述AHB总线解析模块的每一次单次传输控制作用下，所述单次读取模块被激活进入单次读工作模式时，所述单次读取模块从所述激光点云寄存处理模块内部存储的相邻两个地址上的参考坐标(X轴坐标和Y轴坐标)连续传输到所述单次读取模块的缓冲区内中，即连续填入第二FIFO模块FIFO2 内，再控制这些参考坐标由第二FIFO模块FIFO2进入所述栅格信息遍历累加模块，以进一步地执行坐标偏移状态下的定位概率的累加运算，每当所述栅格信息遍历累加模块执行预设待测数量的定位概率的累加运算后，所述单次读取模块停止单次读操作，直到触发所述横纵坐标计算模块对原来突发读取进来的同一批激光点云再执行一次角度偏移操作，所述单次读取模块开始执行单次读操作，以从所述激光点云寄存处理模块读取最新缓存的参考坐标。本实施例通过增加基于AHB总线协议的突发读取模块替代CPU完成逐个地图坐标的读取操作，整个读取坐标存储模块的传输过程不需CPU重复查询等待，进而加速第二FIFO模块从坐标存储模块中查找数据。

如图1所示，所述栅格信息遍历累加模块，用于每当通过所述单次读取模块从所述激光点云寄存处理模块读取出所述参考坐标时，控制所述参考坐标在预配置的栅格地图上的定位概率参与累加运算，直到遍历读取出的参考坐标的数目达到所述预设待测数量时，将这些参考坐标对应的定位概率的累加结果确定为定位概率和值，并逐个传输给所述第三FIFO模块FIFO3；然后，所述栅格信息遍历累加模块还用于控制这些参考坐标先后沿着预配置的栅格地图的X轴方向及其Y轴方向发生坐标偏移，所述栅格信息遍历累加模块每触发一次坐标偏移则控制这些坐标偏移后的参考坐标在预配置的栅格地图上的定位概率进行累加以获得新的定位概率和值，再将累加求得的新的定位概率和值逐个传输给所述第三FIFO模块FIFO3；其中，所述栅格信息遍历累加模块还配置有：所述参考坐标在预配置的栅格地图的X轴方向上的最大配置偏移次数及Y轴方向上的最大配置偏移次数、所述参考坐标在预配置的栅格地图的X轴方向上的坐标偏移步长及Y轴方向上的坐标偏移步长。

本实施例中，通过所述单次读取模块单次传输所述预设待测数量的参考坐标至所述栅格信息遍历累加模块，这些参考坐标按照X轴方向上的坐标偏移步长在X轴方向上每完成偏移一次时，对这些参考坐标偏移处理并将其偏移位置处的定位概率累加得到一个新的定位概率和值，再将这些新的定位概率和值传输给所述第三FIFO模块FIFO3；当这些参考坐标X轴方向上的偏移次数达到其对应坐标轴方向的最大配置偏移次数时，这些参考坐标按照Y轴方向上的坐标偏移步长在Y轴方向上进行偏移，在Y轴方向上进行偏移时，这些参考坐标不是使用上一次X轴方向上的偏移坐标结果，而是保留使用所述单次读取模块单次传输到所述栅格信息遍历累加模块的参考坐标。

本实施例中，这些参考坐标按照Y轴方向上的坐标偏移步长在Y轴方向上完成偏移一次后，再一次按照X轴方向上的坐标偏移步长在X轴方向上完成偏移一次，再对这些参考坐标偏移处理并将其偏移位置处的定位概率累加得到一个新的定位概率和值，其中，这些参考坐标不是使用上一次X轴方向上的偏移坐标结果，而是保留使用所述单次读取模块单次传输到所述栅格信息遍历累加模块的参考坐标；当这些参考坐标Y轴方向上的偏移次数达到其对应坐标轴方向的最大配置偏移次数，且这些参考坐标X轴方向上的偏移次数达到其对应坐标轴方向的最大配置偏移次数时，所述栅格信息遍历累加模块完成控制这些参考坐标先后沿着预配置的栅格地图的X轴方向及其Y轴方向的坐标偏移操作并获得与偏移次数相关的个数的定位概率和值，这里的个数是X轴方向的最大配置偏移次数与Y轴方向的最大配置偏移次数的乘积；然后所述栅格信息遍历累加模块触发所述横纵坐标计算模块继续对已突发读取的预设待测数量的激光点云数据执行同一偏转方向下的角度偏移以换算获得新的参考坐标，其中，所述激光点云数据携带有采样信息，比如扫描半径绕着坐标系原点位置，每按照所述预设旋转步长角度朝着同一偏转方向完成一次旋转，则利用三角函数关系获得所述激光点云数据在预配置的栅格地图上完成角度偏移的参考坐标，且每一次角度偏移都是获得预设待测数量的参考坐标，然后将这些参考坐标逐个交由栅格信息遍历累加模块重复执行前述的X轴方向上的偏移和Y轴方向上的偏移并求取新的定位概率和值，直到所述横纵坐标计算模块执行所述预设旋转次数的角度偏移操作，然后再去接受所述突发读取模块从地图信息存储介质突发读取的新的一批激光点云数据。与现有技术相比，所述栅格信息遍历累加模块在控制计算定位概率的和值时，依次针对激光点云寄存处理模块内保存的栅格地图的X轴方向偏移量、栅格地图的Y轴方向偏移量以及激光探头的旋转角度变化步长进行遍历累加，每发生一次偏移，遍历累加出的概率和值依次进入第三FIFO模块FIFO3缓存，充分考虑到整个系统因移动而产生的坐标定位误差问题，为软件的最后重定位提高足够多的坐标偏移误差。以适应当前激光扫描构建的地图环境。

如图1所示，所述读写状态控制器包括突发写模块，突发写模块内部设置第三FIFO模块FIFO3，第三FIFO模块FIFO3用于以先入先出的存储方式接收所述栅格信息遍历累加模块输出的定位概率和值，当第三FIFO模块FIFO3填充的定位概率和值的个数达到一个突发传输长度时，所述突发写模块在所述AHB总线解析模块的每一次突发传输控制作用下，将一个突发传输长度的定位概率和值突发写入所述概率和值存储介质。与现有技术相比，本实施例通过在CPU与存储模块之间增加基于AHB总线协议的突发写模块替代CPU完成数据的写出，整个写转换过程不需CPU重复查询等待每一次状态转换的结果，进而加速所述定位概率和值的软件判断速度，加快整个系统的重定位处理速度。

需要说明的是，前述的突发读取模块、前述的单次读取模块、前述的突发写模块、前述栅格信息遍历累加模块和前述的横纵坐标计算模块都是由硬件语言实现的状态机，该状态机是由状态寄存器和组合逻辑电路构成，用于调度AHB总线解析模块、激光点云数据的角度偏移、参考坐标的横纵轴方向的偏移及其读写迭代控制状态的自动运转，使得本发明实施例中涉及的功能模块均由数字运算电路组成。

如图1所示，所述概率和值存储介质与所述激光重定位系统外部的CPU存在电性连接，所述概率和值存储介质存储有用于定位一个目标点所需的所有定位概率和值时，在本实施例中，用于定位一个目标点所需的定位概率和值的个数是所述激光重定位系统内部配置的X轴方向的最大配置偏移次数、Y轴方向的最大配置偏移次数与所述预设旋转次数的乘积，而每个定位概率和值是所述预设待测数量的完成角度偏移后的参考坐标（包括发生一次坐标轴方向偏移前或发生一次坐标轴方向偏移后的坐标）对应的位置上的定位概率的和值。其中，所述概率和值存储介质是DDR。外部CPU从所述概率和值存储介质内选择出最大的定位概率和值，并将其对应的栅格位置作为这个目标点在预配置的栅格地图上定位恢复的位置。从整体上看，与现有技术相比，所述激光重定位系统在执行多次角度偏移和多次坐标偏移的过程中不需要对所述概率和值存储介质的读取和回写操作，减小对DDR的访问次数，减小对于DDR的带宽需求。

如图1所示，所述地图信息存储介质包括概率和值地址存储模块和扫描信息存储模块；扫描信息存储模块与所述突发读取模块存在电性连接关系，扫描信息存储模块用于存储所述激光探头旋转过程中实时扫描出的激光点云数据，以使得所述突发读取模块在所述AHB总线解析模块的每一次突发传输控制作用下，根据所述点云遍历控制信息从扫描信息存储模块突发读取一个突发传输长度的激光点云数据进入所述第一FIFO模块FIFO1；如图1所示，栅格信息遍历累加模块与地图信息存储介质存在电性连接关系，栅格信息遍历累加模块用于将每次遍历读取累加得到的定位概率和值的存放地址写入概率和值地址存储模块，使得该激光重定位系统的外围的激光探头扫描构建的栅格地图的定位信息得到保存，以便下次定位恢复时直接读取调用。本实施例将所述地图信息存储介质设置为SRAM，因此，本实施例对SRAM的容量要求较大，以便减少对于所述地图信息存储介质（DDR）的读取和回写的次数，进一步地减小对于DDR的带宽需求。

需要说明的是，所述激光点云寄存处理模块、所述概率和值地址存储模块、所述扫描信息存储模块和所述概率和值存储介质的内部都是一个存储阵列，即将数据“填”进去，和表格的检索原理一样，先指定一个行，再指定一个列，就准确地找到所必需的单元格，这就是内存芯片寻址的基本原理。对于内存，这个单元格可称为存储单元,那么这个表格（存储阵列）就是逻辑 Bank（Logical Bank，下面简称Bank）。在所述读写状态控制器通过所述AHB总线解析模块分别与所述地图信息存储介质和所述概率和值存储介质进行数据的突发传输的过程中，发出的地址都是对齐的，所述对齐地址是对存储空间（逻辑 Bank）的一种划分，在外部访问突发读取或者突发写入数据时，要以这种划分为前提进行，对齐地址是由每拍传输的数据的宽度来决定的。

作为本发明的一种实施方式，CPU发出使能指令，所述激光探头基于第一个目标点采集200个所述激光点云数据，所述预设待测数量设置为200。所述激光重定位系统从CPU配置的基地址开始，在一次突发读取操作中，连续读取扫描信息存储模块内部存储阵列的同一行框定的16个连续地址单元，示例性地，所述扫描信息存储模块实时保存基于一个目标点而扫描200个所述激光点云数据。本发明实施例读写状态控制器一次性连续读取16个连续地址单元上激光点云数据，经过多次突发读取将200个所述激光点云数据传输进入所述激光重定位系统内部的缓存空间。本实施例通过突发读取外部存储器件，加速所述激光点云数据的匹配速度，减缓CPU资源占用紧张的情况。

优选地，所述横纵坐标计算模块控制控制这200个激光点云数据进行角度偏移，以获得所述参考坐标，再在所述读写状态控制器的突发传输控制作用下，将这些参考坐标单次传输到所述栅格信息遍历累加模块内反复进行7次X轴方向的坐标偏移和7次Y轴方向的坐标偏移，以克服所述激光重定位系统在移动过程中产生的定位误差；然后再触发所述横纵坐标计算模块控制先后控制这200个激光点云数据进行150次角度偏移，并在此基础上将换算出的参考坐标传输到所述栅格信息遍历累加模块内，以实现反复进行7次X轴方向的坐标偏移和7次Y轴方向的坐标偏移，在每一次偏移后（包括各个坐标轴方向及角度偏移）所述栅格信息遍历累加模块以200个参考坐标对应的定位概率为遍历单位，进行累加求和，最终所述栅格信息遍历累加模块先后输出的定位概率和值的个数达到150x7x7=7350个，再由突发写模块控制分块写入所述概率和值存储介质。需要说明的是，栅格位置上的定位概率的配置所采用的算法都是本领域技术人员常用的算法，故不再赘述。其中，SRAM和DDR都是外部存储器件。

在前述实施例的基础上，经过实际的数据测算，采用所述激光重定位系统完成前述的参考坐标在预配置的栅格地图上的定位概率的遍历累加处理，在AHB总线频率是300MHz的情况下，待处理的激光栅格地图大小配置为201x3x3时，则耗时6.35ms，而处理同样大小的地图，采用软件CPU的轮询读写的方式，在CPU主频1GHz的条件下，耗时10多个ms。因此，本实施例采用硬件方式，不但对于频率要求大大降低，同时耗时也要大大少于CPU方式，效率提升了至少40%以上，减少对外部DDR的带宽的要求，容易达到了预期的加速设计目标。也方便预先通过软件算法调整参数以适应当前地图环境，较为灵活。

一种芯片，该芯片包括所述的基于硬件加速的激光重定位系统。该芯片满足在不同的处理场景中的大批量激光点云数据处理，对于CPU主频没有要求，只是对于算法硬件化电路频率有要求，但相对于CPU频率来说还是要低得多的，简化各个模块的读写时序，减缓CPU资源占用紧张的情况。提高激光建图重定位的准确性。

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

Claims (10)

1.一种基于硬件加速的激光重定位系统，其特征在于，该激光重定位系统的外围连接有地图信息存储介质与概率和值存储介质，该激光重定位系统的外围接口装配有激光探头；

该激光重定位系统包括读写状态控制器、AHB总线解析模块、激光点云寄存处理模块、横纵坐标计算模块以及栅格信息遍历累加模块；

读写状态控制器与横纵坐标计算模块存在电性连接关系，读写状态控制器用于在AHB总线解析模块的传输控制作用下，将地图信息存储介质内部存储的激光点云数据突发读取入该读写状态控制器内部的缓存区，再传输给横纵坐标计算模块；其中，突发读取到的激光点云数据携带有采样信息；

横纵坐标计算模块，用于在读写状态控制器每完成一次突发读取操作后，根据突发读取的一个突发传输长度的激光点云数据的采样信息和角度偏移信息换算出这批激光点云数据在预配置的栅格地图上发生角度偏移的参考坐标；

激光点云寄存处理模块与横纵坐标计算模块存在电性连接关系，激光点云寄存处理模块用于以存储阵列的形式缓存横纵坐标计算模块换算出的参考坐标；

读写状态控制器与激光点云寄存处理模块存在电性连接关系，读写状态控制器，用于在AHB总线解析模块的传输控制作用下，单次读取激光点云寄存处理模块缓存的参考坐标，并将其传输给栅格信息遍历累加模块；

栅格信息遍历累加模块与读写状态控制器存在电性连接关系，栅格信息遍历累加模块还与横纵坐标计算模块存在电性连接关系，用于在读写状态控制器将激光探头旋转采样到的预设待测数量的激光点云数据全部突发读取进来、并经过横纵坐标计算模块换算处理、激光点云寄存处理模块缓存处理后，在读写状态控制器的单次读取操作作用下，遍历激光点云寄存处理模块内缓存的所有参考坐标，再将每个参考坐标在预配置的栅格地图上的定位概率累加处理，然后分别控制这些参考坐标沿着坐标轴方向偏移，使得这些参考坐标在所述横纵坐标计算模块的角度偏移信息的触发作用下，每完成一次偏移处理则执行累加以得到一个定位概率和值；

读写状态控制器分别与概率和值存储介质和地图信息存储介质都存在电性连接，读写状态控制器，还用于在AHB总线解析模块的传输控制作用下，每当栅格信息遍历累加模块获得的定位概率和值的个数达到一个突发传输长度时，将这些定位概率和值突发写入概率和值存储介质；

其中，预设待测数量的激光点云数据全部被处理为对应偏移位置处的定位概率和值并都写入概率和值存储介质后，外部CPU最终选择定位概率和值最大的一个对应的栅格位置作为所述激光重定位系统在预配置的栅格地图上定位恢复的位置。

2.根据权利要求1所述激光重定位系统，其特征在于，所述读写状态控制器包括突发读取模块，突发读取模块内部设置第一FIFO模块，所述突发读取模块在所述AHB总线解析模块的每一次突发传输控制作用下，将一个突发传输长度的激光点云数据突发读取入第一FIFO模块，再交由第一FIFO模块以先入先出的存储方式将读取到的激光点云数据传输给所述横纵坐标计算模块。

3.根据权利要求2所述激光重定位系统，其特征在于，所述横纵坐标计算模块，用于根据所述角度偏移信息和所述激光点云数据携带的采样信息建立三角函数运算，以获得所述激光点云数据在预配置的栅格地图上完成角度偏移的参考坐标；

其中，所述激光点云数据携带有采样信息包括初始栅格坐标、所述预设待测数量、激光探头的扫描半径以及所述激光点云数据对应位置的偏转角度，扫描半径包括所述激光探头与最近目标点的距离；

其中，角度偏移信息表示：所述激光点云数据对应的坐标位置绕着预配置的栅格地图的坐标系原点位置旋转的预设旋转步长角度和预设旋转次数。

4.根据权利要求3所述激光重定位系统，其特征在于，所述读写状态控制器包括单次读取模块，单次读取模块内部设置第二FIFO模块，所述单次读取模块在所述AHB总线解析模块的每一次单次传输控制作用下，将所述激光点云寄存处理模块内部存储的相邻两个地址上的参考坐标单次读取入第二FIFO模块，再交由第二FIFO模块以先入先出的方式将填满的参考坐标传输给栅格信息遍历累加模块；

其中，所述激光点云寄存处理模块内部存储的参考坐标包括X轴坐标和Y轴坐标。

5.根据权利要求4所述激光重定位系统，其特征在于，所述读写状态控制器包括突发写模块，突发写模块内部设置第三FIFO模块，第三FIFO模块用于以先入先出的存储方式接收所述栅格信息遍历累加模块输出的定位概率和值，当第三FIFO模块填充的定位概率和值的个数达到一个突发传输长度时，所述突发写模块在所述AHB总线解析模块的每一次突发传输控制作用下，将一个突发传输长度的定位概率和值突发写入所述概率和值存储介质。

6.根据权利要求5所述激光重定位系统，其特征在于，所述栅格信息遍历累加模块，用于每当通过所述单次读取模块从所述激光点云寄存处理模块读取出所述参考坐标时，控制所述参考坐标在预配置的栅格地图上的定位概率参与累加运算，直到遍历读取出的参考坐标的数目达到所述预设待测数量时，将这些参考坐标对应的定位概率的累加结果确定为定位概率和值并传输给所述第三FIFO模块；然后，所述栅格信息遍历累加模块还用于控制这些参考坐标先后沿着预配置的栅格地图的X轴方向及其Y轴方向进行坐标偏移，所述栅格信息遍历累加模块每触发一次坐标偏移则控制这些坐标偏移后的参考坐标在预配置的栅格地图上的定位概率进行累加以获得新的定位概率和值，再将这些新的定位概率和值传输给所述第三FIFO模块；

其中，所述栅格信息遍历累加模块还配置有：所述参考坐标在预配置的栅格地图的X轴方向上的最大配置偏移次数及Y轴方向上的最大配置偏移次数、所述参考坐标在预配置的栅格地图的X轴方向上的坐标偏移步长及Y轴方向上的坐标偏移步长。

7.根据权利要求6所述激光重定位系统，其特征在于，所述概率和值存储介质与所述激光重定位系统外部的CPU存在电性连接，所述概率和值存储介质存储有用于定位一个目标点所需的所有定位概率和值时，外部CPU从所述概率和值存储介质内选择出最大的定位概率和值，并将其对应的栅格位置作为这个目标点在预配置的栅格地图上定位恢复的位置；

其中，所述概率和值存储介质是DDR。

8.根据权利要求7所述激光重定位系统，其特征在于，所述地图信息存储介质包括概率和值地址存储模块和扫描信息存储模块；

扫描信息存储模块与所述突发读取模块存在电性连接关系，扫描信息存储模块用于存储所述激光探头旋转过程中实时扫描出的激光点云数据，以使得所述突发读取模块在所述AHB总线解析模块的每一次突发传输控制作用下，根据所述点云遍历控制信息从扫描信息存储模块突发读取一个突发传输长度的激光点云数据进入所述第一FIFO模块；

栅格信息遍历累加模块与地图信息存储介质存在电性连接关系，栅格信息遍历累加模块用于将每次遍历读取累加得到的定位概率和值的存放地址写入概率和值地址存储模块，使得该激光重定位系统的外围的激光探头扫描构建的栅格地图的定位信息得到保存，以便下次定位恢复时直接读取调用；

其中，所述地图信息存储介质是SRAM。

9.根据权利要求8所述激光重定位系统，其特征在于，所述激光点云寄存处理模块是所述激光重定位系统上设置的RAM，用于以存储阵列的形式保存所述参考坐标。

10.一种芯片，其特征在于，该芯片包括权利要求1至9任一项所述的基于硬件加速的激光重定位系统。

Images