最优轨迹生成（二）—— 无约束BVP轨迹优化

   本系列文章是学习深蓝学院-移动机器人运动规划课程第五章最优轨迹生成 过程中所记录的笔记，本系列文章共包含四篇文章，依次介绍了微分平坦特性、无约束BVP轨迹优化、无约束BIVP轨迹优、 带约束轨迹优化等内容

   本系列文章链接如下：

   最优轨迹生成（一）—— 微分平坦

   最优轨迹生成（二）—— 无约束BVP轨迹优化

   最优轨迹生成（三）—— 无约束BIVP轨迹优化

   最优轨迹生成（四）—— 带约束轨迹优化

---

   二、无约束BVP轨迹优化

   通过前文的介绍，可以知道，我们在优化时是对微分平坦输出 z = { r , ψ } ∈ R 3 × S O ( 2 ) z=\{r,\psi\}\in\mathbb{R}^3\times\mathrm{SO}(2) z={r,ψ}∈R3×SO(2)做优化，而不是直接对状态x和输入u做优化。该优化问题的一般化数学描述如下：

   $$\begin{array}{rl}\underset{z(t),T}{\min }& {\int }_0^{T}v(t{)}^{\mathrm{T}}\mathbf{W}v(t)\mathrm{d}t+\rho (T),\\ \text{s. t.}& z^{(s)}(t)=v(t),\forall t\in [0,T],\\ & \mathcal{G}(z(t),\cdots ,z^{(s)}(t))⪯0,\forall t\in [0,T],\\ & z(t)\in \mathcal{F},\forall t\in [0,T],\\ & z^{[s-1]}(0)={\bar{z}}_o,z^{[s-1]}(T)={\bar{z}}_f,\\ & z^{[s-1]}:=(z^{\mathrm{T}},{\dot{z}}^{\mathrm{T}},\cdots ,z^{(s-1{)}^{\mathrm{T}}}{)}^{\mathrm{T}}.\end{array}$$

   上式中，z(t)即微分平坦输出的轨迹，T是轨迹的时间，我们不仅要优化轨迹本身，还要优化轨迹的时长。微分平坦输出的高阶导肯定是存在的，我们选取其高阶导$z^{(s)}(t)=v(t)$作为平滑项，当s=1时，目标函数的含义即让一阶导的能量函数尽可能低，让它更加平滑，若W取单位对角矩阵，则我们优化的就是$v^{T}v$。因为，我们需要使得时间T尽快短，所以要在目标函数中加上一项$\rho (T)$,作为时间T的惩罚项。

   上式中 $z(t)\in \mathcal{F},\forall t\in [0,T],$中的$\mathcal{F}$表示栅格地图或者点云地图中可通行区域。

   上式中，$\mathcal{G}(z(t),\cdots ,z^{(s)}(t))⪯0,\forall t\in [0,T],$是关于微分平坦轨迹输出的约束，这个$\mathcal{G}$里面可以放很多的约束。

   上式中，$z^{[s-1]}(0)={\bar{z}}_o,z^{[s-1]}(T)={\bar{z}}_f$和$z^{[s-1]}:=(z^{\mathrm{T}},{\dot{z}}^{\mathrm{T}},\cdots ,z^{(s-1{)}^{\mathrm{T}}}{)}^{\mathrm{T}}.$ 表示轨迹需要满足的边界条件，${\overline{z}}_o,$和${\overline{z}}_f$是给定的初始和终端的状态。

   当选取不同的平滑项阶次s时，会有不同的效果，当s取为3时，即最小化jerk，同时也是最小化角速度，有利于视觉跟踪。当s取4时，最小化snap，减小差动推力（力矩），节约能源。

---

---

  

   下面来看一下无约束情况下该问题怎么解，其数学描述如下，我们可以仅指定起点和目标点的Z值，也可以再指定中间某些点的z值（即可指定位置也可指定速度）

   $$\begin{array}{rl}& \underset{z(t)}{\min }\begin{array}{r}{\int }_{t_0}^{t_M}v(t{)}^{\mathrm{T}}\mathbf{W}v(t)\mathrm{d}t,\end{array}\\ & s.t.z^{(s)}(t)=v(t),\forall t\in [t_0,t_M],\\ & z^{[s-1]}(t_0)={\bar{z}}_o,z^{[s-1]}(t_M)={\bar{z}}_f,\\ & z^{[d_i-1]}(t_i)={\bar{z}}_i,1\le i<M,\\ & t_{i-1}<t_i,1\le i\le M.\end{array}$$

   以上问题的解$z^{\ast }(t):[t_0,t_M]\mapsto {\mathbb{R}}^m,$是最优的当且仅当以下条件成立时：

   （1）z在每一段轨迹上可以以一个2s-1次的多项式来表示，比如s是3的话，那么最优轨迹一定是5次多项式

   （2）边界条件和中间条件都成立。

   （3）最优解在$t_i$时刻，一定是$2s-d_i-1$次的连续可微，比如s=3，d=1时，需要z是4次连续可微的，即z的位置、速度、加速度、jerk、snap都是存在且连续的，因为此时s取的是3，所以位置、速度、加速度一定是存在且连续的，这个条件告诉我们要想轨迹最优还需保证jerk和snap是连续的，这是最优解的一个特质。

---

   我们把上面的数学描述再简化一下，去掉对终点节点的指定，仅给定起始点和目标点，由多段轨迹变成一段轨迹，其数学描述如下

   $$\begin{array}{rl}& \underset{z(t)}{\min }{\int }_0^{T}v(t{)}^{\mathrm{T}}\mathbf{W}v(t)\mathrm{d}t,\\ & \begin{array}{c}s.t.\end{array}{z}^{(s)}(t)=v(t),\forall t\in [0,T],\\ & z^{[s-1]}(t_0)={\bar{z}}_o,\\ & z^{[s-1]}(t_M)={\bar{z}}_f.\text{Boundary value}\end{array}$$

   在让无人机接乒乓球的示例中，我们可以仅指定当前状态和目标状态，去解这样的BVP问题，得到一系列的轨迹，如下图所示，然后对每个轨迹进行检查是否与障碍物相撞，是否满足动力学约束，然后从符合要求的轨迹中选取能量耗费最少或者目标函数值最小的轨迹。

---

   那么我们如何把一段轨迹求出来呢？比如s取3时，由上面介绍的最优解条件，可知最优轨迹一定是5次多项式，我们可以设其表达式如下，

   $$x(t)=c_0+c_{1}t+c_2{t}^2+c_3{t}^3+c_4{t}^4+c_5{t}^5$$

   我们可以对其求一次导得到速度表达式，求二次导得到加速度表达式，再将上面给定的边界值条件代入，可以得到以下关系式

   $$\left[\begin{array}{c}a\\ v_0\\ 0\\ b\\ v_T\\ 0\end{array}\right]=\left[\begin{array}{cccccc}1& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0\\ 0& 0& 2& 0& 0& 0\\ 1& T& T^2& T^3& T^4& T^4\\ 0& 1& 2T& 3T^2& 4T^3& 5T^4\\ 0& 0& 2& 6T& 12T^2& 20T^3\end{array}\right]\left[\begin{array}{c}{c}_0\\ c_1\\ c_2\\ c_3\\ c_4\\ c_5\end{array}\right]$$

   上式可表示成$d={\mathbf{A}}_{\mathrm{F}}(T)c$，对A矩阵求逆放到左边即可得到该5次多项式的系数c，从而解出该轨迹。

   对于一些由离散的点构成的折线路径，无人机很难进行跟踪，我可以采用以上的方法，在每段折线的起点和终点处解BVP问题，给定起始点和终点状态时保证上一段折线的终点状态和下一段折线的起始点状态相同即可。就可以得到一个由多段多项式构成的光滑路径。

   上面给出了一种比较简单的实例的求解方式，那么对于更一般的BVP问题，该如何求解呢

   首先，我们可以像上面那样把这个2s-1次多项式的系数写出来，写成$d={\mathbf{A}}_{\mathrm{F}}(T)c$的形式，d是由初始和终末的边界条件构成的常量向量，一般情况下${\mathbf{A}}_F(t)=\left[\begin{array}{cc}\mathbf{E}& 0\\ \mathbf{F}(t)& \mathbf{G}(t)\end{array}\right]$的各分块矩阵的构成如下：

   $$\begin{array}{rl}& {\mathbf{E}}_{ij}=\{\begin{array}{llc}(i-1)!& ifi=j,& \\ 0& ifi\ne j,& \end{array}\\ & {\mathbf{F}}_{ij}(t)=\{\begin{array}{cc}(j-1)!/(j-i)!\cdot t^{j-i}& ifi\le j,\\ 0& ifi>j,\end{array}\\ & {\mathbf{G}}_{ij}(t)=\frac{(s+j-1)!}{(s+j-i)!}\cdot t^{s-i+j}.\end{array}$$

   一般情况下，求系数c时需要对矩阵${\mathbf{A}}_F(t)$进行求逆，其实，我们也可以不用每次都直接求逆，在2021的论文Generating Large-Scale Trajectories Efficiently using Double Descriptions of Polynomials   中给出了其逆矩阵${\mathbf{A}}_B(t)=\left[\begin{array}{cc}\mathbf{U}& 0\\ \mathbf{V}(t)& \mathbf{W}(t)\end{array}\right]$的表达形式,如下所示：

   $$\begin{array}{rl}& {\mathbf{U}}_{ij}=\{\begin{array}{cc}1/(i-1)!& ifi=j,\\ 0& ifi\ne j,\end{array}\\ & {\mathbf{V}}_{ij}(t)=\frac{{\sum }_{k=0}^{s-\max (i,j)}(-1{)}^k(\genfrac{}{}{0ex}{}{s}{i+k}\left)\right(\genfrac{}{}{0ex}{}{2s-j-k-1}{s-1})}{(j-1)!(-1{)}^i\cdot t^{s+i-j}},\\ & {\mathbf{W}}_{ij}(t)=\frac{{\sum }_{k=0}^{s-\max (i,j)}\left(\genfrac{}{}{0ex}{}{s-k-1}{i-1}\right)\left(\genfrac{}{}{0ex}{}{2s-j-k-1}{s-1}\right)}{(j-1)!(-1{)}^{i+j}\cdot t^{s+i-j}}.\end{array}$$

   所以，我们可以直接通过$c={\mathbf{A}}_{\mathrm{B}}(T)d$解出所需要的多项式系数c

---

---

   边界条件、中间条件：航路点位置（方向）可以通过路径规划（A*，RRT* 等算法）得到，再利用上述思路求解BVP对其进行平滑。

   此外，一般来说，对于任意次的多项式轨迹，还需要检测其最大速率、加速度是否超过允许范围，是否碰撞障碍物等，那么如何保证所得到的轨迹始终（在0~T时间内均满足，而不是只在一些离散的点满足）满足这些约束呢？

   我们用下面的多元多项式G来表示这些约束:

   $$G(a,b,c):=\sum _{e_1+e_2+e_3\le d_g}^{d_c\in \mathbb{R},e_j\in \mathbb{N}}{d}_c\cdot a^{e_1}{b}^{e_2}{c}^{e_3}$$

   比如速度小于1，可表示成$\sqrt{\dot{X}{(1)}^2+{\dot{X}}^2(2)+{\dot{X}}^2(3)}\le 1$，也即$\dot{X}(1{)}^2+{\dot{X}}^2(2)+{\dot{X}}^2(3)\le 1$这样一个三元二次多项式，再比如圆形障碍物的半径为r，圆心为o，则与该障碍物不碰撞可以表示成$(x-0{)}^T(x-0)\ge r^2$，也是一个关于x1、x2、x3的多元多项式。

   那么如何求类似于上面的多元多项式呢？

   第一种思路是通过离散时间采样的方式，如下图中左下角的示意图所示，其优点是比较灵活，不仅仅适应于多元多项式，它适应于任意形式的约束。其缺点是若采样分辨率较低，会出现不行点恰好没有采到的情况，若采样分辨率很高，则会占用较多的计算资源。

   第二种思路是通过递归边界检查的思路，当检查0 ~ T 时间内加速度是否满足要求时，可以采用加速度的模的最大值一定会小于三个轴里面的每个轴最大值的平方和，即满足$\underset{t\in \mathcal{T}}{\max }\Vert a(t){\Vert }^2\le \sum _{i=1}^3\underset{t\in \mathcal{T}}{\max }{a}_i(t{)}^2$，如果超了，则轨迹不可行，如果没有超，则将轨迹二分，直到上界给出一个答案或者T已经被二分到很小的分辨率。该方法比第一种思路要快，但它不是很灵活，它不一定适合于其他形式的多项式、它的效果也是跟分辨率相关的、且存在一些不确定的情况。

   第三种思路是用的比较多的，直接检查v有没有超过最大值，将v的范数对t求导，其表达式如下：

   $$\begin{array}{rl}\frac{d{v}_{\mathrm{norm}}(t)}{dt}& =\frac{2\left(\dot{p}(t{)}_x\cdot \ddot{p}(t{)}_x+\dot{p}(t{)}_y\cdot \ddot{p}(t{)}_y+\dot{p}(t{)}_z\cdot \ddot{p}(t{)}_z\right)}{-\sqrt{(\dot{p}(t{)}_x{)}^2+(\dot{p}(t{)}_y{)}^2+(\dot{p}(t{)}_z{)}^2}}\end{array}$$

   求这样关于t的多元多项式的值，来得到最大的速度，从而判断其速度是否超过约束，比如下图中右下角的示意图中红色的轨迹超了，蓝色的轨迹没有超。这种方法需要我们去求多项式的根，当轨迹的次数比较高的时候，也需要采用近似迭代的方式，也不能求得解析解。其优点是效率是稳定的，他不需要递归，有时候需要数值迭代。

---

   以上的三种思路都有一定的局限性，那么有没有一种方案，可以对所有的由连续的多元多项式表示的约束进行检查呢？

   第四种方法的思想如下：

   约束是由多元多项式构成的，轨迹本身又是多元多项式，我们可以把轨迹代入到约束中，如下式所示，其中G本身是关于t的单元多项式

   $$G(p_1^{(i)}(t),p_2^{(i)}(t),p_3^{(i)}(t))<0,\forall t\in [0,T]$$

   我们可以先检查t=0和T时，即起始状态和终末状态是否满足多元多项式约束，若满足再检查(0,T)内是否满足多元多项式约束，也就等价于检查轨迹与约束平面有没有相交的点，也就是G（t）=0 在（0，T）内有没有根的问题，若有根则不满足。该问题可通过斯图姆定理（Sturm Theorem）求解

   下面先来了解几个概念，斯图姆序列（Sturm Sequence）中第0项是单元多项式$\mathcal{G}(t)$，第1项是单元多项式的导数$\dot{\mathcal{G}(t)}$，第2项是第0项除以第1项所得的余项取反(这里采用的就是多项式除法)，即$-g_{k+1}(t)=Rem(g_{k-1}(t),g_k(t))$。利用该表达式，我们同样可以得到第3项、第4项 …第n项，每求一次除法，余项的次数就减1，直至一个0次多项式常量，所以n是有限的。

---

   利用上面介绍的斯图姆序列（Sturm Sequence），我们可以很容易把G(t)在（0，T）内根的个数算出来，方法如下，我们定义一个V(t)，将时间t代入到斯图姆序列 $g_0(t)$、$g_1(t)$、$g_2(t)$、… 、$g_n(t)$中，统计从序列开始到结束其值正负变换的次数，如n=5时，将t代入后 $g_0(t)$ ~$g_5(t)$的值的正负分别为 + - - +++，则其符号变换了2次，即由正变为负再变为正，此时就定义V(t)=2。

   取t=0和t=T，分别得到起始状态和终末状态的V(0)、V(T)。则|V(T)-V(0)|的值，即为G(t)在（0，T）上实根的个数。

   再举一个具体的例子，当G(t)=$18t-27t^2+10t^3-t^4$，计算在T取[-1,7]范围上根的个数，首先对端点-1和7处进行验证其值是否为0，若不为0（即两端点处满足约束），再使用以上方法验证(-1,7)上是否满足

   容易得到该实例的斯图姆序列为：

   $$\begin{array}{rl}& G(t)=-t(t-1)(t-3)(t-6)=18t-27t^2+10t^3-t^4,\\ & g_0(t)=G(t),\\ & g_1(t)=18-54t+30t^2-4t^3,\\ & g_2(t)=-11.25+20.25t-5.25t^2,\\ & g_3(t)=13.2245-10.7755t,\\ & \begin{array}{rl}{g}_4(t)=& -\mathrm{5.69473.}\end{array}\end{array}$$

   当t=-1时， $g_0(t)$ ~$g_4(t)$值依次为-56.00，106.00，-36.75，24.00，-5.69，正负号变化了4次，所以V（-1）=4

   当t=7时， $g_0(t)$ ~$g_4(t)$值依为-168.00、-262.00，-126.75，-62.20，-5.69。，正负号变化了0次，所以V（7）=0

   所以G（t）在[-1,7]上根的个数为 |0-4|=4，有根，不满足约束，如下图中的绿色圆圈所示：

   有一种特殊情况需要注意，当序列的值中含有0项时，比如 -5、0、 -1 、0、2则将0项删除后，再判断符号变换次数，比如-5、 -1 、2，符号变换了1次。

---

   一但判断出G(t)=0在某个区间有没有根，我们就知道G（t）在该时间区间内有没有满足连续的约束。利用该方法直接去检查一个轨迹$p(t)=c0+c_{1}t+....+c_5{t}^5$的速度有没有超最大速度约束2时，可得G（t）= ${\dot{p}}_1^2+{\dot{p}}_2^2+{\dot{p}}_3^2-4\le 0$，将$p(t)$代入到G(t)中，算出该速度约束关于t的单元多项式，利用上面介绍的斯图姆定理（Sturm Theorem），可简单判断出是否有根，从而判断出是否满足约束。

   BVP可用于求解在复杂情况下RRT中状态点到状态点之间的连接，下图中给出了上面四种方法的性能对比：

---

---

   参考资料：

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

---