运动规划

硬约束及软约束下的轨迹优化

概述

Minimum snap的优缺点：

优点
1. 轨迹一定会经过中间路点
2. 可以闭式的求解出轨迹上每一处的状态信息
3. 构造了一个凸优化问题，可以闭式和数值求解
缺点

可以生成平滑的轨迹，但没有考虑避障

解决方法：

Adding forces：添加推力或吸力，使轨迹往远离障碍物的地方拉——软约束
Adding bounds：添加区域约束，使轨迹必须处于区域当中——硬约束

硬约束——相当于给优化问题施加一些等式或不等式的约束，要求轨迹必须严格地满足约束

软约束——相当于给优化问题加上惩罚函数（比如靠近障碍物时，惩罚函数就会增大），不一定严格地满足约束

惩罚函数常见形式：二次函数（L2）、一次函数（L1）、Huber function（L1+L2）、Barrier function（例如，靠近障碍物时，惩罚函数的值会急剧增大）

硬约束优化

基于走廊的轨迹优化

步骤：

探测障碍物，生成八叉树地图
生成一个飞行走廊，利用前端的路径规划方法
膨胀飞行走廊
在飞行走廊中生成轨迹

施加的约束：

Instant linear constraints:
- 起点、终点的状态约束
- 两段多项式轨迹交界点的约束（在两个飞行走廊的交界处）
- 连续性约束
Interval(区间) linear constrains:
- 边界约束（保证轨迹在飞行走廊内）
- 动力学约束
💡 如果要施加Interval linear constrains，可以将时间离散成很多份，对离散的每一点进行约束——这样会导致约束过多，导致凸优化问题求解复杂；因此采用后验检查的方法施加区间约束

飞行走廊提供了很大的优化的自由：两段轨迹交界点可以在两个飞行走廊的交界区域移动（提供了一定的优化空间），进而可以使时间分配更加合理（中间段分配时间长——可以让中间段的两个端点在走廊交界区域进行调整）

后验检查——迭代地检查极值点——如果不满足则时间额外的约束（在极值点的位置施加一个约束（ $y_{max}<y_b$ ），之后再进行检查）

贝塞尔曲线优化

Bernstein多项式：其中 $b^i_n(t)=C_n^it^i(1-t)^{n-i}$ ，系数 $c_j^i$ 表示控制点

对于Bernstein多项式，一定能映射成普通多项式（可以用矩阵 $M$ 表示， $p=Mc$ ）

贝塞尔曲线的特性：

Endpoint interpolation(插值)：贝塞尔曲线一定经过第一个控制点和最后一个控制点，不会经过中间其他控制点
Convex hull(凸包)：贝塞尔曲线被包含在控制点构成的凸包内
Hodograph：贝塞尔曲线导数的控制点可以表示为 $n\cdot(c_{i+1}-c_i)$
Fixed time interval：只能被定义在 $[0,1]$ ，使用时需要将时间 $t$ 映射到 $[0,1]$

$s^{(1-l)}_j$ 表示 $s_j$ 的 $(1-l)$ 次方

对于边界约束和连续性约束，都施加在分段贝塞尔曲线的首端或末端，因此可以直接以简单的形式表示出约束（如上图所示）

PS：上图中 $a_{\mu,j}^{l,i}$ 表述应该有问题，应该是 $a_{\mu,j}^{l,i}=(n-l+1)\cdot(a_{\mu,j}^{l-1,i+1}-a_{\mu,j}^{l-1,i})$ ,得到一个递推表达式

Other Options

Dense constrains

稠密地生成飞行走廊，在每段轨迹中加速度恒定（相当于用一个三次函数拟合），最后得到一组最优的 $a$ （求解的变量少，一段轨迹只有一个 $a_i$ ）

但存在着生成的轨迹过于保守、太多限制条件导致计算负担较大的问题

Mixed integer optimization

利用一个很大的整数M和一组选择整数 $c_i$ 将或运算转化为且运算

MIQP问题可以求解更快，但由于约束过多，导致求解较慢

软约束优化

基于距离的轨迹优化

硬约束的轨迹优化存在的一些问题：

把安全区域当做等价的
解空间对噪声很敏感

光滑项闭式求解——相当于把约束直接加到表达式里了

膨胀惩罚项——把轨迹上每个点在距离场中的碰撞惩罚（ $c(p(t))$ ）相加

碰撞惩罚项必须要把积分强行写成数值的解（要用轨迹上离散的点来评判碰撞惩罚）

动力学项：惩罚速度和加速度超过限制的部分

目标函数J不一定是凸的，因此不能将问题化为凸优化问题！只能用非线性优化的方法数值求解

利用链式法则求导

$\frac{\alpha c(p)}{\alpha d_{p\mu}}=\frac{\alpha c(p)}{\alpha p_\mu}\frac{\alpha p_\mu}{\alpha d_{p\mu}}$ （ $\frac{\alpha p_\mu}{\alpha d_{p\mu}}$ 可以用矩阵求导的方法得到结果）

$\frac{\alpha v}{\alpha d_{p\mu}}=\frac{\alpha v}{\alpha v_\mu}\frac{\alpha v_\mu}{\alpha d_{p\mu}}$ （ $v=\sqrt{v_x^2+v_y^2+v_z^2}$ ）

数值优化

Line search的类型：

exact line search——找到使函数下降到最低处的步长 $t$ （实现较困难）
backtracking line search——相当于给一个初始步长 $t$ ，然后判断在步长 $t$ 下是否满足 $f(x+\Delta x)<f(x)+\alpha t \nabla f(x)^T \Delta x$ （图中的表现就是 $f(x+\Delta x)$ 在直线的下方，即 $t<t_0$ ），不满足则迭代缩小步长 $t$

梯度下降法

最速下降法

牛顿法

列文伯格—马夸尔特方法

详情可以看视觉SLAM十四讲

非线性优化库：Ceres（视觉SLAM用得比较多）、NLopt

设置一个全局规划器（在导航一开始的时候，使用前端的路径规划搜索一条起点到终点的路径，作为全局规划器）
找到一个局部的路径
用后端生成一个局部轨迹

💡 根据planning horizon的范围，在全局规划器规划出的路径上选择当次规划的终点。之后无人机在execution horizon的范围内跟踪当次轨迹。在无人机到达当次跟踪的末状态A（根据execution horizon在轨迹上找出的末状态）前，可以以A作为下一次轨迹的初始状态，根据planning horizon的范围，提前生成下一次轨迹。