一种设备快速重定位方法、装置及存储介质与流程

1.本发明涉及重定位技术领域，尤其涉及一种设备快速重定位方法、装置及存储介质。


背景技术：

2.伴随着室内机器人的发展，面向矿山、港口、快递行业的室内机器人重定位技术是机器人自主导航的前提。
3.移动机器人重定位一般指在没有先验信息的前提下，机器人仅依靠自身传感器估计其在已知全局地图中的位姿。在初始位姿未知或发生“绑架”时，机器人均需要实现重定位。传统的方法多采用占用栅格来描述激光雷达测得的局部环境信息以及全局地图信息，再基于概率运算完成候选搜索区域内的重定位。室外良好的gps信号可提供准确的初始位姿，基于激光雷达的室内移动机器人反而没有gnss提供的准确的全局定位信息。cartographer使用占用栅格来表示环境信息，通过匹配栅格化后的激光雷达数据与地图数据并基于极大似然估计来求解最有可能的位姿。cartographer算法由于重定位时需在全局地图上搜索，定位时间较长且在单一结构环境中易定位失败。在导航时由于某种外部因素(如人为搬离、外部碰撞等)机器人位姿发生突变，从而导致原有依赖位姿连续变化的定位算法失效。在室内环境下，组合导航失去了gnss信号，无法提供准确的初始位姿，故室内大场景下cartographer无法实现快速重定位。


技术实现要素：

4.为了解决上述技术问题或者至少部分地解决上述技术问题，本发明提供一种设备快速重定位方法、装置及存储介质。
5.第一方面，本发明提供一种设备快速重定位方法，包括：取重定位前最后一雷达帧对应的稀疏点云地图作为参考帧，取当前时刻的雷达帧作为目标帧；参考帧所占空间分为若干格子，对于每个格子，基于格子内的点计算每个格子的多维正态概率密度函数；利用被迭代优化的变换参数使目标帧变换到参考帧所在的参考坐标系中，其中，变换参数包括旋转矩阵和平移矩阵；计算目标帧转换后每个点落到对应格子中的概率密度；进而将对应点落到对应格子中的概率密度之和作为目标函数；通过牛顿优化算法迭代优化变换参数，以寻找使得目标函数值最大的目标变换参数，基于目标变换参数和参考帧的坐标确定目标帧的第一初始位姿，并发布包含第一初始位姿第一话题；cartographer订阅第一话题获取第一初始位姿，将第一初始位姿的坐标转换到imu坐标系后，传递给位姿推测器实现重定位。
6.更进一步地，将室内场景通过激光雷达、imu和里程计融合的slam技术建立高精度
地图；将所述高精度地图转换为点云地图，对点云地图进行体素滤波得到降采样后的稀疏点云地图。
7.更进一步地，设备导航信号良好时，导航提供的导航位姿，导航位姿的坐标转换到imu坐标系得到第二初始位姿，发布第二初始位姿的第二话题，重定位时，cartographer订阅第二话题获取第二初始位姿，将第二初始位姿传递给位姿推测器实现重定位。
8.更进一步地，预先设定导航信号的延迟时间阈值，若导航信号的延迟时间大于延迟时间阈值或导航信号消失，则在重定位时cartographer订阅第一话题，将第一初始位姿的坐标转换到imu坐标系后，传递给位姿推测器实现重定位；若导航信号存在且导航信号端延迟时间小于延迟时间阈值，则在重定位时cartographer订阅第二话题，获取第二初始位姿，将第二初始位姿传递给位姿推测器实现重定位。
9.更进一步地，cartographer订阅第一话题或第二话题，以获取第一初始位姿或第二初始位姿，将所获的第一初始位姿或第二初始位姿转换后传递给位姿推测器，位姿推测器接收转换后的第一初始位姿或第二初始位姿前删除前序数据。
10.更进一步地，预先设定导航信号的延迟时间阈值，实时监测导航信号是否存在和存在时导航信号的延迟时间，重定位时，获取重定位时刻的导航信号的延迟时间，比较导航信号的延迟时间和延迟时间阈值。
11.第二方面，本发明提供一种设备快速重定位装置，包括：至少一处理单元、存储单元、激光雷达和imu传感器，所述处理单元、所述存储单元、所述激光雷达和imu传感器通过总线单元连接；所述存储单元存储计算机程序，所述处理单元从存储单元读取执行计算机程序实现所述的设备快速重定位方法。
12.更进一步地所述设备快速重定位装置，包括：gps传感器，所述gps传感器通过总线单元连接处理单元和存储单元；所述存储单元存储计算机程序，所述处理单元从存储单元读取执行计算机程序实现所述的设备快速重定位方法。
13.第三方面，本发明提供一种实现设备快速重定位方法的存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现所述的设备快速重定位方法。
14.本发明实施例提供的上述技术方案与现有技术相比具有如下优点：本发明一方面能够利用雷达帧和预先构建的点云地图进行匹配得到第一初始位姿，发布包含第一初始位姿的第一话题，cartographer利用第一初始位姿进行重定位，实现在无导航信号或导航信号延迟高的情况下的重定位。另一方面，本发明能够利用导航提供的导航位姿经转换得到第二初始位姿，cartographer能够利用第二初始位姿进行重定位，且在重定位时，获取导航信号存在情况和导航信号的延迟时间，若导航信号不存在或导航信号存在但导航信号延迟时间大于预设的延迟时间阈值则cartographer订阅第一话题，利用第一初始位姿进行重定位，导航存在且导航信号延迟时间小于预设的延迟时间阈值则cartographer定于第二话题，利用第二初始位姿进行重定位，选择更快的重定位方式进行重定位。
附图说明
15.此处的附图被并入说明书中并构成本说明书的一部分，示出了符合本发明的实施
例，并与说明书一起用于解释本发明的原理。
16.为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，对于本领域普通技术人员而言，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
17.图1为本发明实施例提供的一种设备快速重定位方法的流程图；图2为本发明实施例提供的计算第一初始位姿发布包含第一初始位姿的第一话题的流程图；图3为本发明实施例提供的计算第二初始位姿发布包含第二初始位姿的第二话题的流程图；图4为本发明实施例提供的一种设备快速重定位装置的示意图。
具体实施方式
18.为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明的一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都属于本发明保护的范围。
19.需要说明的是，在本文中，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个
……”
限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。
20.实施例1参阅图1所示，本发明实施例提供一种设备快速重定位方法，包括：预先设定导航信号的延迟时间阈值。所述延迟时间阈值的设定方式为：获取n次计算第一初始位姿所消耗的平均时间，获取n次计算第一初始位姿所消耗时间的方差，取平均时间加3倍的方差作为延迟时间阈值。
21.实时监测导航信号是否存在和存在时导航信号的延迟时间。重定位时，获取重定位时刻的导航信号存在情况和延时时间。
22.判断导航信号是否存在；若导航信号不存在，则在重定位时，cartographer订阅第一话题，通过第一话题获取第一初始位姿，将第一初始位姿的坐标转换到imu坐标系后，传递给位姿推测器实现重定位cartographer订阅第一话题，以获取第一初始位姿将所获的第一初始位姿转换后传递给位姿推测器，位姿推测器接收转换后的第一初始位姿前删除前序数据；若导航信号存在，则比较导航信号的延迟时间和延迟时间阈值，若待重定位的设备的导航信号的延迟时间大于延迟时间阈值，则在重定位时，cartographer订阅第一话题，通过第一话题获取第一初始位姿，将第一初始位姿的坐标转换到imu坐标系后，传递给位姿推测器实现重定位cartographer订阅第一话题，以获取第一初始位姿将所获的第一初始位姿转换后传递给位姿推测器，位姿推测器接收转换后的第一初始位姿前删除前序数据，若导航信号存在且导航信号端延迟时间小于延迟时间阈值，则在重定位时
cartographer订阅第二话题，获取第二初始位姿，将第二初始位姿传递给位姿推测器实现重定位；cartographer订阅第二话题，以获取第二初始位姿，将所获的第二初始位姿转换后传递给位姿推测器，位姿推测器接收转换后的第二初始位姿前删除前序数据。
23.其中，参阅图2所示，计算第一初始位姿发布包含第一初始位姿的第一话题的过程如下：将室内场景通过激光雷达、imu和里程计融合的slam技术建立高精度地图。具体的，通过激光雷达扫描室内场景，同时将imu、里程计信息加入到slam构图环境中，实现对室内场景高精度地图的构建，并保存高精度地图的二进制pbstream格式文件。
24.将所述高精度地图转换为点云地图，对点云地图进行体素滤波得到降采样后的稀疏点云地图。具体的，在点云地图中，通过voxelgrid类创建三维体素网格，遍历体素网格中每个体素内的点云地图点的坐标，利用体素内全部点的坐标计算每个体素的重心坐标，保留每个体素内距离中心最近的点，实现对点云地图的降采样。
25.待重定位设备通过雷达扫描所处室内场景得到雷达帧；取重定位前最后一的雷达帧对应的稀疏点云地图作为参考帧，取当前时刻的雷达帧作为目标帧。
26.将参考帧所占空间分为若干格子，对于每个格子，基于格子内的点计算每个格子的多维正态概率密度函数。
27.具体的，统计每个格子内的点，k=1,2，
……
m，计算格子内点多维正态概率密度的平均值：；计算多维正态概率密度的协方差：；则一个格子的多维正态概率密度函数为：，其中，a为常数。
28.利用被迭代优化的变换参数使目标帧变换到参考帧所在的参考坐标系中，其中，变换参数包括旋转矩阵和平移矩阵；计算目标帧转换后每个点落到对应格子中的概率密度，进而将对应点落到对应格子中的概率密度之和作为目标函数，通过牛顿优化算法迭代优化变换参数，以寻找使得目标函数值最大的目标变换参数，基于目标变换参数和参考帧的坐标确定目标帧的第一初始位姿，并发布包含第一初始位姿的第一话题。
29.参阅图3所示，获取第二位姿，发布包含第二位姿的第二话题的过程如下：设备的导航信号良好时，获取导航提供的导航位姿，导航位姿的坐标转换到imu坐标系得到第二初始位姿，发布包含第二初始位姿的第二话题。
30.实施例2参阅图4所示，本发明实施例提供一种设备快速重定位装置，包括：至少一处理单
元、存储单元、激光雷达、imu传感器和gps传感器，所述处理单元、所述存储单元、所述激光雷达、gps传感器和imu传感器通过总线单元连接；所述存储单元存储计算机程序，所述处理单元从存储单元读取执行计算机程序实现所述的设备快速重定位方法。
31.实施例3本发明实施例提供一种实现设备快速重定位方法的存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现所述的设备快速重定位方法。
32.在本发明所提供的实施例中，应该理解到，所揭露的结构，可以通过其它的方式实现。例如，以上所描述的结构实施例仅仅是示意性的，例如，所述单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些接口，结构或单元的间接耦合或通信连接，可以是电性，机械或其它的形式。
33.所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。
34.另外，在本发明各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。
35.以上所述仅是本发明的具体实施方式，使本领域技术人员能够理解或实现本发明。对这些实施例的多种修改对本领域的技术人员来说将是显而易见的，本文中所定义的一般原理可以在不脱离本发明的精神或范围的情况下，在其它实施例中实现。因此，本发明将不会被限制于本文所示的这些实施例，而是要符合与本文所申请的原理和新颖特点相一致的最宽的范围。