💥💥💞💞欢迎来到本博客❤️❤️💥💥
🏆博主优势:🌞🌞🌞博客内容尽量做到思维缜密,逻辑清晰,为了方便读者。
⛳️座右铭:行百里者,半于九十。
📋📋📋本文目录如下:🎁🎁🎁
目录
⛳️赠与读者
👨💻做科研,涉及到一个深在的思想系统,需要科研者逻辑缜密,踏实认真,但是不能只是努力,很多时候借力比努力更重要,然后还要有仰望星空的创新点和启发点。当哲学课上老师问你什么是科学,什么是电的时候,不要觉得这些问题搞笑。哲学是科学之母,哲学就是追究终极问题,寻找那些不言自明只有小孩子会问的但是你却回答不出来的问题。建议读者按目录次序逐一浏览,免得骤然跌入幽暗的迷宫找不到来时的路,它不足为你揭示全部问题的答案,但若能让人胸中升起一朵朵疑云,也未尝不会酿成晚霞斑斓的别一番景致,万一它居然给你带来了一场精神世界的苦雨,那就借机洗刷一下原来存放在那儿的“躺平”上的尘埃吧。
或许,雨过云收,神驰的天地更清朗.......🔎🔎🔎
💥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
%不同探测半径
🎉3 参考文献
文章中一些内容引自网络,会注明出处或引用为参考文献,难免有未尽之处,如有不妥,请随时联系删除。
🌈4 Matlab代码、文档讲解
资料获取,更多粉丝福利,MATLAB|Simulink|Python资源获取