最优控制理论是现代控制理论的核心分支，它研究如何在满足系统动力学约束的条件下，寻找使某个性能指标达到最优的控制策略。本章将从变分法的基本原理出发，深入探讨Pontryagin最大值原理、线性二次调节器（LQR）、代数Riccati方程以及动态规划方法。我们将通过Apollo登月舱轨迹优化和Boston Dynamics Spot四足机器人步态优化两个经典案例，展示最优控制理论在实际工程中的应用。
变分法是研究泛函极值问题的数学工具，起源于17世纪的最速降线问题。在最优控制理论中，变分法为我们提供了寻找最优轨迹的数学框架。与普通函数的极值问题不同，变分问题研究的是函数的函数（泛函）的极值，这正是控制问题的本质——我们要在所有可能的控制函数中找到最优的那一个。
变分法的核心思想是通过微小扰动来识别极值条件。设想我们有一个候选的最优轨迹，如果它确实是最优的，那么任何微小的扰动都不应该改善性能指标。这个直观的想法导出了变分法的第一变分条件，类似于普通微积分中导数为零的条件。
考虑一般形式的最优控制问题：
\[\min_{u(\cdot)} J = \phi(x(t_f), t_f) + \int_{t_0}^{t_f} L(x(t), u(t), t) dt\]其中：
系统动力学约束为： \(\dot{x}(t) = f(x(t), u(t), t), \quad x(t_0) = x_0\)
这个约束告诉我们系统状态如何随控制输入演化。物理上，$f$可能代表牛顿定律、电路方程或化学反应动力学等。
变分法的局限性：
经典变分法在处理实际控制问题时存在几个重要局限：
控制约束处理困难：实际系统的控制输入总是有界的（如电机的最大力矩、阀门的最大开度），但变分法难以直接处理不等式约束。
奇异问题：当哈密顿函数对控制变量的依赖是线性的时，经典变分法失效，这就是所谓的奇异控制问题。
数值求解困难：变分法导出的是两点边值问题，数值求解时需要同时满足初始条件和终端条件，这在高维情况下极具挑战性。
正是这些局限促使Pontryagin等人发展了最大值原理，为有约束的最优控制问题提供了更一般的理论框架。
对于无约束的变分问题，最优解必须满足Euler-Lagrange方程。这个方程是变分法的基石，由Euler在1744年首次提出，后经Lagrange完善。理解Euler-Lagrange方程的推导过程对掌握最优控制理论至关重要。
考虑简单的泛函：
\[J[y] = \int_{a}^{b} F(x, y, y') dx\]其中$F$是关于自变量$x$、未知函数$y(x)$及其导数$y’(x)$的函数。
推导过程：
设$y^*(x)$是使泛函$J$达到极值的函数，引入扰动$\epsilon \eta(x)$，其中$\eta(x)$是满足边界条件$\eta(a) = \eta(b) = 0$的任意光滑函数。考虑扰动后的泛函：
\[J(\epsilon) = \int_{a}^{b} F(x, y^* + \epsilon\eta, y^{*'} + \epsilon\eta') dx\]极值的必要条件是： \(\frac{dJ}{d\epsilon}\bigg|_{\epsilon=0} = 0\)
通过分部积分和任意性论证，得到Euler-Lagrange方程：
\[\frac{\partial F}{\partial y} - \frac{d}{dx}\left(\frac{\partial F}{\partial y'}\right) = 0\]物理意义：
Euler-Lagrange方程在物理学中有深刻的含义。在分析力学中，它导出了拉格朗日运动方程，统一了牛顿力学的各种问题。在最优控制中，它刻画了最优轨迹必须满足的微分方程。
高阶导数情况：
对于包含高阶导数的泛函： \(J[y] = \int_{a}^{b} F(x, y, y', y'', \ldots, y^{(n)}) dx\)
相应的Euler-Lagrange方程变为： \(\frac{\partial F}{\partial y} - \frac{d}{dx}\left(\frac{\partial F}{\partial y'}\right) + \frac{d^2}{dx^2}\left(\frac{\partial F}{\partial y''}\right) - \cdots + (-1)^n\frac{d^n}{dx^n}\left(\frac{\partial F}{\partial y^{(n)}}\right) = 0\)
多变量情况：
当有多个未知函数$y_1(x), y_2(x), \ldots, y_n(x)$时，每个函数都对应一个Euler-Lagrange方程： \(\frac{\partial F}{\partial y_i} - \frac{d}{dx}\left(\frac{\partial F}{\partial y_i'}\right) = 0, \quad i = 1, 2, \ldots, n\)
这形成了一个耦合的微分方程组，在控制问题中对应于多输入多输出（MIMO）系统。
Pontryagin最大值原理是20世纪控制理论最重要的成就之一，它彻底改变了我们处理最优控制问题的方式。1956年，苏联数学家Lev Pontryagin和他的学生们（Boltyanskii、Gamkrelidze、Mishchenko）在研究时间最优控制问题时发现了这个原理。有趣的是，他们最初是为了解决苏联洲际弹道导弹的轨迹优化问题，这个军事需求催生了一个影响深远的数学理论。
最大值原理的革命性在于它能够处理控制变量受约束的问题，这是经典变分法无法直接解决的。在实际工程中，所有的控制输入都是有界的——火箭推力有最大值、舵面偏转有极限、电机转矩有饱和。最大值原理优雅地处理了这些约束。
哈密顿函数的构造：
定义哈密顿函数（也称为Pontryagin函数）：
\[H(x, u, \lambda, t) = L(x, u, t) + \lambda^T f(x, u, t)\]其中 $\lambda(t) \in \mathbb{R}^n$ 是协态向量（也称为伴随变量或拉格朗日乘子）。
哈密顿函数的物理意义可以理解为”瞬时总代价”：$L$是瞬时运行代价，$\lambda^T f$可以解释为状态变化带来的”影子代价”。协态$\lambda_i$表示状态$x_i$的边际价值——增加一单位$x_i$对总代价的影响。
最大值原理的必要条件：
如果$(x^(t), u^(t))$是最优轨迹和最优控制，则存在协态函数$\lambda(t)$使得：
状态方程（正向动力学）： \(\dot{x} = \frac{\partial H}{\partial \lambda} = f(x, u, t)\)
这就是原始的系统动力学方程，描述系统如何正向演化。
协态方程（反向动力学）： \(\dot{\lambda} = -\frac{\partial H}{\partial x} = -\frac{\partial L}{\partial x} - \left(\frac{\partial f}{\partial x}\right)^T \lambda\)
协态方程描述了”价值”如何反向传播。它告诉我们状态的边际价值如何随时间变化。
最优性条件（Pontryagin最小值条件）： \(H(x^*, u^*, \lambda^*, t) = \min_{u \in U} H(x^*, u, \lambda^*, t)\)
注意：在最小化问题中是最小值条件，在最大化问题中是最大值条件。这个条件说明最优控制使哈密顿函数达到全局最小值。
横截条件（边界条件）：
横截条件取决于问题的边界约束：
| 自由终端状态：$\lambda(t_f) = \frac{\partial \phi}{\partial x}\bigg | _{t=t_f}$ |
| 自由终端时间：$H(t_f) + \frac{\partial \phi}{\partial t}\bigg | _{t=t_f} = 0$ |
对于更复杂的终端约束$\psi(x(t_f), t_f) = 0$，需要引入额外的拉格朗日乘子。
最大值原理vs变分法：
最大值原理可以看作变分法的推广：
最小时间控制问题是最优控制理论中的经典问题，它在工业机器人、航天器姿态控制等领域有广泛应用。这类问题的特点是控制能量有限，目标是以最快速度完成任务。下面通过双积分器系统详细展示最大值原理的应用。
考虑双积分器系统的最小时间控制问题：
系统动力学：
ẋ₁ = x₂ （位置的导数是速度）
ẋ₂ = u （速度的导数是加速度/控制）
控制约束：
|u| ≤ 1 （加速度有界）
初始条件：x₁(0) = x₁₀, x₂(0) = x₂₀
终端条件：x₁(tf) = 0, x₂(tf) = 0
目标：最小化时间 tf
这个模型可以代表多种物理系统：
应用最大值原理：
性能指标：$J = \int_0^{t_f} 1 dt = t_f$（最小化时间）
构造哈密顿函数： \(H = 1 + \lambda_1 x_2 + \lambda_2 u\)
其中”1”来自于被积函数$L = 1$（时间的累积）。
求解协态方程：
根据协态方程 $\dot{\lambda}_i = -\frac{\partial H}{\partial x_i}$：
\[\dot{\lambda}_1 = -\frac{\partial H}{\partial x_1} = 0 \Rightarrow \lambda_1 = c_1 \text{（常数）}\] \[\dot{\lambda}_2 = -\frac{\partial H}{\partial x_2} = -\lambda_1 = -c_1 \Rightarrow \lambda_2 = -c_1 t + c_2\]协态的物理意义：
确定最优控制：
根据最优性条件，最优控制使哈密顿函数最小： \(u^* = \arg\min_{|u| \leq 1} H = \arg\min_{|u| \leq 1} (1 + \lambda_1 x_2 + \lambda_2 u)\)
由于H对u是线性的，最优控制必定在约束边界上： \(u^* = \begin{cases} -1 & \text{if } \lambda_2 > 0 \\ +1 & \text{if } \lambda_2 < 0 \\ \text{未定} & \text{if } \lambda_2 = 0 \end{cases}\)
即：$u^* = -\text{sign}(\lambda_2) = -\text{sign}(-c_1 t + c_2)$
Bang-Bang控制的物理解释：
最优控制是Bang-Bang型的，即控制总是取最大值或最小值，永不在中间值。这个结果有直观的物理解释：
相平面分析：
在$(x_1, x_2)$相平面上，系统轨迹满足：
最优切换曲线（开关曲线）： \(\Gamma^+ : x_1 = -\frac{1}{2}x_2|x_2| \quad (x_2 > 0)\) \(\Gamma^- : x_1 = \frac{1}{2}x_2|x_2| \quad (x_2 < 0)\)
最优控制策略：
这个策略确保以最短时间到达目标，体现了”全力加速-适时切换-全力减速”的最优原则。
线性二次调节器是最优控制理论中最重要和最实用的结果之一。考虑线性时不变系统：
\[\dot{x}(t) = Ax(t) + Bu(t)\]目标是最小化二次性能指标：
\[J = \int_0^\infty (x^T Q x + u^T R u) dt\]其中：
应用最大值原理，哈密顿函数为：
\[H = x^T Q x + u^T R u + \lambda^T (Ax + Bu)\]最优性条件 $\frac{\partial H}{\partial u} = 0$ 给出：
\[2Ru + B^T \lambda = 0 \Rightarrow u^* = -\frac{1}{2}R^{-1}B^T \lambda\]假设协态与状态成线性关系：$\lambda = 2Px$，其中 $P$ 是待定的对称矩阵。
代入协态方程并整理，可得代数Riccati方程（将在5.3节详细讨论）。
LQR的最优控制律为状态反馈形式：
\[u^*(t) = -Kx(t)\]其中反馈增益矩阵： \(K = R^{-1}B^T P\)
$P$ 是代数Riccati方程的解： \(A^T P + PA - PBR^{-1}B^T P + Q = 0\)
稳定性保证：若 $(A, B)$ 可控，$(A, \sqrt{Q})$ 可观，则闭环系统 $\dot{x} = (A - BK)x$ 渐近稳定。
相位裕度：LQR控制器在每个输入通道上至少有60度的相位裕度。
增益裕度：增益裕度为 $[0.5, \infty)$，即系统对增益变化具有很强的鲁棒性。
最优性：LQR给出的是全局最优解，不存在局部最优。
选择合适的 $Q$ 和 $R$ 矩阵是LQR设计的关键：
Bryson规则： \(Q_{ii} = \frac{1}{x_{i,\max}^2}, \quad R_{jj} = \frac{1}{u_{j,\max}^2}\)
输出加权：若只关心输出 $y = Cx$，可设： \(Q = C^T W_y C\)
迭代调整：
代数Riccati方程是LQR和许多其他最优控制问题的核心。标准形式为：
\[A^T P + PA - PBR^{-1}B^T P + Q = 0\]这是一个关于对称矩阵 $P$ 的非线性矩阵方程。
解的存在性：若 $(A, B)$ 可镇定，$(A, \sqrt{Q})$ 无不可观的不稳定模态，则存在唯一的正定解 $P \succ 0$。
解的唯一性：在上述条件下，存在唯一的镇定解，即使闭环系统 $A - BR^{-1}B^T P$ 稳定的解。
对称性：Riccati方程的解必定是对称矩阵。
Schur方法（最常用）： 构造哈密顿矩阵： \(\mathcal{H} = \begin{bmatrix} A & -BR^{-1}B^T \\ -Q & -A^T \end{bmatrix}\)
通过Schur分解找到稳定子空间，从而求得 $P$。
对于离散系统 $x_{k+1} = Ax_k + Bu_k$，对应的DARE为：
\[P = A^T PA - A^T PB(R + B^T PB)^{-1}B^T PA + Q\]Riccati方程的解 $P$ 可以理解为”代价-to-go”矩阵：
\[V(x) = x^T P x\]表示从状态 $x$ 出发，采用最优控制策略到达原点的最小代价。
动态规划基于Bellman的最优性原理：
“最优策略具有这样的性质：无论初始状态和初始决策如何，对于由初始决策所产生的状态，其余的决策必须构成相对于该状态的最优策略。”
定义值函数（最优代价-to-go）：
\[V(x, t) = \min_{u(\cdot)} \left\{ \int_t^{t_f} L(x(\tau), u(\tau), \tau) d\tau + \phi(x(t_f), t_f) \right\}\]Hamilton-Jacobi-Bellman（HJB）偏微分方程：
\[-\frac{\partial V}{\partial t} = \min_u \left\{ L(x, u, t) + \left(\frac{\partial V}{\partial x}\right)^T f(x, u, t) \right\}\]边界条件：$V(x, t_f) = \phi(x, t_f)$
两种方法本质上是等价的：
对于LQR问题，假设值函数为二次形式：
\[V(x, t) = x^T P(t) x\]代入HJB方程，可得Riccati微分方程：
\[-\dot{P} = A^T P + PA - PBR^{-1}B^T P + Q\]稳态时 $\dot{P} = 0$，即得到代数Riccati方程。
对于非线性系统，HJB方程通常没有解析解，需要数值方法：
Apollo登月舱的下降轨迹优化是最优控制理论的经典应用。1969年的登月任务中，需要在有限燃料约束下实现安全着陆。
问题描述：
登月舱从高度 $h_0 = 15000$ m、速度 $v_0 = 450$ m/s开始下降，需要优化推力控制使其软着陆（$h_f = 0$, $v_f = 0$）。
动力学模型： \(\begin{aligned} \dot{h} &= v \\ \dot{v} &= -g + \frac{T}{m} \\ \dot{m} &= -\alpha T \end{aligned}\)
其中：
优化目标：
最小化燃料消耗（等价于最大化终端质量）： \(J = -m(t_f)\)
应用Pontryagin最大值原理：
哈密顿函数： \(H = \lambda_h v + \lambda_v \left(-g + \frac{T}{m}\right) + \lambda_m (-\alpha T)\)
协态方程： \(\begin{aligned} \dot{\lambda}_h &= 0 \Rightarrow \lambda_h = c_1 \\ \dot{\lambda}_v &= -\lambda_h = -c_1 \\ \dot{\lambda}_m &= \lambda_v \frac{T}{m^2} \end{aligned}\)
开关函数： \(S = \frac{\lambda_v}{m} - \alpha \lambda_m\)
最优控制策略（Bang-Bang）： \(T^* = \begin{cases} T_{\max} & \text{if } S > 0 \\ 0 & \text{if } S < 0 \end{cases}\)
工程实现：
实际Apollo制导计算机使用了简化的多项式制导律，基于最优轨迹的近似：
\[T(t) = a_0 + a_1 t + a_2 t^2\]系数通过边界条件和燃料约束确定。这种方法计算简单，适合1969年的计算能力限制。
现代改进：
现代方法使用凸优化技术，将问题转化为二阶锥规划（SOCP）：
\[\begin{aligned} \min_{u, \sigma} \quad & -\ln(m(t_f)) \\ \text{s.t.} \quad & \dot{r} = v \\ & \dot{v} = g + \frac{u}{m} \\ & \dot{m} = -\alpha \|u\| \\ & \|u\| \leq T_{\max} \\ & \cos\theta_{\max} \|u\| \leq u_z \end{aligned}\]这种方法可以处理更复杂的约束，如姿态限制、落点精度等。
Spot机器人的动态步态需要实时优化控制，以实现稳定、高效的运动。
简化模型：
采用单刚体动力学模型（SRBD）：
\[\begin{aligned} m\ddot{r} &= mg + \sum_{i=1}^{4} f_i \\ I\dot{\omega} &= \sum_{i=1}^{4} (p_i - r) \times f_i \end{aligned}\]其中：
优化问题：
在每个控制周期（10ms）内求解：
\[\min_{f_i, \tau_j} J = \sum_{k=0}^{N} \left( \|x_k - x_k^{ref}\|_Q^2 + \sum_{i=1}^{4} \|f_{i,k}\|_R^2 \right)\]约束条件：
| 关节力矩限制：$ | \tau_j | \leq \tau_{\max}$ |
分层控制架构：
高层：步态规划器
↓ (步态参数、落脚点)
中层：MPC控制器
↓ (期望接触力)
底层：全身控制器（WBC）
↓ (关节力矩)
硬件：电机驱动器
MPC实现：
使用凸化的MPC，线性化动力学：
\[x_{k+1} = A_k x_k + B_k u_k + g_k\]其中状态 $x = [r, \dot{r}, \theta, \omega]$，控制 $u = [f_1, …, f_4]$。
通过QP求解器（如OSQP）实时求解，计算时间 < 3ms。
步态优化：
不同步态的优化目标：
地形适应：
使用价值迭代进行地形代价地图学习：
\[V(s) = \min_a \left[ c(s, a) + \gamma V(s') \right]\]其中 $c(s, a)$ 包含：
实验结果：
Lev Semyonovich Pontryagin是20世纪最伟大的数学家之一，他在拓扑学、代数和控制理论等多个领域做出了开创性贡献。
生平与成就：
Pontryagin在14岁时因煤气爆炸事故失明，但这并未阻止他成为杰出的数学家。在母亲的帮助下，他完成了莫斯科大学的学业，并在23岁时成为教授。
1956年，Pontryagin开始研究最优控制问题。当时苏联正在开发洲际弹道导弹，需要解决最优轨迹问题。传统的变分法无法处理控制约束，Pontryagin和他的学生（Boltyanskii、Gamkrelidze、Mishchenko）发展了最大值原理。
最大值原理的意义：
轶事：
考虑随机动力系统：
\[dx = f(x, u)dt + G(x)dW\]其中 $W$ 是维纳过程。对应的HJB方程变为：
\[-\frac{\partial V}{\partial t} = \min_u \left\{ L(x, u) + \mathcal{L}V \right\}\]其中 $\mathcal{L}$ 是Kolmogorov算子： \(\mathcal{L} = f^T \nabla + \frac{1}{2}\text{tr}(GG^T \nabla^2)\)
路径积分方法将最优控制问题转化为统计力学问题：
\[u^*(x) = -R^{-1}g^T(x) \nabla V(x)\]值函数通过路径积分计算：
\[V(x) = -\lambda \log \mathbb{E}\left[ e^{-\frac{1}{\lambda}S(\tau)} | x_0 = x \right]\]其中 $S(\tau)$ 是路径代价。
优势：
最优控制可以表述为KL散度最小化：
\[\min_{\pi} \mathbb{E}_\pi[c(x, u)] + \lambda D_{KL}(\pi \| \pi_0)\]这建立了最优控制与最大熵原理的联系，在机器人学习中有重要应用。
最优控制理论为设计高性能控制系统提供了系统的数学框架。本章的核心概念包括：
Pontryagin最大值原理：处理有约束最优控制问题的基本工具，通过协态方程和哈密顿函数刻画最优性条件。
线性二次调节器（LQR）：最实用的最优控制方法之一，提供状态反馈形式的全局最优解，具有优良的稳定性和鲁棒性。
代数Riccati方程：LQR和其他最优控制问题的核心，其解代表了系统的”代价-to-go”。
动态规划与HJB方程：从值函数角度研究最优控制，与最大值原理形成对偶关系。
工程应用：从Apollo登月到现代机器人，最优控制理论在复杂工程系统中发挥关键作用。
关键公式汇总：
习题5.1 双积分器最小时间控制
| 考虑系统：$\ddot{x} = u$，$ | u | \leq 1$。求从初始状态 $(x_0, \dot{x}_0) = (2, 1)$ 到原点的最小时间控制。 |
习题5.2 LQR增益计算
给定系统： \(A = \begin{bmatrix} 0 & 1 \\ -2 & -3 \end{bmatrix}, \quad B = \begin{bmatrix} 0 \\ 1 \end{bmatrix}\)
使用 $Q = I$，$R = 1$，求LQR反馈增益 $K$。
习题5.3 燃料最优控制
| 卫星姿态控制系统：$\ddot{\theta} = u/I$，其中 $I = 100$ kg·m²。求使燃料消耗 $J = \int_0^T | u | dt$ 最小的控制，使卫星从 $\theta(0) = 30°$，$\dot{\theta}(0) = 0$ 转到 $\theta(T) = 0$，$\dot{\theta}(T) = 0$。 |
习题5.4 离散LQR
离散系统：$x_{k+1} = Ax_k + Bu_k$，其中： \(A = \begin{bmatrix} 1.1 & 0.1 \\ 0 & 0.9 \end{bmatrix}, \quad B = \begin{bmatrix} 0 \\ 1 \end{bmatrix}\)
设计使 $J = \sum_{k=0}^{\infty} (x_k^T Q x_k + u_k^2)$ 最小的控制器，$Q = I$。
习题5.5 奇异最优控制
考虑系统：$\dot{x}_1 = x_2$，$\dot{x}_2 = u$，最小化： \(J = \frac{1}{2}\int_0^1 (x_1^2 + \epsilon u^2) dt\) 当 $\epsilon \to 0$ 时，分析最优控制的性质。
习题5.6 约束LQR
设计倒立摆的LQR控制器，考虑输入约束 $|u| \leq 5$ N。系统在平衡点线性化后： \(A = \begin{bmatrix} 0 & 1 & 0 & 0 \\ 0 & 0 & -9.8 & 0 \\ 0 & 0 & 0 & 1 \\ 0 & 0 & 29.4 & 0 \end{bmatrix}, \quad B = \begin{bmatrix} 0 \\ 1 \\ 0 \\ -2 \end{bmatrix}\)
习题5.7 路径积分控制实现
用路径积分方法求解非线性系统 $\dot{x} = -x + x^3 + u$ 的最优控制，使 $J = \int_0^T (x^2 + u^2)dt$ 最小。
习题5.8 MPC与LQR对比
比较MPC和LQR在处理倒立摆控制问题时的性能差异，特别是存在状态和输入约束时。
错误：直接应用LQR而不检查可控性。
后果：Riccati方程可能无解或解不稳定。
正确做法：
1. 检查可控性矩阵 rank([B, AB, ..., A^(n-1)B]) = n
2. 对不可控模态，确保其稳定
3. 使用可控性分解识别可控子空间
错误：随意选择 $Q$ 和 $R$ 矩阵。
问题表现：
解决方案：
错误：Riccati方程求解不稳定。
原因：
改进：
错误：假设模型完全准确。
风险：实际系统性能退化甚至失稳。
鲁棒化方法：
错误：不当的连续到离散转换。
常见问题：
正确方法：
离散化矩阵：
A_d = e^(AT_s)
B_d = ∫[0,T_s] e^(Aτ) B dτ
错误：有限时域问题的终端处理不当。
表现：接近终端时控制异常。
解决：