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

Kin-Zhang

已于 2024-04-22 18:59:19 修改

阅读量3.4k

点赞数 5

分类专栏： Self-Driving 无人驾驶文章标签：无人驾驶

于 2021-11-10 19:17:02 首次发布

本文链接：https://blog.csdn.net/qq_39537898/article/details/121252690

版权

Self-Driving 无人驾驶专栏收录该内容

16 篇文章 73 订阅

订阅专栏

参考与前言

apollo 代码：https://github.com/ApolloAuto/apollo/tree/master/modules/planning/math/smoothing_spline
apollo readme：https://github.com/ApolloAuto/apollo/blob/master/docs/specs/qp_spline_path_optimizer.md
自我测试的代码：张聪明/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也已经给出

优美格式及后续错误更新见：https://www.cnblogs.com/kin-zhang/p/15532655.html
谁让… 这边广告弹出等太多了… 哎

① 目标函数 Objective function

路径长度

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

样条线段

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

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

$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}) \tag{1}$
实际代码调用时，后者的 5 也就是五次多项式的意思

OsqpSpline2dSolver osqp_spline2d_solver(t_knots, 5);

线段的优化函数

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

$\begin{align} cost = \sum_{i=1}^{n} \Big( w_1 \cdot \int\limits_{0}^{d_i} (f_i')^2(s) ds + w_2 \cdot \int\limits_{0}^{d_i} (f_i'')^2(s) ds + w_3 \cdot \int\limits_{0}^{d_i} (f_i^{\prime\prime\prime})^2(s) ds \Big) \end{align}$

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

mutable_kernel->Add2dThirdOrderDerivativeMatrix(0.5);

将cost函数转为QP形式

QP formulation:

$\begin{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) ＝ \begin{vmatrix} 1 & s & s^2 & s^3 & s^4 & s^5 \end{vmatrix} \cdot \begin{vmatrix} a_{i0} \\ a_{i1} \\ a_{i2} \\ a_{i3} \\ a_{i4} \\ a_{i5} \end{vmatrix}$

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

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

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

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

然后就会获得这样的：

$\int\limits_{0}^{d_i} f_i'(s)^2 ds ＝ \int\limits_{0}^{d_i} \begin{vmatrix} a_{i0} & a_{i1} & a_{i2} & a_{i3} & a_{i4} & a_{i5} \end{vmatrix} \cdot \begin{vmatrix} 0 \\ 1 \\ 2s \\ 3s^2 \\ 4s^3 \\ 5s^4 \end{vmatrix} \cdot \begin{vmatrix} 0 & 1 & 2s & 3s^2 & 4s^3 & 5s^4 \end{vmatrix} \cdot \begin{vmatrix} a_{i0} \\ a_{i1} \\ a_{i2} \\ a_{i3} \\ a_{i4} \\ a_{i5} \end{vmatrix} ds$

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

$\int\limits_{0}^{d_i} f'(s)^2 ds ＝ \begin{vmatrix} a_{i0} & a_{i1} & a_{i2} & a_{i3} & a_{i4} & a_{i5} \end{vmatrix} \cdot \int\limits_{0}^{d_i} \begin{vmatrix} 0 \\ 1 \\ 2s \\ 3s^2 \\ 4s^3 \\ 5s^4 \end{vmatrix} \cdot \begin{vmatrix} 0 & 1 & 2s & 3s^2 & 4s^3 & 5s^4 \end{vmatrix} ds \cdot \begin{vmatrix} a_{i0} \\ a_{i1} \\ a_{i2} \\ a_{i3} \\ a_{i4} \\ a_{i5} \end{vmatrix}$
$＝\begin{vmatrix} a_{i0} & a_{i1} & a_{i2} & a_{i3} & a_{i4} & a_{i5} \end{vmatrix} \cdot \int\limits_{0}^{d_i} \begin{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 \begin{vmatrix} a_{i0} \\ a_{i1} \\ a_{i2} \\ a_{i3} \\ a_{i4} \\ a_{i5} \end{vmatrix}$

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

$\int\limits_{0}^{d_i} f'_i(s)^2 ds =\begin{vmatrix} a_{i0} & a_{i1} & a_{i2} & a_{i3} & a_{i4} & a_{i5} \end{vmatrix} \cdot \begin{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 \begin{vmatrix} a_{i0} \\ a_{i1} \\ a_{i2} \\ a_{i3} \\ a_{i4} \\ a_{i5} \end{vmatrix}$

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

② 约束 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) = \begin{vmatrix} 1 & s_0 & s_0^2 & s_0^3 & s_0^4&s_0^5 \end{vmatrix} \cdot \begin{vmatrix} a_{i0} \\ a_{i1} \\ a_{i2} \\ a_{i3} \\ a_{i4} \\ a_{i5}\end{vmatrix} = l_0$

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

$f''_i(s_0) = \begin{vmatrix} 0&0& 2 & 3\times2s_0 & 4\times3s_0^2 & 5\times4s_0^3 \end{vmatrix} \cdot \begin{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}$ 展开为以下形式

$\begin{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\times2s_0 & 4\times3s_0^2 & 5\times4s_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\times2s_e & 4\times3s_e^2 & 5\times4s_e^3 \end{vmatrix} \cdot \begin{vmatrix} a_{i0} \\ a_{i1} \\ a_{i2} \\ a_{i3} \\ a_{i4} \\ a_{i5} \end{vmatrix} = \begin{vmatrix} l_0\\ l'_0\\ l''_0\\ l_e\\ l'_e\\ l''_e\\ \end{vmatrix}\tag{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)$

以下为计算过程：

$\begin{vmatrix} 1 & s_k & s_k^2 & s_k^3 & s_k^4&s_k^5 \\ \end{vmatrix} \cdot \begin{vmatrix} a_{k0} \\ a_{k1} \\ a_{k2} \\ a_{k3} \\ a_{k4} \\ a_{k5} \end{vmatrix} = \begin{vmatrix} 1 & s_{0} & s_{0}^2 & s_{0}^3 & s_{0}^4&s_{0}^5 \\ \end{vmatrix} \cdot \begin{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}$

$\begin{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 \begin{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问题的不等式约束：

$\geq b$

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

$\begin{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 \begin{vmatrix}a_{i0} \\ a_{i1} \\ a_{i2} \\ a_{i3} \\ a_{i4} \\ a_{i5} \end{vmatrix} \geq \begin{vmatrix} l_{lb,0}\\ l_{lb,1}\\ ...\\ l_{lb,m}\\ \end{vmatrix} \tag{10}$

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

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

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

    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]);

如此手绘图所示：

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

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

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

同样的，上界 $l_{ub,j}$ 同样也可以以这种不等式约束计算：
$\begin{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 \begin{vmatrix} a_{i0} \\ a_{i1} \\ a_{i2} \\ a_{i3} \\ a_{i4} \\ a_{i5} \end{vmatrix} \geq -1 \cdot \begin{vmatrix} l_{ub,0}\\ l_{ub,1}\\ ...\\ l_{ub,m}\\ \end{vmatrix} \tag{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 都有明显的生效（虽然在这个例子中展现的不大）

Kin-Zhang

关注

5
点赞
踩
51

收藏

觉得还不错? 一键收藏
打赏
1
评论
【路径规划】OSQP曲线平滑公式及代码

参考与前言apollo 代码：https://github.com/ApolloAuto/apollo/tree/master/modules/planning/math/smoothing_splineapollo readme：https://github.com/ApolloAuto/apollo/blob/master/docs/specs/qp_spline_path_optimizer.md自我测试的代码：张聪明/OSQP_test (gitee.com)本篇也主要基于
复制链接

扫一扫