自动驾驶垂直泊车路径规划与MPC控制实践

1. 垂直泊车场景的技术挑战

垂直泊车（Perpendicular Parking）是自动驾驶领域公认的高难度场景之一。与平行泊车相比，车辆需要以90度角驶入车位，这对路径规划的几何约束和运动控制精度提出了更严苛的要求。我在参与某量产车型自动泊车系统开发时，曾遇到一个典型案例：当车位宽度仅比车宽多出40cm时（符合ISO标准最低要求），传统基于圆弧拼接的规划算法成功率不足60%。

这个场景的核心难点在于：

• 非完整约束（Nonholonomic Constraints）：前轮转向车辆无法横向移动，导致可行驶路径集合受限
• 狭窄空间下的避碰：前后保险杠与障碍物的最小间隙需控制在5cm以上
• 实时性要求：从识别车位到完成泊入的全过程需在20秒内完成

2. 基于Frenet框架的路径规划方案

2.1 坐标系转换与问题简化

我们采用Frenet坐标系将车辆运动分解为纵向（s轴）和横向（d轴）两个独立维度。在垂直泊车场景中：

• 将车位中心线作为参考路径（s轴）
• d轴表示车辆偏离中心线的距离

通过这种转换，三维空间中的位姿控制问题简化为二维轨迹优化问题。实测数据显示，该方案可使计算耗时降低73%。

2.2 五次多项式轨迹生成

在Frenet框架下，横向运动规划采用五次多项式：

code复制d(s) = a0 + a1s + a2s² + a3s³ + a4s⁴ + a5s⁵

选择五次多项式是因为它能满足边界条件：

• 起点/终点的位置、速度、加速度约束
• 保证曲率连续（C²连续），避免方向盘突变

具体参数求解过程：

1. 设定起点状态：d(0)=d0, d'(0)=d'0, d''(0)=d''0
2. 设定终点状态：d(sf)=df, d'(sf)=d'f, d''(sf)=d''f
3. 构建6×6矩阵方程求解系数a0~a5

2.3 动态障碍物处理

针对停车场行人突然穿行的场景，我们在代价函数中引入动态障碍物势场：

code复制J = w1·J平滑 + w2·J避障 + w3·J终点

其中避障项采用高斯函数建模：

code复制J避障 = Σ exp(-(s-si)²/2σs² - (d-di)²/2σd²)

参数调优经验：

• σs取2m（纵向安全距离）
• σd取0.5m（横向安全距离）
• 权重比w1:w2:w3建议设为3:5:2

3. 模型预测控制（MPC）实现

3.1 车辆动力学建模

采用自行车模型作为预测模型：

code复制ẋ = v·cosθ
ẏ = v·sinθ
θ̇ = v·tanδ/L

其中L为轴距，δ为前轮转角。在低速泊车场景（v<5km/h）下，可忽略轮胎侧偏特性。

3.2 控制量约束处理

实际工程中需要处理三类约束：

1. 转向角速率限制：|δ̇| ≤ 25°/s（EPS系统限制）
2. 加速度限制：|a| ≤ 2.5m/s²
3. 转向角限制：|δ| ≤ 35°

在MPC中采用松弛变量法处理约束违反风险：

code复制min J + ρ·ε²
s.t. g(x,u) ≤ ε

3.3 实时性能优化

通过以下手段将单次求解耗时控制在50ms内：

• 将预测时域从10步缩减到6步
• 采用Warm Start策略复用上一周期解
• 使用ACADO代码生成工具自动生成C代码

实测数据显示，该方案在Jetson AGX Xavier上的平均计算耗时仅38ms。

4. 实际部署中的工程经验

4.1 传感器误差补偿

超声波雷达的测距误差会显著影响控制精度。我们建立误差补偿模型：

code复制d_corrected = d_raw + 0.0023·d_raw² - 0.017·T

其中T为环境温度。经补偿后，测距误差从±5cm降低到±1.5cm。

4.2 地面坡度影响

当停车场坡度超过3°时，需在MPC模型中引入重力分量：

code复制ẋ = v·cosθ/(1 - tanθ·tanγ)

其中γ为坡度角。未考虑坡度会导致最终定位偏差达15cm。

4.3 人机交互设计

为避免用户焦虑，我们设计了三阶段提示：

1. 搜索车位时：声波脉冲频率随距离变化
2. 开始泊车时：短震动提示
3. 完成泊入时：连续两次蜂鸣

这种设计使系统接受度提升40%。

5. 测试验证方法论

5.1 硬件在环测试

使用dSPACE SCALEXIO系统构建测试环境：

• 车辆动力学模型：CarMaker
• 传感器仿真：Prescan
• 执行器接口：CANoe

测试覆盖场景：

• 极端车位（2.1m宽）
• 湿滑路面（μ=0.3）
• 斜向初始位姿（θ=15°）

5.2 实车测试指标

通过千次测试统计关键指标：

• 泊入成功率：98.7%
• 最大横向误差：8.2cm
• 平均耗时：18.3s
• 方向盘抖动次数：≤2次/次

5.3 边缘场景处理

针对常见异常情况的对策：

• 中途开车门：立即制动并保持当前挡位
• 障碍物突然出现：触发紧急制动（减速度≥4m/s²）
• 系统故障：自动切换P挡并亮双闪

这些策略使我们顺利通过ISO 16787认证测试。在实际项目中，最耗时的部分不是算法开发，而是各种极端场景的测试验证。建议至少预留40%时间用于测试环节，特别是在雨雪天气下的性能验证往往能暴露出仿真中难以发现的问题。