总体思路

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

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

生成凸多边形

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

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

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

构造目标函数

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

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

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

 其中,条件1确定优化变量个数，条件2保证路径无碰撞，条件3，4，5保证生成光滑的曲线。目标函数如式（1）所示。

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

式（2）中，U为所有4次多项式的系数，总共有10n个系数（包括x和y方向）

  上述目标函数在没有约束条件的情况下，目标函数最小值和变量均为0。需要添加条件2、 3、 4、 5才能得到最终的曲线。约束条件方程如式（3）所示。

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

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

实验效果

注意事项

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