本篇是新开设的专栏《从零做一个时空联合规划器》的开篇。本文从最基本的概念出发，引出轨迹优化问题和DDP/iLQR，本文主要内容为：轨迹优化问题表述+10个相关问答。通过本文，可以对整个优化问题有一个宏观的了解。

轨迹优化是一种用于控制复杂动力系统的强大框架。虽然已经有许多算法被提出用来解决这些困难问题，但近年来，基于 差分动态规划（DDP, Differential Dynamic Programming） 的方法因其 实现简单 和 计算效率高 而变得非常流行。

然而，最初的 DDP 算法 无法处理额外的路径约束。如果将 DDP 融入增广拉格朗日框架（Augmented Lagrangian Framework, ALM），就可以得到一个既强大又高效的框架，用来解决带约束的轨迹优化问题。

1.轨迹优化的表述和ilqr的引出

1.1 系统动力学

轨迹优化（Trajectory Optimization）是控制复杂机器人系统的强大方法。它的价值主要在于 通用性 —— 可以应用于各种动力学系统，前提是系统动力学满足马尔可夫性质，即： $\dot{x} = f(x, u)$ 其中：

• $\dot{x} \in \mathbb{R}^n$表示状态的时间导数，
• $x \in \mathbb{R}^n$ 是状态向量，
• $u \in \mathbb{R}^n$ 是控制输入。 与几乎所有最优控制方法一样，轨迹优化通常假设 全状态反馈（即所有状态都已知），并假设 初始条件已知。

1.2 轨迹优化一般形式

轨迹优化的一般形式是： $\min_{x(t), u(t)} \; \ell(x(t_f)) + \int_0^{t_f} \ell(x(t), u(t)) \, dt$ 约束条件为：

• 动力学约束： $\dot{x} = f(x, u)$
• 初始条件： $x(0) = x_0$
• 不等式约束： $g(x, u) \leq 0$
• 等式约束： $h(x, u) = 0$ 轨迹优化与普通非线性优化的最大不同在于，它必须满足 微分约束（动力学约束），这也是其核心难点。

1.3 问题的离散化

为了便于数值计算，通常将时间区间 $T$ 分成 $N-1$ 个等长区间（每段长度为 $\Delta t$），得到 $N$ 个离散时刻（knot points）。此时动力学方程变为： $x_{k+1} = f(x_k, u_k, \Delta t)$ 离散化后的轨迹优化问题是： $\min_{x_{0:N}, u_{0:N-1}} \; \ell_N(x_N) + \sum_{k=0}^{N-1} \ell_k(x_k, u_k, \Delta t)$ 约束条件为：

• 动力学： $x_{k+1} = f(x_k, u_k, \Delta t), \quad k=0,\dots,N-1$
• 不等式约束： $g_k(x_k, u_k) \leq 0, \quad \forall k$
• 等式约束： $h_k(x_k, u_k) = 0, \quad \forall k$

1.4 两类主要求解方法

解决此类问题的方法主要分为两类：

(1) 直接法 (Direct methods)

• 例如 DIRCOL（直接配点法）
• 将状态和控制一并作为优化变量，转化为一个大规模非线性规划 (NLP)，交由通用求解器（如 IPOPT、SNOPT）。
• 优点：鲁棒性强、能处理各种约束。
• 缺点：计算慢、依赖大规模优化库。

(2) 间接法 (Indirect methods)

• 例如 DDP / iLQR
• 只优化控制变量，状态通过动力学仿真得到。
• 优点：速度快、内存占用小，适合嵌入式实现。
• 缺点：对初值敏感，数值鲁棒性弱，难以直接处理复杂约束。

附录：问题列表

Q1.DDP和ILQR是同一个东西吗

(1) DDP (差分动态规划)

• 提出时间：1960年代（Mayne）。
• 核心思想：在非线性系统上，用 二阶泰勒展开对 cost-to-go 进行近似。
• 特点：
  • 使用 二阶导数（Hessian + 三阶张量），考虑了动力学和代价的二阶项。
  • 更精确，通常需要更少的迭代次数。
  • 计算量很大（涉及 rank-3 张量），实现复杂。

(2) iLQR (迭代 LQR)

• 提出时间：2004年（Li & Todorov, Iterative LQR）。
• 本质：DDP 的简化版本。
• 特点：
  • 使用 一阶线性化 的动力学和约束。
  • 对 cost-to-go 用 Gauss-Newton 近似（忽略二阶项里张量部分）。
  • 牺牲了一些精度 → 需要更多迭代。
  • 每次迭代计算更快、更轻量 → 总体收敛速度通常更优。
  • 更适合工程应用（机器人、自动驾驶）。

Q2.为什么选择 DDP？

• 直接法（比如 DIRCOL）→ 稳健但很慢。
• DDP/iLQR 属于间接法 → 快速、内存占用小，适合嵌入式。
• DDP 最近流行的原因：实现简单 + 速度快。

Q3.DDP 的缺陷

• 原版 DDP 只能处理动力学约束（即 $\dot{x}=f(x,u)$）。
• 不能处理路径约束（比如输入限幅、障碍物、状态范围）。

Q4.AL-iLQR 的贡献

• 把 DDP 放入 增广拉格朗日框架：
  • 引入 拉格朗日乘子 λ → 处理等式/不等式约束。
  • 引入 罚因子 µ → 加速收敛。
• 最终形成一个 快速且可处理约束 的算法。

Q5.什么是状态量和控制量

5.1 状态向量（State Vector, $x$）

• 定义：描述系统在某一时刻的“完整状态”的一组变量。
• 它包含了系统运行所需的最小信息量，未来的演化完全由当前状态 + 控制输入决定。
• 数学上就是 $x(t)$ 或 $x_k$。 在自动驾驶的自行车模型中：$x = [X, Y, \theta, v]^T$
• $X,Y$ → 车辆位置
• $\theta$ → 航向角
• $v$ → 速度 状态向量回答了：“此时此刻，车在哪？朝哪走？速度多少？”

5.2 控制输入（Control Input, $u$）

• 定义：作用在系统上的“操纵量”，由控制器/驾驶员/算法直接施加。
• 通过控制输入，系统状态会随时间变化。
• 数学上就是 $u(t)$ 或 $u_k$。 在自行车模型中： $u = [a, \delta]^T$
• $a$ → 加速度（油门/刹车）
• $\delta$ → 前轮转角（方向盘转角） 控制输入回答了：“驾驶员/控制器此刻踩多少油门、打多少方向盘？” 状态和控制通过 动力学方程耦合: $\dot{x}(t) = f(x(t), u(t))$

Q6.什么是微分约束

在轨迹优化中，优化变量是 状态轨迹 $x(t)$ 和控制输入$u(t)$，但状态 $x(t)$ 并不是随便选的，它必须满足系统的 动力学方程：$\dot{x}(t) = f(x(t), u(t))$，这个方程就是微分约束 —— 它要求状态随时间的变化率（导数 $\dot{x}$）必须由动力学函数 $f$ 决定。

6.1 差速驱动举例（简化小车模型）

• 状态： $x = [x, y, \theta]^T$
• 表示位置 $(x,y)$ 和航向角 $\theta$。
• 控制输入： $u = [v, \omega]^T$
• 表示线速度 $v$ 和角速度 $\omega$。
• 动力学方程（非完整约束）：$\dot{x} = v \cos \theta, \quad \dot{y} = v \sin \theta, \quad \dot{\theta} = \omega$ 这里的关系就是 微分约束：你不能让车子随便瞬移到任意 $(x,y)$，它必须按照这个方程随时间演化。

6.2 自行车模型举例（常用于自动驾驶规划）

• 状态： $x = [X, Y, \theta, v]^T$
• 表示车辆质心坐标 $(X,Y)$、航向 $\theta$、速度 $v$。
• 控制输入： $u = [a, \delta]^T$
• 表示加速度 $a$ 和前轮转角 $\delta$。
• 动力学方程：$\dot{X} = v \cos \theta$，$\dot{Y} = v \sin \theta$ ， \dot{\theta} = \frac{v}{L} \tan \delta$$，$$\dot{v} = a 其中 $L$ 是轴距。 这里的微分约束要求：车辆的转向、位置、速度的变化必须符合自行车模型，而不能随便在 $(X,Y)$ 平面上画一条轨迹。

Q7.表达式的数学解释

这是轨迹优化问题的目标函数（cost function）： $\min_{x(t), u(t)} \; \ell(x(t_f)) + \int_0^{t_f} \ell(x(t), u(t)) \, dt$

• $\min_{x(t), u(t)}$ ：表示我们要在所有可能的 状态轨迹 $x(t)$ 和 控制输入序列 $u(t)$ 中，找到一个使代价最小的解。 换句话说：我们要优化的是轨迹 $(x(t), u(t))$。
• $\ell(x(t_f)$ ：这是 终端代价 (terminal cost)，即在最终时刻 $t_f$ 时，系统状态 $x(t_f)$ 带来的代价。
• $\int_0^{t_f} \ell(x(t), u(t))d$： 这是 积分型阶段代价 (running cost)，表示从 $t=0$ 到 $t=t_f$ 整个过程中，状态 $x(t)$ 和控制 $u(t)$ 每时每刻产生的代价，并进行累积。
• 常见形式：
  • 惩罚偏离目标状态的误差（例如 $‖x(t) - x_{goal}‖^2$）。
  • 惩罚控制能量（例如 $‖u(t)‖^2$）。 因此，总代价 = 终端代价 + 整个过程中累积的代价。

数学符号总结:

• $x(t)$ ：系统在时刻 $t$ 的状态向量（位置、速度、姿态…）。
• $u(t)$ ：系统在时刻 $t$ 的控制输入（力、力矩、电机转速…）。
• $t_f$ ：优化的终止时间（horizon）。
• $\ell(\cdot)$ ：代价函数（cost function），可以随问题设计。
• $\int_0^{t_f}$ ：对整个时间区间上的代价进行累加（积分）。

举例: 假设小车要从起点移动到终点：

• 终端代价 $\ell(x(t_f)) = ‖x(t_f) - x_{goal}‖^2$ → 惩罚最终位置和目标的差距。
• 阶段代价 $\ell(x(t), u(t)) = ‖x(t) - x_{ref}‖^2 + \alpha ‖u(t)‖^2$ → 惩罚中途偏离参考轨迹、以及控制能量过大。 最后优化器会输出一条轨迹 $(x(t), u(t))$，使得既能到达目标，又在过程中“花费”最小。

Q8.马尔可夫性质

定义： 一个系统满足马尔可夫性质，意味着：

系统未来的演化，只依赖于当前时刻的状态和控制输入，而与过去的历史无关。

数学表达式：$P(x_{t+1} \mid x_t, x_{t-1}, \dots, x_0, u_t) = P(x_{t+1} \mid x_t, u_t)$ 8.1 满足马尔可夫意味着什么

• 问题可以局部化，只需要考虑当前状态。
• 允许使用动态规划、iLQR 等方法。
• 内存和计算开销更小。 如果不满足时，意味着：
• 未来可能依赖“历史轨迹”，而不仅仅是当前状态。
• 如果状态向量没有包含所有必要信息（例如存在延迟、外部隐藏变量），系统就不是马尔可夫的。
• 需要 扩展状态空间，把缺失的历史信息补充进来。

8.2 示例 (1) 一维小车 状态：$x = [p, v]$ 控制：$u = F$ 动力学：$\dot{p} = v, \quad \dot{v} = \frac{F}{m}$ 只依赖当前 $(p,v)$ 和 $F$，即可确定下一步。满足马尔可夫。

(2) 自行车模型 状态：$x = [X, Y, \theta, v]^T$ 控制：$u = [a, \delta]^T$ 动力学：$\dot{X} = v \cos\theta$, $\dot{Y} = v \sin\theta$, $\dot{\theta} = \frac{v}{L}\tan\delta$,$\dot{v} = a$ 只依赖当前状态 $(X,Y,\theta,v)$ 和控制输入 $(a,\delta)$。 满足马尔可夫。

8.3 总结

• 马尔可夫性质：未来状态只依赖于当前状态和控制输入。
• 在轨迹优化和 iLQR/AL-iLQR 中，这是算法成立的前提。
• 差分底盘和自行车模型都满足马尔可夫。
• 如果一个系统不满足，就要扩展状态，把遗漏的信息补进来（例如延迟、外部力）。

不满足马尔可夫的系统主要有：

• 状态定义不完整（漏掉必要变量）。
• 存在延迟（未来依赖过去输入）。
• 存在积分记忆（未来依赖整个历史）。
• 有外部未建模的隐藏变量（环境变化）。 一般的做法：
• 扩展状态向量（把遗漏的变量、历史信息加进去）。
• 比如：从 $x=[p]$ 扩展到 $x=[p,v]$，从 $x=[X,Y,\theta,v]$ 扩展到 $x=[X,Y,\theta,v,\mu]$。这样就能重新满足马尔可夫。

Q9.为什么需要离散化

9.1. 连续时间的轨迹优化问题 一般形式：$\min_{x(t), u(t)} \; \ell(x(t_f)) + \int_0^{t_f} \ell(x(t), u(t)) \, dt$ 约束条件：$\dot{x}(t) = f(x(t), u(t)), \quad g(x(t), u(t)) \leq 0, \quad h(x(t), u(t)) = 0$

9.2. 为什么要离散化 轨迹优化需要离散化的原因：

1. 计算可行性：连续问题无法直接数值求解。
2. 优化可实现性：离散化后才变成有限维优化问题。
3. 递推结构：动态规划 / iLQR 需要离散递推。
4. 工程匹配：控制器本来就是离散执行的。 (1) 计算机不能直接处理连续时间问题

• 积分 $\int_0^{t_f} \ell(x(t), u(t)) d$ 一般无法解析计算。
• 微分方程 $\dot{x} = f(x,u$ 也往往没有解析解。
• 必须转化为有限维的数值优化问题。 (2) 转化为差分方程，便于优化迭代
• 将时间区间 $[0,t_f]$ 划分为 $N-1$ 个等长区间，步长为 $\Delta t$。
• 得到 $N$ 个离散时刻（knot points）。
• 动力学约束变为：$x_{k+1} = f(x_k, u_k, \Delta t), \quad k = 0, \dots, N-1$
• 代价函数变为：$J = \ell_N(x_N) + \sum_{k=0}^{N-1} \ell_k(x_k, u_k, \Delta t)$ 这样问题就转化为一个 有限维优化问题。 (3) 使动态规划 / iLQR 可用
• iLQR 的 Backward/Forward pass 必须在离散时间下递推。
• Riccati 方程的数值计算也是离散形式更稳定。 (4) 符合实际控制需求
• 自动驾驶控制器通常以固定频率运行（如 50Hz、100Hz）。
• 执行时，控制输入 $u_k$ 在一个采样区间内保持不变（零阶保持）。
• 实际控制本身就是离散的，离散化更符合工程实际。

9.3. 类比直观理解

• 连续时间轨迹优化 = 在“光滑曲线”上直接优化，难度大。
• 离散化后 = 用有限个点和控制动作近似整条曲线，更易计算。
• 离散点越多（ $\Delta t$ 越小），解越接近真实连续解。

Q10.直接法间接法

10.1. 自行车模型动力学 状态向量：$x = [X, Y, \theta, v]^T$ 控制输入：$u = [a, \delta]^T$ 动力学方程：\dot{X} = v \cos \theta$$，$$\dot{Y} = v \sin \theta$$，$$\dot{\theta} = \frac{v}{L} \tan \delta$$，$$\dot{v} = a 其中 $L$ 为轴距。

10.2. 直接法 (Direct Methods)

• 将 状态 $x$ 和控制 $u$ 全部作为优化变量。
• 将连续动力学转为 离散约束（如欧拉法、Hermite-Simpson 积分）。
• 得到一个大规模的 非线性规划问题 (NLP)，交给通用求解器 (IPOPT/SNOPT) 处理。 建模形式：
• 优化变量：$\{x_0, x_1, \dots, x_N, u_0, \dots, u_{N-1}\}$
• 约束条件：$x_{k+1} = f(x_k, u_k, \Delta t), \quad k=0,\dots,N-1$
• 目标函数：$J = \ell_N(x_N) + \sum_{k=0}^{N-1} \ell(x_k, u_k)$
• 优点：鲁棒、能处理复杂约束（状态、控制、障碍物）。
• 缺点：变量规模大（$O(N\cdot (n+m))$），计算慢。

10.3. 间接法 (Indirect Methods, iLQR/DDP)

• 只优化控制输入序列 $\{u_0,\dots,u_{N-1}\}$。
• 状态序列 $\{x_k\}$ 由动力学方程 rollout 得到，不作为独立变量。
• 使用 动态规划 / iLQR，在每一步计算控制律的修正。 建模形式
• 优化变量：$\{u_0, u_1, \dots, u_{N-1}\}$
• 状态由动力学递推得到：$x_{k+1} = f(x_k, u_k, \Delta t), \quad x_0 \text{ 给定}$
• 目标函数：$J = \ell_N(x_N) + \sum_{k=0}^{N-1} \ell(x_k, u_k)$
• 优点：变量少（只优化控制 $u$，规模 $O(N \cdot m)$），速度快，内存需求小。
• 缺点：对初值敏感，难以处理复杂约束（需要 AL-iLQR 或 ALTRO 扩展）。

10.4. 对比总结