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 darra_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