先进非线性控制方法 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‘ 这篇文章

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

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

定义跟踪误差为： e = x − x r e=x-x_r e=x−xr​

上式左右求导，可得指令跟踪误差的动态为： e ˙ = x ˙ 0 + g ( x ) Δ u − x ˙ r \dot{e}=\dot{x}_{\tiny 0}+g(x)\Delta{u}-\dot{x}_r e˙=x˙0​+g(x)Δu−x˙r​

根据上述方程，这样如果想要得到一个指数收敛的误差系统，则可以令 Δ u \Delta{u} Δu为： Δ u = − g ( x ) − 1 ( x ˙ 0 − x ˙ r + K p e ) \Delta{u}=-{g(x)}^{-1}(\dot{x}_{\tiny 0}-\dot{x}_r+K_pe) Δu=−g(x)−1(x˙0​−x˙r​+Kp​e)

把上式代入上上式，则可以得到： e ˙ = − K p e \dot{e}=-K_pe e˙=−Kp​e 即误差渐进收敛。

如果想要收敛的快一点，则可以把线性反馈改成非线性反馈，例如令 Δ u \Delta{u} Δu为： Δ u = − g ( x ) − 1 ( x ˙ 0 − x ˙ r + K p ∣ u ∣ α s i g n ( u ) ) \Delta{u}=-{g(x)}^{-1}(\dot{x}_{\tiny 0}-\dot{x}_r+K_p|u|^{\alpha}sign(u)) Δu=−g(x)−1(x˙0​−x˙r​+Kp​∣u∣αsign(u))

最后真实的 u u u 计算方法为： u = u 0 + Δ u u=u_{\tiny 0}+\Delta{u} u=u0​+Δu 其中 u 0 u_{\tiny 0} u0​ 是作动器的当前位置（对于电机来说就是当前的转速）

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

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

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

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

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

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

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

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

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

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

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

也就是： Ω ˙ = J − 1 G M Ω 2 + J − 1 ( − Ω × J Ω + M a + M r ) \dot{\Omega}=J^{-1}G_M{\Omega}^2+J^{-1}(-\Omega{\times}J{\Omega}+M_a+M_r) Ω˙=J−1GM​Ω2+J−1(−Ω×JΩ+Ma​+Mr​) 简化之后就是： Ω ˙ = g ( x s t a t e s ) ω 2 + f ( s t a t e s ) \dot{\Omega}=g(x_{states}){\omega}^2+f(states) Ω˙=g(xstates​)ω2+f(states) 其中 g ( x s t a t e s ) = J − 1 G M g(x_{states})=J^{-1}G_M g(xstates​)=J−1GM​

对于角速度控制环来说，令指令跟踪误差为 e Ω = Ω − Ω r e_{\tiny \Omega}=\Omega-\Omega_r eΩ​=Ω−Ωr​ 则角速度跟踪误差的动态方程可以写成 e ˙ Ω = Ω ˙ 0 − Ω ˙ r + g ( x s t a t e s ) Δ ω 2 \dot{e}_{\Omega}=\dot{\Omega}_{\tiny 0}-\dot{\Omega}_r+g(x_{states})\Delta{\omega^2} e˙Ω​=Ω˙0​−Ω˙r​