飞翔的十号1. 项目背景与核心需求
自动泊车系统作为智能驾驶的关键功能模块，其核心挑战在于如何在有限空间内实现高精度位姿估计与最优路径规划。传统PID控制方案在非线性强、约束多的泊车场景中表现乏力，这正是EKF+MPC组合方案的价值所在。
我去年参与的一个量产项目就遇到过这样的问题：当车辆以低于5km/h的速度进行垂直泊车时，超声波雷达的测距误差会突然增大到±15cm，导致基于纯几何计算的泊车路径频繁失效。后来我们引入EKF进行多传感器融合后，定位精度稳定控制在±3cm以内。
需要模型API调用？ 免费领10W Token，多模型网关一键接入 Claude、DeepSeek 等主流模型。
2. 技术方案设计
2.1 系统架构设计
完整的方案包含三个核心模块：
- 感知层：融合12个超声波雷达（探测距离0.2-4.5m）和4个鱼眼相机（200万像素，60fps）
- 算法层：
- EKF负责状态估计（100Hz更新）
- MPC进行轨迹规划（20Hz更新）
- 执行层：EPS电子转向（分辨率0.1°） + ESP线控制动（压力控制精度0.1MPa）
关键设计要点：EKF的预测周期必须大于MPC的计算耗时，否则会出现控制指令堆积。实测表明当MPC计算超过50ms时，需要降低优化问题维度。
2.2 EKF实现细节
2.2.1 状态空间建模
采用6自由度模型：
code复制x = [x_pos, y_pos, heading, velocity, steering_angle, steering_rate]
协方差矩阵初始化特别重要，我们的经验值是：
matlab复制P0 = diag([0.1 0.1 0.01 0.5 0.05 0.02]).^2; % 单位：m, rad, m/s等
2.2.2 观测模型处理
针对不同传感器特性设计观测矩阵：
- 超声波：H_us = [1 0 0 0 0 0; 0 1 0 0 0 0]
- 视觉：H_cam = [cosθ -sinθ 0 0 0 0; sinθ cosθ 0 0 0 0]
matlab复制function [x_pred, P_pred] = ekf_predict(x, P, u, dt)
% 运动模型雅可比
F = [1 0 -v*sin(θ)*dt cos(θ)*dt 0 0;
0 1 v*cos(θ)*dt sin(θ)*dt 0 0;
0 0 1 0 0 0;
0 0 0 1 0 0;
0 0 0 0 1 dt;
0 0 0 0 0 1];
% 预测步骤
x_pred = f(x,u,dt);
P_pred = F*P*F' + Q;
end
2.3 MPC控制器设计
2.3.1 代价函数构建
采用二次型代价函数：
code复制J = Σ(α·位置误差² + β·航向误差² + γ·方向盘转速²)
权重系数通过实车调参确定典型值：
matlab复制weights = [10, 5, 0.1]; % 位置 > 航向 > 控制量
2.3.2 约束条件处理
- 物理约束：
- 方向盘转角 ±480°
- 转向速率 ±100°/s
- 安全约束：
- 障碍物距离 >0.3m
- 曲率半径 >4.7m（对应最小转弯半径）
使用MATLAB的quadprog求解时需注意：
matlab复制options = optimoptions('quadprog',...
'MaxIterations',200,...
'ConstraintTolerance',1e-6);
3. MATLAB实现关键代码
3.1 主循环框架
matlab复制while ~reach_target
% 传感器数据读取
[z_us, z_cam] = read_sensors();
% EKF更新
[x_est, P] = ekf_update(x_pred, P_pred, z_us, z_cam);
% MPC求解
[u_opt, cost] = mpc_solve(x_est, ref_path);
% 执行控制
send_control(u_opt);
% 延时确保实时性
pause(0.02); % 50Hz控制频率
end
3.2 可视化工具开发
建议使用App Designer创建交互界面：
matlab复制classdef ParkingGUI < matlab.apps.AppBase
properties
Axes_Estimate matlab.ui.control.UIAxes
Slider_Noise matlab.ui.control.Slider
end
methods
function updatePlot(app)
plot(app.Axes_Estimate,...
x_est(1), x_est(2), 'bo',...
'MarkerSize',10);
end
end
end
4. 调试与优化经验
4.1 EKF常见问题排查
-
发散问题：
- 检查Q/R矩阵量级是否匹配：过程噪声Q应比观测噪声R小1-2个数量级
- 使用NEES检验：
NEES = (x_true-x_est)'*inv(P)*(x_true-x_est)
-
延迟补偿：
matlab复制% 预测补偿50ms传感器延迟 x_delayed = x_pred + dt*v*[cos(θ); sin(θ); 0; 0; 0; 0];
4.2 MPC调参技巧
-
预测时域选择：
- 垂直泊车：3s（N=60@20Hz）
- 平行泊车：5s（N=100@20Hz）
-
热启动优化：
matlab复制% 使用上一周期解作为初始猜测 [u_opt, ~, exitflag] = quadprog(H,f,A,b,[],[],lb,ub,... last_u, options);
5. 实车验证结果
在某B级车上进行的测试数据：
| 指标 | 纯PID控制 | EKF+MPC方案 | 提升幅度 |
|---|---|---|---|
| 泊入成功率 | 76% | 98% | +22% |
| 平均泊车时间 | 38.2s | 29.5s | -23% |
| 最大位置误差 | 0.25m | 0.08m | -68% |
| 方向盘抖动次数 | 6.8次 | 2.1次 | -69% |
这个方案最让我惊喜的是对"库位角"这种极端场景的处理能力。当初始位置与库位呈45°夹角时，传统方案需要多次进退调整，而MPC能一次性生成平滑的"S"形轨迹。