本发明属于多个移动机器人对环境进行建模的领域,特别是基于图优化的多机器人slam算法,具体涉及一种基于子地图特征匹配的多机器人slam算法。
背景技术:
同时定位与地图创建(simultaneouslocalizationandmapping,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、定义机器人r1和r2创建的子地图序列分别为
步骤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算法对待匹配的栅格地图提取特征点;将距离较近的特征点聚为一类,并以这一类特征点的几何中心作为聚类中心,用这一类中所有特征点的描述子共同对该聚类中心进行描述;使用汉明距离计算特征点的初始匹配,并且根据特征点的初始匹配计算出聚类中心的初始匹配;使用回溯法从初始匹配中搜索三组满足约束条件的初始匹配,约束条件为:
其中,m0={m1,…,mn}表示使用回溯法对聚类中心的初始匹配的每一行进行访问;
使用位置关联算法,计算最大公共子图,位置关联具体为:
当回溯法搜索到三组满足约束要求的初始匹配时,根据这三组聚类中心的匹配计算初始变换矩阵;根据初始变换矩阵,将待融合栅格地图g2中的聚类中心变换到栅格地图g1的坐标系下;比较变换之后的g2中的聚类中心与g1中的聚类中心之间的距离,将距离小于一定阈值且满足初始匹配要求的匹配作为当前初始变换矩阵的最大公共子图方案。
选择最优最大公共子图方案,具体为:
将最大公共子图方案中匹配聚类中心数目最多的作为最优方案的候选;如果匹配聚类中心数目最多的最大公共子图方案只有一个,那该方案就是最优方案;如果匹配聚类中心数目最多的最大公共子图方案不止一个,则将匹配聚类中心构成多边形面积最大的作为最优方案。
最后根据最优最大公共子图方案,计算变换矩阵。
假设机器人r1子地图匹配结果为:
其中,
在得到机器人在不同时刻的位姿之后,可以对闭环检测的正确性进行判断。假设
将误差e小于阈值∈e的作为候选闭环检测方案。其次通过判断这些候选方案中子地图匹配的特征点的对数,即最大公共子图中聚类中心的匹配对数,将匹配对数n小于阈值∈n的去除。经过上述步骤,可以得到正确的闭环和前后帧子地图之间的匹配关系,如图3所示。
步骤3、根据基于特征匹配的方法,对不同机器人创建的子地图序列进行匹配,将所有匹配结果进行保存,匹配结果包括相对位姿和匹配聚类中心。
步骤4、根据步骤3的结果,计算多个机器人之间的相对位姿。
将步骤3结果中匹配聚类中心数目最多的结果作为多机器人相对位姿,例如机器人r1的子地图
步骤5、根据步骤3和步骤4的计算结果,构建多机器人闭环约束。
具体地,将多个机器人创建的子地图对应的机器人位姿变换到同一坐标系下,按照步骤2计算多个机器人slam的闭环约束。
步骤6、根据步骤2和步骤5的结果,构建多机器人slam算法的前端。
具体地,将单机器人的前端约束和多机器人slam的闭环约束联立,构建多机器人slam的前端。
步骤7、多机器人slam后端优化。
具体地,多机器人slam后端优化具体为:
参考平面的选取;参考平面的选取需要考虑三个因素:
(1)参考平面与哪几张子地图存在重叠区域;
(2)参考平面与各个子地图重叠区域的面积大小;
(3)参考平面与各个子地图重叠区域内的匹配特征点数目;
为了确定子地图gi是否可以被作为参考平面,本发明根据以下公式进行判断:
其中,l(i)表示与子地图gi存在重叠区域的子地图的集合,即相邻的子地图和与其形成闭环的子地图;aij和nij分别表示子地图gi和子地图gj重叠区域的面积和匹配特征点个数,本发明分别使用最大公共子图中匹配聚类中心构成的面积和匹配聚类中心个数表示;λ为调节参数,该参数根据子地图特征分布决定。当子地图提取特征较多时,λ值也可以相应较大,强调特征匹配的重要性;当特征较少时,λ值可以相应较小,强调重叠区域面积对于参考平面选取的重要性。si值越大表示子地图gi在该子地图序列中的作用性越大,因此选择该值最大时对应的子地图作为参考平面。
捆绑调整具体步骤为:
在使用捆绑调整前,需要计算各个子地图到参考平面的变换矩阵。如要将子地图gi变换到参考平面并且已知子地图gi-1到参考平面的变换矩阵为ti-1,则子地图gi到参考平面的变换矩阵为
ti=ti-1ti-1,i。
捆绑调整的过程就是将待融合子地图依次加入到优化器中,并且同时对优化器中的各个子地图到参考平面的变换矩阵进行调整。优化的方法是直接计算子地图gi与其相邻子地图gi-1和与其形成闭环的子地图的匹配聚类中心变换到参考平面上的距离,计算使得这个距离最小时的变换矩阵,从而达到调整变换矩阵的目的。
参见图4所示,
将所有待融合子地图的误差求和可以得到全局误差函数:
其中,mij表示子地图gi和gj的最大公共子图中匹配的聚类中心,
l-m算法具体步骤为:
l-m算法构造误差的最小二乘形式,结合梯度下降和高斯-牛顿法求解非线性最优化问题:
f(x)=∑iei(x)tωiei(x)
其中,ωi为信息矩阵,
(h+λi)δx=-b
其中,λ为松弛因子。当λ较大时,将l-m方法转化为梯度下降法;当λ较小时,将l-m方法转化成g-n。l-m算法退出条件为:达到最大迭代次数或者根据当前优化得到的误差与上一次误差比较接近。
步骤8、全局栅格地图创建。根据优化之后子地图匹配结果,结合栅格地图融合策略将多个子地图序列融合成全局栅格地图,具体为:
根据优化之后子地图之间的匹配关系,可以将子地图g2的栅格坐标做变换:
其次,根据变换之后
其中,上式将灰度值转变成logodd形式,其次将对应坐标处的logodd值进行相加,得到融合后的地图,并且使用概率形式表示全局地图。
图5为通过子地图序列之间的匹配关系,未经优化,直接融合的结果,从图中可以看出,直接融合的结果所创建地图的精确度不高。
图6为使用多机器人slam后端优化对多个子地图序列之间的变换关系进行优化之后融合的结果,可以看出,优化之后创建的全局地图精度高。
当然上述实施例只为说明本发明的技术构思及特点,其目的在于让熟悉此项技术的人能够了解本发明的内容并据以实施,并不能以此限制本发明的保护范围。凡根据本发明主要技术方案的精神实质所做的修饰,都应涵盖在本发明的保护范围之内。