[发明专利]机器人环境地图实时更新方法

说明书

本发明公开了机器人环境地图实时更新方法，包括步骤：(1)机器人处于原始栅格地图中，得到其在地图坐标系中的初始位姿，开始地图更新；(2)通过激光传感器扫描得到当前机器人位姿的激光点云，并采用暴力搜索匹配的方法将当前机器人位姿的激光点云与原始栅格地图进行遍历匹配，得到机器人在当前栅格地图下的位姿；其中，机器人当前位姿根据步骤(1)的机器人初始位姿以及机器人的里程计和IMU数据实时更新得到；(3)对当前栅格地图的每个栅格的概率值进行更新，计算得到当前栅格地图中各个栅格的概率值，完成地图更新。本发明可以实现地图复用，提高机器人在电站环境中的定位精度。

技术领域

本发明涉及机器人技术领域，尤其涉及机器人环境地图实时更新方法。

背景技术

针对目前电力巡检机器人在电站已有地图环境运行时，会由于电站实际环境的改变导致机器人在地图中定位精度降低甚至丢失定位，如果对整个电站环境重新建图，又需要很大的人力、时间成本的问题。

发明内容

发明目的：本发明针对上述不足，提出了一种不需要重新建图，仅在原始地图的基础上对新增地图障碍更新的机器人环境地图实时更新方法。

技术方案：

机器人环境地图实时更新方法，包括步骤：

(1)机器人处于原始栅格地图中，得到其在地图坐标系中的初始位姿，开始地图更新；

(2)通过激光传感器扫描得到当前机器人位姿的激光点云，并采用暴力搜索匹配的方法将当前机器人位姿的激光点云与原始栅格地图进行遍历匹配，得到机器人在当前栅格地图下的位姿；其中，机器人当前位姿根据步骤(1)的机器人初始位姿以及机器人的里程计和IMU数据实时更新得到；

(3)对当前栅格地图的每个栅格的概率值进行更新，计算得到当前栅格地图中各个栅格的概率值，完成地图更新。

所述步骤(2)中，栅格地图中每个栅格的概率为：

其中，p(s＝1)表示栅格状态为occupied的概率，只有栅格存在障碍物的情况下，激光传感器才扫描得到该栅格，其中，n为激光传感器扫描到各栅格的次数，n_all为当前地图下总的激光传感器扫描次数。

所述步骤(2)采用暴力搜索匹配的方法将当前机器人位姿的激光点云与原始栅格地图进行遍历匹配得到机器人在当前栅格地图下的位姿具体如下：

(21)根据当前机器人位姿建立机器人坐标系，并据此得到机器人坐标系与地图坐标系之间的变换关系；

(22)定义当前栅格地图下的搜索空间的平移窗口大小和角度搜索窗口大小，并根据角度搜索窗口生成不同角度的序列，以生成的角度序列对当前帧激光点云进行各个角度的旋转变换得到不同角度姿态，并在各角度状态下根据平移窗口对每一角度姿态进行平移操作得到所有变换位姿；

(23)根据步骤(21)得到的机器人坐标系与地图坐标系之间的变换关系计算得到当前帧的激光点云转换到地图坐标系下；

(24)根据步骤(23)计算在地图坐标系下当前帧激光点云的所有变换位姿，将当前帧激光点云的所有变换位姿与当前栅格地图进行匹配，并据此计算各个变换位姿与当前栅格地图匹配的评分；其中，匹配评分为求取当前帧激光点云某一变换位姿投影到当前栅格地图上后的概率值，该概率值为当前帧激光点云某一变换位姿投影到当前栅格地图上后所有点的概率值之和；

(25)根据步骤(24)获取在地图坐标系下激光点云所有变换位姿中与当前栅格地图匹配的评分最高的变换位姿，得到当前帧激光点云的最优匹配位姿，并据此计算得到当前栅格地图下机器人的最优位姿。