fanuc 机器人通过PR寄存器实现轨迹控制

FANUC 机器人 PR 寄存器（Position Register）轨迹控制完整详解 + 实操方案

结合你现在的 ROS2 + MoveIt2 Servo + 机械臂驱动 场景，我一次性讲透：PR 是什么、怎么存轨迹、怎么逐点执行、怎么通过外部（ROS）下发 PR 轨迹，完全对应你当前项目。

一、先搞懂：FANUC PR 寄存器是什么？

PR = Position Register 位置寄存器 FANUC 机器人系统内置的6 轴笛卡尔位姿寄存器 ，每个 PR 存储：X,Y,Z,W,P,R（笛卡尔直角坐标 + 姿态欧拉角）+ 关节角度，单位：mm、deg

PR [1]、PR [2]、PR [3]...PR [999] 一共几百个位置点
可以存储整条轨迹的所有路径点
KAREL、TP 程序、外部以太网（Socket/PLC/ROS）都可以读写 PR
机器人直接用 PR 点做连续轨迹运动（J/L/C 圆弧）

你 MoveIt 规划 / Servo 输出的每条轨迹点 → 写入对应 PR 寄存器 → 机器人逐 PR 点执行 = PR 轨迹控制

二、PR 寄存器基础结构（FANUC 标准）

每个 PR [i] 完整数据：

plaintext

复制代码

PR[i]={
X:  mm,   Y:  mm,   Z:  mm,
W:  deg,  P:  deg,  R:  deg,
J1: deg, J2: deg, J3: deg, J4: deg, J5: deg, J6: deg
}

笛卡尔位姿（工具坐标系 / 世界坐标系）
6 个关节角度（直接对应你 ROS 里 JM0~YB）
可读写、可掉电保存

三、FANUC 用 PR 寄存器执行轨迹的 3 种方式（你最常用第 3 种）

方式 1：TP 示教程序（手动示教轨迹）

karel

复制代码

L PR[1] 1000mm/min FINE
L PR[2] 1000mm/min CNT50
L PR[3] 1000mm/min CNT50

逐 PR 点直线连续运动 = 完整轨迹

方式 2：KAREL 程序批量加载 PR 轨迹（内部自动执行）

把整条轨迹点批量写入 PR1~PRn，循环运动

方式 3：外部 ROS2 实时下发 PR 轨迹（你的项目方案）

ROS (MoveIt/Servo 规划轨迹)→ 转换 FANUC PR 格式（弧度→角度、米→mm、坐标转换）→ 以太网 Socket 发送给 FANUC 控制器→ KAREL 程序接收→写入 PR 寄存器→ 机器人自动逐 PR 点连续执行轨迹

这就是你现在要做的：ROS2 ↔ FANUC PR 轨迹控制

四、核心：ROS 轨迹 → FANUC PR 格式转换（你代码里必须做）

你 ROS 里 MoveIt/Servo 输出：joint_trajectory：弧度、米、base_link 关节角

FANUC PR 寄存器需要：角度 (deg)、mm、工具 / 世界笛卡尔、关节角 deg

转换公式（直接复制到你驱动代码）

cpp

运行

复制代码

// ROS弧度(rad) → FANUC PR角度(deg)
double rad2deg(double rad){ return rad * 180.0 / M_PI; }

// ROS米(m) → FANUC PR毫米(mm)
double m2mm(double m){ return m * 1000.0; }

PR 关节点对应你的 6 轴：

plaintext

复制代码

ROS: JM0 JM1-2 JM4-3 JM4 JM5 YB
FANUC PR关节: J1 J2 J3 J4 J5 J6

五、完整 PR 轨迹控制流程（适配你现有 ROS2 工程）

你的现有链路：MoveGroup规划 / Servo实时伺服 → /arm_controller/joint_trajectory → 你的myrobot_driver

改成 FANUC PR 轨迹链路：

MoveIt/Servo 输出 JointTrajectory 轨迹点
你的驱动myrobot_driver接收轨迹
每个轨迹点转成 FANUC PR 格式（rad→deg、m→mm）
Socket 发送 PR 编号 + PR 位姿给 FANUC 控制器
FANUC KAREL 程序接收数据 → 写入 PR [1],PR [2],PR [3]...PR [n]
FANUC 执行连续运动指令：L PR[1] → L PR[2] → L PR[3]...
机器人按 PR 轨迹平滑运行

六、FANUC KAREL 读写 PR 寄存器语法（核心指令）

1. 读取 PR 寄存器

karel

复制代码

POSITION pr_pos
pr_pos = PR[1]

2. 写入 PR 寄存器（ROS 下发数据用这个）

karel

复制代码

PR[2] = pr_pos

3. 按 PR 执行直线轨迹（连续平滑）

karel

复制代码

FOR i=1 TO point_num
  L PR[i] speed CNT50  ;连续轨迹，不停止
ENDFOR

CNT50：拐角过渡（轨迹平滑，Servo 同款连续运动）
FINE：到点停止（点位运动）
speed：速度 mm/min

七、对应你现有 C++ 驱动代码修改（最小改动）

你现在驱动是下发自定义 MotorControl，改成下发 PR 轨迹给 FANUC ：在你的trajectoryCallback里追加 PR 发送逻辑：

cpp

运行

复制代码

void trajectoryCallback(const trajectory_msgs::msg::JointTrajectory::SharedPtr msg)
{
    if (msg->points.empty()) return;
    auto last_point = msg->points.back();

    // 1. ROS弧度 → FANUC PR角度deg
    std::vector<double> pr_joint(6);
    for(int i=0;i<6;i++){
        pr_joint[i] = rad2deg(last_point.positions[i]);
    }

    // 2. 组装PR寄存器数据 PR[id]={J1,J2,J3,J4,J5,J6}
    // 3. Socket发送PR编号+6轴角度给FANUC控制器
    send_fanuc_pr_data(pr_id++, pr_joint);

    // 原有RVIZ发布不变
    joint_state_.position = last_point.positions;
    joint_state_pub_->publish(joint_state_);
}

八、关键原理解答（你之前的疑问）

为什么 RVIZ 能动、机器人不动？

RVIZ：看 ROS /joint_states（软件目标位姿）
FANUC 机器人：只认 PR 寄存器里的真实执行位姿
你之前只发 ROS 话题，没写入 FANUC PR 寄存器 → 机器人不动作

PR 轨迹控制优势（FANUC 官方推荐）

连续平滑轨迹（CNT 拐角，适配 MoveIt Servo 实时伺服）
支持高速实时下发（30~100Hz，和你 Servo 33Hz 完全匹配）
支持奇异点、限位、碰撞监控（机器人本体安全）
外部 ROS/PLC 最稳定的轨迹控制方式，优于直接关节指令

九、最简落地步骤（你现在直接做）

FANUC 机器人开启外部以太网 Socket 通信、开启 KAREL 权限
编写极简 KAREL 程序：监听 Socket → 接收 ROS 数据 → 写入 PR 寄存器 → 自动逐 PR 执行轨迹
修改你myrobot_driver.cpp：轨迹转 PR 格式 + Socket 发送
MoveGroup 规划 / Servo 实时指令 → 自动写入 FANUC PR → 机器人运行

十、总结（一句话）

FANUC PR 寄存器轨迹控制 = 把 ROS MoveIt/Servo 的每条轨迹点，转换成 FANUC 标准位姿写入 PR 寄存器，机器人按 PR 序列连续直线运动；RVIZ 显示 ROS 目标，机器人执行 PR 真实轨迹。