6台Delta并联机器人协同分拣

硬件配置

• 主站: Window10 IoT × 1 + Intel 13600KF × 1 + RTX4060 × 1
• 耦合器: EK1100 × 1 + EL5001 高速计数器 × 1 + EL1008 数字量输入 × 1
• 传送带: 变频器 × 1 + 增量编码器 × 1
• 测高: 激光测距传感器 × 6 (安装在吸盘上方)
• 识别系统: 线阵相机 × 1 (编码器触发, GigE) + 线光源 × 1 + RTX4060 推理
• Delta并联机器人: 86闭环一体机 × 18 (NEMA34, 12N.m, EtherCAT总线, 36-60V) + 吸盘 × 6

电机内置 I/O 分配

I/O	功能	说明
DI1	正限位	关节正方向限位开关
DI2	负限位	关节负方向限位开关
DI3	原点	回零传感器
DI4	激光测距信号	仅 Joint 0 电机接入 (物品检测)
DI5	预留
DO1	吸盘控制	仅 Joint 0 电机使用 (电磁阀)
DO2	预留

每台机器人 3 个电机，只有 Joint 0 电机的 DO1 接吸盘电磁阀、DI4 接激光传感器。其余电机的 DO/DI4~5 空置。吸盘控制直接走电机内置 DO。

成本分析

类别	型号/规格	参考单价 (¥)	数量	小计 (¥)
耦合器	EK1100	800	1	800
高速计数器	EL5001	1,000	1	1,000
数字量输入	EL1008	300	1	300
变频器	-	800	1	800
编码器	-	100	1	100
激光测距	-	150	6	900
一体式闭环步进	力川 86闭环 12N.m	540	18	9,720
线阵相机	GigE 线阵 + 线光源	3,000	1	3,000
GPU	RTX4060	2,200	1	2,200
工控机	Intel 13600KF 级别	4,000	1	4,000
整线参考总计				≈ ¥23,000

以上为核心部件参考价 (RMB)，不含机器人本体结构、输送结构、线缆等。

性能指标

指标	数值	说明
输送速度	1.5 m/s
分拣精度	±2 mm
分拣周期	≤1.5 秒/件/机位	含抓取 + 放置 + 回位
单线产能	≈14,400 件/小时	6 机位并行
EtherCAT 周期	1 ms	PDO 刷新率
电机保持转矩	12 N.m
电机电压	48 VDC

应用场景

食品/电子元器件分拣

食品、电子元器件高速分拣产线。6台 Delta 并联机器人沿传送带排列，AI 视觉识别后由主站统一调度分配抓取任务，单线产能约 14,400 件/小时。

再生资源杂塑分拣

废塑料回收产线中，混合塑料（杂塑）经破碎后在传送带上高速通过，线阵相机结合 AI 模型实时识别塑料种类，Delta 机器人按材质分拣到对应料仓。

AI 视觉模型

本案例中的 AI 识别模型（目标检测、塑料材质分类等）及线阵相机标定、图像拼接算法为附加服务，不包含在 EtherCAT 主站软件授权中。我们可根据实际分拣场景提供定制化 AI 模型训练与部署方案。

机器人运动学算法

Delta 并联机器人的逆运动学求解、轨迹规划等运动控制算法为附加服务。
我们是专业的工业自动化技术公司，拥有多家行业专用设备制造商合作者。除软件授权外，同样提供整线方案设计、硬件选型、设备采购及技术支持等一站式服务，欢迎咨询合作。

代码说明

系统架构

关键设计：

1. 相机 AI 入队 — 视觉回调识别后将目标坐标 + 编码器位置写入共享队列
2. 机器人自主抢占 — 6 个机器人线程各自独立，空闲时从队列中抢占距离自己最近的目标
3. 线追踪抓取 — 抓取阶段每 PDO 周期实时读编码器, 重新计算目标 X 偏移, 补偿传送带运动
4. 超距自动剔除 — 主循环定期清理已超出最后一台机器人工作范围的目标 (视为掉落)
5. 固定绝对位置 — 每台机器人有固定的传送带方向绝对坐标，只抓取工作半径内的目标
6. 电机 DO 控制吸盘 — Joint 0 电机的 DO1 直接驱动吸盘电磁阀，无需外部 I/O 模块

代码示例

PDO 结构体定义

using System.Runtime.InteropServices;

/// <summary>
/// 输入 PDO (TxPDO 0x1A00)
/// </summary>
[StructLayout(LayoutKind.Sequential, Pack = 1)]
public struct StepperMotor_Input
{
    public ushort ErrorCode;              // 0x603F 错误代码
    public ushort StatusWord;             // 0x6041 状态字
    public int ActualPosition;            // 0x6064 实际位置
    public uint DigitalInputs;            // 0x60FD 数字量输入 (bit0=DI1 ~ bit4=DI5)
    public int ActualVelocity;            // 0x606C 实际速度
    public int FollowError;               // 0x60F4 跟随误差
    public ushort TouchProbeStatus;       // 0x60B9 探针状态
    public int TouchProbePos1Positive;    // 0x60BA 探针1正沿位置
    public int TouchProbePos1Negative;    // 0x60BB 探针1负沿位置
    public int TouchProbePos2Positive;    // 0x60BC 探针2正沿位置
    public int TouchProbePos2Negative;    // 0x60BD 探针2负沿位置
    public sbyte ModesOfOperationDisplay; // 0x6061 当前运行模式
}

/// <summary>
/// 输出 PDO (RxPDO 0x1600)
/// </summary>
[StructLayout(LayoutKind.Sequential, Pack = 1)]
public struct StepperMotor_Output
{
    public ushort ControlWord;            // 0x6040 控制字
    public int TargetPosition;            // 0x607A 目标位置
    public int TargetVelocity;            // 0x60FF 目标速度
    public ushort TouchProbeFunction;     // 0x60B8 探针功能
    public uint DigitalOutputs;           // 0x60FE:01 数字量输出 (bit0=DO1, bit1=DO2)
}

/// <summary>
/// EL5001 高速计数器输入 PDO
/// </summary>
[StructLayout(LayoutKind.Sequential, Pack = 1)]
public struct EL5001_Input
{
    public byte Status;
    public int CounterValue;
    public int LatchValue;
}

/// <summary>
/// 分拣目标数据 — 由视觉系统创建并入队
/// </summary>
public class SortingTarget
{
    public int Id { get; set; }
    public int EncoderAtDetect { get; set; }  // 检测时编码器位置 (脉冲)
    public int PixelY { get; set; }           // 垂直传送带方向偏移 (像素, 相机坐标)
    public int Category { get; set; }         // AI 分类结果
    public int AssignedRobot { get; set; }    // -1=未分配, >=0=已被抢占
}

控制代码

using System;
using System.Threading;
using System.Collections.Generic;
using System.Linq;
using DarraEtherCAT_Master;

/// <summary>
/// Delta分拣 EtherCAT 控制器
/// 职责: EtherCAT 步进电机管理, CiA402 状态机, PP 模式运动, DO 吸盘控制
/// 视觉 AI 入队, 逆运动学, 激光测距等由外部服务实现
/// </summary>
class DeltaSortingController
{
    const int ROBOT_COUNT = 6;
    const int AXES_PER_ROBOT = 3;

    // 从站索引
    const int SLAVE_EL5001 = 1;   // 编码器
    const int SLAVE_MOTOR_BASE = 3;  // 步进电机起始 (3~20)

    // 电机内置 I/O 位定义
    const uint DI_LASER = 0x08;   // DI4: 激光测距 (仅J0)
    const uint DO_SUCTION = 0x01; // DO1: 吸盘电磁阀 (仅J0)

    // ============ 可配置参数 (传送带方向距离均为编码器脉冲值) ============
    // 编码器 3000 脉冲/圈, 同步轮周长 200mm → 15000 脉冲/米

    /// <summary> 相机到第1台机器人的距离 (编码器脉冲) </summary>
    public int CameraToFirstRobot { get; set; } = 22500;    // ≈1.5m

    /// <summary> 机器人间距 (编码器脉冲), 6台等距排列 </summary>
    public int RobotSpacing { get; set; } = 15000;          // 1m

    /// <summary> 机器人工作半径 (编码器脉冲), 目标在此范围内才可抓取 </summary>
    public int WorkRadius { get; set; } = 3000;             // ≈0.2m

    /// <summary> 输送带总长度 (编码器脉冲), 超出此距离的目标自动剔除 </summary>
    public int ConveyorLength { get; set; } = 120000;       // ≈8m

    /// <summary> 获取机器人绝对位置 (编码器脉冲) </summary>
    int GetRobotPosition(int robotId) => CameraToFirstRobot + robotId * RobotSpacing;

    // ============ 共享目标队列 ============

    readonly List<SortingTarget> targetList = new List<SortingTarget>();
    readonly object listLock = new object();

    /// <summary>
    /// 视觉系统调用此方法将识别结果入队
    /// </summary>
    public void EnqueueTarget(SortingTarget target)
    {
        target.AssignedRobot = -1;
        lock (listLock) { targetList.Add(target); }
    }

    // 机器人状态
    bool[] robotBusy = new bool[ROBOT_COUNT];
    DarraEtherCAT master;

    // ============ 主流程 ============

    static int GetMotorSlaveIndex(int robotId, int joint)
        => SLAVE_MOTOR_BASE + robotId * AXES_PER_ROBOT + joint;

    /// <summary>
    /// 读取编码器当前值 (脉冲)
    /// </summary>
    int GetEncoder()
    {
        ref var enc = ref master.Slaves[SLAVE_EL5001].InputsMapping<EL5001_Input>();
        return enc.CounterValue;
    }

    void Run()
    {
        master = new DarraEtherCAT()
            .LoadENI("config/delta_sorting.xml")
            .Build();

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

        try
        {
            var (success, msg) = master.SetState(EcState.OP);
            if (!success) { Console.WriteLine($"无法进入 OP: {msg}"); return; }

            InitializeMotors();

            // 启动 6 个机器人线程, 各自独立从队列抢占目标
            for (int i = 0; i < ROBOT_COUNT; i++)
            {
                int id = i;
                new Thread(() => RobotLoop(id)) { IsBackground = true }.Start();
            }

            Console.WriteLine($"系统启动: {ROBOT_COUNT} 台机器人, 间距 {RobotSpacing} 脉冲, 工作半径 {WorkRadius} 脉冲");

            // 主循环: 定期清理超距目标
            while (true)
            {
                PurgeExpiredTargets();
                Thread.Sleep(50);
            }
        }
        finally { master.Dispose(); }
    }

    /// <summary>
    /// CiA 402 状态机使能所有电机
    /// </summary>
    void InitializeMotors()
    {
        for (int r = 0; r < ROBOT_COUNT; r++)
        {
            for (int j = 0; j < AXES_PER_ROBOT; j++)
            {
                ref var o = ref master.Slaves[GetMotorSlaveIndex(r, j)].OutputsMapping<StepperMotor_Output>();
                o.ControlWord = 0x0006; o.DigitalOutputs = 0;
                Thread.Sleep(2);
                o.ControlWord = 0x0007;
                Thread.Sleep(2);
                o.ControlWord = 0x000F;
            }
        }
        Console.WriteLine($"已使能 {ROBOT_COUNT * AXES_PER_ROBOT} 台电机");
    }

    // ============ 机器人线程 ============

    /// <summary>
    /// 机器人控制循环 — 空闲时从共享队列抢占距离自己最近的目标
    /// </summary>
    void RobotLoop(int robotId)
    {
        while (true)
        {
            // 抢占: 从队列中找到距离自己最近且在工作半径内的目标
            var target = TryClaimNearest(robotId);

            if (target != null)
            {
                robotBusy[robotId] = true;
                try
                {
                    ExecutePickAndPlace(robotId, target);
                }
                finally
                {
                    robotBusy[robotId] = false;
                }
            }
            else
            {
                Thread.Sleep(1);
            }
        }
    }

    /// <summary>
    /// 从共享队列抢占距离最近的未分配目标
    /// 目标行进距离 = 当前编码器 - 检测时编码器, 与机器人位置比较
    /// </summary>
    SortingTarget TryClaimNearest(int robotId)
    {
        int encoder = GetEncoder();
        int robotPos = GetRobotPosition(robotId);

        lock (listLock)
        {
            SortingTarget best = null;
            int bestDist = int.MaxValue;

            foreach (var t in targetList)
            {
                if (t.AssignedRobot != -1) continue;

                // 目标从相机出发已行进的距离
                int travelDistance = encoder - t.EncoderAtDetect;
                int dist = Math.Abs(travelDistance - robotPos);

                if (dist < WorkRadius && dist < bestDist)
                {
                    best = t;
                    bestDist = dist;
                }
            }

            if (best != null)
                best.AssignedRobot = robotId;

            return best;
        }
    }

    /// <summary>
    /// 清理超距目标 — 行进距离超过输送带总长度的目标视为已掉落
    /// </summary>
    void PurgeExpiredTargets()
    {
        int encoder = GetEncoder();

        lock (listLock)
        {
            int removed = targetList.RemoveAll(t =>
            {
                if (t.AssignedRobot >= 0) return false;
                int travelDistance = encoder - t.EncoderAtDetect;
                return travelDistance > ConveyorLength;
            });

            if (removed > 0)
                Console.WriteLine($"剔除 {removed} 个超距目标 (掉落)");
        }
    }

    // ============ 抓取/放置 (附加服务提供逆运动学) ============

    /// <summary>
    /// 执行抓取 → 放置 → 回位
    /// 抓取阶段启用线追踪: 每 PDO 周期重新计算目标位置, 补偿传送带运动
    /// 逆运动学由外部计算脉冲值
    /// </summary>
    void ExecutePickAndPlace(int robotId, SortingTarget target)
    {
        int robotPos = GetRobotPosition(robotId);

        // 1. 线追踪下降: 持续跟踪目标, 边追踪边下降到抓取高度
        TrackAndDescend(robotId, target, robotPos);

        // 2. 开启吸盘
        SetSuction(robotId, true);
        Thread.Sleep(50);

        // 3. 抬起 (线追踪结束, 目标已被吸住)
        var ik = IKService.Compute(0, target.PixelY);
        MoveAxes(robotId, ik.LiftPulses);

        // 4. 移动到料盒放置
        MoveAxes(robotId, ik.PlacePulses);
        SetSuction(robotId, false);
        Thread.Sleep(30);

        // 5. 回待机位
        MoveAxes(robotId, ik.StandbyPulses);

        // 抓取完成, 从队列移除
        lock (listLock) { targetList.Remove(target); }

        Console.WriteLine($"机器人{robotId}: 完成目标{target.Id}, 类别{target.Category}");
    }

    /// <summary>
    /// 线追踪: 每 PDO 周期实时更新目标 X 坐标, 补偿传送带运动, 同时逐步下降
    /// </summary>
    void TrackAndDescend(int robotId, SortingTarget target, int robotPos)
    {
        // 预获取 3 轴 PDO 映射 (引用指向共享内存, 数据由主站循环自动更新)
        ref var o0 = ref master.Slaves[GetMotorSlaveIndex(robotId, 0)].OutputsMapping<StepperMotor_Output>();
        ref var o1 = ref master.Slaves[GetMotorSlaveIndex(robotId, 1)].OutputsMapping<StepperMotor_Output>();
        ref var o2 = ref master.Slaves[GetMotorSlaveIndex(robotId, 2)].OutputsMapping<StepperMotor_Output>();
        ref var i0 = ref master.Slaves[GetMotorSlaveIndex(robotId, 0)].InputsMapping<StepperMotor_Input>();
        ref var i1 = ref master.Slaves[GetMotorSlaveIndex(robotId, 1)].InputsMapping<StepperMotor_Input>();
        ref var i2 = ref master.Slaves[GetMotorSlaveIndex(robotId, 2)].InputsMapping<StepperMotor_Input>();

        var deadline = DateTime.Now.AddMilliseconds(2000);

        while (DateTime.Now < deadline)
        {
            // 每周期重新计算目标相对机器人的 X 偏移
            int encoder = GetEncoder();
            int relX = (encoder - target.EncoderAtDetect) - robotPos;

            // 超出工作半径则放弃
            if (Math.Abs(relX) > WorkRadius) break;

            // 逆运动学: 实时 X 偏移 + Y 偏移 → 3轴关节脉冲 (含下降高度)
            var ik = IKService.ComputeGrab(relX, target.PixelY);

            // 更新 3 轴目标位置 (PP 模式, 每周期刷新)
            o0.TargetPosition = ik.GrabPulses[0]; o0.ControlWord = 0x001F;
            o1.TargetPosition = ik.GrabPulses[1]; o1.ControlWord = 0x001F;
            o2.TargetPosition = ik.GrabPulses[2]; o2.ControlWord = 0x001F;

            // 检查是否到达抓取位置
            if (Math.Abs(i0.ActualPosition - ik.GrabPulses[0]) <= 10 &&
                Math.Abs(i1.ActualPosition - ik.GrabPulses[1]) <= 10 &&
                Math.Abs(i2.ActualPosition - ik.GrabPulses[2]) <= 10)
                break;

            Thread.Sleep(1);  // PDO 周期
        }
    }

    // ============ EtherCAT 底层操作 ============

    /// <summary>
    /// PP 模式 3 轴同步运动, 等待到位
    /// </summary>
    void MoveAxes(int robotId, int[] pulses)
    {
        // 预获取 3 轴 PDO 映射
        ref var o0 = ref master.Slaves[GetMotorSlaveIndex(robotId, 0)].OutputsMapping<StepperMotor_Output>();
        ref var o1 = ref master.Slaves[GetMotorSlaveIndex(robotId, 1)].OutputsMapping<StepperMotor_Output>();
        ref var o2 = ref master.Slaves[GetMotorSlaveIndex(robotId, 2)].OutputsMapping<StepperMotor_Output>();
        ref var i0 = ref master.Slaves[GetMotorSlaveIndex(robotId, 0)].InputsMapping<StepperMotor_Input>();
        ref var i1 = ref master.Slaves[GetMotorSlaveIndex(robotId, 1)].InputsMapping<StepperMotor_Input>();
        ref var i2 = ref master.Slaves[GetMotorSlaveIndex(robotId, 2)].InputsMapping<StepperMotor_Input>();

        // 设置目标位置
        o0.TargetPosition = pulses[0]; o0.ControlWord = 0x001F;
        o1.TargetPosition = pulses[1]; o1.ControlWord = 0x001F;
        o2.TargetPosition = pulses[2]; o2.ControlWord = 0x001F;

        // 等待到位
        var deadline = DateTime.Now.AddMilliseconds(2000);
        while (DateTime.Now < deadline)
        {
            if (Math.Abs(i0.ActualPosition - pulses[0]) <= 10 &&
                Math.Abs(i1.ActualPosition - pulses[1]) <= 10 &&
                Math.Abs(i2.ActualPosition - pulses[2]) <= 10)
                break;
            Thread.Sleep(1);
        }
    }

    /// <summary>
    /// 控制吸盘 — Joint 0 电机 DO1
    /// </summary>
    void SetSuction(int robotId, bool on)
    {
        ref var o = ref master.Slaves[GetMotorSlaveIndex(robotId, 0)].OutputsMapping<StepperMotor_Output>();
        if (on) o.DigitalOutputs |= DO_SUCTION;
        else    o.DigitalOutputs &= ~DO_SUCTION;
    }

    /// <summary>
    /// 读取激光传感器 — Joint 0 电机 DI4
    /// </summary>
    public bool IsObjectDetected(int robotId)
    {
        ref var inp = ref master.Slaves[GetMotorSlaveIndex(robotId, 0)].InputsMapping<StepperMotor_Input>();
        return (inp.DigitalInputs & DI_LASER) != 0;
    }

    static void Main(string[] args) => new DeltaSortingController().Run();
}

视觉系统入队示例:

// 视觉 AI 线程: 线阵相机识别后调用 EnqueueTarget 入队
// 控制器机器人线程自动从队列抢占最近目标执行

var ctrl = new DeltaSortingController();

// 相机 AI 识别到一个目标
ctrl.EnqueueTarget(new SortingTarget
{
    Id = 42,
    EncoderAtDetect = 156000, // 检测时编码器脉冲值
    PixelY = 1280,            // 垂直传送带方向偏移 (像素, 相机坐标)
    Category = 2,             // AI 分类: PP 塑料
});