CN111060135B - 一种基于局部地图的地图修正方法及系统 - Google Patents

Abstract

本发明公开了一种基于局部地图的地图修正方法，包括以下步骤：机器人采集激光数据；利用采集的激光数据在原始Global地图中进行定位导航，包括：计算机器人在原始Global地图的预测位姿和预测位姿的置信度分值；生成局部地图；判断定位是否丢失，若定位没有丢失，则继续定位导航，若定位丢失，则利用生成的局部地图修正Global地图。本发明增强了机器人自动定位导航的稳定性、安全性，大大降低了机器人跌落到道路之外的风险，有助于实现机器人自救并可持续执行任务。

Description

一种基于局部地图的地图修正方法及系统

技术领域

本发明涉及巡检机器人领域，具体涉及一种基于局部地图的地图修正方法及系统。

背景技术

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

针对以上问题，本专利引入一种基于局部地图的地图修正方法及系统，增强了机器人自动定位导航的稳定性、安全性，大大降低了机器人跌落到道路之外的风险，有助于实现机器人自救并可持续执行任务。

发明内容

为了解决上述问题，本发明提出了一种基于局部地图的地图修正方法，包括以下步骤：

机器人采集激光数据；

利用采集的激光数据在原始Global地图中进行定位导航，包括：

计算机器人在原始Global地图的预测位姿和预测位姿的置信度分值；

生成局部地图；

判断根据预测位姿的置信度分值定位是否丢失，若定位没有丢失，则继续定位导航，若定位丢失，则利用生成的局部地图修正Global地图。

所述利用局部地图修正原始Global地图具体包括：

计算机器人在局部地图中的位姿；

根据机器人在原始Global地图中的位姿和机器人在局部地图中的位姿计算局部地图和原始Global地图的匹配关系；

根据匹配关系将局部地图与原始Global地图进行叠加，完成对原始Global地图的修正。

进一步地，所述计算自身的预测位姿和预测位姿的置信度分值具体为：

利用里程计和惯性导航单元记录的前两帧激光数据对应的机器人位姿，计算机器人位移速度和旋转速度，利用所述位移速度、旋转速度和前一帧位姿计算下一帧的位姿初值即预测位姿初值；

以预测位姿初值为中心，基于扫描参数确定各扫描角度，将各扫描角度的位姿作为预测位姿初值的所有的候选位姿；

计算各候选位姿的置信度和置信度分值，选择置信度分值最高的候选位姿作为机器人的预测位姿；

进一步地，所述根据预测位姿的置信度分值判断定位是否丢失具体为：

若预测位姿的置信度分值大于设定的阈值，则表明定位未丢失；

若连续数帧的预测位姿的置信度分值小于设定的阈值，则表明定位丢失。

进一步地，所述生成局部地图具体为：

通过固定长度的滑动窗口以固定步长向前移动，将采集到的激光数据划分成一段段连续的数据帧；

根据接收的相邻的两帧激光数据时机器人的位姿计算机器人相邻位姿间的转换关系；

利用转换关系拼接相邻的两帧激光数据，从而将每一个滑动窗口内的连续数帧激光数据生成一张子图，上一张子图的尾部和下一张子图的头部有重叠部分。

进一步地，所述计算机器人在局部地图中的位姿具体为：

运用计算预测位姿的各候选位姿的置信度分值的方法，求取机器人在局部地图中局部预测位姿的各候选位姿的置信度分值，选取置信度分值最高的K个位姿，对这K个位姿对应的各项求取平均值，得到机器人在局部地图中的位姿。

进一步地，所述根据机器人在原始Global地图中的位姿和机器人在局部地图中的位姿计算局部地图和原始Global地图的匹配关系具体为：

计算机器人在局部地图中的位姿与原始Global地图中位姿p_原(x,y,γ)的位姿差：

其中，x,y为机器人在原始Global地图中的横坐标和纵坐标，γ为机器人在原始Global地图中的姿态；x′,y′为机器人在局部地图中的横坐标和纵坐标，γ′为机器人在局部地图中的姿态；

计算局部地图与原始Global地图的匹配关系R为：

。

进一步地，所述根据匹配关系将局部地图与原始Global地图进行叠加并完成对原始Global地图的修正具体为：

根据匹配关系R将局部地图与原始Global地图进行叠加；

根据局部地图与原始Global地图的叠加情况，更新各个叠加栅格的置信度从而完成对原始Global地图进行修正，具体为：

其中，P(x_i,y_i)_new为修正后的Global地图中地图坐标为(x_i,y_i)的栅格的置信度，(x′_i,y′_i)为局部地图中第i个栅格的地图坐标，P(x′_i,y′_i)为局部地图中地图坐标为(x′_i,y′_i)的第i个栅格的置信度，(x_i,y_i)为原始Global地图中第i个栅格的地图坐标，P(x_i,y_i)为原始Global地图中地图坐标为(x_i,y_i)的第i个栅格的置信度；

其中，odd_hit表示上次和本次都有激光点落入时栅格的状态更新系数，odd_miSS表示激光点上次落入本次未落入时栅格的状态更新系数；s和t的取值根据地面环境的稀疏程度进行调节。

还提出了一种利用上述方法的地图修正系统，包括激光采集模块、地图生成模块、地图修正模块、地图调度模块和地图数据库，激光采集模块采集激光数据，将激光数据传输给地图生成模块生成局部地图，地图生成模块将其生成的局部地图存储至地图数据库；当激光采集模块的定位丢失时，地图调度模块从地图数据库调取最新的局部地图与原始global地图将其传输给地图修正模块，地图修正模块将最新的局部地图与原始global地图进行融合，完成对原始global地图的修正。

进一步地，所述激光采集模块为搭载里程计、惯性导航单元和激光传感器的机器人。

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

1、当机器人的作业环境发生变化时，机器人依旧能够准确的定位导航，增强了机器人对环境的适应性。

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

附图说明

图1为一种基于局部地图的地图修正系统。

图2为一种基于局部地图的地图修正方法。

图3为扫描窗口示意图。

图4为激光点的选取示意图。

具体实施方式

下面结合附图，对一种基于局部地图的地图修正方法及系统进行详细描述。

一种基于局部地图的地图修正系统如图1所示，包括激光采集模块、地图生成模块、地图修正模块、地图调度模块和地图数据库，激光采集模块采集激光数据，将激光数据传输给地图生成模块生成局部地图，地图生成模块将其生成的局部地图存储至地图数据库；当激光采集模块的定位丢失时，地图调度模块从地图数据库调取最新的局部地图与原始global地图将其传输给地图修正模块，地图修正模块将最新的局部地图与原始global地图进行融合，完成对原始global地图的修正。本实施例中激光采集模块为搭载里程计、惯性导航单元和激光传感器的机器人。

其中机器人搭载里程计、惯性导航单元和激光传感器在原始global地图的指引下进行作业任务，激光传感器不断采集激光数据，生成的激光数据在用于定位的同时，通过地图生成模块可生成局部地图，生成的局部地图存储于地图数据库中，当机器人发生定位丢失时，启用地图调度模块，调度最新生成的局部地图与原始global地图，地图修正模块将最新生成的局部地图与原始global地图进行融合，对原始global地图进行修正。

结合图2，对一种基于局部地图的地图修正方法进行详细阐述，具体流程主要分为机器人在原始Global地图中的定位导航、局部地图构建、原始Global地图修正三部分，其中局部地图构建、原始Global地图修正是本发明的主要内容，整体流程如下：

机器人在原始Global地图中进行定位导航：

机器人在执行巡检任务之前，会采集整个作业的激光数据信息并构建激光栅格地图；之后机器人通过里程计和惯性导航单元记录的前两帧激光数据对应的机器人位姿，计算机器人位移速度和旋转速度，并通过前一帧位姿、位移速度和旋转速度预测下一帧的位姿初值；

根据预测位姿初值和定位扫描参数确定各扫描角度，将地图栅格上各扫描角度的位姿作为所有可能的候选位姿；

计算各可能的候选位姿的置信度，选择置信度分值最高的位姿作为机器人的激光定位结果，具体流程如下：

1.估算机器人的预测位姿初值

利用里程计和惯性导航单元获得当前时刻机器人在地图坐标系下的位姿，假定机器人在时间点t的位姿是p(x_t,y_t,γ_t)，x_t表示时间点t时机器人在原始Global地图中的横坐标,y_t表示时间点t时机器人在原始Global地图中的纵坐标，γ_t为时间点t时机器人在原始Global地图中的姿态，前一个计算位姿的时间点为t-1，相应的位姿是p(x_t-1,y_t-1,γ_t-1)，下一个计算位姿的时间点t+1，相应的位下一位姿是p(x_t+1,y_t+1,γ_t+1)。时间点t和t-1之间的时间间隔为Δt，估算机器人的运动速度V(v_x,v_y,v_γ)，v_x,v_y为线速度，v_γ为角速度，估算公式如下：

使用v_x,v_y,v_γ来预测机器人在时间点t+1时刻的位姿p(x_t+1,y_t+1,γ_t+1)即预测位姿初值，由于激光传感器是均匀采样，时间点t和t+1之间的时间间隔为Δt。

由于实际硬件的误差，机器人在时间点t+1的准确位姿与该预测位姿初值之间会有偏差。下面通过激光测量数据和预测位姿初值对应地图数据之间符合的程度来优化p(x_t+1,y_t+1,γ_t+1)，最终获得最接近准确位姿的最优位置。

2.获取不同扫描角度的离散扫描数据

机器人以预测位姿初值为中心，基于扫描参数确定不同的扫描角度，将各扫描角度的位姿作为预测位姿初值的所有的候选位姿；扫描参数包括位移扫描参数和角度扫描参数，如图3所示，位移扫描参数用于限定机器人进行定位扫描时的位移范围，即在地图坐标系中，以预测位姿初值为中心，前后左右各偏离Lcm，形成边长为2Lcm的正方形，所述正方形的边和地图坐标系的一条坐标轴平行或垂直，所述正方形的边即为位移扫描参数。角度扫描参数用于限定机器人进行定位扫描时的角度范围，即在地图坐标系中，以预测位姿初值的预测角度初值γ_t为中心角度，左右各偏离W度的角度。不同的扫描角度下的机器人的位姿构成预测位姿(x_t,y_t,γ_t)的所有的候选位姿。所述构成候选位姿的定位扫描为虚拟定位扫描，是一个实际定位扫描的模拟，而不需要机器人实际的运动。

根据机器人的定位扫描得到的激光数据计算各扫描角度下各激光反射点对应的地图栅格位置(即计算每个地图栅格在地图坐标系中的坐标)，作为各扫描角度的离散扫描数据。对某个扫描角度的离散扫描数据而言，若其中有重复落在同一地图栅格位置的多个激光反射点，仅取其中一个激光反射点对应的地图栅格在地图坐标系中的坐标，如图4所示，灰色栅格为多个激光反射点落入同一地图栅格的情况，只取灰色栅格中的一个激光反射点的坐标，用于后续步骤的置信度的计算。

3.置信度估算

根据每个候选位姿对应的各个地图栅格的置信度(地图栅格的置信度值与地图构建过程相关，在定位过程中，为已经确定的值)，计算每个候选位姿的置信度σ，公式如下：

其中，m为某个候选位姿对应的扫描角度的离散扫描数据中的地图栅格的总数，设其中第n个栅格的地图坐标为(x_n,y_n)，该栅格置信度是

取值范围为[0，1]。

根据每个候选位姿与位姿估计值的位姿差来计算每个候选位姿对应的置信度权重ω，公式如下：

其中，x_Δ是每个候选位姿与位姿估计值之间沿x轴的位移，y_Δ是每个候选位姿与位姿估计值之间沿y轴的位移，ω_xy是位移权重，Δr是候选位姿与预测位姿之间旋转角度，ω_r是旋转角度权重，一般ω_xy和ω_r取1，表示位移和旋转角度所占权重相同。

将每个候选位姿的置信度σ与置信度权重ω的乘积作为当前位姿的置信度分值，公式如下，

score＝σ·ω

选择置信度分值score最高的位姿更新预测位姿初值，作为最终的预测位姿，即作为t+1时刻的位姿(x_t+1,y_t+1,γ_t+1)，预测位姿对应的置信度分值为score_max。

二、生成局部地图

机器人在定位的过程中，通过固定长度的窗口以一定的滑动步长向前移动，可将采集到的激光数据划分成一段段连续的数据帧，针对每段连续的数据帧运用瓦片式的建图方法生成局部地图，具体流程如下：

首先，机器人通过长度为M帧激光数据的总窗口，步长为n帧激光数据的滑动窗口，对采集到的激光数据进行划分，其中M≥400，n∈(10,20)。

进而，根据机器人接收相邻两帧激光数据时的位姿p_k-1、p_k，求解p_k＝p_k-1r_k-1+t_k-1得到机器人相邻位姿间的转换关系(r_k-1，t_k-1)，其中p_k-1表示接收第k-1帧激光数据时机器人的位姿，p_k表示接收第k帧激光数据时机器人的位姿，r_k-1表示转换关系(r_k-1，t_k-1)中的旋转矩阵，t_k-1表示转换关系(r_k-1，t_k-1)中的平移变量。

然后，用求得的转换关系拼接与转换关系相对应的相邻帧的激光数据，具体拼接公式为：Q_k＝Q_k-1r_k-1+t_k-1，其中Q_k-1、Q_k分别为第k-1帧激光数据和第k帧激光数据。

最后，采用瓦片式建图方法即上一张子图的尾部和下一张子图的头部有重叠部分(重叠部分在单张子图中的占比一般为0.3～0.5，占比的数值由地图的稀疏程度确定)，利用滑动窗口内最新的连续数帧激光数据生成子图，子图的形式由栅格地图形式表示，子图地图的分辨率与原始Global地图的分辨率相同。本实施例中针对的是最新的400帧数据，将每20帧数据生成一张子图，即将第1帧到20帧激光数据进行配准生成子图A1，第10帧到30帧激光数据进行配准生成子图A2，第20帧到40帧激光数据进行配准生成子图A3，……，最终将生成39张子图。将生成的子图以中间文件的方式进行保存。当生成一张新的子图后，丢弃中间文件中最先生成的子图，加入新的子图。所有的子图构成局部地图。

三、利用局部地图对原始Global地图进行修正

当机器人的作业环境发生变化时，原始Global地图的置信度将会降低，机器人将会发生定位丢失的情况，此时需要将中间文件中存储的子图与原始Global地图进行融合，具体流程如下：

首先，对机器人的定位置信度进行判断，实时的激光数据与Global_map中机器人的区域概率栅格地图按照步骤一进行实时匹配后，得到匹配的最高置信度分值score_max(score_max∈[0，1])，score_max的值越高，说明新一帧的激光数据定位的越准确。若导航定位过程中实时匹配的最高置信度分值score_max大于人工设定的经验阈值，则定位一直比较准确，机器人可以一直持续定位导航；若连续若干帧(本实例中设定为100帧)激光数据的最高置信度分值score_max低于人工设定的置信度阈值(本实施例中将此置信度阈值设定为0.4)，则判定定位丢失，此时，触发机器人停止移动原地待命的机制，并开始利用局部地图对原始Global地图进行修复。

然后，对局部地图和原始Global地图进行配准，运用步骤一中的方法，求取机器人在局部地图中局部预测位姿的各候选位姿的置信度分值，选取置信度分值最高的K个位姿，p(x′₁，y′₁，γ′₁)，p(x′₂，y′₂，γ′₂)…p(x′_K，y′_K，γ′_K)，K∈(3，6)，对选取的K个位姿对应的各项求取平均值，得到机器人在局部地图中的位姿p(x′，y′，γ′)：

根据机器人当前在原始Global地图中的位姿，求机器人在局部地图中的位姿与原始Global地图中位姿p_原(x,y,γ)的位姿差：

其中，x,y为机器人在Global地图中的横坐标和纵坐标，γ为机器人在Global地图中的姿态；x′,y′为机器人在局部地图中的横坐标和纵坐标，γ′为机器人在局部地图中的姿态；

求取局部地图与原始Global地图的匹配关系R为：

根据匹配关系R将局部地图与原始Global地图进行叠加。

最后，根据局部地图与原始Global地图的叠加情况，更新各个叠加栅格的置信度，对原始Global地图进行修正：

其中，P(x_i,y_i)_new为修正后的Global地图中地图坐标为(x_i,y_i)的栅格的置信度，(x′_i,y′_i)为局部地图中第i个栅格的地图坐标，P(x′_i,y′_i)为局部地图中地图坐标为(x′_i,y′_i)的第i个栅格的置信度，(x_i,y_i)为原始Global地图中第i个栅格的地图坐标，P(x_i,y_i)为原始Global地图中地图坐标为(x_i,y_i)的第i个栅格的置信度；

其中，odd_hit表示上次和本次都有激光点落入时栅格的状态更新系数，odd_miSS表示激光点上次落入本次未落入时栅格的状态更新系数；s和t的取值根据地面环境的稀疏程度进行调节。

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

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

Claims (9)

1.一种基于局部地图的地图修正方法，其特征在于，包括以下步骤：

机器人采集激光数据；

利用采集的激光数据在原始Global地图中进行定位导航，包括：

计算机器人在原始Global地图的预测位姿和预测位姿的置信度分值；

生成局部地图；

根据预测位姿的置信度分值判断定位是否丢失，若定位没有丢失，则继续定位导航，若定位丢失，则利用生成的局部地图修正Global地图；

所述利用局部地图修正原始Global地图具体包括：

计算机器人在局部地图中的位姿；

根据机器人在原始Global地图中的位姿和机器人在局部地图中的位姿计算局部地图和原始Global地图的匹配关系；

根据匹配关系将局部地图与原始Global地图进行叠加，完成对原始Global地图的修正。

2.根据权利要求1所述的基于局部地图的地图修正方法，其特征在于，所述计算自身的预测位姿和预测位姿的置信度分值具体为：

利用里程计和惯性导航单元记录的前两帧激光数据对应的机器人位姿，计算机器人位移速度和旋转速度，利用所述位移速度、旋转速度和前一帧位姿计算下一帧的位姿初值即预测位姿初值；

以预测位姿初值为中心，基于扫描参数确定各扫描角度，将各扫描角度的位姿作为预测位姿初值的所有的候选位姿；

计算各候选位姿的置信度和置信度分值，选择置信度分值最高的候选位姿作为机器人的预测位姿。

3.根据权利要求2所述的基于局部地图的地图修正方法，其特征在于，所述根据预测位姿的置信度分值判断定位是否丢失具体为：

若预测位姿的置信度分值大于设定的阈值，则表明定位未丢失；

若连续数帧的预测位姿的置信度分值小于设定的阈值，则表明定位丢失。

4.根据权利要求3所述的基于局部地图的地图修正方法，其特征在于，所述生成局部地图具体为：

通过固定长度的滑动窗口以固定步长向前移动，将采集到的激光数据划分成一段段连续的数据帧；

根据接收的相邻的两帧激光数据时机器人的位姿计算机器人相邻位姿间的转换关系；

利用转换关系拼接相邻的两帧激光数据，从而将每一个滑动窗口内的连续数帧激光数据生成一张子图，上一张子图的尾部和下一张子图的头部有重叠部分。

5.根据权利要求4所述的基于局部地图的地图修正方法，其特征在于，所述计算机器人在局部地图中的位姿具体为：

运用计算预测位姿的各候选位姿的置信度分值的方法，求取机器人在局部地图中局部预测位姿的各候选位姿的置信度分值，选取置信度分值最高的K个位姿，对这K个位姿对应的各项求取平均值，得到机器人在局部地图中的位姿。

6.根据权利要求5所述的基于局部地图的地图修正方法，其特征在于，所述根据机器人在原始Global地图中的位姿和机器人在局部地图中的位姿计算局部地图和原始Global地图的匹配关系具体为：

计算机器人在局部地图中的位姿p(x′,y′,γ′)与原始Global地图中位姿p_原(x,y,γ)的位姿差：

其中，x,y为机器人在原始Global地图中的横坐标和纵坐标，γ为机器人在原始Global地图中的姿态；x′,y′为机器人在局部地图中的横坐标和纵坐标，γ′为机器人在局部地图中的姿态；

计算局部地图与原始Global地图的匹配关系R为：

。

7.根据权利要求6所述的基于局部地图的地图修正方法，其特征在于，所述根据匹配关系将局部地图与原始Global地图进行叠加并完成对原始Global地图的修正具体为：

根据匹配关系R将局部地图与原始Global地图进行叠加；

根据局部地图与原始Global地图的叠加情况，更新各个叠加栅格的置信度从而完成对原始Global地图进行修正，具体为：

其中，P(x_i,y_i)_new为修正后的Global地图中地图坐标为(x_i,y_i)的栅格的置信度，(x′_i,y′_i)为局部地图中第i个栅格的地图坐标，P(x′_i,y′_i)为局部地图中地图坐标为(x′_i,y′_i)的第i个栅格的置信度，(x_i,y_i)为原始Global地图中第i个栅格的地图坐标，P(x_i,y_i)为原始Global地图中地图坐标为(x_i,y_i)的第i个栅格的置信度；

其中，odd_hi表示上次和本次都有激光点落入时栅格的状态更新系数，odd_miss表示激光点上次落入本次未落入时栅格的状态更新系数；s和t的取值根据地面环境的稀疏程度进行调节。

8.一种利用权利要求1-7任一项所述的基于局部地图的地图修正方法的地图修正系统，其特征在于，包括激光采集模块、地图生成模块、地图修正模块、地图调度模块和地图数据库，激光采集模块采集激光数据，将激光数据传输给地图生成模块生成局部地图，地图生成模块将其生成的局部地图存储至地图数据库；当激光采集模块的定位丢失时，地图调度模块从地图数据库调取最新的局部地图与原始global地图将其传输给地图修正模块，地图修正模块将最新的局部地图与原始global地图进行融合，完成对原始global地图的修正。

9.根据权利要求8所述的地图修正系统，其特征在于，所述激光采集模块为搭载里程计、惯性导航单元和激光传感器的机器人。

Images