huoyi_deploy/docs/zhishen_joint_module_canfd.md

智身关节模组 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 参考实现：

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）应作为独立限幅先夹紧，再按阈值编码。