企业官网建设流程全解析

简介：面向工业自动化与机器人控制领域的工程师和开发者，这份代码包围绕 Codesys 平台控制 Delta 并联机械手的实现路径，提供了完整的参考方案。内容覆盖从数控代码插补动作到机器人运动控制的转换路径，包含主控制、运动控制、可视化控制三大模块，以及文件读取解析、速度检查、路径绑定等关键环节。对于数控代码解析器、运动学与轨迹规划、三维空间移动控制等功能块，也给出了可运行或可移植的代码实现，方便在工程中对照验证和二次开发。压缩包共13个文件，以Python源码为主，用于运动学解算、轨迹规划与控制算法实现，辅以两个可视化页面、说明文档和依赖配置，整体仅约29KB，结构紧凑。目前已有215人学习浏览，适合正在研究并联机械手自动化控制的中高级开发者参考。 前阵子刚交付一套用Codesys写的Delta机械手控制程序，从运动学建模、EtherCAT组网到最后一版代码跑通，前后折腾了大概三周。说实话，用Codesys控制Delta这件事，真正的门槛不在PLC编程本身，而在于怎么把机构几何、伺服同步和运动插补揉进一个循环周期只有1ms的任务里。这篇把整套实现思路和核心代码拆开讲，希望能帮到正在用通用控制器做并联机器人的朋友。

1. 先想清楚再动手：Delta控制方案的选型逻辑

1.1 为什么盯上Codesys而不是专用机器人控制器

一开始我也纠结过要不要直接用ABB、发那科那类专用机器人控制器，后来又看了几家主流的Delta专用控制方案，最后定的Codesys，原因很直接：

• 成本结构灵活。Codesys Runtime可以跑在工控机、嵌入式PC甚至树莓派上，一套授权价格比专用机器人控制器低一个量级，特别适合中小型设备商。
• 逻辑控制、运动控制、视觉通讯在同一环境里闭环。电气工程师用ST语言顺手，不用跨两个编程平台来回切。
• EtherCAT支持成熟。Delta机械手本质就是三根伺服轴的高精度同步，EtherCAT是最稳的载体，Codesys对EtherCAT的封装比多数专用机器人系统更透明，出了问题自己能排查。
• 代码可移植性强。整个控制逻辑是标准IEC 61131-3语言，换硬件平台不用重写。

当然短板也明显：没有现成的机器人工艺包，示教、轨迹规划、坐标变换全部要自己写，相当于把一个机器人系统拆成了“PLC + 运动库 + 几何算法”三块自己拼。

1.2 项目里的机构参数与控制需求

我做的这台Delta是标准三自由度并联构型：静平台半径R=250mm，动平台半径r=60mm，主动臂长L1=300mm，从动杆长L2=700mm。三个主动臂在静平台上呈120度均布，每个主动臂通过平行四边形从动杆连接动平台，动平台始终保持水平姿态。

控制需求也很有代表性：

这套参数基本覆盖了食品分拣和轻工装配的常见场景。因为不需要控制姿态，三轴Delta的核心控制任务其实就两件事：实时逆解算出三个关节角，再让三根轴精确且平滑地运动到目标角度。

1.3 控制任务的四层拆解

代码动手前，我花了一天时间把整个控制器要承担的工作拆成四层：

1. 模型层：正解、逆解、奇异判断，所有几何相关的计算都在这一层。
2. 规划层：接收目标坐标点，规划连续轨迹，生成笛卡尔空间的插补点序列。
3. 执行层：把插补点经逆解换算成关节角位置指令，发给伺服驱动器。
4. 逻辑层：IO控制、状态机、HMI通信、视觉触发，负责协调设备流程。

这个拆法直接影响后面工程文件怎么组织。模型层我单独做了一个FB，规划层一个FB，执行层一个FB，逻辑层放在主程序里。后续调试时每一层都能单独打点监控，定位问题速度快很多。

2. 硬件拓扑与总线配置：EtherCAT把三根轴拧成一根

2.1 伺服与驱动选型清单

Delta机械手对伺服的要求是“中惯量、高动态、低齿槽”。我一圈对比下来，最终配置如下：

绝对值编码器这条我强烈建议保留。Delta机构没有传统意义上的“零位挡块”，三轴联动才能确定末端位置，用增量编码器每次上电都要做回归，非常痛苦。

2.2 总线周期与同步配置里的坑

EtherCAT组网在Codesys里不算麻烦，但有几个点必须处理对：

• 分布式时钟一定要开。三个伺服节点在同一个EtherCAT总线上，如果不用分布式时钟，理论上主站逐个发帧的时间差会导致各轴实际启动时刻不一致，Delta这种高速同步场景下轨迹会出现肉眼可见的抖动。
• 运动循环周期和EtherCAT同步周期要匹配。我的运动任务运行在1ms，EtherCAT同步设为基线同样周期。单独把运动任务挂到总线同步中断上，保证每个周期开始前的轴位置一值是准确的。
• 伺服使能时序不能省。上电后先读驱动器状态字，等所有节点处于“ReadyToSwitchOn”，再统一发使能命令。直接上使能偶发出现某轴没起来的情况，排查起来很折磨人。

我还给每个轴配置了从站别名，物理排线上贴了标签。虽然每路伺服背后都有PZD数据可查，但总线扫描后三个轴顺序错乱的情况一旦发生，轻则限位误触发，重则直接撞机构。

3. Delta三轴逆解：从几何约束到可运行代码

3.1 向量法推导逆解的过程

逆解的目标很明确：给定动平台中心点坐标P(x,y,z)，求出三个主动臂的关节角θ1、θ2、θ3。

我用向量法来做，比尝试用三角函数硬推方程组直观得多。以第i条支链为例（i=0,1,2，分布角度分别为0°、120°、240°）：

定义径向单位向量u_i、切向单位向量t_i、竖直单位向量z。静平台铰点A_i、动平台铰点B_i、主动臂末端点E_i之间满足：

|E_i - B_i|² = L2²

设S_i = B_i - A_i，把S_i分解到三个方向上，约束方程整理后得到：

a·cosθ_i + b·sinθ_i = (a² + b² + c² + L1² - L2²) / (2·L1)

其中a、b、c分别是S_i在径向、竖直、切向方向的投影。令ρ = sqrt(a²+b²)，φ = atan2(b,a)，方程就变成标准形式：

θ_i = φ ± acos( (a²+b²+c²+L1²-L2²) / (2·L1·ρ) )

这样每个关节角都有两个解，对应主动臂的两种姿态，实际选哪一支，要根据机构装配方向和运动连续性来定。

3.2 Codesys ST语言实现的逆解函数块

代码里我约定：世界坐标系Z轴向上为正，主动臂角度零位为水平朝外，正方向为向下旋转。动平台在基座下方，所以轨迹目标点的Pz通常是负值。

FUNCTION_BLOCK FB_DeltaInverse VAR_INPUT bExecute : BOOL; Px : LREAL; Py : LREAL; Pz : LREAL; END_VAR VAR_OUTPUT bDone : BOOL; bError : BOOL; ThetaA : LREAL; // 关节角1，弧度 ThetaB : LREAL; // 关节角2，弧度 ThetaC : LREAL; // 关节角3，弧度 END_VAR VAR {S7.extern} R : LREAL := 250.0; // 静平台半径 r : LREAL := 60.0; // 动平台半径 L1 : LREAL := 300.0; // 主动臂长度 L2 : LREAL := 700.0; // 从动杆长度 d : LREAL; phi : ARRAY[1..3] OF LREAL := [0.0, 2.094395102, 4.188790205]; i : INT; a : LREAL; b : LREAL; c : LREAL; rho : LREAL; alpha : LREAL; k : LREAL; tmpTheta : ARRAY[1..3] OF LREAL; END_VAR

IF NOT bExecute THEN bDone := FALSE; bError := FALSE; RETURN; END_IF d := R - r; FOR i := 1 TO 3 DO // S_i 在三个方向上的投影 a := Px * COS(phi[i]) + Py * SIN(phi[i]) - d; b := -Pz; // Z轴向上为正，动平台在下方，所以取负 c := -Px * SIN(phi[i]) + Py * COS(phi[i]); rho := SQRT(a * a + b * b); alpha := ATAN2(b, a); k := (a * a + b * b + c * c + L1 * L1 - L2 * L2) / (2 * L1 * rho); IF ABS(k) >= 1.0 THEN bError := TRUE; tmpTheta[i] := tmpTheta[i]; // 保持上一拍值 ELSE // 取 alpha - acos(k) 作为主解，与机械装配方向一致 tmpTheta[i] := alpha - ACOS(k); END_IF END_FOR IF NOT bError THEN ThetaA := tmpTheta[1]; ThetaB := tmpTheta[2]; ThetaC := tmpTheta[3]; END_IF bDone := TRUE;

3.3 多解分支与坐标系陷阱

上面代码里有一行注释值得重点展开——“取alpha - acos(k)作为主解”。这里的正负号不是拍脑袋定的，得看主动臂装配是朝内还是朝外。我调第一款样机时就在这里栽过跟头：

• 角度符号反了，末端轨迹会沿Z轴镜像，即在模型里模拟是正方向，实际动平台却向下砸。
• 多解分支不稳定，机构会走到另一支解去，实际表现是动平台突然反向翻转。
• 处理办法：在逆解函数里额外引入一个“上一拍关节角”输入，输出时对theta做就近匹配，保证相邻周期角度差最小，主动臂不会出现瞬跳。

坐标系约定更要命。有的资料定义Z轴向下为正，有的向上为正，还有的把关节角零位定在竖直向下。这三个约定只要错一个，整套算法全废。我后来把所有约定用注释写在程序文件最前面，包括角度方向、坐标方向、单位，后续再看代码或者别人接手都不容易产生歧义。

4. 运动控制代码框架：从单轴点到连续轨迹

4.1 任务周期与代码结构

控制程序的实时性要求决定了任务结构。我开了三个周期任务：

Motion_Task和EtherCAT_Task共用一个周期，但Motion_Task先算好位置指令，EtherCAT_Task再送出去。两个任务之间用输入输出变量直通，没有用队列，因为1ms内完成计算、逆解和指令输出完全够用。

4.2 点位控制的实现

最基础的模式是“指定关节角目标”。这个模式主要用来机械调试、单轴对点、标定行程。

// 简单实现：每次触发后按固定速度走绝对位置 IF bMoveAbs_Trigger THEN fbMoveAbsA.Execute := TRUE; fbMoveAbsA.Axis := Axis1; fbMoveAbsA.Position := g_dThetaA_Deg; // 单位换算成电机侧位置 fbMoveAbsA.Velocity := 60.0; // 度/秒 fbMoveAbsA.Acceleration := 300.0; fbMoveAbsA.Deceleration := 300.0; END_IF

Codesys运动库里自带MC_MoveAbsolute、MC_MoveVelocity这些标准的PLCopen功能块，直接用就好，没必要自己造轮子。唯一要注意的是位置单位，我整套系统统一采用“用户单位=度”，伺服驱动器内部再做电子齿轮换算。这样逻辑层的数值和机械角度一一对应，排查时少一层换算成本。

4.3 连续轨迹插补方案

实际分拣动作不能一个点一个点地走，末端要平滑扫出一条轨迹。Delta机械手最常见的动作是“拾取—抬升—平移—下降—放置”，对轨迹的平滑性要求集中在首尾加速度不突变。

我的方案是做一个笛卡尔空间轨迹插补器：

1. 在Logic_Task里维护一组目标点队列，每个点包含(X, Y, Z)和该点停留时间。
2. Motion_Task每周期从队列取当前段目标，根据梯形速度曲线算出当前插补点坐标。
3. 每个插补点坐标喂给逆解FB，得到三个关节角，再换算成电机侧目标位置。

轨迹段与段之间，我额外加了拐角圆弧过渡。没有这个过渡的话，目标是走一个直角折线，末端会猛地顿一下，节拍一快就跟不上。

插补器核心代码大致是这个思路：

// Motion_Task 内部每周期执行一次 IF bRunning THEN // 根据速度曲线计算当前段进度，得到笛卡尔坐标 g_dCurX := g_dStartX + (g_dEndX - g_dStartX) * f; g_dCurY := g_dStartY + (g_dEndY - g_dStartY) * f; g_dCurZ := g_dStartZ + (g_dEndZ - g_dStartZ) * f; // 逆解得到关节角 fbInv.bExecute := TRUE; fbInv.Px := g_dCurX; fbInv.Py := g_dCurY; fbInv.Pz := g_dCurZ; // 关节角转电机侧位置 g_dPosCmdA := g_dThetaA / g_dGearRatio * 360.0; g_dPosCmdB := g_dThetaB / g_dGearRatio * 360.0; g_dPosCmdC := g_dThetaC / g_dGearRatio * 360.0; END_IF

代码里的f是当前段的归一化进度，它由速度曲线决定，不是简单的线性递增。我用的速度规划叫“对称梯形加减速”，加速段、匀速段、减速段三段分别计算，每周期更新。

这里要特别提醒一点：如果项目里已经用了Codesys的SoftMotion库，可以同时看看SMC_CNC_REF和电子凸轮相关功能块。在某些重复性拾取场景下，把轨迹预先做成电子凸轮曲线表（类似飞剪的循环曲线），一个主凸轮角度驱动末端轨迹循环，运行时计算量更小、更稳定。我后来把标准拾取动作做成了电子凸轮，效果比纯笛卡尔插补更顺。

5. 整定与调试：没有示教器的日子怎么过

5.1 伺服增益与动态前馈

Delta调试最花时间的环节是伺服参数整定。没有示教器，没有机器人厂家的自动整定工具，只能靠伺服驱动器的自带调试软件和自整定功能。

我的做法分三步：

1. 先用驱动器自整定跑一遍，得到各轴负载惯量比。Delta轴的负载随末端位置变化，惯量比不是常数，所以取一个中间位置的整定值作为基准。
2. 位置环增益先保守一点，设低，跑一段20mm半径圆的连续轨迹，观察跟踪误差曲线。慢慢增大位置增益，直到跟踪误差不再明显下降为止。
3. 增加速度前馈和加速度前馈，主要补偿大加速度段的滞后。Delta在高速拾取时加速度轻松到20m/s²以上，没有加速度前馈，轨迹拐角一定会过冲。

前馈系数不能完全照搬伺服驱动器的默认值。我给每轴单独做了前馈标定：让机构沿Z轴做正弦运动，用一个激光位移传感器测末端实际位置，把三个轴各自的前馈比例修正到跟踪误差在±1mm以内。

5.2 现场踩过的坑

调试期间踩过的坑值得单独记录，因为每个坑都对应一类Delta机械手项目的共性问题：

1. 任务抖动导致轨迹毛刺。初期Motion_Task里顺带做了很多HMI通信处理和数组拷贝，偶发一个周期跑到1.5ms，末端轨迹就在监控画面上出现尖刺。解决方式是严格把周期超过1.2ms的循环用计数器捕捉出来，逐项清理耗时操作。
2. 电机抱闸时序不正确导致动平台下沉。抱闸释放太慢，三个轴在加速度设定下出现拖拽，动静平台之间产生很大的剪切力。后来我增加了抱闸释放完成反馈再开始运动的联锁。
3. 逆解无解时没做保护。工作空间边界外的目标点会让SQRT里出现负数或k值绝对值大于1，代码里虽然置了bError，但主程序没有在触发点阶段拒绝，结果跑到边界附近时有一轴突然反转。后来我在任务规划层就增加目标点工作空间预校验。
4. 原点丢失。前面推荐了绝对编码器，但伺服每次上电还是要建立“绝对位置与机械零位的映射”。实现上是给三个轴设计了一个离线标定点，手动把主动臂摆到水平位置，然后触发一次位置映射写入控制器。

5.3 验收测试怎么做

项目交付前我用的验收方案很简单但有效：

• 空载跑500次标准拾取循环，用示波器记录跟踪误差，观察有无趋势性漂移。
• 安装位置加负载1kg，跑300次，确认精度不掉出±0.5mm。
• 连续运行2小时，监控三轴温升，目标不超过40K。
• 手动上推下压动平台，确认伺服刚性合适、没有异常啸叫和机械抖动。

这些测试数据我全部打包進了交付文档。设备报修时，拿出当时的基线数据对比，能很快判断是机械磨损还是电气参数漂移。

6. 代码的进一步工程化：从能跑到好用

6.1 模块化封装与可配置化

第一版代码把所有变量都放在主程序里，调试起来倒是直观，后期维护很崩溃。交付前我做了重构，核心模块全部封装成带结构化参数的FB：

• FB_DeltaInverse：完成逆解计算，输入输出直接暴露关节角。
• FB_TrajectoryPlanner：管理目标点队列、速度曲线、拐角过渡。
• FB_DeltaControl：整合逆解、位置指令生成、急停、使能、回零逻辑。
• FB_HMIComm：负责触摸屏读写、配方管理和数据记录。

参数全部收敛到UDT里，每个UDT对应一个配方文件。这样换一套Delta机构尺寸时，只需要在HMI上改R、r、L1、L2等参数，不需要重新编译。

6.2 后续升级方向

这个项目交付后，我自己评估了几个可以继续深挖的方向：

• 动力学前馈。目前只用了运动学加简单前馈，对于高速高负载场景，可以在逆解之外再加一个基于拉格朗日方程或牛顿欧拉递推的动力学模型，把重力和惯性力补偿到前馈力矩中，精度和稳定性都能再上一个台阶。
• 视觉引导闭环。增加一个视觉定位相机，把检测到的物料坐标通过UDP发给Codesys控制器，自动进入任务队列。这个方案视觉和运动控制在同一个控制器里协同，延迟比传统PLC+上位机方案低很多。
• 多台Delta协同。Codesys作为软PLC运行在带多个网口的控制器上，可以同时管理两条EtherCAT总线，让两台机械手在共享工作区协同拾放，避免碰撞。

最后再分享一个与我个人习惯有关的建议：像Delta这种机构，代码里一定要把坐标系约定、单位约定、角度方向约定用注释放在文件最前面。开发的三个星期里，我自己就因为这些约定不清改过两版逆解代码，更别提后来接手项目的同事了。先钉死约定，再写逻辑，后面会省下大量不必要的返工时间。

本文还有配套的精品资源，点击获取