5 基于优化的轨迹规划

5.1 全局方法 vs 局部方法

1. 核心概念：全局方法 vs 局部方法
   

   类别	核心思想	特点
   全局方法（Global Methods）	强调 探索（Exploration）+ 利用（Exploitation），在复杂环境中寻找全局最优解或可行路径	在采样次数趋于无穷时，保证趋于最优解
   局部方法（Local Methods）	强调 确定性优化（Deterministic Optimization），从初始点出发，逐步改进轨迹，追求局部最优	在采样次数趋于无穷时，不保证最优解

2. 典型算法对比

   • 全局方法：Sampling + Graph Search

     算法	特点	应用场景
     PRM* / RRT*	随机采样构建图，渐进式优化路径	复杂环境、高维空间（如机械臂）
     A*	基于启发式的网格搜索	已知静态地图（如自动驾驶）
     JPS	跳点搜索（Jump Point Search），加速 A*	规则网格环境（如游戏AI）

   • 局部方法：

     算法	特点	应用场景
     CHOMP	梯度下降优化轨迹，考虑动力学	平滑轨迹生成（如机械臂抓取）
     DDP / iLQR	动态规划/迭代线性二次调节器	高精度控制（如无人机飞行）
     Flatness	利用系统平坦性参数化轨迹	无人机轨迹生成（如四旋翼）
     MPC / NMPC	模型预测控制，滚动优化	实时在线规划（如自动驾驶）

3. 优缺点对比

   维度	全局方法	局部方法
   优点	• 全局最优性 • 处理复杂环境能力强 • 易移植 • 不需要高阶信息（零阶）	• 局部最优性 • 处理动力学复杂性强 • 高维空间中速度快 • 收敛快
   缺点	• 高维空间中慢 • 难以融入动力学模型 • 收敛率差	• 更复杂 • 需要高阶信息（梯度、Hessian） • 易陷入浅层局部极小

4. 实际部署方式：解耦架构，前段（front end）+ 后段（back end）结合

   • 前端（Global Method）：
     • 如 RRT*, A* → 提供一条粗略但安全的路径
     • 解决“能不能到”的问题
   • 后端（Local Method）：
     • 如 CHOMP, MPC → 将粗略路径优化为平滑、动力学可行的轨迹
     • 解决“怎么走才好”的问题

5.2 轨迹规划

1. 轨迹的核心定义

   • 轨迹（Trajectory）是一个从时间 $t$ 到某个空间的映射函数
     $$\mathbf{x}(t):\mathbb{R}\to \mathcal{X}$$

   • 其中 $\mathcal{X}$ 可以是

     • Configuration Space（构型空间）：如 $(x,y)$
     • State-Input Space（状态-输入空间）：如 $(x,y,v_x,v_y,u_x,u_y)$
     • Flat Space（平坦空间）：某些系统可参数化为多项式形式（如四旋翼）

   • 总结：轨迹是时间参数化 time-parameterized 的路径，具有空间和时间两个维度的信息。

2. 光滑性 smoothness 的含义
   

   • 平滑性不是几何概念：很多高质量的轨迹在几何上并不“顺滑”，但在动力学上是可行且高效的。
   • 一个平滑的轨迹应至少满足以下两点
     • 满足系统的微分约束（differential constraints of dynamics），$\dot{x}=f(x,u)$；
     • 最小化状态 $x$ 和输入 $u$ 的能量泛函（energy functional），$\min {\int }_0^T\mathcal{L}(x,u)\mathrm{d}t$。

3. 为什么需要轨迹优化？

   • 如果我们已经有前端（路径发现），为什么还需要后端优化？
     • 因为前端只解决了“存在性”，没解决“质量性”
   • 如果前端已经是动力学可行的，为什么还需要后端优化？
     • 因为“动力学可行” ≠ “最优”

5.3 微分平坦 Differential Flatness

1. 微分平坦的定义

   • 考虑一个动力学系统（以常微分方程描述）
     $$\begin{gathered} \dot{x}=f(x)+g(x)u \\ f:{\mathbb{R}}^n\mapsto {\mathbb{R}}^n,g:{\mathbb{R}}^n\mapsto {\mathbb{R}}^n,x\in {\mathbb{R}}^n,u\in {\mathbb{R}}^m,\mathrm{rank}(g)=m \end{gathered}$$
     如果存在一个输出变量 $z\in {\mathbb{R}}^m$，称为 flat output，其有限阶导数可以唯一确定系统的全部状态 $x$ 和输入 $u$，则称这个动力系统是微分平坦的 differentially flat

   • 平坦输出 $z$ 可以唯一确定系统的全部状态 $x$ 和输入 $u$
     $$\begin{gathered} x={\Psi }_x(z,\dot{z},\cdots ,z^{(s-1)}) \\ u={\Psi }_u(z,\dot{z},\cdots ,z^{(s)}) \end{gathered}$$

2. 微分平坦的意义

   • 微分平坦消除了微分约束
     

   • 左图需满足动力学约束 ${\mathcal{G}}_D(x,u)⪯0$，右图将所有状态和输入都表示为 $z$ 的导数的函数，动力学约束也变成 $\mathcal{G}(z,\dot{z},\cdots ,z^{(s)})⪯0$

   • 但是左图需要满足的微分方程不再出现在右图。

3. 多旋翼的动力学与微分平坦

   • 多种不同的多旋翼模型
     

     • 具有平行螺旋桨的四旋翼是微分平坦的；
     • 受到线性风阻的四旋翼是微分平坦的；
     • 具有平行螺旋桨的八旋翼是微分平坦的；
     • 满足一定秩条件的带倾斜螺旋桨的多旋翼也是微分平坦的。

   • 不同的定义方式会带来不同的奇异点
     

     • Hopf fibration 方法仅在多旋翼 up-side-down 时具有奇异。
     • 微分平坦多旋翼动力学应当考虑阻力同时减少奇异。

4. 多旋翼模型

   • 多旋翼状态向量
     x = { r , v , R , ω } ∈ R 3 × R 3 × S O ( 3 ) × R 3 x=\{r,v,R,\omega\}\in\mathbb{R}^3\times\mathbb{R}^3\times\mathrm{SO(3)}\times\mathbb{R}^3 x={r,v,R,ω}∈R3×R3×SO(3)×R3
     分别对应位置、速度、姿态旋转矩阵、角速度

   • 控制输入（在输入映射之后）
     u = { f , τ } ∈ R ≥ 0 × R 3 u=\{f,\tau\}\in\mathbb{R}_{\geq0}\times\mathbb{R}^3 u={f,τ}∈R≥0​×R3
     分别对应总推力（沿机体 Z 轴，方向由姿态矩阵 $R$ 决定），力矩

   • 非线性动力学方程
     $$\{\begin{array}{ll}\dot{r}& =v,\\ m\dot{v}& =-mg{e}_3-RD{R}^{\top }\sigma (\Vert v\Vert )v+Rf{e}_3,\\ \dot{R}& =R\hat{\omega }\\ M\dot{\omega }& =\tau -\omega \times M\omega -A(\omega )-B(R^{\top }v)\end{array}$$
     其中：

     • $-mg{e}_3$ 表示重力（向下），$-RD{R}^{\top }\sigma (\Vert v\Vert )v$ 代表空气阻力（线性阻尼项），$Rf{e}_3$ 表示推力（沿机体 Z 轴，投影到惯性系）
     • $\tau$ 外部施加的力矩；$\omega \times M\omega$ 科里奥利项；$A(\omega )$ 用户定义的“力矩诱导角速度”项（可忽略或用于建模）；$B(R^{\top }v)$ 用户定义的“速度诱导角速度”项（例如风扰影响）
     • $D=\mathrm{diag}(d_h,d_h,d_v)$ 阻力系数

   • 平坦输出 Flat Output
     z = { r , ψ } ∈ R × S O ( 2 ) z=\{r,\psi\}\in\mathbb{R}\times\mathrm{SO(2)} z={r,ψ}∈R×SO(2)
     其中：$r$ 是三维位置，$\psi$ 是偏航角

5. 求解微分平坦变换

   • 显然成立
     $$r=r,v=\dot{r}$$

   • 利用牛顿方程投影到机体坐标系

     • 定义机体 $X/Y$ 轴在惯性系中的方向
       $$x_b=R{e}_1,y_b=R{e}_2$$

       $(R{e}_i{)}^{\top }$ 则是将向量从惯性系投影到机体 $i$ 轴的操作

     • 对牛顿第二定律点乘机体 $X/Y$ 轴
       ( R e i ) ⊤ m v ˙ = ( R e i ) ⊤ ( − m g e 3 − R D R ⊤ σ ∥ v ∥ v + R f e 3 ) ,    ∀ i ∈ { 1 , 2 } (Re_i)^\top m\dot{v}=(Re_i)^\top(-mge_3-RDR^\top\sigma\|v\|v+Rfe_3),~~\forall i\in\{1,2\} (Rei​)⊤mv˙=(Rei​)⊤(−mge3​−RDR⊤σ∥v∥v+Rfe3​),  ∀i∈{1,2}

     • 化简结果
       ( R e i ) ⊤ ( v ˙ + d n m σ ( ∥ v ∥ ) v + g e 3 ) = 0 ,    ∀ i ∈ { 1 , 2 } (Re_i)^\top(\dot{v}+\frac{d_n}{m}\sigma(\|v\|)v+ge_3)=0,~~\forall i\in\{1,2\} (Rei​)⊤(v˙+mdn​​σ(∥v∥)v+ge3​)=0,  ∀i∈{1,2}
       说明：机体 X/Y 轴必须垂直于某个特定向量，记这个向量为
       $$a_{eff}=\dot{v}+\frac{d_h}{m}\sigma (\Vert v\Vert )v+g{e}_3$$

   • 计算机体 Z 轴方向和阻力大小

     • 推力和机体 Z 轴方向一致，定义该方向在惯性系下的表示为
       $$z_b=\mathcal{N}(a_{eff})=\mathcal{N}(\dot{v}+\frac{d_h}{m}\sigma (\Vert v\Vert )v+g{e}_3)$$
       其中 $\mathcal{N}:=\frac{x}{\Vert x\Vert }$ 表示向量归一化

     • 将牛顿方程点乘 机体 Z 轴 $R{e}_3$
       $$(R{e}_3{)}^{\top }m\dot{v}=(R{e}_3{)}^{\top }(-mg{e}_3-RD{R}^{\top }\sigma (\Vert v\Vert )v+Rf{e}_3)$$
       化简后得到推力大小的显式表达式
       $$f=z_b^{\top }(m\dot{v}+d_v\sigma (\Vert v\Vert )v+mg{e}_3)$$

   • 为了构造完整的姿态四元数 $q$，将总旋转分解为两部分

     • 偏航旋转 Yaw Rotation：绕惯性系 Z 轴旋转角度 $\psi$，对应四元数
       $$q_{\psi }=(\cos (\psi /2),0,0,\sin (\psi /2){)}^{\top }$$

       这是标准的绕 Z 轴旋转的单位四元数表示

     • 倾斜旋转 Tilt Rotation：将机体 Z 轴从“向上”调整到目标方向 $z_b$，这个旋转不包含绕惯性 Z 轴的分量（即没有额外偏航），使用 Hopf fibration 方法进行分解，对应四元数为
       $$q_z=\frac{1}{\sqrt{2(1+z_b(3))}}(1+z_b(3),-z_b(2),z_b(1),0{)}^{\top }$$

   • 总姿态四元数

     • 复合旋转，先做倾斜旋转再做偏航旋转，总旋转为
       $$q=q_z\otimes q_{\psi }$$

     • 展开后的结果：将两个四元数相乘并展开
       $$q=\frac{1}{\sqrt{2(1+z_b(3))}}\left[\begin{array}{c}(1+z_b(3))\cos (\psi /2)\\ -z_b(2)\cos (\psi /2)+z_b(1)\sin (\psi /2)\\ z_b(1)\cos (\psi /2)+z_b(2)\sin (\psi /2)\\ (1+z_b(3))\sin (\psi /2)\end{array}\right]$$

     • 一旦有了 $q$，就可以通过标准公式计算旋转矩阵
       $$R={\mathcal{R}}_{quat}(q)$$
       这样就得到了完整的姿态 $R$，即 x = { r , v , R , ω } x=\{r,v,R,\omega\} x={r,v,R,ω} 中的 $R$

   • 旋转矩阵与角速度

     • 对于刚体运动，有经典公式
       $$\dot{R}=R\hat{\omega }$$
       这意味着角速度 $\omega$ 可以通过 $R$ 和 $\dot{R}$ 计算出来，即
       $$\omega =(R^{\top }\dot{R}{)}^{\vee }$$

     • 使用四元数表示角速度
       $$\omega =2(q_z\otimes q_{\psi }{)}^{-1}\otimes ({\dot{q}}_z\otimes q_{\psi }+q_z\otimes {\dot{q}}_{\psi })$$
       代入具体的表达式
       $$\omega =\left(\begin{array}{c}\frac{{\dot{z}}_b(1)\sin (\psi )-{\dot{z}}_b(2)\cos (\psi )-{\dot{z}}_b(3)(z_b(1)\sin (\psi )-z_b(2)\cos (\psi ))}{1+z_b(3)}\\ \frac{{\dot{z}}_b(1)\cos (\psi )+{\dot{z}}_b(2)\sin (\psi )-{\dot{z}}_b(3)(z_b(1)\cos (\psi )+z_b(2)\sin (\psi ))}{1+z_b(3)}\\ \frac{z_b(2){\dot{z}}_b(1)-z_b(1){\dot{z}}_b(2)}{1+z_b(3)}+\dot{\psi }\end{array}\right)$$

       这个公式看起来复杂，但它完全是由 $z_b(t)$ 和 $\psi (t)$ 的导数构成的。

   • 对 $z_b$ 求导：为了完成整个表达，需要知道 ${\dot{z}}_b$，即机体 Z 轴方向的变化率
     $${\dot{z}}_b=\frac{d_h}{m}\mathcal{D}N(\dot{v}+\frac{d_h}{m}\sigma (\Vert v\Vert )v+g{e}_3{)}^T\left(\frac{m}{d_h}\ddot{v}+\sigma (\Vert v\Vert )\dot{v}+\dot{\sigma }(\Vert v\Vert )\frac{v^T\dot{v}}{\Vert v\Vert }v\right)$$
     其中
     $$\mathcal{D}N(x):=\frac{1}{\Vert x\Vert }\left(I-\frac{x{x}^T}{x^{T}x}\right).$$
     是单位化操作的雅可比矩阵。

   • 计算力矩

     • 从表示旋转动力学的欧拉方程中，解出
       $$\tau =M\dot{\omega }+\omega \times M\omega +A(\omega )+B(R^{\top }v)$$

     • 只要知道 $\omega (t)$，就能算出 $\tau (t)$，而 $\omega (t)$ 已经完全由 $r(t),\psi (t)$ 的导数表示

6. 总结

   • 核心步骤
     • 设定 flat output： z = { r , ψ } z=\{r,\psi\} z={r,ψ}
     • 得到速度 $v=\dot{r}$，加速度 $a=\ddot{r}$
     • 构造了机体 Z 轴方向：$z_b=\mathcal{N}\left(\ddot{r}+\frac{d_h}{m}\sigma (\Vert \dot{r}\Vert )\dot{r}+g{e}_3\right)$
     • 利用 Hopf fibration 和四元数复合，得到完整的姿态 $R$
     • 计算出角速度 $\omega$（由 $z_b$ 和 $\psi$ 的导数表达）
     • 求解力矩 $\tau$
   • 结果：所有变量都由 flat output 表达
     • $r$：直接是 $z$
     • $v$：$\dot{r}$
     • $R$：由 $z_b$ 和 $\psi$ 构造
     • $\omega$：由 ${\dot{z}}_b,\dot{\psi },z_b,\psi$ 表达
     • $f$：由 $z_b$ 和 $\ddot{r},\dot{r}$ 表达
     • $\tau$：由 $\dot{\omega },\omega ,R,v$ 表达
   • [Qwen] 成立的前提条件：多旋翼系统是微分平坦的，当且仅当
     • 推力方向可控（通过姿态调整）
     • 推力大小可独立调节（$f\ge 0$）
     • 偏航角 $\psi$ 可独立控制（通过力矩 ${\tau }_z$）
     • 系统无不可控的内部动态（如柔性结构，未建模执行器延迟等）
     • 空气动力学模型足够简单（如线性阻尼或可忽略）
     • 最核心的物理条件是：推力必须始终沿机体 Z 轴，且该轴可通过姿态任意指向（除奇异点外）。

7. 基于平坦性的规划-控制闭环架构
   

   • 上层：轨迹规划器，输出期望的 flat output 轨迹
   • 中间层：位置控制器，输出期望的姿态指令 ${\varphi }_c,{\theta }_c,{\psi }_{des}$
   • 下层：姿态控制器，输出控制信号 $u_2,u_3,u_4$，即力矩指令
   • 最终执行器：多旋翼无人机，接受总推力 $u_1=f$ 和三个力矩 $u_2,u_3,u_4$

8. 通过样条曲线参数化 flat output

   • flat output
     z = { r , ψ } ∈ R 3 × S O ( 2 ) z=\{r,\psi\} \in\mathbb{R}^3\times\mathrm{SO(2)} z={r,ψ}∈R3×SO(2)

   • 在 flat-output 空间的一条轨迹
     $$z(t):[0,T]\mapsto {\mathbb{R}}^3\times \mathrm{SO}(2)$$

   • 使用 样条（splines） 来参数化这条轨迹的优点

     • 易于确定光滑性准则
     • 容易且闭式计算导数
     • 三维空间中的解耦轨迹生成
     • 高数值稳定性

5.4 轨迹优化

• 通用最优控制问题 General problem formulation
  $$\begin{array}{rl}& \underset{z(t),T}{\min }{\int }_0^{T}v(t{)}^{\top }\mathbf{W}v(t)dt+\rho (T),\\ \text{s. t.}& z^{(s)}(t)=v(t),\forall t\in [0,T],\\ & \mathcal{G}(z(t),\dot{z}(t),\dots ,z^{(s)}(t))\le 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,\\ \text{where}& z^{[s-1]}:={\left(\begin{array}{ccccc}{z}^{\top }& {\dot{z}}^{\top }& {\ddot{z}}^{\top }& \cdots & z^{(s-1)\top }\end{array}\right)}^{\top }.\end{array}$$

  • $\mathbf{W}$ 一般可以是对角矩阵 $\mathrm{diag}(w_1,w_2,\cdots ,w_s)$
  • 通用但是求解困难

• 在平坦输出的空间中显式最小化其高阶导数

  • 为什么最小化高阶导数？

    最小化目标	物理意义	工程优势
    Min jerk	最小化角速度变化率	视觉跟踪稳定，减少抖动
    Min snap	最小化推力变化率	节省能源，延长续航

  • 各阶导数对应什么物理量

    导数阶数	平移（Translation）	旋转（Rotation）	推力（Thrust）
    1	Velocity（速度）	—	—
    2	Acceleration（加速度）	Rotation（旋转角度）	—
    3	Jerk（加加速度）	Angular Velocity（角速度）	Thrust（推力）
    4	Snap（加加加速度）	Angular Acceleration（角加速度）	Differential Thrust（推力变化率）

5.5 无约束情形 Unconstrained case:

5.5.1 边界值问题 BVP

1. 无约束情形
   

   • 不考虑障碍物、边界、动力学极限等限制，只求解一个最简单的最优控制问题。

   • BIVP 数学形式定义，不考虑 $\mathcal{F},\mathcal{G},\rho (T)$
     $$\begin{array}{rl}& \underset{z(t),T}{\min }{\int }_0^{T}v(t{)}^{\top }\mathbf{W}v(t)\mathrm{d}t,\\ \text{s. t.}& z^{(s)}(t)=v(t),\forall t\in [0,T],\\ & \underset{\text{Boundary value}}{\underbrace{z^{[s-1]}(0)={\bar{z}}_o,z^{[s-1]}(T)={\bar{z}}_f,}}\\ & \underset{\text{Intermediate value}}{\underbrace{z^{[d_i-1]}(t_i)={\bar{z}}_i,1\le i<M,}}\\ & t_{i-1}<t_i,1\le i<M.\end{array}$$
     关键点：$P_0$ 起点；$P_1,P_2,\cdots ,P_N$ 中间路径点 waypoints；$P_f$ 终点。

   • 蓝色虚线圈表示中间值约束（intermediate values），即在某些时刻必须满足特定状态。

   • 物理意义：这是在规划一条从 $P_0$ 到 $P_f$ 的平滑轨迹，途中经过若干关键点，并可能在某些时间点满足额外条件（如速度、加速度要求）。

2. 定理：最优性条件 Optimality Conditions

   • 一条轨迹 $z^{\ast }(t)$ 是最优的，当且仅当以下条件成立

     • 边界值 Boundary value：每一段子区间上的轨迹是一个 $2s-1$ 次多项式 → 即每个时间段内用一个高次多项式拟合（如 $s=4$ 时用 $7$ 次多项式）

     • 所有边界和中间条件都满足 → 包括起点、终点、中间点的状态要求

     • 中间值 Intermediate value：在每个中间点 $t_i$，轨迹具有 $2s-d_i-1$ 阶连续可微性 → 保证光滑连接（例如 $s=4,d_i=3$ 时，需要 $C^3$ 连续）

   • 此外：满足这些条件的最优轨迹是唯一的。

   • 总结：在无约束情况下，最小化某阶导数的积分问题，其最优解是分段 $2s-1$ 次多项式，且在边界和中间点满足给定条件——这就是现代无人机轨迹优化的核心理论依据。

3. 边界值问题 Boundary value problem（BVP）

   • BVP 数学形式：只考虑起点和终点的边界条件，不包含中间点约束
     $$\begin{array}{rl}& \underset{z(t)}{\min }{\int }_0^{T}v(t{)}^{\top }\mathbf{W}v(t)dt,\\ & \text{s.t.}{z}^{(s)}(t)=v(t),\forall t\in [0,T],\\ & \underset{\text{Boundary value}}{\underbrace{z^{[s-1]}(t_0)={\bar{z}}_o,z^{[s-1]}(t_M)={\bar{z}}_f.}}\end{array}$$

   • 说明

     • 最小化控制输入 $v(t)$ 的能量，如 jerk 或 snap
     • $z(t)$ 是平坦输出，如位置和偏航角
     • $v(t)=z^{(s)}(t)$：即 $v$ 是 $z$ 的 $s$ 阶导数

   • 物理意义

     • 求一条从初始状态到目标状态的最优轨迹，使运动尽可能平滑。

   • 用于接球任务的不同运动原语
     

     • 从同一个起点出发，到达不同的终点，形成一个“扇形”空间
     • 当 $s=3$（最小化 jerk）时，最优解是 5 次多项式
     • 当 $s=4$（最小化 snap）时，最优解是 7 次多项式

4. 通过 BVP 生成光滑一维轨迹
   

   • 轨迹参数化：设计一条 $5$ 次多项式
     $$x(t)=c_0+c_{1}t+c_2{t}^2+c_3{t}^3+c_4{t}^4+c_5{t}^5$$

   • 要求满足边界条件

     时间	位置	速度	加速度
     $t=0$	$a$	0	0
     $t=T$	$b$	0	0

   • 将边界条件代入多项式及其导数，得到线性方程组
     $$\left[\begin{array}{c}a\\ 0\\ 0\\ b\\ 0\\ 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^5\\ 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}}_F(T)c\Rightarrow c={\mathbf{A}}_F^{-1}(T)d$$

   • 连续性准则：高阶连续性由参数化方式保证

5. 通过 BVPs 生成多段平滑轨迹

   • 轨迹参数化：使用 5 次多项式
     $$x(t)=c_0+c_{1}t+c_2{t}^2+c_3{t}^3+c_4{t}^4+c_5{t}^5$$

   • 边界条件

     时间	位置	速度	加速度
     $t=0$	$a$	$v_0$	0
     $t=T$	$b$	$v_r$	0

   • 求解线性方程组
     $$\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^5\\ 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}}_F(T)c\Rightarrow c={\mathbf{A}}_F^{-1}(T)d$$
     矩阵 $A_F(T)$ 是固定的，只依赖于时间 $T$，可预先计算或缓存。

   • 折线路径被平滑处理后的示例
     

     • 原始路径：由多个直线段组成（红色虚线）
     • 平滑后路径：蓝色曲线，连接所有关键点
     • 关键特征
       • 在每个拐角处有圆弧状过渡
       • 优先保持恒定速度 $v$
       • 优先保持零加速度
       • 需要对短段进行特殊处理

6. 边界值问题 BVP 的显式解

   • 原始优化问题数学形式
     $$\begin{gathered} \underset{z(t)}{\min }{\int }_0^{T}v(t{)}^{\top }\mathbf{W}v(t)dt, \\ \text{s.t.}{z}^{(s)}(t)=v(t),\forall t\in [0,T], \\ {z}^{[s-1]}(0)={\bar{z}}_o,z^{[s-1]}(T)={\bar{z}}_f. \end{gathered}$$
     这个 BVP 问题具有显式解，解总是有 $2s-1$ 次多项式。

   • 正向映射
     $$d={\mathbf{A}}_F(T)c,\text{where}{\mathbf{A}}_F(t)=\left[\begin{array}{cc}\mathbf{E}& 0\\ \mathbf{F}(t)& \mathbf{G}(t)\end{array}\right]$$
     其中：$d$ 是边界条件向量（包含位置、速度、加速度等）；$c$ 多项式系数向量；$A_F(T)$ 依赖于时间 $T$ 的矩阵
     $$\begin{gathered} {\mathbf{E}}_{ij}=\{\begin{array}{ll}(i-1)!& \text{if }i=j,\\ 0& \text{if }i\ne j,\end{array} \\ {\mathbf{F}}_{ij}(t)=\{\begin{array}{ll}(j-1)!/(j-i)!\cdot t^{j-i}& \text{if }i\le j,\\ 0& \text{if }i>j,\end{array} \\ {\mathbf{G}}_{ij}(t)=\frac{(s+j-1)!}{(s+j-i)!}\cdot t^{s-i-j} \end{gathered}$$
     当 $T$ 很小时，求 ${\mathbf{A}}_F(T)$ 的逆存在不稳定的问题，能否直接表示出它的逆 → 逆向映射。

   • 逆向映射
     $$c={\mathbf{A}}_B(T)d,\text{where}{\mathbf{A}}_B(t)=\left[\begin{array}{cc}\mathbf{U}& 0\\ \mathbf{V}(t)& \mathbf{W}(t)\end{array}\right]$$
     其中：${\mathbf{A}}_B(T)$ 逆矩阵，用于从边界条件直接计算系数，即给定边界条件直接得到系数。
     $$\begin{gathered} {\mathbf{U}}_{ij}=\{\begin{array}{ll}1/(i-1)!& \text{if }i=j,\\ 0& \text{if }i\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)!\cdot (-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)!\cdot (-1{)}^{i-j}\cdot t^{s+i-j}} \end{gathered}$$

7. 三维空间中的平滑多段轨迹

   • 边界条件：起点和终点的位置（以及姿态）
   • 中间条件：航点 waypoints 的位置（以及姿态）
     • 先用路径规划找到“安全路径”上的点，如 A*，RRT*，PRM等
     • 再用轨迹优化将这些点连接成平滑曲线
   • 平滑性准则：一般转化为最小化“输入变化率”

8. 如何检查运动原语的可行性？

   • 核心思想

     • 给定一条轨迹 $p(t)=(p_1(t),p_2(t),p_3(t){)}^{\top },t\in [0,T]$
     • 需要验证它是否满足一系列连续时间约束

   • 四类典型约束
     

     • 最大速度限制：$|v(t)\le v_{\max }$
     • 推力上下限：$f_{\min }\le f(t)\le f_{\max }$
     • 几何障碍物：$|x(t)-o_i|\ge r_i$
     • 安全飞行走廊（多边形/球形）：$x(t)\in \mathcal{C}$

   • 所有这些都可以写成同一形式
     $$G(p_1^{(i)}(t),p_2^{(i)}(t),p_3^{(i)}(t))<0,\forall t\in [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}$$

   • 关键问题

     • 如何知道这个不等式在整个时间段内都成立？
     • 这就是连续时间可行性检查器（Continuous-time feasibility checker）的作用

   • 三种可行性检查方法
     

     方法	优点	缺点
     离散时间采样 Discrete time sampling	灵活	低分辨率下可能漏检（false negative），高分辨率下慢
     递归边界检查器 Recursive Bound Checker	效率高	仅适用于5次多项式，结果依赖分辨率，存在不确定情况
     极值检查器 Extreme Value Checker	稳定性好	数值迭代慢，仅适用于5次多项式

9. 对于多元多项式连续性的验证方法：Sturm 序列法

   • 核心思想

     • 如果 $G(p_1^{(i)}(t),p_2^{(i)}(t),p_3^{(i)}(t))$ 是关于 $t$ 的单变量多项式，则可以用 Sturm 定理判断其是否有根
     • 轨迹可行 ⇔ $G(t)<0$ 在 $[0,T]$ 内无零点 ⇔ $G(t)=0$ 无解

   • Sturm 序列定义
     $$\begin{array}{rl}{g}_0(t)& =\mathcal{G}(t),\\ g_1(t)& =\dot{\mathcal{G}}(t),\\ -g_{k+1}(t)& =\text{Rem}(g_{k-1}(t),g_k(t))\end{array}$$
     其中

     • $\mathrm{Rem}$ 表示欧几里得除法的余数（纯代数运算）
     • $\mathcal{G}(t)$ 是将约束函数 $G(\cdot )$ 沿轨迹 $z(t)$ 代入后得到的关于时间 $t$ 的单变量函数。

   • 示例计算
     

     • 给定
       $$G(t)=-t(t-1)(t-3)(t-6)=18t-27t^2+10t^3-t^4$$

     • 构造 Sturm 序列
       $$\begin{array}{rl}{g}_0(t)& =G(t)\\ g_1(t)& =18-54t+30t^2-4t^3\\ g_2(2)& =-11.25+20.25t-5.25t^2\\ g_3(t)& =13.2245-10.7755t\\ g_4(t)& =-5.69473\end{array}$$

     • 计算符号变化数

       • 在 $t=-1$ 处，序列值为 $(-56.00,106.00,-36.75,24.00,-5.69)$ → 符号变化数 = 4

       • 在 $t=7$ 处，序列值为 $(-168.00,-262.00,-126.75,-62.20,-5.69)$ → 符号变化数 = 0

       • $4-0=4$ 个根在区间 $(-1,7)$

     • 不依靠因式分解，纯解析运算得到根的个数，速度快且稳定

5.5.2 边界-中间值问题 BIVP

1. 多段最小 snap 轨迹
   

   • 经过多个航点，在航点的速度和加速度自动生成优化，保持光滑。

2. 边界-中间值问题（BIVP）

   • BIVP 数学形式
     $$\begin{array}{rl}& \underset{z(t)}{\min }{\int }_{t_0}^{t_M}v(t{)}^{\top }\mathbf{W}v(t)dt,\\ \text{s.t.}& z^{(s)}(t)=v(t),\forall t\in [t_0,t_M],\\ & \underset{\text{Boundary value}}{\underbrace{z^{(s-1)}(t_0)={\bar{z}}_o,z^{(s-1)}(t_M)={\bar{z}}_f,}}\\ & \underset{\text{Intermediate value}}{\underbrace{z^{(d_i-1)}(t_i)={\bar{z}}_i,1\le i\le M,}}\\ & t_{i-1}<t_i,1\le i\le M.\end{array}$$

   • 核心思想：给定一系列航点 waypoints ** 和其对应的导数约束**（如位置、速度、加速度等）；要求构造一条光滑轨迹 $z(t)$，满足这些条件；目标是最小化某种能量函数（如 jerk 或 snap）。

   • 术语：Position → Velocity → Acceleration → Jerk → Snap → Crackle → Pop

   • 纯航点中间条件的 BIVP
     

     • 当 $s=3$ 时：使用 5 次分段多项式，保证连续 snap → 最小 jerk 轨迹
     • 当 $s=4$ 时：使用 7 次分段多项式，保证连续 pop → 最小 snap 轨迹
     • 使用 BIVP 可以直接构造最优解
     • 比隐式或显式优化更高效（如 QP 方法）

3. 通过 BIVP 生成样条轨迹

   • 分段多项式形式
     $$f(t)=\{\begin{array}{ll}{f}_1(t)={\sum }_{i=0}^N{p}_{1,i}{t}^i,& T_0\le t\le T_1\\ f_2(t)={\sum }_{i=0}^N{p}_{2,i}{t}^i,& T_1\le t\le T_2\\ ⋮& \\ f_M(t)={\sum }_{i=0}^N{p}_{M,i}{t}^i,& T_{M-1}\le t\le T_M\end{array}$$

   • 关键假设

     • 每段是多项式
     • 多项式阶数固定为 $2s-1$
     • 每段的时间长度必须已知（时间长度未知时，需额外优化）

   • 线性约束条件
     

     • 导数约束（红色，在端点）
       $$\text{Derivative constraints: }\begin{array}{rl}{f}_j^{(k)}(T_{j-1})& =x_{0,j}^{(k)}\\ f_j^{(k)}(T_j)& =x_{T,j}^{(k)}\end{array}$$

     • 连续性约束（蓝色，在连接点）
       $$\text{Continuity constraints: }{f}_j^{(k)}(T_j)=f_{j+1}^{(k)}(T_j)$$

   • 时间轴表示方法

     • 多个相对时间轴：实现方便，数值稳定，适合编程
       

     • 单一公共时间轴：教学清晰，便于理论分析
       

   • 将所有约束整理后，得到一个线性方程组
     $$\mathbf{Mc}=\mathbf{b}$$
     其中 $\mathbf{M}$ 是一个稀疏的块对角矩阵，$\mathbf{b}$ 是由边界和中间条件构成的向量
     $$\begin{gathered} \mathbf{M}=\left(\begin{array}{ccccc}{\mathbf{F}}_0& 0& 0& \cdots & 0\\ {\mathbf{E}}_1& {\mathbf{F}}_1& 0& \cdots & 0\\ 0& {\mathbf{E}}_2& {\mathbf{F}}_2& \cdots & 0\\ ⋮& ⋮& \ddots & \ddots & ⋮\\ 0& 0& 0& {\mathbf{E}}_{M-1}& {\mathbf{F}}_{M-1}\\ 0& 0& 0& \cdots & {\mathbf{D}}_M^{\top }\end{array}\right) \\ \mathbf{b}={\left(\begin{array}{ccccccc}{D}_0^{\top }& D_1^{\top }& 0_{m\times {\bar{d}}_1}& \cdots & D_{M-1}^{\top }& 0_{m\times {\bar{d}}_{M-1}}& D_M^{\top }\end{array}\right)}^{\top } \end{gathered}$$

   • 关键性质

     • $\mathbf{M}$ 是带状矩阵 banded matrix
     • 只要每段时间大于零，$\mathbf{M}$ 非奇异
     • 求解只需线性时间复杂度

   • 最终轨迹
     

4. 分层运动规划

   • 通过 BIVP 的分层方法
     

     将复杂的轨迹规划问题拆分成两个步骤

     • 路径规划：在低维空间中找到一条无碰撞的路径
     • 轨迹生成：在已知路径上，插值出平滑、动力学可行的轨迹

     图解说明

     • 左图：已知路点（waypoints） → 我们知道如何用BIVP拟合多项式轨迹
     • 右图：如何获得这些无碰撞的路点？ → 这就是路径规划的任务

   • 分层方法的安全性问题
     

     • 问题：路径是无碰撞的，但轨迹可能不是
     • 因为：轨迹生成时会引入曲率、加速度、jerk 等动态因素，即时路径远离障碍物，高速转弯或急加速也可能导致机器人“越界”

   • 迭代 BIVP 改进方案（Iterative BIVPs）
     

     • 解决速度：不直接使用原始航点，而是通过迭代添加中间航点，逐步逼近最优轨迹
     • 左图：初始路径是无碰撞的（紫色线），但轨迹（蓝色）在拐角处接近障碍物
     • 右图：在危险区域插入新的路点（绿色点），重新生成轨迹 → 更贴合路径且避免碰撞

   • RRT* + BIVP 分层方法
     

     流程

     • RRT* 在配置空间中搜索无碰撞路径（灰色树状结构）
     • 提取路径上的关键点（蓝线）
     • 用BIVP生成光滑轨迹（红蓝曲线）

     优势

     • RRT* 找到全局无碰撞路径
     • BIVP 生成动力学可行、平滑的轨迹
     • 结合两者实现“安全 + 平滑 + 高效”