【移动机器人运动规划】5 基于优化的轨迹规划 Part2

5 基于优化的轨迹规划

文章目录

[5 基于优化的轨迹规划](#5 基于优化的轨迹规划)
- [5.5 约束情形 Constrained case](#5.5 约束情形 Constrained case)
- - [5.5.1 凸优化简化 Convex Simplification](#5.5.1 凸优化简化 Convex Simplification)
  - [5.5.2 空间-时间变形轨迹优化方法 Spatial-Temporal Deformation](#5.5.2 空间-时间变形轨迹优化方法 Spatial-Temporal Deformation)

5.5 约束情形 Constrained case

受约束的轨迹优化：在满足各种物理、安全和动力学限制的前提下，寻找一条"最优"的运动路径。
优化数学形式
min ⁡ z ( t ) , T ∫ 0 T v ( t ) ⊤ W v ( t ) d t + ρ ( T ) , s.t. z ( s ) ( t ) = v ( t ) , ∀ t ∈ $0 , T$ , G ( z ( t ) , z ˙ ( t ) , ... , z ( s ) ( t ) ) ≤ 0 , ∀ t ∈ $0 , T$ , z ( t ) ∈ F , ∀ t ∈ $0 , T$ , z $s - 1$ ( 0 ) = z ˉ o , z $s - 1$ ( T ) = z ˉ f , where z $s - 1$ : = $z ⊤ z ˙ ⊤ z ¨ ⊤ \dots z ( s - 1 ) ⊤$ ⊤ . \begin{aligned} & \min_{z(t),\, T} \quad \int_0^T v(t)^\top \mathbf{W} v(t)\, dt + \rho(T), \\ \text{s.t.}& \\ & \quad z^{(s)}(t) = v(t), \quad \forall t \in $0, T$ , \\ & \quad \mathcal{G}\big(z(t), \dot{z}(t), \dots, z^{(s)}(t)\big) \leq \mathbf{0}, \quad \forall t \in $0, T$ , \\ & \quad z(t) \in \mathcal{F}, \quad \forall t \in $0, T$ , \\ & \quad z^{ $s-1$ }(0) = \bar{z}_o, \quad z^{ $s-1$ }(T) = \bar{z}_f, \\ \text{where} &\quad z^{ $s-1$ } := \begin{bmatrix} z^\top & \dot{z}^\top & \ddot{z}^\top & \cdots & z^{(s-1)\top} \end{bmatrix}^\top. \end{aligned} s.t.wherez(t),Tmin∫0Tv(t)⊤Wv(t)dt+ρ(T),z(s)(t)=v(t),∀t∈ $0,T$ ,G(z(t),z˙(t),...,z(s)(t))≤0,∀t∈ $0,T$ ,z(t)∈F,∀t∈ $0,T$ ,z $s-1$ (0)=zˉo,z $s-1$ (T)=zˉf,z $s-1$ := $z⊤z˙⊤z¨⊤\dotsz(s-1)⊤$ ⊤.
四大难点
- 最优参数化形式未知
- 时-空剖面涉及很多自由度
- 动态可行性可能非凸
- 安全区域带来高复杂性

简化策略

简化方式	具体做法	优点	缺点
固定参数化	使用多项式样条（如5次、7次）	数学清晰，易求解	无法自适应最优时间
固定时间分配	假设每段飞行时间已知	降低变量数量	不能优化时间效率
简化动态可行性	用范数约束代替精确模型，如 $	a(t)	\leq a_\max$
局部凸化安全约束	将障碍物视为局部凸区域如：使用安全距离函数	可在线优化	仅适用于简单环境

5.5.1 凸优化简化 Convex Simplification

凸优化建模 Convex formulation
- 受线性约束的二次目标
  - 轨迹被划分为多个多项式段 （如 C 0 , C 1 , C 2 C_0,C_1,C_2 C0,C1,C2 ）
  - 每个段由一个多项式函数 Φ i ( t ) \Phi_i(t) Φi(t) 描述
  - 安全区域用"飞行走廊"（safe flight corridors）表示，确保轨迹始终在安全范围内
- 数学模型
  arg ⁡ min ⁡ Φ J q = ∑ i = 0 n − 1 ∫ 0 Δ t i ∥ Φ i ( q ) ( t ) ∥ 2 d t s.t. Φ i ( k ) ( Δ t i ) = Φ i + 1 ( k ) ( 0 ) , k = 0 , ... , q , i = 1 , ... , N − 2 Φ 0 ( k ) ( 0 ) = u s ( k ) , Φ N − 1 ( k ) ( Δ t N − 1 ) = u y ( k ) , k = 0 , ... , q A i T Φ i ( t ) ≤ b i , ⏟ 安全约束 i = 0 , ... , N − 1 \begin{aligned} \arg\min_{\Phi} &\quad J^q = \sum_{i=0}^{n-1} \int_0^{\Delta t_i} \|\Phi_i^{(q)}(t)\|^2 dt \\ \text{s.t.} &\quad \Phi_i^{(k)}(\Delta t_i) = \Phi_{i+1}^{(k)}(0), \quad k = 0,\dots,q, \quad i = 1,\dots,N-2 \\ & \quad \Phi_0^{(k)}(0) = u_s^{(k)}, \quad \Phi_{N-1}^{(k)}(\Delta t_{N-1}) = u_y^{(k)}, \quad k = 0,\dots,q \\ & \quad \underbrace{\mathbf{A}_i^T \Phi_i(t) \leq \mathbf{b}i,}{安全约束} \quad i = 0,\dots,N-1 \end{aligned} argΦmins.t.Jq=i=0∑n−1∫0Δti∥Φi(q)(t)∥2dtΦi(k)(Δti)=Φi+1(k)(0),k=0,...,q,i=1,...,N−2Φ0(k)(0)=us(k),ΦN−1(k)(ΔtN−1)=uy(k),k=0,...,q安全约束 AiTΦi(t)≤bi,i=0,...,N−1
- 目标：最小化第 q q q 阶导数的平方积分，如 q = 3 q=3 q=3 → 最小化 jerk
- 约束
  - 边界条件：起始和终止状态
  - 连续性约束：相邻段之间各阶导数匹配
  - 线性不等式约束：保证轨迹在安全区域内（如障碍物避免）
- 这是一个 二次目标 + 线性约束 的优化问题 → 可转化为 QP（Quadratic Programming）！
代价函数：单段多项式的代价函数推导
- 假设多项式为 f ( t ) = ∑ i p i t i f(t)=\sum_ip_it^i f(t)=∑ipiti，想最小化 4 阶导数的平方积分，即 snap
- 四阶导数
  f ( 4 ) ( t ) = ∑ i ≥ 4 i ( i − 1 ) ( i − 2 ) ( i − 3 ) t i − 4 p i f^{(4)}(t) = \sum_{i\geq4} i(i-1)(i-2)(i-3)t^{i-4}p_i f(4)(t)=i≥4∑i(i−1)(i−2)(i−3)ti−4pi
- 平方后积分
  ( f ( 4 ) ( t ) ) 2 = ∑ i , j ≥ 4 i ( i − 1 ) ( i − 2 ) ( i − 3 ) j ( j − 1 ) ( j − 2 ) ( j − 3 ) t i + j − 8 p i p j J ( T ) = ∫ T j − 1 T j ( f ( 4 ) ( t ) ) 2 d t = ∑ i , j ≥ 4 i ( i − 1 ) ( i − 2 ) ( i − 3 ) j ( j − 1 ) ( j − 2 ) ( j − 3 ) i + j − 7 ( T j i + j − 7 − T j − 1 i + j − 7 ) p i p j \begin{aligned} (f^{(4)}(t))^2 &= \sum_{i,j\geq4} i(i-1)(i-2)(i-3)j(j-1)(j-2)(j-3)t^{i+j-8}p_i p_j \\ J(T) &= \int_{T_{j-1}}^{T_j} (f^{(4)}(t))^2 dt \\ &= \sum_{i,j\geq4} \frac{i(i-1)(i-2)(i-3)j(j-1)(j-2)(j-3)}{i+j-7} (T_j^{i+j-7} - T_{j-1}^{i+j-7}) p_i p_j \\ \end{aligned} (f(4)(t))2J(T)=i,j≥4∑i(i−1)(i−2)(i−3)j(j−1)(j−2)(j−3)ti+j−8pipj=∫Tj−1Tj(f(4)(t))2dt=i,j≥4∑i+j−7i(i−1)(i−2)(i−3)j(j−1)(j−2)(j−3)(Tji+j−7−Tj−1i+j−7)pipj
- 最终的代价函数
  ⇒ J j ( T ) = p j ⊤ Q j p j \Rightarrow J_j(T) = \mathbf{p}_j^\top \mathbf{Q}_j \mathbf{p}_j ⇒Jj(T)=pj⊤Qjpj
  
  每段的代价是系数向量 p j \mathbf{p}_j pj 的二次型
等式约束
- 单段边界条件：如何将状态约束（如位置、速度、加速度）转化为线性方程
  - 一般形式
  f ( k ) ( T j ) = x j ( k ) ⇒ ∑ i ≥ k i ! ( i − k ) ! T j i − k p j , i = x T , j ( k ) f^{(k)}(T_j) = x^{(k)}{j} \\ \Rightarrow\quad \sum{i\geq k}\frac{i!}{(i-k)!}T_j^{i-k}p_{j,i}=x_{T,j}^{(k)} f(k)(Tj)=xj(k)⇒i≥k∑(i−k)!i!Tji−kpj,i=xT,j(k)
  - 矩阵形式
  $\dots i ! ( i - k ) ! T j ( i - k ) \dots ⋮ ⋮ ⋮ \dots l ! ( l - k ) ! T j l - k \dots$ $p j , i ⋮ p j , l$ = $x T j ( k ) ⋮ x T j ( k )$ \begin{bmatrix} \cdots & \frac{i!}{(i-k)!}T_j^{(i-k)} & \cdots \\ \vdots & \vdots & \vdots \\ \cdots & \frac{l!}{(l-k)!}T_j^{l-k} & \cdots \end{bmatrix}\begin{bmatrix} p_{j,i}\\ \vdots \\ p_{j,l} \end{bmatrix}=\begin{bmatrix} x_{T_j}^{(k)} \\ \vdots \\ x_{T_j}^{(k)} \end{bmatrix} ⋯⋮⋯(i−k)!i!Tj(i−k)⋮(l−k)!l!Tjl−k⋯⋮⋯ pj,i⋮pj,l = xTj(k)⋮xTj(k)
  ⇒ A j p j = d j \Rightarrow \mathbf{A}_j \mathbf{p}_j = \mathbf{d}_j ⇒Ajpj=dj
- 段间连续性：如何保证相邻段之间的光滑连接
  - 条件
    f j ( k ) ( T j ) = f j + 1 ( k ) ( T j ) f_j^{(k)}(T_j)=f_{j+1}^{(k)}(T_j) fj(k)(Tj)=fj+1(k)(Tj)
  - 推导
    ∑ i ≥ k i ! ( i − k ) ! T j i − k p j , i − ∑ l ≥ k l ! ( l − k ) ! T j l − k p j + 1 , l = 0 \sum_{i\geq k}\frac{i!}{(i-k)!}T_j^{i-k}p_{j,i}-\sum_{l\geq k}\frac{l!}{(l-k)!}T_j^{l-k}p_{j+1,l}=0 i≥k∑(i−k)!i!Tji−kpj,i−l≥k∑(l−k)!l!Tjl−kpj+1,l=0
  - 矩阵形式
    ⇒ $A j - A j + 1$ $p j p j + 1$ = 0 \Rightarrow \left $\\mathbf{A}_j - \\mathbf{A}_{j+1} \\right$ \begin{bmatrix} \mathbf{p}j \\ \mathbf{p}{j+1} \end{bmatrix} = \mathbf{0} ⇒ $Aj-Aj+1$ $pjpj+1$ =0
凸函数与凸集
- 凸函数 Convex Fuction 的定义
  
  函数 f : R n ↦ R f:\mathbb{R}^n\mapsto\mathbb{R} f:Rn↦R 是凸函数，当且仅当
  - 定义域 d o m f \mathrm{dom}~f dom f 是凸集
  - 对任意 x , y ∈ d o m f x,y\in\mathrm{dom}~f x,y∈dom f， 0 ≤ θ ≤ 1 0\leq\theta\leq1 0≤θ≤1，有
    f ( θ x + ( 1 − θ ) y ) ≤ θ f ( x ) + ( 1 − θ ) f ( y ) f(\theta x+(1-\theta)y)\leq\theta f(x)+(1-\theta)f(y) f(θx+(1−θ)y)≤θf(x)+(1−θ)f(y)
    
    函数图像上任意两点之间的弦在图像上方
- 严格凸函数 Strictly Convex ：不等式在 0 < θ < 1 0<\theta<1 0<θ<1 严格成立
- 凹函数 Concave ：若 − f -f −f 是凸函数，则 f f f 是凹函数
- 凸集 Convex Set
  
  集合 C ⊆ R n C\subseteq\mathbb{R}^n C⊆Rn 是凸集，若对任意 x , y ∈ C x,y\in C x,y∈C， 0 ≤ θ ≤ 1 0\leq\theta\leq1 0≤θ≤1，有
  θ x + ( 1 − θ ) y ∈ C \theta x +(1-\theta)y\in C θx+(1−θ)y∈C
  
  即，集合内任意两点连线仍在集合内。
凸优化问题 Convex optimization
- 标准优化问题形式
  min ⁡ x f 0 ( x ) s.t. f i ( x ) ≤ 0 , i = 1 , ... , m h i ( x ) = 0 , i = 1 , ... , p \begin{aligned} \min_x &\quad f_0(x) \\ \text{s.t.} &\quad f_i(x) \leq 0, \quad i = 1,\dots,m \\ & \quad h_i(x) = 0, \quad i = 1,\dots,p \end{aligned} xmins.t.f0(x)fi(x)≤0,i=1,...,mhi(x)=0,i=1,...,p
  
  其中
  - x ∈ R n x\in\mathbb{R}^n x∈Rn：优化变量
  - f 0 f_0 f0：目标函数
  - f i f_i fi：不等式约束函数
  - h i h_i hi：等式约束函数
- 凸优化问题标准形式
  min ⁡ x f 0 ( x ) s.t. f i ( x ) ≤ 0 , i = 1 , ... , m A x = b \begin{aligned} \min_x &\quad f_0(x) \\ \text{s.t.} &\quad f_i(x) \leq 0, \quad i = 1,\dots,m \\ & \quad Ax = b \end{aligned} xmins.t.f0(x)fi(x)≤0,i=1,...,mAx=b
  
  其中
  - f 0 , f 1 , ⋯ , f m f_0,f_1,\cdots,f_m f0,f1,⋯,fm 是凸函数
  - 等式约束 A x = b Ax=b Ax=b 是仿射函数（线性+常数）
- 凸优化问题关键性质
  - 局部最优 = 全局最优：任何局部最优解都是全局最优
  - 大多数问题初始非凸 → 需要重新建模为凸问题
  - 将问题转化为凸形式是一门艺术，没有系统方法
规范凸优化规划 Disciplined convex optimization programs
- 线性规划 Linear Programming（QP）
  min ⁡ x c ⊤ x + d s.t. G x ≤ h A x = b \begin{aligned} \min_x &\quad c^\top x + d \\ \text{s.t.} &\quad Gx \leq h \\ & \quad Ax = b \end{aligned} xmins.t.c⊤x+dGx≤hAx=b
  - 目标函数：仿射（线性）
  - 约束函数：仿射（线性）
  - 特点：最简单，最成熟
- 二次规划 Quadratic Programming（QP）
  min ⁡ x 1 2 x ⊤ P x + q ⊤ x + r s.t. G x ≤ h A x = b \begin{aligned} \min_x &\quad \frac{1}{2} x^\top P x + q^\top x + r \\ \text{s.t.} &\quad Gx \leq h \\ & \quad Ax = b \end{aligned} xmins.t.21x⊤Px+q⊤x+rGx≤hAx=b
  - 目标函数：二次 x ⊤ P x x^\top Px x⊤Px
  - 约束函数：仿射（线性）
  - 特点： P ∈ S n ≥ 0 P\in S^n \geq 0 P∈Sn≥0 半正定，常用于最小化加速度
- 二次约束二次规划 Quadratically Constrained QP（QCQP）
  min ⁡ x 1 2 x ⊤ P 0 x + q 0 ⊤ x + r 0 s.t. 1 2 x ⊤ P i x + q i ⊤ x + r i ≤ 0 , i = 1 , ... , m A x = b \begin{aligned} \min_x &\quad \frac{1}{2} x^\top P_0 x + q_0^\top x + r_0 \\ \text{s.t.} &\quad \frac{1}{2} x^\top P_i x + q_i^\top x + r_i \leq 0, \quad i=1,\dots,m \\ & \quad Ax = b \end{aligned} xmins.t.21x⊤P0x+q0⊤x+r021x⊤Pix+qi⊤x+ri≤0,i=1,...,mAx=b
  - 目标函数：二次
  - 约束函数：二次不等式
  - 特点： P i ∈ S n ≥ 0 P_i\in S^n \geq 0 Pi∈Sn≥0 半正定，更灵活，如安全区域约束
- 二阶锥规划 Second-Order Cone Programming（SOCP）
  min ⁡ x f ⊤ x s.t. ∥ A i x + b i ∥ ≤ c i ⊤ x + d i , i = 1 , ... , m F x = g \begin{aligned} \min_x &\quad f^\top x \\ \text{s.t.} &\quad \|A_i x + b_i\| \leq c_i^\top x + d_i, \quad i=1,\dots,m \\ & \quad Fx = g \end{aligned} xmins.t.f⊤x∥Aix+bi∥≤ci⊤x+di,i=1,...,mFx=g
  - 目标函数：线性
  - 约束函数：二阶锥约束 ∣ A x + b ∣ ≤ c ⊤ x + d |Ax+b|\leq c^\top x+d ∣Ax+b∣≤c⊤x+d
  - 特点：更通用，支持 L1、L2 范数，其它三种规划都可以转化为二阶锥规划

求解凸优化问题的推荐求解器

目标是将轨迹生成问题转化为 Disciplined Convex Optimization Programs，然后使用合适的求解器求解。

求解器	特点	链接
CVX	MATLAB wrapper，写法像数学公式，自动调用底层求解器	http://cvxr.com/cvx/
Mosek	非常稳健，支持几乎所有凸问题，学术免费	https://www.mosek.com/
OOQP	快速、鲁棒的 QP 求解器，开源	http://pages.cs.wisc.edu/\~swright/ooqp/
GLPK	快速、鲁棒的 LP 求解器，开源	https://www.gnu.org/software/glpk/

归一化 Normalization：避免数值不稳定问题
- 时间归一化 Time normalization
  - 很短的时间段可能导致数值不稳定，如 T → 0 T\rightarrow0 T→0
  - 解决方案 1：将时间缩放到正常范围，如 $0 , 1$ $0,1$ $0,1$ ；
  - 解决方案 2：使用相对时间线 relative timeline
    f ( t ) = { ∑ i = 0 N p 1 , i ( t − T 0 T 1 − T 0 ) i , T 0 ≤ t ≤ T 1 ∑ i = 0 N p 2 , i ( t − T 1 T 2 − T 1 ) i , T 1 ≤ t ≤ T 2 ⋮ ∑ i = 0 N p M , i ( t − T M − 1 T M − T M − 1 ) i , T M − 1 ≤ t ≤ T M f(t) = \begin{cases} \sum_{i=0}^N p_{1,i} \left( \dfrac{t-T_0}{T_1-T_0} \right)^i, & T_0 \leq t \leq T_1 \\ \sum_{i=0}^N p_{2,i} \left( \dfrac{t-T_1}{T_2-T_1} \right)^i, & T_1 \leq t \leq T_2 \\ \vdots \\ \sum_{i=0}^N p_{M,i} \left( \dfrac{t-T_{M-1}}{T_M-T_{M-1}} \right)^i, & T_{M-1} \leq t \leq T_M \end{cases} f(t)=⎩ ⎨ ⎧∑i=0Np1,i(T1−T0t−T0)i,∑i=0Np2,i(T2−T1t−T1)i,⋮∑i=0NpM,i(TM−TM−1t−TM−1)i,T0≤t≤T1T1≤t≤T2TM−1≤t≤TM
- 空间归一化 Spatial normalization
  - 如果场景很大，如 x = 100.0 m x=100.0m x=100.0m
  - 可先在一个小规模"沙盒"中求解，再缩放回原尺度
- 这两种操作能显著提高数值稳定性

带保证避障的轨迹 Trajectory with Guaranteed Obstacle Avoidance

四步流程

步骤	描述
detect obstacles	使用传感器检测障碍物（红色圆圈）
search a flight corridor	在地图中搜索一条无碰撞的可行路径（蓝色虚线矩形）
inflate flight corridor	将飞行走廊向外扩展（黄色虚线），形成"安全缓冲区"
generate dynamically-feasible trajectories	在膨胀后的走廊内生成动力学可行的平滑轨迹（黑色曲线）

关键思想：先找"安全通道"，再在里面插值轨迹

将复杂避障问题分解为两个阶段：

几何层：用简单方法（如 A*、RRT）找到一条不碰撞的路径 → 构造飞行走廊
动力学层：在走廊内用凸优化生成满足加速度、速度等约束的平滑轨迹

实现了 "安全 + 动力学可行性 + 实时性" 的统一

两类线性约束
约束类型 1：瞬时线性约束 Instant linear constraints

发生在特定时间点
- 起点、终点状态约束： A p = b \mathbf{A} \mathbf{p} = \mathbf{b} Ap=b
- 过度点约束： A p = b , A p ≤ b \mathbf{A} \mathbf{p} = \mathbf{b},~\mathbf{A} \mathbf{p} \leq \mathbf{b} Ap=b, Ap≤b
- 段间连续性约束： A p i = A p i + 1 \mathbf{A} \mathbf{p}i =\mathbf{A} \mathbf{p}{i+1} Api=Api+1
约束类型 2：区间线性约束 Interval linear constraints

对整个时间段成立
- 边界约束： A ( t ) p ≤ b , ∀ t ∈ $t l , t r$ \mathbf{A}(t) \mathbf{p} \leq \mathbf{b}, \quad \forall t \in $t_l, t_r$ A(t)p≤b,∀t∈ $tl,tr$
- 动态约束，如速度、加速度约束： A ( t ) p ≤ b , ∀ t ∈ $t l , t r$ \mathbf{A}(t) \mathbf{p} \leq \mathbf{b}, \quad \forall t \in $t_l, t_r$ A(t)p≤b,∀t∈ $tl,tr$

如何让约束在整个时间区间内都生效？
- 迭代检查极值并添加额外约束
  - 先求解一次轨迹（可能不满足约束）
  - 检查轨迹是否在某处超出安全边界（如 y ( t ) > y b y(t)>y_b y(t)>yb）
  - 若有，则在该点添加一个新约束（如 y ( t ∗ ) ≤ y b y(t^*)\leq y_b y(t∗)≤yb）
  - 重新求解 → 再次检查 → 循环直到无违反
  - 缺点：每次都要重新运行优化器，非常耗时；如果根本无解，需要跑很多次才能确认 → 效率低
- 在离散时间点添加大量约束
  - 将整个时间段划分为许多小的时间步 time ticks
  - 在每个时间点 t k t_k tk，添加约束
    A ( t k ) p ≤ b \mathbf{A}(t_k)\mathbf{p}\leq\mathbf{b} A(tk)p≤b
    
    例如 ∥ r ˙ ( t k ) ∥ ≤ v max ⁡ , ∥ r ¨ ( t k ) ∥ ≤ a max ⁡ \| \dot{r}(t_k) \| \leq v_{\max}, \| \ddot{r}(t_k) \| \leq a_{\max} ∥r˙(tk)∥≤vmax,∥r¨(tk)∥≤amax
  - 缺点：总是生成更保守过约束的轨迹；约束越多求解器负担越重。
简化带来的问题 Issues of simplification
- 分段轨迹依赖于时间分配
  - 当我们将轨迹分为多个段（如从起点到中间点再到终点），每一段的持续时间会影响整体运动特性。
  - 即使空间路径相同，不同的时间分配会导致速度、加速度完全不同。
- 时间分配显著影响最终轨迹
  - 比如：在两个点之间用短时间通过 → 高速、高加速度
  - 用长时间通过 → 低速、平滑
- 如何获得合适的时间分配？
  - 这是优化的核心问题之一
  - 常见做法：将时间作为优化变量（time parameterization optimization）
- 梯形速度曲线
  
  这种模型在单一路径段上看起来很"傻"，但是在基于走廊 的轨迹生成中效果很好。
  
  走廊之间的重叠区域提供了很大的自动调整空间 ，允许系统灵活选择最佳路径。

5.5.2 空间-时间变形轨迹优化方法 Spatial-Temporal Deformation

空间-时间变形模型
- 优化问题的数学形式
  min ⁡ z ( t ) , T ∫ 0 T v ( t ) ⊤ W v ( t ) d t + ρ ( T ) , s.t. v ( t ) = z ( s ) ( t ) , ∀ t ∈ $0 , T$ , G ( z ( t ) , z ˙ ( t ) , ... , z ( s ) ( t ) ) ≤ 0 , ∀ t ∈ $0 , T$ , z ( t ) ∈ F , ∀ t ∈ $0 , T$ , z $s - 1$ ( 0 ) = z ˉ o , z $s - 1$ ( T ) = z ˉ f , z $s - 1$ : = ( z ⊤ , z ˙ ⊤ , z ¨ ⊤ , ... , z ( s − 1 ) ⊤ ) ⊤ . \begin{aligned} & \min_{z(t), T} \int_0^T v(t)^\top \mathbf{W} v(t)\, dt + \rho(T), \\ & \text{s.t.} \quad v(t) = z^{(s)}(t), \quad \forall t \in $0, T$ , \\ & \quad\quad \mathcal{G}(z(t), \dot{z}(t), \dots, z^{(s)}(t)) \leq \mathbf{0}, \quad \forall t \in $0, T$ , \\ & \quad\quad z(t) \in \mathcal{F}, \quad \forall t \in $0, T$ , \\ & \quad\quad z^{ $s-1$ }(0) = \bar{z}_o, \quad z^{ $s-1$ }(T) = \bar{z}_f, \\ & \quad\quad z^{ $s-1$ } := (z^\top, \dot{z}^\top, \ddot{z}^\top, \dots, z^{(s-1)\top})^\top. \end{aligned} z(t),Tmin∫0Tv(t)⊤Wv(t)dt+ρ(T),s.t.v(t)=z(s)(t),∀t∈ $0,T$ ,G(z(t),z˙(t),...,z(s)(t))≤0,∀t∈ $0,T$ ,z(t)∈F,∀t∈ $0,T$ ,z $s-1$ (0)=zˉo,z $s-1$ (T)=zˉf,z $s-1$ :=(z⊤,z˙⊤,z¨⊤,...,z(s−1)⊤)⊤.
- 图示
- 核心思想：将轨迹视为一个可变形的"橡皮筋"，可以在空间和时间上同时调整，以满足约束。
  - 空间变形：改变路径形状（如绕开新出现的障碍物）
  - 时间变形：调整各段速度（如减速通过狭窄区域）
性能评估
- 计算效率高
- 轨迹更平滑、更短、更贴近理想路径
- 系统自动调节速度，在狭窄区域减速，在开阔区域提速