机器人编程完整教程
本文档是一份完整的机器人编程教程, 采用"菜鸟教程"风格编写 — 每个概念包含: 原理说明 + 语法定义 + 代码示例 + ASCII 示意图 + 常见错误. 适合自动化工程师从零入门 Darra 机器人编程.
目录
1. 机器人编程基础
1.1 机器人运动学简介
机器人运动学描述机器人关节与末端 (TCP) 之间的位置和姿态关系, 不涉及力/力矩. Darra 支持以下主流机器人类型:
| 机器人类型 | 典型轴数 | 运动学模型 | 典型应用 |
|---|---|---|---|
| 6 轴关节臂 | 6 | 串联 (球腕/偏置腕) | 搬运、焊接、装配、喷涂 |
| SCARA | 4 | 串联 (RRPR) | 高速取放、锁螺丝、电子装配 |
| Delta (蜘蛛手) | 3~4 | 并联 (平行四边形) | 高速分拣、食品包装、医药 |
| 协作机器人 | 6~7 | 串联 (偏置腕+力控) | 人机协作、装配、质检 |
| 龙门/桁架 | 3 | 直角坐标 (线性) | 大范围搬运、CNC 上下料 |
| Stewart 平台 | 6 | 并联 (6 支链) | 运动模拟、精密定位 |
示意图: 6 轴工业臂关节命名
J2 (肩)
┌───┐
J1 │ │
(转台)│ │──── J3 (肘)
│ │ │
│ │ ┌─┴─┐
│ │ │ │ J4 (腕 1)
│ │ │ │
│ │ └─┬─┘
│ │ │ J5 (腕 2)
│ │ ┌─┴─┐
│ │ │ │ J6 (腕 3 — 法兰)
│ │ │ │
└───┘ └───┘
TCP (工具中心点)
SCARA 关节命名
J1 (肩转) ──→ J2 (肘转)
│
│ J3 (Z 轴升降)
│
J4 (腕转)
│
TCP
Delta 结构示意
静平台 (固定)
/ | \
臂1 / 臂2| 臂3\ 主动臂 (L1)
/ | \
○-----○-----○ 动平台 (浮动)
|
TCP
3 组平行四边形从动臂 (L2)
1.2 坐标系体系
Darra 使用4 层坐标系来描述机器人的位置和姿态:
┌─────────────────────────┐
│ 世界坐标系 (World) │
│ 全局固定, 通常为机器人基座│
└──────────┬──────────────┘
│
┌──────────▼──────────────┐
│ 基坐标系 (Base) │
│ 机器人安装底座, 由厂商定义│
└──────────┬──────────────┘
│
┌──────────▼──────────────┐
│ 工具坐标系 (Tool/TCP) │
│ 末端执行器中心点, 用户标定│
└──────────┬──────────────┘
│
┌──────────▼──────────────┐
│ 用户/工件坐标系 (User) │
│ 工件/工作台局部坐标 │
└─────────────────────────┘
世界坐标系 (World)
- 全局固定坐标系, 整个工作空间的绝对参考
- 通常设定在机器人基座安装面中心
- 所有机器人、传感器、工装都统一到世界坐标系
基坐标系 (Base)
- 机器人 J1 轴中心, Z 轴向上 (重力方向)
- 由机器人厂商在 DH 参数中定义, 用户通常不需要修改
工具坐标系 (Tool / TCP)
- TCP (Tool Center Point) = 工具中心点
- 例如: 焊丝尖端、夹爪中心、涂胶嘴出口
- 需要用户标定, 详见 坐标系变换章节
用户坐标系 (User / 工件坐标系)
- 定义在工件或工作台上
- 例如: 焊接工件的某个角点, 码垛托盘的一个角
- 便于编程时以工件为参考, 而不是以机器人为参考
1.3 位姿表示: XYZ + RPY
机器人末端位姿用 6 个自由度 描述: 3 个位置 + 3 个姿态.
位姿 = (X, Y, Z, A, B, C)
X, Y, Z = TCP 在参考坐标系中的位置 (mm)
A = 绕 Z 轴旋转 (Roll, 滚转)
B = 绕 Y 轴旋转 (Pitch, 俯仰)
C = 绕 X 轴旋转 (Yaw, 偏航)
RPY 旋转顺序: 先绕 Z 转 A, 再绕新 Y 转 B, 再绕新 X 转 C.
初始姿态 (TCP 朝下):
┌──┐
│ │
│ │ Z 轴向下
└──┘
绕 Z 转 A (Roll, 工具旋转):
┌──┐ ┌──┐
│ │ → │╱ │ 工具绕自身轴线旋转
└──┘ └──┘
绕 Y 转 B (Pitch, 工具俯仰):
┌──┐ ┌──┐
│ │ → │ ╲ 工具向前/向后倾斜
└──┘ └──┘
绕 X 转 C (Yaw, 工具摆动):
┌──┐ ┌──┐
│ │ → │ ╲│ 工具向左/向右摆动
└──┘ └──┘
四元数表示 (用于插值, 避免万向锁):
位姿 = (X, Y, Z, Qx, Qy, Qz, Qw)
Qx, Qy, Qz, Qw = 单位四元数 (|Q| = 1)
RPY → 四元数 由系统自动转换, 用户一般不需要手算
SCL 中的位姿结构:
// 点位结构 (ROBOT_POSE)
TYPE ROBOT_POSE :
STRUCT
X : LREAL; // mm
Y : LREAL; // mm
Z : LREAL; // mm
A : LREAL; // 绕 Z 旋转 (度)
B : LREAL; // 绕 Y 旋转 (度)
C : LREAL; // 绕 X 旋转 (度)
Config : ROBOT_CONFIG; // 关节构型 (翻转/非翻转等)
END_STRUCT
END_TYPE
// 关节位置结构 (JOINT_POSITION)
TYPE JOINT_POSITION :
STRUCT
J1 : LREAL; // 度
J2 : LREAL; // 度
J3 : LREAL; // 度
J4 : LREAL; // 度
J5 : LREAL; // 度
J6 : LREAL; // 度
END_STRUCT
END_TYPE
1.4 机器人编程语言概览
Darra 机器人编程使用 SCL (Structured Control Language) 扩展, 在标准 IEC 61131-3 SCL 基础上增加了机器人运动指令.
编程方式有两种:
| 方式 | 语法 | 适用场景 |
|---|---|---|
| 简化指令 | MOVJ, MOVL, MOVC | 快速编程, 示教点位 |
| PLCopen 功能块 | MC_MoveJointAbsolute 等 | 需要完整状态机控制 |
简化指令的典型程序:
PROGRAM PickAndPlace
VAR
CycleCount : INT := 0;
END_VAR
// === 主循环 ===
WHILE TRUE DO
// 回到安全位置
HOME Speed := 50;
// 快速到取料上方 (关节运动, 路径不确定)
MOVJ Pick_Above, Speed := 80, Zone := 20;
// 直线下降到取料位 (路径精确)
MOVL Pick, Speed := 15;
// 控制夹爪
DO[1] := TRUE; // 夹爪关闭
DELAY 0.3; // 等待夹紧
// 抬起
MOVL Pick_Above, Speed := 30;
// 快速到放料上方
MOVJ Place_Above, Speed := 80, Zone := 20;
// 直线下降到放料位
MOVL Place, Speed := 15;
// 松开夹爪
DO[1] := FALSE;
DELAY 0.3;
// 抬起
MOVL Place_Above, Speed := 30;
CycleCount := CycleCount + 1;
// 等待启动信号
WAIT DI[1] = TRUE;
END_WHILE;
END_PROGRAM
PLCopen 功能块风格 (等价功能):
PROGRAM PickAndPlaceFB
VAR
Power : MC_GroupEnable;
Home : MC_GroupHome;
MoveJPick : MC_MoveJointAbsolute;
MoveLPick : MC_MoveLinearAbsolute;
MoveJPlace: MC_MoveJointAbsolute;
MoveLPlace: MC_MoveLinearAbsolute;
step : INT := 0;
bDone : BOOL;
END_VAR
// 始终使能
Power(Robot := Robot1, Enable := TRUE);
CASE step OF
0: // 回 Home
Home(Robot := Robot1, Execute := TRUE);
IF Home.Done THEN
Home(Execute := FALSE);
step := 10;
END_IF;
10: // 快速到取料上方
MoveJPick(Robot := Robot1, Execute := TRUE,
TargetPosition := pPick_Above, Velocity := 200);
IF MoveJPick.Done THEN
MoveJPick(Execute := FALSE);
step := 20;
END_IF;
20: // 直线下降到取料位
MoveLPick(Robot := Robot1, Execute := TRUE,
TargetPosition := pPick, Velocity := 50);
IF MoveLPick.Done THEN
MoveLPick(Execute := FALSE);
DO[1] := TRUE;
DELAY 0.3;
step := 30;
END_IF;
// ... 后续步骤类似
END_CASE;
END_PROGRAM
1.5 常见错误
| 错误 | 后果 | 正确做法 |
|---|---|---|
| 使用 MOVJ 接近工件 | 可能碰撞 | 接近工件用 MOVL, 空行程用 MOVJ |
| 位姿使用 RPY 但顺序搞错 | 姿态不对 | 确认 RPY 顺序为 Z-Y-X |
| 不标定 TCP 就编程 | 实际位置与示教偏差大 | 先标定 TCP 再示教 |
| 混淆 Base/World 坐标系 | 坐标全偏移 | 编程时明确指定坐标系 |
2. 轴运动
轴运动是机器人控制的基础 — 控制伺服电机上电、回零、停止等基本操作. 在 Darra PLC 中, 轴通过 PLCopen MC 功能块控制.
2.1 MC_Power: 伺服上电/使能
在使用任何轴或机器人之前, 必须先将伺服上电使能. 这是安全前提 — 伺服未使能时, 电机处于自由状态 (无保持力矩).
语法:
// 功能块实例化
VAR
PowerAxis : MC_Power; // 单轴使能
PowerRobot: MC_GroupEnable; // 整台机器人使能
END_VAR
// 调用
PowerAxis(
Axis := Axis1, // 轴引用
Enable := TRUE, // TRUE = 上电, FALSE = 断电
Status => bStatus, // 输出: 上电状态
Busy => bBusy // 输出: 正在执行
);
PowerRobot(
Robot := Robot1, // 机器人引用
Enable := TRUE
);
参数表:
| 参数 | 类型 | 说明 |
|---|---|---|
| Axis/Robot | 轴/机器人引用 | 要控制的轴或机器人 |
| Enable | BOOL | TRUE=上电使能, FALSE=断电去使能 |
| Status | BOOL (输出) | TRUE=使能成功, 轴就绪 |
| Busy | BOOL (输出) | TRUE=正在执行 |
| Error | BOOL (输出) | TRUE=错误 |
| ErrorID | UINT (输出) | 错误码 |
时序图:
Enable ──┐ ┌────────────────────────────
│ │
└──────┘
┌──────────────────────┐
Status │ │
└──────────────────────┘
^ 使能成功!
Busy ─────┘└───────────────────
完整示例: 机器人上电流程:
PROGRAM RobotPowerUp
VAR
Power : MC_GroupEnable;
ReadState : MC_GroupReadStatus;
step : INT := 0;
bPowerOK : BOOL := FALSE;
END_VAR
CASE step OF
0: // 发上电指令
Power(Robot := Robot1, Enable := TRUE);
step := 1;
1: // 等待使能完成
IF Power.Status THEN
bPowerOK := TRUE;
step := 2;
ELSIF Power.Error THEN
Log.Error('Power failed: %d', Power.ErrorID);
step := 99; // 错误处理
END_IF;
2: // 上电完成, 可以开始运动
ReadState(Robot := Robot1);
IF ReadState.State = MC_GROUP_STANDSTILL THEN
// 就绪!
Log.Info('Robot1 powered and ready');
END_IF;
99: // 错误处理
DELAY 5.0;
Power(Enable := FALSE);
DELAY 1.0;
step := 0; // 重试
END_CASE;
END_PROGRAM
常见错误:
| 现象 | 原因 | 解决 |
|---|---|---|
| Status 一直 FALSE | 急停被按下 / 安全门打开 | 检查安全回路 |
| Error 报驱动错误 | 伺服驱动器通讯中断 | 检查 EtherCAT 总线状态 |
| 上电后伺服剧烈抖动 | 伺服参数未调好 | 检查增益参数 |
2.2 MC_Home: 回原点 (10 种回零方式)
回原点 (Homing) 让机器人各关节找到机械零位. 这是首次开机或编码器丢失后必须执行的操作.
语法:
VAR
HomeAxis : MC_Home; // 单轴回零
HomeRobot: MC_GroupHome; // 整机回零
END_VAR
HomeAxis(
Axis := Axis1,
Execute := TRUE, // 上升沿触发
HomeMode := HOME_REF_DIR, // 回零模式
Position := 0.0 // 回零后设定位置
);
HomeRobot(
Robot := Robot1,
Execute := TRUE,
HomeMode := HOME_SET_REF
);
10 种回零模式:
Darra 支持 PLCopen Part 5 定义的 10 种回零模式:
| 模式 | 缩写 | 原理 | 适用场景 |
|---|---|---|---|
| 1. 参考点限位+零位脉冲 | HOME_REF_LIMIT | 向限位方向找参考点开关, 再找零位脉冲 | 增量编码器, 有参考点开关 |
| 2. 参考点限位反向+零位脉冲 | HOME_REF_LIMIT_REV | 先找限位, 反向离开再找零位脉冲 | 参考点在限位开关中间 |
| 3. 零位脉冲(无参考点) | HOME_INDEX_ONLY | 直接找零位脉冲信号 | 有零位脉冲但无参考点 |
| 4. 当前位置设零 | HOME_SET_REF | 当前位姿直接设为原点 | 绝对值编码器, 首次快速设定 |
| 5. 硬限位+零位脉冲 | HOME_HARD_LIMIT | 撞硬限位后反向找零位脉冲 | 无软限位保护 |
| 6. 硬限位反向+零位脉冲 | HOME_HARD_LIMIT_REV | 同 5 但反向 | 硬限位在边缘 |
| 7. 参考点开关+零位脉冲(正向) | HOME_REF_FWD | 正向移动找参考点 | 标准配置 |
| 8. 参考点开关+零位脉冲(反向) | HOME_REF_REV | 反向移动找参考点 | 标准配置 |
| 9. 外部触发回零 | HOME_EXT_TRIGGER | 外部信号触发时设零 | 有外部定位传感器 |
| 10. 绝对值编码器直接读 | HOME_ABS_ENCODER | 直接读编码器多圈值 | 绝对值编码器, 推荐 |
回零过程示意 (模式 1: 参考点限位+零位脉冲):
←←←←←←← 运动方向
限位开关: ████████████░░░░░░░░░░░░
↑ 参考点触发
零位脉冲: ░░░░░░░░░░|░░░░░░░░░░░░░
↑ 最近零位脉冲
最终原点: ★
JOG 到参考点 → 减速停止 → 反向慢速 → 找零位脉冲 → 设零
SCL 完整示例:
PROGRAM HomingProcedure
VAR
HomeCmd : MC_GroupHome;
Power : MC_GroupEnable;
step : INT := 0;
retry : INT := 0;
END_VAR
// 先上电
Power(Robot := Robot1, Enable := TRUE);
CASE step OF
0: // 等待上电
IF Power.Status THEN
step := 10;
END_IF;
10: // 执行回零 — 绝对值编码器直接读
HomeCmd(
Robot := Robot1,
Execute := TRUE,
HomeMode := HOME_ABS_ENCODER // 绝对值编码器
);
step := 20;
20: // 等待完成
IF HomeCmd.Done THEN
HomeCmd(Execute := FALSE);
Log.Info('Homing completed');
step := 30;
ELSIF HomeCmd.Error THEN
retry := retry + 1;
IF retry < 3 THEN
Log.Warn('Homing failed, retry %d/3', retry);
HomeCmd(Execute := FALSE);
DELAY 1.0;
step := 10; // 重试
ELSE
Log.Error('Homing failed after 3 retries');
step := 99;
END_IF;
END_IF;
30: // 就绪
// 可以开始运动
;
99: // 错误状态
// 报人工处理
;
END_CASE;
END_PROGRAM
常见错误:
| 现象 | 原因 | 解决 |
|---|---|---|
| 回零方向不对 | HomeMode 与机械结构不匹配 | 检查限位方向, 换 HomeMode |
| 回零后位置偏移 | 零位脉冲受干扰 | 检查编码器屏蔽 |
| 回零超时 | 找了一直没触发 | 检查限位开关和参考点信号 |
| 绝对值编码器直接报错 | 编码器电池没电 | 换电池 (CR2032) |
2.3 MC_Stop: 停止轴
紧急或正常停止所有运动. MC_Stop 执行减速停止, 而不是瞬间断电.
语法:
VAR
StopCmd : MC_Stop; // 单轴停止
GroupStop: MC_GroupStop; // 整组停止
END_VAR
StopCmd(
Axis := Axis1,
Execute := TRUE, // 上升沿触发
Deceleration := 500.0, // 减速度
Jerk := 5000.0
);
GroupStop(
Robot := Robot1,
Execute := TRUE
);
参数表:
| 参数 | 类型 | 单位 | 默认 | 说明 |
|---|---|---|---|---|
| Execute | BOOL | - | - | 上升沿触发 |
| Deceleration | LREAL | mm/s² | 当前减速度 | 停止减速度 |
| Jerk | LREAL | mm/s³ | 5000 | 停止跃度 |
| Done | BOOL | - | - | 停止完成 |
| Error | BOOL | - | - | 错误 |
停止过程时序:
速度
│
│ ┌───────── 运动段
│ │
│ │ Execute=TRUE (停止触发)
│ │ ↓
│ │ ┌──╲
│ │ │ ╲____ 减速停止
│ │ │ ╲___ Done=TRUE
│ └────┴───────────→ 时间
与 MC_Halt 的区别:
| 特性 | MC_Stop | MC_Halt |
|---|---|---|
| 停止后状态 | ErrorStop (需 Reset) | Standstill (可直接再运动) |
| 适用场景 | 异常停止 | 正常暂停 |
| 复位需要 | MC_Reset | 不需要 |
完整示例: 安全停止:
PROGRAM SafeStop
VAR
StopRobot : MC_GroupStop;
ResetCmd : MC_GroupReset;
bEStop : BOOL;
END_VAR
// 检测急停或安全门信号
bEStop := NOT DI[1] OR NOT DI[2]; // DI[1]=急停, DI[2]=安全门
IF bEStop THEN
// 紧急停止
StopRobot(Robot := Robot1, Execute := TRUE, Deceleration := 1000);
Log.Warn('Emergency stop triggered');
// 等待停止完成
IF StopRobot.Done THEN
// 此时机器人状态 = ErrorStop
// 复位前不能运动
;
END_IF;
// 等待急停解除 + 手动复位按钮
WAIT DI[1] = TRUE AND DI[2] = TRUE AND DI[3] = TRUE; // DI[3]=复位
// 复位
ResetCmd(Robot := Robot1, Execute := TRUE);
IF ResetCmd.Done THEN
Log.Info('Robot reset, ready to resume');
END_IF;
END_IF;
END_PROGRAM
2.4 MC_Halt: 暂停轴 (减速停止)
正常暂停当前运动, 停止后保持在 Standstill 状态, 不需要 Reset 即可继续运动.
语法:
VAR
HaltCmd : MC_Halt; // 单轴暂停
END_VAR
HaltCmd(
Axis := Axis1,
Execute := TRUE, // 上升沿触发暂停
Deceleration := 200.0
);
使用场景: 暂停程序等待 IO 信号, 然后继续下一段运动.
// 运动到中间等待点
MoveL(Target := pWaitPos, Velocity := 100);
// 暂停 — 等待工件到位
HaltCmd(Axis := Robot1, Execute := TRUE);
// 等待外部信号
WAIT DI[5] = TRUE;
// 继续运动 (不需要 Reset)
MoveL(Target := pNextPos, Velocity := 100);
2.5 MC_Reset: 清除错误
当轴进入 ErrorStop 状态后, 必须用 MC_Reset 清除错误, 恢复到 Standstill 状态.
语法:
VAR
ResetCmd : MC_Reset;
END_VAR
ResetCmd(
Axis := Axis1,
Execute := TRUE
);
// Reset.Done = TRUE → 错误清除, 轴回到 Standstill
状态机转换:
MC_Stop
Standstill ──────────────→ ErrorStop
↑ │
│ MC_Reset
└──────────────────────────┘
错误清除后不能自动恢复运动,
必须重新下发运动指令
2.6 轴运动参数表汇总
| 参数 | MC_Power | MC_Home | MC_Stop | MC_Halt | MC_Reset |
|---|---|---|---|---|---|
| Axis/Robot | ✓ | ✓ | ✓ | ✓ | ✓ |
| Execute | ✓ | ✓ | ✓ | ✓ | ✓ |
| Enable | ✓ | - | - | - | - |
| HomeMode | - | ✓ | - | - | - |
| Position | - | ✓ | - | - | - |
| Deceleration | - | - | ✓ | ✓ | - |
| Jerk | - | - | ✓ | ✓ | - |
| Done | ✓ | ✓ | ✓ | ✓ | ✓ |
| Busy | ✓ | ✓ | ✓ | ✓ | ✓ |
| Error | ✓ | ✓ | ✓ | ✓ | ✓ |
| ErrorID | ✓ | ✓ | ✓ | ✓ | ✓ |
| Status | ✓ | - | - | - | - |
3. 点位运动 (Point-to-Point)
点位运动控制机器人从当前位置运动到目标位置, 不关心中间路径. 分为绝对定位、相对定位、增量定位和速度模式.
3.1 MC_MoveAbsolute: 绝对定位
移动到指定绝对位置. 这是最常用的运动指令.
语法 (简化指令):
// 关节空间绝对运动 (MOVJ)
MOVJ 目标点位名, Speed := 速度%, Zone := 平滑半径;
// 笛卡尔空间绝对运动 (MOVL)
MOVL 目标点位名, Speed := mm/s, Zone := 平滑半径;
语法 (PLCopen 功能块):
VAR
MoveAbs : MC_MoveAbsolute; // 单轴绝对定位
MoveJAbs: MC_MoveJointAbsolute; // 机器人关节绝对
MoveLAbs: MC_MoveLinearAbsolute; // 机器人直线绝对
END_VAR
// 单轴
MoveAbs(
Axis := Axis1,
Execute := TRUE,
Position := 100.0, // 目标位置 (mm 或 度)
Velocity := 50.0, // 速度
Acceleration:= 200.0,
Deceleration:= 200.0
);
// 机器人关节空间
MoveJAbs(
Robot := Robot1,
Execute := TRUE,
TargetPosition := pPick, // 目标点位
Velocity := 200.0, // mm/s
Zone := 0.0
);
// 机器人笛卡尔空间 (直线)
MoveLAbs(
Robot := Robot1,
Execute := TRUE,
TargetPosition := pPick,
Velocity := 50.0
);
参数表:
| 参数 | 类型 | 单位 | 范围 | 默认 | 说明 |
|---|---|---|---|---|---|
| TargetPosition | ROBOT_POSE | - | - | - | 目标位姿 |
| Velocity | LREAL | mm/s 或 % | 0.1..100 | 50 | 运动速度 |
| Acceleration | LREAL | mm/s² | 1..10000 | 500 | 加速度 |
| Deceleration | LREAL | mm/s² | 1..10000 | 500 | 减速度 |
| Jerk | LREAL | mm/s³ | 100..100000 | 5000 | 跃度 |
| Zone | LREAL | mm | 0..100 | 0 | 平滑半径 |
| BufferMode | ENUM | - | - | BUFFER | 衔接模式 |
| Done | BOOL | - | - | - | 到达完成 |
| Busy | BOOL | - | - | - | 运动中 |
| CommandAborted | BOOL | - | - | - | 被中止 |
运动轨迹示意:
MOVJ (关节空间):
起点 ────╮
╰──╮
╰──╮ TCP 路径是弧线 (不可控)
╰──╮
╰── 终点
关节 J1: ──────────→ (同时启动, 同时到达)
关节 J2: ──────────→
关节 J3: ──────────→
MOVL (笛卡尔空间):
起点 ──────────────→ 终点
TCP 路径是精确直线
姿态从起点 Slerp 到终点
完整示例: 多点搬运:
PROGRAM MultiPointPick
VAR
MoveJ1, MoveJ2 : MC_MoveJointAbsolute;
MoveL1, MoveL2, MoveL3 : MC_MoveLinearAbsolute;
step : INT := 0;
END_VAR
CASE step OF
0: // 回 Home
MoveJ1(Robot := Robot1, Execute := TRUE,
TargetPosition := pHome, Velocity := 200);
IF MoveJ1.Done THEN
MoveJ1(Execute := FALSE);
step := 10;
END_IF;
10: // 快速到取料上方 (MOVJ, 空行程)
MoveJ2(Robot := Robot1, Execute := TRUE,
TargetPosition := pPickApproach, Velocity := 300,
Zone := 30);
IF MoveJ2.Done THEN
MoveJ2(Execute := FALSE);
step := 20;
END_IF;
20: // 直线下降到取料位 (MOVL, 工作行程)
MoveL1(Robot := Robot1, Execute := TRUE,
TargetPosition := pPick, Velocity := 50,
Zone := 0);
IF MoveL1.Done THEN
MoveL1(Execute := FALSE);
step := 30;
END_IF;
30: // 夹爪夹紧
DO[1] := TRUE;
DELAY 0.3;
step := 40;
40: // 直线抬起
MoveL2(Robot := Robot1, Execute := TRUE,
TargetPosition := pPickApproach, Velocity := 100);
IF MoveL2.Done THEN
MoveL2(Execute := FALSE);
step := 50;
END_IF;
50: // 快速到放料上方
MoveJ1(Robot := Robot1, Execute := TRUE,
TargetPosition := pPlaceApproach, Velocity := 300,
Zone := 30);
IF MoveJ1.Done THEN
MoveJ1(Execute := FALSE);
step := 60;
END_IF;
60: // 直线下降到放料位
MoveL3(Robot := Robot1, Execute := TRUE,
TargetPosition := pPlace, Velocity := 50);
IF MoveL3.Done THEN
MoveL3(Execute := FALSE);
step := 70;
END_IF;
70: // 松开
DO[1] := FALSE;
DELAY 0.3;
step := 80;
80: // 抬起
MoveL2(Robot := Robot1, Execute := TRUE,
TargetPosition := pPlaceApproach, Velocity := 100);
IF MoveL2.Done THEN
step := 0; // 循环
END_IF;
END_CASE;
END_PROGRAM
3.2 MC_MoveRelative: 相对定位
从当前位置移动指定距离. 不关心起点坐标, 只关心偏移量.
语法:
VAR
MoveRel : MC_MoveRelative;
END_VAR
MoveRel(
Axis := Axis1,
Execute := TRUE,
Distance := 50.0, // 移动距离 (mm 或 度)
Velocity := 30.0
);
示意:
当前位置 目标位置 (当前位置 + 50 mm)
│ │
└──────── 50mm ──────┘
MoveRel(Distance := 50) → 正向移动 50 mm
MoveRel(Distance := -30) → 负向移动 30 mm
使用场景: 传送带上的固定步长移动.
// 每次工件到位, 传送带前进 100 mm
WAIT DI[1] = TRUE; // 工件检测
MoveRel(Axis := Conveyor, Execute := TRUE,
Distance := 100.0, Velocity := 200.0);
3.3 MC_MoveAdditive: 增量定位
在当前运动上追加一段运动, 不打断当前运动. 与 MoveRelative 的区别: Additive 在当前运动未完成时就能叠加.
语法:
VAR
MoveAdd : MC_MoveAdditive;
END_VAR
MoveAdd(
Axis := Axis1,
Execute := TRUE,
Distance := 10.0, // 追加距离
Velocity := 50.0
);
使用场景: 在连续运动中微调位置.
// 正在执行长距离 MoveAbsolute 时
// 追加 5 mm 微调
MoveAdd(Axis := Axis1, Execute := TRUE, Distance := 5.0);
3.4 MC_MoveVelocity: 速度模式
以指定速度持续运动, 直到收到停止指令或触发限位. 没有固定目标位置.
语法:
VAR
MoveVel : MC_MoveVelocity;
END_VAR
// 正向持续运动
MoveVel(
Axis := Axis1,
Execute := TRUE,
Velocity := 100.0, // 速度
Direction := DIR_POSITIVE
);
// 反向
MoveVel(
Axis := Axis1,
Execute := TRUE,
Velocity := 100.0,
Direction := DIR_NEGATIVE
);
// 停止速度模式
MoveVel(Execute := FALSE);
示意:
速度
│
│ Execute=TRUE
│ ↓
│ ┌───┬────────────── Execute=FALSE
│ │ │ ↓
│ │ │ ┌──────────┬───
│ │ │ │ │
└────┴───┴──┴──────────┴───→ 时间
启动 恒速运动 减速停止
使用场景: 传送带同步、涂胶连续供料.
// 涂胶开始: 机器人开始移动, 同时开胶
MoveVel(Robot := Robot1, Execute := TRUE,
Velocity := 40.0, Direction := DIR_POSITIVE);
DO[2] := TRUE; // 开胶
// ... 涂胶完成
DO[2] := FALSE; // 停胶
MoveVel(Execute := FALSE); // 停止运动
3.5 点位运动选择指南
需要什么运动?
│
├── 到固定位置 → 用示教点位 → MoveAbsolute
│
├── 相对当前位置偏移 → MoveRelative
│
├── 在现有运动上微调 → MoveAdditive
│
└── 持续运动不停 → MoveVelocity
MOVJ vs MOVL:
├── 空行程 (不碰东西) → MOVJ (快)
└── 工作行程 (碰东西) → MOVL (路径精确)
3.6 常见错误
| 错误 | 原因 | 解决 |
|---|---|---|
| MoveAbsolute 不动 | 轴在 ErrorStop 状态 | 先 MC_Reset |
| Execute 保持 TRUE | 没有下降沿 | 完成后 Execute := FALSE |
| 关节运动走了 > 180° | 目标点位构型错误 | 检查 Turn 记录 |
| 直线运动报奇异 | 路径经过奇异点 | 插入 MoveJ 段绕开 |
4. 路径运动 (Path Motion)
路径运动控制机器人沿指定几何路径移动, 包括直线、圆弧和组合路径.
4.1 MC_MoveLinear: 直线插补 (LIN)
TCP 沿精确直线从起点到终点, 同时姿态从起点 Slerp 到终点.
语法:
// 简化指令
MOVL 目标点位, Speed := mm/s, Zone := 半径;
// PLCopen 功能块
VAR
mcL : MC_MoveLinearAbsolute;
END_VAR
mcL(
Robot := Robot1,
Execute := TRUE,
TargetPosition := pTarget, // 目标位姿
Velocity := 50.0, // mm/s
Acceleration := 500.0, // mm/s²
Deceleration := 500.0,
Jerk := 5000.0, // mm/s³
Zone := 0.0, // 0 = 精确到位
OriMode := ORI_INTERPOLATE, // 姿态插值模式
ToolId := 1,
UserFrameId := 1
);
直线插补原理:
每个插补周期 (1 ms):
1. 计算归一化参数 u = t / T_total (0 → 1)
2. 位置: pos(u) = pos_start + u * (pos_end - pos_start)
3. 姿态: ori(u) = Slerp(ori_start, ori_end, u)
4. 逆解: q(u) = IK(pos(u), ori(u))
5. 下发关节角到伺服
姿态模式:
| OriMode | 效果 | 典型应用 |
|---|---|---|
| ORI_INTERPOLATE | 起终点 Slerp 插值 | 通用 |
| ORI_KEEP_START | 保持起点姿态 | 搬运 (工具朝下不变) |
| ORI_KEEP_END | 立即跳到终点姿态 | 罕见 |
| ORI_PATH_FOLLOW | 姿态跟随运动切线 | 焊接、涂胶 |
运动轨迹对比:
MOVL:
起点 ─────────────────→ 终点
TCP 沿直线运动, 路径可预测
MOVJ (同样起终点):
起点 ───╮
╰──╮
╰──╮ TCP 走弧线, 路径不可预测
╰──╮
╰── 终点
Zone (平滑过渡) 原理:
Zone = 0 (精确停止):
段1 ──────────● 段2 ──────────●
↑ 完全停稳 ↑ 完全停稳
再启动 再启动
Zone > 0 (平滑过渡):
段1 ──────╱╲──── 段2
╱ ╲
↑ ↑
入弯点 (距离终点 = Zone) 出弯点
TCP 不精确到达端点, 在 Zone 范围内走过渡曲线
完整示例: 沿直线路径焊接:
PROGRAM LinearWeld
VAR
mcL1, mcL2, mcL3, mcL4 : MC_MoveLinearAbsolute;
mcJ1, mcJ2 : MC_MoveJointAbsolute;
step : INT := 0;
END_VAR
CASE step OF
0: // 快速到焊接起点上方
mcJ1(Robot := Robot1, Execute := TRUE,
TargetPosition := pWeldApproach, Velocity := 300);
IF mcJ1.Done THEN mcJ1(Execute := FALSE); step := 10; END_IF;
10: // 直线下降到焊接起点
mcL1(Robot := Robot1, Execute := TRUE,
TargetPosition := pWeldStart, Velocity := 50);
IF mcL1.Done THEN
mcL1(Execute := FALSE);
DO[3] := TRUE; // 起弧
DELAY 0.2;
step := 20;
END_IF;
20: // 焊接段 1 (直线)
mcL2(Robot := Robot1, Execute := TRUE,
TargetPosition := pWeldMid1, Velocity := 8.0, // 焊接速度
OriMode := ORI_PATH_FOLLOW);
IF mcL2.Done THEN mcL2(Execute := FALSE); step := 30; END_IF;
30: // 焊接段 2
mcL3(Robot := Robot1, Execute := TRUE,
TargetPosition := pWeldMid2, Velocity := 8.0,
OriMode := ORI_PATH_FOLLOW);
IF mcL3.Done THEN mcL3(Execute := FALSE); step := 40; END_IF;
40: // 焊接终点
mcL4(Robot := Robot1, Execute := TRUE,
TargetPosition := pWeldEnd, Velocity := 8.0,
OriMode := ORI_PATH_FOLLOW, Zone := 0);
IF mcL4.Done THEN
mcL4(Execute := FALSE);
DO[3] := FALSE; // 收弧
DELAY 0.3;
step := 50;
END_IF;
50: // 退回
mcJ2(Robot := Robot1, Execute := TRUE,
TargetPosition := pWeldApproach, Velocity := 200);
IF mcJ2.Done THEN mcJ2(Execute := FALSE); step := 0; END_IF;
END_CASE;
END_PROGRAM
4.2 MC_MoveCircular: 圆弧插补 (CIRC)
TCP 沿精确圆弧运动. 需要 3 个点定义圆弧: 起点 (当前位置) + 辅助点 + 终点.
语法:
// 简化指令
MOVC 辅助点, 终点, Speed := mm/s;
// PLCopen 功能块
VAR
mcC : MC_MoveCircularAbsolute;
END_VAR
mcC(
Robot := Robot1,
Execute := TRUE,
PathMode := CIRC_BORDER, // CIRC_BORDER 或 CIRC_CENTER
AuxPoint := pArcMid, // 辅助点 (圆弧上的中间点)
EndPoint := pArcEnd, // 终点
Velocity := 50.0,
OriMode := ORI_PATH_FOLLOW
);
两种圆弧定义方式:
CIRC_BORDER (三点定弧):
P1 (起点) ────╮
╲
╲
P2 (辅助点) ──●─── P3 (终点)
由 P1-P2-P3 三点确定唯一圆弧
CIRC_CENTER (圆心+终点):
P1 (起点) ─────
│
O (圆心)
│
└──── P3 (终点)
已知圆心, 起终点到圆心等距
全圆实现 (两个半圆):
两段 MoveC 合成全圆:
段 1: P1 → P2 → P3
段 2: P3 → P4 → P1
P2 (上顶点)
▲
│
P1 ───────●─────── P3
(起点) │ (对点)
│
P4 (下顶点)
完整示例: 圆弧焊接:
PROGRAM CircleWeld
VAR
mcJ : MC_MoveJointAbsolute;
mcL : MC_MoveLinearAbsolute;
mcC1, mcC2 : MC_MoveCircularAbsolute;
step : INT := 0;
END_VAR
CASE step OF
0: // 接近焊接起点
mcJ(Robot := Robot1, Execute := TRUE,
TargetPosition := pCircleApproach, Velocity := 200);
IF mcJ.Done THEN mcJ(Execute := FALSE); step := 10; END_IF;
10: // 下降到起点
mcL(Robot := Robot1, Execute := TRUE,
TargetPosition := pCircleStart, Velocity := 30);
IF mcL.Done THEN
mcL(Execute := FALSE);
DO[3] := TRUE; // 起弧
DELAY 0.2;
step := 20;
END_IF;
20: // 前半圆
mcC1(Robot := Robot1, Execute := TRUE,
AuxPoint := pCircleMid1, EndPoint := pCircleOpposite,
Velocity := 8.0, OriMode := ORI_PATH_FOLLOW, Zone := 2.0);
IF mcC1.Done THEN mcC1(Execute := FALSE); step := 30; END_IF;
30: // 后半圆
mcC2(Robot := Robot1, Execute := TRUE,
AuxPoint := pCircleMid2, EndPoint := pCircleEnd,
Velocity := 8.0, OriMode := ORI_PATH_FOLLOW, Zone := 0);
IF mcC2.Done THEN
mcC2(Execute := FALSE);
DO[3] := FALSE; // 收弧
DELAY 0.3;
step := 40;
END_IF;
40: // 退回
mcL(Robot := Robot1, Execute := TRUE,
TargetPosition := pCircleApproach, Velocity := 50);
IF mcL.Done THEN step := 0; END_IF;
END_CASE;
END_PROGRAM
4.3 MC_MovePath: 路径插补 (组合路径)
由多段 MOVJ + MOVL + MOVC + SPLINE 组成的连续路径, 段间通过 Zone 平滑过渡.
语法:
VAR
PathCmd : MC_MovePath;
segs : ARRAY[1..10] OF PATH_SEGMENT;
segCnt : INT;
END_VAR
// 定义路径段
segs[1].Type := PATH_MOVJ;
segs[1].Target := pApproach;
segs[1].Velocity := 200;
segs[1].Zone := 30;
segs[2].Type := PATH_MOVL;
segs[2].Target := pWork;
segs[2].Velocity := 50;
segs[2].Zone := 5;
segs[3].Type := PATH_MOVC;
segs[3].AuxPoint := pArcMid;
segs[3].Target := pArcEnd;
segs[3].Velocity := 30;
segs[3].Zone := 0;
segCnt := 3;
// 执行路径
PathCmd(
Robot := Robot1,
Execute := TRUE,
Segments := ADR(segs),
SegCount := segCnt,
BufferMode := mcBlendingHigh // 高平滑度过渡
);
路径段类型:
| 类型 | 说明 | 参数 |
|---|---|---|
| PATH_MOVJ | 关节运动 | Target, Velocity, Zone |
| PATH_MOVL | 直线运动 | Target, Velocity, Zone, OriMode |
| PATH_MOVC | 圆弧运动 | AuxPoint, Target, Velocity, Zone |
| PATH_SPLINE | 样条 | Waypoints[], Tension, Velocity |
示意: 组合路径:
MOVJ (空行程)
Home ───────────────→ pApproach
│ Zone=30
│
MOVL ↓ Zone=5
pPick
│
MOVC ↓
pArcMid ──●── pArcEnd
│ Zone=0
│
MOVL ↓
pPlace
4.4 连续路径与精确停止 (Blending)
BufferMode 决定相邻运动段之间的衔接方式:
| 模式 | 行为 | 效果 |
|---|---|---|
| mcAborting | 立即中断当前段, 执行新段 | 速度跳变, 机械冲击 |
| mcBuffered | 等当前段完成再启动下一段 | 段间停顿 |
| mcBlendingLow | 低速平滑过渡 (小 Zone) | 轻微平滑 |
| mcBlendingMedium | 中速平滑过渡 | 推荐 |
| mcBlendingHigh | 高速平滑过渡 (大 Zone) | 高节拍, 偏离大 |
| mcBlendingPrevious | 用前一段的 Zone | 一致行为 |
| mcBlendingNext | 用下一段的 Zone | 一致行为 |
Blending 速度曲线:
无 Blending (mcBuffered):
速度
│ ┌──┐ ┌──┐
│ │ │ │ │
│ │ │ │ │
└───┴──┴──────┴──┴──→ 时间
停顿! 停顿!
有 Blending (mcBlendingHigh):
速度
│ ┌──────────────┐
│ │ ╱╲ ╱╲ │
│ │ ╱ ╲ ╱ ╲ │
└───┴─┴────┴─────┴──→ 时间
平滑! 平滑!
Zone 推荐值:
| 场景 | Zone | 说明 |
|---|---|---|
| 精确取放 | 0 mm | 必须精确到位 |
| 焊接关键点 | 0 mm | 收弧点必须准 |
| 焊接中间点 | 1~3 mm | 速度平滑 |
| 涂胶连续弧 | 2~5 mm | 胶量均匀 |
| 空行程过渡 | 20~50 mm | 高节拍 |
| 快速回 Home | 50~100 mm | 最大节拍 |
4.5 路径运动参数表
| 参数 | MOVL | MOVC | MOVPath |
|---|---|---|---|
| TargetPosition | ✓ | ✓ (EndPoint) | ✓ |
| AuxPoint | - | ✓ | ✓ (MOVC 段) |
| Velocity | ✓ | ✓ | ✓ (每段独立) |
| Acceleration | ✓ | ✓ | ✓ |
| Deceleration | ✓ | ✓ | ✓ |
| Jerk | ✓ | ✓ | ✓ |
| Zone | ✓ | ✓ | ✓ |
| OriMode | ✓ | ✓ | ✓ |
| BufferMode | ✓ | ✓ | ✓ |
| ToolId | ✓ | ✓ | ✓ |
| UserFrameId | ✓ | ✓ | ✓ |
| ConfigStrict | ✓ | - | ✓ |
4.6 常见错误
| 现象 | 原因 | 解决 |
|---|---|---|
| 直线运动报奇异 | 路径经过奇异点 | 插入 MoveJ 过渡段 |
| 圆弧三点共线 | 辅助点在起点和终点连线上 | 调整辅助点位置 |
| 全圆方向反了 | Direction 设置错误 | 切换 CW/CCW |
| Zone 过渡不平滑 | 相邻段速度差太大 | 保持段间速度连续 |
| 焊接路径抖动 | Zone=0 导致段间停顿 | 中间段加 Zone=2~5 |
5. 高级运动功能
高级运动功能实现多轴同步, 包括电子齿轮、电子凸轮、相位偏移和力矩控制.
5.1 MC_GearIn: 电子齿轮
从轴跟随主轴运动, 保持固定速比. 类似机械齿轮箱, 但完全是软件的.
语法:
VAR
GearIn : MC_GearIn;
GearOut : MC_GearOut;
END_VAR
// 使能电子齿轮
GearIn(
SlaveAxis := Axis2, // 从轴
MasterAxis := Axis1, // 主轴
Execute := TRUE,
RatioNumerator := 2, // 分子
RatioDenominator := 1, // 分母
Acceleration := 1000.0,
Deceleration := 1000.0
);
// 从轴速度 = 主轴速度 × 2/1
// 退出电子齿轮
GearOut(
SlaveAxis := Axis2,
Execute := TRUE
);
示意:
主轴速度 ──┬──┬──┬──┬──┬──┬──
│ │ │ │ │ │
从轴速度 ──┴──┴──┴──┴──┴──┴── (Ratio = 1:1)
│ │ │ │ │ │
从轴速度 ──┬──┬──┬──┬──┬──┬── (Ratio = 2:1, 两倍速)
参数表:
| 参数 | 类型 | 说明 |
|---|---|---|
| SlaveAxis | 轴引用 | 从轴 (被驱动的轴) |
| MasterAxis | 轴引用 | 主轴 (参考轴) |
| RatioNumerator | UDINT | 速比分子 |
| RatioDenominator | UDINT | 速比分母 |
| Acceleration | LREAL | 啮合加速度 |
| Deceleration | LREAL | 啮合减速度 |
| MasterOffset | LREAL | 主轴偏移 |
| SlaveOffset | LREAL | 从轴偏移 |
使用场景:
- 传送带同步: 从轴与主轴速度一致
- 双轴同步: 两根轴以固定速比运行
- 龙门同步: 龙门架两侧保持同步
// 传送带同步抓取
// 主轴 = 传送带编码器, 从轴 = 抓取机构
GearIn(
SlaveAxis := PickerAxis,
MasterAxis := ConveyorEncoder,
Execute := TRUE,
RatioNumerator := 1,
RatioDenominator := 1, // 1:1 跟随
Acceleration := 500.0
);
5.2 MC_GearOut: 退出电子齿轮
解除从轴与主轴的同步关系.
语法:
GearOut(
SlaveAxis := Axis2,
Execute := TRUE,
Deceleration := 500.0
);
注意: 退出齿轮后, 从轴不会自动停止. 需要显式下发 MC_Stop 或 MC_Halt.
5.3 MC_CamIn: 电子凸轮
从轴跟随主轴运动, 但不是固定速比, 而是遵循一条凸轮曲线 (非线性关系). 适用于需要精确位置同步的场景.
语法:
VAR
CamIn : MC_CamIn;
CamOut : MC_CamOut;
CamTable : ARRAY[1..100] OF CAM_POINT;
camID : UINT;
END_VAR
// 定义凸轮表 (主轴位置 → 从轴位置)
// 每段定义: 主轴起始位置, 主轴结束位置, 从轴起始位置, 从轴结束位置
CamTable[1] := (MasterStart := 0, MasterEnd := 100,
SlaveStart := 0, SlaveEnd := 50);
CamTable[2] := (MasterStart := 100, MasterEnd := 200,
SlaveStart := 50, SlaveEnd := 100);
CamTable[3] := (MasterStart := 200, MasterEnd := 300,
SlaveStart := 100, SlaveEnd := 0);
// 注册凸轮表
camID := RegisterCamTable(CamTable, PointCount := 3);
// 使能电子凸轮
CamIn(
SlaveAxis := Axis2,
MasterAxis := Axis1,
Execute := TRUE,
CamTableID := camID,
MasterStart := 0,
MasterEnd := 300,
SlaveStart := 0,
SlaveEnd := 100,
MasterScaling := 1.0,
SlaveScaling := 1.0
);
凸轮曲线示意:
从轴位置
│
100 ┤ ┌────
│ ╱
50 ┤ ┌───┘
│ ╱
0 ┤─┘
└──┼────┼────┼──→ 主轴位置
0 100 200 300
段 1: 主轴 0→100, 从轴 0→50 (加速上升)
段 2: 主轴 100→200, 从轴 50→100 (匀速上升)
段 3: 主轴 200→300, 从轴 100→0 (下降回归)
使用场景:
- 飞剪: 跟随传送带速度, 在运动中完成剪切
- 包装机: 横封跟随膜运动, 封口后返回
- 凸轮轴替代: 代替机械凸轮, 柔性更高
// 包装机横封凸轮
// 主轴 = 包装膜编码器, 从轴 = 横封刀
// 凸轮曲线: 膜走 200 mm, 横封刀完成一次"跟随→封口→返回"
CamTable[1] := (0, 180, 0, 180); // 跟随膜同步前进
CamTable[2] := (180, 200, 180, 360); // 封口段
CamTable[3] := (200, 280, 360, 0); // 快速返回原点
5.4 MC_CamOut: 退出电子凸轮
解除凸轮同步, 从轴恢复独立控制.
语法:
CamOut(
SlaveAxis := Axis2,
Execute := TRUE
);
5.5 MC_Phasing: 相位偏移
在齿轮/凸轮同步中, 对从轴施加相位偏移, 微调从轴与主轴的相对位置. 不影响主轴.
语法:
VAR
Phasing : MC_Phasing;
END_VAR
// 施加相位偏移
Phasing(
Axis := Axis2,
Execute := TRUE,
PhaseShift := 10.0, // 正向偏移 10 mm (或度)
Velocity := 50.0 // 偏移速度
);
// 清除偏移
Phasing(
Axis := Axis2,
Execute := TRUE,
PhaseShift := 0.0
);
示意:
主轴: ────┬────┬────┬────┬────
│ │ │ │
从轴(无偏移): ────┬────┬────┬────
│ │ │ │
从轴(+10偏移): ─┬──┬──┬──┬──┬──
↑ 偏移 10 单位
使用场景:
- 传送带上的抓取位置微调
- 印刷套色调整
- 凸轮同步中的相位补偿
5.6 MC_TorqueControl: 力矩控制
控制伺服输出力矩而非位置. 适用于压装、拧紧、打磨等力控场景.
语法:
VAR
TorqueCtrl : MC_TorqueControl;
END_VAR
// 切换到力矩模式
TorqueCtrl(
Axis := Axis1,
Execute := TRUE,
Torque := 10.0, // 目标力矩 (N·m)
TorqueRamp := 5.0 // 力矩爬升率 (N·m/s)
);
// 恢复位置模式
TorqueCtrl(Execute := FALSE);
参数表:
| 参数 | 类型 | 单位 | 说明 |
|---|---|---|---|
| Torque | LREAL | N·m | 目标力矩 |
| TorqueRamp | LREAL | N·m/s | 力矩爬升率 |
| Direction | ENUM | - | 力矩方向 |
| PositionLimit | LREAL | mm/度 | 位置软限位 (防止飞车) |
| VelocityLimit | LREAL | mm/s | 速度软限位 |
力矩-位置混合控制:
// 先位置模式接近工件
MoveL(Target := pContact, Velocity := 20);
// 接近后切力矩模式, 以 10N 力压紧
TorqueCtrl(Axis := Axis1, Execute := TRUE, Torque := 10.0);
// 保持力矩, 直到收到信号
WAIT DI[1] = TRUE;
// 恢复位置模式
TorqueCtrl(Execute := FALSE);
使用场景:
| 场景 | 力矩用途 | 注意 |
|---|---|---|
| 压装轴承 | 监控压入力, 超限报警 | 设位置软限位防过压 |
| 拧螺丝 | 扭矩法/角度法控制 | 需要扭矩传感器 |
| 打磨抛光 | 保持恒定接触力 | 力控 + 轨迹复合 |
| 装配找正 | 柔顺找正 (力寻位) | 小力, 慢速 |
5.7 高级运动参数汇总
| 功能 | 主轴 | 从轴 | 关系 | 典型应用 |
|---|---|---|---|---|
| MC_GearIn | ✓ | ✓ | 固定速比 | 传送带同步 |
| MC_GearOut | - | ✓ | 解除 | 恢复独立 |
| MC_CamIn | ✓ | ✓ | 非线性曲线 | 飞剪、包装机 |
| MC_CamOut | - | ✓ | 解除 | 恢复独立 |
| MC_Phasing | - | ✓ | 相位偏移 | 印刷套色 |
| MC_TorqueControl | - | ✓ | 力控 | 压装、打磨 |
5.8 常见错误
| 现象 | 原因 | 解决 |
|---|---|---|
| GearIn 后从轴剧烈抖动 | 速比太大或加速度太小 | 降低加速度, 平滑啮合 |
| CamIn 后从轴位置不对 | 凸轮表定义错误 | 检查 Master/Slave 起终点 |
| Phasing 无效 | 未在同步模式 (GearIn/CamIn) | 先使能同步 |
| 力矩模式不受控 | 没有限位保护 | 设 PositionLimit 和 VelocityLimit |
6. 机器人坐标系与变换
坐标系是机器人编程的核心 — TCP 标定、工件坐标变换、手眼标定都涉及坐标系变换.
6.1 工具坐标系标定 (TCP)
TCP (Tool Center Point) = 工具中心点, 是机器人末端执行器的有效工作点.
6 点法 (最常用):
通过 6 个不同的姿态让 TCP 接触同一定位锥, 系统自动计算 TCP 相对于法兰的偏移.
步骤:
定位锥 (固定不动)
★
第 1 次: 工具尖端接触锥点 (姿态 1)
┌──┐
│ │──→★
└──┘
第 2 次: 换姿态接触同一点 (姿态 2)
┌──┐
│ │
│ │──→★
└──┘
... 共 6 次, 覆盖不同方向
系统由 6 组 (法兰位姿, TCP 接触同一点) 解出 TCP 偏移
直接输入法:
已知工具设计尺寸时, 可以直接输入 TCP 偏移:
// 直接设置 TCP 偏移
VAR
TCPData : TOOL_DATA;
END_VAR
TCPData.ToolID := 1;
TCPData.X := 0.0; // 相对法兰的 X 偏移 (mm)
TCPData.Y := 0.0; // 相对法兰的 Y 偏移
TCPData.Z := 150.0; // 相对法兰的 Z 偏移 (例如焊枪长度)
TCPData.A := 0.0; // 姿态偏移
TCPData.B := 0.0;
TCPData.C := 0.0;
TCPData.Weight := 2.5; // 工具重量 (kg)
SetToolData(Robot := Robot1, Tool := TCPData);
常见错误:
| 错误 | 后果 | 解决 |
|---|---|---|
| TCP 标定不准确 | 实际位置偏移 1~5 mm | 用 6 点法重标 |
| 6 点法姿态不够多样 | 解不稳定 | 6 次覆盖大范围方向 |
| 忘记设置工具重量 | 重力补偿不准, 精度下降 | 输入正确重量 |
| 换工具不重标 | 沿用旧 TCP, 位置全偏 | 换工具后必须重标 |
6.2 用户坐标系定义 (3 点法)
用户坐标系 (User Frame) 定义在工件上, 方便以工件为参考编程.
3 点法:
步骤:
1. 原点 (Or): 工件的一个角
2. X 方向点 (Xp): 在 X 轴正向上任意点
3. Y 方向点 (Yp): 在 XY 平面内 Y 正向侧的点
示意:
Yp (Y 方向点)
│
│
│
Or ──────── Xp (X 方向点)
原点 X 轴方向点
系统由 Or-Xp-Yp 三点确定一个平面:
- X 轴 = Or → Xp
- XY 平面 = Or, Xp, Yp 确定
- Z 轴 = X × Y (右手定则)
SCL 代码:
VAR
UserFrame : USER_FRAME_DATA;
END_VAR
// 设置用户坐标系
UserFrame.FrameID := 1;
UserFrame.Origin := (X := 500.0, Y := 200.0, Z := 100.0); // 原点
UserFrame.XAxisPoint := (X := 600.0, Y := 200.0, Z := 100.0); // X 方向
UserFrame.YAxisPoint := (X := 500.0, Y := 300.0, Z := 100.0); // Y 方向
SetUserFrame(Robot := Robot1, Frame := UserFrame);
// 编程时使用用户坐标系
MoveL(Target := pPick, UserFrameId := 1);
// pPick 现在是在工件坐标系中的坐标
6.3 基坐标系变换
基坐标系 (Base Frame) 定义机器人的安装位置和方向. 当机器人安装在导轨或旋转台上时, 需要修改基坐标系.
VAR
BaseFrame : BASE_FRAME_DATA;
END_VAR
// 机器人安装在 30° 斜台上
BaseFrame.X := 0.0;
BaseFrame.Y := 0.0;
BaseFrame.Z := 0.0;
BaseFrame.A := 0.0; // 绕 Z 转 0°
BaseFrame.B := 30.0; // 绕 Y 转 30° (前倾)
BaseFrame.C := 0.0;
SetBaseFrame(Robot := Robot1, Frame := BaseFrame);
6.4 齐次变换矩阵
坐标变换的数学基础是 4×4 齐次变换矩阵:
┌ ┐
│ R₁₁ R₁₂ R₁₃ X │
│ R₂₁ R₂₂ R₂₃ Y │
T = │ R₃₁ R₃₂ R₃₃ Z │
│ 0 0 0 1 │
└ ┘
其中 R = 3×3 旋转矩阵
X, Y, Z = 平移向量
变换链:
TCP 在世界坐标系中的位姿:
WorldT_TCP = WorldT_Base · BaseT_Flange · FlangeT_TCP
其中:
WorldT_Base = 基坐标系 (固定)
BaseT_Flange = 正运动学 (由关节角决定)
FlangeT_TCP = TCP 偏移 (固定)
SCL 中的矩阵运算:
VAR
T1, T2, TResult : MATRIX_4X4;
pos : ROBOT_POSE;
END_VAR
// 矩阵乘法
TResult := T1 * T2;
// 求逆矩阵
TResult := MatrixInverse(T1);
// 从矩阵提取位姿
pos := MatrixToPose(TResult);
// 从位姿构造矩阵
T1 := PoseToMatrix(pos);
6.5 手眼标定 (相机-机器人坐标映射)
两种安装方式:
眼在手上 (Eye-in-Hand):
相机装在机器人末端 → 相机随机器人移动
变换链: 像素 → 相机 → 法兰 → 基座 → 世界
眼在手外 (Eye-to-Hand):
相机固定安装在支架上 → 视野固定
变换链: 像素 → 相机 → 世界
9 点标定法 (2D 视觉最常用):
步骤:
1. 在相机视野中放 9 个点 (3×3 网格)
2. 用机器人示教器依次走到每个点, 记录机器人坐标 (Xr, Yr)
3. 用相机拍摄 9 个点, 记录像素坐标 (Xu, Yu)
4. 系统计算仿射变换矩阵:
┌ ┐ ┌ ┐ ┌ ┐
│ Xr │ │ a₁₁ a₁₂ a₁₃ │ │ Xu │
│ Yr │ = │ a₂₁ a₂₂ a₂₃ │ │ Yu │
│ 1 │ │ 0 0 1 │ │ 1 │
└ ┘ └ ┘ └ ┘
5. 最少 3 个点可解, 9 个点用最小二乘提高精度
示意:
相机视野:
(0,0) ──── (320,0) ──── (640,0)
│ ●1 │ ●2 │ ●3
(0,240)──(320,240)──(640,240)
│ ●4 │ ●5 │ ●6
(0,480)──(320,480)──(640,480)
│ ●7 │ ●8 │ ●9
完整 9 点标定流程:
PROGRAM NinePointCalibration
VAR
// 9 个机器人示教点
RobotPoints : ARRAY[1..9] OF ROBOT_POSE;
// 9 个相机检测点 (像素)
CameraPixels : ARRAY[1..9] OF POINT_2D;
// 标定结果
CalibMatrix : MATRIX_3X3;
Residual : LREAL;
bSuccess : BOOL;
END_VAR
// 第 1 步: 示教 9 个点 (在工件平面上均匀分布)
// 这些点需要在机器人和相机视野中同时可见
// 第 2 步: 对每个点, 记录机器人和相机位置
// 例如点 1:
RobotPoints[1] := (X := 400.0, Y := 0.0, Z := 200.0);
CameraPixels[1] := (X := 100.0, Y := 100.0);
RobotPoints[2] := (X := 500.0, Y := 0.0, Z := 200.0);
CameraPixels[2] := (X := 300.0, Y := 120.0);
// ... 共 9 个点
// 第 3 步: 计算标定矩阵
CalibMatrix := ComputeCalibration(
RobotPts := RobotPoints,
CameraPixels := CameraPixels,
PointCount := 9,
Residual => Residual,
Success => bSuccess
);
// 第 4 步: 验证
IF bSuccess AND Residual < 1.0 THEN
Log.Info('Calibration OK, residual = %.2f px', Residual);
SaveCalibration('vision_calib_20260401.json', CalibMatrix);
ELSE
Log.Error('Calibration failed, residual = %.2f px', Residual);
END_IF;
END_PROGRAM
6.6 常见错误
| 错误 | 原因 | 解决 |
|---|---|---|
| 9 点标定残差大 (> 2 px) | 点分布不均匀或标定板不平 | 重新均匀分布 9 点 |
| TCP 标定后位置偏 | 6 点法姿态不够多样 | 覆盖大范围方向 |
| 用户坐标系偏斜 | 3 点法 Y 方向点不在 XY 平面 | Y 点与 X 轴垂直 |
| 手眼标定后抓不准 | 相机内参未先标定 | 先做内参标定 |
7. 轨迹规划
轨迹规划决定机器人如何从起点运动到终点 — 速度曲线、加速度曲线、平滑度等.
7.1 PTP 轨迹 (关节空间)
PTP (Point-to-Point) 在关节空间规划轨迹, 所有关节同时启动、同时到达.
三次多项式 (最简单):
q(t) = a₀ + a₁·t + a₂·t² + a₃·t³
约束:
q(0) = q_start
q(T) = q_end
q'(0) = 0
q'(T) = 0
解:
a₀ = q_start
a₁ = 0
a₂ = 3·(q_end - q_start) / T²
a₃ = -2·(q_end - q_start) / T³
五次多项式 (加加速度可控):
q(t) = a₀ + a₁·t + a₂·t² + a₃·t³ + a₄·t⁴ + a₅·t⁵
约束: 位置、速度、加速度在起终点都给定
特性: 加速度连续, 无冲击
速度曲线对比:
梯形速度曲线:
速度
│ ┌────┐
│ │ │
│ │ │
└───┴────┴───→ 时间
加速 匀速 减速 (C⁰ 连续, 加速度有跃变)
S 曲线:
速度
│ ┌──┐
│ ╱ ╲
│╱ ╲
└──────┴───→ 时间
加速 匀速 减速 (C¹ 连续, 加加速度有跃变)
7 段 S 曲线 (Darra 默认):
速度
│ ┌──┐
│ ╱ ╲
│ ╱ ╲
│ ╱ ╲
└─────────┴───→ 时间
T1 T2 T3 T4 T5 T6 T7 (C² 连续, 机械振动最小)
7 段: 加加速→匀加速→减加速→匀速→加减速→匀减速→减减速
MOVJ 轨迹计算:
T_i = |Δq_i| / v_i (速度限制)
T_i = sqrt(2·|Δq_i| / a_i) (加速度限制)
T_move = max(T_i) (取最慢的轴为同步时间)
所有轴按 T_move 缩放速度曲线, 保证同步到达
7.2 LIN 轨迹 (笛卡尔空间)
LIN (Linear) 在笛卡尔空间规划轨迹, TCP 沿直线运动.
插补公式:
每个周期 t:
u = t / T_total (归一化, 0→1)
位置: P(u) = P_start + u · (P_end - P_start)
姿态: Q(u) = Slerp(Q_start, Q_end, u)
关节: q(u) = IK(P(u), Q(u), config)
姿态球面插值 (Slerp):
Slerp(Q₁, Q₂, u) = Q₁ · sin((1-u)·θ) / sin(θ) + Q₂ · sin(u·θ) / sin(θ)
其中 θ = arccos(Q₁ · Q₂)
7.3 CIRC 轨迹 (笛卡尔空间)
CIRC (Circular) 规划圆弧轨迹.
三点定圆:
给定 P1 (起点), P2 (辅助点), P3 (终点):
1. 计算圆心 O = 三个点所在平面的圆心
2. 计算半径 R = |P1 - O|
3. 计算起始角 θ_start = atan2(P1 - O)
4. 计算终止角 θ_end = atan2(P3 - O)
5. 确定旋转方向 (CW/CCW)
每个周期:
θ(t) = θ_start + u · (θ_end - θ_start)
P(t) = O + R · (cos(θ(t)), sin(θ(t)), 0) // 在弧平面内
再转换到 3D 空间
7.4 样条曲线 (SPLINE)
用于自由曲线路径, 从离散点生成平滑曲线.
Catmull-Rom 样条 (Darra 默认推荐):
给定 N 个路径点 P₀, P₁, ..., Pₙ₋₁:
每段 Pi 到 Pᵢ₊₁ 用三次多项式:
q(u) = 0.5 · (2·P₁ + (P₂ - P₀)·u
+ (2·P₀ - 5·P₁ + 4·P₂ - P₃)·u²
+ (-P₀ + 3·P₁ - 3·P₂ + P₃)·u³)
特性:
- 通过所有路径点
- 切线连续 (C¹)
- Tension 参数控制弯曲程度
B 样条:
不过控制点, 而是"靠近"控制点.
局部控制: 移动一个控制点只影响附近段.
适合: 大范围平滑路径, 如涂胶
NURBS:
B 样条的扩展, 每个控制点有权重.
适合: CAD 导出的高精度曲面路径
样条类型选择:
| 类型 | 过控制点 | 局部控制 | 计算量 | 应用 |
|---|---|---|---|---|
| Catmull-Rom | ✓ | 中 | 低 | 示教点直接连接 |
| B 样条 | ✗ | ✓ | 中 | 涂胶、大曲面 |
| NURBS | ✗ | ✓ | 高 | CAD 高精度 |
| Cubic Spline | ✓ | ✗ | 低 | 通用, 精确过点 |
7.5 奇异点规避
详见 奇异点处理, 此处仅列轨迹规划层面的规避策略.
3 类奇异点:
1. 肩奇异 (Shoulder):
位置: TCP 在机身正上方/下方 (半径 < 50 mm 柱区)
规避: 工件不放在机身正前方
2. 肘奇异 (Elbow):
位置: 大臂+小臂完全伸直 (J3 ≈ 0°)
规避: 工件不放在臂展边缘
3. 腕奇异 (Wrist, 最常见):
位置: J5 ≈ 0°, J4 和 J6 轴共线
规避: 姿态保持 J5 > 15° 倾斜
规避方法:
1. 路径规划时避开奇异区
2. 必须经过时, 用 MOVJ 代替 MOVL
3. 开启 DLS (阻尼最小二乘) 兜底
4. 7 轴机器人利用冗余肘角规避
7.6 轨迹参数汇总
| 参数 | PTP (MOVJ) | LIN (MOVL) | CIRC (MOVC) | SPLINE |
|---|---|---|---|---|
| 规划空间 | 关节 | 笛卡尔 | 笛卡尔 | 笛卡尔 |
| 路径形状 | 弧线 | 直线 | 圆弧 | 自由曲线 |
| 速度曲线 | S 曲线 | S 曲线 | S 曲线 | 切向恒定 |
| 姿态插值 | 自动 | Slerp | Slerp | Slerp/跟随 |
| 奇异风险 | 低 | 高 | 中 | 中 |
| 节拍 | 最快 | 中 | 中 | 中 |
7.7 常见错误
| 错误 | 原因 | 解决 |
|---|---|---|
| MOVJ 路径穿墙 | 关节空间路径不可预测 | 在仿真中验证 |
| MOVL 报奇异 | 直线路径经过奇异点 | 插入 MOVJ 段 |
| 样条曲线偏离示教点 | Tension 太小 | 调 Tension 到 0.5 |
| 速度达不到设定值 | 曲率限速或关节限速 | 降低目标速度 |
| 焊道厚薄不均 | 用了参数均匀模式 | 切换切向速度恒定 |
8. 数字孪生与仿真
数字孪生 (Digital Twin) 是物理机器人在虚拟空间的实时映射.
8.1 IDE 3D 仿真环境
Darra IDE 内置 3D 仿真环境, 支持离线编程和在线监控.
三种连接模式:
| 模式 | 实体连接 | 用途 |
|---|---|---|
| 离线仿真 | 无 | 编程阶段验证路径 |
| 在线监控 | 只读 | 运行时实时监控 |
| 在线控制 | 双向 | 调试/远程示教 |
切换模式:
// 编程阶段: 离线仿真
SetTwinMode(Mode := TWIN_SIMULATION);
// 生产时: 在线监控
SetTwinMode(Mode := TWIN_MONITORING);
// 调试: 在线控制 (需安全确认)
SetTwinMode(Mode := TWIN_COMMAND);
仿真模式 3 级:
| 级别 | 计算内容 | 节拍精度 | 用途 |
|---|---|---|---|
| 运动学仿真 | 仅 FK/IK | 粗 | 快速检查路径 |
| 轨迹仿真 | 运动学+轨迹规划 | 误差 < 5% | 默认模式 |
| HIL 仿真 | 轨迹+伺服响应 | 高 | 调试伺服参数 |
8.2 导入机器人模型 (URDF/STL)
支持的模型格式:
| 格式 | 包含内容 | 来源 |
|---|---|---|
| URDF | 运动学 + 碰撞体 + 可视化模型 | ROS 生态 |
| STL | 三角网格 (仅外观) | 通用 CAD |
| STEP | 精确 B-Rep 模型 | 机械设计 |
| Darra 内置 | 90+ 品牌机器人参数 | IDE 自带 |
导入流程:
IDE → 项目浏览器 → 右键 → 添加机器人
→ 选择 "从文件导入"
→ 选择 URDF/STL/STEP 文件
→ 系统自动:
1. 解析运动学 (URDF)
2. 生成碰撞体 (V-HACD 凸包分解)
3. 添加到 3D 视口
→ 验证: 打开 3D 视口, 机器人应正常显示
Darra 预置机器人 (90+ 品牌):
| 品牌 | 型号 | 轴数 |
|---|---|---|
| ABB | IRB 1200, IRB 2600, IRB 4600, IRB 6700 | 6 |
| KUKA | KR 6, KR 16, KR 60, KR 210 | 6 |
| Fanuc | M-10iA, M-20iA, R-2000iC | 6 |
| Yaskawa | GP7, GP12, GP50, GP100 | 6 |
| 遨博 | AUBO-i5, AUBO-i10, AUBO-i20 | 6 (协作) |
| 埃夫特 | ER3, ER7, ER12, ER20 | 6 |
| 新松 | SR6, SR10, SR20, SR50 | 6 |
| Epson | LS3, LS6, LS10, LS20 | 4 (SCARA) |
| Yamaha | YK400, YK600, YK800 | 4 (SCARA) |
| ABB FlexPicker | IRB 360 | 3 (Delta) |
8.3 离线编程与仿真验证
标准流程:
1. 导入环境模型 (工装、料框、工件)
2. 标定 TCP 和用户坐标系
3. 在 3D 视口中示教路径点
4. 编写 SCL 运动程序
5. 切换仿真模式
6. 运行程序, 观察:
- 路径是否合理
- 有无碰撞
- 有无奇异点
- 节拍是否满足
7. 优化路径
8. 编译下发到实体
9. 实体以 25% 速度首跑验证
SCL 中验证路径:
VAR
bPathValid : BOOL;
cycleTime : LREAL;
maxDist : LREAL;
END_VAR
// 路径验证: 碰撞 + 奇异 + 可达性
bPathValid := ValidatePath(
Robot := Robot1,
Waypoints := [pHome, pPick, pPlace],
CheckCollision := TRUE,
CheckSingular := TRUE,
CheckReach := TRUE
);
// 预估节拍
cycleTime := EstimateCycleTime(
Robot := Robot1,
Waypoints := [pHome, pPick, pPlace],
Velocity := 80
);
IF bPathValid AND cycleTime < 5.0 THEN
Log.Info('Path valid, cycle = %.1f s', cycleTime);
DeployProgram(Program := "PickPlace", Target := RUNTIME_PHYSICAL);
ELSE
Log.Error('Path validation failed');
END_IF;
8.4 碰撞检测
详见 碰撞检测, 此处摘要.
两种碰撞检测:
| 特性 | 仿真碰撞 | 运行时碰撞 |
|---|---|---|
| 位置 | IDE | Service + 驱动 |
| 实时性 | 非实时 | 硬实时 (1 ms) |
| 碰撞体 | 精细网格 | 简化凸包 |
| 响应 | 减速/报警 | 急停 |
碰撞体类型:
AABB (轴对齐包围盒):
┌─────────┐
│ │ 检测最快, 紧致性差
└─────────┘
OBB (有向包围盒):
┌──────┐
╱ ╱│ 精度更好, 需 SAT 检测
└──────┘ │
└──────┘
凸包 (Convex Hull):
┌───╮
╱ ╱ ╲ 最紧致, 计算量适中
╲ ╲ ╱
╰───╯
SCL 配置碰撞检测:
// 仿真碰撞检测
SetCollisionResponse(
Robot := Robot1,
Mode := COLL_MODE_SLOWDOWN,
WarnDist := 30.0,
SlowDist := 15.0,
StopDist := 3.0
);
// 运行时碰撞检测
SetRunCollisionCheck(
Robot := Robot1,
Enabled := TRUE,
SelfColl := TRUE,
EnvColl := TRUE,
SafetyOutput := DO[5],
ReactionTime := 2
);
8.5 碰撞响应策略
| 策略 | 行为 | 适用 |
|---|---|---|
| 急停 | 立即断电 | 运行时碰撞 |
| 减速逼近 | 接近时自动降速 | 仿真验证 |
| 路径重规划 | 自动绕行 | 离线编程 |
| 姿态调整 | 保持 TCP 位置调整姿态 | 狭小空间 |
| 仅报警 | 不停止 | 调试 |
8.6 仿真→真机切换流程
// 第 1 步: 切仿真模式, 验证程序
SetRobotMode(Robot := Robot1, Mode := ROBOT_SIMULATION);
// 运行程序, 检查路径和碰撞
// 第 2 步: 确认无误, 切到真机
// --- 安全确认 ---
// 1. 确认安全门关闭
// 2. 确认急停弹起
// 3. 确认工作区无人
// 第 3 步: 真机低速首跑
SetRobotMode(Robot := Robot1, Mode := ROBOT_PHYSICAL);
SpeedOverride := 25; // 25% 速度
// 第 4 步: 观察运行
// 无异常 → 逐渐提高速度
// 有异常 → 急停, 修改程序
8.7 常见错误
| 现象 | 原因 | 解决 |
|---|---|---|
| 仿真和实物位置不一致 | DH 参数不同 | 确认导入正确的机器人参数 |
| 仿真通过但实物碰撞 | 工装模型未导入 | 导入所有环境 STL |
| 3D 视口黑屏 | GPU 驱动问题 | 更新驱动或用软件渲染 |
| 碰撞检测漏报 | 碰撞体太粗糙 | 用 V-HACD 重生成 |
| 节拍预估差 > 10% | 未配置工具质量/惯量 | 输入正确的工具参数 |
9. 多机器人协同
详见 多机器人协同, 此处摘要关键概念和编程模式.
9.1 协调策略
| 策略 | 耦合度 | 描述 |
|---|---|---|
| 主从协调 | 高 | 从机跟随主机运动 |
| 平等协调 | 中 | 独立编程, 约束同步 |
| 区域划分 | 低 | 独立工作, 不共享空间 |
主从协调示例:
// 双机搬运: Robot1 是主机, Robot2 是从机
SlaveConfig.MasterRobot := Robot1;
SlaveConfig.SlaveRobot := Robot2;
SlaveConfig.RelativeFrame := "Part_Frame";
SlaveConfig.Mode := MS_SYNC_POSE;
ApplyMasterSlave(SlaveConfig);
// 只需控制 Robot1, Robot2 自动跟随
MoveL(Robot := Robot1, Target := pPick, Velocity := 30);
MoveL(Robot := Robot1, Target := pPlace, Velocity := 20);
9.2 干涉区 (Interlock Zone)
共享空间中的互斥访问机制.
定义和编程:
VAR
ILZone : INTERLOCK_ZONE;
ILZoneID : INT;
bGranted : BOOL;
END_VAR
// 定义干涉区
ILZone.Shape := ZONE_BOX;
ILZone.Center := (X := 200, Y := 0, Z := 500);
ILZone.Size := (X := 400, Y := 300, Z := 400);
ILZone.Robots := [Robot1, Robot2];
ILZoneID := RegisterInterlockZone(ILZone);
// 进入前申请
bGranted := RequestInterlock(Zone := ILZoneID,
Robot := Robot1,
Timeout := 5000);
IF bGranted THEN
MoveL(Robot := Robot1, Target := pSharedPick, Velocity := 30);
ReleaseInterlock(Zone := ILZoneID, Robot := Robot1);
END_IF;
9.3 同步指令
SyncMove — 多机同步运动:
VAR
SyncGrp : SYNC_GROUP;
END_VAR
// 创建同步组
SyncGrp := CreateSyncGroup(Robots := [Robot1, Robot2]);
// 同步开始
StartSyncGroup(SyncGrp);
MoveL(Robot := Robot1, Target := P1, Velocity := 50);
MoveL(Robot := Robot2, Target := P2, Velocity := 80);
EndSyncGroup(SyncGrp); // 等待所有完成
WaitFor — 等待同步点:
// 各自运动, 在 P1 点等待对方
MoveL(Robot := Robot1, Target := P1, Velocity := 50);
MoveL(Robot := Robot2, Target := P1, Velocity := 80);
WaitForAll(Robots := [Robot1, Robot2]); // 都到 P1 才继续
9.4 双机协作示例
PROGRAM DualArmTransfer
VAR
SyncGrp : SYNC_GROUP;
ILZone : INT := 1;
bGranted : BOOL;
END_VAR
SyncGrp := CreateSyncGroup(Robots := [Robot1, Robot2]);
// 同步回 Home
StartSyncGroup(SyncGrp);
MoveJ(Robot := Robot1, Target := Home1, Speed := 80);
MoveJ(Robot := Robot2, Target := Home2, Speed := 80);
EndSyncGroup(SyncGrp);
// 同步到取料上方
StartSyncGroup(SyncGrp);
MoveL(Robot := Robot1, Target := PickApproach1, Velocity := 50);
MoveL(Robot := Robot2, Target := PickApproach2, Velocity := 50);
EndSyncGroup(SyncGrp);
// 申请干涉区
bGranted := RequestInterlock(Zone := ILZone, Robot := Robot1, Timeout := 3000);
IF bGranted THEN
// 同步下降到取料位
StartSyncGroup(SyncGrp);
MoveL(Robot := Robot1, Target := Pick1, Velocity := 20);
MoveL(Robot := Robot2, Target := Pick2, Velocity := 20);
EndSyncGroup(SyncGrp);
// 夹紧
DO[1] := TRUE; // Robot1 夹爪
DO[2] := TRUE; // Robot2 夹爪
DELAY 0.3;
// 同步抬起
StartSyncGroup(SyncGrp);
MoveL(Robot := Robot1, Target := PickApproach1, Velocity := 30);
MoveL(Robot := Robot2, Target := PickApproach2, Velocity := 30);
EndSyncGroup(SyncGrp);
// ... 移动到放料位 ...
ReleaseInterlock(Zone := ILZone, Robot := Robot1);
END_IF;
END_PROGRAM
9.5 常见错误
| 现象 | 原因 | 解决 |
|---|---|---|
| 双机不同步 | 未使用同步组 | 用 CreateSyncGroup |
| 干涉区超时 | 对方未释放 | 检查 ReleaseInterlock |
| 协调组卡住 | 容差太小 | 增大 Tolerance |
| 区域划分后越界 | Zone 定义不完整 | 加安全围栏干涉区 |
10. 实际应用示例
10.1 搬运码垛 (Pick & Place)
场景: 从传送带上抓取工件, 码放到托盘上.
点位布局:
托盘 (放置区)
┌──┬──┬──┐
│ 7│ 8│ 9│
├──┼──┼──┤
│ 4│ 5│ 6│
├──┼──┼──┤
│ 1│ 2│ 3│
└──┴──┴──┘
传送带 (取料区)
──●──●──●──
取料 1 2 3
完整程序:
PROGRAM Palletizing
VAR
// 运动功能块
Power : MC_GroupEnable;
Home : MC_GroupHome;
MoveJ1 : MC_MoveJointAbsolute;
MoveL1 : MC_MoveLinearAbsolute;
MoveL2 : MC_MoveLinearAbsolute;
// 码垛变量
PalletGrid : ARRAY[1..3, 1..3] OF ROBOT_POSE; // 3×3 码放位
PickPoses : ARRAY[1..3] OF ROBOT_POSE; // 3 个取料位
Row, Col : INT := 1; // 当前码放位置
PickIdx : INT := 1; // 当前取料索引
step : INT := 0;
bGripper : BOOL;
// 安全高度
pPickApproach : ROBOT_POSE;
pPlaceApproach : ROBOT_POSE;
pHome : ROBOT_POSE;
END_VAR
// 始终使能
Power(Robot := Robot1, Enable := TRUE);
CASE step OF
0: // 上电等待
IF Power.Status THEN
step := 10;
END_IF;
10: // 回 Home
Home(Robot := Robot1, Execute := TRUE);
IF Home.Done THEN
Home(Execute := FALSE);
step := 20;
END_IF;
20: // 等待工件到位
WAIT DI[1] = TRUE; // 工件到位传感器
step := 30;
30: // 快速到取料上方
MoveJ1(Robot := Robot1, Execute := TRUE,
TargetPosition := pPickApproach, Velocity := 200,
Zone := 30);
IF MoveJ1.Done THEN
MoveJ1(Execute := FALSE);
step := 40;
END_IF;
40: // 直线下降到取料位
MoveL1(Robot := Robot1, Execute := TRUE,
TargetPosition := PickPoses[PickIdx], Velocity := 30,
Zone := 0);
IF MoveL1.Done THEN
MoveL1(Execute := FALSE);
step := 50;
END_IF;
50: // 夹紧
bGripper := TRUE;
DELAY 0.3;
step := 60;
60: // 直线抬起
MoveL2(Robot := Robot1, Execute := TRUE,
TargetPosition := pPickApproach, Velocity := 50);
IF MoveL2.Done THEN
MoveL2(Execute := FALSE);
step := 70;
END_IF;
70: // 快速到码放上方
MoveJ1(Robot := Robot1, Execute := TRUE,
TargetPosition := pPlaceApproach, Velocity := 200,
Zone := 30);
IF MoveJ1.Done THEN
MoveJ1(Execute := FALSE);
step := 80;
END_IF;
80: // 直线下降到码放位
MoveL1(Robot := Robot1, Execute := TRUE,
TargetPosition := PalletGrid[Row, Col], Velocity := 30,
Zone := 0);
IF MoveL1.Done THEN
MoveL1(Execute := FALSE);
step := 90;
END_IF;
90: // 松开
bGripper := FALSE;
DELAY 0.3;
step := 100;
100: // 直线抬起
MoveL2(Robot := Robot1, Execute := TRUE,
TargetPosition := pPlaceApproach, Velocity := 50);
IF MoveL2.Done THEN
MoveL2(Execute := FALSE);
step := 110;
END_IF;
110: // 更新码垛位置
Col := Col + 1;
IF Col > 3 THEN
Col := 1;
Row := Row + 1;
IF Row > 3 THEN
Row := 1; // 托盘满, 发出满盘信号
DO[1] := TRUE; // 满盘指示
WAIT DI[2] = TRUE; // 等待换托盘
DO[1] := FALSE;
END_IF;
END_IF;
step := 20; // 循环
END_CASE;
END_PROGRAM
程序执行时序:
┌────┐ ┌────┐
Home ────┤ 10├───────────────┤ 20 ├──→ 等待工件
└────┘ └────┘
┌────┐ ┌────┐ ┌────┐
取料 ────┤30 ├──┤40 ├──┤50 ├──→ 夹紧
└────┘ └────┘ └────┘
┌────┐ ┌────┐
码放 ────┤70 ├──┤80 ├──→ 90 松开 → 100 抬起
└────┘ └────┘
10.2 弧焊轨迹规划
场景: 焊接圆形法兰密封面.
PROGRAM WeldFlange
VAR
mcJ : MC_MoveJointAbsolute;
mcL : MC_MoveLinearAbsolute;
mcC1, mcC2 : MC_MoveCircularAbsolute;
step : INT := 0;
bArcOn : BOOL;
END_VAR
CASE step OF
0: // 接近
mcJ(Robot := Robot1, Execute := TRUE,
TargetPosition := pApproach, Velocity := 200);
IF mcJ.Done THEN mcJ(Execute := FALSE); step := 10; END_IF;
10: // 下降到焊接起点
mcL(Robot := Robot1, Execute := TRUE,
TargetPosition := pWeldStart, Velocity := 30);
IF mcL.Done THEN
mcL(Execute := FALSE);
DO_Arc := TRUE; // 起弧
bArcOn := TRUE;
DELAY 0.3; // 等待熔池形成
step := 20;
END_IF;
20: // 第 1 段弧 (90°)
mcC1(Robot := Robot1, Execute := TRUE,
AuxPoint := pArc90, EndPoint := pArc180,
Velocity := 8.0, OriMode := ORI_PATH_FOLLOW,
Zone := 3.0);
IF mcC1.Done THEN mcC1(Execute := FALSE); step := 30; END_IF;
30: // 第 2 段弧 (90°)
mcC2(Robot := Robot1, Execute := TRUE,
AuxPoint := pArc270, EndPoint := pArc360,
Velocity := 8.0, OriMode := ORI_PATH_FOLLOW,
Zone := 0);
IF mcC2.Done THEN
mcC2(Execute := FALSE);
DO_Arc := FALSE; // 收弧
bArcOn := FALSE;
DELAY 0.5; // 填充弧坑
step := 40;
END_IF;
40: // 退回
mcL(Robot := Robot1, Execute := TRUE,
TargetPosition := pApproach, Velocity := 50);
IF mcL.Done THEN step := 0; END_IF;
END_CASE;
END_PROGRAM
10.3 涂胶/喷涂路径生成
场景: 在工件表面涂布密封胶.
PROGRAM GlueDispensing
VAR
mcJ : MC_MoveJointAbsolute;
mcL : MC_MoveLinearAbsolute;
mcSpline: MC_MoveSplineAbsolute;
waypoints : ARRAY[0..20] OF ROBOT_POSE;
wpCnt : UINT;
step : INT := 0;
bGlueOn : BOOL;
END_VAR
CASE step OF
0: // 接近
mcJ(Robot := Robot1, Execute := TRUE,
TargetPosition := pGlueApproach, Velocity := 200);
IF mcJ.Done THEN mcJ(Execute := FALSE); step := 10; END_IF;
10: // 下降到起点
mcL(Robot := Robot1, Execute := TRUE,
TargetPosition := pGlueStart, Velocity := 30);
IF mcL.Done THEN
mcL(Execute := FALSE);
bGlueOn := TRUE; // 开胶阀
DELAY 0.2; // 等胶流出
step := 20;
END_IF;
20: // 沿样条曲线涂胶
mcSpline(Robot := Robot1, Execute := TRUE,
SplineType := SPLINE_CATMULL_ROM,
Waypoints := ADR(waypoints),
WaypointCount := wpCnt,
Velocity := 40.0, // mm/s, 恒切向速度
VelocityMode := VEL_TANGENT,
OriMode := ORI_PATH_FOLLOW);
IF mcSpline.Done THEN
mcSpline(Execute := FALSE);
bGlueOn := FALSE; // 关胶阀
DELAY 0.3; // 拉丝断胶
step := 30;
END_IF;
30: // 退回
mcL(Robot := Robot1, Execute := TRUE,
TargetPosition := pGlueApproach, Velocity := 50);
IF mcL.Done THEN step := 0; END_IF;
END_CASE;
END_PROGRAM
10.4 装配 (压装/拧紧) 力控
场景: 将轴承压入壳体.
PROGRAM PressFit
VAR
mcJ : MC_MoveJointAbsolute;
mcL_Approach : MC_MoveLinearAbsolute;
mcL_Contact : MC_MoveLinearAbsolute;
TorqueCtrl : MC_TorqueControl;
step : INT := 0;
pressForce : LREAL; // 当前压入力
pressDepth : LREAL; // 当前压入深度
END_VAR
CASE step OF
0: // 快速接近
mcJ(Robot := Robot1, Execute := TRUE,
TargetPosition := pPressApproach, Velocity := 200);
IF mcJ.Done THEN mcJ(Execute := FALSE); step := 10; END_IF;
10: // 慢速接近工件
mcL_Approach(Robot := Robot1, Execute := TRUE,
TargetPosition := pContact, Velocity := 20);
IF mcL_Approach.Done THEN
mcL_Approach(Execute := FALSE);
step := 20;
END_IF;
20: // 切换到力矩模式, 以 50N 力压入
TorqueCtrl(Axis := Robot1, Execute := TRUE,
Torque := 50.0, // 50 N
TorqueRamp := 10.0,
PositionLimit := 10.0); // 最大压入 10 mm
step := 30;
30: // 监控压入过程
pressForce := ReadTorque(Robot1);
pressDepth := ReadPosition(Robot1) - pContact.Z;
// 压入到位或力超限
IF pressDepth >= 8.0 THEN
TorqueCtrl(Execute := FALSE);
step := 40;
ELSIF pressForce > 200.0 THEN
// 过载保护
TorqueCtrl(Execute := FALSE);
Log.Error('Press force exceeded: %.1f N', pressForce);
step := 99;
END_IF;
40: // 保持 1 秒
DELAY 1.0;
step := 50;
50: // 退回
mcL_Approach(Robot := Robot1, Execute := TRUE,
TargetPosition := pPressApproach, Velocity := 50);
IF mcL_Approach.Done THEN step := 0; END_IF;
99: // 错误处理
mcL_Approach(Robot := Robot1, Execute := TRUE,
TargetPosition := pPressApproach, Velocity := 30);
END_CASE;
END_PROGRAM
10.5 视觉引导抓取
场景: 相机检测到工件位置, 机器人自动抓取.
PROGRAM VisionPick
VAR
Visual : MC_VisionDetect;
MoveJ : MC_MoveJointAbsolute;
MoveL_Pick : MC_MoveLinearAbsolute;
MoveL_Place : MC_MoveLinearAbsolute;
pPick : ROBOT_POSE;
pPickApproach : ROBOT_POSE;
step : INT := 0;
bDetected : BOOL;
END_VAR
CASE step OF
0: // 触发拍照
Visual(Camera := Cam1, Execute := TRUE, ObjectId := 1);
step := 10;
10: // 等待检测结果
IF Visual.Done THEN
Visual(Execute := FALSE);
IF Visual.Detected THEN
// 相机坐标已自动变换到机器人坐标系
pPick := Visual.DetectedPose;
// 在工件上方 50 mm 处作为安全高度
pPickApproach := pPick;
pPickApproach.Z := pPick.Z + 50.0;
bDetected := TRUE;
step := 20;
ELSE
Log.Warn('No object detected');
step := 0;
END_IF;
END_IF;
20: // 快速到取料上方
MoveJ(Robot := Robot1, Execute := TRUE,
TargetPosition := pPickApproach, Velocity := 200);
IF MoveJ.Done THEN
MoveJ(Execute := FALSE);
step := 30;
END_IF;
30: // 直线下降到取料位
MoveL_Pick(Robot := Robot1, Execute := TRUE,
TargetPosition := pPick, Velocity := 30);
IF MoveL_Pick.Done THEN
MoveL_Pick(Execute := FALSE);
DO[1] := TRUE; // 夹爪关闭
DELAY 0.3;
step := 40;
END_IF;
40: // 抬起
MoveL_Pick(Robot := Robot1, Execute := TRUE,
TargetPosition := pPickApproach, Velocity := 50);
IF MoveL_Pick.Done THEN
MoveL_Pick(Execute := FALSE);
step := 50;
END_IF;
50: // 放到固定位置
MoveJ(Robot := Robot1, Execute := TRUE,
TargetPosition := pPlaceApproach, Velocity := 200);
IF MoveJ.Done THEN
MoveJ(Execute := FALSE);
step := 60;
END_IF;
60: // 下降放置
MoveL_Place(Robot := Robot1, Execute := TRUE,
TargetPosition := pPlace, Velocity := 30);
IF MoveL_Place.Done THEN
MoveL_Place(Execute := FALSE);
DO[1] := FALSE; // 松开
DELAY 0.3;
step := 0; // 循环
END_IF;
END_CASE;
END_PROGRAM
10.6 常见错误
| 场景 | 错误 | 解决 |
|---|---|---|
| 码垛 | 码放位置计算错误 | 用 PalletGrid 数组预计算所有位置 |
| 弧焊 | 焊道厚薄不均 | 用 VEL_TANGENT 恒切向速度 |
| 涂胶 | 胶量不匀 | 恒速度 + 提前开胶/延时关胶 |
| 压装 | 过压损坏工件 | 力矩+位置双限位, 超限即停 |
| 视觉引导 | 抓空或撞工件 | 视觉结果加安全偏移 + 仿真验证 |
11. 安全与调试
11.1 安全速度与力矩限制
T1/T2/Auto 三模式速度限制:
| 模式 | 速度上限 | 安全门 | 使能要求 | 用途 |
|---|---|---|---|---|
| T1 | 250 mm/s | 可开 | 必须 | 示教/调试 |
| T2 | 无限 | 必须关 | 必须 | 高级调试 |
| Auto | 无限 | 必须关+锁 | 不需要 | 生产 |
SCL 中设置速度限制:
// 设置安全速度
SetSafetyConfig(
Robot := Robot1,
SpeedLimit := 250.0, // T1 模式限速 (mm/s)
ForceLimit := 140.0, // 最大碰撞力 (N)
TorqueLimit := 30.0 // 最大力矩 (N·m)
);
// 运行时速度倍率
SpeedOverride := 50; // 50% 速度
11.2 使能开关与急停
三位使能开关:
档位 1 (松开) → 伺服断电
档位 2 (半按) → 伺服使能 (正常工作区)
档位 3 (按死) → 伺服断电 (等同于急停)
原理: 受惊时人手本能松开或握死, 都断电
急停回路:
安全 PLC (SIL 3)
├─ CH1: 急停按钮 (双路 NC, 交叉监测)
├─ CH2: 三位使能开关 (NC+NO+NC)
├─ CH3: 安全门 (NC, 门磁+锁)
├─ CH4: 光幕 (OSSD)
└─ OSSD → 伺服使能 / KM1 接触器
SCL 中检测安全信号:
// 安全回路监控
IF NOT DI[1] THEN // DI[1] = 急停 (NC)
Log.Fatal('Emergency stop pressed');
MC_GroupStop(Robot := Robot1, Execute := TRUE);
END_IF;
IF NOT DI[2] THEN // DI[2] = 安全门 (NC)
Log.Fatal('Safety door opened');
MC_GroupStop(Robot := Robot1, Execute := TRUE);
END_IF;
11.3 区域监控 (Safety Zone)
定义三维空间区域, 机器人进入时自动限速或停止.
定义安全区域:
VAR
SafeZone : SAFETY_ZONE;
ZoneID : INT;
END_VAR
// 定义减速区 (进入时降到 50% 速度)
SafeZone.Shape := ZONE_BOX;
SafeZone.Center := (X := 300, Y := 0, Z := 400);
SafeZone.Size := (X := 600, Y := 500, Z := 600);
SafeZone.Action := ZONE_SLOW_DOWN;
SafeZone.SpeedLimit := 50; // %
ZoneID := RegisterSafetyZone(Robot := Robot1, Zone := SafeZone);
// 定义停止区 (进入即急停)
SafeZone.Action := ZONE_STOP;
ZoneID := RegisterSafetyZone(Robot := Robot1, Zone := SafeZone);
11.4 调试工具
示教器操作:
| 功能 | 操作 | 说明 |
|---|---|---|
| JOG | 示教器摇杆/按键 | 手动移动机器人 |
| 记录点位 | 示教器"记录"按钮 | 保存当前位置到点位表 |
| 修改点位 | 双击点位, 编辑坐标 | 微调位置 |
| 程序运行 | 选择程序 → 运行 | 执行 SCL 程序 |
| 单步执行 | 逐行运行 | 调试用 |
| 速度倍率 | 旋钮 0~100% | 运行时调速 |
在线监视:
// 在 IDE 变量监视面板添加以下变量:
// - Robot1.JointPosition (6 个关节角)
// - Robot1.TCP (当前 TCP 位姿)
// - Robot1.ServoOn (6 轴伺服状态)
// - Robot1.Current (6 轴电流)
// - Robot1.Torque (6 轴力矩)
// - DO[1..10], DI[1..10] (IO 状态)
强制赋值 (调试用):
// 强制 DI 信号 (模拟传感器)
// 在 IDE 强制面板:
Force DI[1] = TRUE; // 模拟工件到位
Force DI[2] = TRUE; // 模拟安全门关闭
// 注意: 强制值在编译后自动清除
// 调试完成后必须逐条清除强制
11.5 常见错误与排错
| 错误码 | 级别 | 含义 | 排查方向 |
|---|---|---|---|
| 1001 | Fatal | 伺服过流 | 检查负载、短路 |
| 1002 | Fatal | 伺服过压 | 检查制动电阻、电源 |
| 1003 | Fatal | 编码器丢失 | 检查接线、编码器电池 |
| 2001 | Error | 跟踪误差超限 | 增益过低或负载过大 |
| 2002 | Error | 关节超限位 | 反向 JOG 退出 |
| 2101 | Error | 奇异点不可逆解 | 调整路径避开 |
| 3001 | Error | EtherCAT 丢帧 | 检查网线、从站状态 |
| 3002 | Error | SDO 超时 | 从站未进 OP |
| 4001 | Warning | 关节温度高 | 降速或停机冷却 |
| 4002 | Warning | 编码器电池低 | 换电池 (< 2.6V) |
排错流程示例: 机器人不动了
1. 检查安全回路:
- 急停是否弹起? (DI[1])
- 安全门是否关闭? (DI[2])
- 使能是否半按? (T1 模式)
2. 检查伺服状态:
- MC_GroupReadStatus → 当前状态
- 如果是 ErrorStop → 先 MC_Reset
- 如果是 Disabled → 先 MC_GroupEnable
3. 检查报警:
- 查看诊断面板的报警列表
- 按报警码查表
4. 检查通讯:
- EtherCAT 总线状态 (WKC, AL Status)
- 伺服驱动器是否正常
5. 检查程序:
- 是否在正确的程序/步骤
- WAIT 条件是否满足
- 速度倍率是否为 0?
11.6 最佳实践
- 安全第一: 任何调试前确认安全门关闭、急停可用
- 低速首跑: 新程序用 25% 速度跑一遍
- 仿真先行: 仿真通过再下放真机
- 报警不跳过: 每条报警查根因, 不要一键清除
- 定期保养: 每月精度检测, 每半年运动学标定
- 日志归档: 报警日志保留 3 年, 用于趋势分析
- 示教规范: 所有点位命名规范 (如 Pick_01, Place_01)
- 程序注释: 每段运动注明用途和速度选择理由
本文档覆盖了 Darra PLC 机器人编程的完整知识体系. 更多细节请参考各子章节文档.