# 智身关节模组 CANFD 通信协议

> 整理自《智身关节模组使用手册 V1.2（CANFD 版）》，智身新创（苏州）智能科技有限公司，2026-04-22。
> 本文覆盖手册第 3 章协议主体，并补充第 1 章阻抗控制模型与第 2 章中影响协议使用的约束。
> 手册 V1.2 的变更：修改 P65、P85MAX-S 转矩阈值；新增 W110-S、P100MAX-S 关节通信阈值。

## 1. 适用范围

- 硬件：支持 CAN FD 的关节模组控制器与主控设备。
- 场景：电机控制、状态监测、参数配置。
- 控制模式：**阻抗控制模式**。电机使能后默认进入该模式，内部最终执行力矩闭环：

```
力矩指令 = Kp × (位置指令 − 反馈位置)
         + Kd × (速度指令 − 反馈速度)
         + 前馈力矩指令
```

该模型无积分项。Kp 越大位置刚度越高；Kd 越大阻尼越强、动态响应越慢。位置、速度、转矩、阻尼四种“模式”只是同一模型在不同参数配置下的表现：

| 控制方式 | 参数配置 | 说明 |
| --- | --- | --- |
| 位置控制 | 给定 `P_cmd`，`Kp > 0`，建议 `Kd > 0` | 围绕目标位置的弹性约束，允许位置偏差；轮毂电机不支持 |
| 速度控制 | 给定 `V_cmd`，`Kd > 0`，`Kp = 0` | Kd 相当于比例系数，存在稳态速度误差 |
| 转矩控制 | 给定 `T_ff`，其余为 0 | 直接输出恒定转矩；负载小于给定转矩时会持续加速 |
| 阻尼控制 | 仅 `Kd > 0`，其余为 0 | 输出与速度反向、大小成比例的阻力，不约束位置 |

## 2. CANFD 通信参数

| 类别 | 参数项 | 参数说明 |
| --- | --- | --- |
| 参数配置 | 波特率（仲裁段） | 1 Mbps |
| 参数配置 | 波特率（数据段） | 5 Mbps |
| 参数配置 | 帧格式 | 标准帧 |
| 参数配置 | 数据长度（主站发送） | 64 字节（可配置） |
| 参数配置 | 数据长度（关节返回） | 12 字节 |
| CANID | 主站 CANID | `0x00` |
| CANID | 从站关节 CANID | `0x0X`（X = 1~6） |
| CANID | 关节出厂默认 CANID | `0x03` |
| 通信速率 | 主机与关节通信速率 | 最大支持 1 kHz |

多字节字段全部为 **小端（低字节在前）**。下文示例中的 `00 80` 即 uint16 的 `0x8000`。

## 3. 帧格式

### 3.1 主机下发帧

一帧最多控制 6 个关节，每个关节占 10 字节，按 CANID 顺序排列：

| Byte | 功能 | 说明 |
| --- | --- | --- |
| Byte0–Byte1 | MSGID | 主机发送，从机原样返回；用于匹配请求帧，不用可填 0 |
| Byte2–Byte11 | CANID=1 指令数据 | 见第 4 节 |
| Byte12–Byte21 | CANID=2 指令数据 | 同上 |
| Byte22–Byte31 | CANID=3 指令数据 | 同上 |
| Byte32–Byte41 | CANID=4 指令数据 | 同上 |
| Byte42–Byte51 | CANID=5 指令数据 | 同上 |
| Byte52–Byte61 | CANID=6 指令数据 | 同上 |
| Byte62–Byte63 | 保留 | 手册未定义 |

每个关节块的起始字节为 `2 + 10 × (CANID − 1)`。

**下发长度**可按最大 CANID 裁剪：

| 最大关节 CANID | 下发最小字节数 |
| ---: | ---: |
| 1 | 12 |
| 2 | 24 |
| 3 | 32 |
| 4 | 48 |
| 5 / 6 | 64 |

- 最大 CANID=3 时下发 Byte0–Byte31（32 字节）；最大 CANID=1 时下发 Byte0–Byte11（12 字节）。
- 只控制不连续关节时，仍需按最大 CANID 的长度下发：例如只控 CANID=3，则发 64 字节，仅给 Byte22–Byte31 赋值，其余填 0 或保持上次值；控 CANID=1 和 4 时，给 Byte2–Byte11 与 Byte32–Byte41 赋值。
- CANID=4 时手册要求 48 字节，而实际数据只用到 Byte41（42 字节），以手册要求为准。

### 3.2 关节反馈帧

对所有指令，关节均返回固定的 12 字节状态帧：

| Byte | 功能 | 说明 |
| --- | --- | --- |
| Byte0–Byte1 | MSGID | 等于主机发送值 |
| Byte2–Byte3 | 关节输出端当前位置 | uint16，见第 5 节映射 |
| Byte4–Byte5 | 关节输出端当前速度 | uint16，rad/s |
| Byte6–Byte7 | 关节输出端当前扭矩 | uint16，N·m |
| Byte8 | 电机当前温度 | uint8，`0~255` 对应 `−40~215 °C`（即 `T = raw − 40`） |
| Byte9 | 电机当前状态码 | 位定义见第 6 节 |
| Byte10 | 电机当前电压 | uint8，`0~255` 直接对应 `0~255 V` |
| Byte11 | 预留 | — |

## 4. 指令数据（每关节 10 字节）

### 4.1 电机使能

| Byte | 说明 | 取值 |
| --- | --- | --- |
| Byte2–Byte10 | 固定指令码（9 字节） | `0xFF` × 9 |
| Byte11 | 使能指令码 | `0xFC` |

示例（使能所有关节，MSGID = `0x0100`）：

```
Byte0-1   : 00 01
Byte2-11  : FF FF FF FF FF FF FF FF FF FC
Byte12-21 : FF FF FF FF FF FF FF FF FF FC
...
Byte52-61 : FF FF FF FF FF FF FF FF FF FC
Byte62-63 : xx xx
```

### 4.2 电机失能

| Byte | 说明 | 取值 |
| --- | --- | --- |
| Byte2–Byte10 | 固定指令码（9 字节） | `0xFF` × 9 |
| Byte11 | 使能指令码 | `0xFD` |

失能后所有 MOS 管关闭，电机线圈处于断开状态。

### 4.3 电机运行

必须在使能之后发送。运行指令块内容：

| Byte | 说明 | 取值 |
| --- | --- | --- |
| Byte2–Byte3 | 位置指令 | uint16，`0~65535` 映射到 `P_MIN~P_MAX`（rad），`0x8000` = 0 rad |
| Byte4–Byte5 | 速度指令 | uint16，`0~65535` 映射到 `V_MIN~V_MAX`（rad/s），`0x8000` = 0 rad/s |
| Byte6–Byte7 | 刚度 Kp 指令 | uint16，`0~65535` 映射到 `0~KP_MAX`，`0x0000` = 0 |
| Byte8–Byte9 | 阻尼 Kd 指令 | uint16，`0~65535` 映射到 `0~KD_MAX`，`0x0000` = 0 |
| Byte10–Byte11 | 前馈力矩指令 | uint16，`0~65535` 映射到 `T_MIN~T_MAX`（N·m），`0x8000` = 0 N·m |

示例（所有关节 0 位置、0 速度、0 Kp、0 Kd、0 力矩）：

```
Byte0-1   : 00 01
Byte2-11  : 00 80 00 80 00 00 00 00 00 80
Byte12-21 : 00 80 00 80 00 00 00 00 00 80
...
Byte52-61 : 00 80 00 80 00 00 00 00 00 80
```

### 4.4 设置当前位置为机械零位

| Byte | 说明 | 取值 |
| --- | --- | --- |
| Byte2–Byte10 | 固定指令码（9 字节） | `0xFF` × 9 |
| Byte11 | 零位指令码 | `0xEF` |

设置成功且关节角度未变化时，当前反馈位置为 0。电机在任意位置的上电角度上报范围为 `(−π, π)` rad。

> 完成机械零点标定后，关节具备单圈绝对位置记忆：断电及人为转动输出端后重新上电，仍可恢复当前单圈绝对位置。**轮毂电机不支持该功能，也不支持位置控制。**

## 5. 数值编码

所有物理量以 uint16 线性映射。手册写作 `0~65535 → MIN~MAX`。

编码（物理量 → 原始值）：

```
raw = round((x - MIN) / (MAX - MIN) × 65535)
```

解码（原始值 → 物理量）：

```
x = MIN + raw / 65535 × (MAX - MIN)
```

说明：

- 位置、速度、前馈力矩为对称范围，因此 `0x8000`（32768）对应 0。按上式，`raw = 32768` 时 `x ≈ MIN + 0.5000076 × (MAX − MIN)`，与 0 的偏差来自 65536 个码值无法整除；工程上通常直接按 `32768` 作为零点，或用 `(raw − 32768) / 32768 × MAX` 计算对称量。
- Kp、Kd 为非负范围，`0x0000` 对应 0。
- 下发前必须按关节型号的 `P/V/T/KP/KD` 阈值限幅，见第 8 节。

Python 参考实现：

```python
def encode(x, lo, hi):
    r = round((x - lo) / (hi - lo) * 65535)
    return max(0, min(65535, r))

def decode(raw, lo, hi):
    return lo + raw / 65535 * (hi - lo)

def pack_u16(v):
    return bytes((v & 0xFF, (v >> 8) & 0xFF))  # 小端

def run_block(p, v, kp, kd, t, lim):
    return b"".join((
        pack_u16(encode(p, lim["p_min"], lim["p_max"])),
        pack_u16(encode(v, lim["v_min"], lim["v_max"])),
        pack_u16(encode(kp, 0.0, lim["kp_max"])),
        pack_u16(encode(kd, 0.0, lim["kd_max"])),
        pack_u16(encode(t, lim["t_min"], lim["t_max"])),
    ))
```

## 6. 状态码与故障保护

Byte9 状态码按位定义（1 有效）：

| bit | 功能 | 触发条件 | 恢复措施 |
| ---: | --- | --- | --- |
| 0 | 电机使能 | 使能为 1，失能为 0 | — |
| 1 | 过压故障 | 母线电压超过阈值 | 重启或重新使能 |
| 2 | 过流故障 | 电流超过阈值 | 重启或重新使能 |
| 3 | 过温故障 | 电机温度超过阈值 | 重启或重新使能；状态位低于 85 °C 自动恢复 |
| 4 | 过速故障 | 输出端速度超过阈值 | 重启或重新使能 |
| 5 | 编码器故障 | 编码器测量失效 | 重启或重新使能 |
| 6 | 欠压故障 / 预驱故障 | 母线电压低于阈值，或检测到预驱故障 | 重启或重新使能 |
| 7 | 预留 | — | — |

发生故障时电机会自动失能，同时对应状态位置 1。主站应持续检查 Byte9 的 bit1–bit6，非零即进入故障处理。

## 7. 关节型号与阈值

| 关节型号 | P_MAX | P_MIN | V_MAX | V_MIN | T_MAX | T_MIN | KP_MAX | KD_MAX |
| --- | ---: | ---: | ---: | ---: | ---: | ---: | ---: | ---: |
| P55 | 12.5 | −12.5 | 65 | −65 | 30 | −30 | 500 | 5 |
| P65 | 12.5 | −12.5 | 65 | −65 | 50 | −50 | 500 | 5 |
| P65MAX-S | 12.5 | −12.5 | 65 | −65 | 100 | −100 | 500 | 5 |
| P85MAX-S | 12.5 | −12.5 | 65 | −65 | 300 | −300 | 500 | 50 |
| W150-S | 12.5 | −12.5 | 300 | −300 | 30 | −30 | 500 | 5 |
| W190-S | 12.5 | −12.5 | 200 | −200 | 50 | −50 | 500 | 5 |
| W110-S | 12.5 | −12.5 | 300 | −300 | 30 | −30 | 500 | 5 |
| P100MAX-S | 12.5 | −12.5 | 65 | −65 | 650 | −650 | 500 | 50 |

单位：位置 rad，速度 rad/s，转矩 N·m；Kp、Kd 无量纲。轮毂电机（W 系列）速度阈值明显高于摆臂电机。

### 7.1 额定参数与额定速度（厂家参数页补充）

| 型号 | 额定功率 (W) | 额定扭矩 (N·m) | 峰值扭矩 (N·m) | 推算额定速度 (rad/s) |
| --- | ---: | ---: | ---: | ---: |
| P55 | 78.5 | 6.5 | 21 | 12.1 |
| P65 | 330 | 11.5 | 48 | 28.7 |
| P65MAX-S | 330 | 21.3 | 75 | 15.5 |
| P85MAX-S | 706 | 43 | 180 | 16.4 |
| W150-S | 495 | 3.5 | 11 | 141.4 |
| W190-S | 706 | 8.7 | 28.68 | 81.2 |

额定速度 = 额定功率 / 额定扭矩。它和上表的 `V_MAX` 不是一回事：`V_MAX` 是**通信编码阈值**（决定 uint16 的满量程），额定速度是持续工作能力。本机取的三层关系：

| 关节 | 额定速度 | 人为限幅（`[joint_limits].vel_limit`） | 通信阈值（`[motor.*].v_max`） |
| --- | ---: | ---: | ---: |
| 摆臂 P65MAX-S | 15.5 | 22.4 | 65 |
| 轮 W190-S | 81.2 | 100 | 200 |

人为限幅略高于额定、远低于阈值；实际下发按 `min(阈值, 限幅)` 饱和。注意阈值/额定的比值并不统一（P65MAX-S ≈ 4.2×，W190-S ≈ 2.5×），不能用其中一个推另一个。

## 8. 调试给定值参考

手册给出的典型增益（`P_cmd`, `V_cmd`, `T_ff`, `Kp`, `Kd`）：

| 关节型号 | 位置控制 | 速度控制 | 转矩控制 |
| --- | --- | --- | --- |
| P55 | `3.14, 0, 0, 2, 1` | `0, 5, 0, 0, 2` | `0, 0, 1, 0, 0` |
| P65 | `3.14, 0, 0, 2, 1` | `0, 5, 0, 0, 2` | `0, 0, 1, 0, 0` |
| P65MAX-S | `3.14, 0, 0, 3, 1` | `0, 5, 0, 0, 2` | `0, 0, 1, 0, 0` |
| P85MAX-S | `3.14, 0, 0, 5, 1` | `0, 5, 0, 0, 2` | `0, 0, 3, 0, 0` |
| W150-S | — | `0, 5, 0, 0, 0.1` | `0, 0, 0.3, 0, 0` |
| W190-S | — | `0, 5, 0, 0, 0.5` | `0, 0, 0.2, 0, 0` |

阻尼控制只需 `Kd > 0`，其余参数置 0。

使用注意：

1. 目标位置与当前位置差值不宜过大，否则会产生瞬时大转矩和大电流；建议给定值缓慢增加。
2. `Kp`、`Kd` 的比值不宜过大；速度控制时 `Kd` 不宜过大。
3. 速度项启动转矩由速度给定与 `Kd` 决定，突变会产生电流峰值或振荡。
4. 转矩控制下若负载小于给定转矩，转速会一直上升到最大转速甚至失控。
5. 写入参数后需断电重启才能生效；上位机装入参数后需再次“写入参数”。

## 9. 通信时序要点

1. 上电后先发使能（`0xFC`），再发运行指令；失能（`0xFD`）后线圈断开。
2. 需要绝对零位时，在装配完成后发一次置零（`0xEF`），之后反馈位置以该点为 0。
3. 主机发送周期最大支持 1 kHz；实际控制周期按上层需求设定，并保证每帧携带同一 MSGID 以匹配反馈。
4. 一帧可同时控制最多 6 个关节；不同 CANID 的关节共用一个 64 字节帧，按 `2 + 10 × (CANID − 1)` 定位各自数据块。
5. 每个关节独立返回 12 字节状态帧，主站需按 MSGID 关联请求与反馈。
6. 反馈中的位置、速度、转矩均使用与下发相同的 `MIN~MAX` 映射，解码时必须使用对应关节型号的阈值。
7. ⚠️ 编码满量程只能取第 7 节的型号阈值，不能取摇杆/工作范围。台架脚本曾用 `float_to_uint(v, V_MAX=10, ...)` 下发轮速，而 W190-S 的阈值是 ±200：同一 raw 值固件按 ±200 解码，实际轮速被放大 20 倍。工作上限（如 100）应作为独立限幅先夹紧，再按阈值编码。