CN111077495A - 一种基于三维激光的定位恢复方法 - Google Patents

Abstract

本发明公开了一种基于三维激光的定位恢复方法，包括：步骤1、机器人按照原始Global地图进行定位导航；步骤2、机器人通过激光传感器接收新一帧三维激光点云数据；步骤3、新一帧三维激光点云数据与栅格地图进行实时匹配，计算机器人的位姿、原始Global地图坐标系下的三维激光点云数据和匹配置信度作为匹配结果；步骤4、将实时匹配得到的机器人的位姿信息进行发布；实时定量保存匹配结果；步骤5、对定位导航中的置信度进行判断，若连续帧三维激光点云数据的置信度小于设定的置信度阈值，则判定定位丢失，触发机器人原地待命机制，启动定位恢复操作；否则返回步骤2，即机器人接收新一帧三维激光点云数据，继续进行定位导航。本发明能有效防止机器人因定位丢失而导致损坏，提高了机器人的自主定位导航的可靠性和安全性，增强了机器人对环境变化的适应性。

Description

一种基于三维激光的定位恢复方法

技术领域

本发明涉及巡检机器人领域，具体涉及一种基于三维激光的定位恢复方法。

背景技术

近年来，随着科技的发展，巡检机器人已经成为诸如变电站等特殊工程场所设备巡检的重要辅助，巡检机器人可以利用传感器设备感知周边环境实现对高精度环境地图的构建并根据该地图实现机器人可重复的自主导航。随着技术的不断发展进步，3D激光较2D激光，由于具有点云信息丰富、定位更加稳定的优势，在机器人构建地图与自主定位导航方面得到更加广泛的应用。目前，国内外已有的主流3D激光SLAM(即时定位与建图)技术，已经实现了机器人在理想场景环境中基于激光传感器采集数据构建地图以及根据地图自主定位的功能。然而，上述技术构建的地图是静态的，但实际的环境可能会发生动态的改变，例如环境中随季节变化的植物生长与凋零、人为的场景改造等因素都会造成地图与实际环境存在较大的不吻合。图2是现有主流SLAM自主定位导航的技术流程，依据之前的地图在发生变化的场景区域中行进，机器人定位精度会受到影响，不稳定且不可控，乃至发生定位丢失的情况，造成机器人的损坏或耗费较多人力辅助机器人，费时费力。

发明内容

为了解决上述问题，本发明提出了一种基于三维激光的定位恢复方法，包括以下步骤：步骤1、机器人按照原始Global地图进行定位导航；

步骤2、机器人通过激光传感器接收新一帧三维激光点云数据；

步骤3、新一帧三维激光点云数据与栅格地图进行实时匹配，计算机器人的位姿、原始Global 地图坐标系下的三维激光点云数据和匹配置信度作为匹配结果；

步骤4、将实时匹配得到的机器人的位姿信息进行发布；实时定量保存匹配结果；

步骤5、对定位导航中的置信度进行判断，若连续帧三维激光点云数据的置信度小于设定的置信度阈值，则判定定位丢失，触发机器人原地待命机制，启动定位恢复操作；否则返回步骤2，即机器人接收新一帧三维激光点云数据，继续进行定位导航。

进一步地，所述步骤5中的定位恢复操作具体包括：

将匹配结果中的三维激光点云数据进行Ceres优化匹配，得到Ceres优化匹配结果；

将Ceres优化匹配结果保存并形成局部子图，生成具有位置信息的局部子图文件、局部子图描述文件和导航点文件；

终止当前定位导航过程，以局部子图文件作为导航指引地图进行定位导航，机器人回退至可信区域；

机器人终止以局部子图文件作为导航指引地图进行的定位导航过程，再次以原始Global 地图作为导航指引地图继续进行定位导航。

进一步地，所述步骤4中的实时定量保存匹配结果具体包括：

建立一个缓存池，用于数量可配置地保存匹配结果，以待定位丢失后启用。

进一步地，所述Ceres优化匹配具体为：

保存下来的若干帧的点云及其相关数据，通过原始Global地图坐标系和局部坐标系及两个坐标系之间的转换关系在保存下来的三维激光点云数据之间建立位姿残差模型，运用最小二乘的方法，对每一帧三维激光点云数据的位姿进行优化处理，得到准确的点云匹配结果。

进一步地，所述置信度在0到1之间，置信度阈值为0.4。

进一步地，所述实时定量保存的原始Global地图坐标系下的三维激光点云数据为连续300 帧原始Global地图坐标系下的三维激光点云数据；步骤5中所述的连续帧三维激光点云数据为连续100帧三维激光点云数据。

与现有技术相比，本发明具有以下有益效果：

本发明提出的基于三维激光的定位恢复方法能够在机器人自主导航过程中，在定位丢失时，机器人会停止运动，并生成准确实时的局部子图，并依据局部子图自动恢复到可信区域，实现定位恢复，从而有效防止机器人因定位丢失而导致损坏，提高了机器人的自主定位导航的可靠性和安全性，增强了机器人对环境变化的适应性。

附图说明

图1为本发明的流程图。

图2为现有主流SLAM定位导航的技术流程图。

图3为机器人仿真激光导航定位示意图。

图4为机器人激光导航定位丢失后生成的局部子图示意图。

具体实施方式

下面结合附图，对本发明提出的基于三维激光的定位恢复方法进行详细描述，具体实施步骤如图1所示：

步骤1、机器人按照原始Global地图进行定位导航：图3为原始Global地图的示意图，是前期机器人对整个作业区域采集数据并构建的概率栅格地图。其中空心圆圈表示机器人，箭头表示机器人的前进方向，小的虚线框表示现实环境发生较大变化的区域。正常情况下，机器人将该原始Global地图作为指引地图，用实时的三维激光点云数据与原始Global 地图进行匹配，实现机器人定位导航。

步骤2、机器人通过激光传感器接收新一帧三维激光点云数据：机器人在行进过程中，通过激光传感器不断获取实时的三维激光点云数据，并采用逐帧或跳帧的方式对三维激光点云数据进行处理，然后将处理过的三维激光点云数据加入到定位导航系统中。

步骤3、实时匹配：新一帧点云数据通过基于多传感器(里程计、IMU)融合的三维激光点击中概率栅格地图获取最高置信度的方法与原始Global地图中的概率栅格地图进行匹配，得到当前帧点云数据对应的机器人的位姿和原始Global地图坐标系下的三维激光点云数据和匹配置信度。

步骤4.匹配后位姿信息的发布与反馈：对实时匹配得到机器人的位姿信息进行发布，作为后续用UKF(Unscented Kalman Filter，无迹卡尔曼滤波)方法进行预测定位的位置先验值。

实时定量保存匹配结果：建立一个缓存池，用于数量可配置地保存实时匹配后的每一帧三维点云的位姿信息和全局坐标系下的点云数据以及匹配置信度结果数据，以待定位丢失后启用。例如，本实施例中保存的数据数量是连续300帧三维激光点云数据，当连续100帧三维激光点云数据匹配置信度小于阈值时，则判定定位丢失，即在连续300帧三维激光点云数据中还有连续200帧点云数据是定位可信的。

步骤5.定位置信度判断：实时匹配后会得到匹配的置信度(在0和1之间)，置信度越高，说明新一帧的三维激光点云数据定位的越准确。若导航定位过程中实时匹配的置信度一直大于人工设定的经验阈值，则定位一直比较准确，机器人返回步骤2持续进行定位导航；若连续若干帧(本实施例为100帧)三维激光点云数据的置信度低于人工设定的置信度阈值(本实施例将其设定为0.4)，则判定定位丢失，则会触发机器人原地待命机制，如图4所示，在A点机器人判定已经定位丢失，然后停在A点处，并开始进行自动定位恢复的计算。

步骤6.Ceres优化匹配：当机器人在步骤5中被判定为定位丢失后，就会启用保存在缓存池中的三维激光点云数据，进行Ceres(Ceres solver是谷歌开发的一款用于非线性优化的库)优化匹配，即通过原始Global地图坐标系和局部坐标系及两个坐标系之间的转换关系在保存下来的三维激光点云数据之间建立位姿残差模型，运用最小二乘的方法，对每一帧三维激光点云数据的位姿进行优化处理，得到Ceres优化匹配结果。

步骤7.最新局部子图的生成与转换：将步骤6中Ceres优化匹配后的统一坐标系的点云和位姿数据先存储并生成一个实时的局部子图的中间文件中，然后将该中间文件进行转换具有位置信息的局部子图文件、局部子图描述文件和导航点文件。

步骤8.最新局部子图的复用：当步骤7中的局部子图文件生成后将终止当前定位导航过程，并以刚生成的局部子图文件作为导航指引地图进行定位导航。

步骤9.可信区域定位的恢复：如图4所示，机器人在最新局部子图的指引下，将局部子图的终点A作为机器人导航回退的起点，读取导航点文件中的点坐标(图4中空心圆)作为指引，逐步回退到可信区域(局部子图起点B)。

步骤10.原始Global地图的重新启用：当机器人恢复到可信区域后，终止以局部子图作为导航指引地图进行的定位导航过程，再以原始Global地图作为导航指引地图继续进行定位导航，从而完成自动定位恢复执行后续任务。

本发明增强了机器人自动定位导航的稳定性、安全性，大大降低了机器人跌落到道路之外的风险，有助于实现机器人自救并可持续执行任务。

以上所述仅为本发明的较佳实施例，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (6)

1.一种基于三维激光的定位恢复方法，其特征在于，包括以下步骤：

步骤1、机器人按照原始Global地图进行定位导航；

步骤2、机器人通过激光传感器接收新一帧三维激光点云数据；

步骤3、新一帧三维激光点云数据与栅格地图进行实时匹配，计算机器人的位姿、原始Global地图坐标系下的三维激光点云数据和匹配置信度作为匹配结果；

步骤4、将实时匹配得到的机器人的位姿信息进行发布；实时定量保存匹配结果；

步骤5、对定位导航中的置信度进行判断，若连续帧三维激光点云数据的置信度小于设定的置信度阈值，则判定定位丢失，触发机器人原地待命机制，启动定位恢复操作；否则返回步骤2，即机器人接收新一帧三维激光点云数据，继续进行定位导航。

2.根据权利要求1所述的基于三维激光的定位恢复方法，其特征在于，所述步骤5中的定位恢复操作具体包括：

将匹配结果中的三维激光点云数据进行Ceres优化匹配，得到Ceres优化匹配结果；

将Ceres优化匹配结果保存并形成局部子图，生成具有位置信息的局部子图文件、局部子图描述文件和导航点文件；

终止当前定位导航过程，以局部子图文件作为导航指引地图进行定位导航，机器人回退至可信区域；

机器人终止以局部子图文件作为导航指引地图进行的定位导航过程，再次以原始Global地图作为导航指引地图继续进行定位导航。

3.根据权利要求2所述的基于三维激光的定位恢复方法，其特征在于，所述步骤4中的实时定量保存匹配结果具体包括：

建立一个缓存池，用于数量可配置地保存匹配结果，以待定位丢失后启用。

4.根据权利要求3所述的基于三维激光的定位恢复方法，其特征在于，所述Ceres优化匹配具体为：

保存下来的若干帧的点云及其相关数据，通过原始Global地图坐标系和局部坐标系及两个坐标系之间的转换关系在保存下来的三维激光点云数据之间建立位姿残差模型，运用最小二乘的方法，对每一帧三维激光点云数据的位姿进行优化处理，得到准确的点云匹配结果。

5.根据权利要求4所述的基于三维激光的定位恢复方法，其特征在于，所述置信度在0到1之间，置信度阈值为0.4。

6.根据权利要求5所述的基于三维激光的定位恢复方法，其特征在于，所述实时定量保存的原始Global地图坐标系下的三维激光点云数据为连续300帧原始Global地图坐标系下的三维激光点云数据；步骤5中所述的连续帧三维激光点云数据为连续100帧三维激光点云数据。

Images