ROS2
On this page
1. 系统导览与机器人系统全景
机器人系统是怎么组织的？ROS2 在里面扮演什么角色？这一节从整体视角看机器人系统，理解各层怎么协作，以及为什么要先学运动学再学 MoveIt 2。
前言：你将构建什么？
在本教程结束时，你将能够：
- 用 URDF 描述一个两连杆机械臂的结构
- 用 ROS2 发布它的坐标系（tf2）
- 用 MoveIt 2 对它进行运动规划
- 在 RViz2 中看到机械臂从 A 点运动到 B 点的完整仿真
这不是玩具 demo，而是真实机器人系统的完整缩影。
1.1 机器人系统是怎么组织的？
1.1.1 从一个具体场景出发
想象你让机械臂"把桌上的杯子拿起来放到盒子里"。
这句话背后，机器人需要完成以下事情：
| 步骤 | 做什么 | 对应系统层 |
|---|---|---|
| 用相机看到杯子在哪 | 获取环境信息 | 感知层 |
| 建立杯子和机器人的空间关系 | 坐标系转换 | 建模层 |
| 规划一条从当前位置到杯子的路径 | 路径规划 | 规划层 |
| 把路径转换成每个关节的角度指令 | 逆运动学 + 控制 | 控制层 |
| 电机按指令转动，手臂真正移动 | 执行 | 硬件层 |
| 所有模块之间互相通信、协调 | 中间件 | 系统层 |
这就是机器人系统的六层结构。
1.1.2 六层结构详解
第一层：感知层（Perception）
职责：让机器人"看到"世界
常见传感器：
- RGB 相机：获取彩色图像，用于目标检测
- 深度相机（RGBD）：如 Intel RealSense，同时获取颜色和距离
- 激光雷达（LiDAR）：发射激光束，测量周围障碍物距离，输出点云
- IMU（惯性测量单元）：测量加速度和角速度，用于姿态估计
输出：原始数据 → 点云、图像、姿态角
类比：感知层就是机器人的"眼睛和耳朵"。没有感知层，机器人是盲的。
第二层：建模层（Modeling）
职责：让机器人"理解"自己和世界的空间关系
包含三个核心概念：
① 机器人模型（URDF）
URDF（Unified Robot Description Format）是用 XML 描述机器人结构的文件。它定义了：
- 有哪些连杆（link）
- 连杆之间用什么关节（joint）连接
- 每个部分的形状、质量、惯量
<!-- 一个简单的关节定义示例 -->
<joint name="joint1" type="revolute">
<parent link="base_link"/>
<child link="link1"/>
<axis xyz="0 0 1"/>
<limit lower="-3.14" upper="3.14" effort="100" velocity="1.0"/>
</joint>
② 坐标系系统（TF / tf2）
机器人身上每个关节、每个传感器都有自己的坐标系。tf2 负责管理这些坐标系之间的变换关系。
例如：相机看到杯子在"相机坐标系"下的位置是 (0.3, 0, 0.5)，但机械臂需要知道杯子在"机械臂基座坐标系"下的位置，这个转换就由 tf2 完成。
③ 环境地图
移动机器人需要地图来导航，机械臂需要知道工作空间中的障碍物位置。
输出：统一的空间表达，所有模块都能"说同一种语言"
第三层：规划层（Planning）
职责：决定机器人"怎么动"
- 路径规划（Path Planning）：找到从 A 到 B 的无碰撞路径
- 运动规划（Motion Planning）：在路径上加入速度、加速度约束，生成平滑轨迹
- 任务规划（Task Planning）：更高层的决策，如"先拿杯子，再放到盒子里"
输出：轨迹（一系列带时间戳的位姿点）
第四层：控制层（Control）
职责：把轨迹转换成电机能执行的指令
核心工作：
- 逆运动学（IK）：已知末端目标位置，求各关节角度
- 轨迹跟踪：实时计算当前误差，调整控制量
- PID 控制：最常用的反馈控制算法
输出：关节角度指令、力矩指令
第五层：执行层（Hardware）
职责：真正让机器人动起来
- 电机驱动器接收控制指令
- 编码器反馈当前关节角度
- 末端执行器（夹爪、吸盘）执行抓取动作
第六层：系统层（System / Middleware）
职责：让以上所有层能协同工作
这就是 ROS2 的核心价值所在：
- 各模块之间的通信（Topic / Service / Action）
- 统一的时间戳和坐标系管理
- 参数配置、日志记录
- 可视化工具（RViz2）
1.2 运动学——连接建模与规划的桥梁
1.2.1 机械臂的本质
机械臂本质上是一串"关节 + 连杆"的串联系统：
基座 → 关节1 → 连杆1 → 关节2 → 连杆2 → ... → 末端执行器
每个关节有一个自由度（通常是旋转），关节角度的组合决定了末端的位置和姿态。
一个 6 自由度机械臂（如 UR5）有 6 个关节，可以让末端到达工作空间内任意位置和姿态。
1.2.2 正运动学（Forward Kinematics，FK）
问题：已知每个关节的角度，求末端在哪里？
关节角度怎么定义？ 本例与上图一致，采用常见的 相对关节角 约定（与后面逆运动学代码一致）：
- θ1：第一根连杆相对 基座坐标系（通常取 x 轴正方向）的转角，可理解为「第一节在世界系里的朝向」。
- θ2：第二根连杆相对 第一根连杆的指向 的转角，不是相对世界 x 轴。因此第二根连杆在世界系里的绝对朝向是 θ1 + θ2，公式里第二项才会出现
cos(θ1 + θ2)、sin(θ1 + θ2)。 - 转角正方向一般取 逆针向为正（与图中常见画法一致）；若你的实物或 URDF 定义相反，只需在实现里统一符号即可。
Python 实现：
import numpy as np
def forward_kinematics(theta1_deg, theta2_deg, L1=1.0, L2=1.0):
"""
两连杆平面机械臂正运动学
参数：
theta1_deg, theta2_deg: 关节角度（度）；θ2 为相对上一节连杆的相对角
L1, L2: 连杆长度（米），默认 1.0
返回：
(x, y): 末端在基座平面坐标系中的位置
"""
# NumPy 的 cos/sin 以「弧度」为自变量；日常更习惯用「度」输入，故先换算
theta1 = np.radians(theta1_deg)
theta2 = np.radians(theta2_deg)
# 第一节末端：(L1*cos(θ1), L1*sin(θ1))；第二节沿世界系朝向 θ1+θ2 再伸 L2
x = L1 * np.cos(theta1) + L2 * np.cos(theta1 + theta2)
y = L1 * np.sin(theta1) + L2 * np.sin(theta1 + theta2)
return x, y
# 示例：θ1=45°, θ2=30°, L1=1m, L2=0.5m
x, y = forward_kinematics(45, 30, L1=1.0, L2=0.5)
print(f"末端位置: x = {x:.4f} m, y = {y:.4f} m")
# 输出: 末端位置: x = 0.8365 m, y = 1.1899 m
本质：关节空间 → 笛卡尔空间，有唯一解，计算简单。
1.2.3 逆运动学（Inverse Kinematics，IK）
问题：已知末端目标位置，求各关节角度应该是多少？
为什么 IK 更难？
同一个末端位置，可能有两种关节角度组合：
图：末端位置相同，肘下与肘上两种构型对应两组关节角。
怎么看下面这段代码？ 重点是看懂流程：先判断可达性，再用几何关系求 θ2，最后代回求 θ1。能说出「每一块在解决什么问题」即可，无需背诵细节。
参考代码（含双解，读懂思路即可）：
import numpy as np
def inverse_kinematics(x, y, L1=1.0, L2=1.0):
"""
两连杆平面机械臂逆运动学
参数：
x, y: 目标末端位置
L1, L2: 连杆长度（米），默认 1.0
返回：
dict，包含两组解：
{
"elbow_down": (theta1_deg, theta2_deg), # 肘下解，theta2 > 0
"elbow_up": (theta1_deg, theta2_deg), # 肘上解，theta2 < 0
}
如果目标不可达，抛出 ValueError
注意：返回角度单位为度
"""
# 检查目标是否可达
dist = np.sqrt(x**2 + y**2)
if dist > L1 + L2:
raise ValueError(f"目标点 ({x}, {y}) 超出工作空间，最大可达距离为 {L1+L2:.3f} m")
if dist < abs(L1 - L2):
raise ValueError(f"目标点 ({x}, {y}) 太近，最小可达距离为 {abs(L1-L2):.3f} m")
# 用余弦定理求 θ2
cos_theta2 = (x**2 + y**2 - L1**2 - L2**2) / (2 * L1 * L2)
cos_theta2 = np.clip(cos_theta2, -1, 1) # 防止浮点误差
theta2_down = np.arccos(cos_theta2) # 肘下解：θ2 > 0
theta2_up = -theta2_down # 肘上解：θ2 < 0
def solve_theta1(theta2):
k1 = L1 + L2 * np.cos(theta2)
k2 = L2 * np.sin(theta2)
return np.arctan2(y, x) - np.arctan2(k2, k1)
return {
"elbow_down": (np.degrees(solve_theta1(theta2_down)), np.degrees(theta2_down)),
"elbow_up": (np.degrees(solve_theta1(theta2_up)), np.degrees(theta2_up)),
}
# 示例
solutions = inverse_kinematics(0.836, 1.190, L1=1.0, L2=0.5)
for name, (t1, t2) in solutions.items():
print(f"{name}: θ1 = {t1:.2f}°, θ2 = {t2:.2f}°")
再次强调：上面代码是建立直觉用的示例，真正重要的是理解 IK 可能双解、可能无解，以及系统里谁会替你算。
本质：笛卡尔空间 → 关节空间，可能有多个解（甚至无解）。
1.2.4 在系统中的位置
用户指定目标位置 (x, y, z)
↓
逆运动学（IK）
↓
各关节角度 θ1, θ2, ...
↓
控制器执行
↓
机器人运动
反向验证（用于可视化）：
当前关节角度 θ1, θ2, ...
↓
正运动学（FK）
↓
末端当前位置 → 发布到 RViz2 显示
1.3 为什么必须学 ROS2？
1.3.1 没有 ROS2 会怎样？
假设你要自己写一个机械臂控制系统，你需要：
- 自己写相机数据读取程序
- 自己写激光雷达数据处理程序
- 自己设计这两个程序之间的通信协议（用 socket？共享内存？）
- 自己写坐标系转换代码
- 自己写可视化界面
- 自己管理所有程序的启动顺序和崩溃重启
- ...
每一项都需要大量工作，而且不同团队写的代码无法复用。
1.3.2 ROS2 解决了什么？
ROS2（Robot Operating System 2）提供了一套标准化的机器人软件框架：
下面是 ROS2 五个核心能力的速览。命令和代码片段不需要现在动手，第一遍读下来知道 ROS2 能做什么就够了，后续每一节都会把对应能力讲透。
核心能力一：分布式节点系统
每个功能模块是一个独立的节点（Node），可以：
- 独立运行、独立重启
- 运行在不同的机器上（分布式）
- 用不同语言编写（Python / C++）
# 查看当前运行的所有节点
ros2 node list
# 查看某个节点的详细信息
ros2 node info /robot_state_publisher
核心能力二：标准化通信机制
Topic（话题）：发布/订阅模式，适合持续的数据流
# 查看所有话题
ros2 topic list
# 实时查看关节状态数据
ros2 topic echo /joint_states
# 查看相机图像话题的发布频率
ros2 topic hz /camera/image_raw
Service（服务）：请求/响应模式，适合一次性操作
# 调用一个布尔服务
ros2 service call /enable_motor std_srvs/srv/SetBool "{data: true}"
Action（动作）：适合长时间任务，支持中途反馈和取消
# 发送运动目标（MoveIt 2 内部使用 Action）
ros2 action list
ros2 action info /move_action
核心能力三：tf2 坐标系统
tf2 维护一棵"坐标系树"，任意两个坐标系之间的变换都可以自动计算。
# 生成坐标系树的 PDF 图（需要 graphviz）
ros2 run tf2_tools view_frames
# 实时查询两个坐标系之间的变换
ros2 run tf2_ros tf2_echo base_link end_effector
核心能力四：Launch 系统
一个 launch 文件可以同时启动多个节点，并配置参数：
# my_robot/launch/bringup.launch.py
from launch import LaunchDescription
from launch_ros.actions import Node
def generate_launch_description():
return LaunchDescription([
Node(
package='robot_state_publisher',
executable='robot_state_publisher',
parameters=[{'robot_description': open('my_robot.urdf').read()}]
),
Node(
package='rviz2',
executable='rviz2',
arguments=['-d', 'config/view_robot.rviz']
),
])
# 一键启动整个系统
ros2 launch my_robot bringup.launch.py
核心能力五：RViz2 可视化
RViz2 是 ROS2 的标准可视化工具，可以实时显示：
- 机器人模型（URDF）
- 坐标系（TF）
- 传感器数据（点云、图像）
- 规划路径
- 碰撞检测结果
# 启动 RViz2
rviz2
# 带配置文件启动
rviz2 -d my_config.rviz
1.3.3 一句话理解 ROS2
ROS2 = 机器人系统的"操作系统级框架"
就像 Linux 为应用程序提供进程管理、文件系统、网络通信一样， ROS2 为机器人模块提供节点管理、数据通信、坐标系统。
1.4 教程学习路线
1.4.1 整体路线
1. 系统全景认知（本节）
↓
2-3. 运动学基础（坐标系、齐次变换、正/逆运动学）
↓
4-7. ROS2 工程基础（节点、Topic、Service、Action、Launch）
↓
8. tf2 坐标系统（核心中的核心）
↓
9. URDF 机器人建模
↓
10-11. MoveIt 2 运动规划
↓
12. 完整项目整合
1.4.2 每节的学习方式
本教程采用"概念 → 代码 → 运行验证"的三步法：
- 概念：用类比和图示理解原理
- 代码：写出最小可运行的示例
- 运行验证：在终端或 RViz2 中看到实际效果
1.5 环境准备
1.5.1 本教程使用的软件版本
| 软件 | 版本 | 说明 |
|---|---|---|
| Ubuntu | 24.04 LTS | 推荐操作系统 |
| ROS2 | Jazzy | 对应 Ubuntu 24.04 的 LTS 版本 |
| Python | 3.12 | Ubuntu 24.04 系统自带 |
| MoveIt 2 | 2.x (Jazzy) | 运动规划框架 |
如果你使用 Ubuntu 22.04，对应安装 ROS2 Humble，包名把
jazzy替换成humble即可，核心概念完全一致。
1.5.2 安装 ROS2 Jazzy
第一步：确认 locale
Ubuntu 24.04 默认已配置，通常无需操作，验证一下即可：
locale # 确认输出包含 en_US.UTF-8
第二步：安装依赖并添加 GPG Key
sudo apt update && sudo apt install -y curl gnupg lsb-release
sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key \
-o /usr/share/keyrings/ros-archive-keyring.gpg
国内网络说明：
raw.githubusercontent.com在国内经常被墙，上面的命令可能会卡住不动。遇到这种情况，Ctrl+C终止后，改用 Gitee 镜像：
第三步：写入 ROS2 软件源
重要：不要用
echo命令写入。在终端粘贴多行命令时，换行符会被写入文件，导致apt update报错Malformed entry ([option] unparsable)。用 vim 直接写入可完全避免此问题。
sudo vim /etc/apt/sources.list.d/ros2.list
在 vim 中输入以下内容（只有一行）：
deb [arch=amd64 signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] https://mirrors.tuna.tsinghua.edu.cn/ros2/ubuntu noble main
这里使用清华镜像源。官方源
http://packages.ros.org/ros2/ubuntu在国内速度极慢（约 8 KB/s），强烈推荐替换。
写入后验证（确认输出是完整的一行，不能有换行）：
cat /etc/apt/sources.list.d/ros2.list
sudo apt update
第四步：安装 ROS2 Jazzy Desktop
# desktop 版包含 RViz2、示例程序等，推荐安装
sudo apt install -y ros-jazzy-desktop python3-colcon-common-extensions python3-rosdep
包体积较大，使用清华镜像约需数分钟。中断后重新运行同一命令可续传，已下载的包缓存在
/var/cache/apt/archives/，不会重新下载。
第五步：配置环境变量
# 每次打开终端自动加载 ROS2 环境
echo "source /opt/ros/jazzy/setup.bash" >> ~/.bashrc
source ~/.bashrc
第六步：初始化 rosdep（用于自动安装依赖）
国内网络说明：
rosdep update同样需要访问raw.githubusercontent.com，在国内会报Connection reset by peer错误。推荐用rosdepc替代，功能完全相同，走国内镜像：后续所有
rosdep命令均可直接替换为rosdepc。
如果网络正常，也可以直接用原版：
sudo rosdep init
rosdep update
执行时可能出现
DeprecationWarning: pkg_resources is deprecated as an API，这是正常警告，不影响使用，忽略即可。另外，若提示default sources list file already exists，说明已初始化过，跳过init直接执行update即可。
1.5.3 配置 Python 虚拟环境
ROS2 的 Python 包（rclpy 等）通过 apt 安装到系统 Python，不建议用 Anaconda，否则路径隔离会导致 import rclpy 失败。
推荐用 venv + --system-site-packages：
前置依赖：Ubuntu 24.04 默认不带
venv模块，若提示ensurepip is not available，先安装：
# 创建虚拟环境，--system-site-packages 让 venv 能访问系统级 ROS2 包
python3 -m venv ~/ros2_venv --system-site-packages
# 激活（每次开新终端都需要执行）
source ~/ros2_venv/bin/activate
# 安装教程所需的科学计算库
pip install numpy scipy matplotlib
# 验证 ROS2 包仍然可以访问
python3 -c "import rclpy; print('rclpy OK')"
参数说明：
python3 -m venv ~/ros2_venv --system-site-packages
│ │ └─ 允许访问系统 site-packages（含 rclpy）
│ └─ 虚拟环境存放路径
└─ 调用内置 venv 模块创建虚拟环境
建议把激活命令也加入 .bashrc：
echo "source ~/ros2_venv/bin/activate" >> ~/.bashrc
source ~/.bashrc
1.5.4 验证 ROS2 安装
# 终端1：
ros2 run demo_nodes_py talker
# 终端2：
ros2 run demo_nodes_py listener
如果看到 listener 打印出 talker 发送的 Hello World 消息，说明 ROS2 安装正常。
注意：
ros2 --version会报错unrecognized arguments: --version，这是正常的，ROS2 不支持该参数，用 talker/listener 验证即可。
1.5.5 工作空间结构
本教程的代码组织方式：
ros2_ws/ # ROS2 工作空间根目录
├── src/ # 源代码目录
│ ├── my_robot_basics/ # 运动学函数、基础节点（3-8）
│ ├── my_robot_interfaces/ # 自定义 Service/Action 接口（6）
│ ├── my_robot_description/ # 机器人 URDF 模型（9）
│ ├── my_robot_moveit_config/ # MoveIt 2 配置包（10）
│ ├── my_robot_planner/ # 规划客户端节点（10-11）
│ └── my_robot_bringup/ # 一键启动 Launch 文件（11）
├── build/ # 编译输出（自动生成）
├── install/ # 安装目录（自动生成）
└── log/ # 日志目录（自动生成）
各包会在对应章节中逐步创建，不需要现在手动建立。
创建工作空间：
本教程工作空间说明：教程源码统一存放在
$ROS_WS/下，与教程文档平行。后续所有命令均直接使用该路径。
# 创建工作空间目录
mkdir -p $ROS_WS/src
# 进入工作空间，执行首次构建
cd $ROS_WS
colcon build
source install/setup.bash
# 建议把 source 加入 .bashrc，避免每次手动执行
echo "source $ROS_WS/install/setup.bash" >> ~/.bashrc
source ~/.bashrc
1.6 常见误区（重点）
1.6.1 误区一：跳过运动学，直接学 MoveIt 2
很多人觉得运动学数学太难，想直接用 MoveIt 2 的 API。
后果：
- 规划失败时不知道为什么（是目标不可达？还是配置错误？）
- 无法判断机械臂的工作空间范围
- 遇到奇异点（singularity）时完全不知所措
正确做法：先用 Python 手写两连杆的正/逆运动学，理解原理，再用 MoveIt 2。
1.6.2 误区二：把 ROS2 当成"发消息的工具"
ROS2 不只是通信框架，它是：
- 工程管理平台：管理节点生命周期、参数、日志
- 标准化接口：让不同团队的代码能互相配合
- 生态系统：大量现成的驱动、算法包可以直接使用
1.6.3 误区三：认为 MoveIt 2 = 运动规划算法
MoveIt 2 本身不是算法，而是算法集成框架。它：
- 调用 OMPL（Open Motion Planning Library）做路径规划
- 依赖 URDF 和 tf2 获取机器人模型和坐标系
- 依赖 IK 求解器（如 KDL、IKFast）做逆运动学
理解这一点，才能在出问题时知道去哪里找原因。
1.7 本节练习
1.7.1 练习 1：用自己的话描述六层结构
不看笔记，尝试向别人解释：
- 感知层做什么？输出什么？
- 建模层做什么？输出什么？
- 规划层做什么？输出什么？
- 控制层做什么？输出什么？
能流畅解释，说明你真正理解了。
1.7.2 练习 2：手算两连杆正运动学
给定：L1 = 1.0m，L2 = 0.8m，θ1 = 30°，θ2 = 45°
先手算，再用 Python 验证：
import numpy as np
L1, L2 = 1.0, 0.8
theta1 = np.radians(30)
theta2 = np.radians(45)
x = L1 * np.cos(theta1) + L2 * np.cos(theta1 + theta2)
y = L1 * np.sin(theta1) + L2 * np.sin(theta1 + theta2)
print(f"末端位置: x = {x:.4f} m, y = {y:.4f} m")
# 预期输出: x = 1.2071 m, y = 1.2243 m
1.7.3 练习 3：验证 ROS2 环境
# 打开两个终端，分别运行：
# 终端1
ros2 run demo_nodes_py talker
# 终端2
ros2 run demo_nodes_py listener
观察输出，确认通信正常后，用 Ctrl+C 停止。
本节核心总结
| 概念 | 一句话理解 |
|---|---|
| 机器人系统六层结构 | 感知→建模→规划→控制→执行，系统层贯穿全部 |
| 正运动学 | 已知关节角 → 求末端位置，有唯一解 |
| 逆运动学 | 已知末端位置 → 求关节角，可能有多解或无解 |
| ROS2 | 机器人系统的标准化中间件框架 |
| tf2 | 管理所有坐标系变换的核心组件 |
| URDF | 用 XML 描述机器人结构的标准格式 |
| MoveIt 2 | 运动规划算法的集成框架，不是算法本身 |
参考代码
本节为概念导论，无对应源码文件。
参考代码目录结构（后续各章节逐步填充）：
ros_ws/
├── scripts/ ← 2-3. 纯 Python 脚本，可直接运行
│ ├── lesson_02_transforms.py
│ └── lesson_03_two_link_kinematics.py
└── reference/src/ ← 各 ROS2 包的完整参考实现
├── my_robot_basics/ ← 4-8.
├── my_robot_interfaces/ ← 6.
├── my_robot_description/ ← 9.
├── my_robot_planner/ ← 10-11.
└── my_robot_bringup/ ← 11.
使用方式：
scripts/下的脚本可直接python3运行，无需 ROS2 环境reference/src/下的包是完整参考实现，用户走完ros2 pkg create流程后，可将参考文件 cp 到自己的包中对照或直接使用：
# 示例：把参考代码 cp 到自己的包
cp $ROS_WS/reference/src/my_robot_basics/my_robot_basics/hello_node.py \
$ROS_WS/src/my_robot_basics/my_robot_basics/
参考代码中的注释标注了关键逻辑和对应教程章节，建议先自己实现，遇到问题再对照。
1.8 本节小结与下一步
到这里，你已经建立了机器人系统的整体认知：知道了六层结构各自负责什么，也明确了 ROS2、tf2、URDF、MoveIt 2 在整条链路中的位置。
下一步进入第 2 章时，重点不再是“记住术语”，而是把坐标系和变换关系真正算清楚。只要第 2、3 章打稳，后续 ROS2 通信、建模和规划会顺很多。
下一节预告
2. 运动学核心（工程版）
我们将从零推导两连杆机械臂的运动学，内容包括：
- 坐标系的定义与变换
- 齐次变换矩阵的几何意义
- 用 Python 实现正运动学
- 用 Python 实现逆运动学（含多解处理）
- 可视化验证结果
全程代码驱动，不做复杂数学推导，只讲能用来写代码的核心。