[发明专利]一种基于图优化的激光雷达路标地图构建方法

说明书

本发明适用于自动控制技术领域，提供了一种基于图优化的激光雷达路标地图构建方法，包括如下步骤：S1、检测当前帧中是否存在未知反光板，若检测结果为否，则直接执行步骤S2，若检测结果为是，在计算未知反光板在临时路标地图中的坐标，再执行步骤S2；S2、构建节点Node_cur，节点中包括当前帧中所有反光板的ID、及反光板在激光雷达坐标系中的局部坐标；S3、计算从当前节点Node_cur到关联节点Node_i的位姿变换；S4、检测所有反光板的ID是否均位于位姿地图中，若检测结果为是，则执行步骤S5，若检测结果为否，则执行步骤S1；S5、将构建节点时建立的临时路标地图转换为路标地图。基于位姿图来进行路标地图的重构，解决了由于误差累积传播导致的建图精度下降的问题。

技术领域

本发明属于自动控制技术领域，提供了一种基于图优化的激光雷达路标地图构建方法。

背景技术

随着社会的发展和技术的进步，移动机器人越来越深的介入到人类的日常生活中，例如家庭中的清洁机器人、工厂中的搬运机器人以及餐馆中的送餐机器人等。移动机器人想要实现上述功能必须准确的知道自身所在的位置，即实时定位。而移动机器人实现实时定位的先决条件是建立地图，这是机器人导航和其它智能行为的关键。移动机器人定位导航常用的传感器有磁导航、摄像机和激光雷达等。其中磁导航的导航路径单一，不利于扩展和更改，使用的局限性较大。基于视觉的定位导航方法受光源条件影响大，稳定性和精度较差，无法让移动机器人进行稳定、精密的操作。

基于激光雷达的定位方法可以分为基于反光板的定位导航方法和基于轮廓的定位导航方法，其中基于轮廓的定位导航方法计算量大、精度较差，无法适应移动机器人精密操作的要求；基于反光板的激光雷达定位导航方法精度高、稳定性好。激光雷达探测环境中的反光板建立路标地图，该技术方案采用三边法计算激光雷达的位姿，再利用该位姿和反光板在激光雷达坐标系中的坐标计算未知反光板在地图中的坐标。整个建图过程以接力的形式进行，逐步将所有的反光板纳入到地图之中。该技术方案存在以下如下问题：在地图构建的方式上，该方案采用接力的形式，直接利用已知反光板计算未知反光板在地图中的位置，建图过程中的误差会累积向后传播，在反光板数目较多时地图构建结果不准确。

发明内容

本发明实施例提供一种基于图优化的激光雷达路标地图构建方法，旨在解决接力式构图过程中误差会累积向后传播，在反光板数目较多时地图构建存在结果不准确的问题。

为了实现上述目的，本发明提供了一种基于图优化的激光雷达路标地图构建方法，该方法包括如下步骤：

S1、检测当前帧中是否存在未知反光板，若检测结果为否，则直接执行步骤S2，若检测结果为是，在计算未知反光板在临时路标地图中的坐标，再执行步骤S2；

S2、构建节点Node_cur，节点中包括当前帧中所有反光板的ID、及反光板在激光雷达坐标系中的局部坐标；

S3、计算从当前节点Node_cur到关联节点Node_i的位姿变换；

S4、检测所有反光板的ID是否均位于位姿地图中，若检测结果为是，则执行步骤S5，若检测结果为否，则执行步骤S1；

S5、将构建节点时建立的临时路标地图转换为路标地图。

进一步的，若节点Node_i与当前节点Node_cur中ID相同的路标数目大于3，且存在ID不同的路标点，则节点Node_i即为当前节点Node_cur的关联节点。

进一步的，在步骤S4之后，在步骤S5之前还包括：

S6、对位姿地图进行优化。

进一步的，未知反光板的提取方法具体如下：