CN112986982A - 环境地图参照定位方法、装置和移动机器人 - Google Patents

Abstract

本申请涉及环境地图参照定位方法、装置和移动机器人，方法包括：获取激光雷达的当前点云数据帧和毫米波雷达的雷达测量帧；根据当前点云数据帧进行语义物体检测，确定视场中可移动物体的第一语义物体检测框；根据当前点云数据帧的体素网格与第一语义物体检测框，对雷达测量帧的测量点进行降噪和类别赋值处理；根据第一语义物体检测框和处理后的测量点的速度信息，提取显著动态物体；将当前点云数据帧与预设环境地图进行比较，确定第一环境变动点簇与第一测量点集；将第一测量点集用于预设环境地图的参照定位，获得第一位姿估计值；根据第一测量点集和第一位姿估计值确定使用当前点云数据帧重定位。定位精度得到了大幅提升。

Description

环境地图参照定位方法、装置和移动机器人

技术领域

本申请涉及机器人定位导航技术领域，特别是涉及一种环境地图参照定位方法、装置和移动机器人。

背景技术

随着机器人技术的发展，定位技术是移动机器人等智能载具实现自主导航的关键技术，提供包括位置和航向角分量的位姿估计。在移动机器人导航应用中，使用预设的环境地图进行实时定位和路径规划，并且在地图坐标系中执行所规划的路径。其中，激光雷达具有120度或360度等规格的水平视角，能够获得每个测量点的三维位置及点距，在自动驾驶领域被广泛使用。车用毫米波雷达的测量噪声较大，能获得稀疏的测量点以及测量点的径向速度。

环境地图参照定位方法使用事先构造好的预设静态环境地图，在移动机器人运行时通过将预设静态环境地图与当前测量点云数据帧进行配准或查询，结合移动机器人自身局部运动信息，获得机器人在环境地图坐标系中的位姿。然而，在实现本发明过程中，发明人发现前述传统的环境地图参照定位方法，仍存在着定位精度不高的技术问题。

发明内容

基于此，有必要针对上述技术问题，提供一种高定位精度的环境地图参照定位方法、一种环境地图参照定位装置以及一种移动机器人。

为了实现上述目的，本发明实施例采用以下技术方案：

一方面，本发明实施例提供一种环境地图参照定位方法，包括步骤：

获取激光雷达的当前点云数据帧和毫米波雷达的雷达测量帧；

根据当前点云数据帧进行语义物体检测，确定视场中可移动物体的第一语义物体检测框；

根据当前点云数据帧的体素网格与第一语义物体检测框，对雷达测量帧的测量点进行降噪和类别赋值处理；

根据第一语义物体检测框和处理后的测量点的速度信息，提取显著动态物体；

将当前点云数据帧与预设环境地图进行比较，确定第一环境变动点簇与第一测量点集；所述第一环境变动点簇为当前周边环境中相比于预设环境地图中各个点簇已经发生位移且所述位移大于预设位移阈值的激光点簇，所述第一测量点集为不属于所述显著动态物体及所述第一环境变动点簇的测量点集合；

将第一测量点集用于预设环境地图参照的定位，获得第一位姿估计值；

根据第一测量点集和第一位姿估计值确定使用当前点云数据帧重定位。

在其中一个实施例中，根据当前点云数据帧进行语义物体检测的过程，包括：

采用预训练的激光雷达三维物体检测模型，对设定周边范围内的物体进行检测和语义分类处理。

在其中一个实施例中，根据当前点云数据帧的体素网格与第一语义物体检测框，对雷达测量帧的测量点进行降噪和类别赋值处理的步骤，包括：

将雷达测量帧对应的测量点转换到激光雷达的坐标系；

根据各测量点与各第一语义物体检测框的对应关系，确定各相应第一语义物体检测框的ID类别；

采用KD树方法搜索当前点云数据帧中的K个最近邻点作为第一相邻激光点集；

根据第一相邻激光点集中属于各相应第一语义物体检测框的ID类别的各激光点，计算激光三维中心点；

当第一相邻激光点集中的点数大于预设的第一点数阈值，且测量点与激光三维中心点的距离小于第一距离阈值时，确定测量点不属于噪声测量点并用相应第一语义物体检测框的ID进行类别赋值。

在其中一个实施例中，根据当前点云数据帧的体素网格与第一语义物体检测框，对雷达测量帧的测量点进行降噪和类别赋值处理的步骤，还包括：

当第一相邻激光点集中的点数小于预设的第一点数阈值，且测量点与激光三维中心点的距离大于第一距离阈值时，确定测量点属于噪声测量点并进行去噪处理。

在其中一个实施例中，根据第一语义物体检测框和处理后的测量点的速度信息，提取显著动态物体的步骤，包括：

根据第一语义物体检测框的信息，将物体几何宽度信息大于预设的第一宽度阈值的物体提取为第一显著物体；

根据毫米波雷达的测量点的径向速度和类别信息，将物体速度大于预设速度阈值的第一显著物体提取为显著动态物体。

在其中一个实施例中，将当前点云数据帧与预设环境地图进行比较，确定第一环境变动点簇的过程，包括：

根据前一时刻的有效位姿估计值及当前时段内的运动信息，提取预设环境地图中的第一预设局部子图；

将当前点云数据帧转换为第二当前局部地图后，与第一预设局部子图比较确定第一环境变动点簇。

在其中一个实施例中，根据第一测量点集和第一位姿估计值确定使用当前点云数据帧重定位的步骤，包括：

当各显著动态物体及第一环境变动点簇在激光雷达视场中所占总比例小于预设的第一动态比例阈值时，根据第一位姿估计值确定当前点云数据帧与预设环境地图的吻合程度；

当吻合程度低于预设的第一吻合度阈值时，使用当前点云数据帧重定位。

在其中一个实施例中，上述方法还包括：

当各显著动态物体及第一环境变动点簇在激光雷达视场中所占总比例大于第一动态比例阈值时，确定第一测量点集的稀疏程度大于预设的第一稀疏程度阈值；

采用最近时段的位姿增量结合前一时刻的有效位姿估计值进行增量式位姿推算，得到第二位姿估计值并存储当前点云数据帧；第二位姿估计值及当前点云数据帧用于进行离线全局环境地图的更新。

另一方面，还提供一种环境地图参照定位装置，包括：

数据获取模块，用于获取激光雷达的当前点云数据帧和毫米波雷达的雷达测量帧；

语义检测模块，用于根据当前点云数据帧进行语义物体检测，确定视场中可移动物体的第一语义物体检测框；

降噪赋值模块，用于根据当前点云数据帧的体素网格与第一语义物体检测框，对雷达测量帧的测量点进行降噪和类别赋值处理；

显著提取模块，用于根据第一语义物体检测框和处理后的测量点的速度信息，提取显著动态物体；

动态确定模块，用于将当前点云数据帧与预设环境地图进行比较，确定第一环境变动点簇与第一测量点集；第一环境变动点簇为当前周边环境中相比于预设环境地图中各个点簇已经发生位移且位移大于预设位移阈值的激光点簇，第一测量点集为不属于显著动态物体及第一环境变动点簇的测量点集合；

参照位姿模块，用于将第一测量点集用于预设环境地图的参照定位，获得第一位姿估计值；

识别定位模块，用于根据第一测量点集和第一位姿估计值确定使用当前点云数据帧重定位。

又一方面，还提供一种移动机器人，包括存储器和处理器，存储器存储有计算机程序，处理器执行计算机程序时实现上述环境地图参照定位方法的步骤。

上述技术方案中的一个技术方案具有如下优点和有益效果：

上述环境地图参照定位方法、装置和移动机器人，通过结合激光雷达的当前点云数据帧和毫米波雷达的雷达测量帧的数据，与预设环境地图比较进行显著动态物体与动态点的判断，选择不属于显著动态物体及第一环境变动点簇的第一测量点云子集用于定位，获得第一位姿估计值，根据第一测量点和第一位姿估计值确定使用当前点云数据帧重定位。如此，可根据移动载体周边环境中的显著动态物体和动态点簇所占视角范围比例，自适应调节定位机制，避免在周边环境与预设静态环境地图发生较大变化的局部区域的明显定位误差及定位识别误差，达到了大幅提升环境地图参照定位的定位精度的技术效果。

附图说明

图1为一个实施例中环境地图参照定位方法的流程示意图；

图2为一个实施例中测量点降噪与赋值处理的示意图；

图3为另一个实施例中测量点降噪与赋值处理的示意图；

图4为另一个实施例中环境地图参照定位方法的流程示意图；

图5为一个实施例中环境地图参照定位方法的应用流程示意图；

图6为一个实施例中环境地图参照定位装置的模块结构示意图。

具体实施方式

为了使本申请的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本申请进行进一步详细说明。应当理解，此处描述的具体实施例仅仅用以解释本申请，并不用于限定本申请。

除非另有定义，本文所使用的所有的技术和科学术语与属于本申请的技术领域的技术人员通常理解的含义相同。本文中在本申请的说明书中所使用的术语只是为了描述具体的实施例的目的，不是旨在于限制本申请。本文所使用的术语“和／或”包括一个或多个相关的所列项目的任意的和所有的组合。

另外，本发明各个实施例之间的技术方案可以相互结合，但是必须是以本领域普通技术人员能够实现为基础，当技术方案的结合出现相互矛盾或无法实现时，应当认为这种技术方案的结合不存在，也不在本发明要求的保护范围之内。

在实践中，发明人发现传统的环境地图参照定位方法，在使用当前点云数据帧时，对当前各测量点的来源物体与预设静态环境地图中的已有物体是否为同一个物体不进行判断、而且对于各测量点来源物体是否为可移动的静态物体或不可移动的静止物体也不进行判断。

与预设静态环境地图的构建日期相比，随着时间推移，车辆等可移动物体通常会离开原来的位置，而且一些行人及车辆等可移动物体会进入机器人行驶所处周边区域，导致预设静态环境地图与移动机器人当前行驶所处的周边真实环境存在差异，即预设静态环境地图与当前周边环境不一致。已有的移动机器人在预设静态环境地图中定位方法使用整个点云数据帧时，或者所使用的一些测量点来源物体并非预设环境地图中的对应物体，会出现定位误差。

本发明针对前述传统的环境地图参照定位方法，存在着定位精度不高的技术问题，提供一种改进的环境地图参照定位方法，该方法使用物体检测方法来确定可移动物体的位置和距离，进而选择不属于动态点簇的测量点集进行环境地图参照定位和重定位，减小了定位误差，提高了定位精度。

请参阅图1，在一个实施例中，本发明提供一种环境地图参照定位方法，包括如下步骤S12至S24：

S12，获取激光雷达的当前点云数据帧和毫米波雷达的雷达测量帧。

可以理解，激光雷达和毫米波雷达等传感部件可以搭载在移动载体上，例如但不限于移动机器人或无人物流车辆。移动载体上的定位导航控制设备或者云端计算设备，可以通过传感器接口实时获取激光雷达的当前点云数据帧和毫米波雷达的雷达测量帧，以及预先获得的各传感器之间的外参标定矩阵，用于为激光点云帧与毫米波测量帧中的测量值建立对应关系。各传感器包括但不限于各激光雷达与毫米波雷达等传感器。

在一些实施方式中，可选的，毫米波雷达可以具有多个，其视场可以是不重叠的，也可以是有重叠，具体可以根据实际应用场景的需要确定。

S14，根据当前点云数据帧进行语义物体检测，确定视场中可移动物体的第一语义物体检测框。

可以理解，本实施例中点云数据帧、语义物体检测和语义物体检测框等术语的理解，可以参照已有的激光雷达技术、物体检测与语义分割等技术中的已有相应解释说明同理理解，本说明书中不再展开赘述。前述视场是指激光雷达视场，该视场中可以存在着多种不同物体，部分物体为固定位置的物体，另一部分的物体则可以是可移动的物体。利用当前点云数据帧进行语义物体检测确定可移动物体的检测框范围。

S16，根据当前点云数据帧的体素网格与第一语义物体检测框，对雷达测量帧的测量点进行降噪和类别赋值处理。

可以理解，确定可移动物体的第一语义物体检测框后，可以利用当前点云数据帧的体素网格与第一语义物体检测框，对毫米波雷达雷达测量帧的各测量点进行降噪处理，如确定噪声测量点和有效的非噪声测量点，提取有效的非噪声测量点而去掉噪声测量点。对于有效的非噪声测量点则为其进行类别赋值处理，也即用相应的第一语义物体检测框的ID类别赋值给这些有效的非噪声测量点。

S18，根据第一语义物体检测框和处理后的测量点的速度信息，提取显著动态物体。

可以理解，第一语义物体检测框可以包含有方向角范围、距离和类别等信息，可提取出视场中物体的宽度和高度等信息，雷达测量帧的测量点可以包含径向速度信息，可以用于换算出物体的速度信息。通过物体的宽度和高度等信息，以及测量点的径向速度信息即可提取出视场中的显著动态物体，显著动态物体是指视场中相比于预设环境地图中相同位置处的物体发生明显移动的动态物体。

S20，将当前点云数据帧与预设环境地图进行比较，确定第一环境变动点簇和第一测量点集；第一环境变动点簇为当前周边环境中相比于预设环境地图中各个点簇已经发生位移且所述位移大于预设位移阈值的激光点簇，第一测量点集为不属于显著动态物体及第一环境变动点簇的测量点集合。当前点云数据帧中的点可分为静态和动态两类，通过将该点云与预设环境地图进行比较即可区分该点云中的纵向静态点簇与第一环境变动点簇。

S22，将第一测量点集用于预设环境地图参照的定位，获得第一位姿估计值；

S24，根据第一测量点集和第一位姿估计值确定使用当前点云数据帧重定位。

可以理解，根据当前点云数据帧与预设环境地图（也即预设静态环境地图）局部区域范围中匹配程度最高的点云帧的吻合程度，以及第一测量点集的稀疏程度，使用当前点云数据帧重定位，减少定位误差并提高定位精度。吻合程度的判断方式可以采用本领域已有的地图吻合程度判断方式实现。其中，本实施例的重定位可包括地点识别、位置赋值(assignment)与确认（例如限定重定位的地点必须处于地图一定的区域范围中）等操作。

上述环境地图参照定位方法，通过结合激光雷达的当前点云数据帧和毫米波雷达的雷达测量帧的数据，与预设环境地图比较进行显著动态物体与动态点的判断，选择不属于显著动态物体及第一环境变动点簇的第一测量点云子集（也即第一测量点集）用于定位，获得第一位姿估计值，根据第一测量点和第一位姿估计值确定使用当前点云数据帧重定位。如此，可根据移动载体周边环境中的显著动态物体和动态点簇所占视角范围比例，自适应调节定位机制，避免在周边环境与预设静态环境地图发生较大变化的局部区域的明显定位误差及定位识别误差，达到了大幅提升环境地图参照定位的定位精度的技术效果。

在一个实施例中，可选的，关于上述步骤S14中根据当前点云数据帧进行语义物体检测的过程，具体可以包括如下处理步骤：

采用预训练的激光雷达三维物体检测模型，对设定周边范围内的物体进行检测和语义分类处理。

可选的，在本实施例中，采用预训练的激光雷达三维物体检测模型对移动载体设定周边范围内，例如但不限于周边40米或50米范围内的物体进行检测和语义分类。在一些实施方式中，可以采用PointPillars或PointNet++等三维物体检测模型进行前述物体检测和语义分类。

请参阅图2，在一个实施例中，可选的，关于上述步骤S16，具体可以包括如下处理步骤S162至步骤S169：

S162，将雷达测量帧对应的测量点转换到激光雷达的坐标系；

S164，根据各测量点与各第一语义物体检测框的对应关系，确定各相应第一语义物体检测框的ID类别；

S166，采用KD树方法搜索当前点云数据帧中的K个最近邻点作为第一相邻激光点集；

S168，根据第一相邻激光点集中属于各相应第一语义物体检测框的ID类别的各激光点，计算激光三维中心点；

S169，当第一相邻激光点集中的点数大于预设的第一点数阈值，且测量点与激光三维中心点的距离小于第一距离阈值时，确定测量点不属于噪声测量点并用相应第一语义物体检测框的ID进行类别赋值。

可以理解，在本实施例中，最近邻点的个数K的具体取值可以根据实际计算的精度和计算量综合确定。具体的，将毫米波雷达的测量点转换到激光雷达所使用的坐标系；针对每个毫米波雷达的测量点，判断其与激光点云各个第一语义物体检测框的对应关系，找出其对应的第一语义物体检测框的ID类别。用KD树方法搜索激光点云中的K个最近邻作为第一相邻激光点集。根据毫米波雷达的测距精度选择相应的搜索半径，例如但不限于0.5米或1米。

对第一相邻激光点集中属于第一语义物体检测框的ID类别的各激光点，计算激光三维中心点。当第一相邻激光点集中的点数大于预设的第一点数阈值，而且在搜索半径内，毫米波雷达的测量点与激光三维中心点的距离小于第一距离阈值时，判定毫米波雷达的测量点不属于噪声测量点并用第一语义物体检测框的ID为其进行类别赋值。其中，第一点数阈值的具体取值可以由所需的降噪精度大小确定，第一距离阈值可以由选择的搜索半径与所需的降噪精度大小确定。

请参阅图3，在一个实施例中，可选的，关于上述步骤S16，具体还可以包括如下处理步骤S170：

当第一相邻激光点集中的点数小于预设的第一点数阈值，且测量点与激光三维中心点的距离大于第一距离阈值时，确定测量点属于噪声测量点并进行去噪处理。

可以理解，相反的，当第一相邻激光点集中的点数小于预设的第一点数阈值，且测量点与激光三维中心点的距离大于第一距离阈值时，即判定该毫米波雷达的测量点属于噪声测量点，将会消除而不纳入后续处理步骤中的使用的测量点范围。

在一个实施例中，可选的，关于上述步骤S18，具体还可以包括如下处理步骤：

根据第一语义物体检测框的信息，将物体几何宽度信息大于预设的第一宽度阈值的物体提取为第一显著物体；

根据毫米波雷达的测量点的径向速度和类别信息，将物体速度大于预设速度阈值的第一显著物体提取为显著动态物体。

可以理解，物体速度为被测量物体的速度，可采用处理后的测量点的径向速度直接换算得到。优选的，激光雷达通过目标跟踪方法估计得到移动载体周边30米内的物体速度估计值，与毫米波雷达经过降噪处理得到的物体速度测量值进行信息融合，从而可以得到更精确的物体速度。第一宽度阈值和预设速度阈值的具体取值大小均可以根据实际应用场景的移动物体的几何尺寸大小及分辨能力等进行选取，只要能够有效分辨静态与动态物体即可。通过上述步骤，即可有效且高效地实现视场中显著动态物体的提取。

在一个实施例中，可选的，关于上述步骤S20，具体还可以包括如下处理步骤：

根据前一时刻的有效位姿估计值及当前时段内的运动信息，提取预设环境地图中的第一预设局部子图；

将当前点云数据帧转换为第二当前局部地图后，与第一预设局部子图比较确定第一环境变动点簇。

可以理解，以移动载体为移动机器人为例，可根据机器人在前一时刻已经获得的有效位姿估计值及当前时段内的运动信息，直接提取预设环境地图中的第一预设局部子图。将激光雷达的当前点云数据帧转换为第二当前局部地图，将该第二当前局部地图与第一预设局部子图进行比较，即可直观高效地区分纵向静态点簇与第一环境变动点簇。其中，纵向静态点簇与第一环境变动点簇的具体判断确定方法可以使用已有的ERASOR方法（为本领域中用于三维动态物体点簇移除的公知方法）。其中，纵向静态点簇是指当前激光点云数据帧中，与预设环境地图的点云中各对应点簇相比，未发生位移或所发生的位移量小于预设位移阈值的点簇。

在一个实施例中，可选的，关于上述步骤S24，具体还可以包括如下处理步骤：

当各显著动态物体及第一环境变动点簇在激光雷达视场中所占总比例小于预设的第一动态比例阈值时，根据第一位姿估计值确定当前点云数据帧与预设环境地图的吻合程度；

当吻合程度低于预设的第一吻合度阈值时，使用当前点云数据帧重定位。

可以理解，第一动态比例阈值和第一吻合度阈值的具体取值大小，分别可以根据实际应用场景的分辨率和定位精度要求等选择。当各显著动态物体及第一环境变动点簇在激光雷达视场中所占总比例小于或等于预设的第一动态比例阈值时，选择不属于显著动态物体的第一测量点云用于定位。具体的，当各个显著动态物体及第一环境变动点簇的动态点在激光雷达视场中所占总比例小于预设第一动态比例阈值时，根据当前位姿估计值（也即获得的第一位姿估计值），判断当前点云数据帧与预设环境地图局部区域范围中匹配程度最高的点云帧的吻合程度；当吻合程度低于预设的第一吻合度阈值时，使用点云数据帧重定位，限定全局定位误差范围。

请参阅图4和图5，在一个实施例中，上述环境地图参照定位方法具体还可以包括如下处理步骤：

S21，当各显著动态物体及第一环境变动点簇在激光雷达视场中所占总比例大于第一动态比例阈值时，确定第一测量点集的稀疏程度大于预设的第一稀疏程度阈值；

S23，采用最近时段的位姿增量结合前一时刻的有效位姿估计值进行增量式位姿推算，得到第二位姿估计值并存储当前点云数据帧；第二位姿估计值及当前点云数据帧用于进行离线全局环境地图的更新。

可以理解，在本实施例中，当各显著动态物体及第一环境变动点簇在激光雷达视场中所占总比例大于第一动态比例阈值时，判断第一测量点集的稀疏程度大于预设的第一稀疏程度阈值，此时认为不足以进行与预设环境地图参照的有效、高精度定位。

因此，使用最近时段移动载具自身的位姿增量结合前一时刻的有效位姿估计值进行增量式位姿推算，获得第二位姿估计值，并且存储点云关键帧，用于离线手工进行全局环境地图的更新，实现对前述预设环境地图的更新操作，从而为后续定位精度的提高提供支撑。

在一个实施例中，关于上述的步骤S22，具体的实现步骤包括：使用环境地图参照的自适应蒙特卡洛定位方法或NDT定位方法，根据第一测量点集结合移动载体的自身运动信息，进行预设环境地图参照的载具定位，得到第一位姿估计值。可以理解，本实施例中，采用了本领域公知的环境地图参照的自适应蒙特卡洛定位方法或NDT定位方法，基于上述获得的第一测量点集，同时结合上述移动载体的自身运动信息（其自身的传感器实时测量得到）进行相应的参照的定位处理，以获得上述的第一位姿估计值，处理效率高且准确度高。

应该理解的是，虽然图1至图5的流程图中的各个步骤按照箭头的指示依次显示，但是这些步骤并不是必然按照箭头指示的顺序依次执行。除非本文中有明确的说明，这些步骤的执行并没有严格的顺序限制，这些步骤可以以其它的顺序执行。而且图1至图5的至少一部分步骤可以包括多个子步骤或者多个阶段，这些子步骤或者阶段并不必然是在同一时刻执行完成，而是可以在不同的时刻执行，这些子步骤或者阶段的执行顺序也不必然是依次进行，而是可以与其它步骤或者其它步骤的子步骤或者阶段的至少一部分轮流或者交替地执行。

请参阅图6，还提供了一种环境地图参照定位装置100，包括数据获取模块13、语义检测模块15、降噪赋值模块17、显著提取模块19、动态确定模块21、参照位姿模块23和识别定位模块25。其中，数据获取模块13用于获取激光雷达的当前点云数据帧和毫米波雷达的雷达测量帧。语义检测模块15用于根据当前点云数据帧进行语义物体检测，确定视场中可移动物体的第一语义物体检测框。降噪赋值模块17用于根据当前点云数据帧的体素网格与第一语义物体检测框，对雷达测量帧的测量点进行降噪和类别赋值处理。显著提取模块19用于根据第一语义物体检测框和处理后的测量点的速度信息，提取显著动态物体。动态确定模块21用于将当前点云数据帧与预设环境地图进行比较，确定第一环境变动点簇与第一测量点集；第一环境变动点簇为当前周边环境中相比于预设环境地图中各个点簇已经发生位移且位移大于预设位移阈值的激光点簇，第一测量点集为不属于显著动态物体及第一环境变动点簇的测量点集合。参照位姿模块23用于将第一测量点集用于预设环境地图参照的定位，获得第一位姿估计值。识别定位模块25用于根据第一测量点集和第一位姿估计值确定使用当前点云数据帧重定位。

上述环境地图参照定位装置100，通过各模块的协作，结合激光雷达的当前点云数据帧和毫米波雷达的雷达测量帧的数据，与预设环境地图比较进行显著动态物体与动态点的判断，选择不属于显著动态物体及第一环境变动点簇的第一测量点云子集用于定位，获得第一位姿估计值，根据第一测量点和第一位姿估计值确定使用当前点云数据帧重定位。如此，可根据移动载体周边环境中的显著动态物体和动态点簇所占视角范围比例，自适应调节定位机制，避免在周边环境与预设静态环境地图发生较大变化的局部区域的明显定位误差及定位识别误差，达到了大幅提升环境地图参照定位的定位精度的技术效果。

在一个实施例中，上述语义检测模块15在根据当前点云数据帧进行语义物体检测的过程中，具体可以用于采用预训练的激光雷达三维物体检测模型，对设定周边范围内的物体进行检测和语义分类处理。

在一个实施例中，上述降噪赋值模块17具体可以包括：

坐标转换子模块，用于将雷达测量帧对应的测量点转换到激光雷达的坐标系；

类别确定子模块，用于根据各测量点与各第一语义物体检测框的对应关系，确定各相应第一语义物体检测框的ID类别；

邻点搜索子模块，用于采用KD树方法搜索当前点云数据帧中的K个最近邻点作为第一相邻激光点集；

中心计算子模块，用于根据第一相邻激光点集中属于各相应第一语义物体检测框的ID类别的各激光点，计算激光三维中心点；

赋值子模块，用于当第一相邻激光点集中的点数大于预设的第一点数阈值，且测量点与激光三维中心点的距离小于第一距离阈值时，确定测量点不属于噪声测量点并用相应第一语义物体检测框的ID进行类别赋值。

在一个实施例中，上述降噪赋值模块17具体还可以包括去噪子模块，用于当第一相邻激光点集中的点数小于预设的第一点数阈值，且测量点与激光三维中心点的距离大于第一距离阈值时，确定测量点属于噪声测量点并进行去噪处理。

在一个实施例中，上述显著提取模块19具体可以包括：

显著物子模块，用于根据第一语义物体检测框的信息，将物体几何宽度信息大于预设的第一宽度阈值的物体提取为第一显著物体；

动态物子模块，用于根据毫米波雷达的测量点的径向速度和类别信息，将物体速度大于预设速度阈值的第一显著物体提取为显著动态物体。

在一个实施例中，上述动态确定模块21具体可以包括：

第一局部子模块，用于根据前一时刻的有效位姿估计值及当前时段内的运动信息，提取预设环境地图中的第一预设局部子图；

第二局部子模块，用于将当前点云数据帧转换为第二当前局部地图后，与第一预设局部子图比较确定第一环境变动点簇。

在一个实施例中，上述识别定位模块25具体可以包括：

吻合判断子模块，用于当各显著动态物体及第一环境变动点簇在激光雷达视场中所占总比例小于预设的第一动态比例阈值时，根据第一位姿估计值确定当前点云数据帧与预设环境地图局部区域范围中匹配程度最高的点云帧的吻合程度；

阈值比较子模块，用于当吻合程度低于预设的第一吻合度阈值时，使用当前点云数据帧重定位。

在一个实施例中，上述环境地图参照定位装置100还可以包括稀疏确定模块和地图更新模块。稀疏确定模块用于当各显著动态物体及第一环境变动点簇在激光雷达视场中所占总比例大于第一动态比例阈值时，确定第一测量点集的稀疏程度大于预设的第一稀疏程度阈值。地图更新模块用于采用最近时段的位姿增量结合前一时刻的有效位姿估计值进行增量式位姿推算，得到第二位姿估计值并存储当前点云数据帧；第二位姿估计值及当前点云数据帧用于进行离线全局环境地图的更新。

关于环境地图参照定位装置100的具体限定，可以参见上文中环境地图参照定位方法的相应限定，在此不再赘述。上述环境地图参照定位装置100中的各个模块可全部或部分通过软件、硬件及其组合来实现。上述各模块可以硬件形式内嵌于或独立于具体数据处理功能的设备中，也可以软件形式存储于前述设备的存储器中，以便于处理器调用执行以上各个模块对应的操作，前述设备可以是本领域各型移动机器人的微处理/控制设备。

又一方面，还提供一种移动机器人，包括存储器和处理器，存储器存储有计算机程序，处理器执行计算机程序时可以实现以下步骤：获取激光雷达的当前点云数据帧和毫米波雷达的雷达测量帧；根据当前点云数据帧进行语义物体检测，确定视场中可移动物体的第一语义物体检测框；根据当前点云数据帧的体素网格与第一语义物体检测框，对雷达测量帧的测量点进行降噪和类别赋值处理；根据第一语义物体检测框和处理后的测量点的速度信息，提取显著动态物体；将当前点云数据帧与预设环境地图进行比较，确定第一环境变动点簇与第一测量点集；第一环境变动点簇为当前周边环境中相比于预设环境地图中各个点簇已经发生位移且位移大于预设位移阈值的激光点簇，第一测量点集为不属于显著动态物体及第一环境变动点簇的测量点集合；将第一测量点集用于预设环境地图的参照定位，获得第一位姿估计值；根据第一测量点集和第一位姿估计值确定使用当前点云数据帧重定位。

需要说明的是，本实施例中的移动机器人除上述列出的存储器和处理器之外，还可以包括其他必要组成构件，例如但不限于行走机构、抓取机构、收纳机构和/或躯干本体，具体可以根据实际应用场景中的移动机器人的型号确定。

在一个实施例中，处理器执行计算机程序时还可以实现上述环境地图参照定位方法各实施例中增加的步骤或者子步骤。

再一方面，还提供一种计算机可读存储介质，其上存储有计算机程序，计算机程序被处理器执行时实现以下步骤：获取激光雷达的当前点云数据帧和毫米波雷达的雷达测量帧；根据当前点云数据帧进行语义物体检测，确定视场中可移动物体的第一语义物体检测框；根据当前点云数据帧的体素网格与第一语义物体检测框，对雷达测量帧的测量点进行降噪和类别赋值处理；根据第一语义物体检测框和处理后的测量点的速度信息，提取显著动态物体；将当前点云数据帧与预设环境地图进行比较，确定第一环境变动点簇与第一测量点集；第一环境变动点簇为当前周边环境中相比于预设环境地图中各个点簇已经发生位移且位移大于预设位移阈值的激光点簇，第一测量点集为不属于显著动态物体及第一环境变动点簇的测量点集合；将第一测量点集用于预设环境地图的参照定位，获得第一位姿估计值；根据第一测量点集和第一位姿估计值确定使用当前点云数据帧重定位。

在一个实施例中，计算机程序被处理器执行时，还可以实现上述环境地图参照定位方法各实施例中增加的步骤或者子步骤。

本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程，是可以通过计算机程序来指令相关的硬件来完成的，计算机程序可存储于一非易失性计算机可读取存储介质中，该计算机程序在执行时，可包括如上述各方法的实施例的流程。其中，本申请所提供的各实施例中所使用的对存储器、存储、数据库或其它介质的任何引用，均可包括非易失性和/或易失性存储器。非易失性存储器可包括只读存储器（ROM）、可编程ROM（PROM）、电可编程ROM（EPROM）、电可擦除可编程ROM（EEPROM）或闪存。易失性存储器可包括随机存取存储器（RAM）或者外部高速缓冲存储器。作为说明而非局限，RAM以多种形式可得，诸如静态RAM（SRAM）、动态RAM（DRAM）、同步DRAM（SDRAM）、双数据率SDRAM（DDRSDRAM）、增强型SDRAM（ESDRAM）、同步链路（Synchlink） DRAM（SLDRAM）、存储器总线式动态随机存储器（Rambus DRAM，简称RDRAM）以及接口动态随机存储器（DRDRAM）等。

以上实施例的各技术特征可以进行任意的组合，为使描述简洁，未对上述实施例中的各个技术特征所有可能的组合都进行描述，然而，只要这些技术特征的组合不存在矛盾，都应当认为是本说明书记载的范围。

以上实施例仅表达了本申请的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本申请构思的前提下，还可做出若干变形和改进，都属于本申请保护范围。因此本申请专利的保护范围应以所附权利要求为准。

Claims (10)

1.一种环境地图参照定位方法，其特征在于，包括步骤：

获取激光雷达的当前点云数据帧和毫米波雷达的雷达测量帧；

根据所述当前点云数据帧进行语义物体检测，确定视场中可移动物体的第一语义物体检测框；

根据所述当前点云数据帧的体素网格与所述第一语义物体检测框，对所述雷达测量帧的测量点进行降噪和类别赋值处理；

根据所述第一语义物体检测框和处理后的所述测量点的速度信息，提取显著动态物体；

将所述当前点云数据帧与预设环境地图进行比较，确定第一环境变动点簇与第一测量点集；所述第一环境变动点簇为当前周边环境中相比于预设环境地图中各个点簇已经发生位移且所述位移大于预设位移阈值的激光点簇，所述第一测量点集为不属于所述显著动态物体及所述第一环境变动点簇的测量点集合；

将所述第一测量点集用于所述预设环境地图参照的定位，获得第一位姿估计值；

根据所述第一测量点集和所述第一位姿估计值确定使用所述当前点云数据帧重定位。

2.根据权利要求1所述的环境地图参照定位方法，其特征在于，根据所述当前点云数据帧进行语义物体检测的过程，包括：

采用预训练的激光雷达三维物体检测模型，对设定周边范围内的物体进行检测和语义分类处理。

3.根据权利要求1或2所述的环境地图参照定位方法，其特征在于，根据所述当前点云数据帧的体素网格与所述第一语义物体检测框，对所述雷达测量帧的测量点进行降噪和类别赋值处理的步骤，包括：

将所述雷达测量帧对应的测量点转换到所述激光雷达的坐标系；

根据各所述测量点与各所述第一语义物体检测框的对应关系，确定各相应所述第一语义物体检测框的ID类别；

采用KD树方法搜索所述当前点云数据帧中的K个最近邻点作为第一相邻激光点集；

根据所述第一相邻激光点集中属于各相应所述第一语义物体检测框的ID类别的各激光点，计算激光三维中心点；

当所述第一相邻激光点集中的点数大于预设的第一点数阈值，且所述测量点与所述激光三维中心点的距离小于第一距离阈值时，确定所述测量点不属于噪声测量点并用相应所述第一语义物体检测框的ID进行类别赋值。

4.根据权利要求3所述的环境地图参照定位方法，其特征在于，根据所述当前点云数据帧的体素网格与所述第一语义物体检测框，对所述雷达测量帧的测量点进行降噪和类别赋值处理的步骤，还包括：

当所述第一相邻激光点集中的点数小于预设的第一点数阈值，且所述测量点与所述激光三维中心点的距离大于第一距离阈值时，确定所述测量点属于噪声测量点并进行去噪处理。

5.根据权利要求1所述的环境地图参照定位方法，其特征在于，根据所述第一语义物体检测框和处理后的所述测量点的速度信息，提取显著动态物体的步骤，包括：

根据所述第一语义物体检测框的信息，将物体几何宽度信息大于预设的第一宽度阈值的物体提取为第一显著物体；

根据所述毫米波雷达的测量点的径向速度和类别信息，将物体速度大于预设速度阈值的所述第一显著物体提取为所述显著动态物体。

6.根据权利要求1所述的环境地图参照定位方法，其特征在于，将所述当前点云数据帧与预设环境地图进行比较，确定第一环境变动点簇的过程，包括：

根据前一时刻的有效位姿估计值及当前时段内的运动信息，提取所述预设环境地图中的第一预设局部子图；

将所述当前点云数据帧转换为第二当前局部地图后，与所述第一预设局部子图比较确定所述第一环境变动点簇。

7.根据权利要求1所述的环境地图参照定位方法，其特征在于，根据所述第一测量点集和所述第一位姿估计值确定使用所述当前点云数据帧重定位的步骤，包括：

当各所述显著动态物体及所述第一环境变动点簇在激光雷达视场中所占总比例小于预设的第一动态比例阈值时，根据所述第一位姿估计值确定所述当前点云数据帧与所述预设环境地图的吻合程度；

当吻合程度低于预设的第一吻合度阈值时，使用所述当前点云数据帧重定位。

8.根据权利要求7所述的环境地图参照定位方法，其特征在于，所述方法还包括：

当各所述显著动态物体及所述第一环境变动点簇在激光雷达视场中所占总比例大于所述第一动态比例阈值时，确定所述第一测量点集的稀疏程度大于预设的第一稀疏程度阈值；

采用最近时段的位姿增量结合前一时刻的有效位姿估计值进行增量式位姿推算，得到第二位姿估计值并存储所述当前点云数据帧；所述第二位姿估计值及所述当前点云数据帧用于进行离线全局环境地图的更新。

9.一种环境地图参照定位装置，其特征在于，包括：

数据获取模块，用于获取激光雷达的当前点云数据帧和毫米波雷达的雷达测量帧；

语义检测模块，用于根据所述当前点云数据帧进行语义物体检测，确定视场中可移动物体的第一语义物体检测框；

降噪赋值模块，用于根据所述当前点云数据帧的体素网格与所述第一语义物体检测框，对所述雷达测量帧的测量点进行降噪和类别赋值处理；

显著提取模块，用于根据所述第一语义物体检测框和处理后的所述测量点的速度信息，提取显著动态物体；

动态确定模块，用于将所述当前点云数据帧与预设环境地图进行比较，确定第一环境变动点簇与第一测量点集；所述第一环境变动点簇为当前周边环境中相比于预设环境地图中各个点簇已经发生位移且所述位移大于预设位移阈值的激光点簇，所述第一测量点集为不属于所述显著动态物体及所述第一环境变动点簇的测量点集合；

参照位姿模块，用于将所述第一测量点集用于所述预设环境地图的参照定位，获得第一位姿估计值；

识别定位模块，用于根据所述第一测量点集和所述第一位姿估计值确定使用所述当前点云数据帧重定位。

10.一种移动机器人，包括存储器和处理器，所述存储器存储有计算机程序，其特征在于，所述处理器执行所述计算机程序时实现权利要求1至8任一项所述环境地图参照定位方法的步骤。

Images