文章目录
1. Preliminaries2. Multicopter dynamics and differential flatness（多旋翼动力学和微分平坦特性）2.1 Differential Flatness2.2 具体建模2.3 Flatness Transformation的解析推导
3. Trajectory Optimization轨迹优化3.1 Problem formulation3.2 Unconstrained case3.3 Unconstrained Case: BIVP(Multi-segment with intermediate values)3.4 Constrained Case3.4.1 Constrained Case: Convex Simplification3.4.2 Issues of simplification3.4.3 Constrained Case: Spatial-Temporal Deformation (Brief)
4. References
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至少要
满足微分约束（系统方程通常由常微分方程给出），最小化能量泛函(最小化cost，如能量消耗等)
为什么后端是必须的？ 尽管有一些考虑了kinodynamaic的前端生成一些path，给出一些traj的初值，但是后端可以进一步优化，在前端结果不那么好的时候也能收敛到较好的结果。
除了考虑动力学约束，还需要考虑很多其他约束，如时间效率，驱动器限制，任务要求等，综合考虑这么多约束，优化是一种非常好的方法。
2. Multicopter dynamics and differential flatness（多旋翼动力学和微分平坦特性）
2.1 Differential Flatness
简单来说，对于由常微分方程(ODE，Ordinary Differential Equation)描述的系统，在对系统施加了各种约束之后，由系统状态
x
x
x，系统状态的导数
x
˙
\dot{x}
x˙，输入
u
u
u所张开的空间
R
R
R中，满足约束的解
z
z
z 只能在该空间的一个曲面上移动，为了进行求解，引入了微分平坦变换(differential flatness transformation)，能够在一个无约束的空间中对轨迹
z
z
z 进行优化，这个优化结果也是满足
R
R
R中的约束的，微分平坦变换就是上图的
Ψ
x
,
Ψ
u
\Psi_x,\Psi_u
Ψx,Ψu，该变换消除了微分约束，降低优化难度，在优化出
z
z
z之后，可以通过微分平坦变换求出
x
,
u
x,u
x,u，是满足微分约束的。
（该部分比较数学，细节如果需要的话再深入探讨）
桨对齐的四旋翼/多旋翼，线性风阻的多旋翼，桨以一定规律排布的多旋翼都满足differential flatness[5,6,7,8,9].
虽然使用differential flattness能够简化无人机的优化问题，但是有两个问题需要考虑：
无人机实际飞行时需要考虑风阻drag（因为速度快时必须考虑风阻）differential flattness存在奇异点，需要尽可能降低奇异点个数。
上图列出了6种使用differential flattness时会出现奇异点的地方(即出现
1
0
\frac{1}{0}
01的情况)
需要说明，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^3
S3）到三维球面（称为三维球面
S
2
S^2
S2）的映射。
这个映射的特殊之处在于，它将四维球面的每一个点映射到三维空间中的一个点，并且保持了一种特殊的关系：在四维球面上，每一条经线（类似于经度线）都被映射为三维球面上的一个圆，而纬线（类似于纬度线）则被映射为三维球面上的一条线。
Hopf fibration在数学和物理学中都具有重要的应用。它在拓扑学、几何学和量子场论等领域中有着广泛的研究和应用价值。这个概念的提出者是德国数学家Heinz Hopf，他在1931年首次引入了这个概念并进行了深入研究。
2.2 具体建模
参考
state is
x
=
r
,
v
,
R
,
w
x={r,v,R,w}
x=r,v,R,w，分别为位移，速度，旋转，在body系下的角速度，分别为
R
3
×
R
3
×
S
O
(
3
)
×
R
3
\mathbb{R}^3\times\mathbb{R}^3\times SO(3)\times \mathbb{R}^3
R3×R3×SO(3)×R3输入
u
u
u为推力thrust的标量和3轴扭矩向量
τ
\tau
τ动力学方程(ODE)：
x
˙
=
f
x
+
g
(
x
)
u
\dot{x}=f{x}+g(x)u
x˙=fx+g(x)u，后面细讲平坦输出flat output：
z
=
r
,
ψ
z={r,\psi}
z=r,ψ分别为位移
∈
R
3
\in \mathbb{R}^3
∈R3和2D yaw
∈
S
O
(
2
)
\in SO(2)
∈SO(2)
针对无人机动力学方程： 这里的物理量均定义在world系（即惯性系）下。
r
˙
=
v
\dot{r}=v
r˙=v 位置对时间求导是速度。牛顿第二定律
m
a
=
F
ma=F
ma=F 右侧为无人机在飞行过程中收到的和力。
−
m
g
e
3
-mge_3
−mge3：m质量，g重力标量，
e
=
(
0
,
0
,
1
)
T
e=(0,0,1)^T
e=(0,0,1)T表示竖直向上，与重力方向反向。该项表示无人机收到的重力(是向量)。
R
f
e
3
Rfe_3
Rfe3：
R
=
R
w
b
R=R_{wb}
R=Rwb由body到world的旋转，
f
f
f为总推力thrust大小。该项表示系统的推力向量在world系下的向量。
−
R
D
R
T
σ
(
∣
∣
v
∣
∣
)
v
-RDR^T\sigma (||\bm{v}||)v
−RDRTσ(∣∣v∣∣)v：表示无人机在飞行过程中收到的风阻。 该项可如下理解：
−
R
T
v
-R^Tv
−RTv是无人机world系下速度转到body下的速度取反
D
σ
(
∣
∣
v
∣
∣
)
D\sigma (||v||)
Dσ(∣∣v∣∣)中：
∣
∣
v
∣
∣
||v||
∣∣v∣∣表示world系下无人机速度大小，标量
σ
\sigma
σ表示缩放系数，
D
D
D为对角矩阵，是风阻系数（与无人机外形等有关）。故
D
σ
(
∣
∣
v
∣
∣
)
(
−
R
T
v
)
D\sigma (||v||)(-R^Tv)
Dσ(∣∣v∣∣)(−RTv)为body系下收到的风阻力向量，左乘
R
R
R转至world系下为
R
D
σ
(
∣
∣
v
∣
∣
)
(
−
R
T
v
)
RD\sigma (||v||)(-R^Tv)
RDσ(∣∣v∣∣)(−RTv)，整理为
−
R
D
R
T
σ
(
∣
∣
v
∣
∣
)
v
-RDR^T\sigma (||\bm{v}||)v
−RDRTσ(∣∣v∣∣)v整体表示无人机飞行时在world系下所受的风阻力向量。
R
^
=
R
ω
^
\hat{R}=R\hat{\omega}
R^=Rω^：旋转矩阵更新(反对称使用
w
^
\hat{w}
w^表示，搞slam的很熟悉了)
M
ω
˙
=
τ
−
ω
×
M
ω
−
A
(
ω
)
−
B
(
R
T
v
)
M\dot{\omega}=\tau-\omega\times M \omega-A(\omega)-B(R^T v)
Mω˙=τ−ω×Mω−A(ω)−B(RTv)：欧拉公式加上修正项。其中
M
ω
˙
=
τ
−
ω
×
M
ω
M\dot{\omega}=\tau-\omega\times M \omega
Mω˙=τ−ω×Mω为常见的欧拉公式，
M
M
M为惯性张量矩阵，
τ
\tau
τ为三轴力矩。
A
(
ω
)
A(\omega)
A(ω)：飞机转动时产生的阻尼力矩，
B
(
R
T
v
)
B(R^T v)
B(RTv)平动时产生的力矩。
整个系统方程中，
D
,
σ
,
A
,
B
D,\sigma,A,B
D,σ,A,B可以通过参数拟合，系统辨识等方法来分别确定。
2.3 Flatness Transformation的解析推导
如果已经有了flat output，如何求出我们的目标状态呢？这时需要解析地求出微分平坦变换
Ψ
x
,
Ψ
u
\Psi_x,\Psi_u
Ψx,Ψu，需要指出，
Ψ
x
,
Ψ
u
\Psi_x,\Psi_u
Ψx,Ψu的表达式很复杂，表达式也可以通过迭代的方式来给出，也即如果我们知道如何迭代地求出
x
,
u
x,u
x,u，也就相当于知道了
Ψ
x
,
Ψ
u
\Psi_x,\Psi_u
Ψx,Ψu的表达式。
下面分别给出
r
,
v
,
R
,
ω
,
f
,
τ
r,v,R,\omega,f,\tau
r,v,R,ω,f,τ的求解过程。
对系统方程中的牛顿方程两边左乘body系下的x，y轴，即
(
R
e
i
)
T
,
i
∈
(
1
,
2
)
(Re_i)^T,i\in(1,2)
(Rei)T,i∈(1,2)，推导出下图的结论：
v
˙
+
d
h
m
σ
(
∣
∣
v
∣
∣
v
+
g
e
3
)
\begin{align} \dot{v}+\frac{d_h}{m}\sigma (||v||v+ge_3) \end{align}
v˙+mdhσ(∣∣v∣∣v+ge3) body的x，y轴与式(1)垂直
与
x
b
,
y
b
x_b,y_b
xb,yb垂直的只有与
z
b
z_b
zb同向或反向，当无人机悬停时，
v
=
0
v=0
v=0得出式(1)为
g
e
3
ge_3
ge3，为推力，与重力反向，所以式(1)与重力反向，与
z
b
z_b
zb同向。
接着牛顿方程两边同乘
z
b
=
(
R
e
3
)
T
z_b=(Re_3)^T
zb=(Re3)T，可以推导出推力标量
f
f
f的表达式。
旋转
R
R
R可由偏航旋转和倾斜旋转求出，旋转与四元数是1对2的关系，即一个旋转可对应两个不同的四元数，而一个四元数可以确定一个唯一的旋转。
旋转四元数表示为旋转轴
v
\bm v
v和旋转角度
θ
\theta
θ，即
q
=
(
(
c
o
s
θ
)
1
×
1
,
(
v
s
i
n
θ
)
3
×
1
)
=
(
w
,
x
,
y
,
z
)
T
q=((cos\theta)^{1\times1},(\bm{v}sin \theta)^{3\times1})=(w,x,y,z)^T
q=((cosθ)1×1,(vsinθ)3×1)=(w,x,y,z)T
偏航四元数
q
ψ
q_\psi
qψ，仅绕
e
3
=
(
0
,
0
,
1
)
T
e_3=(0,0,1)^T
e3=(0,0,1)T旋转，即
q
ψ
q_\psi
qψ中
x
,
y
=
0
x,y=0
x,y=0倾斜四元数
q
t
i
l
t
=
q
z
q_{tilt}=q_z
qtilt=qz不绕
z
z
z轴进行旋转，可视作如下的旋转：
e
3
e_3
e3与目标
z
b
z_b
zb组成平面
P
P
P，旋转轴为
P
P
P的法向量
v
\bm v
v，垂直向内，由
e
3
e_3
e3方向绕
v
\bm v
v顺时针旋转
θ
\theta
θ得目标
z
b
z_b
zb，无
y
a
w
yaw
yaw方向的旋转，故
q
z
q_{z}
qz中
z
=
0
z=0
z=0
由
q
ψ
q_{\psi}
qψ和
q
z
q_{z}
qz可求出总体旋转四元数
q
=
q
z
⊗
q
ψ
q=q_{z}\otimes q_{\psi}
q=qz⊗qψ，由
q
q
q可唯一地确定旋转矩阵
R
R
R，具体操作可参考[11]。
角速度
ω
\omega
ω可由
R
˙
=
R
ω
^
\dot{R}=R\hat{\omega}
R˙=Rω^求出。 同样也可求出
z
b
˙
\dot{z_b}
zb˙，其中
D
N
(
x
)
\mathcal D\mathcal N(x)
DN(x)表示归一化函数的一阶导数。
得到
ω
\omega
ω的公式后，根据动力学模型的欧拉公式，可以推导出torque的表达式，于是
x
,
u
x, u
x,u中的变量
r
,
v
,
R
,
ω
,
f
,
τ
r,v,R,\omega,f,\tau
r,v,R,ω,f,τ均求出，对应以上PPT中的标红部分，也即flatness transformation
Ψ
x
,
Ψ
u
\Psi_x,\Psi_u
Ψx,Ψu的表达式已经求出。
利用differential flatness可以将带有等式约束的优化问题转换为没有等式约束的优化问题(但仍然存在一些不等式约束)，极大提高优化求解速度。
在控制方面也可以使用微分平坦特性，如反馈和前馈(cal desire，类似串级控制的内环输出)控制中的微分平坦变换。如果task为跟踪一条比较高维的轨迹
x
,
u
x,u
x,u，而我们只能控制一个四维的
z
z
z，可以输入高维的
x
,
u
x,u
x,u，使用flattness计算出低维的flat output，进行控制，再使用
Ψ
x
,
Ψ
u
\Psi_x,\Psi_u
Ψx,Ψu计算出高维的实际输出
x
，
u
x，u
x，u，只要输出可导即可。该过程可以看做一个压缩(flat output)和解压(flattness transformation
Ψ
x
,
Ψ
u
\Psi_x,\Psi_u
Ψx,Ψu)的过程，该过程是解析地进行的，所以速度较快。
实际中，使用样条来获得flat output有以下优点：
容易确定是否可导；容易求出闭式导数，闭式表达式等；可以解耦进行，每个维度上独立进行(如x维使用2阶近似，y使用3维，z使用4维，
ψ
\psi
ψ使用5维等)；计算的数值稳定性已有较多研究。
不清楚的方面：如何使用如spline这样的方法获得flat output?
3. Trajectory Optimization轨迹优化
3.1 Problem formulation
如上即为traj optimization general problem formulation.
我们的优化目标函数是最小化优化项的energy(加权平方积分)，使其更加平滑。函数是基于平坦输出
z
(
t
)
z(t)
z(t)而非直接的
x
,
u
x,u
x,u，具体而言，优化
z
(
s
)
(
t
)
z^{(s)}(t)
z(s)(t)，s取不同值时即优化不同项，
s=1速度，s=2加速度，s=3，jerk；s=4，snap 有不同的效果，后面会讲。 优化
z
(
t
)
z(t)
z(t)的高阶导数的能量，使其更加平滑。 不光优化
z
(
t
)
z(t)
z(t)，还优化时间
T
T
T，如果不优化时间，当
T
→
+
∞
T \to +\infty
T→+∞时，energy可能趋近于0，所以需要加上一个正则项
ρ
(
t
)
\rho (t)
ρ(t)（如
ρ
(
t
)
=
100
T
\rho (t)=100T
ρ(t)=100T），使得整体的优化时间不至于过长。
其中
G
(
z
(
t
)
⋯
z
(
s
)
(
t
)
)
<
0
\mathcal{G(z(t)\cdots z^{(s)}(t))}<0
G(z(t)⋯z(s)(t))<0是约束，不是基于
x
,
u
x,u
x,u，而是基于flat output和flatness transformation
Ψ
x
,
Ψ
u
\Psi_x,\Psi_u
Ψx,Ψu的约束表示。和前面提到的一样，约束中的变量比较灵活，可以添加任何想要加入的变量（如控制时间，无人机倾角限制等）
z
(
t
)
z(t)
z(t)需要在free space
F
\mathcal{F}
F中，是collision-free的：由于一张地图中start->goal的拓扑组合非常多，所以给出
F
\mathcal{F}
F非常困难。
具体问题中会给出起始和末端的状态约束。
优化不同的量会有不同的效果：
如优化jerk的energy可以最小化角速度，便于保持稳定；优化优化snap的energy可以最小化微分推力，节省能量。
3.2 Unconstrained case
学界中已经探讨出了最优性充要条件，如
F
，
G
\mathcal{F，G}
F，G约束不存在，
ρ
(
T
)
\rho({T})
ρ(T)中
T
T
T已给定，约束也不存在。 下面讲解一种Unconstrained case[12]，这也是本节课讲师已经发表的工作：
上图为最优性的条件，约束中没有
F
，
G
\mathcal{F，G}
F，G的约束，
s=3时，最优
z
∗
(
t
)
z^*(t)
z∗(t)为2s-1=5次多项式需满足边界和中间约束(3.3节会讲解没有中间约束，自己生成，而非给定的情况BIVP)d=1时，
z
∗
(
t
)
z^{*}(t)
z∗(t)为2s-d-1=4阶连续，即其0，1，2，3，4阶导数均存在。 将各阶导数组成的等式约束联立求解即可求出最优解。
与[13]一样，此处的思路仍然是先只考虑边界约束，不考虑动力学等其他约束来生成一系列primitives，然后再使用约束对生成的primitives进行check，选出最优的。
根据边界条件联立方程组，求出
x
(
t
)
x(t)
x(t)的系数，也就求出了
x
(
t
)
x(t)
x(t)的表达式。
可以对一段轨迹进行segmentation，分别构造方程
d
=
A
F
(
T
)
∗
c
d=A_F(T)*c
d=AF(T)∗c，对系数矩阵
A
F
(
T
)
A_F(T)
AF(T)求逆可求解每一个seg的BVP，
使得整个轨迹看起来更顺滑，且轨迹会满足各个seg求解时的boundary condition，整段轨迹更倾向于匀速运动对于短的seg，时间T较小，矩阵
A
F
(
T
)
A_F(T)
AF(T)求逆可能存在数值不稳定的问题，在[14]中给出了
A
F
(
T
)
−
1
A_F(T)^{-1}
AF(T)−1的解析形式(也是讲师的工作)，可以offline算出解析解的其余部分，online时只带入
t
t
t的高阶项即可。
如上所示即为
A
F
(
T
)
−
1
A_F(T)^{-1}
AF(T)−1的解析形式[13]，可以使BVP的求解过程更快。 这样的好处是对于primitives-check这样框架的方法来说，可以使得path的光滑逼近问题计算地更快，效率更高(解多个BVP问题更快)。
如上即为multi-segment traj的应用[13]。
如何对生成的primitives进行check？且要求是在连续空间而非离散空间中是feasiable的。 每个维度构成一个safe flight corridor，整个primitives都需要在corridor中才算是feasiable的。
结论：把连续时间约束表示为多元多项式，使得多元多项式满足连续时间约束。 如： 表示速度<1，类似的，我们的目标是把其他约束都表示为类似的多元多项式，对多元多项式进行check。
介绍了三种方法：
离散时间采样：优点：灵活。缺点：低分辨率容易漏检，高分辨率算的太慢。递归check[13]：只针对thrust/a/v的5次多项式的check，构造一个上界，二次的最大值，有解析解，当T->0时是最tight的upper bound，优点：高效。缺点：仅适用于5次多项式check。极值check[15]：对目标函数求导=0，求出极值，对极值进行check。优点：稳定，对所有场景有效（要么有解析解，要么可以用数值解）。缺点：数值解需要进行迭代，慢。
那么有没有一种通用的解决多元多项式时间约束check的方法？[16]给出(高产！)
检查
G
(
p
1
(
i
)
(
t
)
,
p
2
(
i
)
(
t
)
，
p
3
(
i
)
(
t
)
)
<
0
，
∀
t
∈
[
0
,
T
]
\begin{align} \mathcal{G}(p_1^{(i)}(t),p_2^{(i)}(t)，p_3^{(i)}(t))<0，\forall t\in[0,T] \end{align}
G(p1(i)(t),p2(i)(t)，p3(i)(t))<0，∀t∈[0,T]是否成立，转换为在
t
∈
[
0
,
T
]
t\in[0,T]
t∈[0,T]内
G
(
p
1
(
i
)
(
t
)
,
p
2
(
i
)
(
t
)
，
p
3
(
i
)
(
t
)
)
=
0
\mathcal{G}(p_1^{(i)}(t),p_2^{(i)}(t)，p_3^{(i)}(t))=0
G(p1(i)(t),p2(i)(t)，p3(i)(t))=0是否有根，如果有根，则式(2)必定不成立。
感觉这篇学术产出都是把问题转换为一个数学问题，然后看在数学领域有没有现成的比较成熟的求解方法，应用到这里，就完成了一项工作。
关于在给定区间内方程是否有根，在数学上有比较成熟的方法： Sturm Theorem
定义 Sturm Sequence，其中Rem()是求
a
b
\frac{a}{b}
ba的多项式除法的余项，同样也是多项式。
定义
V
(
t
)
V(t)
V(t)为
[
0
,
T
]
[0,T]
[0,T]之间的
g
g
g值的正负跳变次数，计算方法是
V
(
t
)
=
V
(
T
)
=
V
(
0
)
V(t)=V(T)=V(0)
V(t)=V(T)=V(0)，上图所示的算例为4次跳变。 先计算出Sturm Sequence的表达式，然后把左边界(这里是-1)带入各个sequence计算出统计符号变换次数，为4；右边界带入sequence计算出符号变化次数为0，则根的个数为4-0=4个，实际
G
(
t
)
G(t)
G(t)通过因式分解也能看出四个根分别为0,1,3,6。 特殊地，如果sequence值出现0，则直接删除0继续进行判断。
针对具体问题，
根据实际约束（如速度）构建如上图所示的不等式，把多项式P带入，计算出不等式
G
<
0
\mathcal{G}<0
G<0，将左右边界带入看是否满足
G
<
0
\mathcal{G}<0
G<0，若满足，则求取
G
\mathcal{G}
G的Sturm Sequence，利用上述方法判断根的个数，若不为0，则不满足约束；若为0，则满足约束。
[16]中的方法既快速又稳定，可以拓展到很多凸分解的约束checking。
3.3 Unconstrained Case: BIVP(Multi-segment with intermediate values)
这里讨论的是更加General的问题，
p
p
p的0阶导为本身，一阶导
v
v
v，二阶导
a
a
a，三阶导
j
e
r
k
jerk
jerk，四阶导
s
n
a
p
snap
snap，五阶导
c
r
a
c
k
l
e
crackle
crackle，六阶导
p
o
p
pop
pop。
BIVP经常被用于中间状态只有p约束的规划，这样的解具有超出input阶次的更高阶的连续性[12]，如优化jerk的energy，solution使得snap连续；优化snap的energy，solution使得pop连续。
如3.2节所述，构造
M
C
=
b
MC=b
MC=b这样一个方程组，对
M
M
M求逆即可解出最优解的coeffieient。连cost function的形式都无需构建。
由最优性条件可知，最优解一定是一个2s-1的多项式（课程里面说轨迹不一定是多项式，但最优解一定是多项式（最优解应该是指最优的flat output是多项式，而利用flatness trnasformation转换为state，即轨迹，则state不一定为多项式）），可以分段写出每一段的样条（需要知道每一段的time durations）
通过线性条件（导数约束和连续性约束）构建方程组进行求解。（这块听不太懂，需要看论文具体才行）
实际实现过程中，通常分别以每一个seg的开始为0时刻，避免了系数矩阵中出现过大的值导致数值不稳定的情况。
写出M之后会发现是一个稀疏的带状矩阵，可以使用稀疏求解器，如PLU分解来求逆，可以在线性的时间内求完成
c
c
c的求解。
另一种方法是多层次的BIVP，通常是前端的path planning+后端的BIVP 前端用于生成轨迹来获取中间状态，后端使用中间状态构建BIVP进行求解，优化solution。
核心思想：把低维的MP方法用在高维BIVP后端方法的关键点选取上。
前端方法比如：
可以使用RRT*这种方法来global地求出一个solution，然后选取solution中的若干点作为中间状态的p，在使用BIVP，使用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的方法能够计算地很快，但是存在一些问题：
为了使得最后的solution更优，当障碍物较多时，需要加入更多的waypoint，使得无人机很贴合这些拐角的waypoint才能保证飞行的安全，导致solution不那么顺滑。quality static准静态。当无人机飞的足够慢时，可以在任何位置悬停，可以以无限的精度逼近solution，但是这要求是res to res（起始和末尾v，a，高阶导等都为0）。
这类hierarchical的方法的一个研究方向是如何显式地去优化waypoint。
3.4 Constrained Case
考虑free space
F
\mathcal{F}
F和动力学约束
G
D
(
x
,
u
)
<
0
\mathcal{G}_D(x,u)<0
GD(x,u)<0约束，时间
T
T
T，轨迹形状等约束的问题。
可以根据上述蓝字部分的思路进行简化：
用样条来进行参数化固定
T
T
T或者allocate大概的时间占比。固定一些基本的约束，而不管底层约束(角速度，推力等)可以提取Configuration space中的一部分，使得我们关心的问题是locally convex，局部地解一些凸优化问题。
3.4.1 Constrained Case: Convex Simplification
如上所示为对costfunction进行局部简化[17]，将cost function分成多段，约束分为各个凸多面体。
相邻两段之间的连续性约束： 如第1段末尾和第二段开始的p是相等的。
boundary value condition，每一段的起始和末尾的状态是指定的 如第1段起始和末尾的pva都指定。需要注意，这里的中间状态waypoint我们是不知道的，需要进行优化。
安全约束（与BVP和BIVP最大的不同）
A
i
T
Φ
i
(
t
)
≤
b
i
,
i
=
0
,
⋯
,
N
−
1
A_i^T\Phi_i(t)\leq b_i,i=0,\cdots, N-1
AiTΦi(t)≤bi,i=0,⋯,N−1 这里
Φ
i
(
t
)
\Phi_i(t)
Φi(t)表示
t
t
t时刻位置，保证飞行安全，可以在每段内进行采样，check每个采样点是否满足安全约束，只要采样足够密，就能够保证安全。最终，安全约束被转换为QP问题。
“QP”（Quadratic Programming，二次规划），这是一类特殊的数学优化问题。在二次规划问题中，目标函数是一个二次函数，同时约束条件可以是等式和/或不等式，这些约束通常是线性的。
。二次规划问题可以表示为以下的标准形式：
目标函数:
min
(
1
2
x
T
Q
x
+
c
T
x
)
\min \left( \frac{1}{2} x^T Q x + c^T x \right)
min(21xTQx+cTx)
约束条件（不等式约束和等式约束）:
{
A
x
≤
b
E
x
=
d
\begin{align*} \left\{ \begin{array}{l} Ax \leq b \\ Ex = d \end{array} \right. \end{align*}
{Ax≤bEx=d
其中，
x
x
x是决策变量向量，
Q
Q
Q 是一个对称的二次项系数矩阵，
c
c
c 是线性项系数向量，
A
A
A是不等式约束的系数矩阵，
b
b
b 是不等式约束的常数项向量，
E
E
E 是等式约束的系数矩阵，
d
d
d 是等式约束的常数项向量。
如果给定了控制时间T和被积函数的系数
p
j
p_j
pj，则可以把cost function解析地写出。
等式约束(即3.4.1的前两个约束等式)：如果想让轨迹经过某个waypoint，则可构建如上等式，将未知的系数
p
j
,
i
p_{j,i}
pj,i与系数矩阵分离开，即可写出等式约束：
A
j
p
j
=
d
j
A_j p_j =d_j
Ajpj=dj
上图在每个seg使用了绝对时刻，如果使用相对时刻，则分别为
0
0
0和
T
T
T。
不等式约束也类似，将待优化的系数
p
j
,
i
p_{j,i}
pj,i提出，至此cost fucntion和所有的等式、不等式约束均表达出来，下面可以调用凸优化求解器进行求解。
针对凸函数Convex function，如果函数
f
(
x
)
f(x)
f(x)满足
f
(
θ
x
+
(
1
−
θ
)
y
)
≤
θ
f
(
x
)
+
(
1
−
θ
)
f
(
y
)
\begin{align*} f(\theta x+(1-\theta)y) \leq\theta f(x)+(1-\theta)f(y) \end{align*}
f(θx+(1−θ)y)≤θf(x)+(1−θ)f(y) 则
f
(
x
)
f(x)
f(x)是凸函数。（结合上左图理解，两点连线之间的函数值均在连线的下方） 如果严格满足小于
<
<
<，则是strictly convex。(当
f
(
x
)
f(x)
f(x)为线性时，等号成立)
且如果该问题的不等式约束是一个凸集合(convex set，集合中任意两点连线均在集合内部，则该集合为凸集合)，则该问题是一个凸问题。
如果还存在等式约束，则需要是线性等式约束，该问题才能称之为凸问题。
上图左侧是优化问题的standard form，右侧是凸优化问题的standard form。
大多数问题在formulate时是非凸的，reformulateing problem是一门艺术，没有系统的方法…
针对凸优化，下面介绍general的形式：
Disciplined convex optimization programs （“规范凸优化范式”）： ①LP线性规划：cost function，不等式约束，等式约束均为线性。
②对于二次规划（QP）问题，要求系数矩阵
P
P
P半正定，才能称为凸问题 分别假设
P
=
1
2
，
P
=
−
1
2
P=\frac{1}{2}，P=-\frac{1}{2}
P=21，P=−21，同时设
q
=
0
,
r
=
0
q=0,r=0
q=0,r=0，则目标函数分别为
1
2
x
2
\frac{1}{2}x^2
21x2和
−
1
2
x
2
-\frac{1}{2}x^2
−21x2，前者为凸，而后者非凸，如下所示：
③QCQP(Quadratically Constrained Quadratic Programming，二次约束的二次规划)：目标函数是一个二 次函数，同时不等式约束条件也都是二次的（保证系数为半正定）。
④SOCP(Second-Order Cone Programming，二阶锥规划)： 目标函数通常是线性的，约束条件可以包括线性约束、二次约束 主要是不等式约束有变化，举例：
∣
∣
x
2
∣
∣
≤
x
1
||x_2||\leq x_1
∣∣x2∣∣≤x1 则图像如下所示，三轴分别为
x
1
,
x
2
(
0
)
,
x
2
(
1
)
x_1,x_2(0),x_2(1)
x1,x2(0),x2(1)，由于
x
1
≥
0
x_1\geq 0
x1≥0，所以在
x
1
x_1
x1正半轴，
x
2
x_2
x2模长随着
x
1
x_1
x1增大而增大，呈圆锥形(Second-Order Cone 或ice cream cone)，将图像投影到
x
1
x
2
(
1
)
x_1x_2(1)
x1x2(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的方法来提高数值稳定性：
Time normalization：使用相对时间，并进行归一化，归一化为0~1.0之间Scale：如果实际数值非常大，可以进行scale，求解一个数值较小的问题，然后再re-scale回去（如
1
0
5
10^5
105这个偏置去掉，求解0~100的问题）。需要注意，有些问题不是scale invarinat的(不同scale对于最终结果有影响)，有些是（scale对结果无影响）。
实际的一种生成轨迹的步骤：
检测障碍物；生成长方体（一种特殊地凸多面体）的flight corridor；扩展flight corridor，使得其重合区域更大，提升解的质量；在flight corridor中生成feasiable的轨迹。
实现方式： 等式约束，不等式约束（安全边界约束，速度、加速度最值约束）。
3.4.2 Issues of simplification
前面没有关于时间
T
T
T的优化，但是timeallocation对于最终的solutioin影响很大，原因是如果加上时间约束，问题通常是非凸的，无法求解。
使用flight corridor代替waypoint的好处是flight corridor的overlap部分可以提供更大的优化空间，可能找到更优的解。
一种时间分配的方式是按照加速-匀速-减速的方式分配，只能大概计算出相对的时间占比，但是速度大小无法确定，所以绝对时间
T
T
T也无法确定。
3.4.3 Constrained Case: Spatial-Temporal Deformation (Brief)
本节简要讨论更加Gneral的轨迹优化，即考虑
F
,
G
\mathcal{F,G}
F,G和time allocation。
P
,
T
P,T
P,T，优化时间序列和空间序列：
确定了P和T，通过解BIVP问题可以唯一地确定corridor内的轨迹，轨迹在时间上和空间上的形变打破来满足约束，通过梯度实现，如轨迹超出corridor，向梯度内拉伸使其满足corridor约束可以显著降低优化问题的维度。加速求解。
上图所示是FAST-LAB提出的一项工作，计算速度很快，且解质量较高。
同时给出一些多旋翼无人机轨迹优化的参考论文[18~23]
通过flat transformation将施加在state上的约束变换为关于平坦输出
z
z
z及其高阶导数的多项式约束，集合时空形变能够优化推力和角速度，使drone穿过很窄的缝。
本章讲的较深，需要看论文、继续研究才能理解。
4. References
[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.