尧图网站建设 
简介面向工业自动化与机器人控制领域的工程师和开发者这份代码包围绕 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是标准三自由度并联构型静平台半径R250mm动平台半径r60mm主动臂长L1300mm从动杆长L2700mm。三个主动臂在静平台上呈120度均布每个主动臂通过平行四边形从动杆连接动平台动平台始终保持水平姿态。控制需求也很有代表性项目参数末端负载1kg以内定位精度±0.5mm标准节拍60次/分钟拾取放置总线类型EtherCAT周期1ms工作空间直径约900mm高度约350mm这套参数基本覆盖了食品分拣和轻工装配的常见场景。因为不需要控制姿态三轴Delta的核心控制任务其实就两件事实时逆解算出三个关节角再让三根轴精确且平滑地运动到目标角度。1.3 控制任务的四层拆解代码动手前我花了一天时间把整个控制器要承担的工作拆成四层模型层正解、逆解、奇异判断所有几何相关的计算都在这一层。规划层接收目标坐标点规划连续轨迹生成笛卡尔空间的插补点序列。执行层把插补点经逆解换算成关节角位置指令发给伺服驱动器。逻辑层IO控制、状态机、HMI通信、视觉触发负责协调设备流程。这个拆法直接影响后面工程文件怎么组织。模型层我单独做了一个FB规划层一个FB执行层一个FB逻辑层放在主程序里。后续调试时每一层都能单独打点监控定位问题速度快很多。2. 硬件拓扑与总线配置EtherCAT把三根轴拧成一根2.1 伺服与驱动选型清单Delta机械手对伺服的要求是“中惯量、高动态、低齿槽”。我一圈对比下来最终配置如下部件选型建议说明控制器支持Codesys Runtime的工业PC或PLC至少2核主频越大越好因为运动库和逆解都要吃算力伺服驱动器支持EtherCAT的通用伺服建议选带位置/速度双环且支持前馈的伺服电机750W/1kW中惯量带绝对值编码器绝对编码器省掉找原点的大麻烦减速机行星减速比5:1或10:1背隙3弧分背隙直接影响末端精度不能省制动器三轴都要带抱闸断电时防止动平台坠落尤其没用配重的情况绝对值编码器这条我强烈建议保留。Delta机构没有传统意义上的“零位挡块”三轴联动才能确定末端位置用增量编码器每次上电都要做回归非常痛苦。2.2 总线周期与同步配置里的坑EtherCAT组网在Codesys里不算麻烦但有几个点必须处理对分布式时钟一定要开。三个伺服节点在同一个EtherCAT总线上如果不用分布式时钟理论上主站逐个发帧的时间差会导致各轴实际启动时刻不一致Delta这种高速同步场景下轨迹会出现肉眼可见的抖动。运动循环周期和EtherCAT同步周期要匹配。我的运动任务运行在1msEtherCAT同步设为基线同样周期。单独把运动任务挂到总线同步中断上保证每个周期开始前的轴位置一值是准确的。伺服使能时序不能省。上电后先读驱动器状态字等所有节点处于“ReadyToSwitchOn”再统一发使能命令。直接上使能偶发出现某轴没起来的情况排查起来很折磨人。我还给每个轴配置了从站别名物理排线上贴了标签。虽然每路伺服背后都有PZD数据可查但总线扫描后三个轴顺序错乱的情况一旦发生轻则限位误触发重则直接撞机构。3. Delta三轴逆解从几何约束到可运行代码3.1 向量法推导逆解的过程逆解的目标很明确给定动平台中心点坐标P(x,y,z)求出三个主动臂的关节角θ1、θ2、θ3。我用向量法来做比尝试用三角函数硬推方程组直观得多。以第i条支链为例i0,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_VARIF 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 任务周期与代码结构控制程序的实时性要求决定了任务结构。我开了三个周期任务任务名周期优先级内容EtherCAT_Task1ms最高轴数据读写、位置输出Motion_Task1ms高插补点生成、逆解计算、梯形速度规划Logic_Task10ms普通急停、状态机、HMI通信、视觉信号处理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_IFCodesys运动库里自带MC_MoveAbsolute、MC_MoveVelocity这些标准的PLCopen功能块直接用就好没必要自己造轮子。唯一要注意的是位置单位我整套系统统一采用“用户单位度”伺服驱动器内部再做电子齿轮换算。这样逻辑层的数值和机械角度一一对应排查时少一层换算成本。4.3 连续轨迹插补方案实际分拣动作不能一个点一个点地走末端要平滑扫出一条轨迹。Delta机械手最常见的动作是“拾取—抬升—平移—下降—放置”对轨迹的平滑性要求集中在首尾加速度不突变。我的方案是做一个笛卡尔空间轨迹插补器在Logic_Task里维护一组目标点队列每个点包含(X, Y, Z)和该点停留时间。Motion_Task每周期从队列取当前段目标根据梯形速度曲线算出当前插补点坐标。每个插补点坐标喂给逆解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调试最花时间的环节是伺服参数整定。没有示教器没有机器人厂家的自动整定工具只能靠伺服驱动器的自带调试软件和自整定功能。我的做法分三步先用驱动器自整定跑一遍得到各轴负载惯量比。Delta轴的负载随末端位置变化惯量比不是常数所以取一个中间位置的整定值作为基准。位置环增益先保守一点设低跑一段20mm半径圆的连续轨迹观察跟踪误差曲线。慢慢增大位置增益直到跟踪误差不再明显下降为止。增加速度前馈和加速度前馈主要补偿大加速度段的滞后。Delta在高速拾取时加速度轻松到20m/s²以上没有加速度前馈轨迹拐角一定会过冲。前馈系数不能完全照搬伺服驱动器的默认值。我给每轴单独做了前馈标定让机构沿Z轴做正弦运动用一个激光位移传感器测末端实际位置把三个轴各自的前馈比例修正到跟踪误差在±1mm以内。5.2 现场踩过的坑调试期间踩过的坑值得单独记录因为每个坑都对应一类Delta机械手项目的共性问题任务抖动导致轨迹毛刺。初期Motion_Task里顺带做了很多HMI通信处理和数组拷贝偶发一个周期跑到1.5ms末端轨迹就在监控画面上出现尖刺。解决方式是严格把周期超过1.2ms的循环用计数器捕捉出来逐项清理耗时操作。电机抱闸时序不正确导致动平台下沉。抱闸释放太慢三个轴在加速度设定下出现拖拽动静平台之间产生很大的剪切力。后来我增加了抱闸释放完成反馈再开始运动的联锁。逆解无解时没做保护。工作空间边界外的目标点会让SQRT里出现负数或k值绝对值大于1代码里虽然置了bError但主程序没有在触发点阶段拒绝结果跑到边界附近时有一轴突然反转。后来我在任务规划层就增加目标点工作空间预校验。原点丢失。前面推荐了绝对编码器但伺服每次上电还是要建立“绝对位置与机械零位的映射”。实现上是给三个轴设计了一个离线标定点手动把主动臂摆到水平位置然后触发一次位置映射写入控制器。5.3 验收测试怎么做项目交付前我用的验收方案很简单但有效空载跑500次标准拾取循环用示波器记录跟踪误差观察有无趋势性漂移。安装位置加负载1kg跑300次确认精度不掉出±0.5mm。连续运行2小时监控三轴温升目标不超过40K。手动上推下压动平台确认伺服刚性合适、没有异常啸叫和机械抖动。这些测试数据我全部打包進了交付文档。设备报修时拿出当时的基线数据对比能很快判断是机械磨损还是电气参数漂移。6. 代码的进一步工程化从能跑到好用6.1 模块化封装与可配置化第一版代码把所有变量都放在主程序里调试起来倒是直观后期维护很崩溃。交付前我做了重构核心模块全部封装成带结构化参数的FBFB_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这种机构代码里一定要把坐标系约定、单位约定、角度方向约定用注释放在文件最前面。开发的三个星期里我自己就因为这些约定不清改过两版逆解代码更别提后来接手项目的同事了。先钉死约定再写逻辑后面会省下大量不必要的返工时间。本文还有配套的精品资源点击获取 Codesys控制Delta机械手：从逆解到EtherCAT同步的完整实现
简介面向工业自动化与机器人控制领域的工程师和开发者这份代码包围绕 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是标准三自由度并联构型静平台半径R250mm动平台半径r60mm主动臂长L1300mm从动杆长L2700mm。三个主动臂在静平台上呈120度均布每个主动臂通过平行四边形从动杆连接动平台动平台始终保持水平姿态。控制需求也很有代表性项目参数末端负载1kg以内定位精度±0.5mm标准节拍60次/分钟拾取放置总线类型EtherCAT周期1ms工作空间直径约900mm高度约350mm这套参数基本覆盖了食品分拣和轻工装配的常见场景。因为不需要控制姿态三轴Delta的核心控制任务其实就两件事实时逆解算出三个关节角再让三根轴精确且平滑地运动到目标角度。1.3 控制任务的四层拆解代码动手前我花了一天时间把整个控制器要承担的工作拆成四层模型层正解、逆解、奇异判断所有几何相关的计算都在这一层。规划层接收目标坐标点规划连续轨迹生成笛卡尔空间的插补点序列。执行层把插补点经逆解换算成关节角位置指令发给伺服驱动器。逻辑层IO控制、状态机、HMI通信、视觉触发负责协调设备流程。这个拆法直接影响后面工程文件怎么组织。模型层我单独做了一个FB规划层一个FB执行层一个FB逻辑层放在主程序里。后续调试时每一层都能单独打点监控定位问题速度快很多。2. 硬件拓扑与总线配置EtherCAT把三根轴拧成一根2.1 伺服与驱动选型清单Delta机械手对伺服的要求是“中惯量、高动态、低齿槽”。我一圈对比下来最终配置如下部件选型建议说明控制器支持Codesys Runtime的工业PC或PLC至少2核主频越大越好因为运动库和逆解都要吃算力伺服驱动器支持EtherCAT的通用伺服建议选带位置/速度双环且支持前馈的伺服电机750W/1kW中惯量带绝对值编码器绝对编码器省掉找原点的大麻烦减速机行星减速比5:1或10:1背隙3弧分背隙直接影响末端精度不能省制动器三轴都要带抱闸断电时防止动平台坠落尤其没用配重的情况绝对值编码器这条我强烈建议保留。Delta机构没有传统意义上的“零位挡块”三轴联动才能确定末端位置用增量编码器每次上电都要做回归非常痛苦。2.2 总线周期与同步配置里的坑EtherCAT组网在Codesys里不算麻烦但有几个点必须处理对分布式时钟一定要开。三个伺服节点在同一个EtherCAT总线上如果不用分布式时钟理论上主站逐个发帧的时间差会导致各轴实际启动时刻不一致Delta这种高速同步场景下轨迹会出现肉眼可见的抖动。运动循环周期和EtherCAT同步周期要匹配。我的运动任务运行在1msEtherCAT同步设为基线同样周期。单独把运动任务挂到总线同步中断上保证每个周期开始前的轴位置一值是准确的。伺服使能时序不能省。上电后先读驱动器状态字等所有节点处于“ReadyToSwitchOn”再统一发使能命令。直接上使能偶发出现某轴没起来的情况排查起来很折磨人。我还给每个轴配置了从站别名物理排线上贴了标签。虽然每路伺服背后都有PZD数据可查但总线扫描后三个轴顺序错乱的情况一旦发生轻则限位误触发重则直接撞机构。3. Delta三轴逆解从几何约束到可运行代码3.1 向量法推导逆解的过程逆解的目标很明确给定动平台中心点坐标P(x,y,z)求出三个主动臂的关节角θ1、θ2、θ3。我用向量法来做比尝试用三角函数硬推方程组直观得多。以第i条支链为例i0,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_VARIF 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 任务周期与代码结构控制程序的实时性要求决定了任务结构。我开了三个周期任务任务名周期优先级内容EtherCAT_Task1ms最高轴数据读写、位置输出Motion_Task1ms高插补点生成、逆解计算、梯形速度规划Logic_Task10ms普通急停、状态机、HMI通信、视觉信号处理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_IFCodesys运动库里自带MC_MoveAbsolute、MC_MoveVelocity这些标准的PLCopen功能块直接用就好没必要自己造轮子。唯一要注意的是位置单位我整套系统统一采用“用户单位度”伺服驱动器内部再做电子齿轮换算。这样逻辑层的数值和机械角度一一对应排查时少一层换算成本。4.3 连续轨迹插补方案实际分拣动作不能一个点一个点地走末端要平滑扫出一条轨迹。Delta机械手最常见的动作是“拾取—抬升—平移—下降—放置”对轨迹的平滑性要求集中在首尾加速度不突变。我的方案是做一个笛卡尔空间轨迹插补器在Logic_Task里维护一组目标点队列每个点包含(X, Y, Z)和该点停留时间。Motion_Task每周期从队列取当前段目标根据梯形速度曲线算出当前插补点坐标。每个插补点坐标喂给逆解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调试最花时间的环节是伺服参数整定。没有示教器没有机器人厂家的自动整定工具只能靠伺服驱动器的自带调试软件和自整定功能。我的做法分三步先用驱动器自整定跑一遍得到各轴负载惯量比。Delta轴的负载随末端位置变化惯量比不是常数所以取一个中间位置的整定值作为基准。位置环增益先保守一点设低跑一段20mm半径圆的连续轨迹观察跟踪误差曲线。慢慢增大位置增益直到跟踪误差不再明显下降为止。增加速度前馈和加速度前馈主要补偿大加速度段的滞后。Delta在高速拾取时加速度轻松到20m/s²以上没有加速度前馈轨迹拐角一定会过冲。前馈系数不能完全照搬伺服驱动器的默认值。我给每轴单独做了前馈标定让机构沿Z轴做正弦运动用一个激光位移传感器测末端实际位置把三个轴各自的前馈比例修正到跟踪误差在±1mm以内。5.2 现场踩过的坑调试期间踩过的坑值得单独记录因为每个坑都对应一类Delta机械手项目的共性问题任务抖动导致轨迹毛刺。初期Motion_Task里顺带做了很多HMI通信处理和数组拷贝偶发一个周期跑到1.5ms末端轨迹就在监控画面上出现尖刺。解决方式是严格把周期超过1.2ms的循环用计数器捕捉出来逐项清理耗时操作。电机抱闸时序不正确导致动平台下沉。抱闸释放太慢三个轴在加速度设定下出现拖拽动静平台之间产生很大的剪切力。后来我增加了抱闸释放完成反馈再开始运动的联锁。逆解无解时没做保护。工作空间边界外的目标点会让SQRT里出现负数或k值绝对值大于1代码里虽然置了bError但主程序没有在触发点阶段拒绝结果跑到边界附近时有一轴突然反转。后来我在任务规划层就增加目标点工作空间预校验。原点丢失。前面推荐了绝对编码器但伺服每次上电还是要建立“绝对位置与机械零位的映射”。实现上是给三个轴设计了一个离线标定点手动把主动臂摆到水平位置然后触发一次位置映射写入控制器。5.3 验收测试怎么做项目交付前我用的验收方案很简单但有效空载跑500次标准拾取循环用示波器记录跟踪误差观察有无趋势性漂移。安装位置加负载1kg跑300次确认精度不掉出±0.5mm。连续运行2小时监控三轴温升目标不超过40K。手动上推下压动平台确认伺服刚性合适、没有异常啸叫和机械抖动。这些测试数据我全部打包進了交付文档。设备报修时拿出当时的基线数据对比能很快判断是机械磨损还是电气参数漂移。6. 代码的进一步工程化从能跑到好用6.1 模块化封装与可配置化第一版代码把所有变量都放在主程序里调试起来倒是直观后期维护很崩溃。交付前我做了重构核心模块全部封装成带结构化参数的FBFB_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这种机构代码里一定要把坐标系约定、单位约定、角度方向约定用注释放在文件最前面。开发的三个星期里我自己就因为这些约定不清改过两版逆解代码更别提后来接手项目的同事了。先钉死约定再写逻辑后面会省下大量不必要的返工时间。本文还有配套的精品资源点击获取