一种基于CI因子图的多机器人2D地图融合算法

A Multi-robot 2D Map Fusion Algorithm Based on CI Factor Graph

针对地图融合时多机器人位姿估计过程中互协方差未知的问题,提出了一种基于协方差交叉点(covariance intersection,CI)因子图的2D地图融合算法.首先通过坐标变换矩阵实现机器人坐标到全局坐标的转换,然后以最小化非线性性能指标为原则求取局部的估计信息权重,通过算法融合各局部估计信息,计算出融合点的位姿和互协方差.最后通过协方差通用公式,计算出融合点到下一级变量节点的概率约束(协方差),进而完成因子图融合.实验结果表明,该算法具有一定的可行性.

A 2D map fusion algorithm based on covariance intersection(CI) factor graph is proposed for the problem that the covariance is unknown in the process of multi-robot pose estimation. Firstly, the transformation from the robot coordinate system to the global coordinate system is realized by the coordinate transformation matrix. Then, the local estimation information weight is obtained based on the principle of minimized non-linear performance. The local estimation information is merged by the algorithm to calculate the pose and cross-covariance of the fusion point. Finally, the covariance common formula is used to calculate the probability constraint(covariance) of the fusion nodes to the next-level variable nodes, thereby the factor graph fusion is completed. The experimental results show that the algorithm is feasible.