陈陈读书1. 自动泊车系统开发概述
自动泊车系统作为自动驾驶技术中最先落地的功能之一，其核心挑战在于如何在有限空间内实现厘米级定位和精准控制。传统PID控制在低速复杂场景下表现欠佳，而基于EKF（扩展卡尔曼滤波）和MPC（模型预测控制）的组合方案，则能够有效应对车辆动力学非线性、传感器噪声以及环境不确定性等问题。
我在开发这套系统时发现，一个可靠的自动泊车方案需要同时解决三个关键问题：首先是精确的状态估计（知道车在哪），其次是合理的轨迹规划（车该怎么走），最后是稳定的路径跟踪（车要走得准）。EKF主要负责融合多传感器数据，实时估计车辆位姿；MPC则根据当前状态和约束条件，计算最优控制指令。两者配合就像老司机的眼睛和手脚——EKF持续告诉系统"你现在偏了5厘米"，MPC则计算出"方向盘该打多少度能纠正回来"。
需要模型API调用？ 免费领10W Token，多模型网关一键接入 Claude、DeepSeek 等主流模型。
2. 车辆建模与EKF实现
2.1 简化自行车模型
自动泊车场景下（车速<5km/h），采用简化自行车模型既能满足精度要求，又能大幅降低计算复杂度。这个模型的核心假设是将四轮车辆简化为前后两轮，且前轮转向角通过几何关系传递到后轮：
matlab复制function x_next = vehicle_model(x, u, dt)
% 状态量x=[x坐标, y坐标, 航向角, 车速]
% 控制量u=[加速度, 前轮转角]
beta = atan(0.5*tan(u(2))); % 转向几何近似
x_next = x + dt*[x(4)*cos(x(3)+beta);
x(4)*sin(x(3)+beta);
x(4)*0.5*sin(beta)/1.5; % 1.5m为轴距
u(1)];
end
这个模型有两个精妙之处：一是通过beta角近似考虑了转向时前后轮轨迹的差异（阿克曼几何），二是用0.5的系数补偿了低速时轮胎侧偏的影响。实测表明，在2m/s以下速度时，该模型与实际车辆轨迹的误差小于3cm/米，完全满足泊车需求。
2.2 EKF设计与实现
EKF的核心在于处理非线性系统的状态估计。与传统卡尔曼滤波不同，EKF通过一阶泰勒展开对非线性系统进行局部线性化：
matlab复制% 状态转移雅可比矩阵计算
F = zeros(5); % 包含4个状态+1个传感器偏置
for i=1:5
dx = zeros(5,1);
dx(i) = 1e-6;
F(:,i) = (vehicle_model(x+dx(1:4),u,dt) - vehicle_model(x,u,dt))/1e-6;
end
F(5,5) = 1; % 偏置状态不变
这种数值求导方法虽然计算量稍大（约增加15%的CPU负载），但有三大优势：
- 避免手动推导复杂雅可比公式
- 模型参数调整时无需重新推导
- 对非线性函数的适应性更好
实际部署中发现，超声波传感器在近距离（<1m）测量时会出现系统性偏差。为此在状态向量中增加了传感器偏置项，通过EKF实时估计并补偿，使测距误差从平均8cm降低到2cm。
3. MPC控制器设计
3.1 目标函数构建
MPC控制器的性能很大程度上取决于目标函数的设计。经过数十次调参测试，最终采用如下多目标加权形式：
matlab复制cost = 0;
for k = 1:N % 预测时域20步
cost = cost + 1000*(X(k,end)-target_pose(1))^2 ... % 终端x坐标
+ 1000*(X(k,end-1)-target_pose(2))^2 ... % 终端y坐标
+ 500*(X(k,3)-target_pose(3))^2 ... % 终端航向角
+ 0.1*u_traj(k,1)^2 ... % 控制量平滑
+ 10*u_traj(k,2)^2; % 转向角限制
end
各权重系数的设置有其物理意义：
- 终端位置权重较大（1000）确保停车位置准确
- 航向角权重（500）防止"车头进车位，车尾在外"的情况
- 控制量权重避免急加速和转向过猛
3.2 约束条件处理
自动泊车中的约束主要来自三个方面：
- 车辆动力学约束：最大转向角±30度，加速度±0.3g
- 环境约束：障碍物形成的安全边界
- 执行器约束：转向速率限制（≤15度/秒）
在MPC中采用软约束处理障碍物避碰：
matlab复制for k = 1:N
for obs = 1:num_obs
dist = sqrt((X(k,1)-obs_pos(obs,1))^2 + (X(k,2)-obs_pos(obs,2))^2);
cost = cost + 1e4*max(0, safety_dist - dist)^3; % 三次方惩罚
end
end
这种处理方式比硬约束更鲁棒，当无法完全避免碰撞时，至少会最小化碰撞程度。
4. 系统集成与实测
4.1 传感器数据处理
自动泊车系统通常使用12个超声波传感器（前后各6个）构成感知系统。针对超声波的特性，开发了专用滤波算法：
matlab复制function valid_dist = ultrasonic_filter(raw_data)
persistent hist_buf;
% 移动平均滤波
hist_buf = [hist_buf(2:end), raw_data];
avg_dist = mean(hist_buf);
% 变化率检测
if abs(raw_data - avg_dist) > 0.2 % 突变超过20cm
valid_dist = avg_dist; % 使用历史平均值
else
valid_dist = 0.2*raw_data + 0.8*avg_dist; % 一阶低通滤波
end
end
这种组合滤波有效解决了墙角虚假回波（Ghost Echo）问题，将误检率从12%降低到1%以下。
4.2 全系统时序控制
为保证实时性，将系统划分为三个并行线程：
- 感知线程：100Hz运行，处理传感器数据
- 估计线程：50Hz运行，执行EKF更新
- 控制线程：20Hz运行，求解MPC问题
实测在Intel i5-8250U处理器上，单次MPC求解耗时约35ms（使用quadprog求解器），完全满足实时性要求。
5. 调参经验与避坑指南
5.1 EKF参数调试
-
过程噪声矩阵Q：建议从对角线[0.01,0.01,0.001,0.05]开始调整
- x/y坐标噪声与车速相关，可按0.1*v+0.01设置
- 航向角噪声通常很小（<0.001弧度）
-
观测噪声矩阵R：需通过传感器静态测试确定
- 超声波在1m内噪声约0.02m
- IMU的航向角噪声约0.01弧度
5.2 MPC常见问题
-
预测时域过长反而性能下降：
- 泊车场景建议2-3秒（20-30步）
- 超过3秒后轮胎滑移累积误差会主导预测结果
-
终端状态不收敛：
- 检查权重系数是否满足：位置权重 > 航向权重 > 控制量权重
- 增加终端代价的权重（可高达1e6）
-
求解失败：
- 放宽部分约束（如转向角限制）
- 尝试不同的QP求解器（OSQP比quadprog更稳定）
5.3 实车调试技巧
-
轮胎参数校准：
- 实测车辆的最小转弯半径（通常5-6m）
- 通过8字绕桩测试确定转向传动比
-
低速控制补偿：
- 在<0.15m/s时启用蠕行控制
- 添加静摩擦补偿（约0.05m/s²的额外加速度）
-
停车判稳条件：
- 连续5帧位置变化<2cm
- 航向角变化<0.5度
- 车速<0.05m/s
这套系统在实测中达到了以下性能指标：
- 标准车位（2.5×5m）泊入时间：≤30秒
- 最终定位误差：≤5cm（无GPS情况下）
- 航向角误差：≤1度
- 避障成功率：99.2%（静态障碍物）
对于特殊车位（斜向车位、柱状障碍物等），需要在上层路径规划阶段采用样条曲线生成更柔和的参考路径。一个实用的建议是：当检测到车位角度>15度时，自动切换为三次样条规划模式，这样可以减少MPC的跟踪负担。