路径优化

目的

  本文将无人机平面路径优化问题做为曲线优化问题，在满足无人机运动学，动力学约束的条件下，尽可能得到一条平滑的路径，分别使用五次曲线优化和贝塞尔曲线优化的方法。
  无人机动力学模型可以简化为一个线性模型，如式（1）所示。


  因为无人机为线性模型，所以只需要曲线在x xx和y yy方向满足相应的约束即可(加速度约束，jerk约束)。

五次曲线优化

   五次曲线的参数方程如式（2）所示。

  由于五次曲线对时间的二阶导为三次曲线，因此，可以用五次曲线优化得到一条光滑的满足约束的路径。
   优化的目标函数如式（3）所示。

   目标函数J中只将加速度做为优化项，同理可以将jerk做为优化项。在约束方程中，约束条件1-4为起点、终点位置约束，约束条件5-8为起点、终点角度约束，约束条件9-10为速度约束，约束条件11-12为加速度约束，速度约束和加速度约束无法严格满足，可以多选几个时间点进行约束，基本上都能保证这两种约束。

   由于x和y都是关于时间的五次参数方程，目标函数J JJ可以写成线性二次型。将t 0 =0代入式（3）中得：

  通过上述转换，就可以用一些线性二次型的求解器求解出一条连接起点和终点，满足约束条件的最优路径。

贝塞尔曲线优化

   贝塞尔曲线的一般性公式如式（6）所示：

   贝塞尔曲线一阶导、二阶导分别如式（7）（8）所示：

  其中P i 为控制点，P t 为t 时刻贝塞尔曲线上的点。贝塞尔曲线主要具有如下特性：

  1. 贝赛尔曲线对时间求导后仍然是贝塞尔曲线；
  2. 贝塞尔曲线过起点和终点；
  3. 贝塞尔曲线始终会在包含了所有控制点的最小凸多边形中。

  以贝赛尔曲线的加速度做为目标函数得式（9）所示：

  式（9）中，约束条件有初始位置、初始角度、终点位置、终点角度、整条曲线的最大速度和最大加速度。将式（8）代入式（9）得：

 其中，i/.2表示i ii除以2取整数，并且按照C的编程习惯，从0开始算起。根据上述贝塞尔曲线特性3等可得到约矩阵A。

注意事项

  1. 贝塞尔曲线的时域为[0,1]；
  2. 当硬约束太多或太“严格”可能导致无法求解；