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

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

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

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

$$\underset{x,y}{\min }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^{\prime },y_i^{\prime })$满足以下性质：

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

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

对应的部分代码

Eigen::SparseMatrix 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️⃣ 长度项：
$$J_2=\sum _i\left[(x_{i+1}^{\prime }-x_i^{\prime }{)}^2+(y_{i+1}^{\prime }-y_i^{\prime }{)}^2\right]$$
约束相邻点间距，保证路径紧凑，即路径尽量短。
部分代码如下所示：

// 一阶差分（长度项）
Eigen::SparseMatrix 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️⃣ 参考项：
$$J_3=\sum _i\left[(x_i^{\prime }-x_i{)}^2+(y_i^{\prime }-y_i{)}^2\right]$$
保持与原始参考线的几何一致性。
部分代码所示：

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

最终构造优化目标：
$$\underset{x^{\prime }}{\min }\frac{1}{2}{x}^{\prime T}P{x}^{\prime }+f^T{x}^{\prime }$$
这就是标准的 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\ast w_{refer}\ast X_{ref};$$

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

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

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

---

以上内容就构造好了一个二次优化问题的所需信息，然后将对应的矩阵输入到OSQP优化器当中，就可以得到优化后的路径解了

如果你项了解更多的代码详情，可以参考开源项目：

• osqp-eigen 官方示例
• Apollo Path Optimizer 源码分析