机械臂恒力跟随方案设计：位置式力闭环选型、架构与安全
让机械臂末端以恒定力度贴着柔软表面运动，是按摩理疗、打磨抛光、擦拭清洁这类应用共同的核心问题：轨迹会跨过高低不平的表面，而期望输出的是稳定的接触力，不是稳定的位置。本文给出一套完整的方案设计：力控路线怎么选、系统怎么架、控制周期怎么排、安全怎么兜底、落地前要协调哪些事。适合正在为软接触应用做技术选型的机器人软件开发者。方案在「腕部六维力传感器 + 按摩头 + 固定假人」的原型场景上设计，结论可平移到其他软表面恒力接触应用。
导纳控制在 ros2_controllers 里的实现细节，见本站《AdmittanceController 源码精读与全流程走查》与《AdmittanceController 恒力控制二次开发踩坑记》；工业控制器与 ros2_control 两种体系下恒力控制实现路线的对照，见《工业控制器到 ros2_control 的分层映射与流式模式》。本文聚焦更上游的一步：在自研控制器平台上，一个恒力跟随应用从零开始的方案设计。
一、问题特征与边界
这类场景的物理特征决定了方案空间：
- 接触环境刚度低：假人表面、软质工件在受力时变形量大，力控环路不容易被激振；
- 目标固定：接触对象位置已知且不动，工作空间可以预先规划；
- 手法连续：揉、推、压类手法是持续接触，不要求毫秒级的力突变响应。
其中最后一条也是方案的能力边界：敲击、捶打类手法需要力矩级的动态响应，位置式力闭环的带宽不够，方案设计阶段就应明确排除，而不是留到联调时发现。
二、力控路线选型
力控的实现路线按「力误差作用到哪里」分两大类：
- 间接力控（位置式力闭环）：不直接控制力，把力误差换算成位置修正量，叠加到位置目标上。导纳控制是它的正规参数化——力误差经过一个虚拟的「质量-阻尼-刚度」模型积分出位置修正，参数有物理含义，比裸 PI 反馈更好整定；
- 直接力控（阻抗控制等）：需要关节力矩级接口，由控制器直接输出力矩。
对本场景，选位置式力闭环（导纳方向），理由有三：
- 环境匹配：低刚度表面上，位置式力闭环天然稳定、参数好调——环境越软，位置修正对力的敏感度越低，环路越温和；
- 复用既有链路：整套方案只需要「100Hz 主循环 + 订阅力/关节反馈 + 下发流式关节目标」三件套，与遥操等已有应用走完全相同的通道，运动控制与伺服侧零改动；
- 排除项明确：阻抗控制需要力矩级接口，而当前总线是位置同步模式（CANopen CSP / EtherCAT CSP），改动力矩输出要动到总线层，代价与收益不匹配。
一句话概括选型逻辑：环境软、平台只有位置接口、手法不要求高带宽，三条同时成立时，位置式力闭环是最小代价的路线。
三、系统架构
新增软件只有一个力控进程，其余全部复用平台既有服务：
数据流：力数据与轨迹合成后，经恒力控制器产生 Z 向修正量，IK 解算成关节角，通过安全检查，以流式关节目标下发给运动控制服务；运动控制侧用 Ruckig 在线规划兜住目标突变，经实时域插补、总线主站到达伺服；六维力传感器由外设管理服务统一加载，驱动以话题形式发布力数据。
一个值得单独说明的设计取舍：关节状态反馈不参与力控解算。六轴非冗余、轨迹预先示教的场景下，IK 用上一拍目标作初值即可保证解连续，不需要实时 FK 对准。关节反馈只进安全层，承担三个角色：
- 中断后恢复时的实际位置依据；
- 目标-实际偏差的执行健康监测；
- 力传感器失效时，行程限幅的冗余验证（详见下文安全设计）。
四、控制周期与时序
力控环每一拍的处理链路：
频率链路：力控 100Hz → 运动控制跟随 10ms → 实时域插补 1ms。力误差修正的带宽（通常几 Hz 量级）远低于各环节采样率，链路不构成瓶颈。周期抖动由 Ruckig 规划器按速度/加速度约束吸收——这是流式目标控制方式的固有优点：每个新目标到达时，规划器即时生成一条满足约束的平滑轨迹段，力控侧不必自己处理轨迹平滑。
五、安全设计
力控应用直接产生物理接触，安全机制要在方案阶段设计，不能事后补。四项基本机制：
| 机制 | 判据与动作 | 说明 |
|---|---|---|
| 力超限急停 | 法向力超目标值约 1.5 倍立即停 | 倍数是工程初值，联调时按手法力度收紧 |
| 行程硬限幅 | 位置修正量 Δz 限制在 ±3cm 量级 | 防传感器失效时持续下压（见下） |
| 断流锁死 | 力数据 200ms 未更新或关节状态断流，锁住当前位姿并报警 | 通信故障时按最坏情况处理 |
| 启动自检 | 传感器零偏校验，异常拒绝进入工作态 | 带病启动比不启动更危险 |
其中行程限幅有一条隐蔽但关键的作用链：传感器失效 → 力读数趋零 → 力误差持续存在 → 修正量持续增大 → 触发行程硬限 → 急停。也就是说，行程限幅同时是力传感器失效的兜底防线——这也是关节反馈「冗余验证」角色的具体含义。管理措施（如演示时人员不进入臂展范围）是安全的组成部分，但这类措施不进代码，两者在设计文档里应分开列明。
另有两条工程细节值得写进设计：
- 重力补偿必须带符号：传感器零漂与末端自重混在读数里，自重在测量轴上的投影随臂姿态变化、方向可正可负。符号搞错，力控会系统性偏一档——某些姿态下自重是抵消读数而非叠加；
- 滤波截止频率的权衡：力信号要先低通滤波再进控制环，截止频率明显高于控制环路带宽（经验取数倍以上）以免相位滞后削弱环路，同时远低于采样频率以压制噪声。
六、落地协调清单
方案再对，落地卡在跨团队接口上。以下清单按「是否阻塞开发」排列，源自实践沉淀：
| # | 事项 | 对接方 | 要对齐的内容 | 阻塞程度 |
|---|---|---|---|---|
| 1 | 力传感器驱动支持 | 传感器/设备团队 | 型号是否已有驱动；没有则驱动开发归属谁 | 阻塞：决定接入是一天还是一周以上 |
| 2 | 力数据接口定义 | 传感器/设备团队 | 消息字段/单位/时戳、话题名、发布频率是否达标 | 阻塞 |
| 3 | 传感器安装工装 | 机械/电气 | 传感器—末端执行器—法兰的安装接口、末端负载校核、腕部线缆走线 | 阻塞（硬件周期最长，最先启动） |
| 4 | 流式指令参数 | 运动控制团队 | 实际负载下速度/加速度/周期等参数推荐值 | 非阻塞（已有遥操先例可参考） |
| 5 | 接触面标定 | 现场/工艺 | 表面材质软硬（影响导纳参数手感）、固定方式、基准面标定方法 | 非阻塞 |
| 6 | TCP 标定 | 工艺 | 末端执行器尖端的工具坐标系标定 | 非阻塞 |
| 7 | 急停通道 | 安全/平台 | 力超限急停走平台急停还是应用层停止指令，两条路径都要验证 | 非阻塞但上线前必须定案 |
| 8 | 轨迹示教方式 | 应用/工艺 | 按摩/作业路径点位怎么来：示教器逐点，或手动拖动带一遍 | 非阻塞 |
前两项与传感器强相关，实践中最容易被低估：传感器到货不等于能用，驱动归属和消息定义不定，力控开发就无法闭环。
七、要点回顾
- 软表面、位置接口、低带宽手法三个条件成立时，位置式力闭环（导纳方向）是代价最小的恒力路线；
- 系统只新增一个力控进程，轨迹、力控、IK、安全在进程内串联，通过流式关节目标与平台既有链路对接；
- 关节反馈不参与解算，专职安全：恢复依据、健康监测、传感器失效的冗余验证；
- 行程硬限幅是力传感器失效的最终兜底，重力补偿符号错误是最隐蔽的系统性错误源；
- 落地风险集中在传感器驱动与接口定义两件事上，方案阶段就要锁定归属。