企业官网建设流程全解析

简介：一套基于Codesys平台的Delta并联机械手控制代码包，聚焦CNC插补动作到机器人运动的转换实现，面向自动化工程师、PLC编程与机器人控制开发者。资源共13个文件，以7个Python脚本为核心，覆盖运动学、轨迹规划、控制与测试逻辑；另含2个HTML可视化界面用于状态监控与参数调整，1个Markdown说明文档梳理整体思路，整体压缩包仅29KB，轻量且结构清晰。代码包完整呈现主控制程序(Prg_Plc)、运动控制程序(Prg_Motion)与可视化控制程序(Prg_Visu)三部分架构，重点展示SMC_ReadNCFile2、SMC_NCInterpreter等关键功能块对G代码文件读取、解析、速度检查及路径绑定的实现，并引入Tripod并联机械手三维空间移动控制算法，可直接作为学习参考或二次开发基线。已有215人学习下载，适合希望深入理解Codesys机器人控制实现细节的工程师与研究者。 提到用Codesys控制Delta机械手，很多人第一反应是“代码要怎么写”，然后才是“运动学到底能不能算得过来”。我最近正好用Codesys完整做了一套Delta并联机械手的控制系统，从EtherCAT伺服配置、运动学逆解到视觉引导分拣一路调通，中间踩了不少坑。今天把这套方案的关键代码、调试步骤和排查经验整理出来，给正在做并联机器人控制、非标自动化集成，或者打算把Codesys用在运动控制项目里的工程师做个参考。下面的所有代码思路都基于我实际跑过的项目，可以直接往自己的工程里套。

1. 项目整体思路与选型解析

1.1 为什么偏偏选Delta并联结构

Delta机械手是典型的并联机器人，三个主动臂通过平行四边形结构共同驱动一个动平台，动平台在运动过程中始终保持水平。和常见的六轴串联机器人相比，Delta的惯量小、刚性好、响应快，特别适合轻负载、高节拍的分拣和搬运，比如食品、药品、电子元件的理料和包装。但它的运动学关系和串联机器人完全不一样，末端的笛卡尔坐标和三个主动臂转角之间是非线性的映射关系，而且工作空间是一个近似倒扣碗状的曲面区域。这意味着控制器必须做实时坐标变换，不能像普通龙门轴那样直接按XYZ发脉冲。

在实际项目里选择Delta，往往不是因为它“看起来先进”，而是工艺节拍确实需要。比如一个视觉引导的分拣工位，要求每分钟抓取60到80次，每次移动距离不超过300毫米，用串联机器人做也能完成，但成本和能耗都不划算。Delta则凭借低惯量优势，可以在同样伺服电机下跑出更高的加速度。也正因为这样，Delta机械手的控制核心就是对三根轴的同步插补和运动学解算。

1.2 Codesys在这套系统里的定位

Codesys是IEC 61131-3标准下的集成开发环境，支持ST、LD、FBD、CFC、SFC等多种PLC编程语言，同时还集成了SoftMotion运动控制库。对Delta机械手这类设备来说，Codesys能同时承担PLC逻辑控制和运动控制两个角色。过去做非标设备，通常是一个PLC负责逻辑，旁边再挂一个专用运动控制器负责插补，两个系统之间通过总线或IO握手。遇到轴数多、路径复杂的设备，这种架构会让调试变得非常痛苦。

用Codesys做Delta控制，最大的优势是逻辑层和运动层可以写在同一份工程里。比如视觉触发信号来了，ST代码里既要做IO处理，又要调用运动指令，变量可以直接共享，不需要跨控制器通信。另一个优势是SoftMotion提供轴组、笛卡尔插补、电子凸轮等现成功能块，我们不需要自己去编写底层的轨迹插补算法，只需要把运动学正逆解挂到轴组上，然后把目标坐标交给运动引擎执行。代码可复用性也强，换不同品牌的伺服驱动器时，只要改EtherCAT站配置和轴参数，控制逻辑基本不用动。

1.3 控制系统架构与代码分层

我实际用的硬件架构是一台带Codesys Runtime的嵌入式控制器，通过EtherCAT总线连接三台伺服驱动器，分别驱动Delta的三根主动臂。控制器上面还接了一个触摸屏HMI，用于手动操作、配方管理和状态显示。控制程序整体分成两层：逻辑层负责报警、气缸、传送带、视觉触发信号；运动层负责轴控制、坐标变换、插补执行。

代码结构上，我习惯把运动学解算封装成独立的功能块，比如FB_DeltaKinematics。这个功能块只接收目标坐标和机械参数，输出三个关节角，不关心轴是怎么走的。另一个功能块负责轴组控制，调用SoftMotion的运动指令。这样分层的直接好处是，后续如果要换机械结构，只需要改运动学功能块里的参数；如果要换工艺流程，只需要改逻辑层，两边互不干扰。很多初学者上来就把逆解公式写在主程序里，后面越改越乱，等加了视觉、加了传送带同步之后，程序基本没法维护。

2. 核心代码结构与运动学实现

2.1 运动学逆解的基础逻辑

Delta机械手的控制离不开正解和逆解。逆解是已知动平台中心的目标位置，求三根主动臂各自的转角；正解则反过来，已知三个转角求末端位置。实际项目中，逆解使用频率远高于正解，因为自动运行模式下，上位机或视觉系统总是给你目标坐标，而不是给三根轴的角度。

Delta逆解的几何本质可以这样理解：把每一根主动臂看成“一个绕固定轴旋转的连杆”，主动臂末端通过从动臂和动平台相连。给定动平台中心位置后，动平台上的三个铰点位置也就确定了，对于每一根主动臂，问题就变成“已知一个固定球心和一个目标球面，求主动臂旋转到哪个角度能正好连上”。用数学语言描述，就是对于第i根主动臂，转角θ_i满足一个形如：

A_i * cos(θ_i) + B_i * sin(θ_i) = C_i

的方程。其中A_i、B_i、C_i由机械参数和当前目标坐标决定。解这个方程可以用辅助角公式，最终得到：

θ_i = ATAN2(B_i, A_i) ± ACOS(C_i / sqrt(A_i^2 + B_i^2))

这里的±号会产生两组解，实际需要根据机械手当前姿态选择连续的一组。通常在每次运行前保存上一次的角度，选离当前角度更近的那组解，这样能避免机械手在相邻周期里出现角度跳变。

2.2 用ST语言封装逆解功能块

我在Codesys里把逆解写成了一个独立功能块，输入是目标坐标和机械参数结构体，输出是三个关节角和一个有效性标志位。下面是一段接口定义和核心计算框架：

FUNCTION_BLOCK FB_DeltaKinematics VAR_INPUT bExecute : BOOL; posX : LREAL; posY : LREAL; posZ : LREAL; stParams : ST_DeltaParams; END_VAR VAR_OUTPUT angle1 : LREAL; angle2 : LREAL; angle3 : LREAL; bValid : BOOL; END_VAR VAR a1, b1, c1 : LREAL; a2, b2, c2 : LREAL; a3, b3, c3 : LREAL; tmp : LREAL; baseAngle : LREAL; END_VAR

实现时，三根主动臂的基座角度分别是0°、120°和240°，我写了一个私有方法或FOR循环来避免重复代码。核心逻辑是：先根据目标点坐标和机械参数，分别计算三根臂对应的A、B、C系数，然后判断C_i/sqrt(A_i^2+B_i^2)是否在[-1,1]范围内。如果超出，说明目标点不在工作空间内，bValid返回FALSE，程序就不会执行后续的运动指令。只有在所有臂都有效时，才计算最终角度并输出。

这里有一个细节：角度单位必须全局统一。我习惯在Codesys轴配置里把轴的单位设成度，逆解函数返回的也是度，这样MC_MoveAbsolute或轴组指令收到的角度不需要二次转换。如果你把轴单位设成了脉冲数或毫米，一定要在轴参数里把换算关系填对，否则机械手会走出完全错误的轨迹。

2.3 正解、工作空间校验与运行时的角色

正解在运行过程中用得不多，但在上电回零和示教模式下很有用。由于Delta正解没有简单的解析表达式，我没有在PLC里做实时迭代，而是在初始化时把工作空间离散成一张查找表，再用逆解去试算边界点。这种方法实现简单，而且能覆盖机械参数不准确带来的偏差。如果某个目标点逆解bValid为FALSE，就直接当成超出工作空间或接近奇异点处理，不会执行运动。

实际程序里，我在每个运动周期都会先调用一次逆解功能块，但并不是每次都马上执行轴组指令。对于视觉引导来说，目标坐标来自视觉系统，可能含有噪声或误差，直接发给轴组容易产生不必要的抖动。我会在逻辑层加一个“目标点滤波”，比如对连续几帧的坐标做个滑动平均，确认目标稳定后再触发运动。这个步骤看似简单，却能明显提升高速分拣的稳定性。

3. 实操搭建：从新建工程到机械手动起来

3.1 创建工程与EtherCAT轴配置

在Codesys里新建一个标准工程后，第一件事是在设备树中添加EtherCAT主站，然后扫描并添加三台伺服驱动器。添加完驱动器后，要给每个驱动器建立一个SoftMotion轴，轴类型选择“伺服轴”。轴参数里最重要的两项是编码器分辨率和减速比，它们决定了控制器发出的位置命令和实际机械转角之间的关系。

关于单位，我强烈推荐把轴单位统一设成“度”。Delta机械手的运动学逆解输入输出都是角度，轴单位用度可以直接把逆解结果写到轴组命令里，少一层换算。如果机械手的三根主动臂有减速机，就需要在轴配置里设置减速比，比如电机每转200000脉冲，减速比10:1，那么每度对应的指令单位就是200000/3600，换算关系填对后才能保证实际角度准确。这个位置填错，机械手动起来会直接体现出“角度对不上”的症状，而且很难排查。

3.2 轴组与三维空间插补

把三根轴单独配好之后，最重要的步骤是创建轴组。在Codesys SoftMotion里，轴组相当于一个虚拟的笛卡尔坐标系，三根物理轴作为轴组的成员，可以执行MC_MoveLinear、MC_MoveDirectAbsolute这类笛卡尔空间指令。对于Delta机械手，这一步非常关键：如果不用轴组，而是给每根轴单独发MC_MoveAbsolute，机械手末端在空间里走出的是三条独立插补的合成轨迹，而不是一条直线。

轴组配置时，需要选择运动学模型。如果安装的SoftMotion版本内置了Delta机器人模型，直接填入机械参数即可。如果版本里没有，就要用“外部运动学”接口，把我们前面写的逆解和正解挂载进去。完成轴组配置后，还要设定路径模式、速度单位、加速度单位和加加速度单位，建议笛卡尔坐标统一用毫米（mm），时间单位用秒（s），这样调试时直观一些。

3.3 主程序调用与运动缓冲

主程序里调用轴组运动指令的逻辑通常是：先计算目标点，通过逆解校验有效性，再把目标点发给轴组运动功能块。下面是我常用的一段ST调用示例：

// 假设视觉系统已经给出目标坐标 visX := 120.0; visY := 30.0; visZ := -180.0; // 第一步：逆解校验 fbDeltaKin(bExecute := TRUE, posX := visX, posY := visY, posZ := visZ, stParams := stDeltaParams); // 第二步：目标有效才发送给轴组 IF fbDeltaKin.bValid THEN mcMoveDirAbs.Position[1] := visX; mcMoveDirAbs.Position[2] := visY; mcMoveDirAbs.Position[3] := visZ; mcMoveDirAbs.Execute := TRUE; END_IF

这里的mcMoveDirAbs是MC_MoveDirectAbsolute类型的功能块实例，它会在笛卡尔空间里生成一条从当前位置到目标点的路径，并自动完成三轴联动。如果你希望机械手走直线，并且运动路径上的速度可控，应该用MC_MoveLinear并设置好TransitionMode。用一个很小的测试程序就能看出两者区别：让机械手从A点运动到B点，MC_MoveDirectAbsolute走的是关节空间内的一条平滑路径，MC_MoveLinear走的是空间直线。

3.4 可视化与手动调试

调试阶段最好有一个手动模式。我在HMI里做了三个页面：第一个页面实时显示当前末端坐标和三个关节角；第二个页面提供单轴点动按钮，方便检查每个伺服的方向和限位；第三个页面手动输入目标坐标，并显示逆解计算出的角度，这样在跑自动流程之前可以先验证运动学是否正确。

有一个调试技巧：手动模式下输入一个正方形轨迹上的若干点，让机械手末端走一个“回”字。如果每个角点都能准确定位，且运动过程中没有异响，说明运动学基本正确。如果发现机械手某个方向反向运动，或者路径中出现“打摆子”现象，不要急着改程序，先在轴配置里检查每个轴的方向，再回到运动学里检查坐标系定义。方向问题如果混在一起调，很容易越调越乱。

4. 常见问题与排查技巧实录

4.1 “目标不可达”到底是谁的问题

我在调试过程中遇到最多的报警就是目标点不可达，bValid被置成FALSE。刚开始以为是机械手工作空间小，后来才发现大部分情况是坐标系标定错误。比如，视觉系统给出的坐标原点在相机图像中心，而机器人坐标原点在静平台中心，两个坐标系之间没有做偏移补偿，导致目标点经常落在工作空间外。

排查方法是先手动把机械手末端移到几个已知位置，记录控制器读到的坐标，再和视觉系统给出的坐标对比。如果固定偏差可以通过平移解决，就在视觉标定矩阵里加上平移量；如果偏差随位置变化，则要考虑旋转或缩放系数。不要在控制器里盲目加大工作空间判断范围，那会让机械手在极限位置强制动作，很容易损坏机构。

另外，当目标点接近工作空间边缘时，逆解方程会出现两个解接近重合的情况，也就是奇异点。此时即使bValid为TRUE，机械手动作也可能非常剧烈。我在逻辑里加了一个安全策略：当目标坐标离边界还有安全距离时，就提前降低速度，并触发“接近边界”提示。这样既能避免奇异点，又不会白白浪费工作空间。

4.2 轴反向事故的排查顺序

新装的机械手第一次动起来，十有八九会遇到某根轴反向的问题。我踩过的坑是：先改运动学公式里的符号，结果发现没解决，后来才发现是伺服驱动器方向参数不对。轴反向的排查顺序应该是从下往上：先单独点动每一根轴，确认电机旋转方向和控制面板上显示的位置增加方向一致；再手动让主动臂转到某个已知角度，检查编码器反馈是否和实际角度一致；最后才去检查运动学代码。

如果你在轴配置里已经设置了反向，又在运动学公式里加了负号，等于做了两次反向，机械手还是反的。所以我建议“方向问题只在轴层解决，不在运动学层解决”。运动学代码必须保持数学模型的统一，不要为单个轴单独打补丁。等三根轴分别点动正确后，再让机械手走一个简单的单轴运动，观察末端是否沿预期方向移动，逐步确认。

4.3 高速节拍下的抖动与丢步

Delta机械手天然适合高节拍，但高速运行时的抖动和丢步非常考验调参能力。软件层面的问题主要是加加速度设置不合理。很多人把加速度设成很大，以为这样就能缩短节拍，实际上机械臂的刚性是有限的，加速度突变会造成残余振动，反而让定位时间变长。

这种情况下，我优先把轴组的加加速度（Jerk）设为加速度的5到10倍，路径规划会变得更平滑。如果还抖，再降低伺服驱动器的位置环增益或速度环增益，而不是一味增大增益追求跟随精度。机械层面也要检查联轴器、减速机间隙和安装底座的刚性，很多时候软件调不动，其实是机械结构有松动。用Codesys的Trace功能同时记录期望位置、实际位置和跟随误差，遇到抖动问题能很快定位到是哪个轴在什么位置附近出现振荡。

5. 进阶扩展与经验复盘

5.1 视觉引导分拣的坐标标定

当Delta机械手配合视觉系统时，标定比图像识别更影响最终效果。我的做法是：让机械手末端带着一个标定针，依次走到四个已知点，在每个点触发相机拍照，记录像素坐标。然后用一个最小二乘函数求解像素坐标到机器人坐标的变换矩阵。这个矩阵可以写死在程序里，也可以做成配方变量，方便现场调整。

视觉系统通过UDP把目标坐标发送给Codesys时，要注意数据格式和字节序。我习惯在Codesys里用Socket库接收字符串，再用字符串解析函数分成X、Y、Z三个数值。解析出来的坐标不能直接发给轴组，要先经过标定矩阵变换。最后再加一层滑动滤波，避免视觉抖动引起机械手高频摆动。

5.2 配方管理和参数自检

设备运行一段时间后，减速器磨损、皮带张力变化都会影响轨迹精度。我在工程里加入了一个“标准轨迹自检”功能：定期让机械手走一条固定路径，比较实际反馈和理论轨迹的偏差，超过阈值就报警提示保养。这个功能既提升了设备的可维护性，也方便售后远程判断问题。

另一个值得做的是配方管理。不同产品对应不同的视觉标定矩阵、速度参数和抓取高度，这些数据全部放在一个结构体里，通过文件读写保存到U盘或控制器内部存储。换产时只需要在HMI上选择对应配方，不需要重新下载程序。Codesys的文件操作指令可以读写CSV或XML，数据量不大时非常可靠。

5.3 一点实际体会

做完这套系统，我的体会是：控制Delta机械手，真正的难点从来不是把逆解公式抄进去，而是坐标系、单位、安全逻辑和异常保护这些边角工作。如果你正在用Codesys做并联机器人，建议一开始就把运动学功能块独立封装，把参数和逻辑分开，后面对接视觉或工艺调整都会省力很多。调试时多用Trace记录曲线，很多莫名其妙的抖动和丢步问题，看一眼波形比猜半天管用得多。

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