路径规划——软/硬约束下的轨迹优化

Minimum Snap是轨迹优化中的常用方法，其效果如下：

上图中红线为Minimum Snap优化生成的光滑轨迹，可以看出Minimum Snap有利于光滑曲线的生成。该算法虽然可以控制轨迹经过哪些路标点，但是没有考虑环境中的障碍物信息(如图中红色方块)，不利于做避障。

软/硬约束

• 硬约束
  
  硬约束要求严格满足上述等式约束或不等式约束。
• 软约束
  
  软约束将约束加入在目标函数里，即加入惩罚项。

1.硬约束轨迹优化

1.1 Corridor-based 轨迹优化(基于走廊的)

论文名称：Online generation of collision-free trajectories for quadrotor flight in unknown cluttered environments

算法步骤如下：

• 建立八叉树地图
• 使用图搜索方法找到路径，路径经过的节点(即八叉树地图钟的格子)连接起来可以构成走廊，走廊内不包含任何障碍物
• 将搜索到的走廊沿着坐标轴进行膨胀(路径中的每一个格子)，并去掉重复部分
• 生成轨迹

求解过程类似于Minimum Snap，利用微分平坦将无人机状态空间中的12个变量压缩为4个，并构建多项式轨迹：

定义目标函数(这里使用Mnimum jerk）

与minimum snap相比，目标函数的约束没有waypoint(中间节点)的约束，约束改为了走廊的可通行区域。

约束的构建

Instant linear约束

• 起点与终点的约束
• transition point约束(两端轨迹的连接点位于两个方块的交界内)
• 连续性约束

Interval linear约束

• 边界约束(使用启发式方法，进行后验检查，检查轨迹是否超出安全区域)
• 动力学约束(速度、加速度在合理范围内)

关于边界约束和动力学约束

检查给定约束下轨迹(多项式)的极值，然后检查极值是否超过了边界，若超出边界，给定一个更严格的约束，如下图所示：

上述方法需要迭代求解，然而实际情况中可能出现没有解的情况。

总结
corridor-based 轨迹优化方法存在问题是无法施加全局的安全和动力学可行的约束，一般将这些约束施加在轨迹的若干给定点上，求解完毕后检查极值是否违反约束。若存在违反约束的情况，则给定额外的约束，迭代的求解问题。然而实际使用中需要多次迭代求解，没有解的情况下也需要进行多次迭代。

1.2 Bezier曲线优化

原理
原理是替换多项式的形式：

Bezier曲线与普通多项式存在一一映射关系，因此可以找到关系使得$p=M\ast c$。

性质
注：Bezier的系数$c$称为控制点，有实际的物理意义

• 一个Bezier曲线一定起始于第一个控制点，终止于最后一个控制点，而不经过其他控制点，例如对于四阶的Bezier曲线
  
• 一段Bezier曲线一定被其所有控制点围成的凸包包围住
• Hodograph ：一段$n$次的Bezier曲线