1. 牛顿-欧拉方程 —— 动力学模型的基石

这是描述刚体运动最根本的物理定律。它把运动分成了两部分：

牛顿第二定律（平动）

$$F=m\cdot a$$

• F：作用在四旋翼质心上所有外力的合力矢量（重力 + 四个旋翼的总升力）。
• m：四旋翼的质量。
• a：质心运动的加速度矢量。

它回答了“飞机会怎么移动”。

欧拉方程（转动）

$$M=I\cdot \alpha +\omega \times (I\cdot \omega )$$

• M：作用在四旋翼上的所有外力矩的合力矩矢量（由四个旋翼升力差异产生）。
• I：四旋翼的惯性张量（一个 3x3 矩阵，可以简单理解为描述物体绕各个轴转动难易程度的“转动质量”）。

  为什么是 3x3 矩阵？（详细解释）

  在三维空间中，物体不仅可以绕 X、Y、Z 轴（主轴）旋转，还可能因为质量分布不均匀，在绕 X 轴旋转时产生试图让物体绕 Y 轴或 Z 轴偏转的力矩（惯性积）。

  惯性张量 $I$ 的完整形式如下：
  $$I=\left[\begin{array}{ccc}{I}_{xx}& -I_{xy}& -I_{xz}\\ -I_{yx}& I_{yy}& -I_{yz}\\ -I_{zx}& -I_{zy}& I_{zz}\end{array}\right]$$

  • 对角线元素 ($I_{xx},I_{yy},I_{zz}$)：转动惯量。表示绕 X、Y、Z 轴旋转的难易程度。例如，$I_{xx}$ 越大，绕 X 轴加速旋转越费力。
  • 非对角线元素 ($I_{xy},I_{xz},\dots$)：惯性积。表示质量分布的不对称性。

  举个例子：
  假设一个理想的四旋翼，它的质量完全对称地分布在 X 和 Y 轴上（像一个完美的“十”字），且重心在几何中心。那么所有的惯性积（非对角项）都为 0，惯性张量简化为对角矩阵：
  $$I_{ideal}=\left[\begin{array}{ccc}{I}_{xx}& 0& 0\\ 0& I_{yy}& 0\\ 0& 0& I_{zz}\end{array}\right]$$
  这时候，绕 X 轴的力矩只会产生绕 X 轴的角加速度，不会“串台”影响到 Y 轴或 Z 轴，控制起来最简单。但实际无人机可能挂载了不对称的相机或电池，导致非对角项不为 0，产生耦合效应。

• α：角加速度矢量。
• ω：角速度矢量。
• ω × (I * ω)：哥氏项/离心项。这是旋转坐标系特有的项，就像旋转的陀螺会进动一样。

  详解：XY 平面旋转（偏航 ${\omega }_z$）的影响

  很多初学者会问：哥氏项中 XY 平面的旋转（即绕 Z 轴旋转）会影响绕 X 和 Y 的力矩吗？答案是肯定的。

  如果我们将 $\omega \times (I\cdot \omega )$ 展开（假设 $I$ 为对角矩阵），会得到：
  $$M_{cor}=\left[\begin{array}{c}{\omega }_y{\omega }_z(I_{zz}-I_{yy})\\ {\omega }_x{\omega }_z(I_{xx}-I_{zz})\\ {\omega }_x{\omega }_y(I_{yy}-I_{xx})\end{array}\right]$$

  • X 轴力矩：包含 ${\omega }_z$。当飞机同时进行俯仰（${\omega }_y$）和偏航（${\omega }_z$）时，会产生绕 X 轴的哥氏力矩。
  • Y 轴力矩：包含 ${\omega }_z$。当飞机同时进行滚转（${\omega }_x$）和偏航（${\omega }_z$）时，会产生绕 Y 轴的哥氏力矩。

  所以，XY 平面内的旋转（${\omega }_z$）不仅影响 Z 轴力矩，还会通过耦合作用显著影响 X 和 Y 轴的力矩，这是高机动飞行控制中必须考虑的因素。

它回答了“飞机会怎么转动”。

---

小结：牛顿-欧拉方程直接写出了力/力矩与运动（加速度）的关系，构成了原始的非线性动力学模型。

常用四旋翼动力学方程组

在忽略空气阻力、假设机体对称且重心位于几何中心（惯性张量 $I$ 为对角阵）的理想情况下，四旋翼的非线性动力学模型通常写作以下形式（ENU 坐标系）：

1. 位置动力学（牛顿方程）

描述四旋翼在惯性系下的平动：
$$\{\begin{array}{rl}\ddot{x}& =\frac{U_1}{m}(\cos \varphi \sin \theta \cos \psi +\sin \varphi \sin \psi )\\ \ddot{y}& =\frac{U_1}{m}(\cos \varphi \sin \theta \sin \psi -\sin \varphi \cos \psi )\\ \ddot{z}& =\frac{U_1}{m}(\cos \varphi \cos \theta )-g\end{array}$$

• $x,y,z$：惯性系下的位置。
• $\varphi ,\theta ,\psi$：欧拉角（滚转、俯仰、偏航）。
• $U_1$：总升力（垂直于机体平面向上）。

2. 姿态动力学（欧拉方程）

描述四旋翼在机体系下的转动：
$$\{\begin{array}{rl}\dot{p}& =\frac{1}{I_{xx}}[U_2+(I_{yy}-I_{zz})qr]\\ \dot{q}& =\frac{1}{I_{yy}}[U_3+(I_{zz}-I_{xx})pr]\\ \dot{r}& =\frac{1}{I_{zz}}[U_4+(I_{xx}-I_{yy})pq]\end{array}$$

• $p,q,r$：机体坐标系下的角速度。
• $U_2,U_3,U_4$：分别为绕机体 X、Y、Z 轴的控制力矩。

2. 线性化模型基础 —— 从非线性到线性的桥梁

四旋翼的原始动力学模型是非线性的（包含 $\sin ,\cos$ 和角速度耦合项），难以直接用于设计线性控制器（如 PID）。因此，我们需要将其线性化。

2.1 小扰动理论与线性化 —— “化曲为直”的数学魔法

这是处理非线性系统的核心思想，也是将复杂动力学模型转化为简单PID控制器能够处理形式的关键一步。

核心思想：在平衡点附近，一切皆线性

• 它是什么：小扰动理论假设系统在一个稳定的平衡点（Equilibrium Point，例如完美悬停）附近工作。在此状态下，所有的变量（如角度、角速度、控制输入）都只在它们的平衡值附近做微小的波动。
• 为什么用它：原始的动力学方程充满了 sin, cos 和变量相乘的项，这些是非线性的，难以直接设计稳定、可预测的控制器。但在一个极小的局部范围内，任何平滑的曲线都可以被它的切线很好地近似。线性化就是找到并使用这条“切线方程”来替代原始的“曲线方程”。
• 关键近似（小角度假设）：当飞机接近悬停时，姿态角 $\varphi$ (滚转) 和 $\theta$ (俯仰) 都非常小（接近0）。在数学上，当角度 $x$ 以弧度为单位且非常小时：
  • $\sin (x)\approx x$
  • $\cos (x)\approx 1$
  • $x\cdot y\approx 0$ （如果x和y都是小量，它们的乘积是二阶小量，可以忽略）

实战：四旋翼悬停模型的线性化推导

让我们以四旋翼在空中完美悬停为场景，一步步看这些近似如何简化我们的非线性方程。

1. 定义平衡状态

• 状态量 ($x_0$): 姿态水平 ($\varphi =0,\theta =0$)，无旋转运动 ($p=q=r=0$)。
• 控制量 ($u_0$): 各个轴向的控制力矩为零 ($U_2=0,U_3=0,U_4=0$)，总升力 $U_1$ 刚好抵消重力，即 $U_1=mg$。

2. 引入扰动
现在，我们考虑飞机在平衡点附近的一个微小偏移。任何变量都可以写成“平衡值 + 扰动量”的形式：

• $\varphi =0+\Delta \varphi$
• $\theta =0+\Delta \theta$
• $U_1=mg+\Delta U_1$
• $U_2=0+\Delta U_2$
• …等等

3. 将扰动代入非线性方程并简化

案例 1：高度通道 (Z轴)

原始方程:
$$\ddot{z}=\frac{U_1}{m}(\cos \varphi \cos \theta )-g$$
将 $U_1=mg+\Delta U_1,\varphi =\Delta \varphi ,\theta =\Delta \theta$ 代入：
$$\ddot{z}=\frac{mg+\Delta U_1}{m}(\cos (\Delta \varphi )\cos (\Delta \theta ))-g$$
应用小角度近似 $\cos (\Delta \varphi )\approx 1$ 和 $\cos (\Delta \theta )\approx 1$:
$$\ddot{z}\approx (\frac{mg}{m}+\frac{\Delta U_1}{m})\cdot 1\cdot 1-g$$
$$\ddot{z}\approx g+\frac{\Delta U_1}{m}-g$$
最终得到线性化模型：
$$\ddot{z}=\frac{1}{m}\Delta U_1$$
结论：在悬停附近，垂直加速度 $\ddot{z}$ 与总升力的变化量 $\Delta U_1$ 成正比。这完美符合牛顿第二定律 $F=ma$ 的直觉，形式简单，极易控制。

案例 2：水平位置通道 (X轴)

原始方程（为简化，设偏航角 $\psi =0$）:
$$\ddot{x}=\frac{U_1}{m}(\cos \varphi \sin \theta \cos \psi +\sin \varphi \sin \psi )\stackrel{\psi =0}{\to }\frac{U_1}{m}\cos \varphi \sin \theta$$
代入扰动量：
$$\ddot{x}=\frac{mg+\Delta U_1}{m}\cos (\Delta \varphi )\sin (\Delta \theta )$$
应用小角度近似 $\cos (\Delta \varphi )\approx 1$, $\sin (\Delta \theta )\approx \Delta \theta$ 和 $\Delta U_1\cdot \Delta \theta \approx 0$ (二阶小量忽略不计):
$$\ddot{x}\approx (\frac{mg}{m}+\frac{\Delta U_1}{m})\cdot 1\cdot \Delta \theta$$
$$\ddot{x}\approx (g+\frac{\Delta U_1}{m})\Delta \theta \approx g\cdot \Delta \theta +\frac{\Delta U_1}{m}\Delta \theta$$
再次忽略二阶小量 $(\Delta U_1\cdot \Delta \theta )$:
$$\ddot{x}\approx g\cdot \theta$$
结论：在悬停附近，水平加速度 $\ddot{x}$ 近似与俯仰角 $\theta$ 成正比。这同样非常符合直觉：飞机想往前飞，就需要向前低头。这个关系是串级PID中，位置环的输出（期望角度）能够控制飞机位置的根本原因。

案例 3：姿态通道（滚转 Roll）

原始方程:
$$\dot{p}=\frac{1}{I_{xx}}[U_2+(I_{yy}-I_{zz})qr]$$
在小扰动下, $p,q,r$ 都是小量，它们的乘积 $q\cdot r$ 是二阶小量，可以直接忽略。$U_2$ 本身就是扰动输入 $\Delta U_2$。同时，在小角度下，机体角速度的变化率 $\dot{p}$ 等于欧拉角加速度 $\ddot{\varphi }$。
$$\ddot{\varphi }\approx \frac{1}{I_{xx}}[U_2+0]$$
最终得到线性化模型：
$$\ddot{\varphi }=\frac{1}{I_{xx}}{U}_2$$
结论：滚转角加速度 $\ddot{\varphi }$ 与滚转力矩 $U_2$ 成正比。这等价于旋转运动中的 $M=I\alpha$。同样，俯仰和偏航也有类似结论：

• $\ddot{\theta }=\frac{1}{I_{yy}}{U}_3$
• $\ddot{r}=\frac{1}{I_{zz}}{U}_4$

通过线性化，我们把一组复杂的非线性耦合方程，解耦成了几个可以独立控制的、简单的二阶线性系统。这就为后续设计简单高效的PID控制器铺平了道路。

---

3. 从线性模型到 PID 控制 —— 控制器的自然演化

我们已经有了四旋翼的线性化模型。现在，让我们看看 PID 是如何自然而然地从这些公式中“长”出来的。

3.1 比例控制 § —— 引入“虚拟弹簧”

以姿态控制为例。我们的线性模型是 $\ddot{\varphi }=\frac{1}{I_{xx}}{U}_2$。
为了方便理解，令 $b=1/I_{xx}$，则方程为 $\ddot{\varphi }=b\cdot U_2$。

目标：无论飞机歪到什么角度 $\varphi$，都要把它拉回水平 ($\varphi =0$)。
控制律：我们施加一个与角度成正比的反向力矩。
$$U_2=-K_p\cdot \varphi$$

代入方程：
$$\ddot{\varphi }=b(-K_p\varphi )⟹\ddot{\varphi }+(b{K}_p)\varphi =0$$

物理意义：
这是一个标准的简谐振动方程（类似于弹簧振子 $\ddot{x}+kx=0$）。

• $K_p$ 实际上是在给四旋翼装上一个虚拟弹簧，刚度为 $b{K}_p$。
• 现象：如果你推一下飞机，它会围绕水平面左右摇摆，永远停不下来（无阻尼振荡）。

3.2 微分控制 (D) —— 引入“虚拟阻尼”

目标：消除 P 控制带来的振荡，让飞机稳稳停住。
控制律：增加一项与角速度 $\dot{\varphi }$ 成正比的反向力矩（阻尼）。
$$U_2=-K_p\varphi -K_d\dot{\varphi }$$

代入方程：
$$\ddot{\varphi }=b(-K_p\varphi -K_d\dot{\varphi })⟹\ddot{\varphi }+(b{K}_d)\dot{\varphi }+(b{K}_p)\varphi =0$$

物理意义：
这是一个阻尼振动方程。

• $K_d$ 实际上是在给四旋翼装上一个虚拟空气阻尼。
• 现象：振荡能量被 D 项消耗，飞机在受到扰动后能迅速回到水平并静止。
• 结论：这就是 PD 控制器。对于四旋翼的姿态环，这通常就够了。

3.3 积分控制 (I) —— 消除“稳态误差”

以高度控制为例。线性模型为 $\Delta \ddot{z}=\frac{1}{m}\Delta U_1$。
注意：这里的 $\Delta U_1$ 是相对于悬停油门 ($mg$) 的增量。

场景：假设我们要飞到高度 $z_{target}$。
如果我们只用 PD 控制：$U_1=mg+K_p(z_{target}-z)-K_d\dot{z}$。
但如果电池电压下降了，或者我们估算的重量 $m$ 不准（比实际轻了），那么 $mg$ 这个前馈量就不够了。
飞机为了维持悬停，必须产生额外的升力，这就需要 $z_{target}-z$ 保持一个非零的误差（稳态误差）来提供这部分升力。

控制律：
$$\Delta U_1=K_{p}e+K_d\dot{e}+K_i\int edt$$

物理意义：

• 只要高度有误差，积分项 $\int edt$ 就会不断积累。
• 最终，积分项会自动“学习”出电池电压下降或重量增加带来的偏差，并产生一个恒定的补偿力。
• 此时误差 $e$ 可以回到 0。

总结：

• P (比例)：提供虚拟刚度，决定回正的快慢。
• D (微分)：提供虚拟阻尼，决定稳定性。
• I (积分)：提供误差记忆，自动修正模型偏差（如重量估算错误）。

3.5 深入理解闭环系统与根轨迹分析

上一节我们通过求解特征方程的根（极点）来判断系统的收敛性。本节将进一步阐释闭环传递函数的意义，并引入一种更强大的图形化工具——根轨迹法，来直观地理解控制器参数如何影响系统的稳定性。

3.5.1 闭环传递函数的再认识

在3.4节中，我们推导了系统的闭环传递函数为：
$$H(s)=\frac{Y(s)}{R(s)}=\frac{C(s)G(s)}{1+C(s)G(s)}$$

• 它是什么？
  闭环传递函数是整个反馈控制系统的总输入-输出模型。它将我们最关心的两个量——“我们期望的目标” $R(s)$ (Reference) 和“系统实际的输出” $Y(s)$ (Output) 直接关联起来。它是一个高度浓缩的数学表达式，内部已经包含了控制器 $C(s)$、被控对象 $G(s)$ 以及反馈回路本身的所有动态特性。

• 它的意义是什么？
  闭环传递函数 $H(s)$ 是分析和设计控制系统的核心，因为它能告诉我们关于系统性能的一切：

  1. 稳定性（收敛性）：如前所述，其分母的根（极点）的位置决定了系统在受到扰动后，其自身响应是衰减、发散还是振荡。
  2. 动态性能：它描述了系统从一个状态过渡到另一个状态的“过程”好坏。例如，当目标值突然改变时（阶跃响应），系统输出是响应迅速还是迟缓？会产生多大的超调？需要多长时间才能稳定下来？这些性能指标（上升时间、超调量、调节时间）都与闭环传递函数的极点和零点（分子多项式的根）位置密切相关。
  3. 稳态性能：它描述了系统在过渡过程结束后，能否精确地达到期望值。通过对闭环传递函数应用终值定理，我们可以计算出系统的稳态误差，即 lim(t->∞) e(t)。这能回答“控制器最终能否完全消除误差？”这个问题，对于需要高精度控制的系统（如无人机定点悬停）至关重要。

3.5.2 使用根轨迹法 (Root Locus) 分析收敛性

根轨迹法是一种绝妙的图形化分析工具。它清晰地展示了当一个控制器参数（通常是增益 $K$）从0变化到无穷大时，系统的闭环极点在复平面上是如何移动的。这使得我们无需反复求解不同增益下的特征方程，就能直观地看到增益对系统稳定性和动态性能的影响。

基本原理：根轨迹绘制的是满足特征方程 $1+K\cdot L(s)=0$ 的所有根 $s$ 的集合，其中 $L(s)$ 是系统的开环传递函数，K是可变增益。

案例分析1：P控制器的根轨迹

• 特征方程: $s^2+b{K}_p=0⟹1+K_p\cdot \frac{b}{s^2}=0$。
• 开环传递函数: $L(s)=b/s^2$。它在原点有两个重合的开环极点。
• 根轨迹图分析:
  • 轨迹的起点是开环极点（$K_p=0$时）。这里是原点的两个极点。
  • 随着增益 $K_p$ 从0开始增大，这两个闭环极点从原点分离，一个向上，一个向下，并始终沿着虚轴移动。
• 结论:
  根轨迹图直观地显示，无论比例增益 $K_p$ 取何值（只要大于0），闭环极点永远停留在虚轴上，实部永远为0。这再次证明，纯P控制器无法使系统稳定收敛，只能产生持续振荡。

案例分析2：PD控制器的根轨迹

• 特征方程: $s^2+b{K}_{d}s+b{K}_p=0$。为了使用根轨迹法，我们必须选择一个参数作为可变增益 $K$，并固定其他参数。这里我们固定 $K_d$，将 $K_p$ 作为可变增益。
• 方程变形: $s^2+b{K}_{d}s+b{K}_p=0⟹1+K_p\cdot \frac{b}{s^2+b{K}_{d}s}=0$
• 开环传递函数: $L(s)=\frac{b}{s(s+b{K}_d)}$。它有一个开环极点在原点 $s=0$，另一个在 $s=-b{K}_d$。
• 根轨迹图分析:
  • 轨迹的起点是 $s=0$ 和 $s=-b{K}_d$。
  • 随着增益 $K_p$ 从0开始增大，两个闭环极点分别从这两个开环极点出发，沿着实轴相向移动。
  • 它们在实轴上的某一点（分离点，具体为 $s=-b{K}_d/2$）相遇，然后分离。
  • 分离后，一个极点向上进入左上半平面，另一个极点向下进入左下半平面，最终趋近于垂直于实轴的渐近线。
• 结论:
  这次的根轨迹图告诉我们一个至关重要的信息：只要 $K_d>0$，使得一个开环极点被“挪”到了左半平面，那么无论比例增益 $K_p$ 如何从0开始增大，所有的根轨迹都始终位于复平面的左半部分。这从图形上强有力地证明了，D项的引入从根本上保证了系统的稳定性。我们还可以看出，随着 $K_p$ 的增大（极点在轨迹上移动），极点的虚部会变大，说明系统的振荡性会增强，但它始终是收敛的。

3.4 闭环系统收敛性分析（传递函数法）

为了严谨地分析系统为何在PD控制下能够收敛，而在纯P控制下不能，我们引入控制理论中的传递函数法。我们以滚转通道（$\varphi$）为例。

为什么极点位置决定收敛性？

这个问题的核心在于拉普拉斯反变换，它将频域中的传递函数转换回时域中的系统响应。

1. 传递函数与极点：一个线性系统的闭环传递函数 $H(s)$ 可以通过部分分式展开，分解为一系列基本项的和。对于一个单极点 $p_1$，其在传递函数中的形式为 $\frac{A_1}{s-p_1}$。
   $$H(s)=\frac{\text{分子多项式}}{\text{分母多项式}}=\frac{\text{分子多项式}}{(s-p_1)(s-p_2)\cdots }=\frac{A_1}{s-p_1}+\frac{A_2}{s-p_2}+\cdots$$
   这里的 $p_1,p_2,\dots$ 就是系统的极点。

2. 从极点到时域响应：拉普拉斯反变换的关键规则是：
   $${\mathcal{L}}^{-1}\left\{\frac{A}{s-p}\right\}=A{e}^{pt}$$
   这意味着系统的总响应（对于一个脉冲输入的自由响应）是每个极点对应的指数函数之和：
   $$y(t)=A_1{e}^{p_{1}t}+A_2{e}^{p_{2}t}+\cdots$$

3. 极点实部与系统趋势：现在，我们将极点写成复数形式 $p=\sigma +j\omega$，其中 $\sigma$ 是实部，$\omega$ 是虚部。

   • 对应的时域项为 $A{e}^{(\sigma +j\omega )t}=A{e}^{\sigma t}{e}^{j\omega t}$。
   • 根据欧拉公式 $e^{j\omega t}=\cos (\omega t)+j\sin (\omega t)$，虚部 $j\omega$ 决定了响应的振荡部分。
   • 而实部 $\sigma$ 决定了 $e^{\sigma t}$ 这一项，它作为整个响应的包络线，直接决定了系统的长期趋势：
     • 当 $\sigma <0$ (极点在左半平面): $e^{\sigma t}$ 是一个衰减的指数函数（如 $e^{-2t}$）。系统的自由响应会随时间趋近于零。系统收敛，是稳定的。
     • 当 $\sigma >0$ (极点在右半平面): $e^{\sigma t}$ 是一个增长的指数函数（如 $e^{2t}$）。系统的自由响应会无限增大。系统发散，是不稳定的。
     • 当 $\sigma =0$ (极点在虚轴上): $e^{\sigma t}=1$。系统的自由响应是一个幅值不变的持续振荡（如 $\cos (\omega t)$）。系统既不收敛也不发散，是临界稳定的。

结论：一个系统的动态响应由其所有极点共同决定。只要所有极点的实部都为负，系统的每一个响应分量都会衰减，从而保证系统最终能够稳定下来。因此，分析系统收敛性的关键，就在于求解其闭环传递函数的极点，并检查它们是否全部位于复平面的左半部分。

---

前提：

• 被控对象（Plant）: 从线性化模型可知，滚转力矩 $U_2$ 与角加速度 $\ddot{\varphi }$ 的关系为 $\ddot{\varphi }=b{U}_2$（其中 $b=1/I_{xx}$）。通过拉普拉斯变换（$s$为复频率），其传递函数为：
  $$G(s)=\frac{\Phi (s)}{U_2(s)}=\frac{b}{s^2}$$
• 控制器（Controller）: 控制器根据期望角度 ${\varphi }_{des}$ 和当前角度 $\varphi$ 的误差 $e={\varphi }_{des}-\varphi$ 来计算输出 $U_2$。
• 闭环系统: 控制器和被控对象组成一个负反馈系统。根据上面的讨论，我们要做的就是分析其闭环极点的位置。

案例分析1：纯比例（P）控制器 (对应3.1节)

1. 控制器传递函数 $C(s)$：
   P控制器的控制律是 输出 = Kp * 误差，即 $U_2(t)=K_p\cdot e(t)$。对这个式子进行拉普拉斯变换，我们得到 $U_2(s)=K_p\cdot E(s)$。
   传递函数的定义是输出与输入的比值，因此：
   $$C(s)=\frac{U_2(s)}{E(s)}=K_p$$

2. 闭环特征方程 $1+C(s)G(s)=0$：
   在一个标准的负反馈控制环中，误差 $E(s)$ 是目标值 $R(s)$ (期望角度 ${\varphi }_{des}$) 与实际值 $Y(s)$ (实际角度 $\varphi$) 的差，即 $E(s)=R(s)-Y(s)$。
   而实际值 $Y(s)$ 是由误差 $E(s)$ 经过控制器 $C(s)$ 和被控对象 $G(s)$ 共同作用产生的：$Y(s)=E(s)\cdot C(s)\cdot G(s)$。
   将两者合并：$Y(s)=(R(s)-Y(s))\cdot C(s)G(s)$。
   整理可得：$Y(s)(1+C(s)G(s))=R(s)C(s)G(s)$。
   系统的闭环传递函数为 $\frac{Y(s)}{R(s)}=\frac{C(s)G(s)}{1+C(s)G(s)}$。
   我们看到，分母 $1+C(s)G(s)$ 决定了系统响应的固有模式。令分母为零，得到的方程即为特征方程，它的根就是系统的极点。
   $$1+K_p\cdot \frac{b}{s^2}=0⟹s^2+b{K}_p=0$$

3. 极点位置：
   $$s=\pm j\sqrt{b{K}_p}$$

4. 收敛性分析：
   系统的两个极点实部为0，纯粹位于虚轴上。根据我们的理论，这意味着系统处于临界稳定状态。当受到一个扰动后，它会以角频率 $\omega =\sqrt{b{K}_p}$ 进行永不衰减的正弦振荡。系统并不会收敛到目标值，这与3.1节的直观物理分析完全吻合。

案例分析2：比例微分（PD）控制器 (对应3.2节)

1. 控制器传递函数 $C(s)$：
   PD控制器的控制律是 $U_2(t)=K_{p}e(t)+K_d\frac{de(t)}{dt}$。
   利用拉普拉斯变换中“时域的微分等于频域乘以s”的性质，我们得到 $U_2(s)=K_{p}E(s)+K_{d}sE(s)=(K_p+K_{d}s)E(s)$。
   因此，PD控制器的传递函数为：
   $$C(s)=\frac{U_2(s)}{E(s)}=K_p+K_{d}s$$

2. 闭环特征方程为 $1+C(s)G(s)=0$：
   $$1+(K_p+K_{d}s)\cdot \frac{b}{s^2}=0⟹s^2+b{K}_{d}s+b{K}_p=0$$

3. 收敛性分析：
   这是一个标准的二阶系统特征方程，其通用形式为 $s^2+2\zeta {\omega }_{n}s+{\omega }_n^2=0$。

   • 自然频率: ${\omega }_n=\sqrt{b{K}_p}$，它决定了系统响应的“快慢”。
   • 阻尼比: $2\zeta {\omega }_n=b{K}_d⟹\zeta =\frac{b{K}_d}{2{\omega }_n}=\frac{b{K}_d}{2\sqrt{b{K}_p}}=\frac{K_d}{2}\sqrt{\frac{b}{K_p}}$，它决定了系统的“稳定性”和“超调”。

4. 极点位置由求根公式给出：
   $$s=-\zeta {\omega }_n\pm {\omega }_n\sqrt{{\zeta }^2-1}$$
   只要我们选择 $K_p>0$ 和 $K_d>0$，那么 $b,{\omega }_n,\zeta$ 都是正数。因此，极点的实部 $-\zeta {\omega }_n$ 永远是负数。

   • 结论: 只要PD增益为正，系统的极点就一定在复平面的左半部分。根据我们的理论，这保证了系统是稳定且收敛的。
   • 收敛形式：
     • 若 $\zeta >1$ (D项很强)，为过阻尼，系统缓慢收敛无超调。
     • 若 $\zeta =1$ (D项恰到好处)，为临界阻尼，系统最快收敛且无超调。
     • 若 $0<\zeta <1$ (P项占优)，为欠阻尼，系统在振荡中收敛，会有一定超调。这是实际调参中最常见的情况。

通过传递函数法，我们从数学上证明了D项的引入，相当于在系统特征方程中增加了一次项 $s$，从而将极点从虚轴“拉”到了左半平面，确保了系统的收焉性。

4. 四旋翼控制架构 —— 串级 PID

在实际四旋翼中，我们不直接用 PID 控制位置，而是采用串级结构（Cascade Control）。

4.1 为什么要分层？

位置的变化不是直接发生的。

• 想让飞机向前飞（位置变化），必须先让飞机低头（姿态变化）。
• 姿态的变化比位置的变化快得多（电机响应只需几十毫秒，而飞出去几米需要几秒）。
• 时间尺度的分离使得分层控制成为最佳选择。

4.2 控制流图与公式详解

串级PID的本质是将控制任务分解为“快环”和“慢环”，内环快，外环慢。

1. 外环（位置环）：计算期望姿态

外环的目标是根据位置误差，计算出要让飞机飞到正确位置所期望的姿态。

• 运行频率: 较低 (e.g., 10Hz - 50Hz)
• 控制器: 通常是 PID (或 PI, PD)

X-Y 平面位置控制 (以X轴为例)

首先，位置控制器计算出一个“期望的加速度” ${\ddot{x}}_{des}$ 来消除位置误差。
$${\ddot{x}}_{des}=K_{p,x}(x_{des}-x_{curr})+K_{i,x}\int (x_{des}-x_{curr})dt+K_{d,x}({\dot{x}}_{des}-{\dot{x}}_{curr})$$

• 输入:
  • $x_{des}$: 期望的X轴位置 (来自遥控器或任务规划)
  • $x_{curr}$: 当前的X轴位置 (来自GPS或视觉定位)
  • ${\dot{x}}_{des}$: 期望的X轴速度 (通常为0)
  • ${\dot{x}}_{curr}$: 当前的X轴速度 (GPS或状态估计)
• 输出:
  • ${\ddot{x}}_{des}$: 期望的X轴加速度

然后，利用在线性化部分得到的近似关系 $\ddot{x}\approx g\cdot \theta$，将期望加速度转换成期望的俯仰角 ${\theta }_{des}$。
$${\theta }_{des}=\frac{{\ddot{x}}_{des}}{g}$$

• 输入:
  • ${\ddot{x}}_{des}$: 期望的X轴加速度 (来自位置PID控制器)
• 输出:
  • ${\theta }_{des}$: 期望的俯仰角 (将作为内环的输入)

Y轴的控制与X轴完全类似，通过计算 ${\ddot{y}}_{des}$ 来得到期望的滚转角 ${\varphi }_{des}$
$${\varphi }_{des}=-\frac{{\ddot{y}}_{des}}{g}$$
(注意：这里的负号是因为，按照右手坐标系，正的滚转角 $\varphi$ 产生负的Y轴方向加速度)

Z轴（高度）控制

高度控制器的输出是调节总升力 $U_1$。
$$\Delta U_1=K_{p,z}(z_{des}-z_{curr})+K_{i,z}\int (z_{des}-z_{curr})dt+K_{d,z}({\dot{z}}_{des}-{\dot{z}}_{curr})$$

• 输入:
  • $z_{des}$: 期望高度
  • $z_{curr}$: 当前高度 (来自气压计或GPS)
  • ${\dot{z}}_{des}$: 期望垂直速度 (通常为0)
  • ${\dot{z}}_{curr}$: 当前垂直速度
• 输出:
  • $\Delta U_1$: 升力调节量。最终的总升力指令是 $U_1=mg+\Delta U_1$。

2. 内环（姿态环）：执行期望姿态

内环的目标是快速、精确地跟踪外环给出的期望姿态，并输出最终的电机力矩指令。

• 运行频率: 很高 (e.g., 250Hz - 1kHz)
• 控制器: 通常是 PD (或仅P)

姿态控制 (以俯仰角 $\theta$ 为例)

姿态控制器计算出力矩 $U_3$ 来消除姿态角度误差。
$$U_3=K_{p,\theta }({\theta }_{des}-{\theta }_{curr})+K_{d,\theta }({\dot{\theta }}_{des}-{\dot{\theta }}_{curr})$$

• 输入:
  • ${\theta }_{des}$: 期望俯仰角 (来自外环)
  • ${\theta }_{curr}$: 当前俯仰角 (来自IMU)
  • ${\dot{\theta }}_{des}$: 期望俯仰角速度 (通常为0)
  • ${\dot{\theta }}_{curr}$: 当前俯仰角速度 $q$ (来自IMU)
• 输出:
  • $U_3$: 俯仰力矩指令 (将进入电机混控器分配到各个电机)

滚转角 $\varphi$ 和偏航角 $\psi$ 的控制同理，分别输出滚转力矩 $U_2$ 和偏航力矩 $U_4$

---

总结: 控制信号的流动路径

1. 位置目标 $(x_{des},y_{des},z_{des})$
2. -> 外环(位置PID)
3. -> 期望姿态和升力 $({\varphi }_{des},{\theta }_{des},U_1)$
4. -> 内环(姿态PD)
5. -> 期望力矩 $(U_2,U_3,U_4)$
6. -> 电机混控
7. -> 各个电机的转速
8. -> 物理世界: 四旋翼产生力和力矩，改变姿态和位置
9. -> 传感器(IMU, GPS): 测量新的 $(x_{curr},{\varphi }_{curr},...)$，形成闭环。

5. 实用指南：参数整定与实现

5.1 离散化 PID 代码示例

在单片机中，我们使用离散形式：

// 伪代码示例
float update_pid(float target, float current, float dt) {
    float error = target - current;
    
    // P项
    float p_term = Kp * error;
    
    // I项（需限幅，防止积分饱和）
    integral += error * dt;
    integral = constrain(integral, -MAX_I, MAX_I); 
    float i_term = Ki * integral;
    
    // D项（通常对测量值微分，避免设定值突变引起的冲击）
    // 并且通常需要低通滤波
    float derivative = (error - prev_error) / dt;
    float d_term = Kd * derivative;
    
    prev_error = error;
    
    return p_term + i_term + d_term;
}

5.2 调参口诀（经验法）

对于四旋翼这种快速系统，通常遵循 “先内后外，先P后D再I” 的原则：

1. 内环（姿态）：
   • 先把 I 和 D 设为 0，只加 P。
   • 增大 P 直到飞机开始高频振荡，然后回调一点（比如乘 0.6）。
   • 慢慢增加 D，直到振荡消失，飞机反应变得“跟手”且没有回弹。
   • 姿态环通常不需要 I，或者给很小的 I。
2. 外环（位置）：
   • 同样先调 P。
   • 增加 D 抑制过冲。
   • 最后增加 I，直到飞机能稳稳地定在原地，不受微风影响。

核心洞见：
PID 参数的本质是对系统频域特性的整形。P 决定带宽（响应速度），D 增加相位裕度（稳定性），I 增强低频增益（稳态精度）。