B样条曲线在自动泊车路径规划中的应用与优化

1. B样条曲线与自动泊车路径规划概述

平行泊车作为城市驾驶中最具挑战性的场景之一，对路径规划算法提出了严苛要求。传统基于圆弧和直线组合的规划方法存在曲率不连续的缺陷，导致车辆在衔接点处需要停顿转向。而B样条曲线（B-Spline）凭借其局部可控性和连续性优势，成为解决这一痛点的理想选择。

B样条曲线本质上是分段多项式曲线，由控制点和节点向量共同定义。在自动泊车场景中，我们通常采用3次（4阶）准均匀B样条曲线，这种配置能在计算复杂度和曲线平滑度之间取得最佳平衡。其数学表达式为：

python复制def b_spline(t, control_points, degree=3):
    # 计算节点向量
    knots = generate_knots(len(control_points), degree)
    # 使用Cox-de Boor递归公式计算曲线点
    return sum(control_points[i] * basis_function(i, degree, t, knots) 
              for i in range(len(control_points)))

关键特性：准均匀B样条的节点向量使曲线首尾精确通过起止控制点，这对确保泊车起始和终止姿态的精确性至关重要。

2. 平行泊车场景建模与约束分析

2.1 车辆运动学模型

建立准确的自行车模型是路径规划的基础。假设车辆后轴中心为参考点，其运动学方程可表示为：

code复制ẋ = v * cosθ
ẏ = v * sinθ
θ̇ = v/L * tanφ

其中L为轴距，φ为前轮转角，v为车速。在低速泊车场景下，可忽略轮胎侧偏特性，采用简化的运动学模型。

2.2 几何约束条件

平行泊车需满足以下核心约束：

1. 边界避碰约束：

   • 车体四角坐标与障碍物的最小距离
   • 考虑车辆外廓膨胀量（通常增加0.3-0.5m安全余量）

2. 曲率连续性约束：

   • 最大曲率不超过车辆最小转弯半径的倒数
   • 曲率变化率限制（影响转向电机响应）

3. 终端状态约束：

   • 最终位姿与目标停车位的对齐精度（±5cm，±2°）

2.3 优化目标函数

构建多目标优化函数：

code复制min w1*κ_max + w2*∫κ²ds + w3*T

其中κ为曲率，T为泊车时间，权重系数w1:w2:w3建议取3:2:1。

3. 基于B样条的路径规划实现

3.1 控制点初始化策略

采用"三点定位法"生成初始控制点：

1. 起始点：车辆后轴中心当前位置
2. 转折点：根据车位长度取侧向偏移量（经验公式：Δy=0.7*L_space）
3. 终止点：目标车位中心后1/4车长处

python复制def init_control_points(start_pose, end_pose, L_space):
    mid_x = (start_pose.x + end_pose.x)/2
    mid_y = start_pose.y + 0.7*L_space
    return [start_pose, Point(mid_x, mid_y), end_pose]

3.2 约束条件转化

将几何约束转化为控制点坐标的线性不等式：

code复制A·P ≤ b

其中A为约束矩阵，P为控制点坐标向量。例如对左侧障碍约束：

code复制[0, 1, 0]·[Px, Py, 1]^T ≤ road_width - car_width/2 - margin

3.3 数值优化求解

采用序列二次规划（SQP）算法求解：

matlab复制options = optimoptions('fmincon','Algorithm','sqp');
[x,fval] = fmincon(@objfun,x0,A,b,Aeq,beq,lb,ub,@confun,options);

实测数据：在Intel i7处理器上，9控制点的优化问题平均求解时间约120ms，满足实时性要求。

4. 工程实现关键问题

4.1 控制点数量选择

通过大量实验得出经验公式：

code复制N_ctrl = ceil(2.5 * (L_space / L_car))

其中L_space为可用车位长度，L_car为车长。典型场景（6m车位，4.8m车长）建议取7-9个控制点。

4.2 曲率连续性保障

采用4阶（3次）B样条可保证C²连续性，满足：

• 位置连续（C⁰）：路径无断点
• 切线连续（C¹）：方向盘转向平滑
• 曲率连续（C²）：避免转向速率突变

曲率计算式：

python复制def curvature(derivative1, derivative2):
    return np.linalg.norm(np.cross(derivative1, derivative2)) / np.linalg.norm(derivative1)**3

4.3 实时性优化技巧

1. 热启动技术：复用上一帧优化结果作为初始值
2. 控制点冻结：固定首尾3个控制点坐标，减少优化变量
3. 并行计算：使用OpenMP加速Jacobian矩阵计算

5. 典型问题与解决方案

5.1 狭窄车位场景

当车位长度L_space < 1.2*L_car时：

1. 采用"倒车优先"策略，初始侧向偏移增加20%
2. 引入最大曲率约束（典型值：0.2m⁻¹）
3. 增加中间过渡点，形成S形路径

5.2 非结构化环境

针对不规则障碍物：

1. 构建Voronoi图生成初始路径
2. 在碰撞风险区域增加控制点密度
3. 采用自适应采样调整节点向量

5.3 系统延迟补偿

实测某车型的转向系统延迟达300ms，解决方案：

1. 路径预瞄：在曲率极值点前3m开始转向
2. 速度规划：曲率大的区域降速至5km/h以下
3. 前馈补偿：在控制指令中加入微分项

6. 实际部署效果评估

在某量产车型上的测试数据：

7. 前沿改进方向

1. 自适应阶次选择：根据路径复杂度动态调整B样条阶数
2. 时空联合优化：将速度规划与路径规划统一求解
3. 深度学习辅助：用CNN预测初始控制点分布
4. 不确定性建模：考虑定位误差的鲁棒性优化

在实现过程中发现，将B样条的节点向量间隔与车辆最大转向速率关联，能有效避免规划出理论可行但执行困难的路经。具体来说，建议节点向量跨度Δt与车速v满足：

code复制Δt ≥ (κ_max * v) / (2 * ω_max)

其中ω_max为转向电机最大角速度。