3.1 SO3 & SE3

0. 前言

​ PID 调参大概是每个无人机开发者最熟悉的痛：位置环调好了，姿态环开始振荡，姿态环稳了，位置响应又变慢了，想飞个激进的动作，飞机在 pitch=90° 附近突然"死锁"乱转。这些问题的根源并非 PID 本身不好，而是 PID 在欧拉角的空间里做减法，而欧拉角并不适合描述大角度旋转。

如果你已经有了串级 PID 的经验，这篇文章的目标是让你用你已经懂的 PID 直觉来理解 SO(3)/SE(3) 控制。你会发现：几何控制的核心理念和 PID 完全一样：P 项提供回正力、D 项提供阻尼。唯一的区别在于"误差"怎么量。 传统 PID 在欧拉角上量误差（角度减法），几何控制在旋转矩阵上量误差（几何距离）。前者在 pitch=90° 时会"撞墙"（死锁），后者把旋转当作一个整体来处理：消除死锁、参数独立、倒吊也能自动翻回来，全是因为找对了量误差的空间。

读完本文后，你将能够：

• 理解欧拉角表示姿态的三大缺陷（死锁、wraparound、串级耦合）及其数学根源
• 认识 SO(3) 作为旋转的正确空间：什么是旋转矩阵、什么是 so(3)、什么是 hat map 和指数映射
• 掌握 SO(3) 几何姿态控制器的核心公式：$e_R=\frac{1}{2}(R_d^{T}R-R^T{R}_d{)}^{\vee }$，并理解它为何能替代 $e_d-e$
• 理解 SE(3) 位置控制的分层设计：位置误差 → 期望推力方向 → 构造完整目标姿态 → 推力投影
• 对比串联 PID 和几何控制的参数解耦特性，掌握按环独立调参的方法

---

目录

1. 传统PID以及使用欧拉角表示姿态的一些弊端
2. 旋转的正确定义空间：SO(3)
3. SO(3) 几何姿态控制
4. SE(3) 位置控制
5. 总结：PID 和 SO(3)/SE(3) 的对比

---

1. 传统PID以及使用欧拉角表示姿态的一些弊端

问题一：欧拉角死锁

​ 数学上有很多方法描述旋转，欧拉角是其中最为直观的一种，即直接使用在三个旋转方向上(roll, pitch, yaw)旋转的角度来表示一个物体的姿态。这种姿态表示方法在较小角度的飞行情况中完全合适，但是当要执行激进轨迹跟踪或者受到巨大外部扰动的时候可能会出现问题，例如当俯仰角度(pitch)接近于 ±90° 的时候：

初始姿态：飞机水平，机头朝北
  roll=0, pitch=0, yaw=0

现在我们让 pitch 逐渐增大到 90°：
  pitch → 90° 的过程中，飞机的机头从指向前方逐渐变为指向正上方

在 pitch=90° 那一瞬间：
  - yaw 旋转轴（原本是世界的 Z）现在对准了机体的 X
  - roll 旋转轴（原本是机体的 X）现在对准了世界的 Z
  - 两条轴重合了。

两条轴重合意味着什么？roll 和 yaw 变成了同一件事。 你无法区分"飞机绕世界 Z 转了 10°"和"飞机绕机体 X 转了 10°"：因为在这个姿态下，它们一模一样。用数学写：

$$R=R_z(\psi )R_y(\frac{\pi }{2})R_x(\varphi )=\left[\begin{array}{ccc}0& \sin (\varphi -\psi )& \cos (\varphi -\psi )\\ 0& \cos (\varphi -\psi )& -\sin (\varphi -\psi )\\ -1& 0& 0\end{array}\right]$$

旋转矩阵里只出现了 $\varphi -\psi$：你只能知道 roll 和 yaw 的差，无法分别知道它们各自是多少。任何仅依赖欧拉角做姿态误差计算的控制器，在死锁点附近都会遇到本质上无法绕开的数值问题。（实践中可以通过切换到四元数或其他表示来缓解，但控制架构如果仍然在欧拉角空间内定义误差，则从根本上受限于此奇异性。）

类比：用经纬度描述地球。在赤道上，经度和纬度是两个独立的方向，一切正常。当你走到北极点，经度失去定义：你站在北极，经度是多少？任何一个经度都对，这就是坐标系的奇异性。欧拉角的 pitch=90° 就是北极点。

问题二：角度 wraparound：误差走错方向

在 PID 控制器中，姿态误差定义为：

angle_error = target_angle - current_angle

考虑这个场景：当前 yaw = -179°，目标 yaw = +179°。

PID 的减法：+179° - (-179°) = 358°
    → PID 会认为需要转 358° 才能到达目标
    → 实际最短路径只需要转 2°（跨过 180° 边界）

控制器根据 358° 误差输出大力矩 → 飞机大幅反转 → 轻则振荡，重则失稳

这是欧拉角表示的固有问题：角度在 ±180° 处不连续，而减法不关心"最短路径"。实践中通常会对误差做 wrap 处理（映射到 [-180°, 180°] 区间）来缓解，但 wrap 只能解决"绕远路"的问题，无法解决大角度下欧拉角运动学非线性化带来的根本困难。

问题三：串级耦合：调好一个，坏了一堆

串级架构的根本设计是：位置环的输出是姿态环的输入。这看起来合理，但带来了致命的参数耦合：

• 你调大了位置环的 P 增益 → 期望加速度变大 → 期望姿态角变大 → 姿态环的参考输入变大 → 姿态环原来的阻尼不够用 → 姿态开始振荡
• 你调大了姿态环的 D 增益来消振 → 角速率环的工作点变了 → 整体响应变慢 → 位置跟踪又变差了

三个环互相拉扯，需要反复迭代才能找到一组能用的参数。没有经验的调参者很容易陷入"调好一个、坏了一堆"的死循环。

三个问题的共同根源

回看这三个问题：

问题	根因
死锁	欧拉角这个坐标系在 pitch=90° 处是奇异的（roll 和 yaw 退化）
wraparound	角度是一个"圆"，但欧拉角用 ±180° 的线段来表示圆：两端断开了
串级耦合	位置环输出欧拉角给姿态环，欧拉角之间的耦合传递到了参数之间

三个问题指向同一个根源：用三个分离的欧拉角描述一个本质上是整体几何对象的旋转。 旋转是一个 3 自由度的连续封闭曲面，而欧拉角是一种"用平面地图表示球面"的方法：在大部分区域是有效的近似，但在特定位置（pitch = ±90°）会产生奇异性，在 ±180° 边界处则出现不连续性。

---

2. 旋转的正确定义空间：SO(3)

2.1 从一个不同的角度思考旋转

在进入任何"高级数学"之前，先问一个简单的问题：什么是旋转？

在你接触欧拉角之前，你对旋转的理解可能是动手转一个物体：你不知道什么 roll/pitch/yaw，但你完全能想象"绕空间中某根轴转动某个角度"。

回到你的 PX4 代码。每次飞控运行时，陀螺仪给出角速度 $[{\omega }_x,{\omega }_y,{\omega }_z]$，然后代码把它积分成姿态。你最终用的是什么？ 很可能不是欧拉角，而是四元数。为什么？因为四元数没有死锁。

四元数对应什么？四元数对应一个 3×3 的旋转矩阵。这个旋转矩阵 $R$ 就是"我是谁"的答案：它的每一列是世界坐标系中机体三轴的方向向量：

R 的第 1 列 = 机体 X 轴在世界系中的方向（例如 [1,0,0]ᵀ 表示 X 轴指向东）
R 的第 2 列 = 机体 Y 轴在世界系中的方向（例如 [0,1,0]ᵀ 表示 Y 轴指向北）
R 的第 3 列 = 机体 Z 轴在世界系中的方向（例如 [0,0,1]ᵀ 表示 Z 轴指向上）

这就是整个教程最重要的一个认识：姿态控制的核心对象是旋转矩阵。 欧拉角只是你"阅读"这个矩阵的一种方式。四元数只是你"压缩存储"这个矩阵的一种方式。旋转矩阵本身才是姿态的完整几何实体。

2.2 SO(3)：所有旋转矩阵构成的集合

数学家把"所有合法的 3×3 旋转矩阵"这个集合叫做 SO(3)：

$$SO(3)=\{R\in {\mathbb{R}}^{3\times 3}\mid R^{T}R=I,det(R)=1\}$$

不用害怕这个符号。它只是说：

• R 是 3×3 的实数矩阵
• $R^{T}R=I$ — 三列互相正交且长度都为 1（保持向量长度不变 = 纯旋转不缩放）
• $det(R)=1$ — 保定向（右手系不翻转为左手系）

SO(3) 作为一个几何对象，它的形状是什么？它是一个光滑的三维封闭曲面（没有边界、没有孔洞、没有奇点）。旋转矩阵之间可以通过连续路径到达：不存在任何"写不出来"的姿态。

现在回到第 1 节提出的核心问题：我们到底需要什么？

需求	欧拉角	SO(3)
3 个自由度	✓	✓
没有奇异性	✗（死锁）	✓
表示是连续的	✗（wraparound）	✓
两个姿态的"差"有良好定义	✗（角度差会绕远路）	✓（几何距离）

2.3 你已经在用 SO(3) 了：只是名字不叫这个

回顾一下你的 PX4 PID 控制器中姿态更新的流程：

1. 陀螺仪给出角速度 ω = [ωx, ωy, ωz]    ← 一个三维向量
2. 乘以 dt 得到小旋转角度 θ = ω·dt       ← 还是三维向量
3. 把当前四元数更新：q_new = q ⊗ δq     ← 四元数乘法
4. 需要的时候把四元数转成欧拉角做控制    ← 现在问题来了：用欧拉角做了控制

步骤 3 的背后，实际发生的是旋转的复合。在矩阵的世界里，离散时间姿态更新是：

$$R_{k+1}=R_k\cdot \exp ({\hat{\omega }}_k\mathrm{\Delta }t)$$

这条公式的意思是："在当前的旋转 $R_k$ 之上，再复合一个小旋转（角速度 $\omega$ 持续 $\mathrm{\Delta }t$ 秒产生的旋转），得到新的姿态 $R_{k+1}$。"

这和你用四元数的 $q_{new}=q_{current}\otimes q_{delta}$ 是一回事。两者都避免了死锁。

2.4 旋转矩阵的"导数"：角速度的矩阵形式

在 PID 的世界里，角速度是控制的核心。位置环输出速度，姿态环输出角速度。角速度始终是一个向量：你在调 D 增益时面对的就是它。

在 SO(3) 的框架下，旋转矩阵的导数是什么？

$$\dot{R}=R\hat{\omega },\hat{\omega }=\left[\begin{array}{ccc}0& -{\omega }_z& {\omega }_y\\ {\omega }_z& 0& -{\omega }_x\\ -{\omega }_y& {\omega }_x& 0\end{array}\right]$$

这个反对称矩阵 $\hat{\omega }$ 是用 hat map（记作 $\wedge$）从角速度向量变出来的。对这个矩阵不需要害怕：你每时每刻都在用它：

$$\hat{\omega }v=\omega \times v$$

hat map 不过是把叉积换成矩阵乘法的写法而已。 叉积 $\omega \times v$ 和矩阵乘法 $\hat{\omega }v$ 是同一回事。因此：

• $\hat{\omega }$ = 角速度的"矩阵形式"，和向量 $\omega$ 包含完全相同的信息
• vee map（记作 $\vee$）就是反过来：从反对称矩阵提取回向量

这和你已经熟悉的东西是一一对应的：

PID 中的概念	SO(3) 中的概念
角速度向量 $\omega =[p,q,r]$	反对称矩阵 $\hat{\omega }\in \mathfrak{so}(3)$
叉积 $\omega \times v$	矩阵乘法 $\hat{\omega }v$
四元数更新 $q_{new}=q\otimes \delta q$	矩阵更新 $R_{k+1}=R_k\exp (\mathrm{\Delta }t\hat{\omega })$

小结：你不需要深入学"李代数"的所有细节来使用 SO(3) 控制。你只需要知道：把角速度向量 $\omega$ 用 hat map 变成矩阵 $\hat{\omega }$，就可以像"在矩阵的世界里积分角速度"一样更新姿态，而且这个更新是精确的，不依赖小角度近似。李代数 $\mathfrak{so}(3)$ 只是"所有 $\hat{\omega }$ 这样的反对称矩阵"的正式名称。

2.5 指数映射：把角速度积分成旋转，精确执行

上面提到过：$R_{k+1}=R_k\cdot \exp (\mathrm{\Delta }t\cdot \hat{\omega })$，$\exp (\hat{\omega })$ 是什么？

在微积分中，$e^x=1+x+\frac{x^2}{2!}+\frac{x^3}{3!}+\dots$。把 $x$ 换成矩阵 $\hat{\omega }$，得到的还是用同样的无穷级数定义的：

$$\exp (\hat{\theta })=I+\hat{\theta }+\frac{1}{2!}{\hat{\theta }}^2+\frac{1}{3!}{\hat{\theta }}^3+\dots$$

这个无穷级数有一个闭式解，即 Rodrigues 公式：

$$\exp (\hat{\theta })=I+\frac{\sin \parallel \theta \parallel }{\parallel \theta \parallel }\hat{\theta }+\frac{1-\cos \parallel \theta \parallel }{\parallel \theta {\parallel }^2}{\hat{\theta }}^2$$

其中 $\theta =\omega \cdot \mathrm{\Delta }t$ 是旋转向量：它的方向是旋转轴，它的长度是旋转角。你对这个公式应该不陌生：它就是轴-角到旋转矩阵的转换，你很有可能在某个工具函数里见过它。

编程提示：你不需要手算这个级数。Python 的 scipy.linalg.expm()，C++ 的 Eigen 库 Eigen::Matrix3d::exp()，以及 MATLAB 的 expm() 都直接提供了矩阵指数函数。你只需要把角速度向量构建成 3×3 反对称矩阵，然后调用库函数即可。

指数映射做的事情并不复杂：

输入：旋转轴 + 旋转角度 = θ
输出：对应的旋转矩阵 = exp(θ̂)

例：机头俯仰 30° = 绕机体 Y 轴转 30°
    θ = [0, π/6, 0]ᵀ
    exp(θ̂) = 一个旋转矩阵，它的第 3 列从 [0,0,1] 变成了 [sin30°, 0, cos30°]

逆运算叫对数映射：给定旋转矩阵，反推出旋转向量。这两个映射在 SO(3) 和角速度之间双向通行，是"姿态变化"这个操作的精确数学表达。

---

3. SO(3) 几何姿态控制

现在进入核心环节：定义了我们的工作空间（SO(3) 而不是欧拉角空间）之后，控制器怎么写？

3.1 正确定义姿态误差

如果只挑一个最核心的区别，那就是误差的定义方式。 前面那么多铺垫，都是为了让这一步变得自然。除此之外，前馈补偿的结构、推力的投影计算等也存在差异，但它们都可以在 PD 的框架下理解。

回忆 PID 的姿态误差：

PID:
  roll_error  = target_roll  - current_roll   ← 在"角度数轴"上做减法
  pitch_error = target_pitch - current_pitch
  yaw_error   = target_yaw   - current_yaw

在 SO(3) 框架下，给定当前旋转矩阵 $R$ 和目标旋转矩阵 $R_d$，我们怎么定义它们之间的"误差"？

先看 Lee et al. 2010 论文的做法。首先定义一个标量误差函数，衡量两个旋转矩阵的"距离"：

$$\mathrm{\Psi }(R,R_d)=\frac{1}{2}\mathrm{tr}[I-R_d^{T}R]$$

其中 $R_d^{T}R$ 是从当前姿态到目标姿态的旋转差。当 $R=R_d$ 时 $R_d^{T}R=I$，$\mathrm{\Psi }=0$。利用等式 $\mathrm{tr}[R_d^{T}R]=2\cos \theta +1$（$\theta$ 为旋转差的角度），可得 $\mathrm{\Psi }=1-\cos \theta$：旋转差越大，$\mathrm{\Psi }$ 越大，当误差为 180° 时 $\mathrm{\Psi }$ 达到最大值 2。

现在我们不直接使用 $\mathrm{\Psi }$ 做控制（它是一个标量，不包含旋转轴的方向信息），而是取 $\mathrm{\Psi }$ 在 SO(3) 上的梯度。对于以 $\delta R=R\hat{\eta }$ 形式表达的微小姿态扰动 $\eta$，$\mathrm{\Psi }$ 的方向导数为 $e_R\cdot \eta$，其中：

$$e_R=\frac{1}{2}(R_d^{T}R-R^T{R}_d{)}^{\vee }$$

逐层理解这个公式：

• $R_d^{T}R$：从当前到目标的旋转差。记 $R_d^{T}R=\exp (\hat{\varphi })$，其中 $\varphi$ 是旋转向量（方向 = 旋转轴，长度 = 旋转角度）
• $R^T{R}_d=(R_d^{T}R{)}^T=\exp (-\hat{\varphi })$：旋转差的逆
• 两者相减：$\exp (\hat{\varphi })-\exp (-\hat{\varphi })=\frac{2\sin \parallel \varphi \parallel }{\parallel \varphi \parallel }\hat{\varphi }$：它们的差正好是 $\hat{\varphi }$ 乘以 $\frac{2\sin \parallel \varphi \parallel }{\parallel \varphi \parallel }$
• $\frac{1}{2}(\cdot {)}^{\vee }$：提取出 $\frac{\sin \parallel \varphi \parallel }{\parallel \varphi \parallel }\cdot \varphi$

因此 e_R 精确等于：

$$e_R=\frac{\sin \theta }{\theta }\cdot \varphi ,\text{其中 }\theta =\parallel \varphi \parallel$$

给初学者的直觉：你可以把 $e_R$ 理解为一个"旋转误差向量"：它的方向始终是当前姿态到目标姿态的最短旋转轴，它的大小是 $\sin \theta$（乘上一个接近 1 的因子）。当飞机只有微小偏移时，$\sin \theta \approx \theta$，所以 $e_R\approx \varphi \approx [{\mathrm{roll}}_{err},{\mathrm{pitch}}_{err},{\mathrm{yaw}}_{err}{]}^T$：和 PID 的误差几乎一样。当飞机大角度翻转时，$e_R$ 仍然沿着最短旋转轴的方向，不会像欧拉角减法那样算出"绕远路"的结果。当 $\theta \to {180}^{\circ }$ 时 $\sin \theta \to 0$，$e_R$ 自然地趋于零：这是后文要讨论的 180° 边界情况。

关键性质：

• $e_R$ 是标量误差函数 $\mathrm{\Psi }$ 的梯度方向：沿着 $e_R$ 的反方向旋转，能以最快速度减小姿态误差
• 当误差很小时，$e_R\approx [{\mathrm{roll}}_{err},{\mathrm{pitch}}_{err},{\mathrm{yaw}}_{err}{]}^T$：和 PID 的误差退化一致
• $e_R$ 在 SO(3) 上光滑定义（对任何 $\theta <{180}^{\circ }$ 都有良定义且非零），始终指向最短旋转路径

3.2 角速度误差

​ 目标角速度${\mathrm{\Omega }}_d$本来是定义在目标机体坐标系中的。要做比较，需要先把它统一到当前的机体坐标系下：

$$e_{\mathrm{\Omega }}=\mathrm{\Omega }-R^T{R}_d{\mathrm{\Omega }}_d$$

​ 其中$R^T{R}_d$就是前面提到的旋转差：它把目标机体坐标系中的向量${\mathrm{\Omega }}_d$转换到当前机体坐标系中，然后你就可以用当前测量的$\mathrm{\Omega }$去减了。

​ 这和 PID 中角速度误差的定义(Error Rate = Current Rate - Target Rate)在逻辑上完全一致，只是多了一步坐标系对齐。

3.3 控制律：流形上的 PD

误差都定义好了，控制律的核心就是 P 和 D 的组合：

$$M=-k_R{e}_R-k_{\mathrm{\Omega }}{e}_{\mathrm{\Omega }}$$

形式上和你写过的 PD 控制器一致：P 项乘以姿态误差、D 项乘以角速度误差。在原始论文（Lee et al. 2010）中，$k_R$ 和 $k_{\mathrm{\Omega }}$ 都是标量增益。在实际应用中（如本教程配套的参考工程），由于四旋翼三轴转动惯量差异显著，通常会将增益扩展为按轴区分的对角矩阵：例如 Roll/Pitch 和 Yaw 使用不同的增益值。这属于工程上的自然推广，不改变控制器的核心结构。

项	你在 PID 中调过的含义	SO(3) 中的物理含义
$k_R$	P 增益（比例）	姿态刚度 — 决定恢复力矩随姿态误差增长的速度
$e_R$	角度误差	几何姿态误差 — 标量误差函数 $\mathrm{\Psi }$ 的梯度，方向为最短旋转轴
$k_{\mathrm{\Omega }}$	D 增益（微分）	角速度阻尼 — 决定阻尼力矩随角速度误差增长的速度
$e_{\mathrm{\Omega }}$	角速度误差	角速度误差 — 与目标 PID 中的角速率误差一致

对于需要精确跟踪动态轨迹的场景，完整的控制律还包含前馈补偿项（补偿陀螺力矩和目标角加速度），它们不引入新参数，只是在力矩指令中加上已知的动力学补偿：

$$M=-k_R{e}_R-k_{\mathrm{\Omega }}{e}_{\mathrm{\Omega }}+\underset{\text{前馈：补偿陀螺效应 + 目标角加速度}}{\underbrace{\mathrm{\Omega }\times J\mathrm{\Omega }-J(\hat{\mathrm{\Omega }}{R}^T{R}_d{\mathrm{\Omega }}_d-R^T{R}_d{\dot{\mathrm{\Omega }}}_d)}}$$

如果你现在只记住了一件事，记住这个：SO(3) 控制律就是 P 乘误差 + D 乘误差导数。和 PID 一模一样。唯一的区别在于误差怎么算：$e_R=\frac{1}{2}(R_d^{T}R-R^T{R}_d{)}^{\vee }$ 代替了 $e_d-e$。

3.4 参数的物理含义与独立调参

SO(3) 控制器只有 4 个独立的增益参数。Roll/Pitch 和 Yaw 因为转动惯量不同（Z 轴惯量通常最大所以 yaw 响应最慢），所以分开调：

kR_rp  — Roll/Pitch 回正刚度    (Nm/rad)    越大 → Roll/Pitch 姿态误差消失越快
kR_y   — Yaw 回正刚度           (Nm/rad)    越大 → 偏航误差消失越快
kW_rp  — Roll/Pitch 阻尼        (Nm/(rad/s)) 越大 → Roll/Pitch 振荡越小
kW_y   — Yaw 阻尼               (Nm/(rad/s)) 越大 → 偏航振荡越小

每个参数都独立地控制一个物理量：

• 调 kR_rp 只影响 roll/pitch 的回位速度，不影响 yaw
• 调 kW_rp 只影响 roll/pitch 的阻尼，不影响 yaw
• 调位置相关参数（在 SE(3) 中）不影响姿态参数

对比 PID 的串级调参，这里不存在"调一个参数会连锁改变其他环工作点"的问题。

3.5 稳定性与吸引域

Lee et al. 2010 的论文分三个层次证明了控制器的稳定性。注意下述结果区分了姿态子系统（仅姿态动力学，假设推力不影响位置）和完整系统（位置 + 姿态耦合动力学）：

命题	范围	初始姿态误差	结论
Prop 1	姿态子系统	$\mathrm{\Psi }<2$（即 $\theta <{180}^{\circ }$）	指数稳定
Prop 2	完整系统	$\mathrm{\Psi }\le {\psi }_1<1$（即 $\theta <{90}^{\circ }$）	指数稳定
Prop 3	完整系统	$1\le \mathrm{\Psi }<2$（即 ${90}^{\circ }\le \theta <{180}^{\circ }$）	指数吸引

• Prop 1：仅考虑姿态动力学时，控制器可以在几乎整个 SO(3) 上稳定姿态（< 180° 即收敛）。
• Prop 2 & 3 组合：考虑位置和姿态的耦合后，完整系统的收敛分为两个阶段。初始误差 < 90° 时直接指数稳定，初始误差在 90°~180° 之间时，姿态误差先由 Prop 1 驱动减小（指数吸引），进入 90° 范围后再由 Prop 2 保证指数稳定。两者结合，对于几乎所有初始状态（误差 < 180°），完整系统都能收敛。

关于 180° 边界：如果初始姿态误差恰好等于 180°（即当前姿态与目标姿态正好是对径点），此时 $e_R=\frac{\sin \pi }{\pi }\varphi =0$，控制器输出零力矩，系统处于一个不稳定平衡点。这是 SO(3) 拓扑性质决定的必然结果：SO(3) 上不存在处处非零的光滑向量场（"毛球定理"在高维的推广），任何连续控制器在此处都会遇到同样的问题，与该控制器本身的设计无关[^1]。

[^1]: 严格地说，SO(3) 上的光滑向量场至少存在两个零点。SO(3) 的姿态误差函数 $\mathrm{\Psi }$ 的临界点包括 $I$（稳定平衡点）和所有形如 $\exp (\pi \hat{v})$ 的旋转（不稳定平衡点，构成一个二维子流形）。这两类零点共同构成一个零测度集：非收敛的初始状态在全部可能状态中"几乎不存在"。

在物理系统中，传感器噪声、气流扰动和结构微振动都会产生微小的姿态偏差，将系统推离这个零测度集，从而进入收敛域。因此在实际飞行中，即使初始倒吊，飞机也能自动翻回。

---

4. SE(3) 位置控制

姿态控制已经解决，那么下一步就应该加入位置控制了。

4.1 SE(3) 是什么

SO(3) 只描述旋转，但四旋翼实际上有 6 个自由度，除了姿态，还有三维位置。同时表示位置和姿态的数学空间叫 SE(3)（特殊欧几里得群）：

$$SE(3)=\left\{\left[\begin{array}{cc}R& p\\ 0& 1\end{array}\right]\right|R\in SO(3),p\in {\mathbb{R}}^3\}$$

一个 SE(3) 元素就是一个 4×4 齐次变换矩阵，第 1~3 行第 1~3 列是旋转矩阵 $R$，第 1~3 行第 4 列是位置向量 $p$。

4.2 SE(3)位置控制的分层设计

SE(3) 控制器采用两层架构，和传统的PID串级控制器存在一定相似性。

串级 PID                             SE(3) 几何控制
──────────────────────────          ──────────────────────────────
位置指令                           位置指令
  ↓                                  ↓
[位置PID] → 期望加速度           [位置PD] → 期望推力方向 b₃ᵈ
  ↓                                  ↓
 → 期望欧拉角                        b₃ᵈ + 偏航指令 → 构造 R_d
  ↓                                  ↓
[姿态PID] → 期望角速度           [内环 SO(3) PD] → 力矩 M
  ↓                                  ↓
[角速率PID] → 力矩                  总推力 f = 期望力在机体Z轴上的投影

​ 两者的分层控制结构基本一致，均为位置误差产生目标姿态，然后使用姿态控制器进行姿态跟踪。但是两者也存在较大的区别，首先是从位置误差到目标姿态的映射方式存在差别，SE3控制器计算位置和速度的误差矢量，然后叠加上重力矢量作为整体的目标推力矢量，随后将这个矢量作为目标机体姿态的Z轴朝向，这样才能够准确产生向目标方向运动的推力方向，而推力大小则和误差大小相关。而对于传统的PID串级控制器来说，各轴位置误差大小和目标速度成比例，目标速度和目标姿态角度成比例，各个映射环节均为简单的线性映射，这种方式计算的目标姿态并不是理想的目标姿态，因此在大幅度扰动下容易超出机体倾角和动力的限制，因此需要额外的限幅机制，并且调参较为困难。除此，串级PID三层之间是使用欧拉角来表示和传递误差，存在死锁，并且各轴之间的耦合也无法考虑，因此在激进姿态飞行条件下容易发生不稳定。而对于SE3控制器，位置环输出的是一个目标推力方向，然后姿态环去跟踪这个方向，全程没有欧拉角的参与，不存在死锁问题，并且位置速度层（平动维度）和姿态层（旋转维度）较为独立，调参更为直观和容易。

4.3 期望推力方向的计算

​ 首先对位置和速度的误差进行定义，因为他们都定义在平坦的 ℝ³ 空间中（三维笛卡尔坐标系），不需要任何空间变换，用向量减法就够了：

$$e_x=x-x_d,e_v=v-v_d$$

​ 位置误差和速度误差均为三维矢量，通过这两个矢量可以直接计算目标加速度矢量，这里使用一个标准的PD控制器来计算目标加速度矢量，并且可以进一步添加加速度前馈来实现更为精准的轨迹跟踪。

$$a_{des}=-k_x{e}_x-k_v{e}_v+{\ddot{x}}_d$$

​ 但是加速度矢量和机体推力的朝向还并不一致，因为存在重力的影响，机体产生的推力需要使用一部分来抵消重力，因此实际推力方向应该为重力矢量和目标加速度矢量之和，则若要让四旋翼整体产生期望的加速度，机体 Z 轴在世界坐标系中的朝向（单位向量）应该表示如下：

$$b_{3d}=\frac{a_{\text{des}}-\mathbf{g}}{\parallel a_{\text{des}}-\mathbf{g}\parallel }$$

实际编写代码时需要注意重力加速度g的方向，例如在世界系为 ENU（东北天）时，$g=[0,0,-9.8{]}^T$（重力向下）。在世界系为 NED（北东地）时，$g=[0,0,9.8{]}^T$。当飞行器处于悬停状态的时候，若不存在外部扰动，则$a_{des}=[0,0,0{]}^T$计算得出的$b_{3d}=[0,0,1{]}^T$则机体Z轴朝上，很好理解。

给初学者的直觉：这条公式做的事情其实非常朴素。你拿着位置误差，用 PD 算出一个期望加速度$a_{des}$（比如"我希望飞机在 XY 平面上加速"）。然后你把重力向量 g 从$a_{des}$里减去：因为你需要在推力方向上同时克服重力。剩下来的向量的方向，就是推力应该指的方向。

4.4 从推力方向构造完整姿态

​ 之间的计算步骤只确定了机体Z轴目标朝向，但是完整机体姿态还包含绕这个目标方向旋转的维度，也就是航向角度，在这个维度上任意旋转均能保持推力方向不变，我们需要目标推力朝向和航向角度才能够唯一确定目标姿态（旋转矩阵）。

为什么偏航是直接指定，而横滚/俯仰是算出来的？ 四旋翼的偏航（绕 Z 轴的旋转）是欠驱动的，即没有直接产生偏航力矩的执行器，只能靠螺旋桨反扭矩的差动来实现。而且偏航不影响位置控制，即无论机头朝哪，只要 Z 轴方向对了，推力就能把你送到目标位置。因此，控制架构中位置环只决定飞机"身体倾斜的方向和程度"，而偏航角（机头朝向）则由飞行员或路径规划器直接指定。

​ ${\psi }_d$只给出了机头在水平面内的投影方向，我们需要在此基础上正交化出完整的旋转矩阵：

b₁ᵈ = [cos ψ_d, sin ψ_d, 0]ᵀ ← 机头在水平面的投影（不是最终的机体 X 轴）
b₂ᵈ = normalize(b₃ᵈ × b₁ᵈ) ← 机体 Y 轴（右手系：Z × X = Y）
b₁ᵈ = b₂ᵈ × b₃ᵈ ← 重新正交化后的机体 X 轴
R_d  = [b₁ᵈ, b₂ᵈ, b₃ᵈ] ← 期望旋转矩阵

​ [唯一要注意的边界情况：当 $b_{3d}$ 和 $b_{1d}$ 平行时（例如飞机垂直上升/下降，机头朝上），叉积为零，$b_{2d}$ 无法定义。此时可以沿用当前机体的 Y 轴方向，不影响控制效果。]

4.5 推力幅值的计算与修正

​ 我们在之前已经计算出了具体的目标加速度$a_{des}$。并且重力加速$g$也是已知的，所以将总加速度和飞行器质量相乘即可得出期望的推力输出：

$$F_{\text{des}}=m\cdot (a_{\text{des}}-\mathbf{g})$$

​ 这个式子的假设是机体的Z轴方向和$F_{des}$的方向完全重合，此时的推力输出是准确的，但是问题是若$F_{des}$的变化过于迅速或者飞机的姿态受到了巨大的扰动，超过的姿态控制器的跟踪速度，则机体的Z轴方向和$F_{des}$可能存在较大的误差，则此时若仍按照计算的推力直接输出，则会产生错误方向的推力。例如当飞行器从悬停位置快速向前飞行时，$F_{des}$会有一个较大的水平向前的分量，总模值也较大，但是此时姿态控制器还没有来得及将飞机Z轴方向朝向前方，但是此时飞机已经认为已经朝向前方，并直接输出预计的推力，则飞机的状态表现为突然向上冲一段距离，这是不应出现的。因此，为了考虑状态跟踪的不及时，较为简单合理的方法是将$F_{des}$投影到当前机体的Z轴方向上，这样虽然可能无法完全产生目标大小的推力，但是至少不会在错误的方向上产生推力，在实际工程实践中，这种方法的效果也是较好的。

$$f=F_{\text{des}}\cdot (R{e}_3)=m\cdot (a_{\text{des}}-\mathbf{g})\cdot (R{e}_3)$$

这个投影带来了一个天然的"安全保护"：

• 当机体 Z 轴和期望推力方向一致时，投影系数 = 1，全部推力输出
• 当机体姿态还没转过来（偏离期望方向）时，投影系数 < 1，推力自动减小

推力大小随姿态对齐程度自然增长：姿态越接近目标方向，推力越接近全力输出。 这抑制了大姿态偏差下的剧烈加速，使系统的平移和旋转运动更协调。该行为直接来自投影操作的数学性质，不需要额外的限幅逻辑。

4.6 参数完全解耦

SE(3) 控制器的全部 8 个增益参数如下，不同参数控制的物理量之间高度解耦：

位置参数（控制平移响应）              姿态参数（控制旋转响应）
─────────────────────────────      ─────────────────────────────
kx_xy    XY水平位置 P 增益           kR_rp    Roll/Pitch 姿态 P 增益
kx_z     Z垂直位置 P 增益            kR_y     Yaw 姿态 P 增益
kv_xy    XY水平速度 D 增益           kW_rp    Roll/Pitch 角速度 D 增益
kv_z     Z垂直速度 D 增益            kW_y     Yaw 角速度 D 增益

两组参数之间高度解耦：

• 更改 kx_xy 主要只影响水平位置的响应快慢
• 更改 kR_rp 主要只影响姿态的回正刚度
• 位置环的参数调整通常不需要重新调姿态环，反之亦然

这源于 SE(3) 控制器的结构化设计：期望推力方向是从位置误差中计算出来的，而姿态内环只负责跟踪这个方向。两层之间只传递"方向"这个几何信息，不传递增益参数，使得两组参数在设计层面就是分离的。

---

5. 总结：PID 和 SO(3)/SE(3) 的对比

5.1 全维度对比

维度	串级 PID	SO(3)/SE(3)
姿态表示	欧拉角（3 个局部坐标）	旋转矩阵（SO(3) 流形上的一个点）
奇异性	pitch = ±90° 时死锁	无
误差定义	角度减法（可能绕远路）	旋转矩阵之间的几何距离（始终最短路径）
大角度机动	wraparound 需额外处理，大角度线性假设失效	< 180° 初始误差下可稳定恢复
控制本质	PID（在角度空间的坐标轴上）	PD（直接在旋转流形上）
运动学	基于小角度近似的线性化	精确的矩阵指数积分
参数耦合	三层串级，外环增益变化影响内环工作点	位置和姿态参数高度解耦
调参策略	通常需反复迭代、互相妥协	通常可按环分别独立调参

5.2 核心认知

SO(3)/SE(3) 几何控制 = PD 控制 + 正确的误差度量。

你不需要抛弃 PID 的直觉。P 项仍然是"你离目标有多远就使多大力把你拉回来"。D 项仍然是"你的速度有多大就产生多大的阻尼"。唯一的改变是"多远"这个量：从"角度的差"变成"旋转矩阵之间的几何距离"。

一旦换对了量误差的空间，欧拉角带来的死锁不再构成问题（因为不需要角度坐标来表示姿态）、wraparound 不再出现（因为旋转矩阵之间的几何距离始终走最短路径）、位置环和姿态环的参数得以解耦（因为位置环只输出方向而不是角度）。

5.3 调参顺序与直觉

因为参数高度解耦，调参通常可以按顺序推进，每一环调好后不必反复回头大改。每一步都有直观的物理感受可以参考：

第一步：调姿态内环。 给一个阶跃的偏航或横滚指令。

• kR 决定了飞机对指令的"敏感度"：kR 越大，飞机感觉"越硬"，回正越迅猛，过大会导致超调振荡
• kW 决定了飞机停下来时有没有"摇晃"：kW 越大，刹车感越强，停下来越干脆，过大飞机会感觉"迟钝发肉"
• 先调 kR 让响应速度达到预期，再加大 kW 消除超调。Roll/Pitch 和 Yaw 分开各调各的

第二步：调位置外环。 给一个位置阶跃或轨迹跟踪任务。

• kx 决定了飞机从 A 点飞向 B 点的"急切程度"：kx 越大，飞机对位置偏差越敏感，飞得越猛
• kv 决定了它飞向目标时是否"冲过头"：kv 越大，越能克制速度，但过大则飞行缓慢、犹豫不决
• 参照飞控手感的偏好：喜欢"跟手"但稍微有点超调的，加大 kx 减小 kv，喜欢"平稳顺滑"的，减小 kx 加大 kv

本章参考资料与引申阅读已汇总至 参考资料。