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

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

   本系列文章链接如下：

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

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

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

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

---

   四、带约束轨迹优化

   如果我们需要处理更一般的约束，并给定状态点的可行域约束，并且需要进一步优化轨迹的总时间以及轨迹在时间和空间上的形状，则上面介绍的BVP和BIVP就不再适应，且存在以下难点：

   ①、最佳参数化是未知的，即最优的轨迹不一定是2s-1阶多项式了。

   ②、时空剖面涉及多个自由度，我们即要优化轨迹的时域又要优化轨迹的空间域，轨迹的速度、加速度、jerk等都需要优化。

   ③、$\mathcal{G}$可能包含是非凸的约束。

   ④、安全区域（环境中的可行域）可能是高复杂性。

   我们可以对以上问题进行一些可能的简化

   ①、虽然最优解不一定再是多项式曲线，但是我们能不能就在多项式曲线中去寻找最优解呢？

   ②、我们能不能把轨迹的飞行时间固定，从而不再对轨迹的时间进行优化？

   ③、我们能不能只把速度、加速度的幅值约束一下，不要管其他的角速率、推力等约束

   ④、我们能不能使安全约束凸化，从而得到一些局部凸区域约束。

   基于以上思路，我们可以把问题转换成凸优化问题，假设我们给定几个凸的问题，它们首尾相接，如下图所示的例子中，我们把轨迹分为首尾相接的三段，每段各自约束在各自的凸问题中，比如第一段$A_{0}p<b_0$、第二段$A_{1}p<b_1$ …

   $$\begin{array}{rl}\arg \underset{\Phi }{\min }{J}^q& =\sum _{i=0}^{n-1}\underset{0}{\overset{\Delta t_i}{\int }}||{\Phi }_i^{(q)}(t)|{|}^{2}dt\\ {\Phi }_i^{(k)}(\Delta t_i)& ={\Phi }_{i+1}^{(k)}(0),k=0\dots q\mathrm{and}i=1\dots N-2,\\ {\Phi }_0^{(k)}(0)& =u_s^{(k)},{\Phi }_{N-1}^{(k)}(\Delta t_{N-1})=u_g^{(k)},k=0\dots q,\\ {\mathbf{A}}_i^T{\Phi }_i(t)& \le {\mathbf{b}}_i,i=0\dots N-1.\end{array}$$

   该问题与BIVP问题最大的区别在于多了安全约束。上述目标函数中q=3时等价于BIVP中的最小化jerk，上面的第一个等式约束也与BIVP中两段相邻轨迹的连续性约束相似，所不同的是该问题中并不像BIVP中那样对中间位置点进行指定，而是通过优化去确定中间位置点，上面的第二个等式约束也与BIVP中对起始和终末状态进行指定相同。最后的不等式约束${\mathbf{A}}_i^T{\Phi }_i(t)\le {\mathbf{b}}_i$即该问题与BIVP问题的最大区别,对于该不等式约束，可以在每段轨迹的若干个时间点进行采样，看其是否满足。采样点越密集，安全约束就也稠密

   在BVP或BIVP中没有推导过代价泛函或代价函数具有什么样的形式，因为在BVP或BIVP中不需要去解最优解。但这里需要用到。

   我们先来看目标函数，对于一个多项式，我们求它的四阶导，得到snap，再将其平方，把snap多项式的系数进行卷积，在进行积分。然后，我们就可以将第j段轨迹的目标函数写成一个二次型的形式，流程如下所示：

   $$\begin{array}{rl}& f(t)=\sum p_i{t}^i\\ & ⟹f^{(4)}\overline{(\dot{t})}=\sum i(i-1)(i-2)(i-3)t^{i-4}{p}_i\\ & ⟹\left(f^{(4)}(t)\right)\stackrel{2_{i\ge 4}}{=}\sum _{i\ge 4,l\ge 4}i(i-1)(i-2)(i-3)l(l-1)(l-2)(l-3)t^{i+l-8}{p}_i{p}_l\\ & ⟹J(T)={\int }_{T_{j-1}}^{T_j}{\left(f^4(t)\right)}^{2}dt=\sum _{i\ge 4,l\ge 4}\frac{i(i-1)(i-2)(i-3)j(l-1)(l-2)(l-3)}{i+l-7}(T_j^{i+l-7}-T_{j-1}^{i+l-7})p_i{p}_l\\ & ⟹J(T)={\int }_{T_{j-1}}^{T_j}{\left(f^4(t)\right)}^{2}dt\\ & ={\left[\begin{array}{c}⋮\\ p_i\\ ⋮\end{array}\right]}^T\left[\begin{array}{ccc}\cdots & \frac{i(i-1)(i-2)(i-3)l(l-1)(l-2)(l-3)}{i+l-7}{T}^{i+l-7}& \cdots \\ ⋮& ⋮& \end{array}\right]\left[\begin{array}{c}⋮\\ p_l\\ ⋮\end{array}\right]\\ & \Rightarrow J_j(T)={\mathbf{p}}_j^T{\mathbf{Q}}_j{\mathbf{p}}_j\end{array}$$

   其中Q是二次型矩阵，P是多项式的系数，所以给定一个多项式，给定$T_{j-1}$、$T_j$，给定轨迹的总时间$t_j$、系数P，我们就可以把该段轨迹的能量或者说目标函数值的表达式。

   然后，我们再来看等式约束，比如给定的初始条件和终末条件，需要对多项式进行求导，然后把系数和对应的边界条件写出来，把权重和系数分离开，写成矩阵的形式，就可以获得等式约束，如下所示：

   $$\begin{array}{rlr}& f_j^{(k)}(T_j)=x_j^{(k)}& \\ & \Rightarrow \sum _{i\ge k}\frac{i!}{(i-k)!}{T}_j^{i-k}{p}_{j,i}=x_{T,j}^{(k)}& \\ & \Rightarrow \left[\begin{array}{ccc}\cdots & \frac{i!}{(i-k)!}{T}_j^{i-k}& \cdots \end{array}\right]\left[\begin{array}{c}⋮\\ p_{j,i}\\ ⋮\end{array}\right]=x_{T,j}^{(k)}& \\ & \Rightarrow \left[\begin{array}{ccc}...& \frac{i!}{(i-k)!}{T}_{j-1}^{i-k}& ...\\ ...& \frac{i!}{(i-k)!}{T}_j^{i-k}& ...\end{array}\right]\left[\begin{array}{c}⋮\\ p_{j,i}\\ ⋮\end{array}\right]=\left[\begin{array}{c}{x}_{0,j}^{(k)}\\ x_{T,j}^{(k)}\end{array}\right]& \text{|}\\ & \Rightarrow {\mathbf{A}}_j{\mathbf{p}}_j={\mathbf{d}}_j& \end{array}$$

   两段之间的连续性约束：当没有给出特定导数时，确保轨迹段之间的连续性

   $$\begin{array}{rl}& f_j^{(k)}(T_j)=f_{j+1}^{(k)}(T_j)\\ & \Rightarrow \sum _{i\ge k}\frac{i!}{(i-k)!}{T}_j^{i-k}{p}_{j,i}-\sum _{l\ge k}\frac{l!}{(l-k)!}{T}_j^{l-k}{p}_{j+1,l}=0\\ & \Rightarrow \left[\begin{array}{ccccc}\cdots & \frac{i!}{(i-k)!}{T}_j^{i-k}& \cdots & -\frac{l!}{(l-k)!}{T}_j^{l-k}& \cdots \end{array}\right]\left[\begin{array}{c}⋮\\ p_{j,i}\\ ⋮\\ p_{j+1,l}\\ ⋮\end{array}\right]=0\\ & \Rightarrow \left[\begin{array}{cc}{\mathbf{A}}_j& -{\mathbf{A}}_{j+1}\end{array}\right]\left[\begin{array}{c}{\mathbf{p}}_j\\ {\mathbf{p}}_{j+1}\end{array}\right]=0\end{array}$$

---

   凸优化的全局最优解与局部最优解是一致的，凸函数中任意两点连线之间的值始终要比这两点要小，即下式成立

   $$f(\theta x+(1-\theta )y)\le \theta f(x)+(1-\theta )f(y)$$

   对于一个集合，若任意两点连线都属于集合内部，则为凸集合

   $$\theta x+(1-\theta )y\in C.$$

   如果一个问题具有凸的函数，并且它的不等式约束构成的集合是一个凸集合，则将其称为凸优化问题

   一般来说要求等式约束是线性的，不等式约束是凸的

---

   如何把常见的轨迹优化转换成凸优化问题呢，应该转换成什么类的凸优化呢。最简单的就是线性规划LP，线性的目标函数和约束。然后是比较常用的二次规划QP，它的目标函数带有二次项，约束项跟LP类似是线性的。如果我们把QP的不等式约束也放松成二次的，则为QCQP规划。锥规划SOCP的目标函数和等式约束都是线性的，不等式约束是$\Vert A_{i}x+b_i\Vert \le c_i^{T}x+d_i$的形式。另外LP、QP、QCQP都是可以转换成SOCP形式的。

   把轨迹优化转换成凸优化问题后，可以调用一些库来求解凸优化问题，比如锥规划中效率很高的库Mosek，再比如对线性求解效率比较高、稳定性很高的GLPK。此外，还有CVX、OOQP等

   CVX：http://cvxr.com/cvx/

   Mosek：https://www.mosek.com/

   OOQP：https://pages.cs.wisc.edu/~swright/ooqp/

   GLPK：https://www.gnu.org/software/glpk/

---

   凸优化也存在一些数值不稳定问题，我们可以采用相对时间来表示每一段的轨迹，并把每一段轨迹占有的时间归一化为1，来使得问题的条件数更好，我们把一个问题的数值缩放100倍、缩放1000倍和不缩放对凸优化而言它的效率是不一样的

---

   如何把凸优化应用到实际的运动规划中去呢？

   首先，我们可以把障碍物进行栅格化，然后从栅格地图中寻找从起始点到目标点的一系列的由长方形构成的可行区域（搜索飞行走廊），然后把狭窄的可行区域进行扩大，获得更大的长方体，然后再在长方体构成的可行域内进行轨迹优化。

   我们在进行轨迹优化时，需要让状态中的位置不要超出长方体构成的几何区域，同样求导后的速度也不要超过限制（也是一个长方体）依次类推，加速度也适应。

---

   ·前面为了简化问题，我们将每段轨迹的时间进行了任务分配，而时间分配显著影响最终轨迹。那么如何合理分配时间？

   时间分配问题是非凸优化问题，凸优化无法处理，一个比较好的方案是使用飞行走廊。飞行走廊的容错率比较高，比如我们有下面三个有重叠部分的长方体，我们只需要分配各个长方体中的飞行时间即可，由于长方体存在重叠部分，指定的时间就有了浮动范围，但该浮动也存在上限。我们也可以在每段轨迹上采用梯形的速度，先加速，再匀速，再减速

---

   在考虑安全约束、动力学约束，并且要优化时间分配的情况下，该怎样处理呢？

   轨迹p(t)可以被航时T和航点q决定，采用这种新的参数化的方式，直接调整航点q和时间向量T来使得，比如把某段轨迹的时间过短，超出了其最大速度，可以把时间拉伸，同样的轨迹的某段超出了安全区域，可以把航点q进行调整，使得轨迹变为安全轨迹。

   所以，一个轨迹需要有直接进行空间时间形变的能力，这样就可以处理一些列违背约束的情况

   利用上述时空形变的方法可以完成一系列高效的求解

   我们将这种时间空间形变的轨迹用在多面体或者球体空间中，都可以在很短的时间内达到$V_{max}$，即下图中红色曲线，保持这样的最大速度均速飞行。

---

   参考资料：

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

---