CN113916232A - 一种改进图优化的地图构建方法及系统 - Google Patents

Abstract

本发明属于移动机器人的同步定位与构建地图(SLAM)领域，提供了一种改进图优化的地图构建方法及系统。该方法包括，获取数据信息，基于所述数据信息建立顶点与顶点间的约束关系，得到第一位姿和第一位姿图；按照观测距离的不同为第一位姿图中的每个单元格添加修正系数，确定挖掘机的当前位置；对所述第一位姿和第一位姿图进行优化，在优化中根据相机观测到的数据建立似然分布，依据所述似然分布增加固定数目的附加位姿，得到优化后的第二位姿和第二位姿图，其中，所述第二位姿为挖掘机的运动轨迹，所述第二位姿图为地图。

Description

一种改进图优化的地图构建方法及系统

技术领域

本发明属于移动机器人的同步定位与构建地图(SLAM)领域，尤其涉及一 种改进图优化的地图构建方法及系统。

背景技术

本部分的陈述仅仅是提供了与本发明相关的背景技术信息，不必然构成在 先技术。

目前，智能化和无人化技术在挖掘机上运用越来越广泛。挖掘机作为一种 重要的工程机械，一般都工作在环境恶劣，工况危险的场合。为了提高挖掘机 工作的自动化水平以及保障驾驶员的安全，国内外学者已经对可远程遥控的自 动挖掘机进行了研究。而为保证挖掘机作业具有良好的效果，对挖掘机精准定 位是面临的重要挑战之一。

同时定位与建图(Simultaneous Localization and Mapping，简称SLAM)，最 早在机器人领域中提出，其SLAM问题可以简单描述为：一个在未知位置和未 知环境中移动的机器人，在移动过程中根据位置估计和传感器数据进行自身定 位，同时构建增量式地图。而图优化融合方法能将所有的历史信息和当前信息 一起构建成一个位姿图，基于位姿图构建优化代价函数，将所有历史信息和当 前信息一起优化，能够充分利用历史信息估计当前状态，经常用于室外复杂环 境。

传统的图优化SLAM方法用前端构图和后端优化构图的方法来提高构建地 图的精度和准确性，以闭环检测的方法实现构图的完整性。但是在矿山这种环 境恶劣、特征复杂的环境中，节点的有效性不能很好的保证，从而造成建立的 地图无法精确提供导航定位。

发明内容

为了解决上述背景技术中存在的技术问题，本发明提供一种改进图优化的 地图构建方法及系统，其根据相机观测到的数据建立似然分布，依据所述似然 分布增加固定数目的随机节点，以获得更准确的节点分布来有效提高定位精度； 在闭环检测步骤中提出了修正系数，提高了先验概率的计算精度。

为了实现上述目的，本发明采用如下技术方案：

本发明的第一个方面提供一种改进图优化的地图构建方法。

一种改进图优化的地图构建方法，包括：

获取数据信息，基于所述数据信息建立顶点与顶点间的约束关系，得到第 一位姿和第一位姿图；

按照观测距离的不同为第一位姿图中的每个单元格添加修正系数，确定挖 掘机的当前位置；

对所述第一位姿和第一位姿图进行优化，在优化中根据相机观测到的数据 建立似然分布，依据所述似然分布增加固定数目的附加位姿，得到优化后的第 二位姿和第二位姿图，其中，所述第二位姿为挖掘机的运动轨迹，所述第二位 姿图为地图。

进一步地，所述按照观测距离的不同为第一位姿图中的每个单元格添加修 正系数，确定挖掘机的当前位置的过程包括：

将第一位姿图分割为n个单元格；

计算挖掘机出现在当前单元格的概率；

按照观测距离的不同为每个单元格添加修正系数，使修正系数与观测距离 的关系符合高斯分布钟型曲线；

采用修正系数修正先验概率，将先验概率最大的位置作为挖掘机当前位置， 并依此判断挖掘机是否实现闭环。

进一步地，所述获取数据信息，基于所述数据信息建立顶点与顶点间的约 束关系的过程包括：将激光传感器采集的数据信息和里程计采集的数据信息进 行帧间匹配，建立顶点与顶点间的约束关系。

进一步地，所述将激光传感器采集的数据信息和里程计采集的数据信息进行 帧间匹配，建立顶点与顶点间的约束关系的过程为：

根据所述里程计数据从t-1时刻的粒子集合Y_t-1采样获取t时刻的粒子集合 Y_t；

计算t时刻的粒子集合Y_t中的每个粒子的权重；

根据设定的阈值对t时刻的粒子集合Y_t中的粒子筛选以生成最终粒子集，并 使最终粒子集的所有粒子具有相同的权重；

根据最终粒子集的每个粒子中包含的运动轨迹信息和观测信息生成地图。

进一步地，所述在优化中根据相机观测到的数据建立似然分布，依据所述 似然分布增加固定数目的附加位姿的过程包括：根据相机观测数据增加随机附 加位姿，使之符合似然分布。

本发明的第二个方面提供一种改进图优化的地图构建系统。

一种改进图优化的地图构建系统，包括：

数据关联模块，其被配置为：获取数据信息，基于所述数据信息建立顶点 与顶点间的约束关系，得到第一位姿和第一位姿图；

闭环检测模块，其被配置为：按照观测距离的不同为第一位姿图中的每个 单元格添加修正系数，确定挖掘机的当前位置；

位姿优化模块，其被配置为：对所述第一位姿和第一位姿图进行优化，在 优化中根据相机观测到的数据建立似然分布，依据所述似然分布增加固定数目 的附加位姿，得到优化后的第二位姿和第二位姿图，其中，所述第二位姿为挖 掘机的运动轨迹，所述第二位姿图为地图。

进一步地，所述采用里程计数据更新地图步骤为：

根据所述里程计数据从t-1时刻的粒子集合Y_t-1采样获取t时刻的粒子集合 Y_t；

计算t时刻的粒子集合Y_t中的每个粒子的权重；

根据设定的阈值对t时刻的粒子集合Y_t中的粒子筛选以生成最终粒子集，并 使最终粒子集的所有粒子具有相同的权重；

根据最终粒子集的每个粒子中包含的运动轨迹信息和观测信息生成地图。

进一步地，所述按照观测距离的不同为第一位姿图中的每个单元格添加修 正系数，确定挖掘机的当前位置的过程包括：

将第一位姿图分割为n个单元格；

计算挖掘机出现在当前单元格的概率；

按照观测距离的不同为每个单元格添加修正系数，使修正系数与观测距离 的关系符合高斯分布钟型曲线；

采用修正系数修正先验概率，将先验概率最大的位置作为挖掘机当前位置， 并依此判断挖掘机是否实现闭环。

进一步地，所述获取数据信息，基于所述数据信息建立顶点与顶点间的约 束关系的过程包括：将激光传感器采集的数据信息和里程计采集的数据信息进 行帧间匹配，建立顶点与顶点间的约束关系。

进一步地，所述在优化中根据相机观测到的数据建立似然分布，依据所述 似然分布增加固定数目的附加位姿的过程包括：根据相机观测数据增加随机附 加位姿，使之符合似然分布。

与现有技术相比，本发明的有益效果是：

本发明采用改进图优化的建图方法，在闭环检测步骤中提出了修正系数， 提高了先验概率的计算精度。

本发明根据相机观测到的数据建立似然分布，依据所述似然分布增加固定 数目的附加位姿，以获得更准确的位姿分布，使得在使用特征数据对挖掘机的 位姿信息进行预测和更新的精度有明显的提高。

在矿山这种环境恶劣、特征复杂的环境中，提高获取位姿的有效性，提高 了构建地图的精度和准确性，从而为挖掘机行走精确提供导航定位。

本发明附加方面的优点将在下面的描述中部分给出，部分将从下面的描述 中变得明显，或通过本发明的实践了解到。

附图说明

构成本发明的一部分的说明书附图用来提供对本发明的进一步理解，本发 明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。

图1是本发明实施例一的建图流程图；

图2是本发明实施例一的闭环检测流程图；

图3是本发明实施例一的改进图优化流程图。

具体实施方式

下面结合附图与实施例对本发明作进一步说明。

应该指出，以下详细说明都是例示性的，旨在对本发明提供进一步的说明。 除非另有指明，本文使用的所有技术和科学术语具有与本发明所属技术领域的 普通技术人员通常理解的相同含义。

需要注意的是，这里所使用的术语仅是为了描述具体实施方式，而非意图 限制根据本发明的示例性实施方式。如在这里所使用的，除非上下文另外明确 指出，否则单数形式也意图包括复数形式，此外，还应当理解的是，当在本说 明书中使用术语“包含”和/或“包括”时，其指明存在特征、步骤、操作、器件、 组件和/或它们的组合。

需要注意的是，附图中的流程图和框图示出了根据本公开的各种实施例的 方法和系统的可能实现的体系架构、功能和操作。应当注意，流程图或框图中 的每个方框可以代表一个模块、程序段、或代码的一部分，所述模块、程序段、 或代码的一部分可以包括一个或多个用于实现各个实施例中所规定的逻辑功能 的可执行指令。也应当注意，在有些作为备选的实现中，方框中所标注的功能 也可以按照不同于附图中所标注的顺序发生。例如，两个接连地表示的方框实 际上可以基本并行地执行，或者它们有时也可以按照相反的顺序执行，这取决 于所涉及的功能。同样应当注意的是，流程图和/或框图中的每个方框、以及流 程图和/或框图中的方框的组合，可以使用执行规定的功能或操作的专用的基于硬件的系统来实现，或者可以使用专用硬件与计算机指令的组合来实现。

实施例一

如图1所示，本实施例提供了一种改进图优化的地图构建方法，包括以下 步骤：

获取数据信息，基于所述数据信息建立顶点与顶点间的约束关系，得到第 一位姿和第一位姿图；

按照观测距离的不同为第一位姿图中的每个单元格添加修正系数，确定挖 掘机的当前位置；

对所述第一位姿和第一位姿图进行优化，在优化中根据相机观测到的数据 建立似然分布，依据所述似然分布增加固定数目的附加位姿，得到优化后的第 二位姿和第二位姿图，其中，所述第二位姿为挖掘机的运动轨迹，所述第二位 姿图为地图。

具体的，本实施例的技术方案可以根据如下方案实现：

参照图1，本实施例提供了一种改进图优化的地图构建方法。包括以下步骤：

步骤S1：特征提取，通过传感器对挖掘机运动过程中的环境进行观测，并 获得激光传感器采集得到的数据信息和里程计采集得到的数据信息；

步骤S2：数据关联，利用激光数据和里程计数据进行帧间匹配，建立顶点 与顶点间的约束关系来估计相关位姿并创建局部子图；

S3：闭环检测，根据激光雷达获取观测距离，按照距离的不同给每个单元 格添加修正系数，提高计算精度，准确确定挖掘机的当前位置；

S4：位姿优化，对前端构建的位姿图(S2创建的局部子图)进行优化，在 位姿优化中根据相机观测到的数据建立似然分布，依据所述似然分布增加固定 数目的随机节点，减小误差，提高定位精度，经优化后的位姿和位姿图即为挖 掘机的运动轨迹和地图。

所述采用里程计数据更新地图具体为：

根据所述里程计数据从t-1时刻的粒子集合Y_t-1采样获取t时刻的粒子集合Y_t；

计算t时刻的粒子集合Y_t中的每个粒子的权重；

根据设定的第一阈值对t时刻的粒子集合Y_t中的粒子筛选以生成第一最终 粒子集，并使第一最终粒子集的所有粒子具有相同的权重；

根据第一最终粒子集的每个粒子中包含的运动轨迹信息和观测信息生成地 图；

参照图3，所述激光闭环检测具体为：

根据一个检测到的地图碎片r以及已有地图集合m_i∈m，计算挖掘机当前处 于位置l的先验概率p(l|r,m)，根据贝叶斯法则：

先验概率

将地图分割为n个单元格，对于每个单元格m_i∈m计算挖掘机出现在当前单 元格的概率p(m_i)，则

由于矿山这种环境恶劣、特征复杂的环境中，距离会对检测到的地图碎片r 以及已有地图m的概率产生影响，因此为了提高先验概率的计算精度，准确确 定挖掘机的当前位置，本实施例引入了修正系数η_i。

修正系数η_i是针对计算特殊环境下的先验概率提出的，根据激光雷达传感器 获取观测距离，按照观测距离的不同给每个单元格添加修正系数，其中修正系 数η_i和距离的关系符合高斯分布钟型曲线。

添加修正系数能明显提高先验概率的计算精度，提高对挖掘机当前位置的 选取的准确性。

由于地图m是以前已经构建好的，p(l,m)可以简化为p(l)，在匹配区域上 是均匀分布的，可以用均匀分布概率来表示；p(r,m)则表示为检测到的地图碎 片r以及已有地图m的概率，则p(r,m)＝1；则先验概率p(l|r,m)可用如下公式 表示：

选取出先验概率最大的位置作为挖掘机当前位置，并依此判断挖掘机是否 实现闭环。

参照图3，所述改进图优化位姿具体为：

本实施例中挖掘机的位姿被表示成图中的节点，观测信息经过处理后转变 为挖掘机之间位姿间的约束关系，并通过连接节点间的边来表示；

图的每个节点取决于挖掘机位姿的描述，所有节点的状态方程为 X＝(a₁,a₂,a₃...a_w)，其中a₁,a₂,a₃...a_w分别为全局坐标系下的挖掘机的位姿，边则描述 了挖掘机位姿之间的变换关系；

节点Q_i和节点Q_j之间观测方程可以表示为：

b_j＝a_iT_i,j

其中，a_i为节点Q_i相应的挖掘机在全局坐标系下的位姿，a_j为节点Q_j相应 的挖掘机在全局坐标系下的位姿，b_j为节点Q_j的观测值，即a_i经变换矩阵变换 后的估计位姿，T_i,j为节点Q_i与节点Q_j之间的变换关系；

所述改进图优化方法在位姿优化中根据观测似然增加固定数目的随机节 点。

本实施例为减小误差，根据观测似然在相应的位置直接放置附加节点Q_k， 提高定位精度，附加节点的状态方程为Y＝(y₁,y₂,y₃...y_m)；

观测位姿a_i附加位姿y_i与经变换矩阵变换后的估计位姿b_j之间存在误差 e(b_j,a_j,y_j)，计算公式为：

e(b_j,a_j,y_j)＝a_iT_i,j-(X_j+Y_j)

其中，X_j，Y_j分别表示a_j，y_j对应的转换矩阵。

带有若干条边的图的误差目标函数为：

其中，信息矩阵Ω_n是协方差矩阵的逆，是一个对称矩阵；它的每个元素Ω_i,j用作误差的系数，对e(b_i,x_i,a_i)、e(b_j,x_j,a_j)的误差相关性进行预计；

假设a_n增加一个增量Δa，x_n增加一个增量Δx，通过多次迭代最终求得满足 目标方程的收敛值，用求得的Δa更新状态变量X，Δx更新状态变量Y，X、Y中 的各个节点的位姿即可构成最终的建图结果。

本实施例采用改进图优化的建图方法，使得在使用特征数据对挖掘机的位 姿信息进行预测和更新的精度有明显的提高。

实施例二

本实施例提供了一种改进图优化的地图构建系统。

一种改进图优化的地图构建系统，包括：

数据关联模块，其被配置为：获取数据信息，基于所述数据信息建立顶点 与顶点间的约束关系，得到第一位姿和第一位姿图；

闭环检测模块，其被配置为：按照观测距离的不同为第一位姿图中的每个 单元格添加修正系数，确定挖掘机的当前位置；

位姿优化模块，其被配置为：对所述第一位姿和第一位姿图进行优化，在 优化中根据相机观测到的数据建立似然分布，依据所述似然分布增加固定数目 的附加位姿，得到优化后的第二位姿和第二位姿图，其中，所述第二位姿为挖 掘机的运动轨迹，所述第二位姿图为地图。

此处需要说明的是，上述数据关联模块、闭环检测模块和位姿优化模块与 实施例一中的步骤所实现的示例和应用场景相同，但不限于上述实施例一所公 开的内容。需要说明的是，上述模块作为系统的一部分可以在诸如一组计算机 可执行指令的计算机系统中执行。

以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领 域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则 之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之 内。

Claims (10)

1.一种改进图优化的地图构建方法，其特征在于，包括：

获取数据信息，基于所述数据信息建立顶点与顶点间的约束关系，得到第一位姿和第一位姿图；

按照观测距离的不同为第一位姿图中的每个单元格添加修正系数，确定挖掘机的当前位置；

对所述第一位姿和第一位姿图进行优化，在优化中根据相机观测到的数据建立似然分布，依据所述似然分布增加固定数目的附加位姿，得到优化后的第二位姿和第二位姿图，其中，所述第二位姿为挖掘机的运动轨迹，所述第二位姿图为地图。

2.根据权利要求1所述的改进图优化的地图构建方法，其特征在于，所述按照观测距离的不同为第一位姿图中的每个单元格添加修正系数，确定挖掘机的当前位置的过程包括：

将第一位姿图分割为n个单元格；

计算挖掘机出现在当前单元格的概率；

按照观测距离的不同为每个单元格添加修正系数，使修正系数与观测距离的关系符合高斯分布钟型曲线；

采用修正系数修正先验概率，将先验概率最大的位置作为挖掘机当前位置，并依此判断挖掘机是否实现闭环。

3.根据权利要求1所述的改进图优化的地图构建方法，其特征在于，所述获取数据信息，基于所述数据信息建立顶点与顶点间的约束关系的过程包括：将激光传感器采集的数据信息和里程计采集的数据信息进行帧间匹配，建立顶点与顶点间的约束关系。

4.根据权利要求3所述的改进图优化的地图构建方法，其特征在于，所述将激光传感器采集的数据信息和里程计采集的数据信息进行帧间匹配，建立顶点与顶点间的约束关系的过程为：

根据所述里程计数据从t-1时刻的粒子集合Y_t-1采样获取t时刻的粒子集合Y_t；

计算t时刻的粒子集合Y_t中的每个粒子的权重；

根据设定的阈值对t时刻的粒子集合Y_t中的粒子筛选以生成最终粒子集，并使最终粒子集的所有粒子具有相同的权重；

根据最终粒子集的每个粒子中包含的运动轨迹信息和观测信息生成地图。

5.根据权利要求1所述的改进图优化的地图构建方法，其特征在于，所述在优化中根据相机观测到的数据建立似然分布，依据所述似然分布增加固定数目的附加位姿的过程包括：根据相机观测数据增加随机附加位姿，使之符合似然分布。

6.一种改进图优化的地图构建系统，其特征在于，包括：

数据关联模块，其被配置为：获取数据信息，基于所述数据信息建立顶点与顶点间的约束关系，得到第一位姿和第一位姿图；

闭环检测模块，其被配置为：按照观测距离的不同为第一位姿图中的每个单元格添加修正系数，确定挖掘机的当前位置；

位姿优化模块，其被配置为：对所述第一位姿和第一位姿图进行优化，在优化中根据相机观测到的数据建立似然分布，依据所述似然分布增加固定数目的附加位姿，得到优化后的第二位姿和第二位姿图，其中，所述第二位姿为挖掘机的运动轨迹，所述第二位姿图为地图。

7.根据权利要求1所述的改进图优化的地图构建系统，其特征在于，所述按照观测距离的不同为第一位姿图中的每个单元格添加修正系数，确定挖掘机的当前位置的过程包括：

将第一位姿图分割为n个单元格；

计算挖掘机出现在当前单元格的概率；

按照观测距离的不同为每个单元格添加修正系数，使修正系数与观测距离的关系符合高斯分布钟型曲线；

采用修正系数修正先验概率，将先验概率最大的位置作为挖掘机当前位置，并依此判断挖掘机是否实现闭环。

8.根据权利要求1所述的改进图优化的地图构建系统，其特征在于，所述获取数据信息，基于所述数据信息建立顶点与顶点间的约束关系的过程包括：将激光传感器采集的数据信息和里程计采集的数据信息进行帧间匹配，建立顶点与顶点间的约束关系。

9.根据权利要求8所述的改进图优化的地图构建系统，其特征在于，所述将激光传感器采集的数据信息和里程计采集的数据信息进行帧间匹配，建立顶点与顶点间的约束关系的过程为：

根据所述里程计数据从t-1时刻的粒子集合Y_t-1采样获取t时刻的粒子集合Y_t；

计算t时刻的粒子集合Y_t中的每个粒子的权重；

根据设定的阈值对t时刻的粒子集合Y_t中的粒子筛选以生成最终粒子集，并使最终粒子集的所有粒子具有相同的权重；

根据最终粒子集的每个粒子中包含的运动轨迹信息和观测信息生成地图。

10.根据权利要求6所述的改进图优化的地图构建系统，其特征在于，所述在优化中根据相机观测到的数据建立似然分布，依据所述似然分布增加固定数目的附加位姿的过程包括：根据相机观测数据增加随机附加位姿，使之符合似然分布。

Images