我们会继续用大白话把这些讲明白：

编码器到底测到了什么？
↓
IMU到底测到了什么？
↓
加速度计为什么静止还可能看到重力？
↓
陀螺仪为什么会漂？
↓
机器人怎么知道自己身体倾斜多少？
↓
速度为什么比位置更难估？
↓
滤波到底在滤什么？
↓
低通滤波是什么？
↓
互补滤波是什么？
↓
Kalman Filter 到底在干嘛？
↓
为什么状态估计错了，MPC/WBC再高级也救不了？

把这块学懂之后，你就会开始真正理解：

控制器不是直接控制“真实机器人”，而是在控制“状态估计器告诉它的那个机器人”。

运控程序最核心的两类数据

几乎所有控制系统都绕不开：

State
Command

大白话：

State
=
机器人现在怎么样

Command
=
我接下来想让机器人怎么样

所以最核心：

机器人
↓
State
↓
Controller
↓
Command
↓
机器人

就是这么简单。

---

State

假设机器人有很多关节，最基本：

q
=
关节位置

dq
=
关节速度

tau
=
关节力矩

再加人形机器人：

IMU

可能包括：

身体姿态
角速度
线加速度

再高级一些：

foot_contact
foot_force
base_position
base_velocity

等等。

所以你可以脑补：

RobotState
│
├── joint_position q
├── joint_velocity dq
├── joint_torque tau
│
├── imu
│   ├── orientation
│   ├── angular_velocity
│   └── acceleration
│
├── contact
└── timestamp

以后看到很大的 RobotState 结构体，不要怕。

本质就是：

把机器人这一时刻的所有测量装进一个大盒子。

---

Command 

Command 是反方向，比如：

RobotCommand
│
├── q_des
├── dq_des
├── kp
├── kd
└── tau_ff

q_des
=
想去哪

dq_des
=
想以什么速度运动

kp
=
位置控制有多硬

kd
=
阻尼有多强

tau_ff
=
额外前馈力矩

然后：

RobotCommand
↓
发送到底层
↓
电机执行

概念代码：

while (running)
{
    RobotState state = readRobotState();

    RobotCommand cmd;

    cmd = controller.compute(state);

    sendRobotCommand(cmd);
}

大白话：

读取机器人
↓
得到 state

把 state 给控制器
↓
算出 cmd

把 cmd 发给机器人
↓
下一周期

所以：

Read
↓
Compute
↓
Write

是底层控制循环最核心的三步。

---

Timestamp

假设状态数据：

q = 0.5

但你不知道：

这是现在的数据？

还是：

20ms 前的数据？

这就很危险。

因此机器人数据里经常必须有：

timestamp

也就是：

这份数据是什么时刻产生的？

例如：

Frame 100
timestamp = 10.000 s

Frame 101
timestamp = 10.002 s

Frame 102
timestamp = 10.004 s

这样你才能判断：

周期对不对
有没有迟到
有没有掉帧
有没有乱序

---

Sequence Number 帧号

除了时间戳，经常还有：

seq
frame_id
counter

例如：

100
101
102
103

突然：

100
101
105

你马上知道：

102
103
104

中间的数据不见了。

所以：

timestamp
=
什么时候来的

sequence
=
第几帧

两者一起非常有用。

---

控制频率和数据频率不是永远一样

例如：

底层控制
1000 Hz

状态估计
500 Hz

规划
50 Hz

视觉
30 Hz

这完全可能。

为什么？

因为每一层需要的反应速度不一样。

---

比如：

电机控制

必须快：

1000 Hz
≈
1 ms 一次

因为机械系统变化很快。

---

全身控制

可能：

500 Hz

也是比较高频。

---

轨迹规划

可能：

50~100 Hz

不需要每毫秒重新规划整条路径。

---

视觉

可能：

30 Hz

因为相机本身就是几十 FPS。

所以：

整个机器人不是只有一个频率。

这非常重要。

---

分线程

假设你把所有事情写一起：

while (true)
{
    收UDP;
    读相机;
    做视觉;
    做规划;
    做控制;
    写日志;
    发命令;
}

有一天视觉突然：

卡了 30ms

那意味着：

控制器
也跟着30ms没运行

这可能很危险。

所以我们通常希望：

控制
和
慢任务

尽量分开。

于是出现：

Thread，线程。

                 ┌──────────────────┐
                 │ Communication    │
                 │ UDP / DDS / CAN  │
                 └────────┬─────────┘
                          ↓
                    Robot State
                          ↓
                 ┌─────────────────┐
                 │ State Estimator │
                 └────────┬────────┘
                          ↓
                  Estimated State
                          ↓
             ┌────────────┴────────────┐
             │                         │
       ┌────────────┐            ┌──────────┐
       │ Planner    │            │ WBC/MPC  │
       │ 慢一些      │            │ 快一些    │
       └─────┬──────┘            └────┬─────┘
             │                        │
             └────────────┬───────────┘
                          ↓
                     Joint Command
                          ↓
                 ┌────────────────┐
                 │ Low-level Ctrl │
                 │ PD / Torque    │
                 └───────┬────────┘
                         ↓
                      Motor

---

Mutex 

你以后会看到：

std::mutex

先大白话：

一个房间一次只能进一个人。

假设共享数据：

RobotState

通信线程想改：

我先锁门。

lock

改完：

unlock

控制线程想读：

等通信线程出去我再进。

这就是：

Mutex，互斥锁。

---

Double Buffer 双缓冲

这是一种很容易理解的思想，不要让两个线程抢同一个盒子，准备：

盒子A
盒子B

通信线程：

正在往A写完整的新状态

控制线程：

继续读B

A写完：

交换

下一周期：

控制线程读A
通信线程写B

像：

写 A      读 B
  ↓
交换
  ↓
读 A      写 B

这就是双缓冲的直觉。

---

Snapshot

控制器最希望拿到的是：

这一时刻完整一致的一份机器人状态。

比如：

q
dq
imu
contact
timestamp

全部来自同一个合理的时间点，这一整份：

Snapshot，状态快照。

而不是：

q 是 t1
dq 是 t2
IMU 是 t3

乱七八糟，所以状态同步在高级运控里非常重要。

---

Watchdog看门狗。

例如正常：

每2ms收到一次命令

如果突然：

50ms没有新命令

可能意味着：

控制程序死了
网络断了
电脑卡住了

这时候机器人不能：

永远执行最后一条危险命令。

所以 Watchdog 可能触发：

停止
进入阻尼模式
保持安全姿态
切换保护状态

具体行为取决于机器人平台。

---

Heartbeat 心跳

心跳和 Watchdog 经常一起出现。

程序每隔一段时间发送：

我还活着

例如：

heartbeat 1001
heartbeat 1002
heartbeat 1003

突然没了。

另一边：

对面可能挂了。

所以：

Heartbeat
=
证明我还活着

Watchdog
=
太久收不到，就采取措施

---

日志

因为机器人出问题经常：

只发生 20ms。

你肉眼根本看不清。

比如：

机器人突然抖了一下

到底是谁的问题？

可能：

q_des突然跳了
dq异常
IMU异常
通信掉帧
WBC输出爆了
torque饱和
状态延迟

没有日志：

全靠猜。

有日志：

timestamp
q
q_des
dq
dq_des
tau
imu
contact

就可以回放分析。

---

Planner 和 Controller 

假设任务：

右手 2 秒内移动到目标。

Planner：

负责想：
2秒内应该怎么走

Controller：

负责想：
这一毫秒应该出多少力

所以：

Planner
=
想长一点

Controller
=
管眼前

这很好记。

---

真实控制周期

高频线程里：

1. 获取最新状态
2. 更新状态估计
3. 取当前目标
4. 运行控制器
5. 安全限幅
6. 发送命令

就是：

State
↓
Estimate
↓
Reference
↓
Control
↓
Safety
↓
Command

高频线程越干净越好。

---

安全层

假设控制器算：

tau = 150 N·m

但安全规定：

最大 = 80 N·m

那最终发送前还要：

Clamp

所以：

Controller output
↓
Safety check
↓
Final command

控制器就算偶尔算疯了：

安全层还能再挡一次。

Safety Check

q_des
是否超过关节限位

dq_des
是否超过速度限制

tau
是否超过力矩限制

IMU
机器人是否倾倒

timestamp
状态是否过旧

command
是否长时间未更新

然后决定：

正常发送
或
限制
或
进入保护

这就是工程和纯理论的巨大区别。

理论：

算一个漂亮公式。

工程：

还必须保证它不会把机器人弄坏。