RC_WheelLeg/01_doc/control/force_control_plan.md

四足机器人从位控转为力控的建议与实施方案

1. 当前系统本质上是什么控制

你现在这套程序不是“纯位控”，而是位置主导 + 力矩辅助。

从代码上看：

• User/app/lingzu_task.c 负责状态机、站立/趴下/行走切换
• User/driver/Movement.c 负责步态、IK、VMC 力矩计算
• User/driver/leg.c 已经有正逆运动学、Jacobian 对应关系
• User/driver/lingzu_motor.c 发送给 RS02 的指令包含：
  • position
  • speed
  • kp
  • kd
  • torque

这说明你当前已经具备了向力控过渡的基础，不是从零开始。

---

2. 你说的“力控”要先明确是哪一种

建议先区分三类目标：

2.1 关节力矩控制

直接控制每个关节输出的力矩 tau。

特点：

• 最底层
• 柔顺性强
• 最难调
• 风险最大

2.2 足端力控制

控制足端对地的作用力，例如支撑力、推进力。

特点：

• 更适合四足
• 需要 Jacobian：

[ \tau = J^T F ]

2.3 力位混合 / 阻抗控制 / VMC

仍然保留目标位置，但不再靠高刚度死跟位置，而是通过虚拟弹簧阻尼计算力，再映射成关节力矩。

特点：

• 最适合你当前项目
• 最稳妥
• 最容易逐步落地

结论

建议优先做“力位混合控制”，而不是一步切到纯力矩控制。

---

3. 你现在已经有的基础

3.1 电机接口已经支持 torque

User/driver/lingzu_motor.c 中，发送控制时已经把 torque 编码进 CAN 指令。

3.2 已有反馈量

你已经能读到：

• 位置 current_position_f
• 速度 current_speed_f
• 力矩 current_torque_f
• 温度 current_temp_f

3.3 已有运动学模型

User/driver/leg.c 已经提供：

• Inverse_Calculation()
• FK_LegPosition()
• FK_LegVelocity()

3.4 已有初步 VMC 框架

User/driver/Movement.c 中已经有：

• vmc_legacy_torque()
• vmc_full_torque()

也就是说，你已经有：

• 电机力矩入口
• 状态反馈
• Jacobian 所需模型
• 初步足端力控制框架

---

4. 真正切换前必须收集的信息

4.1 电机 torque 通道真实性

这是最重要的。

你必须确认 RS02 的 torque 到底是什么：

1. 真正关节输出力矩？
2. 电流等效量？
3. 只是内部位置环的前馈项？

你必须验证的问题

当你设置：

• kp = 0
• kd = 0
• speed = 0
• position = 0
• torque = 非0

电机是否仍能稳定输出可控力矩？

如果答案是否定的，那么就不能做真正纯 torque 控制，只能做力位混合。

---

4.2 torque 反馈是否可信

你还要确认：

• current_torque_f 单位是不是 Nm
• 是估算值还是直接测得
• 噪声有多大
• 延迟有多大
• 正负号是否一致

如果反馈不可靠，闭环力控会很难做。

---

4.3 机械参数是否足够准确

力控对模型更敏感，所以要重新确认：

1. L1、L2
2. 大腿/小腿零点
3. 左右腿符号方向
4. 同步带耦合关系
5. 力矩正方向对应真实腿部运动方向
6. 机械摩擦、死区、间隙

---

4.4 控制频率和总延迟

力控对频率要求更高。

必须明确：

• 控制循环周期是多少
• 电机反馈周期是多少
• CAN 总线延迟是多少
• 从“采样反馈”到“发出新 torque”总共延迟多少

如果实际闭环频率偏低，纯力控会很难稳定。

---

4.5 接触判断能力

四足做支撑腿力控时，需要知道哪条腿真正接地。

你现在没有足底力传感器，所以至少要有估计方法：

• 关节力矩突变
• 足端速度突然降低
• 足端高度达到接地阈值
• IMU 机身姿态/加速度辅助判断

---

5. 我建议你按什么路线做

第一阶段：低刚度位控 + 力矩前馈

这是最稳的第一步。

核心思路

保留：

• 步态规划
• IK 目标角

但降低位置刚度：

• kp 下降
• kd 下降
• torque 增强

目标

把机器人从“硬跟随”变成“柔顺跟随”。

先做什么

1. 静站重力补偿
2. 支撑腿附加支撑力矩
3. 落地缓冲 torque
4. 摆动腿保持较轻位置跟踪

这是最适合你现阶段的方案。

---

第二阶段：关节阻抗控制

公式通常是：

[ \tau = K_p(q_d - q) + K_d(\dot q_d - \dot q) + \tau_{ff} ]

如果 RS02 的 torque 通道足够可靠，可以把更多控制量放到主控里自己算，而不是完全依赖电机内部位置环。

但就你当前项目而言，更现实的做法是：

• 电机仍保留较小 kp/kd
• 主控侧逐步增加 tau_ff

---

第三阶段：足端阻抗 / VMC

这是四足机器人最值得做的一步。

足端空间定义虚拟力：

[ F_x = -K_x(x - x_d) - B_x \dot x ]

[ F_z = -K_z(z - z_d) - B_z \dot z + F_g ]

再用：

[ \tau = J^T F ]

映射成关节力矩。

你的代码现状

这一步你已经有雏形了，只需要系统化：

• 摆动腿：位置/轨迹主导
• 支撑腿：足端阻抗主导

这是我最推荐的中期目标。

---

第四阶段：纯关节力矩控制

只有在下面条件都满足时才建议：

1. torque 通道已验证可靠
2. torque 反馈可信
3. 控制频率足够高
4. 急停保护很完善
5. 标定很准确

否则风险很大，可能出现：

• 塌腿
• 高速震荡
• 落地冲击失控
• 电机发热严重

---

6. 你现在应该收集的数据清单

6.1 单电机实验

每个关节都做。

实验配置

设置：

• kp = 0
• kd = 0
• speed = 0
• position = 0
• 扫描 torque

记录内容

• 最小起转 torque
• 正负方向是否正确
• 是否平稳
• 空载下速度变化
• current_torque_f 与指令是否一致
• 温升情况

---

6.2 单腿实验

一条腿悬空测试。

要看什么

• 同样 torque 下，不同姿态响应是否一致
• 大腿和小腿耦合程度
• Jacobian 算出来的足端力方向是否正确

---

6.3 单腿接地实验

一条腿轻触地面。

记录

• 落地瞬间 torque 反馈变化
• 落地前后足端速度变化
• 同一支撑高度下保持力矩需求
• 推腿时柔顺性

---

6.4 整机实验顺序

必须按顺序推进：

1. 单电机
2. 单腿悬空
3. 单腿接地
4. 整机架空
5. 整机吊挂半承重
6. 全承重静站
7. 小幅步态
8. 连续步行

不要直接全承重纯力控。

---

7. 软件上建议你补的能力

7.1 完整日志

建议每个关节记录：

• 目标位置
• 实际位置
• 目标速度
• 实际速度
• 指令 torque
• 反馈 torque
• 温度
• 故障码

建议每条腿记录：

• 足端目标位置
• 足端实际位置
• 足端速度
• 当前步态相位
• 支撑/摆动状态
• 估算足端力

没有日志，后面很难调。

---

7.2 安全保护

建议补充：

• torque 限幅
• 速度限幅
• 位置工作空间限幅
• 通信超时保护
• 温度保护
• 姿态异常保护
• 落地冲击保护
• 一键 limp 模式

---

7.3 控制模式枚举

建议明确分模式，不要混在一起：

typedef enum {
    CTRL_MODE_POSITION = 0,
    CTRL_MODE_MIXED,
    CTRL_MODE_VMC,
    CTRL_MODE_TORQUE
} control_mode_e;