1. 从零开始：搭建你的UR5避障开发环境

大家好，我是老张，在机器人行业摸爬滚打十几年了，用过不少机械臂，UR5算是其中非常经典的一款，尤其是在结合ROS和MoveIt做避障开发的时候，它的灵活性和开源性优势就体现出来了。今天我想和你分享的，就是如何一步步搞定UR5机械臂的避障轨迹规划与执行。别被“避障”、“轨迹规划”这些词吓到，说白了，就是让机械臂在移动时，能像人一样，看到或者“感觉”到前方的障碍物，然后聪明地绕过去，而不是一头撞上去。这个过程，从环境配置、算法选择到最终执行，我会把每个环节掰开了、揉碎了讲给你听，保证你跟着做就能上手。

首先，咱们得把“战场”准备好。我强烈建议你使用Ubuntu 20.04搭配ROS Noetic，这是目前最稳定、生态最成熟的组合。很多新手一上来就卡在环境安装上，其实用对方法很简单。原始文章里提到了一个“apt安装大礼包”，这个思路很对，能省去大量编译依赖的麻烦。但我想补充得更细致一些，因为有些包你可能不知道是干嘛的，装起来心里没底。

我通常会把安装分为三个层次。第一层是UR驱动核心，没有它们，你的电脑根本认不出UR5。这包括 ros-noetic-ur-client-library 和 ros-noetic-ur-msgs，它们是UR官方提供的通信库和消息定义。第二层是运动控制基础，机械臂每个关节怎么动、以多快的速度动，靠的是 ros-noetic-joint-trajectory-controller 这类控制器。第三层才是MoveIt及其避障规划的核心，比如 ros-noetic-moveit-planners-ompl，这里面就包含了我们今天要用的多种避障规划算法。

你可以像我一样，用一条命令搞定所有，心里清楚每个包的分工：

sudo apt-get install ros-noetic-ur-client-library ros-noetic-ur-msgs ros-noetic-joint-trajectory-controller ros-noetic-scaled-joint-trajectory-controller ros-noetic-industrial-robot-status-interface ros-noetic-moveit ros-noetic-moveit-planners-ompl ros-noetic-moveit-visual-tools ros-noetic-trac-ik-kinematics-plugin

这里我特别提一下 trac-ik-kinematics-plugin，很多朋友在Rviz里设置好Planning Group后，发现那个代表机械臂末端的小球没出现，问题八成就出在缺了这个运动学插件。它比默认的KDL求解器更快、更稳定，尤其是在奇异点附近。

安装好系统依赖后，就要创建工作空间并获取UR的ROS驱动包。这里有个小坑：Universal Robots提供了两个关键的Git仓库。一个是新版的驱动（Universal_Robots_ROS_Driver），负责底层通信；另一个是旧版的模型和配置包（universal_robot），里面包含了UR5的MoveIt配置文件。这两个都需要，而且要注意分支。我建议你这样操作：

cd ~/catkin_ws/src
git clone https://github.com/UniversalRobots/Universal_Robots_ROS_Driver.git
git clone -b noetic https://github.com/ros-industrial/universal_robot.git
cd ~/catkin_ws
catkin_make

编译过程如果报错，别慌，通常就是缺某个ROS包。错误信息会明确告诉你缺什么，比如“Could not find moveit_core”，那你只需要 sudo apt install ros-noetic-moveit-core 即可。记住，把错误信息里的下划线_换成横线-去安装，这是ROS包命名的惯例。

2. 建立通信桥梁：连接真实UR5机械臂

软件环境齐了，下一步就是让电脑和真实的UR5“握手”成功。这一步是硬件实操，也是事故高发区，所以安全第一，务必确保机械臂工作范围内空旷，急停按钮触手可及。

通信的核心是网络配置。你需要用一根网线，一头插在UR5控制柜的网口，另一头插在你的电脑上（或者你电脑所在的交换机上）。原始文章提到了虚拟机桥接模式，这是虚拟机用户必须做的。如果你是物理机装的双系统，那就简单多了。

配置IP地址是关键，原理就是让电脑和机械臂在同一个局域网网段内。我习惯这么操作：

1. 先在UR5的示教器上，查看并设置机器人的静态IP。路径是“设置机器人”-“网络”。比如我设置为 192.168.1.101。这个地址就是机械臂的地址，我们记为 ROBOT_IP。
2. 然后在你的电脑上，找到连接机械臂的那个有线网络连接，手动设置IPv4地址。IP地址要设置成和机器人同一网段但不同的值，比如 192.168.1.102；子网掩码 255.255.255.0；网关可以填机器人的IP 192.168.1.101 或者留空。
3. 设置完后，打开终端，ping一下机器人的IP：ping 192.168.1.101。如果能收到回复，恭喜你，物理链路通了！

接下来是软件层面的连接。UR5需要一个运行在它控制器上的服务器程序来接收外部指令，这个程序就是 External Control。通常它已经由厂家或前人导入到示教器里了。你需要做的是：

1. 在示教器上，加载 externalcontrol.urp 程序。
2. 进入该程序的“安装设置”，找到 External Control 配置项。
3. 将里面的服务器IP地址设置为你电脑的IP（也就是刚才设置的 192.168.1.102），端口保持默认的 50002。这里最容易搞反！ 是让机器人连接你的电脑，所以IP填电脑的。
4. 确保同一设置页面里的 Ethernet/IP 是 禁用 状态，否则会有协议冲突。

现在，先在终端启动ROS驱动节点，告诉ROS机械臂在哪里：

roslaunch ur_robot_driver ur5_bringup.launch robot_ip:=192.168.1.101 limited:=true

看到终端输出“Ready to receive control commands.”之类的信息后，再回到示教器，按下播放键启动 External Control 程序。此时，驱动节点应该会显示连接成功。如果报错，检查IP地址是否填反，以及防火墙是否屏蔽了50002端口。

3. 初窥门径：在Rviz中手动规划与避障

连接成功后，我们先不急着写代码，用一个强大的可视化工具——Rviz来感受一下MoveIt的规划和避障。这能帮你建立直观印象。启动MoveIt和Rviz：

roslaunch ur5_moveit_config moveit_planning_execution.launch limited:=true
roslaunch ur5_moveit_config moveit_rviz.launch config:=true

Rviz打开后，可能会看到一堆杂乱的点云或模型，别急，我们一步步设置。首先在左侧“Global Options”下的“Fixed Frame”下拉框里，选择 base 或 base_link，这是UR5的基坐标系，选错会导致所有东西位置不对。然后点击左下角的“Add”按钮，添加一个“MotionPlanning”插件，这是MoveIt在Rviz中的控制面板。

添加成功后，在“MotionPlanning”选项卡的“Planning”组里，将“Planning Group”设置为 manipulator。这时，你应该能看到一个透明的UR5模型，以及一个由两个交互式标记（通常是小球或箭头）组成的“位姿交互工具”。一个代表末端位置，一个代表末端朝向。你可以用鼠标拖动它们，来给机械臂指定一个目标位姿。

现在我们来加入障碍物，模拟真实避障场景。 再次点击“Add”，添加“RobotModel”可以再次显示机器人模型，添加“Marker”可以绘制简单形状。更常用的方法是添加“Cube”或“Sphere”等基本形状作为障碍物。例如，添加一个Cube，然后在Rviz主窗口中用鼠标调整它的位置和大小，把它放在机械臂从A点运动到B点的必经之路上。

设置好障碍物后，回到“MotionPlanning”面板。先拖动交互工具，为机械臂设定一个起点（可以点击“Query”下的“Start State”旁边的“Update”来设置当前状态为起点），再拖动设定一个目标点。然后，关键的一步来了：点击“Planning”下的“Plan”按钮。MoveIt会调用默认的规划算法，尝试找出一条从起点到终点、且不会撞上你刚添加的立方体障碍物的路径。规划成功后，你会看到一条彩色的轨迹线显示出来。

如果规划失败（比如障碍物把路完全堵死了），Rviz可能会报“Unable to find a valid plan”。这时你可以尝试调整障碍物的位置，或者换个目标点。规划成功后，点击“Execute”，真实的UR5机械臂（以及Rviz中的模型）就会沿着这条规划好的避障轨迹运动起来。第一次执行前，请务必确认机械臂周围安全！ 可以先在Rviz里观察模型运动是否合理，再对真机执行。

这个手动操作的过程，其实就是后面我们写代码自动化的核心。你通过界面点击“Plan”和“Execute”，底层调用的就是MoveIt的API。理解了这个交互流程，写代码时思路就清晰了。

4. 算法核心：理解并选择MoveIt的避障规划器

在Rviz里能手动规划避障了，那背后到底是哪个“大脑”在算呢？这就要说到MoveIt的规划器（Planner）了。MoveIt本身不生产算法，它是算法的搬运工和调度者。默认情况下，它集成的是OMPL（Open Motion Planning Library），这是一个包含了大量先进运动规划算法的C++库。

OMPL提供了多种适用于避障的规划算法，每种都有其特点。在 /home/你的用户名/catkin_ws/src/universal_robot/ur5_moveit_config/config/ompl_planning.yaml 这个配置文件里，就定义了UR5使用哪些规划器。默认配置通常启用了一组，比如：

• RRTConnect： 这是默认的，也是我最常用、最推荐新手使用的算法。它是基于快速扩展随机树（RRT）的双向搜索改进版。你可以把它想象成从起点和终点同时长出的两棵树，努力向中间生长直到连接。它的特点是规划速度通常很快，对于一般的避障场景非常有效。
• RRT： 经典的快速探索随机树算法，单棵树从起点向目标区域生长。有时不如RRTConnect高效。
• PRM： 概率路线图法，先在整个空间（包括自由空间）随机撒点建图，然后再在图上查询路径。适合在多障碍物、复杂静态环境中进行多次规划，因为图可以重复使用。
• EST： 扩张空间树算法，另一种随机采样方法。

在代码中，你可以指定使用哪种规划器。但作为实战经验，我建议你先用默认的RRTConnect，它已经能解决80%的问题。如果发现规划时间太长（比如超过5秒）或者老是失败，再考虑换别的算法，或者调整算法参数。

那么，避障信息是怎么告诉这些算法的呢？这就要引入“规划场景（PlanningScene）”的概念。在Rviz里我们添加的Cube，就是被加入到当前的规划场景中。在代码里，你需要通过 PlanningSceneInterface 或直接向 /planning_scene 话题发布消息来添加、更新或移除障碍物。障碍物可以用简单的几何形状（立方体、球体、圆柱体）定义，也可以用复杂的网格（Mesh）文件描述，比如一个真实的工作台3D模型。

规划器在计算时，会持续查询规划场景，确保随机采样出来的路径点不在障碍物内部，并且机器人的连杆模型（通过“碰撞检测”功能）也不会与障碍物相交。这个过程是自动的，你只需要负责把障碍物的信息准确、及时地提供给规划场景即可。

5. 代码实战：编写你的第一个避障程序

理论说得再多，不如一行代码。现在，我们抛开Rviz的图形界面，用Python写一个真正的避障控制程序。我会基于原始文章的demo进行大幅增强和解释，让你知其然更知其所以然。

首先，创建一个新的ROS功能包，或者在你已有的包里创建 scripts 文件夹。我们写一个名为 ur5_obstacle_avoidance.py 的文件。

#!/usr/bin/env python
# -*- coding: utf-8 -*-

import rospy
import sys
import moveit_commander
import moveit_msgs.msg
import geometry_msgs.msg
from math import pi
from std_msgs.msg import String
from moveit_commander.conversions import pose_to_list

class UR5ObstacleAvoidance:
    def __init__(self):
        # 初始化MoveIt commander和ROS节点
        moveit_commander.roscpp_initialize(sys.argv)
        rospy.init_node('ur5_obstacle_avoidance_node', anonymous=True)

        # 实例化机器人控制对象
        robot = moveit_commander.RobotCommander()
        scene = moveit_commander.PlanningSceneInterface()
        group_name = "manipulator"
        move_group = moveit_commander.MoveGroupCommander(group_name)

        # 设置规划参数，这些参数直接影响避障效果和速度
        move_group.set_planning_time(5.0)  # 允许规划的最长时间（秒）
        move_group.set_num_planning_attempts(10)  # 规划失败后的重试次数
        move_group.set_goal_joint_tolerance(0.01)  # 关节角度容差（弧度）
        move_group.set_goal_position_tolerance(0.001) # 末端位置容差（米）
        move_group.set_goal_orientation_tolerance(0.01) # 末端姿态容差（弧度）
        # 设置最大速度和加速度缩放因子，安全第一，初次运行建议调小
        move_group.set_max_velocity_scaling_factor(0.3)
        move_group.set_max_acceleration_scaling_factor(0.2)

        # 保存实例为成员变量，方便其他方法使用
        self.move_group = move_group
        self.scene = scene
        self.robot = robot
        self.box_name = ""

    def add_obstacle_box(self):
        """在规划场景中添加一个立方体障碍物"""
        box_pose = geometry_msgs.msg.PoseStamped()
        box_pose.header.frame_id = self.move_group.get_planning_frame() # 通常为"base_link"
        box_pose.pose.orientation.w = 1.0  # 四元数，表示无旋转
        box_pose.pose.position.x = 0.3   # 障碍物在基坐标系x轴方向0.3米处
        box_pose.pose.position.y = 0.0   # 在y轴中心
        box_pose.pose.position.z = 0.2   # 离地0.2米高
        self.box_name = "worktable_obstacle"
        # 添加一个长宽高各0.1米的立方体
        self.scene.add_box(self.box_name, box_pose, size=(0.1, 0.1, 0.1))
        # 等待障碍物确实被添加（ROS异步通信，需要等待确认）
        rospy.sleep(1.0)
        rospy.loginfo("障碍物 '%s' 已添加到场景中", self.box_name)

    def go_to_joint_state(self, joint_goal_list):
        """运动到指定的关节角度，这是最直接、最不易奇异的方式"""
        self.move_group.go(joint_goal_list, wait=True)
        self.move_group.stop() # 确保运动停止
        # 检查是否真的到达了目标（在容差范围内）
        current_joints = self.move_group.get_current_joint_values()
        return all(abs(current_joints[i] - joint_goal_list[i]) < 0.01 for i in range(len(joint_goal_list)))

    def plan_cartesian_path(self, waypoints):
        """规划一条笛卡尔空间下的直线路径（末端走直线），并避障"""
        (plan, fraction) = self.move_group.compute_cartesian_path(
                                   waypoints,   # 路径点列表
                                   0.01,        # 路径点间距（米），越小越精确，计算越慢
                                   0.0)         # 避障？这里设为0，因为避障由规划场景全局保证
        # fraction 代表规划成功的比例，1.0表示100%成功
        rospy.loginfo("笛卡尔路径规划完成，成功率: %.2f%%", fraction * 100)
        return plan, fraction

    def execute_plan(self, plan):
        """执行规划好的轨迹"""
        self.move_group.execute(plan, wait=True)
        rospy.loginfo("轨迹执行完毕。")

    def main_workflow(self):
        """主工作流程：添加障碍物 -> 规划并执行避障运动"""
        rospy.loginfo("=== UR5 避障轨迹规划演示开始 ===")

        # 1. 添加障碍物
        self.add_obstacle_box()

        # 2. 让机械臂运动到一个初始位置（确保起点安全且已知）
        rospy.loginfo("运动到初始位置...")
        joint_home = [0.0, -pi/2, 0.0, -pi/2, 0.0, 0.0] # 经典的“收拢”姿态
        success = self.go_to_joint_state(joint_home)
        if not success:
            rospy.logwarn("未能准确到达初始位置，请检查。")

        # 3. 设置一个目标位姿，这个位姿在障碍物的另一侧
        target_pose = geometry_msgs.msg.Pose()
        target_pose.orientation.w = 1.0
        target_pose.position.x = 0.4  # 目标在x=0.4米处，障碍物在x=0.3米处
        target_pose.position.y = 0.2  # 目标在y方向偏移一些，让路径更明显
        target_pose.position.z = 0.3
        self.move_group.set_pose_target(target_pose)

        rospy.loginfo("规划从当前位置到目标位姿的避障路径...")
        # 4. 进行规划。move_group.plan()会考虑场景中的所有障碍物
        plan = self.move_group.plan()[1] # plan()返回一个元组，第二个元素是详细的规划结果

        # 5. 在真正执行前，我们可以在Rviz里可视化一下这个规划（可选但推荐）
        # 这里需要发布一个DisplayTrajectory消息到 /move_group/display_planned_path 话题
        # 为了简化，我们直接执行。在实际调试时，强烈建议先可视化。

        rospy.loginfo("执行避障轨迹...")
        success = self.move_group.execute(plan, wait=True)
        if success:
            rospy.loginfo("避障运动成功完成！")
        else:
            rospy.logwarn("运动执行可能未完成。")

        # 6. 清理：移除障碍物（可选）
        rospy.sleep(2)
        self.scene.remove_world_object(self.box_name)
        rospy.loginfo("障碍物已移除。")

        # 7. 让机械臂回到安全位置
        rospy.go_to_joint_state(joint_home)

        rospy.loginfo("=== 演示结束 ===")

if __name__ == '__main__':
    try:
        ur5_demo = UR5ObstacleAvoidance()
        ur5_demo.main_workflow()
    except rospy.ROSInterruptException:
        pass
    finally:
        moveit_commander.roscpp_shutdown()

这个代码比原始文章的demo复杂不少，但我加入了大量注释和实用功能。它做了几件关键事：初始化MoveIt、在场景中添加一个具体的立方体障碍物、让机械臂运动到初始位、然后规划并执行一个需要绕过该立方体的末端运动。move_group.plan() 和 move_group.execute() 是核心，它们封装了与OMPL规划器的交互以及轨迹执行的全过程。

6. 进阶技巧：动态避障与规划场景管理

上面的例子是静态避障，障碍物一开始就摆在那里。但真实场景中，障碍物可能是移动的，比如传送带上的工件，或者协作的人类工人。这就需要动态避障。

动态避障的核心是实时更新规划场景。你不能像上面那样只在开始时 add_box，然后就不管了。你需要一个持续的更新机制。通常有两种方式：

方式一：订阅传感器话题，实时更新。 比如你有一个深度相机，通过ROS发布了点云话题（如 /camera/depth/points）。你可以写一个节点，订阅这个话题，将点云数据实时地以“点云”或“Octomap”（八叉树地图）的形式添加到规划场景中。MoveIt提供了相应的接口来处理点云数据。这样，机械臂的“世界模型”就和实时看到的保持一致了。

方式二：定时或按需更新已知物体的位姿。 比如你知道障碍物是一个被另一个机器人移动的托盘。你可以通过ROS话题或服务，获取这个托盘的实时位姿，然后调用 scene.add_box() 时使用新的位姿，或者使用 scene.move_attached_object() 来更新已添加物体的位置。注意，频繁地添加/删除物体会带来开销，更新位姿是更高效的方式。

这里给一个代码片段，展示如何周期性地更新一个障碍物的位置（模拟动态物体）：

def update_obstacle_position(self):
    """模拟动态更新障碍物位置"""
    rate = rospy.Rate(10) # 10Hz更新频率
    x_pos = 0.3
    while not rospy.is_shutdown():
        # 模拟障碍物在x方向上来回移动
        x_pos += 0.01
        if x_pos > 0.5:
            x_pos = 0.3

        box_pose = geometry_msgs.msg.PoseStamped()
        box_pose.header.frame_id = self.move_group.get_planning_frame()
        box_pose.header.stamp = rospy.Time.now()
        box_pose.pose.orientation.w = 1.0
        box_pose.pose.position.x = x_pos
        box_pose.pose.position.y = 0.0
        box_pose.pose.position.z = 0.2

        # 如果障碍物已存在，则更新它；否则添加它
        if self.box_name in self.scene.get_known_object_names():
            self.scene.move_attached_object(self.box_name, box_pose)
        else:
            self.scene.add_box(self.box_name, box_pose, size=(0.1, 0.1, 0.1))

        rate.sleep()

你需要在一个独立的线程中运行这个更新函数，以免阻塞主运动规划线程。同时，规划器 move_group.plan() 在规划时，会获取当前时刻最新的规划场景快照。这意味着，如果障碍物移动得不是特别快，规划出的路径依然是有效的。

7. 避坑指南与性能调优

走通了全流程，最后来聊聊我踩过的那些坑，以及如何让你的避障系统更丝滑。

第一大坑：规划时间过长或失败。 这是最常见的问题。除了换算法（如从RRT换到RRTConnect），你可以尝试：

• 调整工作空间（Workspace）。 默认规划器会在整个关节空间随机采样，但你可以通过 move_group.set_workspace([min_x, min_y, min_z, max_x, max_y, max_z]) 来限定一个更小的三维空间盒子，这能大幅缩减采样范围，提高规划成功率。前提是你对任务空间有先验知识。
• 设置起始状态和目标状态。 确保你设置的起始关节状态（通过 move_group.set_start_state()）是准确的、无碰撞的。一个错误的起始状态会让规划器从一开始就“迷路”。
• 简化碰撞模型。 UR5的URDF模型可能包含很多细节（如电机外壳、线缆）。在MoveIt的配置中，你可以使用简化的碰撞模型（通常是连杆的包围盒），这能极大加快碰撞检测的速度。检查 ur5_moveit_config/config 下的 ur5.srdf 文件，里面定义了用于碰撞检测的简化连杆模型。

第二大坑：规划出的路径抖动、不平滑。 有时候规划出的路径虽然能避障，但关节运动曲线很突兀，机械臂会一顿一顿的。这是因为采样式规划器生成的路径是由离散点连接的。解决方法：

• 使用轨迹滤波（Trajectory Filtering）。 MoveIt提供了 pilz_industrial_motion_planner 等插件，可以在OMPL规划出的粗糙路径基础上，进行时间参数化和平滑处理，生成速度、加速度连续的运动轨迹。你可以通过 move_group.set_planner_id("PTP") 来尝试使用Pilz的PTP规划器。
• 在笛卡尔空间规划。 像我们上面代码中的 compute_cartesian_path，它能保证末端走严格的直线或插值曲线，路径自然平滑。但计算量较大，且对奇异点敏感。

第三大坑：执行时与规划不符，发生碰撞。 这可能是最危险的情况。原因包括：

• 模型误差。 你的URDF模型尺寸、DH参数与实际UR5有细微差别，或者机械臂的“零位”没有标定准确。这会导致Rviz里看着没碰，真机却撞了。务必仔细校准。
• 控制延时与动态误差。 规划时假设瞬时到达某个点，但实际电机有响应时间，高速运动时还有惯性。确保你设置了合理的最大速度 (set_max_velocity_scaling_factor) 和加速度 (set_max_acceleration_scaling_factor)，初次测试建议设在0.3以下。
• 障碍物信息不准或滞后。 动态避障时，传感器数据到规划场景的更新有延迟。如果障碍物移动很快，可能规划时它在A处，执行时它已到B处并发生了碰撞。这就需要你评估系统延迟，并引入一定的安全裕度（比如把障碍物模型膨胀得比实际大一点）。

最后的安全底线： 无论代码写得多完美，物理急停开关和软件监控程序必不可少。写一个简单的监控节点，订阅关节状态和规划场景，实时检查是否即将发生碰撞（例如，机器人与障碍物的最小距离小于某个阈值），一旦发现危险，立即通过 move_group.stop() 停止运动，并发送警报。永远不要完全相信规划器，它只是一个工具，你才是最终的安全员。

把这些点都注意到，你的UR5避障系统就能从“能跑通”进化到“稳定可靠”。这个过程需要耐心调试，每解决一个问题，你对整个系统的理解就会加深一层。机器人编程就是这样，在不断的试错和优化中成长。希望我的这些经验能帮你少走些弯路，祝你玩转UR5，开发顺利！