无人机飞行物理学：从牛顿力学到六自由度运动方程的完整推导（6DOF 通用框架）

四旋翼的仿真与控制，本质上是在求解一组微分方程。本文从牛顿三定律出发，逐步推导刚体平动与转动方程，引入螺旋桨空气动力学模型，最终得到完整的六自由度（6-DOF）运动方程组。每一步都给出物理图像、数学推导和数值示例，并以拉格朗日力学提供第二视角。

---

一、引言：一组微分方程如何让无人机飞起来

无人机仿真的核心工作，就是在每个时间步里回答一个问题：已知当前状态和控制输入，下一个时刻无人机在哪、朝哪、转多快？

这个问题的答案隐藏在一组常微分方程（ODE）中——六自由度运动方程。它的推导需要三块物理知识：

1. 经典力学：牛顿定律 → 刚体的平动和转动方程
2. 空气动力学：螺旋桨如何产生推力与扭矩
3. 数值方法：如何在计算机上高效、稳定地积分这些方程

本文从第一性原理出发，逐步搭建完整的物理模型。如果你已经读过《四旋翼飞行力学基础》建立了直觉，这篇文章会帮你把直觉变成可以编程实现的方程 [1][6]。

---

二、经典力学回顾：从质点到刚体

2.1 牛顿三定律

所有推导的起点是牛顿三定律 [1][2]：

第一定律（惯性定律）：不受外力的物体保持匀速直线运动或静止。

第二定律（运动定律）：

$$\mathbf{F} = m\mathbf{a} = m\frac{d\mathbf{v}}{dt} = \frac{d\mathbf{p}}{dt}$$

其中 $\mathbf{p} = m\mathbf{v}$ 是动量。这是整个力学推导的核心方程。

第三定律（作用-反作用）：$\mathbf{F}_{12} = -\mathbf{F}_{21}$。螺旋桨的反扭矩正是第三定律的直接结果。

2.2 从质点到质点系

无人机不是一个质点，而是由无数质点组成的刚体。对于质点系，牛顿第二定律可以推广为两个基本定理 [1][2]：

质心运动定理：质点系质心的运动如同一个集中了全部质量的质点，受合外力作用：

$$m\ddot{\mathbf{r}}_c = \sum_{\text{ext}} \mathbf{F}_i = \mathbf{F}_{\text{ext}}$$

推导：设系统由 $N$ 个质点组成，质心定义为 $\mathbf{r}_c = \frac{1}{m}\sum_i m_i\mathbf{r}_i$（其中 $m = \sum_i m_i$），对时间求二阶导：

$$m\ddot{\mathbf{r}}_c = \sum_i m_i\ddot{\mathbf{r}}_i = \sum_i \mathbf{F}_i = \sum_i \mathbf{F}_i^{\text{ext}} + \sum_i \mathbf{F}_i^{\text{int}}$$

由牛顿第三定律，内力成对抵消 $\sum_i \mathbf{F}_i^{\text{int}} = \mathbf{0}$，得到 $m\ddot{\mathbf{r}}_c = \mathbf{F}_{\text{ext}}$。$\quad\blacksquare$

角动量定理：质点系相对某参考点的角动量变化率等于合外力矩：

$$\frac{d\mathbf{L}}{dt} = \boldsymbol{\tau}_{\text{ext}}$$

其中角动量 $\mathbf{L} = \sum_i \mathbf{r}_i \times (m_i\mathbf{v}_i)$，力矩 $\boldsymbol{\tau}_{\text{ext}} = \sum_i \mathbf{r}_i \times \mathbf{F}_i^{\text{ext}}$。

推导：

$$\frac{d\mathbf{L}}{dt} = \sum_i \frac{d}{dt}(\mathbf{r}_i \times m_i\mathbf{v}_i) = \sum_i (\dot{\mathbf{r}}_i \times m_i\mathbf{v}_i + \mathbf{r}_i \times m_i\dot{\mathbf{v}}_i)$$

第一项 $\dot{\mathbf{r}}_i \times m_i\mathbf{v}_i = m_i(\mathbf{v}_i \times \mathbf{v}_i) = \mathbf{0}$。第二项利用 $m_i\dot{\mathbf{v}}_i = \mathbf{F}_i$，同样由牛顿第三定律，内力矩成对抵消，剩余 $\sum_i \mathbf{r}_i \times \mathbf{F}_i^{\text{ext}} = \boldsymbol{\tau}_{\text{ext}}$。$\quad\blacksquare$

2.3 刚体约束

定义：刚体是一种特殊的质点系，任意两个质点之间的距离在运动过程中保持不变 [1]：

$$|\mathbf{r}_i - \mathbf{r}_j| = \text{const}, \quad \forall\, i, j$$

这个约束极大地简化了问题：刚体的运动可以完全由质心的平动和绕质心的转动来描述，总共 6 个自由度（3 个平动 + 3 个转动）。

Chasles 定理 [1]：刚体的任意运动都可以分解为质心的平动和绕质心的转动的叠加。

---

三、刚体运动学：描述”怎么动”

运动学只关心几何描述——位置、速度、角速度的关系，不涉及力。

3.1 位姿描述

刚体在空间中的状态由位姿（Pose）完全确定 [3][6]：

$$\text{位姿} = (\mathbf{r}_c,\; \mathbf{R}) \in \mathbb{R}^3 \times SO(3)$$

• $\mathbf{r}_c = [x, y, z]^T$：质心在惯性系中的位置（3 个自由度）
• $\mathbf{R}$：机体系到惯性系的旋转矩阵（3 个自由度，由欧拉角或四元数参数化）

3.2 速度与角速度

平动速度：质心速度在惯性系中的表示：

$$\mathbf{v} = \dot{\mathbf{r}}_c = \frac{d\mathbf{r}_c}{dt}$$

角速度 $\boldsymbol{\omega}$：描述刚体绕质心转动的瞬时快慢和方向 [1][3]。角速度与旋转矩阵的关系：

$$\dot{\mathbf{R}} = \mathbf{R}\,[\boldsymbol{\omega}_b]_\times$$

其中 $\boldsymbol{\omega}_b = [\omega_x, \omega_y, \omega_z]^T$ 是角速度在机体系中的分量（陀螺仪直接测量的就是这个），$[\cdot]_\times$ 是反对称矩阵。

推导：设机体系基向量为 $\mathbf{e}_i'$（是惯性系中的时变向量），则 $\mathbf{R} = [\mathbf{e}_1' \mid \mathbf{e}_2' \mid \mathbf{e}_3']$。刚体绕角速度 $\boldsymbol{\omega}$ 转动时，固连在刚体上的任意向量的变化率满足 [1]：

$$\dot{\mathbf{e}}_i' = \boldsymbol{\omega} \times \mathbf{e}_i'$$

写成矩阵形式即为 $\dot{\mathbf{R}} = [\boldsymbol{\omega}_w]_\times \mathbf{R} = \mathbf{R}[\boldsymbol{\omega}_b]_\times$。

3.3 刚体上任意一点的速度

刚体上某点 $P$ 相对质心的位矢为 $\boldsymbol{\rho}$（机体系中恒定），则 $P$ 在惯性系中的速度为 [1]：

$$\mathbf{v}_P = \mathbf{v}_c + \boldsymbol{\omega} \times (\mathbf{R}\boldsymbol{\rho})$$

应用：计算某个电机安装点的速度，用于估算桨叶来流速度和气动力。

3.4 角速度与欧拉角导数的关系

若使用 ZYX 欧拉角 $(\phi, \theta, \psi)$ 参数化姿态，则机体系角速度与欧拉角导数之间的映射为 [3][6]：

$$\boldsymbol{\omega}_b = \begin{bmatrix} \omega_x \\ \omega_y \\ \omega_z \end{bmatrix} = \underbrace{\begin{bmatrix} 1 & 0 & -\sin\theta \\ 0 & \cos\phi & \cos\theta\sin\phi \\ 0 & -\sin\phi & \cos\theta\cos\phi \end{bmatrix}}_{\mathbf{W}(\phi,\theta)} \begin{bmatrix} \dot{\phi} \\ \dot{\theta} \\ \dot{\psi} \end{bmatrix}$$

逆变换（当 $\cos\theta \neq 0$ 时）：

$$\begin{bmatrix} \dot{\phi} \\ \dot{\theta} \\ \dot{\psi} \end{bmatrix} = \begin{bmatrix} 1 & \sin\phi\tan\theta & \cos\phi\tan\theta \\ 0 & \cos\phi & -\sin\phi \\ 0 & \sin\phi\sec\theta & \cos\phi\sec\theta \end{bmatrix} \begin{bmatrix} \omega_x \\ \omega_y \\ \omega_z \end{bmatrix}$$

注意当 $\theta = \pm 90°$ 时 $\cos\theta = 0$，逆变换奇异——这就是万向锁。实际飞控中使用四元数微分方程避免此问题 [3]。

---

四、刚体动力学：牛顿-欧拉方程的完整推导

4.1 平动方程（牛顿方程）

将质心运动定理应用于无人机刚体 [1][6]：

$$m\ddot{\mathbf{r}}_c = \mathbf{F}_{\text{ext}}$$

在惯性系（NED）中展开，作用力包括重力和螺旋桨推力（从机体系变换到惯性系）：

$$m\begin{bmatrix} \ddot{x} \\ \ddot{y} \\ \ddot{z} \end{bmatrix} = \mathbf{R}_{wb}\begin{bmatrix} 0 \\ 0 \\ -T \end{bmatrix} + \begin{bmatrix} 0 \\ 0 \\ mg \end{bmatrix} + \mathbf{F}_{\text{drag}}$$

其中 $T = \sum_{i=1}^{4} T_i$ 是总推力（NED 约定下推力沿机体 $z$ 轴负方向），$\mathbf{F}_{\text{drag}}$ 是空气阻力。

也可以写在机体系中：在旋转参考系中，需要加上科里奥利力和离心力等非惯性力 [1]：

$$m(\dot{\mathbf{v}}_b + \boldsymbol{\omega}_b \times \mathbf{v}_b) = \mathbf{F}_b$$

其中 $\mathbf{v}_b$ 是质心速度在机体系中的分量，$\mathbf{F}_b$ 是所有外力在机体系中的合力。项 $\boldsymbol{\omega}_b \times \mathbf{v}_b$ 是将惯性系的加速度转换到旋转参考系时出现的附加项。

推导：惯性系中 $\mathbf{F} = m\dot{\mathbf{v}}_w$。由于 $\mathbf{v}_w = \mathbf{R}\mathbf{v}_b$，求导：

$$\dot{\mathbf{v}}_w = \dot{\mathbf{R}}\mathbf{v}_b + \mathbf{R}\dot{\mathbf{v}}_b = \mathbf{R}[\boldsymbol{\omega}_b]_\times\mathbf{v}_b + \mathbf{R}\dot{\mathbf{v}}_b = \mathbf{R}(\dot{\mathbf{v}}_b + \boldsymbol{\omega}_b \times \mathbf{v}_b)$$

两边左乘 $\mathbf{R}^T$，得到 $m(\dot{\mathbf{v}}_b + \boldsymbol{\omega}_b \times \mathbf{v}_b) = \mathbf{R}^T\mathbf{F}_w = \mathbf{F}_b$。$\quad\blacksquare$

4.2 转动方程（欧拉方程）的完整推导

这是六自由度建模中最关键的方程 [1][2][6]。

起点：角动量定理（相对质心）：

$$\frac{d\mathbf{L}}{dt}\bigg|_{\text{惯性系}} = \boldsymbol{\tau}_{\text{ext}}$$

第一步：角动量的表达式

刚体相对质心的角动量 [1]：

$$\mathbf{L} = \int_{\text{body}} \boldsymbol{\rho} \times (\boldsymbol{\omega} \times \boldsymbol{\rho})\,dm$$

其中 $\boldsymbol{\rho}$ 是质量元到质心的位矢。利用向量恒等式 $\mathbf{a}\times(\mathbf{b}\times\mathbf{c}) = \mathbf{b}(\mathbf{a}\cdot\mathbf{c}) - \mathbf{c}(\mathbf{a}\cdot\mathbf{b})$：

$$\boldsymbol{\rho} \times (\boldsymbol{\omega} \times \boldsymbol{\rho}) = \boldsymbol{\omega}(\boldsymbol{\rho}\cdot\boldsymbol{\rho}) - \boldsymbol{\rho}(\boldsymbol{\rho}\cdot\boldsymbol{\omega}) = (\rho^2\mathbf{I}_3 - \boldsymbol{\rho}\boldsymbol{\rho}^T)\boldsymbol{\omega}$$

对整个刚体积分：

$$\mathbf{L} = \underbrace{\left[\int_{\text{body}} (\rho^2\mathbf{I}_3 - \boldsymbol{\rho}\boldsymbol{\rho}^T)\,dm\right]}_{\mathbf{I}\;(\text{惯性张量})} \boldsymbol{\omega} = \mathbf{I}\boldsymbol{\omega}$$

惯性张量在机体系中展开（设 $\boldsymbol{\rho} = [\xi, \eta, \zeta]^T$）：

$$\mathbf{I} = \begin{bmatrix} \int(\eta^2+\zeta^2)\,dm & -\int\xi\eta\,dm & -\int\xi\zeta\,dm \\ -\int\xi\eta\,dm & \int(\xi^2+\zeta^2)\,dm & -\int\eta\zeta\,dm \\ -\int\xi\zeta\,dm & -\int\eta\zeta\,dm & \int(\xi^2+\eta^2)\,dm \end{bmatrix} = \begin{bmatrix} I_{xx} & -I_{xy} & -I_{xz} \\ -I_{xy} & I_{yy} & -I_{yz} \\ -I_{xz} & -I_{yz} & I_{zz} \end{bmatrix}$$

关键性质：$\mathbf{I}$ 是实对称正定矩阵，在机体系中是常数（因为质量分布不随时间变化）。

第二步：在机体系中求角动量的时间导数

角动量定理要求在惯性系中求导。但 $\mathbf{I}$ 在惯性系中是时变的（随刚体旋转而变），在机体系中才是常数。因此用旋转参考系的导数转换 [1]：

$$\frac{d\mathbf{L}}{dt}\bigg|_{\text{惯性}} = \frac{d\mathbf{L}}{dt}\bigg|_{\text{机体}} + \boldsymbol{\omega} \times \mathbf{L}$$

这是”旋转参考系中的导数公式”——任何向量 $\mathbf{A}$ 在惯性系和旋转系中的导数关系为 $\left(\frac{d\mathbf{A}}{dt}\right)_{\text{惯}} = \left(\frac{d\mathbf{A}}{dt}\right)_{\text{转}} + \boldsymbol{\omega} \times \mathbf{A}$ [1]。

在机体系中 $\mathbf{I}$ 是常数，所以：

$$\frac{d\mathbf{L}}{dt}\bigg|_{\text{机体}} = \mathbf{I}\dot{\boldsymbol{\omega}}_b$$

第三步：得到欧拉方程

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

这就是欧拉刚体方程——刚体转动动力学的基本方程 [1][2]。

4.3 欧拉方程的分量形式

设惯性张量为对角阵 $\mathbf{I} = \text{diag}(I_{xx}, I_{yy}, I_{zz})$（四旋翼近似对称），展开：

$$\boldsymbol{\omega}_b \times (\mathbf{I}\boldsymbol{\omega}_b) = \begin{bmatrix} \omega_x \\ \omega_y \\ \omega_z \end{bmatrix} \times \begin{bmatrix} I_{xx}\omega_x \\ I_{yy}\omega_y \\ I_{zz}\omega_z \end{bmatrix} = \begin{bmatrix} (I_{zz}-I_{yy})\omega_y\omega_z \\ (I_{xx}-I_{zz})\omega_x\omega_z \\ (I_{yy}-I_{xx})\omega_x\omega_y \end{bmatrix}$$

三个分量方程为：

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

物理解读：

• 左边 $I\dot{\omega}$ 是”转动的加速度”
• 右边第一项 $\tau$ 是外力矩（电机产生）
• 右边第二项是陀螺效应：当 $I_{xx} \neq I_{yy} \neq I_{zz}$ 时，一个轴上的角速度会通过交叉项影响另一个轴的角加速度

数值示例：设 $I_{xx} = I_{yy} = 0.0125$ kg·m²，$I_{zz} = 0.025$ kg·m²，无人机正在以 $\omega_z = 3$ rad/s 偏航，$\omega_y = 0.5$ rad/s 俯仰：

$$\text{陀螺耦合到 roll 轴} = (I_{zz} - I_{yy})\omega_y\omega_z = (0.025 - 0.0125) \times 0.5 \times 3 = 0.01875 \text{ N·m}$$

这个力矩会导致无意的滚转——飞控的内环必须实时补偿。

4.4 动能与转动动能

刚体的总动能由 König 定理分解为 [1][2]：

$$E_k = \underbrace{\frac{1}{2}m|\mathbf{v}_c|^2}_{\text{质心平动动能}} + \underbrace{\frac{1}{2}\boldsymbol{\omega}^T\mathbf{I}\boldsymbol{\omega}}_{\text{绕质心转动动能}}$$

对于对角惯性张量：

$$E_k^{\text{rot}} = \frac{1}{2}(I_{xx}\omega_x^2 + I_{yy}\omega_y^2 + I_{zz}\omega_z^2)$$

这个表达式在拉格朗日力学推导中至关重要（见第八章）。

---

五、螺旋桨空气动力学：推力与扭矩从何而来

5.1 动量理论（Momentum Theory）

动量理论是分析螺旋桨最简单的模型，将桨盘视为一个无限薄的”致动盘”（Actuator Disk），均匀地对通过的空气做功 [4][6]。

假设：

• 空气不可压缩，流动定常
• 桨盘均匀加速气流
• 无旋涡损失

推导：设桨盘面积 $A = \pi R^2$（$R$ 为桨半径），来流速度 $V_\infty$（悬停时为 0），诱导速度 $v_i$：

由动量定理，推力等于气流的动量变化率：

$$T = \dot{m}\Delta v = \rho A(V_\infty + v_i) \cdot 2v_i$$

其中 $\dot{m} = \rho A(V_\infty + v_i)$ 是质量流量，远处尾流速度增加 $\Delta v = 2v_i$（Froude 理论）[4]。

悬停状态（$V_\infty = 0$）：

$$T = 2\rho A v_i^2$$

解出诱导速度：

$$v_i = \sqrt{\frac{T}{2\rho A}}$$

悬停功率（理想功率下界）：

$$P_{\text{ideal}} = Tv_i = T\sqrt{\frac{T}{2\rho A}} = \sqrt{\frac{T^3}{2\rho A}}$$

数值示例：1.5 kg 四旋翼悬停，每桨承担 $T_1 = mg/4 = 3.68$ N，桨半径 $R = 0.127$ m（5 英寸桨）：

$$A = \pi \times 0.127^2 = 0.0507 \text{ m}^2$$ $$v_i = \sqrt{\frac{3.68}{2 \times 1.225 \times 0.0507}} = \sqrt{29.5} = 5.43 \text{ m/s}$$ $$P_1 = 3.68 \times 5.43 = 20.0 \text{ W}$$

四桨总功率约 80 W——这是理论最低值，实际因桨尖涡、阻力等约为此值的 1.5-2 倍。

5.2 叶素理论（Blade Element Theory）

动量理论无法预测推力随转速的关系。叶素理论（BET）将桨叶沿径向切成无穷多微元，对每个微元应用二维翼型理论 [4]。

核心思想：桨叶上距轴心 $r$ 处的微元，弦长 $c(r)$，桨距角 $\beta(r)$，在旋转角速度 $\Omega$ 下：

• 旋转引起的切向速度：$V_t = \Omega r$
• 诱导速度分量：$v_i(r)$
• 有效攻角：$\alpha(r) = \beta(r) - \arctan\frac{v_i(r)}{V_t}$

微元上的升力和阻力：

$$dL = \frac{1}{2}\rho V_{\text{eff}}^2 c\, C_L(\alpha)\, dr$$ $$dD = \frac{1}{2}\rho V_{\text{eff}}^2 c\, C_D(\alpha)\, dr$$

其中 $V_{\text{eff}} = \sqrt{V_t^2 + v_i^2}$ 是合成来流速度，$C_L$ 和 $C_D$ 是翼型的升力和阻力系数。

沿径向积分并乘以桨叶数 $B$，得到总推力和扭矩。

5.3 工程简化模型：推力和扭矩系数

在仿真和飞控中，通常不做完整的 BET 积分，而是使用经验系数模型 [4][6][8]：

$$T_i = k_T \omega_i^2$$ $$Q_i = k_Q \omega_i^2$$

其中：

• $k_T$ 是**推力系数**，由桨型、空气密度决定，单位 N/(rad/s)²
• $k_Q$ 是**扭矩系数**，单位 N·m/(rad/s)²
• $\omega_i$ 是第 $i$ 个电机的角速度（rad/s）

推力系数的物理来源：量纲分析（Buckingham π 定理 [2]）表明 $T = C_T \rho n^2 D^4$，其中 $C_T$ 是无量纲推力系数，$n$ 是转速（rev/s），$D$ 是桨直径。转换为角速度形式：

$$k_T = \frac{C_T \rho D^4}{4\pi^2}$$

典型数值（5 英寸桨，海平面）：

参数	值	来源
$C_T$	$\sim 0.1$	桨效率测试（如 UIUC 数据库 [13]）
$C_Q$	$\sim 0.005$	同上
$\rho$	1.225 kg/m³	标准大气
$D$	0.127 m（5”）	桨规格
$k_T$	$\sim 8 \times 10^{-6}$ N/(rad/s)²	计算得到

5.4 推力-扭矩比

推力与扭矩之间的比值是一个关键设计参数 [4]：

$$\frac{Q_i}{T_i} = \frac{k_Q}{k_T} = \frac{C_Q D}{C_T} \approx \text{const}$$

这个比值决定了偏航控制的灵敏度——$k_Q/k_T$ 越大，同样的推力变化产生越多的偏航扭矩。

---

六、外力与外力矩的完整模型

6.1 作用在四旋翼上的所有力

在机体系中，合外力 $\mathbf{F}_b$ 为 [6][8]：

$$\mathbf{F}_b = \underbrace{\begin{bmatrix} 0 \\ 0 \\ -\sum_{i=1}^4 T_i \end{bmatrix}}_{\text{推力}} + \underbrace{\mathbf{R}^T\begin{bmatrix} 0 \\ 0 \\ mg \end{bmatrix}}_{\text{重力（转到机体系）}} + \underbrace{\begin{bmatrix} -k_{dx}\,v_x \\ -k_{dy}\,v_y \\ -k_{dz}\,v_z \end{bmatrix}}_{\text{平动空气阻力}}$$

空气阻力模型：低速飞行中，阻力近似与速度成正比（低雷诺数区域），$k_{dx}, k_{dy}, k_{dz}$ 为阻力系数。更精确的模型使用平方关系 $F_{\text{drag}} = -\frac{1}{2}\rho C_D A_{\text{ref}} |\mathbf{v}|\mathbf{v}$，但线性模型在大部分仿真中已足够 [8]。

6.2 作用在四旋翼上的所有力矩

在机体系中，合力矩 $\boldsymbol{\tau}_b$ 为 [6][7][8]：

$$\boldsymbol{\tau}_b = \underbrace{\begin{bmatrix} l(T_2 + T_3 - T_1 - T_4) \\ l(T_1 + T_2 - T_3 - T_4) \\ Q_1 - Q_2 + Q_3 - Q_4 \end{bmatrix}}_{\text{电机推力与反扭矩}} + \underbrace{\begin{bmatrix} -k_{r}\omega_x \\ -k_{r}\omega_y \\ -k_{r}\omega_z \end{bmatrix}}_{\text{旋转空气阻力}}$$

其中（以 X 型布局为例 [7]）：

• 滚转力矩（$\tau_x$）：左右两侧推力差 × 力臂 $l$（质心到电机的距离）
• 俯仰力矩（$\tau_y$）：前后两侧推力差 × 力臂
• 偏航力矩（$\tau_z$）：反扭矩差。CW 和 CCW 桨的扭矩方向相反，不平衡时产生偏航
• 旋转阻力：$k_r$ 为旋转阻力系数，阻止机体转动

注意：具体的电机编号与混控矩阵取决于机架布局（X 型、+ 型）和飞控约定，务必对照实际配置 [7]。

6.3 电机动态特性

实际电机不是瞬时响应的。从控制指令到实际转速之间有延迟，通常建模为一阶惯性环节 [8]：

$$\dot{\omega}_i = \frac{1}{\tau_m}(\omega_i^{\text{cmd}} - \omega_i)$$

其中 $\tau_m \approx 0.02 \sim 0.05$ s 是电机时间常数。在快速机动仿真中，忽略电机动态会导致与实际飞行的显著偏差。

---

七、完整六自由度运动方程

7.1 状态向量

将四旋翼的完整状态定义为 12 维向量（使用欧拉角参数化）[6]：

$$\mathbf{x} = \begin{bmatrix} x & y & z & \phi & \theta & \psi & u & v & w & \omega_x & \omega_y & \omega_z \end{bmatrix}^T$$

变量	含义	参考系
$x, y, z$	质心位置	惯性系
$\phi, \theta, \psi$	欧拉角（roll, pitch, yaw）	—
$u, v, w$	质心速度分量	机体系
$\omega_x, \omega_y, \omega_z$	角速度分量	机体系

控制输入为四个电机转速 $\mathbf{u} = [\omega_1, \omega_2, \omega_3, \omega_4]^T$。

7.2 完整方程组

将前面推导的所有方程整合，得到 $\dot{\mathbf{x}} = f(\mathbf{x}, \mathbf{u})$ [6][8]：

位置运动学（惯性系速度 = 旋转矩阵 × 机体系速度）：

$$\begin{bmatrix} \dot{x} \\ \dot{y} \\ \dot{z} \end{bmatrix} = \mathbf{R}_{wb}(\phi,\theta,\psi) \begin{bmatrix} u \\ v \\ w \end{bmatrix}$$

姿态运动学（欧拉角导数 = 映射矩阵 × 角速度）：

$$\begin{bmatrix} \dot{\phi} \\ \dot{\theta} \\ \dot{\psi} \end{bmatrix} = \begin{bmatrix} 1 & \sin\phi\tan\theta & \cos\phi\tan\theta \\ 0 & \cos\phi & -\sin\phi \\ 0 & \sin\phi/\cos\theta & \cos\phi/\cos\theta \end{bmatrix} \begin{bmatrix} \omega_x \\ \omega_y \\ \omega_z \end{bmatrix}$$

平动动力学（机体系中的牛顿方程）：

$$\begin{bmatrix} \dot{u} \\ \dot{v} \\ \dot{w} \end{bmatrix} = \frac{1}{m}\mathbf{F}_b - \boldsymbol{\omega}_b \times \begin{bmatrix} u \\ v \\ w \end{bmatrix}$$

展开：

$$\dot{u} = \frac{F_{bx}}{m} + rv - qw$$ $$\dot{v} = \frac{F_{by}}{m} + pw - ru$$ $$\dot{w} = \frac{F_{bz}}{m} + qu - pv$$

其中 $(p, q, r) = (\omega_x, \omega_y, \omega_z)$（航空符号惯例 [6]）。

转动动力学（欧拉方程）：

$$\begin{bmatrix} \dot{\omega}_x \\ \dot{\omega}_y \\ \dot{\omega}_z \end{bmatrix} = \mathbf{I}^{-1}\left(\boldsymbol{\tau}_b - \boldsymbol{\omega}_b \times (\mathbf{I}\boldsymbol{\omega}_b)\right)$$

对角惯性张量展开：

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

7.3 方程组的结构分析

这 12 个一阶 ODE 可以分为四组：

方程组	维度	类型	特点
位置运动学	3	线性	仅依赖姿态和速度
姿态运动学	3	非线性	含 $\tan\theta$，有奇异点
平动动力学	3	非线性	含 $\boldsymbol{\omega} \times \mathbf{v}$ 耦合
转动动力学	3	非线性	含 $\boldsymbol{\omega} \times \mathbf{I}\boldsymbol{\omega}$ 耦合

关键观察：

• 方程是强非线性的（三角函数、叉积），不能直接用线性控制理论
• 级联结构：力矩 → 角速度 → 姿态 → 推力方向 → 加速度 → 速度 → 位置。这就是为什么飞控使用内外环串级控制 [7]
• 使用四元数替代欧拉角可消除姿态运动学的奇异点，但状态向量维度变为 13 [3]

7.4 混控矩阵：从控制量到电机指令

定义中间控制量为 $\mathbf{u}_c = [T, \tau_x, \tau_y, \tau_z]^T$（总推力 + 三轴力矩），与电机转速的关系为 [7]：

$$\begin{bmatrix} T \\ \tau_x \\ \tau_y \\ \tau_z \end{bmatrix} = \underbrace{\begin{bmatrix} k_T & k_T & k_T & k_T \\ -lk_T & lk_T & lk_T & -lk_T \\ lk_T & lk_T & -lk_T & -lk_T \\ -k_Q & k_Q & -k_Q & k_Q \end{bmatrix}}_{\mathbf{M}} \begin{bmatrix} \omega_1^2 \\ \omega_2^2 \\ \omega_3^2 \\ \omega_4^2 \end{bmatrix}$$

给定期望的 $\mathbf{u}_c$，电机转速指令为：

$$\begin{bmatrix} \omega_1^2 \\ \omega_2^2 \\ \omega_3^2 \\ \omega_4^2 \end{bmatrix} = \mathbf{M}^{-1}\mathbf{u}_c$$

需检查 $\omega_i^2 \geq 0$（电机不能反转）和 $\omega_i \leq \omega_{\max}$（不超过最大转速），超出范围时需要饱和限幅 [7]。

---

八、拉格朗日力学：第二视角

8.1 拉格朗日力学的基本框架

拉格朗日力学提供了一种基于能量而非力的建模方法，在处理约束系统时更加优雅 [1][2][5]。

定义（拉格朗日量）：

$$\mathcal{L} = E_k - E_p$$

即动能减势能。

拉格朗日方程（Euler-Lagrange 方程）[1][5]：

$$\frac{d}{dt}\frac{\partial \mathcal{L}}{\partial \dot{q}_j} - \frac{\partial \mathcal{L}}{\partial q_j} = Q_j$$

其中 $q_j$ 是广义坐标，$Q_j$ 是对应的广义力（非保守力在该广义坐标方向上的投影）。

与牛顿力学的等价性：对于无约束系统，拉格朗日方程给出与牛顿方程完全相同的运动方程。但当系统有约束（如刚体约束）时，拉格朗日方法自动处理约束力，不需要显式列出 [1]。

8.2 四旋翼的拉格朗日量

选择广义坐标 $\mathbf{q} = [x, y, z, \phi, \theta, \psi]^T$ [5][6]。

动能（König 定理）：

$$E_k = \frac{1}{2}m(\dot{x}^2 + \dot{y}^2 + \dot{z}^2) + \frac{1}{2}\boldsymbol{\omega}^T\mathbf{I}\boldsymbol{\omega}$$

转动动能需要用 $(\dot{\phi}, \dot{\theta}, \dot{\psi})$ 表达 $\boldsymbol{\omega}$：

$$E_k^{\text{rot}} = \frac{1}{2}\begin{bmatrix} \dot{\phi} \\ \dot{\theta} \\ \dot{\psi} \end{bmatrix}^T \mathbf{W}^T \mathbf{I}\, \mathbf{W} \begin{bmatrix} \dot{\phi} \\ \dot{\theta} \\ \dot{\psi} \end{bmatrix}$$

其中 $\mathbf{W}$ 是 3.4 节中的角速度映射矩阵。

势能（仅重力，NED 约定 z 朝下）：

$$E_p = -mgz$$

注意：NED 中 $z$ 增大表示向下，所以高度增加时 $z$ 减小，势能 $-mgz$ 增大，符号正确。

拉格朗日量：

$$\mathcal{L} = \frac{1}{2}m(\dot{x}^2 + \dot{y}^2 + \dot{z}^2) + \frac{1}{2}\boldsymbol{\omega}^T\mathbf{I}\boldsymbol{\omega} + mgz$$

8.3 推导运动方程（平动部分）

以 $z$ 方向为例 [5]：

$$\frac{\partial \mathcal{L}}{\partial \dot{z}} = m\dot{z}, \qquad \frac{\partial \mathcal{L}}{\partial z} = mg$$ $$\frac{d}{dt}(m\dot{z}) - mg = Q_z$$ $$m\ddot{z} = mg + Q_z$$

其中 $Q_z$ 是推力在惯性系 $z$ 方向的分量：$Q_z = (\mathbf{R}\mathbf{F}_b)_z = -(c\theta c\phi)T$。

得到 $m\ddot{z} = mg - (c\theta c\phi)T$，与牛顿方法的结果一致。$\quad\checkmark$

8.4 拉格朗日方法的优势与局限

方面	牛顿-欧拉法	拉格朗日法
出发点	力和力矩	能量
约束处理	需显式写出约束力	自动消去
适合场景	独立刚体、仿真实现	多连杆系统、理论分析
计算效率	高（直接写出 ODE）	低（需对 $\mathcal{L}$ 求偏导）
物理直觉	强（力→加速度）	弱（能量→运动）

对于四旋翼这种单刚体系统，牛顿-欧拉法更直接、更适合编程实现。拉格朗日法的价值在于：

• 验证牛顿方法的正确性
• 为多体系统（如带万向架的无人机、机械臂+无人机复合体）提供系统化建模工具 [5]

---

九、数值积分：让方程”跑”起来

9.1 初值问题

六自由度方程可以统一写成 [8][9]：

$$\dot{\mathbf{x}} = f(\mathbf{x}, \mathbf{u}, t), \qquad \mathbf{x}(0) = \mathbf{x}_0$$

给定初始状态 $\mathbf{x}_0$（如悬停），在每个时间步根据控制输入 $\mathbf{u}$ 计算 $\dot{\mathbf{x}}$，然后积分得到下一时刻的状态。

9.2 欧拉法（一阶）

最简单的积分方法 [9]：

$$\mathbf{x}_{k+1} = \mathbf{x}_k + \Delta t \cdot f(\mathbf{x}_k, \mathbf{u}_k)$$

• 优点：实现简单，计算量小
• 缺点：精度 $O(\Delta t)$，对于快速动态（高角速度）容易发散
• 步长要求：通常需要 $\Delta t \leq 0.001$ s（1 kHz）

9.3 四阶 Runge-Kutta 法（RK4）

工程仿真中最常用的方法 [9]：

$$\mathbf{k}_1 = f(\mathbf{x}_k, \mathbf{u}_k)$$ $$\mathbf{k}_2 = f(\mathbf{x}_k + \frac{\Delta t}{2}\mathbf{k}_1, \mathbf{u}_k)$$ $$\mathbf{k}_3 = f(\mathbf{x}_k + \frac{\Delta t}{2}\mathbf{k}_2, \mathbf{u}_k)$$ $$\mathbf{k}_4 = f(\mathbf{x}_k + \Delta t\,\mathbf{k}_3, \mathbf{u}_k)$$ $$\mathbf{x}_{k+1} = \mathbf{x}_k + \frac{\Delta t}{6}(\mathbf{k}_1 + 2\mathbf{k}_2 + 2\mathbf{k}_3 + \mathbf{k}_4)$$

• 精度：$O(\Delta t^4)$——同样步长下比欧拉法精确得多
• 步长：$\Delta t = 0.005$ s（200 Hz）通常已足够
• 代价：每步需要计算 4 次 $f$

9.4 步长选择与稳定性

仿真场景	推荐步长	推荐方法	说明
悬停/缓慢飞行	5 ms	RK4	动态变化慢
快速机动	1 ms	RK4	高角速度需要更小步长
PX4 HITL	1 ms	欧拉/RK4	与飞控主频匹配
碰撞检测	0.1 ms	专用积分器	接触力刚度大

经验法则：步长应小于系统最快模态周期的 1/10。对于典型四旋翼，最快模态是角速度环（带宽约 50 Hz），所以 $\Delta t \leq 1/(10 \times 50) = 2$ ms [8]。

9.5 积分中的注意事项

1. 四元数归一化：如果使用四元数表示姿态，每步积分后必须重新归一化 $\mathbf{q} \leftarrow \mathbf{q}/|\mathbf{q}|$ [3]
2. 角度环绕：欧拉角 $\psi$ 在 $\pm\pi$ 处需要处理环绕（wrap around）
3. 电机饱和：计算出的电机转速不能为负或超过上限
4. 能量守恒检查：自由落体等已知场景下，检查总能量是否守恒（误差应与 $O(\Delta t^p)$ 一致）

---

十、仿真验证：从方程到代码

10.1 Python 实现框架

将六自由度方程实现为可运行的仿真器：

import numpy as np
from scipy.spatial.transform import Rotation

class QuadrotorDynamics:
    def __init__(self):
        self.m = 1.5          # 质量 (kg)
        self.g = 9.81         # 重力加速度
        self.l = 0.25         # 电机到质心距离 (m)
        self.kT = 8e-6        # 推力系数
        self.kQ = 2e-7        # 扭矩系数
        self.Ixx = 0.0125     # 惯性矩 (kg·m²)
        self.Iyy = 0.0125
        self.Izz = 0.025
        self.I = np.diag([self.Ixx, self.Iyy, self.Izz])
        self.I_inv = np.linalg.inv(self.I)
    
    def forces_and_moments(self, state, motor_speeds):
        """计算机体系下的合力与合力矩"""
        phi, theta, psi = state[3:6]
        omega_sq = motor_speeds ** 2
        
        T = self.kT * np.sum(omega_sq)
        tau_x = self.l * self.kT * (-omega_sq[0] + omega_sq[1] 
                                     + omega_sq[2] - omega_sq[3])
        tau_y = self.l * self.kT * ( omega_sq[0] + omega_sq[1] 
                                     - omega_sq[2] - omega_sq[3])
        tau_z = self.kQ * (-omega_sq[0] + omega_sq[1] 
                           - omega_sq[2] + omega_sq[3])
        
        F_body = np.array([0, 0, -T])
        R = Rotation.from_euler('ZYX', [psi, theta, phi]).as_matrix()
        F_gravity = np.array([0, 0, self.m * self.g])
        F_total_world = R @ F_body + F_gravity
        
        tau_body = np.array([tau_x, tau_y, tau_z])
        return F_total_world, tau_body
    
    def derivatives(self, state, motor_speeds):
        """计算状态导数 dx/dt"""
        pos = state[0:3]
        angles = state[3:6]
        vel = state[6:9]
        omega_b = state[9:12]
        
        F_world, tau_b = self.forces_and_moments(state, motor_speeds)
        
        # 位置导数 = 速度（惯性系）
        d_pos = vel
        
        # 姿态导数（欧拉角运动学）
        phi, theta, _ = angles
        W_inv = np.array([
        ])
        d_angles = W_inv @ omega_b
        
        # 速度导数（牛顿第二定律，惯性系）
        d_vel = F_world / self.m
        
        # 角速度导数（欧拉方程）
        gyro = np.cross(omega_b, self.I @ omega_b)
        d_omega = self.I_inv @ (tau_b - gyro)
        
        return np.concatenate([d_pos, d_angles, d_vel, d_omega])
    
    def step_rk4(self, state, motor_speeds, dt):
        """四阶 Runge-Kutta 积分"""
        k1 = self.derivatives(state, motor_speeds)
        k2 = self.derivatives(state + 0.5*dt*k1, motor_speeds)
        k3 = self.derivatives(state + 0.5*dt*k2, motor_speeds)
        k4 = self.derivatives(state + dt*k3, motor_speeds)
        return state + (dt/6) * (k1 + 2*k2 + 2*k3 + k4)

10.2 悬停验证

基本的正确性检验——悬停时状态应保持不变：

quad = QuadrotorDynamics()

# 悬停状态
state = np.zeros(12)

# 悬停电机转速：T = mg -> 4*kT*omega^2 = mg
omega_hover = np.sqrt(quad.m * quad.g / (4 * quad.kT))
motors = np.array([omega_hover] * 4)

print(f"悬停转速: {omega_hover:.1f} rad/s ({omega_hover*60/(2*np.pi):.0f} RPM)")

# 仿真 5 秒
dt = 0.005
for _ in range(1000):
    state = quad.step_rk4(state, motors, dt)

print(f"5秒后位置: z = {state[2]:.6f} m（应接近 0）")
print(f"5秒后速度: vz = {state[8]:.6f} m/s（应接近 0）")

10.3 自由落体验证

关闭电机（$\omega_i = 0$），检查竖直方向加速度是否为 $g$：

state = np.zeros(12)
motors = np.zeros(4)
dt = 0.001
positions = []

for _ in range(1000):
    state = quad.step_rk4(state, motors, dt)
    positions.append(state[2])

# 1秒后高度：z = 0.5*g*t^2 = 4.905 m（NED下z朝下为正）
print(f"1秒后 z = {state[2]:.4f} m（理论值 4.905 m）")

---

十一、总结

11.1 推导路线图

牛顿三定律
    ├── 质心运动定理 ──→ 平动方程: m(dv/dt + ω×v) = F
    └── 角动量定理 ──→ 转动方程: Iω̇ + ω×(Iω) = τ
                             │
    螺旋桨空气动力学 ──→ 推力/扭矩模型: T = kT·ω², Q = kQ·ω²
                             │
    混控矩阵 ──→ 控制指令 → 各电机转速
                             │
    合成 ──→ 完整 6-DOF ODE: ẋ = f(x, u)
                             │
    数值积分 (RK4) ──→ 仿真器

11.2 核心方程速查

方程	公式	出处
平动（惯性系）	$m\ddot{\mathbf{r}} = \mathbf{R}\mathbf{F}_b + m\mathbf{g}$	牛顿第二定律
平动（机体系）	$m(\dot{\mathbf{v}}_b + \boldsymbol{\omega} \times \mathbf{v}_b) = \mathbf{F}_b$	旋转参考系修正
转动	$\mathbf{I}\dot{\boldsymbol{\omega}} + \boldsymbol{\omega} \times (\mathbf{I}\boldsymbol{\omega}) = \boldsymbol{\tau}$	欧拉方程
推力	$T_i = k_T\omega_i^2$	空气动力学简化模型
扭矩	$Q_i = k_Q\omega_i^2$	同上
姿态运动学	$\dot{\mathbf{q}} = \frac{1}{2}\mathbf{q} \otimes \tilde{\boldsymbol{\omega}}$	四元数微分
拉格朗日方程	$\frac{d}{dt}\frac{\partial\mathcal{L}}{\partial\dot{q}} - \frac{\partial\mathcal{L}}{\partial q} = Q$	能量方法

---

参考文献

经典力学教材

• [1] Herbert Goldstein, Charles P. Poole, John L. Safko. Classical Mechanics, 3rd Edition. Addison-Wesley, 2001. ISBN: 978-0201657029.
  经典力学的权威教材，涵盖牛顿力学、拉格朗日力学、哈密顿力学和刚体动力学的完整理论。

• [2] L.D. Landau, E.M. Lifshitz. Mechanics (Course of Theoretical Physics, Vol. 1), 3rd Edition. Butterworth-Heinemann, 1976. ISBN: 978-0750628969.
  朗道力学，以极简的方式从最小作用量原理推导出全部经典力学。理论物理的经典参考。

无人机建模专著

• [3] Joan Solà. “Quaternion kinematics for the error-state Kalman filter.” arXiv preprint, 2017. https://arxiv.org/abs/1711.02508
  四元数运动学的工程教程，涵盖旋转参数化、微分方程和 EKF 应用。

• [4] J. Gordon Leishman. Principles of Helicopter Aerodynamics, 2nd Edition. Cambridge University Press, 2006. ISBN: 978-0521858601.
  直升机/旋翼空气动力学的标准教材，详细推导了动量理论和叶素理论。对理解螺旋桨推力模型至关重要。

• [5] Reza N. Jazar. Theory of Applied Robotics: Kinematics, Dynamics, and Control, 3rd Edition. Springer, 2022. ISBN: 978-3030932695.
  覆盖刚体运动学、动力学（牛顿-欧拉法和拉格朗日法）及控制，有大量无人机相关的算例。

• [6] Randal W. Beard, Timothy W. McLain. Small Unmanned Aircraft: Theory and Practice. Princeton University Press, 2012. 配套资源：https://github.com/randybeard/uavbook
  小型无人机建模与控制的标准教材，本文六自由度方程的推导主要参考此书第 3-5 章。

飞控与仿真文档

• [7] PX4 开发团队. PX4 Autopilot User Guide. https://docs.px4.io/main/en/
  PX4 飞控文档，包含坐标系约定、混控矩阵定义和控制架构说明。

• [8] Quan Quan. Introduction to Multicopter Design and Control. Springer, 2017. ISBN: 978-9811033810.
  多旋翼设计与控制的系统化教材，详细推导了完整六自由度模型及控制器设计。

• [9] William H. Press et al. Numerical Recipes: The Art of Scientific Computing, 3rd Edition. Cambridge University Press, 2007. http://numerical.recipes/
  数值方法的经典参考，包含 ODE 积分方法（欧拉、RK4、自适应步长等）的详细讨论。

进阶与数据资源

• [10] Vijay Kumar. “Modeling and Control of Quadrotors.” GRASP Lab, University of Pennsylvania. 课程讲义：https://alliance.seas.upenn.edu/~meam620/wiki/
  宾大 GRASP 实验室的四旋翼建模课程，包含清晰的六自由度推导和 MATLAB 仿真代码。

• [11] Peter Corke. Robotics, Vision and Control, 3rd Edition. Springer, 2023. 配套工具箱：https://github.com/petercorke/robotics-toolbox-python
  刚体动力学、旋转表示、轨迹规划的综合参考，附带可运行的 Python 代码。

• [12] 3Blue1Brown (Grant Sanderson). Essence of Linear Algebra & Essence of Calculus 系列视频. https://www.3blue1brown.com/
  通过动画建立向量、矩阵、微积分的几何直觉。B 站有官方中文字幕版。

• [13] UIUC Propeller Data Site. https://m-selig.ae.illinois.edu/props/propDB.html
  伊利诺伊大学的螺旋桨性能数据库，包含数百种桨型的 $C_T$、$C_Q$ 实测数据，是仿真参数标定的重要来源。

---

核心思想：六自由度运动方程是无人机仿真的”操作系统”——所有的控制算法、传感器模型、轨迹规划，都是在这组方程的基础上运行的。理解它的推导过程，就是理解仿真为什么能逼近现实，以及在什么条件下会偏离现实（模型假设被违反时）。把这组方程从头到尾手推一遍，再对着 AirSim 或 PX4 源码逐行核对，你对无人机物理的理解会提升一个量级。