二、基于根轨迹法的PID控制器分析与设计

序
一、四旋翼数学模型
- 1.1 数学模型及参数
- 1.2 模型分析
- 1.3 简化四旋翼模型
二、控制器设计
- 2.1 方框图与结构图
- 2.2 控制器设计
- - 2.2.1 简易的控制器
  - 2.2.2 控制器性能分析
- 2.3 校正方式
- 2.4 控制器校正
三、根轨迹法分析
- 3.1 基础知识
- 3.2 根轨迹分析姿态环
- - 3.2.1 确定内环参数下分析
  - 3.2.2 内环参数的影响
- 3.3 根轨迹分析角速度环
- - 3.3.1 角速度环 P控制
  - 3.3.2 角速度环 PI控制
  - 3.3.3 角速度环 PID控制
  - 3.3.4 根轨迹法确定取值及结果分析
四、单个PID与串级PID比较
五、滚转角、偏航角PID参数确定
- 5.1 滚转角PID参数
- 5.2 偏航角PID参数
- - 5.2.1 偏航角数学模型、结构图
  - 5.2.2 系统传递函数
  - 5.2.3 比例系数P 根轨迹
  - 5.2.4 积分系数 I
  - 5.2.5 微分系数 D
  - 5.2.6 单位阶跃响应
六、结果分析
- 6.1 俯仰角时域复域比较
- 6.2 耦合关系的影响

序

调节PID 是一件令人头疼的事情，特别是无人机，三个姿态PID，三个角速度PID，有时还要加上位置PID，九组PID相互影响，盲目调参效率低下，为之奈何？本文将试图从理论的角度，在对无人机建模的基础上，分析PID参数对整个系统的影响。或许并不能找到最好的PID搭配，但是力图找到其中的规律，用于指导整个调节过程。

真的懂PID吗？

先聊一下 PID。说到 PID，最常见的一个例子是温度控制：假如现在温度是 0°C，想要达到的目标温度是 60°C，那么可是使用 PID，并且假如 K p = 0.5 K_p=0.5 Kp=0.5，那么第一次温度升高 ( 60 − 0 ) × 0.5 = 30 (60-0) \times 0.5=30 (60−0)×0.5=30，达到 30°C；第二次升高 ( 60 − 30 ) × 0.5 = 15 (60-30) \times 0.5=15 (60−30)×0.5=15，达到 45°C；以此类推，每次的增量 = Kp * (期望温度 - 当前温度)，最终逐渐靠近 60°C。然后又讨论 K i , K d K_i, K_d Ki,Kd 的影响。

不得不说，上面真是个误人子弟的例子，但却得到了广泛的引用，还让人感觉真的理解了PID，真是一件有趣的事情，人们的思想是多么容易被误导。

PID分为位置式PID和增量式PID，位置式PID的表达式如下：

u ( k ) = K p e ( k ) + K i ∑ j = 0 k e ( k ) + K d [ e ( k ) − e ( k − 1 ) ] u(k) = K_p e(k) + K_i \sum_{j=0}^k e(k) + K_d [e(k) - e(k-1)] u(k)=Kpe(k)+Kij=0∑ke(k)+Kd[e(k)−e(k−1)]

其中， u ( k ) u(k) u(k) 为输出； e ( k ) = d − r ( k ) e(k) = d-r(k) e(k)=d−r(k) 是期望与输入的误差； K p , K i , K d K_p, K_i, K_d Kp,Ki,Kd 为 PID 系数。

注意与上面的例子的区别，上面温度的例子中，使用所谓的 “PID” 计算出的结果是增量，而真正的 PID 计算出的结果是输出量。那么，难道上面的例子使用的是增量式 PID？那就看看增量式 PID 的表达式，比对比对：

Δ u ( k ) = K p [ e ( k ) − e ( k − 1 ) ] + K i e ( k ) + K d [ e ( k ) − 2 e ( k − 1 ) + e ( k − 2 ) ] \Delta u(k) = K_p [e(k) - e(k-1) ] + K_i e(k) + K_d [e(k) - 2e(k-1) + e(k-2)] Δu(k)=Kp[e(k)−e(k−1)]+Kie(k)+Kd[e(k)−2e(k−1)+e(k−2)]

跟上面描述的例子也不是一回事。所以，上面的例子只能说运用了 PID 的思想 —— 距离目标越近增量越小，并不是真正意义上的 PID。

盲调 PID 参数？

就像不分青红皂白地举例说什么是 PID 一样，怎样调 PID 也愈传愈神，一句“靠经验”拿倒多少好汉。“靠经验”的意思是不需要懂什么理论，看着波形大了小了轮番调节 P、I、D即可。但是本人用实践证明，纯粹的看波形，凭感觉，看运气是一件搞笑的事情。

四旋翼的飞控一般使用串级 PID，一个姿态环（也即角度环）一个角速度环，前者一般会只一个 K p K_p Kp，后者会 K p , K i , K d K_p, K_i, K_d Kp,Ki,Kd 参数一起用，至少APM 和 PX4 都是这样的。问题就来了，为什么要两个PID，为什么姿态环只 P，角速度环 P、I、D都有？如果说只是盲调参，不考虑这些问题，恐怕会根本找不到一个合理的数量级，甚至调反方向，南辕北辙，调了不该调的参数，画蛇添足。

姿态 PID 的输入是期望姿态，通过传感器数据融合得到测量的姿态，输出是期望角速度，如下图：

期望姿态与测量姿态的差，两个角度之差，也就是角度变化量，是不是和角速度有什么关系呢？其实角速度乘以间隔时间的量纲与角度变化量的量纲是一致的。因此，采用位置式 PID，一个 Kp 类似时间的倒数，就可以时间姿态差到角速度的转化。此时用上 I、D都是一种画蛇添足，违反了系统本身的规律。至于数量级，这个需要使用控制领域的分析方法，才能得出合理的参数。

以上说明理解系统并学习控制理论的重要性，不要人云亦云，要有属于自己的理解，感受其中的奥妙。借用无人机教父安德烈的一句话：实际上，我们很难去预测新兴技术给我们带来的影响。而对于我们工程师和科学家来说，享受的是研发和创作的过程。它能够不断地提醒我们生活的宇宙是如此的美妙和神奇，充满创造力和智慧的人类能够以如此惊人的方式去展现宇宙的魅力。事实上，无人机具有巨大的商业和经济潜力只不过是锦上添花而已。

一、四旋翼数学模型

1.1 数学模型及参数

UAV021-V2 四旋翼数学模型一文推导给出四旋翼飞行器数学模型，如下：

{ T m ϖ 1 ′ + ϖ 1 = C R σ 1 + ϖ b T m ϖ 2 ′ + ϖ 2 = C R σ 2 + ϖ b T m ϖ 3 ′ + ϖ 3 = C R σ 3 + ϖ b T m ϖ 4 ′ + ϖ 4 = C R σ 4 + ϖ b f = c T ( ϖ 1 2 + ϖ 2 2 + ϖ 3 2 + ϖ 4 2 ) τ x = 2 2 d c T ( − ϖ 1 2 + ϖ 2 2 + ϖ 3 2 − ϖ 4 2 ) τ y = 2 2 d c T ( ϖ 1 2 + ϖ 2 2 − ϖ 3 2 − ϖ 4 2 ) τ z = c M ( ϖ 1 2 − ϖ 2 2 + ϖ 3 2 − ϖ 4 2 ) (1.1) \begin{cases} T_{_m} \varpi'_{_1} + \varpi_{_1} = C_{_R} \sigma_{_1} + \varpi_{_b} \\ T_{_m} \varpi'_{_2} + \varpi_{_2} = C_{_R} \sigma_{_2} + \varpi_{_b} \\ T_{_m} \varpi'_{_3} + \varpi_{_3} = C_{_R} \sigma_{_3} + \varpi_{_b} \\ T_{_m} \varpi'_{_4} + \varpi_{_4} = C_{_R} \sigma_{_4} + \varpi_{_b} \\ f =c_{_T} (\varpi_{_1}^{^2} + \varpi_{_2}^{^2}+ \varpi_{_3}^{^2} + \varpi_{_4}^{^2} ) \\ \tau_{_x} = \frac{\sqrt{2}}{2}dc_{_T}(-\varpi_{_1}^{^2} + \varpi_{_2}^{^2} + \varpi_{_3}^{^2} - \varpi_{_4}^{^2}) \\ \tau_{_y} = \frac{\sqrt{2}}{2}dc_{_T}( \varpi_{_1}^{^2} + \varpi_{_2}^{^2} -\varpi_{_3}^{^2} - \varpi_{_4}^{^2}) \\ \tau_{_z} = c_{_M}(\varpi_{_1}^{^2} -\varpi_{_2}^{^2} + \varpi_{_3}^{^2} -\varpi_{_4}^{^2} ) \end{cases} \tag{1.1} ⎩⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎧Tmϖ1′+ϖ1=CRσ1+ϖbTmϖ2′+ϖ2=CRσ2+ϖbTmϖ3′+ϖ3=CRσ3+ϖbTmϖ4′+ϖ4=CRσ4+ϖbf=cT(ϖ12+ϖ22+ϖ32+ϖ42)τx=22 dcT(−ϖ12+ϖ22+ϖ32−ϖ42)τy=22 dcT(ϖ12+ϖ22−ϖ32−ϖ42)τz=cM(ϖ12−ϖ22+ϖ32−ϖ42)(1.1)

{ J x ω x ′ = ( J y − J z ) ω y ω z + τ x − C d m ω x ∣ ω x ∣ J y ω y ′ = ( J z − J x ) ω z ω x + τ y − C d m ω y ∣ ω y ∣ J z ω z ′ = ( J x − J y ) ω x ω y + τ z − C d m ω z ∣ ω z ∣ ϕ ′ = ω x + ( tan ⁡ θ sin ⁡ ϕ ) ω y + ( tan ⁡ θ cos ⁡ ϕ ) ω z θ ′ = ( cos ⁡ ϕ ) ω y − ( sin ⁡ ϕ ) ω z ψ ′ = ( sin ⁡ ϕ / cos ⁡ θ ) ω y + ( cos ⁡ ϕ / cos ⁡ θ ) ω z m p x ′ ′ = − ( cos ⁡ ψ sin ⁡ θ cos ⁡ ϕ + sin ⁡ ψ sin ⁡ ϕ ) f m p y ′ ′ = − ( sin ⁡ ψ sin ⁡ θ cos ⁡ ϕ − cos ⁡ ψ sin ⁡ ϕ ) f m p z ′ ′ = m g − ( cos ⁡ ϕ cos ⁡ θ ) f (1.2) \begin{cases} J_{_x} \omega'_{_{x}} = (J_{_y} - J_{_z}) \omega_{_{y}} \omega_{_{z}} +\tau_{_x} - C_{_{dm}} \omega_{_x}|\omega_{_x}| \\ J_{_y} \omega'_{_{y}} = {(J_{_z} - J_{_x})} \omega_{_{z}} \omega_{_{x}} +\tau_{_y} - C_{_{dm}} \omega_{_y}|\omega_{_y}| \\ J_{_z} \omega'_{_{z}} = {(J_{_x} - J_{_y})}\omega_{_{x}} \omega_{_{y}} +\tau_{_z} - C_{_{dm}} \omega_{_z}|\omega_{_z}| \\ \phi' = \omega_{_x} + (\tan \theta \sin \phi) \omega_{_y} + (\tan \theta \cos \phi ) \omega_{_z} \\ \theta' = (\cos \phi)\omega_{_y} -(\sin \phi) \omega_{_z} \\ \psi' = (\sin \phi / \cos \theta) \omega_{_y} + (\cos \phi / \cos \theta) \omega_{_z} \\ mp''_{_x} = - (\cos\psi \sin\theta \cos\phi +\sin\psi \sin\phi ) f\\ mp''_{_y} = -(\sin\psi \sin\theta \cos\phi-\cos\psi \sin\phi) f\\ mp''_{_z} = mg-(\cos\phi \cos\theta) f \\ \end{cases} \tag{1.2} ⎩⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎧Jxωx′=(Jy−Jz)ωyωz+τx−Cdmωx∣ωx∣Jyωy′=(Jz−Jx)ωzωx+τy−Cdmωy∣ωy∣Jzωz′=(Jx−Jy)ωxωy+τz−Cdmωz∣ωz∣ϕ′=ωx+(tanθsinϕ)ωy+(tanθcosϕ)ωzθ′=(cosϕ)ωy−(sinϕ)ωzψ′=(sinϕ/cosθ)ωy+(cosϕ/cosθ)ωzmpx′′=−(cosψsinθcosϕ+sinψsinϕ)fmpy′′=−(sinψsinθcosϕ−cosψsinϕ)fmpz′′=mg−(cosϕcosθ)f(1.2)

式(1.1) 实现从油门到升力与力矩的模型，式(1.2) 根据牛顿—欧拉动力学方程最终得到无人机姿态和位移。式中参数含义及取值如下：

1.2 模型分析

实话实说，上面的模型很复杂（至少个人这样觉得），多输入（四个电机油门输入）多输出（三个姿态角）非线性（力和力矩的计算）且互相耦合（式1.2频繁出现），自动控制原理的那一套变成了屠龙之技，老虎吃刺猬——无从下口。曾尝试画出上面的框图，可是发现复杂得失去框图的意义，框图是为了更加清晰认识系统，现在却不能；也曾尝试进行拉式变换，但是一个平方，时域相乘频域卷积，问题更加复杂。为之奈何！

最简单粗暴的思路是简化问题。假设：

设滚转角偏航角皆为 0，即 ϕ = 0 , ψ = 0 \phi=0, \psi=0 ϕ=0,ψ=0，只考虑俯仰角 θ \theta θ；
滚转角、偏航角对应的角速度皆为0，即 ω x = 0 , ω z = 0 \omega_{_x}=0, \omega_{_z}=0 ωx=0,ωz=0，只考虑俯仰角对应的角速度 ω y \omega_{_y} ωy；
不考虑空气产生的阻力矩；
将转速与力矩的二次函数关系转化成线性的；
多输入变量转化为单输入变量。

集齐上面四个思路，就可以实现单输入单输出线性的系统。

1.3 简化四旋翼模型

根据假设1~假设3，式(1.2) 简化为：

{ J y ω y ′ = τ y θ ′ = ω y (1.3) \begin{cases} J_{_y} \omega'_{_{y}} = \tau_{_y} \\ \theta' = \omega_{_y} \end{cases} \tag{1.3} {Jyωy′=τyθ′=ωy(1.3)

其中，输入是 τ y \tau_{_y} τy。式子一下子简单很多。

只研究俯仰角，只与 τ y \tau_{_y} τy 有关，式(1.1) 简化为：

{ T m ϖ 1 ′ + ϖ 1 = C R σ 1 + ϖ b T m ϖ 2 ′ + ϖ 2 = C R σ 2 + ϖ b T m ϖ 3 ′ + ϖ 3 = C R σ 3 + ϖ b T m ϖ 4 ′ + ϖ 4 = C R σ 4 + ϖ b τ y = 2 2 d c T ( ϖ 1 2 + ϖ 2 2 − ϖ 3 2 − ϖ 4 2 ) (1.4) \begin{cases} T_{_m} \varpi'_{_1} + \varpi_{_1} = C_{_R} \sigma_{_1} + \varpi_{_b} \\ T_{_m} \varpi'_{_2} + \varpi_{_2} = C_{_R} \sigma_{_2} + \varpi_{_b} \\ T_{_m} \varpi'_{_3} + \varpi_{_3} = C_{_R} \sigma_{_3} + \varpi_{_b} \\ T_{_m} \varpi'_{_4} + \varpi_{_4} = C_{_R} \sigma_{_4} + \varpi_{_b} \\ \tau_{_y} = \frac{\sqrt{2}}{2}dc_{_T}( \varpi_{_1}^{^2} + \varpi_{_2}^{^2} -\varpi_{_3}^{^2} - \varpi_{_4}^{^2}) \end{cases} \tag{1.4} ⎩⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎧Tmϖ1′+ϖ1=CRσ1+ϖbTmϖ2′+ϖ2=CRσ2+ϖbTmϖ3′+ϖ3=CRσ3+ϖbTmϖ4′+ϖ4=CRσ4+ϖbτy=22 dcT(ϖ12+ϖ22−ϖ32−ϖ42)(1.4)

虽然简化了，但是依旧是多输入且非线性的，再来解决这个问题。

首先，对于一阶微分方程

a x ′ + x = b (1.5) a x' + x = b \tag{1.5} ax′+x=b(1.5)

两边进行拉式变换，得：

a s X ( s ) + X ( s ) = b s as X(s) + X(s) = \frac{b}{s} asX(s)+X(s)=sb

故
X ( s ) = b s 1 a s + 1 = b ( 1 s − a a s + 1 ) = b ( 1 s − 1 s + 1 / a ) \begin{aligned} X(s) &= \frac{b}{s} \frac{1}{as+1} \\ &=b(\frac{1}{s} - \frac{a}{as + 1}) \\ &=b(\frac{1}{s} - \frac{1}{s + 1/a}) \end{aligned} X(s)=sbas+11=b(s1−as+1a)=b(s1−s+1/a1)

两边进行拉式逆变换的到最终的解：
x ( t ) = b ( 1 − e − t a ) (1.6) x(t) = b(1-e^{-\frac{t}{a}} \tag{1.6} ) x(t)=b(1−e−at)(1.6)

因此，令 a = T m , b = C R σ 1 + ϖ b a=T_{_m}, b=C_{_R}\sigma_{_1}+\varpi_{_b} a=Tm,b=CRσ1+ϖb 得：

T m ϖ 1 ′ + ϖ 1 = C R σ 1 + ϖ b (1.7) T_{_m} \varpi'_{_1} + \varpi_{_1} = C_{_R} \sigma_{_1} + \varpi_{_b} \tag{1.7} Tmϖ1′+ϖ1=CRσ1+ϖb(1.7)

解为：

ϖ 1 ( t ) = ( C R σ 1 + ϖ b ) ( 1 − e − t T m ) (1.8) \varpi_{_1}(t) = (C_{_R} \sigma_{_1}+\varpi_{_b})(1-e^{-\frac{t}{T_m}}) \tag{1.8} ϖ1(t)=(CRσ1+ϖb)(1−e−Tmt)(1.8)

同理得到 ϖ 2 ( t ) , ϖ 3 ( t ) , ϖ 4 ( t ) \varpi_{_2}(t), \varpi_{_3}(t), \varpi_{_4}(t) ϖ2(t),ϖ3(t),ϖ4(t) 代入 τ y \tau_{_y} τy 的计算式的：

τ y = 2 2 d c T ( ϖ 1 2 + ϖ 2 2 − ϖ 3 2 − ϖ 4 2 ) = 2 2 d c T ( 1 − e − t T m ) 2 [ ( C R σ 1 + ϖ b ) 2 + ( C R σ 2 + ϖ b ) 2 − ( C R σ 3 + ϖ b ) 2 − ( C R σ 4 + ϖ b ) 2 ] = 2 2 d c T ( 1 − e − t T m ) 2 { [ C R 2 ( σ 1 2 − σ 3 3 ) + ( σ 2 2 − σ 4 3 ) ] + 2 C R ϖ b [ ( σ 1 − σ 3 ) + ( σ 2 − σ 4 ) ] } = 2 2 d c T ( 1 − e − t T m ) 2 { [ C R 2 ( σ 1 + σ 3 ) + 2 C R ϖ b ] ( σ 1 − σ 3 ) + [ C R 2 ( σ 2 + σ 4 ) + 2 C R ϖ b ] ( σ 2 − σ 4 ) (1.9) \begin{aligned} \tau_{_y} &= \frac{\sqrt{2}}{2}dc_{_T}( \varpi_{_1}^{^2} + \varpi_{_2}^{^2} -\varpi_{_3}^{^2} - \varpi_{_4}^{^2}) \\ &= \frac{\sqrt{2}}{2}dc_{_T}(1-e^{-\frac{t}{T_m}})^{^2}[ (C_{_R} \sigma_{_1}+\varpi_{_b})^{^2} + (C_{_R} \sigma_{_2}+\varpi_{_b})^{^2} - (C_{_R} \sigma_{_3}+\varpi_{_b})^{^2} - (C_{_R} \sigma_{_4}+\varpi_{_b})^{^2} ] \\ &= \frac{\sqrt{2}}{2}dc_{_T}(1-e^{-\frac{t}{T_m}})^{^2} \{ [C_{_R}^{^2}(\sigma_{_1}^{^2} - \sigma_{_3}^{^3})+(\sigma_{_2}^{^2} - \sigma_{_4}^{^3})] + 2C_{_R}\varpi_{_b}[(\sigma_{_1} - \sigma_{_3} ) + (\sigma_{_2} - \sigma_{_4})] \} \\ &= \frac{\sqrt{2}}{2}dc_{_T}(1-e^{-\frac{t}{T_m}})^{^2} \{ [C_{_R}^{^2} (\sigma_{_1}+\sigma_{_3}) + 2C_{_R}\varpi_{_b}] (\sigma_{_1} - \sigma_{_3}) + [C_{_R}^{^2} (\sigma_{_2}+\sigma_{_4}) + 2C_{_R}\varpi_{_b}] (\sigma_{_2} - \sigma_{_4}) \end{aligned} \tag{1.9} τy=22 dcT(ϖ12+ϖ22−ϖ32−ϖ42)=22 dcT(1−e−Tmt)2[(CRσ1+ϖb)2+(CRσ2+ϖb)2−(CRσ3+ϖb)2−(CRσ4+ϖb)2]=22 dcT(1−e−Tmt)2{[CR2(σ12−σ33)+(σ22−σ43)]+2CRϖb[(σ1−σ3)+(σ2−σ4)]}=22 dcT(1−e−Tmt)2{[CR2(σ1+σ3)+2CRϖb](σ1−σ3)+[CR2(σ2+σ4)+2CRϖb](σ2−σ4)(1.9)

在无人机只有俯仰角变化时，1号2号电机加油门，3号4号电机减油门，而且增减量一致，故

{ σ 1 + σ 3 = σ 2 + σ 4 = 2 σ 0 σ 1 − σ 3 = σ 2 − σ 4 = 2 Δ σ (1.10) \begin{cases} \sigma_{_1}+\sigma_{_3} = \sigma_{_2}+\sigma_{_4} = 2\sigma_{_0} \\ \sigma_{_1} - \sigma_{_3} = \sigma_{_2} - \sigma_{_4} = 2 \Delta \sigma \end{cases} \tag{1.10} {σ1+σ3=σ2+σ4=2σ0σ1−σ3=σ2−σ4=2Δσ(1.10)

其中 σ 0 \sigma_{_0} σ0 为维持无人机高度不变所需的油门，一般会在0.5附近，此处取0.5。 Δ σ \Delta \sigma Δσ 可由控制分配直接计算，实际中一般在 15% 以内。代入式(1.9) 得：

τ y = K τ ( 1 − 2 e − t T m + e − 2 t T m ) Δ σ (1.11) \tau_{_y} =K_{_\tau} (1 - 2e^{-\frac{t}{T_m}} + e^{-\frac{2t}{T_m}}) \Delta \sigma \tag{1.11} τy=Kτ(1−2e−Tmt+e−Tm2t)Δσ(1.11)

其中， K τ = 2 2 d c T ( C R 2 σ 0 + C R ϖ b ) K_{_\tau}=2\sqrt{2}dc_{_T} (C_{_R}^{^2} \sigma_{_0} + C_{_R}\varpi_{_b}) Kτ=22 dcT(CR2σ0+CRϖb) 为常数。可见，尽管力矩 τ y \tau_{_y} τy 与四个电机的油门输入是多输入且非线性，但是由于四旋翼等量增减油门，很有对称性，力矩 τ y \tau_{_y} τy 与电机的变化量之间是线性关系。式(1.3) 和式(1.11) 描述了从油门输入到角度输出的关系，构成了简化的四旋翼模型，作为后面的分析对象。

二、控制器设计

2.1 方框图与结构图

为了更清晰展示模型关系，先给出式(1.3) 与(1.11) 表示的方框图：

其中包括三个部分，每个都是线性的模型。

在进行控制分析与设计时，需要使用拉式变换转换到 s 域，将方框图转化为结构图。式(1.11) 描述了输入 Δ σ \Delta \sigma Δσ 的油门差时系统响应为 τ y \tau_{_y} τy，即：

τ y ( s ) = G 1 ( s ) Δ σ s = K τ ( 1 s − 2 s + 1 / T m + 1 s + 2 / T m ) Δ σ \tau_{_y}(s) = G_{_1}(s) \frac{\Delta \sigma}{s} = K_{_\tau}(\frac{1}{s} - \frac{2}{s+1/T_{_m}} + \frac{1}{s+2/T_{_m}}) \Delta \sigma τy(s)=G1(s)sΔσ=Kτ(s1−s+1/Tm2+s+2/Tm1)Δσ

解得：

G 1 ( s ) = K τ ( T m s + 1 ) ( T m 2 s + 1 ) (2.1) G_{_1}(s)= \frac{K_{_\tau}}{(T_{_m}s+1)(\frac{T_{_m}}{2} s + 1)} \tag{2.1} G1(s)=(Tms+1)(2Tms+1)Kτ(2.1)

式(1.3) 两边进行拉式变换，易得：

J m s ω y ( s ) = τ ( s ) s θ ( s ) = ω y ( s ) J_{_m} s \omega_{_y}(s) = \tau(s) \\ s\theta(s) = \omega_{_y}(s) Jmsωy(s)=τ(s)sθ(s)=ωy(s)

故

G 2 ( s ) = 1 / J m s (2.2) G_{_2}(s)= \frac{1/J_{_m}}{s} \tag{2.2} G2(s)=s1/Jm(2.2)

G 3 ( s ) = 1 s (2.3) G_{_3}(s)=\frac{1}{s} \tag{2.3} G3(s)=s1(2.3)

将方框图内模块转化为传递函数，即可得到结构图，如下：

按照北航所给的数据，可计算得： T m = 0.0173 , K τ = 2.8247 , J m = J y = 1.396 × 1 0 − 2 T_{m}=0.0173, K_{\tau} = 2.8247, J_{m}=J_y=1.396\times10^{-2} Tm=0.0173,Kτ=2.8247,Jm=Jy=1.396×10−2。

2.2 控制器设计

2.2.1 简易的控制器

控制器即使用负反馈，实现输出值逐渐接近目标值，最简单的想法是直接引入一个负反馈，得到如下框图：

其中，最下方的方框 1 代表传感器测量的角度值和真实值一样，不考虑误差与噪声。

从框图中可见，如果期望角度 θ d \theta_{_d} θd 与测量角度 θ m \theta_{_m} θm 误差越大，输入油门差越大，使得角度变化量越大；测量角度接近期望角度时，误差小，输入油门差也小，角度变化微小，成功使用了负反馈。

2.2.2 控制器性能分析

上面的控制器性能如何呢？一般以单位阶跃响应来评价。

系统的开环传递函数为：

G ( s ) = G 1 G 2 G 3 = K τ / J y s 2 ( T m s + 1 ) ( T m 2 s + 1 ) (2.4) G(s) = G_{_1}G_{_2}G_{_3}= \frac{K_{_\tau}/J_{_y}}{ s^{^2}(T_{_m}s+1)(\frac{T_{_m}}{2} s + 1)} \tag{2.4} G(s)=G1G2G3=s2(Tms+1)(2Tms+1)Kτ/Jy(2.4)

闭环传递函数为：

Φ ( s ) = 1 1 + G ( s ) ⋅ 1 = K τ / J y s 2 ( T m s + 1 ) ( T m 2 s + 1 ) + K τ / J y (2.5) \Phi(s) = \frac{1}{1+G(s)\cdot 1} = \frac{K_{_\tau}/J_{_y}}{ s^{^2}(T_{_m}s+1)(\frac{T_{_m}}{2} s + 1) + K_{_\tau}/J_{_y}} \tag{2.5} Φ(s)=1+G(s)⋅11=s2(Tms+1)(2Tms+1)+Kτ/JyKτ/Jy(2.5)

由于特征方程 D ( s ) = s 2 ( T m s + 1 ) ( T m 2 s + 1 ) + K τ / J y D(s)=s^{^2}(T_{_m}s+1)(\frac{T_{_m}}{2} s + 1) + K_{_\tau}/J_{_y} D(s)=s2(Tms+1)(2Tms+1)+Kτ/Jy 缺少一次项，系统不稳定。系统不稳定，其他的分析也都将失去意义。如果不十分相信系统不稳定，不妨看看其单位阶跃响应：

d = 0.225;
c_T = 1.201e-5;
C_R = 706.01;
sigma0 = 0.5;
varpi_b = 170.47;
Jy = 1.563e-2;
Tm = 0.0173;
K_tau = 2*sqrt(2)*d*c_T * (C_R^2*sigma0 + C_R*varpi_b);num = K_tau / Jy;
den =[Tm^2/2 3*Tm/2 1 0 K_tau/Jy];
t = 0:0.01:3;
theta = step(num, den, t);
plot(t, theta, 'LineWidth',2); grid on;
xlabel('时间 t (s)'); ylabel('俯仰角 \theta (rad)');title('简易系统的单位阶跃响应');

给单位期望俯仰角 θ d = 1 r a d \theta_{_d}=1 \mathrm{rad} θd=1rad，无人机将发生振荡，俯仰角越荡越大，系统果然不稳定。为了使得系统稳定并且有较好的性能，就需要设计校正装置，改善系统性能。

2.3 校正方式

常见的校正方式有：串联校正、反馈校正（并联校正）和复合校正，定义及框图分别如下：

串联校正装置一般接在系统的前向通道中，接在系统误差测量点之后和放大器之前。

反馈校正是将反并接在系统前向通路的一个或几个环节两端，形成局部反馈回路。

复合校正是在反馈回路中，假如前馈校正通路。

这三种校正方式该怎么选择搭配呢？最终参数又如何确定？这就涉及很多基础知识了。不过，此处将不一一尝试，直接使用成熟的广泛使用的方案进行分析。

2.4 控制器校正

经历了一次次考验，目前流传的四旋翼校正系统如下：

系统添加了一个单位反馈校正和两个串联校正，且第一个串联校正为比例校正，第二个串联校正为PID。现在的关键就是得出这两个校正模块的参数 K a K_{_a} Ka 与 K p , K i , K d K_{_p}, K_{_i}, K_{_d} Kp,Ki,Kd。

对于该系统，首先求出系统的开环传递函数和闭环传递函数。

对于上面的框图， H 1 ( s ) = H 2 ( s ) = 1 H_1(s)=H_2(s)=1 H1(s)=H2(s)=1，易得开环传递函数：

G ( s ) H 2 ( s ) = G 4 G 1 G 2 G 5 1 + G 1 G 2 G 5 G 3 ⋅ 1 = G 1 G 2 G 3 G 4 G 5 1 + G 1 G 2 G 5 G(s)H_{_2}(s) = G_{_4} \frac{G_{_1} G_{_2} G_{_5}}{1 + G_{_1} G_{_2} G_{_5}}G_{_3} \cdot 1=\frac{G_{_1} G_{_2} G_{_3} G_{_4} G_{_5}}{1 + G_{_1} G_{_2} G_{_5}} G(s)H2(s)=G41+G1G2G5G1G2G5G3⋅1=1+G1G2G5G1G2G3G4G5

代入并化为首一标准型可得：

G ( s ) H 2 ( s ) = K c K a ( K d s 2 + K p s + K i ) s 5 + 3 T m s 4 + ( 2 T m 2 + K c K d ) s 3 + K c K p s 2 + K c K i s (2.6) G(s)H_{_2}(s) = \frac{K_{_c}K_{_a}(K_{_d}s^{^2} + K_{_p}s + K_{_i})} {s^{^5} + \frac{3}{T_{_m}}s^{^4} + (\frac{2}{T_{_m}^{^2}} +K_{_c}K_{_d})s^{^3} + K_{_c}K_{_p}s^{^2} + K_{_c}K_{_i}s} \tag{2.6} G(s)H2(s)=s5+Tm3s4+(Tm22+KcKd)s3+KcKps2+KcKisKcKa(Kds2+Kps+Ki)(2.6)

其中， K c = K τ J y 2 T m 2 K_{_c} = \frac{K_{_\tau}}{J_{_y}} \frac{2}{T_{_m}^{^2}} Kc=JyKτTm22。系统的闭环传递函数为：

Φ ( s ) = G ( s ) 1 + G ( s ) H 2 ( s ) \Phi(s) = \frac{G(s)}{1+G(s)H_{_2}(s)} Φ(s)=1+G(s)H2(s)G(s)

代入可得：

Φ ( s ) = K c K a ( K d s 2 + K p s + K i ) s 5 + 3 T m s 4 + ( 2 T m 2 + K c K d ) s 3 + K c ( K p + K a K d ) s 2 + K c ( K i + K a K p ) s + K c K a K i (2.7) \Phi(s)= \frac{K_{_c}K_{_a}(K_{_d}s^{^2} + K_{_p}s + K_{_i})} {s^{^5} + \frac{3}{T_{_m}}s^{^4} + (\frac{2}{T_{_m}^{^2}} +K_{_c}K_{_d})s^{^3} + K_{_c}(K_{_p}+K_{_a}K_{_d})s^{^2} + K_{_c}(K_{_i}+K_{_a}K_{_p})s + K_{_c}K_{_a}K_{_i}} \tag{2.7} Φ(s)=s5+Tm3s4+(Tm22+KcKd)s3+Kc(Kp+KaKd)s2+Kc(Ki+KaKp)s+KcKaKiKcKa(Kds2+Kps+Ki)(2.7)

其中， K c = K τ J y 2 T m 2 K_{_c} = \frac{K_{_\tau}}{J_{_y}} \frac{2}{T_{_m}^{^2}} Kc=JyKτTm22 为常数； T m T_{_m} Tm 为电机的时间常数； K a K_{_a} Ka 为姿态环增益，也即比例系数， K p , K i , K d K_{_p}, K_{_i}, K_{_d} Kp,Ki,Kd 为角速度环PID系数，和 K a K_{_a} Ka 一样是待求参数。

为了确定PID参数（ K a , K p , K i , K d K_{_a}, K_{_p}, K_{_i}, K_{_d} Ka,Kp,Ki,Kd），使用根轨迹法分析并确定其取值。

三、根轨迹法分析

3.1 基础知识

最佳阻尼比
主导零点、极点
一阶二阶系统的感知（时间常数，阻尼的影响）
系统特性随着零极点变化规律（稳定性、动态性能）

3.2 根轨迹分析姿态环

3.2.1 确定内环参数下分析

现在使用姿态环和角速度环，是两个串联PID。一般的工程经验是先外环再内环，先比例后积分最后微分。因此，首先大概给定内环参数，考察外环比例系数对系统的影响。

根轨迹法使用的是开环传递函数，也即式(2.6)，重写如下：

G ( s ) H 2 ( s ) = K c K a ( K d s 2 + K p s + K i ) s 5 + 3 T m s 4 + ( 2 T m 2 + K c K d ) s 3 + K c K p s 2 + K c K i s (3.1) G(s)H_{_2}(s) = \frac{K_{_c}K_{_a}(K_{_d}s^{^2} + K_{_p}s + K_{_i})} {s^{^5} + \frac{3}{T_{_m}}s^{^4} + (\frac{2}{T_{_m}^{^2}} +K_{_c}K_{_d})s^{^3} + K_{_c}K_{_p}s^{^2} + K_{_c}K_{_i}s} \tag{3.1} G(s)H2(s)=s5+Tm3s4+(Tm22+KcKd)s3+KcKps2+KcKisKcKa(Kds2+Kps+Ki)(3.1)

取 K p = 0.1 , K i = 0 , K d = 0 K_{_p}=0.1, K_{_i}=0,K_{_d}=0 Kp=0.1,Ki=0,Kd=0，根据已知参数，可计算得 K c = 1207692 ， T m = 0.0173 K_{_c}=1207692，T_{_m}=0.0173 Kc=1207692，Tm=0.0173，代入式(3.1)，可得：

G ( s ) H 2 ( s ) = 120769 K a s 4 + 173.41 s 3 + 6682.5 s 2 + 120769 s (3.2) G(s)H_{_2}(s) = \frac{120769K_{_a}} {s^{^4} + {173.41s^{^3}} + 6682.5s^{^2} + 120769s} \tag{3.2} G(s)H2(s)=s4+173.41s3+6682.5s2+120769s120769Ka(3.2)

实际计算一下只是为了感受一下参数大概范围，实际中全部交给计算机即可。现在需要查看 K a K_{_a} Ka 增大对系统的影响，很容易作出根轨迹如下：

% 无人机结构决定的参数
d = 0.225;
c_T = 1.201e-5;
C_R = 706.01;
sigma0 = 0.5;
varpi_b = 170.47;
Jy = 1.563e-2;
Tm = 0.0173;
K_tau = 2*sqrt(2)*d*c_T * (C_R^2*sigma0 + C_R*varpi_b);
Kc = K_tau / Jy * 2 / (Tm^2);% PID系数，先设置一个值
Kp = 0.1;
Ki = 0.0;
Kd = 0.0;% 得到传递函数并绘制根轨迹
num = [Kc*Kd, Kc*Kp, Kc*Ki];                    % 分母系数
den = [1, 3/Tm, 2/Tm^2+Kc*Kd, Kc*Kp, Kc*Ki, 0]; % 分子系数（注意缺项补0）
G = tf(num, den);                               % 系统开环传递函数rlocus(G);
axis([-200 120 -120 120]);axis equal;
title('根轨迹')% [K, p] = rlocfind(G)                            % 用于找到一点对应的增益及所有极点

从中可见，有4条根轨迹，与式(3.2) 又4个极点是一致的，式(3.1) 刚好有一对零极点相消。有一个极点（绿色）总是距离其他极点很远，可以忽略其影响。当增益也即姿态环比例系数 K a K_{_a} Ka 增大到35时，系统将变得不稳定。

动态性能根据跟的分布来决定。忽略绿色极点，只看深蓝色、红色和浅蓝色三个极点。当系统增益较小时，深蓝色的极点起主导作用，主极点靠近虚轴，调节时间长，稳定性低；随着增益增大，蓝色极点左移，稳定性增强，红色和浅蓝色组成的共轭极点右移逐渐成为主导极点；继续移动，共轭极点靠近并越过虚轴，系统稳定性降低。因此随着增益增大，系统性能先得到改善，然而物极必反，增益太大系统性能又变差。

随便点几个点，三个极点的位置如下：

% 无人机结构决定的参数
% PID系数，先设置一组值
% 得到传递函数并绘制根轨迹% 以上三个部分和之前程序相同，节省篇幅，省略% 绘图
subplot(221);rlocus(G);axis([-40 20 -30, 30]);axis equal;xlabel('');ylabel('');title('')
subplot(222);rlocus(G);axis([-40 20 -30, 30]);axis equal;xlabel('');ylabel('');title('')
subplot(223);rlocus(G);axis([-40 20 -30, 30]);axis equal;xlabel('');ylabel('');title('')
subplot(224);rlocus(G);axis([-40 20 -30, 30]);axis equal;xlabel('');ylabel('');title('')% 在图上点点，用于找到一点对应的增益及所有极点
[K, p] = rlocfind(G)
[K, p] = rlocfind(G)
[K, p] = rlocfind(G)
[K, p] = rlocfind(G)

其中，左上、右上、左下、右下顺序，各组极点对应的增益分别为：1.8, 4.7, 6.3, 8.6。大概在 7 左右，三个极点距离虚轴距离一致，而且刚好在 ±45° 线附近，有最佳阻尼比。因此姿态环增益选择 7 附近的数字比较合理。

3.2.2 内环参数的影响

刚才考察姿态环比例系数时，设定角速度环PID系数分别为：0.1, 0, 0，但是 K p K_{_p} Kp 会不会根本不是 0.1 这个量级呢，如果不是在这个数量级上面会让上面姿态环的分析失去意义吗？以下稍作分析。

当 K p = 0.01 K_{_p}=0.01 Kp=0.01 和 K p = 1 K_{_p}=1 Kp=1 时，系统的根轨迹如下。可见无论姿态环增益 K a K_{_a} Ka 为多少，系统总在虚轴附近存在没有零点相消的极点，系统频率崩溃，难以稳定。所以 K p K_{_p} Kp 是在 0.1 这个量级上。

当 K p K_{_p} Kp 的取值从 0.01到1过程中，系统根轨迹动态图如下，可见 K p K_{_p} Kp 在 0.1 左右，可以找到合适的姿态环增益。

% 无人机结构决定的参数
% PID系数，先设置一组值
% 得到传递函数并绘制根轨迹% 以上三个部分和之前程序相同，节省篇幅，省略% 绘图与保存动画
for Kp = 0.01:0.01:1num = [Kc*Kd, Kc*Kp, Kc*Ki];                    % 分母系数den = [1, 3/Tm, 2/Tm^2+Kc*Kd, Kc*Kp, Kc*Ki, 0]; % 分子系数（注意缺项补0）G = tf(num, den);                               % 系统开环传递函数rlocus(G);axis([-400, 400, -400, 400]);title(['Kp = ' num2str(Kp)]);pause(0.2);% 以下为生成动画程序F = getframe(1);im = frame2im(F);[I,map] = rgb2ind(im, 256);if Kp == 0.01imwrite(I, map, 'rootlocus.gif', 'gif', 'LoopCount',Inf, 'DelayTime', 0.1);elseimwrite(I, map, 'rootlocus.gif', 'gif','WriteMode', 'append', 'DelayTime', 0.1);end
end

3.3 根轨迹分析角速度环

上一节确定了姿态环比例系数，使用根轨迹分析得出 K a K_{_a} Ka 大约为 7，并简要分析角速度环的比例系数。这一小节使用将进一步分析内环参数。

3.3.1 角速度环 P控制

式(3.1) 中，根轨迹增益刚好和姿态环比例系数 K a K_{_a} Ka 一致，直接根据开环传递函数即可得到 K a K_{_a} Ka 从0到无穷大时的根轨迹。但是现在需要绘制 K p K_{_p} Kp 变化时的根轨迹， K p K_{_p} Kp 同时存在于分母，就要使用广义根轨迹中的参数根轨迹。实际上就是把分母中含 K p K_{_p} Kp 的项移到分子中，令 K i = K d = 0 K_{_i}=K_{_d}=0 Ki=Kd=0，得到：

G ( s ) H 2 ( s ) = ( K c s + K c K a ) K p s 4 + 3 T m s 3 + ( 2 T m 2 ) s 2 (3.3) G(s)H_{_2}(s) = \frac{(K_{_c}s + K_{_c}K_{_a}) K_{_p} } {s^{^4} + \frac{3}{T_{_m}}s^{^3} + (\frac{2}{T_{_m}^{^2}} )s^{^2} } \tag{3.3} G(s)H2(s)=s4+Tm3s3+(Tm22)s2(Kcs+KcKa)Kp(3.3)

可见，系统有四个极点，一个零点，其中，两个极点在原点，零点刚好在 s = − K a s = -K_{_a} s=−Ka 处。系统的根轨迹如下图：

可见，蓝色的轨迹比较靠近虚轴，较大程度上影响系统特性。但是有一个零点，一定程度上减弱了靠近虚轴的极点的影响。绘制出最佳阻尼比时的 45°线，并且点几组特征根，如下图（四组根的增益分别为：0.07，0.10，0.116，0.12）。

首先，容易得出，蓝色和绿色的极点在原点左边比在右边好，但是不要都落在实轴上变成过阻尼。因此，在 0.07~0.12 的范围比较好。不妨使用一下时域法，绘制出单位阶跃响应。当然是闭环的阶跃响应，也即式(2.7)，重写如下：

Φ ( s ) = K c K a ( K d s 2 + K p s + K i ) s 5 + 3 T m s 4 + ( 2 T m 2 + K c K d ) s 3 + K c ( K p + K a K d ) s 2 + K c ( K i + K a K p ) s + K c K a K i (3.3) \Phi(s)= \frac{K_{_c}K_{_a}(K_{_d}s^{^2} + K_{_p}s + K_{_i})} {s^{^5} + \frac{3}{T_{_m}}s^{^4} + (\frac{2}{T_{_m}^{^2}} +K_{_c}K_{_d})s^{^3} + K_{_c}(K_{_p}+K_{_a}K_{_d})s^{^2} + K_{_c}(K_{_i}+K_{_a}K_{_p})s + K_{_c}K_{_a}K_{_i}} \tag{3.3} Φ(s)=s5+Tm3s4+(Tm22+KcKd)s3+Kc(Kp+KaKd)s2+Kc(Ki+KaKp)s+KcKaKiKcKa(Kds2+Kps+Ki)(3.3)

Kc = 1.207692e+06;
Tm = 0.0173;
Ka = 7;
t = 0:0.01:1;
for Kp = 0.07:0.01:0.12num = Kc*Ka* [Kd, Kp, Ki];den = [1, 3/Tm, 2/Tm^2+Kc*Kd, Kc*(Kp+Ka*Kd), Kc*(Ki+Ka*Kp), Kc*Ka*Ki];c = step(num, den, t);plot(t, c);hold on
end
hold off; grid on;grid minor
legend('Kp=0.07', 'Kp=0.08', 'Kp=0.09', 'Kp=0.10', 'Kp=0.11', 'Kp=0.12');
xlabel('时间 t (s)');ylabel('角度 (rad)');title('单位阶跃响应');

从图中可见，当 K p K_{_p} Kp 较小时，超调大，响应慢；当 K p K_{_p} Kp 较大时，系统处于欠阻尼，响应速度慢。 K p = 0.1 K_{_p}=0.1 Kp=0.1 是个不错的选择，但系统响应依旧不够快，调节时间大约 260ms，以下调节 K i , K p K_{_i}, K_{_p} Ki,Kp 继续提升系统性能。

3.3.2 角速度环 PI控制

取 K a = 7 , K p = 0.1 ， K d = 0 K_{_a}=7, K_{_p}=0.1， K_{_d}=0 Ka=7,Kp=0.1，Kd=0，绘制关于 K i K_{_i} Ki 的根轨迹。根据式(3.1) 及广义根轨迹，可得开环传递函数：

G ( s ) H 2 ( s ) = ( K c s + K c K a ) K i s 5 + 3 T m s 4 + 2 T m 2 s 3 + K c K p s 2 + K c K a K p s (3.4) G(s)H_{_2}(s)=\frac{(K_{_c} s + K_{_c} K_{_a}) K_{_i}}{s^{^5} + \frac{3}{T_{_m}}s^{^4} + \frac{2}{T_{_m}^{^2}}s^{^3} + K_{_c}K_{_p}s^{^2} + K_{_c}K_{_a}K_{_p}s} \tag{3.4} G(s)H2(s)=s5+Tm3s4+Tm22s3+KcKps2+KcKaKps(Kcs+KcKa)Ki(3.4)

系统有5个极点，，5条轨迹，1个极点于原点处；1个零点，其中零点于 s = − K a s=-K_{_a} s=−Ka 处。

Ka = 7;
Kp = 0.1;
num = [Kc, Kc*Ka];             % Kc, Tm 见之前程序计算一致
den = [1, 3/Tm, 2/Tm^2, Kc*Kp, Kc*Ka*Kp, 0];
G = tf(num, den);
rlocus(G);
title('Ki 根轨迹');
axis([-200, 100, -100, 100]);axis equal

可见，当 K i K_{_i} Ki 增大到 2.04 时，系统变得不稳定。系统响应主要由靠近虚轴的三个极点与一个零点确定，当 K i K_{_i} Ki 较小时，系统特性主要由深蓝色极点决定，呈过阻尼状态；随着 K i K_{_i} Ki 逐渐增大，零点深蓝色极点相互抵消，且红色与浅蓝色极点右移，逐渐成为主导极点，系统呈欠阻尼状态。

通过 rlocfind() 函数查找增益及对应的各个极点，最终取 K i = 0.3 K_{_i} = 0.3 Ki=0.3，此时各个极点（红色 +）的分布如下图：

3.3.3 角速度环 PID控制

最后调节 K d K_{_d} Kd 参数，根据以上分析，已取 K a = 7 , K p = 0.1 , K i = 0.3 K_{_a}=7, K_{_p}=0.1, K_{_i}=0.3 Ka=7,Kp=0.1,Ki=0.3。根据式(3.1)，将含 K d K_{_d} Kd 的项调节至分子，可得：

G ( s ) H 2 ( s ) = K c K a s 2 ( K d + s ) s 5 + 3 T m s 4 + 2 T m 2 s 3 + K c K p s 2 + ( K c K i + K c K a K p ) s + K c K a K i (3.5) G(s)H_{_2}(s) = \frac{K_{_c}K_{_a}s^{^2}(K_{_d} + s)} {s^{^5} + \frac{3}{T_{_m}}s^{^4} + \frac{2}{T_{_m}^{^2}} s^{^3} + K_{_c}K_{_p}s^{^2} + (K_{_c}K_{_i} + K_{_c}K_{_a}K_{_p})s + K_{_c}K_{_a}K_{_i}} \tag{3.5} G(s)H2(s)=s5+Tm3s4+Tm22s3+KcKps2+(KcKi+KcKaKp)s+KcKaKiKcKas2(Kd+s)(3.5)

可以预测，根轨迹中有3个零点，其中两个位于原点处，5个极点，5条根轨迹。

Ka = 7;
Kp = 0.1;
Ki = 0.3;
num = [Kc*Ka*Kd, Kc*Ka, 0];        % Kc, Tm 定义与前面程序一致
den = [1, 3/Tm, 2/Tm^2, Kc*Kp, Kc*Ki+Kc*Ka*Kp, Kc*Ka*Ki];
G = tf(num, den);
figure(1)
rlocus(G);
title('Kd 根轨迹');
axis([-135, 25, -40, 40]);axis equal

可见，当 K d K_{_d} Kd 大于 0.33 时，系统将变得不稳定。系统特性主要由右侧三个极点及原点处零点决定，可见， K d K_{_d} Kd 越小越好，但是如果超调量较大，可以适当增大 K d K_{_d} Kd，实轴上靠近虚轴的极点使得系统朝着欠阻尼方向发展，超调量减小。

不妨看看单位阶跃响应，取 K d K_{_d} Kd 0到 0.01，单位阶跃响应如下：

可见，取 K d K_{_d} Kd 于 0~0.002 之间都是不错的，不妨取 0.001。

3.3.4 根轨迹法确定取值及结果分析

综上，取 K a = 7 , K p = 0.1 , K i = 0.3 , K d = 0.001 K_{_a}=7, K_{_p}=0.1, K_{_i}=0.3, K_{_d}=0.001 Ka=7,Kp=0.1,Ki=0.3,Kd=0.001，系统的单位阶跃响应如下：

可见，系统响应较快，调节时间大约 240ms，超调量较小，是一组不错的 PID组合。整个流程主要使用根轨迹确定系统稳定时的参数取值范围，比较难以分析时，结合单位阶跃响应确定参数。

四、单个PID与串级PID比较

第二节已经分析，不适用 PID校正装置，系统将不稳定，第三节采用广泛使用串级PID，取得了不错的控制效果。那么，可以只使用一个PID可行吗，效果如何呢？学了根轨迹法，不妨比较比较。

只是用一个PID，系统结构图如下：

系统的开环传递函数为：

G ( s ) H ( s ) = K c ( K d s 2 + K p s + K i ) s 5 + 3 T m s 4 + 2 T m 2 s 3 (4.1) G(s)H(s) = \frac{K_{_c} (K_{_d}s^{^2} + K_{_p}s + K_{_i})} {s^{^5} + \frac{3}{T_{_m}}s^{^4} + \frac{2}{T_{_m}^{^2}} s^{^3}} \tag{4.1} G(s)H(s)=s5+Tm3s4+Tm22s3Kc(Kds2+Kps+Ki)(4.1)

闭环传递函数为：

Φ ( s ) = K c ( K d s 2 + K p s + K i ) s 5 + 3 T m s 4 + 2 T m 2 s 3 + K c K d s 2 + K c K p s + K c K i (4.2) \Phi(s) = \frac{K_{_c} (K_{_d}s^{^2} + K_{_p}s + K_{_i})} {s^{^5} + \frac{3}{T_{_m}}s^{^4} + \frac{2}{T_{_m}^{^2}} s^{^3} + K_{_c} K_{_d}s^{^2} + K_{_c}K_{_p}s + K_{_c}K_{_i}} \tag{4.2} Φ(s)=s5+Tm3s4+Tm22s3+KcKds2+KcKps+KcKiKc(Kds2+Kps+Ki)(4.2)

易得，只有当 K p , K d K_{_p}, K_{_d} Kp,Kd 都不为零时，系统的特征方程才没有零系数项，系统才稳定。通过初步尝试，调节 K p K_{_p} Kp 时先令 K i = 0.01 K_{_i}=0.01 Ki=0.01， K d K_{_d} Kd 从在0~0.2间取值。

首先绘制关于 K p K_{_p} Kp 的根轨迹，根据参数根轨迹方法，可得传递函数变化为：

G ( s ) H ( s ) = K c K p s s 5 + 3 T m s 4 + 2 T m 2 s 3 + K c K d s 2 + K c K i (4.2) G(s)H(s) = \frac{K_{_c} K_{_p}s } {s^{^5} + \frac{3}{T_{_m}}s^{^4} + \frac{2}{T_{_m}^{^2}} s^{^3}+ K_{_c} K_{_d}s^{^2} + K_{_c}K_{_i}} \tag{4.2} G(s)H(s)=s5+Tm3s4+Tm22s3+KcKds2+KcKiKcKps(4.2)

Ki = 0.01;for Kd = 0:0.02:0.2num = [Kc, 0];den = [1, 3/Tm, 2/Tm^2, Kc*Kd, 0, Ki];G = tf(num, den);rlocus(G); hold ontitle(['Kd = ' num2str(Kd)]);axis([-200, 150, -150, 150]);axis equal;plot([-200, 0], [200, 0], '-.');hold offpause(0.1)% 以下为生成动画程序F = getframe(1);im = frame2im(F);[I,map] = rgb2ind(im, 256);if Kd == 0imwrite(I, map, 'rootlocus2.gif', 'gif', 'LoopCount',Inf, 'DelayTime', 0.3);elseimwrite(I, map, 'rootlocus2.gif', 'gif','WriteMode', 'append', 'DelayTime', 0.3);end
end
hold off

取 K d = 0.1 K_{_d}=0.1 Kd=0.1 ， K p K_{p} Kp 的根轨迹如下：

最终， K p = 0.725 , K i = 0.01 , K d = 0.1 K_{_p}=0.725, K_{_i}=0.01, K_{_d}=0.1 Kp=0.725,Ki=0.01,Kd=0.1，单个PID的单位阶跃响应与串级PID的单位阶跃响应比较如下：

其中，蓝色为单个PID效果，橙色为串级PID效果，中间两根点画线为 5 % 5\% 5% 误差带。可见，单个PID上升时间更小，但是超调量和调节时间都更大，串级PID优势更大。

五、滚转角、偏航角PID参数确定

5.1 滚转角PID参数

滚转角和俯仰角一样，采用串级PID，由于两者的相似性，取 K a = 7 , K p = 0.1 , K i = 0.3 , K d = 0.001 K_{_a}=7,K_{_p}=0.1, K_{_i}=0.3, K_{_d}=0.001 Ka=7,Kp=0.1,Ki=0.3,Kd=0.001，不再赘述。

5.2 偏航角PID参数

遥控器一般直接给俯仰角、滚转角及偏航角角速度。因此，遥控器直接给出期望角速度，与反馈角速度进行PID计算。而且偏航角与水平姿态角的机理并不完全相同，需要单独设计PID控制器。

5.2.1 偏航角数学模型、结构图

根据式(1.1) 与 (1.2) 可得，忽略空气阻力，假设水平姿态角为0，从电机油门到偏航角角速度的数学模型为：

{ T m ϖ 1 ′ + ϖ 1 = C R σ 1 + ϖ b T m ϖ 2 ′ + ϖ 2 = C R σ 2 + ϖ b T m ϖ 3 ′ + ϖ 3 = C R σ 3 + ϖ b T m ϖ 4 ′ + ϖ 4 = C R σ 4 + ϖ b τ z = c M ( ϖ 1 2 − ϖ 2 2 + ϖ 3 2 − ϖ 4 2 ) J z ω z ′ = τ z (5.1) \begin{cases} T_{_m} \varpi'_{_1} + \varpi_{_1} = C_{_R} \sigma_{_1} + \varpi_{_b} \\ T_{_m} \varpi'_{_2} + \varpi_{_2} = C_{_R} \sigma_{_2} + \varpi_{_b} \\ T_{_m} \varpi'_{_3} + \varpi_{_3} = C_{_R} \sigma_{_3} + \varpi_{_b} \\ T_{_m} \varpi'_{_4} + \varpi_{_4} = C_{_R} \sigma_{_4} + \varpi_{_b} \\ \tau_{_z} = c_{_M}(\varpi_{_1}^{^2} -\varpi_{_2}^{^2} + \varpi_{_3}^{^2} -\varpi_{_4}^{^2} ) \\ J_{_z} \omega'_{_{z}} = \tau_{_z} \end{cases} \tag{5.1} ⎩⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎧Tmϖ1′+ϖ1=CRσ1+ϖbTmϖ2′+ϖ2=CRσ2+ϖbTmϖ3′+ϖ3=CRσ3+ϖbTmϖ4′+ϖ4=CRσ4+ϖbτz=cM(ϖ12−ϖ22+ϖ32−ϖ42)Jzωz′=τz(5.1)

偏航角增大时，1号3号电机加速转动，2号4号电机减速转动，转化为以油门变化量为输入的模型为：

τ z = C z ( 1 − 2 e − t T m + e − 2 t T m ) Δ σ (5.2) \tau_{_z} =C_{_{z}} (1 - 2e^{-\frac{t}{T_m}} + e^{-\frac{2t}{T_m}}) \Delta \sigma \tag{5.2} τz=Cz(1−2e−Tmt+e−Tm2t)Δσ(5.2)

其中， C z = 4 c M ( C R 2 σ 0 + C R ϖ b ) C_{_z}=4c_{_M}(C_{_R}^{^2} \sigma_{_0}+C_{_R}\varpi_{_b}) Cz=4cM(CR2σ0+CRϖb)， c M c_{_M} cM 为扭力矩系数； Δ σ \Delta \sigma Δσ 为油门差，取值范围为 -0.5~0.5。

偏航角通道的结构图如下：

使用串联PID校正，结构图如下：

5.2.2 系统传递函数

系统的开环传递函数为：

G ( s ) H ( s ) = K c ( K d s 2 + K p s + K i ) s 4 + 3 T m s 3 + 2 T m 2 s 2 (5.3) G(s)H(s) = \frac{K_{_c} (K_{_d} s^{^2} + K_{_p}s + K_{_i}) } {s^{^4} + \frac{3}{T_{_m}}s^{^3} + \frac{2}{T_{_m}^{^2}} s^{^2}} \tag{5.3} G(s)H(s)=s4+Tm3s3+Tm22s2Kc(Kds2+Kps+Ki)(5.3)

闭环传递函数为：

Φ ( s ) = K c ( K d s 2 + K p s + K i ) s 4 + 3 T m s 3 + ( 2 T m 2 + K c K d ) s 2 + K c K p s + K c K i (5.4) \Phi(s) = \frac{K_{_c} (K_{_d} s^{^2} + K_{_p}s + K_{_i}) } {s^{^4} + \frac{3}{T_{_m}}s^{^3} + (\frac{2}{T_{_m}^{^2}} + K_{_c}K_{_d}) s^{^2} + K_{_c}K_{_p}s + K_{_c}K_{_i}} \tag{5.4} Φ(s)=s4+Tm3s3+(Tm22+KcKd)s2+KcKps+KcKiKc(Kds2+Kps+Ki)(5.4)

其中， K c = K z J z 2 T m 2 K_{_c}= \frac{K_{_z}}{J_{_z}} \frac{2}{T_{_m}^{^2}} Kc=JzKzTm22 为无人机结构决定的常数； T m T_{_m} Tm 为电机时间常数； K p , K i , K d K_{_p}, K_{_i}, K_{_d} Kp,Ki,Kd 为待求 PID 参数。

5.2.3 比例系数P 根轨迹

根据式(5.3) 得到关于 K p K_{_p} Kp 的参数根轨迹函数为：

G ( s ) H ( s ) = K p K c s s 4 + 3 T m s 3 + ( 2 T m 2 + K c K d ) s 2 + K c K i (5.5) G(s)H(s) = \frac{ K_{_p} K_{_c}s } {s^{^4} + \frac{3}{T_{_m}}s^{^3} + (\frac{2}{T_{_m}^{^2}} + K_{_c} K_{_d} )s^{^2} + K_{_c} K_{_i} } \tag{5.5} G(s)H(s)=s4+Tm3s3+(Tm22+KcKd)s2+KcKiKpKcs(5.5)

令 K i = K d = 0 K_{_i}=K_{_d}=0 Ki=Kd=0，系统是一个三阶系统，三个极点，无零点。 K p K_{_p} Kp 的根轨迹如下：

c_M = 1.574e-7;
C_R = 706.01;
sigma0 = 0.5;
varpi_b = 170.47;
Jz = 2.562e-2;
Tm = 0.0173;
Kz = 4*c_M*(C_R^2*sigma0 + C_R*varpi_b);
Kc = Kz / Jz * 2 / Tm^2;Ki = 0;
Kd = 0;
num = [Kc, 0];
den = [1, 3/Tm, 2/Tm^2+Kc*Kd, 0, Kc*Ki];
G = tf(num, den);
rlocus(G);
axis([-200, 50, -120, 120]);axis equal;
hold on;
plot([-150, 0], [150,0], '-.');hold off
title('偏航角 Kp 根轨迹');

可见，当 K p K_{_p} Kp 较小时，系统处于欠阻尼状态； K p K_{_p} Kp 增大到 19时，系统变得不稳定；最终取最佳阻尼比时的 K p = 2.07 K_{_p}=2.07 Kp=2.07 。

5.2.4 积分系数 I

根据式(5.3) 得到关于 K i K_{_i} Ki 的参数根轨迹函数为：

G ( s ) H ( s ) = K i K c s 4 + 3 T m s 3 + ( 2 T m 2 + K c K d ) s 2 + K c K p s (5.6) G(s)H(s) = \frac{ K_{_i} K_{_c} } {s^{^4} + \frac{3}{T_{_m}}s^{^3} + (\frac{2}{T_{_m}^{^2}} + K_{_c} K_{_d} )s^{^2} + K_{_c} K_{_p}s } \tag{5.6} G(s)H(s)=s4+Tm3s3+(Tm22+KcKd)s2+KcKpsKiKc(5.6)

取 K p = 2.07 , K d = 0 K_{_p}=2.07, K_{_d}=0 Kp=2.07,Kd=0，得到一个四阶系统，一个极点在原点，无零点。 K i K_{_i} Ki 的根轨迹如下：

Kp = 2.07;
Kd = 0;
num = [Kc];
den = [1, 3/Tm, 2/(Tm^2)+Kc*Kd, Kc*Kp, 0];
G = tf(num, den);
rlocus(G);
axis([-160, 40, -80, 80]);axis equal;
hold on;
plot([-150, 0], [150,0], '-.');hold off
title('偏航角 Ki 根轨迹');

有四条跟轨迹，深蓝色、红色和浅蓝色上的极点为主导极点。 K i K_{_i} Ki 较小时，深蓝色轨迹上点起主导作用，系统接近过阻尼； K i K_{_i} Ki 增大到72时，系统变得不稳定。局部放大，选择最佳阻尼比附近的点，结果如下：

不妨取 K i = 15 K_{_i}=15 Ki=15。

5.2.5 微分系数 D

K d K_{_d} Kd 的参数跟轨迹方程如下：

G ( s ) H ( s ) = K d K c s 2 s 4 + 3 T m s 3 + 2 T m 2 s 2 + K c K p s + K c K i (5.7) G(s)H(s) = \frac{ K_{_d} K_{_c}s^{^2} } {s^{^4} + \frac{3}{T_{_m}}s^{^3} + \frac{2}{T_{_m}^{^2}} s^{^2} + K_{_c} K_{_p}s + K_{_c} K_{_i}} \tag{5.7} G(s)H(s)=s4+Tm3s3+Tm22s2+KcKps+KcKiKdKcs2(5.7)

取 K p = 2.07 ， K i = 15 K_{_p}=2.07， K_{_i}=15 Kp=2.07，Ki=15，系统为四阶，有两个零点在原点。 K d K_{_d} Kd 的跟轨迹如下：

考虑到各个极点的位置关系，最终取 K d = 0.01 K_{_d}=0.01 Kd=0.01。

5.2.6 单位阶跃响应

作为比较，作出 P、PI和 PID 作用时的系统单位阶跃响应，如下：

可见，只是用 P 环节取得不错的效果，使用 I、D环节使得超调量太大，还增加调节时间，因此，取 K p = 2.07 , K i = 0 , K d = 0 K_{_p}=2.07, K_{_i}=0, K_{_d}=0 Kp=2.07,Ki=0,Kd=0 。

六、结果分析

6.1 俯仰角时域复域比较

经过以上工作，使用跟轨迹法分析得出外环比例系数 K a = 7 K_{_a}=7 Ka=7，内环PID系数， K p = 0.1 , K i = 0.3 , K p = 0.001 K_{_p}=0.1,K_{_i}=0.3,K_{_p}=0.001 Kp=0.1,Ki=0.3,Kp=0.001，这组参数真的可行吗？以下将调节得到的参数放入无人机时域模型中进行分析比较：

% 使用时域模型结果（Theta(:,1)俯仰角； Theta(:,2)滚转角；Theta(:,3)偏航角角速度）
atti_K = [7, 0, 0; 7, 0, 0; 0, 0, 0];
rate_K = [0.1, 0.3, 0.001; 0.1, 0.3, 0.001; 2.07, 0, 0];
Theta_d = [0, 1];                   % 期望水平姿态角 [roll_d, pitch_d]
omega_zd = 0;                      % 期望偏航角角速度N = 10000;
global dt;
dt = 1e-4;                          % 时间刻度 10ms
t = 0:dt:dt*(N-1);sigma = zeros(N,4);                 % 四个电机油门
varpi = zeros(N, 4);                % 电机转速
force = zeros(N, 1);                % 螺旋桨产生的升力
tau = zeros(N, 3);                  % 机体绕 x,y,z 三轴的力矩
omega_b = zeros(N, 3);              % 机体角速度
Theta = zeros(N, 3);                % 姿态
dTheta = zeros(N, 3);               % 姿态变化率omega_d = zeros(N, 3);              % 期望角速度（由外环PID得出，实际只绕 x,y 轴角速度）
atti_e = zeros(N, 3);               % 姿态误差
atti_ei = zeros(N, 3);              % 姿态角累计误差omega_m = zeros(N, 3);              % 测量的角速度
rate_ei = zeros(N, 3);              % 角速度误差累计
rate_e = zeros(N, 3);               % 角速度误差
tau_d = zeros(N, 3);                % 期望力矩k=1;
for t1=0:dt:(N-2)*dtk = k+1;% 动力单元模型varpi(k, 1) = power(sigma(k-1, 1), varpi(k-1, 1));       % 电机1转速varpi(k, 2) = power(sigma(k-1, 2), varpi(k-1, 2));       % 电机2转速varpi(k, 3) = power(sigma(k-1, 3), varpi(k-1, 3));       % 电机3转速varpi(k, 4) = power(sigma(k-1, 4), varpi(k-1, 4));       % 电机4转速% 控制效率模型[force(k), tau(k,:)] = efficiency(varpi(k,:));      % 力与力矩% 姿态动力学模型omega_b(k, :) = attitude_dynamics(tau(k, :), omega_b(k-1, :));    % 机体角速度% 姿态运动学模型[Theta(k, :), dTheta(k, :)] = attitude_kinematics(omega_b(k,:), Theta(k-1, :));% 外环PID（输出目标角速度）[omega_d(k, 1), atti_e(k, 1), atti_ei(k,1)] = locpid(atti_K(1,1), atti_K(1,2), atti_K(1,3), Theta_d(1), Theta(k, 1), atti_ei(k-1, 1), atti_e(k-1, 1));    % roll PID -- 绕x轴[omega_d(k, 2), atti_e(k, 2), atti_ei(k,2)] = locpid(atti_K(2,1), atti_K(2,2), atti_K(2,3), Theta_d(2), Theta(k, 2), atti_ei(k-1, 2), atti_e(k-1, 2));    % pitch PID -- 绕x轴% 内环PID（输出期望力矩）omega_m(k,:) = dTheta(k, :);[tau_d(k,1), rate_e(k,1), rate_ei(k,1)] = locpid(rate_K(1,1), rate_K(1,2), rate_K(1,3), omega_d(k,1), omega_m(k,1), rate_ei(k-1,1), rate_e(k-1,1));        % rate x PID -- tau_x[tau_d(k,2), rate_e(k,2), rate_ei(k,2)] = locpid(rate_K(2,1), rate_K(2,2), rate_K(2,3), omega_d(k,2), omega_m(k,2), rate_ei(k-1,2), rate_e(k-1,2));        % rate y PID -- tau_y[tau_d(k,3), rate_e(k,3), rate_ei(k,3)] = locpid(rate_K(3,1), rate_K(3,2), rate_K(3,3), omega_zd, omega_m(k,3), rate_ei(k-1,3), rate_e(k-1,3));            % rate z PID -- tau_z% 期望拉力fd = 0.5;                                       % 默认0.5% 控制分配tau_d(k,:) = tau_d(k, :) ./ 2;                  % 除以2是因为：使用复域模型计算时，使用了油门差（1号3号油门差），回到时域，1号增一半，3号减一半即可sigma(k, :) = [-1,  1,  1, 1; 1,  1, -1, 1; 1, -1,  1, 1; -1, -1, -1, 1] * [tau_d(k,1); tau_d(k,2); tau_d(k,3); fd];
endplot(t, Theta(:,2),'LineWidth', 2);hold on          % 绘制时域得出的俯仰角% 使用频域模型结果（仅俯仰角）
% 无人机结构决定的参数
d = 0.225;
c_T = 1.201e-5;
C_R = 706.01;
sigma0 = 0.5;
varpi_b = 170.47;
Jy = 1.563e-2;
Tm = 0.0173;
K_tau = 2*sqrt(2)*d*c_T * (C_R^2*sigma0 + C_R*varpi_b);
Kc = K_tau / Jy * 2 / (Tm^2);% 传递函数与单位阶跃响应
Ka = atti_K(1,1);
Kp = rate_K(1,1); Ki = rate_K(1,2); Kd = rate_K(1,3);
num = Kc*Ka* [Kd, Kp, Ki];                                             %闭环分子
den = [1, 3/Tm, 2/Tm^2+Kc*Kd, Kc*(Kp+Ka*Kd), Kc*(Ki+Ka*Kp), Kc*Ka*Ki]; %闭环分母
c = step(num, den, t);plot(t, c, 'LineWidth', 2);  hold off       % 绘制传递函数计算得到的俯仰角
grid on;grid minor;
legend('时域模型', '复域模型');
xlabel('时间 t (s)');ylabel('俯仰角 \theta (rad)');title('俯仰角单位阶跃响应');%% 动力单元模型
% 输入：油门大小 sigma（0-1）
%       电机上一时刻的转速（rad/s）
% 输出：此时刻电机转速（rad/s）
function varpi = power(sigma, varpi_last)global dt;Tm = 0.0173;                    % 电机时间常数C_R = 706.01;                   % 油门增大1，电机转速变化(RPM)varpi_b = 170.47;               % 零占空比时电机转速(RPM)dvarpi = (C_R * sigma + varpi_b - varpi_last) / Tm * dt;varpi = varpi_last + dvarpi;
end%% 控制效率模型
% 输入：四个螺旋桨转速 varpi（rad/s）
% 输出：升力 force （N）
%      三轴力矩 tau（N*m）function [force, tau] = efficiency(varpi)c_M = 1.574 * 1e-7;             % N*m/(rad/s)^2c_T = 1.201 * 1e-5;             % N/(rad/s)^2d = 0.225;                 % 螺旋桨电机轴到机体中心距离force  = c_T *                  ( varpi(1)^2 + varpi(2)^2 + varpi(3)^2 + varpi(4)^2);      % 升力tau(1) = sqrt(2)/2 * d * c_T *  (-varpi(1)^2 + varpi(2)^2 + varpi(3)^2 - varpi(4)^2);      % tau_xtau(2) = sqrt(2)/2 * d * c_T *  ( varpi(1)^2 + varpi(2)^2 - varpi(3)^2 - varpi(4)^2);      % tau_ytau(3) = c_M *                  ( varpi(1)^2 - varpi(2)^2 + varpi(3)^2 - varpi(4)^2);      % tau_z
end%% 姿态动力学模型
% 输入：力矩 tau
%       上一次机体角速度 omega_last
% 输出：机体角速度 omega_b
function omega_b = attitude_dynamics(tau, omega_last)global dt;Jx = 1.563e-2;Jy = 1.563e-2;Jz = 2.636e-2;                  % 无人机转动惯量Cdm = 9.012e-3;% domega(1) = ((Jy - Jz) * omega_last(2) * omega_last(3) + tau(1)  - Cdm * omega_last(1) * abs(omega_last(1))) / Jx * dt;% domega(2) = ((Jz - Jx) * omega_last(3) * omega_last(1) + tau(2)  - Cdm * omega_last(2) * abs(omega_last(2))) / Jy * dt;% domega(3) = ((Jx - Jy) * omega_last(1) * omega_last(2) + tau(3) - Cdm * omega_last(3) * abs(omega_last(3))) / Jz * dt;% 线性化假设domega(1) =  tau(1) / Jx * dt;domega(2) =  tau(2) / Jy * dt;domega(3) =  tau(3) / Jz * dt;omega_b = omega_last + domega;
end%% 姿态运动学模型——欧拉角方式
% 输入：机体角速度 omega_b
%       上一次姿态角 Theta_last
% 输出：姿态角 Theta = [roll, pitch, yaw]
%       姿态变化率 dTheta
function [Theta, dTheta] = attitude_kinematics(omega_b, Theta_last)global dt;%W = [1  tan(Theta_last(2)) * sin(Theta_last(1))    tan(Theta_last(2)) * cos(Theta_last(1))%     0      cos(Theta_last(1))                      -sin(Theta_last(1))%     0  sin(Theta_last(1)) / cos(Theta_last(2))      cos(Theta_last(1)) / cos(Theta_last(2))];W = [1 0 0; 0 1 0; 0 0 1]; % 线性假设简化dTheta = W * omega_b';Theta = Theta_last + dTheta' * dt;
end%% PID
% 输入：PID参数 Kp, Ki, Kd;
%       d——目标值
%       m——测量值
%       e_i —— 误差累计
%       e_last —— 上次误差
% 输出：output —— 此次输出
%       e —— 此次误差
function [output,e, e_i2] = locpid(Kp, Ki, Kd, d, m, e_i, e_last)global dt;e       = d - m;e_i2     = e_i + e;e_d     = e - e_last;output  = Kp*e + Ki*e_i2*dt + Kd*e_d/dt;
end

以上两条线中，砖红色为在复频域得到传递函数，进而得到单位阶跃响应；深蓝色为直接在时域根据微分方程，输入期望角度 1rad，系统输出的俯仰角。理论上，如果时域频域变化之间没有误差，两条线应重合。但是稍有差异，经过认真检查程序，仍未找到具体原因，猜测是由于微分方程采用数值积分计算时精度不够导致的。如果读者发现程序谬误之处，欢迎指正。

6.2 耦合关系的影响

上面求解俯仰角参数时，假设滚转角和偏航角都为 0，但实际中是不存在的。比如现在同时期望俯仰角和滚转角达到 1rad，两者就相互耦合。

首先修改程序里的姿态动力学模型，考虑空气阻力及角速度之间的耦合；其次其次修改姿态运动学模型，不使用对角阵描述姿态变化率和机体角速度的关系，而是使用它们之间转换的矩阵。最后修改目标值，俯仰角和滚转角都期望达到 1rad，也即57.3°，只使用时域描述，结果如下：

可见，调节的PID效果很差。当然，这不是我们的错，非线性系统本身就很复杂。幸运的是，大多数时候，无人机的机体角度只有几度，10°已经绰绰有余。此时效果还不错，PID依旧可用：

– 完 –