基于ROS2 Humble的 人形机器人/机械臂运动规划理论及实战(一)模型篇
<geometry></visual></link></joint><visual><geometry>
基于ROS2 Humble的 人形机器人/机械臂运动规划理论及实战
基础环境
ROS2 humble + ubuntu 22.04 + moveit + gazebo(gz)
ROS2(Robot Operating System 2)是机器人开发的开源框架,支持现代机器人运动规划算法的实现。运动规划通常涉及环境建模、路径搜索、轨迹优化等步骤,以下从模型构建到算法实现展开说明。
首先,人形机器人或者机械臂这一类有实体的载体,其路径规划与运动控制是必须要有本体模型的,模型格式需遵循规范,统一使用URDF格式。但往往我们的机器人在设计过程中不可能使用URDF进行设计,URDF可以用来建模,但比较麻烦,且建模方式不够精确。
通常我们会使用机械建模工具solidworks或者其他工具进行精细化建模,为此需要对模型进行转化,这里会用到一个工具插件SW2URDF,下载后安装在SolidWorks内,按照教程来即可。插件在github上,部分国内读者需要科学上网。https://github.com/ros/solidworks_urdf_exporter/releases/tag/1.6.1https://blog.csdn.net/qq_45343085/article/details/146413056?ops_request_misc=&request_id=&biz_id=102&utm_term=sw2urdf%E6%8F%92%E4%BB%B6%E5%AE%89%E8%A3%85%E6%AD%A5%E9%AA%A4&utm_medium=distribute.pc_search_result.none-task-blog-2~all~sobaiduweb~default-5-146413056.142^v102^pc_search_result_base6&spm=1018.2226.3001.4187
注意,这里的插件工具转化生成的配置文件可能是ROS格式的!!!!!若使用ROS2版本在编译时会报错。ROS使用的编译命令为catkin_make,ROS2使用的是colcon build.后续会介绍如何调整配置文件适配ROS2.
1、URDF文件定义及编写
URDF 文件概述
URDF(Unified Robot Description Format)是一种用于描述机器人模型的XML格式文件。它广泛应用于机器人仿真、运动规划和控制系统中,能够定义机器人的关节、连杆、传感器等组件及其相互关系。
示例URDF片段
<robot name="simple_arm">
<link name="base_link">
<visual>
<geometry>
<cylinder length="0.1" radius="0.2"/>
</geometry>
</visual>
</link>
<joint name="joint1" type="revolute">
<parent link="base_link"/>
<child link="arm_link"/>
<axis xyz="0 0 1"/>
<limit lower="-1.57" upper="1.57" effort="30" velocity="1.0"/>
</joint>
<link name="arm_link">
<visual>
<geometry>
<box size="0.5 0.1 0.1"/>
</geometry>
</visual>
</link>
</robot>
<joint name="joint1" type="revolute">
<parent link="base_link"/>
<child link="link1"/>
<axis xyz="0 0 1"/>
<limit effort="100" lower="-3.14" upper="3.14" velocity="0.5"/>
</joint>
作为初学者,不需要去详细了解每个指令的含义,只用知道几个比较重要的的元素就可以,在机器人仿真的URDF文件中,通常这几个比较重要:
关节类型
<joint name="joint1" type="revolute">
fixed固定关节revolute旋转关节prismatic平移关节continuous连续旋转关节floating浮动关节planar平面关节
旋转轴
<axis xyz="0 0 1"/>
关节限制
<limit lower="-1.57" upper="1.57" effort="30" velocity="1.0"/>
其他元素不是说不重要,在实战中上述提到的几个是需要调整与修改的,其他元素基本靠插件自动生成。
2、运动学建模与DH建模
理论基础
机械臂运动学建模
机械臂运动学建模研究机械臂的运动特性,不涉及力和力矩。运动学分为正向运动学和逆向运动学。正向运动学通过关节角度计算机械臂末端执行器的位置和姿态。逆向运动学通过末端执行器的位置和姿态计算关节角度。
运动学建模通常使用齐次变换矩阵描述连杆之间的相对位置和姿态。齐次变换矩阵包含旋转和平移信息,可以表示三维空间中的刚体运动。通过连续变换,可以建立从基座到末端执行器的完整运动学模型。
DH建模
DH(Denavit-Hartenberg)建模是一种常用的机械臂运动学建模方法。DH方法通过四个参数描述相邻连杆之间的几何关系,简化运动学建模过程。四个参数包括连杆长度、连杆扭转角、关节距离和关节角度。
DH建模步骤包括为每个关节建立坐标系,确定DH参数,构建变换矩阵。每个连杆的坐标系根据DH规则确定,确保参数的一致性。通过连续相乘各个连杆的变换矩阵,得到从基座到末端执行器的总变换矩阵。
DH参数的优势在于标准化和简化运动学建模,适用于大多数串联机械臂。然而,DH方法对并联机械臂或特殊结构的机械臂可能不适用。在实际应用中,需根据机械臂的具体结构选择合适的建模方法。
运动学建模与DH建模对后续运动学的正逆解至关重要,需要非常准确的参数支撑后续的运动规划,下面先从DH建模开始介绍如何构建机械臂的DH模型。
构建DH模型
以下图UR机械臂为例,介绍如何构建DH坐标系模型

理论上,有几个旋转轴就需要几个坐标系,上图中六个旋转轴却有7个坐标系,因为最后一个是工具坐标系。建立坐标系需要遵循几个原则:
1、Z轴与旋转轴重合,这是最最最基础的要求,一定要满足的
2、X轴必须与相邻关节的轴线(Z轴)与本坐标系的轴线(Z轴)的公垂线重合【两轴线相交】,或从当前轴线指向下一关节轴线的直线重合【两轴线平行】
3、Y轴可根据右手法则,由X,Z轴共同确定下来,拇指指向Z轴正方向,食指指向X轴正方向,中指指向Y轴正方向
4、注意,上述所属的直线关系是重合,也就是说方向还是不确定的,实际上确定方向也是有原则的,但方向影响不大,统一方向有利于计算,为此建议统一全局的轴方向
在建立完坐标系后,可根据坐标系之间的关系推导出DH参数表,后续可用于逆解求解器,DH参数表所需的参数如下:
| 参数类型 | 参数符号 | 核心物理含义 | 补充说明 |
|---|---|---|---|
| 长度参数 | \(a_i\)(连杆长度) | 沿\(x_i\)轴(第i个连杆坐标系 X 轴),从\(z_{i-1}\)轴(前一关节轴线)到\(z_i\)轴(当前关节轴线)的距离(即两关节轴线公垂线的长度) | 1. 单位:米(m)2. 若两关节轴线(\(z_{i-1}\)和\(z_i\))相交,\(a_i=0\)3. 始终为非负值(建模时可通过\(x_i\)轴方向选择规避负值) |
| 角度参数 | \(\alpha_i\)(连杆扭角 / 扭转角) | 绕\(x_i\)轴,从\(z_{i-1}\)轴正向旋转到\(z_i\)轴正向的角度 | 1. 单位:弧度(rad)或角度(°)2. 旋转方向遵循右手定则(右手四指沿旋转方向,大拇指指向\(x_i\)轴正向)3. 取值范围通常为\([-\pi, \pi]\)或\([0, 2\pi]\) |
| 长度参数 | \(d_i\)(连杆偏距 / 横距) | 沿\(z_{i-1}\)轴(前一关节轴线),从\(x_{i-1}\)轴(前一连杆坐标系 X 轴)到\(x_i\)轴(当前连杆坐标系 X 轴)的距离 | 1. 单位:米(m)2. 对于移动关节(平动关节,如直线模组),\(d_i\)是关节变量(随关节运动变化);对于旋转关节(转动关节,如舵机),\(d_i\)是固定参数(机械结构固有尺寸) |
| 角度参数 | \(\theta_i\)(关节角 / 转角) | 绕\(z_{i-1}\)轴(前一关节轴线),从\(x_{i-1}\)轴正向旋转到\(x_i\)轴正向的角度 | 1. 单位:弧度(rad)或角度(°)2. 旋转方向遵循右手定则(右手四指沿旋转方向,大拇指指向\(z_{i-1}\)轴正向)3. 取值范围通常为\([-\pi, \pi]\)或\([0, 2\pi]\)4. 对于旋转关节,\(\theta_i\)是关节变量(随关节运动变化);对于移动关节,\(\theta_i\)是固定参数(机械结构固有角度) |
到这里,建立完DH参数表后,理论上就可以在matlab内做出一个六轴机械臂的正逆解以及最基础的运动规划算法了

但这种算法是纯理论层面的计算,当我们需要部署到真机时,缺乏完整的机器人几何描述与碰撞检测能力,且不便联机调试
- MATLAB 基于 DH 模型的可视化仅能显示简化的连杆模型(如线段、长方体等基础几何形状,无法还原机器人的真实外观),可视化界面简陋,仅能展示机器人的位姿变化,无法直观呈现规划路径、碰撞区域、环境模型等关键信息。
- 缺乏交互式调试能力:无法通过图形界面手动拖动机器人关节、调整规划目标位姿、实时修改环境障碍物、可视化路径优化过程,调试时需通过修改代码参数、重新运行脚本的方式进行,迭代效率极低,难以快速定位问题(如路径碰撞、规划失败、逆解无解等)。
- MATLAB 的代码是解释型代码,运行效率较低,且难以直接部署到嵌入式硬件(如机器人控制器、ROS 主控板),通常仅能作为离线算法验证工具,无法实现 “仿真规划 - 实际部署” 的无缝衔接,与工程应用存在较大差距。
为此,在这种前提下,我们需要一个在功能完整性、工程实用性、开发效率等方面均具有碾压性优势的方案:ROS+moveit框架,而在ROS框架下,机器人的描述是通过上述的URDF而不是纯数据表述的,下面介绍如何从工程实际出发,构建一个机械臂的URDF文件
URDF建模
预准备工作
在通过sw2urdf插件导出urdf模型包之前,需要对模型进行一些处理,因为使用插件时每个关节和link都会需要你提供参考旋转轴以及参考坐标系。当然插件提供了自动生成参考旋转轴以及参考坐标系选项,但我不推荐自动生成,对于简单的机械臂或者标准构型的UR机械臂等可以采用自动生成,对于自制的特殊构型的人形六轴或七轴机械臂,还是建议自己根据DH建系规则在模型内插入各个轴的旋转轴和参考坐标系。
自动生成的参考旋转轴和参考坐标系导出模型会存在一系列问题,刚开始接触时我也是图方便使用自动生成,后来发现导出的模型关节之间错位,关节方向逆转等问题,当然这些问题可以在生成好的Urdf文件内修改,但很麻烦,需要一点点去试坐标原点。
下面介绍如何在solidworks内进行旋转轴以及参考坐标系的建立,构建一个机械人机械臂的URDF模型。需要少量的solidworks基础。
注意,在确定旋转轴和各个轴的参考坐标系之前,请确认:若运动控制算法与运动学仿真都是你一个在做,可忽略;若你只做运动学仿真,运动控制算法有独立的运控算法工程师在做,在构建URDF的坐标系和旋转轴之前请与算法沟通确认,若后续需要采用该URDF模型为基础测试运控算法可靠性,则要求算法的DH建系与URDF内的建系保持一致!可以省去很多不必要的麻烦。
此处为方便示范,按照怎么方便怎么来原则建系。开始之前请先了解solidworks如何插入点、线、面、坐标系!
1、确定世界坐标系,也就是base_link的坐标系,若为单臂,直接选取底座中心点即可;若为双臂,可选取机器人机身中轴线上的点,位置随便,最好在机器人脚部或者腹部,导入rviz时就不会出现机械臂在网格线以下的情况,虽然也没什么影响。建坐标系之前需要先插入参考点,以参考点为坐标系原点建系,Z轴向上,X轴朝向你自己这一面(射向屏幕外),Y轴根据右手法则确定。
2、对于每一个轴,需要在轴的两端插入两个点,两个点一般位于电机的前端面和后端面,随后选中两个点,插入旋转轴。做好命名工作,如左臂旋转轴1、2、3、4、5、6等

3、对于每个轴的参考坐标系,坐标系原点可以定在第二步的轴线上,最好定在轴线中点,可能需要做基准面选取中点,简单一点的话第二步轴线的两个端点任选一个作为坐标系原点;其次确定坐标系XYZ轴方向,与上述DH建系规则一致,Z轴与旋转轴重合,X轴与两个旋转轴的公垂线重合或指向下一个旋转轴方向,具体参考DH建系规则。X轴朝向无硬性要求,整个手臂统一即可。同样做好命名工作,如左臂坐标系1、2、3、4、5、6.
导出URDF
打开模型,选择左上方 工具---->tools----->export as URDF,左侧出现urdf配置选项,base_link的参考坐标系选择第一步建立的世界坐标系,选择base_link的模型添加。添加完成后点击向上的箭头,双臂机器人选择2,四足机器人选择4,普通单臂机械臂选择1,然后选择下面的empty_link,进入子link选项,修改joint和link名称,选择参考旋转轴和参考坐标系时,如何前面的预准备工作做了的话,那么在下拉框就会出现对应得旋转轴和坐标系,根据关节对应关系选择就可以了。
joint type根据关节类型选择,revolute代表两个link之间是旋转关系,fixed代表固定关系,continuous代表线性移动关系,通常机械臂与底座之间用fixed,link与link之间用revolute,末端夹爪用continuous。
以此类推,构建完一个完整的urdf传动链后,点击preview and export urdf,点击你所构建的link和joint预览一遍,确认没有错误后导出,导出选项中有很多东西需要填,不用管,包括力矩,关节角度的上下限,速度等等都可以在后续生成的urdf文件中修改。导出选择urdf和mesh文件一起导出。
导出时需要给文件夹命名,注意不能出现中文以及首字母大写!否则编译报错!


solidworks转URDF可能出现的问题
1、部分零部件采用面片建模的,导出后可能有零件消失的情况,且面片建模urdf计算质量,质心都会存在问题,需要把面片建模转换为实体建模;
2、部分复杂零部件如灵巧手,齿轮密集处,导出时也会出现零件缺失或者不显示的情况。在缺失零件较多的情况下需要重新导出(solidworks的URDF插件会询问你是否继续上次的配置,选yes);在缺失零件不多的情况下,可以单独导出缺失的零件,不需要整体全部导出,但单独导出时base_link和世界坐标系一定要顺带加上,不管是哪个零件缺失,因为单独导出的零件没办法定位,需要有一个参考坐标系或者参考原点,base_link是整个机器的参考原点,要加上。导出后的配置包只用将mesh文件夹内的缺失零件复制到第一次导出的mesh文件夹替换即可,替换完编译,source 工作空间。
3、某些轴方向反了,可以在urdf内修改axis对应的XYZ轴
3、模型配置包适配ROS2调整
上述插件导出的配置包,通常包含config,launch,meshes,textures,urdf,Cmakelists.txt,package.xml等配置文件,其中meshes文件存放的是模型的stl格式文件,相当于模型的形状,urdf文件夹存放的是.urdf文件,相当与模型的骨骼,launch文件夹提供ROS启动文件(.launch),用于快速加载URDF模型到ROS或Gazebo仿真环境。典型文件如display.launch或gazebo.launch。CMakeLists.txt与package.xml
ROS包的编译和依赖声明文件,确保包能被ROS系统识别和构建。package.xml需包含URDF、xacro等依赖项。

由于插件问题,导出的launch文件,Cmakelists.txt文件,以及package.xml是适配ROS版本的,因此ROS版本的读者可直接使用,ROS2版本需要对里面的文件进行修改,具体修改文件如下:
1、display.launch-------->robot_display.launch.py
2、gazebo.launch--------->robot_gazebo.launch.py
3、新建robot_rviz.launch.py文件
4、Cmakelists.txt修改
5、package.xml修改
6、新建rviz文件夹,内置robot.rviz文件(robot为项目名)
具体修改代码如下:
1、原display.launch文件
<launch>
<arg
name="model" />
<param
name="robot_description"
textfile="$(find robot)/urdf/robot.urdf" />
<node
name="joint_state_publisher_gui"
pkg="joint_state_publisher_gui"
type="joint_state_publisher_gui" />
<node
name="robot_state_publisher"
pkg="robot_state_publisher"
type="robot_state_publisher" />
<node
name="rviz"
pkg="rviz"
type="rviz"
args="-d $(find robot)/urdf.rviz" />
</launch>
修改后:robot_display.launch.py(robot为项目名)
from ament_index_python.packages import get_package_share_path
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.conditions import IfCondition, UnlessCondition
from launch.substitutions import Command, LaunchConfiguration
from launch_ros.actions import Node
from launch_ros.parameter_descriptions import ParameterValue
import os
def generate_launch_description():
robot_path = get_package_share_path('robot')
default_model_path = robot_path / 'urdf/robot.urdf'
default_rviz_config_path = ssa2_path / 'rviz/robot.rviz'
gui_arg = DeclareLaunchArgument(name='gui', default_value='true', choices=['true', 'false'],
description='Flag to enable joint_state_publisher_gui')
model_arg = DeclareLaunchArgument(name='model', default_value=str(default_model_path),
description='Absolute path to robot urdf file')
rviz_arg = DeclareLaunchArgument(name='rvizconfig', default_value=str(default_rviz_config_path),
description='Absolute path to rviz config file')
# Check if the model file is xacro or urdf
model_file = str(LaunchConfiguration('model'))
if model_file.endswith('.xacro'):
robot_description = ParameterValue(Command(['xacro ', LaunchConfiguration('model')]),
value_type=str)
else:
# Read the URDF file directly
robot_description = ParameterValue(Command(['cat ', LaunchConfiguration('model')]),
value_type=str)
robot_state_publisher_node = Node(
package='robot_state_publisher',
executable='robot_state_publisher',
parameters=[{'robot_description': robot_description}]
)
# Depending on gui parameter, either launch joint_state_publisher or joint_state_publisher_gui
joint_state_publisher_node = Node(
package='joint_state_publisher',
executable='joint_state_publisher',
condition=UnlessCondition(LaunchConfiguration('gui'))
)
joint_state_publisher_gui_node = Node(
package='joint_state_publisher_gui',
executable='joint_state_publisher_gui',
condition=IfCondition(LaunchConfiguration('gui'))
)
rviz_node = Node(
package='rviz2',
executable='rviz2',
name='rviz2',
output='screen',
arguments=['-d', LaunchConfiguration('rvizconfig')],
)
return LaunchDescription([
gui_arg,
model_arg,
rviz_arg,
joint_state_publisher_node,
joint_state_publisher_gui_node,
robot_state_publisher_node,
rviz_node
])
2、原gazebo.launch文件
<launch>
<include
file="$(find gazebo_ros)/launch/empty_world.launch" />
<node
name="tf_footprint_base"
pkg="tf"
type="static_transform_publisher"
args="0 0 0 0 0 0 base_link base_footprint 40" />
<node
name="spawn_model"
pkg="gazebo_ros"
type="spawn_model"
args="-file $(find ssa2)/urdf/ssa2.urdf -urdf -model ssa2"
output="screen" />
<node
name="fake_joint_calibration"
pkg="rostopic"
type="rostopic"
args="pub /calibrated std_msgs/Bool true" />
</launch>
修改后robot_gazebo.launch.py(robot为项目名)
#!/usr/bin/env python3
"""
ROS2 Gazebo Launch File for ssa2 robot
Launches Gazebo with the ssa2 robot model.
"""
import os
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription, ExecuteProcess
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch_ros.actions import Node
from launch.substitutions import PathJoinSubstitution
from launch_ros.substitutions import FindPackageShare
from ament_index_python.packages import get_package_share_directory
def generate_launch_description():
# Get package paths
pkg_gazebo_ros = get_package_share_directory('gazebo_ros')
pkg_robot = get_package_share_directory('robot')
# Path to URDF file
urdf_file = os.path.join(pkg_robot, 'urdf', 'robot.urdf')
# Include Gazebo launch file (empty world)
gazebo = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(pkg_gazebo_ros, 'launch', 'gazebo.launch.py')
),
launch_arguments={'verbose': 'false'}.items()
)
# Static transform publisher (base_link to base_footprint)
tf_footprint_base = Node(
package='tf2_ros',
executable='static_transform_publisher',
name='tf_footprint_base',
arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_footprint'],
output='screen'
)
# Spawn robot model in Gazebo
spawn_model = Node(
package='gazebo_ros',
executable='spawn_entity.py',
name='spawn_model',
arguments=[
'-file', urdf_file,
'-entity', 'ssa2',
'-x', '0',
'-y', '0',
'-z', '0'
],
output='screen'
)
# Publish calibrated message (using ros2 topic pub)
# Note: In ROS2, this is typically done differently, but here's the equivalent
fake_joint_calibration = ExecuteProcess(
cmd=[
'ros2', 'topic', 'pub', '-1',
'/calibrated', 'std_msgs/msg/Bool',
'{data: true}'
],
output='screen'
)
return LaunchDescription([
gazebo,
tf_footprint_base,
spawn_model,
fake_joint_calibration,
])
3、新建robot_rviz.launch.py文件(robot为项目名)
from ament_index_python.packages import get_package_share_path
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.conditions import IfCondition, UnlessCondition
from launch.substitutions import Command, LaunchConfiguration
from launch_ros.actions import Node
from launch_ros.parameter_descriptions import ParameterValue
import os
def generate_launch_description():
robot_path = get_package_share_path('robot')
default_model_path = robot_path / 'urdf/robot.urdf'
default_rviz_config_path = robot_path / 'rviz/robot.rviz'
gui_arg = DeclareLaunchArgument(name='gui', default_value='true', choices=['true', 'false'],
description='Flag to enable joint_state_publisher_gui')
model_arg = DeclareLaunchArgument(name='model', default_value=str(default_model_path),
description='Absolute path to robot urdf file')
rviz_arg = DeclareLaunchArgument(name='rvizconfig', default_value=str(default_rviz_config_path),
description='Absolute path to rviz config file')
# Check if the model file is xacro or urdf
model_file = str(LaunchConfiguration('model'))
if model_file.endswith('.xacro'):
robot_description = ParameterValue(Command(['xacro ', LaunchConfiguration('model')]),
value_type=str)
else:
# Read the URDF file directly
robot_description = ParameterValue(Command(['cat ', LaunchConfiguration('model')]),
value_type=str)
robot_state_publisher_node = Node(
package='robot_state_publisher',
executable='robot_state_publisher',
parameters=[{'robot_description': robot_description}]
)
# Depending on gui parameter, either launch joint_state_publisher or joint_state_publisher_gui
joint_state_publisher_node = Node(
package='joint_state_publisher',
executable='joint_state_publisher',
condition=UnlessCondition(LaunchConfiguration('gui'))
)
joint_state_publisher_gui_node = Node(
package='joint_state_publisher_gui',
executable='joint_state_publisher_gui',
condition=IfCondition(LaunchConfiguration('gui'))
)
rviz_node = Node(
package='rviz2',
executable='rviz2',
name='rviz2',
output='screen',
arguments=['-d', LaunchConfiguration('rvizconfig')],
)
return LaunchDescription([
gui_arg,
model_arg,
rviz_arg,
joint_state_publisher_node,
joint_state_publisher_gui_node,
robot_state_publisher_node,
rviz_node
])
4、Cmakelists.txt修改(robot为项目名)
cmake_minimum_required(VERSION 3.8)
project(robot)
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
# find dependencies
find_package(ament_cmake REQUIRED)
# uncomment the following section in order to fill in
# further dependencies manually.
# find_package(<dependency> REQUIRED)
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
# the following line skips the linter which checks for copyrights
# comment the line when a copyright and license is added to all source files
set(ament_cmake_copyright_FOUND TRUE)
# the following line skips cpplint (only works in a git repo)
# comment the line when this package is in a git repo and when
# a copyright and license is added to all source files
set(ament_cmake_cpplint_FOUND TRUE)
ament_lint_auto_find_test_dependencies()
endif()
install(
DIRECTORY launch urdf rviz meshes config
DESTINATION share/${PROJECT_NAME}
)
ament_package()
5、package.xml修改(robot为项目名)
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>robot</name>
<version>1.0.0</version>
<description>
<p>URDF Description package for robot</p>
<p>This package contains configuration data, 3D models and launch files
for robot robot</p>
</description>
<author>TODO</author>
<maintainer email="TODO@email.com" />
<license>BSD</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
<exec_depend>rviz2</exec_depend>
<exec_depend>xacro</exec_depend>
<exec_depend>robot_state_publisher</exec_depend>
<exec_depend>joint_state_publisher</exec_depend>
<exec_depend>joint_state_publisher_gui</exec_depend>
<exec_depend>ros2launch</exec_depend>
</package>
6、新建rviz文件夹,内置robot.rviz文件(robot为项目名)
# RViz2 配置文件 - UR3机器人可视化设置
# 此文件定义了RViz2的显示面板、可视化插件和视图设置
# 面板配置 - 定义RViz2界面中的面板
Panels:
# 显示面板 - 管理所有可视化插件
- Class: rviz_common/Displays
Name: Displays
# 视图面板 - 管理不同的视角和视图模式
- Class: rviz_common/Views
Name: Views
# 可视化管理器 - 核心配置部分
Visualization Manager:
Class: ""
# 显示插件列表 - 定义要显示的可视化内容
Displays:
# 网格显示插件 - 显示3D网格帮助空间定位
- Class: rviz_default_plugins/Grid
Name: Grid
Value: true # 启用网格显示
# 机器人模型显示插件 - 显示UR3机器人的3D模型
- Alpha: 1 # 透明度 (1=完全不透明, 0=完全透明)
Class: rviz_default_plugins/RobotModel
Description Source: Topic # 从ROS话题获取机器人描述
Description Topic:
Value: /robot_description # 机器人URDF描述的话题名
Enabled: true # 启用机器人模型
Name: RobotModel
Value: true
# 坐标变换显示插件 - 显示机器人各关节的坐标系
- Class: rviz_default_plugins/TF
Name: TF
Value: true # 启用TF显示
# 全局选项 - 影响整个可视化的设置
Global Options:
Fixed Frame: base_link # 固定参考坐标系为机器人基座
Frame Rate: 30 # 显示更新频率 (FPS)
Name: root
# 工具配置 - 定义可用的交互工具
Tools:
- Class: rviz_default_plugins/MoveCamera # 相机移动工具
Value: true
# 视图配置 - 定义观察视角
Views:
# 轨道视图 - 围绕焦点旋转的观察模式
- Class: rviz_default_plugins/Orbit
Distance: 1.0 # 相机距离焦点的距离(米)
# 立体渲染设置 (当前禁用)
Enable Stereo Rendering:
Stereo Eye Separation: 0.06 # 立体眼间距
Stereo Focal Distance: 1 # 立体焦距
Swap Stereo Eyes: false # 不交换立体眼
Value: false # 禁用立体渲染
# 焦点设置 - 相机围绕旋转的中心点
Focal Point:
X: 0 # X坐标
Y: 0 # Y坐标
Z: 0 # Z坐标
Focal Shape Fixed Size: true # 焦点标记固定大小
Focal Shape Size: 0.05 # 焦点标记大小
Invert Z Axis: false # 不反转Z轴
Name: Current View # 视图名称
Near Clip Distance: 0.01 # 近裁剪距离(米)
Pitch: 0.0 # 俯仰角 (设置为 0)
Target Frame: <Fixed Frame> # 目标坐标系(使用固定坐标系)
Value: Orbit (rviz_default_plugins) # 视图类型
Yaw: 0.0 # 偏航角 (设置为 0)
完成上述修改后,进入工作空间,进入src文件夹,打开终端,输入colcon build,正常情况下编译成功。
4、moveit配置助手配置机械臂关节并在rviz中可视化显示
上述配置完成后我们可以称之为模型包,但模型包无法在rviz,gazebo等可视化以及仿真工具中打开,模型的关节关系并没有得到体现,且机械臂也没有配置运动规划算法,无法进行移动与仿真,为此需要基于moveit在模型包基础上进行二次配置,需要用到模型包内urdf文件以及moveit配置助手。下面介绍如何为自己的机械臂进行配置。
4.1 安装MoveIt配置助手
确保已安装ROS和MoveIt。若未安装,可通过以下命令安装:
sudo apt-get install ros-<distro>-moveit
替换<distro>为ROS版本(如noetic、humble)。
4.2启动MoveIt配置助手
运行以下命令启动MoveIt Setup Assistant:
ros2 launch moveit_setup_assistant setup_assistant.launch.py
通常会失败,如果你没有source ROS2,请执行命令:
source /opt/ros/<distro>/setup.bash
替换<distro>为实际版本如noetic,humble.
4.3 配置moveit模型包的输出
1、导入urdf文件,这里以UR3机械臂为例程,图中create new moveit选项为创建新的moveit配置包,需要选取上述模型包的URDF文件,Edit existing moveit为修改配置好的moveit配置包,这样就不用重新配置了。

2、优化自碰撞矩阵,通过分析机器人的URDF模型,自动计算并调整各关节之间的碰撞检测规则,以减少不必要的碰撞检查,从而提高运动规划的效率。点击generate自动生成碰撞矩阵

3、配置规划组

运动学求解器:Kinematic Solver------->kdl_kinematics_plugin/KDLKinematicsPlugin
或者可以选择性能更好的Trae_ik, Bio_ik等,不过这些运动学求解器不在选项之内,面对复杂场景下的规划可以单独去下载。当然也可以针对自己机械臂的特殊构型去写求解器。
开源规划算法库:OMPL planning-------->RRT / RRTConnect / RRTStar / PRM / PRMStar /
这里的规划算法只提供接口和一些超参数修改选项,不提供源码,想看源码的可以去github上下载
git clone https://github.com/ompl/ompl.git
添加运动链 Add Kin.Chain
Base_Link选择base_link
Tip_link选择link6,如果有末端执行器如夹爪,就选择夹爪,save保存配置
若为双臂,退回上一个界面继续添加另一个规划组。
注意:若需要双臂同时规划运动,请添加双臂规划组,可以点击上图的Add Subgroups继承配置,也可以在后续的生成文件内手动添加双臂规划组。绝对不可以使用两个单臂规划组同时规划来代替双臂规划组
大部分工业机器人控制器(或 ROS 控制器)是单线程执行轨迹(或采用主从线程架构),即使同时发送两个单臂的轨迹指令,控制器也会按先后顺序或时间片轮转的方式执行,导致两个手臂的运动无法严格同步,出现 “一前一后” 的偏差,无法完成协同任务。且双臂存在碰撞耦合和协同任务约束,两个单臂规划组无法感知和处理

4、设置姿态(下图为双臂的姿态,换了一个模型),这一步是为了方便回原点以及去一些常用的固定姿态,如加工位,待机位等。
此处若滑动条不能拖动,不要紧,因为前面URDF为设置limit的上下限制,代表的关节的限位,未设置则默认为0,无法滑动,后续可在URDF内修改

5、设置可显示的数据以及可写入的数据,不过大部分硬件只支持写入position,不支持velocity写入,此处保持默认即可,若需要读取effort,可以勾选上state interfaces 的efforts

6、点击自动生成ROS2 控制器,该控制器可直接驱动硬件,接收 MoveIt Controller 转发的轨迹指令(或其他节点发送的控制指令),通过 *闭环控制(位置环、速度环、力矩环)*计算出机器人关节的驱动信号,直接驱动机器人的硬件关节或仿真中的关节模),同时实时反馈机器人的实际关节状态(如实际角度、速度、力矩)。

7、点击自动生成moveit控制器,接收 MoveIt 规划器生成的运动轨迹(关节空间轨迹 / 笛卡尔空间轨迹),对轨迹进行简单预处理如轨迹分段、速度校验,然后按照固定接口将轨迹数据格式转换后,转发给底层的 ROS Controller,同时反馈轨迹执行的状态如执行中、完成、失败。

8、最后随便填写一个作者和邮箱,其他部分可填可不填,如末端end effectors,被动关节,虚拟关节,传感器等等,按需填写即可。来到最后导出界面,选择文件夹存储moveit配置文件,点击generate package,退出即可(正常下图文件是全黑,我这是使用了以前配置过的配置包,里面文件修改过,所以有红色和黄色,不是报错,是提醒你文件被修改过)

5、moveit配置包解析及修改调整与报错解决
正常导出的包含config和launch文件,launch文件无需修改,config文件下需要一些调整

进入src文件夹下,打开终端,输入:
ros2 launch moveit_output3 demo.launch.py
若终端报错,请打开joint_limits.yaml文件修改速度,加速度为浮点数
joint_limits:
left_joint1:
has_velocity_limits: true
max_velocity: 0.2
has_acceleration_limits: false
max_acceleration: 0.5
left_joint2:
has_velocity_limits: true
max_velocity: 0.2
has_acceleration_limits: false
max_acceleration: 0.5
打开demo.launch.py后,若看不到模型,选择fixed frame为base_link

点击add,添加需要的组件如MotionPlanning, tf, ponitcloud2等

若点击plan and Execute后只有虚影,模型不动,说明moveit只做了规划,并没有执行你规划的动作,请打开moveit_controllers.yaml,在controller内加入action_ns: follow_joint_trajectory
# MoveIt uses this configuration for controller management
moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager
moveit_simple_controller_manager:
controller_names:
- left_arm_controller
- right_arm_controller
left_arm_controller:
type: FollowJointTrajectory
action_ns: follow_joint_trajectory # 添加这一行
joints:
- left_joint1
- left_joint2
- left_joint3
- left_joint4
- left_joint5
- left_joint6
action_ns 是 ROS 控制器的动作命名空间(Action Namespace)。它告诉 MoveIt 使用哪个 ROS Action Server 来发送轨迹控制命令。FollowJointTrajectory 控制器是 ROS 中常用的控制器类型,它通过 Action Server 接收轨迹命令并控制机械臂的运动。如果不指定 action_ns,MoveIt 无法知道应该连接到哪个 Action Server,从而无法发送轨迹命令。
通常,点击OMPL选项即可选择不同的开源算法库,若未显示,则需要添加ompl_planning.yaml文件,同样放在config文件夹下

ompl_planning.yaml具体代码:
planning_plugin: ompl_interface/OMPLPlanner
default_planner_config: PRMstarkConfigDefault # 新增此行,设置RRTstar为默认规划器
request_adapters: >-
default_planner_request_adapters/AddTimeOptimalParameterization
default_planner_request_adapters/ResolveConstraintFrames
default_planner_request_adapters/FixWorkspaceBounds
default_planner_request_adapters/FixStartStateBounds
default_planner_request_adapters/FixStartStateCollision
default_planner_request_adapters/FixStartStatePathConstraints
start_state_max_bounds_error: 0.1
planner_configs:
SBLkConfigDefault:
type: geometric::SBL
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
ESTkConfigDefault:
type: geometric::EST
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0 setup()
goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05
LBKPIECEkConfigDefault:
type: geometric::LBKPIECE
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9
min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5
BKPIECEkConfigDefault:
type: geometric::BKPIECE
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9
failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5
min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5
KPIECEkConfigDefault:
type: geometric::KPIECE
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05
border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 (0.0,1.]
failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5
min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5
RRTkConfigDefault:
type: geometric::RRT
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05
RRTConnectkConfigDefault:
type: geometric::RRTConnect
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
RRTstarkConfigDefault:
type: geometric::RRTstar
range: 0.01 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05
delay_collision_checking: 1 # Stop collision checking as soon as C-free parent found. default 1
TRRTkConfigDefault:
type: geometric::TRRT
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05
max_states_failed: 10 # when to start increasing temp. default: 10
temp_change_factor: 2.0 # how much to increase or decrease temp. default: 2.0
min_temperature: 10e-10 # lower limit of temp change. default: 10e-10
init_temperature: 10e-6 # initial temperature. default: 10e-6
frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup()
frountierNodeRatio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1
k_constant: 0.0 # value used to normalize expresssion. default: 0.0 set in setup()
PRMkConfigDefault:
type: geometric::PRM
max_nearest_neighbors: 10 # use k nearest neighbors. default: 10
PRMstarkConfigDefault:
type: geometric::PRMstar
range: 0.2 # 基于工作空间尺度设置(如0.2m)
goal_bias: 0.05 # 保持默认,避免采样失衡
max_nearest_neighbors: 20 # 平衡连通性和计算量
sampler: uniform # 均匀采样覆盖全局
preprocessing_time: 10.0 # 预处理10秒构建路标图
num_planning_attempts: 5 # 多解优化时的尝试次数
optimize: true # 开启最优解选择(基于路径长度等目标)
FMTkConfigDefault:
type: geometric::FMT
num_samples: 1000 # number of states that the planner should sample. default: 1000
radius_multiplier: 1.1 # multiplier used for the nearest neighbors search radius. default: 1.1
nearest_k: 1 # use Knearest strategy. default: 1
cache_cc: 1 # use collision checking cache. default: 1
heuristics: 0 # activate cost to go heuristics. default: 0
extended_fmt: 1 # activate the extended FMT*: adding new samples if planner does not finish successfully. default: 1
BFMTkConfigDefault:
type: geometric::BFMT
num_samples: 1000 # number of states that the planner should sample. default: 1000
radius_multiplier: 1.0 # multiplier used for the nearest neighbors search radius. default: 1.0
nearest_k: 1 # use the Knearest strategy. default: 1
balanced: 0 # exploration strategy: balanced true expands one tree every iteration. False will select the tree with lowest maximum cost to go. default: 1
optimality: 1 # termination strategy: optimality true finishes when the best possible path is found. Otherwise, the algorithm will finish when the first feasible path is found. default: 1
heuristics: 1 # activates cost to go heuristics. default: 1
cache_cc: 1 # use the collision checking cache. default: 1
extended_fmt: 1 # Activates the extended FMT*: adding new samples if planner does not finish successfully. default: 1
PDSTkConfigDefault:
type: geometric::PDST
STRIDEkConfigDefault:
type: geometric::STRIDE
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05
use_projected_distance: 0 # whether nearest neighbors are computed based on distances in a projection of the state rather distances in the state space itself. default: 0
degree: 16 # desired degree of a node in the Geometric Near-neightbor Access Tree (GNAT). default: 16
max_degree: 18 # max degree of a node in the GNAT. default: 12
min_degree: 12 # min degree of a node in the GNAT. default: 12
max_pts_per_leaf: 6 # max points per leaf in the GNAT. default: 6
estimated_dimension: 0.0 # estimated dimension of the free space. default: 0.0
min_valid_path_fraction: 0.2 # Accept partially valid moves above fraction. default: 0.2
BiTRRTkConfigDefault:
type: geometric::BiTRRT
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
temp_change_factor: 0.1 # how much to increase or decrease temp. default: 0.1
init_temperature: 100 # initial temperature. default: 100
frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup()
frountier_node_ratio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1
cost_threshold: 1e300 # the cost threshold. Any motion cost that is not better will not be expanded. default: inf
LBTRRTkConfigDefault:
type: geometric::LBTRRT
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05
epsilon: 0.4 # optimality approximation factor. default: 0.4
BiESTkConfigDefault:
type: geometric::BiEST
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
ProjESTkConfigDefault:
type: geometric::ProjEST
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05
LazyPRMkConfigDefault:
type: geometric::LazyPRM
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
LazyPRMstarkConfigDefault:
type: geometric::LazyPRMstar
SPARSkConfigDefault:
type: geometric::SPARS
stretch_factor: 3.0 # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0
sparse_delta_fraction: 0.25 # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25
dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001
max_failures: 1000 # maximum consecutive failure limit. default: 1000
SPARStwokConfigDefault:
type: geometric::SPARStwo
stretch_factor: 3.0 # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0
sparse_delta_fraction: 0.25 # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25
dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001
max_failures: 5000 # maximum consecutive failure limit. default: 5000
TrajOptDefault:
type: geometric::TrajOpt
left_arm:
default_planner: PRMstarkConfigDefault
planner_configs:
# - SBLkConfigDefault
# - ESTkConfigDefault
# - LBKPIECEkConfigDefault
# - BKPIECEkConfigDefault
# - KPIECEkConfigDefault
# - RRTkConfigDefault
- RRTConnectkConfigDefault
- RRTstarkConfigDefault
# - TRRTkConfigDefault
# - PRMkConfigDefault
- PRMstarkConfigDefault
# - FMTkConfigDefault
# - BFMTkConfigDefault
# - PDSTkConfigDefault
# - STRIDEkConfigDefault
# - BiTRRTkConfigDefault
# - LBTRRTkConfigDefault
# - BiESTkConfigDefault
# - ProjESTkConfigDefault
# - LazyPRMkConfigDefault
# - LazyPRMstarkConfigDefault
# - SPARSkConfigDefault
# - SPARStwokConfigDefault
# - TrajOptDefault
right_arm:
default_planner: PRMstarkConfigDefault
planner_configs:
# - SBLkConfigDefault
# - ESTkConfigDefault
# - LBKPIECEkConfigDefault
# - BKPIECEkConfigDefault
# - KPIECEkConfigDefault
# - RRTkConfigDefault
- RRTConnectkConfigDefault
- RRTstarkConfigDefault
# - TRRTkConfigDefault
# - PRMkConfigDefault
- PRMstarkConfigDefault
# - FMTkConfigDefault
# - BFMTkConfigDefault
# - PDSTkConfigDefault
# - STRIDEkConfigDefault
# - BiTRRTkConfigDefault
# - LBTRRTkConfigDefault
# - BiESTkConfigDefault
# - ProjESTkConfigDefault
# - LazyPRMkConfigDefault
# - LazyPRMstarkConfigDefault
# - SPARSkConfigDefault
# - SPARStwokConfigDefault
# - TrajOptDefault
both_arms:
default_planner: PRMstarkConfigDefault
planner_configs:
# - SBLkConfigDefault
# - ESTkConfigDefault
# - LBKPIECEkConfigDefault
# - BKPIECEkConfigDefault
# - KPIECEkConfigDefault
# - RRTkConfigDefault
- RRTConnectkConfigDefault
- RRTstarkConfigDefault
# - TRRTkConfigDefault
# - PRMkConfigDefault
- PRMstarkConfigDefault
# - FMTkConfigDefault
# - BFMTkConfigDefault
# - PDSTkConfigDefault
# - STRIDEkConfigDefault
# - BiTRRTkConfigDefault
# - LBTRRTkConfigDefault
# - BiESTkConfigDefault
# - ProjESTkConfigDefault
# - LazyPRMkConfigDefault
# - LazyPRMstarkConfigDefault
# - SPARSkConfigDefault
# - SPARStwokConfigDefault
# - TrajOptDefault
完成上述修改与调整后,应该就能正常打开rviz进行运动规划了,注意,每次修改调整过后,请重新编译,并加载 ROS 工作空间的环境变量。如果嫌麻烦,将该命令写入配置文件,即可实现永久生效。终端每次启动时,会自动读取并执行其对应的配置文件中的命令。
编译命令:
colcon build --packages-select <你的包名>
永久保存:
# 替换 <你的工作空间路径> 为实际路径(如 ~/catkin_ws 或 ~/ros2_ws)
echo "source <你的工作空间路径>/install/setup.bash" >> ~/.bashrc
立即生效:
source ~/.bashrc
模型相关内容到此结束,后续更新ROS2相关节点,如何使用ROS2实现一些功能,运动学正逆解,运动规划算法,以及真机调试,路径平滑相关内容。
DAMO开发者矩阵,由阿里巴巴达摩院和中国互联网协会联合发起,致力于探讨最前沿的技术趋势与应用成果,搭建高质量的交流与分享平台,推动技术创新与产业应用链接,围绕“人工智能与新型计算”构建开放共享的开发者生态。
更多推荐



所有评论(0)