MATLAB Simmechanics/Simscope四旋翼无人机控制仿真（3）无人机控制器设计（非PID）

如果您是非控制专业，那么PID控制足够控制前文所搭建的无人机了：
MATLAB Simmechanics/Simscope四旋翼无人机控制仿真（2） Simulink模型调节.
本博客需要一些现代控制理论中Lyapunov稳定性的一些理论知识。
https://pan.baidu.com/s/1fp8CGDLyQ4PMTlj3JunSwQ
密码：35nb

1 前言

关于无人机的控制原理这里将不再赘述，控制信息流可以用下面这个图说明：
MATLAB Simmechanics/Simscope四旋翼无人机控制仿真（3）无人机控制器设计（非PID）
关于无人机动力学模型推导，这里也不再赘述，不过根据欧拉角旋转顺序，模型会有一些sin和cos上的细微差别，下面直接引用一种动力学模型：

其中 $\phi$ ， $\theta$ 和 $\psi$ 分别为滚转角，俯仰角和偏航角，其中红框标出的一般被认为是位置干扰，在小姿态角假设下一般会被忽略，因此会变为：
MATLAB Simmechanics/Simscope四旋翼无人机控制仿真（3）无人机控制器设计（非PID）
原文：
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.

2 非线性约束

从前面的控制信息流里面可以看到，无人机只有4个控制输入，其期望的俯仰和滚转角是根据水平控制求解的，而不是给定参考的因此，考虑 ${{u}_{x}}$ 和 ${{u}_{y}}$ ：
MATLAB Simmechanics/Simscope四旋翼无人机控制仿真（3）无人机控制器设计（非PID）
反解上式，得到期望俯仰和滚转角：

3 控制器设计

其实6个通道的控制器设计过程都很相似，下面只以滚转角 $\phi$ 为例。
使用的方法是经典的反步法，如果笔者把书上关于反步法介绍搬过来，那么笔者自己也不能理解书上在说啥哈哈，因此这里将用一种很通俗的方法叙述并设计：
首先滚转角状态方程，令 ${{x}_{1}}=\phi$ ， ${{x}_{2}}=\dot{\phi }$ ：
$\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.$
定义误差：
${{e}_{1}}=x_{1}^{d}-{{x}_{1}}$
首先考虑 ${{x}_{2}}$ 作为系统独立输入，要找到一个能使得 ${{e}_{1}}\to 0$ 的状态反馈控制律 ${{x}_{2}}=\varphi \left( {{x}_{1}} \right)$ ，因此定义误差：
${{e}_{2}}={{x}_{2}}-\varphi \left( {{x}_{1}} \right)$
考虑：
${{V}_{1}}=\frac{1}{2}e_{1}^{2}$
求导：
$\begin{aligned} & {{{\dot{V}}}_{1}}={{{\dot{e}}}_{1}}{{e}_{1}} \\ & ={{e}_{1}}\left( \dot{x}_{1}^{d}+{{e}_{2}}-{{{\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}$
$\varphi \left( {{x}_{1}} \right)$ 中不含 ${{x}_{2}}$ 相关项，因此这里交叉项 ${{e}_{1}{e}_{2}}$ 会保留，令 $\varphi \left( {{x}_{1}} \right)=\dot{x}_{1}^{d}+{{c}_{1}}{{e}_{1}}$ ， ${{c}_{1}}>0$ ，则：
${{\dot{V}}_{1}}=-{{e}_{1}}{{e}_{2}}-{{c}_{1}}e_{1}^{2}$
由上式可知：
${{\dot{e}}_{1}}=-{{e}_{2}}-{{c}_{1}}{{e}_{1}}$
因此，上式求导得：
${{\dot{e}}_{2}}=-{{\ddot{e}}_{1}}-{{c}_{1}}{{\dot{e}}_{1}}$
考虑：
${{V}_{2}}={{V}_{1}}+\frac{1}{2}e_{2}^{2}$
求导:
$\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}$
为了上式稳定，令 $-\ddot{x}_{1}^{d}+{{\dot{x}}_{2}}-{{c}_{1}}{{\dot{e}}_{1}}-{{e}_{1}}=-{{c}_{2}}{{e}_{2}}$ ， ${{c}_{2}}>0$ ，得：
$\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}$
求解 ${{u}_{2}}$ ：
$\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}$
调节系数的时候可以用下式：
${{{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}}$
其中：
$\left\{ \begin{matrix} {{k}_{1}}>0 \\ {{k}_{2}}>1 \\ \end{matrix} \right.$
上式总能解出 ${{c}_{1}}$ 和 ${{c}_{2}}$ 。