基于关键路标增强EKF-SLAM全局优化的方法

摘要

本发明提供一种基于关键路标增强EKF‑SLAM全局优化的方法，包括：机器人的位姿表示为(x，y，θ)，θ为机器人转角；定义在EKF算法中，使用的状态空间为：X＝[x yθmx，1my，1…mx，N my，N]T其中前三项为机器人位姿，后2N项为机器人观测到的路标坐标；其主要改进之处在于，包括以下步骤：步骤S1，构建SLAM里程计模型；得到机器人位姿运动方程；步骤S2，构建SLAM观测模型；计算得到关键路标真实值与观测到的关键路标观测值之间的残差；步骤S3，将所述残差放到下一个运动更新中，进行运动更新；步骤S4，进行EKF算法的优化。本发明使机器人在检测到关键路标时能对自身位姿做出有效的修正。