在自动驾驶或移动机器人中，路径平滑（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优化器当中，就可以得到优化后的路径解了
如果你项了解更多的代码详情，可以参考开源项目：