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

在自动驾驶或移动机器人中，路径平滑（Path Smoothing） 是轨迹规划中非常重要的一环。

基于栅格地图生成的参考线往往存在噪声、不连续或几何突变等问题，而车辆执行层（如 MPC 或 Pure Pursuit）需要连续、光滑且可导的轨迹。

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

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

min ⁡ x , y J = w s m o o t h J 1 + w l e n g t h J 2 + w r e f e r J 3 \min_{x,y} J = w_{smooth}J_1 + w_{length}J_2 + w_{refer}J_3 x,yminJ=wsmoothJ1+wlengthJ2+wreferJ3

其中：

J 1 J_1 J1 ：平滑代价（保证曲率变化平稳）, 用于最小化加速度变化
J 2 J_2 J2 ：路径紧凑代价（避免过长或不均匀）, 用于控制相邻点距离
J 3 J_3 J3 ：几何相似代价（保持在原参考线附近）, 对应偏离参考线的惩罚
w s m o o t h w_{smooth} wsmooth, w l e n g t h w_{length} wlength, w r e f e r w_{refer} wrefer：对应权重

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

对于参考线离散点 X r e f X_{ref} Xref ： ( x i , y i ) {(x_i, y_i)} (xi,yi)，我们希望优化后的路径点 ( x i ′ , y i ′ ) {(x_i', y_i')} (xi′,yi′)满足以下性质：

1️⃣ 平滑项：
J 1 = ∑ i $( x i + 2 ' - 2 x i + 1 ' + x i ' ) 2 + ( y i + 2 ' - 2 y i + 1 ' + y i ' ) 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$ J1=i∑ $(xi+2'-2xi+1'+xi')2+(yi+2'-2yi+1'+yi')2$

代表二阶差分（曲率变化）最小化，使曲线更平滑。曲线平滑计算由相邻3个点计算，所以当路径点有n个时，该项代价由n-2项组成。在构造平滑代价权重时需要注意，待优化变量数为2 * n，而平滑代价权重的行数为2 * n - 4。

为什么是-4？

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

对应的部分代码

C++ 复制代码

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️⃣ 长度项：
J 2 = ∑ i $( x i + 1 ' - x i ' ) 2 + ( y i + 1 ' - y i ' ) 2$ J_2 = \sum_i \left $(x_{i+1}' - x_i')\^2 + (y_{i+1}' - y_i')\^2\\right$ J2=i∑ $(xi+1'-xi')2+(yi+1'-yi')2$

约束相邻点间距，保证路径紧凑，即路径尽量短。

部分代码如下所示：

C++ 复制代码

// 一阶差分（长度项）
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️⃣ 参考项：
J 3 = ∑ i $( x i ' - x i ) 2 + ( y i ' - y i ) 2$ J_3 = \sum_i \left $(x_i' - x_i)\^2 + (y_i' - y_i)\^2\\right$ J3=i∑ $(xi'-xi)2+(yi'-yi)2$

保持与原始参考线的几何一致性。

部分代码所示：

C++ 复制代码

// 单位矩阵（参考项）
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 ′ 1 2 x ′ T P x ′ + f T x ′ \min_{x'} \frac{1}{2}x'^{T} P x' + f^{T}x' x′min21x′TPx′+fTx′

这就是标准的 QP 目标形式。

针对目标函数，其对应的P矩阵为：
P = 2 ( w s m o o t h P s m o o t h T P s m o o t h + w l e n g t h P l e n g t h T P l e n g t h + w r e f P r e f T P r e f ) P = 2(w_{smooth} P_{smooth}^T P_{smooth} + w_{length} P_{length}^T P_{length} + w_{ref} P_{ref}^T P_{ref}) P=2(wsmoothPsmoothTPsmooth+wlengthPlengthTPlength+wrefPrefTPref)

保证P是对称正定矩阵，使得问题凸且可解。
f矩阵为：
f = − 2 ∗ w r e f e r ∗ X r e f ; f = -2 * w_{refer} * X_{ref}; f=−2∗wrefer∗Xref;

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

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

在路径优化当中，不考虑速度时，线性约束基本是将优化后的曲线框定在对应点的一个长方形内，长方形的大小由对应的上下边界决定。对应的线性约束(边界约束)为：
x i ′ ∈ $x i - b u f f e r , x i + b u f f e r$ x_i' \in $x_i - buffer, x_i + buffer$ xi′∈ $xi-buffer,xi+buffer$

代表每个点可以在参考线附近 ±buffer 米范围内调整。

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

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