双机械臂系统仿真与真机控制：基于C++的UR10轨迹跟随设计

简介面向计算机/自动化等专业学生与从业者这份源码包围绕ROS与C实现双机械臂的完整控制链路既可在Gazebo中开展双机仿真也可直接连接两台真实UR10机器人运行适用于期末课程设计、大作业及毕业设计场景。压缩包共203个文件约18.8MB其中launch/xacro/urdf/rviz构成仿真与可视化配置cpp/h、yaml及py承担控制逻辑与参数调优dae/stl提供机械臂与场景三维模型另有md/pdf/txt等说明文档目录按模块组织便于对照代码理解整机控制流程。目前已有70人学习下载且项目经个人毕设答辩评审获得98分代码均已调试通过。除了可直接运行的Gazebo仿真环境和真实UR10控制示例还配套了启动方式、通信链路与排错思路说明适合入门ROS双机械臂开发者作为从仿真迁移到实物的参考模板也方便在此基础上二次开发实现不同任务。1. 双机械臂系统的任务拆解与架构选型如果毕业设计只做单臂 Gazebo 仿真答辩时大概率会被问“真机能不能跑”。这个项目没有回避这个问题一套 C 代码同时驱动仿真和两台真实 UR10仿真里验证算法真机只换底层传输。UR10 对外提供 TCP 接口控制频率和状态回传都靠它因此整个系统围绕 TCP 通信层、轨迹跟随层、ROS 接口层三部分展开。对做机器人方向的学生和从业者来说参考价值在于仿真与真机共用一个控制节点差异被收敛到 Gazebo 插件与 UR 控制柜插件的切换上而不是各写一套逻辑。本文从源码的文件结构切入讲清楚每个模块的职责再给出实际可用的指令和参数最后落在低带宽轨迹跟随的调优技巧上。适合准备机器人方向毕业设计、课程大作业以及正在从仿真转向真机控制的开发者。2. UR10 控制链路的 C 实现TCP 通信、状态回传与轨迹插值2.1 源文件职责梳理源码里几个核心文件直接对应 UR10 开发中的三件事通信、状态、轨迹。tcp_socket.cpp封装了面向 UR 控制柜的 TCP 客户端。UR 控制柜默认开启多个端口30001 是 primary interface30003 是 real-time data exchange29999 是 dashboard server30002 是 secondary interface。这个文件实现的主要是 30003 端口的读写。rt_state.cpp解析 UR 实时状态包。UR 以 125Hz 通过 30003 持续上报机械臂状态每个状态包是固定长度的二进制流包含目标位置、实际位置、电流、电压、数字输入输出、关节温度等字段。trajectory_follower.cpp与lowbandwidth_trajectory_follower.cpp这两个文件实现两种轨迹跟随策略。前者按照常规频率发送关节目标点后者降低发送频率同时通过 UR 控制柜内部的轨迹插值器平滑执行减少网络带宽占用。action_server.cpp提供 ROS action 服务端。上层调用方比如 MoveIt 的规划结果、手动示教点通过 action 接口提交一条路径action_server 负责任务分发、执行反馈和结果返回。master_board.cpp与mb_publisher.cpp读取 UR 控制柜主控板Master Board的状态比如安全信号、电源状态、抱闸状态并把状态发布为 ROS topic。ros_main.cpp节点入口负责初始化 ROS 节点、读取参数、创建各模块实例。这个分层方式的好处是边界清楚通信层只处理字节流状态层只做解析和发布轨迹层只做运动学层面的路径生成ROS 层负责和更上层的规划、决策模块对接。2.2 状态解析的思路UR 30003 端口返回的状态包结构是有固定格式的。下面是rt_state.cpp在解析实际关节角度时常用的一段逻辑// rt_state.cpp 核心解析逻辑 bool RtState::parse(const std::vectoruint8_t buffer, size_t length) { if (length 796) { // 状态包长度不满足时直接丢弃避免半包数据污染状态值 return false; } // 前 4 字节是消息长度第 4 字节之后开始按照固定结构偏移读取 // 实际关节位置在第 252 字节处开始共 6 个 double每个占 8 字节 size_t offset 252; for (int i 0; i 6; i) { joint_positions_[i] decodeDouble(buffer.data() offset); offset 8; } // 目标关节位置的实际偏移是 324 offset 324; for (int i 0; i 6; i) { joint_velocities_[i] decodeDouble(buffer.data() offset); offset 8; } return true; }这里需要说明的是UR 状态包每个字段的字节偏移在官方文档的“Real-Time Data Exchange”章节有明确表格不同 SDK 版本偏移量可能不同。实际调试时如果发现位置跳变或者解析出 NAN第一件事就是打印 buffer 的前几十个字节对照官方文档核一遍偏移。上述代码中的decodeDouble就是按小端序把 8 字节转成doubleUR 数据采用小端存储。注意 30003 端口的实时状态是以固定频率连续推送的客户端不发送请求也会收到数据关键是保证tcp_socket的接收缓冲区及时清空否则会造成旧数据堆积导致状态时间戳偏移。2.3 发送轨迹指令控制 UR10 移动通常有movej、movel、servoj三种指令它们的适用场景不同。这个项目在轨迹跟随层面用的是movej与servoj组合的方式离线规划好的路径用movej按顺序发送在线实时跟踪用servoj。发送 MoveJ 指令// trajectory_follower.cpp 中生成 movej 指令并发送 void TrajectoryFollower::sendMoveJ(const std::vectordouble joint_target, double speed, double acceleration) { std::stringstream ss; // 保留 3 位小数避免发送过长的浮点数占用带宽 ss movej([; for (size_t i 0; i joint_target.size(); i) { ss std::fixed std::setprecision(3) joint_target[i]; if (i joint_target.size() - 1) ss , ; } // a 为关节加速度t 为运动时间0 表示由 UR 控制柜内部自行规划 ss ], a std::fixed std::setprecision(3) acceleration , t std::fixed std::setprecision(3) speed ); socket_-send(ss.str()); }上述代码里speed参数实际被映射为 movej 的t参数。UR 的movej有两种执行语义一种是给a和v表示加速度和速度上限执行时间由控制柜计算另一种是给a和t表示在指定时间内到达目标点。这个项目采用后者因为双机械臂协同作业时两条机械臂的轨迹需要时间同步固定的t更容易做到达时间的对齐。实时追踪 ServoJ// trajectory_follower.cpp 中 servoj 指令生成 void TrajectoryFollower::sendServoJ(const std::vectordouble joint_ref, double lookahead_time, double gain) { std::stringstream ss; ss servoj([; for (size_t i 0; i joint_ref.size(); i) { ss std::fixed std::setprecision(3) joint_ref[i]; if (i joint_ref.size() - 1) ss , ; } // lookahead_time 是前瞻时间gain 是伺服增益系数取值范围[0,1] ss ], std::fixed std::setprecision(3) lookahead_time , std::fixed std::setprecision(3) gain ); socket_-send(ss.str()); }servoj的lookahead_time默认 0.1决定控制柜对目标轨迹的平滑程度数值越大轨迹越平滑但跟踪延迟越大gain默认 300对应此处的 0.3越大跟踪越紧但容易产生抖动。在真机调试时这两个参数需要配合调整太快会产生机械共振太慢则轨迹跟踪误差变大。2.4 Master Board 状态采集UR10 的 Master Board 是控制柜里的主控板它和底层安全回路、电源管理紧密相关。master_board.cpp通过读取控制柜状态寄存器获取电源电压、紧急停止状态、安全模块状态等信息再由mb_publisher.cpp发布成 ROS topicrostopic echo /mb_status正常启动时这个 topic 应持续输出power_on_为 true、emergency_stop_为 false 的状态。如果紧急停止被按下轨迹跟随节点应该立即停止发送指令并且进入错误恢复流程。常见的做法是在action_server中注册状态回调一旦检测到emergency_stop_变为 true就取消当前 action 目标并把机械臂保持为暂停而不是直接掉电。3. Gazebo 双机械臂仿真模型、驱动与时间同步机制3.1 仿真环境搭建的核心步骤这个项目支持双臂仿真意味着 Gazebo 世界中存在两个 UR10 模型并且它们的 ROS 命名空间是独立的。模型文件通常基于ur_description中的 URDF 或 xacro 文件组装。实际搭建时最稳妥的方式是写一个总的 launch 文件加载一个护展的 URDF其中包含两个ur10宏实例launch !-- 加载机械臂参数joint_limit 和传动参数从 ur_description/config 读取 -- param namerobot_description command$(find xacro)/xacro $(find ur_double_description)/urdf/dual_ur10.xacro / !-- arm1 和 arm2 分别使用独立的命名空间避免 topic 冲突 -- node namerobot_state_publisher_1 pkgrobot_state_publisher typerobot_state_publisher remap fromjoint_states to/arm1/joint_states/ /node node namerobot_state_publisher_2 pkgrobot_state_publisher typerobot_state_publisher remap fromjoint_states to/arm2/joint_states/ /node !-- 启动 Gazebo 世界并加载机器人 -- include file$(find gazebo_ros)/launch/empty_world.launch arg namepaused valuefalse/ arg nameuse_sim_time valuetrue/ arg namegui valuetrue/ /include /launch这里需要解释一下use_sim_time的意义。Gazebo 有独立的仿真时钟和系统真实时间不一定同步。如果不开启use_sim_timeROS 节点使用真实时间发布者频率的间隔会与 Gazebo 的迭代周期产生偏差导致机械臂在仿真中动作一顿一顿的。打开它之后所有节点统一订阅/clock保证时间一致。3.2 双机械臂的命名空间隔离双机械臂最容易踩坑的地方是 topic 冲突。两个机械臂如果在同一命名空间下发布joint_states和arm_controller/command那么控制器发布的数据会被两个臂都接收轻则运动不可控重则关节指令互相覆盖导致模型卡死。这个项目的做法是让每条臂有独立命名空间/arm1/joint_states /arm1/arm_controller/command /arm1/gazebo_ros_control/pid_gains /arm2/joint_states /arm2/arm_controller/command /arm2/gazebo_ros_control/pid_gainsgroup nsarm1 node namecontroller_spawner pkgcontroller_manager typespawner argsarm_controller joint_state_controller / /group group nsarm2 node namecontroller_spawner pkgcontroller_manager typespawner argsarm_controller joint_state_controller / /group这样 Gazebo 中两个模型各自响应自己命名空间下的/command。要注意的是gazebo_ros_control 插件加载时robot_namespace参数需要和 group ns 保持一致否则控制器和模型的映射关系会错乱。检查方法是在启动后执行# 查看两个控制器的状态正常应都显示 Running rosservice call /arm1/arm_controller/query_state rosservice call /arm2/arm_controller/query_state这两个查询输出里应该有current_state: running。如果显示not running多半是 PID 参数未正确传给控制器或者 joint name 与 URDF 中的名称对不上。3.3 仿真与真实控制代码的对接源项目里tcp_socket.cpp在 Gazebo 仿真环境下是不需要的因为 Gazebo 中机械臂执行指令走的是 ROS 控制器的follow_joint_trajectoryaction 接口而不是 TCP 发送 URScript。所以ros_main.cpp里做了一个编译期或运行期的选择// ros_main.cpp 中根据实际运行环境选择后端 if (use_gazebo_) { trajectory_follower_ std::make_sharedGazeboTrajectoryFollower(); } else { trajectory_follower_ std::make_sharedUrTrajectoryFollower(); }这两种 follower 都实现同一套接口比如sendMoveJ、sendServoJ、getRobotState。Gazebo 版本内部调用控制器发送 joint states 来驱动模型UR 版本内部调用tcp_socket发送 URScript。接口统一之后上层用 ROS action 发来一条轨迹下层根据use_gazebo_参数决定走哪条链路这样同一个action_server.cpp就能同时服务仿真和真机。3.4 双臂协同中的避碰检查双臂同时运动时如果规划过程不考虑两条臂的碰撞运动到工作空间交集处就可能互相穿透。在 Gazebo 里检查碰撞有两种做法一是开启drake或者moveit的碰撞检测但配置成本较高二是在轨迹下发前用简单几何体做球-球粗检测。这里我比较推荐后者因为毕业设计交答辩够用参数也容易调# 双臂碰撞检测脚本的核心逻辑 def check_collision(joint_arm1, joint_arm2, radius0.15): # 用正运动学求出关键点位置 points1 forward_kinematic(joint_arm1) points2 forward_kinematic(joint_arm2) for p1 in points1: for p2 in points2: dist np.linalg.norm(np.array(p1) - np.array(p2)) if dist radius: return True return False从算法逻辑看这个检测比 MoveIt 的 FCL 精确度低但胜在快适合在发送轨迹前过滤掉肉眼可见的干涉。如果后续需要更精细的判断可以切换到 MoveIt 的碰撞检测矩阵但要注意 MoveIt 对整个机械臂模型做网格碰撞计算对电脑性能要求较高仿真时容易把 Gazebo 的物理迭代拖慢。4. 连接真实 UR10网络配置、远程控制与安全限位4.1 UR10 远程控制端口真机控制中最常见的端口配置如下端口名称用途29999Dashboard Server发送power on、brake release等高级指令30001Primary Client Interface接收状态信息与程序执行反馈30002Secondary Client Interface与 primary 类似但需要二次开发30003Real-Time Data Exchange高频收发数据控制和状态采集主端口30004Real-Time Data Exchange (RTDE)可配置数据字段UR 官方推荐tcp_socket.cpp中实现的是 30003 端口通信。在机器人启动后需要先把 UR 控制柜切换到远程控制模式否则 TCP 发送过去不会执行。切换远程控制的方法是通过 Dashboard Server 发送相应指令# 连接 dashboard nc 192.168.1.50 29999 # 上电 power on # 释放刹车 brake release # 设置远程控制模式 set remote control命令执行后可以通过状态查询确认# 检查机器人是否就绪 robotmode safetystatus输出示例中Normal表示控制柜正常Robotmode: RUNNING表示机器人可接受指令。4.2 真机接入的注意事项连接真机时主控计算机和 UR 控制柜之间用网线直连PC 端 IP 设置和机器人控制柜保持同一网段比如控制柜地址为192.168.1.50则 PC 端配置192.168.1.100子网掩码255.255.255.0。ping 通之后用tcp_socket发送movej指令前建议先用dashboard把机器人置于远程控制模式。另一个容易忽略的点是客户端发送频率不要超过 UR 控制柜的承受能力。UR 控制柜的实时接口虽然有较高的更新频率但网线上如果同时跑大量数据包控制周期的稳定性会下降。此外连接到真机前必须先确认急停按钮处于释放状态。UR 控制柜上电后默认刹车未释放如果直接发送movej指令会被执行但机器人不会动并且报错信息可能不够直观。更危险的是如果程序里没有检测emergency_stop状态一旦急停触发而程序继续发送轨迹指令恢复后机械臂会继续执行旧指令。因此mb_publisher的状态回调里应当加入如下逻辑void MbPublisher::safetyCheck(const MbStatus status) { if (status.emergency_stop) { action_server_-cancelAllGoals(); trajectory_follower_-halt(); ROS_WARN(Emergency stop detected, all goals canceled.); } }4.3 两个 UR10 的部署差异双机的部署要注意两台控制柜的端口冲突。如果同一台 PC 同时连接两台 UR10注意两台机器默认的端口是一样的比如都是 30003需要确保目标 IP 不同即可端口可以相同。不过tcp_socket.cpp中如果connect失败代码逻辑应该能够识别是哪个机械臂连不上常见的实现是在Socket构造函数里加入robot_id参数TcpSocket arm1_socket(192.168.1.50, 30003, 1); TcpSocket arm2_socket(192.168.1.60, 30003, 2);5. 轨迹跟随调优低带宽模式、前瞻时间与真机一致性验证5.1 Low Bandwidth 模式的适用场景lowbandwidth_trajectory_follower.cpp解决的问题是网络压力大时高频率发送轨迹点会造成数据阻塞。常见做法是把 125Hz 的发送频率降低到 10Hz 左右但 UR 控制柜如果收到间隔过大的离散目标点机械臂会在每个点之间插值导致实际轨迹变成折线。URScript 的servoj和movej本身支持控制柜内部平滑处理所以低带宽模式的关键不是单纯降频而是让 UR 内部的轨迹插值器承担平滑重任。使用时控制频率降到 10Hz 后lookahead_time要相应加大。经验上如果发送间隔为 100mslookahead_time至少设置为 0.3 以上否则平滑效果不明显。可以按下面的参数组合测试发送频率lookahead_timegain表现125Hz0.10.3跟踪精准网络依赖高50Hz0.20.3跟踪较稳抖动小10Hz0.30.2折线感明显消失适合低速5Hz0.50.2网络压力低路径明显变圆如果使用movej的固定时间模式发送频率低到 2Hz 也不会出现明显抖动因为控制柜内部会完整规划每段运动轨迹。5.2 真机一致性验证方法仿真跑通之后到真机执行最怕是仿真中不受重力影响或者忽略了一些摩擦特性导致真机轨迹偏出去。这个项目中的验证方法很简单对比 rt_state 中上报的实际关节角和期望关节角的误差# 仿真或真机运行轨迹期间 rostopic echo -n 1 /arm1/joint_states # 查看目标轨迹 topic rostopic echo -n 1 /arm1/arm_controller/command如果两者角度差超过阈值优先检查伺服增益设置。UR10 真机上servoj的gain建议不要超过 1.0。注意 UR 官方文档中以整数百分比表示该参数300 即 3.0如果直接照搬仿真环境中的数值可能造成真机抖动。另一个有用的技巧是记录轨迹执行完成后的末端位置误差。可以让机械臂从同一位置出发执行一组随机路径完成后再回到起点记录重复定位误差。UR10 的重复定位精度通常在微米级别如果误差明显偏大问题往往出在机械结构本身或负载超过额定值。5.3 结合源码定位问题的思路当机器人运动异常时先确认rt_state有没有持续输出。如果状态在某个时刻停更大概率是 tcp 连接断开或控制柜崩溃如果状态更新但机械臂不动检查 action_server 的目标是否被某个更高层的逻辑抢占如果机械臂移动但轨迹与预期不符优先检查发送的关节角是否经过了正确的平移或坐标变换。双机械臂场景里两条机械臂通讯共用同一个 tcp 处理线程时要把 send 和 recv 分离避免一条臂发数据卡住导致另一条臂的状态读不到。将这个系统从仿真切换到真机最快的方式就是设置use_gazebo_参数并重启节点其余配合接口不变如果切换后机械臂抖动把关节加速度和速度参数先调低到0.5 rad/s左右再逐步升起来直到找到一个稳定区间。本文还有配套的精品资源点击获取