📝 docs: 新增控制器教程章节

Author: johnjim0816

CLAUDE.md

@@ -39,7 +39,7 @@ Datawhale 的具身智能开源教程，目标人群是**求职/转行**：应
 按机器人系统能力拆分，4 列。
 
 - **大脑：智能决策** — 强化学习决策、VLA、World-Model
-- **小脑：运动控制** — 强化学习控制、运动规划
+- **小脑：运动控制** — 强化学习控制、控制器、运动规划
 - **感官：感知系统** — VLM、定位与触觉感知
 - **工程底座** — 仿真工具、ROS2、CAN 与 MCU 通信、机械结构、模仿学习 + LeRobot
 

README.md

@@ -73,6 +73,7 @@
 | 章节 | 简介 | 状态 |
 | :--- | :--- | :--- |
 | [强化学习控制](docs/foundations/rl-for-robotics/10.ppo.md) | 把策略学习接到连续控制和机器人任务上 | ✅ 可用 |
+| [控制器](docs/foundations/controllers/intro.md) | PID、LQR、MPC、阻抗控制与系统集成教程 | ✅ 可用 |
 | [运动规划](docs/foundations/robotics-and-ros2/10.moveit2_basics.md) | Motion Planning 与 MoveIt 2 规划闭环 | ✅ 可用 |
 
 ### 感官：感知系统

docs/foundations/controllers/1.feedback-pid.md

@@ -0,0 +1,142 @@
+---
+title: "1. 闭环与 PID"
+sidebar_position: 2
+displayed_sidebar: foundationsControllersSidebar
+---
+
+# 1. 闭环与 PID
+
+控制器最基本的问题是：目标已经给定，机器人却因为惯性、摩擦、负载、外力和模型误差不会自动到达目标。闭环反馈的作用，就是不断比较“想要的状态”和“真实状态”，再用误差修正控制输入。
+
+## 1.1 开环局限
+
+开环控制只按预先设定的命令执行：
+
+```text
+目标命令 -> 执行器 -> 机器人运动
+```
+
+它不关心机器人到底有没有到达目标。比如你给一个关节固定电压，让它转 0.5 秒后停止。如果关节负载变重、摩擦变大、供电变低，它实际转过的角度都会变。
+
+闭环控制多了传感器反馈：
+
+```text
+目标状态 -> 比较误差 -> 控制器 -> 执行器 -> 机器人
+              ^                         |
+              |                         v
+              +-------- 传感器反馈 -------
+```
+
+它每个控制周期都问一次：
+
+- 当前状态是多少？
+- 目标状态是多少？
+- 差了多少？
+- 下一步应该输出多大控制量？
+
+机器人关节、移动底盘、机械臂末端、四足足端接触，几乎都离不开这条闭环。
+
+## 1.2 P 控制
+
+比例控制是最简单的反馈：
+
+$$
+u(t) = K_p e(t)
+$$
+
+其中 $e(t) = x^\*(t) - x(t)$ 是目标和当前状态的误差。$K_p$ 越大，误差带来的修正越强。
+
+直觉上，P 控制像一根弹簧：
+
+- 离目标越远，弹簧拉力越大。
+- 离目标越近，拉力越小。
+- $K_p$ 太小，系统软绵绵，回目标很慢。
+- $K_p$ 太大，系统容易冲过目标，来回振荡。
+
+对一个关节位置控制，可以写成：
+
+$$
+\tau = K_p(q^\* - q)
+$$
+
+这里 $q^\*$ 是目标关节角，$q$ 是当前关节角，$\tau$ 是输出力矩。
+
+## 1.3 D 控制
+
+只用 P 控制时，关节会因为惯性冲过目标。D 项根据误差变化速度输出控制量：
+
+$$
+u_D(t) = K_d \frac{de(t)}{dt}
+$$
+
+在关节控制里，常写成：
+
+$$
+\tau = K_p(q^\* - q) + K_d(\dot{q}^\* - \dot{q})
+$$
+
+如果目标速度 $\dot{q}^\*$ 为 0，那么 D 项会在关节运动过快时产生反向阻尼。它像汽车避震器，主要作用是抑制振荡。
+
+PD 控制是机器人里非常常见的底层控制器。很多强化学习 locomotion 策略并不直接输出电机电流，而是输出目标关节角，再由 PD 控制器在高频闭环里跟踪。
+
+## 1.4 I 控制
+
+积分项累计过去的误差：
+
+$$
+u_I(t) = K_i \int_0^t e(\tau)d\tau
+$$
+
+它适合处理长期存在的小偏差。例如电机负载导致 P 控制总是差一点到不了目标，积分项会不断累积这个小误差，直到控制输入足够抵消负载。
+
+但积分项也有副作用：
+
+- 大误差持续时间太长时，积分会越积越大。
+- 控制量已经饱和时，积分还在继续累积，会导致严重超调。
+- 传感器有偏置或低频噪声时，积分会把偏差放大。
+
+这就是工程里常说的**积分饱和**。它不是理论上的小问题，而是真机上很常见的失稳来源。
+
+## 1.5 PID 组合
+
+完整 PID 写成：
+
+$$
+u(t) = K_p e(t) + K_i \int_0^t e(\tau)d\tau + K_d \frac{de(t)}{dt}
+$$
+
+三项可以这样理解：
+
+| 控制项 | 直觉 | 主要解决 | 常见风险 |
+|---|---|---|---|
+| P | 误差越大，修正越强 | 响应速度 | 过大振荡，过小迟钝 |
+| I | 长期误差慢慢补回来 | 稳态误差 | 积分饱和，超调 |
+| D | 看误差变化趋势 | 阻尼和稳定性 | 放大噪声 |
+
+机器人关节控制里，最常见的是 PD 或带很小积分项的 PID。原因是关节编码器速度反馈通常足够好，而积分项在高动态任务里容易带来额外风险。
+
+## 1.6 离散 PD
+
+真实控制器在固定周期内运行。假设控制周期是 `dt`，关节目标是 `q_des`，当前关节角是 `q`，当前关节速度是 `dq`：
+
+```python
+def pd_control(q_des, dq_des, q, dq, kp, kd):
+    position_error = q_des - q
+    velocity_error = dq_des - dq
+    torque = kp * position_error + kd * velocity_error
+    return torque
+```
+
+如果目标是定点控制，`dq_des` 通常设为 0。如果目标是一条轨迹，`dq_des` 应该来自轨迹本身，而不是简单设 0。
+
+## 1.7 调参流程
+
+PID 调参不要三个参数一起动。推荐顺序：
+
+1. 先只开 P，让系统能朝目标运动。
+2. 慢慢增大 P，直到响应够快但还没有严重振荡。
+3. 加 D，把过冲和振荡压下来。
+4. 如果存在稳定后的长期偏差，再少量加 I。
+5. 一旦加 I，必须考虑积分限幅和输出饱和。
+
+下一章会把这些工程细节拆开：离散化、限幅、抗积分饱和、微分滤波和真机调参顺序。

docs/foundations/controllers/2.pid-engineering.md

@@ -0,0 +1,184 @@
+---
+title: "2. PID 工程"
+sidebar_position: 3
+displayed_sidebar: foundationsControllersSidebar
+---
+
+# 2. PID 工程
+
+PID 公式很短，但工程里真正决定稳定性的，往往是采样周期、单位、限幅、滤波和异常处理。很多“控制器不好用”的问题，不是理论错了，而是这些细节没有处理。
+
+## 2.1 离散实现
+
+真实控制器按固定频率运行。例如：
+
+- 机械臂关节控制：100Hz 到 1kHz 常见。
+- 四足底层电机控制：500Hz 到数 kHz 常见。
+- 上层策略或 VLA 推理：几 Hz 到几十 Hz 常见。
+
+连续 PID：
+
+$$
+u(t) = K_p e(t) + K_i \int e(t)dt + K_d \frac{de(t)}{dt}
+$$
+
+离散实现通常写成：
+
+$$
+I_k = I_{k-1} + e_k \Delta t
+$$
+
+$$
+D_k = \frac{e_k - e_{k-1}}{\Delta t}
+$$
+
+$$
+u_k = K_p e_k + K_i I_k + K_d D_k
+$$
+
+这里的 $\Delta t$ 非常重要。控制频率变了，积分和微分的数值都会变。如果代码里没有显式使用 `dt`，换频率后参数很容易失效。
+
+## 2.2 输出限幅
+
+电机、舵机、液压执行器都有上限：
+
+- 最大力矩
+- 最大速度
+- 最大电流
+- 最大位置范围
+- 最大温度或功率
+
+所以控制器输出必须限幅：
+
+```python
+def clamp(value, lower, upper):
+    return max(lower, min(upper, value))
+
+torque_cmd = clamp(torque_cmd, -max_torque, max_torque)
+```
+
+限幅不是“保守”，而是把控制器接入真实硬件的必要接口。没有限幅，仿真里也许只是动作夸张，真机上可能触发驱动器保护甚至损坏结构。
+
+## 2.3 抗积分饱和
+
+积分项最常见的问题是：输出已经到达上限，积分还在继续累积。
+
+例如目标很远，控制器想输出 `100 N·m`，但电机最多只能输出 `20 N·m`。如果积分项继续累积，等关节接近目标时，积分项仍然很大，系统会继续往前冲，产生巨大超调。
+
+常见抗积分饱和方法有三种。
+
+第一种是积分限幅：
+
+```python
+integral += error * dt
+integral = clamp(integral, -integral_limit, integral_limit)
+```
+
+第二种是输出饱和时暂停积分：
+
+```python
+raw_output = kp * error + ki * integral + kd * derivative
+output = clamp(raw_output, lower, upper)
+
+if raw_output == output:
+    integral += error * dt
+```
+
+第三种是反算抗饱和，把饱和前后的差值反馈给积分项。它更平滑，但实现也更复杂。
+
+入门阶段优先掌握前两种就够了。
+
+## 2.4 微分滤波
+
+微分项对噪声敏感。传感器读数如果有抖动，差分后会被放大：
+
+$$
+D_k = \frac{e_k - e_{k-1}}{\Delta t}
+$$
+
+当 $\Delta t$ 很小，哪怕误差只抖了一点，微分项也可能很大。
+
+工程里常见做法：
+
+- 对测量速度做低通滤波。
+- 使用编码器 / 驱动器估计的速度，而不是自己从位置差分。
+- 做 derivative on measurement：对测量值求微分，而不是对误差求微分，避免目标突变导致 D 项尖峰。
+
+关节定点控制常写成：
+
+$$
+\tau = K_p(q^\* - q) - K_d \dot{q}
+$$
+
+这等价于目标速度为 0 时的 PD 控制，也避免了目标位置阶跃变化时 D 项产生过大冲击。
+
+## 2.5 单位检查
+
+机器人控制里，单位错会直接毁掉调参结果。常见坑包括：
+
+| 变量 | 推荐单位 | 常见错误 |
+|---|---|---|
+| 角度 | rad | 用 degree 调参 |
+| 角速度 | rad/s | 把 rpm 当 rad/s |
+| 力矩 | N·m | 忽略减速比 |
+| 位置 | m | mm 和 m 混用 |
+| 时间 | s | ms 没除以 1000 |
+
+如果你发现参数看起来离谱，比如 `Kp` 要调到几十万才有反应，优先检查单位，而不是继续调参。
+
+## 2.6 PID 代码
+
+下面是一个可读性优先的 PID 骨架：
+
+```python
+class PIDController:
+    def __init__(self, kp, ki, kd, output_limit, integral_limit):
+        self.kp = kp
+        self.ki = ki
+        self.kd = kd
+        self.output_limit = output_limit
+        self.integral_limit = integral_limit
+        self.integral = 0.0
+        self.prev_error = 0.0
+
+    def reset(self):
+        self.integral = 0.0
+        self.prev_error = 0.0
+
+    def step(self, target, measurement, dt):
+        error = target - measurement
+
+        self.integral += error * dt
+        self.integral = clamp(
+            self.integral,
+            -self.integral_limit,
+            self.integral_limit,
+        )
+
+        derivative = (error - self.prev_error) / dt
+        raw = self.kp * error + self.ki * self.integral + self.kd * derivative
+        output = clamp(raw, -self.output_limit, self.output_limit)
+
+        self.prev_error = error
+        return output
+```
+
+实际项目里还会加入：
+
+- `dt` 过小或异常时跳过更新。
+- 目标突变时重置积分项。
+- 根据模式切换清空历史状态。
+- 输出命令做斜率限制，避免瞬间跳变。
+
+## 2.7 排错表
+
+| 现象 | 可能原因 | 优先处理 |
+|---|---|---|
+| 响应很慢 | $K_p$ 太小，输出限幅太低 | 增大 P，检查限幅 |
+| 到目标附近来回振荡 | $K_p$ 太大，$K_d$ 太小 | 降 P 或加 D |
+| 过冲很大 | 阻尼不足，积分过强 | 加 D，减 I |
+| 长期差一点到不了 | 有负载或摩擦偏置 | 少量加 I 或加前馈 |
+| 输出忽大忽小 | 微分放大噪声 | 过滤速度，减 D |
+| 仿真好，真机抖 | 摩擦、延迟、结构柔性、频率不同 | 降增益，从低速开始 |
+
+下一章进入状态空间和 LQR。它们不是替代 PID 的“高级玩具”，而是当系统状态耦合明显时，用模型系统化求反馈增益的方法。