CMU-16-745-最优控制与强化学习笔记-全-

CMU 16-745 最优控制与强化学习笔记（全）

1：HW0 环境配置与提交指南 🚀

在本节课中，我们将学习如何为《最优控制和强化学习》课程完成第零次作业（HW0）的环境配置与提交。我们将从接受GitHub Classroom作业开始，逐步完成Julia环境安装、代码仓库克隆、作业内容填写，直至最终提交作业。

概述

我们将按照以下步骤完成HW0：

通过GitHub Classroom链接接受作业。
安装指定版本的Julia编程语言。
将个人作业仓库克隆到本地计算机。
配置Jupyter Notebook环境并完成作业题目。
将完成的作业提交回GitHub并创建发布版本。

1. 接受GitHub Classroom作业

首先，我们需要通过课程提供的链接接受作业。此步骤将在GitHub上为你创建一个私有的作业代码仓库。

以下是具体操作步骤：

在Piazza或课程页面找到HW0发布的帖子，其中包含GitHub Classroom链接。
点击链接，确保你的浏览器已登录到你想关联的GitHub账户。
系统会请求权限，点击“同意”。
在导入的课程名单中找到你的名字（或测试账户的名字）。
确认所选标识符（通常是你的学校邮箱）无误。
点击“接受此作业”。系统将创建一个以作业名称和你GitHub用户名命名的私有仓库。

注意：仓库创建可能需要几分钟时间。完成后，你可以通过提供的链接访问你的私有仓库。

2. 安装Julia 1.6.5

作业要求使用Julia语言的长期支持版本1.6.5，而非最新版本。请严格按照以下步骤安装。

以下是安装Julia 1.6.5的步骤：

访问Julia官方网站，下载Julia 1.6.5版本。
根据你的操作系统（Windows、macOS或Linux）选择对应的安装包。
对于macOS用户：下载.dmg文件，像安装其他应用程序一样将其拖入“应用程序”文件夹。
为了在终端中方便地调用Julia，可以创建一个符号链接。在终端中执行以下命令：
```
ln -s /Applications/Julia-1.6.app/Contents/Resources/julia/bin/julia /usr/local/bin/julia
```
打开终端，输入julia命令，如果成功启动REPL（交互式环境），则说明安装成功。

提示：如果遇到问题，首先检查你是否安装的是1.6.5版本。

3. 克隆作业仓库到本地

现在，我们将线上GitHub仓库克隆到本地计算机，以便进行编辑和测试。

操作步骤如下：

在本地创建一个文件夹，用于存放本课程的所有作业，例如optimal_control_homeworks。
打开终端，使用cd命令进入该文件夹。
回到你的GitHub作业仓库页面，点击绿色的“Code”按钮，复制仓库的HTTPS或SSH链接。
在终端中，使用git clone <你复制的链接>命令克隆仓库。这会将仓库的所有内容下载到你当前所在的文件夹中。

4. 配置环境并完成作业

上一节我们完成了代码的本地克隆，本节中我们来看看如何配置Jupyter Notebook环境并完成作业内容。

4.1 安装IJulia包

IJulia包是Julia与Jupyter Notebook交互的桥梁。我们需要在Julia环境中安装它。

在终端中启动Julia REPL（输入julia）。
在Julia的julia>提示符后，输入以下命令进入包管理模式：
```
]
```
在pkg>提示符后，输入以下命令添加IJulia包：
```
add IJulia
```
安装完成后，按退格键（Backspace）退出包管理模式，回到julia>提示符。
输入以下命令启动Jupyter Notebook：
```
using IJulia
notebook()
```
这将在你的默认浏览器中打开Jupyter Notebook界面。

4.2 完成作业文件

在Jupyter Notebook界面中，导航到你克隆的作业文件夹。你需要完成以下三个文件：

以下是需要完成的文件列表：

src/homework0.jl：用文本编辑器（如VS Code、Sublime Text）打开此文件，填写你的学生信息（姓名、学号等）。
Q1.ipynb：第一个问题的Jupyter Notebook。打开后，按照单元格内的说明逐步完成计算和编码任务。确保运行每一个单元格，直到所有测试（test cells）都通过。
Q2.ipynb：第二个问题的Jupyter Notebook。操作方式同Q1，仔细阅读说明，完成代码，运行所有单元格并通过测试。

核心操作：在Notebook中，使用 Shift + Enter 运行当前单元格并跳转到下一个。

5. 提交作业

完成所有题目并通过测试后，我们需要将修改推送（Push）到GitHub，并创建一个发布（Release）作为最终提交。

5.1 推送更改到GitHub

首先，我们将本地修改同步到远程仓库。

在终端中，进入你的作业仓库目录。
使用git status命令查看有哪些文件被修改。你应该能看到homework0.jl、Q1.ipynb和Q2.ipynb的状态是“modified”。
使用git add .命令暂存所有更改。
使用git commit -m “提交信息”命令提交更改，例如git commit -m “完成HW0所有题目”。
使用git push命令将本地提交推送到GitHub远程仓库。
回到你的GitHub仓库页面刷新，确认更改已成功上传。

5.2 创建发布版本

最后一步是创建一个标签化的发布（Release），这将被视为你的最终作业提交。

请严格按照以下步骤操作：

在你的GitHub仓库页面上，点击“Releases”标签。
点击“Create a new release”。
在“Choose a tag”输入框中，输入 v1.0，然后点击“Create new tag”。
在“Release title”输入框中，输入 Final Submission。
点击“Publish release”按钮。

完成后，你的仓库“Releases”部分会显示一个名为“Final Submission (v1.0)”的发布，这标志着你的HW0已成功提交。

总结

本节课中我们一起学习了完成CMU《最优控制和强化学习》课程HW0的完整流程。我们掌握了如何通过GitHub Classroom接受作业、安装特定版本的Julia、克隆仓库、配置Jupyter环境、完成并测试作业代码，以及最终通过Git提交和创建发布版本。这套流程是后续所有作业的基础，请务必熟悉。

2：动力学回顾 🚀

在本节课中，我们将学习动力学系统的基本概念，这是进行最优控制和强化学习的基础。我们将从最一般的动力学形式开始，逐步深入到更具体的系统类型，如仿射控制系统、机械臂动力学和线性系统。我们还将探讨平衡点及其稳定性分析。

1. 动力学系统的一般形式

一个光滑动力学系统最通用的形式是一个一阶向量微分方程：

\[\dot{x} = f(x, u) \]

其中：

$ x \in \mathbb{R}^n $ 称为状态，通常包含位置和速度等信息。
$ u \in \mathbb{R}^m $ 称为输入或控制量，例如关节扭矩或推力。
$ f $ 称为动力学函数。

对于大多数机械系统，状态向量 $ x $ 可以分解为构型 $ q $（如位置或关节角）和速度 $ v $：

\[x = \begin{bmatrix} q \\ v \end{bmatrix} \]

需要注意的是，构型 $ q $ 并不总是一个向量（例如，单摆的构型空间是一个圆 $ S^1 $），但速度 $ v $ 总是一个向量。

2. 仿射控制系统

许多系统，特别是机械系统，可以写成一种更具结构性的形式，称为仿射控制系统：

\[\dot{x} = f_0(x) + B(x) u \]

其中：

$ f_0(x) $ 称为漂移项，仅依赖于状态。
$ B(x) $ 称为输入雅可比矩阵。

这种形式之所以称为“仿射”，是因为在控制输入 $ u $ 上是线性的（尽管在状态 $ x $ 上可以是非线性的）。几乎所有光滑系统都可以通过数学技巧（如引入新状态）改写为此形式。

示例：单摆动力学
单摆的动力学方程为 $ mL^2 \ddot{\theta} + mgL \sin\theta = \tau $。将其写成仿射形式：

状态： $ x = [\theta, \dot{\theta}]^T $
动力学： $ \dot{x} = \begin{bmatrix} \dot{\theta} \ -\frac{g}{L}\sin\theta \end{bmatrix} + \begin{bmatrix} 0 \ \frac{1}{mL^2} \end{bmatrix} \tau $
这里， $ f_0(x) = [\dot{\theta}, -\frac{g}{L}\sin\theta]^T $， $ B(x) = [0, \frac{1}{mL^2}]T $。

3. 机械臂动力学（拉格朗日形式）

机械系统通常采用以下形式的动力学方程，这源于拉格朗日力学：

\[M(q) \dot{v} + C(q, v) = B(q) u \]

其中：

$ M(q) $ 是质量矩阵（正定）。
$ C(q, v) $ 包含科里奥利力、离心力和重力等项。
$ B(q) $ 是输入雅可比矩阵。
$ v $ 是速度，通常通过运动学关系 $ \dot{q} = G(q) v $ 与构型导数关联。在许多简单情况下， $ G(q) $ 是单位矩阵，即 $ v = \dot{q} $。

为了将其放入通用形式 $ \dot{x} = f(x, u) $，我们可以解出 $ \dot{v} $：

\[\dot{v} = M(q)^{-1} [B(q) u - C(q, v)] \]

\[\dot{q} = G(q) v \]

然后组合状态 $ x = [q; v] $。

4. 线性系统

线性系统是最经典且研究最透彻的控制系统模型，形式如下：

\[\dot{x} = A(t) x + B(t) u \]

如果 $ A $ 和 $ B $ 是常数矩阵，则系统是时不变的；否则是时变的。线性系统理论非常完善，因此我们经常将非线性系统在其平衡点附近线性化，以利用线性系统的工具进行分析和设计。

线性化方法为：

\[A = \frac{\partial f}{\partial x} \bigg|_{(x^*, u^*)}, \quad B = \frac{\partial f}{\partial u} \bigg|_{(x^*, u^*)} \]

其中 $ (x^, u^) $ 是线性化点（通常是平衡点）。

5. 平衡点

平衡点是系统可以保持静止的状态点，即满足：

\[f(x, u) = 0 \]

示例：单摆的平衡点
对于无输入的单摆动力学 $ \dot{x} = [\dot{\theta}, -\frac{g}{L}\sin\theta]^T $，令其为零：

$ \dot{\theta} = 0 $
$ -\frac{g}{L}\sin\theta = 0 \Rightarrow \theta = 0 \text{ 或 } \pi $
因此，存在两个平衡点：最低点 $ \theta=0 $ 和最高点 $ \theta=\pi $。

通过控制移动平衡点
如果我们希望单摆停留在 $ \theta = \pi/2 $（水平位置），则需要求解：

\[f(x, u) = \begin{bmatrix} \dot{\theta} \\ -\frac{g}{L}\sin(\pi/2) + \frac{1}{mL^2}u \end{bmatrix} = \begin{bmatrix} 0 \\ 0 \end{bmatrix} \]

解得所需扭矩为 $ u = mgL $。这表明可以通过施加控制输入来创造新的平衡点。

6. 平衡点的稳定性

稳定性分析关心的是：如果系统从平衡点附近开始，是否会保持在附近？我们首先通过一维非线性系统来建立直观理解。

一维系统直观分析
绘制 $ \dot{x} = f(x) $ 的曲线。平衡点是曲线与x轴的交点（$ f(x)=0 $）。

如果在平衡点处，当 $ x $ 略微增加时 $ f(x) < 0 $（导数负），系统会往回走，则该平衡点是稳定的。
反之，如果略微增加时 $ f(x) > 0 $（导数正），系统会远离，则该平衡点是不稳定的。

高维系统与线性化稳定性定理
对于高维系统 $ \dot{x} = f(x) $，我们在平衡点 $ x^* $ 处线性化，得到雅可比矩阵 $ J = \frac{\partial f}{\partial x} \big|_{x^*} $。

计算 $ J $ 的特征值。
如果所有特征值的实部都小于零，则平衡点是渐近稳定的。
如果存在实部大于零的特征值，则平衡点是不稳定的。
如果所有特征值实部小于等于零，且存在实部为零的特征值，则线性化方法无法判定，需要进一步分析（称为临界情况）。

示例：单摆稳定性分析
单摆动力学的雅可比矩阵为：

\[J = \frac{\partial f}{\partial x} = \begin{bmatrix} 0 & 1 \\ -\frac{g}{L}\cos\theta & 0 \end{bmatrix} \]

在最低点 $ \theta=0 $： $ J = \begin{bmatrix} 0 & 1 \ -\frac{g}{L} & 0 \end{bmatrix} $，特征值为 $ \lambda = \pm j\sqrt{g/L} $（纯虚数）。这对应于无阻尼振荡，属于临界情况。
在最高点 $ \theta=\pi $： $ J = \begin{bmatrix} 0 & 1 \ \frac{g}{L} & 0 \end{bmatrix} $，特征值为 $ \lambda = \pm \sqrt{g/L} $（一正一负）。因此该平衡点是不稳定的。

通过添加阻尼控制（如 $ u = -K_d \dot{\theta} $）可以使最低点变得稳定。

总结

本节课我们一起回顾了动力学系统的基础知识。我们学习了：

动力学系统的一般微分方程形式 $ \dot{x} = f(x, u) $。
具有重要结构的仿射控制系统 $ \dot{x} = f_0(x) + B(x)u $。
机械臂中常见的拉格朗日动力学形式。
线性系统及其作为非线性系统近似工具的重要性。
如何定义和求解系统的平衡点。
如何通过线性化和分析雅可比矩阵特征值来判断平衡点的稳定性。

这些概念是后续学习最优控制与强化学习算法的基石。在下节课中，我们将探讨离散时间动力学以及数值积分方法，这是将连续时间模型转化为计算机可处理形式的关键步骤。

3：动力学离散化与稳定性 🎯

概述

在本节课中，我们将学习如何将连续时间的动力学系统（常微分方程）转换为离散时间模型，以便在计算机中进行仿真和算法实现。我们还将探讨离散化对系统稳定性的影响，并介绍几种常见的数值积分方法。

回顾：连续时间动力学与稳定性

上一节我们介绍了连续时间动力学系统、平衡点以及稳定性概念。特别是，我们讨论了局部稳定性，它要求系统雅可比矩阵特征值的实部严格为负。

公式：对于连续时间系统 dx/dt = f(x)，在平衡点 x* 处线性化得到 dx/dt ≈ A * (x - x*)。若 A 的所有特征值实部 Re(λ) < 0，则系统局部稳定。

本节中，我们将重点转向离散时间领域，看看当我们将这些连续方程转化为计算机可以处理的离散步骤时，会发生什么。

为什么需要离散化？

在工程实践中，我们通常无法解析地求解这些非线性微分方程。因此，我们需要通过数值模拟来预测系统行为。此外，计算机本质上是离散的，我们需要用有限维的离散点序列来表示连续的状态轨迹 x(t)。

离散时间模型在某些方面甚至比连续时间ODE更通用。例如，碰撞、摩擦等非光滑动力学效应无法用光滑的ODE描述，但可以用离散时间映射来刻画。

显式离散时间动力学系统

首先，我们讨论显式形式的离散时间系统。其一般形式为：

公式：x_{k+1} = f_d(x_k, u_k)

其中，k 是离散时间步索引，f_d 是离散动力学函数。

前向欧拉积分法

以下是实现离散化最简单（但通常效果不佳）的方法：

公式：x_{k+1} = x_k + h * f(x_k, u_k)

这里，h 是时间步长。这个公式直观地使用当前状态的导数来估计下一个状态。

为了演示，我们将其应用于一个简单的无阻尼摆系统（质量 m=1，长度 L=1）。从接近下平衡点（角度 0.1 弧度，速度 0）的初始条件开始，使用 h=0.1 秒进行仿真。

代码示例（概念）：

def forward_euler_step(xk, h, dynamics_func):
    return xk + h * dynamics_func(xk)

仿真结果显示，摆的振荡幅度不断增长，最终“爆炸”——这与物理事实（应保持恒定振幅振荡）相悖。即使将步长减小到 h=0.01，这种不稳定的增长依然会发生，只是时间被推迟了。

离散时间系统的稳定性分析 🧐

上一节我们介绍了连续时间系统的稳定性条件。本节中我们来看看离散时间系统有何不同。

在离散时间中，我们可以将动力学视为一个迭代映射。为了分析线性化系统在平衡点（假设为原点）附近的稳定性，我们考察其雅可比矩阵 A_d。

经过 n 步迭代后，状态变化与 A_d^n 相关。稳定性要求对于所有初始状态 x_0，当 k → ∞ 时，A_d^k * x_0 → 0。这意味着矩阵 A_d^k 必须趋于零。

这引出了离散时间稳定性的关键条件：

核心结论：离散时间系统局部稳定的充要条件是，其雅可比矩阵 A_d 的所有特征值的模长（绝对值）必须小于1。在复平面上，这意味着所有特征值都必须位于单位圆内。

应用于前向欧拉法

对于前向欧拉法，离散雅可比矩阵为：
公式：A_d = I + h * A
其中 A 是连续时间系统在平衡点的雅可比矩阵。

对于我们的无阻尼摆，在平衡点处计算 A_d 的特征值。当 h=0.1 时，特征值模长大于1，这从理论上解释了仿真中观察到的发散行为。对于无阻尼系统，前向欧拉法使用任何有限步长都会导致数值不稳定。

这种不稳定性是积分器人为引入的，并非物理系统的真实属性。积分器使用的分段线性近似持续高估了变化率，导致能量不断被添加到系统中。

重要启示：

离散化过程可能定性改变动力学的行为。
必须始终谨慎检查仿真结果。
检查能量行为是一个很好的方法：对于保守系统，总能量应守恒；对于耗散系统，能量应衰减。

更好的数值积分方法

鉴于前向欧拉法的问题，我们显然需要更好的方法。

四阶龙格-库塔法（RK4）🏃‍♂️

这是工程和科学计算中的标准方法。其思想是在每个时间步内对动力学进行四次评估，从而拟合一个三次多项式，而非线性段。

公式：

k1 = h * f(x_k, u_k)
k2 = h * f(x_k + k1/2, u_{mid})
k3 = h * f(x_k + k2/2, u_{mid})
k4 = h * f(x_k + k3, u_{k+1})
x_{k+1} = x_k + (k1 + 2*k2 + 2*k3 + k4) / 6

对同一个摆系统使用RK4（h=0.1）进行仿真，结果表现出完美的等幅振荡，与物理预期一致。计算其离散雅可比矩阵的特征值，发现模长非常接近1（例如 0.999999...），这表明RK4在保持系统稳定性方面非常出色。

然而，RK4也并非完美。如果我们绘制特征值模长随步长 h 变化的曲线，会发现：对于小步长，它几乎精确为1；但随着步长增大，特征值模长会先小于1（导致人为阻尼），超过某个阈值后则大于1（导致人为能量增长）。

结论：即使像RK4这样高级的积分器，也存在潜在的“陷阱”。必须根据具体问题谨慎选择步长，并始终进行合理性检查。

隐式积分方法

接下来，我们简要介绍隐式方法。其一般形式为：

公式：x_{k+1} = f_d(x_{k+1}, x_k, u_k, u_{k+1})
状态 x_{k+1} 隐含在方程两侧，通常需要求解非线性方程（如使用牛顿法）才能得到。

后向欧拉法

最简单的隐式方法是后向欧拉法：

公式：x_{k+1} = x_k + h * f(x_{k+1}, u_{k+1})

对摆系统应用后向欧拉法（h=0.1），仿真显示振荡幅度不断衰减。这与前向欧拉法正好相反：后向欧拉法引入了人为阻尼。

虽然这不物理，但这种数值阻尼特性使得隐式方法在需要大时间步长或仿真“刚性”系统时非常有用，因为它能防止仿真爆炸。因此，隐式方法常用于计算机图形学、游戏引擎和一些机器人仿真器中。

在轨迹优化中，由于所有时间步的状态和输入是同时求解的，解隐式方程并不构成额外负担，因此隐式方法也被广泛使用。

控制输入的离散化

到目前为止，我们主要讨论了状态 x(t) 的离散化。控制输入 u(t) 的离散化同样重要，但通常更简单，因为我们可以主动选择其表示形式。

零阶保持

这是最简单的方法：在整个时间步内保持控制输入为常数。

公式：u(t) = u_k，对于 t ∈ [t_k, t_{k+1})

实现简单，但如果真实的控制信号是光滑的，则需要很多采样点才能准确近似。

一阶保持（分段线性插值）

在相邻采样点之间进行线性插值。

公式：u(t) = u_k + ( (u_{k+1} - u_k) / h ) * (t - t_k)，对于 t ∈ [t_k, t_{k+1})

如果控制信号本身是连续或光滑的，这种方法可以用更少的节点（采样点）获得更好的近似，从而减小优化问题规模，加快求解速度。它在许多标准的直接配点法中是默认选择。

高阶插值

可以使用更高阶的多项式（如样条）进行插值。这对于轨迹非常光滑的问题（如航天器轨道）是有利的。然而，在机器人控制中，由于执行器饱和限制，控制信号常常是“砰砰”型的非光滑信号，高阶多项式近似效果不佳，甚至可能因龙格现象而产生振荡。

核心要点：选择控制参数化方式时，需权衡近似精度、问题复杂度和实际控制信号的特性。

总结

本节课我们一起学习了以下核心内容：

离散化的必要性：为了在计算机中仿真和求解连续动力学系统。
离散时间稳定性：系统稳定的条件是雅可比矩阵特征值位于复平面的单位圆内（模长<1）。
数值积分器的影响：
- 前向欧拉法：简单但不稳定，会人为增加系统能量，应避免使用。
- 龙格-库塔法（RK4）：精度和计算成本的良好折衷，是标准选择，但仍需注意步长选择。
- 隐式方法（如后向欧拉）：更稳定，能处理刚性系统，但计算成本更高；在轨迹优化中很有用。
控制输入离散化：介绍了零阶保持和一阶保持方法，选择取决于控制信号的光滑性和问题需求。
核心教训：离散化总会引入误差，可能改变系统行为（如稳定性、能量特性）。必须对仿真结果进行合理性检查，例如验证能量是否按物理规律守恒或耗散。

通过理解这些离散化工具及其特性，我们为后续学习基于这些离散模型的优化和控制算法打下了坚实基础。

4：数值优化的基础知识

概述 📚

在本节课中，我们将学习数值优化的基础知识，这是后续最优控制和强化学习内容的重要铺垫。我们将从导数符号约定开始，然后深入探讨两种核心的数值算法：求根算法和无约束最小化算法。

符号约定 📝

上一节我们介绍了课程概述，本节中我们来看看数值优化中一个基础但至关重要的部分：符号约定。我们将统一导数、雅可比矩阵、梯度和海森矩阵的表示方法，以确保后续公式推导和代码实现的清晰与正确。

首先，我们定义函数 F(x)，它是一个标量值函数，输入是 R^n 维向量，输出是一个标量。这类函数通常是我们希望优化的目标，例如损失函数或成本函数。

对于此类函数，其梯度（或称一阶导数）我们记为 ∂F/∂x。在本课程中，梯度始终是一个行向量（1×n）。许多教材将其写作列向量，但采用行向量有两大优势：

维度一致性：在一阶泰勒展开 F(x + Δx) ≈ F(x) + (∂F/∂x) Δx 中，Δx 是 n×1 的列向量。为了使结果 F(x + Δx) 为标量，∂F/∂x 必须为 1×n 的行向量，矩阵乘法才能成立。
链式法则：保持此约定能确保链式法则在矩阵乘法中自然成立，无需额外转置，从而避免代码实现中的常见错误。

类似地，对于一个向量值函数 G(y)（输入 R^m，输出 R^m），其雅可比矩阵 ∂G/∂y 的维度是 m×m。这同样由其一阶泰勒展开的维度匹配所决定。

为了兼顾其他惯例的便利性，我们也定义以下符号：

∇F(x)：表示梯度 ∂F/∂x 的转置，即列向量形式。
∇²F(x)：表示海森矩阵（Hessian），即梯度的导数，∂/∂x (∇F(x))。这是一个 n×n 的对称矩阵。

求根算法 🔍

在明确了符号约定后，我们进入第一个核心算法主题：求根。求根问题在动力学系统分析中非常常见。

求根问题的形式是：给定函数 F(x)，寻找 x* 使得 F(x*) = 0。例如，在连续时间动力学系统中，寻找平衡点（ẋ = F(x) = 0）就是一个求根问题。

一个紧密相关的概念是不动点问题：寻找 x* 使得 F(x) = x。这对应于离散时间动力学系统（x_{k+1} = F(x_k)）的平衡点。两者可以轻松转换（例如，将不动点问题改写为 F(x) - x = 0）。

以下是两种求解方法：

不动点迭代法

这是一种简单直接的方法，适用于寻找稳定不动点。

算法描述：从初始猜测开始，反复将当前值代入函数，即迭代计算 x ← F(x)，直到 x 的变化可忽略不计。其原理是，如果不动点是稳定的，系统动力学本身就会将状态吸引至该点。

特点：

仅对稳定不动点有效。
收敛速度可能很慢。

牛顿法

牛顿法是求根问题的标准方法，具有极快的收敛速度。其核心思想是利用当前点的一阶泰勒展开来逼近函数，并求解该线性近似方程的根。

给定当前点 x，函数的一阶泰勒展开为：
F(x + Δx) ≈ F(x) + (∂F/∂x) Δx

我们希望找到 Δx 使得 F(x + Δx) = 0。令近似式为零：
F(x) + (∂F/∂x) Δx = 0

求解 Δx：
Δx = - (∂F/∂x)^{-1} F(x)

然后更新当前点：
x ← x + Δx

重复此过程直至收敛。

牛顿法的特点：

二次收敛：在解附近，每次迭代有效数字的位数大约翻倍，能快速达到机器精度。
需要导数：需要计算函数值 F(x) 及其雅可比矩阵 ∂F/∂x。
可能失败：需要初始猜测靠近真解；雅可比矩阵需可逆；函数需要足够光滑（至少一阶导数连续）。

代码示例（应用于隐式积分）：
在课程演示中，我们使用牛顿法求解后向欧拉法（一种隐式积分方法）中的非线性方程。我们将离散动力学方程 x_{k+1} = x_k + h F(x_{k+1}) 转化为求根问题 R(x_{k+1}) = x_k + h F(x_{k+1}) - x_{k+1} = 0，并对残差函数 R 应用牛顿法。与不动点迭代法相比，牛顿法仅需3次迭代即可达到高精度，而后者需要14次，展示了牛顿法收敛速度的优势。

无约束最小化 📉

在掌握了求根算法后，我们转向优化领域的核心：无约束最小化问题。本节将学习如何将最小化问题转化为求根问题，并应用牛顿法求解。

我们考虑最小化一个标量值函数 F(x)：
min_x F(x)

假设 F(x) 是光滑的（具有连续的一阶和二阶导数）。根据微积分，局部最小点的一阶必要条件是梯度为零：
∇F(x*) = 0

因此，我们可以将最小化问题转化为对梯度函数 ∇F(x) 的求根问题。这正是牛顿法可以处理的类型。

对梯度函数 ∇F(x) 应用牛顿法：

在当前点 x 对梯度进行一阶泰勒展开：
∇F(x + Δx) ≈ ∇F(x) + ∇²F(x) Δx
其中 ∇²F(x) 是海森矩阵。
希望找到 Δx 使得 ∇F(x + Δx) = 0，故令近似式为零：
∇F(x) + ∇²F(x) Δx = 0
求解 Δx：
Δx = - [∇²F(x)]^{-1} ∇F(x)
更新当前点：
x ← x + Δx

直观理解：该步骤等价于用二阶泰勒展开局部逼近 F(x)，然后精确地最小化这个二次近似函数。

示例与问题：
通过一个多项式函数的例子，我们发现朴素的牛顿法（直接应用于梯度）会收敛到最近的驻点，可能是极小值、极大值或鞍点。这不符合我们“最小化”的初衷。

确保下降方向：
观察牛顿步 Δx = - H^{-1} g（其中 H = ∇²F, g = ∇F）。在标量情况下，-g 指向下降方向，乘以 H^{-1} 可视为步长。要保证下降，需要 H^{-1} > 0，即 H > 0（海森矩阵正定）。
在向量情况下，这一条件推广为：海森矩阵 H 必须正定，才能保证 Δx 是下降方向。如果函数的海森矩阵处处正定，则该函数是强凸函数，牛顿法总能找到全局最小值。但对于机器人等非线性系统，问题通常是非凸的，海森矩阵可能不定。

正则化（海森矩阵修正）：
为了解决海森矩阵非正定导致的上升问题，我们引入正则化技巧。核心思想是修正海森矩阵，使其变得正定。

算法步骤：

计算当前点的海森矩阵 H 和梯度 g。
检查 H 是否正定。如果不是，则循环执行：
H ← H + β I
其中 β > 0 是一个标量参数，I 是单位矩阵。
当 H 正定后，计算牛顿步：
Δx = - H^{-1} g
更新：
x ← x + Δx

正则化的作用：

保证下降方向：通过使 H 正定，确保步进方向是下降方向。
调整步长：添加 βI 会使 H 变大，从而使其逆 H^{-1} 变小，相当于收缩了步长。当远离最优解、泰勒近似不准确时，采取更保守的小步长是合理的。
计算高效：通过尝试Cholesky分解等方法来检查正定性，比计算完整的特征值分解要高效得多，这对于大规模问题至关重要。

在示例中，对初始点应用正则化后的牛顿法，成功找到了局部极小值，而非之前的局部极大值。

遗留问题：
正则化解决了上升问题，但牛顿法仍可能因步长过大而“超调”，越过最小值。我们将在下节课介绍线搜索方法来控制步长，确保每次迭代都使函数值充分下降。

总结 🎯

本节课我们一起学习了数值优化的基础。

我们首先建立了梯度、雅可比矩阵和海森矩阵的符号约定，强调梯度作为行向量的重要性以保证链式法则和代码简洁性。
接着，我们探讨了求根算法，重点介绍了具有二次收敛速度的牛顿法，并将其应用于隐式积分求解。
然后，我们将最小化问题转化为梯度求根问题，引入了用于无约束最小化的牛顿法。我们发现了朴素牛顿法可能收敛到非极小值点的问题。
为了解决该问题，我们引入了海森矩阵正则化技巧，通过添加 βI 迫使海森矩阵正定，从而保证迭代方向是下降方向，并自动调整步长。

这些内容为我们后续学习最优控制中更复杂的数值优化问题奠定了坚实的基础。在下节课中，我们将介绍线搜索方法，以完善牛顿法，解决步长控制问题。

5：优化（第二部分）🚀

概述

在本节课中，我们将继续学习数值优化方法。我们将首先完成关于线搜索的讨论，然后深入探讨带约束的优化问题，特别是等式约束和不等式约束的处理方法。

回顾与过渡：从无约束优化到线搜索

上一节我们介绍了牛顿法及其在求解无约束优化问题中的应用。我们讨论了如何通过正则化来确保算法在远离解时也能朝正确的方向前进。然而，即使方向正确，步长也可能过大，导致“超调”现象。本节中，我们将介绍一种称为“线搜索”的技术来解决这个问题。

回溯线搜索

线搜索的核心思想是：在计算出牛顿步长 Δx 后，不直接采用全步长，而是通过一个缩放因子 α 来调整步长，确保目标函数值有足够的下降。

以下是实现回溯线搜索（Armijo规则）的步骤：

初始化步长 α = 1（即全牛顿步）。
检查以下条件是否成立：
f(x + αΔx) ≤ f(x) + β * α * ∇f(x)^T Δx
其中，β 是一个很小的正数（如 1e-4 到 0.1），∇f(x)^T Δx 是基于一阶泰勒展开的预期下降量。
如果条件不成立，则将步长 α 乘以一个收缩因子 c（通常为 0.5），即 α = c * α，然后重复步骤2。
一旦条件满足，则更新 x = x + αΔx。

代码示例（概念）：

alpha = 1.0
beta = 0.1
c = 0.5
while f(x + alpha * delta_x) > f(x) + beta * alpha * grad_f(x).dot(delta_x):
    alpha = c * alpha
x = x + alpha * delta_x

直觉：这个条件确保实际函数值的下降与基于当前梯度信息预测的下降基本一致。如果差异过大，说明在当前点的一阶近似不准确，因此需要缩短步长。

注意：在每个牛顿迭代的外层循环中，α 都会重置为1。我们只进行必要的回溯，以找到满足条件的足够大的步长。

总结：结合正则化（保证下降方向）和回溯线搜索（保证合适的步长），牛顿法可以变得非常鲁棒和高效。对于足够光滑的函数，可以证明从任意初始点出发，该方法都能保证找到一个局部极小值。

过渡：从无约束问题到等式约束优化

掌握了处理无约束问题的方法后，我们现在将问题复杂化，引入约束条件。首先，我们来看等式约束优化问题。

等式约束优化问题

问题形式为：

minimize f(x)
subject to c(x) = 0

其中，f(x): R^n → R 是目标函数，c(x): R^n → R^m 是约束函数（包含 m 个等式约束）。

一阶必要条件（KKT条件）

对于等式约束问题，最优解必须满足所谓的KKT条件（Karush-Kuhn-Tucker条件）。其几何直觉是：在解处，目标函数的梯度 ∇f 必须与约束函数的梯度 ∇c 平行。

数学表述：
存在一个拉格朗日乘子向量 λ ∈ R^m，使得：

平稳性：∇f(x) + ∂c/∂x^T λ = 0
（即 ∇f(x) 与 ∇c(x) 的列张成的空间平行）
原始可行性：c(x) = 0

我们定义 拉格朗日函数 L(x, λ) = f(x) + λ^T c(x)。上述KKT条件恰好等价于拉格朗日函数对所有变量（x 和 λ）的梯度为零：

∇_x L(x, λ) = ∇f(x) + ∂c/∂x^T λ = 0
∇_λ L(x, λ) = c(x) = 0

用牛顿法求解KKT系统

现在，我们需要求解一个由 x 和 λ 组成的非线性方程组 ∇L(x, λ) = 0。我们可以直接对其应用牛顿法。

对 ∇L(x, λ) = 0 进行一阶泰勒展开，我们得到以下线性系统（称为 KKT系统）：

[ H(x, λ)   J(x)^T ] [ Δx   ] = - [ ∇_x L(x, λ) ]
[ J(x)       0     ] [ Δλ   ]     [ c(x)        ]

其中：

H(x, λ) = ∇²_xx L(x, λ) 是拉格朗日函数关于 x 的Hessian矩阵。
J(x) = ∂c/∂x 是约束的雅可比矩阵。

求解这个线性系统得到步长 (Δx, Δλ)，然后更新 x = x + Δx, λ = λ + Δλ。

高斯-牛顿近似

计算完整的Hessian矩阵 H(x, λ) 可能很昂贵，因为它包含目标函数的Hessian和约束的二阶导数项（可能涉及三阶张量）。一个常见的简化是高斯-牛顿法，它忽略约束的二阶导数项，仅使用目标函数的Hessian来近似 H(x, λ)。

优缺点：

优点：计算量显著降低，迭代更快。
缺点：收敛速度可能略慢于完整的牛顿法（超线性收敛 vs 二次收敛）。
实践：在最优控制等领域，由于动力学约束通常很复杂，计算其二阶导数非常昂贵，因此高斯-牛顿法非常常用，且通常在实践中表现鲁棒。

总结：对于等式约束优化，我们可以通过构造拉格朗日函数并将其梯度设为零，将其转化为一个非线性方程组，然后用牛顿法求解。高斯-牛顿近似是一种常用且高效的计算简化。

过渡：从等式约束到不等式约束优化

处理了等式约束后，我们面临更普遍的不等式约束优化问题。这类问题在机器人学中非常常见，例如执行器的力矩限制、避障等安全约束。

不等式约束优化问题

问题形式为：

minimize f(x)
subject to c(x) ≥ 0

这里 c(x) ≥ 0 表示向量的每个分量都大于等于零。

一阶必要条件（KKT条件）

不等式约束的KKT条件比等式约束更复杂，它包含了一个“开关”行为：约束可能是“活跃的”（紧贴边界，c_i(x)=0）或“非活跃的”（c_i(x)>0）。

完整的KKT条件：
存在拉格朗日乘子 λ ∈ R^m，使得：

平稳性：∇f(x) - ∂c/∂x^T λ = 0 （注意符号约定，这里使用 - 使得 λ ≥ 0）
原始可行性：c(x) ≥ 0
对偶可行性：λ ≥ 0
互补松弛性：λ_i * c_i(x) = 0，对于所有 i=1,...,m

直觉解释（以物体落在冰面上为例）：

c(x) ≥ 0：物体不能穿透冰面（位置必须 ≥ 0）。
λ ≥ 0：冰面只能给物体向上的支持力（法向力 ≥ 0），不能向下拉。
λ_i * c_i(x) = 0：只有当物体接触冰面时（c_i(x)=0），支持力才存在（λ_i > 0）；如果物体在空中（c_i(x)>0），则支持力为零（λ_i=0）。

求解不等式约束问题的方法

由于互补松弛条件引入了不可微性，直接对KKT条件应用牛顿法变得困难。以下是几种主流方法：

以下是几种处理不等式约束的数值优化方法简介：

有效集法
- 思想：猜测哪些约束是活跃的（c_i(x)=0），将其视为等式约束；哪些是非活跃的（c_i(x)>0），将其忽略。然后求解一个等式约束子问题。如果猜错了，根据最优性条件调整猜测，重复过程。
- 优点：如果有效集猜测准确，速度极快。
- 缺点：需要良好的启发式规则来猜测有效集。
内点法（障碍函数法）
- 思想：将不等式约束 c(x) ≥ 0 用“障碍函数”替换并加入目标函数。例如，使用对数障碍函数：min f(x) - μ * Σ log(c_i(x))。当 c_i(x) 接近0时，-log(c_i(x)) 趋于无穷大，从而阻止迭代点违反约束。参数 μ 逐渐减小以逼近边界。
- 优点：理论性质好，是凸优化问题的黄金标准。
- 缺点：对于非凸问题，需要很多技巧来稳定；每次迭代需要求解一个可能病态的系统。
惩罚函数法
- 思想：将约束违反量作为惩罚项加入目标函数。例如，使用二次惩罚：min f(x) + ρ * Σ ( min(0, c_i(x)) )^2。当约束满足时，惩罚项为0；违反时，产生二次惩罚。参数 ρ 逐渐增大以迫使约束满足。
- 优点：实现简单。
- 缺点：数值病态问题严重，难以获得高精度的约束满足，通常需要非常大的 ρ。

实践中的混合策略：通常，可以先用内点法或惩罚函数法让解接近最优区域，此时能较准确地判断有效集，再切换成快速的有效集法进行“抛光”。许多工业级求解器（如OSQP）都采用此类策略。

总结

本节课中我们一起学习了：

回溯线搜索：通过动态调整步长，防止牛顿法超调，提高收敛鲁棒性。
等式约束优化：引入拉格朗日乘子和拉格朗日函数，将问题转化为求解KKT系统，并可用牛顿法或高斯-牛顿法求解。
不等式约束优化：介绍了更复杂的KKT条件（包含平稳性、原始/对偶可行性和互补松弛性），并概述了有效集法、内点法和惩罚函数法等主要求解思路。

这些内容是构建最优控制与强化学习算法的基础数值工具。在接下来的课程中，我们将把这些优化方法应用到具体的动力学系统控制问题中。

6：优化算法（第三部分）与二次规划 🚀

在本节课中，我们将继续探讨约束优化问题。我们将首先学习一种处理不等式约束的强大算法——增广拉格朗日法，它能有效解决传统惩罚法中的病态问题。接着，我们将深入研究二次规划，这是一种在现代模型预测控制中至关重要的凸优化问题。最后，我们会讨论在约束优化背景下，正则化和线搜索等数值技巧的具体应用。

1. 回顾：惩罚法与KKT条件

上一节我们介绍了约束优化，特别是KKT条件和处理不等式约束的惩罚法。

惩罚法的基本思想是将约束违反转化为目标函数的惩罚项。对于一个最小化问题：

min f(x)
s.t. c(x) >= 0

我们将其转化为无约束问题：

min f(x) + (ρ/2) * [min(0, c(x))]^2

其中 ρ 是惩罚权重。

然而，惩罚法存在一个根本问题：为了获得精确的约束满足（即 c(x) = 0），理论上需要将惩罚权重 ρ 增加到无穷大。但在实际计算中，过大的 ρ 会导致海森矩阵病态，使得牛顿法中的线性系统难以求解，从而无法达到高精度的约束满足。

2. 增广拉格朗日法 🛠️

为了克服惩罚法的缺陷，我们引入增广拉格朗日法。它结合了拉格朗日乘子法和惩罚法的优点。

2.1 基本思想

我们定义增广拉格朗日函数 L_ρ：

L_ρ(x, λ̃) = f(x) - λ̃ᵀ c(x) + (ρ/2) * [min(0, c(x))]^2

注意，这里的 λ̃ 是拉格朗日乘子的一个估计值，而不是原始问题中的精确乘子。

2.2 算法步骤

增广拉格朗日法是一个双层循环算法：

内层循环（固定乘子，优化变量）：
固定当前的乘子估计 λ̃ 和惩罚权重 ρ，求解关于 x 的无约束最小化问题：
```
x* = argmin_x L_ρ(x, λ̃)
```

外层循环（更新乘子和惩罚权重）：
- 更新乘子：根据当前解 x* 的约束违反程度更新拉格朗日乘子估计。对于活跃约束（c(x) < 0），更新规则近似为：
```
λ̃_new = max(0, λ̃_old - ρ * c(x*))
```
  这个更新使得乘子估计逐渐逼近真实的最优拉格朗日乘子。
- （可选）增大惩罚权重：为了加速收敛，可以按一个因子（例如10）增大 ρ：
```
ρ_new = α * ρ_old,  (α > 1)
```
  但即使不增大 ρ，仅通过乘子更新，算法也能收敛到正确解。

2.3 优势

解决病态问题：不需要将 ρ 增加到无穷大，通过乘子更新即可获得精确解，避免了海森矩阵的病态。
对非凸问题鲁棒：相比原始-对偶内点法，增广拉格朗日法对非凸问题和不可行初始猜测的容忍度更高，非常稳健。
收敛速度快：虽然达不到原始-对偶方法的超线性收敛速率，但仍能实现快速的超线性收敛。

3. 二次规划：模型预测控制的基石 📐

二次规划是一类特殊的凸优化问题，其目标函数是凸二次函数，约束为线性（等式或不等式）。它在机器人、航空航天等领域的实时模型预测控制中应用广泛。

3.1 标准形式

一个二次规划问题可以写为：

min (1/2) xᵀ Q x + qᵀ x
s.t. A x <= b
     C x = d

其中，矩阵 Q 是严格正定的，这保证了目标函数是凸的，且问题有唯一解。

3.2 求解与应用

由于其结构的特殊性（凸且约束线性），存在非常高效的专用求解器，可以达到千赫兹甚至更快的求解速度，非常适合在线实时控制。

在本次课程的代码演示中，我们使用增广拉格朗日法求解了一个简单的二维二次规划问题。结果表明，即使不更新惩罚权重 ρ，仅通过外层循环的乘子更新，算法也能快速收敛到精确解。

4. 约束优化中的正则化与线搜索 ⚙️

本节我们将讨论在一般非线性约束优化中，如何应用正则化和线搜索技巧。

4.1 KKT系统的正则化

回顾一下，对于包含等式约束的问题，其KKT条件导出的牛顿步线性系统为：

[ ∇²L  ∇cᵀ ] [ Δx ] = - [ ∇f - λ∇c ]
[ ∇c    0  ] [ Δλ ]     [   -c     ]

这个系统需要具有特定的特征值结构：在解处，与变量 x 相关的部分应有正特征值（对应极小值），与乘子 λ 相关的部分应有负特征值（对应极大值）。这种系统被称为拟定系统。

当牛顿迭代远离解时，这个特征值结构可能被破坏。此时，我们需要对KKT矩阵进行正则化：

在左上角的 ∇²L 块添加正正则项 (+αI)，使其更“正定”。
在右下角的零块添加负正则项 (-αI)，使其更“负定”。
正则化后的系统变为：

[ ∇²L + αI    ∇cᵀ    ] [ Δx ] = - [ ∇f - λ∇c ]
[   ∇c       -αI     ] [ Δλ ]     [   -c     ]

这能确保牛顿步指向正确的鞍点方向（在 x 上下降，在 λ 上上升）。

4.2 约束优化中的线搜索

在无约束优化中，我们通过检查目标函数值是否充分下降来进行线搜索（Armijo规则）。在约束优化中，我们没有单一的目标函数来度量进展。因此，我们需要定义一个价值函数来衡量当前点离解的“距离”。

以下是几种常见的价值函数选择：

KKT残差范数：P(x, λ) = ||∇L, c||。其梯度计算成本高昂，实践中不常用。
目标函数加约束违反惩罚：P(x) = f(x) + ρ * ||min(0, c(x))||₁。这是最常用的方法之一，其梯度计算简单（涉及符号函数），且 ρ 可以调节对目标下降和约束满足的权衡。但 ρ 过大可能导致“Maratos效应”，即由于约束曲率，线搜索回溯过多，收敛极慢。
增广拉格朗日函数本身：P(x) = L_ρ(x, λ̃)。对于使用增广拉格朗日法的求解器，这提供了一个天然的价值函数，计算也相对方便。

在代码演示中，我们比较了使用KKT残差和“目标+1范数惩罚”作为价值函数的效果。后者通过调整惩罚权重 ρ，可以在跟踪约束和快速收敛之间取得平衡。

总结 🎯

本节课我们一起学习了处理约束优化问题的几个核心工具：

增广拉格朗日法：通过结合惩罚项和拉格朗日乘子更新，有效解决了纯惩罚法的病态问题，对非凸问题和不可行初值鲁棒性强，是实践中非常受欢迎的算法。
二次规划：我们了解了这种凸优化问题的标准形式及其在实时控制中的重要性，并看到了用增广拉格朗日法求解QP的实例。
高级数值技巧：
- 正则化：在求解KKT系统时，通过对角块添加正/负正则项，可以保证牛顿步方向的正确性，特别是在问题非凸或远离解时。
- 线搜索：在约束优化中，需要构造标量价值函数（如目标加约束惩罚范数）来引导步长选择，并需注意调节惩罚权重以避免“Maratos效应”。

这些内容构成了解决非线性约束优化问题的实用工具箱，也是我们后续学习最优控制算法的重要基础。下一节课，我们将正式进入轨迹优化和确定性最优控制的主题。

7：确定性最优控制 🎯

在本节课中，我们将学习确定性最优控制的基本概念，包括其问题表述、庞特里亚金最小值原理，并初步了解线性二次型调节器。

概述：控制理论简史

在深入数学细节之前，我们先简要回顾控制理论的发展历程。这有助于理解我们当前所学内容的历史背景。

控制思想的起源可以追溯到17世纪的“最速降线问题”。该问题旨在寻找一个斜坡的形状，使得球在重力作用下从起点滚到终点的时间最短。其解是一条被称为“摆线”的曲线。牛顿在求解过程中，实质上开创了“变分法”的思想，即对函数本身进行优化。

18世纪末，詹姆斯·瓦特发明了“离心调速器”，用于蒸汽机的转速调节。詹姆斯·克拉克·麦克斯韦在1868年首次对其进行了数学分析，奠定了经典反馈控制理论的基础，并发展出频域分析工具。

20世纪上半叶，控制理论在两次世界大战的推动下迅速发展，应用于导弹制导等领域。20世纪50-60年代，随着计算机的出现，“现代控制理论”兴起，其特点是基于状态空间和线性代数的时域方法。鲁道夫·卡尔曼（卡尔曼滤波器）和理查德·贝尔曼（动态规划）是这一时期的代表人物。同时，苏联的列夫·庞特里亚金提出了轨迹优化的数学基础——庞特里亚金最小值原理。

随后，控制理论分支发展，包括自适应控制（处理未知或时变参数）、鲁棒控制（保证模型误差下的稳定性）以及模型预测控制（在线求解最优控制问题）。在机器人学领域，控制方法也从早期的动力学逆解，发展到结合了模型预测控制和动态直觉的现代方法，例如波士顿动力公司的机器人。

当前面临的挑战包括处理接触与足式运动、融合模型控制与数据驱动的强化学习，以及博弈论控制等。

确定性最优控制问题

上一节我们回顾了控制理论的历史脉络，本节中我们来看看确定性最优控制问题的标准数学描述。

确定性最优控制问题旨在寻找最优的状态轨迹 $ x(t) $ 和控制输入轨迹 $ u(t) $，以最小化一个给定的性能指标（成本函数），同时满足系统的动力学约束以及其他可能的约束（如输入限制）。

连续时间形式

在连续时间下，标准问题表述如下：

\[\min_{x(\cdot), u(\cdot)} J = \int_{t_0}^{t_f} l(x(t), u(t), t) \, dt + \phi(x(t_f), t_f) \]

\[\text{subject to:} \quad \dot{x}(t) = f(x(t), u(t), t) \]

\[u(t) \in \mathcal{U} \]

其中：

$ J $ 是总成本。
$ l(\cdot) $ 称为阶段成本，衡量运行过程中的代价。
$ \phi(\cdot) $ 称为终端成本，衡量最终状态的代价。
$ \dot{x} = f(x, u, t) $ 是系统的动力学约束。
$ u(t) \in \mathcal{U} $ 表示控制输入必须属于一个可行的集合（如扭矩上下限）。

这是一个无限维优化问题，因为我们在优化连续时间函数 $ x(t) $ 和 $ u(t) $。在计算机中求解时，我们需要对其进行离散化。

离散时间形式

将时间离散化后，问题转化为有限维优化问题，更便于数值求解。离散时间形式如下：

\[\min_{\{x_k\}, \{u_k\}} J = \sum_{k=0}^{N-1} l(x_k, u_k, k) + \phi(x_N) \]

\[\text{subject to:} \quad x_{k+1} = f(x_k, u_k, k), \quad k = 0, \dots, N-1 \]

\[u_k \in \mathcal{U} \]

其中，$ x_k $ 和 $ u_k $ 是在时间步 $ k $ 的状态和控制输入采样点，通常被称为节点。

关键点：在确定性且模型完全已知的理想情况下，最优解是一个开环轨迹 $ u_k^* $。这意味着如果一切完美，只需按此轨迹执行控制输入即可达到目标。然而，现实中存在模型误差和干扰，因此通常需要结合反馈控制，例如模型预测控制（MPC），即在线快速重解上述优化问题，将当前测量状态作为初始条件。

庞特里亚金最小值原理 🧮

上一节我们定义了最优控制问题，本节中我们来看看一个求解该问题的重要理论工具——庞特里亚金最小值原理。它本质上是该优化问题的一阶必要条件（KKT条件）在最优控制中的特定形式。

我们考虑一个简化的离散时间问题，仅包含动力学约束，而不包含其他不等式约束：

\[\min_{\{x_k\}, \{u_k\}} J = \sum_{k=0}^{N-1} l(x_k, u_k) + \phi(x_N) \]

\[\text{s.t.} \quad x_{k+1} = f(x_k, u_k) \]

首先，引入拉格朗日乘子 $ \lambda_k $（也称为协态），构造拉格朗日函数 $ \mathcal{L} $：

\[\mathcal{L} = \sum_{k=0}^{N-1} \left[ l(x_k, u_k) + \lambda_{k+1}^\top (f(x_k, u_k) - x_{k+1}) \right] + \phi(x_N) \]

为了方便，我们定义哈密顿函数 $ H $：

\[H(x_k, u_k, \lambda_{k+1}) = l(x_k, u_k) + \lambda_{k+1}^\top f(x_k, u_k) \]

利用哈密顿函数重写拉格朗日函数，并经过索引变换后，对 $ x_k $、$ u_k $ 和 $ \lambda_k $ 求导并令其为零，即可得到庞特里亚金最小值原理的条件。

以下是离散时间庞特里亚金最小值原理的核心方程：

状态方程（动力学约束）：
\[x_{k+1} = \frac{\partial H}{\partial \lambda_{k+1}} = f(x_k, u_k) \]

协态方程：
\[\lambda_k = \frac{\partial H}{\partial x_k} = \frac{\partial l}{\partial x_k} + \left( \frac{\partial f}{\partial x_k} \right)^\top \lambda_{k+1} \]

控制最优性条件：
\[u_k^* = \arg\min_{u \in \mathcal{U}} H(x_k, u, \lambda_{k+1}) \]
若无控制约束，则简化为 $ \frac{\partial H}{\partial u_k} = 0 $。

边界条件：
\[\lambda_N = \frac{\partial \phi}{\partial x_N} \]

在连续时间形式下，方程非常相似，只需将差分替换为微分：

\[\dot{x} = \frac{\partial H}{\partial \lambda} = f(x, u) \]

\[\dot{\lambda} = -\frac{\partial H}{\partial x} \]

\[u^*(t) = \arg\min_{u \in \mathcal{U}} H(x, u, \lambda) \]

\[\lambda(t_f) = \frac{\partial \phi}{\partial x(t_f)} \]

重要说明：

协态 $ \lambda(t) $ 需要从终端条件 $ \lambda(t_f) $ 开始反向积分，而状态 $ x(t) $ 则从初始条件 $ x(t_0) $ 开始正向积分。这种“前向-后向”迭代求解的方法被称为间接法或打靶法。
从计算角度看，这类方法本质上是沿着轨迹对控制输入 $ u(t) $ 进行梯度下降。虽然历史悠久，但对于复杂的非线性问题，其数值收敛性和鲁棒性通常不如现代的直接优化方法（如直接转录法结合牛顿型优化器）。

线性二次型调节器初步 📈

前面我们讨论了通用的最优控制问题及其求解原理，本节中我们来看一个最重要且具有解析解的特例——线性二次型调节器。

LQR问题描述如下：对于一个线性离散时间系统，寻找控制律以最小化一个二次型成本函数。

\[\min_{\{x_k\}, \{u_k\}} J = \sum_{k=0}^{N-1} (x_k^\top Q_k x_k + u_k^\top R_k u_k) + x_N^\top Q_N x_N \]

\[\text{subject to:} \quad x_{k+1} = A_k x_k + B_k u_k \]

其中：

$ Q_k, Q_N $ 是半正定矩阵，衡量状态偏离原点的代价。
$ R_k $ 是正定矩阵，衡量控制能量的代价。
$ A_k, B_k $ 是系统矩阵，可以是时变的。

成本函数的意义：该成本函数驱使系统状态 $ x $ 和控制输入 $ u $ 趋向于零。矩阵 $ Q $ 和 $ R $ 的相对大小体现了“快速收敛”与“节省控制能量”之间的权衡。$ R $ 必须正定是为了防止控制能量无成本地无限增大，导致问题病态。

如果所有矩阵 $ A_k = A, B_k = B, Q_k = Q, R_k = R $ 都不随时间变化，则称为时不变LQR，通常用于稳定一个固定的平衡点（如保持倒立摆直立）。若矩阵随时间变化，则称为时变LQR，常用于跟踪一条给定的轨迹（如摆的起摆过程）。

LQR的优美之处在于其最优解可以表示为一个线性的状态反馈控制律 $ u_k^* = -K_k x_k $，其中增益矩阵 $ K_k $ 可以通过求解一个相关的黎卡提方程得到。我们将在后续课程中详细推导。

总结

本节课中我们一起学习了确定性最优控制的核心内容。

我们首先回顾了控制理论从最速降线问题到现代机器人控制的发展简史。
接着，我们给出了确定性最优控制问题的标准数学描述，包括连续时间和离散时间形式，并解释了其解在理想情况下是开环轨迹。
然后，我们深入探讨了庞特里亚金最小值原理，它作为最优控制问题的一阶必要条件，给出了状态、协态和控制输入必须满足的方程组。我们了解了其离散和连续时间形式，以及基于此的间接求解方法。
最后，我们介绍了线性二次型调节器这一经典且重要的特例，它针对线性系统和二次型成本，具有解析解和状态反馈形式。

下一节课，我们将深入探讨LQR的求解细节及其性质。

8：线性二次型调节器 (LQR) 的三种求解方法 🎯

在本节课中，我们将深入学习线性二次型调节器问题。我们将从回顾上节课的内容开始，然后介绍三种不同的求解LQR问题的方法：间接打靶法、二次规划法以及利用矩阵稀疏结构的里卡蒂递归法。通过对比这些方法，我们将理解为什么LQR在控制理论中如此重要和实用。

回顾与问题定义

上一节我们介绍了确定性最优控制问题，并讨论了庞特里亚金最小值原理。本节中，我们来看看一个特殊且极其重要的最优控制问题：线性二次型调节器问题。

LQR问题定义如下：

系统模型：离散时间线性动力学
x_{k+1} = A_k x_k + B_k u_k
目标函数：二次型代价函数
J = \sum_{k=1}^{N-1} (x_k^T Q_k x_k + u_k^T R_k u_k) + x_N^T Q_N x_N
约束条件：矩阵 Q_k 需半正定，R_k 需正定。

LQR之所以被称为控制理论的“皇冠明珠”，是因为它不仅是许多非线性控制问题的良好局部近似，而且存在闭式解，计算上易于处理，并具有丰富的理论扩展。

方法一：间接打靶法（庞特里亚金法）🎯

首先，我们尝试使用上节课介绍的间接打靶法（基于庞特里亚金最小值原理）来求解LQR问题。该方法本质上是在控制输入 u 上进行梯度下降，并利用前向-后向传播高效计算梯度。

以下是该方法的核心步骤：

前向仿真：给定初始状态 x_1 和控制序列猜测 {u_k}，通过动力学方程前向仿真得到状态轨迹 {x_k}。
后向传播：从终端时刻 N 开始反向计算协态变量 λ_k 和控制梯度 Δu_k。
- 终端条件：λ_N = Q_N x_N
- 反向递推：λ_k = Q_k x_k + A_k^T λ_{k+1}
- 控制梯度：Δu_k = -R_k^{-1} B_k^T λ_{k+1}
线搜索更新：使用 u_k^{new} = u_k^{old} + α Δu_k 更新控制序列，其中 α 通过线搜索确定。
迭代：重复步骤1-3直至收敛。

我们以“双积分器”系统为例进行演示。该系统模拟了冰面上滑动的砖块，状态为位置和速度，控制输入为力。其离散时间动力学为：

A = [[1, h], [0, 1]]
B = [[0.5*h^2], [h]]

其中 h 为时间步长。

代码实现表明，该方法可以求解LQR问题，但随着时间步数增加，会出现“梯度消失/爆炸”问题，导致收敛速度极慢，数值稳定性差。因此，尽管其原理简单，但在实践中已较少用于非线性问题。

方法二：二次规划法 📐

上一节我们看到了间接法的局限性。本节中，我们来看看如何将LQR问题重新表述为一个标准的二次规划问题，从而一次性精确求解。

其核心思想是将所有时间步的状态和控制变量堆叠成一个大向量 Z，并将代价函数和动力学约束写成该向量的矩阵形式。

以下是构建QP问题的步骤：

定义决策变量：Z = [u_1, x_2, u_2, x_3, ..., u_{N-1}, x_N]^T
注意：初始状态 x_1 是给定的，不是优化变量。
构造代价矩阵：构建块对角矩阵 H，使得总代价 J = 0.5 * Z^T H Z。
H = blkdiag(R_1, Q_2, R_2, ..., Q_N)
构造约束矩阵：构建矩阵 C 和向量 d，使得所有动力学约束 x_{k+1} = A_k x_k + B_k u_k 可写为 C Z = d。
矩阵 C 具有特定的块带状结构，反映了动力学在时间上的耦合。

形成QP问题：

minimize   0.5 * Z^T H Z
subject to C Z = d

求解KKT系统：上述等式约束QP的KKT条件是一个线性方程组：
```
[ H,   C^T ] [ Z     ] = [ 0 ]
[ C,   0   ] [ λ     ]   [ d ]
```
只需一次矩阵求逆或线性求解，即可得到最优解 Z* 和拉格朗日乘子 λ。

代码演示显示，QP方法只需一次求解即可得到机器精度的精确解，计算速度远快于迭代的间接打靶法。然而，直接构建并求解整个KKT系统，其计算复杂度是时间步数 N 的三次方。对于长时间跨度或高维系统，这仍然可能带来计算负担。

方法三：里卡蒂递归法 🔄

上一节我们将LQR转化为QP并一次性求解。本节中，我们来深入探究该QP中矩阵的稀疏结构，并从中推导出一种极其高效且深刻的求解方法——里卡蒂递归法。

通过仔细观察KKT系统的块带状结构，并利用反向代入的技巧，我们可以逐时间步地求解，从而避免构建庞大的稠密矩阵。

以下是里卡蒂递归法的推导要点和算法：

观察稀疏性：LQR的KKT矩阵是高度稀疏的，具有规则的块三对角结构。
反向代入：从最后一个时间步 N 开始，利用KKT方程逐步消元，反向求解。
推导递归关系：通过代数运算，可以导出两个关键的递归方程，分别计算反馈增益矩阵 K_k 和价值函数矩阵 P_k：
- 初始化：P_N = Q_N
- 反向递归（对于 k = N-1, ..., 1）：
```
K_k = (R_k + B_k^T P_{k+1} B_k)^{-1} (B_k^T P_{k+1} A_k)
P_k = Q_k + A_k^T P_{k+1} A_k - A_k^T P_{k+1} B_k K_k
```
前向仿真得到轨迹：获得所有 K_k 后，从初始状态 x_1 开始前向仿真即可得到最优轨迹：
```
u_k = -K_k x_k
x_{k+1} = A_k x_k + B_k u_k
```

里卡蒂递归法带来了两个革命性的优势：

计算效率：计算复杂度从 O(N^3) 降为 O(N)（关于时间步数线性），非常适合长时间跨度问题。
最优反馈策略：我们得到的不是单一的最优轨迹，而是一个最优反馈控制律 u_k = -K_k x_k。这意味着对于任何初始状态，我们都可以立即应用此策略得到最优控制，而无需重新求解优化问题。这对于抑制扰动、实现鲁棒稳定至关重要。

代码演示验证了该方法与QP方法结果完全一致。此外，对于时不变系统和无限时间视野问题，反馈增益 K_k 会收敛到一个常数矩阵 K_∞，即无限时间LQR的解，可通过专门的函数（如MATLAB中的dlqr）直接计算。

总结

本节课中我们一起学习了线性二次型调节器问题的三种求解方法：

间接打靶法：基于庞特里亚金原理的梯度下降法。概念直观，但数值性能差，不适合大规模或非线性问题。
二次规划法：将LQR转化为等式约束QP，通过求解线性KKT系统一次性得到精确解。比间接法更可靠，但直接求解复杂度高。
里卡蒂递归法：利用KKT系统的稀疏结构，通过反向递归高效求解。它不仅计算复杂度低（线性时间），更重要的是直接给出了最优状态反馈控制律，这是LQR理论的核心与精髓。

LQR的闭式解和反馈形式使其成为控制系统设计和分析中不可或缺的工具，无论是用于局部线性化系统的控制，还是作为更复杂非线性优化方法的子步骤，都发挥着重要作用。

9：最优控制与强化学习 | 第8讲：可控性与动态规划 🧭

在本节课中，我们将学习线性二次型调节器（LQR）的剩余概念，包括无限时域问题、系统可控性，并深入探讨动态规划的原理及其与LQR的联系。

无限时域LQR与稳态解 🔄

上一节我们介绍了有限时域LQR的求解。本节中我们来看看当时间趋于无穷时的情况。

在时间不变的情况下，通过反向递归求解黎卡提方程，我们发现增益矩阵 K 和代价矩阵 P 会迅速收敛到稳态值。对于旨在将系统稳定在固定点的控制问题，我们通常直接使用这些稳态矩阵。

求解无限时域稳态值的一种直接方法是寻找黎卡提方程的固定点。我们可以设 P_k = P_k+1，从而得到一个代数方程来直接求解 P。

以下是求解稳态黎卡提方程的两种方法：

固定点迭代或牛顿法：将其作为非线性方程求解。
专用线性代数方法：MATLAB和Julia等工具内置了高效函数。

例如，在MATLAB中：

dare 函数用于求解离散代数黎卡提方程，得到稳态 P 矩阵。
dlqr 函数可直接给出无限时域的最优增益矩阵 K。

系统可控性 🎯

接下来，我们探讨LQR何时有效，即系统是否能够被控制到期望状态。这引出了可控性的概念。

可控性关注的是，对于任意初始状态，是否存在一系列控制输入，能在有限时间内将系统驱动到原点（或其他任意状态）。这对于LQR的稳定性和黎卡提方程的收敛性至关重要。

考虑一个时间不变系统。通过递归代入线性动力学方程，我们可以将未来状态 x_N 表示为初始状态 x₀ 和所有控制输入 u 的函数：

x_N = A^Nx₀ + [B, AB, A²B, ..., A^N-1B] * [u_N-1; u_N-2; ...; u₀]

我们将矩阵 C = [B, AB, A²B, ..., A^N-1B] 称为可控性矩阵。

为了将系统从任意 x₀ 驱动到原点（即 x_N = 0），我们需要求解关于控制输入 u 的方程。这通常是一个最小二乘问题，其解涉及伪逆：

u = - (C^TC)^-1C^T A^Nx₀

该解存在的条件是矩阵 C^TC 可逆，这要求可控性矩阵 C 是满秩的，即其秩等于系统的状态维度 n。

根据凯莱-哈密顿定理，矩阵 A 的幂次高于其维度的项，都可以表示为较低幂次的线性组合。因此，在构建可控性矩阵时，我们只需考虑到 A^n-1B 项：

C = [B, AB, A²B, ..., A^n-1B]

结论：如果该矩阵满秩（rank(C) == n），则系统是可控的，LQR能够稳定该系统，黎卡提方程也会收敛。

动态规划与贝尔曼最优性原理 🧠

现在，我们从另一个视角——动态规划——来重新审视最优控制问题。其核心思想基于贝尔曼最优性原理。

该原理指出：最优策略具有这样的性质，即无论初始状态和初始决策是什么，剩余决策对于由第一个决策产生的状态来说，必须构成最优策略。

换言之，一条最优轨迹的任意子轨迹，对于以其起点为初始条件的子问题而言，也必须是最优的。这源于时间的因果性：过去的决策影响未来，但未来的决策不能影响过去。

这个看似简单的原理意义深远，它揭示了为什么许多最优控制算法（如庞特里亚金最小值原理和黎卡提方程）都是反向进行的。

最优代价函数与值迭代

基于贝尔曼原理，我们定义最优代价函数（或称值函数）V_k(x)。它表示从时刻 k 的状态 x 出发，执行最优控制策略直到终点的最小总代价。

我们从终点开始反向计算：

终端条件：终点的代价函数就是终端代价本身。对于LQR，V_N(x) = (1/2) x^T Q_N x。我们将其记作 P_N = Q_N。
反向递归（贝尔曼方程）：对于前一时刻 k-1，我们有：
V_k-1(x) = min_u [ L(x, u) + V_k( f(x, u) ) ]
其中 L(x, u) 是单步代价，f(x, u) 是系统动力学。

对于LQR问题，单步代价是二次型，动力学是线性的，且 V_k(x) 被证明始终保持为 x 的二次型函数（V_k(x) = (1/2) x^T P_k x）。将其代入贝尔曼方程并求解关于 u 的最小化问题，我们可以推导出增益矩阵 K_k 和矩阵 P_k 的反向递归更新公式——这正是黎卡提方程。

通过动态规划推导出的 P 矩阵更新公式在形式上是对称的，相比之前从QP推导的版本，数值稳定性更好。

得到最优代价函数 V 后，最优反馈控制律可以通过求解一个单步优化问题获得：
u_k*(x) = argmin_u [ L(x, u) + V_k+1( f(x, u) ) ]

动作值函数

在强化学习中，更常用的是动作值函数（Q函数）。它定义为：
Q_k(x, u) = L(x, u) + V_k+1( f(x, u) )

动作值函数包含了动力学信息。使用Q函数的好处在于，在无模型强化学习中，我们可以直接通过数据学习Q函数，而无需知道系统动力学模型 f(x, u)。

动态规划的挑战与价值 💡

动态规划提供了求解最优控制问题的全局最优解，这是一个强大的理论工具。然而，其直接应用面临维数灾难。

问题在于，对于一般的非线性系统，最优代价函数 V(x) 是定义在高维状态空间上的一个复杂非线性函数，我们很难用简洁的形式表示它。无论是通过网格离散化还是使用函数逼近器，其计算和存储成本都随状态维度指数增长。

因此，精确的动态规划通常只适用于两类问题：

LQR等具有特殊结构的问题（代价函数保持为简单的二次型）。
低维非线性问题（可以相对精确地表示值函数）。

尽管存在维数灾难，动态规划仍然极其重要，原因如下：

近似动态规划与强化学习：我们可以使用函数逼近器（如神经网络）来近似 V(x) 或 Q(x, u)，这构成了现代强化学习的基础。
推广到随机控制：动态规划框架可以自然地扩展到随机最优控制问题（通过引入期望算子），而庞特里亚金最小值原理则难以推广。
拉格朗日乘子的解释：在轨迹优化中出现的拉格朗日乘子 λ，其物理意义是最优代价函数关于状态的梯度，即 λ_k = ∇V_k(x_k)。这揭示了不同方法之间的深刻联系。

总结 📚

本节课中我们一起学习了：

无限时域LQR：通过求解代数黎卡提方程获得稳态反馈增益。
系统可控性：通过检验可控性矩阵的秩，判断LQR是否能够稳定系统。
动态规划原理：基于贝尔曼最优性原理，通过反向递归计算最优代价函数来求解控制问题，并重新推导了LQR的黎卡提方程。
动态规划的价值与局限：作为寻求全局最优的理论框架，它是强化学习的基石，但受限于维数灾难，通常需要借助函数逼近进行近似求解。

动态规划为我们理解最优控制的结构提供了深刻的视角，并架起了通往更广泛优化方法（如强化学习）的桥梁。

10：凸模型预测控制 🎯

在本节课中，我们将学习凸模型预测控制（MPC）。我们将从LQR（线性二次调节器）过渡到处理带约束的控制问题。首先，我们会介绍凸优化的基本概念，然后深入探讨凸MPC的原理、动机和实际应用。

从LQR到带约束的问题

上一节我们介绍了LQR和动态规划。本节中，我们来看看如何将LQR扩展到能处理实际系统中常见的约束，例如执行器的力矩或推力限制。

LQR功能强大，能很好地应用于高度非线性系统。但它假设系统没有任何约束。在实践中，最常见的约束是执行器限制（如力矩或推力限制）。当控制器达到这些限制时，LQR无法对其进行规划，可能导致控制器失效。

这些约束通常很简单，几乎总是可以表述为凸约束或凸集。例如，执行器限制通常是简单的边界约束，推力限制可以表述为锥约束。

约束破坏了之前讨论的Riccati方程等优美的LQR求解方法。但是，如果我们回顾之前讨论的LQR的二次规划（QP）形式，QP可以包含线性不等式约束。因此，虽然无法使用Riccati解和反馈增益矩阵K，但我们仍然可以直接求解QP。只要我们能足够快地求解这个QP，它就是完全可行的，并且可以在线运行。在现代计算能力的支持下，我们通常可以在千赫兹速率下求解这些QP，从而在机器人上实时运行。

凸MPC已经变得非常流行。过去几十年，MPC的应用领域随着计算机速度的提升而不断扩大。最初它用于化工过程控制（更新速率可能每分钟一两次），现在则能用于四足机器人或火箭等高速系统。

凸优化基础

在深入MPC之前，我们需要理解一些凸优化的核心概念。凸优化之所以重要，是因为对于凸问题，任何局部最优解都是全局最优解，并且牛顿法等求解器可以快速、可靠地收敛。

凸集

凸集的定义是：集合中任意两点之间的连线上的所有点也都包含在该集合内。

以下是优化和控制中常用的一些标准凸集示例：

线性子空间：表示为 Ax = b。我们的线性动力学方程就是这种形式。
半空间/盒子/多面体：表示为 Ax ≤ b。多面体是多维空间中的“多边形”。
椭球：表示为二次不等式，如 xᵀPx ≤ 1，其中 P 是正定矩阵。
二阶锥：也称为洛伦兹锥或“冰淇淋”锥。定义为 ||x₂:ₙ||₂ ≤ x₁，其中 x₁ 是向量的第一个元素，x₂:ₙ 是其余部分。

凸函数

凸函数的定义是：其图上方的区域（即上图）构成一个凸集。等价地，连接函数图上任意两点的线段位于函数图像的上方。

凸函数是从 Rⁿ 映射到 R 的函数，通常作为我们要最小化的目标函数或成本函数。

以下是一些常见的凸函数示例：

线性函数： f(x) = cᵀx
二次函数： f(x) = ½ xᵀQx + cᵀx，其中 Q 是半正定矩阵。这在最优控制中是最常用的成本函数。
范数： f(x) = ||x||，任何范数都是凸的。

凸优化问题

凸优化问题是：在凸约束集上最小化一个凸函数。

以下是一些标准的凸优化问题类型，它们都有高效的求解器：

线性规划：线性目标函数，线性约束。
二次规划：二次目标函数，线性约束。这正是LQR的QP形式。
二次约束二次规划：二次目标函数，二次约束（凸的，例如椭球）。
二阶锥规划：目标函数通常是线性的（但可以转化为二次型），约束包含二阶锥约束。

这些问题的层次结构是：LP是QP的特例，QP是QCQP的特例，QCQP可以转化为SOCP。

为什么凸性如此重要？对于凸函数，任何局部最优解都是全局最优解。这意味着不存在虚假的局部最优点。从控制工程的角度来看，牛顿法在这类问题上效果极佳，收敛速度非常快（通常只需5-10次迭代），并且可靠。因此，我们可以为在线求解时间提供可证明的保证，这对于实时控制至关重要。

在实践中，我们通常可以选择凸的成本函数（如二次型）。挑战在于动力学方程通常是非线性的，约束也可能非凸。我们需要通过线性化动力学等技巧来近似处理，以将问题转化为凸形式。

凸模型预测控制

现在，我们正式介绍凸模型预测控制。可以将其视为带约束的LQR。我们通常只能处理线性动力学（因为等式约束必须是线性的），但可以对状态和控制输入施加各种凸约束。

从动态规划到MPC

回顾上一节的动态规划讨论，如果我们知道了“成本-to-go”函数，那么要得到反馈策略，只需要求解一个单步优化问题：

uₖ = argminᵤ [ 单步成本 + 成本-to-go(下一状态) ]

其中包含动力学约束。这正是LQR的推导方式。

对于LQR，如果我们只关心输入约束（如力矩限制），一个直观的想法是直接在这个单步最小化问题中加入对 u 的约束。但这样做效果通常很差，甚至可能导致系统不稳定。原因是，这里使用的“成本-to-go”函数是在无约束情况下计算出来的，它完全忽略了未来的约束，因此对于长期行为是一个很差的近似。

然而，如果系统是稳定的，并且我们的控制策略能将其驱向原点（或参考轨迹），那么在足够远的未来，控制输入应该会脱离约束边界，趋于零。此时，无约束的LQR“成本-to-go”函数就是一个良好的近似。

基于这个直觉，MPC的核心思想是：将动态规划中的单步前瞻扩展为多步前瞻。我们在一个有限时域 H 内进行显式的约束优化，然后在时域末端用一个近似的“成本-to-go”函数来代表更远的未来。这个近似的“成本-to-go”通常就取自无约束LQR的解。

MPC问题表述

标准的凸MPC问题表述如下：

最小化：
∑ₖ₌₁ᴴ⁻¹ [ ½ (xₖ - x_ref)ᵀQ(xₖ - x_ref) + ½ (uₖ - u_ref)ᵀR(uₖ - u_ref) ] + ½ (x_H - x_ref)ᵀP(x_H - x_ref)

满足：

xₖ₊₁ = A xₖ + B uₖ （线性动力学约束）
xₖ ∈ X （凸的状态约束集）
uₖ ∈ U （凸的输入约束集）

其中：

H 是预测时域。
Q 和 R 是状态和控制成本的权重矩阵。
P 是终端成本矩阵，取自无约束无限时域LQR的Riccati解，即“成本-to-go”的近似。
x_ref 和 u_ref 是参考状态和输入（例如悬停状态）。

如果没有任何额外的状态和输入约束，这个MPC问题就完全退化为LQR问题。当系统未激活约束时，MPC与LQR的行为是一致的。

MPC也被称为“滚动时域控制”。在每一个时间步，它都基于当前状态求解一个有限时域的最优控制问题，但只实施第一个控制输入，然后在下一个时间步重复这个过程。

时域与终端成本的重要性

终端成本 V(x) = ½ (x - x_ref)ᵀP(x - x_ref) 作为“成本-to-go”的近似，其质量对MPC性能至关重要。

如果 V(x) 是对真实“成本-to-go”的完美近似，那么只需要 H=1 的时域就足够了。
时域 H 越长，MPC的性能通常越好，并且对终端成本近似的质量依赖越小。
反之，终端成本近似得越好，所需的时域 H 就可以越短。

在实践中，我们需要在性能（更长的 H）和计算负担（更短的 H）之间进行权衡。

实例：平面四旋翼控制

为了具体说明，我们考虑一个平面四旋翼（二维）模型。它有三个状态：x（水平位置）、y（垂直高度）、θ（俯仰角），以及两个控制输入：u₁ 和 u₂（两个螺旋桨的推力）。

其非线性动力学方程为：

mẍ = (u₁ + u₂) sinθ
mÿ = (u₁ + u₂) cosθ - mg
Jθ̈ = (L/2)(u₂ - u₁)

在悬停条件（θ=0, u₁=u₂=mg/2）附近进行线性化，得到线性模型：

Δẍ ≈ -g Δθ
Δÿ ≈ (1/m)(Δu₁ + Δu₂)
Δθ̈ ≈ (L/(2J))(Δu₂ - Δu₁)

我们可以将其写成标准状态空间形式 ẋ = A x + B u。

仿真对比：LQR vs. MPC

我们设置一个场景：让四旋翼从初始位置（水平偏移1米，高度偏移1米）飞往目标位置（高度1米，其他为零）。在没有约束的情况下，LQR和MPC（时域 H=1）的表现几乎完全相同。

然而，当我们将初始水平偏移增加到5米时，情况发生了变化：

LQR 会计算出很大的控制指令，但实际执行器存在上限（例如最大推力为1.2倍悬停推力）。由于LQR无法“感知”这个限制，它仍然命令执行器输出超出能力的推力，导致系统实际响应与预期严重不符，最终可能失控坠毁。
MPC 在优化问题中明确包含了 u_min ≤ u ≤ u_max 的约束。因此，它能“规划”出在推力限制内可行的轨迹，虽然响应可能变慢，但能稳定地到达目标。

利用状态约束保证线性化有效性

在线性化中，我们使用了小角度近似（sinθ ≈ θ, cosθ ≈ 1）。当俯仰角 θ 很大时，这个近似不再成立，基于线性模型的MPC预测会变得不准确。

一个巧妙的技巧是，我们可以为MPC问题增加一个状态约束：|θ| ≤ θ_max，其中 θ_max 是一个保证小角度近似合理的小值（例如0.2弧度）。这样，MPC控制器在规划时就会自动将姿态限制在线性模型有效的范围内，从而获得更可靠、更平滑的控制性能。

总结

本节课中，我们一起学习了凸模型预测控制。

动机：实际机器人系统存在执行器限制等约束，而无约束的LQR无法处理这些约束，可能导致性能下降甚至失败。
凸优化基础：我们回顾了凸集、凸函数和凸优化问题的定义。凸问题的优点是局部最优即全局最优，且可用牛顿法等快速可靠地求解。
MPC原理： MPC通过求解一个有限时域内的带约束优化问题来生成控制指令。它在时域末端使用一个近似的“成本-to-go”函数（通常来自无约束LQR）来代表更远的未来。在每个时间步，只实施优化解的第一个控制输入，然后重新求解。
关键要素：预测时域 H 和终端成本函数 V(x) 是影响MPC性能与计算复杂度的关键设计参数。
实例与应用：通过平面四旋翼的仿真，我们直观展示了MPC在处理执行器约束方面的优势，并介绍了如何利用状态约束来保证模型线性近似的有效性。

凸MPC因其能够显式、最优地处理各种约束，已成为现代机器人学中首选的先进控制方法。

11：非线性轨迹优化 🚀

在本节课中，我们将学习非线性轨迹优化的基本概念和一种在机器人学中广泛使用的核心算法——差分动态规划，也称为迭代LQR。我们将从回顾凸优化开始，逐步过渡到非线性问题的挑战，并详细讲解如何利用动态规划和牛顿法的思想来解决这些问题。

回顾与过渡：从凸优化到非线性问题

上一节我们快速回顾了凸优化和凸模型预测控制的基础。本节中，我们来看看当系统动力学变为非线性时，优化问题会变得多么复杂和有趣。

凸优化的威力与局限

线性化动力学模型在许多情况下效果极佳。如果可行，这通常是首选方案。例如，火箭着陆和足式机器人的标准控制器都大量使用了线性化模型和凸优化（如二阶锥规划）。

火箭软着陆：通过将推力矢量约束（推力大小和万向节角度限制）建模为二阶锥约束，可以将问题转化为凸优化问题。
- 推力约束公式：||[f_x, f_y]||_2 ≤ μ * f_z，其中 f_z 是法向推力，μ 与万向节角度限制相关。
足式机器人接触力：脚与地面间的摩擦力约束同样可以建模为摩擦锥，这也是一个二阶锥约束。
- 摩擦锥公式：||[f_x, f_y]||_2 ≤ μ * f_z，其中 μ 是摩擦系数。

在实践中，为了利用更成熟、更快速的QP求解器，常将圆锥约束线性化为多面体锥进行内近似。

引入非线性动力学的挑战

然而，当我们必须在优化问题中引入非线性动力学时，情况就变得复杂了。问题立即变为非凸，我们无法保证求解器总能收敛，也无法界定收敛所需的迭代次数。这使得在线应用变得有风险，需要进行大量的测试和蒙特卡洛仿真。

尽管如此，非线性MPC在实践中仍有应用，例如在自动驾驶中。成功的关键技巧包括：

热启动：利用上一个时间步的MPC解作为当前优化的初始猜测，通常能快速收敛。
正则化：保证算法能收敛到局部最优解。

但本质上，我们只能获取局部梯度信息，然后“顺坡而下”，无法知晓全局最优解的情况。因此，如果线性方法足够好，就应该使用它。

非线性轨迹优化问题定义

现在，让我们回到确定性最优控制的通用框架，正式定义非线性轨迹优化问题。

在离散时间下，问题形式如下：

目标：最小化总成本
J = ∑_{k=0}^{N-1} l_k(x_k, u_k) + l_N(x_N)

约束：

动力学约束：x_{k+1} = f_k(x_k, u_k) （非线性）
状态与输入约束：x_k ∈ X_k, u_k ∈ U_k （可能非凸）

问题特点：

非凸成本函数：通常仍选择易于处理的二次型。
非线性动力学：f_k 是非线性函数。
非凸约束：集合 X_k 和 U_k 可能非凸。
光滑性假设：通常假设成本和动力学至少是二阶连续可微的，以便计算梯度和海森矩阵，从而应用牛顿法。

差分动态规划与迭代LQR 🧮

DDP/iLQR 是一种结合了动态规划和牛顿法的强大算法。其核心思想是：在动态规划的后向递归中，对“成本至Go”函数进行二阶泰勒展开，并在此基础上计算牛顿更新步长。

算法核心：二阶近似与后向递归

我们首先定义价值函数 V_k(x) 和动作价值函数 Q_k(x, u) 的二阶泰勒展开。

价值函数展开：
V_k(x+δx) ≈ V_k(x) + p_k^T δx + 1/2 δx^T P_k δx
其中 p_k 是梯度，P_k 是海森矩阵。终端条件 p_N 和 P_N 来自终端成本 l_N(x_N)。
动作价值函数展开：
Q_k(x+δx, u+δu) ≈ Q_k(x, u) + [g_x^T, g_u^T] [δx; δu] + 1/2 [δx; δu]^T [G_{xx}, G_{xu}; G_{ux}, G_{uu}] [δx; δu]
其中 g_x, g_u 是梯度块，G_{xx}, G_{xu}, G_{ux}, G_{uu} 是海森矩阵块，且 G_{xu} = G_{ux}^T。

后向递归步骤

从已知的终端成本 V_N 开始，向后递归计算 k = N-1, ..., 0。

最小化动作价值函数：对 Q_k 的近似式关于 δu 求导并令其为零。
∂Q_k/∂(δu) = g_u + G_{uu} δu + G_{ux} δx = 0
求解最优控制增量：
δu_k^* = -G_{uu}^{-1} g_u - G_{uu}^{-1} G_{ux} δx
我们将其定义为：
δu_k^* = d_k + K_k δx
其中：
- 前馈项 d_k = -G_{uu}^{-1} g_u：由梯度项驱动，将系统拉向期望状态。
- 反馈项 K_k = -G_{uu}^{-1} G_{ux}：由海森矩阵项决定，与LQR中的增益类似，用于稳定系统。
更新价值函数：将最优的 δu_k^* 代回 Q_k 的展开式，合并关于 δx 的项，即可得到前一时刻价值函数的参数 p_{k-1} 和 P_{k-1}。
P_{k-1} = G_{xx} + G_{xu} K_k + K_k^T G_{ux} + K_k^T G_{uu} K_k
p_{k-1} = g_x + G_{xu} d_k + K_k^T g_u + K_k^T G_{uu} d_k

这个递归过程与LQR的Riccati方程递归非常相似，只是多了前馈项 d_k。

计算梯度与海森矩阵：向量化技巧

上述递归依赖于计算 g_x, g_u, G_{xx}, G_{xu}, G_{uu}。这些项涉及对动力学模型 f_k(x,u) 的导数。计算 G_{xx} 等项时需要动力学雅可比矩阵 A = ∂f/∂x 和 B = ∂f/∂u 的导数，这会产生三阶张量，计算较为复杂。

为了在标准矩阵库中清晰处理这些运算，我们引入两个工具：

克罗内克积：将矩阵维度“放大”。
向量化算子：将矩阵按列堆叠成一个长向量。

利用 “Vec技巧”，可以将矩阵连乘的导数转化为涉及克罗内克积的向量化运算，从而避免直接处理三阶张量。例如，对于 ABC，有：
vec(ABC) = (C^T ⊗ A) vec(B)

通过这种方法，我们可以系统地计算出所有需要的梯度 g 和海森矩阵块 G。其中，包含三阶张量（即 ∂A/∂x, ∂B/∂x 等）的项最为复杂。

DDP 与 iLQR 的区别

一个关键的实践细节是：

差分动态规划：保留所有项，包括那些由三阶张量贡献的项。这相当于完整的牛顿法。
迭代LQR：忽略上述复杂的三阶张量项。这相当于高斯-牛顿法。

在机器人领域，几乎所有人都使用iLQR。原因在于：

计算简便：忽略张量项大大简化了实现。
数值稳定性：高斯-牛顿近似能保证海森矩阵 P_k 保持半正定，而完整的牛顿法可能产生不定矩阵，需要额外的正则化。
性能相近：对于许多问题，忽略这些高阶项对收敛速度的影响不大。

因此，iLQR 是实践中更受欢迎的选择。

完整算法流程：前向滚动与线搜索

DDP/iLQR 算法是一个迭代过程，每次迭代包含一个后向传递和一个前向传递。

以下是算法的伪代码概述：

初始化：给定初始状态 x_0 和控制序列 {u_k} 的初始猜测。

循环直到收敛：

前向滚动：使用当前控制序列 {u_k}，通过动力学方程 x_{k+1} = f(x_k, u_k) 模拟出状态轨迹 {x_k}，并计算总成本 J。
后向传递：从 k = N 到 1，利用上述递归公式计算反馈增益 {K_k} 和前馈项 {d_k}，同时计算期望成本下降量 ΔJ。
前向传递与线搜索：
- 设置线搜索参数 α = 1。
- 进行新的前向滚动，但使用更新后的控制律：u_k^{new} = u_k^{old} + α d_k + K_k (x_k^{new} - x_k^{old})。
- 使用Armijo条件检查新轨迹的成本 J_{new}：如果 J_{new} > J_{old} + c * α * ΔJ（c 是一个小常数，如 1e-2），则减小 α（例如 α = 0.5α）并重试。
- 接受使成本下降的 α 和对应的新轨迹。
更新：将新得到的控制序列和状态轨迹作为下一次迭代的初始猜测。

算法核心：通过迭代地线性化动力学、求解近似LQR问题、并沿下降方向进行线搜索，最终收敛到原非线性问题的一个局部最优解。

实例演示：Acrobot摆起控制 🤖

为了直观展示iLQR的效果，我们将其应用于一个经典控制问题：Acrobot（双连杆欠驱动摆）的摆起控制。

系统：双连杆，仅在肘部关节有电机驱动，肩部关节被动。
目标：从自然下垂的静止状态，摆动到倒立平衡位置。
方法：使用iLQR算法，成本函数为跟踪倒立状态的二次型，初始控制猜测为加入噪声的零扭矩。

运行算法后，我们可以看到Acrobot成功地从一个随机的初始控制序列开始，通过数次迭代优化，最终学习到能将系统摆起到目标位置的优美轨迹。这生动地展示了iLQR处理非线性、欠驱动系统轨迹优化问题的强大能力。

总结

本节课中我们一起学习了非线性轨迹优化的核心内容：

问题定义：明确了带有非线性动力学和非凸约束的轨迹优化问题形式。
算法核心：深入讲解了差分动态规划及其变体迭代LQR的原理。该算法本质上是动态规划与牛顿法的结合，通过在后向递归中对价值函数进行二阶泰勒展开，并求解一系列近似LQR问题来更新策略。
关键步骤：理解了算法中的后向递归（计算反馈和前馈增益）、前向滚动以及线搜索的重要性。
实践区别：了解了DDP（完整牛顿法）与iLQR（高斯-牛顿法）的主要区别，以及为何iLQR在机器人领域更受青睐。
应用实例：通过Acrobot摆起控制的演示，直观看到了iLQR算法的有效性。

非线性轨迹优化为处理复杂机器人系统提供了强大的工具，尽管其理论保证不如凸优化完善，但在精心实现和大量测试下，已成为许多先进机器人系统的核心技术。

12：微分动态规划（DDP）详解与代码实践 🚀

在本节课中，我们将深入学习微分动态规划（DDP）及其简化版本迭代线性二次调节器（iLQR）。我们将回顾其核心算法，通过代码演示对比不同实现，并探讨如何处理约束等扩展问题。

概述

上一节我们介绍了非线性轨迹优化的基本概念，并推导了动态规划（DP）和迭代线性二次调节器（iLQR）的算法框架。本节我们将在此基础上，深入探讨DDP的细节、扩展应用，并通过丰富的代码示例来加深理解。

算法回顾：DDP与iLQR

DDP是一种用于求解以下无约束轨迹优化问题的高效方法：

问题公式：

min_{x_1...x_N, u_1...u_{N-1}} ∑_{k=1}^{N-1} l_k(x_k, u_k) + l_N(x_N)
s.t. x_{k+1} = f(x_k, u_k)

其中 l_k 是阶段成本，l_N 是终端成本，f 是系统动力学模型。

整个DDP/iLQR框架可以看作是一种利用问题特定结构的、高效的稀疏矩阵求解技术。

反向传播（Backward Pass）

算法的核心是反向传播过程，它类似于标准LQR中的Riccati方程，但包含额外的项。我们从时间步 N 开始，使用终端成本的泰勒展开来初始化价值函数：

初始化：

P_N = ∇² l_N(x_N)  # 终端成本的Hessian矩阵
p_N = ∇ l_N(x_N)   # 终端成本的梯度向量

然后，我们进行标准的动态规划递归。对于时间步 k = N-1, ..., 1，我们最小化动作价值函数 Q_k 的二阶泰勒近似：

动作价值函数近似：

Q_k(δx, δu) ≈ 常数项 + [q_x, q_u]ᵀ [δx; δu] + 1/2 [δx; δu]ᵀ [Q_xx, Q_xu; Q_ux, Q_uu] [δx; δu]

其中，q_x, q_u 是梯度，Q_xx, Q_xu, Q_ux, Q_uu 是Hessian矩阵块。

通过求解这个二次优化问题，我们得到最优控制增量更新：

控制更新公式：

δu_k = -Q_uu⁻¹ * q_u - Q_uu⁻¹ * Q_ux * δx_k
     = d_k + K_k * δx_k

其中 d_k 是前馈项，K_k 是反馈增益矩阵。

接着，我们将这个解代回，得到前一时刻的价值函数参数 P_{k-1} 和 p_{k-1} 的更新公式。如此反复，直到回溯到时间步1，我们就得到了整条轨迹上所有时间步的 d_k 和 K_k。

前向滚动（Forward Rollout）与线搜索

在得到控制策略后，我们进行前向滚动，并结合线搜索来更新轨迹：

算法伪代码：

x‘_1 = x_1  # 初始状态
for k = 1 to N-1:
    u‘_k = u_k - α * (d_k + K_k * (x‘_k - x_k))  # 应用控制更新
    x‘_{k+1} = f(x‘_k, u‘_k)                     # 动力学前向模拟
    J‘ += l_k(x‘_k, u‘_k)                        # 计算新成本
J‘ += l_N(x‘_N)

我们使用Armijo条件进行线搜索：从 α=1 开始，如果新成本 J‘ 没有充分下降（即不满足 J‘ < J - c*α*预期下降量），则按因子 β 减小 α，直到条件满足。

收敛判断： 我们通常检查前馈校正项 d_k 的范数（例如无穷范数）。当这些项足够小时，意味着无法再通过更新控制来改进，算法收敛。

代码演示与实践

现在，让我们通过具体的代码示例来观察算法的运行。我们将比较完整的DDP（牛顿法）和其简化版本iLQR（高斯-牛顿法）在经典控制问题上的表现。

示例一：倒立摆（Cartpole） Swing-up

倒立摆是一个经典的控制基准问题。目标是通过对小车施加力，将自由悬挂的摆杆摆动到直立位置，同时将小车稳定在原点。

以下是实现中的关键点：

自动微分（Automatic Differentiation）：我们使用自动微分来精确计算动力学模型的雅可比矩阵（A, B）甚至二阶导数（张量项），而不是数值微分。自动微分通过重载数值类型和运算符链式法则，能以接近函数本身计算成本的方式，精确到机器精度地计算梯度，这是现代机器学习框架（如PyTorch）的核心。
初始化技巧：对于对称性问题（如向左或向右摆动成本相同），使用全零初始化可能导致梯度消失，陷入平坦点。更好的做法是使用小的随机噪声或一个有偏的微小控制输入来打破对称性。
正则化（Regularization）：在完整的DDP中，由于包含了动力学的二阶（张量）项，动作价值函数的Hessian矩阵 Q_uu 可能变得非正定。我们需要进行正则化（例如向Hessian添加一个单位矩阵的倍数 μI）来保证其正定性，从而确保优化方向是下降方向。而在iLQR中，通常由于省略了这些张量项，Hessian能保持半正定，但出于数值稳定性考虑，有时也需要轻微的正则化。

运行结果对比：

iLQR (Gauss-Newton)：收敛耗时短（例如691次迭代），计算速度快。
Full DDP (Newton)：可能使用更少的迭代次数（例如552次），但由于每次迭代需要计算昂贵的二阶导数，总墙钟时间通常更长。

两者最终都能找到相似的轨迹和成本，验证了iLQR在实践中的高效性。

示例二：Acrobot Swing-up

Acrobot（双连杆欠驱动系统）是另一个经典问题，只有肘部关节有驱动器，肩部关节是被动的。目标同样是摆动到直立状态。

关键观察：

局部最优解：当使用不同的随机噪声初始化控制序列时，算法可能收敛到不同的局部最优解。这些解可能具有相似的成本值，但轨迹形态截然不同（例如，单次泵动与双次泵动）。这是因为非线性轨迹优化本质上是非凸的，存在多个局部极小点。最终收敛到哪个解高度依赖于初始猜测。
实践意义：在许多机器人应用中，我们更关心的是能否安全、动态可行地到达目标，而不是达到全局最优。因此，找到一个“足够好”的局部最优解通常是可接受的。如果确实需要更好的解，可以采用多次随机重启等启发式方法。

DDP/iLQR 的优缺点总结

优点 ✅

高效快速：是已知最快的轨迹优化算法之一，非常适合在线实时应用。
动态可行：每次迭代都进行前向滚动，因此中间解总是满足动力学约束。即使提前终止求解，得到的轨迹也能在机器人上执行。
免费获得跟踪控制器：算法收敛后，得到的反馈增益矩阵 K_k 构成了一个时变LQR（TVLQR）跟踪控制器，可用于处理执行过程中的小扰动。

缺点 ❌

无法原生处理约束：只能处理“成本+动力学”的基本问题形式。对于输入限幅、状态避障等约束，需要额外的技巧（后续讨论）。
不支持不可行初始猜测：初始猜测必须是一条满足动力学的轨迹（可通过前向滚动产生）。不能直接使用一条仅运动学可行但不满足动力学的轨迹作为起点，这有时会限制灵活性。
对复杂环境不友好：在包含复杂障碍物（如迷宫陷阱）的问题上可能表现不佳。
长时域数值问题：对于非常长的轨迹，反向传播过程可能遇到类似深度学习中的“梯度消失/爆炸”问题，导致数值不稳定。

扩展讨论：如何处理约束

由于DDP/iLQR不原生支持约束，实践中发展出多种“技巧”：

1. 输入约束（如扭矩限制）

压缩函数（Squashing Function）：例如使用 tanh 函数将控制输入饱和到 [−U_max, U_max] 范围内，即 ũ = U_max * tanh(u / U_max)。这种方法简单，但会在动力学中引入非线性，可能减慢收敛。
控制受限DDP（Control-Limited DDP）：在反向传播的每一步，将无约束的二次最小化问题替换为一个带框约束的二次规划（QP） 问题，即 min_δu Q_k(δx, δu), s.t. u_min ≤ u_k + δu ≤ u_max。这是更严格和推荐的方法。

2. 状态约束（如避障）

惩罚函数法：在成本函数中添加一个惩罚项，以惩罚违反状态约束的情况。这种方法简单，但为了达到较小的约束违反，需要设置很大的惩罚权重，可能导致数值病态。
增广拉格朗日法（Augmented Lagrangian）：这是更优的方法。它将约束以拉格朗日乘子项（线性）和二次惩罚项的形式加入到成本函数中。这种形式与DDP中使用的二次价值函数近似完美契合，能更稳健、高效地处理约束，达到工程上可接受的精度。

代码演示：带约束的倒立摆
我们使用实验室开发的基于增广拉格朗日法的求解器（ALTRO），在倒立摆问题上施加严格的扭矩限制（±3 N·m）。结果，控制器无法再进行单次大力泵动，而是采用了多次“砰砰（bang-bang）”控制策略，反复在扭矩极限附近切换，最终成功摆起。这个问题的求解仅需约20毫秒，展示了良好实现下算法在线应用的潜力。

其他扩展思路

自由初始状态或参数优化：如果想同时优化系统参数（如机器人尺寸、质量）或初始状态，可以引入一个虚拟的“第0步”，将参数作为额外的控制输入 u_0，并通过一个简单的动力学 x_1 = u_0 将其与后续轨迹连接起来。
自由终端时间：通过将时间也作为优化变量，并引入相应的动力学变换，可以处理时间最优控制等问题。

总结

本节课我们一起深入探讨了微分动态规划（DDP）及其实用变体iLQR。我们回顾了算法的反向传播和前向滚动过程，并通过倒立摆和Acrobot的代码演示，对比了完整DDP与iLQR的性能，观察了非凸优化中的局部最优现象。我们总结了该方法的优缺点，特别是其高效性和动态可行性，以及处理约束时的局限性。最后，我们介绍了处理输入和状态约束的常用技巧，如控制受限QP和增广拉格朗日法，并通过一个带扭矩限制的倒立摆实例展示了约束处理的效果。理解这些核心概念和实用技巧，是应用DDP/iLQR解决实际机器人轨迹优化问题的基础。

13：直接轨迹优化方法 🚀

在本节课中，我们将学习直接轨迹优化方法。我们将首先完成关于动态规划中最小时间/自由时间问题的讨论，然后介绍直接轨迹优化的核心概念，包括非线性规划、序列二次规划，并重点讲解一种经典算法——直接配点法。

📝 概述与回顾

上一节我们讨论了动态规划及其处理约束的细节，但未涉及最小时间或自由时间问题。本节我们将完成这部分内容，并开始介绍直接轨迹优化。

直接轨迹优化是解决最优控制问题的另一类主流方法，与动态规划/迭代LQR等方法并列。其核心思想是将原始最优控制问题直接离散化，并转化为一个非线性规划问题，然后使用现成的求解器进行求解。

⏱️ 最小时间与自由时间问题

这类问题在许多应用中都很常见。最小时间问题的目标是尽可能快地到达目标状态，通常会导致“砰砰”最优控制策略。自由时间问题则适用于我们关心到达某个目标状态，但不确定或不需要预先指定具体时间的情况。

连续时间最小时间问题

经典的连续时间最小时间问题形式如下：
最小化成本函数 $ J = \int_{0}^{T} 1 , dt $，即最小化总时间 $ T $。
同时需要满足动力学约束 $ \dot{x} = f(x, u) $、目标状态约束 $ x(T) = x_{\text{goal}} $ 以及控制输入约束。

离散化处理

在计算机上数值求解时，我们通常使用固定数量的时间节点。为了优化时间，我们不改变节点数量，而是将每个时间步长 $ h_k $ 也作为优化变量（控制输入的一部分）。因此，动力学方程变为：

\[x_{k+1} = F(x_k, u_k, h_k) \]

其中 $ F $ 是龙格-库塔积分步骤。同时，成本函数也需要用矩形法则进行离散化积分：

\[J \approx \sum_{k} h_k \cdot \ell(x_k, u_k) \]

核心注意事项：

将步长 $ h_k $ 作为决策变量后，即使原始动力学是线性的，离散化后的优化问题也是非线性、非凸的。
必须对步长 $ h_k $ 施加约束（例如上下界），以防止求解器利用离散化误差得到无意义的解（如极大、极小或负的步长）。

🔧 直接轨迹优化与非线性规划

直接轨迹优化的基本策略是：转录。即将原始的最优控制问题（包含成本函数、动力学约束和其他约束）直接离散化，然后馈送给一个求解器。

离散化后的问题形式被称为非线性规划：

\[\begin{aligned} \min_{x} \quad & f(x) \\ \text{s.t.} \quad & c(x) = 0 \quad \text{(等式约束，如动力学)} \\ & d(x) \leq 0 \quad \text{(不等式约束，如输入限幅、障碍物)} \end{aligned} \]

其中 $ f(x) $ 是目标函数，$ c(x) $ 和 $ d(x) $ 需要是二阶连续可微的。

由于轨迹优化问题通常有成千上万个决策变量，因此需要使用大规模、现成的NLP求解器，例如开源的 IPOPT 或商业的 SNOPT、KNITRO。

📊 序列二次规划

序列二次规划是许多NLP求解器（如SNOPT）采用的核心策略。

SQP的基本思想

在每次迭代中，对当前点 $ x $ 进行如下操作：

构造拉格朗日函数 $ \mathcal{L}(x, \lambda, \mu) = f(x) + \lambda^T c(x) + \mu^T d(x) $ 的二阶泰勒展开。
将等式约束 $ c(x) $ 和不等式约束 $ d(x) $ 线性化。
得到一个局部二次规划近似子问题：
\[\begin{aligned} \min_{\Delta x} \quad & \frac{1}{2} \Delta x^T H \Delta x + g^T \Delta x \\ \text{s.t.} \quad & A \Delta x + c = 0 \\ & B \Delta x + d \leq 0 \end{aligned} \]
其中 $ H $ 是拉格朗日函数的Hessian矩阵，$ g $ 是其梯度，$ A, B $ 分别是约束 $ c, d $ 的雅可比矩阵。
求解该QP子问题，得到搜索方向 $ \Delta x, \Delta \lambda, \Delta \mu $。
沿搜索方向进行线搜索（使用价值函数），更新迭代点。

SQP与牛顿法的关系

SQP可以看作是牛顿法在处理不等式约束时的推广。如果只有等式约束，SQP便退化为之前讨论的求解KKT系统的牛顿法。不等式约束的存在使得在每一步需要求解一个QP子问题，而不是简单的线性系统。

实现要点：

高效的SQP求解器通常采用有效集策略来求解内部的QP子问题，并利用热启动加速收敛（因为相邻迭代间的有效集通常变化不大）。
必须利用KKT系统的稀疏性才能高效求解大规模轨迹优化问题。

🧩 直接配点法

直接配点法是直接轨迹优化中非常经典和标准的算法，尤其在航空航天领域应用广泛。

从显式积分到隐式积分

在动态规划/迭代LQR中，我们通常使用显式龙格-库塔法（如RK4）进行前向积分，因为每一步计算直接明了。
然而，在直接优化中，动力学是作为约束条件写入NLP的。在这种情况下，使用显式或隐式积分方法在计算上没有本质区别，因为求解器会统一处理这些非线性等式约束。而隐式方法通常具有更好的数值稳定性和精度。

埃尔米特-辛普森配点法

直接配点法的核心是使用多项式样条（如三次样条）来表示状态轨迹，并在配点处强制动力学约束。

算法步骤：

状态：在相邻节点 $ x_k $ 和 $ x_{k+1} $ 之间，用三次埃尔米特样条进行插值。该样条由两端点的状态 $ x $ 和状态导数 $ \dot{x} $ 唯一确定。
控制：在相邻节点 $ u_k $ 和 $ u_{k+1} $ 之间，采用分段线性插值（一阶保持）。
配点：在每个时间段的中点（配点）处，计算样条给出的状态导数 $ \dot{x}_c $。
动力学约束：强制在配点处，由样条计算出的导数 $ \dot{x}_c $ 必须等于真实动力学 $ f(x_c, u_c) $ 在该点计算出的值。这就构成了动力学等式约束。
节点连续性：在节点处，强制相邻样条的状态 $ x $ 和状态导数 $ \dot{x} $ 相匹配，这保证了轨迹的整体连续性。

数学形式（对于第k个区间）：
动力学约束 $ C_k $ 写作：

\[f(x_c, u_c) - \dot{x}_c = 0 \]

其中中点状态 $ x_c $ 和控制 $ u_c $ 由样条公式根据 $ x_k, x_{k+1}, u_k, u_{k+1} $ 计算得出。

优势

计算高效：埃尔米特-辛普森法是三阶精度的隐式龙格-库塔法。关键在于，每个区间端点处的动力学计算 $ f(x_k, u_k) $ 会被相邻区间共享。渐进来看，每个时间步平均只需要约2次动力学函数调用，而相同阶数的显式RK3需要3次。动力学及其雅可比计算通常是主要开销，因此这能节省大量计算。
数值稳定：隐式方法通常比显式方法更稳定，尤其对于刚性系统。
初始猜测灵活：与动态规划不同，直接配点法不要求初始猜测是动力学可行的。可以提供一个粗略的、甚至违反动力学的初始轨迹（例如来自其他规划器的结果），求解器会逐步将其优化至可行且最优。这在实际应用中是一个巨大优势。

💻 代码示例：倒立摆的直接配点法

以下是一个使用直接配点法求解倒立摆摆动上升问题的简化代码框架，体现了上述原理。

# 定义动力学约束（埃尔米特-辛普森形式）
function dynamics_constraint(xk, uk, xkp1, ukp1, dt)
    # 计算中点状态 (xc) 和控制 (uc)
    xc = 0.5 * (xk + xkp1) + (dt/8) * (f(xk, uk) - f(xkp1, ukp1))
    uc = 0.5 * (uk + ukp1)
    
    # 计算中点处的状态导数（来自样条）
    xdot_spline = (3/(2*dt)) * (xkp1 - xk) - 0.25 * (f(xk, uk) + f(xkp1, ukp1))
    
    # 计算中点处的真实动力学
    xdot_dynamics = f(xc, uc)
    
    # 约束：两者必须相等
    return xdot_dynamics - xdot_spline
end

![](https://github.com/OpenDocCN/dsai-notes-pt2-zh/raw/master/docs/cmu-16745-rl/img/9c9489fb46f3c80d70ed8127b0c2752e_382.png)

![](https://github.com/OpenDocCN/dsai-notes-pt2-zh/raw/master/docs/cmu-16745-rl/img/9c9489fb46f3c80d70ed8127b0c2752e_383.png)

![](https://github.com/OpenDocCN/dsai-notes-pt2-zh/raw/master/docs/cmu-16745-rl/img/9c9489fb46f3c80d70ed8127b0c2752e_385.png)

# 构建完整的NLP问题
# 决策变量：所有状态节点 x0, x1, ..., xN 和控制节点 u0, u1, ..., u_{N-1}
# 成本函数：最小化控制能量等
# 等式约束：1. 初始状态固定 2. 每个区间的动力学约束 3. (可选)终端状态约束
# 不等式约束：控制输入上下限
# 调用求解器（如IPOPT）进行求解

运行与热启动：

首次求解可能需要较多迭代。
如果得到一个解后，将其作为下一次求解的初始猜测，收敛会非常快。
即使只提供部分状态变量的粗略猜测（例如，仅给角度一个粗略轨迹，其他状态设为零），求解器也能利用这个信息快速找到可行且最优的解，这体现了其对“不可行初始猜测”的良好处理能力。

🎯 本节总结

本节课我们一起学习了直接轨迹优化方法的核心内容：

最小时间问题：通过将时间步长也作为优化变量，在固定节点数的框架下处理自由时间问题。
直接轨迹优化框架：将最优控制问题离散化为非线性规划问题，并利用现成的大规模求解器求解。
序列二次规划：作为求解NLP的主流方法，通过反复求解局部QP子问题来逼近原问题的最优解。
直接配点法：一种经典高效的直接优化算法。它使用多项式样条表示状态轨迹，并在配点处强制动力学约束。其优势在于计算高效（共享动力学计算）、数值稳定，且对初始猜测的要求非常宽松，允许使用不可行的初始轨迹。

直接轨迹优化与动态规划/迭代LQR是轨迹优化领域两大并行的技术路线，各有优劣，适用于不同的应用场景。理解这两种方法能帮助你为具体问题选择最合适的工具。

13：处理三维旋转 🚀

在本节课中，我们将回顾之前讨论过的确定性最优控制算法，并开始探讨一个在机器人学中非常实际且重要的话题：三维旋转。我们将学习如何用旋转矩阵和四元数表示姿态，以及相关的运动学。

📊 算法回顾：确定性最优控制

上一节我们介绍了序列二次规划和直接配点法等非线性轨迹优化技术。本节中，我们将对目前讨论过的所有算法进行一个总结，并分析它们的优缺点。

我们讨论过的算法主要分为两大类：适用于线性或局部线性化问题的算法，以及用于非线性轨迹优化的算法。

线性或局部控制问题

这类问题适用于动态系统本身是线性的，或者可以通过线性化（例如在参考轨迹或平衡点附近）进行局部控制的情况。以下是决策流程：

首先，判断问题是否有约束（指除动力学方程外的约束，如控制限幅、障碍物等）。

如果没有约束，可以使用 LQR。这是一个闭式解。
- 如果是跟踪问题（跟随一条时变轨迹），则使用时变LQR。
- 如果是镇定问题（稳定在一个固定点），则使用经典的时不变LQR（常值增益矩阵K）。
如果有约束，则需要使用 模型预测控制。
- 如果约束是线性不等式，则问题是一个二次规划。这是目前机器人控制中非常成熟和快速的方法。
- 如果约束是锥形约束（如摩擦锥、火箭软着陆约束），则问题是一个二阶锥规划。这仍处于研究前沿，但也是可行的。

非线性轨迹优化/规划

这类问题用于离线的轨迹设计或规划，特别是当动态高度非线性或存在复杂约束时。我们主要讨论了两种算法：微分动态规划 和 直接配点法。

以下是它们的优缺点比较：

动态可行性：
- 直接配点法：将动力学作为优化问题中的等式约束。只有在求解器收敛到KKT条件的解时，才严格满足动力学。
- DDP：通过前向积分（rollout）始终保证动力学可行性。
初始猜测：
- 直接配点法：可以提供一个不满足动力学的“卡通”轨迹作为初始猜测，可能有助于更快收敛。
- DDP：只能提供控制量的初始猜测，然后通过动力学积分得到状态轨迹。对于高度非线性系统，这可能难以提供一个好的起点。
约束处理：
- 直接配点法：可以相对容易地处理任意形式的约束。
- DDP：处理约束需要额外技巧，如使用压缩函数处理控制约束，或使用惩罚函数/增广拉格朗日法处理状态约束。
额外输出：
- DDP：在反向传播过程中会计算出反馈增益矩阵 K。在收敛时，这些 K 矩阵与时变LQR的结果相同，相当于“免费”得到了一个跟踪控制器。
- 直接配点法：需要单独设计跟踪控制器。
速度与效率：
- DDP：通常以局部收敛速度快、内存效率高著称，更容易在嵌入式系统上实现。
- 直接配点法：通常不如DDP高效，且需要大规模序列二次规划求解器，在嵌入式系统上实现更困难。
数值鲁棒性：
- 直接配点法：使用工业级NLP求解器（如SNOPT、IPOPT），内置了智能的缩放和数值稳定技巧，对长时域和病态问题更鲁棒。
- DDP：可能存在病态条件问题，特别是当变量缩放不当或使用惩罚函数处理约束时。长时域问题可能导致梯度消失/爆炸。

总结：

DDP 通常是在线实时控制的不错选择，特别是当计算速度至关重要，且对约束精度要求不那么极端时。
直接配点法 等直接方法通常是离线轨迹设计的不错选择，特别是当问题具有长时域、高度非线性或复杂约束时。

🧭 为什么需要专门处理三维旋转？

本节我们将转向一个在机器人学中至关重要的话题：三维旋转。许多机器人系统，如四旋翼、飞机、航天器、水下机器人和足式机器人，在运行中都会涉及大范围旋转。

人们常用欧拉角来处理旋转，但这种方法存在奇异性问题（例如，当俯仰角为90度时，方向余弦矩阵映射角速度到欧拉角导数的雅可比矩阵会奇异），可能导致数值计算失败甚至系统崩溃。因此，我们不推荐使用欧拉角。

常见的无奇异性替代方案是旋转矩阵和四元数。

旋转矩阵：总是有效，但用9个（或6个独立）参数表示3个自由度，存在冗余。
四元数：用4个参数表示3个自由度，只有1个约束（单位范数），在动力学和最优控制中更高效，是计算机图形学、航空航天和机器人领域的常用选择。

📐 旋转矩阵基础

姿态通常定义为物体坐标系（B系）到惯性世界坐标系（N系）的旋转。设旋转矩阵为 Q，则有：

v^N = Q * v^B

其中 v^B 是物体坐标系下的向量，v^N 是世界坐标系下的同一向量。

旋转矩阵 Q 属于特殊正交群 SO(3)，满足：

Q^T * Q = I
det(Q) = +1

这意味着 Q 的逆等于其转置，且其列（或行）是两两正交的单位向量。

旋转运动学 描述了如何将角速度 ω 积分得到旋转矩阵。推导可得：

Q_dot = Q * ω_hat

其中 ω_hat 是角速度向量 ω 对应的叉乘矩阵（反对称矩阵）：

ω_hat = [ 0,    -ω_z,  ω_y;
          ω_z,   0,    -ω_x;
         -ω_y,   ω_x,  0 ]

通过此式，结合来自陀螺仪的 ω 测量值，可以对姿态 Q 进行积分（航位推算）。

🧮 四元数：更高效的表示

四元数 q 是一个四维向量，可以看作由一个标量部分和一个向量部分组成：

q = [s, v]^T = [cos(θ/2), a * sin(θ/2)]^T

其中 a 是单位旋转轴向量，θ 是绕该轴旋转的角度。

重要性质：

单位范数：有效的旋转四元数满足 ||q|| = 1。数值上很容易进行归一化。
双覆盖：q 和 -q 代表同一个物理旋转。这是因为将 θ 增加 2π 后，cos(θ/2) 和 sin(θ/2) 会变号。
四元数乘法：对应旋转矩阵的乘法。对于两个四元数 q1 = [s1, v1]^T 和 q2 = [s2, v2]^T，其乘积定义为：
```
q1 * q2 = [ s1*s2 - v1^T*v2,  s1*v2 + s2*v1 + v1 × v2 ]^T
```
这个运算可以写成矩阵形式，定义左乘矩阵 L(q) 和右乘矩阵 R(q)，使得：
```
q1 * q2 = L(q1) * q2 = R(q2) * q1
```
单位四元数：对应于无旋转，q_identity = [1, 0, 0, 0]^T。
共轭四元数：对应于逆旋转，q† = [s, -v]^T。可以定义矩阵 T 使得 q† = T * q。
用四元数旋转向量：要将物体坐标系下的向量 v^B 旋转到世界坐标系，可以构造一个纯向量四元数 [0, v^B]^T，然后进行运算：
```
[0, v^N] = q * [0, v^B] * q†
```
这等价于 v^N = (R(q)^T * L(q)) * v^B，其中括号内的部分就是对应的旋转矩阵。
四元数运动学：四元数随角速度变化的微分方程为：
```
q_dot = (1/2) * q * [0, ω]^T = (1/2) * L(q) * H * ω
```
其中 H 是一个将3维向量提升为4维纯向量四元数的矩阵。这个方程用于结合陀螺仪数据进行姿态积分。

🎯 本节课总结

本节课中我们一起学习了：

对线性/局部控制和非线性轨迹优化的各类算法进行了系统回顾和比较，梳理了它们各自的适用场景和优缺点。
引入了机器人学中至关重要的三维旋转问题，解释了为什么欧拉角存在缺陷。
详细介绍了旋转矩阵的数学定义、性质及其运动学方程 Q_dot = Q * ω_hat。
引入了更高效的四元数表示，解释了其几何意义、核心性质（单位范数、双覆盖）、乘法运算以及运动学方程 q_dot = 0.5 * q * [0, ω]^T。

这些关于旋转的数学工具是描述和优化具有三维旋转自由度系统（如无人机、人形机器人）的基础。下一节课，我们将继续探讨如何在优化问题中处理四元数这类带有约束的状态变量。

15：四元数优化 🚀

在本节课中，我们将深入学习如何利用四元数进行优化，特别是如何将之前学到的确定性最优控制工具（如LQR、MPC、轨迹优化）扩展到处理涉及大角度旋转的问题，例如让机器人完成空翻等复杂动作。

四元数快速回顾 🔄

上一节我们介绍了四元数的基础知识。本节中，我们来看看其核心概念和几何意义，为后续的优化工作打下基础。

首先，四元数是四维单位向量，记作 q，满足 q^T q = 1。

其次，四元数乘法（我们用星号 * 表示）定义了旋转的组合规则，其行为类似于矩阵乘法，不满足交换律。对于两个四元数 q1 = [s1, v1] 和 q2 = [s2, v2]，其乘积为：

q1 * q2 = [s1*s2 - v1^T v2,  s1*v2 + s2*v1 + v1 × v2]

我们可以用左乘矩阵 L(q) 来表示这个乘法，使得 q1 * q2 = L(q1) q2。矩阵 L(q) 定义为：

L(q) = [ s,  -v^T;
         v,  s*I + v^ ]

其中 v^ 是向量 v 的斜对称矩阵（叉乘矩阵）。

此外，我们还有共轭运算 q† = [s, -v]（类似于矩阵转置），以及将三维向量提升为纯向量四元数的 H 矩阵：H = [0; I]。

四元数的几何与运动学 📐

上一节我们介绍了四元数的代数运算。本节中，我们来探讨其几何意义和运动学，理解导数所在的“空间”。

四元数 q 是四维空间中的单位向量，其集合构成一个三维球面（3-sphere）。关键点在于：虽然 q 本身是四维的，但其时间导数 q̇ 并不生活在这个球面上，而是生活在球面在当前 q 点处的三维切平面上。

这与角速度 ω（一个三维向量）的概念相联系。四元数运动学方程为：

q̇ = (1/2) * q * [0, ω] = (1/2) * L(q) * H * ω

这个方程的作用是：将定义在单位四元数（“北极”或恒等元）切平面上的角速度 ω，通过左乘 q “旋转”到当前姿态 q 所在的切平面上，从而得到正确的 q̇。

核心概念：在进行优化时，姿态用四维四元数表示，但所有导数（速度、梯度等）本质上是三维的切向量。

对四元数函数求导 📈

上一节我们理解了导数的几何意义。本节中，我们来看看如何实际计算涉及四元数的函数的梯度、雅可比矩阵和黑塞矩阵。

由于旋转通过乘法组合（而非加法），我们需要用“无穷小旋转”来定义扰动。我们使用一个三维扰动向量 φ（可以是轴角、四元数向量部分等）。对于小扰动，扰动后的四元数可写为：

q_new ≈ q * ( [1, 0] + (1/2) * H * φ ) = q + (1/2) * L(q) * H * φ

我们定义姿态雅可比矩阵 G(q) = (1/2) * L(q) * H，它是一个 4x3 的矩阵，将三维扰动 φ 映射为四维的四元数扰动。

利用 G(q)，我们可以推导出各种导数公式：

标量函数 f(q) 的梯度（例如成本函数）：
```
∇f(q) = (∂f/∂q) * G(q)
```
其中 ∂f/∂q 是将 q 视为普通 4D 向量求得的 1x4 梯度。

四元数函数 F(q) 的雅可比矩阵（例如动力学方程）：
```
J_F(q) = G(F(q))^T * (∂F/∂q) * G(q)
```
这是一个 3x3 矩阵，描述了输入切空间扰动 φ 如何映射到输出切空间扰动 φ'。

标量函数 f(q) 的黑塞矩阵（用于牛顿法）：
```
∇²f(q) ≈ G(q)^T * (∂²f/∂q²) * G(q)   (高斯-牛顿近似)
```
完整的牛顿黑塞矩阵还包括一个额外项，但通常高斯-牛顿近似已足够。

核心思想：对任何涉及四元数的函数求导时，先按常规方法对四元数（视为向量）求导，然后在适当的位置乘上姿态雅可比矩阵 G(q)。

实例：Wahba问题与高斯-牛顿法 🛰️

上一节我们掌握了求导工具。本节中，我们通过一个经典问题——Wahba问题（姿态确定）来实践如何用高斯-牛顿法优化四元数。

问题描述：已知惯性系中 M 个特征向量的方向 x_i^N，以及在机体坐标系中测量到的对应方向 x_i^B。求最优的四元数 q（及其对应的旋转矩阵 R(q)），使得以下最小二乘损失最小：

min_q J(q) = (1/2) * Σ_i || R(q) * x_i^B - x_i^N ||^2

我们可以将所有的误差项堆叠成一个残差向量 r(q)，目标即最小化 r(q)^T r(q)。

以下是使用高斯-牛顿法求解的步骤：

初始化：猜测一个初始单位四元数 q_0。
迭代直至收敛：
a. 计算当前残差 r(q_k) 及其雅可比矩阵 J_r(q_k) = (∂r/∂q) * G(q_k)。
b. 求解高斯-牛顿步长 φ（三维向量）：
```
[J_r(q_k)^T * J_r(q_k)] * φ = -J_r(q_k)^T * r(q_k)
```
c. 将三维扰动 φ 转换为单位四元数增量 Δq(φ)（例如，使用 Δq ≈ [1, φ/2] 然后归一化，或精确的指数映射）。
d. 通过乘法更新四元数：q_{k+1} = q_k * Δq(φ)。
e. （可选）进行线搜索以确保成本下降。
收敛判断：当残差 ||r(q_k)|| 或步长 ||φ|| 足够小时停止。

重要提示：由于四元数的“双覆盖”性质（q 和 -q 代表同一旋转），优化可能会收敛到两者之一，这取决于初始猜测。在设计实际控制器时需要考虑这一点，以避免不必要的全局旋转（“解绕问题”）。

总结 🎯

本节课中我们一起学习了如何将优化技术应用于四元数。

我们首先回顾了四元数的代数与几何，明确了导数存在于三维切空间这一关键概念。
接着，我们引入了姿态雅可比矩阵 G(q)，它提供了将三维扰动与四维四元数联系起来的标准方法，并给出了梯度、雅可比矩阵和黑塞矩阵的计算公式。
最后，我们通过Wahba问题的实例，演示了如何使用高斯-牛顿法求解一个基于四元数的非线性最小二乘问题，并强调了乘法更新和双覆盖特性等实践细节。

掌握这些工具后，你现在可以将之前学到的轨迹优化、模型预测控制等方法，扩展到包含复杂三维旋转的机器人运动规划与控制问题中了。

15：基于四元数和四旋翼的LQR控制 🚁

在本节课中，我们将继续探讨四元数的应用，并学习如何将四元数技巧融入线性二次调节器（LQR）框架中。我们将以四旋翼飞行器为例，展示如何正确处理姿态动力学以实现稳定控制。掌握了这些方法后，您就能将四元数技巧应用到我们讨论过的任何其他控制方法中，例如迭代LQR或动态规划。

概述

上一节我们介绍了四元数微分和线性化的技巧。本节中，我们将把这些技巧应用到动力学系统的线性化过程中，特别是针对包含四元数姿态的状态向量。我们将看到，如果对包含四元数的动力学系统进行“朴素”的线性化，会导致系统不可控，从而无法使用标准的LQR方法。通过引入姿态雅可比矩阵，我们可以将问题转化为一个可控的、降维的线性系统，从而顺利应用LQR。

朴素线性化的问题

首先，回顾一下我们之前讨论过的可控性。对于一个线性系统，我们可以通过检查可控性矩阵的秩来判断LQR的里卡蒂方程是否会收敛。

如果动力学模型中包含四元数，并且我们以最朴素的方式（将其视为4维向量）进行线性化，得到的A矩阵总是奇异的，其秩始终为3。这是因为四元数实际上只代表3个自由度。因此，由此得到的可控性矩阵也总是秩亏的。从理论上讲，这意味着线性系统总是不可控的，因此无法计算无限时域的LQR增益矩阵K。我们稍后在代码部分会看到这个问题的数值表现。

核心问题公式：
对于一个状态包含四元数 q 的系统，朴素线性化得到的 A 矩阵（4维）是奇异的：
rank(A_naive) = 3

解决方案：应用姿态雅可比矩阵

为了解决上述问题，我们需要将上一节学到的四元数微分技巧应用到动力学系统的线性化中。基本思路是：利用姿态雅可比矩阵，将四元数部分的误差从4维映射到3维，从而得到一个可控的、降维的线性系统。

我们假设有一个参考轨迹（或一个固定点），状态向量通常包含位置、四元数姿态以及其他状态（如速度、关节角等）。我们希望在四元数部分使用3维参数（如旋转向量 φ）来表示误差，而不是使用4维的四元数差。

定义误差状态：
我们想要的误差状态 Δx̃ 结构如下：

Δx̃ = [ Δp (3维位置误差)
        φ   (3维姿态误差，来自四元数误差映射)
        Δz  (其余状态误差) ]

为了得到这个降维系统的雅可比矩阵（Ã 和 B̃），我们需要在原始线性化矩阵 A 和 B 的适当位置插入姿态雅可比矩阵 G(q) 及其转置。

降维雅可比矩阵公式：

Ã = E(x̄_{k+1})^T * A_naive * E(x̄_k)
B̃ = E(x̄_{k+1})^T * B_naive

其中，矩阵 E 是一个分块矩阵，它在四元数对应的位置放置姿态雅可比矩阵 G(q)，在其他位置放置单位矩阵 I。E(x̄_k) 用于将k时刻的3维输入误差映射到4维空间，E(x̄_{k+1})^T 用于将k+1时刻的4维输出误差映射回3维空间。

经过这样的变换后，我们得到了维数降低、且满秩的雅可比矩阵 Ã 和 B̃。只要原系统是可控的，这个降维系统就是可控的，我们可以对其应用标准的LQR算法。

在线反馈控制

在离线计算出LQR增益矩阵 K 后，我们需要在线运行反馈控制器。由于增益 K 是针对降维误差状态 Δx̃ 计算的，因此在线运行时，我们需要根据实际状态 x_k 和参考状态 x̄_k 计算出这个降维误差。

在线误差计算与控制律公式：

计算降维误差状态 Δx̃_k：
- 位置误差：Δp = p_k - p̄_k
- 姿态误差：先计算四元数误差 δq = L(q̄_k)^T * q_k（即参考四元数的共轭乘以当前四元数），然后将 δq 映射到3维参数 φ（例如，取其矢量部分或使用罗德里格斯参数）。
- 其余状态误差：直接相减，如 Δv = v_k - v̄_k。
应用控制律：
u_k = ū_k - K * Δx̃_k

关于姿态误差计算的说明：
计算 δq = L(q̄_k)^T * q_k 本质上是计算从参考姿态到当前姿态的旋转差，并将其表示在机体坐标系中。这与用旋转矩阵时计算 R_{ref}^T * R 是类似的。选择这种顺序（参考的共轭乘以当前）是一种常见约定，它对应于在机体坐标系中表示误差，通常能带来更好的控制性能。

案例研究：四旋翼动力学与控制

接下来，我们将以四旋翼飞行器作为具体案例，应用上述理论。

四旋翼模型

四旋翼的状态向量通常定义为：
x = [p (3维位置), q (4元数姿态), v (3维线速度-机体坐标系), ω (3维角速度-机体坐标系)]
选择线速度 v 在机体坐标系下，通常有利于控制。

动力学包括运动学（位置和姿态的导数）和动力学（牛顿-欧拉方程）。

运动学公式：

ṗ = R(q) * v          # 位置导数：机体速度旋转到惯性系
q̇ = 1/2 * Ω(ω) * q    # 四元数导数

其中 R(q) 是由四元数 q 得到的旋转矩阵，Ω(ω) 是由角速度 ω 构成的四元数乘法矩阵。

动力学公式：

平动动力学（旋转坐标系下的牛顿第二定律）：
v̇ = (1/m) * (R(q)^T * [0, 0, -mg]^T + [0, 0, Σ c_t * u_i]^T) - ω × v
其中，- ω × v 是科里奥利项，c_t 是推力系数，u_i 是电机输入。
转动动力学（欧拉方程）：
J * ω̇ + ω × (J * ω) = τ
其中，τ 是由电机推力差和反扭矩产生的总力矩。

LQR控制在四旋翼上的实现

在代码实现中，我们首先在悬停平衡点对动力学进行朴素线性化，得到13维的 A_naive 和 B_naive。可以验证，A_naive 的秩为12（不满秩），且由其构成的可控性矩阵秩也为12，系统不可控。

然后，我们构造矩阵 E，计算降维的雅可比矩阵 Ã (12x12) 和 B̃ (12x4)。此时，可控性矩阵满秩（秩为12），系统可控。接着，我们可以使用离散时间LQR求解器计算增益矩阵 K (4x12)。

在线控制时，我们按照前述方法计算降维误差状态 Δx̃（包含3维姿态误差 φ），并应用控制律 u = u_hover - K * Δx̃。

仿真结果表明，这种结合了四元数技巧的LQR控制器非常鲁棒，即使从很大的初始姿态误差（例如接近倒置）开始，也能成功恢复并稳定悬停。

总结

本节课中我们一起学习了如何将四元数正确地整合到LQR控制框架中。

我们首先指出了问题：对包含四元数的动力学进行朴素线性化会导致系统不可控。
然后我们引入了解决方案：利用姿态雅可比矩阵，在系统线性化的输入和输出端进行映射，将四元数误差从4维降为3维，从而得到一个可控的降维线性系统。
我们详细推导了降维雅可比矩阵 Ã、B̃ 的计算公式，以及在线运行时如何计算降维误差状态 Δx̃。
最后，我们以四旋翼为例，完整展示了从模型建立、线性化、LQR设计到仿真验证的整个流程。该方法（本质上是SE(3)控制的一种形式）能够处理大范围的姿态误差，性能优异。

掌握了这些技巧，您就可以在需要处理三维旋转的任何机器人控制问题中， confidently 地应用LQR及其他基于线性化的高级控制方法了。

17：混合系统与足式机器人 🦿

在本节课中，我们将学习如何为包含接触和碰撞的系统（例如足式机器人）建模和控制。我们将介绍两种主要方法：基于优化的时间步进方法和基于混合系统的事件驱动方法，并最终应用这些知识来规划一个单足跳跃机器人的运动。

1. 回顾：接触建模的两种视角

上一节我们介绍了四元数及其在最优控制中的应用。本节中，我们来看看机器人学中另一个常见但非经典最优控制主题的问题：接触动力学。

接触问题在机器人学中非常普遍，它本质上是一种切换系统。我们将探讨两种主要的建模方法。

1.1 时间步进方法（QP方法）

第一种方法是基于优化的时间步进方法，这也是许多机器人模拟器（如Bullet、DART）内部使用的方法。其核心思想是将动力学方程和接触约束离散化，并表述为一个二次规划问题。

我们以一个下落的砖块为例。砖块受重力作用，并可能与地面发生接触。

动力学方程：

\[M \dot{v} = -Mg + J^T \lambda \]

其中：

$ M $ 是质量。
$ v $ 是速度。
$ g $ 是重力加速度。
$ J $ 是接触雅可比矩阵。
$ \lambda $ 是接触力（拉格朗日乘子）。

符号距离函数：
我们定义一个函数 $ \phi(q) $ 来描述物体与地面的距离。对于砖块，$ \phi(q) = y $（垂直高度）。约束条件为：

\[\phi(q) \ge 0 \]

这表示砖块不能穿透地面。

离散化与QP公式：
使用隐式欧拉法对时间进行离散化，我们可以得到一组包含动力学、运动学和非穿透约束的方程。这些方程恰好是一个不等式约束二次规划问题的KKT条件。

以下是该QP问题：

\[\min_{v_{k+1}} \frac{1}{2} v_{k+1}^T M v_{k+1} + v_{k+1}^T M (v_k - h g) \]

\[\text{s.t.} \quad J v_{k+1} \ge -\frac{\gamma}{h} \phi(q_k) \]

其中 $ h $ 是时间步长，$ \gamma $ 是一个参数。

该方法的优缺点：

优点：是机器人模拟的标准方法；能显式计算接触力 $ \lambda $。
缺点：无法精确解析碰撞发生的时刻；通常只能使用低阶积分器（如欧拉法），需要较小的时间步长；自然地产生非弹性碰撞。

1.2 混合系统方法（事件驱动方法）

第二种方法是混合系统或事件驱动方法。这在机器人控制和轨迹优化中更为常见。其核心思想是：大部分时间动力学是平滑的，只在特定事件（如碰撞）发生时发生瞬时跳跃。

该方法包含三个核心部分：

平滑动力学： $ \dot{x} = f(x) $。例如，砖块在空中的飞行动力学。
守卫条件： $ \phi(q) = 0 $。用于检测事件（如碰撞）何时发生。
跳跃映射： $ x^+ = g(x^-) $。定义事件发生时状态的瞬时变化。对于砖块碰撞地面，跳跃映射将垂直速度置零（非弹性碰撞）或反向并乘以恢复系数（弹性碰撞）。

模拟流程伪代码：

while t < t_final:
    if phi(q) > 0:  # 未接触
        x = integrate_ode(f, x, dt)  # 使用RK4等积分器
    else:           # 检测到接触
        x = g(x)                     # 应用跳跃映射

该方法的优缺点：

优点：能解析精确的碰撞时间；可以使用高阶积分器；是轨迹优化的标准方法。
缺点：不直接提供接触力；当存在多种接触模式（如滑动、粘滞）时，模式组合会爆炸性增长。

2. 应用：单足跳跃机器人轨迹优化

现在，我们将混合系统方法应用于最简单的足式机器人：一个2D单足跳跃机器人。

2.1 机器人模型

机器人由一个身体质量 $ M_b $ 和一个足端质量 $ M_f $ 组成，两者通过一个可伸缩的棱柱关节连接。我们可以施加力 $ F $ 于该关节，并施加扭矩 $ \tau $ 来摆动腿部。

状态： $ x = [q_b, q_f, v_b, v_f]^T $ （8维）
控制输入： $ u = [F, \tau]^T $ （2维）

我们定义两种平滑动力学模式：

支撑相动力学 $ f_1 $：假设足端固定在地面。
飞行相动力学 $ f_2 $：机器人在空中，只受重力影响。

跳跃映射 $ g_{21} $ 定义了从飞行相到支撑相的转换（即足端撞击地面）。我们假设为非弹性碰撞并粘滞接触，因此足端速度被置零：

\[g_{21}(x) : \quad v_f^+ = 0 \]

从支撑相到飞行相的转换（抬脚）是平滑的，因此跳跃映射是恒等变换。

2.2 轨迹优化问题构建

为了进行轨迹优化，我们预先指定接触模式的序列。例如，交替分配5个节点为支撑相，接着5个节点为飞行相，如此循环。

优化问题包含：

成本函数：跟踪一个给定的参考轨迹（例如，让身体高度呈正弦变化以实现原地跳跃，或让水平位置线性变化以向前移动）。
约束条件：
- 在支撑相节点，施加支撑相动力学 $ f_1 $ 和足端位置约束 $ \phi(q_f)=0 $。
- 在飞行相节点，施加飞行相动力学 $ f_2 $ 和足端高度约束 $ \phi(q_f) > 0 $。
- 在模式切换的节点，施加跳跃映射 $ g_{21} $ 作为等式约束。
- 棱柱关节的长度限制等物理约束。

通过使用IPOPT等求解器处理这个带约束的轨迹优化问题，我们可以生成使机器人跳跃的控制器。

2.3 结果演示

通过调整参考轨迹，我们可以让机器人：

原地跳跃：身体高度跟踪正弦波参考轨迹。
向前跳跃：身体水平位置跟踪线性增加的参考轨迹。

这种方法本质上是通过绘制一个粗略的“卡通”运动草图（参考轨迹），然后让优化器在满足真实物理动力学的前提下，尽可能好地跟踪这个草图。

3. 总结

本节课中我们一起学习了：

接触建模的两种范式：基于QP的时间步进方法（适用于模拟）和基于混合系统的事件驱动方法（适用于规划与控制）。
混合系统方法的核心要素：平滑动力学、守卫条件和跳跃映射。
如何应用混合系统方法进行轨迹优化：以单足跳跃机器人为例，通过预先指定接触序列，构建并求解一个约束优化问题来生成复杂的动态行为。

这种混合系统轨迹优化框架是当前许多先进足式机器人（如波士顿动力的Atlas）完成复杂动作的基础。虽然我们处理的是简化模型，但核心思想——将不连续的接触动力学分解为多个平滑阶段并分别处理——可以扩展到更复杂的机器人系统中。

17：迭代学习控制 (Iterative Learning Control) 🎯

在本节课中，我们将学习如何处理模型误差或模型不确定性。我们将首先回顾并完善上节课关于接触和摩擦的内容，然后重点介绍一种名为“迭代学习控制”的算法。该算法能有效利用真实系统的实验数据，修正名义模型的误差，从而显著提升控制性能。

回顾：处理摩擦的混合方法

上一节我们介绍了处理接触的混合轨迹优化方法，并聚焦于足式机器人运动。本节中，我们来看看如何将摩擦因素纳入考虑。

在之前的混合设置中，我们假设接触是“粘性”的，即脚一旦接触地面就被“钉”住。这相当于假设了无限大的摩擦力（无滑动）。然而，在实际中，如果切向力过大，摩擦力会被突破，导致滑动甚至摔倒。

库仑摩擦模型与摩擦锥

处理摩擦的标准方法是使用库仑摩擦模型，并将其表述为一组约束。核心思想是：当处于接触状态时，添加约束，确保切向摩擦力位于“摩擦锥”内，不违反库仑摩擦定律。

对于一个接触点，其数学描述为：

公式：

\[\| \mathbf{b} \|_2 \leq \mu n \]

其中：

b 是二维切向摩擦力向量。
μ 是摩擦系数（正标量）。
n 是法向力。

几何上，这表示摩擦力向量 b 必须位于一个以法向力 n 为高度的圆锥体内。锥体内部对应“粘滞”状态，边界对应“滑动”状态，这种切换是非光滑的。

将摩擦锥线性化以用于QP求解

理论上，我们可以将上述圆锥约束直接放入优化问题。但实践中，大多数模型预测控制使用基于二次规划的求解器。因此，一个标准技巧是将非线性的摩擦锥线性化，使其能放入QP框架。

以下是具体做法：我们引入一个新的4维向量 d（b 是2维），并建立以下约束关系：

公式：

\[\begin{aligned} &\mathbf{d} \geq 0 \\ &\mathbf{1}^T \mathbf{d} \leq \mu n \\ &\mathbf{b} = \begin{bmatrix} \mathbf{I} & -\mathbf{I} \end{bmatrix} \mathbf{d} \end{aligned} \]

这里，1 是全1向量。d 的前两个元素代表 b 的正部分，后两个元素代表 b 的负部分。所有 d 的元素必须非负。

这样做的效果是，我们用一组线性不等式约束，将原始的2-范数约束（圆形摩擦锥）近似为1-范数约束（棱锥形摩擦锥）。这个棱锥是原始圆锥的一个更保守的内近似，意味着满足此约束的系统更不容易滑动。虽然这种近似在几何上存在各向异性，可能导致非物理的“卡角”现象，但对于许多旨在避免滑动的机器人应用来说，它足够有效且计算高效。

应对模型误差的策略概述

在基于模型的控制中，模型总是近似的。当模型不够精确时，我们有几种策略可以选择。

以下是主要的几种思路：

参数估计（灰箱模型）：通过实验数据拟合物理模型中的参数（如质量、惯量）。
- 优点：数据效率高，模型可泛化用于不同任务。
- 缺点：无法捕获模型结构中未包含的动态特性。
黑箱系统辨识：使用通用函数逼近器（如神经网络）从头学习动态模型或残差项。
- 优点：理论上可以学习任何动态，模型可泛化。
- 缺点：数据效率低，需要大量数据。

直接学习控制策略（如强化学习）：优化参数化的反馈策略。
- 优点：对系统和模型假设少，非常通用。
- 缺点：数据效率低，学到的策略通常针对特定任务，难以泛化。

迭代学习控制：利用真实系统数据，迭代改进一条基于名义模型生成的参考轨迹。
- 优点：对模型假设少，数据效率极高（通常5-10次实验即可）。
- 缺点：需要有一个“尚可”的名义模型，结果针对特定任务。

接下来，我们将深入探讨迭代学习控制。

迭代学习控制详解 🔄

ILC的核心思想是：我们有一个基于可能不准确的名义模型离线优化得到的参考轨迹 (x̄, ū)。然后，通过在真实系统上反复执行、测量误差并更新前馈控制输入 ū，来逐步修正模型误差带来的影响。

问题设定

考虑一个标准的轨迹优化问题，我们最小化一个二次型跟踪代价：

公式：

\[\min_{\mathbf{x}, \mathbf{u}} \sum_{k} \left( (\mathbf{x}_k - \bar{\mathbf{x}}_k)^T \mathbf{Q}_k (\mathbf{x}_k - \bar{\mathbf{x}}_k) + (\mathbf{u}_k - \bar{\mathbf{u}}_k)^T \mathbf{R}_k (\mathbf{u}_k - \bar{\mathbf{u}}_k) \right) + \text{终端代价} \]

\[\text{subject to: } \mathbf{x}_{k+1} = f(\mathbf{x}_k, \mathbf{u}_k) \]

ILC 算法步骤

ILC可以看作是在真实系统上进行的一种特殊SQP方法：

离线计算：使用名义模型 f(x,u) 求解上述优化问题，得到参考轨迹 (x̄, ū)。同时，计算并存储该轨迹处的KKT系统矩阵（即Hessian H 和约束Jacobian C）。

KKT系统形式：

\[\begin{bmatrix} \mathbf{H} & \mathbf{C}^T \\ \mathbf{C} & \mathbf{0} \end{bmatrix} \begin{bmatrix} \Delta \mathbf{z} \\ \lambda \end{bmatrix} = \begin{bmatrix} -\nabla J \\ -\mathbf{c} \end{bmatrix} \]
其中 z 是状态和控制的堆叠向量，c 是动态约束。
在线迭代：
- 执行与测量：将当前的前馈控制序列 ū （结合一个可选的跟踪控制器）应用于真实系统，执行一次实验，记录实际的状态轨迹 x 和控制轨迹 u。
- 构建右端项：利用真实实验数据计算KKT系统的右端项。
  - 梯度项 ∇J：将真实的 (x, u) 代入代价函数计算梯度。
  - 约束项 c：在真实物理系统上执行的轨迹必然满足真实动力学，因此 c = 0。
- 求解更新量：使用离线计算好的、固定的左侧KKT矩阵，与在线测量的右端项一起，求解线性系统，得到更新量 Δz（包含 Δx 和 Δu）。
- 更新控制：提取 Δu，更新前馈控制序列：ū ← ū + Δu。
- 重复：用新的 ū 进行下一次实验，直到性能满意。

关键点：算法利用真实数据提供误差方向（右端项），但利用名义模型提供近似的梯度/曲率信息（左侧矩阵）来指导更新。只要名义模型误差不是特别大，这个混合方法就能快速收敛。

算法优势与示例

ILC非常高效，通常只需5-10次真实系统实验就能显著提升性能。它可以处理各种未建模的动态，例如恒定的风扰、复杂的非线性摩擦等。

一个典型示例是让四旋翼在恒定侧风下进行直线飞行。基于无风模型设计的轨迹会导致偏离。使用ILC，经过几次飞行实验并更新前馈控制输入后，无人机就能学会补偿风力，实现直线飞行。

在课程演示中，对一个存在参数误差和未建模非线性摩擦的倒立摆摆起任务，仅通过约5-7次ILC迭代，开环控制性能就从失败提升到了几乎完美的摆起。

总结

本节课中我们一起学习了：

摩擦处理：介绍了库仑摩擦模型和摩擦锥概念，并讲解了如何通过线性化技巧将其纳入QP优化框架，用于足式机器人等接触控制。
模型误差应对策略：概述了从参数估计、黑箱辨识到策略学习等多种处理模型不确定性的方法。
迭代学习控制：深入探讨了ILC算法。该算法巧妙结合离线名义模型计算（提供更新方向）和在线真实系统实验（提供误差信息），以极高的数据效率迭代优化前馈控制指令，从而补偿模型误差，实现高性能控制。

ILC是一种非常实用且强大的技术，特别适用于模型大体正确但存在未知误差，且能在硬件上进行多次实验的系统。

19：随机最优控制与LQG 🎯

在本节课中，我们将首先完成上节课关于迭代学习控制的讨论，然后深入探讨随机最优控制的基本理论，并重点学习著名的线性二次高斯问题。

回顾：迭代学习控制

上一节我们讨论了摩擦和迭代学习控制。由于时间关系，我们未能完成最后几个要点，因此今天先快速回顾并完成这部分内容。

迭代学习控制是一种非常酷的算法，大约十年前人们用它做出了一些非常出色的成果。虽然随着深度强化学习的兴起，这个领域像许多其他控制理论主题一样不再那么流行，但我认为它仍然很酷，并且还有一些有趣的事情可以做。

ILC算法总结

以下是ILC算法的伪代码级总结，并讨论其为何有效。

算法目标：
给定一条由名义模型计算出的名义轨迹 X_bar 和 U_bar，我们希望在真实系统上通过多次“试运行”来更新控制输入 U，即计算一个增量 ΔU，使得真实系统能够跟踪上名义轨迹 X_bar。

算法步骤：

在真实系统上进行试运行：
在每次迭代中，从初始状态 X0_bar 出发，使用当前的开环控制序列 U_bar 在真实系统上进行一次完整的轨迹运行，得到实际状态序列 X_k。
```
X_k = rollout_real_system(X0_bar, U_bar)
```

求解局部最优控制问题（QP）：
基于名义模型和实际运行得到的误差，求解一个二次规划问题，计算状态和控制输入的增量 ΔX 和 ΔU。
代价函数 J 是 ΔX 和 ΔU 的二次型，通过对原代价函数在参考轨迹处进行泰勒展开，并代入实际运行误差得到。
约束包括基于名义模型线性化的动力学方程，以及可能存在的其他约束（如扭矩限制）。
```
min_{ΔX, ΔU} J(ΔX, ΔU)
s.t.  ΔX_{k+1} = A_k ΔX_k + B_k ΔU_k  (线性化动力学)
      其他约束（如 U_bar_k + ΔU_k 在扭矩限内）
```

更新控制序列：
使用求解得到的 ΔU 更新名义控制序列。
```
U_bar_new = U_bar_old + ΔU
```
循环：
重复步骤1-3，直到跟踪误差 X_k - X_bar_k 小于某个容差。

这个循环看起来非常像序列二次规划，唯一的区别在于“试运行”步骤是在真实硬件系统上进行的，而不是在仿真中使用某个动力学模型。实际上，你可以用SQP、动态规划等任何我们讨论过的轨迹优化方法来求解内部QP问题，关键区别在于用真实硬件试运行替代了动力学模型。

关于收敛性的讨论

一个自然的问题是：我们使用已知有误的名义模型来计算雅可比矩阵等信息，为什么算法仍然能收敛？

答案在于，牛顿法即使在KKT矩阵中的信息存在相当大误差时，通常仍然有效。这类似于拟牛顿法（如BFGS、L-BFGS）背后的理论。这些方法以各种方式近似海森矩阵，虽然无法达到使用精确导数的完整牛顿法的收敛速度，但仍然可以实现相当快（甚至是超线性）的收敛。即使雅可比矩阵存在较大误差，它们仍然比梯度下降法好得多。

更形式化地说，存在一系列“不精确牛顿法”和“拟牛顿法”。只要计算出的增量步长 Δx 满足某个与收敛速率相关的不等式，算法就能收敛。这意味着，只要名义模型没有偏离真实系统太远，使用它计算的牛顿步长仍然能提供一个下降方向，并最终收敛。

演示：ILC在倒立摆上的应用

我们通过一个倒立摆摆起任务的演示来直观理解ILC。

离线规划： 首先使用名义模型进行离线轨迹优化，得到一个摆起的开环轨迹和LQR跟踪控制器。
引入模型误差： 我们创建一个“真实”模型，其中改变了小车和摆的质量、摆长，并添加了一个复杂的非线性摩擦项来模拟库仑摩擦。
开环执行： 在真实模型上执行名义开环轨迹，任务失败。
加入LQR反馈： 加入LQR跟踪控制器后，摆起成功，但违反了执行器的扭矩限制（因为模型变重且有摩擦，需要更大扭矩）。
强制执行扭矩限制： 在仿真中强制执行扭矩限制后，LQR控制器再次失败。
运行ILC： 移除LQR，运行ILC循环。算法在每次试运行后求解QP来更新控制序列。经过大约5-8次试运行后，系统能够在严格遵守扭矩限制的前提下，完美地完成摆起任务。

这个演示表明，ILC能够用很少的几次真实系统试验，补偿模型误差和未建模动态，实现高性能跟踪。

实际应用与考量

ILC非常适用于任务确定且可重复的场景（如工业机器人执行固定任务）。只要系统动态在多次试验间保持一致，通过几次学习就能获得完美跟踪性能，之后便可一直使用。

如果环境发生变化（如恒定的侧风），ILC也能通过学习新的开环控制量来进行补偿。对于更复杂的随机扰动或动态时变的情况，可能需要结合在线自适应策略。

ILC本质上也可以看作一种策略梯度方法，其中策略参数就是开环控制序列 U_bar。它使用拟牛顿更新而非梯度下降，因此收敛速度更快。

随机最优控制与LQG 🎲

现在，我们完全切换话题，开始讨论随机最优控制。

到目前为止，我们一直假设系统是确定性的，并且我们完全知道系统状态。现在我们要放松这个假设。在实际中，我们通常无法直接测量所有状态，只能得到与状态相关的噪声测量值 y，例如 y = g(x)。

问题设定

在随机设定下，我们无法获得确切的状态 x，只能获得基于实际测量 y 的状态概率分布，即条件概率密度函数 p(x|y)。

我们可以写出随机最优控制问题的一般形式，即在我们之前求解的最优控制问题基础上，给代价函数加上期望算子：

min_u E[ J(x, u) ]

理论上，我们可以像以前一样用动态规划求解这个问题，但对于一般情况，这非常困难，会遭遇“维数灾难”。

线性二次高斯问题

LQG是随机最优控制中一个可以解析求解的特殊案例。

L 代表线性动力学。
Q 代表二次代价。
G 代表高斯噪声。

这基本上是我们已经讨论过的LQR问题的随机版本。

系统模型：

动力学（过程噪声）： x_{k+1} = A x_k + B u_k + w_k，其中 w_k ~ N(0, W) 是过程噪声。
测量模型（测量噪声）： y_k = C x_k + v_k，其中 v_k ~ N(0, V) 是测量噪声。
噪声序列 {w_k} 和 {v_k} 假设是零均值、白噪声，且彼此不相关。

目标： 最小化期望代价 J = E[ Σ (x_k^T Q x_k + u_k^T R u_k) + x_N^T Q_N x_N ]。

动态规划求解

我们尝试用动态规划求解。从终端代价 V_N(x) = E[ x_N^T Q_N x_N ] 开始，向后递归。

计算 V_{N-1}(x) 时，我们会得到包含标准LQR项和噪声项的表达式。关键步骤在于分析噪声项：

由于噪声 w_k 与当前状态 x_k 和控制 u_k 不相关，且均值为零，因此所有形如 E[ x_k^T * w_k ] 或 E[ u_k^T * w_k ] 的交叉项期望值为零。
形如 E[ w_k^T P w_k ] 的项是一个常数，等于 trace(W * P)。

结论： 在LQG问题中，所有噪声项要么期望为零，要么为常数。这意味着噪声的存在并不影响最优控制器的形式，它只是使总代价增加了一个常数偏移量。因此，随机LQG问题的最优控制器与确定性LQR问题的最优控制器完全相同。

确定性等价原理与分离原理

上述结论引出了两个经典的控制理论结果：

确定性等价原理： 对于LQG问题，最优控制器就是在确定性LQR控制器中，用状态的条件期望 E[x|y] 替换真实状态 x。
```
u*_k = -K_k * E[x_k | y_{0:k}]
```

分离原理： 对于LQG问题，我们可以独立地设计两部分：
- 一个最优状态估计器（卡尔曼滤波器）： 根据测量 y 产生状态估计 \hat{x}_k = E[x_k | y_{0:k}]。
- 一个最优反馈控制器（LQR）： 将上述状态估计视为真实状态，计算控制量 u_k = -K_k \hat{x}_k。
  将这两部分组合起来，就构成了原随机最优控制问题的最优解。

重要提示： 这两个原理仅在线性系统、二次代价、高斯噪声的假设下严格成立。对于一般的非线性、非高斯问题，这些原理不再成立。此时，估计器和控制器会紧密耦合（例如，为了更好估计状态，可能需要执行特定的控制动作）。然而，在实践中，我们通常仍然采用这种“估计+控制”的分离架构，只要状态估计足够准确，并且噪声分布是单峰的，它通常能取得很好的效果。

总结 🎉

本节课中，我们一起学习了两个重要主题。

首先，我们完成了对迭代学习控制的探讨。ILC是一种利用真实系统多次试验数据来补偿模型误差、优化开环控制序列的方法。其核心在于在真实硬件上“试运行”，然后基于名义模型求解一个局部二次规划来更新控制量。即使模型不精确，算法也能快速收敛，是实现“仿真到现实”转移的强大工具。

随后，我们深入研究了随机最优控制，并重点分析了线性二次高斯问题。我们推导发现，在LQG的假设下，过程噪声和测量噪声并不改变最优控制器的结构。由此引出了确定性等价原理和分离原理，即最优解可以分解为一个独立的最优状态估计器（卡尔曼滤波器）和一个独立的最优反馈控制器（LQR）的级联。虽然这些原理在更一般的非线性随机控制问题中不成立，但分离架构仍然是实际工程中广泛采用且非常有效的范式。

20：卡尔曼滤波器与对偶性 🧮

在本节课中，我们将学习线性二次高斯（LQG）问题的后半部分，即卡尔曼滤波器的最优状态估计。我们将探讨卡尔曼滤波器的工作原理、其与线性二次调节器（LQR）的深刻联系（即对偶性），并了解如何将估计器与控制器分离设计。

概述与回顾

上一节我们介绍了LQG问题，并讨论了确定性等价原理和分离原理。这些原理表明，在LQG设定下，我们可以将控制器和估计器分开设计，然后将估计器输出的状态估计（如最大似然估计）直接输入控制器，这构成了最优策略。

本节中，我们来看看如何获得这个最优的状态估计，即卡尔曼滤波器。我们将从最小均方误差的角度推导卡尔曼滤波器，并揭示其与LQR问题的对偶关系。

最优估计的目标

在进行最优状态估计时，首先需要明确优化目标。以下是几种常见的优化准则：

最小均方误差估计：最小化估计状态与真实状态之间误差的平方的期望值。其目标函数为：
arg min over x_hat of E[ (x - x_hat)^T (x - x_hat) ]
利用迹技巧，这等价于最小化估计误差协方差矩阵的迹：trace( Sigma )。
最大后验估计：最大化在给定测量数据条件下状态的后验概率：arg max over x of p(x | measurements)。

对于一般的非线性、非高斯分布，这两种估计可能给出不同结果。但在LQG问题假设的线性高斯情况下，最小均方误差估计与最大后验估计是等价的。我们将采用最小均方误差的推导方式，因为它更简洁直观。

卡尔曼滤波器推导

卡尔曼滤波器是一种递归的线性最小均方误差估计器。它假设我们拥有一个包含截至当前时刻所有测量信息的初始状态估计 x_hat_k|k 及其协方差 Sigma_k|k。滤波器的目标是在收到新测量 y_{k+1} 后，更新得到 x_hat_{k+1|k+1} 和 Sigma_{k+1|k+1}。

滤波器运行分为两个步骤：预测步和更新步。

预测步

预测步利用系统动力学，在不考虑新测量的情况下，将上一时刻的状态估计向前推进一步。

状态预测：
x_hat_{k+1|k} = A * x_hat_{k|k} + B * u_k
这里，u_k 是已知的控制输入。

协方差预测：
Sigma_{k+1|k} = A * Sigma_{k|k} * A^T + W
其中，W 是过程噪声 w_k 的协方差矩阵。这个公式直观地表示：旧的不确定性（Sigma_{k|k}）通过动力学矩阵 A 进行传播，然后加上本步新增的过程噪声不确定性（W）。

更新步

更新步将新的传感器测量信息融合到预测的状态中。

首先，计算新息，即实际测量值与基于状态预测的预期测量值之间的误差：
z_{k+1} = y_{k+1} - C * x_hat_{k+1|k}

接着，计算新息协方差，它衡量了这个预测误差的不确定性：
S_{k+1} = C * Sigma_{k+1|k} * C^T + V
其中，V 是测量噪声 v_k 的协方差矩阵。

然后，计算关键的卡尔曼增益 L_{k+1}。增益决定了我们应在多大程度上信任新测量来修正预测：
L_{k+1} = Sigma_{k+1|k} * C^T * S_{k+1}^{-1}

最后，使用卡尔曼增益来更新状态估计和协方差估计：

状态更新：
x_hat_{k+1|k+1} = x_hat_{k+1|k} + L_{k+1} * z_{k+1}

协方差更新（约瑟夫形式）：
Sigma_{k+1|k+1} = (I - L_{k+1} * C) * Sigma_{k+1|k} * (I - L_{k+1} * C)^T + L_{k+1} * V * L_{k+1}^T
这个形式能保证更新后的协方差矩阵始终对称正定，数值稳定性更好。

卡尔曼滤波器与LQR的对偶性

观察卡尔曼滤波器的方程，会发现其结构与LQR控制器惊人地相似。这种相似性并非偶然，它体现了一种深刻的对偶性：

LQR：解决的是从未来终端代价反向传播到当前的最优控制问题。
卡尔曼滤波：解决的是从过去初始不确定性正向传播到当前的最优估计问题。

具体来说，卡尔曼滤波中的状态预测/更新序列与LQR中的代价函数反向传播序列在数学形式上对偶。卡尔曼增益 L 的计算类似于LQR中反馈增益矩阵 K 的计算。

这种对偶性意味着，许多为控制问题（如LQR、模型预测控制MPC）开发的算法，可以转化为对应的估计问题算法（如卡尔曼滤波、滑动窗口估计器）。

非线性扩展与总结

对于非线性系统，卡尔曼滤波的直接应用是扩展卡尔曼滤波器，其核心思想是在当前估计点对系统动力学和测量模型进行线性化，然后应用标准卡尔曼滤波公式。

更高级的方法如无迹卡尔曼滤波器，通过采样（Sigma点）来近似传播非线性变换下的均值和协方差，通常比EKF有更好的精度和稳定性。

在本节课中，我们一起学习了卡尔曼滤波器的原理与推导，理解了它作为LQG问题中“估计器”部分的核心角色。我们看到了其递归预测-更新结构的优雅，并探讨了它与LQR控制器之间重要的对偶关系。这种分离设计估计器与控制器的范式（分离原理），以及估计与控制问题之间的对偶性，是控制理论中强大而深刻的思想。

21：鲁棒控制与极小极大优化 🛡️

在本节课中，我们将学习鲁棒控制的基本概念，并探讨一种名为“极小极大DDP”的算法。我们将了解鲁棒控制与之前学习的随机最优控制有何不同，以及如何通过优化方法设计能够应对模型不确定性的控制器。

回顾与安排

上一节课我们完成了LQG（线性二次高斯）控制器的讨论，包括卡尔曼滤波器与LQR（线性二次调节器）的结合，并探讨了控制与估计之间的对偶性。

本节课我们将转向一个不同的方向：鲁棒控制。这是一个广阔的领域，我们将进行简要概述，并重点介绍一种与我们已学内容紧密衔接的算法——极小极大DDP。

鲁棒控制简介

鲁棒控制的核心思想与之前讨论的随机最优控制不同。随机控制处理的是具有已知统计特性的噪声，而鲁棒控制处理的是模型不确定性，例如参数误差或未建模的动态特性。

以下是鲁棒控制与相关概念的对比：

LQG控制：假设噪声为零均值，擅长处理随机扰动，但对参数不确定性不鲁棒。
迭代学习控制：属于自适应控制范畴，通过多次试验在线学习并适应未知的系统参数。
鲁棒控制：采用一种保守的离线设计方法。其目标是设计一个固定的控制策略，该策略能够保证在模型存在有界不确定性的情况下，系统依然稳定且性能可接受。这通常意味着需要牺牲一部分最优性能来换取鲁棒性。

历史上，John Doyle在1978年的著名论文指出，LQG控制器对微小的模型扰动可能变得不稳定，这直接催生了鲁棒控制这一研究领域。

鲁棒控制问题定义

我们定义一个包含扰动的系统模型：
x_{k+1} = f(x_k, u_k, w_k)
其中，w_k 代表扰动。与卡尔曼滤波器中的噪声不同，这里对 w_k 的统计特性没有假设。它可以表示参数误差、未建模动态或有界的预测误差。

典型的做法是对 w_k 施加一些约束，例如假设其范数有界或属于某个集合，这是为了在数学上可处理。

相应的鲁棒最优控制问题可以表述为一个极小极大优化问题：

min_{x, u} max_{w} J(x, u, w)
subject to:
    x_{k+1} = f(x_k, u_k, w_k)
    (可能的其他约束)

这个问题的含义是：寻找一个控制策略，使其在最坏情况的扰动 w 下，性能指标 J 尽可能好。

从优化角度看，这是一个寻找鞍点的问题。它也可以被解释为一个双人博弈：控制玩家 u 试图最小化成本，而扰动玩家 w 试图最大化成本。这属于Stackelberg博弈，因为存在顺序（通常扰动玩家先行动）。

极小极大DDP算法

极小极大DDP是标准DDP（微分动态规划）算法在鲁棒控制问题上的扩展。其核心思想与DDP相同：利用动力学和价值函数的局部泰勒展开，迭代地寻找局部最优解（此处为鞍点）。

我们重点关注与标准DDP的不同之处。

1. 动力学与Q函数展开

首先，动力学需要围绕标称轨迹 (x, u, w) 展开：

δx_{k+1} ≈ A_k δx_k + B_k δu_k + D_k δw_k

这里新增了关于扰动 w 的雅可比矩阵 D。

动作价值函数 Q（在原文中记为 S）现在是状态 x、控制 u 和扰动 w 的函数。我们对其进行二阶泰勒展开：

Q(δx, δu, δw) ≈ Q0 + [Q_x^T, Q_u^T, Q_w^T] * [δx; δu; δw] + 0.5 * [δx; δu; δw]^T * [[Q_xx, Q_xu, Q_xw]; [Q_ux, Q_uu, Q_uw]; [Q_wx, Q_wu, Q_ww]] * [δx; δu; δw]

2. 贝尔曼备份与最优性条件

在动态规划的后向传递中，我们对 δu 求最小化，对 δw 求最大化：

V(x) = min_{δu} max_{δw} Q(δx, δu, δw)

这体现了博弈的思想。

通过令 ∂Q/∂(δu) = 0 和 ∂Q/∂(δw) = 0，我们得到一阶最优性条件。注意，为了确保是极小极大问题的鞍点，需要满足：

Q_uu（控制部分的Hessian）必须是正定的（保证最小化）。
Q_ww（扰动部分的Hessian）必须是负定的（保证最大化）。

3. 求解反馈策略

联立两个一阶条件方程，可以解出最优的 δu 和 δw。它们都具有“前馈项 + 状态反馈项”的形式：

对于控制 u：

δu^* = k + K δx

其中，增益 K 和前馈项 k 的表达式比标准LQR更复杂，包含了来自扰动 w 的额外项（在原文中用红色标出）。你可以将其理解为对标准LQR策略的“鲁棒化”修正。

类似地，对于扰动 w，我们也会得到一个形式对称的解：

δw^* = l + L δx

这代表了在最坏情况假设下，扰动玩家所采取的策略。

4. 算法实现要点

准定Hessian矩阵：动作价值函数的Hessian矩阵 [[Q_xx, Q_xu, Q_xw]; ...] 现在是准定的，同时具有正特征值和负特征值。
正则化：在迭代过程中，可能需要特殊的正则化策略，分别增大 Q_uu 的正定性和 Q_ww 的负定性，以确保算法收敛到正确的鞍点。
成本函数设计：在极小极大DDP的原始论文中，扰动 w 的影响是通过一个负定的二次成本项 w^T W w（其中 W 为负定矩阵）来引入的。调整 W 的范数可以影响系统的鲁棒性：W 的惩罚越轻（范数越小），允许的扰动越大，控制器越鲁棒；反之则鲁棒性降低。当 W 的惩罚趋于无穷大时，算法退化为标准DDP。

总结

本节课我们一起学习了鲁棒控制的基本思想。鲁棒控制旨在设计能够应对模型不确定性的固定控制器，其核心是性能与鲁棒性之间的权衡。

我们重点介绍了一种具体的算法——极小极大DDP。它扩展了标准的DDP框架，通过求解一个极小极大优化问题（或双人博弈问题）来同时优化控制策略和对抗最坏情况扰动。该算法最终会产生一个经过鲁棒化修正的轨迹和线性反馈控制器。

需要注意的是，原始的极小极大DDP使用软约束（成本项）来处理扰动，而更经典的鲁棒控制理论（如H∞控制）通常采用硬约束（集合边界）。前者更易于与现有的轨迹优化框架结合，而后者能提供更清晰的物理含义和保证。

22：凸松弛与火箭着陆 🚀

概述

在本节课中，我们将学习如何将凸优化的思想应用于一个极具挑战性的实际问题——火箭的精确软着陆。我们将重点探讨如何通过“凸松弛”这一技巧，将原本非凸的推力约束问题转化为一个可以高效求解的凸优化问题，并理解其背后的原理。

安排与项目说明

上一节我们讨论了鲁棒控制，本节我们来看一个具体的案例研究。

以下是关于课程最后阶段和项目展示的安排：

小组将进行4分钟的闪电演讲，展示项目初步成果。
演讲将安排在最后两节课，具体时间将通过在线表格分配。
最终报告需以会议论文格式（约6页）提交。

火箭软着陆问题

火箭软着陆是一个经典的优化控制基准问题。其核心目标是从一个初始的位置和速度状态，安全、精准地降落到目标地点。

问题定义如下：

初始状态： 位置 r_initial，速度 v_initial。
终端约束： 最终高度 z_final = 0（着陆），最终速度 v_final = 0（软着陆）。
目标函数： 通常最小化燃料消耗和着陆点误差的组合，例如 min (||r_final - r_goal|| + fuel_cost)。
主要约束：
1. 动力学约束（牛顿第二定律）。
2. 推力大小约束：T_min <= ||T|| <= T_max。
3. 推力矢量方向（万向节角度）约束。
4. 安全约束（如避免撞地、保证视野等）。

这项技术已成功应用于多个著名任务，例如SpaceX的猎鹰9号、星舰回收，以及NASA的好奇号、毅力号火星车着陆。

控制系统架构

在实际工程中，火箭控制系统通常采用分层解耦的设计，以简化问题。

总体架构分为高层位置控制和底层姿态跟踪控制：

高层位置控制器（慢，~5-10 Hz）： 将火箭视为质点，基于简化的动力学模型（如v_dot = -g + T/m）进行轨迹规划。它负责处理安全约束、推力限制和燃料优化，并输出期望的加速度或推力指令。这是我们本节课讨论的重点。
底层姿态控制器（快，~数十Hz）： 接收高层指令，通过控制发动机推力和万向节角度，使火箭姿态快速跟踪以实现所需的加速度。这一层需要处理复杂的非线性姿态动力学、结构柔性模态和燃料晃动等问题，通常采用鲁棒线性控制方法。

状态估计模块为两者提供必要的信息（位置、速度、姿态、角速度）。在地球上，主要依赖GPS和IMU；在火星等无GPS环境，则需结合IMU、雷达高度计和视觉地形相对导航。

凸松弛的核心思想

现在，我们聚焦于高层位置控制中的优化问题。其难点在于推力约束 T_min <= ||T|| <= T_max 定义了一个“球形壳”可行域，这是一个非凸集。

凸松弛是一种将非凸问题转化为凸问题的技巧。其核心思想是：有时一个非凸约束可以表示为某个更大凸集的边界。

一个简单的例子：

原约束（非凸）： ||x|| = 1 （一个球面）。
松弛后的约束（凸）： ||x|| <= 1 （一个实心球体）。
原非凸集是新凸集的边界。

如果优化问题的成本函数性质良好（例如是线性的），其解会自动被“推”到凸集的边界上，从而恰好满足原约束。这种情况下，我们称这个凸松弛是紧的。

应用于火箭推力约束

我们将上述思想应用于火箭的推力约束。

原非凸约束为： T_min <= ||T|| <= T_max。

引入松弛变量 Gamma：
我们引入一个新的标量变量 Gamma，并令其等于推力大小：Gamma = ||T||。则原约束可重写为：

Gamma = ||T|| （非凸，因为这是球面约束）
T_min <= Gamma <= T_max （凸，简单的区间约束）
其他凸约束（如方向约束 (T/||T||)·e >= cos(theta_max) 现在可写为 (T/Gamma)·e >= cos(theta_max)）

关键步骤：
将第一个等式约束 Gamma = ||T|| 松弛为不等式约束 Gamma >= ||T||。现在，所有约束都变成了凸约束。

几何解释：
Gamma >= ||T|| 定义了一个 二阶锥，它是一个凸集。结合 T_min <= Gamma <= T_max，我们得到了一个凸的可行域。对于火箭软着陆中常用的某些成本函数（如最小着陆误差），可以证明最优解必然满足 Gamma = ||T||，即松弛是紧的，我们求得的解就是原问题的最优解。

总结

本节课我们一起学习了如何利用凸松弛技术解决火箭软着陆中的非凸推力约束问题。

核心要点回顾：

火箭软着陆控制采用分层架构，高层位置控制基于质点模型进行凸优化。
凸松弛 通过用更大的凸集代替非凸约束来转化问题。
通过引入松弛变量 Gamma 并将等式 Gamma = ||T|| 松弛为 Gamma >= ||T||，成功地将非凸的推力上下限约束转化为一个二阶锥规划问题。
对于合适的成本函数，该松弛是紧的，保证了凸优化解就是原问题的最优解。

这项技术是过去十年间航天控制领域的一项重大进展，它使得在线、实时生成安全可靠的着陆轨迹成为可能，并已成功应用于多个真实的太空任务中。

23：从控制视角看强化学习 🧠

在本节课中，我们将从最优控制的视角出发，探讨强化学习（RL）的核心思想。我们将回顾最优控制问题，并分析当系统动力学模型未知时，如何通过强化学习的方法来求解。课程将重点介绍策略梯度方法，并通过简单的线性二次调节器（LQR）问题来直观展示其原理与实现。

概述

强化学习本质上是在解决与最优控制相同的问题，但核心假设是我们没有关于系统动力学的先验模型。其理想目标是仅通过系统上的试错实验来学习如何完成任务，尤其是在机器人等硬件平台上。

强化学习的主要方法

上一节我们回顾了最优控制问题，本节中我们来看看当模型未知时，主要的几种强化学习范式。

以下是几种主要的强化学习方法：

基于模型的强化学习：首先从数据中学习一个动力学模型 F_θ(x, u)，然后使用标准的控制技术（如我们之前讨论过的）基于该模型进行控制。这本质上是一个回归问题，其历史可追溯到自适应控制等领域。
Q学习：学习一个动作价值函数 Q_θ(x, u)。这个函数来自动态规划，代表了从状态 x 执行动作 u 开始，到任务结束的预期总成本。通过从大量实验数据中拟合 Q 函数，我们可以通过求解单步最小化问题 u = argmin_u Q(x, u) 来得到控制策略。学习 Q 而非价值函数 V 的好处在于，我们可以完全不需要动力学模型。
策略梯度方法：直接参数化并优化控制策略 u_θ(x)（例如使用神经网络）。目标是最小化关于策略参数 θ 的期望总成本 J(θ)。通过在实际系统上采样轨迹并利用随机梯度估计来优化 θ。这是近年来非常热门的一类方法。
演员-评论家方法：将上述第二和第三种方法结合，同时学习策略（演员）和价值函数（评论家）。这通常有助于改善随机梯度下降的收敛性。

所有这些方法的哲学核心是，尽可能少地假设关于系统或环境的先验知识。然而，在工程实践中，我们通常知道很多，利用这些知识往往能得到更好的结果。

策略梯度方法详解

上一节我们概述了各类方法，本节我们将深入探讨策略梯度方法，这是当前机器人强化学习取得许多成功的关键。

为了使这些方法可行，我们需要做出一个关键假设：控制策略是随机的，即使真实系统是确定性的。我们将策略写为一个条件概率分布 π_θ(u|x)。这意味着，即使动力学是确定性的，由于我们在控制中注入了噪声，整个轨迹也会变得随机，从而诱导出一个轨迹分布 p_θ(τ)。

我们这样做的核心原因是为了通过采样来估计梯度。在策略中加入噪声（例如高斯噪声 u = u_θ(x) + v, v ~ N(0, Σ)）允许我们收集多样化的轨迹样本，从而近似计算成本函数关于策略参数的梯度。

策略梯度定理推导

我们的目标是最小化期望成本：
J(θ) = E_{τ~p_θ(τ)} [J(τ)]

我们需要计算其梯度 ∇_θ J(θ)。通过一系列数学变换（包括著名的对数似然技巧），我们可以得到策略梯度定理：

∇_θ J(θ) = E_{τ~p_θ(τ)} [ J(τ) * ∇_θ log p_θ(τ) ]

进一步，由于轨迹分布 p_θ(τ) 可分解为动力学转移概率和策略概率的乘积，而动力学部分不依赖于 θ，其梯度为零。因此，上式简化为：

∇_θ J(θ) = E_{τ~p_θ(τ)} [ J(τ) * Σ_{k} ∇_θ log π_θ(u_k | x_k) ]

这个结果是策略梯度方法的核心。它表明，我们可以通过从随机策略中采样轨迹，并计算这些轨迹的总成本 J(τ) 与策略对数概率梯度 ∇_θ log π_θ(u_k | x_k) 的乘积的期望，来估计真实梯度。动力学模型从这个表达式中消失了，这正是我们想要的。

应用于LQR问题

为了使概念更具体，我们将策略梯度应用于熟悉的LQR问题。假设一个线性策略 u = -Kx + v，其中 v 是高斯噪声。在这个设定下，策略分布 π_θ(u|x) 是一个高斯分布，其对数概率的梯度可以解析求出（或通过自动微分轻松获得）。

基本的策略梯度算法步骤如下：

以下是基本的策略梯度算法伪代码：

初始化：初始化策略参数 θ（例如，反馈矩阵 K）。
采样：使用当前随机策略 π_θ，在系统上运行并收集一批（S 条）轨迹 {τ_i}。
梯度估计：利用收集的轨迹样本，近似计算梯度：
g ≈ (1/S) Σ_{i=1}^S [ J(τ_i) * Σ_{k} ∇_θ log π_θ(u_{i,k} | x_{i,k}) ]
（实践中常会引入一个基线（如平均成本）来减少方差）。
策略更新：沿梯度负方向更新参数：
θ ← θ - α * g
其中 α 是学习率。
重复：返回步骤2，直到收敛。

实验演示与比较

上一节我们推导了算法，本节我们通过具体实验来看看它的表现，并与利用模型信息的方法进行对比。

我们在两个问题上进行了测试：

简单稳定系统：一个一维双积分器LQR问题。使用基本的策略梯度（配合基线技巧）或更先进的优化器（如Adam），经过大量采样（数万次 rollout）后，可以得到接近最优解的策略，尽管参数 K 可能不完全相同。
不稳定系统：一个线性化的二维四旋翼飞行器LQR问题。策略梯度方法很难收敛到一个稳定的控制器。因为初始随机策略会导致这个开环不稳定系统迅速失控，算法难以从这些“糟糕”的样本中学习到稳定策略。

作为对比，我们尝试了利用模型梯度的方法：即使我们仍然“不知道”精确模型，但如果我们能访问一个可微的模型（哪怕是近似的），并直接通过这个模型对成本函数 J(θ) 进行自动微分来获得精确梯度，然后使用梯度下降（甚至配合线搜索）。在这种方法下，仅需数百次迭代和少得多的数据，就能快速、精确地收敛到最优解，并且能轻松处理不稳定系统。

总结与思考

本节课中，我们一起学习了从控制视角理解强化学习，重点剖析了策略梯度方法。

核心：强化学习旨在解决无模型的最优控制问题。策略梯度方法通过向策略注入噪声、采样轨迹，并利用策略梯度定理来估计优化方向。
实践观察：在简单的稳定问题上，策略梯度可以工作，但需要大量样本，且对超参数（如基线、优化器）敏感。在不稳定或更复杂的问题上，其样本效率低和收敛困难的缺点更为明显。
关键洞见：在拥有模型信息（即使是近似模型）时，利用模型梯度的方法通常比完全无模型的策略梯度样本效率高得多，收敛更快、更稳定。
观点：许多在仿真中训练的“无模型”RL，其仿真器本身就是一个模型。忽略这个模型信息而进行黑盒采样，可能不是最有效的方式。将基于模型的控制思想与学习相结合，是未来提高机器人学习效率与可靠性的重要方向。

总结：强化学习，特别是策略梯度，为无模型控制提供了强大的框架。然而，在机器人等实际工程应用中，充分利用任何可获得的模型信息（包括可微仿真器），往往能带来显著的性能提升。理解这些方法的联系与权衡，对于设计和选择正确的控制与学习算法至关重要。

posted @ 2026-03-26 01:39 绝不原创的飞龙阅读(3) 评论(0) 收藏举报

刷新页面返回顶部

龙哥盟

掠夺·扩张·投机·博弈

CMU-16-745-最优控制与强化学习笔记-全-

CMU 16-745 最优控制与强化学习笔记（全）

1：HW0 环境配置与提交指南 🚀

概述

1. 接受GitHub Classroom作业

2. 安装Julia 1.6.5

3. 克隆作业仓库到本地

4. 配置环境并完成作业

4.1 安装IJulia包

4.2 完成作业文件

5. 提交作业

5.1 推送更改到GitHub

5.2 创建发布版本

总结

2：动力学回顾 🚀

1. 动力学系统的一般形式

2. 仿射控制系统

3. 机械臂动力学（拉格朗日形式）

4. 线性系统

5. 平衡点

6. 平衡点的稳定性

总结

3：动力学离散化与稳定性 🎯

概述

回顾：连续时间动力学与稳定性

为什么需要离散化？

显式离散时间动力学系统

前向欧拉积分法

离散时间系统的稳定性分析 🧐

应用于前向欧拉法

更好的数值积分方法

四阶龙格-库塔法（RK4）🏃‍♂️

隐式积分方法

后向欧拉法

控制输入的离散化

零阶保持

一阶保持（分段线性插值）

高阶插值

总结

4：数值优化的基础知识

概述 📚

符号约定 📝

求根算法 🔍

不动点迭代法

牛顿法

无约束最小化 📉

总结 🎯

5：优化（第二部分）🚀

概述

回顾与过渡：从无约束优化到线搜索

回溯线搜索

过渡：从无约束问题到等式约束优化

等式约束优化问题

一阶必要条件（KKT条件）

用牛顿法求解KKT系统

高斯-牛顿近似

过渡：从等式约束到不等式约束优化

不等式约束优化问题

一阶必要条件（KKT条件）

求解不等式约束问题的方法

总结

6：优化算法（第三部分）与二次规划 🚀

1. 回顾：惩罚法与KKT条件

2. 增广拉格朗日法 🛠️

2.1 基本思想

2.2 算法步骤

2.3 优势

3. 二次规划：模型预测控制的基石 📐

3.1 标准形式

3.2 求解与应用

4. 约束优化中的正则化与线搜索 ⚙️

4.1 KKT系统的正则化

4.2 约束优化中的线搜索

总结 🎯

7：确定性最优控制 🎯

概述：控制理论简史

确定性最优控制问题

连续时间形式