【技术实现步骤摘要】
一层第二待搜索地图上进行扫描匹配，再利用匹配出的结果计算并更新出当前搜索的节点的第二上界值； CPU还用于将最新获得的第二上界值对应的位姿或最新获得的第一上界值对应的位姿配置为机器人的位 姿；其中，所有层第一待搜索地图和所有层第二待搜索地图构建有一棵以上分支定界搜索树，使得所有棵 分支定界搜索树的根节点对应的栅格点都在分辨率最低的一层第一待搜索地图中；每一层第一待搜索地图 的分辨率都低于任意一层第二待搜索地图的分辨率；所述第一待搜索地图和所述第二待搜索地图是分层设 置的。
[0007]本技术方案公开的硬件加速定位系统通过软硬件模块调度的方式对多层地图进行分支定界处理，专用 于处理回环检测的实时性问题。本硬件加速定位系统将分辨率相对低的多层地图的搜索过程交由所述相关 性扫描匹配硬件电路用于以硬件并行处理的方式实现，将分辨率相对高的多层地图的搜索过程交由CPU实 现，从而在同一系统内合理分配了计算资源，最大限度提高了纯软件方式执行算法的计算效率，且节省部 分硬件资源。
[0008]进一步地，所述硬件加速定位系统存储有预先构建的第一分辨率的栅格地图，第一分辨率的栅格地图 被CPU构建为多层级栅格地图，该多层级栅格地图包括第一预置层数的第一待搜索地图和第二预置层数的 第二待搜索地图，所有层的第二待搜索地图中的最大分辨率等于第一分辨率；在该多层级栅格地图中，所 述第一待搜索地图和所述第二待搜索地图是分层设置的；第一预置层数的第一待搜索地图和第二预置层数 的第二待搜索地图是按照分辨率的由小到大的顺序纵向排列的，其中，第一预置层数的第一待搜索地图的 排序号...
【技术保护点】
【技术特征摘要】
1.软硬协同下的硬件加速定位系统，硬件加速定位系统与激光雷达存在电性连接关系；其中，激光雷达是安装在机器人的机体上；其特征在于，硬件加速定位系统包括CPU和相关性扫描匹配硬件电路；相关性扫描匹配硬件电路用于以硬件并行处理的方式来搜索每一层第一待搜索地图，并更新出当前搜索的节点的第一上界值；CPU用于每当相关性扫描匹配硬件电路搜索到预设深度的一层地图的栅格点对应的节点时，通过执行分支定界算法来搜索每一层第二待搜索地图，并通过执行相关性扫描匹配算法来将激光雷达采集的点云在对应一层第二待搜索地图上进行扫描匹配，再利用匹配出的结果计算并更新出当前搜索的节点的第二上界值；CPU还用于将最新获得的第二上界值对应的位姿或最新获得的第一上界值对应的位姿配置为机器人的位姿；其中，所有层第一待搜索地图和所有层第二待搜索地图构建有一棵以上分支定界搜索树，使得所有棵分支定界搜索树的根节点对应的栅格点都在分辨率最低的一层第一待搜索地图中；每一层第一待搜索地图的分辨率都低于任意一层第二待搜索地图的分辨率；所述第一待搜索地图和所述第二待搜索地图是分层设置的。2.根据权利要求1所述硬件加速定位系统，其特征在于，所述硬件加速定位系统存储有预先构建的第一分辨率的栅格地图，第一分辨率的栅格地图被CPU构建为多层级栅格地图，该多层级栅格地图包括第一预置层数的第一待搜索地图和第二预置层数的第二待搜索地图，所有层的第二待搜索地图中的最大分辨率等于第一分辨率；第一预置层数的第一待搜索地图和第二预置层数的第二待搜索地图是按照分辨率的由小到大的顺序纵向排列的，其中，第一预置层数的第一待搜索地图的排序号和第二预置层数的第二待搜索地图的排序号都表示对应一层地图的深度。3.根据权利要求2所述硬件加速定位系统，其特征在于，所述硬件加速定位系统还包括总线接口，所述CPU和所述相关性扫描匹配硬件电路都与总线接口存在电性连接；所述CPU被配置为执行以下步骤：步骤A、所述CPU通过总线接口向所述相关性扫描匹配硬件电路配置用于执行分支定界算法所需的参数和用于执行相关性扫描匹配算法所需的参数；其中，用于执行分支定界算法所需的参数包括第一预置层数、每一层第一待搜索地图的分辨率、第二预置层数、每一层第二待搜索地图的分辨率以及根节点的数量；步骤B、所述CPU通过总线接口向所述相关性扫描匹配硬件电路传输分辨率最低的一层第一待搜索地图；步骤C、所述CPU启动所述相关性扫描匹配硬件电路去搜索所有根节点，并触发所述相关性扫描匹配硬件电路计算出相应的根节点的得分值；同时，所述CPU按照所述用于执行相关性扫描匹配算法所需的参数对点云进行离散化；步骤D、所述CPU通过总线接口向所述相关性扫描匹配硬件电路传输剩余的第一待搜索地图；同时，所述CPU按照得分值的由小到大的顺序对所述相关性扫描匹配硬件电路当前搜索的所有根节点进行排序，再按照当前排序将根节点存入栈内；其中，该栈是所述硬件加速定位系统的内部开辟的支持按照先进后出的缓存空间；每一层第一待搜索地图的深度和每一层第二待搜索地图的深度分别等于分支定界搜索树的
同一纵向高度的节点的深度。4.根据权利要求3所述硬件加速定位系统，其特征在于，所述CPU还被配置为执行以下步骤：步骤E、所述CPU控制所述栈的栈顶的节点出栈，然后所述CPU判断当前出栈的节点是否为叶节点，是则进入步骤F，否则进入步骤G；步骤F、所述CPU判断当前出栈的节点的得分值是否大于当前最优上界值，是则将该节点的得分值更新为当前最优上界值，并确定更新后的当前最优上界值为所述当前搜索的节点的第一上界值或所述当前搜索的节点的第二上界值，再进入步骤H，否则直接进入步骤H，其中，当前最优上界值的初始值是预先设置的初始阈值；步骤G、当所述CPU判断到当前出栈的节点的得分值大于当前最优上界值时，所述CPU判断当前出栈的节点的深度与分辨率最低的一层第一待搜索地图的深度的差值的绝对值是否大于或等于参考深度差，是则进入步骤I，并确定所述相关性扫描匹配硬件电路搜索到预设深度的一层地图的栅格点对应的节点，否则进入步骤J；其中，参考深度差等于所述第一预置层数与数值1的差值；当所述CPU判断到当前出栈的节点的得分值小于或等于当前最优上界值时，不执行分支操作，再执行步骤H；步骤I、所述CPU对步骤G所述的节点进行分支操作，搜索出步骤G所述的节点的所有子节点，再计算当前搜索出的子节点的得分值；然后进入步骤K；其中，步骤I的分支操作是在分辨率比步骤G所述的节点所属的一层地图高的相邻一层第二待搜索地图中，搜索出步骤G所述的节点的所有子节点；步骤J、所述相关性扫描匹配硬件电路以硬件并行处理的方式执行分支操作，搜索出步骤G所述的节点的所有子节点，并计算出当前搜索出的子节点的得分值；然后进入步骤K；其中，步骤J的分支操作是在分辨率比步骤G所述的节点所属的一层地图高的相邻一层第一待搜索地图中，搜索出步骤G所述的节点的所有子节点；步骤K、所述CPU按照步骤D所述的顺序对步骤I搜索出的子节点或步骤J搜索出的子节点进行排序，再控制当前排序好的子节点按照步骤D所述的顺序入栈；然后返回步骤E；步骤H、所述CPU判断所述栈是否为空，是则将步骤F所述的当前最优上界值对应的节点匹配的位姿配置为机器人的位姿，否则返回步骤E；其中，一个所述根节点对应一棵分支定界搜索树，搜索每一棵分支定界搜索树的一个节点采用所述分支定界算法所包括的分支操作；每一层第一待搜索地图的深度和每一层第二待搜索地图的深度分别等效于分支定界搜索树上的相应节点的深度；其中，在所述硬件加速定位系统中，逐层传输到所述相关性扫描匹配硬件电路的第一待搜索地图的分辨率是增加的，逐层被所述CPU处理的第二待搜索地图的分辨率也是增加的。5.根据权利要求4所述硬件加速定位系统，其特征在于，在步骤E中，所述栈的栈顶节点是第一节点或第二节点，其中，第一节点被配置有第一待搜索地图的栅格点的坐标和该栅格点所属的第一待搜索地图的深度信息，第二节点被配置有第二待搜索地图的栅格点的坐标和该栅格点所属的第二待搜索地图的深度信息；每当所述CPU在所述步骤F中判断到当前出栈的第一节点的得分值大于所述当前最优上界值时，将该第一节点的得分值更新为当前最优上界值，并确定该第一节点的得分值是
当前搜索的节点的第一上界值；每当所述CPU在所述步骤F中判断到当前出栈的第二节点的得分值大于所述当前最优上界值时，将该第二节点的得分值更新为当前最优上界值，并确定该第二节点的得分值是当前搜索的节点的第二上界值。6.根据权利要求4所述硬件加速定位系统，其特征在于，所述相关性扫描匹配硬件电路在步骤C中计算根节点的得分值的过程中，若所述相关性扫描匹配硬件电路触发中断，则所述相关性扫描匹配硬件电路转而进入步骤D；所述CPU在步骤C中对第二待搜索地图内的点云进行离散化的过程中，若检测到触发中断，则所述CPU转而执行步骤D；在所述相关性扫描匹配硬件电路对所述剩余的第一待搜索地图执行分支操作的过程中，所述CPU采用轮询机制，用于等待所述相关性扫描匹配硬件电路计算并输出相应的节点的得分值，以避免采用中断机制而造成延迟，其中，当前搜索的节点不是根节点。7.根据权利要求4至6任一项所述硬件加速定位系统，其特征在于，所述相关性扫描匹配硬件电路包括存储器模块、点云处理模块、节点搜索模块、状态机控制模块和互连总线，其中，存储器模块、点云处理模块、节点搜索模块和状态机控制模块都通过所述互连总线建立数据传输关系；点云处理模块，用于在状态机控制模块的控制下，从存储器模块读取当前存储的点云，再控制读取到的点云执行旋转变换；点云处理模块，用于在状态机控制模块的控制下，读取当前一次旋转变换的结果，再控制当前一次旋转变换的结果执行坐标系变换，再将坐标系变换的结果设置为离散点云，实现将当前存储的点云对齐到预先扩展的栅格地图的坐标系内，同时确定完成将当前存储的点云的离散化；然后将离散点云通过所述互连总线存储到存储器模块内；其中，预先扩展的栅格地图是步骤C搜索根节点所需的一层第一待搜索地图、或所述步骤J搜索子节点所需的一层第一待搜索地图；节点搜索模块，用于在状态机控制模块的控制下，从所述存储器模块内获取离散点云，再按照预设坐标偏移值和搜索步长，计算出离散点云映射到地图存储空间的索引值，并将索引值设置为点云的占用概率的读取地址；节点搜索模块，还用于在状态机控制模块的控制下，按照当前设置出的读取地址从所述地图存储空间内读取出相应点云的占用概率，并调用节点搜索模块内置的累加器对当前读取的点云的占用概率进行累加，再将累加结果输出，并设置为所述预先扩展的栅格地图中相应的节点的得分值；存储器模块，设置有地图存储空间，用于存储所述总线接口传输过来的第一待搜索地图；存储器模块，用于存储预先设置出的三角函数值；其中，旋转变换的次数等于前述根节点的数量，一次旋转变换下对应执行一次以上的所述坐标系变换；所述搜索窗口当前搜索的一个节点是由一个特定的旋转参数和一个特定的平移参数决定的；其中，点云的占用概率是点云经过所述点云处理模块处理后，在预先扩展的栅格地图内相匹配的栅格点的概率值；点云在每一层第一待搜索地图中的占用概率都存在相匹配的一个索引值。
8.根据权利要求7所述硬件加速定位系统，其特征在于，所述地图存储空间包括第一地图存储空间和第二地图存储空间；第一地图存储空间用于存储第一预制分辨率的第一待搜索地图，第二地图存储空间用于存储第二预制分辨率的第一待搜索地图，第一预制分辨率的第一待搜索地图的深度级别与第二预制分辨率的第一待搜索地图的深度级别是相差一级，使得两层第一待搜索地图是相邻的；第一预制分辨率小于第二预制分辨率；所述相关性扫描匹配硬件电路，用于在一次执行分支操作的过程中，调度所述点云处理模块将所述当前存储的点云对齐匹配到第二预制分辨率的第一待搜索地图上的同时，也调度所述节点搜索模块计算所述当前存储的点云的占用概率在第一地图存储空间内的索引值。9.根据权利要求8所述硬件加速定位系统，其特征在于，所述第一预置层数是3，所述第二预置层数是4；在CPU执行步骤B的过程中，所述CPU通过总线接口向所述相关性扫描匹配硬件电路传输的第一层第一待搜索地图，再由总线接口将第一层第一待搜索地图写入所述第一地图存储空间；在CPU执行步骤D时，所述CPU先向所述相关性扫描匹配硬件电路传输的第二层第一待搜索地图，所述CPU再向所述相关性扫描匹配硬件电路传输的第三层第一待搜索地图，然后由总线接口将第二层第一待搜索地图写入所述第一地图存储空间，并覆盖掉第一层第一待搜索地图；再由总线接口将第三层第一待搜索地图写入所述第二地图存储空间；其中，在完全覆盖掉第一层第一待搜索地图之前，已经获得所有根节点的得分值。10.根据权利要求7所述硬件加速定位系统，其特征在于，所述节点搜索模块包括选择器和累加器；所述累加器，用于在所述状态机控制模块的控制下，对来源于所述地图存储空间内的相应点云的占用概率进行累加，其中，所述地图存储空间内的相应点云的占用概率是在所述状态机控制模块的控制下，先按照所述索引值对应的地址传输给所述互连总线，再由所述互连总线依次传输到所述累加器的输入端；选择器的输入端与累加器的输出端连接；选择器，用于每隔一个预设计数周期，则选择将所述累加器的累加结果输出，并将当前输出的累加结果设置为对应一层第一待搜索地图中相应的一个节点的得分值；其中，所述点云处理模块每完成地图的相关性扫描匹配算法中的一次坐标系变换时，则触发选择器将所述累加器当前累加获得的累加结果输出至所述互连总线，并将当前获得的累加结果配置为所述搜索窗口对应搜索的一个新的节点的得分值。11.根据权利要求10所述硬件加速定位系统，其特征在于，所述节点搜索模块还包括栅格索引子模块，栅格索引子模块包括第一索引减法器、第二索引减法器、第一索引加法器和第二索引加法器，用于构建为流水线结构；其中，第一索引减法器和第二索引减法器都属于减法器，第一索引加法器和第二索引加法器都属于加法器；第一索引减法器的第一输入端用于接收所述互连总线传输过来的离散点云的横坐标，其中，所述互连总线传输给第一索引减法器的离散点云的横坐标是来源于所述存储器模块；第一索引减法器的第二输入端用于接收所述互连总线传输过来的横轴坐标偏移值，其
中，所述预设坐标偏移值包括横轴坐标偏移值，所述互连总线传输过来的横轴坐标偏移值是来源于地图偏移值寄存器内存储的所述预设坐标偏移值；地图偏移值寄存器是所述相关性扫描匹配硬件电路内部设置的参数寄存器，用于存储所述预先扩展的栅格地图相关联的坐标偏移值；第一索引加法器的第一输入端与第一索引减法器的输出端连接；第一索引加法器的第二输入端用于接收所述互连总线传输过来的横轴坐标搜索步长；其中，所述搜索步长包括横轴坐标搜索步长，所述互连总线传输过来的横轴坐标搜索步长是来源于搜索窗口参数寄存器内存储的所述搜索步长；搜索窗口参数寄存器是所述相关性扫描匹配硬件电路内部设置的参数寄存器，用于存储所述搜索窗口所存在的节点的位置及相关联的子节点搜索信息，包括所述搜索步长；第一索引加法器输出的和值被配置为所述离散点云映射到所述地图存储空间的横轴方向索引值；第二索引减法器的第一输入端用于接收所述互连总线传输过来的离散点云的纵坐标，其中，所述互连总线传输给第二索引减法器的离散点云的纵坐标是来源于所述存储器模块；第二索引减法器的第二输入端用于接收所述互连总线传输过来的纵轴坐标偏移值，其中，所述预设坐标偏移值还包括纵轴坐标偏移值，所述互连总线传输过来的纵轴坐标偏移值也是来源于地图偏移值寄存器内存储的所述预设坐标偏移值；第二索引加法器的第一输入端与第二索引减法器的输出端连接；第二索引加法器的第二输入端用于接收所述互连总线传输过来的纵轴坐标搜索步长；其中，所述搜索步长还包括纵轴坐标搜索步长，所述互连总线传输过来的纵轴坐标搜索步长也是来源于搜索窗口参数寄存器内存储的所述搜索步长；第二索引加法器输出的和值被配置为所述离散点云映射到所述地图存储空间的纵轴方向索引值。1...
【专利技术属性】
技术研发人员：王珂，肖刚军，包敏杰，马宝腾，周和文，许登科，孙明，
申请(专利权)人：哈尔滨工业大学，
类型：发明
国别省市：
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。