langxin11 · 学习笔记

使用 Mizuki 搭建 Astro 博客并部署到 GitHub Pages

Fri, 22 Aug 2025 00:00:00 GMT

这篇文章记录我在 2025 年使用 Mizuki 搭建 Astro 博客并部署到 GitHub Pages 的过程。主题配置文件和 Actions 版本会变化，下面的历史配置应结合所用主题版本核对。

**阅读路线：**准备本地环境 → 配置文章与站点 → 区分部署网址 → 使用 GitHub Actions 构建发布。

需要准备 Git、GitHub 账号，以及符合项目要求的 Node.js 和包管理器。版本要求以所用主题的 package.json 和官方说明为准。

基于Mizuki主题模板创建起始项目

原先使用的主题是 Mizuki，配置细节参考主题文档。

从模板创建自己的仓库，或 Fork 后克隆自己的仓库。只有用户/组织主页需要命名为 <用户名>.github.io；项目站点可以使用其他仓库名，网址通常多一层仓库路径。

git clone  https://github.com/你的用户名/你的仓库名.git my-blog
cd my-blog

本地配置与预览

优先使用项目声明的包管理器，避免混用 npm 和 pnpm 生成两份锁文件。以 pnpm 项目为例，在 Windows PowerShell 中可以使用：

pnpm.cmd install
pnpm.cmd dev

随后完成三类配置：

**站点信息：**标题、作者、语言和公开网址。旧版 Mizuki 可能集中在 src/config.ts，其他版本可能拆分到 src/config/，以实际项目为准。
**内容：**文章通常位于 src/content/posts/；当前博客已采用独立内容仓库，需要通过对应同步流程接入主题。
**静态资源：**核对头像、文章图片和封面路径，并在本地预览中逐项检查。

不要把含 posts/ 的内容仓库直接当作 Astro 程序运行；实际构建入口位于主题/站点项目。

部署Astro站点到GitHub Pages

区分用户站点与项目站点

Astro 的选项名是 site，不是 size。site 设置公开网址，base 用于指定子路径。用户站点部署在域名根目录时，一般不需要额外的 base：

import { defineConfig } from 'astro/config';

export default defineConfig({
  site: 'https://你的用户名.github.io',
});

如果部署的是名为 my-blog 的项目站点，配置示例为：

export default defineConfig({
  site: 'https://你的用户名.github.io',
  base: '/my-blog',
});

以上仅展示相关选项，应合并到现有配置中，保留主题的 integrations 等设置。自定义域名或主题自动生成配置时，应以最终访问地址和主题入口为准。

配置 GitHub Actions

下面按 2026-10-04 查阅的 Astro 官方部署示例更新 Actions 版本。旧文的 withastro/action@v3 和默认 Node 20 注释不应继续作为新建站点的当前配置；旧版本能否继续构建仍需结合项目验证。

示例适用于含 Astro 程序和锁文件的站点仓库。当前独立内容仓库还需要站点自己的内容同步步骤，以及内容更新后触发站点构建的机制；直接复制该工作流不会自动同步另一仓库的文章。Node 与包管理器应匹配主题要求，并提交相应锁文件。

在站点项目的 .github/workflows/ 创建 deploy.yml：

name: Deploy to GitHub Pages

on:
  # 每次推送到 `main` 分支时触发这个“工作流程”
  # 如果你使用了别的分支名，请按需将 `main` 替换成你的分支名
  push:
    branches: [ main ]
  # 允许你在 GitHub 上的 Actions 标签中手动触发此“工作流程”
  workflow_dispatch:

# 允许 job 克隆 repo 并创建一个 page deployment
permissions:
  contents: read
  pages: write
  id-token: write

jobs:
  build:
    runs-on: ubuntu-latest
    steps:
      - name: Checkout your repository using git
        uses: actions/checkout@v7
      - name: Install, build, and upload your site
        uses: withastro/action@v6
        # with:
          # path: . # 存储库中 Astro 项目的根位置。（可选）
          # node-version: 24 # 此版本 action 默认使用 24；若主题有其他要求，显式设置。
          # package-manager: pnpm@指定版本 # 按项目锁定的版本填写；默认会检测锁文件。

  deploy:
    needs: build
    runs-on: ubuntu-latest
    environment:
      name: github-pages
      url: ${{ steps.deployment.outputs.page_url }}
    steps:
      - name: Deploy to GitHub Pages
        id: deployment
        uses: actions/deploy-pages@v5

在 GitHub 上，跳转到存储库的 Settings 选项卡并找到设置的 Pages 部分
选择 GitHub Actions 作为你网站的 Source，然后按 Save。
提交（commit）这个新的“工作流程文件”（workflow file）并将其推送到 GitHub

如果已经克隆自己的仓库，远程地址通常已经存在，可以先检查，再提交并推送本次改动：

git remote -v
git add .github/workflows/deploy.yml
git commit -m "配置 GitHub Pages 部署"
git push

工作流中的分支名应与实际发布分支一致。若改动了站点配置，也需要把对应文件纳入提交。构建成功后，继续检查首页、文章页、图片、公式、搜索和子路径链接，而不只看 Actions 是否变绿。

常见问题

**图片或样式 404：**核对 base 是否匹配公开网址，以及资源路径是否支持子路径部署。
**内容更新未出现：**检查提交是否进入发布分支，独立内容仓库是否成功同步，构建是否重新执行。
**依赖安装失败：**核对 Node.js、包管理器版本和锁文件，不直接删除锁文件试错。
**主题升级后配置失效：**对照所用版本的配置说明重新映射设置。

整理与核查说明

核查状态：部分验证（2026-10-04）。 已核对官方部署方法；本轮迁移的实际部署结果另行记录，不把旧工作流示例当作当前模板。

2026-10-04 整理时修正了链接、命令和部署路径说明。文中的配置以当时的 Mizuki 使用经历为背景；主题与 Actions 版本会变化，实际部署仍须结合所用版本和仓库类型核对。

参考资料

WSL 2 无法启动与更新失败：一次修复记录

Sun, 10 Aug 2025 00:00:00 GMT

我遇到过一次 WSL 2 无法正常启动、更新也失败的故障。最后 Ubuntu 恢复了启动，但当时没有完整记录卸载、重新注册或恢复数据的过程，因此只能确认“环境恢复可用”，不能据此断言某一步就是根本修复原因。

故障现象

当时在 PowerShell 中执行 wsl、wsl -l -v 和 wsl --shutdown 都没有正常返回。尝试 wsl --update 时，安装程序报告旧版本无法移除：

正在更新适用于 Linux 的 Windows 子系统: 2.5.10。
The older version of Windows Subsystem for Linux cannot be removed.
Contact your technical support group.
更新失败(退出代码: 1603)。
错误代码: Wsl/UpdatePackage/ERROR_INSTALL_FAILURE

这里的错误码能确认更新安装失败，但不足以单独定位原因。安装器提示的日志文件应保留，用于后续排查。

先区分 WSL 程序、发行版和数据

WSL 运行组件、Ubuntu 发行版应用和发行版里的文件是不同层次的问题。处理启动故障之前，需要先确认数据可以恢复。

此前的笔记曾写“找到 ext4.vhdx，压缩为 tar 后即可卸载”。这个表述不准确：把 VHDX 文件打包成 tar，并不等于 wsl --export 导出的发行版 tar。 两者不能混用恢复命令。

如果 WSL 命令仍可用，可以用官方导出命令生成备份。下面是备份方式示例，并非这次故障中已经成功执行的记录；D:\WSL-Backup 需要提前创建。

wsl --shutdown
wsl --export Ubuntu-24.04 D:\WSL-Backup\Ubuntu-24.04.tar

如果命令已无响应，不能继续把上述导出命令当成可执行的备份方案。应先保留安装日志并排查 WSL 运行组件；若需复制原始 ext4.vhdx，必须先确保它未被挂载或写入，不能复制正在运行的发行版硬盘并宣称得到一致备份。备份的恢复方式需要用副本验证，不能在尚未确认可恢复时卸载发行版或执行 wsl --unregister；Microsoft 明确说明 unregister 会永久删除该发行版的数据、设置和软件。

官方分别提供 tar 导入和 VHDX 导入方式。恢复时应使用副本，并核对发行版名与存储位置，具体参数见 Microsoft 的导出与导入说明。

当时的处理经过

当时参考了褐瞳さん的 WSL 修复记录，尝试保留虚拟硬盘、卸载 Ubuntu 应用，并切换 Windows 的 WSL 功能后重启。

但笔记中没有记录可验证的备份、发行版重新注册及原数据恢复过程。这些操作可能涉及数据损失，因此不再把它们列为供读者照做的修复步骤。现有证据只能证明后来能进入 Ubuntu，不能证明这条操作链完整可靠，也不能确定哪一步解决了故障。

恢复后的验证

当时留下的日志显示，检查时的环境为：

项目	日志中的版本或状态
Windows	10.0.26100.4652
WSL	2.5.7.0
Linux 内核	6.6.87.1-microsoft-standard-WSL2
Ubuntu	24.04.2 LTS
Ubuntu-24.04	WSL 2，启动前状态为 Stopped
docker-desktop	Installing，不能据此确认 Docker 已恢复

用于验证的命令：

wsl --version
wsl -l -v
wsl -d Ubuntu-24.04

当时已经能够进入 Ubuntu 终端。注意，更新失败时尝试安装的是 2.5.10，恢复后显示的是 2.5.7.0；这说明“能启动”不等于“已成功升级到目标版本”。

这次排障留下的经验

保留报错、安装日志和修复前后的版本，区分启动恢复与更新成功。
备份不仅是“文件复制完成”，还要明确备份格式及恢复路径。
进入 shell 后，还应检查项目文件、软件环境和挂载是否完整；这些检查未在原日志中记录。
记录中没有 Docker Desktop 恢复的验证结果，也没有确定更新安装失败根因的依据。

整理与核查说明

核查状态：待复现（2026-10-04）。 保留为历史故障记录；未验证完整备份恢复链条，已撤下可照做的卸载重装步骤。

2026-10-04 整理时更正了 VHDX 与 tar 备份格式的说明，并区分了历史操作和可供读者参考的备份示例。未重新执行卸载、恢复或重装操作，也没有补充原日志中未记录的数据恢复结论。

参考资料

CMU 最优控制笔记 5：旋转矩阵、四元数与刚体姿态

Thu, 07 Aug 2025 00:00:00 GMT

学习三维姿态表示时，我把旋转矩阵和单位四元数放在一起比较。下面先明确坐标系约定，再整理角速度、姿态微分方程以及数值积分后的约束检查。

**先修知识：**线性代数、叉乘、刚体动力学与常微分方程数值积分。

阅读路线：

从世界坐标系与机体坐标系之间的变换理解旋转矩阵。
用四元数乘法、共轭与单位长度约束表示旋转。
对照两种姿态动力学实现，检查积分后的正交性和四元数模长。

Lecture 14 旋转

欧拉角直观，但在特定姿态存在参数化奇异性。旋转矩阵和单位四元数可以避免欧拉角的这类奇异性，代价是使用冗余参数并满足相应约束；此外，$q$ 与 $-q$ 表示同一旋转，四元数表示也不是唯一的。

本文约定 $Q$ 将机体系向量变换到世界系，四元数按“实部在前”排列，姿态运动学中的角速度用机体系表示。换用其他约定时，乘法顺序和符号都要重新核对。

旋转矩阵

三维空间坐标点在以$[\mathbf{e}{1},\mathbf{e}{2},\mathbf{e}{3}]$单位正交基组成的世界坐标系$\mathcal{N}$中的描述和在体坐标系$\mathcal{B}$（基底$[\mathbf{e}{1}',\mathbf{e}{2}',\mathbf{e}{3}']$）是一致的，即有

$$ [\mathbf{e}{1},\mathbf{e}{2},\mathbf{e}{3}]\left[\begin{array}{c} {^N}x_1 \ {^N}x_2 \ {^N}x_3 \ \end{array} \right] =[\mathbf{e}{1}',\mathbf{e}{2}',\mathbf{e}{3}']\left[ \begin{array}{c} {^B}x_1 \ {^B}x_2 \ {^B}x_3 \ \end{array} \right] $$

由于基底的正交性，可以得出在世界坐标系N和体坐标系下坐标旋转变换关系

$$ \left[ \begin{array}{c} {^N}x_1 \ {^N}x_2 \ {^N}x_3 \ \end{array} \right] = Q \left[ \begin{array}{c} {^B}x_1 \ {^B}x_2 \ {^B}x_3 \ \end{array} \right] $$

$Q$是旋转矩阵，是一个行列式为1的正交矩阵，它的逆就是它的转置，并且旋转矩阵构成一个$SO(3)$李群（特殊正交群）

$Q^\mathrm{T}Q=I$
$det(Q)=1$
$Q\in SO(3)$ special orthogonal in 3D 拓展：特殊欧式群（special Euclidean Group）SE(3)

$$ SE(3)={T=\left[\begin{array}{cc} R & t\ \mathbf{0}_{1\times3}&1 \end{array}\right] \in \mathbb{R}^{4\times4}|R\in SO(3),t\in \mathbb{R}^{3} } $$

描述陀螺仪的旋转

以陀螺仪举例，考虑固定在本身的体坐标系B和地面坐标系的坐标描述

$$ {^N}\mathbf{x} =Q(t){^B}\mathbf{x} $$

对时间t求导

$$ \begin{align}{^N}\dot {\mathbf{x} }&=\dot Q(t){^B}\mathbf{x}+Q(t) {^B}\dot {\mathbf{x}}\ &=\dot Q(t){^B}\mathbf{x} \end{align} $$

当物体以$\omega$的角速度旋转，那么${^N}\mathbf{x}$的导数

$$ {^N}\dot{x}={^N}\omega\times{^N}x=Q({^B}\omega\times{^B}x) $$

那么

$$ \dot Q(t){^B}\mathbf{x}=Q({^B}\omega\times{^B}x)\quad\Longrightarrow\quad\dot{Q}=Q\widehat{{^B}\omega} $$

$$ Q_{k+1} = Q_{k}+\dot{Q}_{k}\Delta t $$

四元数：一个实部与三个虚部

$$ \mathbf{q}=w+x\mathbf{i}+y\mathbf{j}+z\mathbf{k} $$

本文讨论的都是单位四元数

$$ ||\mathbf{q}||=\sqrt{w^2+x^2+y^2+z^2}=1 $$

也可以通过轴角描述(给定一个旋转轴$\mathbf{u}$和旋转角$\theta$)

$$ \mathbf{q} = [ cos\frac{\theta}{2},\mathbf{u}sin\frac{\theta}{2}] $$

四元数的乘法

$$ \begin{align} \mathbf{q}_1\otimes \mathbf{q}_2&=\left[ \begin{array}{c} {w}_1\ \mathbf{v}_1\ \end{array} \right]\otimes\left[ \begin{array}{c} {w}_2\ \mathbf{v}_2\ \end{array} \right]=\left[ \begin{array}{c} {w}_1{w}_2-\mathbf{v}_1\cdot \mathbf{v}_2\ {w}_1\mathbf{v}_2+{w}_2\mathbf{v}_1+\mathbf{v}_1\times \mathbf{v}_2\ \end{array} \right]\ &=\begin{bmatrix} w_1 &-\mathbf{v}_1^\mathrm{T}\ \mathbf{v}_1& w_1I+\hat{\mathbf{v}} _1 \end{bmatrix}\begin{bmatrix} w_2\\mathbf{v}_2\end{bmatrix}= L(\mathbf{q}_1)\begin{bmatrix} w_2\\mathbf{v}_2\end{bmatrix}\ &=\begin{bmatrix} w_2 &-\mathbf{v}_2^\mathrm{T}\ \mathbf{v}_2& w_2I-\hat{\mathbf{v}} _2 \end{bmatrix}\begin{bmatrix} w_1\\mathbf{v}_1\end{bmatrix}= R(\mathbf{q}_2)\begin{bmatrix} w_1\\mathbf{v}_1\end{bmatrix}\ \end{align} $$

旋转一个向量(使用纯虚四元数表示姿态向量$H\mathbf{v}=\begin{bmatrix}0 \ \mathbf{v}\end{bmatrix}$)

$$ \begin{align}H \mathbf{v}^{\prime} &=\mathbf{q}\otimes \begin{bmatrix}0 \ \mathbf{v}\end{bmatrix}\otimes \mathbf{q}^{-1}\ &= L(\mathbf{q})R(\mathbf{q})^TH\mathbf{v}\ & = R(\mathbf{q})^TL(\mathbf{q})H\mathbf{v} \end{align} $$

其中$R(\mathbf{q})$和$L(\mathbf{q})$四元数的左乘和右乘矩阵,从这里可以得到四元数和旋转矩阵的关系

$$ Q(\mathbf{q})=H^{\mathrm{T}}R(\mathbf{q})^TL(\mathbf{q})H $$

四元数的逆与共轭四元数的关系

$$ \mathbf{q}^{-1}=\frac{\mathbf{q}^*}{||\mathbf{q}||^2} $$

单位四元数有$\mathbf{q}^{-1}=\mathbf{q}^*$

用四元数表示刚体的姿态运动学quaternion Kinematics

$$ \dot{\mathbf{q}}=\frac{1}{2}L(\mathbf{q})H\boldsymbol{\omega} $$

那么完整的位置和姿态运动学方程和速度和角速度动力学方程如下

$$ \dot{\mathbf{x}} = \begin{bmatrix} \dot{\mathbf{r}} \ \dot{\mathbf{q}} \ \dot{\mathbf{v}} \ \dot{\boldsymbol{\omega}} \end{bmatrix} = \begin{bmatrix} \mathbf{v} \ \frac{1}{2}L(\mathbf{q})H\boldsymbol{\omega} \ \frac{1}{m} {}^W\mathbf{F}(\mathbf{x}, \mathbf{u}) \ \mathbf{J}^{-1} \left( {}^B\mathbf{\tau}(\mathbf{x}, \mathbf{u}) - \boldsymbol{\omega} \times \mathbf{J} \boldsymbol{\omega} \right) \end{bmatrix} $$

其中 $H\omega=[0;\omega]$ 为纯虚四元数，不能与 $3\times3$ 叉乘矩阵 $\hat\omega$ 混用。$^W\mathbf F$ 为世界系的总外力（有重力时应包含它），$^B\tau$ 为机体系的总外力矩，$J$ 为机体系中恒定的惯性矩阵。

刚体姿态动力学分析

使用旋转矩阵表示法（9参数）和四元数表示法（4参数）实现一个刚体姿态动力学仿真系统

核心库

using LinearAlgebra   # 线性代数运算
using ForwardDiff     # 自动微分

关键函数定义

<details> <summary>点击展开代码</summary>

# 向量的反对称矩阵
function hat(v)
   [0 -v[3] v[2];
    v[3] 0 -v[1];
    -v[2] v[1] 0]
end
function L(q)  # 四元数左乘矩阵
   s = q[1]
   v = q[2:4]
   [s    -v';
    v  s*I+hat(v)]
end

function R(q)  # 四元数右乘矩阵
   s = q[1]
   v = q[2:4]
   [s    -v';
    v  s*I-hat(v)]
end

</details>

初始条件

# 旋转矩阵表示
Q0 = I(3)         # 初始姿态(单位矩阵)
ω0 = randn(3)     # 随机初始角速度
x0 = [vec(Q0); ω0]

# 四元数表示
q0 = [1; 0; 0; 0] # 单位四元数(无旋转)
x0q = [q0; ω0]

动力学模型

# 旋转矩阵表示
function dynamics(x)
    Q = reshape(x[1:9],3,3)
    ω = x[10:12]

    Q̇ = Q*hat(ω)        # 姿态微分方程
    ω̇ = -J\(hat(ω)*J*ω) # 欧拉动力学方程

    [vec(Q̇); ω̇]
end
# 四元数
function qdynamics(x)
    q = x[1:4]
    ω = x[5:7]

    q̇ = 0.5*L(q)*H*ω    # 四元数微分方程
    ω̇ = -J\(hat(ω)*J*ω) # 欧拉动力学方程

    [q̇; ω̇]
end

数值积分方法RK4

function rkstep(x)
    f1 = dynamics(x)
    f2 = dynamics(x + 0.5*h*f1)
    f3 = dynamics(x + 0.5*h*f2)
    f4 = dynamics(x + h*f3)
    xn = x + (h/6.0)*(f1 + 2*f2 + 2*f3 + f4)
    # 四元数表示，额外进行归一化
    #xn[1:4] .= xn[1:4]./norm(xn[1:4])  # 保持单位四元数
end

仿真结果验证

旋转矩阵验证

Qk'*Qk  # 应保持正交性≈ I(3) 
3×3 Matrix{Float64}:
  0.962748    -0.00123354  -0.00149965
 -0.00123354   0.977413     0.0178415
 -0.00149965   0.0178415    0.984187

四元数验证

norm(qk)  # 应保持单位长度≈ 1.0 
0.9999999999999999
Q(qk)'*Q(qk)   # 转换矩阵应正交≈ I(3)
3×3 Matrix{Float64}:
 1.0          1.73472e-17  2.77556e-17
 1.73472e-17  1.0          0.0
 2.77556e-17  0.0          1.0

复现与数值误差

这里的 Julia 片段仍依赖未在本文完整列出的惯性矩阵 J、四元数嵌入矩阵 H、步长 h 和仿真循环；当前不应视为可以独立运行的完整程序。rkstep 默认调用旋转矩阵模型，切换到四元数时必须同时更换动力学函数与状态布局，不能只取消归一化注释。

原日志中的 $Q^\top Q$ 已明显偏离单位矩阵，说明直接积分没有严格保持正交性。这是需要检查的误差，而不是正交性验证通过。对四元数归一化有助于维持单位长度，但不能代替积分精度和动力学结果的验证。

整理与核查说明

本笔记原有许可为 CC BY 4.0。引用的课程材料、代码和图片仍须遵守其各自的许可。

核查状态：待复现（2026-10-04）。 已核对四元数顺序与坐标系说明；原旋转积分实验未重新运行。

2026-10-04 整理时修正了已定位的公式和实现问题。文中的图片、动画和输出保留自学习时的实验记录，不代表修订后的代码已经完整重跑。作业片段依赖原项目环境，不能直接作为完整可运行教程；具体核查范围与尚未复现事项见正文。

参考资料

Quaternions and Rotations∗
Planning With Attitude

CMU 最优控制笔记 1：动力学、离散化与数值优化

Mon, 28 Jul 2025 00:00:00 GMT

我从连续时间动力学开始整理这组学习笔记，梳理离散化、求导与数值优化的基础工具，为后续 LQR 和 MPC 做准备。

**先修知识：**线性代数、多元微积分，以及基本的 Julia 语法。

阅读路线：

先建立状态、输入和动力学方程，再讨论平衡点附近的线性化。
比较显式欧拉、RK4 和隐式中点法，关注离散化误差与稳定性。
结合作业理解自动微分、牛顿法及二次规划中的约束处理。

笔记结合 CMU 16-745 的课程与作业整理，相关材料见课程作业页面。文中的代码片段保留学习时的实现，运行时还需使用对应作业的依赖与上下文。

其他学习视角可参考向阳的笔记和知乎我爱科研的整理

Lecture 1：动力学、平衡点与线性化

以质量为 $m$、长度为 $l$、输入为关节力矩 $u$ 的理想单摆为例：

$$ m l^2\ddot{\theta}+mgl\sin\theta=u. $$

取状态 $x=[\theta,\dot\theta]^\top$，连续时间模型为

$$ \dot{x}=f(x,u)= \begin{bmatrix} x_2\ -\frac{g}{l}\sin x_1+\frac{u}{ml^2} \end{bmatrix}. $$

它对状态是非线性的，对输入则是仿射的。更一般地，输入仿射系统可以写成 $\dot{x}=f_0(x)+G(x)u$。

机械系统常用的动力学形式为

$$ M(q)\ddot q+C(q,\dot q)\dot q+g(q)=Bu, $$

其中 $M$ 为惯性矩阵，$C(q,\dot q)\dot q$ 表示科里奥利力与离心力项，$g(q)$ 为重力项。此前的笔记把加速度写成了速度，此处作了修正。

在平衡点 $(\bar x,\bar u)$ 附近，先令 $f(\bar x,\bar u)=0$，再定义扰动 $\delta x=x-\bar x$、$\delta u=u-\bar u$，得到一阶近似

$$ \delta\dot x\approx A\delta x+B\delta u,\qquad A=\left.\frac{\partial f}{\partial x}\right|{\bar x,\bar u},\quad B=\left.\frac{\partial f}{\partial u}\right|{\bar x,\bar u}. $$

在输入固定、模型足够光滑时，若 $A$ 的所有特征值实部严格为负，可判断该平衡点局部渐近稳定；出现零实部特征值时，仅靠这个线性化判据不能下结论。参考课程的动力学笔记。

Lecture 2：动力学离散化

以下假设每个采样区间内输入保持为 $u_k$，采样周期为 $h$。

显式欧拉法

$$ x_{k+1}=x_k+h f(x_k,u_k). $$

经典四阶 Runge–Kutta（RK4）

$$ \begin{aligned} k_1&=f(x_k,u_k),\ k_2&=f(x_k+\tfrac h2 k_1,u_k),\ k_3&=f(x_k+\tfrac h2 k_2,u_k),\ k_4&=f(x_k+h k_3,u_k),\ x_{k+1}&=x_k+\tfrac h6(k_1+2k_2+2k_3+k_4). \end{aligned} $$

此前的笔记遗漏了中间两项的系数，并把最后一项误写为 $k_3$；上式为修正后的表达。

隐式中点法

$$ x_{k+1}=x_k+h f\left(\frac{x_k+x_{k+1}}2,u_k\right). $$

由于右端含有未知的 $x_{k+1}$，每一步通常需要求解隐式方程。原先写的 $x_{k+1}=x_k+h f(x_{k+1},u_k)$ 是后向欧拉法，不是隐式中点法。

比较积分方法时，应同时考察步长、误差、稳定性和每一步的求解成本。课程材料见 Dynamics Discretization & Stability。

Lecture 3：数值优化基础

基础概念（梯度，雅可比矩阵，Hessian矩阵）

标量函数$ f:\mathbb{R} ^n→\mathbb{R}$的梯度向量(Gradient)

$$ \nabla f(\mathbf{x} )=\left [ \frac{\partial f}{\partial x_1}, \frac{\partial f}{\partial x_2},..,\frac{\partial f}{\partial x_n}\right ]^\top $$

示例$f(x,y)=x^2+y^3$的梯度为$\nabla{f}=\left[2x,3y^2\right]^\top$

向量值函数$\mathbf{F}(\mathbf{x}):\mathbb{R} ^n→\mathbb{R}^m$的雅可比矩阵(Jacobian)

$$ \mathbf{J}_{\mathbf{F}} = \begin{bmatrix} \frac{\partial f_1}{\partial x_1} & \cdots & \frac{\partial f_1}{\partial x_n} \ \vdots & \ddots & \vdots \ \frac{\partial f_m}{\partial x_1} & \cdots & \frac{\partial f_m}{\partial x_n} \end{bmatrix} $$

$\mathbf{F}(x, y) = \begin{bmatrix} x^2 y \ \sin y \end{bmatrix}$的雅可比矩阵为

$$ \mathbf{J} = \begin{bmatrix} 2xy & x^2 \ 0 & \cos y \end{bmatrix} $$

标量函数$ f:\mathbb{R} ^n→\mathbb{R}$的Hessian 矩阵：$n\times n$对称矩阵

$$ \mathbf{H}_f = \begin{bmatrix} \frac{\partial^2 f}{\partial x_1^2} & \cdots & \frac{\partial^2 f}{\partial x_1 \partial x_n} \ \vdots & \ddots & \vdots \ \frac{\partial^2 f}{\partial x_n \partial x_1} & \cdots & \frac{\partial^2 f}{\partial x_n^2} \end{bmatrix} $$
- 标量函数梯度的雅可比矩阵即是Hessian矩阵
- $m=1$时，雅可比矩阵退化为梯度的转置
- 行主导和列主导向量函数的复合求导会导致链式法则的式子不一样，由于矩阵的维度不一致)

$$ J_{F\circ g}(\mathbf{x})=J_F(g(\mathbf{x}))J_g(\mathbf{x}) $$

HW1-Q1

Julia 求导————————ForwardDiff.jl

标量函数$f(x)$对标量$x$的导数

import ForwardDiff as FD
function f(x)
    return x^2
end
x=randn()
dx=FD.derivative(f,x)

标量函数$f(x)$对向量$X=[x_1,x_2,...,x_n]$的导数--雅可比矩阵/梯度
向量函数$f(X)=[f_1(X),f_2(X),...,f_m(X),]$对向量$X=[x_1,x_2,...,x_n]$的Jacobian矩阵

求解一个非线性方程$f(x)=0$的方法（求根）

不动点迭代法

离散系统动力学求平衡点

$$ \begin{gathered} x_{k+1}=f(x_k,u_k)\ f^*=x-f \end{gathered} $$
Newton方法

$$ \begin{gathered} f(x+dx)=f(x)+\frac{df}{dx}\Delta x=0\ \Delta x= -\frac{df}{dx}^{-1}f\ x \leftarrow x+\Delta x\ \text{Loop until convergence} \end{gathered} $$

在无约束优化问题中，可以用 Newton 法求解 $\nabla f(x)=0$（一阶必要条件）；驻点是否为局部极小值还需要进一步判断

HW1_S25_Q3 QP求解器（Log-Domain Interior Point Method）

$$ \begin{align} \min_x \quad & \frac{1}{2}x^TQx + q^Tx \ \text{s.t.}\quad & Ax -b = 0 \ & Gx - h \geq 0 \end{align} $$

引入拉格朗日乘子

$\mu \in \mathcal{R}^p$ 对应等式约束
$\lambda \in \mathcal{R}^m$对应不等式约束（要求 $\lambda≥ 0$）

拉格朗日函数为：

$$ \mathcal{L}(x, \mu,\lambda) = \frac{1}{2}x^\top Q x + q^\top x

\mu^\top (Ax - b)

\lambda^\top (Gx - h) $$

KKT条件：梯度条件、原始可行性、对偶可行性、互补松弛条件

$$ \begin{align} Qx+q+A^\top\mu - G^\top \lambda&= 0 \quad \quad \text{(stationarity)} \ Ax - b&= 0 \quad \quad \text{(primal feasibility)} \ Gx - h &\geq 0 \quad \quad \text{(primal feasibility)} \ \lambda &\geq 0 \quad \quad \text{(dual feasibility)} \ \lambda \circ(Gx - h) &= 0 \quad \quad \text{(complementarity)} \end{align} $$

引入非负松弛变量 $s\geq0$，使得 $Gx-h=s$；内点迭代时取 $s>0$。

新的拉格朗日函数

$$ \mathcal{L}(x, \lambda, \mu) = \frac{1}{2}x^\top Q x + q^\top x

\mu^\top (Ax - b)
\lambda^\top [s-(Gx - h)] $$

$$ \begin{align} Qx+q+A^\top\mu - G^\top \lambda&= 0 \quad \quad \text{(stationarity)} \ Ax - b&= 0 \quad \quad \text{(primal feasibility)} \ Gx - h-s &=0 \quad \quad \text{(primal feasibility)} \ \lambda &\geq 0 \quad \quad \text{(dual feasibility)} \ s &\geq 0\ \lambda \circ s &= \rho\mathbf{1} \quad \quad \text{(perturbed complementarity)} \end{align} $$

这里使用 $\rho>0$ 的扰动互补条件；原始 KKT 的右侧应为零。令 $\lambda=\sqrt{\rho}e^{-\sigma},s=\sqrt{\rho}e^{\sigma}$（指数逐元素计算），得到内点中心路径上的方程。需要逐步减小 $\rho$ 才能逼近原问题，不能固定一个正数就宣称解满足原始互补条件。

$$ \begin{align} Qx+q+A^\top\mu - G^\top \sqrt{\rho}e^{-\sigma}&= 0 \quad \quad \text{(stationarity)} \ Ax - b&= 0 \quad \quad \text{(primal feasibility)} \ Gx - h-\sqrt{\rho}e^{\sigma} &=0 \quad \quad \text{(primal feasibility)} \ \end{align} $$

定义关于 $z=[x;\mu;\sigma]$ 的残差向量 $r_\rho(z)$，及其 Jacobian $D r_\rho(z)$。残差向量不是 Jacobian，下面的分块矩阵也不是目标函数的 Hessian。

$$ r_\rho(z)=\begin{bmatrix} Qx+q+A^\top\mu - G^\top \sqrt{\rho}e^{-\sigma}&\ Ax - b\ Gx - h-\sqrt{\rho}e^{\sigma}\ \end{bmatrix} $$

$$ D r_\rho(z)=\begin{bmatrix} Q& A^\top& G^\top \text{diag}(\sqrt{\rho}\odot e^{-\sigma})\ A&\mathbf{0}&\mathbf{0}\ G&\mathbf{0} &-\text{diag}( \sqrt{\rho}e^{\sigma}) \ \end{bmatrix} $$

每个 Newton 步求解 $D r_\rho(z)\Delta z=-r_\rho(z)$，再配合线搜索和中心路径参数更新。这是算法推导，尚未构成完整求解器；终止时还需检查原始可行性、对偶可行性和互补残差。这里假设 $Q$ 对称半正定；一般非凸 QP 的 KKT 驻点不能直接认定为全局最优解。

这里的$P_0$和$P_1$代表原始KKT和IP_KKT的残差，也就是

$$ \begin{align} P_0=\begin{bmatrix} Qx+q+A^\top\mu - G^\top \lambda&\ Ax - b&\ min.(Gx - h,\mathbf{0})\ min.(\lambda,\mathbf{0})\ \lambda \circ(Gx - h) & \end{bmatrix} \end{align} $$

$$ \begin{align} P_1=\begin{bmatrix} Qx+q+A^\top\mu - G^\top \lambda&\ Ax - b&\ Gx - h-s\ \end{bmatrix} \end{align} $$

砖块掉落仿真

不考虑砖块的旋转，一个掉落的砖块的动力学方程可以写成

$$ \begin{gathered} M \dot{v} + M g = J^T \mu \ \text{ where } M = mI_{2\times 2}, ; g = \begin{bmatrix} 0 \ 9.81 \end{bmatrix},; J = \begin{bmatrix} 0 & 1 \end{bmatrix} \end{gathered} $$

其中，$v=[v_x;v_z]$ 是速度，$q=[q_x;q_z]$ 是位置，竖直向上为正；$\mu$ 是法向接触力。这里忽略旋转、摩擦和反弹，采用非穿透接触与 backward Euler 离散，不能直接当作一般刚体碰撞模型。

$$ \begin{bmatrix} v_{k+1} \ q_{k+1} \end{bmatrix} = \begin{bmatrix} v_k \ q_k \end{bmatrix}+ \Delta t \cdot \begin{bmatrix} \frac{1}{m} J^T \mu_{k+1} - g \ v_{k+1} \end{bmatrix} $$

约束

$$ \begin{align} J q_{k+1} &\geq 0 &&\text{(不会穿透地面)} \ \mu_{k+1} &\geq 0 &&\text{(接触力方向只向上)} \ \mu_{k+1} J q_{k+1} &= 0 &&\text{(没接触无接触力)} \end{align} $$

等价转换成如下的QP问题(可以通过KKT条件证明)

$$ \begin{align} &\text{minimize}{v{k+1}} && \frac{1}{2} v_{k+1}^T M v_{k+1} + [M (\Delta t \cdot g - v_k)]^Tv_{k+1} \ &\text{subject to} && J(q_k + \Delta t \cdot v_{k+1}) \geq 0 \ \end{align} $$

引入拉格朗日乘子$\mu \geq 0$

$$ \mathcal{L}=\frac{1}{2} v_{k+1}^T M v_{k+1} + [M (\Delta t \cdot g - v_k)]^Tv_{k+1} -\mu \cdot J(q_k + \Delta t \cdot v_{k+1}) $$

KKT 条件

$$ \begin{align} Mv_{k+1} + M(\Delta{t}\cdot g-v_k)-J^\mathrm{T}\mu\Delta t&=0\qquad\text{梯度条件}\ J(q_k + \Delta t \cdot v_{k+1}) &\geq 0\qquad\text{原始可行性}\ \mu &\geq 0\qquad\text{对偶可行性}\ \mu \cdot J(q_k + \Delta t \cdot v_{k+1})&=0\qquad\text{互补松弛 } \end{align} $$

引入间隙松弛变量 $s=J(q_k+\Delta t,v_{k+1})$，并令 $\mu=\sqrt{\rho}e^{-\sigma}$、$s=\sqrt{\rho}e^{\sigma}$，使 $\mu s=\rho$。这是与上文一致的数值参数化；在本例中 $\rho$ 带有接触力乘间隙的单位，实际实现还应考虑尺度归一化。

$$ \begin{align} Mv_{k+1} + M(\Delta{t}\cdot g-v_k)-J^\mathrm{T}\sqrt{\rho}e^{-\sigma}\Delta t&=0\ J(q_k + \Delta t \cdot v_{k+1}) -\sqrt{\rho}e^{\sigma}&= 0\

\end{align} $$

Julia 简介

下载：windows 通过winget下载juliaup下载指定julia版本

在 VS Code 中安装 Julia 扩展，并检查自动发现的 Julia 路径。只有日志明确提示缺少 Juliaup release 通道时，才按提示补装；这不是所有补全故障的通用修复。
```
winget install --name Julia --id 9NJNWW8PVKMN -e -s msstore
juliaup status
# 若扩展提示缺少 release 通道，再执行 juliaup add release
```
安装命令见 Juliaup 官方说明。release 随时间变化，课程复现应保留原项目的 Project.toml、Manifest.toml 和 Julia 版本。

Julia 项目环境创建（管理依赖，不等同于隔离整个解释器的 Python 虚拟环境）

#创建虚拟环境
using Pkg
Pkg.activate(@__DIR__)
#初始化虚拟环境
Pkg.instantiate()
#安装包
Pkg.add("Plots")
#检查虚拟环境状态
Pkg.status()

pyplot的使用

# 在 Julia 中执行（替换为你的 Python 路径）
ENV["PYTHON"] = raw"C:\Python39\python.exe"  # Windows 示例
# ENV["PYTHON"] = "/usr/bin/python3"        # Linux/macOS 示例

# 重新构建 PyCall
using Pkg
Pkg.build("PyCall")
# 显示图片
display(gcf())

eltype和typeof

#返回容器（collection）里元素的类型（数组、向量、矩阵、迭代器）
eltype([1, 2, 3])          # Int64
eltype([1.0, 2.0])         # Float64
eltype(["a", "b"])         # String
eltype(1:10)               # Int64
eltype(1.0:0.1:2.0)        # Float64
#返回某个值的具体类型
typeof(1)            # Int64
typeof(1.0)          # Float64
typeof("hello")      # String
typeof(true)         # Bool

向量集合与矩阵的转换

将等长向量按列拼成矩阵，再用 eachcol 拆回列向量：

columns = [[1, 2], [3, 4]]
A = hcat(columns...)
restored = [collect(column) for column in eachcol(A)]

整理与核查说明

本笔记原有许可为 CC BY 4.0。引用的课程材料、代码和图片仍须遵守其各自的许可。

核查状态：部分验证（2026-10-04）。 已核对主要公式，并用小型数值例子检验 KKT Jacobian；整套课程作业未重新运行。

CMU 最优控制笔记 2：LQR、Riccati 递推与轨迹跟踪

Mon, 28 Jul 2025 00:00:00 GMT

学习线性二次型调节器（LQR）时，我把二次规划和 Riccati 递推放在一起整理：同一个最优控制问题，既可以写成二次规划，也可以利用时间结构通过 Riccati 递推求解。

**先修知识：**离散状态空间模型、二次型、矩阵求导与等式约束优化。

阅读路线：

先明确代价、动力学约束和初始状态，比较 QP 与 Riccati 两种求解思路。
再区分有限时域、无限时域和平衡点附近的局部控制。
最后阅读非线性系统案例及 TVLQR 轨迹跟踪，观察线性化的适用范围。

笔记结合 CMU 16-745 的课程与作业整理，相关材料见课程作业页面。文中的代码片段保留学习时的实现，运行时还需使用对应作业的依赖与上下文。

其他学习视角可参考向阳的笔记和知乎我爱科研的整理

LQR：二次规划与 Riccati 递推

按2025 年课程目录，LQR in 3 Ways 对应 Lecture 8；此前的笔记记为 Lecture 9，此处按主题组织。

线性二次型最优控制问题

$$ \begin{align} \min_{x_{1:N},{u}{1:N-1}}& \quad \sum{k=1}^{N-1}(\frac{1}{2}{x_k}^TQx_k + \frac{1}{2}{u_k}^TRu_k)+\frac{1}{2}{x_N}^TQ_Nx_N \ \text{s.t.}&\quad x_{k+1}=A_kx_k+B_ku_k,\quad x_1=x_{\mathrm{init}} \end{align} $$

其中，$Q\succeq 0,R\succ 0$。

根据$A,B,Q,R$是否随时间变化可以分为时不变和时变LQR，时不变 LQR 常用于平衡点附近的稳定控制，TVLQR 用于轨迹跟踪
可以在线性化点附近设计非线性系统的局部控制器；一次线性化得到的 LQR 解不等于原非线性最优控制问题的全局解。

将LQR转换为一个标准的QP问题求解

定义优化变量$z$

$$ z=\begin{bmatrix} u_1 \ x_2 \ u_2 \ . \ . \ x_N \end{bmatrix} $$

$J=\frac{1}{2}z^THz$

$$ H= \begin{bmatrix} R_1 & 0 & ... & 0 \ 0 & Q_2 & ... & 0 \ & & . \ 0 & 0 & ... & Q_N \end{bmatrix} $$

$Cz=d$

$$ \begin{aligned} & C= \begin{bmatrix} B_1 & (-I) & ... & ... & ... & 0 \ 0 & A & B & (-I) & ... & 0 \ & & . \ 0 & 0 & ... & A_{N-1} & B_{N-1} & (-I) \end{bmatrix} \ & d= \begin{bmatrix} -A_1x_1 \ 0 \ . \ 0 \end{bmatrix} \end{aligned} $$

这样就形式上转换成了一个标准的QP问题

$$ \begin{aligned} & \min_z\frac{1}{2}z^THz \ & s.t.\quad Cz=d \end{aligned} $$

通过引入拉格朗日乘子给出拉格朗日函数，得到 KKT 线性方程组，可直接求解该等式约束二次规划；数值实现时还需检查相应矩阵的可解性

$$ \begin{bmatrix} H & C^T \ C & 0 \end{bmatrix} \begin{bmatrix} z \ \lambda \end{bmatrix}= \begin{bmatrix} 0 \ d \end{bmatrix} $$

Riccati 方程求解（利用KKT中的稀疏性）

下面先用 $N=4$、时不变 $A,B,Q,R$ 的示意例子推导，终端权重为 $Q_N$。时变情形需要给各步矩阵加上下标。

$$ \begin{bmatrix} R & & & & & & . & B^T & \ & Q & & & & & . & -I & A^T \ & & R & & & & . & & B^T \ & & & Q & & & . & & -I & A^T \ & & & & R & & . & & & B^T \ & & & & & Q_N & . & & & -I \ . & . & . & . & . & . & . & . & . & . \ B & -I & & & & & . & 0 & 0 & 0 \ & A & B & -I & & & . & 0 & 0 & 0 \ & & & A & B & -I & . & 0 & 0 & 0 \end{bmatrix} \begin{bmatrix} u_1 \ x_2 \ u_2 \ x_3 \ u_3 \ x_4 \ \lambda_2 \ \lambda_3 \ \lambda_4 \end{bmatrix}= \begin{bmatrix} 0 \ 0 \ 0 \ 0 \ 0 \ 0 \ -Ax_1 \ 0 \ 0 \end{bmatrix} $$

从末态$x_4$开始

$$ Q_Nx_4-\lambda_4=0 \Longrightarrow\lambda_4 =Q_Nx_4 $$

考虑$u_3$(分别代入$\lambda_4 =Q_Nx_4$和$x_4=Ax_3+Bu_3$)

$$ \begin{aligned} &Ru_3+B^T\lambda_4=Ru_3+B^TQ_Nx_4=Ru_3+B^TQ_N(Ax_3+Bu_3)=0\ &\Longrightarrow u_3 =-(R+B^TQ_NB)^{-1}B^TQ_NAx_3 \end{aligned} $$

记成$u_3=-K_3x_3$

到$x_3$

$$ \begin{align} &Qx_3-\lambda_3+A^T\lambda_4=0\ \Longrightarrow& Qx_3-\lambda_3+A^TQ_Nx_4=0\ \Longrightarrow& Qx_3-\lambda_3+A^TQ_N(Ax_3+Bu_3)=0\ \Longrightarrow& Qx_3-\lambda_3+A^TQ_N(A-BK_3)x_3=0\ \Longrightarrow& \lambda_3= [Q+A^TQ_N(A-BK_3)]x_3\ \end{align} $$

记为$\lambda_3=P_3x_3$

这样依次递推出$K_n$和$P_n$，便可求出控制序列$u_{1:N-1}$

$$ \begin{align} P_N& = Q_N\ K_n& = (R+B^TP_{n+1}B)^{-1}B^TP_{n+1}A\ P_n& = Q+A^TP_{n+1}(A-BK_n) \end{align} $$

数值实现应求解线性方程组来计算增益，避免显式计算逆矩阵。时变形式为 $K_n=(R_n+B_n^\top P_{n+1}B_n)^{-1}B_n^\top P_{n+1}A_n$、$P_n=Q_n+A_n^\top P_{n+1}(A_n-B_nK_n)$。

例子-HW2 Q1

Part A 离散化动力学模型

考虑一个二阶积分系统，状态和控制变量如下

$$ \begin{align} x &= [p_1, p_2, v_1, v_2] \ u &= [a_1, a_2] \end{align} $$

状态空间方程$\dot{x}=Ax+Bu$

$$ \begin{align} \dot{x} = \begin{bmatrix} 0 & 0 & 1 & 0 \ 0 & 0 & 0 & 1 \ 0 & 0 & 0 & 0 \ 0 & 0 & 0 & 0 \end{bmatrix} x + \begin{bmatrix} 0 & 0 \ 0 & 0 \ 1 & 0 \ 0 & 1 \end{bmatrix} u\end{align} $$

离散状态空间方程$x_{k+1}=A_dx_k+B_du_k$

在输入零阶保持、$A^2=0$ 时，$A_d=I+A\Delta t$、$B_d=(I\Delta t+A\Delta t^2/2)B$。原式遗漏右乘 $B$，会使输入矩阵的维度错误。对本例：

$$ A_d=\begin{bmatrix}I_2&\Delta t I_2\0&I_2\end{bmatrix},\qquad B_d=\begin{bmatrix}\frac12\Delta t^2 I_2\\Delta t I_2\end{bmatrix}. $$

Part B: Finite Horizon LQR via Convex Optimization

定义性能指标和约束方程，使用 Convex.jl求解得到 Xcvx,Ucvx = convex_trajopt(A,B,Q,R,Qf,N,x_ic)

$$ \begin{align} \min_{x_{1:N},u_{1:N-1}} \quad & \sum_{i=1}^{N-1} \bigg[ \frac{1}{2} x_i^TQx_i + \frac{1}{2} u_i^TRu_i \bigg] + \frac{1}{2}x_N^TQ_fx_N\ \text{st} \quad & x_1 = x_{\text{IC}} \ & x_{i+1} = A x_i + Bu_i \quad \text{for } i = 1,2,\ldots,N-1 \end{align} $$

初态$x_{ic} = [5,7,2,-1.4]$

**验证 Bellman 最优性原理：**从原最优轨迹在时刻 $L$ 的状态 $x_L^*$ 出发，保留剩余时域、代价和约束，原最优控制序列的后缀仍是这个子问题的最优解。这不表示系统在任意中间时刻都已到达平衡点；若解不唯一，也不能要求重新求解后的轨迹必然逐点相同。

$$ \begin{align} \min_{x_{L:N},u_{L:N-1}} \quad & \sum_{i=L}^{N-1} \bigg[ \frac{1}{2} x_i^TQx_i + \frac{1}{2} u_i^TRu_i \bigg] + \frac{1}{2}x_N^TQ_fx_N\ \text{st } \quad & x_L = x^*L \ & x{i+1} = A x_i + Bu_i \quad \text{for } i = L,L + 1,\ldots,N-1 \end{align} $$

Part C：Finite-Horizon LQR via Riccati

使用Riccati 方程递推求解离散LQR问题的解析解并与convex.jl的求解结果进行对比，结果是一致的

多次随机初始状态结果也是一致的

Part D: Why LQR is so great LQR的优异性

求出控制序列后，给实际的动力学系统加入噪声

$$ x_{k+1} = Ax_k + Bu_k + \text{noise} $$

noise = [.005*randn(2);.1*randn(2)]

此处比较的是一次求解后直接执行的开环控制序列，与每步根据实际状态计算的反馈控制 $u_k=-K_kx_k$。抗扰效果来自反馈；对于同一个无约束 LQR 问题，QP 与 Riccati 是等价的求解方法，不能据此认定 Riccati 求解器本身更鲁棒。QP 若每步重新求解，也能形成反馈。

设定非零目标 $x_{goal}=[-3.5,-3.5,0,0]$。对本例双积分系统，它在零输入下仍是平衡点，因此可在误差坐标中使用 $u=-K(x-x_{goal})$。一般系统需要先求满足平衡条件的 $(x_{goal},u_{goal})$，再使用 $u=u_{goal}-K(x-x_{goal})$。

Part E: Infinite -horizon LQR 无限时间二次型调节问题

矩阵$P K$各元素的变化如图所示，可以看出对于一个有限时间的LQR系统，矩阵$P(n_x\times n_x)$和$K(n_u\times n_x)$在Ricatti反向迭代的过程中，经过一段时间就很快收敛。

对于时不变无限时域 LQR，在 $(A,B)$ 可稳定、$(Q^{1/2},A)$ 可检测且 $R\succ0$ 等标准条件下，可得到稳定化 Riccati 解和常数增益 $K$。有限时域曲线在本例中趋于平稳，不表示任意系统都能快速收敛。

例子-HW Q2 LQR for nonlinear systems

Part 0 预备知识

非线性系统线性化

给定参考状态轨迹$\bar{x}{1:N}$和参考控制轨迹$\bar{u}{1:N-1}$，定义增量坐标

$$ x_k=\bar{x}_k+\Delta x_k,u_k=\bar{u}_k+\Delta u_k. $$

对离散非线性动力学系统$x_{k+1}=f(x_k,u_k)$进行线性化（一阶泰勒展开）

$$ x_{k+1}\approx f(\bar{x}_k,\bar{u}k)+\underbrace{\frac{\partial f}{\partial x}|{\bar{x}k,\bar{u}k}}{A_k}\Delta x_k+\underbrace{\frac{\partial f}{\partial u}|{\bar{x}_k,\bar{u}k}}{B_k}\Delta u_k $$

其中，$A_k$是状态雅可比矩阵$(n_x\times n_x)$，$B_k$是控制雅可比矩阵$(n_x\times n_u)$

如果参考轨迹是动态可行的（即满足$\bar{x}_{k+1}=f(\bar{x}_k,\bar{u}_k)$），则泰勒展开式可简化为

$$ \bar{x}{k+1}+\Delta x{k+1}\approx f(\bar{x}_k,\bar{u}_k)+A_k \Delta x_k+B_k \Delta u_k $$

得到增量动力学方程

$$ \Delta x_{k+1}\approx A_k \Delta x_k+B_k \Delta u_k $$

线性时不变系统离散化方法

$$ \dot{x}(t)=Ax(t)+Bu(t) $$

连续系统的解为

$$ x(t)=e^{A(t-t_0)}x(t_0)+\int_{t_0}^{t}e^{A(t-\tau)}Bu(\tau)d\tau $$

使用零阶保持器(ZOH)控制$u(t)=u_k$在$t\in[t_k,t_{k+1}]$，则

$$ x_{k+1}=e^{A\Delta t}x_k+(\int_{0}^{\Delta t}e^{A\tau}d\tau)Bu_k $$

构造增广系统

$$ \frac{d}{dt} \begin{bmatrix} x(t) \ u(t) \end{bmatrix}= \begin{bmatrix} A & B \ 0 & 0 \end{bmatrix} \begin{bmatrix} x(t) \ u(t) \end{bmatrix} $$

在ZOH假设下$\dot{u}(t)=0$，计算该系统的状态转移矩阵

$$ \exp\left( \begin{bmatrix} A & B \ 0 & 0 \end{bmatrix}\Delta t\right)= \begin{bmatrix} e^{A\Delta t} & \int_{0}^{\Delta t}e^{A\tau}d\tau B \ 0 & I \end{bmatrix} $$

通过计算增广矩阵的指数

$$ \exp\left(\begin{bmatrix}A&B\0&0\end{bmatrix}\Delta t\right)=\begin{bmatrix}A_k&B_k\0&I\end{bmatrix} $$

$A_k$为左上$n\times n$块
$B_k$为右上$n\times m$块

Part A: Infinite Horizon LQR about an equilibrium

小车倒立摆（CartPole）的动力学方程

$$ H(q)\ddot{q}+C(q,\dot{q})\dot{q}+G(q)=Bu $$

广义坐标$q=[p,\theta]$
质量惯性矩阵$H= \begin{bmatrix} m_c+m_p & m_pl\cos\theta \ m_pl\cos\theta & m_pl^2 \end{bmatrix}$
科里奥利矩阵$C= \begin{bmatrix} 0 & -m_pl\dot{\theta}\sin\theta \ 0 & 0 \end{bmatrix}$
重力向量$G=\begin{bmatrix}0\m_pgl\sin\theta\end{bmatrix}$（此处 $\theta=0$ 为摆向下）
输入映射矩阵$B=\begin{bmatrix}1\0\end{bmatrix}$

使用Infinite Horizon LQR将倒立摆小车稳定到平衡位置

xgoal = [0, pi, 0, 0]
x0 = [0, pi, 0, 0] + [1.5, deg2rad(-20), .3, 0]

Part B : Basin of Attraction吸引域分析

LQR控制器是基于系统在$(x_{goal},u_{goal})$处的线性近似设计的，当系统状态远离线性化点时，真实非线性动力学与线性模型差异变大，导致控制器性能下降甚至失效。

这里测试LQR控制器在不同初始条件下的稳定性，绘制吸引域，即能成功稳定的初始状态范围。

# create a span of initial configurations 
M=20
ps = LinRange(-7, 7, M)
thetas = LinRange(deg2rad(180-60), deg2rad(180+60), M)

Part C : 无限时域LQR调参

本例通过调整 $Q,R$ 并对输入限幅，检查给定初态下 5 秒末的误差是否小于 0.1。LQR 本身不显式处理 $-3\le u\le3$；截断控制会改变闭环系统，某次仿真满足条件不能保证其他初态也满足。需要硬约束时，应采用相应的约束优化控制方法。

Part D: TVLQR for trajectory tracking

在参考轨迹的每个点上线性化系统，设计时变反馈增益$K(t)$，使系统能稳定跟踪时变目标。

function TVlqr(A_list::Vector{Matrix{Float64}}, B_list::Vector{Matrix{Float64}}, Q::Matrix, R::Matrix,
        Qf::Matrix,N::Int64)::Tuple{Vector{Matrix{Float64}}, Vector{Matrix{Float64}}}
  
        nx, nu = size(B_list[1])
  
        P = [zeros(nx,nx) for i = 1:N]
        K = [zeros(nu,nx) for i = 1:N-1]
        P[N] = deepcopy(Qf)
        for i = N-1:-1:1
            A,B = A_list[i],B_list[i]
            K[i] = (R + B'*P[i+1]*B) \ (B'*P[i+1]*A)
            P[i] = Q + A'*P[i+1]*(A - B*K[i])  
        end
        return P,K
    end

整理与核查说明

本笔记原有许可为 CC BY 4.0。引用的课程材料、代码和图片仍须遵守其各自的许可。

核查状态：部分验证（2026-10-04）。 已核对离散化、LQR 与二次规划关系及维度；完整摆杆实验未重新运行。

CMU 最优控制笔记 3：凸 MPC、航天器交会与无人机悬停

Mon, 28 Jul 2025 00:00:00 GMT

学习模型预测控制（MPC）时，我主要关注模型、代价和约束如何组成一个滚动求解的问题。这篇笔记重点整理凸 MPC，以及它与一次性轨迹优化和 LQR 的区别。

先修知识： LQR、线性系统离散化、凸二次规划与基本的稀疏矩阵运算。

阅读路线：

用航天器交会案例对照 LQR、凸轨迹优化和凸 MPC。
通过平面无人机悬停，连接非线性模型、局部线性化与离散模型。
推导堆叠预测矩阵，再把代价和约束整理为 OSQP 所需的形式。

笔记结合 CMU 16-745 的课程与作业整理，相关材料见课程作业页面。文中的代码片段保留学习时的实现，运行时还需使用对应作业的依赖与上下文。

其他学习视角可参考向阳的笔记和知乎我爱科研的整理

Lecture 10：凸模型预测控制

LQR（线性二次调节器）是控制理论中的经典方法，但存在一些局限

仅适用于线性系统和局部线性化的非线性系统
代价函数需要是二次型
无法直接处理控制输入约束或状态约束

MPC通过滚动优化克服LQR的局限性，在每一个时间步求解一个有限时域的优化问题，考虑未来若干步的动力学和约束，并仅应用优化结果的第一步控制输入，下一时间步重新优化，具有以下优势

显式处理约束：将控制限幅、状态约束直接写入优化问题
可扩展性：MPC 框架可用于非线性问题，但非线性 MPC 通常不再是凸问题。本文的凸 MPC 依赖线性/仿射动力学、凸代价和凸约束，不能直接套用到任意非线性模型。
适应性：可实时响应环境变化（如障碍物移动）

HW2_Q3 Optimal Rendezvous and Docking航天器交汇

接下来将针对SpaceX Dragon飞船与国际空间站（ISS）的交会对接，使用LQR、凸轨迹优化、凸MPC三种控制方法

状态变量$x \in \mathbb{R}^6$为$x,y,z$的位置和速度，控制变量$u \in \mathbb{R}^3$为飞船三轴推力

$$ \begin{align} x &= [r_x, r_y, r_z, v_x, v_y, v_z]^T,\ u &= [t_x, t_y, t_z]^T \end{align} $$

系统的连续时间动力学模型$\dot{x}=Ax+Bu$如下(Clohessy-Wiltshire 方程)

$$ \begin{align} \dot{x} &= \begin{bmatrix}0 & 0 & 0 & 1 & 0 & 0 \
0 & 0 & 0 & 0 & 1 & 0 \ 0 & 0 & 0 & 0 & 0 & 1\ 3n^2 &0 & 0 & 0 & 2n &0 \ 0 & 0 & 0 & -2n &0 & 0\ 0 & 0 &-n^2 & 0 & 0 & 0 \end{bmatrix}x + \begin{bmatrix} 0 & 0 & 0 \ 0 & 0 & 0 \ 0 & 0 & 0 \ 1 & 0 & 0 \ 0 & 1 & 0 \ 0 & 0 & 1 \end{bmatrix} u \end{align} $$

$A$矩阵包含轨道动力学效应（科里奥利力、离心力）
$n=\sqrt{\mu/a^3}$ 为轨道平均角速度。下方代码采用 $\mu=3.986004418\times10^{14},\mathrm{m^3/s^2}$、$a=6971100,\mathrm m$；复现时统一按代码参数，不混用其他轨道半径。
上式输入矩阵为单位加速度输入形式；若 $u$ 表示推力，应乘相应的质量倒数。下方代码使用 0.1*I(3)，复现时须核对这一输入缩放与单位。

Part A: Discretize the dynamics系统离散化

使用增广矩阵进行系统离散化

function create_dynamics(dt::Real)::Tuple{Matrix,Matrix}
    mu = 3.986004418e14 # standard gravitational parameter
    a = 6971100.0       # semi-major axis of ISS
    n = sqrt(mu/a^3)    # mean motion

    # continuous time dynamics ẋ = Ax + Bu
    A = [0     0  0    1   0   0; 
         0     0  0    0   1   0;
         0     0  0    0   0   1;
         3*n^2 0  0    0   2*n 0;
         0     0  0   -2*n 0   0;
         0     0 -n^2  0   0   0]
   
    B = Matrix([zeros(3,3);0.1*I(3)])

    # TODO: convert to discrete time X_{k+1} = Ad*x_k + Bd*u_k
    nx,nu =size(B)
    M = exp([A B; zeros(3,6) zeros(3,3)] * dt)  # 增广矩阵
    Ad = M[1:nx, 1:nx]
    Bd = M[1:nx, nx+1:nx+nu]
    return Ad, Bd
end

Part B:LQR

使用有限时域 LQR跟踪给定的参考轨迹

$$ \begin{align} \min_{x_{1:N},u_{1:N-1}} \quad & \sum_{i=1}^{N-1} \bigg[ \frac{1}{2} (x_i - x_{ref, i})^TQ(x_i - x_{ref, i}) + \frac{1}{2} u_i^TRu_i \bigg] + \frac{1}{2}(x_N- x_{ref, N})^TQ_f (x_N- x_{ref, N})\ \text{st} \quad & x_1 = x_{\text{IC}} \ & x_{i+1} = A x_i + Bu_i \quad \text{for } i = 1,2,\ldots,N-1 \end{align} $$

下方实现使用 $u_i=-K_i(x_i-x_{ref,i})$ 并进行限幅。它是围绕参考状态的反馈实验；对一般参考轨迹，仅计算调节器增益并减去参考状态，不足以求解上面完整的跟踪最优化问题，还需要参考输入/仿射前馈项及轨迹可行性处理。限幅也会改变无约束 LQR 的最优性与稳定性结论。

# TODO: FHLQR 
function fhlqr(A::Matrix, # A matrix 
           B::Matrix, # B matrix 
           Q::Matrix, # cost weight 
           R::Matrix, # cost weight 
           Qf::Matrix,# term cost weight 
           N::Int64   # horizon size 
           )::Tuple{Vector{Matrix{Float64}}, Vector{Matrix{Float64}}} # return two matrices 

    # check sizes of everything 
    nx,nu = size(B)
    @assert size(A) == (nx, nx)
    @assert size(Q) == (nx, nx)
    @assert size(R) == (nu, nu)
    @assert size(Qf) == (nx, nx)

    # instantiate S and K 
    P = [zeros(nx,nx) for i = 1:N]
    K = [zeros(nu,nx) for i = 1:N-1]

    # initialize S[N] with Qf 
    P[N] = deepcopy(Qf)

    # Ricatti 
    for n in N-1:-1:1
        # TODO
        K[n] = (R+B'*P[n+1]B)\B'*P[n+1]*A
        P[n] = Q + A'*P[n+1]*(A-B*K[n])
    end

    return P, K 
end

# Solve LQR
_, K = fhlqr(A,B,Q,R,Qf,N)

# simulation 
X_sim = [zeros(nx) for i = 1:N]
U_sim = [zeros(nu) for i = 1:N-1]
X_sim[1] = x0 
for i = 1:(N-1) 
    # TODO: put LQR control law here 
    # make sure to clamp 
    U_sim[i] = clamp.(-K[i]*(X_sim[i]-X_ref[i]),u_min,u_max)

    # simulate 1 step 
    X_sim[i+1] = A*X_sim[i] + B*U_sim[i]
end

Part C: Convex Trajectory Optimization

$$ \begin{align} \min_{x_{1:N},u_{1:N-1}} \quad & \sum_{i=1}^{N-1} \bigg[ \frac{1}{2} (x_i - x_{ref, i})^TQ(x_i - x_{ref, i}) + \frac{1}{2} u_i^TRu_i \bigg] \

\text{st} \quad & x_1 = x_{\text{IC}} \

& x_{i+1} = A x_i + Bu_i \quad \text{for } i = 1,2,\ldots,N-1 \

& u_{min} \leq u_i \leq u_{max} \quad \text{for } i = 1,2,\ldots,N-1 \

& x_i[2] \leq x_{goal} [2]\quad \text{for } i = 1,2,\ldots,N \

& x_N = x_{goal}

\end{align} $$

"""
Xcvx,Ucvx = convex_trajopt(A,B,X_ref,x0,xg,u_min,u_max,N)

setup and solve the above optimization problem, returning 
the solutions X and U, after first converting them to 
vectors of vectors with vec_from_mat(X.value)
"""
function convex_trajopt(A::Matrix, # discrete dynamics A 
                        B::Matrix, # discrete dynamics B 
                        X_ref::Vector{Vector{Float64}}, # reference trajectory 
                        x0::Vector, # initial condition 
                        xg::Vector, # goal state 
                        u_min::Vector, # lower bound on u 
                        u_max::Vector, # upper bound on u
                        N::Int64, # length of trajectory 
                        )::Tuple{Vector{Vector{Float64}}, Vector{Vector{Float64}}} # return Xcvx,Ucvx
  
    # get our sizes for state and control
    nx,nu = size(B)
  
    @assert size(A) == (nx, nx)
    @assert length(x0) == nx 
    @assert length(xg) == nx 
  
    # LQR cost
    Q = diagm(ones(nx))
    R = diagm(ones(nu))

    # variables we are solving for
    X = cvx.Variable(nx,N)
    U = cvx.Variable(nu,N-1)

    # TODO: implement cost
    obj = 0
    for k =1:N-1
        x_k,u_k = X[:,k]-X_ref[k],U[:,k]
        obj += 0.5*cvx.quadform(x_k,Q)+0.5*cvx.quadform(u_k,R)
    end

    # create problem with objective
    prob = cvx.minimize(obj)

    # TODO: add constraints with prob.constraints = vcat(prob.constraints, ...)
    prob.constraints = vcat(prob.constraints,(X[:,1]==x0))
    prob.constraints = vcat(prob.constraints,(X[:,end]==xg))
    for i =1:N-1
        #dynamics constraints
        prob.constraints = vcat(prob.constraints,(X[:,i+1]==A*X[:,i]+B*U[:,i]))
        # control constraint
        prob.constraints = vcat(prob.constraints,(U[:,i]<=u_max))
        prob.constraints = vcat(prob.constraints,(U[:,i]>=u_min))
    end

    for i = 1:N
        prob.constraints = vcat(prob.constraints,(X[2,i]<=xg[2]))
    end
    cvx.solve!(prob, ECOS.Optimizer; silent = true)

    X = X.value
    U = U.value
  
    Xcvx = vec_from_mat(X)
    Ucvx = vec_from_mat(U)
  
    return Xcvx, Ucvx
end

Part D: Convex MPC

在航天器交会对接任务中，（Part C）开环控制无法处理系统中的不确定性，MPC通过滚动时域优化使用反馈控制，弥补“sim-to-real gap”

给定当前时刻的参考轨迹窗口$\tilde{x}{ref} = x{ref}[i,(i + N_{mpc} - 1)]$，MPC将求解以下的凸优化问题

$$ \begin{align} \min_{x_{1:N},u_{1:N-1}} \quad & \sum_{i=1}^{N-1} \bigg[ \frac{1}{2} (x_i - \tilde{x}{ref, i})^TQ({x}i - \tilde{x}{ref, i}) + \frac{1}{2} u_i^TRu_i \bigg] + \frac{1}{2}(x_N- \tilde{x}{ref, N})^TQ ({x}N- \tilde{x}{ref, N})\ \text{st} \quad & x_1 = x_{\text{IC}} \ & x_{i+1} = A x_i + Bu_i \quad \text{for } i = 1,2,\ldots,N-1 \ & u_{min} \leq u_i \leq u_{max} \quad \text{for } i = 1,2,\ldots,N-1 \ & x_i[2] \leq x_{goal} [2]\quad \text{for } i = 1,2,\ldots,N \end{align} $$

参数说明：

$Q,R,Q_f$:状态、控制输入和终端状态的权重矩阵
$N_mpc$：预测时域长度
$x_\mathbf{IC}$:当前状态估计（来自传感器滤波）

"""
`u = convex_mpc(A,B,X_ref_window,xic,xg,u_min,u_max,N_mpc)`

setup and solve the above optimization problem, returning the 
first control u_1 from the solution (should be a length nu 
Vector{Float64}).  
"""
function convex_mpc(A::Matrix, # discrete dynamics matrix A
                    B::Matrix, # discrete dynamics matrix B
                    X_ref_window::Vector{Vector{Float64}}, # reference trajectory for this window 
                    xic::Vector, # current state x 
                    xg::Vector, # goal state 
                    u_min::Vector, # lower bound on u 
                    u_max::Vector, # upper bound on u 
                    N_mpc::Int64,  # length of MPC window (horizon)
                    )::Vector{Float64} # return the first control command of the solved policy 
  
    # get our sizes for state and control
    nx,nu = size(B)
  
    # check sizes 
    @assert size(A) == (nx, nx)
    @assert length(xic) == nx 
    @assert length(xg) == nx 
    @assert length(X_ref_window) == N_mpc 
  
    # LQR cost
    Q = diagm(ones(nx))
    R = diagm(ones(nu))
    Qf = 10*Q

    # variables we are solving for
    X = cvx.Variable(nx,N_mpc)
    U = cvx.Variable(nu,N_mpc-1)

    # TODO: implement cost function
    obj = cvx.quadform(X[:,N_mpc]-X_ref_window[N_mpc],Qf)
    for i = 1:N_mpc-1
        obj +=cvx.quadform(X[:,i]-X_ref_window[i],Q)+cvx.quadform(U[:,i],R)
    end
    # create problem with objective
    prob = cvx.minimize(obj)

    # TODO: add constraints with prob.constraints = vcat(prob.constraints, ...)
    prob.constraints = vcat(prob.constraints,(X[:,1]==xic))
    for i =1:N_mpc-1
        #dynamics constraints
        prob.constraints = vcat(prob.constraints,(X[:,i+1]==A*X[:,i]+B*U[:,i]))
        # control constraint
        prob.constraints = vcat(prob.constraints,(U[:,i]<=u_max))
        prob.constraints = vcat(prob.constraints,(U[:,i]>=u_min))
    end

    for i = 1:N_mpc
        prob.constraints = vcat(prob.constraints,(X[2,i]<=xg[2]))
    end

    # solve problem 
    cvx.solve!(prob, ECOS.Optimizer; silent = true)

    # get X and U solutions 
    X = X.value
    U = U.value
  
    # return first control U 
    return U[:,1]
end

无人机悬停案例

平面无人机动力学

为与下方实现一致，这里令 $l$ 表示两电机间距，因此每侧推力到质心的力臂为 $l/2$。

$$ \begin{align}

\ddot{x}&=\frac{1}{m}(u_1+u_2)sin\theta\ \ddot{y}&=\frac{1}{m}(u_1+u_2)cos\theta-g\ \ddot{\theta}&=\frac{l}{2J}(u_2-u_1)\ \end{align} $$

在平衡点线性化$(u_1=u_2 =\frac{1}{2}mg,\theta=0)$写成矩阵形式

$$ \Longrightarrow \begin{align} \Delta\ddot{x}&=g\theta\ \Delta\ddot{y}&=\frac{1}{m}(\Delta u_1+\Delta u_2)\ \Delta\ddot{\theta}&=\frac{l}{2J}(\Delta u_2-\Delta u_1)\ \end{align} $$

$$ \begin{align} \begin{bmatrix}\Delta\dot{x}\ \Delta\dot{y} \ \Delta\dot{\theta} \\Delta\ddot{x}\ \Delta\ddot{y} \ \Delta\ddot{\theta}\end{bmatrix}= \begin{bmatrix}0&0 &0 & 1&0 &0 \ 0& 0&0 &0 &1 &0 \0&0 &0 &0 &0 &1\ 0&0&g&0&0&0\0&0&0&0&0&0\0&0&0&0&0&0\end{bmatrix}

\begin{bmatrix}{\Delta x}\ {\Delta y} \ {\Delta \theta}\\Delta \dot{x}\ \Delta \dot{y} \ \Delta\dot{\theta} \end{bmatrix} + \begin{bmatrix}0&0\0&0 \0&0 \0&0 \ \frac{1}{m}&\frac{1}{m}\-\frac{l}{2J}&\frac{l}{2J}\end{bmatrix} \begin{bmatrix}\Delta u_1\ \Delta u_2\end{bmatrix} \end{align} $$

using LinearAlgebra
using ForwardDiff
using OSQP
#Model parameters
g = 9.81 #m/s^2
m = 1.0 #kg 
ℓ = 0.3 #meters
J = 0.2*m*ℓ*ℓ

h = 0.05 #time step (20 Hz)

#Planar Quadrotor Dynamics
function quad_dynamics(x,u)
    θ = x[3]
    ẍ = (1/m)*(u[1] + u[2])*sin(θ)
    ÿ = (1/m)*(u[1] + u[2])*cos(θ) - g
    θ̈ = (1/J)*(ℓ/2)*(u[2] - u[1])
    return [x[4:6]; ẍ; ÿ; θ̈]
end
function quad_dynamics_rk4(x,u)
    #RK4 integration with zero-order hold on u
    f1 = quad_dynamics(x, u)
    f2 = quad_dynamics(x + 0.5*h*f1, u)
    f3 = quad_dynamics(x + 0.5*h*f2, u)
    f4 = quad_dynamics(x + h*f3, u)
    return x + (h/6.0)*(f1 + 2*f2 + 2*f3 + f4)
end

#Linearized dynamics for hovering
x_hover = zeros(6)
u_hover = [0.5*m*g; 0.5*m*g]
A = ForwardDiff.jacobian(x->quad_dynamics_rk4(x,u_hover),x_hover);
B = ForwardDiff.jacobian(u->quad_dynamics_rk4(x_hover,u),u_hover);
quad_dynamics_rk4(x_hover, u_hover)

MPC

离散时不变系统的状态空间方程

$$ {x}_{k+1}=Ax_k+Bu_k $$

定义预测时域为 $p$，状态维度为 $n_x$，输入维度为 $n_u$。堆叠变量为

$$ X_k=\begin{bmatrix}x_{k+1|k}\\vdots\x_{k+p|k}\end{bmatrix}\in\mathbb R^{pn_x},\qquad U_k=\begin{bmatrix}u_{k|k}\\vdots\u_{k+p-1|k}\end{bmatrix}\in\mathbb R^{pn_u}. $$

逐步代入动力学，可以得到

$$ x_{k+i|k}=A^i x_{k|k}+\sum_{j=0}^{i-1}A^{i-1-j}B u_{k+j|k}, \qquad i=1,\ldots,p. $$

因此 $X_k=\Phi x_{k|k}+\Gamma U_k$，其中

$$ \Phi=\begin{bmatrix}A\A^2\\vdots\A^p\end{bmatrix},\qquad \Gamma=\begin{bmatrix} B&0&\cdots&0\ AB&B&\cdots&0\ \vdots&\vdots&\ddots&\vdots\ A^{p-1}B&A^{p-2}B&\cdots&B \end{bmatrix}. $$

对于零参考状态的调节问题，选取

$$ J=\frac12\sum_{i=1}^{p-1}x_{k+i|k}^{\top}Qx_{k+i|k} +\frac12x_{k+p|k}^{\top}Q_Nx_{k+p|k} +\frac12\sum_{i=0}^{p-1}u_{k+i|k}^{\top}Ru_{k+i|k}. $$

令 $\Omega=\operatorname{diag}(Q,\ldots,Q,Q_N)$、$\Psi=\operatorname{diag}(R,\ldots,R)$，则

$$ J=\frac12X_k^\top\Omega X_k+\frac12U_k^\top\Psi U_k =\frac12U_k^\top H U_k+U_k^\top F x_{k|k}+\text{const}, $$

$$ H=\Gamma^\top\Omega\Gamma+\Psi,\qquad F=\Gamma^\top\Omega\Phi. $$

常数项只与当前状态有关，不影响最优输入。此前笔记的最后一步输入索引和 $\Gamma$ 最后一行次序有误，已按上述递推统一。跟踪非零参考时，还需把参考轨迹引入代价中的线性项。

OSQP 求解器的使用

OSQP 求解器是一个用于求解凸二次规划（形式如下）的数值优化软件包

$$ \begin{split}\begin{array}{ll} \text{minimize} & \frac{1}{2} x^T P x + q^T x \ \text{subject to} & l \leq A x \leq u \end{array}\end{split} $$

其中 $x$ 是优化变量，$P\in\mathbf S_+^n$ 是对称半正定矩阵。

考虑一个线性时不变动力学系统到某个参考状$x_r\in \mathcal{R}^{n_x}$的问题

$$ \begin{split}\begin{array}{ll} \text{minimize} & (x_N-x_r)^T Q_N (x_N-x_r) + \sum_{k=0}^{N-1}\left[(x_k-x_r)^T Q (x_k-x_r) + u_k^T R u_k\right] \ \text{subject to} & x_{k+1} = A x_k + B u_k \ & x_{\rm min} \le x_k \le x_{\rm max} \ & u_{\rm min} \le u_k \le u_{\rm max} \ & x_0 = \bar{x} \end{array}\end{split} $$

1.给出系统的$Q,R,Q_N,A,B$

Nx = 6     # number of state
Nu = 2     # number of controls
Tfinal = 10.0 # final time
Nt = Int(Tfinal/h)+1    # number of time steps
thist = Array(range(0,h*(Nt-1), step=h));

# Cost weights
Q = Array(1.0*I(Nx));
R = Array(.01*I(Nu));
Qn = Array(1.0*I(Nx));

#Thrust limits
umin = [0.2*m*g; 0.2*m*g]
umax = [0.6*m*g; 0.6*m*g]

转换成标准 QP 问题

优化变量

$$ z=[u_0^T,x_1^T,u_1^T,x_2^T,\dots,u_{p-1}^T,x_p^T]^\top $$

$$ J=z^\top Wz-2c^\top z+\mathrm{const},\qquad W=\operatorname{diag}(R,Q,\ldots,R,Q_N),\quad c=[0;Qx_r;\ldots;0;Q_Nx_r]. $$

对于这里不含 $1/2$ 的代价，OSQP 应取 $P=2W$、$q=-2c$；跟踪代价的线性项是负号。以下悬停示例改用含 $1/2$ 的误差代价，参考是平衡点，变量依次为 $[\Delta u_0;\Delta x_1;\ldots]$，因此 H=W、b=0。此示例只添加推力限制，未添加上面通式的状态限制。

原代码的 rob/prob 名称不一致，且缺少 Nh、终端权重定义和非零初态右端项；已改为明确分块组装。依赖前面的离散模型和参数；尚未在锁定的 OSQP.jl 环境中运行，下面提供建模示例而非经过仿真验证的控制器。

using SparseArrays
Nh = 20
nb = Nu + Nx
uidx(k) = ((k-1)*nb+1):((k-1)*nb+Nu)
xidx(k) = ((k-1)*nb+Nu+1):(k*nb)
H = spzeros(Nh*nb, Nh*nb)
C = spzeros(Nh*Nx, Nh*nb)
S = spzeros(Nh*Nu, Nh*nb)  # 提取输入
for k in 1:Nh
    rows = ((k-1)*Nx+1):(k*Nx)
    H[uidx(k), uidx(k)] = R
    H[xidx(k), xidx(k)] = k == Nh ? Qn : Q
    C[rows, uidx(k)] = B
    C[rows, xidx(k)] = -Matrix{Float64}(I, Nx, Nx)
    if k > 1
        C[rows, xidx(k-1)] = A
    end
    S[((k-1)*Nu+1):(k*Nu), uidx(k)] = Matrix{Float64}(I, Nu, Nu)
end
b = zeros(Nh*nb)
D = [C; S]
rhs = zeros(Nh*Nx)
lb = [rhs; repeat(umin-u_hover, Nh)]
ub = [rhs; repeat(umax-u_hover, Nh)]
prob = OSQP.Model()
OSQP.setup!(prob; P=H, q=b, A=D, l=lb, u=ub, verbose=false)

function hover_control(x_now)
    # B*δu₀ - δx₁ = -A*δx₀，初态必须进入每次优化。
    rhs[1:Nx] = -A*(x_now-x_hover)
    lb[1:Nh*Nx] = rhs
    ub[1:Nh*Nx] = rhs
    OSQP.update!(prob; l=lb, u=ub)
    result = OSQP.solve!(prob)
    # 演示采用严格成功判据；失败时交由调用方处理，不能盲用 result.x。
    result.info.status == :Solved || error("QP 未成功求解：$(result.info.status)")
    return u_hover + result.x[uidx(1)]
end

接口和求解状态见 OSQP 的 Julia 文档与所安装的 OSQP.jl 版本。真正的 MPC 仿真还需每步调用控制器、用真实模型推进，并记录约束残差、闭环状态与求解耗时；求解成功不等于非线性系统一定稳定。

整理与核查说明

本笔记原有许可为 CC BY 4.0。引用的课程材料、代码和图片仍须遵守其各自的许可。

核查状态：部分验证（2026-10-04）。 Julia 1.10.10 验证 MPC 分块尺寸、初态残差及 Hessian；未执行完整 OSQP 和航天器实验。

CMU 最优控制笔记 4：非线性轨迹优化、DDP 与 iLQR

Mon, 28 Jul 2025 00:00:00 GMT

学完线性二次型问题后，我继续整理非线性轨迹优化，包括直接配点、DDP 与 iLQR 的方法和作业实践。推导部分与长代码可以分开阅读。

先修知识： LQR、动态规划、二阶泰勒展开，以及非线性动力学的数值积分。

阅读路线：

先区分轨迹离散化和优化求解的不同思路，理解直接配点的作用。
再沿值函数、局部二次近似、反向递推和前向线搜索阅读 DDP/iLQR。
最后按小车倒立摆、四旋翼轨迹优化和姿态调整三个案例查看实现。

笔记结合 CMU 16-745 的课程与作业整理，相关材料见课程作业页面。文中的代码片段保留学习时的实现，运行时还需使用对应作业的依赖与上下文。

其他学习视角可参考向阳的笔记和知乎我爱科研的整理

Lecture 11-12 Nonlinear Trajectory Optimization/Differential Dynamic Programming

非线性轨迹优化可以采用不同的参数化与求解方式。Shooting（打靶）和 collocation（配点）不能简单地分别等同于直接法和间接法：直接打靶和直接配点都属于常见的直接方法；间接方法通常从最优性必要条件出发构造待求解方程。

直接配点把离散状态和控制作为优化变量，将动力学离散为约束，再交给非线性优化求解器。Hermite–Simpson 是常用配点方案，但不能笼统地说它在所有问题中都比 RK4 更稳定或成本更低。方法比较需结合步长、误差、问题规模和求解器。分类与直接配点可参考 MIT 的轨迹优化讲义。

注意

课程演示的代码 dircol.ipynb 在本地运行到 z_sol = solve(z0,prob) 时给出报错

ERROR: TypeError: in typeassert, expected Vector{Tuple{Int64, Int64}}, got a value of type Vector{Tuple{Any, Any}}

在当时使用的接口与代码组合中，通过修改 row_col! 和 sparsity_jacobian 的索引类型处理了该错误。原记录没有完整依赖版本，这只能作为类型不匹配的排查线索，不能认定所有版本都必须改为 Int64。

row = Int64[]#而不是row=[]
col = Int64[]

当时执行 using MeshCat 时还遇到报错

InitError: could not load library "C:\Users\26583\.julia\artifacts\8f71eb37d5b304026b6363d835f8c65ff1920339\bin\avdevice-61.dll"
The specified module could not be found.
......
during initialization of module FFMPEG_jll

这表明 FFMPEG 动态库或其依赖加载失败，不能仅凭错误推断需要重新编译。原记录曾尝试对 FFMPEG_jll 和 MeshCat 执行 build/add，但没有保留可确认因果关系的日志，因此移除这组“一键修复”命令。

JLL 通常通过 artifacts 提供预构建二进制，Pkg.build 不是重新编译任意 JLL 动态库的通用办法。先在原作业环境记录以下信息，再根据具体缺失文件、平台支持与依赖加载错误排查；见 BinaryBuilder 的 JLL 说明。

using Pkg
versioninfo()
Pkg.status()
# 在新 Julia 进程中单独执行 using MeshCat，并保留完整错误栈。
# 不要先改动依赖版本、添加间接依赖或删除整个 artifacts 目录。

微分动态规划（Differential Dynamic Programming, DDP）是一种用于求解非线性最优控制问题的迭代算法，结合动态规划和二阶泰勒展开的思想，通过局部近似和反向传播来高效地优化控制策略。

参考文章

Differential Dynamic Programming with Nonlinear Constraints

Second-Order Differential Dynamic Programming for Whole-Body MPC of Legged Robots

iLQR Tutorial

AL-iLQR Tutorial

考虑离散动力学系统，目标是找到控制序列$\mathbf{u}_{1:N-1}$，最小化总代价（通常假设代价函数和约束函数二阶可导）

$$ \begin{aligned} \min_{x_{1:N},u_{1:N-1}} J&=\sum_{k=1}^{N-1}{\ell (\mathbf{x}_k,\mathbf{u}_k)}+\ell _N(\mathbf{x}N)\ \mathrm{s}.\mathrm{t}.\quad & \mathbf{x}{k+1}=\mathbf{f}\left( \mathbf{x}_k,\mathbf{u}_k \right)\ &\mathbf{x}_k\in \mathbf{X}_k\ &\mathbf{u}_k\in \mathbf{U}_k\ \end{aligned} $$

DDP/iLQR：反向递推与前向更新

反向传播（Backward Pass）

定义值函数(value function)

在时间步，值函数$V_k(\mathbf{x})$为从状态$\mathbf{x}_k$出发，采用最优控制策略$\pi^*$的最小总代价

$$ V_k(\mathbf{x})=\min_{\mathbf{u}k,...,\mathbf{u}{N-1}} \left( \sum_{j=k}^{N-1}{\ell (\mathbf{x}_j,\mathbf{u}_j)}+\ell _N(\mathbf{x}_N) \right) $$
贝尔曼方程

值函数满足贝尔曼方程(反映状态动作值函数$Q_k(\mathbf{x}_k,\mathbf{u}_k)$和状态值函数的关系)

$$ Q_k(\mathbf{x}_k,\mathbf{u}_k)={\ell (\mathbf{x}_k,\mathbf{u_k})}+V {k+1}(\mathbf{x}{k+1}) $$

$$ V_k(\mathbf{x}k)=\min{\mathbf{u}} \left[ {\ell (\mathbf{x}k,\mathbf{u_k})}+V {k+1}(\mathbf{x}{k+1}) \right]=\min{\mathbf{u}}Q_k(\mathbf{x}_k,\mathbf{u}_k) $$
二阶泰勒展开

DDP 对Q 函数（即当前步代价+下一步值函数）进行二阶泰勒展开

$$ \begin{array}{l} Q_k(\mathbf{x}+\Delta \mathbf{x},\mathbf{u}+\Delta \mathbf{u})\approx Q_k(\mathbf{x},\mathbf{u})+\left[ \begin{array}{c} Q_{\mathbf{x}}\ Q_{\mathbf{u}}\ \end{array} \right] ^{\mathrm{T}}\left[ \begin{array}{c} \Delta \mathbf{x}\ \Delta \mathbf{u}\ \end{array} \right]\ +\frac{1}{2}\left[ \begin{array}{c} \Delta \mathbf{x}\ \Delta \mathbf{u}\ \end{array} \right] ^{\mathrm{T}}\left[ \begin{matrix} Q_{\mathbf{xx}}& Q_{\mathbf{xu}}\ Q_{\mathbf{ux}}& Q_{\mathbf{uu}}\ \end{matrix} \right] \left[ \begin{array}{c} \Delta \mathbf{x}\ \Delta \mathbf{u}\ \end{array} \right] ,\left( Q_{\mathbf{xu}}=Q_{\mathbf{ux}}^{\mathrm{T}} \right)\ \end{array} $$

其中

$$ \begin{aligned} &Q_{\mathbf{x}}=\ell {\mathbf{x}}+\mathbf{f}{\mathbf{x}}^{\top}V_{\mathbf{x}}^{\prime}\ &Q_{\mathbf{u}}=\ell {\mathbf{u}}+\mathbf{f}{\mathbf{u}}^{\top}V_{\mathbf{x}}^{\prime}\ &Q_{\mathbf{xx}}=\ell {\mathbf{xx}}+\mathbf{f}{\mathbf{x}}^{\top}V_{\mathbf{xx}}^{\prime}\mathbf{f}{\mathbf{x}}+V{\mathbf{x}}^{\prime}\cdot \mathbf{f}{\mathbf{xx}}\ &Q{\mathbf{uu}}=\ell {\mathbf{uu}}+\mathbf{f}{\mathbf{u}}^{\top}V_{\mathbf{xx}}^{\prime}\mathbf{f}{\mathbf{u}}+V{\mathbf{x}}^{\prime}\cdot \mathbf{f}{\mathbf{uu}}\ &Q{\mathbf{ux}}=\ell {\mathbf{ux}}+\mathbf{f}{\mathbf{u}}^{\top}V_{\mathbf{xx}}^{\prime}\mathbf{f}{\mathbf{x}}+V{\mathbf{x}}^{\prime}\cdot \mathbf{f}_{\mathbf{ux}}\ \end{aligned} $$

iLQR的不同在于忽略$\bf f_{xx}$等二阶项
最优控制修正

固定当前状态扰动，对局部二次近似的 $Q_k$ 关于输入扰动求极小，得到控制修正（假设 $Q_{\mathbf{uu}}$ 正定；否则需要正则化）

$$ \Delta \mathbf{u}{k}^{\star}=-Q{\mathbf{uu}}^{-1}Q_{\mathbf{u}}-Q_{\mathbf{uu}}^{-1}Q_{\mathbf{ux}}\Delta \mathbf{x}_k $$

其中，反馈增益$K_k=-Q_{\mathbf{uu}}^{-1}Q_{\mathbf{ux}}$，前馈增益$j_k=-Q_{\mathbf{uu}}^{-1}Q_{\mathbf{u}}$
更新值函数的二次近似

$$ V_k\left( \mathbf{x}+\Delta \mathbf{x} \right) \leftarrow V_k\left( \mathbf{x} \right) +p_{k}^{\mathrm{T}}\Delta \mathbf{x}+\frac{1}{2}\Delta \mathbf{x}^{\mathrm{T}}P_k\Delta \mathbf{x} $$

其中

$$ \begin{aligned} P_k&=Q_{\mathbf{xx}}+K_{k}^{\mathrm{T}}Q_{\mathbf{uu}}K_k+Q_{xu}K_k+K_{k}^{\mathbf{T}}Q_{\mathbf{ux}},\ p_k&=Q_{\mathbf{x}}+K_{k}^{\mathrm{T}}Q_{\mathbf{uu}}j_k+Q_{\mathbf{xu}}j_k+K_{k}^{\mathbf{T}}Q_{\mathbf{u}},\ \end{aligned} $$

从$p_N=\nabla _xl_N(\mathbf{x}),P_N=\nabla _{xx}^{2}l_N(\mathbf{x})$开始，递推出$K_k,j_k,P_k$和$p_k$。

前向更新（Forward Pass）

更新控制序列$(\mathbf{x}_0^{new}=\mathbf{x}_0)$

$$ \begin{aligned} \mathbf{u}{k}^{new}&=\mathbf{u}k+K_k(\mathbf{x}{k}^{new}-\mathbf{x}k)+\alpha j_k\ \mathbf{x}{k+1}^{new}&=f(\mathbf{x}{k}^{new},\mathbf{u}_{k}^{new})\ \end{aligned} $$

通过比较新旧轨迹代价进行线搜索，选择合适的步长 $\alpha$。这里统一令前馈修正 $j_k=-Q_{uu}^{-1}Q_u$，因此前向更新使用加号；不能把正号定义的 $j_k$ 直接代入这一更新式。

上面的反向递推是局部无约束子问题的形式。开头列出的状态和输入约束需要额外的约束处理机制，例如增广拉格朗日；不能仅写入问题描述就认为普通 iLQR 已处理约束。参考 AL-iLQR Tutorial。

HW3_Q1 倒立摆小车DIRCOL示例

IPOPT是一个基于内点法的开源大规模非线性优化求解器，专门用于求解具有约束的连续优化问题。设定如下标准问题（这个HW自带了一个fmincon函数将IPOPT的接口进一步封装）

$$ \begin{align} \min_{x} \quad & \ell(x) & \text{cost function}\ \text{st} \quad & c_{eq}(x) = 0 & \text{equality constraint}\ & c_L \leq c_{ineq}(x) \leq c_U & \text{inequality constraint}\ & x_L \leq x \leq x_U & \text{primal bound constraint} \end{align} $$

"""
x = fmincon(cost,equality_constraint,inequality_constraint,x_l,x_u,c_l,c_u,x0,params,diff_type)

This function uses IPOPT to minimize an objective function 

`cost(params, x)` 

With the following three constraints: 

`equality_constraint(params, x) = 0`
`c_l <= inequality_constraint(params, x) <= c_u` 
`x_l <= x <= x_u` 

Note that the constraint functions should return vectors. 

Problem specific parameters should be loaded into params::NamedTuple (things like 
cost weights, dynamics parameters, etc.). 

args:
    cost::Function                    - objective function to be minimzed (returns scalar)
    equality_constraint::Function     - c_eq(params, x) == 0 
    inequality_constraint::Function   - c_l <= c_ineq(params, x) <= c_u 
    x_l::Vector                       - x_l <= x <= x_u 
    x_u::Vector                       - x_l <= x <= x_u 
    c_l::Vector                       - c_l <= c_ineq(params, x) <= c_u 
    c_u::Vector                       - c_l <= c_ineq(params, x) <= c_u 
    x0::Vector                        - initial guess 
    params::NamedTuple                - problem parameters for use in costs/constraints 
    diff_type::Symbol                 - :auto for ForwardDiff, :finite for FiniteDiff 
    verbose::Bool                     - true for IPOPT output, false for nothing 

optional args:
    tol                               - optimality tolerance 
    c_tol                             - constraint violation tolerance 
    max_iters                         - max iterations 
    verbose                           - verbosity of IPOPT 

outputs:
    x::Vector                         - solution 

You should try and use :auto for your `diff_type` first, and only use :finite if you 
absolutely cannot get ForwardDiff to work. 

This function will run a few basic checks before sending the problem off to IPOPT to 
solve. The outputs of these checks will be reported as the following:

---------checking dimensions of everything----------
---------all dimensions good------------------------
---------diff type set to :auto (ForwardDiff.jl)----
---------testing objective gradient-----------------
---------testing constraint Jacobian----------------
---------successfully compiled both derivatives-----
---------IPOPT beginning solve----------------------

If you're getting stuck during the testing of one of the derivatives, try switching 
to FiniteDiff.jl by setting diff_type = :finite. 
""";

Part B:Cart pole Swingup

倒立摆小车的状态向量定义如下

$$ x = [p,\theta,\dot{p},\dot{\theta}]^T $$

求解优化问题

$$ \begin{align} \min_{x_{1:N},u_{1:N-1}} \quad & \sum_{i=1}^{N-1} \bigg[ \frac{1}{2} (x_i - x_{goal})^TQ(x_i - x_{goal}) + \frac{1}{2} u_i^TRu_i \bigg] + \frac{1}{2}(x_N - x_{goal})^TQ_f(x_N - x_{goal})\ \text{st} \quad & x_1 = x_{\text{IC}} \ & x_N = x_{goal} \ & f_{hs}(x_i,x_{i+1},u_i,dt) = 0 \quad \text{for } i = 1,2,\ldots,N-1 \ & -10 \leq u_i \leq 10 \quad \text{for } i = 1,2,\ldots,N-1 \end{align} $$

给定水平推力(限幅)使得倒立摆小车从$x_{IC} = [0,0,0,0]$到$x_{goal} = [0, \pi, 0, 0]$。

采用ZOH和Hermite Simpson来描述离散动力学系统，$f_{hs}(x_i,x_{i+1},u_i)$代表Hermite Simpson残差。

$$ \begin{align} x_{k+1/2} &= \frac{1}{2}(x_k + x_{k+1}) + \frac{\Delta t}{8}(\dot{x}k - \dot{x}{k+1})\ f(x_k,x_{k+1},\Delta t) &= x_k + \frac{\Delta t}{6} \cdot (\dot{x}k + 4\dot{x}{k+1/2} + \dot{x}{k+1}) - x{k+1}= 0\quad \quad \text{Hermite-Simpson} \end{align} $$

# cartpole 
function dynamics(params::NamedTuple, x::Vector, u)
    # cartpole ODE, parametrized by params. 

    # cartpole physical parameters 
    mc, mp, l = params.mc, params.mp, params.l
    g = 9.81
  
    q = x[1:2]
    qd = x[3:4]

    s = sin(q[2])
    c = cos(q[2])

    H = [mc+mp mp*l*c; mp*l*c mp*l^2]
    C = [0 -mp*qd[2]*l*s; 0 0]
    G = [0, mp*g*l*s]
    B = [1, 0]

    qdd = -H\(C*qd + G - B*u[1])
    xdot = [qd;qdd]
    return xdot 

end
function hermite_simpson(params::NamedTuple, x1::Vector, x2::Vector, u, dt::Real)::Vector
    # TODO: input hermite simpson implicit integrator residual 
    x1_dot=dynamics(params,x1,u)
    x2_dot=dynamics(params,x2,u)
    xm=(x1+x2)/2+dt*(x1_dot-x2_dot)/8
    xm_dot = dynamics(params,xm,u)
    return x1+dt/6*(x1_dot+4*xm_dot+x2_dot)-x2
end

定义优化变量

$$ Z = \begin{bmatrix}x_1 \ u_1 \ x_2 \ u_2 \ \vdots \ x_{N-1} \ u_{N-1} \ x_N \end{bmatrix} \in \mathbb{R}^{N \cdot nx + (N-1)\cdot nu} $$

function create_idx(nx,nu,N)
    # This function creates some useful indexing tools for Z 
    # x_i = Z[idx.x[i]]
    # u_i = Z[idx.u[i]]
    # Feel free to use/not use anything here.
    # our Z vector is [x0, u0, x1, u1, …, xN]
    nz = (N-1) * nu + N * nx # length of Z 
    x = [(i - 1) * (nx + nu) .+ (1 : nx) for i = 1:N]
    u = [(i - 1) * (nx + nu) .+ ((nx + 1):(nx + nu)) for i = 1:(N - 1)]
    # constraint indexing for the (N-1) dynamics constraints when stacked up
    c = [(i - 1) * (nx) .+ (1 : nx) for i = 1:(N - 1)]
    nc = (N - 1) * nx # (N-1)*nx 
    return (nx=nx,nu=nu,N=N,nz=nz,nc=nc,x= x,u = u,c = c)
end
function cartpole_cost(params::NamedTuple, Z::Vector)::Real
    idx, N, xg = params.idx, params.N, params.xg
    Q, R, Qf = params.Q, params.R, params.Qf
    # TODO: input cartpole LQR cost
    J = 0 
    for i = 1:(N-1)
        xi = Z[idx.x[i]]
        ui = Z[idx.u[i]]
        J += 0.5*(xi-xg)'*Q*(xi-xg)+0.5*ui'*R*ui 
    end
    xN =Z[idx.x[N]]
    J += 0.5*(xN-xg)'*Qf*(xN-xg)
    # dont forget terminal cost  
    return J 
end
function cartpole_dynamics_constraints(params::NamedTuple, Z::Vector)::Vector
    idx, N, dt = params.idx, params.N, params.dt  
    # TODO: create dynamics constraints using hermite simpson 
    # create c in a ForwardDiff friendly way (check HW0)
    c = zeros(eltype(Z), idx.nc)  
    for i = 1:(N-1)
        xi = Z[idx.x[i]]
        ui = Z[idx.u[i]] 
        xip1 = Z[idx.x[i+1]]     
        # TODO: hermite simpson 
        c[idx.c[i]] = hermite_simpson(params, xi,xip1, ui, dt)
    end
    return c 
end
function cartpole_equality_constraint(params::NamedTuple, Z::Vector)::Vector
    N, idx, xic, xg = params.N, params.idx, params.xic, params.xg   
    # TODO: return all of the equality constraints      
    return [
    Z[idx.x[1]] - xic;
    Z[idx.x[N]] - xg;
    cartpole_dynamics_constraints(params, Z)
    ]            # 10 is an arbitrary number 
end
function solve_cartpole_swingup(;verbose=true)   
    # problem size 
    nx = 4 
    nu = 1 
    dt = 0.05
    tf = 2.0 
    t_vec = 0:dt:tf 
    N = length(t_vec)   
    # LQR cost 
    Q = diagm(ones(nx))
    R = 0.1*diagm(ones(nu))
    Qf = 10*diagm(ones(nx))  
    # indexing 
    idx = create_idx(nx,nu,N)   
    # initial and goal states 
    xic = [0, 0, 0, 0]
    xg = [0, pi, 0, 0]  
    # load all useful things into params 
    params = (Q = Q, R = R, Qf = Qf, xic = xic, xg = xg, dt = dt, N = N, idx = idx,mc = 1.0, mp = 0.2, l = 0.5)
  
    # TODO: primal bounds 
    x_l = -Inf*ones(idx.nz)
    x_u = Inf*ones(idx.nz)
    for i = 1:N-1
        x_l[idx.u[i]].=-10
        x_u[idx.u[i]].=10
    end
    # inequality constraint bounds (this is what we do when we have no inequality constraints)
    c_l = zeros(0)
    c_u = zeros(0)
    function inequality_constraint(params, Z)
        return zeros(eltype(Z), 0)
    end 
    # initial guess 
    z0 = 0.001*randn(idx.nz)   
    # choose diff type (try :auto, then use :finite if :auto doesn't work)
    diff_type = :auto 
     #diff_type = :finite  
    Z = fmincon(cartpole_cost,cartpole_equality_constraint,inequality_constraint,
                x_l,x_u,c_l,c_u,z0,params, diff_type;
                tol = 1e-6, c_tol = 1e-6, max_iters = 10_000, verbose = verbose)  
    # pull the X and U solutions out of Z 
    X = [Z[idx.x[i]] for i = 1:N]
    U = [Z[idx.u[i]] for i = 1:(N-1)]  
    return X, U, t_vec, params 
end
@testset "cartpole swingup" begin 
  
    X, U, t_vec = solve_cartpole_swingup(verbose=true)   
    # --------------testing------------------
    @test isapprox(X[1],zeros(4), atol = 1e-4)
    @test isapprox(X[end], [0,pi,0,0], atol = 1e-4)
    Xm = hcat(X...)
    Um = hcat(U...)  
    # --------------plotting-----------------
    display(plot(t_vec, Xm', label = ["p" "θ" "ṗ" "θ̇"], xlabel = "time (s)", title = "State Trajectory"))
    display(plot(t_vec[1:end-1],Um',label="",xlabel = "time (s)", ylabel = "u",title = "Controls"))  
    # meshcat animation
    display(animate_cartpole(X, 0.05))
  
end

输出

---------checking dimensions of everything----------
---------all dimensions good------------------------
---------diff type set to :auto (ForwardDiff.jl)----
---------testing objective gradient-----------------
---------testing constraint Jacobian----------------
---------successfully compiled both derivatives-----
---------IPOPT beginning solve----------------------
This is Ipopt version 3.14.17, running with linear solver MUMPS 5.8.0.

Number of nonzeros in equality constraint Jacobian...:    34272
Number of nonzeros in inequality constraint Jacobian.:        0
Number of nonzeros in Lagrangian Hessian.............:        0

Total number of variables............................:      204
                     variables with only lower bounds:        0
                variables with lower and upper bounds:       40
                     variables with only upper bounds:        0
Total number of equality constraints.................:      168
Total number of inequality constraints...............:        0
        inequality constraints with only lower bounds:        0
   inequality constraints with lower and upper bounds:        0
        inequality constraints with only upper bounds:        0

iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
   0  2.4668025e+02 3.14e+00 3.14e-04   0.0 0.00e+00    -  0.00e+00 0.00e+00   0
   1  2.7495083e+02 2.38e+00 8.00e+00  -5.0 1.28e+01    -  4.90e-01 2.43e-01h  3
   2  2.9800850e+02 2.16e+00 1.03e+01  -0.5 1.05e+01    -  6.11e-01 9.26e-02h  4
   ..............

Number of Iterations....: 79

                                   (scaled)                 (unscaled)
Objective...............:   3.9344833576222919e+02    3.9344833576222919e+02
Dual infeasibility......:   7.5868099118641311e-07    7.5868099118641311e-07
Constraint violation....:   1.5276668818842154e-13    1.5276668818842154e-13
Variable bound violation:   9.9997231828297117e-08    9.9997231828297117e-08
Complementarity.........:   1.0000650824516029e-11    1.0000650824516029e-11
Overall NLP error.......:   7.5868099118641311e-07    7.5868099118641311e-07


Number of objective function evaluations             = 185
Number of objective gradient evaluations             = 80
Number of equality constraint evaluations            = 185
Number of inequality constraint evaluations          = 0
Number of equality constraint Jacobian evaluations   = 80
Number of inequality constraint Jacobian evaluations = 0
Number of Lagrangian Hessian evaluations             = 0
Total seconds in IPOPT                               = 1.664

EXIT: Optimal Solution Found.

Part C: Track DIRCOL Solution

类似HW2 Q2 PartC,使用DIRCOL得到的开环状态轨迹$X$和控制轨迹$U$，通过TVLQR跟踪该轨迹以补偿模型失配。DIRCOL的动力学约束使用了Hermite-Simpson积分方法，但闭环控制使用RK4进行数值积分，并且对闭环控制进行截断（clamp.(U[k],-10,10)）防止违约。

HW3_Q2 四旋翼无人机iLQR 示例

使用iLQR求解四旋翼无人机(6DOF)的轨迹优化问题，使用如下特定的代价函数促使无人机完成指定的机动动作(∞)

四旋翼的连续时间动力学模型在 quadrotor.jl中完成，通过rk4进行离散化，四旋翼的状态向量表示如下

$x = [r,v,{}^Np^B{},\omega]$

其中 $r\in\mathbb{R}^3$ 是无人机在世界坐标系(N)的位置， $v\in\mathbb{R}^3$ 是无人机在世界坐标系(N)的速度(N)， $^Np^B\in\mathbb{R}^3$ 是无人机姿态的修正罗德里格斯参数（Modified Rodrigues Parameter, MRP）表示（即机体坐标系相对于世界坐标系的旋转），$\omega\in\mathbb{R}^3$ 是无人机在机体坐标系的角速度。

补充：MRP使用三个参数表示旋转，在360°会遇到奇点（singularity）但在本次给定的规划动作不会使姿态旋转接近奇点。

include(joinpath(@__DIR__, "utils", "quadrotor.jl"))
functiondiscrete_dynamics(params::NamedTuple, x::Vector, u, k)
    # discrete dynamics
    # x - state
    # u - control
    # k - index of trajectory
    # dt comes from params.model.dt
    returnrk4(params.model, quadrotor_dynamics, x, u, params.model.dt)
end

Part A: iLQR for a quadrotor (25 pts)

iLQR用来解决如下的优化控制问题 :

$$ \begin{align} \min_{x_{1:N},u_{1:N-1}} \quad & \bigg[ \sum_{i=1}^{N-1} \ell(x_i,u_i)\bigg] + \ell_N(x_N)\

\text{st} \quad & x_1 = x_{{IC}} \

& x_{k+1} = f(x_k, u_k) \quad\text{for } i = 1,2,\ldots,N-1 \

\end{align} $$

$x_{IC}$是初始状态， $x_{k+1} = f(x_k, u_k)$是系统离散动力学模型， $\ell(x_i,u_i)$ 是状态代价函数， $\ell_N(x_N)$ 是终端代价函数。由于优化问题可以是非凸的，因此不能保证收敛到全局最小值，但实际可以取得良好的效果。

针对此问题，选取如下的状态代价函数和终端代价函数来促使无人机跟踪设定的参考轨迹$x_{ref}$:

$$ \begin{gathered} \ell(x_i,u_i) = \frac{1}{2} (x_i - x_{ref,i})^TQ(x_i - x_{ref,i}) + \frac{1}{2}(u_i - u_{ref,i})^TR(u_i - u_{ref,i})\

\ell_N(x_N) = \frac{1}{2}(x_N - x_{ref,N})^TQ_f(x_N - x_{ref,N}) \end{gathered} $$

这里需要补充 iLQR的实现过程，用反向传播（backward pass）计算的值函数增量来判断iLQR的收敛$\Delta J < \text{atol}$，并在solve_quadrotor_trajectory函数中调用。

cost function的实现

# starter code: feel free to use or not use 

function stage_cost(p::NamedTuple, x::Vector, u::Vector, k::Int)
    # TODO: return stage cost at time step k 
    Q, R, xref, uref = p.Q, p.R, p.Xref[k], p.Uref[k]
    return 0.5 * (x - xref)' * Q * (x - xref) + 0.5 * (u - uref)' * R * (u - uref)
end
function term_cost(p::NamedTuple, x)
    # TODO: return terminal cost
    Qf, xref = p.Qf, p.Xref[end]
    return 0.5 * (x - xref)' * Qf * (x - xref)
end
function stage_cost_expansion(p::NamedTuple, x::Vector, u::Vector, k::Int)
    # TODO: return stage cost expansion
    # if the stage cost is J(x,u), you can return the following
    # ∇ₓ²J, ∇ₓJ, ∇ᵤ²J, ∇ᵤJ
    Q, R, xref, uref = p.Q, p.R, p.Xref[k], p.Uref[k]
    Jxx = Q
    Jx = Q * (x - xref)
    Juu = R
    Ju = R * (u - uref)
    return Jxx, Jx, Juu, Ju
end
function term_cost_expansion(p::NamedTuple, x::Vector)
    # TODO: return terminal cost expansion
    # if the terminal cost is Jn(x,u), you can return the following
    # ∇ₓ²Jn, ∇ₓJn

    Qf, xref = p.Qf, p.Xref[end]
    Jn_xx = Qf
    Jn_x = Qf * (x - xref)
    return Jn_xx, Jn_x
end

反向传播（backward pass）

原实现曾在 K[k] 出现 NaN 时直接替换为 K[k+1]。这会掩盖反向递推失败，相邻时刻的增益也不能随意替代，因此已移除。对称标量代价的二阶导数应满足 Qxu = Qux'；单独重算交叉项不能证明 NaN 的原因是累计误差。

下面增加有限值检查、对称化和基于 Cholesky 的正定性检查。对称化只适合小幅数值漂移；若不对称误差较大，应追查导数和递推实现。代码仍依赖作业的 discrete_dynamics 等函数，未完成原四旋翼实验复现。

function backward_pass(params::NamedTuple,          # useful params 
    X::Vector{Vector{Float64}},  # state trajectory 
    U::Vector{Vector{Float64}})  # control trajectory 
    # compute the iLQR backwards pass given a dynamically feasible trajectory X and U
    # return d, K, ΔJ  

    # outputs:
    #     d  - Vector{Vector} feedforward control  
    #     K  - Vector{Matrix} feedback gains 
    #     ΔJ - Float64        expected decrease in cost 

    nx, nu, N = params.nx, params.nu, params.N

    # vectors of vectors/matrices for recursion 
    P = [zeros(nx, nx) for i = 1:N]   # cost to go quadratic term
    p = [zeros(nx) for i = 1:N]   # cost to go linear term
    d = [zeros(nu) for i = 1:N-1] # feedforward control
    K = [zeros(nu, nx) for i = 1:N-1] # feedback gain

    # TODO: implement backwards pass and return d, K, ΔJ 
    N = params.N
    ΔJ = 0.0


    P[N], p[N] = term_cost_expansion(params, X[end])
    for k = N-1:-1:1
        Jxx, Jx, Juu, Ju = stage_cost_expansion(params, X[k], U[k], k)

        A_k = FD.jacobian(_x -> discrete_dynamics(params, _x, U[k], k), X[k])
        B_k = FD.jacobian(_u -> discrete_dynamics(params, X[k], _u, k), U[k])
        @assert all(isfinite, A_k) "A_k has NaN or Inf at step $k"
        @assert all(isfinite, B_k) "B_k has NaN or Inf at step $k"
        gx = Jx + A_k' * p[k+1]
        gu = Ju + B_k' * p[k+1]
  
        Qxx = Jxx + A_k' * P[k+1] * A_k
        Quu = Juu + B_k' * P[k+1] * B_k
        Qux = B_k' * P[k+1] * A_k
        Qxu = Qux'
  
        β = 1e-1
        @assert all(isfinite, Quu) && all(isfinite, Qux) && all(isfinite, gu)
        Quu = (Quu + Quu')/2
        F = nothing
  
        for i =1:15
            candidate = cholesky(Symmetric(Quu + β*I); check=false)
            if issuccess(candidate)
                F = candidate
                break
            end
            β *= 10
        end
  
        F === nothing && error("Quu regularization failed at step $k")
        d[k] = -(F \ gu)
        K[k] = -(F \ Qux)
        @assert all(isfinite, d[k]) && all(isfinite, K[k])

        p[k] = gx + K[k]' * gu + K[k]' * Quu * d[k]  + Qxu * d[k]
        P[k] = Qxx + K[k]' * Quu * K[k] + Qxu * K[k] + K[k]' * Qux
        P[k] = (P[k] + P[k]')/2
        # α=1 的局部二次模型预测下降；实际下降仍需前向验证。
        ΔJ += -(dot(d[k], gu) + 0.5*dot(d[k], Quu*d[k]))
    end
    return d, K, ΔJ
end

前向更新（foward pass）

function trajectory_cost(params::NamedTuple,          # useful params 
    X::Vector{Vector{Float64}},  # state trajectory 
    U::Vector{Vector{Float64}}) # control trajectory 
    # compute the trajectory cost for trajectory X and U (assuming they are dynamically feasible)
    N = params.N
    cost = 0
    for i = 1:N-1
        cost += stage_cost(params, X[i], U[i], i)
    end
    cost += term_cost(params, X[N])
    # TODO: add trajectory cost 
    return cost
end

function forward_pass(params::NamedTuple,           # useful params 
    X::Vector{Vector{Float64}},   # state trajectory 
    U::Vector{Vector{Float64}},   # control trajectory 
    d::Vector{Vector{Float64}},   # feedforward controls 
    K::Vector{Matrix{Float64}};   # feedback gains
    max_linesearch_iters=20)    # max iters on linesearch 
    # forward pass in iLQR with linesearch 
    # use a line search where the trajectory cost simply has to decrease (no Armijo)

    # outputs:
    #     Xn::Vector{Vector}  updated state trajectory  
    #     Un::Vector{Vector}  updated control trajectory 
    #     J::Float64          updated cost  
    #     α::Float64.         step length 

    nx, nu, N = params.nx, params.nu, params.N
    Xn = [zeros(nx) for i = 1:N]      # new state history 
    Un = [zeros(nu) for i = 1:N-1]    # new control history 

    # initial condition 
    Xn[1] = 1 * X[1]
    # initial step length 
    α = 1.0
    # TODO: add forward pass 
    #current cost
    J = trajectory_cost(params, X, U)
    Jn = trajectory_cost(params, Xn, Un)
    for i = 1:max_linesearch_iters
        for k = 1:N-1
            Un[k] = U[k] + K[k] * (Xn[k] - X[k]) + α * d[k]
            Xn[k+1] = discrete_dynamics(params, Xn[k], Un[k], k)
        end
        Jn = trajectory_cost(params, Xn, Un)
        if isfinite(Jn) && Jn < J
            return Xn, Un, Jn, α
        else
            α = 0.5 * α
        end
    end
    error("forward pass failed")
end

iLQR实现

function iLQR(params::NamedTuple,         # useful params for costs/dynamics/indexing 
    x0::Vector,                 # initial condition 
    U::Vector{Vector{Float64}}; # initial controls 
    atol=1e-3,                  # convergence criteria: ΔJ < atol 
    max_iters=250,            # max iLQR iterations 
    verbose=true)             # print logging

    # iLQR solver given an initial condition x0, initial controls U, and a 
    # dynamics function described by `discrete_dynamics`

    # return (X, U, K) where 
    # outputs:
    #     X::Vector{Vector} - state trajectory 
    #     U::Vector{Vector} - control trajectory 
    #     K::Vector{Matrix} - feedback gains K 

    # first check the sizes of everything
    @assert length(U) == params.N - 1
    @assert length(U[1]) == params.nu
    @assert length(x0) == params.nx

    nx, nu, N = params.nx, params.nu, params.N
    # TODO: initial rollout
    X = [x0 for i = 1:N]
    for i = 1:N-1
        X[i+1] = discrete_dynamics(params, X[i], U[i], i)
    end

    for ilqr_iter = 1:max_iters
        d, K, ΔJ = backward_pass(params, X, U)
        # 只在有限、非负的预测下降足够小时停止；不能将负值当作收敛。
        if isfinite(ΔJ) && 0 <= ΔJ < atol
            if verbose
                @info "iLQR converged"
            end
            return X, U, K
        end
        Xn, Un, J, α = forward_pass(params, X, U, d, K)
        X, U = Xn, Un

        # ---------------logging -------------------
        if verbose
            dmax = maximum(norm.(d))
            if rem(ilqr_iter - 1, 10) == 0
                @printf "iter     J           ΔJ        |d|         α         \n"
                @printf "-------------------------------------------------\n"
            end
            @printf("%3d   %10.3e  %9.2e  %9.2e  %6.4f    \n",
                ilqr_iter, J, ΔJ, dmax, α)
        end

    end
    error("iLQR failed")
end

给出参考轨迹和控制，定义模型参数

function create_reference(N, dt)
    # create reference trajectory for quadrotor 
    R = 6
    Xref = [ [R*cos(t);R*cos(t)*sin(t);1.2 + sin(t);zeros(9)] for t = range(-pi/2,3*pi/2, length = N)]
    for i = 1:(N-1)
        Xref[i][4:6] = (Xref[i+1][1:3] - Xref[i][1:3])/dt
    end
    Xref[N][4:6] = Xref[N-1][4:6]
    Uref = [(9.81*0.5/4)*ones(4) for i = 1:(N-1)]
    return Xref, Uref
end
function solve_quadrotor_trajectory(;verbose = true)
  
    # problem size 
    nx = 12
    nu = 4
    dt = 0.05 
    tf = 5 
    t_vec = 0:dt:tf 
    N = length(t_vec)

    # create reference trajectory 
    Xref, Uref = create_reference(N, dt)
  
    # tracking cost function
    Q = 1*diagm([1*ones(3);.1*ones(3);1*ones(3);.1*ones(3)])
    R = .1*diagm(ones(nu))
    Qf = 10*Q 

    # dynamics parameters (these are estimated)
    model = (mass=0.5,
            J=Diagonal([0.0023, 0.0023, 0.004]),
            gravity=[0,0,-9.81],
            L=0.1750,
            kf=1.0,
            km=0.0245,dt = dt)

  
    # the params needed by iLQR 
    params = (
        N = N, 
        nx = nx, 
        nu = nu, 
        Xref = Xref, 
        Uref = Uref, 
        Q = Q, 
        R = R, 
        Qf = Qf, 
        model = model
    )

    # initial condition 
    x0 = 1*Xref[1]
  
    # initial guess controls 
    U = [(uref + .0001*randn(nu)) for uref in Uref]
  
    # solve with iLQR
    X, U, K = iLQR(params,x0,U;atol=1e-4,max_iters = 250,verbose = verbose)
  
    return X, U, K, t_vec, params
end

结果（迭代过程，位置，速度，姿态，角速度和控制量变化）

iter     J           ΔJ        |d|         α       
-------------------------------------------------
  1    2.988e+02   1.37e+05   2.88e+01  1.0000  
  2    1.075e+02   5.31e+02   1.35e+01  0.5000  
  3    4.903e+01   1.33e+02   4.73e+00  1.0000  
  4    4.429e+01   1.15e+01   2.48e+00  1.0000  
  5    4.402e+01   8.05e-01   2.51e-01  1.0000  
  6    4.398e+01   1.44e-01   8.32e-02  1.0000  
  7    4.396e+01   3.78e-02   7.28e-02  1.0000  
  8    4.396e+01   1.29e-02   3.76e-02  1.0000  
  9    4.396e+01   5.06e-03   3.19e-02  1.0000  
 10    4.396e+01   2.28e-03   1.94e-02  1.0000  
iter     J           ΔJ        |d|         α       
-------------------------------------------------
 11    4.396e+01   1.14e-03   1.61e-02  1.0000  
 12    4.395e+01   6.21e-04   1.09e-02  1.0000  
 13    4.395e+01   3.62e-04   8.94e-03  1.0000  
 14    4.395e+01   2.22e-04   6.61e-03  1.0000  
 15    4.395e+01   1.40e-04   5.39e-03  1.0000

Part B:Tracking solution with TVLQR

通过iLQR生成的轨迹是开环，实际中会因模型误差（如风扰、参数不准）而偏离，这里使用iLQR得到的参考轨迹${\mathbf{x}{ilqr,k},\mathbf{U}{ilqr,k}}{k=1}^{N}$和时变增益矩阵${\mathbf{K}{ilqr,k}}_{k=1}^{N-1}$获得TVLQR反馈控制(注意K的正负号，这里输出的k=-Quu\Qux,自带了负号)

$$ \begin{gathered} \mathbf{u}{sim,k}=\mathbf{u}{ilqr,k}+K_{ilqr,k}(\mathbf{x}{sim,k}-\mathbf{x}{ilqr,k})\ \mathbf{x}{sim,k+1} =\operatorname{rk4}(\text{real model},\mathbf{x}{sim,k},\mathbf{u}_{sim,k},\Delta t) \end{gathered} $$

# set verbose to false when you submit 
    Xilqr, Uilqr, Kilqr, t_vec, params =  solve_quadrotor_trajectory(verbose = false)
  
    # real model parameters for dynamics 
    model_real = (mass=0.5,
            J=Diagonal([0.0025, 0.002, 0.0045]),
            gravity=[0,0,-9.81],
            L=0.1550,
            kf=0.9,
            km=0.0365,dt = 0.05)
  
    # simulate closed loop system 
    nx, nu, N = params.nx, params.nu, params.N
    Xsim = [zeros(nx) for i = 1:N]
    Usim = [zeros(nu) for i = 1:(N-1)]
  
    # initial condition 
    Xsim[1] = 1*Xilqr[1]
  
    # TODO: simulate with closed loop control 

    for i = 1:(N-1) 
        Usim[i] = Uilqr[i]+Kilqr[i]*(Xsim[i]-Xilqr[i])
        Xsim[i+1] = rk4(model_real, quadrotor_dynamics, Xsim[i], Usim[i], model_real.dt)
    end

TVLQR结果如下

HW3_Q3:Quadrotor Reorientation (40 pts)

$$ x = \begin{bmatrix} p_x \ p_z \ \theta \ v_x \ v_z \ \omega \end{bmatrix},\qquad \dot{x} = \begin{bmatrix}v_x \ v_z \ \omega \ \frac{1}{m}(u_1 + u_2)\sin\theta \ \frac{1}{m}(u_1 + u_2)\cos\theta-g \ \frac{\ell}{2J}(u_2 - u_1)\end{bmatrix} $$

问题要求

实现三架平面无人机的无碰撞的轨迹优化问题，要求如下

无人机的初始位置$x1ic,x2ic,x3ic$和末端期望位置$x1g,x2g,x3g$给定
无人机彼此的距离不能小于$0.8\mathrm{m}$

一般的iLQR无法直接处理碰撞约束，需要在代价函数中纳入距离成本，但无法保证完全满足距离约束，这里选择使用直接配点法DIRCOL，求解问题的数学形式设计如下

$$ \begin{align} \min_{x_{1:N},u_{1:N-1}} \quad & \sum_{i=1}^{N-1} \bigg[ \frac{1}{2} (x_i - x_{goal})^TQ(x_i - x_{goal}) + \frac{1}{2} u_i^TRu_i \bigg] + \frac{1}{2}(x_N - x_{goal})^TQ_f(x_N - x_{goal})\ \text{st} \quad & x_1 = x_{\text{IC}} \ & x_N = x_{goal} \ & f_{hs}(x_i,x_{i+1},u_i,dt) = 0 \quad \text{for } i = 1,2,\ldots,N-1 \ &||p_{1,i}-p_{2,i}||>=0.8,||p_{1,i}-p_{3,i}||>=0.8,||p_{2,i}-p_{3,i}||>=0.8\ \end{align} $$

实际中将三架无人机各自的状态变量和控制变量进行合并，$z$为整个要求解的优化变量

$$ \begin{gathered} x_i =[x1_{i};x2_{i};x3_{i}]\ u = [u1_{i};u2_{i};u3_{i}]\ z=[x_1;u_1;...;u_{N-1};x_N ] \end{gathered} $$

碰撞约束可写成 $(p_i-p_j)^\top(p_i-p_j)\ge R^2$，代码用 sum(abs2, p_i-p_j)，直接表达平滑的平方距离，避免先计算零点不可微的 norm 再平方。该约束是非凸的，且只在离散节点满足距离下限不足以保证节点之间或有尺寸的无人机无碰撞；还需加密轨迹验证并纳入几何尺寸。

初始轨迹的生成可以使用 x_initialize = range(xic, xg, length = N)

整体的代码框架与HW3_Q1基本一致，结果如下

整理与核查说明

本笔记原有许可为 CC BY 4.0。引用的课程材料、代码和图片仍须遵守其各自的许可。

核查状态：部分验证（2026-10-04）。 Julia 1.10.10 的简化线性测试检验 iLQR 递推与线搜索拒绝逻辑；原四旋翼和 DIRCOL 实验待复现。