6轴非加工型机器手

硬件配置

• 主站: Windows 10 × 1 + Intel N100 × 1
• 运动控制: Leadshine 2CL3-EC507 双轴闭环步进驱动器 × 3 (CiA 402, 含内置 DI)
• 末端执行器: 真空吸盘 × 1 + 真空发生器 × 1
• 传感器: 光电传感器 × 1 + 真空压力开关 × 1

性价比设计

采用雷赛双轴闭环步进驱动器，一个驱动器控制两个关节，3 个驱动器即可驱动 6 轴。 驱动器内置 DI 通过 PDO 实时读取光电和真空传感器，DO 通过 SDO 控制真空（非实时但够用）。 闭环步进防丢步，适合轻负载 (≤2kg) 非加工场景，整线成本约为商用 6 轴机器人的 1/3。

成本分析

类别	型号/规格	参考单价 (¥)	数量	小计 (¥)
双轴闭环步进驱动器	Leadshine 2CL3-EC507	900	3	2,700
闭环步进电机 (J1-J3)	57 闭环步进	120	3	360
闭环步进电机 (J4-J6)	42 闭环步进	60	3	180
光电传感器	对射式 NPN	30	1	30
真空压力开关	SMC ZSE30A	80	1	80
真空吸盘	SMC ZP2	100	1	100
真空发生器	小型真空发生器	80	1	80
工控机	Intel N100 级别	1,500	1	1,500
整线参考总计				≈ ¥5,030

以上为核心部件参考价 (RMB)，不含机械臂本体结构、线缆、气路配件、电磁阀等。

性能指标

• 定位精度: ±0.05mm (闭环步进)
• 最大速度: 180°/s
• 重复定位精度: ±0.02mm
• 闭环防丢步: 堵转自动报警

应用场景

电子装配线码垛工位。闭环步进电机 6 轴机械臂，真空吸盘抓取 PCB 板 / 小型零件码垛。
大关节 (J1-J3) 采用 57 闭环步进提供足够扭矩，小关节 (J4-J6) 采用 42 闭环步进降低成本和惯量。
每个双轴驱动器控制两个关节 (Axis A + Axis B)，减少设备数量和接线复杂度。适合轻负载 (≤2kg) 非加工场景。

代码示例

PDO 结构体定义

using System.Runtime.InteropServices;

/// <summary>
/// Leadshine 2CL3-EC507 双轴驱动器输出 PDO (RxPDO)
/// CSP 模式: 0x1600 (Axis A) + 0x1610 (Axis B), SM2
/// </summary>
[StructLayout(LayoutKind.Sequential, Pack = 1)]
public struct DualAxis_Output
{
    // Axis A (对象索引 0x60xx)
    public ushort ControlWord_A;       // 0x6040 控制字
    public int TargetPosition_A;       // 0x607A 目标位置 (脉冲)
    public ushort TouchProbeFunc_A;    // 0x60B8 探针功能

    // Axis B (对象索引 0x68xx)
    public ushort ControlWord_B;       // 0x6840 控制字
    public int TargetPosition_B;       // 0x687A 目标位置 (脉冲)
    public ushort TouchProbeFunc_B;    // 0x68B8 探针功能
}

/// <summary>
/// Leadshine 2CL3-EC507 双轴驱动器输入 PDO (TxPDO)
/// 0x1A00 (Axis A) + 0x1A10 (Axis B), SM3
/// </summary>
[StructLayout(LayoutKind.Sequential, Pack = 1)]
public struct DualAxis_Input
{
    // Axis A (对象索引 0x60xx)
    public ushort LastErrorCode_A;     // 0x603F 最近错误代码
    public ushort StatusWord_A;        // 0x6041 状态字
    public sbyte ModesOfOpDisplay_A;   // 0x6061 当前运行模式
    public int ActualPosition_A;       // 0x6064 实际位置 (脉冲)
    public ushort TouchProbeStatus_A;  // 0x60B9 探针状态
    public int TouchProbe1PosVal_A;    // 0x60BA 探针1捕获位置
    public uint DigitalInputs_A;       // 0x60FD 数字量输入
    public int AdditionalPos_A;        // 0x5500:05 附加位置

    // Axis B (对象索引 0x68xx)
    public ushort LastErrorCode_B;     // 0x683F 最近错误代码
    public ushort StatusWord_B;        // 0x6841 状态字
    public sbyte ModesOfOpDisplay_B;   // 0x6861 当前运行模式
    public int ActualPosition_B;       // 0x6864 实际位置 (脉冲)
    public ushort TouchProbeStatus_B;  // 0x68B9 探针状态
    public int TouchProbe1PosVal_B;    // 0x68BA 探针1捕获位置
    public uint DigitalInputs_B;       // 0x68FD 数字量输入
    public int AdditionalPos_B;        // 0x5D00:05 附加位置
}

/// <summary>
/// 关节角度
/// </summary>
public class JointAngles
{
    public double[] Angles { get; set; } = new double[6];

    public double J1 { get => Angles[0]; set => Angles[0] = value; }
    public double J2 { get => Angles[1]; set => Angles[1] = value; }
    public double J3 { get => Angles[2]; set => Angles[2] = value; }
    public double J4 { get => Angles[3]; set => Angles[3] = value; }
    public double J5 { get => Angles[4]; set => Angles[4] = value; }
    public double J6 { get => Angles[5]; set => Angles[5] = value; }
}

控制代码

using System;
using System.Threading;
using DarraEtherCAT_Master;

class RobotArmController
{
    // 从站索引: 3个双轴驱动器菊花链直连
    const int SLAVE_DRIVE1 = 0;  // 驱动器1: Axis A = J1 (基座旋转), Axis B = J2 (肩部)
    const int SLAVE_DRIVE2 = 1;  // 驱动器2: Axis A = J3 (肘部),     Axis B = J4 (腕部旋转)
    const int SLAVE_DRIVE3 = 2;  // 驱动器3: Axis A = J5 (腕部摆动), Axis B = J6 (末端旋转)

    // CiA 402 状态机控制字
    const ushort CW_SHUTDOWN         = 0x0006;  // Ready to Switch On
    const ushort CW_SWITCH_ON        = 0x0007;  // Switched On
    const ushort CW_ENABLE_OPERATION = 0x000F;  // Operation Enabled

    // CiA 402 状态字掩码
    const ushort SW_FAULT            = 0x0008;  // bit3: 故障
    const ushort SW_OP_ENABLED_MASK  = 0x006F;  // 检查位: 0,1,2,3,5,6
    const ushort SW_OP_ENABLED_VAL   = 0x0027;  // Operation Enabled 状态值

    // 传感器分配 (利用驱动器内置 DI, 通过 0x60FD/0x68FD PDO 实时读取)
    // 驱动器1 Axis A 0x60FD: 光电传感器 (J1 驱动器靠近取料台)
    // 驱动器3 Axis B 0x68FD: 真空压力开关 (J6 驱动器靠近末端执行器)
    const uint DI_WORKPIECE = 0x00010000;  // 0x60FD bit16: 通用输入 (光电)
    const uint DI_VACUUM_OK = 0x00010000;  // 0x68FD bit16: 通用输入 (真空压力)

    // 真空控制 (0x60FE 不在默认 PDO 中, 通过 SDO 写入)
    const uint DO_VACUUM_ON   = 0x00010000;  // 0x60FE:01 bit16: 真空开
    const uint DO_VACUUM_BLOW = 0x00020000;  // 0x60FE:01 bit17: 吹气释放

    // ============ 可配置参数 (外部设置) ============

    /// <summary>每度脉冲数 (J1-J3: 57电机高分辨率, J4-J6: 42电机)</summary>
    public static int[] PulsesPerDegree { get; set; } = { 1000, 1000, 800, 500, 500, 400 };

    /// <summary>最大速度 (度/秒)</summary>
    public static double[] MaxSpeedDps { get; set; } = { 180, 150, 180, 200, 200, 250 };

    /// <summary>关节下限 (度)</summary>
    public static double[] JointLimitsMin { get; set; } = { -170, -90, -120, -180, -120, -360 };

    /// <summary>关节上限 (度)</summary>
    public static double[] JointLimitsMax { get; set; } = { 170, 90, 120, 180, 120, 360 };

    /// <summary>原点位置</summary>
    public static JointAngles HomePosition { get; set; } = new JointAngles { J1 = 0, J2 = 0, J3 = 0, J4 = 0, J5 = 0, J6 = 0 };

    /// <summary>取料位置</summary>
    public static JointAngles PickPosition { get; set; } = new JointAngles { J1 = 45, J2 = -30, J3 = 60, J4 = 0, J5 = -30, J6 = 0 };

    /// <summary>放料位置</summary>
    public static JointAngles PlacePosition { get; set; } = new JointAngles { J1 = -45, J2 = -30, J3 = 60, J4 = 0, J5 = -30, J6 = 0 };

    /// <summary>抬起偏移量 (J2 偏移, 度)</summary>
    public static double LiftOffset { get; set; } = 20;

    /// <summary>真空确认超时 (毫秒)</summary>
    public static int VacuumTimeoutMs { get; set; } = 500;

    /// <summary>吸取失败最大重试次数</summary>
    public static int VacuumRetryMax { get; set; } = 3;

    // 当前关节角度
    static JointAngles currentAngles = new JointAngles();
    static bool isHomed = false;

    static void Main(string[] args)
    {
        // 1. 使用 ENI 文件导入配置 (ENI 由 2CL3-EC507 ESI 生成)
        var master = new DarraEtherCAT()
            .LoadENI("config/robot_arm_6axis.xml")
            .Build();

        if (master == null)
        {
            Console.WriteLine("主站初始化失败!");
            return;
        }

        try
        {
            // 2. 切换到 OP 状态 (ESI InitCmd 自动设置 CSP 模式: 0x6060=8, 0x6860=8)
            var (success, msg) = master.SetState(EcState.OP);
            if (!success)
            {
                Console.WriteLine($"无法进入 OP 状态: {msg}");
                return;
            }

            // 3. CiA 402 状态机: 使能所有轴
            InitializeAxes(master);

            Console.WriteLine("6轴机器手启动!");
            Console.WriteLine("按 Ctrl+C 退出...\n");

            // 4. 执行回原点
            Console.WriteLine("开始回原点...");
            HomeAllJoints(master);

            // 5. 映射传感器 PDO (引用指向共享内存, 数据由主站循环自动更新)
            ref var drive1In = ref master.Slaves[SLAVE_DRIVE1].InputsMapping<DualAxis_Input>();
            ref var drive3In = ref master.Slaves[SLAVE_DRIVE3].InputsMapping<DualAxis_Input>();

            // 6. 触发式码垛循环
            int cycleCount = 0;

            while (true)
            {
                // ---- 等待光电触发: 驱动器1 Axis A DI 检测工件到达 ----
                Console.WriteLine("等待工件...");
                while ((drive1In.DigitalInputs_A & DI_WORKPIECE) == 0)
                    Thread.Sleep(5);

                Console.WriteLine($"[周期 {++cycleCount}] 检测到工件, 开始取料");

                // ---- 移动到取料位置 ----
                MoveToJointPosition(master, PickPosition, 50);

                // ---- 吸取工件 (带确认和重试) ----
                bool picked = false;
                for (int retry = 0; retry < VacuumRetryMax; retry++)
                {
                    // SDO 写入驱动器3 Axis A DO: 真空开
                    master.Slaves[SLAVE_DRIVE3].WriteSDO<uint>(0x60FE, 1, DO_VACUUM_ON);

                    // 等待真空压力开关确认 (驱动器3 Axis B DI)
                    if (WaitVacuumConfirm(ref drive3In))
                    {
                        picked = true;
                        Console.WriteLine("真空吸取确认");
                        break;
                    }

                    Console.WriteLine($"真空未达标, 重试 ({retry + 1}/{VacuumRetryMax})");
                    master.Slaves[SLAVE_DRIVE3].WriteSDO<uint>(0x60FE, 1, DO_VACUUM_BLOW);
                    Thread.Sleep(100);
                    master.Slaves[SLAVE_DRIVE3].WriteSDO<uint>(0x60FE, 1, 0);
                    Thread.Sleep(200);
                }

                if (!picked)
                {
                    Console.WriteLine("错误: 吸取失败, 跳过本次循环!");
                    master.Slaves[SLAVE_DRIVE3].WriteSDO<uint>(0x60FE, 1, 0);
                    MoveToJointPosition(master, HomePosition, 30);
                    continue;
                }

                // ---- 抬起 ----
                var liftPosition = new JointAngles
                {
                    J1 = PickPosition.J1, J2 = PickPosition.J2 + LiftOffset,
                    J3 = PickPosition.J3, J4 = PickPosition.J4,
                    J5 = PickPosition.J5, J6 = PickPosition.J6
                };
                MoveToJointPosition(master, liftPosition, 30);

                // ---- 搬运途中检查真空 (PDO 实时读取) ----
                if ((drive3In.DigitalInputs_B & DI_VACUUM_OK) == 0)
                {
                    Console.WriteLine("警告: 搬运途中真空丢失!");
                    master.Slaves[SLAVE_DRIVE3].WriteSDO<uint>(0x60FE, 1, 0);
                    MoveToJointPosition(master, HomePosition, 30);
                    continue;
                }

                // ---- 移动到放料位置 ----
                MoveToJointPosition(master, PlacePosition, 80);

                // ---- 释放工件 ----
                master.Slaves[SLAVE_DRIVE3].WriteSDO<uint>(0x60FE, 1, DO_VACUUM_BLOW);
                Thread.Sleep(100);
                master.Slaves[SLAVE_DRIVE3].WriteSDO<uint>(0x60FE, 1, 0);
                Console.WriteLine("工件释放");

                // ---- 回到原点, 等待下一次触发 ----
                MoveToJointPosition(master, HomePosition, 50);
                Console.WriteLine($"[周期 {cycleCount}] 码垛完成\n");
            }
        }
        finally
        {
            master.Dispose();
        }
    }

    /// <summary>
    /// 等待真空压力开关确认 (通过驱动器3 Axis B DI PDO 实时读取)
    /// </summary>
    static bool WaitVacuumConfirm(ref DualAxis_Input drive3In)
    {
        int waited = 0;
        while (waited < VacuumTimeoutMs)
        {
            if ((drive3In.DigitalInputs_B & DI_VACUUM_OK) != 0)
                return true;
            Thread.Sleep(10);
            waited += 10;
        }
        return false;
    }

    /// <summary>
    /// CiA 402 状态机: 使能所有轴 (每个驱动器两轴同时初始化)
    /// ESI InitCmd 已在 PreOp→SafeOp 时设置 CSP 模式 (0x6060=8, 0x6860=8)
    /// </summary>
    static void InitializeAxes(DarraEtherCAT master)
    {
        // CiA 402 状态机转换: Shutdown → Switch On → Enable Operation
        ushort[] sequence = { CW_SHUTDOWN, CW_SWITCH_ON, CW_ENABLE_OPERATION };
        foreach (var cmd in sequence)
        {
            for (int i = 0; i < 3; i++)
            {
                ref var output = ref master.Slaves[i].OutputsMapping<DualAxis_Output>();
                output.ControlWord_A = cmd;
                output.ControlWord_B = cmd;
            }
            Thread.Sleep(50);
        }

        // 确认所有轴进入 Operation Enabled
        string[] axisNames = { "J1", "J2", "J3", "J4", "J5", "J6" };
        for (int i = 0; i < 3; i++)
        {
            ref var input = ref master.Slaves[i].InputsMapping<DualAxis_Input>();

            if ((input.StatusWord_A & SW_OP_ENABLED_MASK) != SW_OP_ENABLED_VAL)
                Console.WriteLine($"警告: {axisNames[i * 2]} 未进入运行状态 (StatusWord=0x{input.StatusWord_A:X4})");
            if ((input.StatusWord_B & SW_OP_ENABLED_MASK) != SW_OP_ENABLED_VAL)
                Console.WriteLine($"警告: {axisNames[i * 2 + 1]} 未进入运行状态 (StatusWord=0x{input.StatusWord_B:X4})");
        }

        Console.WriteLine("6轴全部使能 (CSP 模式)");
    }

    /// <summary>
    /// 回原点 (简化实现: 假设上电位置为原点)
    /// </summary>
    static void HomeAllJoints(DarraEtherCAT master)
    {
        // 简化实现: 假设上电位置为原点, 直接将当前位置设为零位
        // TODO: 生产环境中应使用 RxPDO 4 (0x1603) Homing 模式实现精确回原点
        //       2CL3-EC507 支持 CiA 402 标准回原点方法 (0x6098)

        for (int i = 0; i < 6; i++)
            currentAngles.Angles[i] = 0;

        for (int i = 0; i < 3; i++)
        {
            ref var output = ref master.Slaves[i].OutputsMapping<DualAxis_Output>();
            output.TargetPosition_A = 0;
            output.TargetPosition_B = 0;
        }

        isHomed = true;
        Console.WriteLine("回原点完成! (简化模式, 以上电位置为零位)");
    }

    /// <summary>
    /// 移动到目标关节位置 (6轴同步插补, CSP 模式)
    /// </summary>
    static void MoveToJointPosition(DarraEtherCAT master, JointAngles target, double speedPercent)
    {
        if (!isHomed)
        {
            Console.WriteLine("错误: 未回原点!");
            return;
        }

        // 检查关节限位
        for (int i = 0; i < 6; i++)
        {
            if (target.Angles[i] < JointLimitsMin[i] || target.Angles[i] > JointLimitsMax[i])
            {
                Console.WriteLine($"错误: J{i + 1} 超出限位 ({target.Angles[i]:F1}°)!");
                return;
            }
        }

        // 检查驱动器故障 (CiA 402 StatusWord bit3)
        for (int i = 0; i < 3; i++)
        {
            ref var input = ref master.Slaves[i].InputsMapping<DualAxis_Input>();
            if ((input.StatusWord_A & SW_FAULT) != 0)
            {
                Console.WriteLine($"错误: J{i * 2 + 1} 驱动器故障 (ErrorCode=0x{input.LastErrorCode_A:X4})!");
                return;
            }
            if ((input.StatusWord_B & SW_FAULT) != 0)
            {
                Console.WriteLine($"错误: J{i * 2 + 2} 驱动器故障 (ErrorCode=0x{input.LastErrorCode_B:X4})!");
                return;
            }
        }

        // 计算最长运动时间 (以最慢关节为基准, 保证同步到达)
        double maxTime = 0;
        for (int i = 0; i < 6; i++)
        {
            double delta = Math.Abs(target.Angles[i] - currentAngles.Angles[i]);
            double speed = MaxSpeedDps[i] * speedPercent / 100.0;
            if (speed > 0)
            {
                double time = delta / speed;
                if (time > maxTime) maxTime = time;
            }
        }

        int steps = (int)(maxTime * 1000);  // 1ms 插补周期
        if (steps < 1) steps = 1;

        // 梯形速度曲线参数: 加速20% + 匀速60% + 减速20%
        int accelSteps = steps / 5;
        int decelStart = steps - accelSteps;

        // 一次性映射 3 个驱动器的 PDO
        ref var d1Out = ref master.Slaves[SLAVE_DRIVE1].OutputsMapping<DualAxis_Output>();
        ref var d2Out = ref master.Slaves[SLAVE_DRIVE2].OutputsMapping<DualAxis_Output>();
        ref var d3Out = ref master.Slaves[SLAVE_DRIVE3].OutputsMapping<DualAxis_Output>();

        // 同步插补 (梯形速度曲线, CSP 模式每周期更新目标位置)
        for (int s = 0; s <= steps; s++)
        {
            double ratio;
            if (s < accelSteps && accelSteps > 0)
            {
                double t = (double)s / accelSteps;
                ratio = 0.5 * t * t * ((double)accelSteps / steps);
            }
            else if (s >= decelStart && accelSteps > 0)
            {
                double t = (double)(steps - s) / accelSteps;
                ratio = 1.0 - 0.5 * t * t * ((double)accelSteps / steps);
            }
            else
            {
                ratio = (double)s / steps;
            }

            // 驱动器1: J1 (Axis A) + J2 (Axis B)
            d1Out.TargetPosition_A = (int)((currentAngles.J1 + (target.J1 - currentAngles.J1) * ratio) * PulsesPerDegree[0]);
            d1Out.TargetPosition_B = (int)((currentAngles.J2 + (target.J2 - currentAngles.J2) * ratio) * PulsesPerDegree[1]);

            // 驱动器2: J3 (Axis A) + J4 (Axis B)
            d2Out.TargetPosition_A = (int)((currentAngles.J3 + (target.J3 - currentAngles.J3) * ratio) * PulsesPerDegree[2]);
            d2Out.TargetPosition_B = (int)((currentAngles.J4 + (target.J4 - currentAngles.J4) * ratio) * PulsesPerDegree[3]);

            // 驱动器3: J5 (Axis A) + J6 (Axis B)
            d3Out.TargetPosition_A = (int)((currentAngles.J5 + (target.J5 - currentAngles.J5) * ratio) * PulsesPerDegree[4]);
            d3Out.TargetPosition_B = (int)((currentAngles.J6 + (target.J6 - currentAngles.J6) * ratio) * PulsesPerDegree[5]);

            Thread.Sleep(1);
        }

        // 更新当前位置
        for (int i = 0; i < 6; i++)
            currentAngles.Angles[i] = target.Angles[i];

        Console.WriteLine($"到达位置: J1={target.J1:F1}°, J2={target.J2:F1}°, J3={target.J3:F1}°");
    }
}