本文把”传感器 → 状态估计 → 现代控制”这条链路讲透：IMU 为什么纯积分必漂移 → 互补滤波本质是两个一阶滤波器的叠加 → 卡尔曼滤波为什么”最优” → LQR 为什么最后落到 Riccati 方程 → MPC 为什么能处理约束 → WBC 的零空间投影在干什么。全程结合 RoboMaster（M3508/C620、云台）与 rm_pid_tuner 自动调参系统的实战数据。不满足于”知道有这些方法”，而是让你拿到每个方法能写进代码、能解释现象的版本。
传感器链路：一切控制的地基
IMU：为什么纯积分必漂移
陀螺仪输出角速度 $\omega$，姿态角理论上就是积分：
$$\theta(t) = \theta(0) + \int_0^t \omega(\tau),d\tau$$
问题出在误差模型。MEMS 陀螺的输出近似：
$$\tilde{\omega} = \omega + b + n$$
其中 $b$ 是零偏（bias），$n$ 是白噪声。积分后：
$$\theta_{est}(t) = \theta(t) + \underbrace{b \cdot t}{\text{零偏累积}} + \underbrace{\int_0^t n(\tau),d\tau}{\text{随机游走}}$$
零偏项随时间线性增长，白噪声积分变成随机游走（方差 $\propto t$）。一个零偏 0.01 rad/s 的消费级陀螺，10 秒后姿态误差就有 0.1 rad ≈ 5.7°，云台瞄准直接废掉。所以纯积分只能撑几秒，必须有别的传感器来”兜底”。
为什么加速度计能校正横滚/俯仰、校正不了航向
加速度计测的是比力（specific force）：静止时它只感受重力，所以有：
$$\theta_{roll} = \operatorname{atan2}(a_y,, a_z), \qquad \theta_{pitch} = \operatorname{atan2}(-a_x,, \sqrt{a_y^2 + a_z^2})$$
关键在可观性：重力方向 $\vec{g}$ 在水平面内旋转（yaw）时不变，所以航向角对加速度计不可观。yaw 只能靠磁力计（地球磁场方向）、视觉或编码器（轮式里程计）来校正——这就是为什么四足/人形机器人上电后 yaw 总要”找北”或依赖视觉。实战含义：云台 yaw 轴如果只靠 IMU，开机后必须回中/用编码器清零，别指望 IMU 自己知道朝哪。
互补滤波：两个一阶滤波器各取所长
陀螺仪高频准、低频漂；加速度计低频准、高频抖（振动污染）。互补滤波就是把两者按频率切开：
$$\hat{\theta} = \frac{1}{\tau s + 1},\theta_{acc} + \frac{\tau s}{\tau s + 1},\theta_{gyro}$$
陀螺通道是高通（$\tau s/(\tau s+1)$），加速度计通道是低通（$1/(\tau s+1)$），转折频率 $\omega_c = 1/\tau$。离散化（一阶后向差分）后就是一个 4 行代码的循环：
1 | // 互补滤波，dt 为采样周期，tau 为时间常数（典型 0.1~1s） |
tau 选大 → 更信任陀螺（平滑但响应慢、漂移时间长）；选小 → 更信任加速度计（跟手但噪声大）。工程上先调 tau 再谈卡尔曼——很多”姿态抖”的问题只是互补滤波系数没调对，上卡尔曼属于杀鸡用牛刀。
编码器：M3508 的”8192 线”真相与断电丢零点
增量式编码器输出 A/B 两路正交方波，4 倍频：一个机械周期内 A、B 各有一次上升/下降沿，共 4 个沿，所以分辨率 = 线数 × 4。RoboMaster 常用电机的反馈角度来自 C620 电调：每圈 8192 个计数（13 bit），即 0.0439°/LSB。
致命特性：增量式断电即丢位置。M3508 的”机械角度”是电调根据霍尔/反电动势估算的，上电时不知道转子在哪。所以：
- 云台/底盘上电必须回中（找机械限位或光电开关），否则位置环输出的是”假零点”下的控制量，轻则飞车重则撞限位
- 关节角换算要过减速比：M3508 减速比 3591:187 ≈ 19.2:1，输出轴角度 = 机械角 / 19.2，分辨率提升到 ~0.0023°/LSB——这就是为什么位置环能稳定到 0.1° 量级
1 | // C620 反馈的机械角度（0~8191）→ 输出轴角度（rad） |
链路延迟预算：控制周期不是拍脑袋定的
从传感器到执行器的总延迟决定能压多高的带宽：
| 环节 | 典型延迟 | 说明 |
|---|---|---|
| 编码器采样 → CAN 反馈帧 | ~0.5ms | C620 1kHz 反馈周期 |
| CAN 传输（1Mbps，8 字节帧） | ~0.13ms | 帧长 ≈ 130 bit |
| 控制计算（PID/状态机） | <0.1ms | 单核 1kHz 轻松 |
| 电调电流环响应 | ~1ms | C620 内部 20kHz 电流环，外环受限于通信 |
总延迟 1.52ms。经验法则：控制频率取最高谐振频率的 10 倍以上，且每个控制周期至少要收到 1 帧新反馈。RoboMaster 步兵 1kHz 控制频率、云台 1kHz、底盘速度环 1kHz，就是这个预算下的选择——再高 CAN 带宽不够，再低云台会抖。
状态估计：卡尔曼滤波为什么”最优”
一维推导：K 就是”信任比”
先看一维系统（比如云台 yaw 角），状态 $x$，预测与观测：
$$\hat{x}^- = \hat{x} + \dot{x},dt, \qquad z = x + v,\ v \sim \mathcal{N}(0, R)$$
融合的最优估计是方差加权平均：
$$\hat{x} = \hat{x}^- + K(z - \hat{x}^-), \qquad K = \frac{P^-}{P^- + R}$$
$K \in [0,1]$ 就是信任比：预测方差 $P^-$ 远大于观测噪声 $R$ 时 $K \to 1$（信观测），反之 $K \to 0$（信预测）。为什么是最优：在线性高斯假设下，最小均方误差估计就是条件期望，而卡尔曼滤波递推地计算它——这是从贝叶斯滤波推出来的，不是拍脑袋。
工程实现：三个必踩的坑
1 | // 一维卡尔曼（云台角度滤波），注意数值细节 |
- Q/R 不是随便填的：$Q$ 描述”模型有多不可信”（加速度大的场合要放大），$R$ 描述”传感器有多吵”（可从静止数据方差标定）。$Q$ 填太小 → 滤波器”自信过头”，输出滞后真实值；$R$ 填太小 → 输出跟噪声一起抖
- P 初始值别填 0：$P_0 = 0$ 意味着”我完全知道初始状态”，滤波器前几步几乎不更新，收敛慢。$P_0$ 填大（如 1）让滤波器快速收敛
- 发散保护：模型错误或野值时 $P$ 可能塌缩到 0，之后任何观测都改不动状态。工程上给 $P$ 设下限，或干脆用带野值剔除的版本（$|z - \hat{x}^-| > 3\sigma$ 时跳过更新）
LQR：最优控制的起点
问题设置：为什么代价函数是二次型
LQR 最小化：
$$J = \int_0^\infty \left( x^T Q x + u^T R u \right) dt$$
二次型不是随便选的：① 平方惩罚让大误差的代价增长比线性快，逼控制器优先消灭大偏差；② 二次型下的最优问题有解析解（Riccati），可以离线算完在线直接用。$Q$ 惩罚状态偏差（调大 → 响应快、控制猛），$R$ 惩罚控制量（调大 → 动作温和、省能量）。直觉：$Q_{ii}/R_{jj}$ 的比值决定”这条状态轴值得花多少控制代价去压”，调 LQR 本质是调这个比值而不是绝对大小。
为什么最后落到 Riccati 方程
用动态规划：假设最优值函数是二次型 $V(x) = x^T P x$，代入 HJB 方程：
$$\min_u \left[ x^T Q x + u^T R u + \nabla V^T (Ax + Bu) \right] = 0$$
对 $u$ 求导得 $u^* = -R^{-1} B^T P x$，代回得到 代数 Riccati 方程：
$$A^T P + P A - P B R^{-1} B^T P + Q = 0$$
工程要点：真实系统是离散的（1kHz 控制），必须用零阶保持（ZOH）精确离散化，而不是欧拉近似：
$$A_d = e^{AT}, \qquad B_d = \int_0^T e^{A\tau} d\tau \cdot B$$
欧拉近似 $A_d \approx I + AT$ 在 $T=1\text{ms}$ 时误差不大，但采样周期大（>10ms）时反馈增益会明显偏离最优，云台会表现出”算出来该稳却振荡”。用 Python 一行拿解：
1 | import control as ct |
工程案例：云台 yaw 轴 LQR 设计
把云台 yaw 轴近似为带粘滞阻尼的二阶模型（转动惯量 $J$、阻尼 $b$）：
$$\ddot{\theta} = -\frac{b}{J}\dot{\theta} + \frac{1}{J}\tau$$
状态 $x = [\theta,\ \dot{\theta}]^T$，$Q = \operatorname{diag}(100, 1)$、$R = 1$ 的含义：允许 0.1 rad 角度误差的代价 = 1 单位控制量的代价。调参路径：固定 $R$，$Q_{11}$ 从 1 按 10 倍步进往上试，直到阶跃响应超调量 ~10%、调节时间 ~100ms——这就是把”先 P 后 I 再 D”的经验翻译成了矩阵语言。
LQR 的坑：它假设无约束。云台大角度阶跃时最优控制量可能远超电机最大电流，实际输出被限幅 → 表现就是”理论最优”变成”实际超调+振荡”。解法：限幅后按受限 LQR（把饱和当作已知非线性）处理，或干脆上 MPC（见下）。
MPC：为什么能处理约束
滚动时域优化
每个采样时刻解一个有限时域最优问题，只执行第一步，下个时刻重新解：
$$\min_{u_{0:N-1}} \sum_{k=0}^{N-1} \left( x_k^T Q x_k + u_k^T R u_k \right) + x_N^T P_N x_N$$
$$\text{s.t.}\quad x_{k+1} = A_d x_k + B_d u_k,\qquad u_{min} \le u_k \le u_{max},\qquad x_k \in \mathcal{X}$$
为什么 MPC 能处理约束而 LQR 不能：约束下的最优控制律不再是线性反馈 $u = -Kx$，而是分段仿射（PWA）函数——解析解不存在，只能在线数值求解。MPC 本质是”每个时刻解一次带约束的优化”，把约束直接写进问题里。
实时性工程：1kHz 跑 MPC 的可行性
- 预测时域 N 的选择：$N \cdot T$ 要覆盖系统主要动态时间常数（云台
0.2s 动态 → $T=1\text{ms}$ 时 $N \ge 50$；实际上云台用 MPC 通常把周期放到 510ms、$N=20~30$） - 求解器：QP 用 OSQP/ProxQP，热启动（warm start）后 20 维问题 <1ms
- 超时保护：求解失败/超时必须沿用上一拍的可行解并报警，绝不能输出 0（输出 0 等于电机断电，云台直接砸下来）
1 | // MPC 求解失败时的安全策略（伪代码） |
RoboMaster 里的实际分工：底盘轨迹跟踪用 MPC（约束：电机最大电流、底盘最大速度），云台角度环用 LQR/PID（快、无强约束）。别把 MPC 当万能药——它贵在”处理约束”，便宜方案能解决就别上。
WBC：任务优先级与零空间投影
运动学 WBC：低优先级任务在高优先级任务的零空间里做
全身控制把多个任务（底盘前进、云台指向、保持姿态）排优先级，低优先级任务只能在不妨碍高优先级任务的自由度里执行。两级任务的速度层解：
$$\dot{q} = J_1^+ \dot{x}1 + \underbrace{(I - J_1^+ J_1)}{\text{任务1的零空间投影}} J_2^+ \left( \dot{x}_2 - J_2 J_1^+ \dot{x}_1 \right)$$
推导逻辑：先满足任务 1 得到 $\dot{q}_1^* = J_1^+ \dot{x}_1$；再在零空间 $\operatorname{Null}(J_1)$ 里找补偿项 $\Delta q$ 让任务 2 尽量接近——零空间投影 $I - J_1^+ J_1$ 保证 $\Delta q$ 完全不改变任务 1 的输出（$J_1 \Delta q = 0$）。这就是”低优先级不干扰高优先级”的数学含义。
RoboMaster 特例：步兵底盘 + 云台就是一个天然的两级系统——底盘跟随（任务 1，优先级高，因为底盘动了云台指向就被带着偏）→ 云台补偿（任务 2，在底盘运动造成的零空间里把指向拉回来）。云台手看到的”指哪打哪”，本质是底盘/云台解耦 + 前馈补偿，没必要上完整 WBC，但理解零空间投影能让你看懂为什么云台补偿要加底盘速度前馈：$\dot{\theta}{yaw}^{ff} = -\dot{\theta}{chassis}$。
力矩级 WBC：从运动学到动力学
速度级 WBC 不考虑力，力矩级 WBC 直接优化关节力矩：
$$\min_{\ddot{q},, \tau} \sum_i w_i \left| J_i \ddot{q} + \dot{J}_i \dot{q} - \ddot{x}_i^{des} \right|^2$$
$$\text{s.t.}\quad M(q)\ddot{q} + C(q,\dot{q})\dot{q} + g(q) = \tau + J_c^T f_c$$
（$M$ 惯量矩阵、$C$ 科氏/离心、$g$ 重力、$J_c^T f_c$ 接触力）。这是四足/人形（MIT Cheetah、Cassie）的标准做法：优先级 + 动力学约束 + 接触约束同一个 QP 里解。实战提醒：力矩级 WBC 依赖 $M/C/g$ 模型精度，模型错了不如简单 PID——RoboMaster 场景没有全身动力学需求，用两级解耦是工程上更稳的选择。
踩坑清单（实战汇总）
- IMU 纯积分当姿态用：零偏累积几秒就飘，必须融合加速度计（互补滤波起步，别一上来就卡尔曼）
- yaw 不可观：加速度计校正不了航向，上电回中/磁力计/视觉三选一
- 编码器断电丢零点：增量式上电必须回中，过减速比换算别忘（M3508 19.2:1）
- 控制频率与谐振：频率低于机械谐振 10 倍，云台必抖；先查机械再调参
- LQR 忽略约束：大偏差限幅饱和 → 实际超调，先检查控制量有没有顶到限幅
- MPC 求解超时输出 0：必须沿用上一拍可行解 + 连续失败急停
- 离散化用欧拉近似：采样周期大时反馈增益偏离最优，用 ZOH 精确离散
- 卡尔曼 Q/R 瞎填：$Q$ 太小输出滞后、$R$ 太小输出跟噪，静止数据标定 $R$ 起步
- WBC 模型误差：动力学参数不准时力矩级 WBC 不如解耦 PID，先评估模型可信度再上全身控制
选型速查：无约束调节 → LQR（离线 Riccati）；带约束跟踪 → MPC（在线 QP）；多任务优先级 → WBC（零空间投影）；RoboMaster 云台/底盘 → 两级解耦 + PID/LQR 足够。