无人机建模中的线性代数：从坐标变换到姿态控制的数学基础

线性代数是无人机建模与控制的数学基石。本文从工程实际出发，系统讲解向量空间、矩阵、旋转、四元数、特征值等核心概念，每个知识点都给出数学定义、推导过程与无人机应用场景，帮你建立「定义 → 公式推导 → 物理直觉 → 代码实现」的完整认知链路。

---

一、为什么无人机工程师需要线性代数

打开任何一份无人机动力学代码，你会发现线性代数无处不在：

• 坐标变换：地面站用经纬高（NED 系），飞控内部用机体系，传感器各有安装系——在它们之间转换全靠矩阵乘法 [1]
• 姿态表示：四旋翼「朝哪飞、歪了多少」用旋转矩阵或四元数描述 [5]
• 动力学方程：$\mathbf{I}\dot{\boldsymbol{\omega}} + \boldsymbol{\omega} \times \mathbf{I}\boldsymbol{\omega} = \boldsymbol{\tau}$ 里的惯性张量 $\mathbf{I}$ 是一个 $3\times3$ 矩阵 [7]
• 控制器设计：PID 调参要看特征值判断稳定性，卡尔曼滤波核心是矩阵方程 [9]
• 传感器融合：多传感器标定要解超定方程组，用最小二乘拟合 [3]

本文不是线性代数教科书的复刻，而是挑出无人机建模中真正高频使用的知识点，用工程例子串起来。如果你已经在看 AirSim 源码或 PX4 飞控代码，这篇文章能帮你把那些矩阵运算「看懂」。

---

二、向量空间与坐标系：数学框架与物理实体

2.1 向量空间的严格定义

在深入无人机应用之前，先明确向量空间（Vector Space）的数学定义 [1][2]：

定义：一个集合 $V$ 连同标量域 $\mathbb{F}$（通常取实数域 $\mathbb{R}$），如果定义了加法和标量乘法两种运算，且满足以下八条公理，则称 $(V, \mathbb{F})$ 为向量空间：

公理	表达式	含义
加法交换律	$\mathbf{u} + \mathbf{v} = \mathbf{v} + \mathbf{u}$	顺序无关
加法结合律	$(\mathbf{u}+\mathbf{v})+\mathbf{w} = \mathbf{u}+(\mathbf{v}+\mathbf{w})$	括号无关
零向量存在	$\exists\,\mathbf{0}: \mathbf{v}+\mathbf{0}=\mathbf{v}$	存在”不动”元素
逆元素存在	$\exists\,(-\mathbf{v}): \mathbf{v}+(-\mathbf{v})=\mathbf{0}$	可以”抵消”
标量乘法结合律	$a(b\mathbf{v}) = (ab)\mathbf{v}$	缩放可组合
单位标量	$1\cdot\mathbf{v} = \mathbf{v}$	乘1不变
标量对向量的分配律	$a(\mathbf{u}+\mathbf{v}) = a\mathbf{u}+a\mathbf{v}$	缩放可分配
向量对标量的分配律	$(a+b)\mathbf{v} = a\mathbf{v}+b\mathbf{v}$	缩放可分配

为什么要关心这个定义？ 因为无人机状态量（位置、速度、角速度等）之所以能相加、能缩放、能做线性组合，正是因为它们生活在 $\mathbb{R}^3$ 这个向量空间里。而旋转不满足加法交换律（先偏航后俯仰 ≠ 先俯仰后偏航），所以旋转不是向量空间的元素——这个区分在后面讨论旋转矩阵和四元数时至关重要。

2.2 基底、维数与坐标

定义（线性无关）：向量组 $\{\mathbf{v}_1, \ldots, \mathbf{v}_n\}$ 是线性无关的，当且仅当：

$$c_1\mathbf{v}_1 + c_2\mathbf{v}_2 + \cdots + c_n\mathbf{v}_n = \mathbf{0} \quad \Longrightarrow \quad c_1 = c_2 = \cdots = c_n = 0$$

定义（基底）：向量空间 $V$ 的一组基底是一组线性无关的向量 $\{\mathbf{e}_1, \ldots, \mathbf{e}_n\}$，使得 $V$ 中任何向量都可以唯一表示为它们的线性组合：

$$\mathbf{v} = v_1\mathbf{e}_1 + v_2\mathbf{e}_2 + v_3\mathbf{e}_3$$

系数 $(v_1, v_2, v_3)$ 就是 $\mathbf{v}$ 在这组基底下的坐标。

关键认识：同一个物理向量（比如无人机的速度），在不同基底（坐标系）下有不同的坐标表示。坐标变换的本质就是基底之间的转换 [1]。

2.3 无人机常用坐标系

坐标系	缩写	基底定义	典型用途	参考
北-东-地	NED	$\mathbf{e}_x$ 指北、$\mathbf{e}_y$ 指东、$\mathbf{e}_z$ 朝地心	PX4 飞控、MAVLink	[7]
东-北-天	ENU	$\mathbf{e}_x$ 指东、$\mathbf{e}_y$ 指北、$\mathbf{e}_z$ 朝天	ROS、部分学术文献	[8]
机体系	Body	$\mathbf{e}_x$ 机头、$\mathbf{e}_y$ 右翼、$\mathbf{e}_z$ 朝下	飞控内环、传感器	[7]
惯性系	Inertial	固定在地面的参考基底	动力学推导	[6]

NED 与 ENU 之间的转换：它们之间的关系是一个固定的坐标变换矩阵：

$$\mathbf{v}_{\text{ENU}} = \begin{bmatrix} 0 & 1 & 0 \\ 1 & 0 & 0 \\ 0 & 0 & -1 \end{bmatrix} \mathbf{v}_{\text{NED}}$$

即交换前两个分量、翻转第三个分量（z 朝下变 z 朝上）。搞混这个转换是新手炸机的常见原因。

2.4 向量的内积与外积

定义（内积/点积）：$\mathbb{R}^n$ 上的标准内积定义为 [1]：

$$\langle\mathbf{a}, \mathbf{b}\rangle = \mathbf{a} \cdot \mathbf{b} = \sum_{i=1}^{n} a_i b_i = |\mathbf{a}||\mathbf{b}|\cos\theta$$

它引出了两个核心概念：

• 范数（长度）：$|\mathbf{a}| = \sqrt{\mathbf{a} \cdot \mathbf{a}}$
• 正交性：$\mathbf{a} \perp \mathbf{b} \Leftrightarrow \mathbf{a} \cdot \mathbf{b} = 0$

工程用途：

• 计算推力在竖直方向的分量：$T_z = \mathbf{T} \cdot \hat{\mathbf{z}} = |\mathbf{T}|\cos\alpha$，其中 $\alpha$ 是推力向量与竖直方向的夹角
• 判断两个向量是否垂直（正交基底的验证）

定义（外积/叉积）：仅在 $\mathbb{R}^3$ 中定义 [1]：

$$\mathbf{a} \times \mathbf{b} = \begin{bmatrix} a_y b_z - a_z b_y \\ a_z b_x - a_x b_z \\ a_x b_y - a_y b_x \end{bmatrix} = \det\begin{bmatrix} \mathbf{e}_x & \mathbf{e}_y & \mathbf{e}_z \\ a_x & a_y & a_z \\ b_x & b_y & b_z \end{bmatrix}$$

核心性质：

• 反交换律：$\mathbf{a} \times \mathbf{b} = -(\mathbf{b} \times \mathbf{a})$
• 几何意义：结果向量垂直于 $\mathbf{a}$ 和 $\mathbf{b}$ 所成平面，大小 $|\mathbf{a} \times \mathbf{b}| = |\mathbf{a}||\mathbf{b}|\sin\theta$
• 力矩公式：$\boldsymbol{\tau} = \mathbf{r} \times \mathbf{F}$（力臂叉乘力等于力矩）[6]

数值示例：假设电机 1 位于机体系 $\mathbf{r}_1 = [0.175,\, 0.175,\, 0]^T$ m，产生推力 $\mathbf{F}_1 = [0,\, 0,\, -4.0]^T$ N，则力矩为：

$$\boldsymbol{\tau}_1 = \mathbf{r}_1 \times \mathbf{F}_1 = \begin{bmatrix} 0.175 \times 0 - 0 \times (-4.0) \\ 0 \times 0 - 0.175 \times (-4.0) \\ 0.175 \times 0 - 0.175 \times 0 \end{bmatrix} = \begin{bmatrix} 0 \\ 0.7 \\ 0 \end{bmatrix} \text{ N·m}$$

这个力矩绕 $y$ 轴（俯仰轴），使机体抬头。

---

三、线性映射与矩阵：变换的语言

3.1 线性映射的严格定义

定义（线性映射）：一个函数 $T: V \to W$（从向量空间 $V$ 到向量空间 $W$）称为线性映射（或线性变换），如果对所有 $\mathbf{u}, \mathbf{v} \in V$ 和标量 $c$ 满足 [1][2]：

$$T(\mathbf{u} + \mathbf{v}) = T(\mathbf{u}) + T(\mathbf{v}) \qquad \text{（保加法）}$$ $$T(c\mathbf{v}) = cT(\mathbf{v}) \qquad \text{（保标量乘法）}$$

等价地，可以合并为一条：$T(a\mathbf{u} + b\mathbf{v}) = aT(\mathbf{u}) + bT(\mathbf{v})$。

定理：有限维向量空间之间的每一个线性映射都可以用一个矩阵表示 [1]。选定输入和输出空间的基底后，线性映射 $T$ 对应唯一的矩阵 $\mathbf{A}$，使得 $T(\mathbf{x}) = \mathbf{A}\mathbf{x}$。

这就是为什么在无人机代码中，旋转、坐标变换、混控——所有的”输入向量→输出向量”操作都写成矩阵乘法。

3.2 矩阵乘法：变换的组合

矩阵乘法定义为 $(\mathbf{AB})_{ij} = \sum_k A_{ik} B_{kj}$，其核心意义是变换的串联 [1]：

$$T_2 \circ T_1 \quad \longleftrightarrow \quad \mathbf{C} = \mathbf{B}\mathbf{A}$$

如果 $\mathbf{A}$ 把向量从坐标系 1 变到坐标系 2，$\mathbf{B}$ 从坐标系 2 变到坐标系 3，则 $\mathbf{C} = \mathbf{BA}$ 直接从坐标系 1 到坐标系 3。

关键性质：

• 不可交换：$\mathbf{AB} \neq \mathbf{BA}$（一般情况）——先偏航再俯仰 ≠ 先俯仰再偏航
• 结合律成立：$(\mathbf{AB})\mathbf{C} = \mathbf{A}(\mathbf{BC})$——多次变换可以任意分组
• 转置规则：$(\mathbf{AB})^T = \mathbf{B}^T\mathbf{A}^T$——注意顺序反转

3.3 行列式：矩阵的”体积缩放因子”

定义：$n \times n$ 方阵 $\mathbf{A}$ 的行列式 $\det(\mathbf{A})$ 是一个标量，几何上表示矩阵对应的线性变换对体积的缩放倍数（带符号）[1][2]。

$3\times3$ 矩阵的行列式公式（Sarrus 法则）： $$\det(\mathbf{A}) = a_{11}(a_{22}a_{33} - a_{23}a_{32}) - a_{12}(a_{21}a_{33} - a_{23}a_{31}) + a_{13}(a_{21}a_{32} - a_{22}a_{31})$$

核心性质与无人机应用：

性质	公式	工程含义
$\det(\mathbf{R}) = 1$	旋转矩阵	旋转不改变体积，不包含反射
$\det(\mathbf{A}) = 0$	矩阵奇异	变换”压扁”了空间，信息丢失，方程组无唯一解
$\det(\mathbf{AB}) = \det(\mathbf{A})\det(\mathbf{B})$	乘法规则	组合变换的缩放是各自缩放的乘积
$\det(\mathbf{A}^{-1}) = 1/\det(\mathbf{A})$	逆矩阵	逆变换”放大”回原来的体积

3.4 转置、逆与正交矩阵

转置 $\mathbf{A}^T$：$(A^T)_{ij} = A_{ji}$，行列互换。

逆矩阵 $\mathbf{A}^{-1}$：满足 $\mathbf{A}\mathbf{A}^{-1} = \mathbf{A}^{-1}\mathbf{A} = \mathbf{I}$。逆矩阵存在 $\Leftrightarrow$ $\det(\mathbf{A}) \neq 0$。物理含义：撤销变换。

定义（正交矩阵）：若方阵 $\mathbf{Q}$ 满足 [1]：

$$\mathbf{Q}^T\mathbf{Q} = \mathbf{Q}\mathbf{Q}^T = \mathbf{I}$$

则 $\mathbf{Q}$ 为正交矩阵，此时 $\mathbf{Q}^{-1} = \mathbf{Q}^T$。

正交矩阵的等价刻画：

• 列向量（行向量）构成标准正交基
• 保持向量长度不变：$|\mathbf{Q}\mathbf{v}| = |\mathbf{v}|$
• 保持向量夹角不变：$\langle\mathbf{Q}\mathbf{u}, \mathbf{Q}\mathbf{v}\rangle = \langle\mathbf{u}, \mathbf{v}\rangle$

工程意义：旋转矩阵是正交矩阵，所以逆旋转只需转置，计算代价极低（9 次赋值 vs 通用逆的 $O(n^3)$ 运算）：

$$\mathbf{v}_{\text{body}} = \mathbf{R}_{wb}^T \mathbf{v}_{\text{world}} \qquad \text{（世界系→机体系，转置即可）}$$

3.5 矩阵的秩

定义（秩）：矩阵 $\mathbf{A}$ 的秩 $\text{rank}(\mathbf{A})$ 是其列向量组的最大线性无关组的大小，等价于行空间的维数 [1]。

秩与方程组解的关系（Rouché-Capelli 定理）：

对于 $\mathbf{Ax} = \mathbf{b}$（$\mathbf{A}$ 为 $m \times n$ 矩阵）：

• $\text{rank}(\mathbf{A}) = \text{rank}([\mathbf{A}|\mathbf{b}])$：有解
• $\text{rank}(\mathbf{A}) = n$：解唯一
• $\text{rank}(\mathbf{A}) < n$：有无穷多解（解空间维数 $= n - \text{rank}(\mathbf{A})$）

无人机应用：四旋翼混控矩阵 $\mathbf{M}$ 是 $4 \times 4$，$\text{rank}(\mathbf{M}) = 4$，所以控制指令到电机转速的映射是唯一的。六旋翼的 $\mathbf{M}$ 是 $4 \times 6$，$\text{rank}(\mathbf{M}) = 4 < 6$，有 2 个自由度的冗余——可用于优化功耗或容错。

---

四、旋转矩阵与 SO(3) 群：姿态表示的核心

4.1 旋转矩阵的严格定义

定义：旋转矩阵 $\mathbf{R}$ 属于特殊正交群 $SO(3)$（Special Orthogonal Group），满足 [4][5]：

$$SO(3) = \{\mathbf{R} \in \mathbb{R}^{3\times3} \mid \mathbf{R}^T\mathbf{R} = \mathbf{I},\; \det(\mathbf{R}) = +1\}$$

两个条件缺一不可：

• $\mathbf{R}^T\mathbf{R} = \mathbf{I}$（正交）：保长保角
• $\det(\mathbf{R}) = +1$（特殊）：排除反射（$\det = -1$ 的是"镜像+旋转"）

列向量的几何意义：$\mathbf{R} = [\mathbf{e}_x' \mid \mathbf{e}_y' \mid \mathbf{e}_z']$，三列分别是新坐标系的三个基向量在原坐标系中的表示。

4.2 SO(3) 的群结构

$SO(3)$ 构成一个**李群**（Lie Group），具有以下性质 [4]：

群公理	在 SO(3) 中的表现	物理含义
封闭性	$\mathbf{R}_1\mathbf{R}_2 \in SO(3)$	两次旋转的结果仍是旋转
结合律	$(\mathbf{R}_1\mathbf{R}_2)\mathbf{R}_3 = \mathbf{R}_1(\mathbf{R}_2\mathbf{R}_3)$	旋转可任意分组
单位元	$\mathbf{I} \in SO(3)$	“不转”也是合法旋转
逆元	$\mathbf{R}^{-1} = \mathbf{R}^T \in SO(3)$	任何旋转都可以”转回来”
不可交换	$\mathbf{R}_1\mathbf{R}_2 \neq \mathbf{R}_2\mathbf{R}_1$（一般）	旋转顺序很重要！

不可交换性的直观演示：拿一本书，先绕竖轴转 90°再绕前后轴转 90°，与反过来的结果完全不同。这就是为什么欧拉角必须指定旋转顺序。

4.3 绕单轴旋转的推导

以**绕 z 轴旋转角度 $\psi$**为例，从几何推导旋转矩阵 [1][5]。

在 $xy$ 平面内，点 $(x, y)$ 绕原点逆时针旋转 $\psi$ 后变为 $(x', y')$：

$$x' = x\cos\psi - y\sin\psi$$ $$y' = x\sin\psi + y\cos\psi$$

推导过程：设原始点的极坐标为 $(r, \alpha)$，则 $x = r\cos\alpha$，$y = r\sin\alpha$。旋转后角度变为 $\alpha + \psi$：

$$x' = r\cos(\alpha+\psi) = r(\cos\alpha\cos\psi - \sin\alpha\sin\psi) = x\cos\psi - y\sin\psi$$ $$y' = r\sin(\alpha+\psi) = r(\sin\alpha\cos\psi + \cos\alpha\sin\psi) = y\cos\psi + x\sin\psi$$ $z$ 分量不变，写成矩阵形式： $$\mathbf{R}_z(\psi) = \begin{bmatrix} \cos\psi & -\sin\psi & 0 \\ \sin\psi & \cos\psi & 0 \\ 0 & 0 & 1 \end{bmatrix}$$

验证正交性：

$$\mathbf{R}_z^T\mathbf{R}_z = \begin{bmatrix} \cos\psi & \sin\psi & 0 \\ -\sin\psi & \cos\psi & 0 \\ 0 & 0 & 1 \end{bmatrix}\begin{bmatrix} \cos\psi & -\sin\psi & 0 \\ \sin\psi & \cos\psi & 0 \\ 0 & 0 & 1 \end{bmatrix} = \begin{bmatrix} \cos^2\psi+\sin^2\psi & 0 & 0 \\ 0 & \cos^2\psi+\sin^2\psi & 0 \\ 0 & 0 & 1 \end{bmatrix} = \mathbf{I} \quad \checkmark$$

类似推导可得另外两个基本旋转矩阵：

绕 x 轴旋转 $\phi$（滚转 Roll）：

$$\mathbf{R}_x(\phi) = \begin{bmatrix} 1 & 0 & 0 \\ 0 & \cos\phi & -\sin\phi \\ 0 & \sin\phi & \cos\phi \end{bmatrix}$$

绕 y 轴旋转 $\theta$（俯仰 Pitch）：

$$\mathbf{R}_y(\theta) = \begin{bmatrix} \cos\theta & 0 & \sin\theta \\ 0 & 1 & 0 \\ -\sin\theta & 0 & \cos\theta \end{bmatrix}$$

4.4 组合旋转：ZYX 欧拉角旋转矩阵的完整推导

PX4/AirSim 中常用 ZYX 顺序（先偏航 $\psi$、再俯仰 $\theta$、再滚转 $\phi$）[5][7]：

$$\mathbf{R}_{wb} = \mathbf{R}_z(\psi)\,\mathbf{R}_y(\theta)\,\mathbf{R}_x(\phi)$$

逐步展开：先计算 $\mathbf{R}_y\mathbf{R}_x$：

$$\mathbf{R}_y\mathbf{R}_x = \begin{bmatrix} c\theta & s\theta s\phi & s\theta c\phi \\ 0 & c\phi & -s\phi \\ -s\theta & c\theta s\phi & c\theta c\phi \end{bmatrix}$$

再左乘 $\mathbf{R}_z$：

$$\mathbf{R}_{wb} = \begin{bmatrix} c\psi c\theta & c\psi s\theta s\phi - s\psi c\phi & c\psi s\theta c\phi + s\psi s\phi \\ s\psi c\theta & s\psi s\theta s\phi + c\psi c\phi & s\psi s\theta c\phi - c\psi s\phi \\ -s\theta & c\theta s\phi & c\theta c\phi \end{bmatrix}$$

其中 $c$ 表示 $\cos$，$s$ 表示 $\sin$。这个 9 元素矩阵虽然看起来复杂，但它编码了完整的三维姿态信息。

实际应用：四旋翼平动动力学方程 $m\ddot{\mathbf{r}} = \mathbf{R}_{wb}\mathbf{F}_b + m\mathbf{g}$，其中 $\mathbf{F}_b = [0,\, 0,\, -T]^T$ 是机体系下的总推力 [6][7]。展开后可以看到：

$$m\ddot{x} = (c\psi s\theta c\phi + s\psi s\phi)(-T) + 0$$ $$m\ddot{y} = (s\psi s\theta c\phi - c\psi s\phi)(-T) + 0$$ $$m\ddot{z} = (c\theta c\phi)(-T) + mg$$

悬停时 $\phi = \theta = 0$，得 $\ddot{z} = -T/m + g = 0$，即 $T = mg$，推力等于重力。

4.5 旋转矩阵性质速查

性质	公式	证明要点
正交性	$\mathbf{R}^T = \mathbf{R}^{-1}$	定义
保范性	$\lVert\mathbf{Rv}\rVert = \lVert\mathbf{v}\rVert$	$\lVert\mathbf{Rv}\rVert^2 = \mathbf{v}^T\mathbf{R}^T\mathbf{R}\mathbf{v} = \mathbf{v}^T\mathbf{v}$
行列式	$\det(\mathbf{R}) = 1$	定义中的”特殊”条件
封闭性	$\mathbf{R}_1\mathbf{R}_2 \in SO(3)$	$(\mathbf{R}_1\mathbf{R}_2)^T(\mathbf{R}_1\mathbf{R}_2) = \mathbf{R}_2^T\mathbf{R}_1^T\mathbf{R}_1\mathbf{R}_2 = \mathbf{I}$
自由度	3（9 个元素，6 个约束）	正交条件给出 6 个方程

---

五、欧拉角与万向锁：直觉与陷阱

5.1 欧拉角的定义与直觉

欧拉角用三个标量 $(\phi, \theta, \psi)$ 参数化 $SO(3)$，与飞行员的直觉一致 [5]：

• $\phi \in (-\pi, \pi]$（Roll/滚转）：飞机左右倾斜
• $\theta \in [-\pi/2, \pi/2]$（Pitch/俯仰）：机头抬起或低下
• $\psi \in (-\pi, \pi]$（Yaw/偏航）：机头朝向

优势：直观、仅 3 个参数（最小参数化）、便于人机交互。

注意：欧拉角有 12 种旋转顺序（XYZ, XZY, YXZ, YZX, ZXY, ZYX, XYX, XZX, YXY, YZY, ZXZ, ZYZ），航空领域最常用 ZYX（Tait-Bryan 角），务必确认所用约定 [5][6]。

5.2 万向锁的数学本质

定理（毛球定理，Hairy Ball Theorem 的推论 [4]）：不存在 $SO(3)$ 到 $\mathbb{R}^3$ 的全局光滑参数化。

具体到 ZYX 欧拉角，当 $\theta = \pm\pi/2$ 时出现万向锁（Gimbal Lock）[5]。

严格推导：$\theta = \pi/2$ 时，$\cos\theta = 0$，$\sin\theta = 1$，旋转矩阵退化为：

$$\mathbf{R}\big|_{\theta=\pi/2} = \begin{bmatrix} 0 & \cos\psi\sin\phi - \sin\psi\cos\phi & \cos\psi\cos\phi + \sin\psi\sin\phi \\ 0 & \sin\psi\sin\phi + \cos\psi\cos\phi & \sin\psi\cos\phi - \cos\psi\sin\phi \\ -1 & 0 & 0 \end{bmatrix}$$

利用三角恒等式 $\cos(A-B) = \cos A\cos B + \sin A\sin B$，$\sin(A-B) = \sin A\cos B - \cos A\sin B$，简化为：

$$\mathbf{R}\big|_{\theta=\pi/2} = \begin{bmatrix} 0 & -\sin(\psi - \phi) & \cos(\psi - \phi) \\ 0 & \cos(\psi - \phi) & \sin(\psi - \phi) \\ -1 & 0 & 0 \end{bmatrix}$$

此时旋转矩阵只依赖于差值 $(\psi - \phi)$，而不是 $\psi$ 和 $\phi$ 各自的值——丢失了一个自由度。

从微分角度理解：欧拉角到角速度的映射矩阵（Jacobian）为 [5]：

$$\boldsymbol{\omega} = \begin{bmatrix} 1 & 0 & -\sin\theta \\ 0 & \cos\phi & \cos\theta\sin\phi \\ 0 & -\sin\phi & \cos\theta\cos\phi \end{bmatrix} \begin{bmatrix} \dot{\phi} \\ \dot{\theta} \\ \dot{\psi} \end{bmatrix}$$

当 $\theta = \pm\pi/2$ 时，$\cos\theta = 0$，该矩阵的行列式 $= \cos\theta = 0$，矩阵奇异——无法从角速度唯一地反解出欧拉角的导数。这意味着积分会失败。

对无人机的实际影响：

• 正常飞行中 $|\theta| < 45°$，万向锁不会触发
• 但在仿真和数值积分中，接近 $\pm 90°$ 时会导致数值爆炸
• 因此 PX4、AirSim 内部用四元数积分，仅在显示时转成欧拉角 [7]

---

六、四元数：工程中姿态表示的首选

6.1 姿态表示方式的对比

表示方式	参数个数	约束数	自由度	万向锁	组合运算	插值	归一化
欧拉角	3	0	3	有	需转为矩阵	差	不需要
旋转矩阵	9	6	3	无	矩阵乘法	差	需要（Gram-Schmidt）
四元数	4	1	3	无	四元数乘法	好（SLERP）	简单（除以模长）
轴角	4	1	3	无	需转换	中	归一化轴

四元数在无万向锁、计算效率和插值质量上完胜，这就是飞控代码的首选 [5][10]。

6.2 四元数的数学基础

定义（Hamilton，1843 [10]）：四元数是一种扩展了复数的超复数系统。四元数 $\mathbf{q}$ 定义为：

$$\mathbf{q} = q_w + q_x\mathbf{i} + q_y\mathbf{j} + q_z\mathbf{k}$$

其中虚单位满足 Hamilton 的基本方程：

$$\mathbf{i}^2 = \mathbf{j}^2 = \mathbf{k}^2 = \mathbf{ijk} = -1$$

由此推导出虚单位的乘法表：

$\times$	$\mathbf{i}$	$\mathbf{j}$	$\mathbf{k}$
$\mathbf{i}$	$-1$	$\mathbf{k}$	$-\mathbf{j}$
$\mathbf{j}$	$-\mathbf{k}$	$-1$	$\mathbf{i}$
$\mathbf{k}$	$\mathbf{j}$	$-\mathbf{i}$	$-1$

注意：$\mathbf{ij} = \mathbf{k}$ 但 $\mathbf{ji} = -\mathbf{k}$，四元数乘法不可交换。

单位四元数与旋转的对应（Euler 旋转定理 [4]）：

任何旋转都可以表示为绕某个单位轴 $\hat{\mathbf{u}} = (u_x, u_y, u_z)$ 旋转角度 $\alpha$。对应的单位四元数为：

$$\mathbf{q} = \cos\frac{\alpha}{2} + \sin\frac{\alpha}{2}(u_x\mathbf{i} + u_y\mathbf{j} + u_z\mathbf{k}) = \begin{bmatrix} \cos\frac{\alpha}{2} \\ u_x\sin\frac{\alpha}{2} \\ u_y\sin\frac{\alpha}{2} \\ u_z\sin\frac{\alpha}{2} \end{bmatrix}$$

为什么是半角 $\alpha/2$？ 因为旋转操作需要左右各乘一次四元数（$\mathbf{q}\mathbf{v}\mathbf{q}^*$），每次贡献 $\alpha/2$，合计才是完整的 $\alpha$。这也导致了 $\mathbf{q}$ 和 $-\mathbf{q}$ 表示同一个旋转（double cover 性质）[5]。

6.3 四元数的核心运算

四元数乘法（Hamilton 积）：

$$\mathbf{p} \otimes \mathbf{q} = \begin{bmatrix} p_w q_w - p_x q_x - p_y q_y - p_z q_z \\ p_w q_x + p_x q_w + p_y q_z - p_z q_y \\ p_w q_y - p_x q_z + p_y q_w + p_z q_x \\ p_w q_z + p_x q_y - p_y q_x + p_z q_w \end{bmatrix}$$

也可以写成矩阵形式 $\mathbf{p} \otimes \mathbf{q} = [\mathbf{p}]_L\,\mathbf{q} = [\mathbf{q}]_R\,\mathbf{p}$，其中 [5]：

p_w & -p_x & -p_y & -p_z \\ p_x & p_w & -p_z & p_y \\ p_y & p_z & p_w & -p_x \\ p_z & -p_y & p_x & p_w \end{bmatrix}

旋转向量：用四元数 $\mathbf{q}$ 旋转向量 $\mathbf{v}$：

$$\mathbf{v}' = \mathbf{q} \otimes \tilde{\mathbf{v}} \otimes \mathbf{q}^*$$

其中 $\tilde{\mathbf{v}} = [0, v_x, v_y, v_z]^T$ 是纯四元数，$\mathbf{q}^* = [q_w, -q_x, -q_y, -q_z]^T$ 是共轭。

组合旋转：先旋转 $\mathbf{q}_1$，再旋转 $\mathbf{q}_2$：

$$\mathbf{q}_{\text{total}} = \mathbf{q}_2 \otimes \mathbf{q}_1$$

注意顺序：后执行的旋转在左边（与旋转矩阵的组合规则一致）。

共轭 = 逆旋转：对于单位四元数，$\mathbf{q}^{-1} = \mathbf{q}^*$，类似旋转矩阵的 $\mathbf{R}^{-1} = \mathbf{R}^T$。

6.4 四元数与旋转矩阵的互转

四元数 → 旋转矩阵（推导见 [5]）：

$$\mathbf{R}(\mathbf{q}) = \begin{bmatrix} 1 - 2(q_y^2+q_z^2) & 2(q_x q_y - q_w q_z) & 2(q_x q_z + q_w q_y) \\ 2(q_x q_y + q_w q_z) & 1 - 2(q_x^2+q_z^2) & 2(q_y q_z - q_w q_x) \\ 2(q_x q_z - q_w q_y) & 2(q_y q_z + q_w q_x) & 1 - 2(q_x^2+q_y^2) \end{bmatrix}$$

旋转矩阵 → 四元数（Shepperd 方法 [10]，数值稳定）：

$$q_w = \frac{1}{2}\sqrt{1 + R_{11} + R_{22} + R_{33}}$$

当 $q_w \neq 0$ 时：

$$q_x = \frac{R_{32} - R_{23}}{4q_w}, \quad q_y = \frac{R_{13} - R_{31}}{4q_w}, \quad q_z = \frac{R_{21} - R_{12}}{4q_w}$$

当 $q_w \approx 0$（接近 $180°$ 旋转）时，需要选择其他分量作为主元避免除零，这是代码实现中的常见细节。

四元数 → 欧拉角（ZYX 顺序 [5]）：

$$\phi = \text{atan2}\big(2(q_w q_x + q_y q_z),\; 1 - 2(q_x^2 + q_y^2)\big)$$ $$\theta = \arcsin\big(2(q_w q_y - q_z q_x)\big)$$ $$\psi = \text{atan2}\big(2(q_w q_z + q_x q_y),\; 1 - 2(q_y^2 + q_z^2)\big)$$

注意使用 atan2 而非 atan 以正确处理象限。

6.5 姿态微分方程：四元数积分

推导：设机体角速度为 $\boldsymbol{\omega} = [\omega_x, \omega_y, \omega_z]^T$（由陀螺仪测得），四元数的时间导数为 [5][9]：

$$\dot{\mathbf{q}} = \frac{1}{2}\mathbf{q} \otimes \tilde{\boldsymbol{\omega}}$$

其中 $\tilde{\boldsymbol{\omega}} = [0, \omega_x, \omega_y, \omega_z]^T$。展开为矩阵形式：

$$\dot{\mathbf{q}} = \frac{1}{2}\boldsymbol{\Omega}(\boldsymbol{\omega})\,\mathbf{q}, \qquad \boldsymbol{\Omega}(\boldsymbol{\omega}) = \begin{bmatrix} 0 & -\omega_x & -\omega_y & -\omega_z \\ \omega_x & 0 & \omega_z & -\omega_y \\ \omega_y & -\omega_z & 0 & \omega_x \\ \omega_z & \omega_y & -\omega_x & 0 \end{bmatrix}$$

离散化（一阶近似，适用于小步长 $\Delta t$）：

$$\mathbf{q}_{k+1} \approx \mathbf{q}_k + \frac{\Delta t}{2}\boldsymbol{\Omega}(\boldsymbol{\omega}_k)\,\mathbf{q}_k = \left(\mathbf{I}_4 + \frac{\Delta t}{2}\boldsymbol{\Omega}\right)\mathbf{q}_k$$

积分后必须重新归一化：$\mathbf{q} \leftarrow \mathbf{q}/|\mathbf{q}|$，因为数值误差会使 $|\mathbf{q}|$ 偏离 1。

更高精度的方法（如四阶 Runge-Kutta 或矩阵指数 $e^{\frac{\Delta t}{2}\boldsymbol{\Omega}}$）见 Solà [5] 的详细讨论。

6.6 SLERP：四元数的球面插值

定义（Spherical Linear Interpolation [10]）：在两个姿态 $\mathbf{q}_0$ 和 $\mathbf{q}_1$ 之间以恒定角速度插值：

$$\text{SLERP}(\mathbf{q}_0, \mathbf{q}_1, t) = \frac{\sin((1-t)\Omega)}{\sin\Omega}\mathbf{q}_0 + \frac{\sin(t\Omega)}{\sin\Omega}\mathbf{q}_1$$

其中 $\cos\Omega = \mathbf{q}_0 \cdot \mathbf{q}_1$，$t \in [0, 1]$。

应用：无人机轨迹规划中的姿态平滑过渡——欧拉角的线性插值会导致不均匀的角速度和万向锁风险，SLERP 则保证均匀、无奇异的姿态插值。

---

七、叉积与反对称矩阵：力矩计算的数学本质

7.1 叉积的矩阵表示

定理：叉积 $\mathbf{a} \times \mathbf{b}$ 可以等价地表示为矩阵-向量乘法 [1]：

$$\mathbf{a} \times \mathbf{b} = [\mathbf{a}]_\times\, \mathbf{b}$$

其中 $[\mathbf{a}]_\times$ 是 $\mathbf{a}$ 的反对称矩阵（skew-symmetric matrix）：

$$$$

证明：直接展开 $[\mathbf{a}]_\times\,\mathbf{b}$ 并与叉积定义对比即可验证。

为什么要矩阵化？ 把叉积写成矩阵乘法后，可以利用线性代数的全套工具：求导（$\frac{d}{dt}[\mathbf{a}\times\mathbf{b}]$ 变成矩阵乘法的 Leibniz 法则）、与其他矩阵运算组合、以及在代码中用高效的 BLAS 库计算。

7.2 反对称矩阵的性质

定义：矩阵 $\mathbf{S}$ 是反对称的，当且仅当 $\mathbf{S}^T = -\mathbf{S}$。

性质	公式	证明思路
主对角线全零	$S_{ii} = 0$	由 $S_{ii} = -S_{ii}$
特征值为纯虚数	$\lambda = 0$ 或 $\pm i\lVert\mathbf{a}\rVert$	反对称矩阵的特征值满足 $\bar{\lambda} = -\lambda$ [1]
迹为零	$\text{tr}(\mathbf{S}) = 0$	主对角线全零之和
向量空间	$\dim = 3$（对 $3\times3$）	3 个独立参数 $(a_x, a_y, a_z)$

关键定理：$3\times3$ 反对称矩阵与 $\mathbb{R}^3$ 向量一一对应，这个对应关系建立了 $SO(3)$ 的李代数 $\mathfrak{so}(3)$，是理解旋转微分和小角度近似的基础 [4]。

7.3 在无人机动力学中的应用

力矩计算：电机 $i$ 的推力 $\mathbf{F}_i$ 在力臂 $\mathbf{r}_i$ 处产生的力矩 [6]：

$$\boldsymbol{\tau}_i = \mathbf{r}_i \times \mathbf{F}_i = [\mathbf{r}_i]_\times\, \mathbf{F}_i$$

欧拉刚体方程中的陀螺效应项 [6][7]：

$$\mathbf{I}\dot{\boldsymbol{\omega}} = \boldsymbol{\tau} - \boldsymbol{\omega} \times (\mathbf{I}\boldsymbol{\omega})$$

其中 $\boldsymbol{\omega} \times (\mathbf{I}\boldsymbol{\omega}) = [\boldsymbol{\omega}]_\times \mathbf{I}\boldsymbol{\omega}$ 是陀螺力矩——旋转刚体因角动量方向改变而产生的额外力矩。

数值示例：假设四旋翼以角速度 $\boldsymbol{\omega} = [0, 0, 2]^T$ rad/s（绕 z 轴偏航），惯性张量 $\mathbf{I} = \text{diag}(0.0125, 0.0125, 0.025)$：

$$\mathbf{I}\boldsymbol{\omega} = \begin{bmatrix} 0 \\ 0 \\ 0.05 \end{bmatrix}, \quad$$ $$\boldsymbol{\omega} \times (\mathbf{I}\boldsymbol{\omega}) = \begin{bmatrix} 0 & -2 & 0 \\ 2 & 0 & 0 \\ 0 & 0 & 0 \end{bmatrix}\begin{bmatrix} 0 \\ 0 \\ 0.05 \end{bmatrix} = \begin{bmatrix} -0.1 \\ 0 \\ 0 \end{bmatrix} \text{ N·m}$$

因为 $I_{xx} = I_{yy}$（轴对称），陀螺力矩仅当 $I_{xx} \neq I_{yy}$ 时才产生耦合效果。当四旋翼挂载不对称负载时，这一项变得不可忽略。

7.4 Rodrigues 旋转公式与矩阵指数

定理（Rodrigues，1840 [4]）：绕单位轴 $\hat{\mathbf{u}}$ 旋转角度 $\alpha$ 的旋转矩阵可以表示为：

$$\mathbf{R}(\hat{\mathbf{u}}, \alpha) = \mathbf{I} + \sin\alpha\,[\hat{\mathbf{u}}]_\times + (1 - \cos\alpha)\,[\hat{\mathbf{u}}]_\times^2$$

推导：利用矩阵指数 $\mathbf{R} = e^{\alpha[\hat{\mathbf{u}}]_\times}$，并将指数展开为 Taylor 级数 [4]：

$$e^{\alpha[\hat{\mathbf{u}}]_\times} = \sum_{k=0}^{\infty} \frac{(\alpha[\hat{\mathbf{u}}]_\times)^k}{k!}$$

利用 $[\hat{\mathbf{u}}]_\times^3 = -[\hat{\mathbf{u}}]_\times$（可验证），奇数幂和偶数幂分别收敛到 $\sin$ 和 $\cos$ 级数，得到上述封闭形式。

应用场景：

• 从陀螺仪的角速度向量 $\boldsymbol{\omega}\Delta t$ 直接构造旋转增量矩阵
• 小角度近似：当 $\alpha \ll 1$ 时，$\sin\alpha \approx \alpha$，$(1-\cos\alpha) \approx 0$，得到 $\mathbf{R} \approx \mathbf{I} + \alpha[\hat{\mathbf{u}}]_\times$

---

八、特征值与特征向量：惯性张量与稳定性分析

8.1 基本定义与求解

定义：方阵 $\mathbf{A} \in \mathbb{R}^{n \times n}$ 的特征值 $\lambda$ 和特征向量 $\mathbf{v} \neq \mathbf{0}$ 满足 [1][2]：

$$\mathbf{A}\mathbf{v} = \lambda\mathbf{v}$$

求解方法：上式等价于 $(\mathbf{A} - \lambda\mathbf{I})\mathbf{v} = \mathbf{0}$。要有非零解，需要：

$$\det(\mathbf{A} - \lambda\mathbf{I}) = 0 \qquad \text{（特征方程/特征多项式）}$$

对于 $n \times n$ 矩阵，这是一个 $n$ 次多项式，有 $n$ 个根（含重根，可能为复数）。

$2\times2$ 示例：设 $\mathbf{A} = \begin{bmatrix} 4 & 2 \\ 1 & 3 \end{bmatrix}$

$$\det\begin{bmatrix} 4-\lambda & 2 \\ 1 & 3-\lambda \end{bmatrix} = (4-\lambda)(3-\lambda) - 2 = \lambda^2 - 7\lambda + 10 = (\lambda-5)(\lambda-2) = 0$$

特征值 $\lambda_1 = 5$，$\lambda_2 = 2$。

对 $\lambda_1 = 5$：$(\mathbf{A} - 5\mathbf{I})\mathbf{v} = \begin{bmatrix} -1 & 2 \\ 1 & -2 \end{bmatrix}\mathbf{v} = \mathbf{0}$，解得 $\mathbf{v}_1 = \begin{bmatrix} 2 \\ 1 \end{bmatrix}$。

8.2 谱定理：实对称矩阵的对角化

定理（谱定理 [1][2]）：实对称矩阵 $\mathbf{A} = \mathbf{A}^T$ 具有以下性质：

1. 所有特征值都是实数
2. 不同特征值对应的特征向量相互正交
3. 存在正交矩阵 $\mathbf{P}$（列为特征向量），使得 $\mathbf{A} = \mathbf{P}\boldsymbol{\Lambda}\mathbf{P}^T$

其中 $\boldsymbol{\Lambda} = \text{diag}(\lambda_1, \lambda_2, \ldots, \lambda_n)$ 是特征值对角矩阵。

证明核心思路：

1. 实对称矩阵的特征值为实数：设 $\mathbf{Av} = \lambda\mathbf{v}$，取共轭转置 $\mathbf{v}^*\mathbf{A} = \bar{\lambda}\mathbf{v}^*$，由 $\mathbf{A} = \mathbf{A}^T$ 得 $\lambda\mathbf{v}^*\mathbf{v} = \bar{\lambda}\mathbf{v}^*\mathbf{v}$，因 $\mathbf{v}^*\mathbf{v} > 0$，故 $\lambda = \bar{\lambda}$，即 $\lambda \in \mathbb{R}$。
2. 不同特征值的特征向量正交：设 $\mathbf{Av}_1 = \lambda_1\mathbf{v}_1$，$\mathbf{Av}_2 = \lambda_2\mathbf{v}_2$，则 $\lambda_1\mathbf{v}_2^T\mathbf{v}_1 = \mathbf{v}_2^T\mathbf{Av}_1 = (\mathbf{Av}_2)^T\mathbf{v}_1 = \lambda_2\mathbf{v}_2^T\mathbf{v}_1$。若 $\lambda_1 \neq \lambda_2$，必须 $\mathbf{v}_2^T\mathbf{v}_1 = 0$。

8.3 惯性张量的特征值分解

四旋翼的惯性张量 $\mathbf{I}$ 是实对称矩阵 [6]：

$$\mathbf{I} = \begin{bmatrix} I_{xx} & -I_{xy} & -I_{xz} \\ -I_{xy} & I_{yy} & -I_{yz} \\ -I_{xz} & -I_{yz} & I_{zz} \end{bmatrix}$$

其中对角元素 $I_{xx} = \int(y^2+z^2)\,dm$ 等是惯性矩（总为正），非对角元素 $I_{xy} = \int xy\,dm$ 等是惯性积（可正可负可零）。

由谱定理，$\mathbf{I}$ 可对角化：

$$\mathbf{I} = \mathbf{P}\,\text{diag}(I_1, I_2, I_3)\,\mathbf{P}^T$$

• 特征值 $I_1, I_2, I_3$ 是主惯性矩——绕三个主轴的转动惯量
• $\mathbf{P}$ 的列向量定义了**主轴方向**

工程意义：四旋翼通常几何对称，主轴与机体轴对齐，$I_{xy} \approx I_{xz} \approx I_{yz} \approx 0$，惯性张量近似对角。此时欧拉方程大幅简化为三个解耦方程 [6]：

$$I_{xx}\dot{\omega}_x = \tau_x - (I_{zz} - I_{yy})\omega_y\omega_z$$ $$I_{yy}\dot{\omega}_y = \tau_y - (I_{xx} - I_{zz})\omega_x\omega_z$$ $$I_{zz}\dot{\omega}_z = \tau_z - (I_{yy} - I_{xx})\omega_x\omega_y$$

AirSim 中的典型数值设定：

Eigen::Matrix3f inertia;
inertia << 0.0125,  0,       0,       // Ixx：绕机头轴
           0,       0.0125,  0,       // Iyy：绕右翼轴
           0,       0,       0.0250;  // Izz：绕竖轴（更大，桨盘平面质量分布）

8.4 特征值与系统稳定性

线性系统稳定性理论 [3][9]：

对于线性化后的系统 $\dot{\mathbf{x}} = \mathbf{A}\mathbf{x}$，其解为 $\mathbf{x}(t) = e^{\mathbf{A}t}\mathbf{x}_0$。系统的行为完全由 $\mathbf{A}$ 的特征值决定：

设特征值 $\lambda_i = \sigma_i + j\omega_i$（$\sigma_i$ 为实部，$\omega_i$ 为虚部），则对应的分量解为：

$$x_i(t) \propto e^{\sigma_i t}\cos(\omega_i t + \varphi_i)$$

特征值性质	条件	时域行为	无人机含义
实部 $\sigma < 0$	稳定	指数衰减	扰动后自动恢复
实部 $\sigma = 0$	临界	等幅振荡或匀速	不收敛也不发散
实部 $\sigma > 0$	不稳定	指数增长	发散，必须靠控制器纠正
虚部 $\omega \neq 0$	振荡	正弦振荡	伴随振荡的收敛或发散
纯实数 $\omega = 0$	非振荡	纯指数	无振荡的收敛或发散

PID 调参的本质：闭环系统 $\dot{\mathbf{x}} = (\mathbf{A} - \mathbf{BK})\mathbf{x}$ 中，调增益矩阵 $\mathbf{K}$ 就是在调 $(\mathbf{A} - \mathbf{BK})$ 的特征值位置。增益太小 $\to$ 特征值实部不够负（响应慢），增益太大 $\to$ 虚部增大（振荡）或实部变正（不稳定）[3][9]。

数值示例：一个简化的俯仰角控制系统，状态 $\mathbf{x} = [\theta, \dot{\theta}]^T$：

$$\mathbf{A} = \begin{bmatrix} 0 & 1 \\ 0 & 0 \end{bmatrix}, \quad \mathbf{B} = \begin{bmatrix} 0 \\ 1/I_{yy} \end{bmatrix}$$

开环特征值 $\lambda_{1,2} = 0$（临界稳定，无控制时俯仰角不回零）。加入 PD 控制 $u = -K_p\theta - K_d\dot{\theta}$：

$$\mathbf{A}_{\text{cl}} = \begin{bmatrix} 0 & 1 \\ -K_p/I_{yy} & -K_d/I_{yy} \end{bmatrix}$$

特征方程 $\lambda^2 + (K_d/I_{yy})\lambda + K_p/I_{yy} = 0$，由韦达定理，所有特征值实部为负需要 $K_p > 0$ 且 $K_d > 0$。

---

九、线性方程组与最小二乘：传感器融合基础

9.1 线性方程组 $\mathbf{Ax} = \mathbf{b}$

Fredholm 替代定理（简化版）[1]：对 $m \times n$ 矩阵 $\mathbf{A}$：

情况	条件	解的情况	无人机例子
恰定	$m = n$，$\det(\mathbf{A}) \neq 0$	唯一解 $\mathbf{x} = \mathbf{A}^{-1}\mathbf{b}$	四旋翼混控
欠定	$m < n$	无穷多解	六旋翼混控（冗余）
超定	$m > n$	一般无精确解	传感器标定
奇异	$\det(\mathbf{A}) = 0$	无解或无穷多解	传感器故障退化

9.2 最小二乘法的严格推导

当超定方程组 $\mathbf{Ax} = \mathbf{b}$ 无精确解时，最小二乘目标为 [1][3]：

$$\mathbf{x}^* = \arg\min_{\mathbf{x}} \lVert\mathbf{Ax} - \mathbf{b}\rVert^2$$

推导：定义残差 $\mathbf{r} = \mathbf{Ax} - \mathbf{b}$，目标函数：

$$J(\mathbf{x}) = \mathbf{r}^T\mathbf{r} = (\mathbf{Ax}-\mathbf{b})^T(\mathbf{Ax}-\mathbf{b}) = \mathbf{x}^T\mathbf{A}^T\mathbf{A}\mathbf{x} - 2\mathbf{b}^T\mathbf{A}\mathbf{x} + \mathbf{b}^T\mathbf{b}$$

对 $\mathbf{x}$ 求梯度并令其为零：

$$\nabla_{\mathbf{x}} J = 2\mathbf{A}^T\mathbf{A}\mathbf{x} - 2\mathbf{A}^T\mathbf{b} = \mathbf{0}$$

得到正规方程（Normal Equation）：

$$\mathbf{A}^T\mathbf{A}\,\mathbf{x}^* = \mathbf{A}^T\mathbf{b}$$

当 $\mathbf{A}$ 列满秩时，$\mathbf{A}^T\mathbf{A}$ 可逆，唯一解为：

$$\mathbf{x}^* = (\mathbf{A}^T\mathbf{A})^{-1}\mathbf{A}^T\mathbf{b}$$

矩阵 $\mathbf{A}^+ = (\mathbf{A}^T\mathbf{A})^{-1}\mathbf{A}^T$ 称为Moore-Penrose 伪逆。

几何解释：$\mathbf{Ax}^*$ 是 $\mathbf{b}$ 在 $\mathbf{A}$ 列空间上的正交投影——最小二乘解就是使残差与列空间正交的那个 $\mathbf{x}$ [1]。

9.3 奇异值分解（SVD）

定理 [1][2]：任何 $m \times n$ 矩阵 $\mathbf{A}$ 都可以分解为：

$$\mathbf{A} = \mathbf{U}\boldsymbol{\Sigma}\mathbf{V}^T$$

其中：

• $\mathbf{U} \in \mathbb{R}^{m \times m}$：正交矩阵（左奇异向量）
• $\boldsymbol{\Sigma} \in \mathbb{R}^{m \times n}$：对角矩阵，$\sigma_1 \geq \sigma_2 \geq \cdots \geq 0$（奇异值）
• $\mathbf{V} \in \mathbb{R}^{n \times n}$：正交矩阵（右奇异向量）

SVD 与伪逆：

$$\mathbf{A}^+ = \mathbf{V}\boldsymbol{\Sigma}^+\mathbf{U}^T$$

其中 $\boldsymbol{\Sigma}^+$ 是将 $\boldsymbol{\Sigma}$ 中非零对角元素取倒数、然后转置。SVD 方法比正规方程数值更稳定，是实际代码中解最小二乘的首选。

条件数：$\kappa(\mathbf{A}) = \sigma_{\max}/\sigma_{\min}$。条件数越大，方程组对噪声越敏感。无人机中，若传感器配置导致 $\kappa$ 很大，标定结果会不可靠。

9.4 实际应用：磁力计硬铁标定

磁力计原始读数受硬铁偏置 $\mathbf{b} = [b_x, b_y, b_z]^T$ 影响 [7]。理想情况下，标定后的读数应落在球面上：

$$(m_x - b_x)^2 + (m_y - b_y)^2 + (m_z - b_z)^2 = r^2$$

展开：$m_x^2 + m_y^2 + m_z^2 - 2b_x m_x - 2b_y m_y - 2b_z m_z + (b_x^2+b_y^2+b_z^2-r^2) = 0$

令 $\mathbf{x} = [b_x, b_y, b_z, c]^T$（$c = b_x^2+b_y^2+b_z^2-r^2$），对每组测量写成：

$$2m_x^{(i)} b_x + 2m_y^{(i)} b_y + 2m_z^{(i)} b_z + c = (m_x^{(i)})^2 + (m_y^{(i)})^2 + (m_z^{(i)})^2$$ $N$ 组测量构成 $\mathbf{A}\mathbf{x} = \mathbf{b}$（$N \times 4$，超定），用最小二乘求解。

9.5 卡尔曼滤波中的线性代数

卡尔曼滤波（KF）是无人机状态估计的基石 [3][9]，其每一步都是矩阵运算：

预测步（Time Update）：

$$\hat{\mathbf{x}}_{k|k-1} = \mathbf{F}_k\hat{\mathbf{x}}_{k-1|k-1} + \mathbf{B}_k\mathbf{u}_k$$ $$\mathbf{P}_{k|k-1} = \mathbf{F}_k\mathbf{P}_{k-1|k-1}\mathbf{F}_k^T + \mathbf{Q}_k$$

更新步（Measurement Update）：

$$\mathbf{K}_k = \mathbf{P}_{k|k-1}\mathbf{H}_k^T\big(\mathbf{H}_k\mathbf{P}_{k|k-1}\mathbf{H}_k^T + \mathbf{R}_k\big)^{-1}$$ $$\hat{\mathbf{x}}_{k|k} = \hat{\mathbf{x}}_{k|k-1} + \mathbf{K}_k\big(\mathbf{z}_k - \mathbf{H}_k\hat{\mathbf{x}}_{k|k-1}\big)$$ $$\mathbf{P}_{k|k} = (\mathbf{I} - \mathbf{K}_k\mathbf{H}_k)\mathbf{P}_{k|k-1}$$

符号	含义	维度	线性代数角色
$\mathbf{F}$	状态转移矩阵	$n \times n$	线性映射（动力学模型）
$\mathbf{H}$	观测矩阵	$m \times n$	线性映射（传感器模型）
$\mathbf{P}$	状态协方差	$n \times n$	对称半正定矩阵（不确定性）
$\mathbf{K}$	卡尔曼增益	$n \times m$	加权伪逆
$\mathbf{Q}$	过程噪声协方差	$n \times n$	对称半正定矩阵
$\mathbf{R}$	观测噪声协方差	$m \times m$	对称正定矩阵

线性代数视角的深层理解：

1. 卡尔曼增益是加权最小二乘的解：$\mathbf{K}$ 本质上在”相信预测”（协方差 $\mathbf{P}$ 小 → $\mathbf{K}$ 小）和”相信测量”（噪声 $\mathbf{R}$ 小 → $\mathbf{K}$ 大）之间找最优平衡 [3]
2. $\mathbf{P}$ 的特征值：代表各状态分量的估计不确定性大小；特征向量代表不确定性的主方向
3. 可观测性：$[\mathbf{H}^T, (\mathbf{FH})^T, \ldots, (\mathbf{F}^{n-1}\mathbf{H})^T]^T$ 的秩等于 $n$ 时，所有状态可被估计 [3]

---

十、实战：AirSim 与 PX4 源码中的线性代数

10.1 AirSim 中的旋转矩阵应用

AirSim 使用 Eigen 库进行所有线性代数运算 [11]。动力学更新的核心代码：

// 推力从机体系变换到世界系（旋转矩阵应用）
Vector3r thrust_body(0, 0, -total_thrust);
Vector3r thrust_world = pose.orientation.toRotationMatrix() * thrust_body;

// 加上重力（世界系下，NED 约定 z 朝下）
Vector3r gravity(0, 0, 9.81);
Vector3r total_force = thrust_world + mass * gravity;

// 牛顿第二定律
Vector3r acceleration = total_force / mass;

10.2 PX4 中的四元数姿态更新

PX4 的 EKF2 姿态估计器核心循环 [7]：

// 从陀螺仪获取角速度（减去估计的零偏）
Vector3f gyro_corrected = gyro_raw - gyro_bias;

// 四元数微分方程的一阶离散化
float dt = 0.001f;  // 1kHz 更新频率
Quatf delta_q;
delta_q.w() = 1.0f;
delta_q.x() = 0.5f * gyro_corrected.x() * dt;
delta_q.y() = 0.5f * gyro_corrected.y() * dt;
delta_q.z() = 0.5f * gyro_corrected.z() * dt;

// 姿态更新：四元数乘法（组合旋转）
attitude_q = attitude_q * delta_q;

// 重新归一化（修正数值漂移）
attitude_q.normalize();

10.3 混控矩阵的线性代数本质

四旋翼混控的矩阵形式 [7]：

$$\underbrace{\begin{bmatrix} T \\ \tau_\phi \\ \tau_\theta \\ \tau_\psi \end{bmatrix}}_{\text{期望力/力矩}} = \underbrace{\begin{bmatrix} k_T & k_T & k_T & k_T \\ -dk_T & dk_T & dk_T & -dk_T \\ dk_T & dk_T & -dk_T & -dk_T \\ -k_Q & k_Q & -k_Q & k_Q \end{bmatrix}}_{\mathbf{M}\;(4\times4,\;\text{满秩})} \underbrace{\begin{bmatrix} \omega_1^2 \\ \omega_2^2 \\ \omega_3^2 \\ \omega_4^2 \end{bmatrix}}_{\text{电机转速平方}}$$

• 四旋翼：$\mathbf{M}$ 是 $4\times4$ 满秩方阵，$\boldsymbol{\omega}^2 = \mathbf{M}^{-1}\mathbf{f}$，唯一解
• 六旋翼：$\mathbf{M}$ 是 $4\times6$，$\text{rank} = 4 < 6$，欠定方程组，用伪逆 $\mathbf{M}^+$ 求最小范数解（最小功耗），或加约束做优化
• 八旋翼：类似六旋翼但冗余度更高，可容忍 1-2 个电机失效

10.4 Eigen 库常用操作速查

#include <Eigen/Dense>
#include <Eigen/Geometry>

// === 向量运算 ===
Eigen::Vector3f v(1, 2, 3);
float len = v.norm();              // 长度（L2范数）
Eigen::Vector3f v_hat = v.normalized();  // 单位化（不修改原向量）
float dot = a.dot(b);             // 点积
Eigen::Vector3f cross = a.cross(b);  // 叉积

// === 旋转矩阵构造 ===
Eigen::Matrix3f R;
R = Eigen::AngleAxisf(yaw,   Eigen::Vector3f::UnitZ())
  * Eigen::AngleAxisf(pitch, Eigen::Vector3f::UnitY())
  * Eigen::AngleAxisf(roll,  Eigen::Vector3f::UnitX());

// === 四元数 ===
Eigen::Quaternionf q(R);              // 旋转矩阵 → 四元数
Eigen::Matrix3f R2 = q.toRotationMatrix();  // 四元数 → 旋转矩阵
Eigen::Quaternionf q_combined = q2 * q1;    // 组合旋转
Eigen::Quaternionf q_inv = q.conjugate();   // 逆旋转

// === 解线性方程组 ===
// 方阵直接求解
Eigen::Vector4f x = A.colPivHouseholderQr().solve(b);
// 最小二乘（超定系统）
Eigen::VectorXf x_ls = A.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b);

// === 特征值分解（对称矩阵） ===
Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> solver(I);
Eigen::Vector3f eigenvalues = solver.eigenvalues();
Eigen::Matrix3f eigenvectors = solver.eigenvectors();

// === SVD 分解 ===
Eigen::JacobiSVD<Eigen::MatrixXf> svd(A, Eigen::ComputeThinU | Eigen::ComputeThinV);
Eigen::VectorXf singular_values = svd.singularValues();
float condition_number = singular_values(0) / singular_values(singular_values.size()-1);

---

十一、总结与学习路线

11.1 知识点回顾

章节	核心概念	数学要点	无人机应用
二	向量空间、基底	八公理、线性无关、内积外积	坐标系定义、力矩计算
三	线性映射、矩阵	行列式、秩、正交矩阵	坐标变换、可逆性判断
四	旋转矩阵 $SO(3)$	正交群、ZYX 展开推导	机体系↔世界系变换
五	欧拉角、万向锁	Jacobian 奇异性	旋转顺序选择、数值稳定性
六	四元数	Hamilton 积、SLERP、微分方程	飞控姿态积分
七	反对称矩阵	李代数、Rodrigues 公式	陀螺效应、旋转增量
八	特征值分解	谱定理、特征多项式	惯性主轴、系统稳定性
九	最小二乘、SVD	正规方程推导、条件数	传感器标定、卡尔曼滤波
十	Eigen 库实战	—	AirSim/PX4 源码理解

11.2 推荐学习路线

第一阶段：建立直觉（1-2 周）

• 观看 3Blue1Brown《线性代数的本质》系列视频 [12]，建立几何直觉
• 手算 $2\times2$ 和 $3\times3$ 旋转矩阵，画出旋转前后的向量
• 用 Python（NumPy）验证矩阵运算结果

第二阶段：掌握核心工具（2-4 周）

• 系统学习一本线性代数教材 [1] 或 [2]
• 推导 ZYX 旋转矩阵的完整表达式
• 手写四元数乘法和旋转，与旋转矩阵结果对比
• 实现简单的欧拉角↔四元数↔旋转矩阵转换代码
• 阅读 Solà 的四元数教程 [5]

第三阶段：对接工程实践（持续）

• 阅读 AirSim/PX4 源码中的 Eigen 调用 [7][11]，逐行理解
• 自己实现一个简化版 EKF 姿态估计器
• 修改 AirSim 的惯性参数，观察仿真行为变化
• 阅读 Beard & McLain 的无人机教材 [6] 中的完整动力学推导

---

参考文献

教材

• [1] Gilbert Strang. Introduction to Linear Algebra, 6th Edition. Wellesley-Cambridge Press, 2023.
  经典线性代数教材，侧重直觉与应用。配套 MIT OCW 课程：https://ocw.mit.edu/courses/18-06-linear-algebra-spring-2010/

• [2] Sheldon Axler. Linear Algebra Done Right, 4th Edition. Springer, 2024. 开放获取：https://linear.axler.net/
  从线性映射出发的现代教材，数学推导更严谨，适合深入理解理论基础。

• [3] Dan Simon. Optimal State Estimation: Kalman, H∞, and Nonlinear Approaches. Wiley, 2006. ISBN: 978-0471708582.
  状态估计的权威教材，涵盖卡尔曼滤波的完整线性代数推导与无人机应用。

论文与技术报告

• [4] Richard M. Murray, Zexiang Li, S. Shankar Sastry. A Mathematical Introduction to Robotic Manipulation. CRC Press, 1994. 全文开放获取：http://www.cds.caltech.edu/~murray/mlswiki/
  机器人学中的李群/李代数、旋转矩阵 $SO(3)$ 理论的经典参考。

• [5] Joan Solà. “Quaternion kinematics for the error-state Kalman filter.” arXiv preprint, 2017. https://arxiv.org/abs/1711.02508
  四元数运动学的最佳工程教程，涵盖四元数定义、微分方程、与旋转矩阵/欧拉角的互转，以及在 EKF 中的应用。强烈推荐。

• [6] Randal W. Beard, Timothy W. McLain. Small Unmanned Aircraft: Theory and Practice. Princeton University Press, 2012. 配套资源：https://github.com/randybeard/uavbook
  小型无人机建模与控制的标准教材，完整推导了坐标系、旋转、动力学方程和控制器设计。

• [7] PX4 开发团队. PX4 Autopilot User Guide. https://docs.px4.io/main/en/
  PX4 飞控的官方文档，包含坐标系约定、EKF2 实现细节和参数说明。

• [8] Open Robotics. REP 103 - Standard Units of Measure and Coordinate Conventions. https://www.ros.org/reps/rep-0103.html
  ROS 的坐标系与单位约定标准，定义了 ENU 坐标系的使用规范。

进阶参考

• [9] Peter Corke. Robotics, Vision and Control, 3rd Edition. Springer, 2023. 配套 MATLAB/Python 工具箱：https://github.com/petercorke/robotics-toolbox-python
  涵盖旋转表示、齐次变换、视觉伺服等，有大量可运行的代码示例。

• [10] Jack B. Kuipers. Quaternions and Rotation Sequences: A Primer with Applications to Orbits, Aerospace and Virtual Reality. Princeton University Press, 2002. ISBN: 978-0691102986.
  四元数专著，从 Hamilton 的历史发现讲到航天应用，数学推导详尽。

• [11] Gaël Guennebaud, Benoît Jacob, et al. Eigen: a C++ template library for linear algebra. https://eigen.tuxfamily.org/
  AirSim、PX4 等项目使用的 C++ 线性代数库，官方文档包含详细的 API 参考和教程。

视频课程

• [12] 3Blue1Brown (Grant Sanderson). Essence of Linear Algebra 系列视频, 2016. https://www.3blue1brown.com/topics/linear-algebra
  用精美动画讲解线性代数的几何直觉，是建立矩阵变换、特征值等概念的最佳入门资源。B 站有官方中文字幕版。

• [13] MIT OpenCourseWare. Gilbert Strang 教授 Linear Algebra (18.06) 课程录像. https://ocw.mit.edu/courses/18-06sc-linear-algebra-fall-2011/
  MIT 的经典线性代数课程，与教材 [1] 配套，适合系统学习。

---

核心思想：线性代数在无人机建模中不是抽象数学，而是描述旋转、变换、融合的工程语言。每个矩阵运算背后都有清晰的物理图像——旋转矩阵是”坐标系之间的桥梁”，特征值是”系统稳不稳”，最小二乘是”怎么从噪声里提取信号”。建立了这种对应关系，再看飞控源码就不再是一堆符号，而是一幅完整的物理画面。