NDI 与 INDI 以及变体

## 非线性动态逆 (NDI) —— 从模型直接反解控制量

### 直觉

飞行控制的本质是：给定一个期望的运动（比如"我希望飞机以 30°/s 的速率滚转"），你要算出舵面该打多少度。

NDI 的做法很直接——把飞机的动力学模型"反过来用"。如果模型是准确的，那么从期望运动反推出的舵面指令就能精确实现目标。

### 推导

我们先把飞机写成一个**仿射系统**。仿射的意思是：状态的变化速率可以写成"仅与状态有关的部分 + 与控制输入成线性关系的部分"：

$$

\begin{align*}

\dot x &= f(x) + g(x) \, u \tag{1}\\

y &= h(x) \tag{2}

\end{align*}

$$

这里的符号：$x$ 是飞机的状态（速度、姿态角等），$u$ 是控制输入（舵面偏角、油门等），$y$ 是我们关心的输出（比如滚转速率）。

对输出求导，看看 $y$ 的变化由什么决定：

$$

\dot y = \frac{\partial h}{\partial x} \dot x

= \frac{\partial h}{\partial x} f(x) + \frac{\partial h}{\partial x} g(x) \, u \tag{3}

$$

这个式子的结构很清晰：$\dot y$ 由两部分组成——一部分是飞机自身动力学贡献的（$\frac{\partial h}{\partial x}f(x)$），另一部分是舵面贡献的（$\frac{\partial h}{\partial x}g(x)u$）。

现在给定一个**期望的输出变化率** $\dot y_{\text{des}} \triangleq v$，我们就可以反解出 $u$：

$$

u = \left( \frac{\partial h}{\partial x} g \right)^{-1}

\bigl( v - \frac{\partial h}{\partial x} f(x) \bigr) \tag{4}

$$

这个公式的含义很直观：先用 $v$ 减去飞机自身动力学的贡献（$v - \frac{\partial h}{\partial x}f(x)$），剩下的就是"需要舵面来产生的那部分效果"，再除以舵面的控制效能（$\frac{\partial h}{\partial x}g$），就得到了舵面指令。

### NDI 的两个硬性要求

从 (4) 可以直接看出：

1. 模型必须精确——$f(x)$ 和 $g(x)$ 如果差得远，算出的 $u$ 自然不准；

2. 系统必须是仿射的——$\frac{\partial h}{\partial x}g$ 必须可逆，如果 $u$ 不是线性进来的（非仿射），(4) 式根本没法写。

> 实际飞行器的动力学往往不能完美地写成仿射形式——比如气动系数本身就是舵面的非线性函数。这时候就需要另一种思路：**INDI**。

---

## 增量非线性动态逆 (INDI) —— 不去硬算全部，只修正"差多少"

### 直觉

NDI 的痛点是：你必须精确知道 $f(x)$（飞机自身动力学）才能反解。如果模型不准，算出来的 $u$ 可能一塌糊涂。

INDI 换个思路：**我不从零开始算控制量，我只需要知道"比现在多打或少打多少舵面"就够了。** 这个"增量"可以靠传感器反馈来修正，大幅降低对模型的依赖。

### 推导

先写出更一般的系统（允许非仿射）：

$$

\begin{align*}

\dot x &= F(x, u) = f(x) + g(x, u) \tag{5}\\

y &= h(x) \tag{6}

\end{align*}

$$

对输出求导：

$$

\dot y = \frac{\partial h}{\partial x} \dot x

= \frac{\partial h}{\partial x} f(x) + \frac{\partial h}{\partial x} g(x, u)

= a(x) + b(x, u) \tag{7}

$$

这里关键的区别在于 $b(x, u) = \frac{\partial h}{\partial x}g(x, u)$——$u$ 不一定是线性出现的。

- 如果系统恰好是仿射的，$b(x,u) = \frac{\partial h}{\partial x}g(x)u$，依然可以像 NDI 那样直接求逆；

- 如果不是仿射的，$b(x,u)$ 对 $u$ 的依赖关系太复杂，直接求逆走不通。

这就是 INDI 要解决的问题。

---

设定期望输出变化率 $\dot y_{\text{des}} \triangleq v$，则有 $v = a(x) + b(x, u)$。

INDI 的核心思路是：**在当前工作点 $(x_0, u_0)$ 附近做泰勒展开，只保留一阶项**。这相当于用一条切线去近似原来的曲线——不太准，但在工作点附近够用了：

$$

\begin{align*}

v &\approx v_0+ \left. \frac{\partial (a + b)}{\partial x} \right|_{x_0, u_0} (x - x_0)+ \left. \frac{\partial b}{\partial u}\right|_{x_0, u_0} (u - u_0) \triangleq v_0 + A_0 (x - x_0) + B_0 (u - u_0) \tag{8}

\end{align*}

$$

其中：

$$

A_0 = \left. \frac{\partial (a + b)}{\partial x} \right|_{x_0, u_0},

B_0 = \left. \frac{\partial b}{\partial u} \right|_{x_0, u_0}

$$

这里 $A_0$ 刻画了"状态变化对输出变化率的影响"，$B_0$ 刻画了"控制输入变化对输出变化率的影响"——也就是控制效能。

从 (8) 反解 $u$：

$$

u = u_0 + B_0^{-1} \bigl( v - v_0 - A_0 (x - x_0) \bigr) \tag{9}

$$

这个式子的含义是：**基准控制量 $u_0$ 加上一个修正量**。修正量里包含了三项信息——期望的变化 ($v$)、当前的变化 ($v_0$)、以及状态偏移可能带来的影响 ($A_0(x-x_0)$)。

### 关键简化：一个周期内状态变化很小

在飞控的典型采样周期（毫秒级）内，飞机状态不会突变，所以 $x \approx x_0$。令 $x - x_0 = 0$，(9) 大幅简化为：

$$

\boxed{ u = u_0 + B_0^{-1} (v - v_0) } \tag{10}

$$

这就是 INDI 的核心公式。 它只需要知道三样东西：

- $u_0$：上一时刻的控制输入（已知）；

- $v_0$：当前输出变化率的估计（可以通过传感器或模型获得）；

- $B_0$：当前的控制效能（可以从模型近似或在线辨识获得）。

跟 NDI 的 (4) 式对比：NDI 需要完整的 $f(x)$ 和 $g(x)$，而 INDI 只需要 $B_0$ 和 $v_0$。**模型依赖度大幅降低**。

---

## INDI 和 NDI 什么时候等价？

一个很自然的问题：INDI 比 NDI 少了那么多信息，它和 NDI 的结果是一样的吗？

为方便分析，取 $h(x) = x$（即 $\partial h / \partial x = I$，输出就是状态本身）：

$$

\begin{align*}

\dot x &= F(x, u) = f(x) + g(x, u) \tag{11}\\

y &= x \tag{12}

\end{align*}

$$

这种情况下两种方法的控制律分别简化为：

- NDI：从 $g(x_0, u) = v - f(x_0)$ 求解 $u$

- INDI：从 $v - v_0 = \left. \frac{\partial g}{\partial u} \right|_{x_0, u_0} (u - u_0)$ 求解 $u$

下面分三种情况讨论。

### 情况一：仿射 + 全驱动（输入数 = 输出数）→ 完全等价

仿射系统意味着 $g(x, u) = g(x)u$（$u$ 线性出现）。全驱动意味着输入输出维度相同，$g(x_0)$ 是方阵可逆。

NDI 控制律：$u = g(x_0)^{-1}(v - f(x_0))$

把它代入 INDI 方程右侧：

$$

\begin{align*}

\text{RHS} &= g(x_0) \bigl[ g(x_0)^{-1}(v - f(x_0)) - u_0 \bigr] \\

&= v - f(x_0) - g(x_0) u_0 \\

&= v - \underbrace{\bigl(f(x_0) + g(x_0)u_0\bigr)}_{=\dot x_0 = v_0} \\

&= v - v_0 = \text{LHS}

\end{align*}

$$

结论：全驱动仿射系统下，INDI 和 NDI 等价。 这不是巧合——当模型精确且系统仿射时，泰勒展开的一阶项就已经包含了全部信息，高阶项为零。

### 情况二：仿射 + 过驱动（输入数 > 输出数）→ 取决于求解方式

过驱动意味着舵面比控制目标多（比如 8 个舵面控制 3 个姿态角），方程数少于未知数，不能直接求逆，需要用**控制分配**来选一个"好的"解。

对比三种求解策略：

| 方法 | 约束方程 | 优化对象 | 和 NDI 的关系 |

|------|---------|---------|--------------|

| NDI | $g(x_0)u = v - f(x_0)$ | 总控制量 $u$ | 基准 |

| INDI 增量式 | $g(x_0)\Delta u = v - v_0$ | 增量 $\Delta u$ | 不同（代价函数变了） |

| INDI 全量式 | $g(x_0)u = v - v_0 + g(x_0)u_0 = v - f(x_0)$ | 总控制量 $u$ | 近似（约束相同，仅差截断误差） |

关键洞察：**INDI 全量式的约束方程和 NDI 一模一样**（展开 $v_0$ 后消去了相同项），区别仅在于 INDI 经过了泰勒近似。所以全量式 INDI 的结果和 NDI 非常接近。

### 情况三：非仿射系统 → INDI 有天然优势

非仿射系统的 $g(x, u)$ 中 $u$ 以复杂非线性形式出现，NDI 根本没法直接反解 $u$。而 INDI 通过泰勒展开，总能把 $b(x,u)$ 对 $u$ 的依赖在局部近似成线性（$B_0 \Delta u$），从而绕过了非仿射这个难题。

一句话总结：INDI 就是通过"只看增量、局部线性化"来规避 NDI 对精确模型和非仿射结构的依赖。

---

## 状态导数 $v_0$ 怎么来？

INDI 的公式 $u = u_0 + B_0^{-1}(v - v_0)$ 中，$u_0$ 是已知的（上一拍的指令），但 $v_0$（当前的输出变化率 $\dot y_0$）需要估计。这里有三条路：

### 方案一：直接用传感器测

$$

u = u_{\text{sensor}} + B_0^{-1} (v - v_{\text{sensor}}) \tag{14}

$$

- 位置控制回路：$v_0$ 就是速度，陀螺仪/加速度计可以直接测，效果不错。

- 姿态内回路：$v_0$ 是角加速度，大多数飞控传感器没有这个量，直接测量很困难。

### 方案二：用机载模型 (OBM) 算

$$

u = u_{\text{OBM}} + B_0^{-1} \bigl( v - \dot y_{\text{OBM}} \bigr)

= u_{\text{OBM}} + B_0^{-1} \bigl( v - f_{\text{OBM}}(x) - g_{\text{OBM}}(x, u) \bigr) \tag{15}

$$

- 优点是不依赖特殊传感器，缺点是对模型精度有要求——不过注意 INDI 只在 $v_0$ 这一项用到模型，远不如 NDI 那么敏感。

### 方案三：传感器 + 模型混合

把传感器的 $y_0$（低频可靠）和模型的 $\dot y_{\text{OBM}}$（全频段但可能偏）按频率域组合，类似互补滤波的思路：

$$

\hat{\dot y} = \frac{s^2}{s^2 + 2\zeta s + \omega_0^2}\, \dot y_{\text{OBM}}+ \frac{2\zeta s + \omega_0^2}{s^2 + 2\zeta s + \omega_0^2}\, s\, y_0 \tag{16}

$$

另外 ESO（扩张状态观测器）和卡尔曼滤波也可以用来估计 $v_0$。

---

## 从 INDI 到 E-INDI —— 把执行机构也考虑进去

### 为什么需要 E-INDI？

前面的 INDI 推导隐含了一个假设：**舵面指令 $u_{\text{cmd}}$ 发出去后立刻实现**。但实际上舵机有自己的动态——你发一个 $10°$ 的指令，舵机可能要几十毫秒才能真的转到位。

E-INDI（扩展 INDI）就是把这个执行机构延迟纳入计算，让控制律"知道"舵机不是瞬间响应的。

### 执行机构的数学模型

舵机通常用一阶或二阶传递函数描述。二阶形式为：

$$

\frac{\omega^2}{s^2 + 2\zeta\omega_0 s + \omega^2} \tag{17}

$$

当阻尼比 $\zeta$ 较大时，可以近似看成两个一阶环节的串联：内环带宽 $\omega_{\text{IL}} = 2\zeta\omega_0$，外环带宽 $\omega_{\text{OL}} = \omega_0 / (2\zeta)$。

一阶形式更简单——内环退化为 $1$，外环 $\omega_{\text{OL}} = 1/T$（$T$ 是舵机时间常数）。

### 关键观测：$u$ 反馈被抵消了

看一阶执行机构的框图。INDI 控制指令 $u_{\text{cmd}} = u_0 + \Delta u_{\text{cmd}}$ 进入回路时，$u_0$ 恰好和内部 $u$ 反馈相互抵消，只剩下 $\Delta u_{\text{cmd}}$ 起作用：

此时舵机实际运动的速率为：

$$

\dot u = \Delta u_{\text{cmd}} \, \omega_{\text{OL}} \tag{18}

$$

物理含义：增量指令 $\Delta u_{\text{cmd}}$ 本质上直接驱动了舵机速率 $\dot u$。

### 把这个关系纳入 INDI

取状态方程（简单起见 $y = x$）：

$$

\begin{align*}

\dot x &= F(x, u) \tag{19}\\

y &= x \tag{20}

\end{align*}

$$

期望状态导数 $\dot y_{\text{des}} \triangleq v$，则 $v = \dot y = F(x, u)$。

对 $v$ 再求一次导（因为我们想控制 $\dot v$，即输出的加速度）：

$$

\dot v = \frac{\partial F}{\partial x} \dot x + \frac{\partial F}{\partial u} \dot u

= A_m \dot x + B_m \dot u \tag{22}

$$

这里 $A_m$ 反映了"状态变化如何影响输出的加速度"，$B_m$ 反映了"舵面速率如何影响输出的加速度"。

把 (18) 代入：

$$

\dot v = A_m \dot x + B_m \, \Delta u_{\text{cmd}} \, \omega_{\text{OL}} \tag{23}

$$

对 $\dot v$ 施加一阶期望动态（希望 $v$ 以带宽 $\omega_{\text{des}}$ 趋向目标）：

$$

\dot v = \omega_{\text{des}} \, \Delta v \tag{24}

$$

联立解得：

$$

\Delta u_{\text{cmd}} = \frac{\omega_{\text{des}} \Delta v - A_m \dot x}{B_m \, \omega_{\text{OL}}} \tag{25}

$$

这个式子里有两个带宽参数：

- $\omega_{\text{OL}}$：舵机外环带宽（硬件特性，决定了舵机能多快跟上指令）；

- $\omega_{\text{des}}$：你期望的虚拟控制带宽（设计参数，决定了控制系统多快收敛）。

如果你想用参考模型来规划状态轨迹，把 $\dot x$ 换成参考模型的 $\dot x_{\text{ref}}$：

$$

\Delta u_{\text{cmd}} = \frac{\dot v_{\text{des}} - A_m \dot x_{\text{ref}}}{B_m \, \omega_{\text{OL}}} \tag{27}

$$

### MIMO 推广

多输入多输出时，带宽变成对角矩阵（每个通道有自己的带宽）。过驱动下用广义逆来做控制分配：

$$

u_{\text{cmd}} = \bigl( B_m \, \omega_{\text{OL}} \bigr)^{I\dagger}

\bigl( \dot \nu_{\text{des}} - A_m \dot x_{\text{ref}} \bigr) + u_0 \tag{29}

$$

### 使用注意事项

- 一阶执行机构模型假设已知 $\omega_{\text{OL}}$，此时推导是精确的。

- 二阶执行机构也可以用 E-INDI，取一个"有效"外环带宽近似就行——不再精确解耦，但仍比不加补偿好。

- 核心局限：INDI 律的 $u$ 反馈和舵机内部的 $u$ 反馈在数字实现中是两个回路，采样率和延迟不同，不会完美抵消。解决方向：把飞行控制和舵机控制放在一个统一框架里设计——不再分家。

---

## 从 NDI 到 ANDI —— E-INDI 的思想用到 NDI 上

E-INDI 给了一个启发：既然 INDI 能纳入执行机构动态，NDI 也应该可以。这就是 ANDI（增强 NDI）。

### 推导

系统方程：

$$

\begin{align*}

\dot x &= f(x, u) \tag{30}\\

y &= h(x) \tag{31}

\end{align*}

$$

执行机构一阶动态（$\Omega$ 是 $p \times p$ 对角矩阵，每个对角元素是一个舵机的带宽）：

$$

\dot u = \Omega \, (u_{\text{cmd}} - u) \tag{32}

$$

对输出 $y$ 求两次导（因为一阶导 $\dot y$ 里执行机构动态还没体现出来，要二阶导才出现 $u_{\text{cmd}}$）：

一阶导：

$$

\dot y = \frac{\partial h}{\partial x} \dot x

= \frac{\partial h}{\partial x} f(x, u)

\triangleq F(x, u) \tag{33}

$$

二阶导：

$$

\ddot y = \frac{\partial F}{\partial x} \dot x + \frac{\partial F}{\partial u} \dot u

= F_x \dot x + F_u \dot u \tag{34}

$$

现在代入执行机构方程 (32)：

$$

\ddot y = F_x \dot x + F_u \, \Omega \, (u_{\text{cmd}} - u) \tag{35}

$$

设 $v = \ddot y$ 为期望伪控制量。

令 $w = \Omega(u_{\text{cmd}} - u)$。

注意到 $w$ 恰好就是 $\dot u$——舵机运动的速率。方程变为：

$$

F_u \, w = v - F_x \dot x \tag{36}

$$

这就变成了一个标准的线性方程！对 $F_u$ 取广义逆：

$$

w = F_u^{I\dagger}(v - F_x \dot x)

$$

回代 $w$ 的定义：

$$

\boxed{ u_{\text{cmd}} = \Omega^{-1} F_u^{I\dagger} \bigl( v - F_x \dot x \bigr) + u } \tag{37}

$$

> 物理直觉：换元 $w = \dot u$ 后对 $F_u$ 求伪逆，等价于在所有满足控制要求的解中选一个让 $\|\dot u\|_2$ 最小的——即让舵机动得最"温柔"的那个解。这很符合实际飞控的需求。

### $v = \ddot y$ 该怎么设计？

跟踪误差 $e = y_{\text{des}} - y$，我们希望它按二阶系统收敛：

$$

\ddot e + k_1 \dot e + k_0 e = 0 \tag{38}

$$

在 $s$ 域可以看成快慢两个一阶环节的串联——快的是内环 $\omega_i$，慢的是外环误差动态 $\omega_e$：

$$

E(s)(s + \omega_e)(s + \omega_i) = E(s)\bigl(s^2 + (\omega_e+\omega_i)s + \omega_i\omega_e\bigr) \tag{39}

$$

对应时域：

$$

\ddot e + \omega_e \dot e + \omega_i \dot e + \omega_i\omega_e e = 0 \tag{40}

$$

因为 $\ddot e = \ddot y_{\text{des}} - \ddot y = \ddot y_{\text{des}} - v$，移项得：

$$

v = \ddot y_{\text{des}} + \omega_e \dot e + \omega_i \dot e + \omega_i\omega_e e \tag{41}

$$

MIMO 形式（每个通道有自己的带宽）：

$$

v = \ddot y_{\text{des}} + \Omega_e \dot e + \Omega_i \dot e + \Omega_i\Omega_e e \tag{42}

$$

把 $v$ 代入控制律 (37)，展开为两项：

$$

\begin{align*}

u_{\text{cmd}} &= \underbrace{\Omega^{-1} F_u^{I\dagger} \bigl( \ddot y_{\text{des}} + \Omega_e \dot e - F_x \dot x \bigr)}_{\text{第一项}} \\

&\quad + \underbrace{\Omega^{-1} F_u^{I\dagger} \bigl( \Omega_i \dot e + \Omega_i \Omega_e e \bigr)}_{\text{第二项}} + u \tag{43}

\end{align*}

$$

### 舵机够快时：退化为 INDI

如果舵机带宽 $\Omega$ 非常大，$\Omega^{-1} \approx 0$，第一项趋近于零。只留第二项：

$$

\begin{align*}

u_{\text{cmd}}

&= \Omega^{-1} F_u^{I\dagger} \Omega_i \bigl( \dot e + \Omega_e e \bigr) + u \\[4pt]

&= \Omega^{-1} F_u^{I\dagger} \Omega_i \bigl( v_{\text{des}} - \dot y_0 \bigr) + u \tag{44}

\end{align*}

$$

其中 $v_{\text{des}} = \dot y_{\text{des}} + \Omega_e(y_{\text{des}} - y)$。

如果更进一步，所有舵机带宽相同（$\Omega = \omega I_{p \times p}$）且内环带宽也匹配（$\Omega_i = \omega I_{m \times m}$），则 $\Omega^{-1}\Omega_i = I$，(44) 彻底退化为：

$$

u_{\text{cmd}} = F_u^{I\dagger} \bigl( v_{\text{des}} - \dot y_0 \bigr) + u \tag{45}

$$

这就是标准 INDI。

### ANDI vs INDI 一句话

ANDI 比 INDI 多了 (43) 的第一项。这一项只在舵机不够快时才重要——当舵机带宽相对于期望内环带宽不是"无限大"时，ANDI 显式补偿了舵机延迟，跟踪更准。舵机带宽越大，ANDI 和 INDI 的差距越小。