4.2 状态预测器

0. 前言

在基于模型的控制系统中（LQR、MPC、NMPC 等），控制器依赖系统模型和当前状态来求解最优控制量。然而，从控制指令发出到执行器实际输出之间存在不可忽略的延迟。

以四旋翼电机为例(包括无刷电机和有刷电机)，其动态特性可建模为一阶惯性系统：

$$\tau \dot{T}+T=u_{cmd}$$

其中 $\tau$ 为电机时间常数，$T$ 为实际推力，$u_{cmd}$ 为控制指令。在离散实现中表现为指数移动平均：

$$T_k=\alpha T_{k-1}+(1-\alpha )u_k,\alpha =e^{-\mathrm{\Delta }t/\tau }$$

以 $\tau =0.031\text{s}$、$\mathrm{\Delta }t=0.0025\text{s}$ 为例，$\alpha \approx 0.9225$，最新指令在当前实际推力中的权重仅约 7.75%。当前推力是所有历史指令的指数加权混合，而非某一条指令的瞬时响应。

若控制器直接用当前传感器测量值 $x_k$ 计算反馈误差：

$$e_k=x_{goal}-x_k$$

则已发出但尚未生效的纠正指令会被控制器"遗忘"，从而在新的控制周期中再次叠加同类纠正量，导致超调、振荡甚至失稳。

状态预测器的核心思路：维护一个历史控制量的缓冲区，利用系统模型将这些"已发出但尚未完全生效"的指令代入模型进行开环推演，得到一个预测状态 ${\hat{x}}_{pred}$，然后用预测状态替代当前测量状态来计算反馈误差。

本教程介绍两种状态预测器的实现方式：适用于线性系统的离散矩阵预测器，以及适用于非线性系统的RK4 积分预测器。

读完本文后，你将能够：

• 量化电机延迟对控制性能的影响，理解为什么"用当前状态算误差"会引入振荡
• 实现离散矩阵预测器：用矩阵指数法离散化 $A_c,B_c$，配合 FIFO 控制历史做开环多步推演
• 实现 RK4 积分预测器：用四阶龙格-库塔法对非线性动力学做高精度前向积分
• 根据控制器类型（线性 vs. 非线性）和姿态表示（欧拉角 vs. 四元数）选择合适的预测器
• 理解两种预测器与 Smith 预估器的关系，掌握预测时域的工程选择方法

---

目录

2. 方法一：离散矩阵预测器（线性系统）
3. 方法二：RK4 积分预测器（非线性系统）
4. 两种方法的对比
5. 统一的物理参数
6. 总结

---

2. 方法一：离散矩阵预测器（线性系统）

2.1 适用场景

• LQR 控制、线性 MPC 等基于线性状态空间模型的控制器
• 系统可表示为 $\dot{x}=A_{c}x+B_{c}u$ 的标准形式
• 模型维度适中，矩阵运算开销可接受

2.2 连续模型与精确离散化

已知连续状态空间方程：

$$\dot{x}(t)=A_{c}x(t)+B_{c}u(t)$$

为得到可用于离散迭代的形式，采用矩阵指数法在零阶保持 (ZOH) 假设下精确离散化。构造增广矩阵：

$$M=\left[\begin{array}{cc}{A}_c& B_c\\ 0& 0\end{array}\right]$$

利用矩阵指数：

$$M_d=e^{M\cdot \mathrm{\Delta }t}=\sum _{k=0}^{\mathrm{\infty }}\frac{(M\cdot \mathrm{\Delta }t{)}^k}{k!}$$

提取离散化结果：

$$M_d=\left[\begin{array}{cc}{A}_d& B_d\\ 0& I\end{array}\right]$$

得到离散状态空间方程：

$$x_{k+1}=A_d{x}_k+B_d{u}_k$$

该方法等价于在 ZOH 假设下对连续系统做精确积分。当采样频率远高于系统带宽时，精度远优于前向欧拉法。

2.3 状态向量的选取

以 12 维状态向量为例（欧拉角姿态表示）：

$$x=[\varphi ,\theta ,\psi ,{\omega }_x,{\omega }_y,{\omega }_z,p_x,p_y,p_z,v_x,v_y,v_z{]}^T$$

其中 $\varphi ,\theta ,\psi$ 为欧拉角，$\omega$ 为角速度，$p$ 为位置，$v$ 为速度。控制向量为四个电机推力：

$$u=[T_1,T_2,T_3,T_4{]}^T$$

$A_c$ 的结构包含重力耦合项 $G$，反映小角度假设下俯仰/滚转产生水平加速度的物理机理：

$$A_c=\left[\begin{array}{cccc}{0}_{3\times 3}& I_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& I_{3\times 3}\\ G& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}\end{array}\right],G=\left[\begin{array}{ccc}0& g& 0\\ -g& 0& 0\\ 0& 0& 0\end{array}\right]$$

2.4 预测器设计

维护一个长度为 $N$ 的控制历史 FIFO 缓冲区：

$$\mathcal{H}=[u_{k-N},u_{k-N+1},\dots ,u_{k-1}]$$

单步预测

$${\hat{x}}_{i+1}=A_d{\hat{x}}_i+B_d{u}_i$$

多步开环推演

x̂ ← x_k                       (以当前测量为起点)
for i = 0 to N-1:
    x̂ ← A_d · x̂ + B_d · u_{k-N+i}
x̂_pred ← x̂

完整数学形式：

$${\hat{x}}_{pred}=A_d^N{x}_k+\sum _{i=0}^{N-1}{A}_d^{N-1-i}{B}_d\cdot u_{k-N+i}$$

用于反馈的误差计算方法

关键差异：反馈误差用预测状态而非当前测量状态：

$$e_k=x_{goal}-{\hat{x}}_{pred}✓$$

而非：

$$e_k=x_{goal}-x_k\times$$

控制器基于 $e_k$ 求解控制量（LQR 中为 $u_k=K{e}_k$，线性 MPC 中为二次规划求解），随后更新控制历史：

$$\mathcal{H}\leftarrow [u_{k-N+1},\dots ,u_{k-1},u_k]$$

2.5 控制环路完整流程

传感器 → x_k (当前测量)
              │
              ▼
  predictUpdate(x_k, H)  → 开环推演 N 步
              │
              ▼
       x̂_pred (预测状态)
              │
              ▼
  e = x_goal - x̂_pred
              │
              ▼
  控制器求解 u_k (LQR / 线性 MPC / ...)
              │
              ▼
  限幅 + 悬停前馈
              │
              ▼
  motorUpdate(u_k)  → 一阶惯性滤波
              │
              ▼
  实际推力 → 被控对象
              │
              ▼
  H ← FIFO-shift(H, u_k)

2.6 预测时域的选择

$$T_{predict}=N\cdot \mathrm{\Delta }t\approx \tau$$

因素	缩短时域	延长时域
未完成指令覆盖	不足（$1\tau$ 仅覆盖 63%）	更充分（$2\tau$ 86%, $3\tau$ 95%）
预测误差累积	较小	增大（线性化误差 $\times N$）
旧指令权重	—	$5\tau$ 前指令权重仅剩 $\approx 0.7\mathrm{\%}$，收益递减

$T_{predict}\approx \tau$ 是工程折中，非理论最优。

2.7 方法局限性

模型失配：预测模型假设控制量瞬时生效（$x_{k+1}=A_d{x}_k+B_d{u}_k$），但真实执行器有一阶惯性（$x_{k+1}=A_d{x}_k+B_d\cdot {\mathcal{F}}_{1st}(u_k)$），结构性误差无法通过调整 $N$ 根除。

开环误差累积：线性化误差、参数误差、未建模动态随 $N$ 累积，适用于短时域。

改进方向：将执行器动态增广进状态空间，使控制器内建延迟补偿，无需外挂预测器。代价是状态维数增加。

2.8 与 Smith 预估器的关系

本预测器在思想上属于 Smith 预估器 的离散化变体。标准 Smith 预估器针对纯延迟系统 $G(s)e^{-\tau s}$：

         ┌─── G(s) ───┐
u ──────→│            │──→ + ──→ y
    │     └───────────┘     +
    │                       -
    └──→ G(s)(1 - e^{-τs}) ──┘

本方法将纯延迟替换为一阶惯性 $\frac{1}{\tau s+1}$，通过 $A_d,B_d$ 和 FIFO 缓冲区实现了等效的内模补偿。

---

3. 方法二：RK4 积分预测器（非线性系统）

3.1 适用场景

• NMPC（非线性模型预测控制）等基于非线性动力学的控制器
• 系统动力学不能或不应被线性化（大角度机动、全姿态控制）
• 需要使用四元数姿态表示以避免欧拉角的奇异性问题

3.2 状态空间表示

状态向量采用 13 维，使用四元数表示姿态以避免万向节死锁：

$$x=[q_w,q_x,q_y,q_z,{\omega }_x,{\omega }_y,{\omega }_z,p_x,p_y,p_z,v_x,v_y,v_z{]}^T$$

符号	含义	坐标系
$\mathbf{q}\in {\mathbb{R}}^4$	姿态四元数（$q_w$ 为标量部分）	机体→世界的旋转
$\mathit{\omega }\in {\mathbb{R}}^3$	三轴角速度	机体坐标系
$\mathbf{p}\in {\mathbb{R}}^3$	三维位置	世界坐标系（惯性系）
$\mathbf{v}\in {\mathbb{R}}^3$	线速度	世界坐标系（惯性系）

控制输入不变：

$$u=[T_1,T_2,T_3,T_4{]}^T$$

3.3 非线性动力学方程

系统满足 $\dot{x}=f(x,u)$，各通道方程如下。

3.3.1 姿态运动学（四元数导数）

四元数微分方程：

$$\dot{\mathbf{q}}=\frac{1}{2}\mathbf{q}\otimes \left[\begin{array}{c}0\\ \mathit{\omega }\end{array}\right]$$

该方程保证四元数的时间演化始终对应于合法的三维旋转。

3.3.2 姿态动力学（欧拉方程）

刚体转动动力学：

$$\mathit{\tau }=\mathbf{I}\dot{\mathit{\omega }}+\mathit{\omega }\times (\mathbf{I}\mathit{\omega })$$

求解角加速度：

$$\dot{\mathit{\omega }}={\mathbf{I}}^{-1}(\mathit{\tau }-\mathit{\omega }\times (\mathbf{I}\mathit{\omega }))$$

其中 $\mathbf{I}=\text{diag}(I_{xx},I_{yy},I_{zz})$。展开形式：

$$\begin{array}{rl}{\dot{\omega }}_x& =\frac{{\tau }_x+(I_{yy}-I_{zz}){\omega }_y{\omega }_z}{I_{xx}}\\ {\dot{\omega }}_y& =\frac{{\tau }_y+(I_{zz}-I_{xx}){\omega }_x{\omega }_z}{I_{yy}}\\ {\dot{\omega }}_z& =\frac{{\tau }_z+(I_{xx}-I_{yy}){\omega }_x{\omega }_y}{I_{zz}}\end{array}$$

3.3.3 位置运动学

$$\dot{\mathbf{p}}=\mathbf{v}$$

即 ${\dot{p}}_x=v_x,{\dot{p}}_y=v_y,{\dot{p}}_z=v_z$。

3.3.4 线加速度（牛顿第二定律）

电机混合矩阵：将四个电机推力映射为机体坐标系合力与力矩：

机体合力（十字型布局下，推力仅沿机体 Z 轴）：

$${\mathbf{F}}_b=\left[\begin{array}{c}0\\ 0\\ T_1+T_2+T_3+T_4\end{array}\right]$$

机体合力矩：

$$\begin{array}{rl}{\tau }_x& =d(T_1-T_2-T_3+T_4)\\ {\tau }_y& =d(-T_1-T_2+T_3+T_4)\\ {\tau }_z& =k(-T_1+T_2-T_3+T_4)\end{array}$$

其中 $d$ 为力臂，$k$ 为反扭系数。

四元数旋转变换：将机体合力转至世界坐标系：

$${\mathbf{F}}_w=\mathbf{q}\otimes [0,{\mathbf{F}}_b]\otimes {\mathbf{q}}^{\ast }$$

其中 ${\mathbf{q}}^{\ast }=[q_w,-q_x,-q_y,-q_z]$ 为四元数共轭。

含重力的加速度：

$$\dot{\mathbf{v}}=\frac{{\mathbf{F}}_w}{m}-\left[\begin{array}{c}0\\ 0\\ g\end{array}\right]$$

3.4 数值积分：四阶龙格-库塔法（RK4）

由于非线性动力学 $\dot{x}=f(x,u)$ 无法解析求解，需要数值积分。选用 RK4 在精度和计算开销之间取得良好平衡。

对于给定步长 $\mathrm{\Delta }t$ 和控制量 $u_k$（在一个步长内视为恒定）：

$$\begin{array}{rl}{k}_1& =f(x_k,u_k)\\ k_2& =f(x_k+\frac{\mathrm{\Delta }t}{2}{k}_1,u_k)\\ k_3& =f(x_k+\frac{\mathrm{\Delta }t}{2}{k}_2,u_k)\\ k_4& =f(x_k+\mathrm{\Delta }t\cdot k_3,u_k)\\ x_{k+1}& =x_k+\frac{\mathrm{\Delta }t}{6}(k_1+2k_2+2k_3+k_4)\end{array}$$

精度特性：

指标	量级
局部截断误差	$O(\mathrm{\Delta }{t}^5)$
全局截断误差	$O(\mathrm{\Delta }{t}^4)$

3.5 四元数后处理

数值积分会使四元数偏离单位球面，每步积分后需归一化：

$$\mathbf{q}\leftarrow \frac{\mathbf{q}}{\parallel \mathbf{q}\parallel }$$

3.6 预测接口

单步预测

$$x_{k+1}=\text{RK4}(x_k,u_k,\mathrm{\Delta }t)$$

输入当前状态和单一控制向量，输出下一时刻状态。

多步轨迹预测

x[0] ← x_init
for i = 0 to N-1:
    x[i+1] ← RK4(x[i], u[i], Δt)

输入初始状态和控制序列 $[u_0,u_1,\dots ,u_{N-1}]$，输出长度为 $N+1$ 的状态轨迹（含初始状态）。在 NMPC 中，此轨迹用于评估代价函数和约束违反。

---

4. 两种方法的对比

维度	离散矩阵预测器	RK4 积分预测器
适用系统	线性或可线性化的系统	非线性系统
典型控制器	LQR、线性 MPC	NMPC
状态表示	欧拉角（小角度假设）	四元数（无奇异性）
单步计算	一次矩阵-向量乘法 $A_{d}x+B_{d}u$	四次函数求值 $f(x,u)$
计算复杂度	$O(n^2)$，$n$ 为状态维数	$O(n)$ 每步，但需 4 次函数求值
模型精度	受限于线性化精度（仅在小角度附近有效）	保留完整非线性动力学，大角度有效
离散化	一次性精确离散化（矩阵指数）	每步数值积分（RK4）
实现难度	低	中等
扩展性	需重新离散化	直接修改 $f(x,u)$ 即可

选择指南

控制器是线性的（LQR / 线性 MPC）？
        │
   ┌────┤
   │    │
  是    否（NMPC / 非线性控制器）
   │    │
   ▼    ▼
 离散   系统是否需要
 矩阵   全姿态 / 大角度？
 预测        │
 器     ┌────┤
        │    │
       是    否
        │    │
        ▼    ▼
      RK4   可考虑
      积分   线性化后
      预测   使用离散
      器     预测器

关于姿态表示的补充说明

两种方法采用不同的姿态表示，这并非随意选择，而是由其底层状态迭代机制的根本差异决定的。

离散矩阵预测器依赖加法迭代：$x_{k+1}=A_d{x}_k+B_d{u}_k$。每一次状态更新都是矩阵-向量乘加运算，本质上要求状态演化是各分量的线性叠加。

欧拉角在小角度假设下满足这一条件：运动学退化为 $\dot{\varphi }\approx {\omega }_x$、$\dot{\theta }\approx {\omega }_y$，可以自然写入 $A_c$。而四元数姿态运动学是双线性形式：

$$\dot{\mathbf{q}}=\frac{1}{2}\mathbf{q}\otimes \mathit{\omega }$$

状态分量（$\mathbf{q}$ 与 $\mathit{\omega }$）之间是乘性耦合，无法写成 $\dot{x}=A_{c}x+B_{c}u$ 的标准线性形式。若强行将四元数四个分量各自当独立标量做加法迭代，结果将偏离单位球面，不再表示合法旋转。

因此在采用了线性化模型的前提下，欧拉角不是"更好的选择"，而是唯一可行的姿态表示。代价是必须接受小角度假设和万向节死锁的理论风险，好在悬停/近悬停工况下这些条件自然满足。

RK4 积分预测器没有形式限制。RK4 每步直接调用 $f(x,u)$ 做四次函数求值：

k1 = f(x_k, u_k)
k2 = f(x_k + dt/2·k1, u_k)
...

$f(x,u)$ 内部可以自由使用四元数乘法、叉乘、归一化等非线性运算，不存在"必须写成加法"的约束。NMPC 场景下飞行器可能需要进行大角度机动，欧拉角的奇异性变得不可接受，四元数是必要选择。积分后追加一步归一化 $\mathbf{q}\leftarrow \mathbf{q}/\parallel \mathbf{q}\parallel$ 即可保持旋转表示的合法性。

小结：姿态表示的差异不是设计偏好，而是两种状态迭代范式的直接产物：加法迭代决定了线性预测器必须用欧拉角，函数求值赋予了非线性预测器自由使用四元数的能力。

---

5. 统一的物理参数

两套预测器共享同一组飞行器物理参数：

参数	符号	说明
质量	$m$	四旋翼总质量 [kg]
转动惯量	$I_{xx},I_{yy},I_{zz}$	三轴转动惯量 [kg·m²]
反扭系数	$k$	电机反扭与推力比例
力臂	$d$	电机轴到质心距离 [m]
重力加速度	$g$	9.81 m/s²
控制周期	$\mathrm{\Delta }t$	预测步长 [s]
电机时间常数	$\tau$	一阶惯性主导时间尺度 [s]

---

6. 总结

两种状态预测器的本质相同：利用系统模型和历史控制量，开环推演执行器延迟之后系统的等效状态，从而使控制器的反馈建立在更接近真实物理状态的基础上。

区别在于模型的形式和求解手段：

• 离散矩阵预测器将连续模型一次性精确离散化为 $A_d,B_d$，后续每次预测只需做矩阵乘法，计算极轻量。适合线性模型和欧拉角姿态表示。
• RK4 积分预测器保留完整非线性动力学，每步通过四阶龙格-库塔法进行数值积分。精度高、适用面广，但计算开销较大。适合 NMPC 和四元数全姿态控制。

在实际工程中，应根据控制器类型和姿态表示方式选择对应的预测器。两类预测器的控制历史管理、FIFO 移位、预测时域选择等上层逻辑完全一致，可以统一抽象。

本章参考资料与引申阅读已汇总至 参考资料。