基于EKF的二维SLAM系统实现与Matlab实践

1. 项目背景与核心价值

这个课程设计项目源自西北工业大学最优估计课程中的经典实践环节——基于扩展卡尔曼滤波(EKF)的二维平面SLAM系统实现。作为机器人定位与建图领域的入门级实践案例，它完美融合了理论推导与工程实现的双重挑战。

我在研究生阶段首次接触这个项目时，曾花费两周时间才完成基础功能。如今经过多年工业级SLAM系统开发历练，回头看这个课程设计，发现它确实包含了SLAM系统的核心骨架：

• 传感器层：模拟了激光雷达的测距数据和编码器的运动信息
• 算法层：实现了EKF的前向预测和观测更新两大核心步骤
• 可视化层：通过Matlab图形界面展示位姿轨迹和特征点地图

这个不到200行的Matlab实现，其实暗藏了自动驾驶、服务机器人等实际SLAM系统的关键设计思想。下面我将从实现细节到工程经验，完整还原这个经典课程设计的全貌。

2. 系统建模与EKF原理

2.1 机器人运动模型

采用差分驱动模型描述机器人运动，状态向量包含位姿和路标点坐标：

code复制x = [x_r, y_r, θ_r, x_1, y_1, ..., x_n, y_n]^T

其中(x_r,y_r,θ_r)是机器人位姿，(x_i,y_i)是第i个路标点的全局坐标。

运动更新方程采用速度运动模型：

code复制x_r(t+1) = x_r(t) + Δt*v*cos(θ)
y_r(t+1) = y_r(t) + Δt*v*sin(θ) 
θ_r(t+1) = θ_r(t) + Δt*w

这里v和w分别是线速度和角速度，Δt为采样时间。

实际编码时需要注意角度归一化问题，θ_r更新后需限制在[-π,π]范围内

2.2 观测模型

假设激光雷达可以测量路标点的相对距离和角度：

code复制z = [r, φ]^T
r = sqrt((x_i - x_r)^2 + (y_i - y_r)^2) + v_r
φ = atan2(y_i - y_r, x_i - x_r) - θ_r + v_φ 

v_r和v_φ是高斯白噪声，模拟传感器测量误差。

2.3 EKF实现框架

扩展卡尔曼滤波分为预测和更新两个阶段：

1. 预测阶段：

   • 状态预测：x̂_k|k-1 = f(x_k-1, u_k)
   • 协方差预测：P_k|k-1 = F_k P_k-1 F_k^T + Q_k

2. 更新阶段：

   • 卡尔曼增益：K_k = P_k|k-1 H_k^T (H_k P_k|k-1 H_k^T + R_k)^-1
   • 状态更新：x̂_k = x̂_k|k-1 + K_k(z_k - h(x̂_k|k-1))
   • 协方差更新：P_k = (I - K_k H_k) P_k|k-1

其中F_k是状态转移雅可比矩阵，H_k是观测雅可比矩阵。

3. Matlab实现详解

3.1 主程序框架

matlab复制% 初始化
init_pose = [0; 0; 0];  % 初始位姿 [x;y;θ]
map