本文档属于 Robotics Tutorial 项目，作者：Pengfei Guo，达妙科技。采用 CC BY 4.0 协议，转载请注明出处。
F10 综合实战——协作装配任务从零搭建¶
本章定位：本章是力控子课程（F01-F09）的综合实战项目——Mini-ForceControl 的最终集成。目标是在 MuJoCo Franka Panda 仿真中从零搭建一个完整的力控系统，实现三个力控任务（零力引导、恒力打磨、peg-in-hole 装配），并集成安全监控。本章不引入新理论，而是将 F01-F09 的所有知识**综合应用**到一个工程项目中，体验从"单一控制律"到"完整系统"的工程鸿沟。
前置依赖：F01-F09 全部章节
建议用时：2 周（环境搭建 + 控制器 + 任务 + 安全 + 评测）
前置自测 ⭐¶
📋 答不出 >= 3 题 → 建议先回 F01-F09 复习
| 编号 | 问题 | 答不出时回顾 |
|---|---|---|
| 1 | 阻抗控制和导纳控制的因果性区别是什么？Franka 的力矩接口应该用哪种？ | F01 |
| 2 | 笛卡尔阻抗控制律的完整形式是什么？写出重力补偿项。 | F03 |
| 3 | 动量观测器 \(\hat{p} = M\dot{q}\) 的递推公式是什么？它如何检测碰撞？ | F06 |
| 4 | 阻抗参数 \((K_d, D_d)\) 的选择如何影响接触过渡的力冲击？ | F02, F04 |
| 5 | 状态机（FSM）在力控系统中的作用是什么？为什么不能只用一个固定的控制律？ | F10 本章核心 |
本章目标¶
学完本章后，你应该能够：
- 搭建 MuJoCo Franka 仿真环境 + ros2_control 接口
- **实现**笛卡尔阻抗控制器（1 kHz），参数可动态调整
- **实现**三个力控任务：零力引导、恒力打磨、peg-in-hole 装配
- **集成**有限状态机（FSM）管理任务切换和接触过渡
- **添加**动量观测器碰撞检测 + 安全停止
- **设计**评价指标并定量评测系统性能
- **诊断**常见失败模式并掌握系统级调试方法
F10.1 项目设计——多步 peg-in-hole 装配 ⭐¶
动机——为什么需要综合实战¶
回顾 F01-F09：每一章聚焦一个知识点。但真实力控系统不是"一个算法"，而是"一个系统"：多个模块协同工作，处理各种边界情况。
从算法到系统的鸿沟：
| 维度 | 算法级（F01-F09） | 系统级（F10） |
|---|---|---|
| 控制律 | 一个控制律 | 多个控制律 + 状态机切换 |
| 参数 | 固定参数 | 动态调参 + 自适应 |
| 异常处理 | 不考虑 | 碰撞检测 + 安全停止 + 超时 |
| 评价 | "看起来对" | 定量指标（成功率/力峰值/周期时间） |
| 调试 | 单步调试 | 日志 + 可视化 + 重放 |
项目需求¶
任务: 在 MuJoCo 仿真中完成以下操作序列
1. 零力引导（Human-in-the-Loop）:
- 操作员通过虚拟力导引机器人到初始位置
- K_d -> 0, 末端柔顺跟随虚拟力
- 验证: 末端力 < 2N
2. 恒力打磨:
- z 轴维持 10N 法向力
- xy 轴跟踪圆形轨迹（半径 3cm, 周期 4s）
- 验证: 力稳态误差 < 1N, 轨迹误差 < 2mm
3. Peg-in-hole 装配:
- 8mm 圆柱销插入 8.1mm 孔
- 四阶段: 接近 -> 搜索 -> 对准 -> 插入
- 验证: 20 次试验成功率 > 90%
系统需求:
- 安全: 碰撞检测 + 紧急停止（末端力 > 20N 或关节残差异常）
- 性能: 1 kHz 控制循环，无抖动
- 可配置: 所有参数可通过 YAML 文件修改
系统架构¶
┌──── MuJoCo 仿真 ────────────────────────────────┐
│ Franka Panda + 桌面 + peg + hole + 打磨表面 │
│ 力传感器: 末端 6D wrench │
└──────────────── mujoco_ros2_control ───────────────┘
↕ ros2_control HardwareInterface
┌──── 控制层 ──────────────────────────────────────┐
│ CartesianImpedanceController (1 kHz) │
│ 参数可动态调整: K_d, D_d, x_d, wrench_ff │
│ 安全: 误差裁剪, 力矩饱和 │
└──────────────────────────────────────────────────┘
↕ ROS2 话题/服务
┌──── 任务层 ──────────────────────────────────────┐
│ FSM（有限状态机）: │
│ IDLE -> APPROACH -> CONTACT -> TASK -> RETRACT │
│ 三个任务模式（由参数切换）: │
│ A. 零力引导（K_d -> 0） │
│ B. 恒力打磨（z 力控 + xy 位控） │
│ C. Peg-in-hole（四阶段阻抗切换） │
└──────────────────────────────────────────────────┘
↕
┌──── 安全层 ──────────────────────────────────────┐
│ 末端力阈值（N）+ 动量观测器残差阈值（Nm） │
│ 关节限位监控 │
│ 力矩饱和保护 │
│ 紧急停止服务 │
└──────────────────────────────────────────────────┘
跨领域类比：这个三层架构就像飞机的控制系统——任务层是飞行管理系统（FMS，决定飞哪里），控制层是自动驾驶仪（跟踪航线），安全层是包线保护系统（防止失速/超速）。每层都可以独立调试，但最终必须协同工作。
从 F01 到 F09 知识点综合应用 Mapping ⭐¶
本项目综合应用了前面所有章节的知识。以下 mapping 表帮助读者快速定位"在做这个模块时需要回顾哪章"。
| 项目模块 | 使用的知识点 | 来源章节 | 核心公式/概念 |
|---|---|---|---|
| 仿真环境 | 接触模型参数 | F01 第 4 节 | 环境阻抗 \(Z_e = K_e + D_e s\) |
| 阻抗控制器 | 笛卡尔阻抗控制律 | F03 第 2-3 节 | 本章取 \(e_x=x_d-x\)：\(\tau = J^T(K_d e_x - D_d \dot{x}) + g\) |
| 力位混合 | 选择矩阵 | F03 第 5 节 | \(S \cdot F_{pos} + (I-S) \cdot F_{force}\) |
| 操作空间 | 雅可比、正运动学 | F02 全章 | \(J(q), \text{FK}(q)\) |
| 力传感器标定 | 传感器模型 | F01 第 3 节 | \(f_{true} = C \cdot f_{raw} - b\) |
| 导纳控制 (打磨) | 导纳控制律 | F05 全章 | 世界 \(z\) 向上时：\(\Delta z = -A(f_{target}-f_{contact})dt\) |
| 碰撞检测 | 动量观测器 | F06 第 2 节 | \(\hat{p} = M\dot{q}\), 残差估计外力 |
| 变阻抗 | 无源性约束 | F06 第 3 节 | \(\dot{V} \leq 0\) (能量不增) |
| 参数平滑过渡 | 阻抗参数插值 | F04 第 4 节 | \(K(t) = K_1 + (K_2 - K_1) \cdot \text{smooth}(t)\) |
| MPC 轨迹 (可选) | 操作空间 MPC | F08 第 5-6 节 | Crocoddyl/OCS2 MPC |
| RL 策略 (可选) | VICES/SERL | F09 第 2-3 节 | SAC + 阻抗动作空间 |
| 安全层 | 碰撞检测+安全停止 | F06 第 4 节 | 残差阈值判断 |
多步 peg-in-hole 完整系统设计 ⭐⭐¶
不同于 F10.4 中简化的四阶段 PiH，真实的多步装配系统需要更细致的设计。以下是工业级多步 PiH 的完整设计。
任务规格：
零件规格:
销 (peg): 直径 8.0mm, 长度 30mm, 铝合金
孔 (hole): 直径 8.1mm, 深度 25mm, 带 0.5mm 倒角
间隙: 0.1mm (单侧 0.05mm)
公差: IT7 级 (H7/g6 配合)
性能要求:
成功率: > 95% (100 次试验)
周期时间: < 15s (不含搬运)
最大接触力: < 30N (防止零件损伤)
力峰值: < 20N (正常操作)
末端精度: < 0.05mm (定位阶段)
七阶段流程（比四阶段更细致）：
Phase 0: 初始化
- 力传感器偏置校准 (2s 自由运动取均值)
- 重力补偿校准 (多构型辨识)
- 安全系统自检
Phase 1: 抓取 (GRASP)
- 移到 peg 上方 50mm
- 下降抓取, 夹爪闭合
- 验证: 夹爪力 > 5N
- K_d = [500, 500, 500, 50, 50, 50]
Phase 2: 搬运 (TRANSPORT)
- 抬升 peg 到安全高度
- 平移到 hole 上方
- 下降到 hole 上方 10mm
- K_d = [400, 400, 400, 40, 40, 40]
Phase 3: 精定位 (FINE_POSITION)
- 视觉/力反馈精确对准孔口
- 误差 < 0.5mm 才进入下一阶段
- K_d = [200, 200, 100, 20, 20, 20]
- 超时 5s -> 回到 Phase 2
Phase 4: 搜索接触 (SEARCH)
- 螺旋搜索: r(t) = r_max * t/T, theta(t) = 2*pi*n*t/T
- z 轴推力: F_ff_z = -5N (轻柔下压)
- K_xy = [100, 100], K_z = 30
- 终止: z 下降 > 0.5mm (进入倒角)
- 超时 10s -> 扩大搜索半径重试
Phase 5: 对准 (ALIGNMENT)
- 利用倒角的被动对准效应
- 极低横向刚度: K_xy = [20, 20]
- z 轴维持 5N
- 终止: z 下降 > 2mm (过了倒角进入直孔段)
- 异常: 侧向力 > 15N -> 回到 Phase 4
Phase 6: 插入 (INSERTION)
- z 轴力控: F_target = 10N (恒力推入)
- xy 极低刚度: K_xy = [15, 15] (允许被动导向)
- 监控: |f_xy| < 10N (防楔死)
- 终止: z 达到目标深度
- 异常: |f_xy| > 15N 持续 0.5s -> 微撤退+旋转重试
Phase 7: 验证 + 释放 (VERIFY)
- 检查插入深度是否达标
- 缓慢释放夹爪
- 轻拉验证 peg 不脱出
- 缩回安全位置
⚠️ 常见陷阱¶
🧠 思维陷阱：认为"先把所有模块写完再集成"
新手想法："先写控制器、再写 FSM、再写安全，最后组装"
实际上：这种"瀑布式"开发在机器人项目中经常失败。
模块之间有大量隐式依赖。
正确做法：增量式开发——
Day 1-2: 环境 + 最简控制器
Day 3-4: 阻抗控制器 + 简单到达任务
Day 5-6: FSM + 零力引导
Day 7-8: 恒力打磨
Day 9-10: Peg-in-hole
Day 11-12: 安全 + 评测
每天都有可运行的系统
练习¶
- ⭐ 架构设计：画出系统的 ROS2 节点图，列出所有节点、话题、服务，标注消息类型和频率。
- ⭐ 接口定义：为控制层定义完整的 ROS2 接口。
F10.2 环境搭建——MuJoCo + ros2_control ⭐¶
动机——为什么选择 MuJoCo + ros2_control¶
| 选项 | 优点 | 缺点 |
|---|---|---|
| MuJoCo + ros2_control | 高精度力仿真，接口标准化 | 需自己写 HW Interface |
| Gazebo + ros2_control | 社区支持好 | 力仿真精度不如 MuJoCo |
| Isaac Sim | GPU 加速，渲染好 | 重，学习曲线陡 |
仿真场景¶
<!-- MuJoCo MJCF: mini_force_control.xml -->
<mujoco>
<option timestep="0.001" gravity="0 0 -9.81"/>
<worldbody>
<!-- 桌面 -->
<body name="table" pos="0.5 0 0.3">
<geom type="box" size="0.3 0.3 0.01" rgba="0.8 0.6 0.4 1"/>
</body>
<!-- Peg (8mm) -->
<body name="peg" pos="0.4 0 0.35">
<joint name="peg_free" type="free"/>
<geom type="cylinder" size="0.004 0.03" mass="0.05"
rgba="0.9 0.1 0.1 1"/>
</body>
<!-- Hole (8.1mm clearance proxy)
MuJoCo geom 不支持布尔减法；这里用四个薄壁 box 留出
8.1mm 的方形近似孔，避免把“孔”建成实心圆柱。 -->
<body name="hole_base" pos="0.5 0 0.31">
<geom name="hole_wall_px" type="box" pos="0.01905 0 0"
size="0.015 0.035 0.01" rgba="0.3 0.3 0.8 1"/>
<geom name="hole_wall_nx" type="box" pos="-0.01905 0 0"
size="0.015 0.035 0.01" rgba="0.3 0.3 0.8 1"/>
<geom name="hole_wall_py" type="box" pos="0 0.01905 0"
size="0.035 0.015 0.01" rgba="0.3 0.3 0.8 1"/>
<geom name="hole_wall_ny" type="box" pos="0 -0.01905 0"
size="0.035 0.015 0.01" rgba="0.3 0.3 0.8 1"/>
</body>
<!-- 打磨表面 -->
<body name="grinding_surface" pos="0.5 0.2 0.31">
<geom type="box" size="0.1 0.1 0.005"
friction="0.8 0.005 0.0001"
rgba="0.5 0.5 0.5 1"/>
</body>
<!-- Franka Panda -->
<include file="franka_panda.xml"/>
</worldbody>
<sensor>
<force name="ee_force" site="ee_site"/>
<torque name="ee_torque" site="ee_site"/>
</sensor>
</mujoco>
关节 effort 接口只有在 MJCF 中使用直接 torque actuator 且关节传动 gear=1 时，ctrl 才能按关节力矩解释。若模型使用电机、滑轮、非 1 gear 或 tendon transmission，ctrl 和 actuator_force 都是 actuator 空间量，必须通过显式 transmission/gear 做正反向映射。
ros2_control Hardware Interface¶
class MujocoHWInterface : public hardware_interface::SystemInterface {
public:
std::vector<hardware_interface::StateInterface> export_state_interfaces() override {
std::vector<hardware_interface::StateInterface> interfaces;
for (int i = 0; i < 7; ++i) {
interfaces.emplace_back(joint_names_[i], "position", &q_[i]);
interfaces.emplace_back(joint_names_[i], "velocity", &dq_[i]);
interfaces.emplace_back(joint_names_[i], "effort", &tau_measured_[i]);
}
// 力/力矩传感器接口名应在配置中固定顺序：
// ["force.x","force.y","force.z","torque.x","torque.y","torque.z"]。
// 控制器端按名称查找，不依赖导出顺序。
for (int i = 0; i < 6; ++i) {
interfaces.emplace_back("ft_sensor", ft_names_[i], &f_ext_[i]);
}
return interfaces;
}
std::vector<hardware_interface::CommandInterface> export_command_interfaces() override {
std::vector<hardware_interface::CommandInterface> interfaces;
for (int i = 0; i < 7; ++i) {
interfaces.emplace_back(joint_names_[i], "effort", &tau_cmd_[i]);
}
return interfaces;
}
hardware_interface::return_type read(const rclcpp::Time&, const rclcpp::Duration&) override {
// 使用 MuJoCo 的 jnt_qposadr / jnt_dofadr 健壮索引
// 避免硬编码偏移量（当模型含自由关节或多体时直接用 [i] 会错位）
for (int i = 0; i < 7; ++i) {
int jnt_id = mj_name2id(mj_model_, mjOBJ_JOINT, joint_names_[i].c_str());
if (jnt_id < 0) {
return hardware_interface::return_type::ERROR;
}
int qpos_adr = mj_model_->jnt_qposadr[jnt_id];
int dof_adr = mj_model_->jnt_dofadr[jnt_id];
q_[i] = mj_data_->qpos[qpos_adr];
dq_[i] = mj_data_->qvel[dof_adr];
// actuator_force 是 actuator 空间量，不一定等于关节广义力矩。
// qfrc_actuator 已经是 MuJoCo 经 actuator/transmission 后得到的
// generalized force，按 dof_adr 读取才是该关节的广义力矩。
tau_measured_[i] = mj_data_->qfrc_actuator[dof_adr];
}
// MJCF 中定义的是两个 3D 传感器: ee_force 和 ee_torque。
// 不能假设存在名为 ft_sensor 的 6D 连续传感器。
int force_id = mj_name2id(mj_model_, mjOBJ_SENSOR, "ee_force");
int torque_id = mj_name2id(mj_model_, mjOBJ_SENSOR, "ee_torque");
if (force_id < 0 || torque_id < 0 ||
mj_model_->sensor_dim[force_id] != 3 ||
mj_model_->sensor_dim[torque_id] != 3) {
return hardware_interface::return_type::ERROR;
}
int force_adr = mj_model_->sensor_adr[force_id];
int torque_adr = mj_model_->sensor_adr[torque_id];
int site_id = mj_name2id(mj_model_, mjOBJ_SITE, "ee_site");
if (site_id < 0) {
return hardware_interface::return_type::ERROR;
}
Eigen::Vector3d f_site, tau_site;
for (int i = 0; i < 3; ++i) {
f_site[i] = mj_data_->sensordata[force_adr + i];
tau_site[i] = mj_data_->sensordata[torque_adr + i];
}
Eigen::Map<const Eigen::Matrix<double,3,3,Eigen::RowMajor>> R_WS(
&mj_data_->site_xmat[9 * site_id]); // site S -> world W
Eigen::Vector3d f_world = ft_sensor_sign_ * (R_WS * f_site);
Eigen::Vector3d tau_world = ft_sensor_sign_ * (R_WS * tau_site);
for (int i = 0; i < 3; ++i) {
f_ext_[i] = f_world[i];
f_ext_[i + 3] = tau_world[i];
}
// 注意: force/torque site sensor 在 site 局部系表达，且符号约定
// 需要用已知外力单元测试确认；控制器内部只使用这里转换后的约定帧。
return hardware_interface::return_type::OK;
}
hardware_interface::return_type write(const rclcpp::Time&, const rclcpp::Duration&) override {
for (int i = 0; i < 7; ++i) {
int act_id = mj_name2id(mj_model_, mjOBJ_ACTUATOR,
actuator_names_[i].c_str());
if (act_id < 0) {
return hardware_interface::return_type::ERROR;
}
// 若 actuator 是 torque motor 且 gear=1，则 ctrl 就是关节力矩。
// 否则必须在 actuator_ctrl_from_joint_tau() 中做 transmission/gear
// 的反向映射，不能把 joint torque 直接写入 actuator ctrl。
mj_data_->ctrl[act_id] =
actuator_ctrl_from_joint_tau(i, tau_cmd_[i]);
}
mj_step(mj_model_, mj_data_);
return hardware_interface::return_type::OK;
}
};
⚠️ 常见陷阱¶
⚠️ 编程陷阱：MuJoCo timestep 与 ros2_control 不匹配
错误做法：timestep=0.002 但控制器期望 1kHz
现象：物理步长 2ms 但控制器 1ms 更新 -> 不稳定
正确做法：timestep=0.001 匹配 1kHz
⚠️ 编程陷阱：力传感器坐标系错误
错误做法：直接使用 sensordata 不转换坐标系
现象：力方向与预期相反或偏转
根本原因：MuJoCo force/torque site sensor 是两个 3D 传感器，
读数在 site 局部坐标系中，符号还取决于你定义的是
“环境作用在机器人”还是“机器人作用在环境”
正确做法：分别读取 ee_force/ee_torque，旋转到约定控制帧；
写单元测试：在 +X/+Y/+Z 施加已知力并检查轴向和符号
练习¶
- ⭐ 环境搭建：搭建仿真环境，验证关节读写正常。
- ⭐ 力传感器验证：末端碰触桌面，验证力方向和量级合理。
F10.2b 力/力矩传感器标定流程 ⭐⭐¶
动机——为什么力传感器需要标定¶
回顾 F01 第 3 节：力传感器的原始读数 \(f_{raw}\) 包含偏置、交叉耦合、温漂。不标定直接使用会导致力控精度下降 50-80%。
六维标定矩阵 ⭐⭐¶
工业六维力传感器（如 ATI Mini45、OnRobot HEX）的标定模型：
\[f_{true} = C \cdot (f_{raw} - b)\]
其中 \(C \in \mathbb{R}^{6 \times 6}\) 是标定矩阵（解耦+增益校正），\(b \in \mathbb{R}^6\) 是零偏置。
标定矩阵 \(C\) 的物理含义：
理想传感器: C = I (单位矩阵), b = 0
施加纯 Fz 时: 只有 z 轴有读数
真实传感器: C != I
施加纯 Fz 时: xy 轴也有读数 (交叉耦合)
例如: C 的非对角元素 C[0,2] = 0.03 意味着
1N 的 Fz 会在 Fx 通道产生 0.03N 的串扰
偏置 b 的来源:
1. 电子零漂 (~0.1-0.5N)
2. 传感器自重力 (安装姿态变化时改变)
3. 温度漂移 (~0.01 N/°C, 24h 可累积 0.5-2N)
完整标定流程（6 步）：
Step 1: 零偏置校准
- 机器人保持静止, 末端无接触, 记录 2s 数据
- b = mean(f_raw[0:2000])
- 每次开机或温度变化 > 2°C 时重做
Step 2: 重力补偿标定 (如果传感器在末端)
- 传感器承载末端工具重力 wrench，而不只是 3D 力
- 力项: f_g^S = R_{WS}^T * [0, 0, -m_tool*g]
- 力矩项: tau_g^S = r_com^S x f_g^S
- 需要在多个姿态下测量，分离偏置、质量和工具质心
在姿态 i:
w_raw_i - b = C^{-1} * [f_g^S_i; r_com^S x f_g^S_i]
其中 R_{WS,i} 是传感器帧到世界帧的旋转矩阵
工程建议:
已知工具质量和质心 -> 直接用模型计算重力 wrench
未知工具质心 -> 用多姿态最小二乘同时估计 m_tool 和 r_com^S
不要只补偿前三维力，否则偏心工具会在力矩通道留下姿态相关偏置
Step 3: 交叉耦合标定 (可选, 出厂已标定)
- 施加已知单轴力 (使用标准砝码)
- 测量所有通道的响应
- 构建 6x6 灵敏度矩阵
Step 4: 动态标定 (高级)
- 传感器有带宽限制 (~2kHz 典型)
- 动态接触时需要频率响应校正
- 通常用 FIR 逆滤波器
Step 5: 噪声评估
- 静止记录 10s, 计算各通道 std
- 典型值: 力 0.1-0.5N, 力矩 0.01-0.05 Nm
- 力控精度的下限 = 噪声 std 的 3 倍
Step 6: 在线偏置补偿
- 每次力控开始前, 自由运动 2s 取均值
- 运行中每 30 分钟重新校准一次 (温漂)
// 力传感器标定代码
class FTCalibration {
Eigen::Matrix<double, 6, 6> C_; // 标定矩阵
Eigen::Vector<double, 6> bias_; // 偏置
double tool_mass_ = 0.0; // 工具质量 [kg]
Eigen::Vector3d tool_com_sensor_; // 工具质心在传感器帧的位置 [m]
Eigen::Vector3d gravity_world_ = {0.0, 0.0, -9.81};
void calibrateBias(const std::vector<Eigen::Vector<double, 6>>& samples) {
bias_.setZero();
for (const auto& s : samples) bias_ += s;
bias_ /= samples.size();
}
void setToolGravityModel(double mass,
const Eigen::Vector3d& com_sensor) {
tool_mass_ = mass;
tool_com_sensor_ = com_sensor;
}
Eigen::Vector<double, 6> compensate(
const Eigen::Vector<double, 6>& raw,
const Eigen::Matrix3d& R_world_sensor) const {
Eigen::Vector<double, 6> gravity_sensor;
Eigen::Vector3d f_g_sensor =
R_world_sensor.transpose() * (tool_mass_ * gravity_world_);
gravity_sensor.head<3>() = f_g_sensor;
gravity_sensor.tail<3>() = tool_com_sensor_.cross(f_g_sensor);
return C_ * (raw - bias_) - gravity_sensor;
}
};
F10.2c 阻抗参数调优方法论——逐维度扫描法 ⭐⭐¶
动机——为什么不能随意调参¶
回顾 F04：阻抗参数 \((K_d, D_d)\) 有 12 个分量（6 维位姿 x 2 参数）。直接暴力搜索不可行。
逐维度扫描法（系统化调参）¶
核心原则：一次只调一个参数，其余固定。从最安全的配置开始，逐步放开。
Step 1: 安全基线 (所有维度高阻尼、低刚度)
K_d = [50, 50, 50, 5, 5, 5] -- 很软
D_d = 2 * sqrt(K_d * m_eff) -- 临界阻尼
验证: 末端可以被手轻松推动, 无振荡
Step 2: z 轴 (力控方向) 调优
目标: z 轴维持 10N
扫描 K_z: [10, 20, 50, 100, 200, 500]
每个 K_z 做 5 次接触实验, 记录:
- 力过冲 (overshoot)
- 力稳态误差 (steady-state error)
- 力稳定时间 (settling time)
- 振荡频率 (如果有)
选择 K_z: 力稳态误差 < 1N 且无振荡的最小 K_z
典型结果: K_z = 50-100 N/m (对刚性表面)
Step 3: xy 轴 (位控方向) 调优
目标: xy 跟踪精度 < 2mm
固定 K_z 为 Step 2 结果
扫描 K_xy: [100, 200, 300, 500, 800]
每个 K_xy 做 5 次圆形轨迹跟踪, 记录:
- 位置 RMSE
- 力矩平滑度
选择 K_xy: RMSE < 2mm 的最小 K_xy
典型结果: K_xy = 200-400 N/m
Step 4: 旋转轴调优
目标: 姿态稳定
通常 K_rot = K_trans / 10 (量纲不同)
扫描 K_rot: [5, 10, 20, 50]
典型结果: K_rot = 10-20 Nm/rad
Step 5: 阻尼比调优
固定所有 K_d, 扫描阻尼比 zeta:
zeta = D_d / (2 * sqrt(K_d * m_eff))
扫描: [0.5, 0.7, 1.0, 1.5, 2.0]
zeta < 0.7: 欠阻尼, 有振荡
zeta = 1.0: 临界阻尼, 无振荡但最快收敛
zeta > 1.5: 过阻尼, 响应慢
通常选择 zeta = 0.8-1.2
Step 6: 接触过渡测试
用 Step 2-5 的参数做完整接触过渡测试:
自由 -> 接触 -> 稳态 -> 脱离
如果过渡力冲击 > 目标, 降低 K_d 或降低接近速度
Step 7: 鲁棒性测试
在 Step 6 的参数上做变化:
- 环境刚度 +-50%
- 添加 2N 随机外力
- 增加 1mm 位置偏差
确认系统仍然稳定
调参决策流程图：
开始: K_d = [50]*6, zeta = 1.0
│
├── 力稳态误差 > 1N?
│ ├── 是 -> 增大 K_z (但不超过 200)
│ └── 否 -> OK
│
├── 有振荡?
│ ├── 是 -> 增大 zeta (1.0 -> 1.5)
│ │ 如果仍振荡 -> 减小 K_d
│ └── 否 -> OK
│
├── 位置跟踪 > 2mm?
│ ├── 是 -> 增大 K_xy (但不超过 500)
│ └── 否 -> OK
│
├── 接触力冲击 > 20N?
│ ├── 是 -> 降低接近速度 或 降低 K_d
│ └── 否 -> OK
│
└── 全部 OK -> 参数确定
练习¶
- ⭐ 参数扫描：实现自动化 K_z 扫描脚本，绘制 K_z vs 力稳态误差曲线。
F10.3 笛卡尔阻抗控制器实现 ⭐⭐¶
动机——控制层核心模块¶
回顾 F03-F04：如果误差定义为 \(\tilde{x}=x-x_d\)，阻抗力写作 \(-K_d\tilde{x}-D_d\dot{x}\)；本章代码采用 \(e_x=x_d-x\)，因此等价写成 $\(\tau = J^T(K_d e_x - D_d \dot{x} + F_{ff}) + g(q)\)$
重力补偿版实现骨架¶
下面代码实现的是**重力补偿 + 笛卡尔弹簧阻尼 + 前馈 wrench** 的教学骨架，不是完整操作空间动力学补偿。它没有显式补偿 \(\Lambda,\mu,p\) 或关节科氏项；若要做高动态自由空间跟踪，应回到 F02/F03 的操作空间逆动力学形式。
class CartesianImpedanceController : public controller_interface::ControllerInterface {
Eigen::Matrix<double, 6, 6> K_d_;
Eigen::Matrix<double, 6, 6> D_d_;
Eigen::Vector<double, 6> x_d_;
Eigen::Vector<double, 6> F_ff_;
pinocchio::Model model_;
pinocchio::Data data_;
controller_interface::return_type update(
const rclcpp::Time& time, const rclcpp::Duration& period) override {
// 教学示例为突出控制律，使用了 VectorXd / MatrixXd。
// 若这段代码进入真实 1 kHz update()，应改为固定大小 Eigen 类型
// 或成员变量预分配，并用 EIGEN_RUNTIME_NO_MALLOC 审计，避免堆分配。
// 1. 读取关节状态
// 教学陷阱: 不要依赖 state_interfaces_[i*3] 这种顺序假设。
// ros2_control 的接口顺序由 controller 导出的 state_interface_configuration()
// 和 Resource Manager 匹配结果决定。工程实现应在 on_configure()/on_activate()
// 建立 (joint_name, interface_name) -> index 的映射。
Eigen::VectorXd q(7), dq(7);
for (int i = 0; i < 7; ++i) {
q[i] = get_state_interface_value(joint_names_[i], "position");
dq[i] = get_state_interface_value(joint_names_[i], "velocity");
}
// 2. 正运动学
pinocchio::forwardKinematics(model_, data_, q, dq);
pinocchio::updateFramePlacements(model_, data_);
auto ee_id = model_.getFrameId("panda_hand");
Eigen::Vector3d p_ee = data_.oMf[ee_id].translation();
Eigen::Matrix3d R_ee = data_.oMf[ee_id].rotation();
// 3. 雅可比
Eigen::MatrixXd J(6, 7);
J.setZero();
pinocchio::computeFrameJacobian(model_, data_, q, ee_id,
pinocchio::LOCAL_WORLD_ALIGNED, J);
// 4. 位姿误差（Log map 旋转误差）
Eigen::Vector<double, 6> e_x;
e_x.head<3>() = x_d_.head<3>() - p_ee;
Eigen::Matrix3d R_d = /* from x_d_ rotation */;
Eigen::Matrix3d R_err = R_d * R_ee.transpose();
// 使用 log3（需包含 <pinocchio/spatial/explog.hpp>）避免
// 轴角法在 angle≈0 时轴方向不稳定。
// 这里 e_x = x_d - x，因此取 current -> desired 的左误差。
e_x.tail<3>() = pinocchio::log3(R_err);
// 5. 速度
Eigen::Vector<double, 6> dx = J * dq;
// 6. 阻抗力
Eigen::Vector<double, 6> F_imp = K_d_ * e_x - D_d_ * dx + F_ff_;
// 7. 重力补偿（本骨架只补偿 g(q)，不是完整动力学补偿）
pinocchio::computeGeneralizedGravity(model_, data_, q);
// 8. 关节力矩
Eigen::VectorXd tau = J.transpose() * F_imp + data_.g;
// 9. 力矩饱和（按 Panda 各关节额定力矩限幅）
const Eigen::Matrix<double, 7, 1> tau_max =
(Eigen::Matrix<double, 7, 1>() << 87, 87, 87, 87, 12, 12, 12).finished();
tau = tau.cwiseMax(-tau_max).cwiseMin(tau_max);
// 10. 写入。和 state interface 一样，command interface 也按名称映射。
for (int i = 0; i < 7; ++i)
get_command_interface(joint_names_[i], "effort").set_value(tau[i]);
return controller_interface::return_type::OK;
}
};
参数配置¶
cartesian_impedance_controller:
ros__parameters:
stiffness:
linear: [150.0, 150.0, 150.0]
angular: [10.0, 10.0, 10.0]
damping_ratio: 1.0 # D = 2 * ratio * sqrt(K * m_eff)
⚠️ 常见陷阱¶
⚠️ 编程陷阱：旋转误差用欧拉角差值
错误做法：e_rot = euler_d - euler_current
现象：万向节锁附近误差跳变
正确做法：Log map: e_rot = angle * axis from R_d * R^T
练习¶
- ⭐ 控制器实现：实现上述控制器，验证末端可被"虚拟力"推动。
- ⭐ 阻尼比实验：阻尼比 0.5 到 2.0，记录阶跃响应。
- ⭐⭐ 动态调参：实现 ROS2 参数回调，运行时修改 K_d。
F10.4 有限状态机与任务集成 ⭐⭐¶
动机——为什么需要状态机¶
没有状态机:
if (distance < 0.01) switch_to_force_control();
问题: 接触瞬间力冲击导致 distance 抖动 -> 反复切换 -> 不稳定
有状态机:
state = APPROACH
if (state == APPROACH && distance < 0.01 && force > 1N) {
state = CONTACT
start_timer(0.5s)
}
if (state == CONTACT && timer_expired && force_stable) {
state = TASK
}
FSM 设计¶
状态:
IDLE -> 等待命令
APPROACH -> 向目标移动（位置控制）
PRE_CONTACT -> 接近表面，降低速度
CONTACT -> 建立稳定接触
TASK -> 执行力控任务
RETRACT -> 缩回安全位置
ERROR -> 异常处理
转换条件:
IDLE -> APPROACH: 收到开始命令
APPROACH -> PRE_CONTACT: 距表面 < 5mm
PRE_CONTACT -> CONTACT: 力 > 1N 且持续 0.2s
CONTACT -> TASK: 力稳定在目标 +-2N 持续 0.5s
TASK -> RETRACT: 任务完成
任何 -> ERROR: 碰撞 || 力 > 50N || 关节限位
ERROR -> IDLE: 安全停止完成
三个任务模式¶
任务 A：零力引导
K_d -> [5, 5, 5, 1, 1, 1] 极低刚度
D_d -> [10, 10, 10, 2, 2, 2] 适当阻尼
x_d = x_current 跟随当前位置
F_ff = 0 无前馈
效果: 末端"漂浮"，操作员可轻松推动
验证: 施加 1N 外力末端移动 > 1mm
任务 B：恒力打磨
z 轴: 导纳控制 — 力误差 -> 位置修正
约定: 世界 z 向上，打磨下压方向为 -z。
FT 原始 z 读数 F_sensor_z 表示"环境作用在机器人上的力"，下压时 F_sensor_z > 0。
若驱动给的是"机器人作用在环境上的力"，先取 f_contact = -F_robot_on_env_z。
f_contact = F_sensor_z # 正数表示接触压力大小
f_error = f_target - f_contact
delta_z -= admittance * f_error * dt # 力偏小时向 -z 下压，力偏大时向 +z 回退
xy 轴: 位控 — 跟踪圆形轨迹
x_d(t) = x_center + R*cos(2*pi*t/T)
y_d(t) = y_center + R*sin(2*pi*t/T)
参数: f_target=10N, admittance=0.001 m/s/N, R=0.03m, T=4s
理论到工程衔接：z 轴用导纳控制（F05）而非阻抗控制——因为力控方向需要精确的力反馈。阻抗控制直接输出力矩但力精度依赖模型，导纳控制修正位置参考但力精度依赖力传感器。两者在本质上等价（F01 二端口分析），但工程实现各有优劣。
任务 C：Peg-in-hole 四阶段
阶段 1 — 接近: K = [500, 500, 500, 50, 50, 50], 目标孔上方 10mm
阶段 2 — 搜索: K = [100, 100, 50, 10, 10, 10], 螺旋搜索半径 0-2mm
z 轴推力 F_ff_z = -3N
终止: z < z_hole_top
阶段 3 — 对准: K = [30, 30, 10, 5, 5, 5], 利用倒角被动对准
终止: z < z_hole_top - 2mm
阶段 4 — 插入: F_ff_z = -10N, K_xy = [20, 20]
终止: z < z_target || 力 > 30N
反事实推理：如果不用四阶段而是固定参数阻抗插入？ - 高刚度 -> 找到孔口前大侧向力 -> 损坏 - 低刚度 -> 找到孔口后推力不足 -> 插不进去 - 必须分阶段调参——这正是 FSM 的价值
⚠️ 常见陷阱¶
⚠️ 编程陷阱：状态切换时参数突变
错误做法：从 SEARCH(K=100) 直接跳到 INSERT(K=20)
现象：力矩跳变 -> 力冲击
正确做法：参数平滑过渡 K(t) = K_old + (K_new-K_old) * smooth(t)
过渡时间 0.1-0.5s
💡 概念误区：FSM 状态越多越好
新手想法："更细粒度 = 更精确"
实际上：状态越多 -> 转换条件越多 -> 调试越难 -> bug 越多
4-6 个状态通常够了。需要更复杂行为考虑行为树（BT）
行为树（BT）+ 力控条件切换——FSM 的进阶替代 ⭐⭐⭐¶
FSM 适合简单的线性流程，但当状态增多、条件复杂时（如多步装配+异常恢复），FSM 的转换矩阵爆炸。行为树（Behavior Tree, BT）是更好的选择。
为什么 BT 优于 FSM（在复杂场景下）：
| 对比项 | FSM | BT |
|---|---|---|
| 新增状态 | 需修改所有相关转换 | 只需添加新子树 |
| 异常恢复 | 每个状态都要写 ERROR 转换 | Fallback 节点统一处理 |
| 并行任务 | 需额外实现 | Parallel 节点原生支持 |
| 可读性 | 状态多时难以理解 | 树结构直观 |
| 调试 | 打印当前状态 | 可视化整棵树的运行状态 |
PiH 任务的 BT 设计：
Root (Sequence)
├── CheckPrerequisites (Sequence)
│ ├── [Condition] FT_Sensor_OK?
│ ├── [Condition] Safety_System_OK?
│ └── [Action] Calibrate_FT_Bias
│
├── GraspPeg (Sequence with Memory)
│ ├── [Action] MoveTo(peg_approach_pose)
│ ├── [Action] SetImpedance(K=[500,500,500,50,50,50])
│ ├── [Action] MoveDown(until: contact_force > 1N)
│ ├── [Action] CloseGripper
│ └── [Condition] GripForce > 5N
│
├── TransportToHole (Fallback) <-- 自动异常恢复
│ ├── NormalTransport (Sequence)
│ │ ├── [Action] LiftUp(10mm)
│ │ ├── [Action] MoveTo(hole_approach_pose)
│ │ └── [Condition] Distance(ee, hole) < 10mm
│ └── RecoverTransport (Sequence) <-- 如果失败,自动重试
│ ├── [Action] RetractToSafe
│ └── [Action] MoveTo(hole_approach_pose, slow=True)
│
├── InsertPeg (Sequence with Memory)
│ ├── FineTune (Sequence)
│ │ ├── [Action] SetImpedance(K=[200,200,100,20,20,20])
│ │ └── [Action] SpiralSearch(r_max=2mm, F_z=5N)
│ │
│ ├── AlignAndInsert (Fallback)
│ │ ├── NormalInsert (Sequence)
│ │ │ ├── [Condition] Z_Dropped > 0.5mm (找到倒角)
│ │ │ ├── [Action] SetImpedance(K=[20,20,10,5,5,5])
│ │ │ ├── [Action] PushDown(F_target=10N)
│ │ │ └── [Condition] Z_Depth >= target_depth
│ │ │
│ │ └── JammingRecovery (Sequence) <-- 卡死自动恢复
│ │ ├── [Condition] LateralForce > 15N for 0.5s
│ │ ├── [Action] RetractZ(2mm)
│ │ ├── [Action] RotateEE(5_deg)
│ │ └── [Action] Retry(InsertPeg)
│ │
│ └── Verify (Sequence)
│ ├── [Action] ReleaseGripper
│ ├── [Action] PullUp(F=5N, max_dist=1mm)
│ └── [Condition] PegStillInserted?
│
└── RetractToHome (Action)
BT 中的力控条件节点实现：
// 力条件节点: 检测接触力是否满足条件
class ForceCondition : public BT::ConditionNode {
BT::NodeStatus tick() override {
auto force = getInput<Eigen::Vector3d>("current_force").value();
auto threshold = getInput<double>("threshold").value();
auto axis = getInput<int>("axis").value(); // 0=x, 1=y, 2=z
auto comparison = getInput<std::string>("comparison").value();
double f = force[axis];
if (comparison == "greater") return f > threshold ? SUCCESS : FAILURE;
if (comparison == "less") return f < threshold ? SUCCESS : FAILURE;
if (comparison == "abs_greater") return std::abs(f) > threshold ? SUCCESS : FAILURE;
return FAILURE;
}
};
// 阻抗切换动作节点
class SetImpedanceAction : public BT::SyncActionNode {
BT::NodeStatus tick() override {
auto K = getInput<std::vector<double>>("stiffness").value();
auto transition_time = getInput<double>("transition_time").value_or(0.3);
// 平滑过渡 (不直接跳变!)
controller_->smoothTransition(K, transition_time);
return BT::NodeStatus::SUCCESS;
}
};
// 螺旋搜索动作节点
class SpiralSearchAction : public BT::StatefulActionNode {
BT::NodeStatus onRunning() override {
double t = (now() - start_time_).seconds();
double progress = t / search_duration_;
// 螺旋轨迹
double r = r_max_ * progress;
double theta = 2.0 * M_PI * n_turns_ * progress;
Eigen::Vector3d offset;
offset << r * cos(theta), r * sin(theta), 0.0;
controller_->setPositionOffset(offset);
controller_->setForceFF(Eigen::Vector3d(0, 0, -f_z_));
// 检测是否找到孔 (z 方向下降)
if (getCurrentZ() < start_z_ - 0.5e-3) {
return BT::NodeStatus::SUCCESS; // 找到了!
}
if (progress >= 1.0) {
return BT::NodeStatus::FAILURE; // 搜索超时
}
return BT::NodeStatus::RUNNING;
}
};
不是 X 而是 Y：BT 的 Fallback 节点**不是**简单的 try-catch，**而是**定义了"正常路径和备选路径的优先级关系"。当 NormalInsert 失败时，JammingRecovery 自动执行——这种自动异常恢复在 FSM 中需要为每个状态手动编写 ERROR 转换，在 BT 中只需要添加 Fallback 子节点。
完整 launch/config/代码架构 ⭐⭐¶
以下是 Mini-ForceControl 项目的完整工程目录结构。
mini_force_control/
├── CMakeLists.txt
├── package.xml
│
├── config/
│ ├── robot/
│ │ ├── franka_panda.yaml # URDF 路径, 关节限位
│ │ └── joint_limits.yaml # 力矩限, 速度限
│ │
│ ├── controller/
│ │ ├── impedance_default.yaml # 默认阻抗参数
│ │ ├── impedance_polishing.yaml # 打磨专用参数
│ │ ├── impedance_pih.yaml # PiH 各阶段参数
│ │ └── safety.yaml # 碰撞阈值, 关节余量
│ │
│ ├── task/
│ │ ├── zero_force_guide.yaml # 零力引导参数
│ │ ├── polishing.yaml # 打磨轨迹参数
│ │ ├── peg_in_hole.yaml # PiH 七阶段参数
│ │ └── bt_tree.xml # 行为树定义
│ │
│ └── evaluation/
│ ├── metrics.yaml # 指标定义和阈值
│ └── failure_modes.yaml # 失败模式分类
│
├── launch/
│ ├── sim.launch.py # MuJoCo 仿真启动
│ ├── controllers.launch.py # 控制器启动
│ ├── task.launch.py # 任务启动 (参数化)
│ └── evaluate.launch.py # 自动评测启动
│
├── src/
│ ├── controllers/
│ │ ├── cartesian_impedance_controller.cpp
│ │ ├── cartesian_impedance_controller.hpp
│ │ └── safety_monitor.cpp
│ │
│ ├── tasks/
│ │ ├── task_manager.cpp # FSM/BT 管理器
│ │ ├── zero_force_guide.cpp
│ │ ├── polishing_task.cpp
│ │ ├── peg_in_hole_task.cpp
│ │ └── bt_nodes/ # BT 自定义节点
│ │ ├── force_condition.cpp
│ │ ├── set_impedance_action.cpp
│ │ └── spiral_search_action.cpp
│ │
│ ├── calibration/
│ │ ├── ft_calibration.cpp # 力传感器标定
│ │ └── gravity_compensation.cpp # 重力补偿
│ │
│ └── evaluation/
│ ├── metrics_recorder.cpp # 指标记录
│ └── failure_analyzer.cpp # 失败分析
│
├── scripts/
│ ├── auto_evaluate.py # 自动化评测 (N 次试验)
│ ├── plot_results.py # 结果可视化
│ └── param_sweep.py # 参数扫描
│
└── test/
├── test_impedance_controller.cpp # 单元测试
├── test_safety_monitor.cpp
└── test_bt_nodes.cpp
launch 文件示例：
# launch/task.launch.py
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch_ros.actions import Node
from launch.substitutions import LaunchConfiguration
def generate_launch_description():
return LaunchDescription([
DeclareLaunchArgument('task', default_value='peg_in_hole',
description='Task: zero_force/polishing/peg_in_hole'),
DeclareLaunchArgument('use_bt', default_value='true',
description='Use BT instead of FSM'),
DeclareLaunchArgument('n_trials', default_value='20',
description='Number of evaluation trials'),
# 控制器节点
Node(
package='mini_force_control',
executable='cartesian_impedance_controller',
name='impedance_controller',
parameters=[
{'config_file': ['config/controller/impedance_',
LaunchConfiguration('task'), '.yaml']},
'config/controller/safety.yaml',
],
output='screen',
),
# 任务管理器节点
Node(
package='mini_force_control',
executable='task_manager',
name='task_manager',
parameters=[
{'task': LaunchConfiguration('task')},
{'use_bt': LaunchConfiguration('use_bt')},
{'bt_tree': 'config/task/bt_tree.xml'},
{'config_file': ['config/task/',
LaunchConfiguration('task'), '.yaml']},
],
),
# 评测节点
Node(
package='mini_force_control',
executable='auto_evaluate',
name='evaluator',
parameters=[
{'n_trials': LaunchConfiguration('n_trials')},
'config/evaluation/metrics.yaml',
],
),
])
练习¶
- ⭐ 零力引导：实现并验证施加 1N 外力时末端柔顺移动。
- ⭐ 恒力打磨：记录力跟踪误差和位置跟踪误差时间曲线。
- ⭐⭐ PiH 装配：实现四阶段（或 BT 七阶段），20 次试验记录成功率。分析失败原因。
- ⭐⭐ BT 实现：用 BehaviorTree.CPP 库实现上述 PiH 行为树。对比 BT 与 FSM 的代码行数和异常恢复能力。
F10.5 安全监控——动量观测器与碰撞检测 ⭐⭐¶
动机——力控系统的安全底线¶
回顾 F06：动量观测器通过比较预期和实际力矩估计外力，超阈值触发碰撞检测。
实现¶
class SafetyMonitor {
Eigen::VectorXd p_hat_; // 估计动量 [kg*m^2/s]（关节空间）
Eigen::VectorXd r_; // 残差 = 估计外力矩 [Nm]（关节空间）
Eigen::VectorXd tau_safe_; // 安全制动力矩 [Nm]
bool observer_initialized_ = false;
double K_obs_ = 50.0; // 观测器增益 [1/s]
double joint_residual_thresh_Nm_ = 20.0; // 关节空间残差阈值 [Nm]，不是末端力 [N]
double joint_margin_ = 0.05; // rad
public:
explicit SafetyMonitor(int n_joints)
: p_hat_(Eigen::VectorXd::Zero(n_joints)),
r_(Eigen::VectorXd::Zero(n_joints)),
tau_safe_(Eigen::VectorXd::Zero(n_joints)) {}
void update(const Eigen::VectorXd& q, const Eigen::VectorXd& dq,
const Eigen::VectorXd& tau_applied) {
if (p_hat_.size() != dq.size()) {
p_hat_ = Eigen::VectorXd::Zero(dq.size());
r_ = Eigen::VectorXd::Zero(dq.size());
tau_safe_ = Eigen::VectorXd::Zero(dq.size());
observer_initialized_ = false;
}
pinocchio::computeAllTerms(model_, data_, q, dq);
pinocchio::computeCoriolisMatrix(model_, data_, q, dq);
// 动量观测器递推
Eigen::VectorXd p = data_.M * dq;
if (!observer_initialized_) {
// 首帧必须用当前动量初始化，避免 p_hat_=0 造成一次性大残差。
p_hat_ = p;
r_.setZero();
observer_initialized_ = true;
} else {
r_ = K_obs_ * (p - p_hat_);
// F06 推导: p_dot = tau + tau_ext + C^T(q,dq)dq - g(q)
// 观测器积分项必须使用实际施加到关节广义坐标上的力矩：
// MuJoCo 中用 qfrc_actuator[dof_adr]，真机中用饱和/限幅后的 torque。
// 不要使用未饱和的 tau_cmd，也不要使用 actuator 空间 actuator_force。
p_hat_ += (tau_applied + data_.C.transpose() * dq - data_.g + r_) * dt_;
}
// 碰撞检测（r_ 的单位是 Nm，即关节空间外力矩）
// joint_residual_thresh_Nm_ 需按机器人、负载和摩擦模型标定。
if (r_.norm() > joint_residual_thresh_Nm_) {
triggerCollisionStop(dq);
}
// 关节限位
for (Eigen::Index i = 0; i < q.size(); ++i) {
if (q[i] < q_min_[i] + joint_margin_ ||
q[i] > q_max_[i] - joint_margin_) {
triggerJointLimitStop(i);
}
}
}
void triggerCollisionStop(const Eigen::VectorXd& dq) {
// 高阻尼刹车
for (Eigen::Index i = 0; i < dq.size(); ++i)
tau_safe_[i] = -50.0 * dq[i];
fsm_->transitionTo(ERROR);
}
};
⚠️ 常见陷阱¶
⚠️ 编程陷阱：碰撞阈值太低导致误触发
错误做法：把末端力阈值(N)和动量观测器残差阈值(Nm)混成一个参数
现象：正常打磨(10N)或模型误差都会频繁触发碰撞停止
正确做法：分别配置 task_space_force_thresh_N 和 joint_residual_thresh_Nm
打磨 10N -> 末端力阈值可取 20N；关节残差阈值需按机器人和负载标定
💡 概念误区：安全层可以事后添加
新手想法："先把功能做完，最后加安全"
实际上：安全层影响 FSM 设计（ERROR 状态）和控制器接口（安全力矩覆盖）。
应从第一天就设计好安全接口，即使实现延后。
练习¶
- ⭐ 碰撞检测：实现动量观测器，验证 25N 外力触发安全停止。
- ⭐ 误触发测试：正常打磨(10N)不触发碰撞检测。
F10.6 评价指标与调试方法论 ⭐⭐¶
评价指标体系¶
| 指标 | 计算方法 | 期望值 | 意义 |
|---|---|---|---|
| 成功率 | 成功 / 总试验 | > 90% | 可靠性 |
| 周期时间 | 开始到完成 | < 30s (PiH) | 效率 |
| 力峰值 | max(|f|) | < 30N | 安全性 |
| 力稳态误差 | mean(|f-f_target|) 稳态 | < 1N | 力控精度 |
| 位置跟踪 | RMSE(x-x_ref) | < 2mm | 位控精度 |
| 力矩平滑度 | std(diff(tau)) | 越小越好 | 控制品质 |
调试决策树¶
力跟踪误差大?
├── 稳态误差大?
│ ├── 检查重力补偿
│ ├── 检查力传感器标定
│ └── 检查导纳增益
├── 振荡?
│ ├── 阻尼比太低 -> 增大 D_d
│ ├── 控制频率太低
│ └── 力传感器噪声 -> 加低通滤波
└── 接触过渡力冲击?
├── 接近速度太快 -> 降低刚度
└── 参数切换太突然 -> 加平滑过渡
完整评价指标体系 ⭐⭐¶
一级指标（必须评测）：
| 指标 | 符号 | 计算方法 | 期望值 | 测量方式 |
|---|---|---|---|---|
| 成功率 | \(SR\) | \(N_{success} / N_{total}\) | > 90% | 自动判断插入深度 |
| 周期时间 | \(T_{cycle}\) | \(t_{end} - t_{start}\) | < 15s (PiH) | 时间戳差 |
| 力峰值 | \(F_{peak}\) | \(\max_t \|f(t)\|\) | < 30N | 1kHz 力传感器 |
| 力稳态误差 | \(e_{f,ss}\) | \(\text{mean}(\|f - f_{target}\|)\), \(t > t_{settle}\) | < 1N | 稳态段均值 |
| 位置 RMSE | \(e_{pos}\) | \(\sqrt{\text{mean}(\|x - x_{ref}\|^2)}\) | < 2mm | 正运动学 |
二级指标（深入分析时使用）：
| 指标 | 符号 | 计算方法 | 意义 |
|---|---|---|---|
| 力矩平滑度 | \(\sigma_{\Delta\tau}\) | \(\text{std}(\Delta\tau / \Delta t)\) | 控制品质，越小越好 |
| 接触过渡时间 | \(T_{trans}\) | 从首次接触到力稳态的时间 | 过渡效率 |
| 力过冲率 | \(OS_f\) | \((F_{peak} - F_{target}) / F_{target}\) | 过渡安全性 |
| 能耗 | \(E\) | \(\int_0^T \tau^T \dot{q} \, dt\) | 能效 |
| 安全余量 | \(M_s\) | \(\min_t (F_{limit} - F(t))\) | 安全裕度 |
| 重复精度 | \(\sigma_{pos}\) | 多次试验末端终点标准差 | 一致性 |
| 恢复次数 | \(N_{recover}\) | 异常恢复（BT Fallback）触发次数 | 鲁棒性 |
三级指标（对比不同方法时使用）：
| 指标 | 计算方法 | 用途 |
|---|---|---|
| CPU 占用率 | top / perf |
实时性评估 |
| 控制周期抖动 | std(实际周期 - 期望周期) | 实时性质量 |
| 参数调优时间 | 工程师调参耗时 | 方法可用性 |
| 代码行数 | wc -l | 方法复杂度 |
"""
自动评测脚本 — N 次试验 + 统计分析
"""
import numpy as np
from dataclasses import dataclass
@dataclass
class TrialResult:
success: bool
cycle_time: float
force_peak: float
force_steady_error: float
position_rmse: float
torque_smoothness: float
energy: float
failure_mode: str # 'none', 'timeout', 'jamming', 'overshoot', ...
class Evaluator:
def __init__(self, n_trials=100):
self.n_trials = n_trials
self.results = []
def run_evaluation(self, task_runner):
for i in range(self.n_trials):
result = task_runner.run_single_trial()
self.results.append(result)
print(f"Trial {i+1}/{self.n_trials}: "
f"{'OK' if result.success else f'FAIL({result.failure_mode})'} "
f"T={result.cycle_time:.1f}s F_peak={result.force_peak:.1f}N")
def report(self):
successes = [r for r in self.results if r.success]
failures = [r for r in self.results if not r.success]
print(f"\n{'='*60}")
print(f"评测报告 ({self.n_trials} trials)")
print(f"{'='*60}")
# 一级指标
print(f"\n--- 一级指标 ---")
print(f"成功率: {len(successes)}/{self.n_trials} "
f"= {100*len(successes)/self.n_trials:.1f}%")
if successes:
times = [r.cycle_time for r in successes]
forces = [r.force_peak for r in successes]
errors = [r.force_steady_error for r in successes]
pos_rmses = [r.position_rmse for r in successes]
print(f"周期时间: {np.mean(times):.1f} +- {np.std(times):.1f} s")
print(f"力峰值: {np.mean(forces):.1f} +- {np.std(forces):.1f} N "
f"(max: {np.max(forces):.1f})")
print(f"力稳态误差: {np.mean(errors):.2f} +- {np.std(errors):.2f} N")
print(f"位置 RMSE: {np.mean(pos_rmses):.2f} +- {np.std(pos_rmses):.2f} mm")
# 失败分析
if failures:
print(f"\n--- 失败模式分析 ---")
modes = {}
for r in failures:
modes[r.failure_mode] = modes.get(r.failure_mode, 0) + 1
for mode, count in sorted(modes.items(), key=lambda x: -x[1]):
print(f" {mode}: {count} 次 "
f"({100*count/len(failures):.0f}% of failures)")
完整失败模式分析表 ⭐⭐¶
| 失败模式 | 频率 | 表现 | 根本原因 | 检测方法 | 修复策略 | 预防措施 |
|---|---|---|---|---|---|---|
| 搜索超时 | 高 | 螺旋搜索找不到孔 | 初始位置偏差 > 搜索半径 | \(t > T_{search,max}\) | 增大半径/加视觉 | 改善初始定位精度 |
| 楔死 (Jamming) | 中-高 | 侧向力持续增大，peg 不动 | 横向 \(K\) 过高或倒角磨损 | \(\|f_{xy}\| > 15\)N 持续 0.5s | 撤退+旋转 5 度重试 | 降低 \(K_{xy}\)，确保倒角完好 |
| 楔入 (Wedging) | 中 | peg 倾斜卡在孔口 | 姿态偏差导致单侧接触 | \(\|M_{xy}\| > 1.5\) Nm | 撤退+姿态修正 | 加旋转阻抗约束 |
| 接触力冲击 | 中 | 首次接触 \(> 30\)N | 接近速度过快或 \(K\) 过高 | \(\|f\|\) 突增 > 阈值 | 降低 \(K\)，加减速段 | 力导数限幅 |
| 打磨力振荡 | 中 | 力在目标附近振荡 \(\pm 3\)N | 导纳增益过大或延迟 | FFT 检测周期分量 | 减小增益，加低通滤波 | 正确设置阻尼比 |
| 安全误触发 | 低-中 | 正常操作被急停 | 碰撞阈值过低 | 统计正常操作力分布 | 阈值 = 最大正常力 x 2 | 按任务状态动态调阈值 |
| 夹爪滑移 | 低 | peg 在夹爪中转动/滑动 | 夹持力不足或表面油污 | 抓取力监控 < 阈值 | 增大夹持力 | 清洁零件表面 |
| 传感器飘移 | 低 | 力读数逐渐偏移 | 温漂 | 每 30 分钟自检偏置 | 在线重标定 | 恒温环境 |
| 通信超时 | 低 | 控制周期 > 2ms | 系统负载高 | 周期抖动监控 | 优化代码/降低负载 | 实时内核 + CPU 隔离 |
| 关节限位 | 低 | 末端到达奇异或关节极限 | 轨迹规划未考虑关节限 | 关节余量 < 5% | 缩回 + 重规划 | IK 加关节限约束 |
失败模式的分级处理：
Level 0 (信息): 记录但不停止
例: 力波动略大于预期
动作: 记录日志，继续执行
Level 1 (警告): 降级运行
例: 传感器噪声增大
动作: 降低速度，增大安全余量
Level 2 (干预): 暂停等待
例: 搜索超时
动作: 停止当前阶段，BT Fallback 自动重试
最大重试: 3 次
Level 3 (停止): 安全停止
例: 碰撞检测触发
动作: 高阻尼刹车，通知操作员
恢复: 人工确认安全后才能重启
从仿真到实机的完整 Checklist ⭐⭐¶
Phase A: 仿真验证（必须在实机前全部通过）
A1. 控制器基础验证
□ 重力补偿: 末端静止漂移 < 1mm/min
□ 阻抗响应: 阶跃输入 -> 无超过 10% 过冲
□ 力矩饱和: tau_max 限制生效
□ 频率一致: 实际控制周期 = 期望值 +-5%
A2. 任务功能验证
□ 零力引导: 施加 1N 外力时末端柔顺移动
□ 恒力打磨: 力稳态误差 < 1N, 位置 RMSE < 2mm
□ PiH: 20 次成功率 > 90%
A3. 安全验证
□ 碰撞检测: 25N 冲击 -> 1ms 内检测
□ 关节限位: 接近限位 -> 自动减速
□ 力矩饱和: 异常力矩 -> 不超过安全值
□ 急停: 触发 -> 100ms 内停止
Phase B: 仿真到实机差异清单
| 维度 | 仿真 | 实机 | 需要额外做的 |
|---|---|---|---|
| 力传感器 | 完美 | 有偏置/噪声/温漂 | F10.2b 标定流程 |
| 动力学模型 | 精确 | 有摩擦/间隙 | SysID 辨识 M, C, g |
| 通信 | 零延迟 | 0.5-2ms | 延迟补偿/预测 |
| 实时性 | 保证 | 需 PREEMPT_RT | 内核配置 + CPU 隔离 |
| 重置 | 1ms | 人工 30s+ | 自动重置夹具 |
| 安全 | 仿真不会坏 | 机器人会损坏 | 物理急停 + 限力 |
| 状态 | 完美 | 编码器量化/噪声 | 滤波 + 状态估计 |
Phase C: 实机调试顺序
C1. 无控制测试 (手动模式)
- 用 Franka Desk 手动移动, 验证硬件正常
- 检查力传感器读数方向和量级
C2. 重力补偿测试
- 只开重力补偿, 验证末端悬浮
- 调整 URDF 惯性参数直到漂移 < 2mm/min
C3. 低刚度阻抗测试
- K = [50]*6, 手推末端验证柔顺
- 确认无振荡
C4. 无接触轨迹测试
- 跟踪圆形轨迹 (不接触)
- 验证位置跟踪精度
C5. 接触测试 (泡沫/海绵)
- 先在柔软表面测试接触
- 验证力跟踪和安全系统
C6. 接触测试 (真实工件)
- 在真实金属表面测试
- 逐步增加力目标
C7. 完整任务测试
- 运行完整 PiH / 打磨
- 先手动监控 10 次, 再自动 100 次
反事实推理：如果跳过 Phase C 直接跑完整任务？ - C2 未做 -> 重力补偿不准 -> 阻抗力偏差 -> 力控精度差 - C3 未做 -> 高刚度直接接触 -> 力冲击 -> 传感器/工件损坏 - C5 未做 -> 在硬表面直接测试 -> 第一次接触力可能超限 - 结论：调试顺序是从最安全到最危险，每步验证上一步的结论
统计显著性与置信区间 ⭐⭐¶
100 次试验的成功率 85% 与 95% 的差异是否显著？需要统计分析。
二项比例置信区间（Wilson 区间，推荐）：
\[\hat{p} \pm \frac{z_{\alpha/2}^2}{2n} \pm z_{\alpha/2} \sqrt{\frac{\hat{p}(1-\hat{p})}{n} + \frac{z_{\alpha/2}^2}{4n^2}} \bigg/ \left(1 + \frac{z_{\alpha/2}^2}{n}\right)\]
# 95% 置信区间计算
from scipy import stats
def wilson_ci(successes, trials, confidence=0.95):
"""Wilson 置信区间 (比 Wald 更准确, 尤其小样本)"""
n, p_hat = trials, successes / trials
z = stats.norm.ppf(1 - (1 - confidence) / 2)
denom = 1 + z**2 / n
center = (p_hat + z**2 / (2*n)) / denom
margin = z * np.sqrt(p_hat * (1 - p_hat) / n + z**2 / (4*n**2)) / denom
return center - margin, center + margin
# 示例:
# 20 次试验, 17 次成功:
# wilson_ci(17, 20) = (0.641, 0.955) -> 95% CI 很宽!
# 100 次试验, 85 次成功:
# wilson_ci(85, 100) = (0.767, 0.910) -> CI 更窄
# 100 次试验, 95 次成功:
# wilson_ci(95, 100) = (0.886, 0.982)
# 结论: 20 次试验不足以区分 85% 和 95% 的真实成功率
# 至少需要 100 次试验才能获得有意义的对比
本质洞察：20 次试验的成功率只是一个"估计"，其 95% 置信区间宽达 +-15%。声称"成功率 90%"但只做了 20 次试验是**统计不严谨**的。工业验证标准（如 IEC 62443）通常要求 100+ 次试验。
⚠️ 常见陷阱¶
🧠 思维陷阱：只看成功率不看失败原因
新手想法："成功率 85%，需要提高"
实际上：15% 失败可能有 5 种不同原因。盲目调参可能修一种引入另一种。
正确做法：
1. 记录失败时完整状态
2. 分类失败模式
3. 针对最频繁模式修复
4. 回归测试验证无新问题
练习¶
- ⭐ 评测：PiH 20 次，记录成功率/周期时间/力峰值。
- ⭐⭐ 失败分析：对每次失败做根因分析，分类模式，提出修复方案。
- ⭐⭐⭐ 跨章综合题：将阻抗控制器替换为 SERL RL 策略（F09）。对比手工 FSM 与 RL 的成功率和周期时间。RL 能否发现人类没想到的插入策略？
F10.7 进阶路线 ⭐⭐⭐¶
路线 alpha（基础，1.5 周）¶
Day 1-2: MuJoCo + ros2_control
Day 3-4: 笛卡尔阻抗控制器
Day 5-6: FSM + 零力引导 + 打磨
Day 7-8: Peg-in-hole 四阶段
Day 9-10: 安全 + 评测
路线 beta（进阶，2 周）¶
Week 1: 路线 alpha
Week 2:
Day 1-2: 集成 CRISP 控制器
Day 3-4: PPO 训练 peg-in-hole（F09 VICES）
Day 5: 对比手工 vs RL
路线 gamma（研究级，3+ 周）¶
本章小结¶
| 知识点 | 核心内容 | 难度 | 关联章节 |
|---|---|---|---|
| F10.1 项目设计 | 系统架构，三层设计 | ⭐ | 全部 |
| F10.2 环境搭建 | MuJoCo + ros2_control | ⭐ | M12 |
| F10.3 阻抗控制器 | 笛卡尔阻抗 C++ 实现 | ⭐⭐ | F03, F04 |
| F10.4 FSM + 任务 | 状态机，三个力控任务 | ⭐⭐ | F01-F05 |
| F10.5 安全监控 | 动量观测器，碰撞检测 | ⭐⭐ | F06 |
| F10.6 评测调试 | 指标定义，失败分析 | ⭐⭐ | — |
| F10.7 进阶路线 | 仿真到实机路线图 | ⭐⭐⭐ | F09 |
累积项目：最终形态¶
Mini-ForceControl 完整项目:
F01: 力控概念框架
F02: 操作空间数学工具
F03: 阻抗/混合力控算法
F04: libfranka 笛卡尔阻抗
F05: ros2_control 导纳控制
F06: 变阻抗+无源性+碰撞安全
F07: WBC-QP 框架
F08: MPC+WBC 联合力控
F09: 学习型力控
F10: 综合实战系统 <-- 最终集成
- MuJoCo 仿真环境
- 笛卡尔阻抗控制器 (1kHz)
- FSM + 三个力控任务
- 安全监控
- 评测框架
延伸阅读¶
| 资源 | 类型 | 难度 | 内容 |
|---|---|---|---|
| libfranka 文档 | 文档 | ⭐⭐ | Franka 真机接口 |
| mujoco_ros2_control | 代码 | ⭐⭐ | MuJoCo + ros2_control |
| CRISP (Stanford) | 代码 | ⭐⭐⭐ | 学习型力控标准架构 |
| serl_franka_controllers | 代码 | ⭐⭐ | SERL 底层阻抗 |
| De Luca 2006 "Collision Detection" | 论文 | ⭐⭐ | 动量观测器 |
| Whitney 1982 "Quasi-Static Assembly" | 论文 | ⭐⭐⭐ | PiH 经典理论 |
故障排查手册¶
| 症状 | 可能原因 | 排查步骤 | 相关章节 |
|---|---|---|---|
| 末端漂移 | 重力补偿不准 | 1. 打印 g(q) 2. 对比不同构型 3. 检查 URDF 惯性 | F10.3 |
| 力跟踪振荡 | 阻尼不足 | 1. 增大 D_d 2. 加低通滤波 3. 减小导纳增益 | F10.4 |
| PiH 搜索超时 | 搜索范围不够 | 1. 增大半径 2. 检查孔位置 3. 加视觉 | F10.4 |
| PiH 卡死 | 横向 K 太高 | 1. 降低 K_xy 2. 加旋转搜索 3. 检查间隙 | F10.4 |
| 接触力冲击 | 速度太快 | 1. 降低 APPROACH K 2. 加减速 3. 分段过渡 | F10.4 |
| 碰撞误触发 | 阈值太低 | 1. 提高阈值 2. 加任务状态判断 3. 滤波 | F10.5 |
| 仿真力不稳定 | timestep 太大 | 1. 减小到 0.0005s 2. 增加求解器迭代 | F10.2 |