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

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曲线$B(t)$求导后的结果$B^{{}^{\prime }}(t)$还是Bezier曲线，并且$$B^{{}^{\prime }}(t)$的控制点可以写作$n\ast (c_{i+1}-c_i)$
• Bezier曲线的t一定被定义在区间[0,1]

凸包性质的应用

对于一段轨迹而言，将控制点限制在corridor-based 轨迹优化方法的走廊内，意味着轨迹就被约束在走廊内。同样，轨迹的导数即速度可以利用凸包范围内进行限制，加速度同理。

目标函数的定义

• 根据Bezier曲线的第一个性质，连续性约束只需要令两个轨迹相交的控制点相等就可以；
• 安全性约束即避障可以使用凸包性质实现
• 动力学约束可以使用凸包性质和Hodograph性质实现
  如上所示，使用Bazier曲线的约束条件仍然为线性等式约束和线性不等式约束，仍然为QP问题。

2.软约束轨迹优化

2.1 Distance-based轨迹优化 （基于距离的）

硬约束的轨迹优化借助可通行的区域进行轨迹优化，障碍物对可通行区域的影响是一致的，因此生成的轨迹可能离障碍物很近。
软约束的轨迹优化利用障碍物对轨迹形成推力，使其远离障碍物。对目标函数的设计具有较高的要求，它使得约束尽可能满足(无法保证一定满足)，因此目标函数设计不好的情况下轨迹可能会撞到障碍物。

问题建模
利用微分平坦性将无人机参数空间进行压缩，并用多项式模拟轨迹

目标函数定义如下：

第一项为平滑项、第二项为碰撞项、第三项为动力学约束项。

• 光滑项可以使用Minimum Snap的求解公式
  
• 碰撞项，惩罚距离最近的障碍物的距离（对曲线进行积分），$c(p(t))$为惩罚函数
  
• 动力学项，惩罚速度和加速度

得到的目标函数不是一个凸函数，需要通过一步步求导求解。光滑项求解见第1部分，碰撞项对自由变量$d_{p\mu }$求导，得到：

上式中，F和G分别为

上式中，$L_{dp}$为矩阵$M^{-1}C$的右半部分。$V_m$为位置参数到速度参数的映射矩阵。$T=[T_k^0,T_k^1,\dots ,T_k^n]$。

二阶求导结果为：

数值优化

基于梯度的数值优化方法，通过$minimizef(x)$求得关于$x^{(k)}$的数值序列使得$f(x^{(k)})$逼近最优解，或者使$f(x)$的导数为0。有以下求解方法：

• 一阶法，即梯度下降，向导数的负方向搜索：
  

• 二阶法，牛顿法：
  

• LM法：