凸优化问题 & Minimum Snap

凸函数

可以看看我的这几篇Blog1，Blog2，Blog3。

函数像左边这样就是凸的，像右边那样就是凹的。（高数同济定义是与国际相反的）

以下是数学上的定义。

---

如果像那种坑坑洼洼的函数，就可能会存在很多个局部最小值，使用梯度下降法求最优解的话就很容易陷入到局部最小值当中，从而找不到一个最优解。

优化问题

一个问题要是凸的，其函数一定要是凸函数，满足上述对凸函数的一些定义，其不等式约束一定是凸的，其等式约束是符合仿射性质的(ax+by)。

---

以下是一些Disciplined convex optimization programs

如果做轨迹生成/规划相关的，写出来后发现是以上几种情况的一种，就可以笑了，因为总能转化为凸优化问题找到全局最优解。

Closed-form Solution to Minimum Snap

直接求轨迹生成的问题是把其变成一个QP问题，然后使用求解器去求解（数值），这次的方法是直接求QP问题的解析解，代数上就直接求出来，一步到位。

$J$是多项式的系数，之前轨迹生成问题当中是$P_0......P_5$表示的多项式系数，其实这是没有实际的物理含义的。把对轨迹系数的优化问题转化为对分段移动机器人轨迹生成问题转化为边界的导数约束问题。（等价为求各个节点当中的$v和a$）。

也就是从优化多项式系数$P_0......P_5$这种没有实际物理意义的且会使得优化后的数值不稳定的决策变量转化为在轨迹上的这些点上的$v和a$作为决策变量，这下就会更稳定，因为轨迹上的点的$v和a$是有实际物理含义的。

$d_M$就是端点的导数，就是$v和a$。

核心思想

把要问题当中要优化的变量和已经固定不变的变量进行分解，分解的方法如下：

把不能动的先分解到前面去（即第一行的$d_F$），能动的都弄到后面去（即第二行的$d_P$）。假设现在已经找到了$C$矩阵，那现在的决策变量就能被上方公式进行替换成固定的量$d_F和能动的量d_P$。

关于之前的导数约束：因为求出了$d_F以及d_P$所以导数方面的约束是满足的，再来说一下连续性约束。假如两段轨迹是拼接起来的，比如说像如下这样：

---

在拼接点的地方，是共用一个$v和a$的，同时可以根据之前的导数$d_F和d_P$来映射成$d_1.....d_M$，这就隐性的完成了连续性的约束。
将$J$展开，对$d_P$进行求偏导，因为真正需要优化的变量就是$d_P$。

---

求C

轨迹生成实例

有障碍物的时候

不光滑的地方局部进行优化拉回来就行。

工程上解决问题的一些细节

凸优化求解器的推荐

数值稳定性

比如说之前多项式系数有个$P_5$，因为其是$t_5$的系数，只能取的很小，因为越到后期$t_5$就会越大，此时系数$P_5$就只能取的很大的时候就会造成数值不稳定的问题存在。

时间分配

每一段时间都得分配的，T如果给的不合适，那么就会一点也不光滑。

---

一个简单的时间分配解法，根据一个已经给出的速度曲线，常用的就是这个梯形的速度曲线，就是无人机在每一段曲线当中都先进行一个以最大加速度进行匀加速，加到顶之后，再进行一个匀速运动巡航，之后在每段停止之前把速度减下来。根据两个坐标点，可以算出距离，又可以根据加速度，速度，距离，算出来对应的时间，然后就用时间来给轨迹进行时间分配，虽然思想很简单，但是使用的很广。

以下这种提供了很高的自由度，缓解了时间分配不好的问题。

比较好的做法就是，生成轨迹后，考察一下这个问题，因为之前生成轨迹只是最小化能量，从来没有考虑过时间，完全可以把时间加入到这个权重当中。

对每个多项式的系数等等加上一个扰动，之后再求偏导，之后可以进行梯度下降法，来不断优化$P和T$，就可以把一个分配的不好的时间初值，给分配的很好。

总结

还得考虑动力学可行性的约束（Kinodynamic）。