💥💥💞💞欢迎来到本博客❤️❤️💥💥
🏆博主优势:🌞🌞🌞博客内容尽量做到思维缜密,逻辑清晰,为了方便读者。
⛳️座右铭:行百里者,半于九十。
📋📋📋本文目录如下:🎁🎁🎁
目录
⛳️赠与读者
👨💻做科研,涉及到一个深在的思想系统,需要科研者逻辑缜密,踏实认真,但是不能只是努力,很多时候借力比努力更重要,然后还要有仰望星空的创新点和启发点。当哲学课上老师问你什么是科学,什么是电的时候,不要觉得这些问题搞笑。哲学是科学之母,哲学就是追究终极问题,寻找那些不言自明只有小孩子会问的但是你却回答不出来的问题。建议读者按目录次序逐一浏览,免得骤然跌入幽暗的迷宫找不到来时的路,它不足为你揭示全部问题的答案,但若能让人胸中升起一朵朵疑云,也未尝不会酿成晚霞斑斓的别一番景致,万一它居然给你带来了一场精神世界的苦雨,那就借机洗刷一下原来存放在那儿的“躺平”上的尘埃吧。
或许,雨过云收,神驰的天地更清朗.......🔎🔎🔎
💥1 概述
该程序依赖于扫描匹配过程,没有后端优化和回环检测支持。在计算位姿时,首先通过估计(采用匀速运动模型),然后使用贪心算法在估计的位姿附近进行优化,目标是最大化局部图像与当前扫描数据的相似度。程序的主要执行步骤如下:
1. **初始化**:使用匀速运动模型估计初始位姿。
2. **扫描匹配**:将当前扫描数据与局部地图进行匹配,找到最佳位姿。
3. **贪心优化**:在估计的位姿附近,通过贪心算法优化位姿,以最大化局部图像与当前扫描数据的匹配度。
4. **更新地图**:根据优化后的位姿更新局部地图。
该过程重复进行,处理新的扫描数据并逐步构建地图。然而,由于缺乏后端优化和回环检测,程序的建图精度可能受到局部优化和误差累积的限制。
该程序实现了一个基本的扫描匹配算法,并采用贪心算法进行位姿优化。然而,由于贪心算法的局限性,位姿估计在许多情况下都不够精确,特别是在数据量增加时,容易出现显著的误差点和重影点。这些误差会导致生成的地图精度不高,甚至出现严重的失真。与目前广泛使用的主流激光SLAM算法如Cartographer相比,本程序的建图效果显得相当逊色。Cartographer等先进算法通过更加复杂和鲁棒的优化策略,能够在更大范围内提供更精确的位姿估计和地图构建,显著减少错误点和重影点。然而,本程序作为入门级工具,具有教育意义。它简化了激光SLAM的算法流程,使用户能够轻松上手,理解基本原理和步骤,为进一步学习和应用更复杂的SLAM算法打下基础。通过该程序,用户可以逐步熟悉激光SLAM的核心概念,如扫描匹配、位姿优化、误差处理等,为将来在实际应用中实现更高级的SLAM算法奠定理论和实践基础。
📚2 运行结果
主函数代码:
%主函数
clear; close all; clc;
cfig = figure(1);
%cfig = figure('Position', [10,10,1280,1080]);
% 激光雷达的传感器参数
lidar = SetLidarParameters();
% 地图参数
borderSize = 1; % 边界尺寸
pixelSize = 0.2; % 栅格地图的一个单元的边长 对应 实际距离pixelSize米(这里设置为0.2米)
miniUpdated = false; %
miniUpdateDT = 0.1; % 单位m 若机器人在x方向或y方向移动超过miniUpdateDT 则更新位姿
miniUpdateDR = deg2rad(5); % 单位rad 若机器人旋转超过miniUpdateDR 则更新位姿
% 如果机器人从最后一次键扫描移动了0.1米或旋转了5度,我们将添加一个新的键扫描并更新地图
% 扫描匹配参数
fastResolution = [0.05; 0.05; deg2rad(0.5)]; % [m; m; rad]的分辨率
bruteResolution = [0.01; 0.01; deg2rad(0.1)]; % not used
% 读取激光雷达数据
%lidar_data = load('horizental_lidar.mat');
lidar_data = load('new_laser_data.mat');
N = size(lidar_data.timestamps, 1);%扫描次数(控制下面的循环次数)
% 构造一个空全局地图
map.points = [];%地图点集
map.connections = [];
map.keyscans = [];%keyscans保存当前正确位姿的扫描数据 如果预测得到的下一位姿出现错误 则返回到距其最近的前一位姿重新计算
pose = [0; 0; 0];%初始位姿为(x=0,y=0,theta=0)
path = pose;%位姿并置构成路径
%是否将绘制过程保存成视频
saveFrame=0;
if saveFrame==1
% 视频保存文件定义与打开
writerObj=VideoWriter('SLAMprocess.avi'); % 定义一个视频文件用来存动画
open(writerObj); % 打开该视频文件
end
% Here we go!!!!!!!!!!!!!!!!!!!!
for scanIdx = 1 : 1 : N
disp(['scan ', num2str(scanIdx)]);% 显示当前处理数据索引
% 得到当前的扫描 [x1,y1; x2,y2; ...]
%time = lidar_data.timestamps(scanIdx) * 1e-9;%时间设置成每1e-9扫描一次
scan = ReadAScan(lidar_data, scanIdx, lidar, 24);%得到该次扫描数据的局部笛卡尔坐标
% 如果是第一次扫描 则初始化
if scanIdx == 1
map = Initialize(map, pose, scan);%把扫描数据scan坐标 通过位姿pose 转换为全局地图map坐标
miniUpdated = true;
continue;
end
% 1. 如果我们在最后一步执行了 mini更新,我们将更新 局部点集图 和 局部栅格地图(粗略)
% 1. If we executed a mini update in last step, we shall update the local points map and local grid map (coarse)
if miniUpdated
localMap = ExtractLocalMap(map.points, pose, scan, borderSize);%得到当前数据附近的地图点
gridMap1 = OccuGrid(localMap, pixelSize);%从点集localMap 栅格单元尺寸对应实际长度以pixelSize 创建占用栅格地图
gridMap2 = OccuGrid(localMap, pixelSize/2);%从点集localMap 栅格单元尺寸对应实际长度以pixelSize/2 创建占用栅格地图,更精细的地图
end
% 2. 使用恒定速度运动模型预测当前位姿(即用前一状态到本状态的过程 作为本状态到下一状态的过程 从而由本状态预测下一状态)
if scanIdx > 2
pose_guess = pose + DiffPose(path(:,end-1), pose);%预测下一位姿=当前位姿+(当前位姿与上一位姿的差) pose是一个全局坐标
else
pose_guess = pose;
end
% 3. 快速匹配 当前帧--扫描匹配--
if miniUpdated
[pose, ~] = FastMatch(gridMap1, scan, pose_guess, fastResolution);%根据当前栅格地图 优化 预测的下一位姿
else
[pose, ~] = FastMatch(gridMap2, scan, pose_guess, fastResolution);
end
% 4. 使用较高的分辨率再细化 预测下一位姿
% gridMap = OccuGrid(localMap, pixelSize/2);
[pose, hits] = FastMatch(gridMap2, scan, pose, fastResolution/2);%返回进一步更新的下一位姿pose
% 如果机器人移动了一定距离,则执行mini更新
dp = abs(DiffPose(map.keyscans(end).pose, pose));%两次位姿的差
if dp(1)>miniUpdateDT || dp(2)>miniUpdateDT || dp(3)>miniUpdateDR
miniUpdated = true;
[map, pose] = AddAKeyScan(map, gridMap2, scan, pose, hits,...
pixelSize, bruteResolution, 0.1, deg2rad(3));
else
miniUpdated = false;
end
path = [path, pose]; %把当前位姿pose 并入路径path
% ===== Loop Closing =========================================
% if miniUpdated
% if TryLoopOrNot(map)
% map.keyscans(end).loopTried = true;
% map = DetectLoopClosure(map, scan, hits, 4, pi/6, pixelSize);
% end
% end
%----------------------------------------------------------------------
% 绘图
if mod(scanIdx, 30) == 0%每30步画一次图
PlotMap(cfig, map, path, scan, scanIdx);
%获取视频帧并保存成视频
if saveFrame==1
frame = getframe(cfig);
writeVideo(writerObj, frame);
end
end
end
if saveFrame==1
close(writerObj); %关闭视频文件句柄
end
%对图像进行分类分析
figure(2);
subplot(1,2,1);%地图
world = map.points;
plot(world(:,1), world(:,2), '+', 'MarkerSize', 1, 'color', [0,0,0]);%画全局地图点集
subplot(1,2,2);%路径
plot(path(1,:), path(2,:), '-.', 'LineWidth', 1);
🎉3 参考文献
文章中一些内容引自网络,会注明出处或引用为参考文献,难免有未尽之处,如有不妥,请随时联系删除。
🌈4 Matlab代码实现
资料获取,更多粉丝福利,MATLAB|Simulink|Python资源获取