垂直车位自动泊车几何路径规划算法解析

1. 垂直车位路径规划概述

垂直车位自动泊车（APA）是当前智能驾驶系统中最具挑战性的场景之一。与平行泊车相比，垂直泊车需要更精确的几何计算和路径控制，特别是在有限空间内完成多步规划时。我在开发某量产车型的自动泊车系统时，发现几何法路径规划在垂直车位场景中展现出独特的优势——计算量小、响应快，且能满足大多数实际工况需求。

这个方案的核心思路是将复杂路径拆解为直线段和圆弧段的组合。第一步规划尤为关键，它需要同时满足两个看似矛盾的条件：既要避开前侧障碍物（前避碰点），又要尽可能贴近后侧参考点（后避碰点）。这种"前松后紧"的设计理念，实际上是为后续的调整步骤预留操作空间。

2. 几何法核心算法解析

2.1 切线圆弧生成算法

Tangent_LC_Margin函数实现了路径规划中的关键几何构造。其数学本质是求解与给定直线相切，同时与障碍物安全圆相切的圆弧。具体参数包括：

• 起点所在直线g（车辆初始方向）
• 前避碰点D（障碍物最近点）
• 安全裕量TrialMargin（通常取车宽1/2加15cm）

求解过程采用几何作图法：

1. 以D为中心，构建半径为车宽/2 + TrialMargin的安全圆
2. 作与直线g平行且距离为R（规划圆弧半径）的平行线
3. 该平行线与安全圆的交点即为圆心候选点
4. 验证切线条件，确定唯一有效圆心

cpp复制// 示例代码核心逻辑
Circle FindTangentCircle(Line g, Point D, float margin) {
    float safety_radius = car_width/2 + margin;
    Line parallel_g = g.Offset(planning_radius); 
    Circle safety_circle(D, safety_radius);
    auto candidates = Intersect(parallel_g, safety_circle);
    // 验证切线条件并返回有效圆
}

2.2 圆弧行驶极限计算

VehicleUtils::VerticalTrial函数解决的是运动学约束问题。当车辆沿圆弧行驶时，需要计算在不碰撞后侧障碍物的情况下，最大允许旋转角度。这里