25伺服人型机器人

硬件配置

• 主站: Linux (MCU+RTL8309N) × 1 + 单核RT驱动
• EtherCAT 拓扑: 8 * 4芯线 (4路冗余)
• 关节驱动: 一体化关节模组 × 25 (PDO , 支持位置/力矩/阻抗模式)
• 关节分布 (25 DOF):
  • 环线1 (左臂路): 腰偏航×1 + 腰俯仰×1 + 左肩×3 + 左肘×1 + 左腕×1 = 7轴
  • 环线2 (右臂路): 颈偏航×1 + 右肩×3 + 右肘×1 + 右腕×1 = 6轴
  • 环线3 (左腿): 左髋×3 + 左膝×1 + 左踝×2 = 6轴
  • 环线4 (右腿): 右髋×3 + 右膝×1 + 右踝×2 = 6轴
• 传感器:
  • 六维力传感器 × 2 (左脚/右脚, 分别挂载于环线3/环线4末端)
  • IMU × 1 (躯干, 挂载于环线1, 与腰部关节相邻)
• 走线拓扑 (MCU 位于胸腔中央):
  • 环线1: MCU → IMU → 腰偏航 → 腰俯仰 → ↗ 左肩 → 左肘 → 左腕 → 返回
  • 环线2: MCU → 颈偏航 → ↗ 右肩 → 右肘 → 右腕 → 返回
  • 环线3: MCU → ↘ 左髋 → 左膝 → 左踝 → 左脚力传感器 → 返回
  • 环线4: MCU → ↘ 右髋 → 右膝 → 右踝 → 右脚力传感器 → 返回
• 网络隔离: RTL8309N 端口隔离, 4 路环线互不干扰
• 算法: Isaac Lab 离线训练 RL 策略 (ONNX部署) + 复合视觉模型触发

性能指标

• 控制周期: 500 µs (2 kHz)
• 关节总数: 25轴 (腰2 + 左臂5 + 颈1 + 右臂5 + 左腿6 + 右腿6)
• EtherCAT 节点总数: 28 (25关节模组 + 2力传感器 + 1 IMU)
• 环线负载: 8 / 6 / 7 / 7 节点
• 关节位置精度: ±0.01°
• 全身运动规划频率: 500 Hz

应用场景

基于 Linux 实时内核的人形机器人全身运动控制系统。MCU 位于胸腔中央，通过 RTL8309N 九口交换芯片（1口连 MCU、8口连4路冗余环线的收发端）驱动25个一体化关节模组。

腰部和颈部关节不单独占用环线，而是就近串入臂部环线：环线1经过腰部后上行至左臂（7轴），环线2经过颈部后上行至右臂（6轴），环线3/4分别独立下行至左/右腿（各6轴）。

每条环线末端挂载传感器（脚部力传感器或躯干IMU），线缆沿肢体内部走线。关节模组支持位置/力矩/阻抗混合控制，主站以 2kHz 频率下发 PDO 指令。

代码说明

整体架构

系统架构

系统采用异步双频率控制，策略推理线程与 PDO 伺服回调解耦：

策略网络由 Isaac Lab 在仿真环境中训练完成后，导出为 ONNX 模型部署到真实硬件。部署时不依赖 Isaac Lab，仅需 ONNX Runtime。

推理线程独立运行，通过锁将最新目标位置写入共享缓冲区；PDO 回调以 2 kHz 读取最新目标并下发，不阻塞于推理耗时。

观测空间 (96 维) 组成： projected_gravity(3) + base_ang_vel(3) + joint_pos-default(25) + joint_vel(25) + last_actions(25) + foot_contact(6+6) + commands(3)

定制服务

本案例中的 RL 策略训练 (Isaac Lab 仿真环境搭建、奖励函数设计、PPO 训练调参)、 ONNX 模型部署优化等底层时序逻辑为附加定制服务， 不包含在 EtherCAT 主站软件授权中。

如需人形机器人全身控制方案定制，请联系技术团队。

Darra 驱动优化: 双向冗余 , 热插拔检测与恢复

4 路环线均采用双向冗余——主站从环线两端同时发送 LRW 帧。任意一处线缆断裂不丢失从站数据，对应用层透明。

人形机器人关节处线缆受反复弯折，环断线概率高于固定安装场景。断线后自动从 DUAL 切换到 DEGRADED 模式，PDO 数据不中断。

关节模组断电重连 (碰撞保护、维修更换) 时，驱动通过 WKC 监控 + 逐站探测自动检测从站离线，并区分环断线与真正的热插拔。从站重新上线后自动恢复，应用层收到上线回调。

代码示例

PDO 结构体定义

import ctypes

class JointInput(ctypes.LittleEndianStructure):
    """关节模组反馈 PDO (TxPDO)"""
    _pack_ = 1
    _fields_ = [
        ("Mode",         ctypes.c_uint8),   # 当前模式
        ("ErrorCode",    ctypes.c_uint8),   # 错误码
        ("Position",     ctypes.c_int32),   # 实际位置 (0.001°)
        ("Velocity",     ctypes.c_int32),   # 实际速度 (0.01°/s)
        ("Torque",       ctypes.c_int16),   # 实际力矩 (0.01Nm)
        ("Temperature",  ctypes.c_uint8),   # 温度 (°C)
    ]

class JointOutput(ctypes.LittleEndianStructure):
    """关节模组指令 PDO (RxPDO)"""
    _pack_ = 1
    _fields_ = [
        ("Mode",              ctypes.c_uint8),   # 0=空闲 1=位置 2=速度 3=力矩
        ("TargetPosition",    ctypes.c_int32),   # 目标位置 (0.001°)
        ("FeedforwardTorque", ctypes.c_int16),   # 前馈力矩 (0.01Nm)
        ("Kp",                ctypes.c_uint16),  # 位置增益
        ("Kd",                ctypes.c_uint16),  # 阻尼增益
    ]

class ForceInput(ctypes.LittleEndianStructure):
    """六维力传感器反馈 PDO"""
    _pack_ = 1
    _fields_ = [
        ("Fx", ctypes.c_int32), ("Fy", ctypes.c_int32), ("Fz", ctypes.c_int32),
        ("Mx", ctypes.c_int32), ("My", ctypes.c_int32), ("Mz", ctypes.c_int32),
    ]  # 单位: 0.001N / 0.001Nm

class ImuInput(ctypes.LittleEndianStructure):
    """IMU 反馈 PDO"""
    _pack_ = 1
    _fields_ = [
        ("AccX",  ctypes.c_int16), ("AccY",  ctypes.c_int16), ("AccZ",  ctypes.c_int16),
        ("GyroX", ctypes.c_int16), ("GyroY", ctypes.c_int16), ("GyroZ", ctypes.c_int16),
        ("Roll",  ctypes.c_int16), ("Pitch", ctypes.c_int16), ("Yaw",   ctypes.c_int16),
    ]

控制代码

RL策略部署 → EtherCAT PDO 异步桥接 (策略由 Isaac Lab 离线训练)。推理线程异步运行（50 Hz），PDO 回调只读取最新目标并下发（2 kHz），推理耗时不阻塞 PDO 周期。

import math
import time
import threading
import numpy as np
import onnxruntime as ort
from ethercat import DarraEtherCAT

NUM_JOINTS = 25
OBS_DIM = 96
INFERENCE_HZ = 50

# 逻辑关节索引 → 物理从站索引 映射表 (仅关节, 传感器通过独立常量访问)
# EtherCAT 扫描顺序沿环线拓扑, 传感器穿插其中, 关节不一定从0开始连续编号
# 环线1: [0]=IMU, [1]=腰偏航, [2]=腰俯仰, [3~7]=左肩×3+左肘+左腕
# 环线2: [8]=颈偏航, [9~13]=右肩×3+右肘+右腕
# 环线3: [14~19]=左髋×3+左膝+左踝×2, [20]=左脚力传感器
# 环线4: [21~26]=右髋×3+右膝+右踝×2, [27]=右脚力传感器
JOINT_SLAVE_MAP = [
    1, 2,                   # 腰偏航, 腰俯仰         (环线1, IMU后)
    3, 4, 5, 6, 7,          # 左肩×3, 左肘, 左腕     (环线1)
    8,                      # 颈偏航                  (环线2)
    9, 10, 11, 12, 13,      # 右肩×3, 右肘, 右腕     (环线2)
    14, 15, 16, 17, 18, 19, # 左髋×3, 左膝, 左踝×2   (环线3)
    21, 22, 23, 24, 25, 26, # 右髋×3, 右膝, 右踝×2   (环线4)
]
IMU_SLAVE_IDX = 0           # IMU 从站索引 (环线1首个节点)
FORCE_L_SLAVE_IDX = 20      # 左脚力传感器从站索引
FORCE_R_SLAVE_IDX = 27      # 右脚力传感器从站索引

# 关节限位 (°) — 与训练环境 URDF 一致
JOINT_MIN = np.array([
    -90,-30, -180,-90,-90,0,-180,       # 环线1: 腰×2 + 左臂×5
    -90, -180,-90,-90,0,-180,            # 环线2: 颈×1 + 右臂×5
    -90,-45,-45,0,-45,-30,               # 环线3: 左腿×6
    -90,-45,-45,0,-45,-30,               # 环线4: 右腿×6
], dtype=np.float32)

JOINT_MAX = np.array([
    90,30, 180,90,90,135,180,
    90, 180,90,90,135,180,
    90,45,45,135,45,30,
    90,45,45,135,45,30,
], dtype=np.float32)


class RLPolicyBridge:
    """RL策略部署桥接 (Isaac Lab训练 → ONNX部署 → EtherCAT PDO)

    推理线程以 50 Hz 异步运行, PDO 回调以 2 kHz 读取最新目标。
    用法与 C# 类库一致: InputsMapping/OutputsMapping 映射 PDO 结构体。
    """

    def __init__(self, master: DarraEtherCAT,
                 onnx_path: str, norm_path: str):
        self.master = master
        self.policy = ort.InferenceSession(
            onnx_path, providers=['CPUExecutionProvider'])
        self.input_name = self.policy.get_inputs()[0].name

        # 加载训练导出的归一化参数
        norm = np.load(norm_path)
        self.obs_mean = norm['obs_mean'].astype(np.float32)
        self.obs_var  = norm['obs_var'].astype(np.float32)
        self.action_scale_deg = np.degrees(norm.get(
            'action_scale',
            np.full(NUM_JOINTS, 0.5, dtype=np.float32)))
        self.default_pos = norm.get(
            'default_pos',
            np.zeros(NUM_JOINTS, dtype=np.float32))

        self.obs = np.zeros(OBS_DIM, dtype=np.float32)
        self.last_actions = np.zeros(NUM_JOINTS, dtype=np.float32)
        self.commands = np.zeros(3, dtype=np.float32)

        # 预获取所有 PDO 映射 (引用指向共享内存, 数据由主站循环自动更新)
        self._joint_inputs = [
            self.master.Slaves[JOINT_SLAVE_MAP[i]].InputsMapping(JointInput)
            for i in range(NUM_JOINTS)
        ]
        self._joint_outputs = [
            self.master.Slaves[JOINT_SLAVE_MAP[i]].OutputsMapping(JointOutput)
            for i in range(NUM_JOINTS)
        ]
        self._imu_input = self.master.Slaves[IMU_SLAVE_IDX].InputsMapping(ImuInput)
        self._force_inputs = [
            self.master.Slaves[s].InputsMapping(ForceInput)
            for s in (FORCE_L_SLAVE_IDX, FORCE_R_SLAVE_IDX)
        ]

        # 共享目标缓冲区 (推理线程写, PDO回调读)
        self._lock = threading.Lock()
        self._targets = self.default_pos.copy()
        self._running = False

    # ── 异步推理线程 (50 Hz) ──────────────────────────

    def start(self):
        """启动异步推理线程"""
        self._running = True
        self._thread = threading.Thread(
            target=self._inference_loop, daemon=True)
        self._thread.start()

    def _inference_loop(self):
        """独立线程: 50 Hz 采集观测 → 推理 → 更新共享目标"""
        interval = 1.0 / INFERENCE_HZ
        while self._running:
            t0 = time.monotonic()
            self._build_obs()
            self._infer()
            elapsed = time.monotonic() - t0
            if elapsed < interval:
                time.sleep(interval - elapsed)

    def _build_obs(self):
        """构建观测向量 (与 Isaac Lab 训练环境完全一致)"""
        idx = 0

        # ① projected_gravity (3)
        imu = self._imu_input
        roll  = math.radians(imu.Roll * 0.01)
        pitch = math.radians(imu.Pitch * 0.01)
        self.obs[idx]     = -math.sin(pitch)
        self.obs[idx + 1] =  math.sin(roll) * math.cos(pitch)
        self.obs[idx + 2] = -math.cos(roll) * math.cos(pitch)
        idx += 3

        # ② base_ang_vel (3)
        self.obs[idx]     = math.radians(imu.GyroX * 0.01)
        self.obs[idx + 1] = math.radians(imu.GyroY * 0.01)
        self.obs[idx + 2] = math.radians(imu.GyroZ * 0.01)
        idx += 3

        # ③ joint_pos - default (25) + ④ joint_vel (25)
        for i in range(NUM_JOINTS):
            inp = self._joint_inputs[i]
            self.obs[idx + i] = \
                inp.Position * 0.001 - self.default_pos[i]
            self.obs[idx + NUM_JOINTS + i] = \
                inp.Velocity * 0.01
        idx += NUM_JOINTS * 2

        # ⑤ last_actions (25)
        self.obs[idx:idx + NUM_JOINTS] = self.last_actions
        idx += NUM_JOINTS

        # ⑥ foot_contact_force (12)
        for ft in self._force_inputs:
            for val in [ft.Fx, ft.Fy, ft.Fz,
                        ft.Mx, ft.My, ft.Mz]:
                self.obs[idx] = val * 0.001
                idx += 1

        # ⑦ commands (3)
        self.obs[idx:idx + 3] = self.commands

    def _infer(self):
        """ONNX 推理 → 更新共享目标"""
        obs_norm = (self.obs - self.obs_mean) \
            / np.sqrt(self.obs_var + 1e-8)

        try:
            actions = self.policy.run(
                None,
                {self.input_name: obs_norm.reshape(1, OBS_DIM)}
            )[0][0]
        except Exception as e:
            print(f"[警告] ONNX 推理异常: {e}")
            return

        actions = np.clip(actions, -1.0, 1.0).astype(np.float32)
        self.last_actions = actions

        targets = self.default_pos + actions * self.action_scale_deg
        np.clip(targets, JOINT_MIN, JOINT_MAX, out=targets)

        with self._lock:
            self._targets = targets

    # ── 2 kHz PDO 回调 (不阻塞) ──────────────────────

    def step(self):
        """每个 PDO 周期调用, 仅读取共享目标并写入 PDO"""
        with self._lock:
            targets = self._targets

        for i in range(NUM_JOINTS):
            outp = self._joint_outputs[i]
            outp.Mode = 1
            outp.TargetPosition = int(targets[i] * 1000)
            outp.Kp = 600
            outp.Kd = 40

    def close(self):
        self._running = False
        self._thread.join(timeout=1)
        del self.policy