CN110119144A - 基于子地图特征匹配的多机器人slam算法 - Google Patents

Abstract

本发明公开了一种基于子地图特征匹配的多机器人SLAM算法，包括：步骤1、采用多个机器人采集环境信息，并据此创建子地图序列；步骤2、对单个机器人创建的子地图进行匹配，构建单机器人SLAM算法的前端；步骤3、基于最大公共子图的栅格地图融合算法对不同机器人创建的子地图序列进行匹配，将所有匹配结果进行保存；步骤4、根据步骤3的结果，计算多个机器人之间的相对位姿；步骤5、根据步骤3和步骤4的计算结果，构建多机器人闭环约束；步骤6、根据步骤2和步骤5的结果，构建多机器人SLAM算法的前端；步骤7、多机器人SLAM后端优化；步骤8、全局栅格地图创建。本发明能够解决对大尺度环境或者对工作效率要求较高的环境进行建模的问题。

Description

基于子地图特征匹配的多机器人SLAM算法

技术领域

本发明属于多个移动机器人对环境进行建模的领域，特别是基于图优化的多机器人SLAM算法，具体涉及一种基于子地图特征匹配的多机器人 SLAM算法。

背景技术

同时定位与地图创建(simultaneous localization and mapping，SLAM)是指移动机器人在未知环境中运行，根据自身携带的传感器观测周围环境，对环境进行描述，并且根据所创建的地图实现自身定位的一项技术。拥有该项技术的机器人可以在未知环境中完成导航、路径规划、探索等更加复杂的工作。因此SLAM技术也是移动机器人真正实现自主化和智能化的关键。但是，当面对机场或者博物馆这种大尺度环境，以单个机器人的电量不可能一次性完成环境的建模；或者当面对设备维修、货物搬运等工作效率要求较高的任务，单个机器人很难满足快速的对环境进行精确建模的要求。

相比于单机器人，多机器人SLAM通过多个机器人之间的相互协作、并行运行来提高工作效率；多个机器人可以搭载不同的传感器，不同的传感器可以提供更加丰富的环境信息，提高所创建地图的精确度；使用多个机器人对环境建模时，当其中一个或者几个机器人出现故障时，其余机器人可以继续协作完成环境建模工作，因此，多机器人更加的稳定。

因此，有必要提供一种有效的多机器人SLAM方法解决需要快速地环境进行精确建模的问题。

发明内容

本发明目的是提供一种基于子地图特征匹配的多机器人SLAM算法，用来解决对大尺度环境或者对工作效率要求较高的环境进行建模的问题。

本发明的技术方案是：一种基于子地图特征匹配的多机器人SLAM算法，包括如下步骤：

步骤1、采用多个机器人在环境中运动，采集环境信息，并根据信息创建子地图序列；

步骤2、根据基于特征匹配的方法，对单个机器人创建的子地图进行匹配，构建单机器人SLAM算法的前端；

步骤3、根据基于特征匹配的方法，对不同机器人创建的子地图序列进行匹配，将所有匹配结果进行保存；

步骤4、根据步骤3的结果，计算多个机器人之间的相对位姿；

步骤5、根据步骤3和步骤4的计算结果，构建多机器人闭环约束；

步骤6、根据步骤2和步骤5的结果，构建多机器人SLAM算法的前端；

步骤7、多机器人SLAM后端优化；

步骤8、全局栅格地图创建。

上述技术方案中，所述步骤1中的多个机器人中，其中一个机器人的运动轨迹至少与其余机器人中的一个机器人的轨迹有重叠。

上述技术方案中，所述步骤1中的多个机器人上均安装有激光雷达传感器。

上文中，所述步骤1中的子地图创建具体为：

机器人在环境中运行，当采集到足够的激光数据之后，根据这些观测信息创建环境的栅格子地图。

上述技术方案中，所述步骤2中构建单机器人SLAM的前端具体包括：

步骤21、定义机器人R₁和R₂创建的子地图序列分别为和

步骤22、使用基于最大公共子图的栅格地图融合算法对单个机器人创建的子地图进行两两匹配；

步骤23、将前后两帧子地图的匹配结果和满足闭环要求的子地图匹配结果保存，构建单机器人SLAM的前端。

上述技术方案中，所述步骤3中多机器人子地图序列匹配具体为：

使用基于最大公共子图的栅格地图融合算法将来自不同机器人创建的子地图进行匹配，保存所有的匹配结果。

上述技术方案中，所述步骤4中计算多机器人相对位姿具体为：

根据步骤3的结果，将不同机器人子地图匹配结果中匹配特征数目最多的结果作为多机器人相对位姿。

上述技术方案中，所述步骤5中计算多机器人闭环约束具体为：

步骤51、根据步骤2的单机器人子地图序列的匹配结果，计算各个子地图相对于各自机器人坐标系的位姿；

步骤52、根据步骤4的多机器人相对位姿计算结果，将不同机器人子地图序列的位姿变换到同一坐标系下；

步骤53、将满足闭环要求的来自不同机器人创建的子地图的匹配结果保存，作为多机器人闭环约束。

上述技术方案中，所述步骤6中构建多机器人SLAM前端具体为：

将单机器人SLAM前端和多机器人闭环约束联立，构建多机器人SLAM 的前端。

上述技术方案中，所述步骤7中多机器人后端优化具体为：

使用捆绑调整的思想，对子地图之间的相对位姿进行优化。

上述技术方案中，所述步骤8中的全局地图创建具体为：

根据步骤7的优化结果，将不同机器人创建的子地图序列进行拼接，创建全局栅格地图。

本发明的优点是：

本发明根据图优化的方法将多机器人SLAM分为前端和后端，根据不同机器人创建子地图序列之间的匹配关系构建多机器人SLAM的前端约束，并且使用优化方法将误差降到最小，最后根据优化结果实现多个子地图序列的拼接，创建全局栅格地图。

附图说明

下面结合附图及实施例对本发明作进一步描述：

图1为本发明实施例一的算法的流程图

图2为多个机器人在环境中行走轨迹示意图。

图3为基于特征的子地图匹配算法的流程图。

图4为捆绑调整示意图。

图5为多机器人优化之前创建的环境地图。

图6为多机器人优化之后创建的环境地图。

具体实施方式

实施例一：

参见图1所示，一种基于子地图特征匹配的多机器人SLAM算法，包括如下步骤：

步骤1、采用多个机器人在环境中运动，采集环境信息，并根据信息创建子地图序列。

本实施例中，参见图2所示，所述步骤1中的多个机器人中，其中一个机器人的运动轨迹至少与其余机器人中的一个机器人的轨迹有重叠。

本实施例中，所述步骤1中的多个机器人上均安装有激光雷达传感器，根据激光雷达的观测信息创建环境的栅格地图。

具体地，多个机器人在环境中按照预定轨迹行走，当采集到的激光数据达到阈值时，则使用这些激光数据创建环境的栅格子地图，因此，每个机器人可以根据自己的观测数据创建子地图序列。

步骤2、根据基于特征匹配的方法，对单个机器人创建的子地图进行匹配，构建单机器人SLAM算法的前端。

参见图3所示，基于特征的匹配方法首先使用ORB算法对待匹配的栅格地图提取特征点；将距离较近的特征点聚为一类，并以这一类特征点的几何中心作为聚类中心，用这一类中所有特征点的描述子共同对该聚类中心进行描述；使用汉明距离计算特征点的初始匹配，并且根据特征点的初始匹配计算出聚类中心的初始匹配；使用回溯法从初始匹配中搜索三组满足约束条件的初始匹配，约束条件为：

其中，M₀＝{m₁，…，m_n}表示使用回溯法对聚类中心的初始匹配的每一行进行访问；表示G₁中第i个聚类中心和G₂中第i′个聚类中心满足初始匹配要求；和分别表示聚类中心在各自栅格地图上的坐标；∈₀为误差阈值；当M₀中只有一组匹配时，默认满足约束条件。

使用位置关联算法，计算最大公共子图，位置关联具体为：

当回溯法搜索到三组满足约束要求的初始匹配时，根据这三组聚类中心的匹配计算初始变换矩阵；根据初始变换矩阵，将待融合栅格地图G₂中的聚类中心变换到栅格地图G₁的坐标系下；比较变换之后的G₂中的聚类中心与G₁中的聚类中心之间的距离，将距离小于一定阈值且满足初始匹配要求的匹配作为当前初始变换矩阵的最大公共子图方案。

选择最优最大公共子图方案，具体为：

将最大公共子图方案中匹配聚类中心数目最多的作为最优方案的候选；如果匹配聚类中心数目最多的最大公共子图方案只有一个，那该方案就是最优方案；如果匹配聚类中心数目最多的最大公共子图方案不止一个，则将匹配聚类中心构成多边形面积最大的作为最优方案。

最后根据最优最大公共子图方案，计算变换矩阵。

假设机器人R₁子地图匹配结果为：

其中，表示通过子地图与子地图的最大公共子图计算出来的变换矩阵，该变换矩阵将变换到的坐标系下实现栅格地图的融合。当j-i＝1时，对应前端中的顺序数据关联；当j-i≠1时，对应前端中的环路闭合检测。子地图是机器人在不同时刻观测到的局部环境，假设子地图对应的机器人位姿为则根据顺序数据关联，可以计算每个子地图对应的机器人位姿：

在得到机器人在不同时刻的位姿之后，可以对闭环检测的正确性进行判断。假设表示使用最大公共子图算法计算出来的子地图G_j ¹和子地图G_i ¹的匹配结果，且j-i≠1。此时通过判断机器人位姿之间的变换与闭环检测的变换的相似性来判断闭环的正确性：

将误差E小于阈值∈_E的作为候选闭环检测方案。其次通过判断这些候选方案中子地图匹配的特征点的对数，即最大公共子图中聚类中心的匹配对数，将匹配对数N小于阈值∈_N的去除。经过上述步骤，可以得到正确的闭环和前后帧子地图之间的匹配关系，如图3所示。

步骤3、根据基于特征匹配的方法，对不同机器人创建的子地图序列进行匹配，将所有匹配结果进行保存，匹配结果包括相对位姿和匹配聚类中心。

步骤4、根据步骤3的结果，计算多个机器人之间的相对位姿。

将步骤3结果中匹配聚类中心数目最多的结果作为多机器人相对位姿，例如机器人R₁的子地图与机器人R₂的子地图匹配的聚类中心的数目最多，通过匹配聚类中心计算出来的变换矩阵为则可以通过该变换矩阵和机器人R₂前后帧子地图之间的相对位姿将机器人R₂的子地图变换到机器人R₁的坐标系下，即可以将两个机器人的位姿图变换到同一坐标系下。

步骤5、根据步骤3和步骤4的计算结果，构建多机器人闭环约束。

具体地，将多个机器人创建的子地图对应的机器人位姿变换到同一坐标系下，按照步骤2计算多个机器人SLAM的闭环约束。

步骤6、根据步骤2和步骤5的结果，构建多机器人SLAM算法的前端。

具体地，将单机器人的前端约束和多机器人SLAM的闭环约束联立，构建多机器人SLAM的前端。

步骤7、多机器人SLAM后端优化。

具体地，多机器人SLAM后端优化具体为：

参考平面的选取；参考平面的选取需要考虑三个因素：

(1)参考平面与哪几张子地图存在重叠区域；

(2)参考平面与各个子地图重叠区域的面积大小；

(3)参考平面与各个子地图重叠区域内的匹配特征点数目；

为了确定子地图G_i是否可以被作为参考平面，本发明根据以下公式进行判断：

其中，L(i)表示与子地图G_i存在重叠区域的子地图的集合，即相邻的子地图和与其形成闭环的子地图；A_ij和N_ij分别表示子地图G_i和子地图G_j重叠区域的面积和匹配特征点个数，本发明分别使用最大公共子图中匹配聚类中心构成的面积和匹配聚类中心个数表示；λ为调节参数，该参数根据子地图特征分布决定。当子地图提取特征较多时，λ值也可以相应较大，强调特征匹配的重要性；当特征较少时，λ值可以相应较小，强调重叠区域面积对于参考平面选取的重要性。S_i值越大表示子地图G_i在该子地图序列中的作用性越大，因此选择该值最大时对应的子地图作为参考平面。

捆绑调整具体步骤为：

在使用捆绑调整前，需要计算各个子地图到参考平面的变换矩阵。如要将子地图G_i变换到参考平面并且已知子地图G_i-1到参考平面的变换矩阵为 T_i-1，则子地图G_i到参考平面的变换矩阵为

T_i＝T_i-1T_i-1，i。

捆绑调整的过程就是将待融合子地图依次加入到优化器中，并且同时对优化器中的各个子地图到参考平面的变换矩阵进行调整。优化的方法是直接计算子地图G_i与其相邻子地图G_i-1和与其形成闭环的子地图的匹配聚类中心变换到参考平面上的距离，计算使得这个距离最小时的变换矩阵，从而达到调整变换矩阵的目的。

参见图4所示，表示子地图G_i的聚类中心与子地图G_j的聚类中心在最大公共子图中，分别将匹配的聚类中心变换到参考平面上，并且计算它们之间的距离r：

将所有待融合子地图的误差求和可以得到全局误差函数：

其中，M_ij表示子地图G_i和G_j的最大公共子图中匹配的聚类中心，表示最大公共子图中的第k个匹配聚类中心变换到参考平面上的距离。通过求解全局误差函数的最小值可以得到最优的变换矩阵。全局误差函数可以使用L-M方法进行迭代求解。

L-M算法具体步骤为：

L-M算法构造误差的最小二乘形式，结合梯度下降和高斯-牛顿法求解非线性最优化问题：

F(X)＝∑_ie_i(X)^TΩ_ie_i(X)

其中，Ω_i为信息矩阵， H为海森矩阵，其形式为J^TJ，J为雅克比矩阵。对式 F(X)＝∑_ie_i(X)^TΩ_ie_i(X)求解最小值，即求解累积误差的最小值，只需要找到一个ΔX对变换矩阵进行调整。求解ΔX只需要对其进行求导，使得倒数为零：

(H+λI)ΔX＝-b

其中，λ为松弛因子。当λ较大时，将L-M方法转化为梯度下降法；当λ较小时，将L-M方法转化成G-N。L-M算法退出条件为：达到最大迭代次数或者根据当前优化得到的误差与上一次误差比较接近。

步骤8、全局栅格地图创建。根据优化之后子地图匹配结果，结合栅格地图融合策略将多个子地图序列融合成全局栅格地图，具体为：

根据优化之后子地图之间的匹配关系，可以将子地图G₂的栅格坐标做变换：

其次，根据变换之后与G₁中栅格的灰度值实现地图融合。融合方法为：

其中，上式将灰度值转变成log odd形式，其次将对应坐标处的log odd 值进行相加，得到融合后的地图，并且使用概率形式表示全局地图。

图5为通过子地图序列之间的匹配关系，未经优化，直接融合的结果，从图中可以看出，直接融合的结果所创建地图的精确度不高。

图6为使用多机器人SLAM后端优化对多个子地图序列之间的变换关系进行优化之后融合的结果，可以看出，优化之后创建的全局地图精度高。

当然上述实施例只为说明本发明的技术构思及特点，其目的在于让熟悉此项技术的人能够了解本发明的内容并据以实施，并不能以此限制本发明的保护范围。凡根据本发明主要技术方案的精神实质所做的修饰，都应涵盖在本发明的保护范围之内。

Claims (9)

1.一种基于子地图特征匹配的多机器人SLAM算法，其特征在于，包括如下步骤：

步骤1、采用多个机器人在环境中运动，采集环境信息，并根据信息创建子地图序列；

步骤2、根据基于特征匹配的方法，对单个机器人创建的子地图进行匹配，构建单机器人SLAM算法的前端；

步骤3、根据基于特征匹配的方法，对不同机器人创建的子地图序列进行匹配，将所有匹配结果进行保存；

步骤4、根据步骤3的结果，计算多个机器人之间的相对位姿；

步骤5、根据步骤3和步骤4的计算结果，构建多机器人闭环约束；

步骤6、根据步骤2和步骤5的结果，构建多机器人SLAM算法的前端；

步骤7、多机器人SLAM后端优化；

步骤8、全局栅格地图创建。

2.根据权利要求1所述的基于子地图特征匹配的多机器人SLAM算法，其特征在于：所述步骤1中的多个机器人中，其中一个机器人的运动轨迹至少与其余机器人中的一个机器人的轨迹有重叠。

3.根据权利要求1所述的基于子地图特征匹配的多机器人SLAM算法，其特征在于：所述步骤2中构建单机器人SLAM的前端具体包括：

步骤21、定义机器人R₁和R₂创建的子地图序列分别为和

步骤22、使用基于最大公共子图的栅格地图融合算法对单个机器人创建的子地图进行两两匹配；

步骤23、将前后两帧子地图的匹配结果和满足闭环要求的子地图匹配结果保存，构建单机器人SLAM的前端。

4.根据权利要求1所述的基于子地图特征匹配的多机器人SLAM算法，其特征在于：所述步骤3中多机器人子地图序列匹配具体为：

使用基于最大公共子图的栅格地图融合算法将来自不同机器人创建的子地图进行匹配，保存所有的匹配结果。

5.根据权利要求1所述的基于子地图特征匹配的多机器人SLAM算法，其特征在于：所述步骤4中计算多机器人相对位姿具体为：

根据步骤3的结果，将不同机器人子地图匹配结果中匹配特征数目最多的结果作为多机器人相对位姿。

6.根据权利要求1所述的基于子地图特征匹配的多机器人SLAM算法，其特征在于：所述步骤5中计算多机器人闭环约束具体为：

步骤51、根据步骤2的单机器人子地图序列的匹配结果，计算各个子地图相对于各自机器人坐标系的位姿；

步骤52、根据步骤4的多机器人相对位姿计算结果，将不同机器人子地图序列的位姿变换到同一坐标系下；

步骤53、将满足闭环要求的来自不同机器人创建的子地图的匹配结果保存，作为多机器人闭环约束。

7.根据权利要求1所述的基于子地图特征匹配的多机器人SLAM算法，其特征在于：所述步骤6中构建多机器人SLAM前端具体为：

将单机器人SLAM前端和多机器人闭环约束联立，构建多机器人SLAM的前端。

8.根据权利要求1所述的基于子地图特征匹配的多机器人SLAM算法，其特征在于：所述步骤7中多机器人后端优化具体为：

使用捆绑调整的思想，对子地图之间的相对位姿进行优化。

9.根据权利要求1所述的基于子地图特征匹配的多机器人SLAM算法，其特征在于：所述步骤8中的全局地图创建具体为：

根据步骤7的优化结果，将不同机器人创建的子地图序列进行拼接，创建全局栅格地图。