即时定位与地图构建（SLAM）作为机器人发展的重要技术受到研究者们的广泛关注，后端优化作为SLAM的重要组成部分也被广泛研究。由于后端优化面对大量的待优化数据，优化过程中存在计算不稳定、效率低、系统鲁棒性差等问题，导致优化效果不理想。为了进一步提高优化精度，促进机器人的智能化应用和发展，本文对SLAM后端优化算法进行研究。

本文研究了SLAM后端图优化方法中的光束平差法（BA）。BA求解过程中的LM迭代算法在计算过程中产生的雅可比矩阵可能是奇异的，导致算法存在奇异或病态问题。针对该问题，本文提出一种改进的LM算法，通过定义迭代参数的计算方式，将前一次的迭代结果引入到后一次的迭代计算中，可有效减小因当前解远离解集时函数较大产生的影响，同时可在不假设雅可比矩阵是非奇异的条件下具有二阶收敛性，保证算法稳定的前提下提高计算效率。经实验验证，改进LM算法可有效提高计算效率，在达到相同结果的情况下，其迭代次数相较于LM和C-LM算法减少23.53%和13.33%；与LM-BA和C-LM-BA相比，I-LM-BA对轨迹图优化的误差最小，且优化中的轨迹图毛刺明显减少，稳定性更高。

由于待优化的数据中存在因错误识别等原因造成的异常值，降低优化精度，本文对后端鲁棒性的优化算法进行研究。针对典型算法中存在计算复杂和优化精度低的问题，本文提出一种具有自适应性的动态协方差缩放（ADCS）算法。ADCS算法只需要对节点进行计算，然后根据推导出的计算公式和每次的迭代结果动态更新参数S和Ø的值，可以在简化计算的同时，有效减弱错误闭环对整体优化的影响，实现后端鲁棒性。将ADCS算法应用到不同的数据集上进行仿真实验。结果表明，ADCS算法对添加错误闭环的数据集仍可以完成高精度估计图优化，实现后端鲁棒性；且能够简化计算过程，运行时间相对于DCS和CPS算法，分别减少28.05%和7.65%；并能完成对不同数据集的高精度轨迹图优化，相较于更精确的CPS算法，该算法对RingCity和Manhattan数据集优化误差分别减少11.36%和22.18%。

Simultaneous localization and mapping (SLAM) has received extensive attention from researchers as an important technology for robot development, and back-end optimization as an important component of SLAM has also been widely studied. Since back-end optimization faces a large amount of data to be optimized, the optimization process suffers from computational instability, low efficiency, and poor system robustness, leading to unsatisfactory results. In order to further improve the accuracy and promote the intelligent application and development of robots, this paper investigates the SLAM back-end optimization algorithm.

This paper investigates the bundle adjustment (BA) in the SLAM back-end graph optimization method. The Jacobi matrix generated by the LM iterative method in BA solving process may be singular, resulting in singular or ill-conditioned algorithms. To address this problem, an improved LM algorithm is proposed in this paper. By defining the calculation of the iterative parameters, the previous iteration result is introduced into the next iteration calculation, which can reduce the problem that the function is large when the current solution is far away from the solution set. And at the same time, it can have second-order convergence without assuming that the Jacobi matrix is non-singular, and the computational efficiency is improved while ensuring the stability of the algorithm. Experimental results show that the improved LM algorithm can effectively improve the computational efficiency. Under the condition of achieving the same result, the number of iterations is reduced by 23.53% and 13.33% compared with the LM and C-LM algorithms; Compared with LM-BA and C-LM-BA, I-LM-BA has the smallest error for trajectory map optimization, and the burr of the trajectory graph in optimization is significantly reduced and more stable.

Since there are outliers in the data to be optimized due to misidentification and other reasons, which reduces the optimization accuracy, the optimization algorithm for back-end robustness is investigated in this paper. Aiming at the problems of computational complexity and low optimization accuracy in typical algorithms, an adaptive dynamic covariance scaling (ADCS) algorithm is proposed. The ADCS algorithm only needs to calculate the nodes, and then dynamically update the value of the parameter S and Ø based on the derived calculation formula and each iteration result, which can simplify the calculation while effectively attenuating the impact of the error closed loop on the overall optimization and achieving the robustness of the back end. The ADCS algorithm is applied to different datasets for simulation experiments. The results show that the ADCS algorithm can still complete the high-precision estimation graph optimization for the datasets with error closed loop added, and realize the back-end robustness. It can simplify the calculation process and reduce the running time by 28.05% and 7.65% compared with the DCS and CPS algorithms. It also can complete the high-precision trajectory graph optimization for different datasets, and compared with the more accurate CPS algorithm, this algorithm reduces the optimization errors of RingCity and Manhattan datasets by 11.36% and 22.18%.

[1]Wang X, Nie T, Zhu D. Indoor Robot Path Planning Assisted by Wireless Network[J]. EURASIP Journal on Wireless Communications and Networking, 2019, 2019(1): 1-9.

[2]Scott K, Robin D, Maurice F, et al. Optimization-Based Locomotion Planning, Estimation, and Control Design for the Atlas Humanoid Robot[J]. Autonomous Robots, 2016, 40(3): 429-455.

[3]吴建清, 宋修广. 同步定位与建图技术发展综述[J]. 山东大学学报(工学版), 2021, 51(05): 16-31.

[4]Li F, Chen W, Xu W, et al. A Mobile Robot Visual SLAM System with Enhanced Semantics Segmentation[J]. IEEE Access, 2020, 8: 25442-25458.

[5]Chen Y, Huang S, Fitch R. Active SLAM for Mobile Robots with Area Coverage and Obstacle Avoidance[J]. IEEE/ASME Transactions on Mechatronics, 2020, 25(3): 1182-1192.

[6]高翔, 张涛, 刘毅. 视觉SLAM十四讲: 从理论到实践[M]. 电子工业出版社, 2017.

[7]Tang J, Chen Y, Kukko A, et al. SLAM-Aided Stem Mapping for Forest Inventory with Small-Footprint Mobile Li DAR[J]. Forests, 2015, 6(12): 4588-4606.

[8]Cui L, Ma C. SOF-SLAM: A Semantic Visual SLAM for Dynamic Environments[J]. IEEE access, 2019, 7: 166528-166539.

[9]赵挽东. 复杂场景下机器人SLAM算法研究[D]. 哈尔滨: 哈尔滨工程大学, 2019.

[10]Wang H, Zhang C, Song Y, et al. Robot SLAM with Ad hoc Wireless Network Adapted to Search and Rescue Environments[J]. Journal of Central South University, 2018, 25(12): 3033-3051.

[11]Bustos Á P, Chin T J, Eriksson A, et al. Visual SLAM: Why Bundle Adjust?[C] //2019 international Conference on Robotics and Automation (ICRA). IEEE, 2019: 2385-2391.

[12]邓琳蔚, 王治, 张梦梦, 等. 一种基于光束平差法的相机标定方法[J]. 兵工自动化, 2020, 39(02): 8-13.

[13]周治国, 曹江微, 邸顺帆. 3D激光雷达SLAM算法综述[J]. 仪器仪表学报, 2021, 42(09): 13-27.

[14]张毅, 陈起, 罗元. 室内环境下移动机器人三维视觉SLAM[J]. 智能系统学报, 2015, 10(4): 615-619.

[15]Liu Y, Zhu D, Peng J, et al. Real-time Robust Stereo Visual SLAM System Based on Bionic Eyes[J]. IEEE Transactions on Medical Robotics and Bionics, 2020, 2(3): 391-398.

[16]王录涛, 吴林峰. 基于图优化的视觉SLAM研究进展与应用分析[J]. 计算机应用研究, 2020, 37(01): 9-15.

[17]王柯赛, 姚锡凡, 黄宇, 等. 动态环境下的视觉SLAM研究评述[J]. 机器人, 2021, 43(06): 715-732.

[18]Davison A J , Reid I D, Molton N D, et al. Mono SLAM: Real-Time Single Camera SLAM[J]. IEEE Transactions on Pattern Analysis and Machine Intelligence, 2007, 29(6): 1052-1067.

[19]Eade E, Drummond T. Scalable Monocular SLAM[C]//2006 IEEE Computer Society Conference on Computer Vision and Pattern Recognition (CVPR'06). IEEE, 2006, 1: 469-476.

[20]Li X, Zhang Y, Yang F, et al. A Rectification Strategy for State Estimation of Qudarotors Based on Parallel Tracking and Mapping[C]//2015 IEEE International Conference on Information and Automation. IEEE, 2015: 3046-3050.

[21]Mur-Artal R,Montiel J M M, Tardos J D. ORB-SLAM: A Versatile and Accurate Monocular SLAM System[J]. IEEE Transactions on Robotics, 2015. 31(5): 1147-1163.

[22]Li H, Zhang Q, Zhao D. Comparison of Methods to Efficient Graph SLAM under General Optimization Framework[C]//2017 32nd Youth Academic Annual Conference of Chinese Association of Automation (YAC). IEEE, 2017: 321-326.

[23]Mur-Artal R, Tardós J D. ORB-SLAM2:An Open-Source SLAM System for Monocular Stereo and RGB-D Cameras[J].IEEE Transactions on Robotics, 2017. 33(5): 1255-1262.

[24]Tang F, Li H, Wu Y. FMD Stereo SLAM: Fusing MVG and Direct Formulation Towards Accurate and Fast Stereo SLAM[C]//2019 International Conference on Robotics and Automation (ICRA). IEEE, 2019: 133-139.

[25]周彦, 李雅芳, 王冬丽, 等. 视觉同时定位与地图创建综述[J]. 智能系统学报, 2018, 13(1): 97-106.

[26]Wolf Á, Troll P, Romeder-Finger S, et al. A Benchmark of Popular Indoor 3D Reconstruction Technologies: Comparison of ARCore and RTAB-Map[J]. Electronics, 2020, 9(12): 2091.

[27]Dehghannasiri R, Esfahani M S, Qian X, et al. Optimal Bayesian Kalman Filtering with Prior Update[J]. IEEE Transactions on Signal Processing, 2018, 66(8): 1982-1996.

[28]芦宝娟, 赵大磊, 徐广允. 改进卡尔曼滤波的井下组合导航定位算法[J]. 计算机应用与软件, 2022, 39(03): 139-145.

[29]Menegaz H M T, Ishihara J Y, Kussaba H T M. Unscented Kalman Filters for Riemannian State-Space Systems[J]. IEEE Transactions on Automatic Control, 2019, 64(4):1487-1502.

[30]杨傲雷, 金宏宙, 陈灵, 等. 融合深度学习与粒子滤波的移动机器人重定位方法[J]. 仪器仪表学报, 2021, 42(07): 226-233.

[31]Deng X, Mousavian A, Xiang Y, et al. PoseRBPF: A Rao-Blackwellized Particle Filter for 6-D Object Pose Tracking[J]. IEEE Transactions on Robotics, 2021, 37(5), 1328-1342.

[32]罗景文, 秦世引. 基于智能优化箱粒子滤波的移动机器人FastSLAM[J]. 北京航空航天大学学报, 2022, 48(01): 53-66.

[33]Ouabi O L, Pomarede P, Geist M, et al. A FastSLAM Approach Integrating Beamforming Maps for Ultrasound-based Robotic Inspection of Metal Structures[J]. IEEE Robotics and Automation Letters, 2021, 6(2): 2908-2913.

[34]李二超, 高振磊. 改进粒子速度和位置更新公式的粒子群优化算法[J]. 南京师大学报(自然科学版), 2022, 45(01): 118-126.

[35]Ye Z, Wang T, Wen Q, et al. Improved Rao-Blackwellised Particle Filter based on Randomly Weighted Particle Swarm Optimization[J]. Computers & Electrical Engineering, 2018, 71: 477-484.

[36]Zhang Q, Wang P, Chen Z. An Improved Particle Filter for Mobile Robot Localization Based on Particle Swarm Optimization[J]. Expert Systems with Applications, 2019, 135(NOV.):181-193.

[37]崔昊杨, 张宇, 周坤, 等. 基于仿生算法改进粒子滤波的SLAM算法精度预测[J]. 控制与决策, 2021, 36(01): 166-172.

[38]李维刚, 李阳, 赵云涛, 等. 基于改进灰狼算法的粒子滤波算法研究[J]. 系统仿真学报, 2021, 33(01): 37-45.

[39]周宁亚, 黄友锐, 韩涛. 基于狮群优化的FastSLAM算法[J]. 计算机应用与软件, 2020, 37(07): 206-211.

[40]孙弋,张笑笑.结合退火优化和遗传重采样的RBPF算法[J].西安科技大学学报, 2020, 40(2): 349-355.

[41]王忠立, 赵杰, 蔡鹤皋. 大规模环境下基于图优化SLAM的图构建方法[J]. 哈尔滨工业大学学报, 2015, 47(01): 75-85.

[42]Li Y, Fan S, Sun Y, et al. Bundle Adjustment Method Using Sparse BFGS Solution[J]. Remote Sensing Letters, 2018, 9(8): 789-798.

[43]Zhang L, Koch R. Structure and Motion from Line Correspondences: Representation, Projection, Initialization and Sparse Bundle Adjustment[J]. Journal of Visual Communication and Image Representation, 2014, 25(5): 904-915.

[44]Liu K, Sun H, Ye P. Research on Bundle Adjustment for Visual SLAM under Large-scale Scene[C]//2017 4th International Conference on Systems and Informatics (ICSAI), 2017: 220-224.

[45]Peng J, Liu J, Wei H. A Scalable Parallel Preconditioned Conjugate Gradient Method for Bundle Adjustment[J]. Applied Intelligence, 2022, 52(1): 753-765.

[46]Peng J, Liu J, Wei H. A Compressed Matrix Sequence Method for Solving Normal Equations of Bundle adjustment[J]. Machine Vision and Applications, 2021, 32(4): 1-10.

[47]赵帅华, 李言言, 曹健, 等. 基于BFGS修正的高斯牛顿光束法平差解算方法[J]. 北京大学学报(自然科学版), 2020, 56(06): 1013-1019.

[48]朱子健, 张贵阳, 杨明, 等. 基于法化矩阵降维的多相机快速光束法平差算法[J]. 红外与激光工程, 2021, 50(02): 263-271.

[49]Amini K, Rostami F, Caristi G. An efficient Levenberg–Marquardt Method with a New LM Parameter for Systems of Nonlinear Equations[J]. Optimization, 2018, 67(5): 637-650.

[50]Fan J, Huang J, Pan J. An Adaptive Multi-step Levenberg–Marquardt Method[J]. Journal of Scientific Computing, 2019, 78(1): 531-548.

[51]Chen L, Ma Y. Shamanskii-like Levenberg-Marquardt Method with a New Line Search for Systems of Nonlinear Equations[J]. Journal of Systems Science and Complexity, 2020, 33(5): 1694-1707.

[52]Wang H, Fan J. Convergence Rate of the Levenberg-Marquardt Method under Hölderian Local Error Bound[J]. Optimization Methods and Software, 2020, 35(4): 767-786.

[53]王丽星, 曹付元. 基于Huber损失的非负矩阵分解算法[J]. 计算机科学, 2020, 47(11): 80-87.

[54]段菲, 王慧敏, 张超. 面向数据表示的Cauchy非负矩阵分解[J]. 计算机科学, 2021, 48(06): 96-102.

[55]Bai F, Vidal-Calleja T, Huang S. Robust Incremental SLAM Under Constrained Optimization Formulation[J]. IEEE Robotics and Automation Letters, 2018, 3(2): 1207-1214.

[56]Jackson J, Brink K, Forsgren B, et al. Direct Relative Edge Optimization, A Robust Alternative for Pose Graph Optimization[J]. IEEE Robotics and Automation Letters, 2019, 4(2): 1932-1939.

[57]Agarwal S, Parunandi K S, Chakravorty S. Robust Pose-Graph SLAM Using Absolute Orientation Sensing[J]. IEEE Robotics and Automation Letters, 2019, 4(2): 981-988.

[58]Wu F, Beltrame G. Cluster-based Penalty Scaling for Robust Pose Graph Optimization[J]. IEEE Robotics and Automation Letters, 2020, 5(4): 6193-6200.

[59]Rahman I M H, Hollitt C, Zhang M. Zhang. Feature Map Quality Score Estimation Through Regression[J]. IEEE Transactions on Image Processing, 2018, 27(4): 1793-1808.

[60]Li Z, Wegner J D, Lucchi A. Topological Map Extraction from Overhead Images[C]//Proceedings of the IEEE International Conference on Computer Vision. 2019: 1715-1724.

[61]Yoo W, Kim H, Hong H, et al. Scan Similarity-based Pose Graph Construction Method for Graph SLAM[C]//2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2018: 131-136.

[62]Ma C, Liu X, Wen Z. Globally Convergent Levenberg-Marquardt Method for Phase Retrieval[J]. IEEE Transactions on Information Theory, 2019, 65(4):, 2343-2359.

[63]史加荣, 王丹, 尚凡华, 等. 随机梯度下降算法研究进展[J]. 自动化学报, 2021, 47(09): 2103-2119.

[64]万琴, 颜金娥, 李智, 等. 基于改进RANSAC算法的全景图像拼接技术[J]. 光电子·激光, 2021, 32(12): 1253-1261.