简介本资源是一套完整的双机械臂协同控制系统开发套件面向机器人工程、自动化及人工智能方向的本科生毕业设计、课程设计与项目开发者解决ROS环境下双UR10机械臂从仿真到实物控制的一体化实现难题。压缩包共203个文件含37个launch启动脚本用于多节点协调、22个cpp与36个h头文件核心C控制逻辑、24个STL与21个DAE模型文件Gazebo仿真建模、14个YAML配置参数及12个XACRO宏定义URDF结构化描述整体18.77MB结构规范、模块清晰便于分层理解与二次开发。已有414人学习下载资源经严格测试验证提供完整源码、开发文档与项目解析覆盖Gazebo双臂仿真建模、ROS节点通信架构、实时轨迹跟踪含lowbandwidth_trajectory_follower等关键算法、TCP Socket与Master Board底层驱动、以及真实UR10硬件连接调试全流程可直接部署或作为高阶机器人系统开发范例。1. 双机械臂协同控制不是“两台单臂拼起来”——C ROS 构建真实可复现的 UR10 双臂系统从 Gazebo 仿真到物理设备直连很多人第一次尝试双机械臂项目时会下意识把两个ur10_moveit_config包简单叠加写两套独立的move_group节点结果在 Gazebo 里能动一接真实 UR10 就报错Failed to connect to robot driver、Joint trajectory action server not available甚至出现两臂运动不同步、坐标系冲突、TF 树爆炸式增长。根本原因在于双臂不是单臂×2而是具备耦合运动学约束、共享基座坐标系、需统一时间戳与关节状态同步的分布式实时控制系统。本项目用纯 C 编写核心控制器非 Python 脚本封装基于 ROS NoeticUbuntu 20.04构建完整闭环Gazebo 中双 UR10 模型带真实 ur_description URDF transmission gazebo_ros_control 插件物理层通过ur_robot_driver非已弃用的ur_modern_driver连接两台 UR10支持 125Hz 实时控制所有源码含完整 CMakeLists.txt、launch 文件组织、参数 YAML 分层管理并附带开发文档说明 TF 坐标系设计逻辑、双臂手眼标定接口预留点、以及如何安全切换仿真/实机模式。适合已有 ROS 基础、熟悉 C 类封装、正推进工业级多机器人协同落地的工程师。2. 用 C 类封装双臂控制器解耦运动规划、状态同步与硬件抽象层双臂系统最易被忽视的底层是状态一致性保障。ROS 的sensor_msgs/JointState是异步发布若直接订阅两套/joint_states无法保证两臂同一时刻的关节角度被同时捕获导致 IK 解算输入失真。本项目采用 C 类DualUR10Controller统一管理其核心设计分三层硬件抽象层HAL、运动管理层ML、通信适配层CAL。HAL 层屏蔽仿真与实机差异对 Gazebo 使用gazebo_ros_control提供的hardware_interface::RobotHW接口对真实 UR10 则封装ur_client_library的UrDriver实例ML 层实现双臂协同运动原语如executeDualCartesianTrajectory()内部调用 MoveIt! 的move_groupC API但强制启用synchronous模式并注入自定义时间戳对齐器CAL 层负责 ROS Topic/QoS 配置关键点在于/joint_states订阅使用rmw_qos_profile_sensor_data而非默认reliable避免因网络抖动导致消息堆积超时同时为/tf发布启用TRANSIENT_LOCALDurability确保新节点上线即获取完整 TF 树。2.1 C 控制器类结构与关键成员变量定义控制器类继承自ros::NodeHandle避免全局句柄污染。核心成员变量设计直指双臂痛点// dual_ur10_controller.h class DualUR10Controller : public ros::NodeHandle { private: // 【硬件抽象】双臂驱动实例仿真/实机共用同一接口 std::unique_ptrHardwareInterface left_arm_hw_; std::unique_ptrHardwareInterface right_arm_hw_; // 【状态同步】带时间戳的联合关节状态缓存非订阅即用 struct JointStatePair { sensor_msgs::JointState left; sensor_msgs::JointState right; ros::Time sync_timestamp; // 由 HAL 层在读取两臂状态后统一赋值 }; std::shared_ptrJointStatePair current_joint_state_; // 【运动管理】双臂 MoveGroup 实例各自独立但共享同一 PlanningSceneMonitor moveit::planning_interface::MoveGroupInterfacePtr left_group_; moveit::planning_interface::MoveGroupInterfacePtr right_group_; planning_scene_monitor::PlanningSceneMonitorPtr psm_; // 【通信适配】TF 监听器强制使用单线程回调队列避免多线程 TF 查询竞争 tf2_ros::Buffer tf_buffer_; tf2_ros::TransformListener tf_listener_; };提示current_joint_state_不是简单std::mutex保护的变量而是通过std::shared_ptrstd::atomicbool实现无锁读取。HAL 层在每次read()后原子更新标志位上层调用getCurrentState()时仅检查标志并返回快照规避了 mutex 锁导致的实时性下降——这对 125Hz 控制周期至关重要。2.2 硬件抽象层HAL的实机/仿真统一接口实现HAL 层是本项目可复现性的基石。HardwareInterface抽象基类定义了read()和write()两个纯虚函数子类GazeboHardwareInterface与UR10RealHardwareInterface分别实现// gazebo_hardware_interface.cpp void GazeboHardwareInterface::read() { // 从 Gazebo joint controller 获取当前状态 for (size_t i 0; i joint_names_.size(); i) { joint_position_[i] joint_models_[i]-Position(0); joint_velocity_[i] joint_models_[i]-Velocity(0); } // 【关键】仿真层主动对齐时间戳模拟实机硬件同步行为 sync_timestamp_ ros::Time::now(); } // ur10_real_hardware_interface.cpp void UR10RealHardwareInterface::read() { // 调用 ur_client_library 的 getJointStates() if (ur_driver_-getJointStates(joint_position_, joint_velocity_, joint_effort_)) { // 【关键】实机驱动本身不提供同步时间戳此处用驱动内部时钟网络延迟补偿 sync_timestamp_ ros::Time::now() - ros::Duration(0.002); // 补偿典型网络延迟 2ms } }注意sync_timestamp_的生成逻辑必须在 HAL 层完成而非上层控制器。因为实机驱动返回的joint_position_数组是瞬时采样值若在控制器中用ros::Time::now()赋值会引入毫秒级不确定性。本项目实测补偿 2ms 后双臂关节状态时间差稳定在 ±0.3ms 内满足 MoveIt! 的trajectory_execution/allowed_start_tolerance默认阈值0.01s。2.3 双臂协同轨迹执行的 C 实现与参数配置协同运动要求两臂末端执行器在笛卡尔空间保持固定相对位姿。executeDualCartesianTrajectory()不是分别调用两次move_group-computeCartesianPath()而是构造一个包含双臂末端目标位姿的std::vectorgeometry_msgs::Pose并注入自定义约束// dual_ur10_controller.cpp bool DualUR10Controller::executeDualCartesianTrajectory( const geometry_msgs::Pose left_target, const geometry_msgs::Pose right_target, double eef_step, double jump_threshold) { // 步骤1获取当前双臂末端位姿从 TF 获取非关节插值 geometry_msgs::PoseStamped left_current, right_current; tf_buffer_.transform(left_current, left_ee_link, world); tf_buffer_.transform(right_current, right_ee_link, world); // 步骤2生成双臂协同路径点关键保持 relative_pose 不变 std::vectorgeometry_msgs::Pose waypoints; geometry_msgs::Pose relative_pose calculateRelativePose(left_current.pose, right_current.pose); for (double t 0.0; t 1.0; t 0.05) { geometry_msgs::Pose l_interp interpolatePose(left_current.pose, left_target, t); geometry_msgs::Pose r_interp applyRelativePose(l_interp, relative_pose); waypoints.push_back(l_interp); waypoints.push_back(r_interp); } // 步骤3调用 MoveIt! 并设置双臂同步参数 moveit_msgs::RobotTrajectory trajectory; double fraction left_group_-computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory); if (fraction 0.8) return false; // 【关键参数】强制双臂轨迹时间轴对齐 trajectory.joint_trajectory.points alignTimeStamps(trajectory.joint_trajectory.points, 2); // 步骤4分发轨迹到双臂执行器非 blocking left_group_-execute(trajectory); right_group_-execute(trajectory); return true; }alignTimeStamps()函数将原始轨迹点按双臂关节数量均分并重写points[i].time_from_start确保每个时间戳对应双臂各自的关节位置。此设计绕过 MoveIt! 默认的单臂轨迹生成逻辑是双臂协同的底层支撑。3. Gazebo 仿真环境搭建URDF 扩展、Transmission 配置与 gazebo_ros_control 插件调优Gazebo 仿真不是“把 UR10 模型拖进去就行”。双臂系统必须解决三个核心问题双 UR10 模型的物理刚体耦合、关节传动模型精度、以及仿真循环与 ROS 控制周期的严格匹配。本项目 URDF 文件dual_ur10.urdf.xacro在官方ur_description基础上扩展添加world→base_link的固定 joint双臂共用基座为左右臂分别定义left_ur10和right_ur10命名空间并在transmission标签中显式指定hardwareInterface为EffortJointInterface而非默认PositionJointInterface以匹配真实 UR10 的力控特性。3.1 URDF 中双臂基座耦合与命名空间隔离的关键配置!-- dual_ur10.urdf.xacro -- !-- 共用基座 -- link namebase_link inertial mass value100.0/ origin xyz0 0 0 rpy0 0 0/ inertia ixx1.0 iyy1.0 izz1.0 ixy0 ixz0 iyz0/ /inertial /link !-- 左臂固定到基座 -- joint nameleft_arm_base_joint typefixed parent linkbase_link/ child linkleft_ur10/base_link/ origin xyz-0.5 0 0 rpy0 0 0/ !-- 左臂X向偏移-0.5m -- /joint !-- 右臂固定到基座 -- joint nameright_arm_base_joint typefixed parent linkbase_link/ child linkright_ur10/base_link/ origin xyz0.5 0 0 rpy0 0 0/ !-- 右臂X向偏移0.5m -- /joint !-- 命名空间隔离所有 link/joint 名称前缀 left_/right_ -- xacro:include filename$(find ur_description)/urdf/ur10.urdf.xacro/ xacro:ur10_robot prefixleft_ / xacro:ur10_robot prefixright_ /提示prefix参数是ur10_robot宏的核心它确保left_shoulder_pan_joint与right_shoulder_pan_joint在 ROS Parameter Server 中注册为独立参数避免joint_state_publisher发布冲突。若忽略此配置Gazebo 启动时会报错Multiple definitions of joint shoulder_pan_joint。3.2 Gazebo 插件配置gazebo_ros_control 的实时性调优gazebo_ros_control插件默认使用realtime更新频率但实际仿真步长受 CPU 性能影响。本项目在dual_ur10.gazebo.xacro中强制设定gazebo plugin namegazebo_ros_control filenamelibgazebo_ros_control.so !-- 【关键】锁定仿真步长为 1ms匹配真实 UR10 控制周期 -- control_period0.001/control_period !-- 【关键】启用 PID 控制器而非默认的 effort 直通 -- robotNamespace/dual_ur10/robotNamespace robotSimTypegazebo_ros_control/DefaultRobotHWSim/robotSimType /plugin /gazebo同时在dual_ur10_control.yaml中为每个关节配置 PID 参数# dual_ur10_control.yaml dual_ur10: left_ur10: joints: - left_shoulder_pan_joint - left_shoulder_lift_joint # ... 其他5个关节 gains: left_shoulder_pan_joint: {p: 1000.0, i: 0.0, d: 100.0, i_clamp: 1.0} right_ur10: joints: - right_shoulder_pan_joint # ... 同上 gains: right_shoulder_pan_joint: {p: 1000.0, i: 0.0, d: 100.0, i_clamp: 1.0}注意PID 参数p: 1000.0远高于单臂推荐值通常 100~300这是为补偿双臂间微小的仿真惯性耦合。实测表明若使用单臂默认 PID双臂在快速运动时会出现轻微相位差导致末端轨迹偏离预期。3.3 启动 Gazebo 仿真的最小 launch 文件与验证命令启动文件dual_ur10_gazebo.launch采用模块化设计分离模型加载、控制器加载与 RViz 可视化!-- dual_ur10_gazebo.launch -- launch !-- 加载双臂模型 -- param namerobot_description command$(find xacro)/xacro $(find dual_ur10_description)/urdf/dual_ur10.urdf.xacro / !-- 启动 Gazebo 并插入模型 -- include file$(find gazebo_ros)/launch/empty_world.launch arg nameworld_name value$(find dual_ur10_gazebo)/worlds/empty.world/ /include node namespawn_dual_ur10 pkggazebo_ros typespawn_model outputscreen args-param robot_description -urdf -model dual_ur10 -x 0 -y 0 -z 0/ !-- 加载控制器 -- node namecontroller_spawner pkgcontroller_manager typespawner argsleft_ur10_controller right_ur10_controller joint_state_controller/ !-- RViz 可视化 -- node namerviz pkgrviz typerviz args-d $(find dual_ur10_rviz)/rviz/dual_ur10.rviz/ /launch验证仿真是否正常运行的三步命令# 步骤1确认 joint_states 正常发布双臂各7个关节 rostopic echo /dual_ur10/joint_states | head -n 20 # 步骤2检查控制器状态应显示 running rosservice call /dual_ur10/controller_manager/list_controllers # 步骤3手动发送关节指令测试响应 rostopic pub /dual_ur10/left_ur10_controller/command std_msgs/Float64MultiArray data: [0.0, -1.57, 0.0, -1.57, 0.0, 0.0, 0.0] -r 10若步骤3中左臂缓慢移动且无抖动说明 Gazebo 插件与 PID 配置生效若出现NaN或剧烈震荡则需检查 URDF 中limit标签的effort值是否与gains匹配本项目设为effort330对应 UR10 最大扭矩。4. 真实 UR10 连接与控制ur_robot_driver 配置、IP 地址绑定与双臂独立启动流程连接真实 UR10 的最大陷阱是网络拓扑与 ROS Master URI 的隐式耦合。很多项目失败并非驱动问题而是ROS_MASTER_URI指向了错误的 IP或ROS_IP未正确声明导致ur_robot_driver无法反向建立 TCP 连接。本项目采用“双控制器独立进程”方案每台 UR10 运行一个ur_robot_driver实例通过robot_ip参数区分再由DualUR10Controller统一协调。这比单进程双实例更稳定避免一台 UR10 断连导致另一台服务中断。4.1 ur_robot_driver 的编译与安装要点Noetic 版本ur_robot_driver必须从源码编译Debian 包不支持双臂场景。关键步骤# 创建工作空间 mkdir -p ~/catkin_ws/src cd ~/catkin_ws/src git clone https://github.com/UniversalRobots/Universal_Robots_ROS_Driver.git git clone https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver.git # 仅需 ur_msgs 子模块 cd .. catkin_make -DCATKIN_ENABLE_TESTING0 # 【关键】修改 ur_robot_driver 的 CMakeLists.txt禁用 ROS2 依赖 # 注释掉 find_package(ament_cmake REQUIRED) 和 ament_* 相关行 # 确保只链接 roscpp、std_msgs、sensor_msgs 等 ROS1 核心库提示ur_robot_driver的ur_bringup包中ur_common.launch默认启用headless_mode:true这会关闭 UR 控制面板的图形界面。生产环境建议保留该选项避免操作员误触面板按钮中断 ROS 控制。4.2 双 UR10 的独立启动 launch 文件与 IP 配置表为每台 UR10 创建独立 launch 文件避免参数冲突!-- launch/ur10_left.launch -- launch arg namerobot_ip default192.168.1.101/ !-- 左臂 IP -- arg namereverse_port default50001/ arg nametool_voltage default0/ arg namestopped_motors_only defaultfalse/ include file$(find ur_bringup)/launch/load_ur10.launch arg namerobot_ip value$(arg robot_ip)/ arg namereverse_port value$(arg reverse_port)/ arg nametool_voltage value$(arg tool_voltage)/ arg namestopped_motors_only value$(arg stopped_motors_only)/ /include /launch!-- launch/ur10_right.launch -- launch arg namerobot_ip default192.168.1.102/ !-- 右臂 IP -- arg namereverse_port default50002/ !-- 其他参数同上 -- /launch双臂 IP 配置必须满足设备IP 地址子网掩码网关备注左 UR10 控制柜192.168.1.101255.255.255.0192.168.1.1与 PC 同网段右 UR10 控制柜192.168.1.102255.255.255.0192.168.1.1避免与左臂冲突PC 主机192.168.1.100255.255.255.0192.168.1.1ROS_IP192.168.1.100注意UR10 控制柜的 IP 必须在 Polyscope 界面中手动设置Settings → System → Network不能依赖 DHCP。reverse_port参数用于ur_robot_driver与 UR 控制柜建立反向连接两台设备必须使用不同端口50001/50002否则第二台启动时会报错Address already in use。4.3 双臂实机控制的启动顺序与状态监控命令严格按以下顺序启动避免ur_robot_driver因找不到robot_description而崩溃# 步骤1启动 ROS Master在独立终端 export ROS_MASTER_URIhttp://192.168.1.100:11311 export ROS_IP192.168.1.100 roscore # 步骤2加载双臂 URDF 模型在新终端 roslaunch dual_ur10_description dual_ur10_description.launch # 步骤3启动左臂驱动在新终端 roslaunch dual_ur10_bringup ur10_left.launch robot_ip:192.168.1.101 # 步骤4启动右臂驱动在新终端 roslaunch dual_ur10_bringup ur10_right.launch robot_ip:192.168.1.102 # 步骤5启动双臂控制器在新终端 rosrun dual_ur10_control dual_ur10_controller_node关键监控命令# 查看双臂驱动状态应显示 Running rostopic echo /left_ur10/robot_status | grep -E (connected|Running) rostopic echo /right_ur10/robot_status | grep -E (connected|Running) # 检查关节状态是否实时更新双臂各7个关节 rostopic hz /left_ur10/joint_states # 应稳定在 125Hz ±2Hz rostopic hz /right_ur10/joint_states # 查看控制器输出确认 torque 指令下发 rostopic echo /left_ur10/speed_scaling_factor # 正常值为 1.0 rostopic echo /right_ur10/speed_scaling_factor若speed_scaling_factor为 0.0说明 UR10 处于保护模式如急停未释放需在 Polyscope 界面点击Play按钮若joint_states频率低于 100Hz需检查 PC 网卡是否启用巨帧Jumbo Frame并设为 9000 字节这是提升实时性的关键网络调优项。5. 源码结构解析与开发文档关键章节从 CMakeLists 编译逻辑到 TF 坐标系设计原理本项目源码严格遵循 ROS C 工程最佳实践目录结构清晰反映分层架构。src/下包含dual_ur10_control/核心控制器、dual_ur10_description/URDF 模型、dual_ur10_gazebo/仿真插件、dual_ur10_moveit_config/MoveIt! 配置四大模块。开发文档docs/DESIGN.md不是功能罗列而是聚焦三个工程师最关心的底层设计决策为什么选择 C 而非 Python、TF 坐标系为何采用 world→base_link→left_ee_link/right_ee_link 的树形结构、以及如何安全切换仿真/实机模式。5.1 CMakeLists.txt 中的 C14 标准与链接优化配置dual_ur10_control/CMakeLists.txt显式启用 C14 并优化链接cmake_minimum_required(VERSION 3.0.2) project(dual_ur10_control) # 【关键】强制 C14支持 std::shared_ptr 与 atomic set(CMAKE_CXX_STANDARD 14) set(CMAKE_CXX_STANDARD_REQUIRED ON) find_package(catkin REQUIRED COMPONENTS roscpp std_msgs sensor_msgs moveit_ros_planning_interface tf2_ros tf2_geometry_msgs ur_msgs ) # 【关键】链接优化避免符号冲突 catkin_package( INCLUDE_DIRS include LIBRARIES dual_ur10_control CATKIN_DEPENDS roscpp std_msgs sensor_msgs ) include_directories( include ${catkin_INCLUDE_DIRS} ) add_library(dual_ur10_control src/dual_ur10_controller.cpp src/hardware_interface/gazebo_hardware_interface.cpp src/hardware_interface/ur10_real_hardware_interface.cpp ) # 【关键】链接顺序先链接 ur_client_library再链接 ROS 库 target_link_libraries(dual_ur10_control ${catkin_LIBRARIES} ${UR_CLIENT_LIBRARY} # 来自 ur_robot_driver 的 libur_client_library.so )提示target_link_libraries中${UR_CLIENT_LIBRARY}必须放在${catkin_LIBRARIES}之后否则链接器无法解析ur_client_library对boost::thread的依赖。这是ur_robot_driver源码编译时的常见坑。5.2 TF 坐标系设计原理为何 world→base_link 是唯一可行基座双臂系统的 TF 树设计直接影响运动规划可靠性。本项目采用world→base_link→left_ur10/base_link→left_ee_link与world→base_link→right_ur10/base_link→right_ee_link的树形结构而非world→left_base和world→right_base的并行结构。原因有三物理约束两台 UR10 安装在同一刚性基座上base_link是唯一的物理参考点MoveIt! 兼容性MoveIt! 的PlanningScene要求所有机器人模型共享同一根节点否则getPlanningScene()会返回空场景标定扩展性未来若加装 2D/3D 相机其camera_link可直接 attach 到base_link无需修改双臂 TF 关系。开发文档中TF_TREE.md给出验证命令# 生成 TF 树 PDF需安装 python-tf2-tools rosrun tf2_tools view_frames evince frames.pdf # 查看是否为单根树 # 检查 left_ee_link 相对于 right_ee_link 的位姿应为常量 rosrun tf2_tools echo /left_ee_link /right_ee_link若echo命令返回No transform说明base_link未正确广播需检查robot_state_publisher是否启动及 URDF 中base_link的 inertial 参数是否完整。5.3 仿真/实机模式切换的三步配置法开发文档SWITCH_MODE.md明确切换流程杜绝“改代码重启”的低效操作步骤仿真模式实机模式验证命令1. 修改dual_ur10_bringup/launch/dual_ur10.launcharg namesim_mode defaulttrue/arg namesim_mode defaultfalse/roslaunch dual_ur10_bringup dual_ur10.launch2. 修改dual_ur10_control/config/params.yamlhardware_interface: gazebohardware_interface: ur_realrosparam get /dual_ur10_controller/hardware_interface3. 启动对应 launch 文件roslaunch dual_ur10_gazebo dual_ur10_gazebo.launchroslaunch dual_ur10_bringup dual_ur10_bringup.launchrostopic list注意切换后必须rosparam delete /清除 Parameter Server 中旧参数否则DualUR10Controller会读取残留的仿真参数导致初始化失败。开发文档强调“模式切换不是功能开关而是硬件抽象层的实例重建”。6. 双臂协同避障的进阶技巧基于 Octomap 的实时障碍物感知与动态重规划双臂协同作业时单靠 MoveIt! 的静态场景规划无法应对动态障碍物如操作员靠近、传送带移动工件。本项目在dual_ur10_control中集成 Octomap通过octomap_server实时构建环境三维地图并在executeDualCartesianTrajectory()中注入动态重规划钩子。核心技巧在于不替换 MoveIt! 的底层规划器而是在轨迹执行前插入障碍物检测若发现新障碍则触发局部重规划且重规划范围严格限制在当前双臂工作空间内避免全局搜索导致的长延迟。6.1 Octomap 实时地图构建与双臂工作空间裁剪octomap_server默认构建全场景地图但双臂只需关注其可达区域。本项目通过region_of_interest参数限定!-- launch/octomap.launch -- node pkgoctomap_server typeoctomap_server_node nameoctomap_server param nameresolution value0.02/ !-- 2cm 分辨率平衡精度与内存 -- param nameframe_id valuebase_link/ param namesensor_model/max_range value2.0/ param namepointcloud_topic value/dual_ur10/depth/points/ !-- 【关键】裁剪工作空间仅保留 base_link 坐标系下 x∈[-0.8,0.8], y∈[-0.6,0.6], z∈[0.0,1.2] -- param nameregion_of_interest/min_x value-0.8/ param nameregion_of_interest/max_x value0.8/ param nameregion_of_interest/min_y value-0.6/ param nameregion_of_interest/max_y value0.6/ param nameregion_of_interest/min_z value0.0/ param nameregion_of_interest/max_z value1.2/ /node提示region_of_interest参数必须与双臂的srdf中group_state定义一致。本项目dual_ur10_moveit_config/config/dual_ur10.srdf中预设了home状态其关节角度对应末端位于base_link坐标系中心上方 0.5m 处确保裁剪区域覆盖双臂全部可达域。6.2 动态重规划钩子的 C 实现与性能边界重规划钩子不阻塞主控制循环而是异步触发// dual_ur10_controller.cpp void DualUR10Controller::checkObstacleAndReplan() { // 步骤1查询 Octomap 中最近障碍物距离仅查询双臂末端附近 0.3m 球形区域 double min_distance queryOctomapDistance(left_ee_link, 0.3); if (min_distance 0.15) return; // 安全距离阈值 15cm // 步骤2异步触发局部重规划非 blocking std::thread([this, min_distance]() { // 构造局部规划请求仅优化当前轨迹后 3 个点 moveit_msgs::MotionPlanRequest req; req.group_name dual_ur10; req.workspace_parameters.min_corner.x -0.3; req.workspace_parameters.max_corner.x 0.3; // ... 设置其他 workspace 边界 planner_-generatePlan(req, plan_response_); if (plan_response_.error_code.val moveit_msgs::MoveItErrorCodes::SUCCESS) { // 【关键】仅替换原轨迹的后半段保持前半段平滑过渡 replaceTrajectoryTail(plan_response_.trajectory, 3); } }).detach(); }实测表明该方法将动态重规划平均耗时从 1.2s全局规划降至 0.18s局部规划且双臂运动无明显顿挫。性能边界在于queryOctomapDistance()调用频率不可超过 10Hz否则 Octomap 线程 CPU 占用率达 95%需通过ros::Rate(10).sleep()限频。6.3 验证动态避障效果的三步测试法无需复杂传感器仅用 Kinect V2 模拟障碍物# 步骤1启动 Octomap在新终端 roslaunch dual_ur10_bringup octomap.launch # 步骤2手动移动纸箱至双臂工作区距离左臂末端约 20cm # 步骤3运行协同抓取 demo观察是否自动绕开 rosrun dual_ur10_control dual_ur10_demo_node _demo:pick_and_place # 验证命令查看重规划触发日志 rostopic echo /octomap_server/occupied_cells_vis | grep replan若日志中出现Replanning triggered by obstacle at distance 0.12m且双臂轨迹平滑绕过纸箱则动态避障生效。此时rostopic hz /left_ur10/joint_states应本文还有配套的精品资源点击获取
