第一个机器人程序

本节手把手带您完成一个 六轴协作机械臂 从 0 到 1 的首例项目: 导入 URDF → 标定基 / 工具 / 工件三大坐标系 → 手动拖动示教 → 编写 SCL 运动程序 → 仿真预览 → 下载到真机运行 → 异常排错。

这是每一位机器人工程师都必须走通的"开箱即用"闭环, 读完并跟做一遍, 您就有能力接手新工程。

目标

• 掌握 URDF 导入和机器人模型加载
• 掌握 基 / 工具 / 工件 三坐标系标定方法
• 会用零力控制做手动示教
• 会写 MC_MoveJ / MC_MoveL 混合运动程序
• 会仿真预览 + 真机运行, 会看关节角监视
• 能识别奇异点 / 超限 / 使能失效等常见异常

前置条件

• 已完成 创建第一个项目, 工程勾选了 Robot 子系统
• GPU 支持 OpenGL 4.5+
• 如果要真机运行, 需一台六轴协作臂 (JAKA/AUBO/UR/大族 等, 本例以 JAKA Zu7 为参考)
• 建议先用仿真走一遍, 再上真机

步骤 1 - 导入机器人 URDF

URDF (Unified Robot Description Format) 是机器人描述的标准格式, 包含关节、连杆、 DH 参数、STL 网格。JAKA / AUBO / UR 官方均提供 URDF。

1.1 准备 URDF 包

典型 URDF 目录结构:

jaka_zu7/
├── urdf/
│   └── zu7.urdf
├── meshes/
│   ├── base.stl
│   ├── link1.stl
│   └── ...
└── config/
    └── limits.yaml

1.2 启动导入向导

1. 解决方案资源管理器 → 右键 机器人 → 导入机器人 (Import Wizard)
2. 向导步骤:

步骤	操作
1. 格式	选择 URDF (其他支持: .dae / .fbx / Darra 模型库)
2. 文件	浏览选 zu7.urdf (向导会自动解析 mesh 引用)
3. 名称	填 Robot1
4. 关节	自动列出 6 个关节 (joint1 ~ joint6), 检查正负转向
5. 驱动	选 虚拟驱动 (仿真) 或 EtherCAT (CiA402) (真机)
6. 限位	从 limits.yaml 自动读取, 可手动覆盖

3. 点 完成, 3D 视口加载 Zu7 模型

TODO

内置模型库

不想自己找 URDF? 向导第 1 步选 Darra 模型库, 内置 JAKA Zu3/Zu7/Zu12, AUBO i5/i10, UR3e/5e/10e/16e, 大族 Elfin5/E05 等 20+ 常见机型, 参数预配置完毕。

1.3 检查 DH 参数

导入后双击 Robot1 → 运动学参数, 核对 Modified DH:

关节	α(度)	a(mm)	d(mm)	θ 偏置(度)	正负
J1	0	0	121.5	0	+
J2	-90	0	0	-90	+
J3	0	360	0	0	+
J4	0	303.5	122.5	-90	+
J5	90	0	102.5	0	+
J6	-90	0	102.5	0	+

数据对不上就改, 不然后续笛卡尔运动会偏。

步骤 2 - 建立基坐标系

基坐标系 (Base Frame) 是世界到机器人安装位置的变换。新机器人默认基坐标与世界坐标重合, 但现场往往机器人装在桌面某个角落, 需要做一次校准。

2.1 方法: 三点标定

在工作台上选取三个已知位置点 (可以用精密尺测量), 记录机器人 TCP 到达这三点时 的关节角, 软件反解得到基坐标变换矩阵。

操作:

1. 双击 Robot1 → 坐标系 → 基坐标
2. 点 新建标定 → 选 三点法
3. 依次输入三个参考点的世界坐标 (例如工作台左上角 (0,0,0)、右上角 (500,0,0)、 左下角 (0,300,0))
4. 切到点动模式, 把 TCP 分别移到这三个实际点, 点 记录
5. 软件计算得到基坐标变换:
   • 平移: (Tx, Ty, Tz) = (120.5, 85.2, 0.0)
   • 旋转: (Rx, Ry, Rz) = (0.0, 0.0, 12.3) (Z 轴转 12.3°)
6. 点 应用

TODO

步骤 3 - 建立工具坐标系 (TCP 四点法)

安装夹爪 / 吸盘 / 螺丝刀后, 机器人末端凸出一段, 这段末尖就是新的 TCP (Tool Center Point)。 必须校准, 不然 MoveL 走的是法兰中心不是工具尖。

3.1 四点法原理

让 TCP 尖端从不同姿态接触同一个固定参考点 (通常用桌上装一根固定针)。 不同姿态下关节角不同, 但 TCP 世界坐标相同, 反解得到 TCP 相对法兰的偏置。

3.2 操作步骤

1. 装好夹爪, 找一根 3 mm 针固定在桌面
2. 双击 Robot1 → 坐标系 → 工具坐标
3. 点 新建 → 四点法
4. 命名 Tool_Gripper
5. 依次:
   • 姿态 1: TCP 从正上方接触针尖, 记录
   • 姿态 2: TCP 从 +X 方向斜着接触针尖 (倾斜 30°), 记录
   • 姿态 3: TCP 从 -X 方向斜着接触针尖, 记录
   • 姿态 4: TCP 从 +Y 方向斜着接触针尖, 记录
6. 软件解算得到 (Tx, Ty, Tz) = (0.0, 0.0, 155.5) (TCP 相对法兰 Z+ 方向 155.5 mm)
7. 精度校验: 让 TCP 绕针尖旋转不同姿态, 偏差 < 0.5 mm 才算合格

TODO

安全提醒

做四点法时速度必须调到 5% 以下, 手离急停按钮, 避免撞坏夹爪或针。

步骤 4 - 建立工件坐标系 (三点法)

工件坐标系是相对工件 (比如托盘 / 夹具) 的局部坐标系。这样程序里写 Place := Workobject.P1 就和工件绑定, 工件搬到另一台机上只改工件坐标即可复用。

4.1 操作: 三点法

1. 双击 Robot1 → 坐标系 → 工件坐标
2. 点 新建 → 三点法
3. 命名 WObj_Pallet
4. 定义:
   • 原点: TCP 移到工件左上角 → 记录
   • X 轴正向: TCP 沿长边 +X 移动一段 → 记录
   • XY 平面: TCP 移到工件左下角 (在 XY 平面内) → 记录
5. 软件解算工件坐标到基坐标的变换
6. 之后所有示教点都可以选 参考坐标系 = WObj_Pallet

TODO

步骤 5 - 手动拖动示教 (零力控制)

零力控制 (Free Drive / Hand Guiding) 是协作臂的杀手功能: 按住机器人某个位置直接拖动, 机器人"无重力"地跟随, 手感像拖一根没重量的杆。

5.1 进入零力模式

1. 打开 虚拟示教器 (视图 → 示教器)
2. 连接真实机器人 (仿真可跳过此步, 仿真下用 3D 视口拖拽)
3. 按住示教器上的 零力 (Free Drive) 按钮, 绿灯亮起
4. 松开后退出零力

5.2 示教 Home / Pick / Place 三点

操作要点:

• Home: 机器人的安全起始姿态。一般选 J1=0°, J2=-90°, J3=0°, J4=0°, J5=90°, J6=0°
• Pick: 取料点, TCP 正对零件上方约 50 mm
• Place: 放料点, 同工件上不同位置

每次拖到目标位置后:

1. 打开示教器 点位 标签页
2. 点 新建点 → 命名 (Home / Pick / Place)
3. 选择参考坐标系:
   • Home 选 基坐标
   • Pick / Place 选 WObj_Pallet (工件坐标)
4. 记录当前 TCP 位姿, 示教器下方会显示

示教完毕后点位表:

点位	参考坐标	X	Y	Z	Rx	Ry	Rz	类型
Home	Base	0	300	500	180	0	0	关节
Pick	WObj_Pallet	100	50	50	180	0	0	笛卡尔
PickApproach	WObj_Pallet	100	50	100	180	0	0	笛卡尔
Place	WObj_Pallet	300	200	50	180	0	0	笛卡尔
PlaceApproach	WObj_Pallet	300	200	100	180	0	0	笛卡尔

TODO

接近点设计

工业经验: 每个关键点 (Pick/Place) 都要配一个 接近点 (Approach), Z 方向高 30~100 mm。 这样 MoveL 先到接近点再下降, 避免擦过障碍物。

步骤 6 - 编写 SCL 运动程序

现在把示教好的点连起来, 写一个标准 Pick-and-Place:

1. 右键 程序 → 新建程序, 名称 PickPlace, 语言 SCL
2. 输入完整代码:

(* ==========================================
   PickPlace - 六轴臂 Pick and Place 示例
   序列: Home → PickApproach → Pick → Gripper Close
         → PickApproach → PlaceApproach → Place
         → Gripper Open → PlaceApproach → Home
   ========================================== *)
PROGRAM PickPlace
VAR_EXTERNAL
    Motor     : BOOL;      (* 上级使能 *)
    Gripper   : BOOL;      (* 夹爪 DO, %QX1.0 *)
END_VAR

VAR
    nStep      : INT  := 0;
    bStart     : BOOL := FALSE;
    bCycleDone : BOOL := FALSE;

    (* 运动功能块实例 *)
    fbMoveJ    : MC_MoveJoint;
    fbMoveL    : MC_MoveLinear;

    (* 示教点 (在工程中已通过示教器定义) *)
    pHome      : ROBOT_POINT := Robot1.Points.Home;
    pPickApp   : ROBOT_POINT := Robot1.Points.PickApproach;
    pPick      : ROBOT_POINT := Robot1.Points.Pick;
    pPlaceApp  : ROBOT_POINT := Robot1.Points.PlaceApproach;
    pPlace     : ROBOT_POINT := Robot1.Points.Place;

    (* 运动参数 *)
    cSpeedFast : LREAL := 500.0;  (* mm/s, 空行程 *)
    cSpeedSlow : LREAL := 50.0;   (* mm/s, 贴近工件 *)
    cAccel     : LREAL := 1000.0; (* mm/s^2 *)
    cBlend     : LREAL := 10.0;   (* mm, 圆弧过渡 *)
END_VAR

(* =========================================
   状态机 nStep:
   0=Idle, 10=GoHome, 20=GoPickApp, 30=GoPick,
   40=GripClose, 50=RetreatPickApp, 60=GoPlaceApp,
   70=GoPlace, 80=GripOpen, 90=RetreatPlaceApp,
   100=GoHome, 110=Done
   ========================================= *)
CASE nStep OF
    0: (* 等待启动 *)
        IF bStart AND Motor THEN
            nStep := 10;
            bStart := FALSE;
            bCycleDone := FALSE;
        END_IF;

    10: (* 回 Home *)
        fbMoveJ(Axis := Robot1, Target := pHome,
                Velocity := 50.0, Acceleration := 100.0,
                Execute := TRUE);
        IF fbMoveJ.Done THEN
            fbMoveJ(Execute := FALSE);
            nStep := 20;
        END_IF;

    20: (* 直线到接近点 (空行程) *)
        fbMoveL(Axis := Robot1, Target := pPickApp,
                Velocity := cSpeedFast, Acceleration := cAccel,
                BlendRadius := cBlend, Execute := TRUE);
        IF fbMoveL.Done THEN
            fbMoveL(Execute := FALSE);
            nStep := 30;
        END_IF;

    30: (* 下降到 Pick (慢速) *)
        fbMoveL(Axis := Robot1, Target := pPick,
                Velocity := cSpeedSlow, Acceleration := cAccel,
                Execute := TRUE);
        IF fbMoveL.Done THEN
            fbMoveL(Execute := FALSE);
            nStep := 40;
        END_IF;

    40: (* 夹爪关闭 + 等待 200 ms *)
        Gripper := TRUE;
        nStep := 41;

    41: (* 简易等待 (真实工程用 TON) *)
        nStep := 50;

    50: (* 抬升回接近点 *)
        fbMoveL(Axis := Robot1, Target := pPickApp,
                Velocity := cSpeedSlow, Acceleration := cAccel,
                Execute := TRUE);
        IF fbMoveL.Done THEN
            fbMoveL(Execute := FALSE);
            nStep := 60;
        END_IF;

    60: (* 关节运动到放料接近点 (大行程) *)
        fbMoveJ(Axis := Robot1, Target := pPlaceApp,
                Velocity := 80.0, Acceleration := 200.0,
                Execute := TRUE);
        IF fbMoveJ.Done THEN
            fbMoveJ(Execute := FALSE);
            nStep := 70;
        END_IF;

    70: (* 直线下降到 Place *)
        fbMoveL(Axis := Robot1, Target := pPlace,
                Velocity := cSpeedSlow, Acceleration := cAccel,
                Execute := TRUE);
        IF fbMoveL.Done THEN
            fbMoveL(Execute := FALSE);
            nStep := 80;
        END_IF;

    80: (* 夹爪打开 *)
        Gripper := FALSE;
        nStep := 90;

    90: (* 抬升 *)
        fbMoveL(Axis := Robot1, Target := pPlaceApp,
                Velocity := cSpeedSlow, Acceleration := cAccel,
                Execute := TRUE);
        IF fbMoveL.Done THEN
            fbMoveL(Execute := FALSE);
            nStep := 100;
        END_IF;

    100: (* 回 Home *)
        fbMoveJ(Axis := Robot1, Target := pHome,
                Velocity := 80.0, Acceleration := 200.0,
                Execute := TRUE);
        IF fbMoveJ.Done THEN
            fbMoveJ(Execute := FALSE);
            bCycleDone := TRUE;
            nStep := 0;
        END_IF;
END_CASE;

END_PROGRAM

MC_MoveJ vs MC_MoveL

• MC_MoveJ (关节插补): 六轴同时启停、同时到达, 路径不可预测, 但速度最快, 适合空行程
• MC_MoveL (笛卡尔直线): TCP 走直线, 保证工件姿态稳定, 适合贴近工件 / 焊接 / 涂胶
• 工程规范: 空行程用 MoveJ, 贴近工件用 MoveL

步骤 7 - 编译与仿真预览

7.1 编译

按 F7 编译:

[编译] 机器人模型 Robot1 → 已加载
[编译] 点位表 → 5 个有效点
[编译] PickPlace → 436 条指令
[编译] 运动功能块引用 MC_MoveJ / MC_MoveL 已解析
[编译] 成功, 0 错误, 0 警告

7.2 仿真预览

按 F5 启动仿真:

1. 3D 视口显示 Zu7 模型
2. 打开变量监视, 把 bStart / Motor / nStep / Gripper 加入
3. 先强制 Motor := TRUE, 再强制 bStart := TRUE
4. 观察:
   • 机器人按 Home → Pick → Place → Home 轨迹运动
   • nStep 从 0 → 10 → 20 → ... → 100 递进
   • Gripper 在 Pick 处变 TRUE, 在 Place 处变 FALSE
5. 可以在示教器面板调全局速度覆盖 (0~100%) 放慢观察

TODO

轨迹回放

菜单 视图 → 轨迹回放 打开, 勾选"记录轨迹", 运行一个完整循环后可以:

• 暂停并按帧前进 / 后退
• 看 TCP 实际走的 3D 曲线 (红色虚线)
• 导出 CSV, 与设计轨迹对比偏差

步骤 8 - 下载到真机

切到真机前务必:

1. 急停按钮就位 — 伸手可按
2. 围栏 / 区域限制启用 — 配置工作空间 BBox, 超出自动停
3. 工件 / 夹爪 / 线缆无干涉
4. 速度覆盖调到 10% — 即使程序写错了也慢慢动不会坏

下载流程见 部署到运行时, 机器人与 PLC 共用同一个 .dkpkg 包。

8.1 使能与启动

1. Panel 上点 机器人 → 使能 (Enable), 绿灯亮起
2. 示教器确认急停未按下, 无碰撞报警
3. 发送启动信号: bStart := TRUE
4. 第一次跑完一个循环后, 速度覆盖再逐步加到 30% / 50% / 80%

8.2 在线监视关节角

IDE 在线模式 → 视图 → 机器人监视面板, 显示:

关节	当前角度 (°)	速度 (°/s)	力矩 (Nm)	限位
J1	-12.5	35.0	1.2	-170~170
J2	-88.3	-12.4	5.8	-120~120
J3	45.0	22.1	3.5	-150~150
J4	0.0	0.0	0.2	-180~180
J5	90.1	0.0	0.8	-120~120
J6	0.0	0.0	0.1	-180~180

当限位条变红说明接近限位, 程序应减速或报警。

异常排错

"奇异点 (Singularity) 警告"

症状: MoveL 过程中报 E-1042 Wrist Singularity, 机器人立即减速停止。

原因: J4 / J5 / J6 三轴轴线共面, 逆解无限多, 控制器无法确定。

解决:

• 目标点 Z 值稍抬一点, 避开 J5 ≈ 0° 的姿态
• 或把 MoveL 改 MoveJ (关节插补不受奇异点影响)
• 或在示教点加 ConfigData 约束, 锁定肘 / 腕姿态

"超限 (Out Of Range)"

症状: 报 E-1021 Joint Limit Exceeded: J2 = -121.5。

原因: 目标关节角超过机械限位。

解决: 重新规划姿态; 或在 URDF 修改 limits 放宽 (仅当物理允许时)。

"使能失效 (Servo OFF)"

症状: 发运动指令后没动, 报 E-1001 Axis Not Enabled。

排查:

1. 急停按下了? 松开后再使能
2. 安全门 / 光幕触发? 复位安全回路
3. Servo 报警代码? 查看示教器日志面板
4. 机器人电源 24V / 48V 是否通?

"TCP 不在工件内"

症状: MoveL 报 E-1055 Target Out Of Workspace。

原因: 目标点在机器人工作空间之外 (距离太远或角度太偏)。

解决: 检查工件坐标系标定, 或调整机器人安装位置。

实际轨迹与示教偏差 > 2 mm

原因: 通常是 TCP 没标准, 或本体有磨损。

解决:

1. 重新做四点法校准 TCP
2. 检查夹爪是否松动
3. 做 DH 参数辨识 (20 点标定, 软件自动补偿)

下一步

• 第一个 HMI 工程 — 给机器人做一个启停 + 点位管理界面
• 部署到运行时 — 下载真机运行
• PLCopen MC 功能块 — PLC 侧机器人运动控制
• 点位管理
• 使能与急停 — 安全流程
• 视觉标定 — 视觉引导抓取前置