【应用于同步定位与地图构建(SLAM)的卡尔曼滤波】基于卡尔曼滤波和同步定位与地图构建(SLAM)技术实现二维地图绘制研究（Matlab代码实现）-CSDN博客

本文链接：https://blog.csdn.net/weixin_46039719/article/details/147692500

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

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

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

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

目录

⛳️赠与读者

💥1 概述

基于卡尔曼滤波与SLAM的二维地图绘制研究

一、卡尔曼滤波在SLAM中的基本原理与作用

二、二维地图绘制的关键技术

三、系统架构设计

四、案例研究：EKF与二维地图绘制的整合

五、卡尔曼滤波的优化策略

六、未来研究方向

📚2 运行结果

🎉3 参考文献

🌈4 Matlab代码、文档讲解

⛳️赠与读者

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

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

💥1 概述

基于卡尔曼滤波与SLAM的二维地图绘制研究

一、卡尔曼滤波在SLAM中的基本原理与作用

卡尔曼滤波（KF）是一种基于贝叶斯框架的递归状态估计算法，通过预测-更新两步骤处理线性高斯系统的噪声问题。在SLAM中，其核心作用包括：

状态估计：结合运动模型（预测）与传感器观测（更新），动态估计机器人位姿及环境特征的位置。KF通过卡尔曼增益动态调整预测与观测的权重，降低不确定性。
误差协方差管理：KF维护协方差矩阵以量化状态估计的不确定性，并通过更新步骤逐步收敛误差。
非线性扩展：针对非线性系统，扩展卡尔曼滤波（EKF）通过一阶泰勒展开对模型线性化，实现非线性状态估计。例如，EKF-SLAM将机器人运动方程和观测方程线性化后，应用于机器人位姿与地图特征联合估计。

数学表达：

二、二维地图绘制的关键技术

SLAM算法选择：
- Gmapping：基于粒子滤波（RBPF），通过自适应重采样减少粒子退化，适用于低特征场景。
- Hector SLAM：依赖高精度激光雷达数据，采用多分辨率地图优化，避免局部最优。
- EKF-SLAM：适用于中小规模场景，通过线性化处理联合估计状态与地图。
地图表示方法：
- 栅格地图：将环境划分为网格单元，记录每个单元的占据概率（如Gmapping生成的Occupancy Grid Map）。
- 特征地图：提取环境中的点、线等几何特征，适用于EKF-SLAM中的稀疏表示。
路径规划与地图优化：
- A*算法：结合启发式搜索，生成最短路径，需与SLAM输出的栅格地图集成以实现动态避障。
- 后端优化：如加权非线性最小二乘（WNLLS），用于全局地图一致性优化，与EKF的前端实时估计结合可提升精度。

三、系统架构设计

模块划分：
- 传感器层：激光雷达、IMU、里程计等多源数据融合，预处理（如去畸变、时间同步）。
- 状态估计层：EKF/UKF实现位姿预测与地图特征更新，协方差矩阵传递不确定性。
- 数据关联：通过最近邻（NN）或JCBB算法匹配观测与地图特征，解决特征对应问题。
- 地图构建：根据更新后的状态动态生成或修正栅格/特征地图。
混合架构：
- 局部滤波+全局优化：前端使用EKF进行实时状态估计，后端采用图优化（如g2o）闭环检测与全局调整，兼顾效率与精度。
- 动态权重融合：根据传感器置信度调整卡尔曼增益，例如在激光雷达与视觉融合中，动态分配Q、R矩阵权重以提升鲁棒性。

四、案例研究：EKF与二维地图绘制的整合

动态环境下的EKF-SLAM：
- 实验设置：TurtleBot3机器人搭载2D激光雷达，在Gazebo模拟环境中运动。
- 结果：EKF-SLAM在纹理丰富区域（如标志物b、d）达到99%以上定位精度，但复杂形状或反射表面（如物体c）精度下降至23.6%，凸显数据关联与噪声模型的挑战。
EKF与WNLLS的协同优化：
- 方法：前端EKF实时估计，后端WNLLS对历史数据进行批量优化，减少累积误差。
- 仿真验证：在MATLAB中模拟噪声环境，EKF+WNLLS相比纯EKF的地图误差降低40%，尤其在传感器噪声较高时效果显著。

五、卡尔曼滤波的优化策略

算法改进：
- 平方根UKF（SR-UKF） ：通过Cholesky分解协方差矩阵，避免数值不稳定，计算效率提升25%。
- 四元数表示：用四元数替代欧拉角描述姿态，减少奇异性问题，提升UKF在三维旋转中的精度。
参数整定：
- 自适应Q/R调整：基于残差协方差在线调整过程噪声Q与观测噪声R，适应动态环境变化。
- 多传感器融合：激光雷达与IMU的EKF融合中，通过实验标定Q、R初始值，平衡预测与观测权重。

六、未来研究方向

多模态融合：结合视觉SLAM的稠密点云与激光雷达的精确测距，提升复杂场景重建能力。
边缘计算优化：在嵌入式平台部署轻量化EKF，通过FPGA加速协方差矩阵运算。
自适应SLAM：利用深度学习动态调整卡尔曼滤波参数，应对动态障碍物与传感器失效。

通过上述多维度整合，基于卡尔曼滤波的SLAM系统能够在二维地图绘制中实现高精度定位与稳健建图，为自主导航提供可靠基础。

📚2 运行结果

......

运行结果图比较多，就不一一展示。

部分代码：

%Script principal
clear;
close all;
addpath (genpath('.')); %sous repertoire

% Specifier le nom du fichier
nom_fichier = 'inputs/FullObservation.data';

% Ouvrir le fichier en mode lecture
fid = fopen(nom_fichier, 'r');

% Lecture des premi猫res perceptions
ligne = fgetl(fid);

format_percep = '%s : %f %f %f %f %f %f %f %f %f %f';
data_percep = textscan(ligne, format_percep, 'Delimiter', ' ');

% Initialiser variables
k_odo = 0.1;
k_y = 0.1;

Yt = zeros(10, 1);
for i = 1:10
    Yt(i) = data_percep{i+1};
end
[Xt, Pt, A, B, Ht] = init(Yt, k_y);

% Affichage carte
%affichage(Xt,Pt);
%pause(1);

% Initialisation variables d'affichage
u_t = zeros(2,1);
Xt_kalman = zeros(2,1);

% Lire le fichier ligne par ligne
while ~feof(fid) %TQ fichier non vide
    % Lecture de la ligne d'odometrie
    ligne = fgetl(fid);
    
    taille_ligne = size(ligne);
    if taille_ligne(1) == 1
        % Utiliser le format specifie pour les donnees d'odometrie A CHANGER
        format_odom = '%s : %f %f';
        data_odom = textscan(ligne, format_odom, 'Delimiter', ' ');
    
        % Stockage des donnees d'odometrie
        donnees_odom = zeros(2, 1);
        for i = 1:2
            donnees_odom(i) = data_odom{i+1};
        end
        Q = cov_odo (donnees_odom, k_odo);
    
        % Prediction de l'etat
        [Xt_star, Pt_star] = prediction_etat(Xt,donnees_odom, A, B,Pt,Q); %On a juste recup l'odometrie, R脿S
        
        % Affichage de la carte
        %affichage (Xt_star,Pt_star);
        %pause(1);

        % Lecture de la ligne des perceptions
        ligne = fgetl(fid);
     
        % Utiliser le format specifie pour les donnees de perception
        format_percep = '%s : %f %f %f %f %f %f %f %f %f %f';
        data_percep = textscan(ligne, format_percep, 'Delimiter', ' ');
        Yt = zeros(10, 1);
        for i = 1:10
            Yt(i) = data_percep{i+1};
        end
        P_Y = cov_obs(Yt, k_y);
    
        % Prediction sur la position
        [Xt_star,Pt_star] = prediction_etat(Xt,donnees_odom,A, B,Pt,Q);
    
        % Prediction sur l'observation
        [Yt_star] = prediction_observateur(Xt_star, Ht);
    
        % Correction de la prediction
        [Xt, Pt] = correction_etat(Xt_star, Pt_star, Yt_star, Yt, Ht, P_Y);
        
        % Affichage de la carte 
        %affichage(Xt,Pt);
        %pause(1);
    end
    u_t = [u_t; (u_t(end-1:end) + donnees_odom)]; %On suppose odometrie initialement nulle
    Xt_kalman = [Xt_kalman; Xt(1:2)];
end                
affichage_ameliore(u_t, Xt_kalman, Pt, Xt);

% Fermer le fichier
fclose(fid);