CN106651821A - 一种基于二阶矩保持传播算法的拓扑地图融合方法及系统 - Google Patents

Abstract

本发明提供一种基于二阶矩保持传播算法的拓扑地图融合方法及系统。所述方法包括：S1，基于概率性广义Voronoi图方法对不同地图进行融合；S2，通过二阶矩保持传播方法消除地图融合的非线性不确定度。本发明首先在保留地图显著性信息的情况下，对地图进行骨架的提取，这样可以简化地图中的信息，使运算更加简便；考虑到了使用PGVD时占用栅格地图中的不确定度，利用线性化来解决转换过程中的不确定度问题。比起其他算法，它是快速且鲁棒的，能够对相似度更大的区域进行优先匹配。

Description

一种基于二阶矩保持传播算法的拓扑地图融合方法及系统

技术领域

本发明涉及拓扑地图融合领域，更具体地，涉及一种基于二阶矩保持传播算法的拓扑地图融合方法及系统。

背景技术

目前，SLAM(simultaneous localization and mapping，即时定位与地图构建，或并发建图与定位)是探索未知环境并融合传感器数据，并生成一个全局地图的过程，可以检测机器人的位姿并建立环境地图。

移动机器人能够在环境中进行自主探索和自身定位，针对单机器人的SLAM来说已经有大量的文献，但是多机器人对未知环境的探索却没有受到同样的关注，它相对单机器人有以下几项显著优点：探索和地图构建都可以进行得更迅速；构建的分布式系统的鲁棒性更强；由于数据的冗余，结果往往更准确。

多机器人SLAM在带来诸多好处的同时，也在实现当中带来了新的挑战，其中有两个主要的问题需要克服：首先是在不知道全局地图基础坐标时的地图融合，其次是在地图融合过程中产生的不确定性。

对于高水平的推演和信息共享来说，首先需要将地图抽象成几何结构是基础。当使用多个机器人对未知环境进行探索并合作建立地图时，需要提供一个基础的逻辑结构，让机器人得以储存它们各自的局部空间结构，并能在该结构下使用共享信息。如果地图中的显著性信息得以提取并在机器人之间共享的话，计算速度和共有地图的准确度都会增加，而且在沟通过程中也不会有大量未处理数据占据空间。

拓扑地图是一种使用连通路径和交叉节点的抽象表示方法。人类和昆虫经常使用拓扑地图来进行导航，计算路径和位置，以及避免障碍。例如，鸽子已经被证实在飞行长距离时，使用高速公路和它们的交叉点来作为拓扑地图。

合作性SLAM可以根据是共享原始传感器数据还是共享处理后的地图分为两种。共享原始传感器数据可以获得更大的灵活性，但是需要机器人之间的通信更可靠，并且需要更宽的带宽和更高的处理能力。共享地图有助于减少带宽并且减少对原始数据的处理，但是，性能主要依赖于地图质量。

此外，多机器人的SLAM也可以根据测量数据的方法来进行分类。在基于特征的SLAM中，一般是使用照相机来进行数据采集，并从测量之中提取独特性物体作为特征或地标，以用于定位。特征的空间分布可以作为全局环境的一个表现方式。另一种方法叫做基于视图的SLAM[，这种方法主要采用的是激光扫描的方法获取数据。

Thrun提出了一种概率的多机器人SLAM算法。这种方法是鲁棒的，但是在进行具体的任务之前，我们需要得知机器人的近似初始位置。

基于视图的多机器人SLAM是由Howard提出的，它假定，多个机器人需要在环境中相遇来确定相对位姿，这种算法的计算速度一般，但是需要比较高的计算能力，因为它是基于粒子滤波。

对于多机器人SLAM来说，有一类有效且快速的方法是基于地图融合的概念提出的。Howard提出了一种基于占用地图融合的算法。这种算法使用地图中的距离作为相似参数，并通过一种随机游走算法来找到两幅地图中的相似模式。这种方法的缺点是，当两幅地图中的相似模式比较少时容易失败。而且由于这种方法比较耗时，所以对于室内环境中的大范围地图很有可能会效率低下。之后还提出了一个相似的方法，它使用模拟退火的方法进行地图融合，但是当两幅图像的重叠较少时，该方法也会变得效率很低。

总结来看，虽然针对多机器人SLAM提出过多样的拓扑方法，但是这些方法有的效率低下，有的准确度较低，并且并没有之前的算法针对非线性转换过程中存在的不确定度提出切实有效的方法进行解决。

发明内容

本发明提供一种克服上述问题或者至少部分地解决上述问题的基于二阶矩保持传播算法的拓扑地图融合方法及系统。

根据本发明的一个方面，提供一种基于二阶矩保持传播算法的拓扑地图融合方法，包括：

S1，基于概率性广义Voronoi图方法对不同地图进行融合；

S2，通过二阶矩保持传播方法消除地图融合的非线性不确定度。

S1进一步包括：

S1.1，利用Radon变换方法获取两幅地图之间的相对旋转角度；

S1.2，利用概率性广义Voronoi图方法提取两幅地图的拓扑骨架，获取两幅地图之间的平移矩阵；

S1.3，根据所述相对旋转角度和所述平移矩阵对两幅地图进行融合处理。

S2进一步包括：

S2.1，根据融合之后的地图的显著点进行撒点采样，获取采样点集；

S2.2，通过非线性变换传递所述采样点的不确定度，获得新采样点集；

S2.3，获取新采样点集的均值和方差，消除地图融合的非线性不确定度。

根据本发明的另一个方面，提供一种基于二阶矩保持传播算法的拓扑地图融合系统，包括地图融合模块和非线性消除模块，

所述地图融合模块，用于基于概率性广义Voronoi图方法对不同地图进行融合；

所述非线性消除模块，用于通过二阶矩保持传播方法消除地图融合的非线性不确定度

本申请提出的基于二阶矩保持传播算法的拓扑地图融合方法及系统，考虑到使用PGVD时占用栅格地图中的不确定度，利用线性化来解决转换过程中的不确定度问题。比起其他算法，它是快速且鲁棒的，能够对相似度更大的区域进行优先匹配。

附图说明

图1为本发明基于二阶矩保持传播算法的拓扑地图融合方法流程图；

图2为本发明第一实施例对规则地图的融合示意图；

图3为本发明第二实施例对不规则地图的融合示意图；

图4为本发明基于二阶矩保持传播算法的拓扑地图融合系统示意图。

具体实施方式

下面结合附图和实施例，对本发明的具体实施方式作进一步详细描述。以下实施例用于说明本发明，但不用来限制本发明的范围。

本发明应用于多机器人的环境探索和地图融合领域，合作性SLAM共享处理后的地图进行地图融合，并采样激光扫描的方法获取地图数据作为本发明算法的输入数据。

如图1所示，一种基于二阶矩保持传播算法的拓扑地图融合方法，包括：

S1，基于概率性广义Voronoi图方法对不同地图进行融合；

S2，通过二阶矩保持传播方法消除地图融合的非线性不确定度。

S1包括S1.1、S1.2和S1.3：

S1.1，利用Radon变换方法获取两幅地图之间的相对旋转角度。

具体实现为：

Radon变换是沿着一个特定角度的径向线，进行在该角度上的线积分。针对二维图像f(x,y)来说，垂直方向的线积分是f(x,y)投影到x轴，水平方向的线积分是f(x,y)投影到y轴。沿不同角度的线积分可用下面的公式进行计算：

Radon变换的峰值对应的是图像中的直线部分。

分别对两副地图进行Radon变换，分别获取两幅图像中的峰值，确定两幅图像中对应峰值之间对应的角度差，就可以得到两幅图像之间的相对旋转角度。

S1.2，利用概率性广义Voronoi图方法提取两幅地图的拓扑骨架，获取两幅地图之间的平移矩阵。

在进行S1.2的具体实现之前需要先确定如下问题：

A.联络点。一个区域的联络点指的是距离区域点最近且与其他某个区域距离相同的所有被占用区域点的集合。占用栅格地图中联络点的概率特性会被用来建立概率性广义Voronoi图。

B.概率性广义Voronoi图。广义Voronoi图中的每一个区域都可以用一个二维随机变量来表示它的占用联络点超过2个的概率，这也是基于占用栅格地图的占用概率。

具体实现为：

1.在二维占用栅格地图中通过数学上的形态学操作提取地图的广义Voronoi图。

所述广义Voronoi图和原始地图拥有相同的大小，是对于原始地图中显著性信息的提取；通过提取广义Voronoi图，可以在保留地图重要信息的前提下，大大减小计算的复杂度和效率。

2.基于占用栅格地图的实际概率来计算在广义Voronoi图中每个区域的相应概率。通过计算每个区域点的概率特性，可以最终创建出一个包含概率信息的拓扑性地图。

以上分别对两幅地图进行处理，得到两幅地图的概率性广义Voronoi图。

3.将提取得到的概率性广义Voronoi图用边的形式表现出来，通过计算两幅地图中任意两条边的相似度，得到最相似的两条边，以此计算平移矩阵。

本发明算法首先提取图像的显著性拓扑图，然后进一步寻找它们之间的重合部分，比起在普通的度量空间的匹配中会更快速而且高效。

S1.3，根据所述相对旋转角度和所述平移矩阵对两幅地图进行融合处理。

具体实现为：

以一副地图为基准，按相对旋转角度对另一幅地图进行旋转，使两幅地图角度一致；按平移矩阵进行图像平移，使两幅地图图像融合。

由于S1.3的角度旋转和矩阵平移处理为非线性变换，会产生非线性不确定度的问题，因此S2中对S1地图融合之后的非线性不确定度问题进行处理，采用二阶矩保持传播方法。

二阶矩保持传播方法是当非线性系统中输入随机变量时，对结果进行估计的一种“黑盒”方法。二阶矩保持传播是通过建立一系列叫做σ点的采样点来表现分布函数的具体特性，σ点和它们的权重可以被用来估计随机变量值。

二阶矩保持传播方法可以解决非线性系统的问题，并且不需要进行线性近似。二阶矩保持传播只通过有限的计算就可以得到输出的统计特性(例如均值和方差)。当输入变量是正态分布时，输出变量也可以用正态分布表示，这就使得二阶矩保持传播方法在处理正态分布随机变量时非常有效。

S2包括S2.1、S2.2和S2.3：

S2.1，根据融合之后的地图的显著点进行撒点采样，获取采样点集；

具体实现为：

首先针对输入量，选取相应合适的σ点即采样点。对于每个显著点，本发明都选取两个点来表示它的分布，而不是采用均值，这两个点一个大于均值，一个小于均值，此外还要提出一个独立的点代表均值，所以针对一幅有N个显著点的地图图像可以建立起(2N+1)个点的采样点集。

所述采样点集满足：

其中，x⁽ⁱ⁾为非线性变换之前地图上不同的采样点的二维坐标值；为采样点的二阶高斯分布均值，所述二阶为采样点的二维坐标值；为每个采样点偏离采样中心点的偏移量；N为每个显著点的采样点的个数；P为采样点的二阶高斯分布方差；T为矩阵转置；

每个采样点有一个对应的权重W⁽ⁱ⁾，满足：

每个采样点的权重通过下式计算：

其中，k可以是满足(N+k)≠0的任意值。

优选的，采用下式计算k的值：

k＝α(N+λ)-N

初始，λ＝N-3是为了降低对高维的依赖性；通常会选择λ＝0，α∈(0,1]来划定σ点的范围。

S2.2，通过非线性变换传递所述采样点的不确定度，获得新采样点集；

所述非线性传递满足下式：

y⁽ⁱ⁾＝h(x⁽ⁱ⁾)

其中，h为非线性变换函数，所述非线性变换指地图通过旋转角度和平移矩阵进行融合的非线性处理；x⁽ⁱ⁾为非线性处理之前的地图采样点信息，y⁽ⁱ⁾为非线性变换之后的地图采样点信息，即新采样点集。

S2.3，获取新采样点集的均值和方差，消除地图融合的非线性不确定度。

获取新采样点集的均值和方差的公式为：

其中，y⁽ⁱ⁾代表x⁽ⁱ⁾通过非线性函数之后的输出，W⁽ⁱ⁾代表权值，为y⁽ⁱ⁾的均值，P_y为y⁽ⁱ⁾的方差。

图2与图3中都是首先使用Radon变换进行旋转角度的计算，然后通过边匹配的方法进行平移矩阵的计算。计算过后，为了解决过程中存在的非线性不确定度，采用了二阶矩保持传播方法进行解决。

图2中(a1)和(a2)分别为融合前的两幅地图，(a3)为进行角度旋转和矩阵平移，并采用二阶矩保持传播方法进行解决了非线性不确定度的融合地图。

图3中(b1)和(b2)分别为融合前的两幅地图，(b3)为进行角度旋转和矩阵平移，并采用二阶矩保持传播方法进行解决了非线性不确定度的融合地图。

如图4所示，本发明还提供一种基于二阶矩保持传播算法的拓扑地图融合系统，包括地图融合模块和非线性消除模块，

所述地图融合模块，用于基于概率性广义Voronoi图方法对不同地图进行融合；

所述非线性消除模块，用于通过二阶矩保持传播方法消除地图融合的非线性不确定度。

本发明发现了在拓扑地图融合过程之中存在的非线性不确定度问题，并利用二阶矩保持传播算法进行解决，能减小计算量、提高运算速度和效率，提高地图融合的准确度。

本发明为了减少运算过程中的计算量，提高运算速度，首先对图像进行概率性广义Voronoi图的提取，此算法在保留了地图基本结构和显著性信息的同时，采用点和边的方式对地图进行表示，同时采用概率性的方法，将原地图的概率信息也包含在内，在保证准确性的同时，大大的提高了运算速度和效率。同时采用Radon变换的方法进行旋转角度的算，采用计算两幅地图边相似度的方法进行平移矩阵的计算，最后通过旋转角度和平移矩阵进行两幅地图的融合。

本发明经过实际测评，在对拓扑地图进行融合并消除非线性问题的运算速度和准确度上具有较好的效果。测评方法如下：

通过分析二维互相关计算，可以进行地图匹配的准确度检测，同时可以在设定需要满足的准确度的前提下，将不符合条件的错误匹配舍去。假设有一个存在于之中点数目为l₁的边和一个存在于之中点数目为l₂的边，并且假设在二维互相关中的最佳匹配点数目为L，将中的匹配的子集定为然后就可以将匹配边的平均置信度定义为：

同时，可以将匹配准确度进行如下定义，代表两幅图像中最相似的两条边的互相关系数最大值：

通过这两个指标，可以证明该基于概率性广义Voronoi图的拓扑地图融合方法在不降低地图融合准确度的情况下，可以大大提升运算速度，以证明本文算法的优越性。

测试结果如下：

表1分别显示的是与不同算法在时间和准确度方面的对比，其中包括ARW mapmerging(A.Birk and S.Carpin,“Merging occupancy grid maps from multiplerobots”Proc.IEEE:Special Issue Multi-Robot System.,vol.94,no.7,pp.1384C1387,2006.)和Map Segmentation(S.Saeedi,L.Paull,M.Trentini,and H.Li,“Multiple robotsimultaneouslocalization and mapping”,in Proc.IEEE/RSJ Int.Conf.IntelligentRobotsSystems,2013,pp.853C858.)

表1

方法	运算时间(秒)	准确度(％)
			ARW Map Merging	168	93
Map Segmentation	105	95
			本文算法	20	96

表1是上述三类算法在融合两幅数据库地图时进行测试的平均结果，可以看出本发明方法在准确度不输给其他算法的同时，在计算速度上有明显提高。

最后，本申请的方法仅为较佳的实施方案，并非用于限定本发明的保护范围。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (9)

1.一种基于二阶矩保持传播算法的拓扑地图融合方法，其特征在于，包括：

S1，基于概率性广义Voronoi图方法对不同地图进行融合；

S2，通过二阶矩保持传播算法消除地图融合的非线性不确定度。

2.如权利要求1所述的方法，其特征在于，S1进一步包括：

S1.1，利用Radon变换方法获取两幅地图之间的相对旋转角度；

S1.2，利用概率性广义Voronoi图方法提取两幅地图的拓扑骨架，获取两幅地图之间的平移矩阵；

S1.3，根据所述相对旋转角度和所述平移矩阵对两幅地图进行融合处理。

3.如权利要求1所述的方法，其特征在于，S2进一步包括：

S2.1，根据融合之后的地图的显著点进行撒点采样，获取采样点集；

S2.2，通过非线性变换传递所述采样点的不确定度，获得新采样点集；

S2.3，获取新采样点集的均值和方差，消除地图融合的非线性不确定度。

4.如权利要求2所述的方法，其特征在于，S1.1.1进一步包括：

S1.1.1，分别获取两幅地图沿不同角度二维图像的投影；

S1.1.2，通过投影变换后的两幅地图的峰值获取相对旋转角度差。

5.如权利要求2所述的方法，其特征在于，S1.2进一步包括：

S1.2.1，基于二维占用栅格地图，通过数学上的形态学操作分别提取每幅地图的广义Voronoi图；

S1.2.2，基于二维占用栅格地图的实际概率分别计算每幅地图的广义Voronoi图中每个区域的相应概率，获取每幅地图的概率性广义Voronoi图；

S1.2.3，计算两幅概率性广义Voronoi图的相似度获取平移矩阵。

6.如权利要求3所述的方法，其特征在于，S2.1中所述采样点集满足：

$x^{\left(0\right)}=\stackrel{\‾}{x}$

$x^{\left(i\right)}=\stackrel{\‾}{x}+{\tilde{P}}^{\left(i\right)},i=1,...,2N$

${\tilde{P}}^{\left(i\right)}={\left(\sqrt{(N+k)P}\right)}^T,i=1,...,N$

其中，x⁽ⁱ⁾为非线性变换之前地图上不同的采样点的二维坐标值；为采样点的二阶高斯分布均值，所述二阶为采样点的二维坐标值；为每个采样点偏离采样中心点的偏移量；N为每个显著点的采样点的个数；P为采样点的二阶高斯分布方差；T为矩阵转置；

每个采样点有一个对应的权重W⁽ⁱ⁾，满足：

$\underset{i=0}{\overset{N}{\Σ}}{W}^{\left(i\right)}=1$

每个采样点的权重通过下式计算：

$W^{\left(0\right)}=\frac{k}{N+k}$

$W^{\left(i\right)}=\frac{k}{2(N+k)},i=1,...,2N$

其中，k可以是满足(N+k)≠0的任意值。

7.如权利要求3所述的方法，其特征在于，S2.2中所述非线性传递满足下式：

y⁽ⁱ⁾＝h(x⁽ⁱ⁾)

其中，h为非线性变换函数，所述非线性变换指地图通过旋转角度和平移矩阵进行融合的非线性处理；x⁽ⁱ⁾为非线性处理之前的地图采样点信息，y⁽ⁱ⁾为非线性变换之后的地图采样点信息，即新采样点集。

8.如权利要求3所述的方法，其特征在于，S2.3中获取新采样点集的均值和方差的公式为：

$\stackrel{\‾}{y}=\underset{i=0}{\overset{N}{\Σ}}{W}^{\left(i\right)}{y}^{\left(i\right)}$

$P_y=\underset{i=0}{\overset{N}{\Σ}}{W}^{\left(i\right)}(y^{\left(i\right)}-\stackrel{\‾}{y}){(y^{\left(i\right)}-y)}^T$

其中，y⁽ⁱ⁾代表x⁽ⁱ⁾通过非线性函数之后的输出，W⁽ⁱ⁾代表权值，为y⁽ⁱ⁾的均值，P_y为y⁽ⁱ⁾的方差。

9.一种基于二阶矩保持传播算法的拓扑地图融合系统，其特征在于，包括地图融合模块和非线性消除模块，

所述地图融合模块，用于基于概率性广义Voronoi图方法对不同地图进行融合；

所述非线性消除模块，用于通过二阶矩保持传播方法消除地图融合的非线性不确定度。