MoveIt 机械臂碰撞检测：从环境搭建到障碍物规避

在机械臂运动规划中，碰撞检测是确保机器人安全运行的核心功能。MoveIt 作为 ROS 生态中强大的运动规划框架，提供了完善的碰撞检测机制。本文将通过一个实战 demo，带您从零开始实现机械臂的碰撞检测与避障功能。

一、环境准备

1. 安装必要依赖

确保已安装 ROS 和 MoveIt 相关包（以 Noetic 为例）：

bash 复制代码

sudo apt update
sudo apt install ros-noetic-desktop-full ros-noetic-moveit ros-noetic-moveit-setup-assistant

2. 创建工作空间

bash 复制代码

mkdir -p ~/moveit2_ws/src
cd ~/moveit2_ws
catkin_make
source devel/setup.bash
# 建议添加到bashrc中
echo "source ~/moveit2_ws/devel/setup.bash" >> ~/.bashrc

二、创建碰撞检测演示项目

1. 创建功能包

bash 复制代码

cd ~/moveit2_ws/src
catkin_create_pkg collision_demo roscpp moveit_core moveit_ros_planning moveit_ros_planning_interface

2. 编写碰撞检测代码

在 collision_demo/src 目录下创建 collision_demo.cpp 文件：

cpp 复制代码

#include <ros/ros.h>
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <moveit_visual_tools/moveit_visual_tools.h>
#include <moveit_msgs/CollisionObject.h>
#include <shape_msgs/SolidPrimitive.h>

int main(int argc, char**argv)
{
    ros::init(argc, argv, "collision_demo");
    ros::NodeHandle nh;
    ros::AsyncSpinner spinner(1);
    spinner.start();

    // 1. 初始化规划组和场景接口
    static const std::string PLANNING_GROUP = "arm"; // 替换为你的规划组名称
    moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);
    moveit::planning_interface::PlanningSceneInterface planning_scene_interface;

    // 2. 初始化可视化工具
    namespace rvt = rviz_visual_tools;
    moveit_visual_tools::MoveItVisualTools visual_tools("world");
    visual_tools.deleteAllMarkers();
    visual_tools.loadRemoteControl();

    // 3. 添加碰撞物体（障碍物）
    moveit_msgs::CollisionObject collision_object;
    collision_object.header.frame_id = move_group.getPlanningFrame();
    collision_object.id = "box_obstacle";

    // 定义障碍物形状（立方体）
    shape_msgs::SolidPrimitive primitive;
    primitive.type = primitive.BOX;
    primitive.dimensions.resize(3);
    primitive.dimensions[0] = 0.3; // x尺寸
    primitive.dimensions[1] = 0.1; // y尺寸
    primitive.dimensions[2] = 0.5; // z尺寸

    // 设置障碍物位置
    geometry_msgs::Pose box_pose;
    box_pose.orientation.w = 1.0;
    box_pose.position.x = 0.5;    // x坐标
    box_pose.position.y = 0.0;    // y坐标
    box_pose.position.z = 0.25;   // z坐标（一半高度埋在地下）

    collision_object.primitives.push_back(primitive);
    collision_object.primitive_poses.push_back(box_pose);
    collision_object.operation = collision_object.ADD;

    // 将障碍物添加到规划场景
    std::vector<moveit_msgs::CollisionObject> collision_objects;
    collision_objects.push_back(collision_object);
    planning_scene_interface.addCollisionObjects(collision_objects);

    // 可视化提示
    visual_tools.publishText(Eigen::Isometry3d::Identity(), "Added Obstacle", rvt::WHITE, rvt::XLARGE);
    visual_tools.trigger();
    ROS_INFO("障碍物已添加到规划场景");

    // 4. 设置目标位姿（可能与障碍物发生碰撞）
    geometry_msgs::Pose target_pose;
    target_pose.orientation.w = 1.0;
    target_pose.position.x = 0.6;  // 目标位置（可能与障碍物冲突）
    target_pose.position.y = 0.0;
    target_pose.position.z = 0.4;
    move_group.setPoseTarget(target_pose);

    // 5. 执行规划
    moveit::planning_interface::MoveGroupInterface::Plan my_plan;
    bool success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
    
    if(success) {
        visual_tools.publishText(Eigen::Isometry3d::Identity(), "规划成功 - 无碰撞", rvt::GREEN, rvt::XLARGE);
        move_group.execute(my_plan);
        ROS_INFO("运动规划成功，正在执行...");
    } else {
        visual_tools.publishText(Eigen::Isometry3d::Identity(), "规划失败 - 存在碰撞", rvt::RED, rvt::XLARGE);
        ROS_WARN("运动规划失败，可能存在碰撞");
    }
    visual_tools.trigger();

    // 保持节点运行
    ros::Duration(10.0).sleep();
    ros::shutdown();
    return 0;
}

3. 配置 CMakeLists.txt

修改功能包中的 CMakeLists.txt：

bash 复制代码

cmake_minimum_required(VERSION 3.0.2)
project(collision_demo)

find_package(catkin REQUIRED COMPONENTS
  roscpp
  moveit_core
  moveit_ros_planning
  moveit_ros_planning_interface
  moveit_visual_tools
)

catkin_package()

include_directories(
  ${catkin_INCLUDE_DIRS}
)

add_executable(collision_demo src/collision_demo.cpp)
target_link_libraries(collision_demo
  ${catkin_LIBRARIES}
)

三、编译与运行

1. 编译项目

bash 复制代码

cd ~/moveit2_ws
catkin_make
source devel/setup.bash

2. 启动演示

首先启动 MoveIt 配置（以 Panda 机械臂为例）：

bash 复制代码

roslaunch panda_moveit_config demo.launch rviz_tutorial:=true

在新终端中运行碰撞检测节点：

bash 复制代码

source ~/moveit2_ws/devel/setup.bash
rosrun collision_demo collision_demo

四、核心功能解析

1.** 规划场景接口 **(PlanningSceneInterface)：用于管理环境中的碰撞物体，支持添加、移除和更新障碍物。

2.** 碰撞物体定义 **：通过 moveit_msgs::CollisionObject 消息定义障碍物的形状、尺寸和位姿，支持多种 primitive 形状（立方体、圆柱体等）。

3.** 自动避障规划 **：MoveIt 的规划器会自动检测路径中的碰撞，并尝试规划一条无碰撞的路径。如果无法找到有效路径，规划将失败。

4.** 可视化工具 **：moveit_visual_tools 用于在 RViz 中显示文本提示和标记，帮助直观理解规划状态。

五、常见问题与解决方案

1.** 规划总是失败？**- 检查障碍物是否与机械臂初始位置重叠

确认目标位姿是否在工作空间内
检查规划组名称是否与配置一致

2.** 障碍物不显示？**- 确保参考坐标系正确（通常为 "world" 或机械臂基座坐标系）

检查尺寸设置是否合理（避免过小或过大）

3.** 如何允许特定碰撞？** 可以通过 AllowedCollisionMatrix 设置允许特定链接间的碰撞：

cpp 复制代码

// 获取当前规划场景
moveit::planning_interface::PlanningSceneInterface psi;
moveit_msgs::PlanningScene ps;
psi.getPlanningScene(ps);

// 修改允许碰撞矩阵
collision_detection::AllowedCollisionMatrix& acm = ps.allowed_collision_matrix;
acm.setEntry("link_name1", "link_name2", true); // 允许两个链接碰撞
psi.applyPlanningScene(ps);

通过本 demo，您可以快速掌握 MoveIt 碰撞检测的核心用法。在实际应用中，您可以根据需要扩展功能，如动态添加障碍物、设置禁止碰撞区域等，从而实现更安全可靠的机械臂运动规划。