[发明专利]基于关键路标增强EKF-SLAM全局优化的方法

说明书

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

技术领域

本发明涉及机器人自主导航方法，尤其是一种基于关键路标增强EKF-SLAM全局优化的方法。

背景技术

在本发明中，一些名词的解释如下：

SLAM：同时定位与地图构建；

KF：卡尔曼滤波；

EKF：拓展卡尔曼滤波；

EKF-SLAM：基于拓展卡尔曼滤波的同时定位与地图构建；

IMU：惯性测量单元；

GPS：全球定位系统；

SLAM是实现机器人自主导航的重要途径；KF是线性高斯系统的最优无偏估计，是自动驾驶领域常用的数据最优估计算法。而EKF将KF拓展至了非线性系统，用于解决非线性系统的估计问题。

在SLAM系统中，通常使用IMU、激光雷达、GPS或毫米波雷达等传感器来对机器人进行状态估计。而不同的传感器获得的预测结果或测量结果例如速度、转向角度等会因为信号、温度等外界原因产生噪声，往往是不准确的。而EKF为这种带有噪声的，来自不同传感器的数据结果提供了一种相结合的方法。根据对一个特定传感器的信任程度，来更新预测值和测量值之间的可信权重，从而获得相对准确的最佳估计。

EKF-SLAM的缺陷在于对所有的数据都做了正态分布的假设，而现实世界中不管是机器人本身的运动模型还是观测模型，包括传感器的数据，都是带有噪声的。

在现有技术中，仅通过观测值和预测值之间的差，来修正运动方程中的先验估计误差的协方差。然而所有的路标依然是未知的，观测出来的数据仍然会存在较大误差的情况。例如，现实环境中移动机器人的轮胎打滑或人为移动会使得EKF-SLAM的定位产生较大的影响；虽然EKF-SLAM对误差有修正能力，但是出现较大误差时修正时间过长，严重时甚至无法修正。

发明内容

本发明的目的是在于现有技术中存在的不足，提供一种基于关键路标增强EKF-SLAM全局优化的方法，通过在环境中布置少量已知全局坐标的关键路标，来为EKF-SLAM提供先验信息，使机器人在检测到关键路标时能对自身位姿做出有效的修正，可以有效改善机器人在检测到关键路标时对全局地图的优化。

本发明实施例采用的技术方案是：

一种基于关键路标增强EKF-SLAM全局优化的方法，包括：

机器人的位姿表示为(x,y,θ)，θ为机器人转角；

定义在EKF算法中，使用的状态空间为：

X＝[x y θ m_x,1 m_y,1 … m_x,N m_y,N]^T

其中前三项为机器人位姿，后2N项为机器人观测到的路标坐标；

其主要改进之处在于，包括以下步骤：