自动驾驶入门:用EKF-SLAM理解车辆定位原理(MATLAB/Simulink案例)
想象一下,你第一次驾驶一辆车进入一个完全陌生的地下停车场,没有GPS信号,也没有任何地图指引。你需要一边小心翼翼地探索未知的通道,一边在脑海中勾勒出停车位的布局、柱子的位置,并时刻记住自己是从哪个入口进来的,以免迷路。对于一辆追求高度自主的智能汽车而言,同时定位与地图构建 就是它在这个陌生环境里必须掌握的“生存技能”。这不仅仅是感知环境,更是在感知的同时,回答“我在哪里”和“周围有什么”这两个相互依赖的核心问题。
在自动驾驶研发的早期阶段,理解定位与建图如何协同工作,是搭建整个系统认知框架的基石。而扩展卡尔曼滤波 作为处理这类非线性估计问题的经典工具,提供了一个绝佳的理论与实践切入点。它不像深度学习那样像个“黑箱”,其每一步的数学推导和状态更新都清晰可见,非常适合初学者亲手搭建仿真,直观感受误差如何产生、传播,又如何被传感器观测一步步修正。本文将带你抛开复杂的数学恐惧,通过一个在MATLAB中可运行的EKF-SLAM案例,从车辆的运动模型开始,一步步拆解这个“同步定位与地图构建”的过程。我们会看到激光雷达数据如何模拟,协方差矩阵这个抽象概念如何可视化地揭示定位置信度,最终让你获得一个可以直接上手实验、并深刻理解其原理的实用指南。
1. 从车辆运动到状态估计:为什么需要EKF?
在讨论SLAM之前,我们必须先解决一个更基本的问题:如何描述和预测车辆的运动?这对于定位至关重要。在实际的自动驾驶系统中,车辆通过轮速计、惯性测量单元等获取原始控制信息(如速度、角速度),但这些信息充满噪声,且车辆运动本身是非线性的。直接积分这些带噪声的控制量来推算位置,误差会迅速累积,这就是所谓的“航位推算”漂移。
1.1 自行车模型:简化但有效的运动抽象
为了在算法中描述车辆运动,我们引入自行车模型。它把四轮车辆简化为两轮,假设车辆像自行车一样,以后轴中心为参考点,通过前轮转向。这个模型在低速、低侧向加速度场景下非常有效。
在二维平面中,车辆在时刻 t 的状态通常用一个三维向量表示: x_t = [x, y, θ]^T 其中 (x, y) 是后轴中心在全局坐标系中的坐标,θ 是车辆的航向角(车头朝向)。
给定一个短时间间隔 Δt,如果已知控制输入——线速度 v 和转向角(或角速度 ω)——那么车辆的下一个状态可以通过运动学方程推算。例如,一个常用的基于角速度的模型是:
% MATLAB 代码片段:基于速度v和角速度w的自行车模型预测
function x_pred = bicycle_model(x_prev, v, w, dt)
x = x_prev(1);
y = x_prev(2);
theta = x_prev(3);
if abs(w) < 1e-6 % 近似直线运动
x_pred = x + v * cos(theta) * dt;
y_pred = y + v * sin(theta) * dt;
theta_pred = theta;
else % 圆弧运动
R = v / w; % 转弯半径
x_pred = x + R * (sin(theta + w*dt) - sin(theta));
y_pred = y - R * (cos(theta + w*dt) - cos(theta));
theta_pred = theta + w*dt;
end
x_pred = [x_pred; y_pred; theta_pred];
end
注意:这个模型忽略了轮胎滑移、悬架动力学等复杂因素,但对于理解SLAM中的预测步骤已经足够。在实际仿真中,我们会在控制输入
v和w上添加高斯噪声,以模拟真实传感器的误差。
1.2 卡尔曼滤波的局限与EKF的登场
如果我们有一个完美的线性运动模型和线性观测模型,并且所有噪声都服从高斯分布,那么卡尔曼滤波 就是最优的状态估计器。它通过“预测-更新”的递归框架,优雅地融合模型预测和传感器测量。
但问题在于,无论是上面提到的自行车模型,还是激光雷达的观测模型(测量到地标的距离和角度),都是非线性的。卡尔曼滤波的核心方程要求系统是线性的,直接套用会失效。
扩展卡尔曼滤波 的智慧在于“局部线性化”。它不在全局范围内强行线性化,而是在当前最可能的状态估计点(即均值处)进行一阶泰勒展开,用这个局部线性近似来代替原来的非线性函数。这就好比在一条弯曲的山路上,每一小段我们都用它的切线来近似,然后沿着切线走一小步。
% 概念性说明:EKF预测步骤中的线性化
% F_jacobian 是状态转移函数 f(x,u) 对状态 x 的雅可比矩阵(偏导数矩阵)
% 在状
130



被折叠的 条评论
为什么被折叠?



