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

最新推荐文章于 2024-06-28 09:41:49 发布

wlz249

最新推荐文章于 2024-06-28 09:41:49 发布

阅读量673

点赞数 24

文章标签：平面课程设计 matlab

本文链接：https://blog.csdn.net/weixin_66436111/article/details/139983137

版权

💥💥💞💞欢迎来到本博客❤️❤️💥💥

🏆博主优势：🌞🌞🌞博客内容尽量做到思维缜密，逻辑清晰，为了方便读者。

⛳️座右铭：行百里者，半于九十。

📋📋📋本文目录如下：🎁🎁🎁

目录

⛳️赠与读者

💥1 概述

📚2 运行结果

🎉3 参考文献

🌈4 Matlab代码、文档讲解

⛳️赠与读者

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

或许，雨过云收，神驰的天地更清朗.......🔎🔎🔎

💥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部分。

📚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
%不同探测半径