1. 自动泊车系统技术背景自动泊车系统作为智能驾驶的关键功能模块其核心挑战在于如何在复杂环境中实现厘米级定位和精准控制。传统PID控制在低速、大滞后场景下表现欠佳这正是EKFMPC组合方案的优势所在。车辆运动学模型是整套系统的基础采用简化自行车模型主要基于三点考量泊车场景车速通常低于5km/h可忽略轮胎动力学影响简化模型计算量小适合实时控制MPC需在线求解实测表明在低速状态下简化模型精度足够满足需求关键提示模型中的beta角近似处理atan(0.5*tan(u(2)))是针对前轮转向角到等效后轮转向角的转换这种处理方式比标准阿克曼几何模型计算量减少40%同时保持转向精度在±0.3°以内。2. 扩展卡尔曼滤波实现细节2.1 状态方程构建EKF需要处理的状态变量包括位置(x,y)航向角θ车速v传感器偏置b应对超声波测距的系统误差状态转移矩阵采用数值雅可比计算相比解析求导有三个显著优势模型修改时无需重新推导雅可比公式避免手动求导可能出现的符号错误可灵活处理非线性环节如三角函数% 数值雅可比计算示例 F zeros(5); for i1:5 dx zeros(5,1); dx(i) 1e-6; F(:,i) (vehicle_model(xdx(1:4),u,dt) - vehicle_model(x,u,dt))/1e-6; end F(5,5) 1; % 偏置状态保持2.2 传感器数据处理超声波传感器在自动泊车中面临三大挑战多径反射墙角虚假回波测量延迟约50ms角度分辨率有限±15°解决方案移动平均滤波窗口选择3~5个采样点变化率阈值设为0.5m/s超出则视为异常值融合轮速里程计数据补偿延迟3. 模型预测控制器设计3.1 代价函数构建MPC的核心在于代价函数设计需要平衡多个冲突目标目标项权重系数物理意义终端位置误差1000确保最终停入车位航向角误差500避免斜着入库加速度惩罚0.1保证乘坐舒适性转向角惩罚10防止方向盘频繁打满实际调试中发现终端位置权重需达到航向角的20倍以上否则会出现车头入库、车尾外露的尴尬情况。3.2 预测时域选择通过大量仿真测试得出以下经验最优预测时域2.5~3秒时域过短1.5秒控制短视容易振荡时域过长3.5秒轮胎滑移误差累积% MPC求解核心代码片段 options optimoptions(fmincon,Algorithm,sqp,... MaxIterations,100,... Display,none); [u_opt,~,exitflag] fmincon((u) mpc_cost(u,x0,target),... u_guess,[],[],[],[],... lb,ub,[],options);4. 系统集成与实测4.1 硬件在环测试搭建HIL测试平台时需特别注意车辆参数标定实测轴距误差需1cm执行器延迟补偿转向电机响应时间传感器时钟同步CAN总线时间对齐测试数据对比平行车位成功率98.7%平均耗时23.5秒垂直车位成功率95.2%平均耗时27.3秒斜向车位成功率89.5%需路径重规划4.2 实车调试技巧低速控制增益单独标定0~3km/h区间方向盘死区补偿约±3°需软件过滤刹车释放延迟补偿约0.2秒提前量遇到过的典型问题及解决方案问题1入库最后阶段位置抖动 原因车速低于0.15m/s时轮速计噪声占比过大 方案切换到位移传感器融合模式问题2狭窄车位反复进退 原因MPC约束条件过于保守 方案动态调整安全边界根据障碍物距离5. 工程实践建议模型验证流程静态参数验证轴距、轮距等开环响应测试阶跃转向输入闭环功能测试不同初始位姿代码优化技巧预分配矩阵内存避免MPC求解时动态分配使用Coder生成Mex文件提升EKF计算速度关键路径采用定点数运算如状态预测常见故障排查定位突然发散检查IMU供电电压控制指令振荡降低MPC求解频率路径规划失败检查碰撞检测网格分辨率这个项目最让我意外的是实际车辆参数与模型假设的微小差异会带来巨大影响。有次测试车换了冬季轮胎半径变化1.2cm导致泊车成功率直接下降30%。后来在状态估计中增加了轮胎半径在线辨识模块才算彻底解决问题。建议大家在工程化时务必留出参数自适应调整的接口。