【路径规划】OSQP曲线平滑 公式及代码

参考与前言

---

1. apollo 代码：https://github.com/ApolloAuto/apollo/tree/master/modules/planning/math/smoothing_spline

2. apollo readme：https://github.com/ApolloAuto/apollo/blob/master/docs/specs/qp_spline_path_optimizer.md

3. 自我测试的代码：张聪明/OSQP_test (gitee.com)

本篇也主要基于参考二翻译而来，背景是因为 GPIR代码 里有这层做初始化，不太懂怎么干的和干啥，所以就搜索了一下，一开始一直在弄参考三使其能逐步断点debug 以让我能明了整个步骤，不过后来搜了一下看到了参考二 emm 就是我想要了解的 smooth到底是怎样编程二次规划问题的。

以下大部分翻译至参考二，其中添加了自己的思考句子（有时也可能就是问题

实际这个包的名字就：QP-Spline-Path Optimizer，基于二次规划的曲线路径优化器

方法：Quadratic programming + Spline interpolation，二次规划 + 样条差值

注意这全文都是讲的path，而不是trajectory，至于两者区别：Motion Planning 是什么 总结了下：

Path是静态的，不包括时间；Trajectory是除了路径还有其他信息可以说是时间，speed profile，这条路径上速度的变化关系

这一段是我看完了写的：emmm 看完了发现包装的太好了 以至于就算看了整个公式 对应起来都是要跳转进去的 果然是做软件一套的。但是想着用的话 这样也算是够了的。所以整体其实还是拿五次样条先生成了reference point 然后根据他们的s,l点来进行约束，cost function也已经给出

① 目标函数 Objective function

路径长度

首先路径的坐标系是以SL 也就是frenet坐标系下。其中 s 是从车辆当前位置到规划路径的长度，类似于我们会提前给定一个 我们要规划多长的距离。

样条线段

将路径拆分成 n 段。每段都用多项式表示，比如这样一个式子：

每段 i 沿着参考线的累积距离 (d_i) 的多项式函数公式如下：

[l = f_i(s) = a_{i0} + a_{i1} cdot s + a_{i2} cdot s^2 + a_{i3} cdot s^3 + a_{i4} cdot s^4 + a_{i5} cdot s^5 (0 leq s leq d_{i}) ag{1} ]

实际代码调用时，后者的 5 也就是五次多项式的意思

OsqpSpline2dSolver osqp_spline2d_solver(t_knots, 5);

线段的优化函数

也就是一个cost 和为速度，加速度，加加速度：

[egin{align} cost = sum_{i=1}^{n} Big( w_1 cdot intlimits_{0}^{d_i} (f_i')^2(s) ds + w_2 cdot intlimits_{0}^{d_i} (f_i'')^2(s) ds + w_3 cdot intlimits_{0}^{d_i} (f_i^{primeprimeprime})^2(s) ds Big) end{align} ]

但实际test.cc这个代码中只对加加速度进行了weight赋值，所以最后在计算曲率 curvature的时候 smooth后的效果明显好很多（虽然点之间看不出来啥）

mutable_kernel->Add2dThirdOrderDerivativeMatrix(0.5);

将cost函数转为QP形式

QP formulation:

[egin{aligned} minimize & frac{1}{2} cdot x^T cdot H cdot x + f^T cdot x \ s.t. qquad & LB leq x leq UB \ & A_{eq}x = b_{eq} \ & Ax geq b end{aligned} ]

下面是如何被优化函数objective function/cost function转成QP形式，首先我们 (1)式 可以转成向量相乘的形式，那么 (f_i(s)) 可以这样表示

[f_i(s) ＝ egin{vmatrix} 1 & s & s^2 & s^3 & s^4 & s^5 end{vmatrix} cdot egin{vmatrix} a_{i0} \ a_{i1} \ a_{i2} \ a_{i3} \ a_{i4} \ a_{i5} end{vmatrix} ]

(f_i'(s)) 求导再次表示

[f_i'(s) = egin{vmatrix} 0 & 1 & 2s & 3s^2 & 4s^3 & 5s^4 end{vmatrix} cdot egin{vmatrix} a_{i0} \ a_{i1} \ a_{i2} \ a_{i3} \ a_{i4} \ a_{i5} end{vmatrix} ]

(f_i'(s)^2) 两个相乘一下：

[f_i'(s)^2 = egin{vmatrix} a_{i0} & a_{i1} & a_{i2} & a_{i3} & a_{i4} & a_{i5} end{vmatrix} cdot egin{vmatrix} 0 \ 1 \ 2s \ 3s^2 \ 4s^3 \ 5s^4 end{vmatrix} cdot egin{vmatrix} 0 & 1 & 2s & 3s^2 & 4s^3 & 5s^4 end{vmatrix} cdot egin{vmatrix} a_{i0} \ a_{i1} \ a_{i2} \ a_{i3} \ a_{i4} \ a_{i5} end{vmatrix} ]

然后就会获得这样的：

[intlimits_{0}^{d_i} f_i'(s)^2 ds ＝ intlimits_{0}^{d_i} egin{vmatrix} a_{i0} & a_{i1} & a_{i2} & a_{i3} & a_{i4} & a_{i5} end{vmatrix} cdot egin{vmatrix} 0 \ 1 \ 2s \ 3s^2 \ 4s^3 \ 5s^4 end{vmatrix} cdot egin{vmatrix} 0 & 1 & 2s & 3s^2 & 4s^3 & 5s^4 end{vmatrix} cdot egin{vmatrix} a_{i0} \ a_{i1} \ a_{i2} \ a_{i3} \ a_{i4} \ a_{i5} end{vmatrix} ds ]

把常数项提到积分外面去：

[intlimits_{0}^{d_i} f'(s)^2 ds ＝ egin{vmatrix} a_{i0} & a_{i1} & a_{i2} & a_{i3} & a_{i4} & a_{i5} end{vmatrix} cdot intlimits_{0}^{d_i} egin{vmatrix} 0 \ 1 \ 2s \ 3s^2 \ 4s^3 \ 5s^4 end{vmatrix} cdot egin{vmatrix} 0 & 1 & 2s & 3s^2 & 4s^3 & 5s^4 end{vmatrix} ds cdot egin{vmatrix} a_{i0} \ a_{i1} \ a_{i2} \ a_{i3} \ a_{i4} \ a_{i5} end{vmatrix} ]

[＝egin{vmatrix} a_{i0} & a_{i1} & a_{i2} & a_{i3} & a_{i4} & a_{i5} end{vmatrix} cdot intlimits_{0}^{d_i} egin{vmatrix} 0 & 0 &0&0&0&0\ 0 & 1 & 2s & 3s^2 & 4s^3 & 5s^4\ 0 & 2s & 4s^2 & 6s^3 & 8s^4 & 10s^5\ 0 & 3s^2 & 6s^3 & 9s^4 & 12s^5&15s^6 \ 0 & 4s^3 & 8s^4 &12s^5 &16s^6&20s^7 \ 0 & 5s^4 & 10s^5 & 15s^6 & 20s^7 & 25s^8 end{vmatrix} ds cdot egin{vmatrix} a_{i0} \ a_{i1} \ a_{i2} \ a_{i3} \ a_{i4} \ a_{i5} end{vmatrix} ]

然后给矩阵里的都积分一下从0到(d_i) 积分一下 就可以获得这样的：

[intlimits_{0}^{d_i} f'_i(s)^2 ds =egin{vmatrix} a_{i0} & a_{i1} & a_{i2} & a_{i3} & a_{i4} & a_{i5} end{vmatrix} cdot egin{vmatrix} 0 & 0 & 0 & 0 &0&0\ 0 & d_i & d_i^2 & d_i^3 & d_i^4&d_i^5\ 0& d_i^2 & frac{4}{3}d_i^3& frac{6}{4}d_i^4 & frac{8}{5}d_i^5&frac{10}{6}d_i^6\ 0& d_i^3 & frac{6}{4}d_i^4 & frac{9}{5}d_i^5 & frac{12}{6}d_i^6&frac{15}{7}d_i^7\ 0& d_i^4 & frac{8}{5}d_i^5 & frac{12}{6}d_i^6 & frac{16}{7}d_i^7&frac{20}{8}d_i^8\ 0& d_i^5 & frac{10}{6}d_i^6 & frac{15}{7}d_i^7 & frac{20}{8}d_i^8&frac{25}{9}d_i^9 end{vmatrix} cdot egin{vmatrix} a_{i0} \ a_{i1} \ a_{i2} \ a_{i3} \ a_{i4} \ a_{i5} end{vmatrix} ]

所以现在应该发现了：如果是五次样条曲线的导数cost函数是会获得一个(6 imes6)的矩阵

② 约束 Constraints

初始点/end点约束

init point/初始点

假设第一个点是 ((s_0), (l_0)), ((s_0), (l'_0)) and ((s_0), (l''_0)), 其中 (l_0) , (l'_0) and (l''_0) 是规划初始点的横向offset及其一阶导、二阶导，可以由 (f_i(s)), (f'_i(s)), and (f_i(s)'') 计算得知

使用这个式子，将这些约束转为QP等式约束

[A_{eq}x = b_{eq} ]

下面就是转换的步骤：

[f_i(s_0) = egin{vmatrix} 1 & s_0 & s_0^2 & s_0^3 & s_0^4&s_0^5 end{vmatrix} cdot egin{vmatrix} a_{i0} \ a_{i1} \ a_{i2} \ a_{i3} \ a_{i4} \ a_{i5}end{vmatrix} = l_0 ]

[f'_i(s_0) = egin{vmatrix} 0& 1 & 2s_0 & 3s_0^2 & 4s_0^3 &5 s_0^4 end{vmatrix} cdot egin{vmatrix} a_{i0} \ a_{i1} \ a_{i2} \ a_{i3} \ a_{i4} \ a_{i5} end{vmatrix} = l'_0 ]

[f''_i(s_0) = egin{vmatrix} 0&0& 2 & 3 imes2s_0 & 4 imes3s_0^2 & 5 imes4s_0^3 end{vmatrix} cdot egin{vmatrix} a_{i0} \ a_{i1} \ a_{i2} \ a_{i3} \ a_{i4} \ a_{i5} end{vmatrix} = l''_0 ]

其中 (i) 是指那段拥有初始点 (s_0) 的分段区，应该说是最初的那段，因为后面平滑约束的时候还有 (f_{k+1}(s_0))上面三个一起形成了一个等式，即最后的形式如 式(2).

end point/结束点

与上述的initial point 初始点步骤一致，结束点 ((s_e, l_e)) 也是已知的，而且应该是和前面的约束一致的形式，所以总一总可以得出

等式约束 (A_{eq}x = b_{eq}) 展开为以下形式

[egin{vmatrix} 1 & s_0 & s_0^2 & s_0^3 & s_0^4&s_0^5 \ 0&1 & 2s_0 & 3s_0^2 & 4s_0^3 & 5s_0^4 \ 0& 0&2 & 3 imes2s_0 & 4 imes3s_0^2 & 5 imes4s_0^3 \ 1 & s_e & s_e^2 & s_e^3 & s_e^4&s_e^5 \ 0&1 & 2s_e & 3s_e^2 & 4s_e^3 & 5s_e^4 \ 0& 0&2 & 3 imes2s_e & 4 imes3s_e^2 & 5 imes4s_e^3 end{vmatrix} cdot egin{vmatrix} a_{i0} \ a_{i1} \ a_{i2} \ a_{i3} \ a_{i4} \ a_{i5} end{vmatrix} = egin{vmatrix} l_0\ l'_0\ l''_0\ l_e\ l'_e\ l''_e\ end{vmatrix} ag{2} ]

实际代码中为：

// init point constraint
mutable_constraint->Add2dPointConstraint(0, Eigen::Vector2d(spline(a, 0), spline(b, 0)));
mutable_constraint->Add2dPointDerivativeConstraint(0, Eigen::Vector2d(spline_1st(a, 0), spline_1st(b, 0)));

// end point constraint
mutable_constraint->Add2dPointConstraint(t_end, Eigen::Vector2d(spline(a, t_end), spline(b, t_end)));
mutable_constraint->Add2dPointDerivativeConstraint(t_end, Eigen::Vector2d(spline_1st(a, t_end), spline_1st(b, t_end)));

分段之间平滑约束

这部分的约束主要在于将分段点连接处进行平滑操作，比如一阶连续，二阶连续，三阶连续。

假设 (seg_k) 和 (seg_{k+1}) 是相连接的, (seg_k) 段累积的 s 距离值为 (s_k). 注意这里的 (s_0) 不同与前面说的为整段路径的init point初始点，而是每段自身的起点处，所以对 (seg_k) 来说他的累积距离 (s_k) 正是 (seg_{k+1}) 的起点 (s_0)

[f_k(s_k) = f_{k+1} (s_0) ]

以下为计算过程：

[egin{vmatrix} 1 & s_k & s_k^2 & s_k^3 & s_k^4&s_k^5 \ end{vmatrix} cdot egin{vmatrix} a_{k0} \ a_{k1} \ a_{k2} \ a_{k3} \ a_{k4} \ a_{k5} end{vmatrix} = egin{vmatrix} 1 & s_{0} & s_{0}^2 & s_{0}^3 & s_{0}^4&s_{0}^5 \ end{vmatrix} cdot egin{vmatrix} a_{k+1,0} \ a_{k+1,1} \ a_{k+1,2} \ a_{k+1,3} \ a_{k+1,4} \ a_{k+1,5} end{vmatrix} ]

[egin{vmatrix} 1 & s_k & s_k^2 & s_k^3 & s_k^4&s_k^5 & -1 & -s_{0} & -s_{0}^2 & -s_{0}^3 & -s_{0}^4&-s_{0}^5\ end{vmatrix} cdot egin{vmatrix} a_{k0} \ a_{k1} \ a_{k2} \ a_{k3} \ a_{k4} \ a_{k5} \ a_{k+1,0} \ a_{k+1,1} \ a_{k+1,2} \ a_{k+1,3} \ a_{k+1,4} \ a_{k+1,5} end{vmatrix} = 0 ]

使用 (s_0) = 0 在上式中，然后同样添加如下的等式约束：

[f'_k(s_k) = f'_{k+1} (s_0) \ f''_k(s_k) = f''_{k+1} (s_0) \ f'''_k(s_k) = f'''_{k+1} (s_0) ]

实际代码中只添加了三阶连续：(但是三阶连续的前提不是一二阶也连续吗？)

mutable_constraint->Add2dThirdDerivativeSmoothConstraint();

对采样点的边界约束

沿着路径均匀采样 m 个点，检查这些点是否触碰到了障碍物的边界，然后将其转成QP问题的不等式约束：

[Ax geq b ]

首先根据道路宽度和周围障碍物找到点 ((s_j, l_j)) and (jin[0, m]) 的下边界 (l_{lb,j})，然后不等式约束的计算如下：

[egin{vmatrix} 1 & s_0 & s_0^2 & s_0^3 & s_0^4&s_0^5 \ 1 & s_1 & s_1^2 & s_1^3 & s_1^4&s_1^5 \ ...&...&...&...&...&... \ 1 & s_m & s_m^2 & s_m^3 & s_m^4&s_m^5 \ end{vmatrix} cdot egin{vmatrix}a_{i0} \ a_{i1} \ a_{i2} \ a_{i3} \ a_{i4} \ a_{i5} end{vmatrix} geq egin{vmatrix} l_{lb,0}\ l_{lb,1}\ ...\ l_{lb,m}\ end{vmatrix} ag{10} ]

• [x] 为什么会有下界？根据采样的想法应该只有上界才对，或者说下界可以为完全不进行横向采样扩张

  杰哥回复：这是一个box形状，不是说从0-10是我的采样增加区间 而是以ref为0，-10 - 10采样增加区间

• [x] 为什么实际代码中是横纵向都有限制？采样点扩张如上问中

      lower_bound.emplace_back(d_lateral - lat_tol[i]);
      lower_bound.emplace_back(d_longitudinal - lon_tol[i]);
  
      upper_bound.emplace_back(d_lateral + lat_tol[i]);
      upper_bound.emplace_back(d_longitudinal + lon_tol[i]);
  

  如此手绘图所示：

  

• [x] 这里没有办法根据障碍物加进去边界条件？

  好像可以对某一个 (l_{ub,j}) 进行扩大，使其表达障碍物的膨胀。但是我感觉这个点应该是说已知环境中，不包括动态障碍物，类似于在地图已经建好的这层上去做边界约束，而不是实时根据障碍物去做约束

  这样的 因为输入的边界约束是station lateral boundary，其实是依据你的reference line 参考线然后有设定纵向和横向的最大便宜角度进去，比如test代码中是0.2

同样的，上界 (l_{ub,j}) 同样也可以以这种不等式约束计算：

[egin{vmatrix} -1 & -s_0 & -s_0^2 & -s_0^3 & -s_0^4&-s_0^5 \ -1 & -s_1 & -s_1^2 & -s_1^3 & -s_1^4&-s_1^5 \ ...&...-&...&...&...&... \ -1 & -s_m & -s_m^2 & -s_m^3 & -s_m^4&-s_m^5 \ end{vmatrix} cdot egin{vmatrix} a_{i0} \ a_{i1} \ a_{i2} \ a_{i3} \ a_{i4} \ a_{i5} end{vmatrix} geq -1 cdot egin{vmatrix} l_{ub,0}\ l_{ub,1}\ ...\ l_{ub,m}\ end{vmatrix} ag{11} ]

③ 代码总结

首先完整的test代码可见gitee链接，此处仅将上述提到的进行说明

  // solver
  OsqpSpline2dSolver osqp_spline2d_solver(t_knots, 5);

  mutable_kernel->Add2dReferenceLineKernelMatrix(t_coord, ref_ft, 0.5);

  // 添加连续平滑约束 三阶
  mutable_constraint->Add2dThirdDerivativeSmoothConstraint();
  
  // 添加初始点
  mutable_constraint->Add2dPointConstraint(0, Eigen::Vector2d(spline(a, 0), spline(b, 0)));
  mutable_constraint->Add2dPointDerivativeConstraint(0, Eigen::Vector2d(spline_1st(a, 0), spline_1st(b, 0)));
  
  // 添加end point
  double t_end = t_coord.back();
  mutable_constraint->Add2dPointConstraint(t_end, Eigen::Vector2d(spline(a, t_end), spline(b, t_end)));
  mutable_constraint->Add2dPointDerivativeConstraint(t_end, Eigen::Vector2d(spline_1st(a, t_end), spline_1st(b, t_end)));

  // 添加边界约束
  mutable_constraint->Add2dStationLateralBoundary(t_coord, ref_ft, ref_theta,lon_tol, lat_tol);

实际上看上去 数学公式都被隐去了 是因为osqp这层 apollo加了自己的spline，所以最后main呈现的就是这样的形式，如果跳转各个部分的更为仔细的可以发现更多，比如添加边界约束：

bool Spline2dConstraint::Add2dStationLateralBoundary(
    const std::vector<double>& t, const vector_Eigen<Eigen::Vector2d>& ref_xy,
    const std::vector<double>& ref_theta, const std::vector<double>& lon_tol,
    const std::vector<double>& lat_tol) {
  if (t.size() != ref_xy.size() || ref_xy.size() != ref_theta.size() ||
      ref_theta.size() != lon_tol.size() || lon_tol.size() != lat_tol.size()) {
    return false;
  }

  Eigen::MatrixXd sl_constraints =
      Eigen::MatrixXd::Zero(2 * t.size(), total_param_);
  std::vector<double> lower_bound, upper_bound;

  for (uint32_t i = 0; i < t.size(); ++i) {
    const uint32_t index = FindSegStartIndex(t[i]);
    const double d_lateral = SignDistance(ref_xy[i], ref_theta[i]);
    const double d_longitudinal =
        SignDistance(ref_xy[i], ref_theta[i] - M_PI_2);
    const double t_corrected = t[i] - t_knots_[index];

    std::vector<double> lateral_coef = AffineCoef(ref_theta[i], t_corrected);
    std::vector<double> longitudianl_coef =
        AffineCoef(ref_theta[i] - M_PI_2, t_corrected);

    const uint32_t index_offset = index * spline_param_num_;

    // 构建公式10,11的左边矩阵
    for (uint32_t j = 0; j < spline_param_num_; ++j) {
      sl_constraints(2 * i, index_offset + j) = lateral_coef[j];
      sl_constraints(2 * i, index_offset + total_param_ / 2 + j) =
          lateral_coef[spline_param_num_ + j];
      sl_constraints(2 * i + 1, index_offset + j) = longitudianl_coef[j];
      sl_constraints(2 * i + 1, index_offset + total_param_ / 2 + j) =
          longitudianl_coef[spline_param_num_ + j];
    }

    lower_bound.emplace_back(d_lateral - lat_tol[i]);
    lower_bound.emplace_back(d_longitudinal - lon_tol[i]);

    upper_bound.emplace_back(d_lateral + lat_tol[i]);
    upper_bound.emplace_back(d_longitudinal + lon_tol[i]);
  }

  return coxy_constraint_.AddConstraint(sl_constraints, lower_bound,
                                        upper_bound);
}

由这里我们可以看到公式 10 和公式 11的矩阵形式的构建

④ 效果示意

运行上面给的Gitee代码，可以得到下面这样一幅图：

ref就是每小段都是五次样条出来的，做图就是sl坐标系下生成出来的，右图为计算了他们的每个点连接处的曲率，可以看到经过osqp spline曲率约束 cost 都有明显的生效（虽然在这个例子中展现的不大）