【使用激光雷达传感器和角点提取的EKF-SLAM】SLAM导航使用激光雷达2D传感器来感知墙壁，并使用分割和合并算法以及LSM进行直线估计来提取角点（Matlab代码实现）

Ps.729

于 2025-05-12 09:42:33 发布

阅读量571

点赞数 15

文章标签：算法 matlab 数据结构

本文链接：https://blog.csdn.net/weixin_67304359/article/details/147888421

版权

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

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

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

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

目录

⛳️赠与读者

💥1 概述

基于激光雷达2D传感器与角点提取的EKF-SLAM导航系统研究

1. 引言

2. 激光雷达2D传感器的核心作用

3. 分割与合并算法的数据处理

4. 最小二乘法（LSM）在直线估计中的应用

5. 角点提取的关键技术

6. EKF-SLAM的核心流程

7. 系统整合与实现案例

8. 挑战与优化方向

9. 结论

📚2 运行结果

🎉3 参考文献

🌈4 Matlab代码实现

⛳️赠与读者

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

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

💥1 概述

基于激光雷达2D传感器与角点提取的EKF-SLAM导航系统研究

1. 引言

EKF-SLAM（扩展卡尔曼滤波同步定位与建图）是机器人导航中的经典方法，结合传感器数据与概率模型实现环境建模与自主定位。本文聚焦于利用2D激光雷达、分割合并算法、最小二乘法（LSM）直线估计及角点提取技术构建的EKF-SLAM系统，旨在提升定位精度与地图构建效率。

2. 激光雷达2D传感器的核心作用

2D激光雷达通过发射激光束获取环境轮廓的二维点云数据，其高覆盖率和精准测距特性使其成为室内机器人（如扫地机器人）的核心传感器。其优势包括：

实时性：单线激光雷达可快速扫描平面信息，适用于平面导航。
低成本与适应性：低线束激光雷达在商用服务机器人中广泛应用，如迎宾、运输等场景。
数据预处理：原始点云需经过滤波、降噪等处理，为后续特征提取提供高质量输入。

3. 分割与合并算法的数据处理

分割算法将点云划分为独立线段，常用方法包括：

基于距离的分割：相邻点间距超过阈值则分割为不同线段。
基于角度的分割：点云方向突变时分割，保留局部几何一致性。

合并算法用于优化过度分割，常见策略包括：

直线拟合：通过LSM或RANSAC合并相邻线段。
角度合并：若相邻线段夹角小于阈值，则合并为单一特征。

分治策略（D&C SLAM） ：将地图分割为局部子图后合并，显著降低计算复杂度（从O(n³)降至O(n²)）。例如，D&C SLAM通过二叉合并策略优化全局地图构建，时间效率优于传统EKF。

4. 最小二乘法（LSM）在直线估计中的应用

LSM通过最小化点到直线的距离平方和拟合直线参数，是激光雷达数据处理的基础方法：

优势：在数据符合正态分布时，LSM可快速获得最优解。
局限性：对噪声或偏移敏感，需结合RANSAC或概率线性方法（PLM）提升鲁棒性。
多帧优化：利用多帧数据实时优化直线参数，增强环境模型的稳定性。

5. 角点提取的关键技术

角点作为环境中的显著特征，可提升SLAM的数据关联精度：

传统方法：
- Harris角点：基于自相关矩阵检测角点，具备旋转不变性。
- Shi-Tomasi角点：通过最小特征值阈值筛选，鲁棒性优于Harris。
快速算法：
- FAST角点：通过邻域像素灰度差异快速检测，适用于实时系统。
- ORB特征：结合FAST角点与BRIEF描述子，支持尺度与旋转不变性。
融合策略：结合线段交点检测与角点算法，增强复杂环境下的特征提取能力。

6. EKF-SLAM的核心流程

EKF-SLAM通过状态向量（机器人位姿+环境特征）与协方差矩阵实现联合估计：

预测步骤：基于运动模型更新机器人位姿预测值。
观测步骤：利用激光雷达数据提取特征（直线、角点），构建观测模型。
数据关联：匹配当前观测与地图特征，常用RJC（随机联合兼容性）算法提升效率。
更新步骤：通过卡尔曼增益修正状态估计与协方差矩阵，减少不确定性。

优化策略：

多传感器融合：结合IMU或超声波传感器补偿激光雷达的局限性。
一致性维护：D&C SLAM通过局部子图限制线性化误差，提升全局一致性。

7. 系统整合与实现案例

典型系统架构（以文献为例）：

数据采集：2D激光雷达获取点云数据。
特征提取：
- 分割合并算法生成线段。
- LSM拟合直线参数，结合线段交点提取角点。
EKF-SLAM更新：
- 角点与直线作为观测输入，更新状态向量。
- 分治策略优化大规模地图构建效率。
地图输出：生成2D栅格地图或3D八叉树地图。

实验结果：

在室内结构化环境中，融合角点的EKF-SLAM定位误差降低30%以上。
D&C SLAM在维多利亚公园数据集上表现优于传统EKF，计算时间减少50%。

8. 挑战与优化方向

动态环境处理：引入语义分割（如YOLOv5）剔除动态物体干扰。
计算效率：采用GPU加速特征提取与匹配。
大规模场景：结合图优化后端（如iSAM）进一步提升精度。

9. 结论

基于2D激光雷达的EKF-SLAM系统通过分割合并、LSM直线估计与角点提取技术，实现了高效的环境建模与定位。未来研究可聚焦多模态传感器融合与深度学习增强，以应对复杂动态场景的挑战。

📚2 运行结果

部分代码：

% Standard deviation error regarding observation
RTrue = diag([0.1, 0.5*pi/180]).^2;

REst = 2*RTrue;

% control input movement
deltaS =[0.28 0.30];

kr=0.0001;

% Gate value for measure acceptacnce
gateAccept =10;

F(nSteps) = struct('cdata',[],'colormap',[]);
% loop
for k =1:nSteps

% get true states
xVehicleTrue = xTrue;
xMapTrue =cornersLandFeatures;
% get estimated states
xVehicleEst = xEst(1:3); % get estimated state vehicle
xMapEst = xEst(4:end); % rest are the mapped corners estimated
% generate controls true value
u = getControl(xVehicleTrue,deltaS,0,0);
xVehicleTrue = moveRobot(xVehicleTrue,u);
% generate controls estimated value value
q = generateNoise(-0.005, 0.005, [3,1]); % noise
u = getControl(xVehicleEst,deltaS, 1, q);

xVehicleEst = moveRobot(xVehicleEst,u);