EKF-SLAM算法
在机器人和自主导航领域,准确的测绘和定位对于成功运行至关重要。 在为此目的开发的各种算法中,扩展卡尔曼滤波器 (EKF) 因其在处理与现实世界动态相关的非线性方面的有效性而脱颖而出。 EKF 是卡尔曼滤波器的扩展,可对当前均值和协方差的估计进行线性化,使其适用于需要精度和可靠性的各种应用。
扩展卡尔曼滤波器同步定位与建图 (EKF-SLAM) 算法建立在 EKF 的基础上,集成了 EKF 的功能,可解决机器人技术中最具挑战性的问题之一:同时定位与建图 (SLAM)。 EKF-SLAM采用EKF方法来同时估计机器人的状态和周围环境的布局,而无需事先了解位置。 这种双重功能对于在未知环境中运行的自主系统至关重要,使它们能够在绘制区域地图时有效导航。 EKF-SLAM 算法一直是自动驾驶车辆和机器人系统开发的基石,为处理现实世界环境中固有的不确定性提供了一个强大的框架。
坐标系定义和机器人模型
1. 坐标系定义
在本项目中,我在全局坐标中定义了小车的状态转换模型和观察模型。 在我的定义中,左方向被认为是正方向,车辆在时间 的方向表示为
,定义为车辆的航向轴与全局坐标y轴之间的角度,且向左偏转为正。

2. 汽车状态转移模型

是状态向量
是控制向量
是单位时间内移动的距离,其值为
(速度 x 时间)
是单位时间内车辆的角度变化,也就是舵角
是车辆在时间
时基于y轴正方向的角度,向左偏转为正
(速度)和
(舵角)都包含均值为0,方差为
的高斯白噪声
- 加上
是为了将相对于y轴的角度转化为相对于x轴的角度,保证cos和sin的计算值正确
3. 观测模型

是第
个路标(landmark)和车辆当前坐标的距离
是第
个路标(landmark)和车辆当前坐标的角度差
和
分别是第
个路标(landmark)的真实
和
坐标
和
分别是车辆当前的
和
坐标
是车辆在时间
时基于y轴正方向的角度,向左偏转为正
是均值为0,方差为
的高斯白噪声
- 减去
是为了将相对于x轴的角度转化为相对于y轴的角度,保证
的定义不会出错
EKF-SLAM状态变量定义
1. 系统的状态变量
![]()
和
是第
个被观测到的路标的
和
值,也就是说,此处的坐标和路标的真实坐标在有噪声的情况下是不会完全相同的
2. 系统的控制变量
![]()
= 速度
= 舵角
3. 系统方程
和
分别是模型的过程噪声和测量噪声
- 显然,
和
对应的就是刚刚的车辆状态转移模型和测量模型,而这两个模型也显然是非线性的
对于以上这个非线性系统,直接使用传统的卡尔曼滤波是不行的,因为其只针对线性系统,比如下图:
所以,需要先对非线性系统进行局部的线性化,将线性化周围的系统状态看作是线性系统,这样对于非线性不太强的系统,可以认为线性化点周围的系统状态是满足卡尔曼滤波算法的条件的。而扩展卡尔曼算法(EKF)正是这样的一种算法。
扩展卡尔曼算法所使用的线性化方法为泰勒展开。对于非线性系统
和
进行一阶泰勒展开:


1万+



被折叠的 条评论
为什么被折叠?



