当前位置：首页 > news >正文

【EKF实现2维平面上的SLAM】【EKF-SLAM】NWPU 最优估计课程设计（Matlab代码实现）

news 2026/6/14 19:31:16

💥💥💞💞欢迎来到本博客❤️❤️💥💥
🏆博主优势：🌞🌞🌞博客内容尽量做到思维缜密，逻辑清晰，为了方便读者。
⛳️座右铭：行百里者，半于九十。

⛳️赠与读者

👨‍💻做科研，涉及到一个深在的思想系统，需要科研者逻辑缜密，踏实认真，但是不能只是努力，很多时候借力比努力更重要，然后还要有仰望星空的创新点和启发点。当哲学课上老师问你什么是科学，什么是电的时候，不要觉得这些问题搞笑。哲学是科学之母，哲学就是追究终极问题，寻找那些不言自明只有小孩子会问的但是你却回答不出来的问题。建议读者按目录次序逐一浏览，免得骤然跌入幽暗的迷宫找不到来时的路，它不足为你揭示全部问题的答案，但若能让人胸中升起一朵朵疑云，也未尝不会酿成晚霞斑斓的别一番景致，万一它居然给你带来了一场精神世界的苦雨，那就借机洗刷一下原来存放在那儿的“躺平”上的尘埃吧。
或许，雨过云收，神驰的天地更清朗.......🔎🔎🔎

💥1 概述

运动更新

运动更新时，固定点坐标不变，所以只需要根据运动方程更新(x,y,α)及其协方差与互协方差即可。

观测到曾经观测过的固定点

这时候依次对观测到的特征点信息进行EKF更新

Landmark observations are processed in the EKF usually one-by-one

观测到新的固定点

这时候观测到新的固定点，需要进行状态增广。根据逆观测方程，使用观测信息推测出新加的增广状态均值与方差，然后加入到总体的状态与协方差矩阵中。

文章目录：

同时定位与建图（SLAM）的本质是一个估计问题，它要求移动机器人利用传感器信息实

时地对外界环境结构进行估计，并且估算出自己在这个环境中的位置，Smith 和 Cheeseman

在上个世纪首次将 EKF 估计方法应用到 SLAM。

以滤波为主的 SLAM 模型主要包括三个方程：

1）运动方程：它会增加机器人定位的不确定性

2）根据观测对路标初始化的方程：它根据观测信息，对新的状态量初始化。

3）路标状态对观测的投影方程：根据观测信息，对状态更新，纠正，减小不确定度。

详细文档见第4部分。

EKF实现2维平面上的SLAM——NWPU最优估计课程设计研究

一、引言

SLAM（Simultaneous Localization and Mapping，即同时定位与建图）是机器人学和人工智能领域的一个重要研究方向。它旨在让机器人或移动设备在未知环境中，通过传感器数据进行自定位，并构建环境地图。EKF-SLAM（扩展卡尔曼滤波器同步定位与建图）算法是解决SLAM问题的一种有效方法，它利用扩展卡尔曼滤波器（EKF）处理非线性问题，以估计机器人的位置和地图的状态。

二、EKF-SLAM算法原理

1. 扩展卡尔曼滤波器（EKF）基础

卡尔曼滤波是一种有效的递归滤波器，能够从一系列含有噪声的测量中估计动态系统的状态。EKF是其非线性版本，适用于处理系统模型非线性的情况。在EKF中，系统的非线性模型通过雅可比矩阵线性化处理，然后应用标准卡尔曼滤波器的框架。

2. EKF-SLAM状态向量构建

在EKF-SLAM中，状态向量通常包括机器人的位姿（位置和方向）以及环境中特征点的位置。状态向量的准确构建是保证EKF-SLAM性能的关键因素之一。

3. EKF-SLAM观测模型

观测模型描述了传感器测量值与状态向量之间的关系。在EKF-SLAM中，常见的传感器观测模型包括距离和角度测量。噪声分析是为了理解和建模观测值中的随机误差，这对于更新过程中的权重分配至关重要。

4. EKF-SLAM工作流程

初始化：设定机器人的初始状态和协方差矩阵，初始化地图为空。
运动预测：根据机器人的控制输入和运动模型，预测机器人下一时刻的位姿变化，同时更新地图特征点的位置预测值，以及状态向量的协方差矩阵。
测量更新：获取传感器测量数据，将测量值与预测值进行匹配。对于已有的地图特征点，计算测量值与预测值之间的误差，并通过卡尔曼增益更新状态向量和协方差矩阵。对于新发现的地图特征点，将其添加到状态向量中，并初始化其协方差。
重复迭代：不断重复运动预测和测量更新步骤，随着机器人的移动和传感器数据的积累，逐渐构建出准确的地图，并估计出机器人的状态。

三、NWPU最优估计课程设计实现

1. 系统初始化

初始状态设定：设定机器人的初始位姿（位置和方向）以及初始协方差矩阵。初始协方差矩阵表达了状态估计的不确定性，通常设置得较为宽泛以反映初期知识的缺乏。
地图初始化：初始化一个空的地图，用于存储后续观测到的特征点位置。

2. 运动模型实现

运动方程构建：根据机器人的运动特性（如轮式机器人的差速运动模型），构建运动方程。运动方程用于预测机器人下一时刻的位姿变化。
协方差传播：在运动预测过程中，协方差矩阵需要根据运动方程进行更新，以反映状态估计的不确定度变化。

3. 观测模型实现

观测方程构建：根据传感器的特性（如激光雷达的距离和角度测量模型），构建观测方程。观测方程用于描述传感器测量值与状态向量之间的关系。
数据关联处理：在测量更新过程中，需要解决数据关联问题，即确定新观测到的特征点与已有地图中的哪一点对应。常见的数据关联策略包括最近邻法、联合概率数据关联和多假设跟踪等。

4. EKF更新实现

预测阶段：根据运动方程预测下一时刻的状态向量和协方差矩阵。
更新阶段：根据观测方程和新的传感器数据，计算测量值与预测值之间的误差，并通过卡尔曼增益更新状态向量和协方差矩阵。

5. 地图构建与维护

特征点添加：当观测到新的地图特征点时，将其添加到状态向量中，并初始化其协方差。
地图优化：随着机器人的移动和传感器数据的积累，地图特征点的位置需要不断更新和优化，以减少累积误差。

四、实验结果与分析

1. 实验环境设置

机器人模型：采用轮式机器人模型进行仿真实验。
传感器配置：配置激光雷达作为主要传感器，用于获取环境信息。
环境设置：在仿真环境中设置多个特征点作为地图的初始结构。

2. 实验结果展示

定位精度：通过对比机器人的真实位姿和EKF-SLAM算法估计的位姿，评估定位精度。实验结果表明，EKF-SLAM算法能够有效地估计机器人的位姿，定位误差在可接受范围内。
地图构建质量：通过观察构建的地图特征点位置与真实环境中的特征点位置之间的匹配程度，评估地图构建质量。实验结果表明，EKF-SLAM算法能够逐步构建出准确的环境地图。

3. 结果分析

算法性能：EKF-SLAM算法在定位精度和地图构建质量方面表现出色，能够满足机器人自主导航的需求。
影响因素：实验结果表明，传感器的精度、数据关联策略的选择以及地图优化算法的性能等因素都会影响EKF-SLAM算法的性能。

📚2 运行结果

传感器探测范围与路标点

第一次状态增广
状态持续扩大

状态增广已停止

部分代码：

% 运动噪声
q = [0.01;0.01];
Q = diag(q.^2);
% 测量噪声
m = [.15; 1*pi/180];
M = diag(m.^2);

% R: 机器人初始位置
% u: 控制量
R = [0;-2.2;0];
u = [0.1;0.05];

% 设置外界路标点环境
% 环形摆放的landmarks
% W: 设置所有路标点位置
jiaodu_perLandMark =6; %取1,3,6,15,30,60...(360的倍数均可)
r1=2;
r2=3;
r3=3.5;
W = landmarks(r1,r2,r3,jiaodu_perLandMark);

% 传感器探测半径
sensor_r = 2.5;

% Id容器用来判别当前探测到的路标点曾经是否被观测过；若没有观测过，那么此时需要将其加入Id容器。
% 这里使用W中每个点的索引作为路标点的id；Id初始化为一个足够大的零数组即可。
% Id(类型)==1，表示曾经观测过；Id(类型)==0，表示曾经没有观测过。
% 如果用c++实现，建议使用map结构。
Id = zeros(1,size(W,2));

% y_news表示当前新探测到的路标点，y_news(:,i)记录观测量和路标点类型
% 同理y_olds
y_olds = zeros(3,size(W,2));
y_news = zeros(3,size(W,2));

% 状态量及协方差初始化
x = zeros(numel(R)+numel(W), 1);
P = zeros(numel(x),numel(x));

% id_to_x_map：id------>>>id对应的状态变量在x中的位置
id_to_x_map = zeros(1,size(W,2));

% x和P初始化
r = [1 2 3];
x(r) = R;
%x(r) = [8;-2.5;0];
P(r,r) = 0;

% 每次状态增广在x中的位置
s = [4 5];

%主循环次数
% 125/每圈
loop =250;

% 存放位姿仿真量
poses_ = zeros(3,loop);

% 存放位姿历史估计量
poses = zeros(3,loop);

% 绘图
mapFig = figure(1);
cla;
axis([-5 5 -5 5])
axis square
%axis equal
% 所有路标点
WG = line('parent',gca,...
'linestyle','none',...
'marker','.',...
'color','m',...
'xdata',W(1,:),...
'ydata',W(2,:));
% 仿真下机器人位置
RG = line('parent',gca,...
'marker','+',...
'color','r',...
'xdata',R(1),...
'ydata',R(2));
% 估计的机器人位置
rG = line('parent',gca,...
'linestyle','none',...
'marker','+',...
'color','b',...
'xdata',x(r(1)),...
'ydata',x(r(2)));
% 估计的路标点位置
lG = line('parent',gca,...
'linestyle','none',...
'marker','+',...
'color','k',...
'xdata',[],...
'ydata',[]);

% 估计的路标点协方差
eG1 = zeros(1,size(W,2));
for i = 1:numel(eG1)
eG1(i) = line(...
'parent', gca,...
'color','k',...
'xdata',[],...
'ydata',[]);
end

% 估计的机器人位置
reG = line(...
'parent', gca,...
'color','r',...
'xdata',[],...
'ydata',[]);

% 传感器探测范围（以真实位置为圆心）
sensor1 = line(...
'parent', gca,...
'color','m',...
'xdata',[],...
'ydata',[],...
'LineStyle','--');
sensor2 = line(...
'parent', gca,...
'color','m',...
'xdata',[],...
'ydata',[],...
'LineStyle','--');

%传感器探测范围（以估计位置为圆心）
Sensor1 = line(...
'parent', gca,...
'color','m',...
'xdata',[],...
'ydata',[],...
'LineStyle','--');
Sensor2 = line(...
'parent', gca,...
'color','m',...
'xdata',[],...
'ydata',[],...
'LineStyle','--');

true_pose = line(...
'parent', gca,...
'color','r',...
'xdata',[],...
'ydata',[],...
'LineWidth',0.8);
%'LineStyle','--');

estimate_pose = line(...
'parent', gca,...
'color','b',...
'xdata',[],...
'ydata',[],...
'LineWidth',0.8);
% 'LineStyle','--');

% II. 主循环；
% 机器人每前进一步，循环一次
for t = 1:loop
% if t == 125
% u(1) = 0.2;
% sensor_r = 4;
% end
% if t == 375
% u(1) = 0.2;
% sensor_r = 5;
% end
%不同探测半径