一种基于几何尺度密度和信息熵的移动机器人地图动态维护方法及系统

1.本发明属于移动机器人地图维护领域，具体涉及一种基于几何尺度密度和信息熵的移动机器人地图动态维护方法及系统。


背景技术：

2.近些年，随着机器人产业的迅速发展，移动机器人的工作环境由原来单一实验测试场景变换到了复杂多变的现实场景。在早期的研究工作中，人们通常只考虑移动机器人在特定时间和场合下的工作性能，必须假定环境特征信息是随着时间不会有较大变化，不会出现结构性、大面积的改变。在这种情况下，经典的定位算法已经具备良好的精确度，可以先建立静态地图，再基于该地图基础上使用激光雷达和里程计等作为传感器进行自主定位。但是现实环境中存在的诸多不确定因素会时刻影响着移动机器人的定位准确性，比如停车场内车辆的变化，工业车间内货物的堆叠变化等，都会引起环境特征的结构性改变，从而导致先验静态地图信息不准确，影响长期定位性能。要使移动机器人能够在复杂动态的现实环境中具有长期高精度、高鲁棒的定位性能，需要移动机器人具备长期适应环境和作业的能力成为现在亟待解决的关键性问题。
3.目前有些学者在传统移动机器人建图及定位技术的基础上进行拓展和延伸，他们将移动机器人建图过程视为一个永久的过程。机器人行运行到新位置时，就会检测当前的环境是否和已建地图环境存在较大的差异，如果存在较大差异，则会将当前环境视为新环境，并将原先地图中的关键帧数据去除掉，换成当前的观测关键帧数据作为环境信息。传统移动机器人建图技术对地图的维护方式是将单一关键帧的直接替换，会引起不必要信息的丢失，影响机器人定位精确性。公开号为113340296的专利《一种基于自建图的静态地图在线更新方法和装置》公开了一种自动更新移动机器人地图的方法和装置，该方法利用在环境中增加反光柱的方式，为移动机器人提供现实场景中准确的锚点，有效避免由于局部环境发生剧烈的变化造成移动机器人定位精度丢失和地图更新效果不佳的情况发生。但这种方法在大型环境下需要事先部署较多的反光柱，只能适用于提前改造好的现实环境，动态适应性较差，另外也会造成人力、物料资源成本的增加。


技术实现要素：

4.针对现有技术存在的不足，本发明提出了一种基于几何尺度密度和信息熵的移动机器人地图动态维护方法及系统，该方法包括：
5.s1：获取系统位姿图；系统位姿图包括若干子图；
6.s2：根据系统位姿图对位姿图中的子图进行裁剪，得到子图裁剪后的位姿图；
7.s3：将子图裁剪后的位姿图中的所有子图组合，得到全局地图。
8.优选的，获取系统位姿图的过程包括：
9.s11：获取传感器数据；
10.s12：根据传感器数据构建初始位姿图；
11.s13：对初始位姿图进行优化，得到系统位姿图。
12.进一步的，传感器数据包括激光雷达的点云数据、imu传感器数据和odometer传感器数据。
13.优选的，对位姿图中的子图进行裁剪的过程包括：
14.s21：计算位姿图中所有子图的质心位置；
15.s22：根据子图的质心位置构建子图的几何密度衰减函数；根据几何密度衰减函数计算子图的几何尺度密度；
16.s23：根据几何尺度密度对子图进行筛选，得到筛选后的子图；
17.s24：计算筛选后的子图的信息增益并根据信息增益对子图进行升序排序；
18.s25：根据筛选后的子图数量计算选择参数s；删除排序中前s个子图，得到子图裁剪后的位姿图。
19.进一步的，子图的几何密度衰减函数为：
[0020][0021]
其中，f(r)表示两个子图质心位置之间欧式距离为r时的几何密度，α表示衰减常值系数，r表示两个子图质心位置之间的欧式距离。
[0022]
进一步的，计算子图的几何尺度密度的公式为：
[0023][0024]
其中，d(ei)表示第i个子图的几何尺度密度，α表示衰减常值系数，ei表示第i个子图的质心位置，ej表示第j个子图的质心位置。
[0025]
进一步的，计算子图的信息增益的公式为：
[0026][0027]
其中，g表示第j个子图的信息增益，d表示机器人位置坐标向量的维度，∑k表示位姿图中所有子图的观测信息矩阵，∑
k,j
表示删除第j个子图及其观测信息后的所有子图的观测信息矩阵。
[0028]
进一步的，计算选择参数的公式为：
[0029]
s＝mod(size(m)
×
γ)
[0030]
其中，s表示选择参数，size(m)表示筛选后的子图信息增益集合的大小，γ表示删除子图数量比率常值。
[0031]
一种基于几何尺度密度和信息熵的移动机器人地图动态维护系统，包括：前端里程计模块、位姿优化模块和地图维护模块；
[0032]
所述前端里程计模块用于获取系统位姿图并生成新的子图；
[0033]
所述位姿优化模块用于对系统位姿图进行优化，得到最优的系统位姿图；
[0034]
所述地图维护模块用于计算系统位姿图中子图的几何尺度密度和信息增益，并根据几何尺度密度和信息增益对子图进行裁剪，组合剩余子图，得到全局地图。
[0035]
本发明的有益效果为：本发明的基于几何尺度密度和信息熵的新型移动机器人地图动态维护方法根据子图的几何尺度密度和信息增益对子图进行裁剪以更新地图进而对地图进行维护；本发明将研究对象从地图中的占用栅格转变到全局地图的局部子图，研究内容从单一关键帧的选取转变到重合叠加子图的删减，并结合位姿观测信息增益指标描述子图对机器人准确定位的影响程度；相对于传统移动机器人地图维护算法，本发明可有效避免由于局部环境发生剧烈的变化造成移动机器人定位精度丢失和地图更新效果不佳的情况发生，定位精度高，同时具有应用大规模场景的能力，能及时适应环境变化。本发明对移动机器人传感器设备需求简单，只需激光雷达传感器、里程计信息即可，无需在运行场景中部署其他感知设备，具有应用成本低廉、部署简单灵活的特性。
附图说明
[0036]
图1为本发明中基于几何尺度密度和信息熵的移动机器人地图动态维护方法流程图；
[0037]
图2为本发明中基于几何尺度密度和信息熵的移动机器人地图动态维护系统结构示意图。
具体实施方式
[0038]
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
[0039]
本发明提出了一种基于几何尺度密度和信息熵的移动机器人地图动态维护方法及系统，所述方法包括以下内容：
[0040]
移动机器人采用多阶段建图的方式，其历史轨迹上会首先建立多个具有交叉重叠的局部子图，接着将这些局部子图进行拼接就可以获得完整的全局地图。首先，本发明通过逐一计算每个局部子图相对于相邻子图的几何尺度密度，获取子图在几何位置分布上对其他所有子图的影响程度，对指标结果比较阈值后筛选出待处理的子图集合。然后，本发明对待处理的子图集合中的每个子图计算对位姿观测信息增益强度，获取每个子图在位姿优化过程中对定位精度的影响程度，选择删除对定位精度影响较小的旧子图，达到控制局部子图数量不能无限增长并动态维护移动机器人地图的目的。如图1所示，具体的步骤如下：
[0041]
s1：获取系统位姿图；系统位姿图包括若干子图；
[0042]
s11：获取传感器数据。
[0043]
实时获取移动机器人在移动过程中的传感器数据，包括激光雷达的点云数据、imu传感器数据和odometer传感器数据。
[0044]
s12：根据传感器数据构建初始位姿图。
[0045]
前端里程计模块中的前端里程计使用ekf滤波(扩展卡尔曼滤波)方法融合imu传感器(惯性传感器)数据和odometer传感器(里程表传感器)数据，得到融合后的位姿；根据2d激光雷达(2d-lidar)的点云数据，采用scan-map配准方式对ekf融合后的位姿进行矫正，得到矫正后的位姿；发布机器人当前位姿的里程计观测信息，同时根据矫正的位姿将点云
数据插入到子图中，当子图累积一定数量的点云数据时会生成新的子图，从而得到初始位姿图。
[0046]
s13：对初始位姿图进行优化，得到系统位姿图。
[0047]
将机器人不同时刻的观测到的位姿抽象为“点”(以下称位姿节点)，机器人在不同位姿上观测所产生的约束被抽象为点之间的“边”(以下称位姿约束，指机器人在不同的位姿节点上对子图相对位姿观测，是一种观测约束)，对其中的观测位姿使用最小二乘法进行优化，得到最优位姿并得到系统位姿图。
[0048]
s2：根据系统位姿图对位姿图中的子图进行裁剪，得到子图裁剪后的位姿图。
[0049]
s21：计算位姿图中所有子图的质心位置。
[0050]
某个子图中每个格子的占用栅格概率表示为pi，则该子图质心表述为：
[0051][0052]
其中，xi和yi表示第i个障碍物栅格在子图坐标系统中的横坐标和纵坐标值，和分别表示该质心在子图坐标系中的横坐标和纵坐标值。将质心位置坐标从子图坐标系转化到世界坐标系有：
[0053][0054][0055]
其中，表示子图质心在世界坐标系下的位置，表示子图质心在子图坐标系下的位置，是子图坐标系转化到世界坐标系的位置变换矩阵；在本发明中，第i个子图的质心e在世界坐标下的位置被表示为ei。
[0056]
s22：根据子图的质心位置构建子图的几何密度衰减函数；根据几何密度衰减函数计算子图的几何尺度密度。
[0057]
根据子图质心在世界坐标系下的位置计算两个子图质心位置之间的欧式距离r；根据两个子图质心位置之间的欧式距离构建子图的几何密度衰减函数：
[0058][0059]
其中，f(r)表示两个子图质心位置之间欧式距离为r时的几何密度，α表示衰减常值系数，通常取值为整数1；r表示两个子图质心位置之间的欧式距离。
[0060]
子图i上以该子图在世界坐标系下的质心位置ei为圆心且半径为r圆形范围内其
他子图的平均几何密度为：
[0061][0062]
离散化可得圆形范围内子图i和子图j之间的几何密度：
[0063][0064]
其中，||e
i-ej||表示子图i和子图j质心之间的欧式几何距离。
[0065]
进一步，可得子图i对所有子图几何密度之和即子图i的几何尺度密度：
[0066][0067]
其中，d(ei)表示第i个子图的几何尺度密度，ei表示第i个子图在世界坐标系下的质心位置，ej表示第j个子图在世界坐标系下的质心位置。
[0068]
s23：根据几何尺度密度对子图进行筛选，得到筛选后的子图。
[0069]
定义符号函数：
[0070][0071]
其中，ε密度常阈值，根据全局地图的大小进行设定，通常取值整数1。
[0072]
对所有的子图[1,m]都进行符号函数的计算，将返回值非零的子图组合成待删除子图集合，完成一次对所有子图按照几何密度度量的筛选：
[0073]
l＝{(i,ei)|signal(ei)≠0,i∈z∩1≤i≤m}
[0074]
s24：计算筛选后的子图的信息增益并根据信息增益对子图进行升序排序。
[0075]
删除子图j上所有的位姿观测信息后对整体观测信息的影响度量，可以使用删除前后信息矩阵行列式对数比表示：
[0076][0077]
其中，g表示第j个子图的信息增益，d表示机器人位置坐标向量的维度，本发明机器人工作在室内环境，位姿图中的观测位姿均位于二维平面坐标系，因此d取值为2，∑k表示位姿图中所有子图的观测信息矩阵，∑
k,j
表示删除第j个子图及其观测信息后的所有子图的观测信息矩阵。
[0078]
机器人在位姿节点xi对于子图mj的观测可以定义为
[0079][0080]
[0081]
其中，和θi分别是机器人在位姿节点xi处的平面坐标位置和方向角，是子图j在位姿图中的平面坐标位置。假定子图特征分布服从均值为零的正态分布，则有所有关联约束位姿对子图mj观测方差：
[0082][0083]
其中，是观测噪声误差，可以将噪声误差矩阵设为单位对角矩阵，因此上式可以化简为：
[0084][0085]
其中，aj是所有位姿节点集合中观测到子图j的约束位姿的总数。子图j在位姿图中的信息矩阵可以表示为：
[0086]
∑j＝σ
j-1
＝aji
[0087]
因此，计算子图j的信息增益的公式为：
[0088][0089]
其中，相比于aj是所有位姿节点集合中观测到子图j的约束位姿的总数，a
k,j
表示删除子图j后位姿图中所有的位姿节点集合观测到所有子图位姿约束总数。
[0090]
计算集合l中每个子图位姿在全局位姿图中的信息增益，得到信息增益的集合m，并按照升序排序得到新的序列a：
[0091][0092]
其中，gi是子图i的信息增益，δ表示为升序排序运算符。
[0093]
s25：根据筛选后的子图数量计算选择参数s；删除排序中前s个子图，得到子图裁剪后的位姿图。
[0094]
计算选择参数的公式为：
[0095]
s＝mod(size(m)
×
γ)
[0096]
其中，s表示选择参数，size(m)表示筛选后的子图信息增益集合的大小即筛选后
的子图数量，γ表示删除子图数量比率常值。
[0097]
删除排序中前s个子图：
[0098]aremain
＝{gi|gi∈a∪i≥s}
[0099]aremain
是从序列a中保留下来的子图序列，其中的子图将被重置到全局位姿图中，得到子图裁剪后的位姿图；γ是删除子图数量比率常值，通常设置为0.1。至此，完成了一轮对旧子图的删除过程。
[0100]
重复上述过程可不断对系统位姿图中的旧子图进行删除。
[0101]
s3：将子图裁剪后的位姿图中的所有子图组合，得到全局地图。
[0102]
系统不断生成新子图，重复子图裁剪过程对系统位姿图进行裁剪，并不断对子图裁剪后的位姿图中的所有子图进行组合，得到全局地图，实现对移动机器人地图的动态维护。
[0103]
本发明还提出一种基于几何尺度密度和信息熵的移动机器人地图动态维护系统，如图2所示，该系统用于执行上述基于几何尺度密度和信息熵的移动机器人地图动态维护方法；包括：前端里程计模块、位姿优化模块和地图维护模块；
[0104]
所述前端里程计模块用于获取系统位姿图并生成新的子图；
[0105]
所述位姿优化模块用于对系统位姿图进行优化，得到最优的系统位姿图；
[0106]
所述地图维护模块用于计算系统位姿图中子图的几何尺度密度和信息增益，并根据几何尺度密度和信息增益对子图进行裁剪，组合剩余子图，得到全局地图。
[0107]
该系统执行基于几何尺度密度和信息熵的移动机器人地图动态维护方法的过程与上述基于几何尺度密度和信息熵的移动机器人地图动态维护方法的实现步骤类似，此处不再赘述。
[0108]
以上所举实施例，对本发明的目的、技术方案和优点进行了进一步的详细说明，所应理解的是，以上所举实施例仅为本发明的优选实施方式而已，并不用以限制本发明，凡在本发明的精神和原则之内对本发明所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。