3. 轨迹表示与参数化

在轨迹优化中，连续时间的最优控制问题最终要转化为有限维非线性规划问题。这个转化的关键一步，就是选择一种轨迹表示方法，把无限维的曲线压缩成有限个决策变量。表示方法不仅决定了轨迹的平滑性、可行性与几何直观性，还直接决定了优化问题的变量规模、约束数量、雅可比矩阵稀疏结构，进而影响求解速度与实时性。本节围绕五类主流表示方法展开，并分析参数化对问题规模与求解效率的影响。

3.1 核心原理：从无限维到有限维

轨迹本质上是时间到状态（或位置、姿态）的映射 x(t)。若直接以离散时间点上的状态作为决策变量，则变量数等于采样点数乘以状态维数，规模随精度线性膨胀。参数化方法则用一组基函数 φ_i(t) 与系数 c_i 的线性组合逼近轨迹：

x(t) ≈ Σ c_i · φ_i(t),  i = 1..N

决策变量：系数 c_i（N 个）
轨迹光滑性：由基函数 φ_i 的连续性决定
约束施加：可在采样点、控制点或积分点上评估

因此，参数化的本质是用基函数的光滑性换取变量维度的降低。基函数越光滑、支撑越局部，轨迹越容易满足动力学约束，同时雅可比矩阵越稀疏，求解器越容易利用结构加速。

3.2 五类表示方法的结构与特性

多项式表示是最基础的形式，如五次多项式常用于单段轨迹，可同时满足位置、速度、加速度边界条件。其优点是导数解析、计算极快；缺点是全局支撑，一段多项式无法灵活描述复杂避障路径，高阶多项式还容易出现龙格现象。

贝塞尔曲线由伯恩斯坦基函数定义，具有凸包性质：曲线始终位于控制点凸包内，这为避障约束提供了天然的保守边界。但贝塞尔曲线的控制点对曲线的影响是全局的，局部修改会牵动整条曲线，且高阶时数值条件较差。

B样条是贝塞尔曲线的推广，采用局部支撑基函数，具有凸包性、变差缩减性和局部修改性。控制点只影响相邻若干段曲线，非常适合多段轨迹优化。其导数可由控制点差分直接得到，便于施加速度、加速度约束。

样条插值（如三次样条、C²连续样条）要求曲线严格通过给定路径点，适合已知精确航路点的场景。但插值条件会引入等式约束，增加问题耦合度，且路径点密集时变量数仍然较大。

离散点序列直接以采样点作为决策变量，表达最灵活，可配合差分或积分格式描述动力学。缺点是变量规模大、光滑性依赖约束显式保证，且相邻点耦合强，稀疏结构不如B样条清晰。

3.3 参数化对优化问题规模与求解效率的影响

参数化方式直接改变非线性规划问题的三个关键指标：决策变量数、约束数、雅可比矩阵稀疏度。以一段 5 秒、10 个路径点的二维避障轨迹为例，不同表示方法的规模差异如下表所示。

表示方法	典型决策变量数	光滑性来源	雅可比稀疏度	适用场景
五次多项式	6～12	解析保证	稠密	单段、边界条件明确
贝塞尔曲线	20～40	基函数保证	较稠密	凸包避障、平滑路径
B样条	30～60	基函数保证	带状稀疏	多段、实时MPC
样条插值	40～80	插值条件保证	中等稀疏	精确航路点跟踪
离散点序列	100～500+	差分约束保证	三对角/带状	高保真动力学、离线优化

从表中可见，B样条在变量规模与稀疏结构之间取得了很好的平衡。其带状雅可比允许求解器使用稀疏线性代数，单次迭代的计算量随控制点数近似线性增长，而不是立方增长。这正是B样条在实时轨迹优化中被广泛采用的原因。

3.4 案例：无人机穿越走廊的轨迹参数化

考虑一架四旋翼在狭窄走廊中飞行，需要在 3 秒内从起点到达终点，同时满足速度上限 3 m/s、加速度上限 5 m/s²，并避开两侧墙壁。若采用离散点序列，以 0.05 秒采样，则状态变量约 60 个点 × 6 维 = 360 个，加上动力学等式约束，问题规模迅速膨胀，单次求解可能超过 50 ms，难以满足 100 Hz 闭环。

改用三次B样条，设置 12 个控制点，决策变量为 12 × 3 = 36 个。速度与加速度约束通过控制点差分直接表达，避障约束利用B样条的凸包性质转化为控制点凸包与墙壁的不相交约束。问题规模缩小一个数量级，雅可比呈带状，单次求解可压缩到 5～10 ms，满足实时性要求。

# 伪代码：B样条轨迹参数化核心结构
knots   = uniform_knots(n_ctrl=12, degree=3)
ctrl_pts = decision_variables(12, 3)   # 36 维

def trajectory(t):
    return bspline(t, knots, ctrl_pts, degree=3)

def velocity(t):
    return bspline_derivative(t, knots, ctrl_pts, degree=3, order=1)

def acceleration(t):
    return bspline_derivative(t, knots, ctrl_pts, degree=3, order=2)

# 约束：凸包避障 + 速度/加速度上限
constraints = [
    convex_hull_clearance(ctrl_pts, walls),
    norm(velocity(t)) <= v_max,
    norm(acceleration(t)) <= a_max
]

3.5 常见误区与复盘

综上，轨迹表示与参数化是轨迹优化问题的“第一性选择”。多项式适合简单边界条件，贝塞尔适合凸包避障，B样条适合多段实时优化，样条插值适合精确过点，离散点序列适合高保真离线场景。理解它们在变量规模、稀疏结构与光滑性上的差异，才能在实时性与最优性之间做出正确权衡。