当前位置：首页 > news >正文

路径平滑优化详解(二次规划): 数学建模与目标函数推导

news 2025/10/14 11:01:02

在自动驾驶或移动机器人中，路径平滑（Path Smoothing） 是轨迹规划中非常重要的一环。
基于栅格地图生成的参考线往往存在噪声、不连续或几何突变等问题，而车辆执行层（如 MPC 或 Pure Pursuit）需要连续、光滑且可导的轨迹。

一种常见做法是将路径平滑问题建模为 二次规划（Quadratic Programming, QP），
$l≤Ax≤u\min \frac{1}{2}x^T P x + f^T x \quad \text{s.t. } l \le A x \le u$

在路径优化当中，不考虑速度、加速度优化，通常会考虑优化路径的光滑性、紧凑性以及相似性。根据以上三个优化项，构造以下目标函数，利用优化算法最小化以下目标：

$min_{x,y} J = w_{smooth}J_1 + w_{length}J_2 + w_{refer}J_3$

其中：

$J_1$ ：平滑代价（保证曲率变化平稳）, 用于最小化加速度变化
$J_2$ ：路径紧凑代价（避免过长或不均匀）, 用于控制相邻点距离
$J_3$ ：几何相似代价（保持在原参考线附近）, 对应偏离参考线的惩罚
$w_{smooth}$ , $w_{length}$ , $w_{refer}$ ：对应权重

根据目标函数，使用 OSQP 优化器来求解该问题。

对于参考线离散点 $X_{ref}$ ： ${(x_i, y_i)}$ ，我们希望优化后的路径点 ${(x_i', y_i')}$ 满足以下性质：

1️⃣ 平滑项：
$J1=∑i[(xi+2′−2xi+1′+xi′)2+(yi+2′−2yi+1′+yi′)2]J_1 = \sum_i \left[(x_{i+2}' - 2x_{i+1}' + x_i')^2 + (y_{i+2}' - 2y_{i+1}' + y_i')^2\right]$
代表二阶差分（曲率变化）最小化，使曲线更平滑。曲线平滑计算由相邻3个点计算，所以当路径点有n个时，该项代价由n-2项组成。在构造平滑代价权重时需要注意，待优化变量数为2 * n，而平滑代价权重的行数为2 * n - 4。

为什么是-4？
在计算平滑代价时，路径的首尾两个点是没有前驱后继点的，只能计算中间点，而每个点都包含两个待优化的变量位，所以共计4个。

对应的部分代码

Eigen::SparseMatrix<double> P_smooth(num_var - 4, num_var);  // num_var = 2 * n
for (int row = 0; row < num_var - 4; row += 2) {// 对应x轴部分的矩阵赋值P_smooth.insert(row, row) = 1.0;P_smooth.insert(row, row + 2) = -2.0;P_smooth.insert(row, row + 4) = 1.0;// 对应y轴部分的矩阵赋值P_smooth.insert(row + 1, row + 1) = 1.0;P_smooth.insert(row + 1, row + 3) = -2.0;P_smooth.insert(row + 1, row + 5) = 1.0;
}

2️⃣ 长度项：
$J2=∑i[(xi+1′−xi′)2+(yi+1′−yi′)2]J_2 = \sum_i \left[(x_{i+1}' - x_i')^2 + (y_{i+1}' - y_i')^2\right]$
约束相邻点间距，保证路径紧凑，即路径尽量短。
部分代码如下所示：

// 一阶差分（长度项）
Eigen::SparseMatrix<double> P_length(num_var - 2, num_var);
for (int row = 0; row < num_var - 2; row += 2) {P_length.insert(row, row) = 1.0;P_length.insert(row, row + 2) = -1.0;P_length.insert(row + 1, row + 1) = 1.0;P_length.insert(row + 1, row + 3) = -1.0;
}

3️⃣ 参考项：
$J3=∑i[(xi′−xi)2+(yi′−yi)2]J_3 = \sum_i \left[(x_i' - x_i)^2 + (y_i' - y_i)^2\right]$
保持与原始参考线的几何一致性。
部分代码所示：

// 单位矩阵（参考项）
Eigen::SparseMatrix<double> P_ref(num_var, num_var);
for (int row = 0; row < num_var; ++row)P_ref.insert(row, row) = 1.0;

最终构造优化目标：
$min⁡x′12x′TPx′+fTx′\min_{x'} \frac{1}{2}x'^{T} P x' + f^{T}x'$
这就是标准的 QP 目标形式。

针对目标函数，其对应的P矩阵为：
$P = 2(w_{smooth} P_{smooth}^T P_{smooth} + w_{length} P_{length}^T P_{length} + w_{ref} P_{ref}^T P_{ref})$
保证P是对称正定矩阵，使得问题凸且可解。
f矩阵为：
$f = -2 * w_{refer} * X_{ref};$

构造线性约束：
$\le x \le u$

$l$ 表示 lower_bound
$u$ 表示upper_bound

在路径优化当中，不考虑速度时，线性约束基本是将优化后的曲线框定在对应点的一个长方形内，长方形的大小由对应的上下边界决定。对应的线性约束(边界约束)为：
$xi′∈[xi−buffer,xi+buffer]x_i' \in [x_i - buffer, x_i + buffer]$
代表每个点可以在参考线附近 ±buffer 米范围内调整。