先进非线性控制方法 INDI 快速部署到PX4用于四旋翼控制（part1）

增量非线性控制方法是2010年以后流行的非线性控制方法，主要用于设计先进的飞行控制法。该方法的典型代表是增量动态反向（incremental nonlinear dynamic inversion）以及增量反步法（incremental backstepping）。
目前，INDI和IBKS在多个领域(包括但不限于直升机模型，四旋翼模型/真机，固定翼UAV模型/真机，Cessna的Citation(证书)模型/真机，旋转机模型，evtol应用和验证(电动垂直起降)真机。它被广泛证明是一种控制算法，可以简单地应用，具有很强的鲁棒性和容错性。可自行提供相关文献google。

虽然是非线性控制方法，但在我看来，它依赖于模型PID类似地，它甚至低于现代控制方法的模型依赖（因为现代控制，如LQR，MPC一般需要工作点的线性模型)。

我已知在四旋翼上实现了这种非线性控制方法TU delft有人一直在paparazzi上验证，所以我只想PX毕竟(我觉得)亚子并不难实现。

该方法的应用难度不在于模型知识，而在于系统状态的获取。因为除了PID除了所需的状态量，它还需要知道两个量：被控量的导数和作动器的当前位置。

参考 ’Cascaded incremental nonlinear dynamic inversion for MAV disturbance‘ 这篇文章

例如，对于以下一步系统：
$\dot{x}=f(x)+g(x)u$

写成增量模型，则为：
$\dot{x}={\dot{x}}_0+g(x)\Delta u+\Delta f$
(其中，${\dot{x}}_0$是上一采样时刻的$\dot{x}$；$\Delta u$是这一采样时刻与上一采样时刻的输入量之差。$\Delta f$是模型简化的误差，采样频率越高这个值就越小，在设计INDI控制器时可以忽略。）

定义跟踪误差为：
$e=x-x_r$

上式左右求导，可得指令跟踪误差的动态为：
$\dot{e}={\dot{x}}_0+g(x)\Delta u-{\dot{x}}_r$

根据上述方程，这样如果想要得到一个指数收敛的误差系统，则可以令$\Delta u$为：
$\Delta u=-{g(x)}^{-1}({\dot{x}}_0-{\dot{x}}_r+K_{p}e)$

把上式代入上上式，则可以得到：
$\dot{e}=-K_{p}e$
即误差渐进收敛。

如果想要收敛的快一点，则可以把线性反馈改成非线性反馈，例如令$\Delta u$为：
$\Delta u=-{g(x)}^{-1}({\dot{x}}_0-{\dot{x}}_r+K_p|u{|}^{\alpha }sign(u))$

最后真实的 $u$ 计算方法为：
$u=u_0+\Delta u$
其中$u_0$ 是作动器的当前位置（对于电机来说就是当前的转速）

关于的四旋翼控制的一点解释说明：
基于时标分离假设，对于四旋翼的位置控制来说可以分成四环：
位置-> 速度-> （力/加速度）->姿态-> 角速度-> 电机转速（力矩）
（从力/加速度到姿态实际上只是一个换算）

其实这四环分别对应了两个二阶动态。也就是力到位移的二阶动态，以及力矩到姿态的二阶动态。这两个二阶动态又分别由一个动力学方程和一个运动学方程组成。。。不过这些概念只是帮助理解，就不细说了

根据位置误差会计算出三个加速度指令，一方面可以计算出两个 tilt 角度的指令，也就是 $\theta$ 和 $\varphi$ 的指令。另一方面可以计算出总的加速度指令来计算推力指令。最后再指定$\psi$的角度，就正好是四个指令，由四个电机来控制。总的来看其实就是四个电机（输入）控制 $x,y,z,\psi$ 这四个量

由于涉及模型知识的只是力方程和力矩方程里的动力学方程，外环的运动方程不涉及建模偏差问题（只有测量偏差问题），所以在外环只用普通的NDI就够了，不需要INDI。

事实上，我打算只在姿态控制上用INDI，在推力控制上不用INDI，因为PX4里姿态控制和推力控制也是分开的，如果推力控制也改的话，工作量有点大。。。所以INDI这里只是算一个力矩，然后跟推力结合在一起，再由mixer去分配

另外，本着由简单入手的原则，在角度环我也打算保留原来的，毕竟px4本来的角度环控制也只有一个P。因为四旋翼这个东西，如果tilt角度不大的话，外环到内环的线化程度就是比较高的。

从方法论的角度来说，这种工程就应该是从简单到复杂去做，先用简单的例子去验证关键技术，然后再慢慢增加复杂度。再说INDI本来就是在动力学这环和其他控制方法的差别比较大，所以在角速度环进行修改是非常合适的。

OK，要做的工作总结来说就是，把PX4的角速度环改成INDI，其他基本不变。

%%%%%%%%%%%%%%%%%%%%%%%%%%%

首先用于四旋翼的INDI需要简单推导一下：

首先给出四旋翼角速度环动力学方程：
$J\dot{\Omega }+\Omega \times J\Omega =M_c+M_a+M_r$
其中$\Omega$为角速度（p, q, r），$J$ 是惯量矩阵，$M_c=G_M{\omega }^2$为控制力矩，$M_a$为气动力矩，$M_r$为陀螺力矩。$G_M$ 是操纵效率矩阵，$\omega$为电机转速

也就是：
$\dot{\Omega }=J^{-1}{G}_M{\Omega }^2+J^{-1}(-\Omega \times J\Omega +M_a+M_r)$
简化之后就是：$\dot{\Omega }=g(x_{states}){\omega }^2+f(states)$
其中$g(x_{states})=J^{-1}{G}_M$

对于角速度控制环来说，令指令跟踪误差为$e_{\Omega }=\Omega -{\Omega }_r$
则角速度跟踪误差的动态方程可以写成
${\dot{e}}_{\Omega }={\dot{\Omega }}_0-{\dot{\Omega }}_r$