先进非线性控制方法 INDI 快速部署到PX4用于四旋翼控制（part1）
关于INDI的前言：
增量型非线性控制方法是一种2010年以后才流行起来的非线性控制方法，目前主要用于设计先进飞行控制律。这种方法的典型代表是增量型动态逆（incremental nonlinear dynamic inversion）以及增量型反步法（incremental backstepping）。
目前，INDI和IBKS在多个领域（包括但不限于直升机模型，四旋翼模型/真机，固定翼UAV模型/真机，Cessna的Citation（奖状）模型/真机，倾转旋翼机模型，evtol（电动垂直起降）真机）里得到应用及验证。已被广泛证明是一种能够简单应用并具备较强鲁棒性和容错性能的控制算法。相关文献可以自行google。
虽然是非线性控制方法，但是在我看来它对模型的依赖程度与PID差不多，甚至比现代控制方法的模型依赖程度还低（因为现代控制比如LQR，MPC等一般需要工作点的线性模型）。
这种非线性控制方法在四旋翼上的实现，我已知的就TU delft有人一直在paparazzi上验证，所以我就想在PX4上实现一下，毕竟（我觉得）也不是很难实现的亚子。
这种方法的应用难度不在于关于模型的知识，而在于系统状态的获取。因为除了PID需要的状态量之外，它还需要知道两个量：被控量的导数，以及作动器的当前位置。
对于飞机的姿态控制来说，一般都需要角加速度测量值，这个值一般可以通过对陀螺仪计算出来的角速度滤波来获得。但是看文献最近也有用专门的角加速度传感器来获得的，控制效果会比前者更好。
关于作动器的位置，如果可以测的话肯定是最好，如果没有的话可以通过对作动器环节建模，然后计算出估计的 u 0 u_{\tiny 0} u0 来，保证 x 0 x_{\tiny 0} x0 和 u 0 u_{\tiny 0} u0 的时刻是一致的。这样增量模型的假设才能成立。
%%%%%%%%%%%%%%%%%%%%%%%%%%%
INDI简单推导
参考 ’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+Kpe)
把上式代入上上式，则可以得到：
e ˙ = − K p e \dot{e}=-K_pe e˙=−Kpe
即误差渐进收敛。
如果想要收敛的快一点，则可以把线性反馈改成非线性反馈，例如令 Δ 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，其他基本不变。
%%%%%%%%%%%%%%%%%%%%%%%%%%%
在PX4中修改的部分
首先用于四旋翼的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+g(xstates)Δω2
那么可以得到（电机引起的）力矩的增量
G M Δ ω 2 = − J ( Ω ˙ 0 − Ω ˙ r + K p e Ω ) G_M\Delta{\omega^2}=-J(\dot{\Omega}_{\tiny 0}-\dot{\Omega}_r+K_pe_{\tiny \Omega}) GMΔω2=−J(Ω˙0−Ω˙r+KpeΩ)
然后可以得到力矩的指令：
M c = G M ⋅ ω 0 2 + G M ⋅ Δ ω 2 M_c=G_M\cdot{\omega_{\tiny 0}^2}+G_M\cdot\Delta{\omega^2} Mc=GM⋅ω02+GM⋅Δω2
其中 ω 0 2 \omega_{\tiny 0}^2 ω02是估计出来的或者测量回来的转速。
这个 M c M_c Mc算出来之后就可以用来分配了
也就是 ω r 2 = G M − 1 M c \omega_r^2=G_M^{-1}M_c ωr2=GM−1Mc
其中 ω r \omega_r ωr是电机转速指令，即我们希望电机到达的转速
OK，以上即是关于INDI如何应用到四旋翼的第一部分，理论部分，关于代码修改部分在part2给出