问题描述

在2.4的文章中我们已经求出了一个可行的凸空间

我们需要在这个凸空间内求解一条满足车辆动力学特性、边界约束的轨迹$path$

$[s_1,s_2,...,s_n]$已知，实际上我们的决策变量为$\pmb{x}$

约束条件

车辆动力学约束

这个约束的推导使用分段加加速度法，即认为在$sl$空间下，车辆的合理轨迹中：相邻的两个轨迹点之间的三阶导数为常数，即$jerk$为常数，4阶等更高阶的导数为0。

我们使用泰勒展开：

相邻点之间存在一次约束计算，共$n-1$次约束计算：

凸空间边界约束

边界约束要考虑车辆的形状，如果只考虑车辆中心点在凸空间内，无法保证整车均在凸空间范围内。

我们考虑车辆的形状为矩形

车辆中心的坐标为$(s_i,l_i)$,考虑四个角点$P_1,P_2,P_3,P_4$的$l$坐标为$l^{P_1}_i,l^{P_2}_i,l^{P_3}_i,l^{P_4}_i$,依据的简单的几何关系我们可以得到下式（由于三角函数值是非线性的，我们做了偏于安全的近似处理）

设$P_1,P_2,P_3,P_4$所处的$s$坐标附近的凸空间边界最大$l$值分别为：$lbu^{p_1}_i,lbu^{p_2}_i,lbu^{p_3}_i,lbu^{p_4}_i$，最小$l$值分别为：$lbl^{p_1}_i,lbl^{p_2}_i,lbl^{p_3}_i,lbl^{p_4}_i$，四个角点分别满足凸空间边界限制：

大于等于的不等式约束改为小于等于约束：

写为决策变量表示的矩阵形式：

每个轨迹点处有一个边界约束，共$n$个约束，使用矩阵形式表示：

约束总结

因此我们的约束条件共有两个：

一个等式约束：

一个不等式约束：

目标函数

参考线代价

轨迹点的$l_i$坐标代表了点到参考线的距离，所以参考线代价项：

所以矩阵形式为：

平滑代价

平滑代价由一阶导数项、二阶导数项、三阶导数项构成：

一阶导数项、二阶导数项使用矩阵形式表达：

三阶导数项使用矩阵形式表达：

所以矩阵形式为：

凸空间中心代价

我们希望车辆的轨迹不要离凸空间的中心线太远，所以设置了凸空间中心代价：

目标函数总结

由于二次规划问题的一般形式为：

在本文描述的问题中，参考线代价、平滑代价、凸空间中心代价构成了模型的目标函数：

$H$、$f$的计算如上图所示，约束项矩阵、向量：

二次规划问题求解

直接使用matlab的二次规划求解函数或者python的二次规划求解函数即可。

代码实现

还没写完，写完更新。

参考来源

space.bilibili.com/287989852