控制算法类型与论证：主要分为两种控制类型，第一种是将三轴旋转和三轴平动进行解耦，其中推力控制量属于平动范围，旋转扭矩控制仅仅控制矢量摆动机构而不是推力大小。第二种控制类型是六自由度全耦合控制，将所有状态量和控制量进行统一计算，如全状态的LQR MPC NMPC控制，其中LQR MPC在激进飞行姿态下可能存在稳态跟踪误差，NMPC高速求解较慢，所以需要进一步对各种控制方案进行理论验证。
| 符号 |
含义 |
| $\mathcal{W}$ |
世界坐标系（惯性系），Z 轴向上 |
| $\mathcal{B}$ |
机体坐标系，原点在发动机安装点，Z 轴沿箭体向上 |
| $\mathbf{R}_{wb} \in SO(3)$ |
机体系→世界系的旋转矩阵 |
| $\mathbf{R}{bw} = \mathbf{R}{wb}^\top$ |
世界系→机体系的旋转矩阵 |
| $\mathbf{q} = [w, x, y, z]$ |
机体系→世界系的单位四元数 |
| $\boldsymbol{\omega} \in \mathbb{R}^3$ |
机体角速度（机体系表示） |
| $\mathbf{p} \in \mathbb{R}^3$ |
世界系位置 |
| $\mathbf{v} \in \mathbb{R}^3$ |
世界系速度 |
| 参数 |
符号 |
值 |
单位 |
| 质量 |
$m$ |
0.5 |
kg |
| 转动惯量 |
$\mathbf{J}$ |
$\mathrm{diag}(0.00891, 0.00891, 0.00016)$ |
kg·m² |
| 发动机力臂 |
$L$ |
0.229 |
m |
| 最大推力 |
$T_{\max}$ |
10.0 |
N |
| 最大摆角 |
$\delta_{\max}$ |
30 |
deg |
发动机位于机体系原点 $\mathbf{r}_e = [0, 0, 0]^\top$，质心位于 $\mathbf{r}_c = [0, 0, L]^\top$。发动机相对质心的矢量：
$$\mathbf{r}_{ec} = \mathbf{r}_e - \mathbf{r}_c = [0, 0, -L]^\top$$
推力 $T$ 经两个万向节角度 $(\alpha_x, \alpha_y)$ 偏转后在机体系产生三轴分力：
$$\mathbf{F}_\mathcal{B} = \begin{bmatrix} F_x \ F_y \ F_z \end{bmatrix} = T \begin{bmatrix} \cos\alpha_x \sin\alpha_y \ \sin\alpha_x \ \cos\alpha_x \cos\alpha_y \end{bmatrix}$$
对于小角度：$F_x \approx T\alpha_y,; F_y \approx T\alpha_x,; F_z \approx T$
该力在质心处产生的力矩：
$$\boldsymbol{\tau} = \mathbf{r}_{ec} \times \mathbf{F}_\mathcal{B} = \begin{bmatrix} L \cdot F_y \ -L \cdot F_x \ 0 \end{bmatrix} \approx \begin{bmatrix} L T \alpha_x \ -L T \alpha_y \ 0 \end{bmatrix}$$
关键结论：单台摆角发动机无法产生滚转力矩（$\tau_z = 0$），仅可控俯仰和偏航两个姿态轴。
平动（世界系）：
$$\dot{\mathbf{p}} = \mathbf{v}, \qquad \dot{\mathbf{v}} = \frac{1}{m} \mathbf{R}_{wb} \mathbf{F}_\mathcal{B} + \mathbf{g}$$
其中 $\mathbf{g} = [0, 0, -9.81]^\top ;\mathrm{m/s^2}$。
转动（机体系）：
$$\dot{\mathbf{q}} = \frac{1}{2} \mathbf{q} \otimes \begin{bmatrix} 0 \ \boldsymbol{\omega} \end{bmatrix}, \qquad \dot{\boldsymbol{\omega}} = \mathbf{J}^{-1}\big(\boldsymbol{\tau} - \boldsymbol{\omega} \times \mathbf{J}\boldsymbol{\omega}\big)$$
两种算法共用相同的串级架构：
位置误差 → 外环 PD（位置/速度）→ 期望推力方向 + 推力大小
↓
姿态误差计算
↓
┌─────────────────────────┴─────────────────────────┐
↓ ↓
INDI 内环 LADRC 内环
ω̇_now 滤波 → Δτ = J(ω̇_des - ω̇_now) ESO 估计 f̂
↓ ↓
└─────────────────────────┬─────────────────────────┘
↓
力矩 → 摆角映射 → 三轴分力
外环（位置控制）在两种算法中完全相同，区别仅在内环（姿态控制）。
位置和速度误差：
$$\mathbf{e}_p = \mathbf{p}_{ref} - \mathbf{p}, \qquad \mathbf{e}_v = \mathbf{v}_{ref} - \mathbf{v}$$
期望加速度（PD + 前馈）：
$$\mathbf{a}_{des} = \mathbf{K}_p \odot \mathbf{e}_p + \mathbf{K}_d \odot \mathbf{e}_v + \mathbf{a}_{ref}$$
期望世界系推力：
$$\mathbf{F}_{\mathcal{W}} = m(\mathbf{a}_{des} - \mathbf{g})$$
推力大小（经饱和处理）：
$$T_{cmd} = \mathrm{clamp}\big(|\mathbf{F}_{\mathcal{W}}|,; 0.2 T_{hover},; T_{\max}\big)$$
其中悬停推力 $T_{hover} = mg \approx 4.9\mathrm{N}$。
期望推力方向（世界系单位矢量）：
$$\mathbf{d}_{\mathcal{W}} = \frac{\mathbf{F}_{\mathcal{W}}}{|\mathbf{F}_{\mathcal{W}}|}$$
仅控制箭体 Z 轴与期望推力方向对齐（滚转自由）：
$$\mathbf{e}_{att}^{\mathcal{W}} = (\mathbf{R}_{wb}\mathbf{e}_z) \times \mathbf{d}_{\mathcal{W}}$$
转换到机体系：
$$\mathbf{e}_{att}^{\mathcal{B}} = \mathbf{R}_{bw} , \mathbf{e}_{att}^{\mathcal{W}}$$
期望机体角速度（比例控制）：
$$\boldsymbol{\omega}_{des} = K_{att} \cdot \mathbf{e}_{att}^{\mathcal{B}}$$
INDI 的核心假设：在高控制频率下，状态变化远慢于控制输入变化。因此增量形式的动力学方程中，与当前状态相关的非线性项相互抵消，仅需考虑控制输入的增量效果。
角加速度动力学：
$$\dot{\boldsymbol{\omega}} = \mathbf{J}^{-1}\big(\boldsymbol{\tau}(\mathbf{u}) - \boldsymbol{\omega} \times \mathbf{J}\boldsymbol{\omega}\big)$$
在上一时刻 $t_0$ 对当前时刻 $t$ 做增量展开：
$$\dot{\boldsymbol{\omega}} \approx \dot{\boldsymbol{\omega}}_0 + \mathbf{J}^{-1} \frac{\partial \boldsymbol{\tau}}{\partial \mathbf{u}}\bigg|_0 \Delta\mathbf{u}$$
其中 $\dot{\boldsymbol{\omega}}_0$ 为当前测量的角加速度（由陀螺仪数值微分获得），非线性项 $\boldsymbol{\omega} \times \mathbf{J}\boldsymbol{\omega}$ 因在短时间内近似不变而消去。
令 $\Delta\dot{\boldsymbol{\omega}} = \dot{\boldsymbol{\omega}}_{des} - \dot{\boldsymbol{\omega}}_0$，得：
$$\Delta\boldsymbol{\tau} = \mathbf{J} \cdot \Delta\dot{\boldsymbol{\omega}}$$
期望角加速度（角速度误差的比例控制）：
$$\dot{\boldsymbol{\omega}}_{des} = K_{rate} \cdot (\boldsymbol{\omega}_{des} - \boldsymbol{\omega})$$
INDI 总力矩指令：
$$\boldsymbol{\tau}_{total} = \mathbf{J}\big(\dot{\boldsymbol{\omega}}_{des} - \dot{\boldsymbol{\omega}}_{now}\big)$$
其中 $\dot{\boldsymbol{\omega}}_{now}$ 由低通滤波微分器从 $\boldsymbol{\omega}$ 估计：
$$\dot{\boldsymbol{\omega}}_{now}[k] = \alpha \cdot \frac{\boldsymbol{\omega}[k] - \boldsymbol{\omega}[k-1]}{\Delta t} + (1-\alpha) \cdot \dot{\boldsymbol{\omega}}_{now}[k-1]$$
$$\alpha = \min(2\pi f_c \Delta t,; 1.0)$$
力矩→摆角映射（利用 $\tau_x = L T \alpha_x,; \tau_y = -L T \alpha_y$）：
$$\alpha_x = \mathrm{clip}\left(\frac{\tau_x}{L \cdot T_{eff}},; -\delta_{\max},; \delta_{\max}\right)$$
$$\alpha_y = \mathrm{clip}\left(-\frac{\tau_y}{L \cdot T_{eff}},; -\delta_{\max},; \delta_{\max}\right)$$
最终三轴分力由 $\alpha_x, \alpha_y, T_{cmd}$ 通过 $\mathrm{gimbalToForce}()$ 计算。
| 参数 |
物理意义 |
典型值 |
| $f_c$ |
ω̇ 低通滤波截止频率 |
25 Hz |
| $K_{att}$ |
姿态误差→角速度增益 |
16.0 |
| $K_{rate}$ |
角速度误差→角加速度增益 |
50.0 |
优点：响应快，不依赖精确动力学模型，对气动参数误差鲁棒，计算量小。
缺点：依赖角加速度测量（陀螺仪数值微分），对传感器噪声敏感。
LADRC 将系统建模为积分器串连标准型，把所有与标准型不符的部分（外部扰动 + 未建模动态 + 参数摄动 + 耦合项）统一视为"总扰动" $f$，由线性扩张状态观测器（LESO） 实时估计并在控制律中直接抵消。
对每个旋转轴建立一阶动力学模型：
$$\dot{\omega}_i = b_{0,i} \cdot u_i + f_i$$
其中 $u = [\alpha_x, \alpha_y]^\top$ 为摆角输入，$b_0$ 为名义控制增益，$f$ 为总扰动（包含 $\boldsymbol{\omega}\times\mathbf{J}\boldsymbol{\omega}$ 耦合、外部风扰力矩、模型误差等）。
名义控制增益（在悬停推力处评估，偏差由 ESO 吸收）：
$$b_{0,x} = \frac{L T_{hover}}{J_{xx}} \approx 126.2, \qquad b_{0,y} = -\frac{L T_{hover}}{J_{yy}} \approx -126.2$$
将总扰动 $f$ 视为扩展状态，对一阶系统设计二阶 LESO：
$$\begin{cases}
\dot{z}_1 = z_2 + \beta_1(y - z_1) + b_0 u \\
\dot{z}_2 = \beta_2(y - z_1)
\end{cases}$$
其中 $y = \omega_i$ 为测量值，$z_1$ 估计 $\omega_i$，$z_2$ 估计 $f_i$。
带宽参数化（将观测器极点配置在 $-\omega_o$）：
$$\beta_1 = 2\omega_o, \qquad \beta_2 = \omega_o^2$$
离散化更新（欧拉法）：
$$\begin{aligned}
e[k] &= y[k] - z_1[k] \\
z_1[k+1] &= z_1[k] + \Delta t \big(z_2[k] + \beta_1 e[k] + b_0 u[k]\big) \\
z_2[k+1] &= z_2[k] + \Delta t \big(\beta_2 e[k]\big)
\end{aligned}$$
期望角加速度（同 INDI 外环）：
$$\dot{\omega}_{i,des} = K_{rate} \cdot (\omega_{i,des} - \omega_i)$$
LADRC 扰动补偿控制律：
$$u_i = \frac{\dot{\omega}_{i,des} - \hat{f}_i}{b_{0,i}}$$
摆角限幅后通过 $\mathrm{gimbalToForce}()$ 输出三轴分力。
| 参数 |
物理意义 |
典型值 |
| $\omega_o$ |
ESO 观测器带宽 |
200 rad/s |
| $K_{att}$ |
姿态误差→角速度增益 |
16.0 |
| $K_{rate}$ |
角速度误差→角加速度增益 |
50.0 |
调参准则：$\omega_o$ 应为 $K_{rate}$ 的 3~5 倍，保证 ESO 估计速度远快于控制器响应速度。
优点：不依赖角加速度测量（仅需陀螺仪角速度），抗传感器噪声能力强；不需精确模型，参数摄动自动归入总扰动被补偿。
缺点：ESO 收敛需要时间（相比 INDI 有一阶滞后），大扰动下 ESO 估计可能出现短暂误差。
| 维度 |
INDI |
LADRC |
| 扰动处理方式 |
角加速度滤波微分 $\dot{\omega}_{now}$ 直接抵消 |
ESO 在线估计 $\hat{f}$ 后前馈补偿 |
| 传感器需求 |
陀螺仪（需数值微分） |
陀螺仪（仅需角速度） |
| 噪声敏感度 |
较高（微分放大噪声） |
较低（ESO 天然滤波） |
| 响应速度 |
极快（一步到位） |
较快（ESO 带宽决定） |
| 计算复杂度 |
$O(1)$，仅滤波+矩阵乘法 |
$O(1)$，ESO 递推+矩阵乘法 |
| 调参难度 |
2 个核心参数 |
3 个核心参数（多一个 $\omega_o$） |
| 理论保证 |
增量形式，时标分离假设 |
收敛性由 $\omega_o$ 保证 |
两者在数学形式上高度相似。将 INDI 的控制律展开：
$$\alpha_x^{INDI} = \frac{J_{xx}}{L T}\big(K_{rate}(\omega_x^{des} - \omega_x) - \dot{\omega}_x^{now}\big)$$
将 LADRC 的控制律展开：
$$\alpha_x^{LADRC} = \frac{J_{xx}}{L T}\big(K_{rate}(\omega_x^{des} - \omega_x) - \hat{f}_x\big)$$
区别仅在于：INDI 用 $\dot{\omega}_x^{now}$ 估计扰动，LADRC 用 ESO 的 $\hat{f}_x$ 估计扰动。INDI 可以视为一种特殊的 LADRC，其 ESO 退化为简单的滤波微分器。
| 场景 |
推荐 |
| 陀螺仪噪声低、需要极快响应 |
INDI |
| 传感器噪声较大、需要平滑控制 |
LADRC |
| 气动参数高度不确定 |
两者均可（均不依赖气动模型） |
| 快速原型/调试 |
LADRC（仅有角速度即可工作） |