跳到主要内容

MIT 模式 CANopen 使用教程

适用驱动：一体化通讯模组
适用场景：全量 CANopen 模式下，通过固定 PDO4/MIT 数据通道进行关节侧 MIT/阻抗控制

MIT 模式用于上位机周期下发期望位置、期望速度、位置刚度、速度阻尼和力矩前馈，由执行器在内部快速环中计算关节力矩，再换算为电流环指令。它适合腿足机器人、外骨骼、力控夹具、柔顺跟随和需要高频阻抗调节的场景。

注意

MIT 模式会直接影响输出力矩。首次调试请让关节离开机械硬限位，使用较小 Kp、Kd 和 tau_ff，并确保急停、电源限流和机械保护可用。

1. 控制原理​

MIT 命令由五个关节侧物理量组成：

字段含义单位
q_des期望关节位置rad
dq_des期望关节速度rad/s
Kp位置刚度，决定位置误差产生的力矩Nm/rad
Kd速度阻尼，决定速度误差产生的力矩Nm*s/rad
tau_ff力矩前馈，可用于重力、负载或外部模型补偿Nm

控制器可以理解为在关节侧计算参考力矩：

tau_ref = Kp * (q_des - q_meas) + Kd * (dq_des - dq_meas) + tau_ff

其中 q_meas 和 dq_meas 分别为当前关节位置和关节速度。Kp 项提供位置回复力，Kd 项提供速度阻尼，tau_ff 项直接叠加到参考力矩中。

固件内部会根据电机参数、减速比和运行时配置完成关节力矩到电流指令的换算。用户使用 MIT 模式时只需要按关节侧单位下发 q_des/dq_des/Kp/Kd/tau_ff，不需要在 MIT 命令中配置转矩常数、减速比或磁链等电机参数。

实际输出不会无条件等于上式计算值。固件还会根据当前配置做位置命令限幅、位置误差限幅、速度误差限幅、Kp/Kd 增益限幅、力矩限幅、电流限制、力矩变化率限制和超时保护。

2. 典型用法与调参建议​

MIT 模式可以通过不同的 Kp/Kd/tau_ff 组合实现阻尼、力矩前馈、位置保持和轨迹跟踪。以下用法均应从小幅值、低刚度开始验证。

参数组合典型用途调试提示
Kp = 0, Kd > 0速度阻尼、速度跟随可能存在稳态误差；Kd 过大可能振荡，应从小值逐步增加
Kp = 0, Kd = 0, tau_ff != 0关节侧力矩前馈轻载或空载时可能持续加速；实际输出仍受力矩、电流和变化率限制
Kp > 0, Kd = 0不推荐作为常规配置缺少速度阻尼，位置误差可能引起明显振荡，首次调试不要这样使用
Kp > 0, Kd > 0定点阻抗、位置保持、连续轨迹跟踪定点控制时通常令 dq_des = 0；轨迹跟踪时 dq_des 应与 q_des 的导数一致

推荐调参顺序：

  1. 先设置 Kp = 0、Kd = 0、tau_ff = 0，确认模式切换、使能状态和周期通信正常。
  2. 做纯阻尼测试：保持 Kp = 0，逐步增加 Kd，观察关节速度反馈、电流和温升。
  3. 做低刚度位置保持：设置较小 Kp，例如 5..20 Nm/rad，Kd 保持较小阻尼。
  4. 需要外力或负载补偿时，再逐步加入 tau_ff，并持续监控电流、母线电压、温度和错误码。
  5. 提高刚度前，先确认机械结构没有间隙冲击、限位接近风险和电流饱和。

调参时请记住：协议里的 Kp/Kd/tau_ff 是用户给定值，最终执行还会被设备当前的软件限位、运行参数和保护逻辑裁剪。若位置或力矩达不到期望，应先查看反馈和告警，再调整上位机命令或设备参数。

3. 控制入口概览​

当前固件支持三类 MIT 下发入口。三者内部使用同一套 8 字节 MIT payload，区别在外层 CAN ID、是否带控制位，以及适用的通信配置。

入口CAN IDDLC适用场景数据说明
PDO4/MIT 固定通道0x500 + Dev_ID8全量 CANopen、标准 8B PDO 传输只发送 MIT 五元组 payload；不消费控制字节，需已通过 CANopen 控制面进入 Operation enabled + MIT 模式
MIT 单轴快控0x110 + Dev_ID9自定义快控单轴控制Byte0 为使能/抱闸/清错/模式控制位，Byte1..8 与 PDO4/MIT payload 完全一致
MIT 多轴快控0x21064自定义快控多轴同步下发最多 6 个 9B MIT 子帧，子帧数据机制与单轴快控一致
备注

全量 CANopen 模式下，0x500 + Dev_ID 是固件固定的 PDO4/MIT 数据通道。该通道借用 RPDO4 COB-ID，但不按对象字典 1603h 映射解析；固件初始化时会禁止普通 RPDO4 注册同一 COB-ID，并在接收中断中直通拦截 0x500 + Dev_ID。用户无需、也不能通过对象字典修改该通道映射，只需按本文固定 8B payload 周期下发。

4. 全量 CANopen 使用流程​

PDO4/MIT 通道只承载 MIT 五元组，不承载使能和清错控制。用户主站应先通过标准 CANopen 控制面让设备进入 MIT 运行模式，再周期发送 0x500 + Dev_ID。

以 Dev_ID = 1 为例：

步骤CAN IDDLCData (HEX)说明
10x000201 01NMT 启动节点 1
20x60182F 60 60 00 C0 00 00 00写 6060h = -64，进入厂家自定义 MIT 模式
30x60182B 40 60 00 06 00 00 006040h = 0x0006，Ready to switch on
40x60182B 40 60 00 07 00 00 006040h = 0x0007，Switch on
50x60182B 40 60 00 0F 00 00 006040h = 0x000F，Operation enabled
60x501880 00 80 00 A4 33 38 00周期下发 MIT payload

设备进入 MIT 模式后，上位机应持续发送 0x500 + Dev_ID。推荐发送周期为 1 ms 到 10 ms；周期过长会触发 MIT 命令超时保护。当前默认参数通常为约 50 ms 后进入安全阻尼，继续约 500 ms 后请求 quick stop；具体阈值可随设备参数配置变化。

如设备处于故障状态，可先写 6040h = 0x0080 清错，再按上表重新进入 Operation enabled。

5. PDO4/MIT 8B payload​

PDO4/MIT payload 固定为 8 字节，大端位打包：

字段位宽载荷位置物理量映射范围
q_des16 bitByte0..1期望关节位置-P_MAX .. +P_MAX，单位 rad
dq_des12 bitByte2 + Byte3 高 4 bit期望关节速度-V_MAX .. +V_MAX，单位 rad/s
Kp12 bitByte3 低 4 bit + Byte4位置刚度0 .. 500，单位 Nm/rad
Kd12 bitByte5 + Byte6 高 4 bit速度阻尼0 .. 5，单位 Nm*s/rad
tau_ff12 bitByte6 低 4 bit + Byte7力矩前馈-T_MAX .. +T_MAX，单位 Nm

范围含义：

  • P_MAX：当前生效的位置映射半量程。若启用关节行程限位或软件位置限位，取正负限位绝对值较大者；否则回退 pi rad。
  • V_MAX：设备当前关节侧速度限制。
  • T_MAX：设备当前可用关节力矩限制，会受到力矩参数、电流限制和电机配置共同约束。
  • Kp 和 Kd 的协议满量程分别固定为 500 Nm/rad 和 5 Nm*s/rad。协议满量程不是最终执行上限，固件还会裁剪 Kp/Kd、位置误差、速度误差、力矩、电流和变化率。

位打包关系如下：

Byte0 = q_des[15:8]
Byte1 = q_des[7:0]
Byte2 = dq_des[11:4]
Byte3 = dq_des[3:0] << 4 | Kp[11:8]
Byte4 = Kp[7:0]
Byte5 = Kd[11:4]
Byte6 = Kd[3:0] << 4 | tau_ff[11:8]
Byte7 = tau_ff[7:0]
提示

不要把全 0 的 payload 当作有效 MIT 命令使用。需要停止或失能时，请通过 CANopen 6040h 控制字完成。

6. 可复制的 C 组包代码​

下面代码只依赖标准 C 库。用户需要根据实际设备参数设置 p_max_rad、v_max_rad_s 和 t_max_nm。

#include <math.h>
#include <stdint.h>

static uint16_t float_to_uint(float x, float x_min, float x_max, uint8_t bits)
{
    const float span = x_max - x_min;

    if (!(span > 0.0f) || bits == 0U || bits > 16U) {
        return 0U;
    }

    const uint32_t raw_max = (1UL << bits) - 1UL;

    if (x < x_min) {
        x = x_min;
    } else if (x > x_max) {
        x = x_max;
    }

    uint32_t raw = (uint32_t)lroundf(((x - x_min) * (float)raw_max) / span);
    if (raw > raw_max) {
        raw = raw_max;
    }
    return (uint16_t)raw;
}

void pack_mit_pdo4_payload(float q_des_rad,
                           float dq_des_rad_s,
                           float kp_nm_per_rad,
                           float kd_nm_s_per_rad,
                           float tau_ff_nm,
                           float p_max_rad,
                           float v_max_rad_s,
                           float t_max_nm,
                           uint8_t out[8])
{
    const uint16_t p = float_to_uint(q_des_rad, -p_max_rad, p_max_rad, 16U);
    const uint16_t v = float_to_uint(dq_des_rad_s, -v_max_rad_s, v_max_rad_s, 12U);
    const uint16_t kp = float_to_uint(kp_nm_per_rad, 0.0f, 500.0f, 12U);
    const uint16_t kd = float_to_uint(kd_nm_s_per_rad, 0.0f, 5.0f, 12U);
    const uint16_t t = float_to_uint(tau_ff_nm, -t_max_nm, t_max_nm, 12U);

    out[0] = (uint8_t)(p >> 8);
    out[1] = (uint8_t)(p & 0xFFU);
    out[2] = (uint8_t)(v >> 4);
    out[3] = (uint8_t)(((v & 0x0FU) << 4) | (kp >> 8));
    out[4] = (uint8_t)(kp & 0xFFU);
    out[5] = (uint8_t)(kd >> 4);
    out[6] = (uint8_t)(((kd & 0x0FU) << 4) | (t >> 8));
    out[7] = (uint8_t)(t & 0xFFU);
}

示例参数：

P_MAX = pi rad
V_MAX = 20 rad/s
T_MAX = 10 Nm

q_des = 0 rad
dq_des = 0 rad/s
Kp = 20 Nm/rad
Kd = 1 Nm*s/rad
tau_ff = 0 Nm

组包结果：

80 00 80 00 A4 33 38 00

完整 PDO4/MIT 帧：

Dev_IDCAN IDDLCData (HEX)
10x501880 00 80 00 A4 33 38 00

7. 正弦轨迹跟踪示例​

连续轨迹跟踪时，建议让 dq_des 与 q_des 的导数一致。下面示例展示 1 ms 周期下发正弦位置轨迹的写法，发送函数名请替换为用户主站实际 CAN 接口。

#include <math.h>
#include <stdint.h>

#define MIT_TWO_PI 6.2831853071795864769f

void send_mit_sine_step(uint8_t dev_id, float dt_s)
{
    static float time_s = 0.0f;
    uint8_t payload[8];

    const float amplitude_rad = 0.20f;
    const float freq_hz = 0.25f;
    const float omega_rad_s = MIT_TWO_PI * freq_hz;

    const float kp_nm_per_rad = 10.0f;
    const float kd_nm_s_per_rad = 0.2f;
    const float tau_ff_nm = 0.0f;

    const float p_max_rad = 3.1415926f;
    const float v_max_rad_s = 20.0f;
    const float t_max_nm = 10.0f;

    time_s += dt_s;

    const float phase = omega_rad_s * time_s;
    const float q_des_rad = amplitude_rad * sinf(phase);
    const float dq_des_rad_s = amplitude_rad * omega_rad_s * cosf(phase);

    pack_mit_pdo4_payload(q_des_rad,
                          dq_des_rad_s,
                          kp_nm_per_rad,
                          kd_nm_s_per_rad,
                          tau_ff_nm,
                          p_max_rad,
                          v_max_rad_s,
                          t_max_nm,
                          payload);

    can_send_standard((uint16_t)(0x500U + dev_id), payload, 8U);
}

示例中的幅值、频率和增益只用于说明方法。实际使用时请确保 amplitude_rad 小于当前生效的 P_MAX，amplitude_rad * 2*pi*freq_hz 小于当前生效的 V_MAX，并让 tau_ff 落在当前 T_MAX 范围内。进入 MIT 模式后应以 1 ms 到 10 ms 周期持续发送，停止发送会触发超时阻尼或 quick stop 策略。

8. 快控入口如何套用同一 payload​

快控入口用于自定义快控协议场景。它们与 PDO4/MIT 的差异只是多了外层控制字节和多轴槽位，MIT 五元组的 8B payload 完全相同。

0x110/0x210 的 Byte0 中 mode_proto = 0x06 是旧快控协议内部模式号。固件会把它映射到 CANopen 6060h = -64 的厂家自定义 MIT 模式，它不是 CiA402 标准中的 mode 6。

8.1 0x110 + Dev_ID 单轴快控​

单轴快控的 Byte0 为控制位：

位含义
Bit7使能控制，1 表示上使能
Bit6抱闸控制，1 表示抱闸释放
Bit5清除错误，1 表示触发清错
Bit4..1控制模式，MIT 固定为 0x06
Bit0保留，填 0

MIT 使能并释放抱闸时，Byte0 通常为：

0x80 | 0x40 | (0x06 << 1) = 0xCC

当 Byte0 中 enable = 1 时，Byte1..8 的 MIT payload 必须有效，不能为全 0；否则固件不会发布本帧 MIT 控制命令。

沿用上文示例 payload 时，完整单轴快控帧为：

Dev_IDCAN IDDLCData (HEX)
10x1119CC 80 00 80 00 A4 33 38 00

8.2 0x210 多轴快控​

多轴快控使用 64 字节数据区，最多放 6 个 MIT 子帧。每个子帧 9 字节，格式与单轴快控一致。

字节范围含义
Byte0..8槽位 1 的 9B MIT 子帧
Byte9..17槽位 2 的 9B MIT 子帧
Byte18..26槽位 3 的 9B MIT 子帧
Byte27..35槽位 4 的 9B MIT 子帧
Byte36..44槽位 5 的 9B MIT 子帧
Byte45..53槽位 6 的 9B MIT 子帧
Byte54..55保留，填 00 00
Byte56..61槽位对应的 Dev_ID 列表
Byte62..63保留，填 00 00

示例：槽位 1 控制 Dev_ID = 1，槽位 2 控制 Dev_ID = 2，其余槽位不使用。

0x210 DLC 64:
CC 80 00 80 00 A4 33 38 00
CC 80 00 80 00 A4 33 38 00
00 00 00 00 00 00 00 00 00
00 00 00 00 00 00 00 00 00
00 00 00 00 00 00 00 00 00
00 00 00 00 00 00 00 00 00
00 00
01 02 00 00 00 00
00 00

9. 反馈与确认​

全量 CANopen 接入时，建议用以下方式确认状态：

检查项方法期望结果
模式设置SDO 读取 6061h返回 -64 表示当前显示模式为 MIT
使能状态SDO 读取 6041h 或 TPDO 状态字进入 Operation enabled
位置、电流、错误读取已有 TPDO/SDO 反馈位置、电流和错误码正常更新
快控反馈若使用 0x110/0x210 快控，可查看 0x300 + Dev_ID状态机实际进入 MIT 后，Byte10 模式反馈为 0x06；Byte11 状态位显示使能/抱闸/错误

0x300 + Dev_ID 快控反馈的数据格式可参考关节模组协议中的执行器反馈报文。Byte10 是实际执行模式反馈，只有状态机真正进入 MIT 模式后才会显示 0x06。

10. 常见问题​

发送 PDO4 后没有动作​

请依次检查：

  • CAN ID 是否为 0x500 + Dev_ID，DLC 是否为 8。
  • 是否已通过 6060h = -64 设置 MIT 模式。
  • 是否已通过 6040h 进入 Operation enabled。
  • payload 是否不是全 0。
  • 发送周期是否持续小于超时阈值。
  • 设备是否存在故障、抱闸未释放或软件限位抑制。

位置或力矩达不到期望​

MIT 命令会受设备当前位置限位、速度限位、力矩限位、电流限制和温度保护共同约束。请先降低 Kp、Kd 和 tau_ff，确认反馈无报错，再逐步提高。

能否用 SDO 修改 PDO4 映射​

不支持。MIT 的 0x500 + Dev_ID 是固定数据通道，只借用 RPDO4 COB-ID，不作为用户可重映射的普通 RPDO4 使用。即使对象字典中存在 1603h 映射项，该 MIT 通道也不按 1603h 解析；修改 PDO 映射不会改变 MIT payload 格式，反而可能影响标准 CANopen 通信行为。