Ego Planner规划算法（上）——Ego Planner流程介绍

   本系列文章用于学习记录Ego-Planner规划算法的相关内容，本系列文章是在在Fast Planner规划算法系列文章的基础上，展开对Ego Planner的相关介绍，主要包括Ego Planner算法流程和程序框架两部分，主要学习资料是深蓝学院的移动机器人运动规划课程

   一、Ego Planner流程介绍

   本系列文章，在Fast Planner规划算法系列文章的基础上，展开对Ego Planner的相关介绍，Ego Planner最大的优点是不需要ESDF地图，在轨迹优化时不需要使用ESDF去构建避障的cost项，ESDF是一种栅格距离场，每个格子都存放着距离该格子最近的障碍物的距离，从下图右下角的柱状图可以看出EWOK、Fast-Planner算法都需要较长的时间去构建这个ESDF地图，如果不构建ESDF可以使得规划时间更短，如Ego Planner算法

   除了构建ESDF地图需要耗费较长时间之外，在建图的时候只能看见障碍物的表面，如下图中的右图所示，看不见障碍物的后面/内部，在优化的过程中会使用ESDF去产生一个排斥轨迹远离障碍物，在下图所示的例子中，上面的障碍物将轨迹往下推，下面的障碍物将轨迹往上推，会出现轨迹卡在障碍物中的情况。所以ESDF存在构建时间长、对避障cost构建的缺点等问题

   接下来，我们来看一下Ego Planner使用什么方法来代替ESDF

   ESDF一般有一个范围，一般是在轨迹的周围去构建ESDF，如下图所示，优化过程中ESDF场会把轨迹推到无障碍物区域

   在不使用ESDF地图的前提下，Ego Planner是如何实现将轨迹推到无障碍物区域呢？其核心思想是，轨迹在优化的过程中，如果轨迹与障碍物发生碰撞，我们就基于这个碰撞对轨迹产生一个反方向的力来把轨迹推出来，远离刚刚碰到的障碍物

   接下来，我们对比着Fast Planner的框架来看一下Ego Planner的框架

   前端：Fast Planner的前端使用的是Hybrid A * 算法，而Ego Planner并不需要一个与障碍物不发生碰撞的初值轨迹，所以它的前端不需要Hybrid A * 这种比较复杂的算法，它只需要一条不考虑障碍物，满足给定起点和目标点状态的轨迹即可，然后在后端优化中再把轨迹优化成无碰撞的

   后端轨迹优化：相比于Fast Planner，Ego Planner在后端优化中在每次迭代中对优化后的轨迹需要增加检查轨迹是否发送碰撞的流程，如下图的check collision所示，如果没有发生碰撞，则继续迭代优化轨迹，如果发生了碰撞，就需要针对这个碰撞，加一个新的推力的cost，此时优化问题发生了变化，不能再针对原问题继续优化下去，需要针对新问题重新开始优化。即基于当前优化的结果再加上新的cost，再去进行优化。

   后端时间重分配：由于Ego Planner的后端优化也是基于优化的控制点，采用均匀B样条来生成轨迹，所以也需要跟Fast Planner一样，进行时间重分配，所不同的是Ego Planner的时间重分配与Fast Planner的时间重分配所使用的方法思路是完全不一样的。

---

   接下来，我们来看一下轨迹与障碍物发生碰撞后，与碰撞反方向的推力是如何产生的

   首先，我们要检查轨迹上的所有控制点，找出所有在障碍物里面的控制点，以及与该位于障碍物内部的控制点邻近的控制点，如下图中的左图所示，然后我们需要以这两个不在障碍物内部的邻近的控制点为起点和终点使用传统的A * 算法 搜索得到两点之间的一条可行路径，如下图中中间图的蓝色路径所示，然后计算在障碍物内部的点Q的速度方向，然后以这个速度方向为法向量，找到一个垂直于这个速度方向且过点Q的平面，如下图中右图的绿色平面所示，然后找这个平面与障碍物表面的交点，如下图中右图的p点所示，然后取向量v为由Q点指向p点方向上的单位向量，即$v=\frac{p-Q}{||p-Q||}$，这样就可以得到一个针对Q点的{p，v}对

   我们要定义一个障碍物距离$d=(\mathbf{Q}-\mathbf{p}{)}^T\mathbf{v}.$，比如我们要求障碍物距离d要大于0.6，假设Q点与p点间距离为0.8，则此时Q点处的d值为-0.8，我们可以施加一个$(d-0.6{)}^2$的代价项，这样在优化的过程会逐渐将Q点推到沿着向量v的方向上，p点前方0.6距离处，动态演示如下：

---

   接下来，我们来看一下Ego Planner是如何构建优化问题的，目标函数的各个组成项的具体表达式是什么

   Ego Planner中控制点 { Q 1 , Q 2 , . . . , Q N } \{\mathbf{Q}_{1},\mathbf{Q}_{2},...,\mathbf{Q}_{N}\} {Q1​,Q2​,...,QN​}对应的时间区间$[t_1,t_2,...,t_M]$的下标是从1开始的，这区别于Fast Planner的从0开始，因为是均匀B样条，时间区间的间隔相同，均为△t，所以速度轨迹、加速度轨迹、jerk轨迹的控制点可由下式进行计算：

   $${\mathbf{V}}_i=\frac{{\mathbf{Q}}_{i+1}-{\mathbf{Q}}_i}{\Delta t},{\mathbf{A}}_i=\frac{{\mathbf{V}}_{i+1}-{\mathbf{V}}_i}{\Delta t},{\mathbf{J}}_i=\frac{{\mathbf{A}}_{i+1}-{\mathbf{A}}_i}{\Delta t}$$

   与Fast Planner类似，Ego Planner的目标函数由平滑项、碰撞项、动力学这三项构成，这三项也是轨迹优化问题标配的内容。

   $$\underset{\mathbf{Q}}{\min }\mathcal{J}={\lambda }_s\boxed{{\mathcal{J}}_s}+{\lambda }_d\boxed{{\mathcal{J}}_c}+{\lambda }_d\boxed{{\mathcal{J}}_d}$$

   首先，来看平滑项的构建，Ego Planner中通过直接惩罚速度和加速度的大小来达到一个平滑的效果，得益于B样条的凸包性质，我们只需要对控制点进行惩罚就可以了，因此，平滑项的代价函数可写成如下所示的形式：

   $$\begin{array}{r}{\mathcal{J}}_s=\sum _{i=1}^{N_c-2}\Vert {\mathbf{A}}_i{\Vert }_2^2+\sum _{i=1}^{N_c-3}\Vert {\mathbf{J}}_i{\Vert }_2^2\end{array}$$

   然后接着看动力学可行项，其与Fast Planner的思路类似，让每个维度上的高阶量不能大于限定值，所不同的是Ego Planner多了jerk项，即对速度、加速度、jerk进行检查，同样由于B样条曲线的特性，只需要对控制点处进行检查即可。

   $$\begin{array}{r}{\mathcal{J}}_d=\sum _{i=1}^{N_c-1}{\omega }_{v}F({\mathbf{V}}_i)+\sum _{i=1}^{N_c-2}{\omega }_{a}F({\mathbf{A}}_i)+\sum _{i=1}^{N_c-3}{\omega }_{j}F({\mathbf{J}}_i)\end{array}$$

   其中，

   $$F(\mathbf{C})=\sum _{r=x,y,z}f(c_r)f(c_r)=\{\begin{array}{llc}{a}_1{c}_r^2+b_1{c}_r+c_1& & (c_r\le -c_j)\\ (-\lambda c_m-c_r{)}^3& & (-c_j<c_r\le -\lambda c_m)\\ 0& & (-\lambda c_m<c_r\le \lambda c_m)\\ (-\lambda c_m+c_r{)}^3& & (\lambda c_m<c_r\le c_j)\\ a_2{c}_r^2+b_2{c}_r+c_2& & (c_j\le c_r)\end{array}$$

   系数$a_1,b_1,c_1,a_2,b_2,c_2$可通过满足二阶连续性条件来获得，$c_m$是设定的最大值，$c_j$是二次区间和三次区间的分裂点。$\lambda <1-ϵ$，是一个弹性系数，其中，$ϵ$的值远小于1，以使最终结果满足约束条件，因为成本函数是所有加权项的折衷。

   然后，就是Ego Planner的代价函数中最重要的一部分，即期望轨迹远离障碍物项，我们基于前面的介绍的{p,v}对来实现控制点不在障碍物里面，控制点不在障碍物中由B样条曲线的凸包性就可以保证整条轨迹均不在障碍物中。

   我们期望使得前面介绍过的障碍物距离$d_{ij}=({\mathbf{Q}}_i-{\mathbf{p}}_{ij})\cdot {\mathbf{v}}_{ij}$大于设定的安全距离$s_f$，在优化的过程中，可能出现同一个控制点会碰到多个不同的障碍物的情况。所以对于一个控制点可能有多个 { p , v } {\{ p,v \} } {p,v}对，设$N_p$为该控制点 { p , v } {\{ p,v \} } {p,v}对的个数，则该控制点的代价值可写成如下的形式：

   $$j_c({\mathbf{Q}}_i)=\sum _{j=1}^{N_p}{j}_c(i,j)$$

   其中，

   $$\begin{array}{rl}{d}_{ij}& =({\mathbf{Q}}_i-{\mathbf{p}}_{ij})\cdot {\mathbf{v}}_{ij}\\ c_{ij}& =s_f-d_{ij}\end{array}$$

   $$j_c(i,j)=\{\begin{array}{ll}0& (c_{ij}\le 0)\\ c_{ij}^3& (0<c_{ij}\le s_f)\\ 3s_f{c}_{ij}^2-3s_f^2{c}_{ij}+s_f^3& (s_f<c_{ij})\end{array}$$

   所以，所有控制点的避障代价表达式可如下式所示：

   $${\mathcal{J}}_c=\sum _{i=1}^{N_c}{j}_c({\mathbf{Q}}_i)=\sum _{i=1}^{N_c}\sum _{j=1}^{N_p}{j}_c(i,j)$$

---

   接下来，我们来看一下Ego Planner与Fast Planner设计碰撞代价项的区别

   Fast Planner是传统的基于ESDF的方法，如果要求cost的梯度的话，需要使用插值的方法去求ESDF场的梯度，这个插值使用三线性插值来做（3D环境）。而Ego Planner直接让${\mathcal{J}}_c$对控制点求偏导$\frac{\partial {\mathcal{J}}_c}{\partial {\mathbf{Q}}_i}$，如下所示，是可以直接解析求解出来的

   $$\frac{\partial {\mathcal{J}}_c}{\partial {\mathbf{Q}}_i}=\sum _{i=1}^{N_c}\sum _{j=1}^{N_p}{\mathbf{v}}_{ij}\{\begin{array}{cc}0& (c_{ij}\le 0)\\ -3c_{ij}^2& (0<c_{ij}\le s_f)\\ -6s_f{c}_{ij}+3s_f^2& (s_f<c_{ij})\end{array}$$

   详细过程如下：

   此外，ESDF的梯度都是不连续的，而Ego Planner使用的方法计算出的梯度是连续的。当然Ego Planner最主要的提升是不需要再使用ESDF方法

---

   接下来，我们来看一下Ego Planner是如何进行时间重分配的

   Fast-Planner将轨迹参数化为非均匀B样条，检查井将超过速度、加速度限制的控制点所影响的时间区间采用迭代的方式进行扩大。·如果在轨迹初状态的周围调整时间，会导致轨迹的初状态的高阶量发生变化，从而导致轨迹在飞行的时候会有一些抖动。

   所以，在Ego Planner中，会根据安全轨迹重新生成合理时间分配的均匀B样条轨迹。它的目标是使得时间分配更合理的B样条在位置上更加贴合之前优化出来的轨迹，并尽可能的满足动力学要求和平滑要求。

   其实现框架如下图所示：

   首先，我们需要寻找新的△t，然后，我们需要得到轨迹的初值（优化初值），最后再进行优化。下面详细对这三个步骤进行介绍

提出了一种各向异性曲线拟合方法，使自由优化其控制点以满足高阶导数约束，同时保持与几乎相同的形状𝚽𝚽𝚽𝚽𝑓𝑓

   我们先来看如何得到新的适合的时间间隔△t，我们去查询每个控制点每个维度的速度、加速度、jerk是否超出了限制，来计算时间扩展系数$r_e$,如下所示：

   r e = m a x { ∣ V i , r / v m ∣ , ∣ A j , r / a m ∣ , ∣ J k , r / j m ∣ 3 , 1 } i ∈ { 1 , . . . , N c − 1 } , j ∈ { 1 , . . . , N c − 2 } , k ∈ { 1 , . . . , N c − 3 } , r ∈ { x , y , z } \begin{aligned}r_e&=max\left\{|\mathbf{V}_{i,r}/v_m|,\sqrt{|\mathbf{A}_{j,r}/a_m|},\sqrt[3]{|\mathbf{J}_{k,r}/j_m|},1\right\}\\\\i&\in\{1,...,N_c-1\},j\in\{1,...,N_c-2\},k\in\{1,...,N_c-3\},r\in\{x,y,z\}\end{aligned} re​i​=max{∣Vi,r​/vm​∣,∣Aj,r​/am​∣ ​,3∣Jk,r​/jm​∣ ​,1}∈{1,...,Nc​−1},j∈{1,...,Nc​−2},k∈{1,...,Nc​−3},r∈{x,y,z}​

   然后将现有时间乘以该扩展系数，即可得到新的时间间隔，使得$[t_1,t_2,...,t_M]$→$[t_1,{t_2}^{\prime },\dots ,{t_M}^{\prime }]$

   $$\Delta t^{\prime }=r_e\Delta t$$

---

   接着，我们来看一下如何基于这个新的时间间隔来计算轨迹的初值，我们可以通过求解一个封闭形式的最小二乘问题来求解该问题，我们先写成之前优化出来的轨迹，如下图左边的式子所示，然后再写成需要重新分配时间的轨迹，如下图右边的式子所示，它们的差别是时间间隔不同，控制点不同。新的控制点就是我们要求的东西。

   我们假设最开始的时间节点$t_1=0$，$t_m^{\prime }=r_e{t}_m$，然后，我们希望新的轨迹${\mathbf{\Phi }}_f$在每一个新扩展的时间$r_{e}t$上与之前优化出来的轨迹${\mathbf{\Phi }}_s$在旧的时间t上的位置均相同，即期望${\Phi }_f(r_{e}t)={\Phi }_s(t)$，将这个等式展开，代入时间区间中的每个时间，构建如下的等式约束，同时，我们期望这两个等式的初始和终末状态也是一样的，即在起始点和终点处满足$\begin{array}{c}{\dot{\Phi }}_f(r_{e}t)={\dot{\Phi }}_s(t)\\ {\ddot{\Phi }}_f(r_{e}t)={\ddot{\Phi }}_f(t)\end{array}$,将这些条件综合整理后就得到了一个闭型最小二乘问题：$\mathbf{A}{\mathbf{Q}}^{\prime }=\mathbf{b}，{\mathbf{Q}}^{\prime }=[{\mathbf{Q}}_1^{\prime }\cdot {\mathbf{Q}}_2^{\prime }...\cdot {\mathbf{Q}}_N^{\prime }{]}^{\mathrm{T}}$

   可以利用最小二乘的闭式解，$Q^{\prime }=(A^{T}A{)}^{-1}{A}^{T}b$来求得新轨迹的控制点，从而得到优化所需的初始解。就可以调用优化算法进行迭代优化了，这里优化的目标函数的第二项与之前的不一样，如下所示：

   那么这个新出现的曲线拟合项具体是什么呢？

   它的设计思路，跟上面计算初值是相同的，就是希望新的轨迹在新的扩展的时间上面与旧的轨迹对应的时间上的位置尽可能的相同，这里的旧轨迹即原轨迹${\Phi }_s$，它是由后端优化得到的，已经考虑了避障，是无碰撞的轨迹，所以只有新的轨迹与该轨迹的位置尽可能的相同，也就可以实现无碰撞，所以这里优化的目标函数里不需要包含避障项。

   在Ego Planner中希望在与速度垂直的方向上，新的轨迹与原轨迹尽可能的贴合。而沿着速度的方向，可以不贴合的那么好，放松一点，来使得轨迹更加平滑。下图中黑色的是原轨迹，绿色的是新轨迹，我们想要垂直速度方向上位置的差值$d_r$小一些，沿速度方向的$d_a$可以大一些，即让$d_r$对应的cost大一些，让$d_a$对应的cost小一些，即让下面第三个式子分母中的a的值大一些，b的值小一些。

   $$\begin{array}{rl}& d_a(\alpha )=\left({\Phi }_f(\alpha T^{\prime })-{\Phi }_s(\alpha T)\right)\cdot \frac{{\Phi }_s^{\prime }(\alpha T)}{\Vert {\dot{\Phi }}_s(\alpha T)\Vert }\\ & d_r(\alpha )=\Vert \left({\Phi }_f(\alpha T^{\prime })-{\Phi }_s(\alpha T)\right)\times \frac{{\dot{\Phi }}_s(\alpha T)}{\Vert {\dot{\Phi }}_s(\alpha T)\Vert }\Vert \\ & {\mathcal{J}}_f={\int }_0^1\left(\frac{d_a(\alpha {)}^2}{a^2}+\frac{d_r(\alpha {)}^2}{b^2}\right)\mathrm{d}\alpha \end{array}$$

   至此，Ego Planner的时间重分配部分也介绍完了

---

   最后，再来总结一下Ego Planner框架的流程

   首先，我们可以通过非常简单天真的方法（不考虑障碍物），得到一条比较光滑的从起点到目标点的B样条曲线的初值，然后使用均匀的B样条去做轨迹优化，轨迹优化过程中，如果发现控制点碰到障碍物，就将该碰撞产生的新cost项，加入到优化的目标函数中，重新进行轨迹优化，直至收敛得到无碰撞的满足需求的解，然后再进行时间的重分配，依然使用均匀B样条，先计算出新的时间间隔△t，然后用最小二乘计算出一个新轨迹控制点初值，然后进行优化，使得新轨迹在新的时间与原轨迹在对应旧时间的位置上尽可能贴合，并满足光滑，动力学约束等，这个过程更像是一个曲线拟合问题，最终得到我们需要的轨迹。

---

   参考资料：

   1、深蓝学院-移动机器人运动规划

---