凸优化在路径优化中的应用

总体思路

  凸优化虽然做为优化中的一个特例，但非常多的工程问题都可以转变为凸优化问题，因此应用需求比较多，被大量学者研究，成果颇丰。由于本文重点在应用，不讲解凸优化，如果感兴趣可以参考书籍《Convex Optimization》。
使用凸优化对路径进行优化的过程可以如下：

  生成初始路径是为了更容易生成凸多边形，而凸多边形则是为了给优化的目标函数添加约束条件（保证无碰撞）。其中生成初始路径有非常多的方法，如A* 算法、hybirdA*、rrt等，此处不对这些算法进行讲解，以下将重点介绍生成凸多边形区域和构造优化问题上。具体程序可以参考这里

生成凸多边形

  在地图中生成凸多边形应当满足以下条件：

1. 凸多边形与障碍物不相交；
2. 相邻的两个凸多边形有重叠部分；
3. 路径起点和终点包含在凸多边形中；

  由于使用栅格地图，可以选用矩形做为凸多边形可以减化一部分操作。由于初始路径已知，沿着初始路径进行搜索即可得到一系列凸多边形区域，由于方法多种多样，便不做详细说明。最终生成的凸多边形区域如下图所示。

构造目标函数

  当生成凸多边形区域后，只需要保证路径在凸多边形区域中即可保证路径与障碍物无碰撞，同时还有保证路径尽可能光滑，当然这样的路径存在无数条（目标函数不同，得到的路径就会不同）。
为了构造出凸的目标函数，可以使用特定的曲线表示路径，本文采用多条多项式曲线表示路径，将曲线的参数做为优化的变量，如下图所示。

  为了生成合适的路径，需要这些曲线满足如下条件：

1. 每个凸多边形都对应一条曲线（体现在变量个数上）；
2. 每条曲线都在凸多边形中（可以不严格满足，体现在约束方程中）；
3. 曲线之间的交点在凸多边形的相交区域当中（体现在约束方程中）；
4. 相邻的两条曲线终点和起点的一阶导相等（体现在约束方程中）；
5. 所有曲线二阶导平方的积分尽可能小（体现在目标函数中）；
6. 第一条曲线的起点为路径起点，最后一条曲线的终点为路径的终点；

  其中,条件1确定优化变量个数，条件2保证路径无碰撞，条件3，4，5保证生成光滑的曲线。目标函数如式（1）所示。
$$J=\sum _{i=0}^N{\int }_0^{t_{fi}}{\ddot{x}}_i^2+{\ddot{y}}_i^{2}dt\text{\hspace{1em}(1)}$$

  式（1）中，$t_{fi}$可根据每个凸多边形的长度确定（方式不唯一），通过将多项式（4次多项式）代入式（1）中，得式（2）。

$$\underset{U}{\min }J=U^{T}HU=\begin{array}{c}\left[\begin{array}{cccc}M{M}_{1}M& 0& 0& \dots \\ 0& M{M}_{2}M& 0& \dots \\ 0& 0& M{M}_{3}M& \dots \\ ⋮& ⋮& ⋮& \ddots \end{array}\right]\end{array}U\text{\hspace{1em}(2)}$$

  式（2）中，$U$为所有4次多项式的系数，总共有10n个系数（包括$x$和$y$方向），$M=\begin{array}{c}\left[\begin{array}{ccccccccc}0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 2& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 6& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 12& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 2& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 6& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 12\end{array}\right]\end{array}$，$M_i=\begin{array}{c}\left[\begin{array}{ccccccccc}0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& t_i& t_i^2/2& t_i^3/3& 0& 0& 0& 0\\ 0& 0& t_i^2/2& t_i^3/3& t_i^4/4& 0& 0& 0& 0\\ 0& 0& t_i^3/3& t_i^4/4& t_i^5/5& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& t_i& t_i^2/2& t_i^3/3\\ 0& 0& 0& 0& 0& 0& t_i^2/2& t_i^3/3& t_i^4/4\\ 0& 0& 0& 0& 0& 0& t_i^3/3& t_i^4/4& t_i^5/5\end{array}\right]\end{array}$

  上述目标函数在没有约束条件的情况下，目标函数最小值和变量均为0。需要添加条件2、 3、 4、 5才能得到最终的曲线。约束条件方程如式（3）所示。
$$lb\le AU\le ub\text{\hspace{1em}(3)}$$

  式（3）中，$lb$ 和 $ub$包含了起点、终点位置、凸多边形的边界等，$A$为相关的常数矩阵，具体内容可以参考github上的程序：https://github.com/Shenyii/navigation_1.git 。

  构造完目标函数后，可以发现这是一个线性QP问题，也是一个凸优化问题，因此可以使用相关工具对目标函数进行求解，如OSQP求解器。

实验效果

注意事项

1. 目标函数的定义多种多样，不同的目标函数将得到不同的路径；
2. 更高次的多项式增加了求解变量的个数，使得优化时间增加；
3. 通过增加曲线中间点约束，使曲线尽可能在凸多边形中；
4. 只适用与将机器人质点化处理的路径优化；