1. Preliminaries

Ch2讲了基于搜索的路径规划方法，Ch3讲了基于采样的路径规划方法，这些都是global methods，框架都是Exploration and Exploitation，且在算力足够大的情况下，一定能够找到全局最优解。

除了global methods，还有local methods，主要是Deterministic Optimization确定性优化。基于优化的方法，主要是利用cost function的0阶和高阶导数来进行优化（如梯度下降，迅速地寻找到一个局部极小值local minimum），与global methods不同的是，此类方法少了探索的过程，所以不能保证结果最优。

PRM*，RRT* 是AO(Asymptotic Optimality)渐进最优的方法，A*，JPS是RO(Resolution Optimality)分辨率最优的方法。这些均属于global methods，在低维时(如4维一以下)工作较好，但高维时就需要实时性更好的方法。

右侧的均为local methods，CHOMP用于多机械臂轨迹优化，DDP/iLQR用于无人车规划，Flatness用于无人机，MPC/NMPC用于一般的机器人轨迹优化，这类方法一般实时性较高。

如上为global methods和local methods的优缺点对比。

global和local methods在实现上经常被分为前端和后端，可以把来自环境的复杂度和来自于动力学约束的复杂度分而治之[1,2,3,4]。

Q1：什么是轨迹？
A1：Trajectories are time-parameterized paths.
轨迹是时间参数化的路径。

Q2：Are smooth curves trajectories?
A2：smooth curves不是traj，因为没有时域信息，没有时域信息的均当作curves处理。

Q3：Are trajectories always smooth?
A3：几何学与动力学的smoothness不同，所以traj不一定smooth。

Q4：Can nonsmooth paths be trajectories?
A4：几何上非光滑的path也可以是traj。

trajectory是同时包含时间和空间特性的。

traj就是把时间映射到各种space中，space中可能包含位置，速度，偏航角，旋转等。

一个smooth的traj至少要

1. 满足微分约束（系统方程通常由常微分方程给出），
2. 最小化能量泛函(最小化cost，如能量消耗等)

为什么后端是必须的？
尽管有一些考虑了kinodynamaic的前端生成一些path，给出一些traj的初值，但是后端可以进一步优化，在前端结果不那么好的时候也能收敛到较好的结果。

除了考虑动力学约束，还需要考虑很多其他约束，如时间效率，驱动器限制，任务要求等，综合考虑这么多约束，优化是一种非常好的方法。

2. Multicopter dynamics and differential flatness（多旋翼动力学和微分平坦特性）

2.1 Differential Flatness

简单来说，对于由常微分方程(ODE，Ordinary Differential Equation)描述的系统，在对系统施加了各种约束之后，由系统状态$x$，系统状态的导数$\dot{x}$，输入$u$所张开的空间$R$中，满足约束的解 $z$ 只能在该空间的一个曲面上移动，为了进行求解，引入了微分平坦变换(differential flatness transformation)，能够在一个无约束的空间中对轨迹 $z$ 进行优化，这个优化结果也是满足$R$中的约束的，微分平坦变换就是上图的${\Psi }_x,{\Psi }_u$，该变换消除了微分约束，降低优化难度，在优化出$z$之后，可以通过微分平坦变换求出$x,u$，是满足微分约束的。

（该部分比较数学，细节如果需要的话再深入探讨）

桨对齐的四旋翼/多旋翼，线性风阻的多旋翼，桨以一定规律排布的多旋翼都满足differential flatness[5,6,7,8,9].

虽然使用differential flattness能够简化无人机的优化问题，但是有两个问题需要考虑：

1. 无人机实际飞行时需要考虑风阻drag（因为速度快时必须考虑风阻）
2. differential flattness存在奇异点，需要尽可能降低奇异点个数。

上图列出了6种使用differential flattness时会出现奇异点的地方(即出现$\frac{1}{0}$的情况)

需要说明，yaw在无人机领域不一定是传统意义上的定义，可根据应用场景定义。

1，2使用不同转序的Euler角，不考虑风阻。
3将body x轴投影至world系，与world系的x轴所成夹角即为yaw，显然奇异点在body的x轴投影与world系x轴平行时出现。
4考虑了线性的风阻。
5通过Hopf fibration定义yaw，旋转通过yaw和tilt定义(tilt即body的z轴与world的z轴所成夹角)，此方法仅有一个奇异点，出现在body的z轴与重力方向平行时(即机身完全翻转时)。
6对5中的奇异点在数学上进行了证明[10]。

---

补充：

Hopf fibration（霍普夫纤维化）是一种在纯数学中的概念，它描述了一个四维球面如何映射到三维空间上的球面。具体而言，Hopf fibration是一个从四维单位球面（称为四维球面S^{3）到三维球面（称为三维球面S}2）的映射。

这个映射的特殊之处在于，它将四维球面的每一个点映射到三维空间中的一个点，并且保持了一种特殊的关系：在四维球面上，每一条经线（类似于经度线）都被映射为三维球面上的一个圆，而纬线（类似于纬度线）则被映射为三维球面上的一条线。

Hopf fibration在数学和物理学中都具有重要的应用。它在拓扑学、几何学和量子场论等领域中有着广泛的研究和应用价值。这个概念的提出者是德国数学家Heinz Hopf，他在1931年首次引入了这个概念并进行了深入研究。

---

2.2 具体建模

参考

• state is $x=r,v,R,w$，分别为位移，速度，旋转，在body系下的角速度，分别为
  ${\mathbb{R}}^3\times {\mathbb{R}}^3\times SO(3)\times {\mathbb{R}}^3$
• 输入$u$为推力thrust的标量和3轴扭矩向量$\tau$
• 动力学方程(ODE)：$\dot{x}=fx+g(x)u$，后面细讲
• 平坦输出flat output：$z=r,\psi$分别为位移$\in {\mathbb{R}}^3$和2D yaw $\in SO(2)$

针对无人机动力学方程：
这里的物理量均定义在world系（即惯性系）下。

1. $\dot{r}=v$
   位置对时间求导是速度。
2. 牛顿第二定律$ma=F$
   右侧为无人机在飞行过程中收到的和力。

• $-mg{e}_3$：m质量，g重力标量，$e=(0,0,1{)}^T$表示竖直向上，与重力方向反向。该项表示无人机收到的重力(是向量)。

• $Rf{e}_3$：$R=R_{wb}$由body到world的旋转，$f$为总推力thrust大小。该项表示系统的推力向量在world系下的向量。

• $-RD{R}^T\sigma (||\mathbf{v}||)v$：表示无人机在飞行过程中收到的风阻。
  该项可如下理解：
  

  • $-R^{T}v$是无人机world系下速度转到body下的速度取反
  • $D\sigma (||v||)$中：$||v||$表示world系下无人机速度大小，标量$\sigma$表示缩放系数，$D$为对角矩阵，是风阻系数（与无人机外形等有关）。
  • 故$D\sigma (||v||)(-R^{T}v)$为body系下收到的风阻力向量，左乘$R$转至world系下为$RD\sigma (||v||)(-R^{T}v)$，整理为$-RD{R}^T\sigma (||\mathbf{v}||)v$整体表示无人机飞行时在world系下所受的风阻力向量。

• $\hat{R}=R\hat{\omega }$：旋转矩阵更新(反对称使用$\hat{w}$表示，搞slam的很熟悉了)

• $M\dot{\omega }=\tau -\omega \times M\omega -A(\omega )-B(R^{T}v)$：欧拉公式加上修正项。其中$M\dot{\omega }=\tau -\omega \times M\omega$为常见的欧拉公式，$M$为惯性张量矩阵，$\tau$为三轴力矩。$A(\omega )$：飞机转动时产生的阻尼力矩，$B(R^{T}v)$平动时产生的力矩。

整个系统方程中，$D,\sigma ,A,B$可以通过参数拟合，系统辨识等方法来分别确定。

2.3 Flatness Transformation的解析推导

如果已经有了flat output，如何求出我们的目标状态呢？这时需要解析地求出微分平坦变换${\Psi }_x,{\Psi }_u$，需要指出，${\Psi }_x,{\Psi }_u$的表达式很复杂，表达式也可以通过迭代的方式来给出，也即如果我们知道如何迭代地求出$x,u$，也就相当于知道了${\Psi }_x,{\Psi }_u$的表达式。

下面分别给出$r,v,R,\omega ,f,\tau$的求解过程。

对系统方程中的牛顿方程两边左乘body系下的x，y轴，即$(R{e}_i{)}^T,i\in (1,2)$，推导出下图的结论：
$$\begin{array}{r}\dot{v}+\frac{d_h}{m}\sigma (||v||v+g{e}_3)\end{array}$$
body的x，y轴与式(1)垂直

与$x_b,y_b$垂直的只有与$z_b$同向或反向，当无人机悬停时，$v=0$得出式(1)为$g{e}_3$，为推力，与重力反向，所以式(1)与重力反向，与$z_b$同向。

接着牛顿方程两边同乘$z_b=(R{e}_3{)}^T$，可以推导出推力标量$f$的表达式。

旋转$R$可由偏航旋转和倾斜旋转求出，旋转与四元数是1对2的关系，即一个旋转可对应两个不同的四元数，而一个四元数可以确定一个唯一的旋转。

旋转四元数表示为旋转轴$\mathbf{v}$和旋转角度$\theta$，即$q=((cos\theta {)}^{1\times 1},(\mathbf{v}sin\theta {)}^{3\times 1})=(w,x,y,z{)}^T$

• 偏航四元数$q_{\psi }$，仅绕$e_3=(0,0,1{)}^T$旋转，即$q_{\psi }$中$x,y=0$
• 倾斜四元数$q_{tilt}=q_z$不绕$z$轴进行旋转，可视作如下的旋转：
  
  $e_3$与目标$z_b$组成平面$P$，旋转轴为$P$的法向量$\mathbf{v}$，垂直向内，由$e_3$方向绕$\mathbf{v}$顺时针旋转$\theta$得目标$z_b$，无$yaw$方向的旋转，故$q_z$中$z=0$

由$q_{\psi }$和$q_z$可求出总体旋转四元数$q=q_z\otimes q_{\psi }$，由$q$可唯一地确定旋转矩阵$R$，具体操作可参考[11]。

角速度$\omega$可由$\dot{R}=R\hat{\omega }$求出。
同样也可求出$\dot{z_b}$，其中$\mathcal{DN}(x)$表示归一化函数的一阶导数。

得到$\omega$的公式后，根据动力学模型的欧拉公式，可以推导出torque的表达式，于是$x,u$中的变量
$r,v,R,\omega ,f,\tau$均求出，对应以上PPT中的标红部分，也即flatness transformation ${\Psi }_x,{\Psi }_u$的表达式已经求出。

利用differential flatness可以将带有等式约束的优化问题转换为没有等式约束的优化问题(但仍然存在一些不等式约束)，极大提高优化求解速度。

在控制方面也可以使用微分平坦特性，如反馈和前馈(cal desire，类似串级控制的内环输出)控制中的微分平坦变换。如果task为跟踪一条比较高维的轨迹$x,u$，而我们只能控制一个四维的$z$，可以输入高维的$x,u$，使用flattness计算出低维的flat output，进行控制，再使用${\Psi }_x,{\Psi }_u$计算出高维的实际输出$x，u$，只要输出可导即可。该过程可以看做一个压缩(flat output)和解压(flattness transformation ${\Psi }_x,{\Psi }_u$)的过程，该过程是解析地进行的，所以速度较快。

实际中，使用样条来获得flat output有以下优点：

1. 容易确定是否可导；
2. 容易求出闭式导数，闭式表达式等；
3. 可以解耦进行，每个维度上独立进行(如x维使用2阶近似，y使用3维，z使用4维，$\psi$使用5维等)；
4. 计算的数值稳定性已有较多研究。

不清楚的方面：如何使用如spline这样的方法获得flat output?

3. Trajectory Optimization轨迹优化

3.1 Problem formulation

如上即为traj optimization general problem formulation.

1. 我们的优化目标函数是最小化优化项的energy(加权平方积分)，使其更加平滑。函数是基于平坦输出$z(t)$而非直接的$x,u$，具体而言，优化$z^{(s)}(t)$，s取不同值时即优化不同项，

• s=1速度，
• s=2加速度，
• s=3，jerk；
• s=4，snap
  有不同的效果，后面会讲。
  优化$z(t)$的高阶导数的能量，使其更加平滑。
  不光优化$z(t)$，还优化时间$T$，如果不优化时间，当$T\to +\infty$时，energy可能趋近于0，所以需要加上一个正则项$\rho (t)$（如$\rho (t)=100T$），使得整体的优化时间不至于过长。

2. 其中$\mathcal{G}(z(t)\cdots z^{(s)}(t))<0$是约束，不是基于$x,u$，而是基于flat output和flatness transformation ${\Psi }_x,{\Psi }_u$的约束表示。和前面提到的一样，约束中的变量比较灵活，可以添加任何想要加入的变量（如控制时间，无人机倾角限制等）

3. $z(t)$需要在free space $\mathcal{F}$中，是collision-free的：由于一张地图中start->goal的拓扑组合非常多，所以给出$\mathcal{F}$非常困难。

4. 具体问题中会给出起始和末端的状态约束。

优化不同的量会有不同的效果：

• 如优化jerk的energy可以最小化角速度，便于保持稳定；优化
• 优化snap的energy可以最小化微分推力，节省能量。

3.2 Unconstrained case

学界中已经探讨出了最优性充要条件，如$\mathcal{F}，\mathcal{G}$约束不存在，$\rho (T)$中$T$已给定，约束也不存在。
下面讲解一种Unconstrained case[12]，这也是本节课讲师已经发表的工作：

上图为最优性的条件，约束中没有$\mathcal{F}，\mathcal{G}$的约束，

• s=3时，最优$z^{\ast }(t)$为2s-1=5次多项式
• 需满足边界和中间约束(3.3节会讲解没有中间约束，自己生成，而非给定的情况BIVP)
• d=1时，$z^{\ast }(t)$为2s-d-1=4阶连续，即其0，1，2，3，4阶导数均存在。
  将各阶导数组成的等式约束联立求解即可求出最优解。

与[13]一样，此处的思路仍然是先只考虑边界约束，不考虑动力学等其他约束来生成一系列primitives，然后再使用约束对生成的primitives进行check，选出最优的。

根据边界条件联立方程组，求出$x(t)$的系数，也就求出了$x(t)$的表达式。

可以对一段轨迹进行segmentation，分别构造方程$d=A_F(T)\ast c$，对系数矩阵$A_F(T)$求逆可求解每一个seg的BVP，

• 使得整个轨迹看起来更顺滑，且轨迹会满足各个seg求解时的boundary condition，
• 整段轨迹更倾向于匀速运动
• 对于短的seg，时间T较小，矩阵$A_F(T)$求逆可能存在数值不稳定的问题，在[14]中给出了$A_F(T{)}^{-1}$的解析形式(也是讲师的工作)，可以offline算出解析解的其余部分，online时只带入$t$的高阶项即可。

如上所示即为$A_F(T{)}^{-1}$的解析形式[13]，可以使BVP的求解过程更快。
这样的好处是对于primitives-check这样框架的方法来说，可以使得path的光滑逼近问题计算地更快，效率更高(解多个BVP问题更快)。

如上即为multi-segment traj的应用[13]。

如何对生成的primitives进行check？且要求是在连续空间而非离散空间中是feasiable的。
每个维度构成一个safe flight corridor，整个primitives都需要在corridor中才算是feasiable的。

结论：把连续时间约束表示为多元多项式，使得多元多项式满足连续时间约束。
如：

表示速度<1，类似的，我们的目标是把其他约束都表示为类似的多元多项式，对多元多项式进行check。

介绍了三种方法：

1. 离散时间采样：优点：灵活。缺点：低分辨率容易漏检，高分辨率算的太慢。
2. 递归check[13]：只针对thrust/a/v的5次多项式的check，构造一个上界，二次的最大值，有解析解，当T->0时是最tight的upper bound，优点：高效。缺点：仅适用于5次多项式check。
3. 极值check[15]：对目标函数求导=0，求出极值，对极值进行check。优点：稳定，对所有场景有效（要么有解析解，要么可以用数值解）。缺点：数值解需要进行迭代，慢。

那么有没有一种通用的解决多元多项式时间约束check的方法？[16]给出(高产！)

检查
$$\begin{array}{r}\mathcal{G}(p_1^{(i)}(t),p_2^{(i)}(t)，p_3^{(i)}(t))<0，\forall t\in [0,T]\end{array}$$是否成立，转换为在$t\in [0,T]$内$\mathcal{G}(p_1^{(i)}(t),p_2^{(i)}(t)，p_3^{(i)}(t))=0$是否有根，如果有根，则式(2)必定不成立。

---

感觉这篇学术产出都是把问题转换为一个数学问题，然后看在数学领域有没有现成的比较成熟的求解方法，应用到这里，就完成了一项工作。

---

关于在给定区间内方程是否有根，在数学上有比较成熟的方法： Sturm Theorem

定义 Sturm Sequence，其中Rem()是求$\frac{a}{b}$的多项式除法的余项，同样也是多项式。

定义$V(t)$为$[0,T]$之间的$g$值的正负跳变次数，计算方法是$V(t)=V(T)=V(0)$，上图所示的算例为4次跳变。
先计算出Sturm Sequence的表达式，然后把左边界(这里是-1)带入各个sequence计算出统计符号变换次数，为4；右边界带入sequence计算出符号变化次数为0，则根的个数为4-0=4个，实际$G(t)$通过因式分解也能看出四个根分别为0,1,3,6。
特殊地，如果sequence值出现0，则直接删除0继续进行判断。

针对具体问题，

1. 根据实际约束（如速度）构建如上图所示的不等式，
2. 把多项式P带入，计算出不等式$\mathcal{G}<0$，
3. 将左右边界带入看是否满足$\mathcal{G}<0$，若满足，则
4. 求取$\mathcal{G}$的Sturm Sequence，利用上述方法判断根的个数，若不为0，则不满足约束；若为0，则满足约束。

[16]中的方法既快速又稳定，可以拓展到很多凸分解的约束checking。

3.3 Unconstrained Case: BIVP(Multi-segment with intermediate values)

这里讨论的是更加General的问题，

$p$的0阶导为本身，一阶导$v$，二阶导$a$，三阶导$jerk$，四阶导$snap$，五阶导$crackle$，六阶导$pop$。

BIVP经常被用于中间状态只有p约束的规划，这样的解具有超出input阶次的更高阶的连续性[12]，如优化jerk的energy，solution使得snap连续；优化snap的energy，solution使得pop连续。

如3.2节所述，构造$MC=b$这样一个方程组，对$M$求逆即可解出最优解的coeffieient。连cost function的形式都无需构建。

由最优性条件可知，最优解一定是一个2s-1的多项式（cost function不一定是多项式），可以分段写出每一段的样条（需要知道每一段的time durations）

通过线性条件（导数约束和连续性约束）构建方程组进行求解。（这块听不太懂，需要看论文具体才行）

实际实现过程中，通常分别以每一个seg的开始为0时刻，避免了系数矩阵中出现过大的值导致数值不稳定的情况。

写出M之后会发现是一个稀疏的带状矩阵，可以使用稀疏求解器，如PLU分解来求逆，可以在线性的时间内求完成$c$的求解。

---

另一种方法是多层次的BIVP，通常是前端的path planning+后端的BIVP
前端用于生成轨迹来获取中间状态，后端使用中间状态构建BIVP进行求解，优化solution。

核心思想：把低维的MP方法用在高维BIVP后端方法的关键点选取上。

前端方法比如：

1. 可以使用RRT*这种方法来global地求出一个solution，然后选取solution中的若干点作为中间状态的p，在使用BIVP，
2. 使用DP算法求出关键点，然后BIVP。

---

“Douglas-Peucker算法”（也称为Ramer-Douglas-Peucker算法, 或简称DP算法）这是一个用于减少点数或简化路径的算法，常用于轨迹简化或地图绘制领域。

Douglas-Peucker算法会递归地选择轨迹上的点，以在简化轨迹和保留轨迹形状之间达到平衡。它通过设定一个容差值，然后找到与直线段起点和终点形成最大距离的点。如果这个距离大于容差值，那么这个点会被保留，轨迹会被分成两部分，算法会递归地应用到分割后的两个轨迹上。如果距离小于容差值，则这个点可以被移除。通过这种方式，算法减少轨迹点数，同时尽可能保持轨迹的总体形状。

---

如何保证BVIP的轨迹是collision-free的？

当RRT* 发生碰撞时，尝试把RRT*中间更多的waypoint加入作为关键点给BIVP(来自一几年的IJRR的一篇论文，Bry IJRR 2015 adopted the unconstrained QP formulation)。

把RRT* 这种只能用于低维的motion planning的方法用在决定后端优化的高维的BIVP的关键点选取上。

RRT*+BIVPs比Kinodynamic RRT*速度更快，质量更高。

这种RRT*+BIVP的方法能够计算地很快，但是存在一些问题：

1. 为了使得最后的solution更优，当障碍物较多时，需要加入更多的waypoint，使得无人机很贴合这些拐角的waypoint才能保证飞行的安全，导致solution不那么顺滑。
2. quality static准静态。当无人机飞的足够慢时，可以在任何位置悬停，可以以无限的精度逼近solution，但是这要求是res to res（起始和末尾v，a，高阶导等都为0）。

这类hierarchical的方法的一个研究方向是如何显式地去优化waypoint。

3.4 Constrained Case

考虑free space $\mathcal{F}$和动力学约束${\mathcal{G}}_D(x,u)<0$约束，时间$T$，轨迹形状等约束的问题。

可以根据上述蓝字部分的思路进行简化：

1. 用样条来进行参数化
2. 固定$T$或者allocate大概的时间占比。
3. 固定一些基本的约束，而不管底层约束(角速度，推力等)
4. 可以提取Configuration space中的一部分，使得我们关心的问题是locally convex，局部地解一些凸优化问题。

3.4.1 Constrained Case: Convex Simplification

如上所示为对costfunction进行局部简化[17]，将cost function分成多段，约束分为各个凸多面体。

1. 相邻两段之间的连续性约束：
   
   如第1段末尾和第二段开始的p是相等的。

2. boundary value condition，每一段的起始和末尾的状态是指定的
   
   如第1段起始和末尾的pva都指定。需要注意，这里的中间状态waypoint我们是不知道的，需要进行优化。

3. 安全约束（与BVP和BIVP最大的不同）
   
   $$A_i^T{\Phi }_i(t)\le b_i,i=0,\cdots ,N-1$$
   这里${\Phi }_i(t)$表示$t$时刻位置，保证飞行安全，可以在每段内进行采样，check每个采样点是否满足安全约束，只要采样足够密，就能够保证安全。最终，安全约束被转换为QP问题。

---

“QP”（Quadratic Programming，二次规划），这是一类特殊的数学优化问题。在二次规划问题中，目标函数是一个二次函数，同时约束条件可以是等式和/或不等式，这些约束通常是线性的。

。二次规划问题可以表示为以下的标准形式：

目标函数:
$$\min \left(\frac{1}{2}{x}^{T}Qx+c^{T}x\right)$$

约束条件（不等式约束和等式约束）:
$$\begin{array}{r}\{\begin{array}{l}Ax\le b\\ Ex=d\end{array}\end{array}$$

其中，$x$是决策变量向量，$Q$ 是一个对称的二次项系数矩阵，$c$ 是线性项系数向量，$A$是不等式约束的系数矩阵，$b$ 是不等式约束的常数项向量，$E$ 是等式约束的系数矩阵，$d$ 是等式约束的常数项向量。

---

如果给定了控制时间T和被积函数的系数$p_j$，则可以把cost function解析地写出。

等式约束(即3.4.1的前两个约束等式)：如果想让轨迹经过某个waypoint，则可构建如上等式，将未知的系数$p_{j,i}$与系数矩阵分离开，即可写出等式约束：
$$A_j{p}_j=d_j$$

上图在每个seg使用了绝对时刻，如果使用相对时刻，则分别为$0$和$T$。

不等式约束也类似，将待优化的系数$p_{j,i}$提出，至此cost fucntion和所有的等式、不等式约束均表达出来，下面可以调用凸优化求解器进行求解。

针对凸函数Convex function，如果函数$f(x)$满足
$$\begin{array}{r}f(\theta x+(1-\theta )y)\le \theta f(x)+(1-\theta )f(y)\end{array}$$
则$f(x)$是凸函数。（结合上左图理解，两点连线之间的函数值均在连线的下方）
如果严格满足小于$<$，则是strictly convex。(当$f(x)$为线性时，等号成立)

且如果该问题的不等式约束是一个凸集合(convex set，集合中任意两点连线均在集合内部，则该集合为凸集合)，则该问题是一个凸问题。

如果还存在等式约束，则需要是线性等式约束，该问题才能称之为凸问题。

上图左侧是优化问题的standard form，右侧是凸优化问题的standard form。

大多数问题在formulate时是非凸的，reformulateing problem是一门艺术，没有系统的方法…

---

针对凸优化，下面介绍general的形式：

Disciplined convex optimization programs （“规范凸优化范式”）：

①LP线性规划：cost function，不等式约束，等式约束均为线性。

②对于二次规划（QP）问题，要求系数矩阵$P$半正定，才能称为凸问题
分别假设$P=\frac{1}{2}，P=-\frac{1}{2}$，同时设$q=0,r=0$，则目标函数分别为$\frac{1}{2}{x}^2$和$-\frac{1}{2}{x}^2$，前者为凸，而后者非凸，如下所示：

③QCQP(Quadratically Constrained Quadratic Programming，二次约束的二次规划)：目标函数是一个二
次函数，同时不等式约束条件也都是二次的（保证系数为半正定）。

④SOCP(Second-Order Cone Programming，二阶锥规划)：
目标函数通常是线性的，约束条件可以包括线性约束、二次约束
主要是不等式约束有变化，举例：$||x_2||\le x_1$
则图像如下所示，三轴分别为$x_1,x_2(0),x_2(1)$，由于$x_1\ge 0$，所以在$x_1$正半轴，$x_2$模长随着$x_1$增大而增大，呈圆锥形(Second-Order Cone 或ice cream cone)，将图像投影到$x_1{x}_2(1)$平面即为圆锥的投影。

SOCP是最general的凸优化问题形式，①②③均可转换为SOCP，且二阶锥约束使得SOCP能够模拟一些特定的非线性关系，同时保持问题的凸性质，这意味着可以使用高效的算法找到全局最优解。也即LP，QP，QCQP，SOCP均可找到全局最优解。

给出几种凸优化问题的solver：
CVX：http://cvxr.com/cvx/
Mosek：https://www.mosek.com/
OOQP：http://pages.cs.wisc.edu/~swright/ooqp/
GLPK：https://www.gnu.org/software/glpk/

在求解凸优化问题时可能会遇到数值不稳定的问题，可以使用一些Normalization的方法来提高数值稳定性：

1. Time normalization：使用相对时间，并进行归一化，归一化为0~1.0之间
2. Scale：如果实际数值非常大，可以进行scale，求解一个数值较小的问题，然后再re-scale回去（如$10^5$这个偏置去掉，求解0~100的问题）。需要注意，有些问题不是scale invarinat的(不同scale对于最终结果有影响)，有些是（scale对结果无影响）。

实际的一种生成轨迹的步骤：

1. 检测障碍物；
2. 生成长方体（一种特殊地凸多面体）的flight corridor；
3. 扩展flight corridor，使得其重合区域更大，提升解的质量；
4. 在flight corridor中生成feasiable的轨迹。

实现方式：
等式约束，不等式约束（安全边界约束，速度、加速度最值约束）。

3.4.2 Issues of simplification

前面没有关于时间$T$的优化，但是timeallocation对于最终的solutioin影响很大，原因是如果加上时间约束，问题通常是非凸的，无法求解。

使用flight corridor代替waypoint的好处是flight corridor的overlap部分可以提供更大的优化空间，可能找到更优的解。

一种时间分配的方式是按照加速-匀速-减速的方式分配，只能大概计算出相对的时间占比，但是速度大小无法确定，所以绝对时间$T$也无法确定。

3.4.3 Constrained Case: Spatial-Temporal Deformation (Brief)

本节简要讨论更加Gneral的轨迹优化，即考虑$\mathcal{F},\mathcal{G}$和time allocation。

$P,T$，优化时间序列和空间序列：

1. 确定了P和T，通过解BIVP问题可以唯一地确定corridor内的轨迹，轨迹在时间上和空间上的形变打破来满足约束，通过梯度实现，如轨迹超出corridor，向梯度内拉伸使其满足corridor约束
2. 可以显著降低优化问题的维度。加速求解。

上图所示是FAST-LAB提出的一项工作，计算速度很快，且解质量较高。

同时给出一些多旋翼无人机轨迹优化的参考论文[18~23]

通过flat transformation将施加在state上的约束变换为关于平坦输出$z$及其高阶导数的多项式约束，集合时空形变能够优化推力和角速度，使drone穿过很窄的缝。

---

本章讲的较深，需要看论文、继续研究才能理解。

---

4. Reference

[1] Bry et al., Aggressive flight of fixed-wing and quadrotor aircraft in dense indoor environments, IJRR 2015.

[2] Gao et al., Gradient-based online safe trajectory generation for quadrotor flight in complex environments, IROS 2017.

[3] Zhou et al., Ego-planner: An esdf-free gradient-based local planner for quadrotors. RAL 2020.

[4] Wang et al., Generating Large-Scale Trajectories Efficiently using Double Descriptions of Polynomials, ICRA 2021.

[5] Mellinger et al., Minimum Snap Trajectory Generation and Control for Quadrotors, ICRA 2011.

[6] Faessler et al., Differential Flatness of Quadrotor Dynamics Subject to Rotor Drag for Accurate Tracking of High-Speed Trajectories, RAL 2017.

[7] Ferrin et al., Differential Flatness Based Control of a Rotorcraft For Aggressive Maneuvers. IROS 2011.

[8] Mu et al., Trajectory Generation for Underactuated Multirotor Vehicles with Tilted Propellers via a Flatness-based Method, AIM 2019.

[9] Watterson et al., Control of Quadrotors Using the Hopf Fibration on SO(3), ISRR 2017.

[10] Lee et al., Geometric tracking control of a quadrotor UAV on SE(3), CDC 2010.

[11] Vince, Quaternions for Computer Graphics, Springer 2011.

[12] Wang et al., Geometrically Constrained Trajectory Optimization for Multicopters, TRO 2022.

[13] Mueller et al., A Computationally Efficient Motion Primitive for Quadrocopter Trajectory Generation, TRO 2015

[14] Wang et al., Generating Large-Scale Trajectories Efficiently using Double Descriptions of Polynomials, ICRA 2021.

[15] Burri et al., Real-Time Visual-Inertial Mapping, Re-localization and Planning Onboard MAVs in Unknown Environments, IROS 2015.

[16] Wang et al., Alternating Minimization Based Trajectory Generation for Quadrotor Aggressive Flight, RAL 2020.

[17] Liu et al., Planning Dynamically Feasible Trajectories for Quadrotors Using Safe Flight Corridors in 3-D Complex Environments, RAL 2017.

[18] Gao F, Wang L, Zhou B, Zhou X, Pan J and Shen S (2020) Teach-Repeat-Replan: A complete and robust system for aggressive flight in complex environments. IEEE Transactions on Robotics 36(5): 1526–1545.

[19] Deits R and Tedrake R (2015b) Efficient mixed-integer planning for UAVs in cluttered environments. In: IEEE International Conference on Robotics and Automation. Seattle, USA, pp. 42–49.

[20] Mellinger D and Kumar V (2011) Minimum snap trajectory generation and control for quadrotors. In: IEEE International Conference on Robotics and Automation. Shanghai, China, pp. 2520–2525.

[21] Tordesillas J, Lopez BT and How JP (2019) FASTER: Fast and safe trajectory planner for flights in unknown environments. In: IEEE International Conference on Intelligent Robots and Systems. Macau, China, pp. 1934–1940.

[22] Sun W, Tang G and Hauser K (2020) Fast UAV trajectory optimization using bilevel optimization with analytical gradients. In: American Control Conference. pp. 82–87.

[23] Patterson MA and Rao AV (2014) GPOPS-II: A MATLAB software for solving multiple-phase optimal control problems using hpadaptive gaussian quadrature collocation methods and sparse nonlinear programming. ACM Transactions on Mathematical Software 41(1): 1–37.