四旋翼无人机动力学
本笔记是笔者学习《Small Unmanned Aircraft: Theory and Practice》中四旋翼无人机动力学部分后整理的学习笔记。本书链接：github.com/byu-magicc/mavsim_public。
1. 坐标系与旋转矩阵（Rotation Matrices）#
有坐标系 $\mathcal{F}^0$（$\vec{i}^0, \vec{j}^0, \vec{k}^0$）和 $\mathcal{F}^1$（$\vec{i}^1, \vec{j}^1, \vec{k}^1$）。
点 $\vec{P}$ 在两个坐标系下的表示：
$$\vec{P} = p_x^0\,\vec{i}^0 + p_y^0\,\vec{j}^0 + p_z^0\,\vec{k}^0 \quad \text{在 } \mathcal{F}^0 \text{ 下}$$ $$\vec{P} = p_x^1\,\vec{i}^1 + p_y^1\,\vec{j}^1 + p_z^1\,\vec{k}^1 \quad \text{在 } \mathcal{F}^1 \text{ 下}$$利用正交性（$\vec{i}\cdot\vec{j} = 0$，$\vec{i}^2 = 1$），两边点乘 $\vec{i}^1$：
$$\vec{i}^1 \cdot \left(p_x^1\vec{i}^1 + p_y^1\vec{j}^1 + p_z^1\vec{k}^1\right) = p_x^1 = \vec{i}^1 \cdot \left(p_x^0\vec{i}^0 + p_y^0\vec{j}^0 + p_z^0\vec{k}^0\right) = \begin{pmatrix} \vec{i}^1\cdot\vec{i}^0 & \vec{i}^1\cdot\vec{j}^0 & \vec{i}^1\cdot\vec{k}^0 \end{pmatrix} \begin{pmatrix} p_x^0 \\ p_y^0 \\ p_z^0 \end{pmatrix}$$于是 $\vec{P}$ 在 $\mathcal{F}^1$ 下的坐标为：
$$\vec{P}^1 \triangleq \begin{pmatrix} p_x^1 \\ p_y^1 \\ p_z^1 \end{pmatrix} = \begin{pmatrix} \vec{i}^1\cdot\vec{i}^0 & \vec{i}^1\cdot\vec{j}^0 & \vec{i}^1\cdot\vec{k}^0 \\ \vec{j}^1\cdot\vec{i}^0 & \vec{j}^1\cdot\vec{j}^0 & \vec{j}^1\cdot\vec{k}^0 \\ \vec{k}^1\cdot\vec{i}^0 & \vec{k}^1\cdot\vec{j}^0 & \vec{k}^1\cdot\vec{k}^0 \end{pmatrix} \begin{pmatrix} p_x^0 \\ p_y^0 \\ p_z^0 \end{pmatrix}$$即
$$\vec{P}^1 = R_0^1\,\vec{P}^0$$1.1 例：绕轴旋转 $\theta$
$$R_0^1 \triangleq \begin{pmatrix} \cos\theta & \sin\theta & 0 \\ -\sin\theta & \cos\theta & 0 \\ 0 & 0 & 1 \end{pmatrix}$$如何理解：要求 $\mathcal{F}^1$ 坐标系下的横坐标在 $\mathcal{F}^0$ 下的值，就把线段 $a$ 通过三角函数分解到 $\mathcal{F}^0$ 下，即 $(a\cos\theta,\ a\sin\theta)$。
1.2 旋转矩阵的三个性质
- $(R_a^b)^{-1} = (R_a^b)^T = R_b^a$
- $R_b^c\,R_a^b = R_a^c$
- $\lambda = 1$
原稿写作 $\lambda = 1$，应指特征值——旋转矩阵必有一个特征值为 1，对应特征向量即转轴（其余两个为 $e^{\pm i\theta}$）；教材中这条也常写作 $\det(R) = 1$。
1.3 坐标系旋转 vs 向量旋转
旋转矩阵既可用在坐标系的旋转，也可用在向量旋转：原坐标系下 $\vec{q}$ 的坐标，等价于坐标系逆转 $\theta$ 后 $\vec{p}$ 的坐标，即
$$\vec{q} = R_0^1\,\vec{p}$$2. 无人机常用坐标系#
2.1 惯性系 inertial frame $\mathcal{F}^i$
北东地（NED）：
$$\begin{cases} \text{North} \to x \\ \text{East} \to y \\ \text{Down} \to z \end{cases}$$2.2 vehicle frame $\mathcal{F}^v$
原点在质心，坐标轴方向与惯性系一致（只平移、不旋转）。
2.3 vehicle-1 frame $\mathcal{F}^{v1}$
偏航（yaw）$\psi$ 后的机体坐标系，有
$$\vec{p}^{v1} = R_v^{v1}(\psi)\,\vec{p}^v$$2.4 vehicle-2 frame $\mathcal{F}^{v2}$
在 $\mathcal{F}^{v1}$ 基础上做俯仰（pitch）旋转 $\theta$（仰头为正），有
$$\vec{p}^{v2} = R_{v1}^{v2}(\theta)\,\vec{p}^{v1}$$2.5 body frame $\mathcal{F}^b$
vehicle frame 加上 Roll（横滚 $\phi$）之后的机体坐标系：
$$\vec{p}^b = R_{v2}^b(\phi)\,\vec{p}^{v2}$$（$\psi, \theta, \phi$ 都通过右手定则确定正方向。）
联立之后，得到从 vehicle frame 到 body frame 的变换：
$$R_v^b(\psi, \theta, \phi) = R_{v2}^b(\phi)\,R_{v1}^{v2}(\theta)\,R_v^{v1}(\psi)$$ $$= \begin{pmatrix} 1 & 0 & 0 \\ 0 & \cos\phi & \sin\phi \\ 0 & -\sin\phi & \cos\phi \end{pmatrix} \begin{pmatrix} \cos\theta & 0 & -\sin\theta \\ 0 & 1 & 0 \\ \sin\theta & 0 & \cos\theta \end{pmatrix} \begin{pmatrix} \cos\psi & \sin\psi & 0 \\ -\sin\psi & \cos\psi & 0 \\ 0 & 0 & 1 \end{pmatrix}$$ $$= \begin{pmatrix} \cos\theta\cos\psi & \cos\theta\sin\psi & -\sin\theta \\ \sin\phi\sin\theta\cos\psi - \cos\phi\sin\psi & \sin\phi\sin\theta\sin\psi + \cos\phi\cos\psi & \sin\phi\cos\theta \\ \cos\phi\sin\theta\cos\psi + \sin\phi\sin\psi & \cos\phi\sin\theta\sin\psi - \sin\phi\cos\psi & \cos\phi\cos\theta \end{pmatrix}$$3. 欧拉角与四元数#
$\phi, \theta, \psi$ 称为欧拉角。
3.1 欧拉角的问题：万向节锁（gimbal lock）
旋转顺序 $\text{Yaw} \rightarrow \text{Pitch} \rightarrow \text{Roll}$。当 $\text{Pitch} = \pm 90^\circ$ 时，Yaw 与 Roll 的效果其实是一样的，两者无法独立区分，丢失了一个自由度 → 万向节锁。
解决方法：四元数。
| 四元数 | 说明 |
|---|---|
| 优点 | 无数学上的奇点，计算效率更高 |
| 缺点 | 不直观 |
3.2 从复数域理解四元数
两个复数相乘：
$$\vec{z}\cdot\vec{w} = (z_r + j z_i)(w_r + j w_i) = (z_r w_r - z_i w_i) + j(z_i w_r + w_i z_r)$$等价于矩阵形式：
$$\begin{pmatrix} z_r & -z_i \\ z_i & z_r \end{pmatrix}\begin{pmatrix} w_r \\ w_i \end{pmatrix} = M(\vec{z})\,\vec{w}$$$\vec{z}$ 归一化后（$\vec{z} = e^{j\theta} = \cos\theta + j\sin\theta$）：
$$M(\vec{z}) = \begin{pmatrix} \cos\theta & -\sin\theta \\ \sin\theta & \cos\theta \end{pmatrix} \quad \text{（正是二维旋转矩阵）}$$3.3 四元数介绍
四元数 $\vec{e} = e_0 + i e_x + j e_y + k e_z$，其中 $i, j, k$ 满足：
$$i^2 = j^2 = k^2 = ijk = -1, \qquad ij = k,\ jk = i,\ ki = j \ \text{（右手定则）}$$四元数乘法同样可以写成矩阵形式 $\vec{q}\cdot\vec{e} = M(\vec{q})\,\vec{e}$：
$$M(\vec{q}) = \begin{pmatrix} q_0 & -q_x & -q_y & -q_z \\ q_x & q_0 & q_z & -q_y \\ q_y & -q_z & q_0 & q_x \\ q_z & q_y & -q_x & q_0 \end{pmatrix}$$3.4 四元数旋转
四元数归一化之后，可以表示一个 3D 旋转（不够直观，但数学上没问题）。
unit quaternion：$e = (e_0, e_x, e_y, e_z)^T$，$\|e\| = 1$。$e_0$ 是实部，$\vec{e} = (e_x, e_y, e_z)^T$ 是 vector part。
设转轴为单位向量 $\vec{v}$，转动角度为 $\theta$，则：
$$e_0 = \cos\frac{\theta}{2}, \qquad \begin{pmatrix} e_x \\ e_y \\ e_z \end{pmatrix} = \vec{v}\sin\frac{\theta}{2}$$4. 状态量与运动学#
4.1 状态变量（State Variables）
- $P_n, P_e, P_d$ —— 惯性系下无人机的坐标
- $u, v, w$ —— 机体系下 $x, y, z$ 方向的地速
- $\phi, \theta, \psi$ —— Roll、Pitch、Yaw
- $p, q, r$ —— 机体系下 $x, y, z$ 方向的角速度（不是简单的欧拉角求导）
4.2 位置运动学
$$\frac{d}{dt}\begin{pmatrix} P_n \\ P_e \\ P_d \end{pmatrix} = R_b^v \begin{pmatrix} u \\ v \\ w \end{pmatrix} = \left(R_v^b\right)^T \begin{pmatrix} u \\ v \\ w \end{pmatrix}$$（因为 $u,v,w$ 是沿 body frame 的 $x,y,z$ 的，所以需要 $R_b^v$ 转回。）
4.3 姿态运动学
角度与角速度的运动学不太一样：三个欧拉角的旋转不是都按照 body frame 的轴旋转的（只有 Roll 是）。
$$\begin{pmatrix} p \\ q \\ r \end{pmatrix} = \begin{pmatrix} \dot{\phi} \\ 0 \\ 0 \end{pmatrix} + R_{v2}^b(\phi) \begin{pmatrix} 0 \\ \dot{\theta} \\ 0 \end{pmatrix} + R_{v2}^b(\phi)\,R_{v1}^{v2}(\theta) \begin{pmatrix} 0 \\ 0 \\ \dot{\psi} \end{pmatrix}$$解得：
$$\begin{pmatrix} \dot{\phi} \\ \dot{\theta} \\ \dot{\psi} \end{pmatrix} = \begin{pmatrix} 1 & \sin\phi\tan\theta & \cos\phi\tan\theta \\ 0 & \cos\phi & -\sin\phi \\ 0 & \sin\phi\sec\theta & \cos\phi\sec\theta \end{pmatrix} \begin{pmatrix} p \\ q \\ r \end{pmatrix}$$5. 刚体动力学（flat earth model）#
机体速度 $\vec{V}_g^b = (u,v,w)^T$：地速，但沿 body frame 的 $x,y,z$ 方向。
5.1 平移
惯性系下牛顿第二定律：
$$m\,\frac{d\vec{V}_g}{dt_i} = \vec{f}$$转到 body frame 下（输运定理）：
$$\frac{d\vec{V}_g}{dt_i} = \frac{d\vec{V}_g}{dt_b} + \vec{\omega}_{b/i} \times \vec{V}_g$$如何理解牵连项：
- 三维降到二维方便理解；
- 假设无角速度：$\vec{V}_g$ 在 $u,v$ 方向上分量的微分的向量和与 $x,y$ 一致；
- 假设有角速度：站在 body frame 向 $x$ 正方向看，$\vec{V}_g$ 相对我的方向不是正前方，而是右前方（由于旋转的存在）。
$\therefore$ 需要牵连项 $\vec{\omega} \times \vec{V}_g$ 作为补偿——把机体系测到的相对变化率，修正成惯性系需要的绝对变化率。
演示画面为 z 向下右手系（与笔记 NED 约定一致）；向量合成关系与坐标取向无关。
对无人机（$\vec{V}_g^b = (u,v,w)^T$，$\vec{\omega}_{b/i}^b = (p,q,r)^T$）：
$$m\left(\frac{d\vec{V}_g^b}{dt_b} + \vec{\omega}_{b/i}^b \times \vec{V}_g^b\right) = \vec{f}^b, \qquad \vec{f}^b \triangleq (f_x, f_y, f_z)^T$$展开得：
$$\begin{pmatrix} \dot{u} \\ \dot{v} \\ \dot{w} \end{pmatrix} = \begin{pmatrix} rv - qw \\ pw - ru \\ qu - pv \end{pmatrix} + \frac{1}{m}\begin{pmatrix} f_x \\ f_y \\ f_z \end{pmatrix}$$5.2 旋转
$$\frac{d\vec{h}}{dt_i} = \vec{m} \qquad \begin{array}{l} \vec{h}\ \text{角动量} \\ \vec{m}\ \text{合外力矩} \end{array}$$同样有：
$$\frac{d\vec{h}^b}{dt_i} = \frac{d\vec{h}^b}{dt_b} + \vec{\omega}_{b/i}^b \times \vec{h}^b = \vec{m}^b$$其中 $\vec{h}^b \triangleq J\,\vec{\omega}_{b/i}^b$，$J$ 是惯性矩阵：
$$J_{xx} = \int (y^2 + z^2)\,dm \quad \text{转动惯量（moments of inertia）}$$ $$J_{xy} = \int xy\,dm \quad \text{惯性积（products of inertia）：反映质量分布不对称导致的轴间耦合}$$ $$J = \begin{pmatrix} J_x & -J_{xy} & -J_{xz} \\ -J_{xy} & J_y & -J_{yz} \\ -J_{xz} & -J_{yz} & J_z \end{pmatrix}$$实际工作中 $J$ 的获得不是通过公式计算，而是 ① CAD 等软件数值计算；② 各种实验装置测量。
$J$ 只和 body 本身有关，当 $x,y,z$ 轴不变时 $J$ 为常数——$\therefore$ 在 body frame 中 $J$ 不随时间变化。于是：
$$J\,\frac{d\vec{\omega}_{b/i}^b}{dt_b} + \vec{\omega}_{b/i}^b \times \left(J\,\vec{\omega}_{b/i}^b\right) = \vec{m}^b$$ $$\dot{\vec{\omega}}_{b/i}^b = J^{-1}\left(\vec{m}^b - \vec{\omega}_{b/i}^b \times \left(J\,\vec{\omega}_{b/i}^b\right)\right)$$航空器一般沿 body frame 的 $xoz$ 平面对称 $\Rightarrow J_{xy} = J_{yz} = 0$：
$$J = \begin{pmatrix} J_x & 0 & -J_{xz} \\ 0 & J_y & 0 \\ -J_{xz} & 0 & J_z \end{pmatrix}$$对四旋翼有更多对称性：
$$J = \begin{pmatrix} J_x & 0 & 0 \\ 0 & J_y & 0 \\ 0 & 0 & J_z \end{pmatrix}$$6. 四旋翼建模（基于四元数）#
6.1 坐标系与状态量选取
坐标系选取：
$$\begin{cases} \text{北东地（NED）—— 航空航天} \\ \text{东北天（ENU）—— 机器人、计算机视觉} \end{cases}$$这里取 NED。vehicle frame $\mathcal{F}^v$ 原点在质心，方向与 NED 一致。
| Yaw | Pitch | Roll |
|---|---|---|
| 偏航 $\psi$ | 俯仰 $\theta$ | 横滚 $\phi$ |
状态量：
- 位置向量（NED 惯性系下机体位置）：$\vec{p}_{b/i}^i \triangleq (P_n, P_e, P_d)^T$
- 速度：$\vec{v}_{b/i}^i \triangleq (U_n, U_e, U_d)^T$
和固定翼不一样，四旋翼的地速几乎不影响气动，所以不用 body frame 下的速度。
- 姿态四元数（从 body 到 inertial）：$\bar{q}_b^i \triangleq (e_0, \vec{q}^T)^T$，$\vec{q} = (e_x, e_y, e_z)^T$
- 角速度：$\vec{\omega}_{b/i}^b \triangleq (p, q, r)^T$
6.2 运动学
$$\dot{\vec{p}}_{b/i}^i = \vec{v}_{b/i}^i$$（看上标：上标是 $i$ 就是惯性系下的量。）
$$\dot{\bar{q}}_b^i = \tfrac{1}{2}\,\Omega\left(\vec{\omega}_{b/i}^b\right)\bar{q}_b^i$$（姿态四元数表征的就是 UAV 的旋转姿态；该式在教材 B.6 处证明。）
$$\Omega\left(\vec{\omega}_{b/i}^b\right) \triangleq \begin{pmatrix} 0 & -p & -q & -r \\ p & 0 & r & -q \\ q & -r & 0 & p \\ r & q & -p & 0 \end{pmatrix}$$ $$\dot{\vec{v}}_{b/i}^i = \frac{1}{m}\,\vec{f}^i$$6.3 转动动力学展开
$$\dot{\vec{\omega}}_{b/i}^b = J^{-1}\left[-\vec{\omega}_{b/i}^b \times \left(J\,\vec{\omega}_{b/i}^b\right) + \vec{m}^b\right], \qquad \vec{m}^b = \begin{pmatrix} l \\ m \\ n \end{pmatrix}\ \text{(control torques)}$$由对角惯性矩阵 $J = \mathrm{diag}(J_x, J_y, J_z)$ 展开得：
$$\begin{pmatrix} \dot{p} \\ \dot{q} \\ \dot{r} \end{pmatrix} = \begin{pmatrix} \frac{J_y - J_z}{J_x}\,qr \\ \frac{J_z - J_x}{J_y}\,pr \\ \frac{J_x - J_y}{J_z}\,pq \end{pmatrix} + \begin{pmatrix} l/J_x \\ m/J_y \\ n/J_z \end{pmatrix}$$6.4 完整动力学方程
位置与速度：
$$\dot{P}_n = U_n \qquad \dot{U}_n = f_n/m$$ $$\dot{P}_e = U_e \qquad \dot{U}_e = f_e/m$$ $$\dot{P}_d = U_d \qquad \dot{U}_d = f_d/m$$姿态（四元数 — 角速度 — 转动力矩）：
$$\dot{\bar{q}}_b^i = \tfrac{1}{2}\,\Omega\left(\vec{\omega}_{b/i}^b\right)\bar{q}_b^i$$即
$$\dot{e}_0 = \tfrac{1}{2}\left(-p e_x - q e_y - r e_z\right)$$ $$\dot{e}_x = \tfrac{1}{2}\left(p e_0 + r e_y - q e_z\right)$$ $$\dot{e}_y = \tfrac{1}{2}\left(q e_0 - r e_x + p e_z\right)$$ $$\dot{e}_z = \tfrac{1}{2}\left(r e_0 + q e_x - p e_y\right)$$角速度：
$$\dot{p} = \frac{J_y - J_z}{J_x}\,qr + l/J_x$$ $$\dot{q} = \frac{J_z - J_x}{J_y}\,pr + m/J_y$$ $$\dot{r} = \frac{J_x - J_y}{J_z}\,pq + n/J_z$$7. 力与力矩#
7.1 斜对称算子与罗德里格斯公式
斜对称算子（skew operator）：
$$\left\lfloor \begin{pmatrix} a \\ b \\ c \end{pmatrix} \right\rfloor_{\times} \triangleq \begin{pmatrix} 0 & -c & b \\ c & 0 & -a \\ -b & a & 0 \end{pmatrix}$$⭐ 罗德里格斯公式：
$$R = I + \sin\theta \lfloor \hat{k} \rfloor_{\times} + (1-\cos\theta)\lfloor \hat{k} \rfloor_{\times}^2$$其中 $\hat{k}$ 为单位转轴（$|\hat{k}|=1$），$\theta$ 为绕该轴的转角。
用四元数表示：
$$R(\bar{q}) = I + 2e_0 \lfloor \vec{e} \rfloor_{\times} + 2\lfloor \vec{e} \rfloor_{\times}^2$$7.2 合外力与合外力矩
$$\vec{f}^i = \vec{f}_g^{\,i} + \vec{f}_t^{\,i} + \vec{f}_d^{\,i}$$三项依次为：重力、桨叶升力、旋转桨叶产生的阻力（一般考虑诱导阻力 induced drag）。
$$\vec{m}^b = \vec{m}_p^{\,b} \quad \text{（桨叶产生的力矩）}$$重力：
$$\vec{f}_g^{\,i} = mg\,\vec{e}_3, \qquad \vec{e}_3 = (0, 0, 1)^T$$诱导阻力：
$$\vec{f}_d^{\,b} \approx -T C_d \, \mathrm{diag}(1,1,0)\,\vec{v}_{b/i}^{\,b}$$其中 $T$ 是桨叶产生的总升力。直观理解：① 升力越大，桨叶转得越快，induced drag 越大；② 速度越快，与速度方向相反的 induced drag 越大。
为了 ① 使阻力模型独立于控制量 $T$、② 方便系统辨识，取 $T \approx mg$，令
$$D = \begin{pmatrix} gC_d & 0 & 0 \\ 0 & gC_d & 0 \\ 0 & 0 & 0 \end{pmatrix}$$则惯性系下：
$$\vec{f}_d^{\,i} = -m R(\bar{q}_b^i)\, D\, R^T(\bar{q}_b^i)\,\vec{v}_{b/i}^{\,i}$$（因为是从机体转到惯性系，所以用 $RDR^T$ 而不是 $R^TDR$。）
速度方程：
$$\dot{\vec{v}}_{b/i}^{\,i} = \frac{\vec{f}^i}{m} = g\vec{e}_3 - R(\bar{q}_b^i)\, D\, R^T(\bar{q}_b^i)\,\vec{v}_{b/i}^{\,i} - \frac{T}{m}R^T(\bar{q}_b^i)\,\vec{e}_3$$（最后一项：机体系 $T\vec{e}_3$，加 $R^T$ 转回惯性系。）
7.3 桨叶（电机）产生的力和力矩
$$\vec{f}_p^{\,b} = -T\vec{e}_3 \quad \Rightarrow \quad \vec{f}_p^{\,i} = -T R^T(\bar{q}_b^i)\,\vec{e}_3$$下面研究总推力 $T$、力矩 $\vec{\tau}^b = (\tau_x, \tau_y, \tau_z)^T$ 与电机转速 $\omega_1, \omega_2, \cdots, \omega_N$（rad/s）的关系。
单个电机产生的升力和力矩：
$$T_i = C_T \frac{\rho D^4}{4\pi^2}\,\omega_i^2, \qquad Q_i = C_Q \frac{\rho D^5}{4\pi^2}\,\omega_i^2$$（$D$ 是桨叶直径，$C_T$、$C_Q$ 是无量纲气动参数，$\rho$ 是空气密度。）
总升力：
$$T = \sum_{i=1}^{N} T_i = C_T \frac{\rho D^4}{4\pi^2} \sum_{i=1}^{N} \omega_i^2$$7.4 电机布局与力矩分配
先规定电机旋转方向：
$$d_i = \begin{cases} +1 & \text{CCW（counter-clockwise，逆时针）} \\ -1 & \text{CW（clockwise，顺时针）} \end{cases}$$设第 $i$ 个旋翼相对质心的位置矢量为 $\vec{l}_i$，$\vec{l}_i$ 与机头方向单位向量 $\vec{i}^b$ 的夹角为 $\varphi_i$。
影响 Roll 的力矩（$T_i$ 沿 $\vec{i}^b$ 向上时是使 Roll 负方向的力，故有负号）：
$$\tau_x = -\sum_{i=1}^{N}(l_i\sin\varphi_i)\,T_i$$影响 Pitch 的力矩：
$$\tau_y = \sum_{i=1}^{N}(l_i\cos\varphi_i)\,T_i$$影响 Yaw 的力矩（$d_i$ 给方向，$Q_i$ 给大小）：
$$\tau_z = \sum_{i=1}^{N} d_i\,Q_i = C_Q\frac{\rho D^5}{4\pi^2}\sum_{i=1}^{N} d_i\,\omega_i^2$$$\tau_x$、$\tau_y$ 代入 $T_i$ 展开时，原稿写成了 $C_Q \rho D^5$ 的系数（量纲上应为 $C_T \rho D^4$），原作者本人已在旁边批注"这里应该是原文书代入错了"。
写成矩阵形式（控制分配）：
$$\begin{pmatrix} T \\ \tau_x \\ \tau_y \\ \tau_z \end{pmatrix} = M \begin{pmatrix} \omega_1^2 \\ \vdots \\ \omega_N^2 \end{pmatrix} \qquad \Longrightarrow \qquad \begin{pmatrix} \omega_1^2 \\ \omega_2^2 \\ \omega_3^2 \\ \omega_4^2 \end{pmatrix} = M^{-1}\begin{pmatrix} T \\ \tau_x \\ \tau_y \\ \tau_z \end{pmatrix} \ \text{（四旋翼情况）}$$本示例取 $T_i=\omega_i^2$、$Q_i=0.1\,\omega_i^2$、$l_i=1$、$\varphi_i=45^\circ/135^\circ/225^\circ/315^\circ$，数值为示意比例。
8. 动力学汇总与线性化#
8.1 非线性模型（四元数形式）
$$\dot{\vec{p}}_{b/i}^{\,i} = \vec{v}_{b/i}^{\,i}$$ $$\dot{\vec{v}}_{b/i}^{\,i} = g\vec{e}_3 - R(\bar{q}_b^i)\, D\, R^T(\bar{q}_b^i)\,\vec{v}_{b/i}^{\,i} - \frac{T}{m}R^T(\bar{q}_b^i)\,\vec{e}_3$$ $$\dot{\bar{q}}_b^i = \tfrac{1}{2}\,\Omega(\vec{\omega}_{b/i}^b)\,\bar{q}_b^i$$ $$\dot{\vec{\omega}}_{b/i}^{\,b} = J^{-1}\left[-\vec{\omega}_{b/i}^{\,b}\times\left(J\vec{\omega}_{b/i}^{\,b}\right) + \vec{\tau}^b\right]$$系统输入是总推力 $T$ 与 body torque $\vec{\tau}^b$。
8.2 欧拉角形式
$$\dot{\vec{p}}_{b/i}^{\,i} = \vec{v}_{b/i}^{\,i}$$ $$\dot{\vec{v}}_{b/i}^{\,i} = g\vec{e}_3 - R_b^i(\Theta)\, D\, R_b^{iT}(\Theta)\,\vec{v}_{b/i}^{\,i} - \frac{T}{m}R_b^{iT}(\Theta)\,\vec{e}_3$$ $$\dot{\Theta} = S(\Theta)\,\vec{\omega}_{b/i}^{\,b}, \qquad \Theta = (\varphi, \theta, \psi)^T$$ $$\dot{\vec{\omega}}_{b/i}^{\,b} = J^{-1}\left[-\vec{\omega}_{b/i}^{\,b}\times\left(J\vec{\omega}_{b/i}^{\,b}\right) + \vec{\tau}^b\right]$$其中由 $R_v^b(\varphi,\theta,\psi) = R_{v_2}^b(\varphi)\,R_{v_1}^{v_2}(\theta)\,R_v^{v_1}(\psi)$ 得 $R_b^i(\Theta) = \left(R_v^b\right)^T = R_{v_1}^v(\psi)\,R_{v_2}^{v_1}(\theta)\,R_b^{v_2}(\varphi)$：
$$R_b^i(\Theta) = \begin{pmatrix} \cos\psi & -\sin\psi & 0 \\ \sin\psi & \cos\psi & 0 \\ 0 & 0 & 1 \end{pmatrix}\begin{pmatrix} \cos\theta & \sin\varphi\sin\theta & \cos\varphi\sin\theta \\ 0 & \cos\varphi & -\sin\varphi \\ -\sin\theta & \sin\varphi\cos\theta & \cos\varphi\cos\theta \end{pmatrix}$$8.3 小角度线性化
假设偏航任意，横滚与俯仰是小量：$\cos\varphi \approx \cos\theta \approx 1$，$\sin\varphi \approx \varphi$，$\sin\theta \approx \theta$，并忽略二阶小量。
旋转矩阵近似：
$$R_b^i(\Theta) \approx \begin{pmatrix} \cos\psi & -\sin\psi & \theta\cos\psi + \varphi\sin\psi \\ \sin\psi & \cos\psi & \theta\sin\psi - \varphi\cos\psi \\ -\theta & \varphi & 1 \end{pmatrix}$$姿态运动学矩阵近似：
$$S(\Theta) \approx \begin{pmatrix} 1 & 0 & \theta \\ 0 & 1 & -\varphi \\ 0 & \varphi & 1 \end{pmatrix}$$阻力项：假设速度与角度的非线性交叉项近似为 0（如 $v_d \cdot (-\theta\cos\psi - \varphi\sin\psi)$ 这类项），则
$$-RDR^T\vec{v} \approx -gC_d\begin{pmatrix} v_n \\ v_e \\ v_d \end{pmatrix}$$升力项：认为横向上 $T \approx mg$，则
$$-\frac{T}{m}R\,\vec{e}_3 \approx \begin{pmatrix} -g(\theta\cos\psi + \varphi\sin\psi) \\ g(\theta\sin\psi - \varphi\cos\psi) \\ -T/m \end{pmatrix}$$8.4 线性化后的模型
$$\begin{pmatrix} \dot{p}_n \\ \dot{p}_e \\ \dot{p}_d \end{pmatrix} = \begin{pmatrix} v_n \\ v_e \\ v_d \end{pmatrix}$$ $$\begin{pmatrix} \dot{v}_n \\ \dot{v}_e \\ \dot{v}_d \end{pmatrix} = g\begin{pmatrix} -\theta\cos\psi - \varphi\sin\psi \\ \varphi\cos\psi - \theta\sin\psi \\ 1 \end{pmatrix} - gC_d\begin{pmatrix} v_n \\ v_e \\ 0 \end{pmatrix} - \frac{T}{m}\begin{pmatrix} 0 \\ 0 \\ 1 \end{pmatrix}$$阻力项第三分量原稿写 $0$（即 $-gC_d(v_n, v_e, 0)^T$），与 §8.3 末尾的 $-gC_d(v_n, v_e, v_d)^T$ 略有出入，均按原样保留。$$\begin{pmatrix} \dot{\varphi} \\ \dot{\theta} \\ \dot{\psi} \end{pmatrix} = \begin{pmatrix} p \\ q \\ r \end{pmatrix}$$ $$\begin{pmatrix} \dot{p} \\ \dot{q} \\ \dot{r} \end{pmatrix} = \begin{pmatrix} \tau_x/J_x \\ \tau_y/J_y \\ \tau_z/J_z \end{pmatrix}$$
感觉是为了凑线性项而做了许多假设。
8.5 整理为二阶形式（含补偿项）
水平通道：
$$\begin{pmatrix} \ddot{p}_n \\ \ddot{p}_e \end{pmatrix} = -gC_d\begin{pmatrix} \dot{p}_n \\ \dot{p}_e \end{pmatrix} + \underbrace{g\begin{pmatrix} \cos\psi & -\sin\psi \\ \sin\psi & \cos\psi \end{pmatrix}\begin{pmatrix} -\theta \\ \varphi \end{pmatrix}}_{=\,(u_n,\ u_e)^T} + \begin{pmatrix} d_n \\ d_e \end{pmatrix}$$即俯仰 + 横滚驱动平移（$\psi = 0$ 时 $\theta$ 为负 $\Rightarrow$ 向北飞）。$d_*$ 是在线性化过程中的补偿项。
高度通道：
$$\ddot{p}_d = g - \frac{1}{m}T + d_d, \qquad \text{令 } T = m(g + u_d)$$姿态通道：
$$\ddot{\varphi} = \tau_x/J_x + d_\varphi, \qquad \ddot{\theta} = \tau_y/J_y + d_\theta, \qquad \ddot{\psi} = \tau_z/J_z + d_\psi$$