A fully distributed Maximum Likelihood (ML) trajectory estimation method based on the Successive Over-Relaxation (SOR) is proposed to estimate the 3D trajectories of multiple collaborative robots from relative pose measurements, which can minimize the amount of exchanged information and scales well to large teams. First, the trajectory estimation is approximated by a sequence of two quadratic sub-problems. Then, these two sub-problems are further re-parameterized into two linear optimization problems. Finally, the two linear problems are solved in a distributed manner, using the distributed SOR algorithm with flagged-initialization to reduce the amount of data transmission. Extensive experiment shows that the proposed method can reach the accuracy level of the centralized algorithm. The position estimation error is less than 0.15 m and the rotation error is less than 0.03° under the condition of 49 robot scales. What's more, the minimum data transmission of the proposed method is only 0.06% of DDF-SAM. Therefore, the proposed method can be well adopted to the application scenario of large-scale teams.
[1] LUFT L, SCHUBERT T, ROUMELIOTIS S I, et al. Recursive decentralized localization for multi-robot systems with asynchronous pairwise communication[J]. The International Journal of Robotics Research, 2018, 37(10):1152-1167.
[2] 张国良, 汤文俊, 曾静, 等. 考虑通信状况的多机器人CSLAM问题综述[J].自动化学报, 2014, 40(10):2073-2088. ZHANG G L, TANG W J, ZENG J, et al. An overview on the cooperative SLAM problem of multi-robot systems considering communication conditions[J]. Acta Automatica Sinica, 2014, 40(10):2073-2088(in Chinese).
[3] BAILEY T, BRYSON M, MU H, et al. Decentralised cooperative localisation for heterogeneous teams of mobile robots[C]//IEEE International Conference on Robotics and Automation. Piscataway, NJ:IEEE Press, 2011:2859-2865.
[4] INDELMAN V, GURFIL P, RIVLIN E, et al. Graph-based distributed cooperative navigation for a general multi-robot measurement model[J]. The International Journal of Robotics Research, 2012, 31(9):1057-1080.
[5] BRESSON G, AUFRERE R, CHAPUIS R. A general consistent decentralized simultaneous localization and mapping solution[J]. Robotics and Autonomous Systems, 2015, 74:128-147.
[6] CIESLEWSKI T, SCARAMUZZA D. Efficient decentralized visual place recognition using a distributed inverted index[J]. IEEE Robotics and Automation Letters, 2017, 2(2):640-647.
[7] ROUMELIOTIS S I, BEKEY G A. Distributed multirobot localization[J]. IEEE Transactions on Robotics and Automation, 2002, 18(5):781-795.
[8] WEBSTER S E, WALLS J M, WHITCOMB L L, et al. Decentralized extended information filter for single-beacon cooperative acoustic navigation:Theory and experiments[J]. IEEE Transactions on Robotics, 2013, 29(4):957-974.
[9] HOWARD A. Multi-robot simultaneous localization and mapping using particle filters[J]. The International Journal of Robotics Research, 2006, 25(12):1243-1256.
[10] CARLONE L, NG M K, DU J, et al. Simultaneous localization and mapping using rao-blackwellized particle filters in multi robot systems[J]. Journal of Intelligent & Robotic Systems, 2011, 63(2):283-307.
[11] KIM B, KAESS M, FLETCHER L, et al. Multiple relative pose graphs for robust cooperative mapping[C]//IEEE International Conference on Robotics and Automation. Piscataway, NJ:IEEE Press, 2010:3185-3192.
[12] KIA S S, HECHTBAUER J, GOGOKHIYA D, et al. Server-assisted distributed cooperative localization over unreliable communication links[J]. IEEE Transactions on Robotics, 2018(99):1-8.
[13] SCHMUCK P, CHLI M. CCM-SLAM:Robust and efficient centralized collaborative monocular simultaneous localization and mapping for robotic teams[J]. Journal of Field Robotics, 2019,36(4):763-781
[14] LI F, YANG S, YI X, et al. CORB-SLAM:A collaborative visual slam system for multiple robots[C]//International Conference on Collaborative Computing:Networking, Applications and Worksharing. Berlin, Heidelberg:Springer, 2017:480-490.
[15] ARAGUES R, CARLONE L, CALAFIORE G, et al. Multi-agent localization from noisy relative pose measurements[C]//IEEE International Conference on Robotics and Automation. Piscataway, NJ:IEEE Press, 2011:364-369.
[16] KNUTH J, BAROOAH P. Collaborative localization with heterogeneous inter-robot measurements by Riemannian optimization[C]//IEEE International Conference on Robotics and Automation. Piscataway, NJ:IEEE Press, 2013:1534-1539.
[17] SCHUSTER M J, SCHMID K, BRAND C, et al. Distributed stereo vision-based 6D localization and mapping for multi-robot teams[J]. Journal of Field Robotics, 2019, 36(2):305-332.
[18] CUNNINGHAM A, PALURI M, DELLAERT F. DDF-SAM:Fully distributed SLAM using constrained factor graphs[C]//IEEE/RSJ International Conference on Intelligent Robots and Systems. Piscataway, NJ:IEEE Press, 2010:3025-3030.
[19] CUNNINGHAM A, INDELMAN V, DELLAERT F. DDF-SAM 2.0:Consistent distributed smoothing and mapping[C]//IEEE international conference on robotics and automation. Piscataway, NJ:IEEE Press, 2013:5220-5227.
[20] DELLAERT F, KAESS M. Factor graphs for robot perception[J]. Foundations and Trends® in Robotics, 2017, 6(1-2):1-139.
[21] BAROOAH P, DA SILVA N M, HESPANHA J P. Distributed optimal estimation from relative measurements for localization and time synchronization[C]//International Conference on Distributed Computing in Sensor Systems. Berlin, Heidelberg:Springer, 2006:266-281.
[22] CARLONE L, ROSEN D M, CALAFIORE G, et al. Lagrangian duality in 3D SLAM:Verification techniques and optimal solutions[C]//IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). Piscataway, NJ:IEEE Press, 2015:125-132.
[23] DELLAERT F. Factor graphs and GTSAM:A hands-on introduction:GT-RIM-CP&R-2012-002[R]. Atlanta, GA:Georgia Institute of Technology, 2012.
[24] CARLONE L, TRON R, DANIILIDIS K, et al. Initialization techniques for 3D SLAM:A survey on rotation estimation and its use in pose graph optimization[C]//IEEE International Conference on Robotics and Automation (ICRA). Piscataway, NJ:IEEE Press, 2015:4597-4604.
[25] ROSEN D M, CARLONE L, BANDEIRA A S, et al. SE-Sync:A certifiably correct algorithm for synchronization over the special Euclidean group[J]. The International Journal of Robotics Research, 2019, 38(2-3):95-125.
[26] MARTINEC D, PAJDLA T. Robust rotation and translation estimation in multiview reconstruction[C]//IEEE Conference on Computer Vision and Pattern Recognition. Piscataway, NJ:IEEE Press, 2007:1-8.
[27] 李庆扬, 王能超, 易大义. 数值分析[M]. 北京:清华大学出版社, 2008:193-197 LI Q Y, WANG N C, YI D Y. Numerical analysis[M]. Beijing:Tsinghua University Press, 2008:193-197(in Chinese)
[28] STURM J, ENGELHARD N, ENDRES F, et al. A benchmark for the evaluation of RGB-D SLAM systems[C]//IEEE/RSJ International Conference on Intelligent Robots and Systems. Piscataway, NJ:IEEE Press, 2012:573-580.
CJA