【路径规划】OSQP曲线平滑 公式及代码
作者：互联网
参考与前言
-
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也已经给出
① 目标函数 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}) \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问题的不等式约束：
\[Ax \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} \]-
[ ] 为什么会有下界？根据采样的想法应该只有上界才对，或者说下界可以为完全不进行横向采样扩张
-
[x] 这里没有办法根据障碍物加进去边界条件？
好像可以对某一个 \(l_{ub,j}\) 进行扩大，使其表达障碍物的膨胀。但是我感觉这个点应该是说已知环境中，不包括动态障碍物，类似于在地图已经建好的这层上去做边界约束，而不是实时根据障碍物去做约束
同样的，上界 \(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 都有明显的生效（虽然在这个例子中展现的不大）
标签：i3,begin,end,cdot,平滑,路径,vmatrix,OSQP,spline 来源： https://www.cnblogs.com/kin-zhang/p/15532655.html