如果您是非控制专业，那么PID控制足够控制前文所搭建的无人机了：
MATLAB Simmechanics/Simscape四旋翼无人机控制仿真（1） SolidWorks模型的导入.
MATLAB Simmechanics/Simscape四旋翼无人机控制仿真（2） Simulink模型调节.
本博客需要一些现代控制理论中Lyapunov稳定性的一些理论知识。
模型详见：
https://github.com/mkb9559/Simscape-Quadrotor
MATLAB Simmechanics/Simscape四旋翼无人机控制仿真（4） 串级姿态控制.
Demo视频.
最近更新了第二代模型，可以参考：
MATLAB Simmechanics/Simscape四旋翼无人机控制仿真（5） F250旋翼仿真搭建.
关于无人机的控制原理这里将不再赘述，控制信息流可以用下面这个图说明：
关于无人机动力学模型推导，这里也不再赘述，不过根据欧拉角旋转顺序，模型会有一些sin和cos上的细微差别，下面直接引用一种动力学模型：
其中
ϕ
\phi
ϕ，
θ
\theta
θ和
ψ
\psi
ψ分别为滚转角，俯仰角和偏航角
原文：
Bouabdallah, S. , P. Murrieri , and R. Siegwart . “Design and control of an indoor micro quadrotor.” IEEE International Conference on Robotics and Automation, 2004. Proceedings. ICRA '04. 2004 IEEE, 2004.
从前面的控制信息流里面可以看到，无人机只有4个控制输入，其期望的俯仰和滚转角是根据水平控制求解的，而不是给定参考的因此，考虑
u
x
{{u}_{x}}
ux和
u
y
{{u}_{y}}
uy：
反解上式，得到期望俯仰和滚转角：
其实6个通道的控制器设计过程都很相似，下面只以滚转角
ϕ
\phi
ϕ为例。
使用的方法是经典的反步法，如果笔者把书上关于反步法介绍搬过来，那么笔者自己也不能理解书上在说啥哈哈，因此这里将用一种很通俗的方法叙述并设计：
首先滚转角状态方程，令
x
1
=
ϕ
{{x}_{1}}=\phi
x1=ϕ，
x
2
=
ϕ
˙
{{x}_{2}}=\dot{\phi }
x2=ϕ˙：
{
x
˙
1
=
x
2
x
˙
2
=
l
u
2
+
θ
˙
ψ
˙
(
I
y
−
I
z
)
I
x
\left\{ \begin{matrix} {{{\dot{x}}}_{1}}={{x}_{2}} \\ {{{\dot{x}}}_{2}}=\frac{l{{u}_{2}}+\dot{\theta }\dot{\psi }\left( {{I}_{y}}-{{I}_{z}} \right)}{{{I}_{x}}} \\ \end{matrix} \right.
{x˙1=x2x˙2=Ixlu2+θ˙ψ˙(Iy−Iz)
定义误差：
e
1
=
x
1
d
−
x
1
{{e}_{1}}=x_{1}^{d}-{{x}_{1}}
e1=x1d−x1
首先考虑
x
2
{{x}_{2}}
x2作为系统独立输入，要找到一个能使得
e
1
→
0
{{e}_{1}}\to 0
e1→0的状态反馈控制律
x
2
=
φ
(
x
1
)
{{x}_{2}}=\varphi \left( {{x}_{1}} \right)
x2=φ(x1)，因此定义误差：
e
2
=
φ
(
x
1
)
−
x
2
{{e}_{2}}=\varphi \left( {{x}_{1}} \right)-{{x}_{2}}
e2=φ(x1)−x2
考虑：
V
1
=
1
2
e
1
2
{{V}_{1}}=\frac{1}{2}e_{1}^{2}
V1=21e12
求导：
V
˙
1
=
e
˙
1
e
1
=
e
1
(
x
˙
1
d
−
x
˙
1
)
=
e
1
(
x
˙
1
d
−
x
2
)
=
e
1
(
x
˙
1
d
−
e
2
+
φ
(
x
1
)
)
\begin{aligned} & {{{\dot{V}}}_{1}}={{{\dot{e}}}_{1}}{{e}_{1}} \\ & ={{e}_{1}}\left( \dot{x}_{1}^{d}-{{{\dot{x}}}_{1}} \right) \\ & ={{e}_{1}}\left( \dot{x}_{1}^{d}-{{x}_{2}} \right) \\ & ={{e}_{1}}\left( \dot{x}_{1}^{d}-{{e}_{2}}+\varphi \left( {{x}_{1}} \right) \right) \\ \end{aligned}
V˙1=e˙1e1=e1(x˙1d−x˙1)=e1(x˙1d−x2)=e1(x˙1d−e2+φ(x1))
φ
(
x
1
)
\varphi \left( {{x}_{1}} \right)
φ(x1)中不含
x
2
{{x}_{2}}
x2相关项，因此这里交叉项
e
1
e
2
{{e}_{1}{e}_{2}}
e1e2会保留，令
φ
(
x
1
)
=
−
x
˙
1
d
+
c
1
e
1
\varphi \left( {{x}_{1}} \right)=-\dot{x}_{1}^{d}+{{c}_{1}}{{e}_{1}}
φ(x1)=−x˙1d+c1e1，
c
1
>
0
{{c}_{1}}>0
c1>0，则：
V
˙
1
=
−
e
1
e
2
−
c
1
e
1
2
{{\dot{V}}_{1}}=-{{e}_{1}}{{e}_{2}}-{{c}_{1}}e_{1}^{2}
V˙1=−e1e2−c1e12
由上式可知：
e
˙
1
=
−
e
2
−
c
1
e
1
{{\dot{e}}_{1}}=-{{e}_{2}}-{{c}_{1}}{{e}_{1}}
e˙1=−e2−c1e1
因此，上式求导得：
e
˙
2
=
−
e
¨
1
−
c
1
e
˙
1
{{\dot{e}}_{2}}=-{{\ddot{e}}_{1}}-{{c}_{1}}{{\dot{e}}_{1}}
e˙2=−e¨1−c1e˙1
考虑：
V
2
=
V
1
+
1
2
e
2
2
{{V}_{2}}={{V}_{1}}+\frac{1}{2}e_{2}^{2}
V2=V1+21e22
求导:
V
˙
2
=
V
˙
1
+
e
˙
2
e
2
=
−
c
1
e
1
2
+
e
2
(
e
˙
2
−
e
1
)
=
−
c
1
e
1
2
+
e
2
(
−
e
¨
1
−
c
1
e
˙
1
−
e
1
)
=
−
c
1
e
1
2
+
e
2
(
−
x
¨
1
d
+
x
˙
2
−
c
1
e
˙
1
−
e
1
)
\begin{aligned} & {{{\dot{V}}}_{2}}={{{\dot{V}}}_{1}}+{{{\dot{e}}}_{2}}{{e}_{2}} \\ & =-{{c}_{1}}e_{1}^{2}+{{e}_{2}}\left( {{{\dot{e}}}_{2}}-{{e}_{1}} \right) \\ & =-{{c}_{1}}e_{1}^{2}+{{e}_{2}}\left( -{{{\ddot{e}}}_{1}}-{{c}_{1}}{{{\dot{e}}}_{1}}-{{e}_{1}} \right) \\ & =-{{c}_{1}}e_{1}^{2}+{{e}_{2}}\left( -\ddot{x}_{1}^{d}+{{{\dot{x}}}_{2}}-{{c}_{1}}{{{\dot{e}}}_{1}}-{{e}_{1}} \right) \\ \end{aligned}
V˙2=V˙1+e˙2e2=−c1e12+e2(e˙2−e1)=−c1e12+e2(−e¨1−c1e˙1−e1)=−c1e12+e2(−x¨1d+x˙2−c1e˙1−e1)
为了上式稳定，令
−
x
¨
1
d
+
x
˙
2
−
c
1
e
˙
1
−
e
1
=
−
c
2
e
2
-\ddot{x}_{1}^{d}+{{\dot{x}}_{2}}-{{c}_{1}}{{\dot{e}}_{1}}-{{e}_{1}}=-{{c}_{2}}{{e}_{2}}
−x¨1d+x˙2−c1e˙1−e1=−c2e2，
c
2
>
0
{{c}_{2}}>0
c2>0，得：
x
¨
2
=
x
¨
1
d
+
c
1
e
˙
1
+
e
1
−
c
2
e
2
=
x
¨
1
d
+
c
1
e
˙
1
+
e
1
−
c
2
(
−
e
˙
1
−
c
1
e
1
)
=
x
¨
1
d
+
(
c
1
+
c
2
)
e
˙
1
+
(
1
+
c
1
c
2
)
e
1
\begin{aligned} & {{{\ddot{x}}}_{2}}=\ddot{x}_{1}^{d}+{{c}_{1}}{{{\dot{e}}}_{1}}+{{e}_{1}}-{{c}_{2}}{{e}_{2}} \\ & =\ddot{x}_{1}^{d}+{{c}_{1}}{{{\dot{e}}}_{1}}+{{e}_{1}}-{{c}_{2}}\left( -{{{\dot{e}}}_{1}}-{{c}_{1}}{{e}_{1}} \right) \\ & =\ddot{x}_{1}^{d}+\left( {{c}_{1}}+{{c}_{2}} \right){{{\dot{e}}}_{1}}+\left( 1+{{c}_{1}}{{c}_{2}} \right){{e}_{1}} \\ \end{aligned}
x¨2=x¨1d+c1e˙1+e1−c2e2=x¨1d+c1e˙1+e1−c2(−e˙1−c1e1)=x¨1d+(c1+c2)e˙1+(1+c1c2)e1
求解
u
2
{{u}_{2}}
u2：
l
u
2
+
θ
˙
ψ
˙
(
I
y
−
I
z
)
I
x
=
x
¨
1
d
+
(
c
1
+
c
2
)
e
˙
1
+
(
1
+
c
1
c
2
)
e
1
u
2
=
[
x
¨
1
d
+
(
c
1
+
c
2
)
e
˙
1
+
(
1
+
c
1
c
2
)
e
1
]
I
x
−
θ
˙
ψ
˙
(
I
y
−
I
z
)
l
\begin{aligned} & {\frac{l{{u}_{2}}+\dot{\theta }\dot{\psi }\left( {{I}_{y}}-{{I}_{z}} \right)}{{{I}_{x}}}=\ddot{x}_{1}^{d}+\left( {{c}_{1}}+{{c}_{2}} \right){{\dot{e}}_{1}}+\left( 1+{{c}_{1}}{{c}_{2}} \right){{e}_{1}}} \\ & {{{u}_{2}}=\frac{\left[ \ddot{x}_{1}^{d}+\left( {{c}_{1}}+{{c}_{2}} \right){{{\dot{e}}}_{1}}+\left( 1+{{c}_{1}}{{c}_{2}} \right){{e}_{1}} \right]{{I}_{x}}-\dot{\theta }\dot{\psi }\left( {{I}_{y}}-{{I}_{z}} \right)}{l}} \end{aligned}
Ixlu2+θ˙ψ˙(Iy−Iz)=x¨1d+(c1+c2)e˙1+(1+c1c2)e1u2=l[x¨1d+(c1+c2)e˙1+(1+c1c2)e1]Ix−θ˙ψ˙(Iy−Iz)
调节系数的时候可以用下式：
u
2
=
[
x
¨
1
d
+
k
1
e
˙
1
+
k
2
e
1
]
I
x
−
θ
˙
ψ
˙
(
I
y
−
I
z
)
l
{{{u}_{2}}=\frac{\left[ \ddot{x}_{1}^{d}+{{k}_{1}}{{{\dot{e}}}_{1}}+{{k}_{2}}{{e}_{1}} \right]{{I}_{x}}-\dot{\theta }\dot{\psi }\left( {{I}_{y}}-{{I}_{z}} \right)}{l}}
u2=l[x¨1d+k1e˙1+k2e1]Ix−θ˙ψ˙(Iy−Iz)
其中：
{
k
1
>
0
k
2
>
1
\left\{ \begin{matrix} {{k}_{1}}>0 \\ {{k}_{2}}>1 \\ \end{matrix} \right.
{k1>0k2>1
上式总能解出
c
1
{{c}_{1}}
c1和
c
2
{{c}_{2}}
c2。
这里简单放一下：
