4、路径规划核心：基于多项式的路径生成、边界约束下的路径优化、代价函数设计
各位同学，今天我们来啃一块硬骨头——路径规划的核心技术。说实话，这部分内容我当年自学的时候也绕了不少弯路。多项式、约束、代价函数，听起来像是三个独立的东西，但实际工程中它们是环环相扣的。我个人习惯把这三者比作「骨架、尺子、天平」——多项式是路径的骨架，约束是丈量可行域的尺子，代价函数则是权衡利弊的天平。
4.1 基于多项式的路径生成：为什么是它？
先问大家一个问题：为什么自动驾驶里普遍用多项式来表示路径？
原因其实很简单。多项式有很好的数学性质——连续、可导、计算简单。你想想看，车辆的运动轨迹需要位置、速度、加速度都连续，否则乘客会感觉一顿一顿的。多项式天然满足这些要求。
我在项目中遇到过最常用的两种：五次多项式和三次样条。
- 五次多项式：可以同时约束起点和终点的位置、速度、加速度。一共6个系数，正好对应6个边界条件。
- 三次样条：分段低次多项式拼接，适合长距离路径，计算量小，但需要处理连接点的连续性。
举个例子，假设我们要从起点 (x₀, y₀) 到终点 (x₁, y₁)，同时要求起止速度、加速度为零。用五次多项式可以这样写：
// 五次多项式系数求解
// 边界条件: p(0)=0, p(1)=1, v(0)=0, v(1)=0, a(0)=0, a(1)=0
// 解出系数矩阵
double[] coeff = new double[6];
coeff[0] = 0; // a0
coeff[1] = 0; // a1
coeff[2] = 0; // a2
coeff[3] = 10; // a3
coeff[4] = -15; // a4
coeff[5] = 6; // a5
// 路径点生成
for (double t = 0; t <= 1.0; t += 0.01) {
double x = coeff[0] + coeff[1]*t + coeff[2]*t*t + coeff[3]*t*t*t + coeff[4]*t*t*t*t + coeff[5]*t*t*t*t*t;
// 同理计算 y
}
嗯，这里要注意：系数求解其实是个线性方程组，直接用矩阵求逆就行。我早期犯过一个错——直接用浮点数硬算，结果数值稳定性很差。后来改用 Eigen 或 LAPACK 库，问题就解决了。
4.2 边界约束下的路径优化：别让车跑出路沿
多项式生成了路径，但这条路径不一定满足道路边界约束。说白了，你不能让车跑到路肩外面去。
边界约束通常分为两类：
- 硬约束：路径必须完全在道路边界内，不能越界。这通常用不等式约束来处理。
- 软约束：允许轻微越界，但会施加惩罚。适合一些非结构化场景。
我曾经在一个园区项目中吃过亏。当时只做了硬约束，结果在狭窄弯道处，优化器死活找不到可行解。后来改成软约束 + 惩罚项，路径虽然稍微贴边，但整体可行了。避坑指南：不要只用硬约束，留一点余量给软约束。
具体怎么做？我常用的方法是离散化 + 二次规划：
- 将路径离散成 N 个点
- 每个点施加左右边界约束：
left_bound[i] <= path[i] <= right_bound[i] - 用 QP 求解器（如 OSQP、qpOASES）求解
核心思路：把连续的多项式路径离散成有限个点，每个点加上边界约束，然后用优化器求解。这样既保留了多项式的平滑性，又满足了道路边界限制。
4.3 代价函数设计：平衡的艺术
路径优化不能只满足约束，还得「好」。什么是好？不同场景定义不同。代价函数就是用来量化这个「好」的。
我一般把代价函数分成三部分：
| 代价项 | 含义 | 典型权重 |
|---|---|---|
| 平滑性代价 | 路径曲率变化小，乘坐舒适 | 0.5 ~ 1.0 |
| 偏离代价 | 尽量靠近道路中心线 | 0.1 ~ 0.3 |
| 障碍物代价 | 远离障碍物 | 1.0 ~ 5.0 |
代价函数的设计有个经验：权重不要拍脑袋定。我习惯先跑几组仿真，观察不同权重下的路径形态，再根据实际需求调整。比如在高速场景，平滑性权重可以高一些；在城区狭窄路段，偏离代价要适当降低，否则路径会过于居中导致无法避障。
一个典型的代价函数长这样：
// 代价函数示例
double cost = 0.0;
// 1. 平滑性代价：曲率变化率
for (int i = 1; i < N-1; i++) {
double curvature = computeCurvature(path[i-1], path[i], path[i+1]);
cost += w_smooth * curvature * curvature;
}
// 2. 偏离代价：到中心线的距离
for (int i = 0; i < N; i++) {
double deviation = distanceToCenterLine(path[i]);
cost += w_dev * deviation * deviation;
}
// 3. 障碍物代价：到最近障碍物的距离
for (int i = 0; i < N; i++) {
double obsDist = distanceToNearestObstacle(path[i]);
if (obsDist < safeDist) {
cost += w_obs * (safeDist - obsDist) * (safeDist - obsDist);
}
}
小技巧：代价函数中的权重最好归一化处理。比如平滑性代价的量级可能是 10⁻³，偏离代价是 10⁰，如果不归一化，权重调起来会很痛苦。我一般先跑一次无权重优化，统计各项代价的均值，然后用均值做归一化。
4.4 知识体系总览
下面这张图是我自己总结的路径规划核心逻辑，大家可以对照着理解：
注意：多项式阶数不是越高越好。我见过有人用七次、九次多项式，结果路径出现剧烈振荡。五次多项式在大多数场景下已经够用。如果觉得不够灵活，可以考虑分段三次样条。
好了，这一章的内容就到这里。多项式生成、边界约束、代价函数，这三块是路径规划的基石。我个人觉得，理解它们之间的关系比死记公式更重要。下次遇到路径规划问题，不妨先画个图，把这三块拆开想清楚，再动手写代码。
可可精品资料库