ROS机械臂实战：用MoveIt!实现UR5避障轨迹规划（附Python代码）

ROS机械臂实战：用MoveIt!实现UR5避障轨迹规划（附Python代码）

如果你正在为工业机械臂开发复杂的应用，尤其是在充满障碍物的动态环境中，那么运动规划与避障能力就是你必须跨越的技术门槛。过去，开发者需要从零开始实现碰撞检测、路径搜索和轨迹优化，这不仅耗时费力，而且极易出错。如今，借助ROS生态中的MoveIt!框架，我们可以将精力聚焦于应用逻辑本身，而非底层算法的实现。本文将以经典的UR5六轴协作机械臂为例，深入探讨如何利用MoveIt!在复杂场景下实现高效、可靠的避障轨迹规划。我们将超越基础教程，重点解析OctoMap动态障碍物处理、轨迹优化参数调优等实战技巧，并提供可直接复用的Python脚本，帮助你将理论迅速转化为生产力。

1. 环境搭建与MoveIt!配置

在开始编写避障规划代码之前，一个稳定且配置正确的开发环境是成功的一半。对于UR5机器人，我们通常从官方或社区维护的ROS包开始。

1.1 基础环境准备

首先，确保你的ROS工作空间已经包含了UR5机器人的相关功能包。如果你使用的是ROS Noetic，可以通过以下命令安装UR5的MoveIt!配置包：

sudo apt-get install ros-noetic-universal-robot
sudo apt-get install ros-noetic-moveit

接下来，在工作空间中克隆UR机器人的描述和仿真包：

cd ~/catkin_ws/src
git clone -b melodic-devel https://github.com/ros-industrial/universal_robot.git
cd ~/catkin_ws
rosdep install --from-paths src --ignore-src -y
catkin_make
source devel/setup.bash

注意：不同版本的ROS（如Melodic、Noetic）对应的分支可能不同，请根据你的ROS版本选择正确的分支。上述命令以Noetic为例，使用了melodic-devel分支，因为该分支通常也兼容Noetic。

1.2 启动MoveIt!与仿真环境

为了验证配置是否正确，我们可以同时启动MoveIt!规划节点和Gazebo仿真环境。这将为我们提供一个可视化的测试平台。

roslaunch ur_gazebo ur5.launch

在新的终端中，启动MoveIt!规划节点和RViz可视化界面：

roslaunch ur5_moveit_config ur5_moveit_planning_execution.launch sim:=true

如果一切顺利，你将在RViz中看到UR5机器人的模型，并可以通过Motion Planning插件交互式地设置目标位姿并进行规划。这个步骤至关重要，它能快速验证你的URDF模型、规划组（Planning Groups）和运动学插件是否配置正确。

1.3 规划组与运动学求解器配置

MoveIt!的核心概念之一是规划组（Planning Group）。对于UR5，我们通常将六个关节定义为一个名为manipulator的规划组。这个配置在通过MoveIt! Setup Assistant生成的SRDF（Semantic Robot Description Format）文件中完成。检查你的ur5_moveit_config/config/ur5.srdf文件，应该能看到类似以下内容：

<group name="manipulator">
  <chain base_link="base_link" tip_link="wrist_3_link" />
</group>

同时，运动学求解器的配置在kinematics.yaml文件中。UR5通常使用**KDL（Kinematica and Dynamics Library）**作为默认的数值逆运动学求解器。其配置示例如下：

manipulator:
  kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
  kinematics_solver_search_resolution: 0.005
  kinematics_solver_timeout: 0.05
  kinematics_solver_attempts: 3

关键参数解析：

• kinematics_solver_search_resolution: 数值迭代的搜索步长，值越小精度越高，但计算时间越长。
• kinematics_solver_timeout: 单次求解的最大时间（秒），超时则视为失败。
• kinematics_solver_attempts: 求解失败后的重试次数。

在实际应用中，如果机械臂经常在奇异点附近规划失败，可以考虑切换到TRAC-IK求解器，它通常比KDL更鲁棒，特别是在接近关节限位时。

2. 基础运动规划Python接口实战

掌握了环境配置后，我们开始编写第一个Python控制脚本。MoveIt!为Python提供了moveit_commander库，它封装了底层的ROS消息和服务，让规划代码变得简洁直观。

2.1 初始化与机器人状态获取

首先，我们创建一个完整的Python脚本框架，初始化MoveIt! commander并连接到规划组。

#!/usr/bin/env python3
import sys
import copy
import rospy
import moveit_commander
import moveit_msgs.msg
import geometry_msgs.msg
from math import pi, tau, dist, fabs, cos

class UR5MoveItPlanner:
    def __init__(self):
        # 初始化moveit_commander和rospy节点
        moveit_commander.roscpp_initialize(sys.argv)
        rospy.init_node('ur5_moveit_planner', anonymous=True)

        # 实例化RobotCommander对象，提供机器人整体的信息
        robot = moveit_commander.RobotCommander()

        # 实例化PlanningSceneInterface对象，用于与周围环境交互
        scene = moveit_commander.PlanningSceneInterface()

        # 实例化MoveGroupCommander对象，针对"manipulator"规划组
        group_name = "manipulator"
        move_group = moveit_commander.MoveGroupCommander(group_name)

        # 获取规划组的参考坐标系名称
        planning_frame = move_group.get_planning_frame()
        print(f"============ Planning frame: {planning_frame}")

        # 获取末端执行器链接的名称
        eef_link = move_group.get_end_effector_link()
        print(f"============ End effector link: {eef_link}")

        # 获取机器人中所有规划组的名称
        group_names = robot.get_group_names()
        print(f"============ Available Planning Groups: {group_names}")

        # 有时为了调试，打印整个机器人的状态
        print("============ Printing robot state")
        print(robot.get_current_state())
        print("")

        # 将对象保存为实例变量
        self.robot = robot
        self.scene = scene
        self.move_group = move_group
        self.planning_frame = planning_frame
        self.eef_link = eef_link
        self.group_names = group_names

这个初始化过程建立了与MoveIt!核心节点move_group的通信。MoveGroupCommander是我们与规划器交互的主要接口。

2.2 关节空间与笛卡尔空间规划

MoveIt!支持两种主要的目标指定方式：关节空间（直接指定每个关节的角度）和笛卡尔空间（指定末端执行器的位姿）。让我们看看如何实现这两种规划。

关节空间规划示例：

def go_to_joint_state(self):
    # 获取当前关节状态
    joint_goal = self.move_group.get_current_joint_values()
    print(f"Current joint values: {joint_goal}")

    # 设置新的关节目标（单位：弧度）
    # UR5关节顺序: shoulder_pan, shoulder_lift, elbow, wrist1, wrist2, wrist3
    joint_goal[0] = 0.0    # shoulder_pan
    joint_goal[1] = -pi/4  # shoulder_lift
    joint_goal[2] = 0.0    # elbow
    joint_goal[3] = -pi/2  # wrist1
    joint_goal[4] = 0.0    # wrist2
    joint_goal[5] = 0.0    # wrist3

    # 执行规划与运动
    self.move_group.go(joint_goal, wait=True)

    # 确保没有残留运动
    self.move_group.stop()

    # 验证是否到达目标
    current_joints = self.move_group.get_current_joint_values()
    tolerance = 0.01  # 弧度
    success = all(abs(current_joints[i] - joint_goal[i]) < tolerance 
                  for i in range(len(joint_goal)))
    
    return success

笛卡尔空间规划示例：

def go_to_pose_goal(self):
    # 创建位姿目标
    pose_goal = geometry_msgs.msg.Pose()
    
    # 设置位置（相对于规划坐标系）
    pose_goal.position.x = 0.4
    pose_goal.position.y = 0.1
    pose_goal.position.z = 0.4
    
    # 设置姿态（四元数）
    # 这里设置末端执行器朝下
    pose_goal.orientation.x = 0.0
    pose_goal.orientation.y = 0.7071  # sin(45°)
    pose_goal.orientation.z = 0.0
    pose_goal.orientation.w = 0.7071  # cos(45°)
    
    # 设置目标位姿
    self.move_group.set_pose_target(pose_goal)
    
    # 规划并执行
    plan = self.move_group.go(wait=True)
    
    # 停止并清除目标
    self.m