2.2 四旋翼模型线性化

0. 前言

​ 在 2.1 四旋翼物理模型 中，我们建立了四旋翼的完整非线性模型 $\dot{\mathbf{x}}=f(\mathbf{x},\mathbf{u})$。但如果你试着把那个 13 维的非线性方程直接塞给 LQR 求解器，会立刻碰壁：LQR 只能处理线性的 $\dot{x}=Ax+Bu$。

​ 打个比方：2.1 建立的是一张 1:1 的高精度地图，标注了所有地形细节，但在实际导航时，如果你只是在小区周边开车，一张简化过的街道地图反而更方便、更高效。线性化就是"在悬停点附近画一张简化地图"：牺牲远处的精度，换取近处的计算便利和数学上的优雅。

​ 本文的任务是：在悬停平衡点附近对非线性模型进行一阶泰勒展开，得到线性时不变（LTI）状态空间模型，再通过精确离散化方法将其转换为离散时间模型，供数字控制器直接使用。

读完本文后，你将能够：

• 理解一阶泰勒展开在线性化中的数学原理
• 解释为什么线性化时需要从四元数切换到欧拉角，以及两种姿态表示在小角度下的等价性
• 独立推导纯姿态模型（6 维）和全状态模型（12 维）的 $A_c$、$B_c$ 矩阵
• 使用矩阵指数法完成 ZOH 精确离散化，得到 $A_d$、$B_d$
• 了解线性模型的有效范围（小角度 < 30°、小角速度、小推力偏差），知道何时必须转向非线性方法

---

目录

1. 线性化数学基础
2. 从四元数到欧拉角：线性化的必要转换
3. 纯姿态控制模型线性化与离散化
4. 全状态模型线性化与离散化
5. 线性化有效范围与小结

---

1. 线性化数学基础

对于一般的连续时间非线性系统：

$$\dot{\mathbf{x}}=f(\mathbf{x},\mathbf{u})$$

在平衡点 $({\mathbf{x}}_0,{\mathbf{u}}_0)$ 处满足 $f({\mathbf{x}}_0,{\mathbf{u}}_0)=\mathbf{0}$。对其进行一阶泰勒展开：

$$f(\mathbf{x},\mathbf{u})\approx \underset{=\mathbf{0}}{\underbrace{f({\mathbf{x}}_0,{\mathbf{u}}_0)}}+{\frac{\partial f}{\partial \mathbf{x}}|}_{{\mathbf{x}}_0,{\mathbf{u}}_0}(\mathbf{x}-{\mathbf{x}}_0)+{\frac{\partial f}{\partial \mathbf{u}}|}_{{\mathbf{x}}_0,{\mathbf{u}}_0}(\mathbf{u}-{\mathbf{u}}_0)$$

令偏差变量 $\mathrm{\Delta }\mathbf{x}=\mathbf{x}-{\mathbf{x}}_0$，$\mathrm{\Delta }\mathbf{u}=\mathbf{u}-{\mathbf{u}}_0$，得到线性偏差模型：

$$\mathrm{\Delta }\dot{\mathbf{x}}=A\mathrm{\Delta }\mathbf{x}+B\mathrm{\Delta }\mathbf{u}$$

其中系统矩阵 $A$ 和控制矩阵 $B$ 为雅可比矩阵在平衡点处的取值：

$$A=\frac{\partial f}{\partial \mathbf{x}}{|}_{{\mathbf{x}}_0,{\mathbf{u}}_0},B=\frac{\partial f}{\partial \mathbf{u}}{|}_{{\mathbf{x}}_0,{\mathbf{u}}_0}$$

后续为书写简洁，将 $\mathrm{\Delta }\mathbf{x}$ 和 $\mathrm{\Delta }\mathbf{u}$ 直接记为 $\mathbf{x}$ 和 $\mathbf{u}$，理解为相对于平衡点的偏差量。

---

2. 从四元数到欧拉角：线性化的必要转换

2.1 为什么需要切换姿态表示

之前我们使用四元数来表示姿态，原因如下：

• 全局无奇异性（不存在万向节锁问题）
• 适合大规模角度机动的数值仿真
• 旋转组合运算高效

但在平衡点线性化的场景下，四元数存在明显的不便：

• 四元数的四个分量受单位长度约束 $\parallel \mathbf{q}\parallel =1$，在计算雅可比矩阵时需要处理约束条件
• 四元数各分量在平衡点附近的偏差没有十分清晰直观的几何意义

欧拉角在悬停平衡点附近则具有天然优势：

• 三个角度 $\varphi ,\theta ,\psi$ 对应 roll、pitch、yaw 三个直观的旋转自由度
• 小角度假设下，欧拉角运动学简化为 $\dot{\varphi }\approx {\omega }_x$，$\dot{\theta }\approx {\omega }_y$，$\dot{\psi }\approx {\omega }_z$
• 雅可比矩阵的解析形式简洁

2.2 两种表示在小角度下的等价性

在悬停平衡点 $(\varphi =\theta =\psi =0)$ 附近，四元数与欧拉角的对应关系为：

$$\mathbf{q}\approx \left[\begin{array}{c}1\\ \varphi /2\\ \theta /2\\ \psi /2\end{array}\right],\left[\begin{array}{c}\varphi \\ \theta \\ \psi \end{array}\right]\approx 2\left[\begin{array}{c}{q}_1\\ q_2\\ q_3\end{array}\right]$$

两者的运动学在小角度下一阶近似等价。因此，从四元数切换到欧拉角进行线性化，不会损失悬停附近的动态精度。

本文约定： 后续所有推导均采用欧拉角 $[\varphi ,\theta ,\psi {]}^T$ 作为姿态状态量，分别对应滚转角（roll）、俯仰角（pitch）和偏航角（yaw）。

---

3. 纯姿态控制模型线性化与离散化

本节针对纯姿态模型，在悬停平衡点附近进行线性化。

3.1 状态空间与平衡点

状态向量（6 维）：

$$\mathbf{x}=[\varphi ,\theta ,\psi ,{\omega }_x,{\omega }_y,{\omega }_z{]}^T$$

控制向量（4 维）：

$$\mathbf{u}=[t_1,t_2,t_3,t_4{]}^T$$

悬停平衡点：

状态变量	平衡点值	说明
$\varphi ,\theta ,\psi$	$0$	水平姿态
${\omega }_x,{\omega }_y,{\omega }_z$	$0$	无旋转

悬停时每个电机需提供 $1/4$ 的总重力补偿推力。总推力 $F_{\text{total}}=mg$，因此：

$$t_h=\frac{mg}{4}$$

3.2 非线性模型回顾

回顾 2.1 四旋翼物理模型 §2 中的纯姿态非线性模型，本节将其修改为以欧拉角形式表述运动学部分。

姿态运动学（欧拉角形式）：

$$\left[\begin{array}{c}\dot{\varphi }\\ \dot{\theta }\\ \dot{\psi }\end{array}\right]=\left[\begin{array}{ccc}1& \sin \varphi \tan \theta & \cos \varphi \tan \theta \\ 0& \cos \varphi & -\sin \varphi \\ 0& \sin \varphi /\cos \theta & \cos \varphi /\cos \theta \end{array}\right]\left[\begin{array}{c}{\omega }_x\\ {\omega }_y\\ {\omega }_z\end{array}\right]$$

角速度动力学：

$$\left[\begin{array}{c}{\dot{\omega }}_x\\ {\dot{\omega }}_y\\ {\dot{\omega }}_z\end{array}\right]=\left[\begin{array}{c}{\displaystyle \frac{{\tau }_x+(I_{yy}-I_{zz}){\omega }_y{\omega }_z}{I_{xx}}}\\ {\displaystyle \frac{{\tau }_y+(I_{zz}-I_{xx}){\omega }_x{\omega }_z}{I_{yy}}}\\ {\displaystyle \frac{{\tau }_z+(I_{xx}-I_{yy}){\omega }_x{\omega }_y}{I_{zz}}}\end{array}\right]$$$$\begin{array}{rl}{\tau }_x& =d(t_1-t_2-t_3+t_4)\\ {\tau }_y& =d(-t_1-t_2+t_3+t_4)\\ {\tau }_z& =k(-t_1+t_2-t_3+t_4)\end{array}$$

3.3 线性化过程

3.3.1 运动学线性化（小角度近似）

在悬停平衡点 $(\varphi ,\theta ,\psi )\approx (0,0,0)$ 处：

$$\begin{array}{rl}\sin \varphi \approx \varphi ,& \cos \varphi \approx 1\\ \sin \theta \approx \theta ,& \cos \theta \approx 1\\ \tan \theta \approx \theta \end{array}$$

运动学矩阵简化为：

$$\left[\begin{array}{c}\dot{\varphi }\\ \dot{\theta }\\ \dot{\psi }\end{array}\right]\approx \left[\begin{array}{ccc}1& \varphi \theta & \theta \\ 0& 1& -\varphi \\ 0& \varphi & 1\end{array}\right]\left[\begin{array}{c}{\omega }_x\\ {\omega }_y\\ {\omega }_z\end{array}\right]\approx \left[\begin{array}{c}{\omega }_x\\ {\omega }_y\\ {\omega }_z\end{array}\right]+\mathcal{O}(\varphi \omega ,\theta \omega )$$

忽略高阶项，得到十分直观的线性化姿态运动学：

$$\dot{\varphi }\approx {\omega }_x,\dot{\theta }\approx {\omega }_y,\dot{\psi }\approx {\omega }_z$$

3.3.2 动力学线性化

在平衡点 $({\omega }_x,{\omega }_y,{\omega }_z)=(0,0,0)$ 处，陀螺耦合项 ${\omega }_i{\omega }_j$ 为二阶小量可以忽略，因此角加速度可简化如下：

$$\begin{array}{rl}{\dot{\omega }}_x& \approx \frac{{\tau }_x}{I_{xx}}\\ {\dot{\omega }}_y& \approx \frac{{\tau }_y}{I_{yy}}\\ {\dot{\omega }}_z& \approx \frac{{\tau }_z}{I_{zz}}\end{array}$$

3.3.3 控制矩阵

力矩与推力的映射本身即为线性关系，无需近似：

$$\left[\begin{array}{c}{\tau }_x\\ {\tau }_y\\ {\tau }_z\end{array}\right]=\underset{M_{\tau }}{\underbrace{\left[\begin{array}{cccc}d& -d& -d& d\\ -d& -d& d& d\\ -k& k& -k& k\end{array}\right]}}\left[\begin{array}{c}{t}_1\\ t_2\\ t_3\\ t_4\end{array}\right]$$

3.4 连续时间状态空间模型

综合以上线性化结果，得到纯姿态线性模型 $\dot{\mathbf{x}}=A_c\mathbf{x}+B_c\mathbf{u}$：

$$A_c={\left[\begin{array}{cc}{0}_{3\times 3}& I_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 3}\end{array}\right]}_{6\times 6}$$$$B_c={\left[\begin{array}{c}{0}_{3\times 4}\\ M_{\text{mix}}\end{array}\right]}_{6\times 4}$$

其中力矩混合矩阵 $M_{\text{mix}}$ 将推力直接映射到角加速度：

$$M_{\text{mix}}=\left[\begin{array}{cccc}d/I_{xx}& -d/I_{xx}& -d/I_{xx}& d/I_{xx}\\ -d/I_{yy}& -d/I_{yy}& d/I_{yy}& d/I_{yy}\\ -k/I_{zz}& k/I_{zz}& -k/I_{zz}& k/I_{zz}\end{array}\right]$$

3.5 精确离散化

对于连续系统 $\dot{\mathbf{x}}=A_c\mathbf{x}+B_c\mathbf{u}$，采用矩阵指数精确离散化方法，得到离散时间模型：

$${\mathbf{x}}_{k+1}=A_d{\mathbf{x}}_k+B_d{\mathbf{u}}_k$$

步骤：

1. 构造增广矩阵：

$$M=\left[\begin{array}{cc}{A}_c& B_c\\ 0_{4\times 6}& 0_{4\times 4}\end{array}\right]\in {\mathbb{R}}^{10\times 10}$$

2. 计算矩阵指数：

$$M_d=e^{M\cdot T_s}$$

​ 其中 $T_s$ 为采样周期。

3. 提取离散系统矩阵：

$$\begin{array}{rl}{A}_d& =M_d[0:6,0:6]\in {\mathbb{R}}^{6\times 6}\\ B_d& =M_d[0:6,6:10]\in {\mathbb{R}}^{6\times 4}\end{array}$$

矩阵指数离散化在采样周期内对连续动力学进行精确积分，不引入额外的离散化近似误差，对应零阶保持（ZOH）假设下的精确解。

---

4. 全状态模型线性化与离散化

4.1 状态空间与平衡点

状态向量（12 维）：

$$\mathbf{x}=[\varphi ,\theta ,\psi ,{\omega }_x,{\omega }_y,{\omega }_z,x,y,z,v_x,v_y,v_z{]}^T$$

状态	符号	含义	单位
欧拉角	$\varphi ,\theta ,\psi$	滚转、俯仰、偏航角	rad
角速度	${\omega }_x,{\omega }_y,{\omega }_z$	机体坐标系角速度	rad/s
位置	$x,y,z$	世界坐标系位置	m
线速度	$v_x,v_y,v_z$	世界坐标系速度	m/s

控制向量（4 维）：

$$\mathbf{u}=[t_1,t_2,t_3,t_4{]}^T$$

采用增量控制形式，以悬停推力 $t_h=mg/4$ 为基准：

$$\mathrm{\Delta }{t}_i=t_i-t_h$$

在悬停平衡点处 $\mathrm{\Delta }{t}_i=0$。

悬停平衡条件：

变量	平衡点值	说明
$\varphi ,\theta ,\psi$	$0$	水平姿态
${\omega }_x,{\omega }_y,{\omega }_z$	$0$	无旋转
$x,y,z$	任意	位置无约束
$v_x,v_y,v_z$	$0$	静止悬停
$\mathrm{\Delta }{t}_1,\dots ,\mathrm{\Delta }{t}_4$	$0$	悬停推力

4.2 非线性模型回顾

回顾 2.1 四旋翼物理模型 §3 中建立的全状态非线性模型，此处以欧拉角形式表述平动动力学部分。

姿态运动学（欧拉角形式）：

$$\left[\begin{array}{c}\dot{\varphi }\\ \dot{\theta }\\ \dot{\psi }\end{array}\right]=\left[\begin{array}{ccc}1& \sin \varphi \tan \theta & \cos \varphi \tan \theta \\ 0& \cos \varphi & -\sin \varphi \\ 0& \sin \varphi /\cos \theta & \cos \varphi /\cos \theta \end{array}\right]\left[\begin{array}{c}{\omega }_x\\ {\omega }_y\\ {\omega }_z\end{array}\right]$$

姿态动力学（同 §3.2）：

$$\begin{array}{rl}{\dot{\omega }}_x& =\frac{{\tau }_x+(I_{yy}-I_{zz}){\omega }_y{\omega }_z}{I_{xx}}\\ {\dot{\omega }}_y& =\frac{{\tau }_y+(I_{zz}-I_{xx}){\omega }_x{\omega }_z}{I_{yy}}\\ {\dot{\omega }}_z& =\frac{{\tau }_z+(I_{xx}-I_{yy}){\omega }_x{\omega }_y}{I_{zz}}\end{array}$$

位置运动学：

$$\dot{x}=v_x,\dot{y}=v_y,\dot{z}=v_z$$

平动动力学（完整非线性形式）：

$$\left[\begin{array}{c}{\dot{v}}_x\\ {\dot{v}}_y\\ {\dot{v}}_z\end{array}\right]={\mathbf{R}}_b^w(\varphi ,\theta ,\psi )\left[\begin{array}{c}0\\ 0\\ {\displaystyle \frac{F_{\text{total}}}{m}}\end{array}\right]-\left[\begin{array}{c}0\\ 0\\ g\end{array}\right]$$

其中旋转矩阵 ${\mathbf{R}}_b^w$（欧拉角参数化）为：

$${\mathbf{R}}_b^w=\left[\begin{array}{ccc}c\theta c\psi & s\varphi s\theta c\psi -c\varphi s\psi & c\varphi s\theta c\psi +s\varphi s\psi \\ c\theta s\psi & s\varphi s\theta s\psi +c\varphi c\psi & c\varphi s\theta s\psi -s\varphi c\psi \\ -s\theta & s\varphi c\theta & c\varphi c\theta \end{array}\right]$$

此旋转矩阵与之前的四元数旋转矩阵描述同一几何变换，仅参数化方式不同。

4.3 线性化过程

4.3.1 小角度近似

在悬停平衡点 $(\varphi ,\theta ,\psi )\approx (0,0,0)$ 处：

$$\sin \alpha \approx \alpha ,\cos \alpha \approx 1(\alpha =\varphi ,\theta ,\psi )$$

旋转矩阵简化为：

$${\mathbf{R}}_b^w\approx \left[\begin{array}{ccc}1& -\psi & \theta \\ \psi & 1& -\varphi \\ -\theta & \varphi & 1\end{array}\right]$$

4.3.2 平动动力学线性化

总推力以悬停推力为基准展开：

$$F_{\text{total}}=t_1+t_2+t_3+t_4=mg+\sum _{i=1}^4\mathrm{\Delta }{t}_i$$

世界坐标系下的加速度：

$$\left[\begin{array}{c}{\dot{v}}_x\\ {\dot{v}}_y\\ {\dot{v}}_z\end{array}\right]=\frac{F_{\text{total}}}{m}\left[\begin{array}{c}\theta \\ -\varphi \\ 1\end{array}\right]-\left[\begin{array}{c}0\\ 0\\ g\end{array}\right]+\text{高阶项}$$

代入 $F_{\text{total}}=mg+\sum \mathrm{\Delta }{t}_i$ 并忽略二阶小量（$\theta \sum \mathrm{\Delta }{t}_i$ 等项）：

$$\begin{array}{rl}{\dot{v}}_x& \approx g\theta \\ {\dot{v}}_y& \approx -g\varphi \\ {\dot{v}}_z& \approx \frac{1}{m}\sum _{i=1}^4\mathrm{\Delta }{t}_i\end{array}$$

补充说明：

• 水平加速度通过姿态倾斜（$\varphi ,\theta$）产生，这是四旋翼作为一个欠驱动系统能够实现水平位置速度控制的唯一方式。
• 垂直加速度通过总推力变化直接控制，因此高度与姿态控制可部分解耦。

4.3.3 姿态运动学与动力学线性化

与纯姿态情况（§3.3.1–§3.3.2）完全一致：

$$\dot{\varphi }\approx {\omega }_x,\dot{\theta }\approx {\omega }_y,\dot{\psi }\approx {\omega }_z$$$${\dot{\omega }}_x\approx \frac{{\tau }_x}{I_{xx}},{\dot{\omega }}_y\approx \frac{{\tau }_y}{I_{yy}},{\dot{\omega }}_z\approx \frac{{\tau }_z}{I_{zz}}$$

4.4 连续时间线性模型

全状态线性模型 $\dot{\mathbf{x}}=A_c\mathbf{x}+B_c\mathbf{u}$：

系统矩阵 $A_c$（12×12）

$$A_c=\left[\begin{array}{cccc}{0}_{3\times 3}& I_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& I_{3\times 3}\\ A_{41}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}\end{array}\right]$$

其中 $A_{41}$ 为姿态-加速度耦合矩阵，体现了 ${\dot{v}}_x=g\theta$ 和 ${\dot{v}}_y=-g\varphi$ 的耦合关系：

$$A_{41}=\left[\begin{array}{ccc}0& g& 0\\ -g& 0& 0\\ 0& 0& 0\end{array}\right]$$

控制矩阵 $B_c$（12×4）

$$B_c=\left[\begin{array}{c}{0}_{3\times 4}\\ M_{\text{torque}}\\ 0_{3\times 4}\\ B_{\text{thrust}}\end{array}\right]$$

力矩混合矩阵 $M_{\text{torque}}$（与 §3.4 中 $M_{\text{mix}}$ 相同，仅名称区分以强调全状态语境）：

$$M_{\text{torque}}=\left[\begin{array}{cccc}d/I_{xx}& -d/I_{xx}& -d/I_{xx}& d/I_{xx}\\ -d/I_{yy}& -d/I_{yy}& d/I_{yy}& d/I_{yy}\\ -k/I_{zz}& k/I_{zz}& -k/I_{zz}& k/I_{zz}\end{array}\right]$$

推力混合矩阵（注意此处使用增量控制 $\mathrm{\Delta }{t}_i$，悬停推力 $t_h$ 已在平衡点中抵消）：

$$B_{\text{thrust}}=\left[\begin{array}{cccc}0& 0& 0& 0\\ 0& 0& 0& 0\\ 1/m& 1/m& 1/m& 1/m\end{array}\right]$$

$B_{\text{thrust}}$ 仅第三行非零，对应 ${\dot{v}}_z=\frac{1}{m}\sum \mathrm{\Delta }{t}_i$。水平速度通道（第一、二行）通过 $A_{41}$ 中的 $g$ 耦合项间接由姿态控制。

4.5 零阶保持离散化

对于需要离散预测模型的控制方法，对连续模型进行零阶保持（ZOH）离散化。

离散化方法与 §3.5 的矩阵指数法原理相同，区别在于此处系统矩阵维度为 $16\times 16$（12 维状态 + 4 维控制）：

$$M=\left[\begin{array}{cc}{A}_c& B_c\\ 0_{4\times 12}& 0_{4\times 4}\end{array}\right]\in {\mathbb{R}}^{16\times 16}$$$$M_d=e^{M\cdot T_s},A_d=M_d[0:12,0:12],B_d=M_d[0:12,12:16]$$

---

5. 线性化有效范围与小结

5.1 线性模型的有效条件

线性化模型的精度依赖于以下假设的成立程度：

假设条件	推荐范围	超出影响
小角度假设	$|\varphi |,|\theta |,|\psi |<{30}^{\circ }$	姿态角 > 30° 时运动学线性化误差 > 15%
小角速度假设	$|{\omega }_{x,y,z}|$ 较小	角速度 > 1 rad/s 时陀螺耦合不可忽略
推力偏差小	$|\mathrm{\Delta }{t}_i|\ll t_h$	大推力变化导致平动非线性误差增大

对于超出上述范围的大机动飞行，需要使用非线性控制方法如NMPC等。

线性化模型建立后，可应用于以下控制方法：

• LQR 最优控制：基于 $A_d,B_d$ 求解离散代数 Riccati 方程
• MPC 模型预测控制：将线性模型作为预测模型，构建 QP 优化问题
• 卡尔曼滤波：线性模型作为状态估计的预测模型

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