在我上篇博客《四旋翼无人机Matlab建模》中，我建立了四旋翼的模型，并在simulink中搭建了仿真，但并没有设计控制器。本章便针对四旋翼设计最常见的串级PID控制器，本篇文章主要从两个部分讲述控制器设计，一是理论部分，二是实践部分，阅读本文需要了解PID算法，推荐阅读https://blog.csdn.net/nemol1990/article/details/45131603
在四旋翼的控制中，最常见也是最方便使用的控制便是PID控制。主要是因为PID算法简单，适应性强，不依赖模型。而四旋翼无人机是一个四输入六输出的系统，系统模型参见我上篇博客《四旋翼无人机Matlab建模》，对于该模型，我们设计串级PID，下面我先给出四旋翼模型：
{
x
¨
=
u
1
m
(
s
ψ
s
ϕ
+
c
ψ
c
ϕ
s
θ
)
−
k
1
m
x
˙
+
d
1
y
¨
=
u
1
m
(
s
ψ
s
θ
c
ϕ
−
c
ψ
s
ϕ
)
−
k
2
m
y
˙
+
d
2
z
¨
=
u
1
m
c
θ
c
ϕ
−
g
−
k
3
m
z
˙
+
d
3
ϕ
¨
=
θ
˙
ψ
˙
(
I
y
y
−
I
z
z
I
x
x
)
+
u
2
I
x
x
+
d
4
θ
¨
=
ϕ
˙
ψ
˙
(
I
z
z
−
I
x
x
I
y
y
)
+
u
3
I
y
y
+
d
5
ψ
¨
=
ϕ
˙
θ
˙
(
I
x
x
−
I
y
y
I
z
z
)
+
u
4
I
z
z
+
d
6
(
1
)
\left\{\begin{matrix} \ddot{x}=\frac{ u_{1}}{m}(s\psi s\phi +c\psi c\phi s\theta )-\frac{k_{1}}{m}\dot{x}+d_{1}\\ \ddot{y}=\frac{ u_{1}}{m}(s\psi s\theta c\phi - c\psi s\phi)-\frac{k_{2}}{m}\dot{y}+d_{2}\\ \ddot{z}=\frac{ u_{1}}{m}c\theta c\phi -g-\frac{k_{3}}{m}\dot{z}+d_{3}\\ \ddot{\phi }=\dot{\theta }\dot{\psi }(\frac{I_{yy}-I_{zz}}{I_{xx}})+\frac{u_{2}}{I_{xx}}+d_{4}\\ \ddot{\theta }=\dot{\phi }\dot{\psi }(\frac{I_{zz}-I_{xx}}{I_{yy}})+\frac{u_{3}}{I_{yy}}+d_{5}\\ \ddot{\psi }=\dot{\phi }\dot{\theta }(\frac{I_{xx}-I_{yy}}{I_{zz}})+\frac{u_{4}}{I_{zz}}+d_{6} \end{matrix}\right.(1)
⎩⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎧x¨=mu1(sψsϕ+cψcϕsθ)−mk1x˙+d1y¨=mu1(sψsθcϕ−cψsϕ)−mk2y˙+d2z¨=mu1cθcϕ−g−mk3z˙+d3ϕ¨=θ˙ψ˙(IxxIyy−Izz)+Ixxu2+d4θ¨=ϕ˙ψ˙(IyyIzz−Ixx)+Iyyu3+d5ψ¨=ϕ˙θ˙(IzzIxx−Iyy)+Izzu4+d6(1)具体建立过程可见我上一篇博客，在这里我们同样忽略干扰
d
i
d_{i}
di,即
d
i
=
0
,
i
=
1...6
d_{i}=0,i=1...6
di=0,i=1...6,忽略空气阻力，即
k
i
=
0
k_{i}=0
ki=0,只为方便理解，有需要可以自己最后在模型里加上。
首先由PID原理设计四旋翼速度环，即
x
¨
=
K
p
x
(
x
0
−
x
)
+
K
i
x
∫
(
x
0
−
x
)
d
t
+
K
d
x
d
(
x
0
−
x
)
d
t
(
2
)
\ddot{x}=K_{px}(x_{0}-x)+K_{ix}\int (x_{0}-x)dt+K_{dx}\frac{d(x_{0}-x)}{dt}(2)
x¨=Kpx(x0−x)+Kix∫(x0−x)dt+Kdxdtd(x0−x)(2)
y
¨
=
K
p
y
(
y
0
−
y
)
+
K
i
y
∫
(
y
0
−
y
)
d
t
+
K
d
y
d
(
y
0
−
y
)
d
t
(
3
)
\ddot{y}=K_{py}(y_{0}-y)+K_{iy}\int (y_{0}-y)dt+K_{dy}\frac{d(y_{0}-y)}{dt}(3)
y¨=Kpy(y0−y)+Kiy∫(y0−y)dt+Kdydtd(y0−y)(3)
z
¨
=
K
p
z
(
z
0
−
z
)
+
K
i
z
∫
(
z
0
−
z
)
d
t
+
K
d
z
d
(
z
0
−
z
)
d
t
(
4
)
\ddot{z}=K_{pz}(z_{0}-z)+K_{iz}\int (z_{0}-z)dt+K_{dz}\frac{d(z_{0}-z)}{dt}(4)
z¨=Kpz(z0−z)+Kiz∫(z0−z)dt+Kdzdtd(z0−z)(4)其中
x
0
,
y
0
,
z
0
x_{0},y_{0},z_{0}
x0,y0,z0是期望位置，
x
,
y
,
z
x,y,z
x,y,z是实际位置，此为速度环PID公式
由于最后控制四旋翼的控制量是
u
1
,
u
2
,
u
3
,
u
4
u_{1},u_{2},u_{3},u_{4}
u1,u2,u3,u4,而
x
,
y
,
z
x,y,z
x,y,z与
u
1
u_{1}
u1有关，因此可以计算出
u
1
u_{1}
u1的表达式
u
1
=
m
x
¨
2
+
y
¨
2
+
(
z
¨
+
g
)
2
(
5.1
)
u_{1}=m\sqrt{\ddot{x}^2+\ddot{y}^2+(\ddot{z}+g)^2}(5.1)
u1=mx¨2+y¨2+(z¨+g)2
(5.1)
u
1
=
m
z
¨
c
θ
c
ϕ
(
5.2
)
u_{1}=\frac{m\ddot{z}}{c\theta c\phi }(5.2)
u1=cθcϕmz¨(5.2)
虽然式5.1与5.2都可以计算u1的值，但是式5.1中涉及到
x
¨
,
y
¨
,
z
¨
\ddot{x},\ddot{y},\ddot{z}
x¨,y¨,z¨，存在耦合，会导致PID参数调节困难，因此计算
u
1
u_{1}
u1采取式5.2。
这一部分在simulink中搭建如下
接下来开始姿态环设计，我们都知道，当四旋翼保持一定的横滚角或俯仰角时，四旋翼一定不能静止，一定向着x或y方向运动，即你可以理解为这两个姿态角的作用在一定程度上相当于速度的作用，改变这两个角可以改变四旋翼的飞行速度，从而改变无人机位置。从这里可以看到，x，y与
ϕ
,
θ
\phi, \theta
ϕ,θ之间存在耦合，因此
ϕ
,
θ
\phi, \theta
ϕ,θ的初始姿态
ϕ
0
,
θ
0
\phi_{0}, \theta_{0}
ϕ0,θ0需要通过模型(1)得到，即
ϕ
0
=
a
r
c
s
i
n
[
m
(
x
¨
s
i
n
ψ
−
y
¨
c
o
s
ψ
)
/
u
1
]
(
6
)
\phi _{0}=arcsin[m(\ddot{x}sin\psi-\ddot{y}cos\psi)/u_{1}](6)
ϕ0=arcsin[m(x¨sinψ−y¨cosψ)/u1](6)
θ
0
=
a
r
c
s
i
n
[
m
(
x
¨
c
o
s
ψ
+
y
¨
s
i
n
ψ
)
/
(
u
1
c
o
s
ϕ
)
]
(
7
)
\theta_{0}=arcsin[m(\ddot{x}cos\psi+\ddot{y}sin\psi)/(u_{1}cos\phi)](7)
θ0=arcsin[m(x¨cosψ+y¨sinψ)/(u1cosϕ)](7)之后仿照(2),(3),(4)式可得姿态环，如下
ϕ
¨
=
K
p
ϕ
(
ϕ
0
−
ϕ
)
+
K
i
ϕ
∫
(
ϕ
0
−
ϕ
)
d
t
+
K
d
ϕ
d
(
ϕ
0
−
ϕ
)
d
t
(
8
)
\ddot{\phi}=K_{p\phi}(\phi_{0}-\phi)+K_{i\phi}\int (\phi_{0}-\phi)dt+K_{d\phi}\frac{d(\phi_{0}-\phi)}{dt}(8)
ϕ¨=Kpϕ(ϕ0−ϕ)+Kiϕ∫(ϕ0−ϕ)dt+Kdϕdtd(ϕ0−ϕ)(8)
θ
¨
=
K
p
θ
(
θ
0
−
θ
)
+
K
i
θ
∫
(
θ
0
−
θ
)
d
t
+
K
d
θ
d
(
θ
0
−
θ
)
d
t
(
9
)
\ddot{\theta}=K_{p\theta}(\theta_{0}-\theta)+K_{i\theta}\int (\theta_{0}-\theta)dt+K_{d\theta}\frac{d(\theta_{0}-\theta)}{dt}(9)
θ¨=Kpθ(θ0−θ)+Kiθ∫(θ0−θ)dt+Kdθdtd(θ0−θ)(9)
ψ
¨
=
K
p
ψ
(
ψ
0
−
ψ
)
+
K
i
ψ
∫
(
ψ
0
−
ψ
)
d
t
+
K
d
ψ
d
(
ψ
0
−
ψ
)
d
t
(
10
)
\ddot{\psi}=K_{p\psi}(\psi_{0}-\psi)+K_{i\psi}\int (\psi_{0}-\psi)dt+K_{d\psi}\frac{d(\psi_{0}-\psi)}{dt}(10)
ψ¨=Kpψ(ψ0−ψ)+Kiψ∫(ψ0−ψ)dt+Kdψdtd(ψ0−ψ)(10)然后根据（1）式后三个式子可以计算出
u
2
,
u
3
,
u
4
u_{2},u_{3},u_{4}
u2,u3,u4的值
u
2
=
ϕ
¨
I
x
x
(
11
)
u_{2}=\ddot{\phi }I_{xx}(11)
u2=ϕ¨Ixx(11)
u
3
=
θ
¨
I
y
y
(
12
)
u_{3}=\ddot{\theta }I_{yy}(12)
u3=θ¨Iyy(12)
u
4
=
ψ
¨
I
z
z
(
13
)
u_{4}=\ddot{\psi }I_{zz}(13)
u4=ψ¨Izz(13)其中，根据线性化的思想，将
ϕ
˙
,
ψ
˙
,
θ
˙
\dot{\phi },\dot{\psi },\dot{\theta}
ϕ˙,ψ˙,θ˙相互乘积的部分忽略，至此双环PID控制器设计完成，接下来搭建simulink模型和整定PID参数。
本部分包括模型搭建以及PID参数调节，先给出整体模型图，整体simulink模型如下
由于系统采取双环PID控制，因此调节参数时先确定内环参数，在内环参数确定后再开始调节外环。此外，内环的达到稳定的时间应小于外环。调整好pid参数后，就可以进行相关实验，比如让四旋翼画螺旋线，x输入sint，y输入cost，z输入0.1t,仿真时间50s，无人机螺旋线轨迹如下：
如果设定期望x为1，则无人机x变化曲线如下
如果设定期望y为1，则无人机y变化曲线如下
如果设定期望z为1，则无人机z变化曲线如下
simulink模型已经开源在网上，github下载地址：链接: 四旋翼pid控制simulink模型.gitee下载链接: 四旋翼pid控制.
