---

   本系列文章用于学习记录Fast-Planner规划算法的相关内容，主要学习资料是深蓝学院的移动机器人运动规划课程

   三、Fast Planner后端之轨迹优化

   在上一篇文章中，我们给出了 Fast Planner中的B样条曲线的表达式，在给定时间区间后，唯一的变量就是控制点的位置

   $$\begin{array}{rl}\mathbf{p}(s(u))& =\mathbf{s}(u{)}^{\mathrm{T}}{\mathbf{M}}_4{\mathbf{q}}_m\\ \mathbf{s}(u)& =[1s(u)s^2(u)s^3(u){]}^{\mathrm{T}}\\ {\mathbf{q}}_m& =[{\mathbf{Q}}_{m-3}{\mathbf{Q}}_{m-2}{\mathbf{Q}}_{m-1}{\mathbf{Q}}_m{]}^{\mathrm{T}}\\ \mathbf{s}(u)& =(u-u_m)/\Delta u\end{array}{\mathbf{M}}_4=\frac{1}{3!}\left[\begin{array}{cccc}1& 4& 0& 0\\ -3& 0& 3& 0\\ 3& -6& 3& 0\\ -1& 3& -3& 1\end{array}\right]$$

   接下来介绍如何构建优化问题来得到满足各种约束的轨迹，需要注意的是两端的控制点，即起点和目标点是固定的，在考虑到一些求导及特性，我们仅选取中间部分的N+1-2$p_b$个控制点，即 { Q p b , Q p b + 1 , … , Q N − p b } \{\mathbf{Q}_{p_b},\mathbf{Q}_{p_b+1},\ldots,\mathbf{Q}_{N-p_b}\} {Qpb​​,Qpb​+1​,…,QN−pb​​}作为优化变量

   目标函数由三部分组成，对应的期望轨迹是顺滑的，期望轨迹是无碰撞的，期望轨迹的速度和加速度是有上限的。即

   $$f_{total}={\lambda }_1{f}_s+{\lambda }_2{f}_c+{\lambda }_3(f_v+f_a)$$

   我们想要得到就是使上面$f_{total}$极小的一系列控制点的位置x

   接下来，我们依次来具体看一下构成目标函数的三部分内容的具体函数表达式，首先是期望轨迹顺滑的项$f_s$

   Fast Planner中通过捕获轨迹的几何信息并且不依赖于时间分配的函数来定义平滑成本$f_s$，如下式所示，原因是优化后的时间分配可能会有所调整，这与许多使用snap或jerk的平方的积分作为平滑成本的方法不同

   $$f_s=\sum _{i=p_b-1}^{N-p_b+1}\Vert ({\mathbf{Q}}_{i+1}-{\mathbf{Q}}_i)+({\mathbf{Q}}_{i-1}-{\mathbf{Q}}_i){\Vert }^2$$

   在优化时，需要计算$f_s$的梯度，即$\frac{\partial \Vert ({\mathbf{Q}}_{i+1}-{\mathbf{Q}}_i)+({\mathbf{Q}}_{i-1}-{\mathbf{Q}}_i){\Vert }^2}{\partial {\mathbf{Q}}_i}$

---

   我们接着看期望轨迹的速度和加速度是有上限的代价项$f_v+f_a$的具体表达式，我们可以规定在每一维上（ μ ∈ { x , y , z } \mu\in\{x,y,z\} μ∈{x,y,z}），若加速度或速度的平方小于设定的最大值的平方时，该项代价值为0，大于设定的最大值的平方时为${\left({a_{\mu }}^2-{a_{max}}^2\right)}^2$或${\left(v_{\mu }{}^2-v_{max}{}^2\right)}^2$，如下所示：

   $$F_{\nu }(v_{\mu })=\{\begin{array}{cc}{\left(v_{\mu }{}^2-v_{max}{}^2\right)}^2& v_{\mu }{}^2>v_{max}{}^2\\ 0& v_{\mu }{}^2\le v_{max}{}^2\end{array}$$

   $$F_a(a_{\mu })=\{\begin{array}{cc}{\left(a_{\mu }{}^2-a_{max}{}^2\right)}^2& a_{\mu }{}^2>a_{max}{}^2\\ 0& a_{\mu }{}^2\le a_{max}{}^2\end{array}$$

   但是对于一条轨迹而言，检查每一时刻的速度和加速度是比较麻烦的事情，所以，我们对三阶B样条轨迹求导，借助得到的二阶B样条，也就是速度轨迹，利用B样条的凸包性，只需要让顶点处的速度小于最大值，速度轨迹上所有点的速度就均小于最大值，同理，B样条轨迹求二次导，得到一阶的加速度轨迹，同样只需保证顶点处的加速度小于设定的最大值，则整条轨迹的加速度都小于最大值。所以代价函数可以写为下式，即只把速度轨迹、加速度轨迹的各个顶点(控制点)，分别代入到上面给出的表达式，然后求和即可，如下式所示：

   $$f_v=\sum _{\begin{array}{c}\mu \in \end{array}}\sum _{\begin{array}{c}i=p_b-1\end{array}}^{N-p_b}{F}_v\left(({\mathbf{V}}_i{)}_{\mu }\right),f_a=\sum _{\begin{array}{c}\mu \in \end{array}}\sum _{\begin{array}{c}i=p_b-2\end{array}}^{N-p_b}{F}_a\left(({\mathbf{A}}_i{)}_{\mu }\right)$$

   它的梯度也比较容易求，如下式所示：

   $$\begin{array}{rl}& {\mathbf{V}}_i=\frac{1}{\Delta t}({\mathbf{Q}}_{i+1}-{\mathbf{Q}}_i){\mathbf{A}}_i=\frac{1}{\Delta t}({\mathbf{V}}_{i+1}-{\mathbf{V}}_i)\\ & \frac{\partial F_v\left(({\mathbf{V}}_i{)}_{\mu }\right)}{\partial {\mathbf{Q}}_i}=\frac{\partial F_v\left(({\mathbf{V}}_i{)}_{\mu }\right)}{\partial {\mathbf{V}}_i}\frac{\partial {\mathbf{V}}_i}{\partial {\mathbf{Q}}_i}\end{array}$$

---

  

   最后，我们来看期望轨迹是无碰撞的代价项$f_c$的具体表达式，Fast Planner中的碰撞成本被公式化为作用在每个控制点上的障碍物的排斥力，前面我们已经得到无碰撞的约束表达式 d c > 0 , r j , j + 1 < d c 3 , ( j ∈ { 1 , 2 , 3 } ) d_{c}>0,r_{j,j+1}<\frac{d_{c}}{3},(j\in\{1,2,3\}) dc​>0,rj,j+1​<3dc​​,(j∈{1,2,3})，它可以写成下式所示的形式

   $$\begin{gathered} f_c=\sum _{i=p_b}^{N-p_b}{F}_c(d({\mathbf{Q}}_i)) \\ {F}_c(d({\mathbf{Q}}_i))=\{\begin{array}{ll}(d({\mathbf{Q}}_i)-d_{thr}{)}^2& d({\mathbf{Q}}_i)\ge d_{thr}\\ 0& d({\mathbf{Q}}_i)<d_{thr}\end{array} \end{gathered}$$

   其中，$d(Q_i)$表示$Q_i$到他最近的障碍物的距离，上式给出了顶点与障碍物的距离约束，我们还需要满足 r j , j + 1 < d c 3 , ( j ∈ { 1 , 2 , 3 } ) r_{j,j+1}<\frac{d_c}3,(j\in\{1,2,3\}) rj,j+1​<3dc​​,(j∈{1,2,3})这个条件，其实这个条件是否满足完全取决于我们在构造B样条的时候，如何进行参数化，如果控制点取的密一些，时间间隔短一些，这个条件就很容易满足了，比如当$r_{j,j+1}<0.2$时，在大多数情况下都是满足这个条件的，所以在优化时，不必太关心这个条件。

   那么如何获得$Q_i$到他最近的障碍物的距离$d(Q_i)$呢？

   这就涉及到地图的表示方式了，在Fast Planner中采用ESDF来对地图进行表示，他是一种特殊的栅格地图，每个栅格中存放的是该栅格距离他最近的障碍物的距离，如下图所示，颜色越浅，表示与障碍物的距离越远，这种特殊的栅格地图比较容易求出它的梯度。ESDF其实也可以理解成一种距离场。

---

   有了目标函数各个组成部分的具体表达式，总的目标函数的表达式也就知道了，然后使用优化算法的求解器，就可以求解该优化问题了，得到使得目标函数值最小的那个控制点的位置x。

---

   四、Fast Planner后端之时间重分配

   当环境中障碍物比较多时，可能出现远离障碍物与满足动力学约束相违背的情况，如下图所示的情况，我们最初认为从起点到终点可以拉一条直线过去，因此，分配的时间比较少，但是由于障碍物的存在，在远离障碍物的约束下，轨迹被向上拉，拉成了比较长的曲线，这样控制点之间的距离就变大了，如果维持原有的分配时间的话，速度及加速度可能会超出限制。所以，我们先将远离障碍物项的权重给的大一些，生成一条无碰撞轨迹，然后再对时间进行重分配来使得速度及加速度满足约束。

---

   Fast Planner中是通过非均匀B样条来进行时间重分配的，由均匀B样条的表达式来推非均匀B样条的表达式，只需要将固定的时间差△t换成$t_{i+p+1}-t_{i+1}$就行了，非均匀B样条的一阶和二阶导数形式如下式所示：

   $$\mathbf{C}\dot{(}t)=\sum _{i=0}^{n-1}{N}_{i+1,p-1}(t){\mathbf{V}}_i{\mathbf{V}}_i=\frac{p({\mathbf{Q}}_{i+1}-{\mathbf{Q}}_i)}{t_{i+p+1}-t_{i+1}}\mathbf{C}(\ddot{t})=\sum _{i=0}^{n-2}{N}_{i+2,p-2}(t){\mathbf{A}}_i{\mathbf{A}}_i=\frac{(p-1)({\mathbf{V}}_{i+1}-{\mathbf{V}}_i)}{t_{i+p+1}-t_{i+2}}$$

   只改变时间分配，不改变控制点的位置，生成轨迹的几何情况是不会发生变化的

   我们对速度和加速度进行检查其是否超出了限制，以速度为例,由${\mathbf{V}}_i=\frac{p({\mathbf{Q}}_{i+1}-{\mathbf{Q}}_i)}{t_{i+p+1}-t_{i+1}}$，可获得速度控制点，然后找出速度超出限制的速度控制点，将它们中速度最大的那个速度控制点记为$V_{i,\mu }^{\prime }$，其速度的绝对值记为$v_m$，若其对应的时间区间为$({\hat{t}}_{i+p+1}-{\hat{t}}_{i+1})$,则将其增大为${\mu }_{\nu }(t_{i+p+1}-t_{i+1})$，其中${\mu }_v=\frac{v_m}{v_{max}}$,这样该速度控制点的速度就可降为$v_{max}$，满足约束。

   同理，我们可以对加速度控制点进行检查，${\mathbf{A}}_i=\frac{(p-1)({\mathbf{V}}_{i+1}-{\mathbf{V}}_i)}{t_{i+p+1}-t_{i+2}}$，找出加速度超出限制的加速度控制点，将它们中加速度最大的那个速度控制点记为$A_{i,\mu }^{\prime }$，其速度的绝对值记为$a_m$，所不同的是加速度控制点受两个速度控制点的影响，每个速度控制点都对应一个时间区间，所以加速度控制点受影响的时间范围更大一些，为$(t_{i+p+2}-t_{i+1})$，同样对超出约束的加速度控制点对应的时间区间进行扩大，即将$(t_{i+p+2}-t_{i+1})$中每个小区间都进行扩大，如下所示：

   $$\Delta t_m=t_{m+1}-t_m\text{ → }\Delta {\hat{t}}_m={\mu }_a\Delta t_m$$

   其中， m ∈ { i + 1 , i + 2 , . . . , i + p + 1 } m\in\{i+1,i+2,...,i+p+1\} m∈{i+1,i+2,...,i+p+1}、${\mu }_a={\left(\frac{a_m}{a_{max}}\right)}^{0.5}$

---

   可以发现每个速度控制点、加速度控制点都会影响好几个小区间，若将超限的速度或加速度控制点对应的每个小区间的时间都进行增大，可能会使得整条轨迹的速度变得比较慢，所以在Fast Planner中是采用迭代的方式进行时间扩展的

   在每次迭代时，都会去找超出约束的速度、加速度控制点，然后仅仅对超出约束最大的那个速度、加速度控制点对应的时间区间进行扩大，而且扩大的系数进行了抑制，扩大系数分别选为 min ⁡ { α v , v m v m a x } \min\{\alpha_{v},\frac{v_{m}}{v_{max}}\} min{αv​,vmax​vm​​}、 min ⁡ { α a , ( a m a m a x ) 1 2 } \min\{\alpha_{a},\big(\frac{a_{m}}{a_{max}}\big)^{\frac{1}{2}}\big\} min{αa​,(amax​am​​)21​}，其中，${\alpha }_v$和${\alpha }_a$都是比1稍微大一点的数，然后循环迭代以上流程，直至所有的速度、加速度控制点都满足约束。

  

---

   Fast Planner规划的框架及演示如下：

---

   参考资料：

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

---