基于后退走廊的轨迹优化方法（Bubble Planner）

0 摘要

这是高老师最新的一篇文章，提出了一种基于走廊约束最小控制力的轨迹优化框架的运动规划算法（Minimum Control Effort Trajectory Optimization）（MINCO），这个方法具体来说就是使用了一系列重叠的球体（飞行走廊区域）来表示环境的自由空间，并且该算法能实现实时规划UAV的轨迹；一个创新点就是，这个走廊是基于采样的方法而生成的，生成的两个相邻球体之间具有很大的重叠区域；另一个创新点就是后退走廊策略（Receding Horizon Corridors (RHC) Strategy），就是，部分先前生成好的走廊，在每一个重新规划当中被重新使用，综合以上两种创新点，根据UAV当前的状态，扩大了安全飞行走廊的空间，从而允许UAV保持了高速机动性，实现了超过13.7m/s的飞行速度。

1 建模

---

---

整个轨迹由M块组成，每块轨迹都包含在各个球体当中，$B_i$这些就代表的是球体（$i=\mathrm{1...}M$），橙色点$q_i$就代表中间路点，被限制在两球相交的部分，$T_i$就是时间的分配，参考$Minimum$ $Snap$当中的多段轨迹的时间分配，$d_0、d_g$相当于起点和终点了。

1.1 球形走廊

---

---

定义一个球体，这个球心和最近障碍点$n$之间的距离减去$r_d$就是球的半径$r$，因为距离障碍物有安全距离，所以只需要把每一段轨迹都约束在相应的球体当中，就可以满足安全性的约束。

生成一个球体，首先使用障碍点云构建一个$KD-Tree$，然后给定球心，通过$NN-Search$确定最近的障碍物点，最后通过以上步骤确定球的半径，生成这一个球形走廊，这个步骤称为$GenerateOneSphere(o)$，在后续当中使用。

---

---

1.2 轨迹分段

同理于$Minimum$ $Snap$，这也是将轨迹进行分段，如下所示：
$$p(t)=p_i\left(t-t_{i-1}\right)\in {\mathcal{B}}_i,t\in \left[t_{i-1},t_i\right]$$

同时，相邻的轨迹片段会在时间$t_i$，在同一点$q_i$进行相遇，再考虑动力学约束（$Minimum$ $Snap$当中的$PVA$条件），则轨迹优化可以表示为以下，记为公式2：
$$\begin{array}{ll}{\min }_{p(t)}& {\int }_0^{t_M}{\Vert p^{(s)}(t)\Vert }_2^{2}dt+{\rho }_T{t}_M\\ \text{ s.t. }& p^{(0:s-1)}(0)={\mathrm{d}}_0,p^{(0:s-1)}\left(t_M\right)={\mathrm{d}}_g\\ & p\left(t_i\right)=q_i,\forall 1\le i<M\\ & t_{i-1}<t_i,\forall 1\le i\le M\\ & {\Vert p^{(1)}(t)\Vert }_2^2\le v_{\max }^2,{\Vert p^{(2)}(t)\Vert }_2^2\le a_{\max }^2\\ & p(t)=p_i\left(t-t_i\right)\in {\mathcal{B}}_i,\forall 1\le i\le M,t\in \left[t_{i-1},t_i\right]\end{array}$$

可以看出，$min$的目标函数，与之前的鹅方法类似，还是与$Minimum$ $Snap$相关的顺滑性约束，这其中的优化分为两部分，第一部分，固定中间路点$q$，以及时间分配相邻$T$，并且只是优化第一部分，即顺滑性约束那部分，之后，就只考虑第2式子和第3式子中的约束，然后这个优化问题导致了一个最优解，如下式所示：
$$p_i(t)={\mathbf{c}}_i(\mathbf{q},\mathbf{T}{)}^T\beta (t),t\in \left[0,T_i\right]$$

而第二部分就是，将上面2个式子的约束转化为上式的轨迹优化约束，因为$p_i(t)$这个式子自然满足之前上面那一堆式子中的第二个和第三个的约束，因此完整的约束只要考虑第4到第6个式子了。为了解决这个问题，使用$MINCO$框架将其转换为一个无约束的优化问题，使用下面的函数进行转换即可：

$${\mathcal{L}}_{\mu }(x)=\{\begin{array}{ll}0& \text{ if }x\le 0,\\ (\mu -x/2)(x/\mu {)}^3& \text{ if }0<x<\mu ,\\ x-\mu /2& \text{ if }x\ge \mu .\end{array}$$

---

所以公式2当中的模型转换为下述式子：

---

${\rho }_{\mathrm{vel}},{\rho }_{\mathrm{acc}},{\rho }_{\mathrm{c}}$分别是最大速度、最大加速度、以及无碰撞惩罚的对应权重。 $o_i$是对应的球心， $r_i$ 是第$i$个球体的半径，$\mathrm{q}$是航点， $\tau$ 是时间分配的，所有梯度都可以进行解析计算。

1.3 生成飞行走廊

---

---

---

飞行走廊生成的策略：尽可能使球体变大，重叠面积变大，好留给UAV高速机动性能的施展，以及避免高速飞行状态下陷入局部极小值以及无解的状态。

1.4 随机采样

$Score={\rho }_r{V}_{cand}+{\rho }_v{V}_{inter}$

1.5 路径必经点以及时间分分配

---

---

对于给定的飞行走廊，采用默认初始化策略，其中航路点被初始化为两个相邻球体之间重叠空间的中心（图中的粉红色点），时间分配初始化为 $Ti=\Vert qi-qi-1{\Vert }_2/v_{max}$

1.6 重新规划

看上图，当需要重新规划的时候，选择$q$最近的未来航点$d_{rp}$作为初始状态，减少空间后的一些球体走廊被重新使用，组成新走廊的第一部分，这种重规划方案确保在每次重新规划中，可飞行区域总是有足够的空间让四旋翼从其当前状态(因为当前四旋翼状态是在先前走廊中包含的先前轨迹上)机动，从而大大扩大了后端轨迹优化的可行性区域。

2 结果

---

---

提出的方法生成球体较少，但是半径较大，并且重叠部分很多，比使用类似$RRT\ast$的方法生成速度要快。

---

---