基于ISRCDKF的移动机器人同时定位与建图研究

为解决移动机器人在同时定位和建图(Simultaneous localization and mapping，SLAM)技术中普遍存在状态精度不高、稳定性差、计算复杂等问题，提出一种基于迭代平方根中心差分卡尔曼滤波(Iterated square root central difference Kalman filter，ISRCDKF)的SLAM自主定位算法，以满足SLAM过程中的实时性、准确性等要求。该算法使用中心差分变换处理SLAM的非线性问题，避免了泰勒公式展开中雅可比矩阵复杂运算；同时在滤波更新过程中，通过直接传递协方差矩阵的平方根因子减少算法的复杂度；在迭代观测更新过程中，使用列文伯格-马夸尔特(Levenberg-Marquardt，L-M)优化方法引入调节参数，实时修正协方差矩阵，达到提高算法精度、增强稳定性的目的。仿真结果表明，在相同的数据模型和噪声环境下，本文提出的ISRCDKF-SLAM算法与基于扩展卡尔曼滤波(Extended Kalman filter，EKF)的SLAM算法、无迹卡尔曼滤波(Unscented Kalman filter，UKF)的SLAM算法和容积卡尔曼滤波(Cubature Kalman filter，CKF)的SLAM算法相比，均方根误差分别降低了47.3%、32.7%和25.0%；与相同计算复杂度的UKF-SLAM算法和CKF-SLAM算法相比，新算法的运行时间分别减少了15.1%和10.8%。将新算法嵌入到移动机器人平台进行现场实验验证，进一步证明了该算法的实用性和有效性。

Simultaneous Localization and Mapping of Mobile Robot Based on ISRCDKF Algorithm

In the simultaneous localization and mapping (SLAM) technology, mobile robots generally has problems such as low state accuracy, poor stability, and complicated calculation, which can not meet the requirements of real-time and accuracy in the SLAM process. In order to improve this problem, an SLAM autonomous positioning algorithm was proposed based on iterated square root central difference Kalman filter (ISRCDKF). The central difference transform was used to deal with the nonlinear problem of SLAM, avoiding complex operations such as Jacobian matrix in the Taylor formula expansion, and directly transmitting the square root factor reduction algorithm of the covariance matrix in the filter update process. In the complexity, the Levenberg-Marquardt (L-M) optimization method was used to introduce the real-time modified covariance matrix of the adjustment parameters in the iterated observation update process to improve the accuracy and stability of the algorithm. The simulation results showed that under the same data model and noise environment, the proposed ISRCDKF-SLAM algorithm was compared with SLAM algorithm based on extended Kalman filter (EKF-SLAM),SLAM algorithm based on unscented Kalman filter (UKF-SLAM) and SLAM algorithm based on cubature Kalman filter (CKF-SLAM), the root mean square error was reduced by 47.3%, 32.7% and 25.0%, respectively. At the same time, compared with the UKF-SLAM algorithm and CKF-SLAM algorithm with the same computational complexity, the running time of the proposed algorithm was reduced by 15.1% and 10.8%, respectively, which proved the effectiveness of the algorithm. Finally, the proposed algorithm was embedded into the mobile robot platform for field experiment verification, which further proved the practicability and effectiveness of the algorithm.