CN113375683A - 机器人环境地图实时更新方法 - Google Patents

Abstract

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

Description

机器人环境地图实时更新方法

技术领域

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

背景技术

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

发明内容

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

技术方案：

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

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

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

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

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

其中，p(s＝1)表示栅格状态为occupied的概率，只有栅格存在障碍物的情况下，激光传感器才扫描得到该栅格，

其中，n为激光传感器扫描到各栅格的次数，n_all为当前地图下总的激光传感器扫描次数。

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

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

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

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

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

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

所述步骤(22)中，平移窗口x方向上的长为2w_x，y方向上的长度为2w_y，角度搜索窗口大小为2w_θ；搜索窗口定义为：

W＝{ξ₀+(rj_x,rj_y,δ_θj_θ)}

其中，ξ₀表示当前机器人位姿；r表示地图分辨率，

j_x、j_y分别表示x、y方向上的单位搜索距离，j_θ表示单位角度搜索大小；

表示搜索范围，

δ_θ表示角度分辨率。

所述步骤(23)具体为：

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

针对激光传感器扫描得到的激光点云为p，当前帧的激光点云数据表示为：

H＝{h_k}_k＝1,...,K,h_k∈R²

其中，h_k表示当前帧的激光点云数据，K表示激光传感器扫描得到的一帧激光点云中点的个数；

将当前帧激光点云转换到地图坐标系下即为：

其中，

为当前帧激光点云转换到地图坐标系下的旋转矩阵，

为当前帧激光点云转换到地图坐标系下的平移矩阵，ξ_θ表示当前机器人的偏航角，ξ_x、ξ_y分别表示当前机器人在地图坐标系下在x方向、y方向的平移距离。

步骤(25)中，根据步骤(24)获取在地图坐标系下激光点云所有变换位姿中与当前栅格地图进行匹配的最高评分：

其中，

表示激光点云的某一变换位姿与当前栅格地图匹配的评分，

表示当前机器人位姿与地图坐标系之间的变换关系，

表示扫描得到的最远激光点云距离；

进而得到当前帧激光点云的最优匹配状态作为最优值；

将得到的最优值作为优化初值通过Ceres求解器求解，得到当前机器人的最优位置为：

式中，F为将栅格地图中各点概率值进行插值处理后的平滑函数。

每次激光传感器扫描匹配完成后对地图进行更新，即是对概率栅格地图的每个栅格的概率值进行更新：

其中，odds(s)′表示更新后栅格的概率值，p(s＝0)＝1-p(s＝1)；

free的更新即将p(s＝1)替换为1-p(s＝1)。

设置栅格地图概率表，该概率表中将各个栅格进行编号，并在对概率栅格地图的每个栅格的概率值进行更新对相应编号所对应的栅格概率值进行更新。

根据栅格地图中各栅格的概率大小将所有地图栅格的栅格概率值转换为对应的0～255的灰度值，得到栅格地图的灰度图片。

有益效果：本发明设置机器人在当前地图环境中的定位，控制机器人对实际环境中新增障碍进行激光扫描，根据扫描得到的激光数据更新当前地图，然后保存更新后的地图，最后使用更新后的地图覆盖原始地图，实现地图复用，提高机器人在电站环境中的定位精度。

附图说明

图1为本发明的框架流程图。

图2为本发明的机器人实时定位流程图。

图3为本发明的地图更新流程图。

具体实施方式

下面结合附图和具体实施例，进一步阐明本发明。

图1是本发明的整体框架流程图，如图1所示，在网页点击开始更新地图后，设置机器人在当前地图环境中的定位，控制机器人对实际环境中新增障碍进行激光扫描，根据扫描得到的激光数据更新当前地图，然后保存更新后的地图，最后使用更新后的地图覆盖原始地图，实现地图复用，提高机器人在电站环境中的定位精度。

机器人所建立和使用的地图为概率栅格地图，概率栅格地图由栅格(pixel)组成，其状态可以表示为occupied(栅格存在障碍物)和free(栅格不存在障碍物)两种，并分别用s＝1和s＝0表示；于是每个栅格的概率即为：

其中，p(s＝1)表示栅格状态为occupied的概率，只有栅格存在障碍物的情况下，激光传感器才扫描得到该栅格，

其中，n为激光传感器扫描到各栅格的次数，n_all为当前地图下总的激光传感器扫描次数；

具体步骤如下：

(1)机器人处于原始栅格地图中，得到其在地图坐标系中的初始位姿，地图坐标系的原点为0∈R²，其中R²表示地图坐标系所在平面；在机器人控制页面点击开始地图更新；

(2)机器人通过激光传感器扫描得到当前帧激光点云，激光点云中的点即为栅格地图中的栅格；机器人在运动过程中，根据步骤(1)的机器人初始位姿以及机器人的里程计和IMU数据实时更新得到在地图坐标系下机器人的当前位姿；

(3)机器人实时定位，图2是本发明的机器人实时定位流程图，如图2所示，本发明的机器人实时定位方法采用暴力搜索匹配的方法使用当前激光帧点云在当前栅格地图中进行遍历匹配，从而获得最佳匹配位置，并据此进行实时定位得到机器人在当前栅格地图下的最优位置；

具体如下：

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

(32)定义当前栅格地图下的搜索空间的平移窗口大小和角度搜索窗口大小，其中，平移窗口x方向上的长度为2w_x，y方向上的长度为2w_y，角度搜索窗口大小为2w_θ；搜索窗口定义为：

W＝{ξ₀+(rj_x,rj_y,δ_θj_θ)}

其中，ξ₀表示当前机器人位姿；r表示地图分辨率，

j_x、j_y分别表示x、y方向上的单位搜索距离，j_θ表示单位角度搜索大小；

表示搜索范围，

δ_θ表示角度分辨率；

(33)在搜索范围内根据角度搜索窗口生成不同角度的序列，以生成的角度序列对当前帧激光点云进行各个角度的旋转变换得到不同角度姿态，并在各角度姿态下在搜索范围内根据平移窗口对每一角度姿态进行平移操作得到一系列位姿；进而得到当前帧激光点云在x，y，θ三种窗口中的所有变换位姿；

(34)根据步骤(31)得到的机器人坐标系与地图坐标系之间的变换关系计算得到当前帧的激光点云在地图坐标系下的位姿为T_ξ；

针对激光传感器扫描得到的激光点云为p，当前帧的激光点云数据表示为：

H＝{h_k}_k＝1,...,K,h_k∈R²

其中，h_k表示当前帧的激光点云数据，K表示激光传感器扫描得到的一帧激光点云中点的个数；

将当前帧激光点云转换到地图坐标系下即为：

其中，

为当前帧激光点云转换到地图坐标系下的旋转矩阵，

为当前帧激光点云转换到地图坐标系下的平移矩阵，ξ_θ表示当前机器人的偏航角，ξ_x、ξ_y分别表示当前机器人在地图坐标系下在x方向、y方向的平移距离；

(35)根据步骤(34)计算在地图坐标系下当前帧激光点云在x，y，θ三种窗口中的所有变换位姿，将当前帧激光点云的所有变换位姿与当前栅格地图进行匹配，并据此计算各个变换位姿与当前栅格地图匹配的评分；其中，匹配评分为求取激光点云的某一变换位姿投影到当前栅格地图上后的概率值，该概率值为激光点云某一变换位姿投影到当前栅格地图上后所有点的概率值之和，该概率值越大表示重合度越高；

其中，激光点云某一变换位姿投影到当前栅格地图上后某一点的概率值通过式(1)计算得到；

(36)根据步骤(35)获取在地图坐标系下当前帧激光点云所有变换位姿中与当前栅格地图匹配的最高评分：

其中，

表示当前帧激光点云的某一变换位姿与当前栅格地图匹配的评分，

表示当前帧激光点云某一变换位姿投影到当前栅格地图上后第k个点的概率值，

表示当前机器人位姿与地图坐标系之间的变换关系，

表示扫描得到的最远激光点云距离；

进而得到当前帧激光点云的最优匹配状态作为最优值；

(37)通过前述遍历搜索得到的最优值作为优化初值再通过Ceres求解器求解，将机器人的位置求解转化为一个最小二乘优化问题，于是，当前机器人的最优位置为：

式中，F为将栅格地图中各点概率值进行插值处理后的平滑函数；

本发明将激光点云h_k从激光传感器坐标系转换到地图坐标系下，其中优化主要针对通过激光传感器实时获取的激光点云在栅格地图中的匹配度、机器人平移位姿的优化以及机器人角度位姿的优化三个方面，最终得到当前栅格地图下机器人的最优位姿。

(4)机器人实时更新栅格地图；

图3为本发明的地图更新流程图，如图3所示，每次激光传感器扫描匹配完成后对地图进行更新，即是对概率栅格地图的每个栅格的概率值进行更新：

其中，odds(s)′表示更新后栅格的概率值，p(s＝0)＝1-p(s＝1)；

free的更新即将p(s＝1)替换为1-p(s＝1)；

在机器人扫描新增地图环境时，每一帧激光点云数据与当前栅格地图经过步骤(3)的匹配后，将得到的当前栅格地图各个栅格的概率值新增到当前概率栅格地图中。地图的更新是在当前栅格地图的栅格概率的基础上进行，将更新完成后的栅格地图的栅格概率存储到相应的概率表内，已经更新完成的栅格便不再计算，通过快速查找得到各栅格概率值。当激光扫描得到栅格状态为存在障碍物时，则更新状态为occupied的概率表格；相反的，当激光扫描到的栅格状态为空，即没有障碍物时，则更新状态为free的概率表格。地图数据的更新本质是对概率的更新，概率表的使用便于快速查找栅格概率，避免了重复计算。最后更新得到的概率值即得到概率栅格地图。使用机器人扫描完新增地图环境后，点击完成地图更新，即根据栅格概率大小将所有地图栅格概率值转换为对应的0～255灰度值，得到地图灰度图片，其中灰度值越高的地方代表地图栅格occupied概率越大，即为障碍物。最终得到更新后的机器人环境地图，将其覆盖原始地图即完成了移动机器人环境在线实时更新与保存。

(5)机器人保存更新后的栅格地图，并重新载入更新后的地图。

以上详细描述了本发明的优选实施方式，但是本发明并不限于上述实施方式中的具体细节，在本发明的技术构思范围内，可以对本发明的技术方案进行多种等同变换(如数量、形状、位置等)，这些等同变换均属于本发明的保护范围。

Claims (9)

1.机器人环境地图实时更新方法，其特征在于：包括步骤：

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

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

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

2.根据权利要求1所述的机器人环境地图实时更新方法，其特征在于：所述步骤(2)中，栅格地图中每个栅格的概率为：

其中，p(s＝1)表示栅格状态为occupied的概率，只有栅格存在障碍物的情况下，激光传感器才扫描得到该栅格，

其中，n为激光传感器扫描到各栅格的次数，n_all为当前地图下总的激光传感器扫描次数。

3.根据权利要求1所述的机器人环境地图实时更新方法，其特征在于：所述步骤(2)采用暴力搜索匹配的方法将当前机器人位姿的激光点云与原始栅格地图进行遍历匹配得到机器人在当前栅格地图下的位姿具体如下：

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

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

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

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

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

4.根据权利要求3所述的机器人环境地图实时更新方法，其特征在于：所述步骤(22)中，平移窗口x方向上的长为2w_x，y方向上的长度为2w_y，角度搜索窗口大小为2w_θ；搜索窗口定义为：

W＝{ξ₀+(rj_x，rj_y，δ_θj_θ)}

其中，ξ₀表示当前机器人位姿；r表示地图分辨率，

j_xj_y分别表示x、y方向上的单位搜索距离，j_θ表示单位角度搜索大小；

表示搜索范围，

δ_θ表示角度分辨率。

5.根据权利要求3所述的机器人环境地图实时更新方法，其特征在于：所述步骤(23)具体为：

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

针对激光传感器扫描得到的激光点云为p，当前帧的激光点云数据表示为：

H＝{h_k}_{k＝1，...，K}，h_k∈R²

其中，h_k表示当前帧的激光点云数据，K表示激光传感器扫描得到的一帧激光点云中点的个数；

将当前帧激光点云转换到地图坐标系下即为：

其中，

为当前帧激光点云转换到地图坐标系下的旋转矩阵，

为当前帧激光点云转换到地图坐标系下的平移矩阵，ξ_θ表示当前机器人的偏航角，ξ_x、ξ_y分别表示当前机器人在地图坐标系下在x方向、y方向的平移距离。

6.根据权利要求5所述的机器人环境地图实时更新方法，其特征在于：步骤(25)中，根据步骤(24)获取在地图坐标系下激光点云所有变换位姿中与当前栅格地图进行匹配的最高评分：

其中，

表示激光点云的某一变换位姿与当前栅格地图匹配的评分，

表示当前机器人位姿与地图坐标系之间的变换关系，

表示扫描得到的最远激光点云距离；

进而得到当前帧激光点云的最优匹配状态作为最优值；

将得到的最优值作为优化初值通过Ceres求解器求解，得到当前机器人的最优位置为：

式中，F为将栅格地图中各点概率值进行插值处理后的平滑函数。

7.根据权利要求2所述的机器人环境地图实时更新方法，其特征在于：每次激光传感器扫描匹配完成后对地图进行更新，即是对概率栅格地图的每个栅格的概率值进行更新：

其中，odds(s)′表示更新后栅格的概率值，p(s＝0)＝1-p(s＝1)；

free的更新即将p(s＝1)替换为1-p(s＝1)。

8.根据权利要求7所述的机器人环境地图实时更新方法，其特征在于：设置栅格地图概率表，该概率表中将各个栅格进行编号，并在对概率栅格地图的每个栅格的概率值进行更新对相应编号所对应的栅格概率值进行更新。

9.根据权利要求7所述的机器人环境地图实时更新方法，其特征在于：根据栅格地图中各栅格的概率大小将所有地图栅格的栅格概率值转换为对应的0～255的灰度值，得到栅格地图的灰度图片。

Images