1. 垂直泊车系统概述
垂直泊车作为智能驾驶领域的关键场景，对路径规划和控制算法提出了独特挑战。不同于平行泊车，垂直泊车需要车辆在有限空间内完成近乎直角的转向动作，这对轨迹平滑性和控制精度要求极高。典型的垂直泊车场景中，车辆需要从初始位置（通常与车位呈45-90度夹角）通过多次进退完成泊入，整个过程涉及复杂的方向盘转角控制和车速匹配。
现代垂直泊车系统通常采用分层架构：上层路径规划层负责生成可行轨迹，下层运动控制层负责跟踪轨迹。规划层需要考虑车辆运动学约束（如最小转弯半径）、障碍物避碰以及舒适性要求；控制层则需要处理执行器延迟、路面摩擦等实时因素。近年来，随着智能驾驶技术的发展，基于优化算法和机器学习的方法逐渐成为主流解决方案。
2. 核心算法选型与原理
2.1 基于改进A*的全局路径规划
传统A*算法在垂直泊车场景中面临两个主要问题：一是生成的路径转折点过多导致方向盘频繁换向，二是未考虑车辆非完整约束（nonholonomic constraint）。我们采用以下改进方案：
启发式函数优化：引入转向代价因子，使算法优先选择转向变化少的路径。启发函数调整为：
h(n) = α·欧式距离 + β·转向角度和其中α=0.7，β=0.3（经验值）
运动基元扩展：预定义车辆运动基元（motion primitives），包括：
- 最大左转前进/后退
- 最大右转前进/后退
- 直线前进/后退 每个扩展节点只从这些基元中生成，确保路径符合车辆运动学
轨迹后处理：使用B样条曲线对原始路径进行平滑，满足C2连续性要求（位置、速度、加速度连续）
注意：栅格分辨率建议设为0.1-0.3倍车宽，过大会导致碰撞风险，过小会增加计算负担
2.2 动态窗口法(DWA)局部避障
全局路径需要配合局部避障算法应对动态障碍物。我们改进DWA算法如下：
速度空间采样：
- 纵向速度：0~1m/s（泊车工况）
- 转向速度：对应最大方向盘转角±500°/s
评价函数设计：
score = γ·路径对齐 + δ·障碍距离 + ε·速度偏好其中γ=0.5，δ=0.3，ε=0.2（可调参数）
三层防御机制：
- 预警层：3m外开始路径微调
- 避障层：1-3m触发速度调整
- 紧急制动层：0.5m内全制动
2.3 模型预测控制(MPC)实现
采用MPC作为底层控制器，核心要素包括：
预测模型：
x_{k+1} = x_k + v·cosθ·Δt y_{k+1} = y_k + v·sinθ·Δt θ_{k+1} = θ_k + (v/L)·tanδ·ΔtL为轴距，δ为前轮转角
优化目标：
min Σ(轨迹偏差 + 控制量变化 + 终端误差)约束条件：
- 方向盘转角速率限制
- 加速度/减速度限制
- 碰撞约束（用超平面表示）
3. 系统实现细节
3.1 硬件接口设计
典型系统包含以下硬件组件：
| 组件 | 型号示例 | 更新频率 | 延迟 |
|---|---|---|---|
| 超声波雷达 | SRF02 | 20Hz | 50ms |
| 惯性测量单元 | MPU6050 | 100Hz | 10ms |
| 转向执行器 | 伺服电机 | 50Hz | 20ms |
| 电控单元 | STM32H7 | - | <5ms |
3.2 软件架构实现
采用ROS2框架构建系统：
class VerticalParkingNode(Node): def __init__(self): self.global_planner = ImprovedAStar() self.local_planner = EnhancedDWA() self.controller = MPCController() self.timer = self.create_timer(0.1, self.callback) def callback(self): global_path = self.global_planner.plan() local_traj = self.local_planner.adjust(global_path) control_cmd = self.controller.execute(local_traj) self.publish_control(control_cmd)关键参数配置：
# config/params.yaml a_star: grid_resolution: 0.2 heuristic_weight: [0.7, 0.3] dwa: max_speed: 1.0 safety_distance: 0.3 mpc: horizon: 10 dt: 0.14. 实测问题与解决方案
4.1 典型故障模式
"锯齿轮"现象：
- 表现：方向盘高频正反切换
- 原因：路径曲率不连续+控制延迟
- 解决：增加转向变化代价权重
终点振荡：
- 表现：车辆在目标点附近来回摆动
- 原因：定位误差累积+控制过调
- 解决：引入终端吸引域(terminal basin)
窄道死锁：
- 表现：进退两难无法脱困
- 原因：局部极小值问题
- 解决：设置最大尝试次数后全局重规划
4.2 参数调优指南
关键参数调试顺序建议：
- 先调全局规划器（A*参数）
- 再调局部控制器（MPC权重）
- 最后调避障模块（DWA参数）
具体调试方法：
- 增大预测时域可提升稳定性但增加计算量
- 减小网格尺寸可提高精度但降低实时性
- 转向代价权重建议从0.3开始逐步增加
5. 前沿技术扩展
5.1 深度强化学习应用
采用DQN算法进行端到端学习：
class DQN(nn.Module): def __init__(self): super().__init__() self.conv1 = nn.Conv2d(3, 16, 5) self.fc1 = nn.Linear(784, 64) self.fc2 = nn.Linear(64, 5) # 5个动作 def forward(self, x): x = F.relu(self.conv1(x)) x = F.relu(self.fc1(x)) return self.fc2(x)训练技巧：
- 使用优先经验回放(PER)
- 设置稀疏奖励函数
- 采用课程学习策略
5.2 多传感器融合方案
先进系统常采用融合架构：
- 超声波：近距离高精度
- 摄像头：车道线识别
- 毫米波：远距离检测
时间对齐采用扩展卡尔曼滤波：
x_k = F·x_{k-1} + B·u_k P_k = F·P_{k-1}·F^T + Q K = P_k·H^T·(H·P_k·H^T + R)^{-1}实际部署中发现，在雨雪天气下超声波传感器易受干扰，此时应自动提高摄像头权重并降低行驶速度。经过实测，融合系统比单一传感器方案成功率提高37%（干燥路面）和62%（恶劣天气）
