ROS2系列教程：运动学基础（位姿与坐标变换）

本文是 ROS2 系列教程的第 13 篇

本文是 ROS2 系列教程的第 13 篇，也是机器人学篇的开篇：运动学基础——位姿与坐标变换。机器人编程绕不开坐标系：机器人在哪里？朝向哪里？传感器测到的点在什么坐标系下？这两个问题的数学回答就是"位姿（Pose）“与"坐标变换（Transform）”。本篇文章从数学原理讲到 ROS2 代码：位姿怎么表示（位置+姿态）、旋转的三种表示（旋转矩阵/欧拉角/四元数）及互转、齐次变换矩阵的串联与求逆、ROS2 中姿态的规范与消息类型。学完你将有能力读懂 tf2、URDF、导航、机械臂所有篇章的数学地基。

一、为什么要先学位姿与坐标变换

1.1 机器人世界里的一切都是"在哪个坐标系下"

先看一个真实场景：你的机器人底盘中心装了一台激光雷达，雷达前方 1.0 米处有一面墙。

• 雷达说：“墙在我 x 轴正方向 1.0 米处。” —— 这是雷达坐标系下的测量。
• 但导航模块需要知道：“墙在世界地图的什么位置？” —— 这是地图坐标系下的问题。
• 两个答案相差多少？取决于雷达装得偏不偏、机器人当前在哪儿、朝哪儿。

坐标变换就是把"一个坐标系下的描述"换算成"另一个坐标系下的描述"。它是所有机器人系统的通用语言：感知、建图、定位、导航、规划、控制，每一环都在做这件事。

1.2 位姿 = 位置 + 姿态

描述一个物体（或坐标系）在空间中的状态，需要 6 个自由度：

• 位置（Position）：3 个自由度，物体原点在参考系中的坐标 (x, y, z)。
• 姿态（Orientation）：3 个自由度，物体坐标系相对参考系"转了多少"。

位姿（Pose）就是二者的合称。ROS2 中最常见的消息 geometry_msgs/msg/Pose 就是 position（点）+ orientation（四元数）两部分：

geometry_msgs/msg/Pose
├── Point position # 位置 x, y, z
│ ├── float64 x
│ ├── float64 y
│ └── float64 z
└── Quaternion orientation # 姿态（四元数）
├── float64 x
├── float64 y
├── float64 z
└── float64 w

注意：位置和姿态是两回事，但常被初学者混为一谈。一台机器人可以站在同一个位置 (0, 0, 0)，却可以朝向任何方向——位置没变，姿态变了。

1.3 姿态为什么难？

位置就是三个数，人人都会。姿态难在：旋转不能用"三个数"简单地表示（至少不能直观地表示）。

• 欧拉角（roll/pitch/yaw）直观但有万向锁问题，且结果依赖旋转顺序。
• 旋转矩阵无奇异但 9 个数有冗余（实际只有 3 个自由度），还伴随正交性约束。
• 四元数紧凑、无奇异、便于插值，但不直观，新手容易在 x/y/z/w 上搞混。

ROS2 的规范答案：内部统一用四元数。你可以在脑中用欧拉角思考，在代码里必须转成四元数。这就是本篇文章的核心任务：把三种表示都讲透，并给出可运行的互转代码。

二、旋转的三种表示（核心数学）

2.1 旋转矩阵 R

绕坐标轴的基本旋转矩阵（右手系，逆时针为正）：

绕 x 轴转 α：

R_x(α) = | 1 0 0 |
| 0 cos α -sin α |
| 0 sin α cos α |

绕 y 轴转 β：

R_y(β) = | cos β 0 sin β |
| 0 1 0 |
| -sin β 0 cos β |

绕 z 轴转 γ：

R_z(γ) = | cos γ -sin γ 0 |
| sin γ cos γ 0 |
| 0 0 1 |

旋转矩阵的三个性质（务必记住，后面校验全靠它）：

旋转矩阵把"基向量在新坐标系中的表达"写成列向量，所以它天然携带"坐标系"信息：R 的第 i 列就是原坐标系第 i 个基向量在新坐标系下的坐标。

2.2 欧拉角（RPY）

欧拉角用"绕三个轴依次转三次"描述姿态。ROS2/机器人领域最常用的是 RPY 约定（Roll 横滚、Pitch 俯仰、Yaw 偏航）：

R = R_z(yaw) · R_y(pitch) · R_x(roll)

注意：矩阵乘法不满足交换律，旋转顺序必须固定。ROS2 默认约定是"先绕 z（yaw）、再绕 y（pitch）、最后绕 x（roll）"，即从参考系出发依次施加 yaw → pitch → roll 的外旋（或等效地，对物体自身依次做 roll → pitch → yaw 的内旋）。你在代码里写 RPY 转四元数时，用的就是这个约定。

万向锁（Gimbal Lock）：当 pitch = ±90° 时，roll 与 yaw 的旋转轴重合，丢失一个自由度——表现为"两个角可以互相抵消，姿态描述不唯一"。这是欧拉角的固有缺陷，无解，只能换表示方法。

飞机类比：roll 是机身侧倾，pitch 是机头抬起，yaw 是机头左右转。
当飞机笔直朝上（pitch=90°）时，侧倾和转向都在绕同一根轴，就"锁死"了。

2.3 四元数（Quaternion）

四元数是一个超复数：q = w + xi + yj + zk，其中 w 是实部，(x, y, z) 是虚部。ROS2 消息里把四个分量按 x, y, z, w 排列（注意顺序！w 在最后）。

旋转轴-角表示：绕单位向量 (nx, ny, nz) 转 θ 角，对应的四元数是：

w = cos(θ/2)
x = nx · sin(θ/2)
y = ny · sin(θ/2)
z = nz · sin(θ/2)

四元数的性质：

为什么 ROS2 用它：无奇异点（没有万向锁）、只占 4 个数（比旋转矩阵少 5 个）、便于球面插值（slerp）做平滑过渡。代价是不直观——这也是你必须学会互转的原因。

2.4 三种表示互转公式

四元数 → 旋转矩阵（单位四元数 q）：

R = | 1-2(y²+z²) 2(xy-wz) 2(xz+wy) |
| 2(xy+wz) 1-2(x²+z²) 2(yz-wx) |
| 2(xz-wy) 2(yz+wx) 1-2(x²+y²)|

旋转矩阵 → 欧拉角（RPY 约定，atan2 保象限）：

roll = atan2(R[2][1], R[2][2])
pitch = asin(-R[2][0])
yaw = atan2(R[1][0], R[0][0])

四元数 → 欧拉角：

roll = atan2(2(wx+yz), 1-2(x²+y²))
pitch = asin(2(wy-xz))
yaw = atan2(2(wz+xy), 1-2(y²+z²))

欧拉角 → 四元数（yaw → pitch → roll 顺序）：

cy = cos(yaw/2), sy = sin(yaw/2)
cp = cos(pitch/2), sp = sin(pitch/2)
cr = cos(roll/2), sr = sin(roll/2)

w = cr·cp·cy + sr·sp·sy
x = sr·cp·cy – cr·sp·sy
y = cr·sp·cy + sr·cp·sy
z = cr·cp·sy – sr·sp·cy

记住这条主线：欧拉角（人脑）→ 四元数（ROS2 内部）→ 旋转矩阵（数值计算）。你在 ROS2 里发布姿态用四元数，与 tf2 交互用旋转矩阵或四元数，跟人交流用欧拉角。

三、齐次变换矩阵：把位置和姿态打包

3.1 从"旋转+平移"到 4×4 矩阵

单有旋转矩阵 R 只能描述姿态，坐标变换还需要平移 t。把两者合成一个 4×4 的齐次变换矩阵 T：

T = | R t |
| 0 1 |

展开：

T = | r11 r12 r13 | tx |
| r21 r22 r23 | ty |
| r31 r32 r33 | tz |
| 0 0 0 | 1 |

点变换：把点 p（齐次坐标 [x, y, z, 1]）从 A 系变换到 B 系：

p_B = T_B←A · p_A

为什么加一行 0 0 0 1：因为矩阵乘法要求维度匹配，加这一行后平移可以写进矩阵乘法里，实现"旋转和平移统一为一次矩阵乘法"。

3.2 变换的串联：链式相乘

假设三个坐标系 A → B → C，已知 T_B←A 和 T_C←B，则：

T_C←A = T_C←B · T_B←A

顺序不可交换：T_C←B · T_B←A ≠ T_B←A · T_C←B。写代码时务必从右往左读：先经过 B，再经过 C。

这就是 TF 树（第 15 篇详讲）的数学本质：tf2 把整个机器人所有坐标系之间的变换存成一张树，查询任意两个坐标系之间的变换，就是沿着树做矩阵链乘法。

3.3 变换的求逆

已知 T_B←A，求 T_A←B（反向变换）：

T_A←B = T_B←A⁻¹ = | R^T -R^T·t |
| 0 1 |

即：旋转部分取转置，平移部分取 -R^T·t。这是 tf2 里"反向查询"的实现基础。

3.4 一个手算示例

设 B 系相对 A 系：绕 z 轴转 90°（yaw=90°），再沿 x 轴平移 1 米。则：

R = | 0 -1 0 |
| 1 0 0 |
| 0 0 1 |

t = | 1 |
| 0 |
| 0 |

B 系原点 (0,0,0) 在 A 系中的坐标 = t = (1, 0, 0)。B 系中 (1, 0, 0) 的点在 A 系中：

p_A = R·(1,0,0) + t = (0, 1, 0) + (1, 0, 0) = (1, 1, 0)

验证直觉：绕 z 转 90° 后，x 轴正向指向原 y 轴正向，所以 (1,0,0) 变成 (0,1,0)，再整体平移到 (1,1,0)。✓

四、ROS2 中的姿态规范与消息类型

4.1 关键消息类型

必须记住的规范：

4.2 常用工具函数

C++（tf2 库，随 ros2 自带）：

#include <tf2/LinearMath/Quaternion.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>

// 欧拉角 → 四元数
tf2::Quaternion q;
q.setRPY(roll, pitch, yaw); // 弧度制！
geometry_msgs::msg::Quaternion qmsg = tf2::toMsg(q);

// 四元数 → 欧拉角
tf2::Quaternion q2;
tf2::fromMsg(qmsg, q2);
double r, p, y;
tf2::Matrix3x3(q2).getRPY(r, p, y); // 得到弧度

Python（tf_transformations，随 ros2 自带）：

from tf_transformations import quaternion_from_euler, euler_from_quaternion

# 欧拉角 → 四元数（返回 [x, y, z, w]）
q = quaternion_from_euler(roll, pitch, yaw) # 弧度制

# 四元数 → 欧拉角
roll, pitch, yaw = euler_from_quaternion([qx, qy, qz, qw])

两个库都遵循同一约定：RPY 顺序，弧度制，结果 w 在最后。这保证了 C++ 和 Python 代码可以互换结果。

五、可运行示例（4 个）

下面 4 个示例覆盖"理解互转 → 动手发布 → 动手变换"的完整链路。请逐个在你的工作空间中创建、编译、运行。

示例 1：Python 位姿互转计算器（不进 ROS2，纯数学验证）

目的：验证你对互转公式的理解，并熟悉 tf_transformations。

#!/usr/bin/env python3
"""pose_math.py：欧拉角 <-> 四元数 <-> 旋转矩阵 互转演示"""
import math
from tf_transformations import quaternion_from_euler, euler_from_quaternion

def normalize(q):
"""归一化四元数"""
n = math.sqrt(q[0]**2 + q[1]**2 + q[2]**2 + q[3]**2)
return [v / n for v in q]

def quat_rotate_point(q, p):
"""用四元数旋转一个点（罗德里格斯等价实现）"""
qx, qy, qz, qw = q
# 公式：p' = p + 2w(v×p) + 2(v×(v×p))
v = [qx, qy, qz]
cross1 = [v[1]*p[2]–v[2]*p[1], v[2]*p[0]–v[0]*p[2], v[0]*p[1]–v[1]*p[0]]
cross2 = [v[1]*cross1[2]–v[2]*cross1[1], v[2]*cross1[0]–v[0]*cross1[2], v[0]*cross1[1]–v[1]*cross1[0]]
return [p[i] + 2*qw*cross1[i] + 2*cross2[i] for i in range(3)]

if __name__ == '__main__':
# 1) 欧拉角 -> 四元数
yaw = math.radians(90.0)
pitch = math.radians(0.0)
roll = math.radians(0.0)
q = quaternion_from_euler(roll, pitch, yaw)
print(f"yaw=90° -> q = {[round(v,4) for v in q]}") # 期望约 [0, 0, 0.7071, 0.7071]

# 2) 校验归一化
qn = normalize(q)
print(f"归一化后模长 = {math.sqrt(sum(v*v for v in qn)):.6f}")

# 3) 四元数 -> 欧拉角（验证往返）
r, p, y = euler_from_quaternion(q)
print(f"q -> 欧拉角 = ({math.degrees(r):.1f}, {math.degrees(p):.1f}, {math.degrees(y):.1f}) deg")

# 4) 用四元数旋转点 (1, 0, 0)：绕 z 转 90° 应得到 (0, 1, 0)
rotated = quat_rotate_point(q, [1.0, 0.0, 0.0])
print(f"绕 z 转 90° 后 (1,0,0) -> {[round(v,4) for v in rotated]}")

运行：

python3 pose_math.py

预期输出（前两行）：

yaw=90° -> q = [0.0, 0.0, 0.7071, 0.7071]
归一化后模长 = 1.000000

示例 2：C++ 旋转矩阵与四元数互转节点

目的：在 ROS2 节点里用 tf2 库做互转，并学会"发布前校验四元数"。

// quat_node.cpp：欧拉角<->四元数互转 + 合法性校验
#include <rclcpp/rclcpp.hpp>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#include <cmath>

class QuatNode : public rclcpp::Node
{
public:
QuatNode() : Node("quat_node")
{
// 1) 欧拉角(度) -> 四元数
double roll_deg = 30.0, pitch_deg = 20.0, yaw_deg = 45.0;
double roll = roll_deg * M_PI / 180.0;
double pitch = pitch_deg * M_PI / 180.0;
double yaw = yaw_deg * M_PI / 180.0;

tf2::Quaternion q;
q.setRPY(roll, pitch, yaw);
q.normalize();
geometry_msgs::msg::Quaternion qmsg = tf2::toMsg(q);

RCLCPP_INFO(this->get_logger(),
"RPY(%0.1f, %0.1f, %0.1f) -> quat(x=%0.4f, y=%0.4f, z=%0.4f, w=%0.4f)",
roll_deg, pitch_deg, yaw_deg, qmsg.x, qmsg.y, qmsg.z, qmsg.w);

// 2) 校验模长是否为 1
double norm = std::sqrt(qmsg.x*qmsg.x + qmsg.y*qmsg.y
+ qmsg.z*qmsg.z + qmsg.w*qmsg.w);
if (std::abs(norm – 1.0) > 1e-6) {
RCLCPP_ERROR(this->get_logger(), "四元数未归一化！norm=%f", norm);
} else {
RCLCPP_INFO(this->get_logger(), "四元数归一化校验通过 (norm=%f)", norm);
}

// 3) 四元数 -> 欧拉角（验证往返）
tf2::Quaternion q2;
tf2::fromMsg(qmsg, q2);
tf2::Matrix3x3 m(q2);
double r, p, y;
m.getRPY(r, p, y);
RCLCPP_INFO(this->get_logger(),
"quat -> RPY(deg) = (%0.1f, %0.1f, %0.1f)",
r * 180.0 / M_PI, p * 180.0 / M_PI, y * 180.0 / M_PI);
}
};

int main(int argc, char** argv)
{
rclcpp::init(argc, argv);
auto node = std::make_shared<QuatNode>();
rclcpp::shutdown();
return 0;
}

CMakeLists.txt 要点（find_package 加上 tf2 和 tf2_geometry_msgs）：

find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(tf2 REQUIRED)
find_package(tf2_geometry_msgs REQUIRED)

add_executable(quat_node src/quat_node.cpp)
ament_target_dependencies(quat_node rclcpp tf2 tf2_geometry_msgs)
install(TARGETS quat_node DESTINATION lib/${PROJECT_NAME})

运行：

colcon build –packages-select quat_demo
source install/setup.bash
ros2 run quat_demo quat_node

预期日志：

RPY(30.0, 20.0, 45.0) -> quat(x=0.2393, y=0.1449, z=0.4277, w=0.8536)
四元数归一化校验通过 (norm=1.000000)
quat -> RPY(deg) = (30.0, 20.0, 45.0)

示例 3：C++ 发布带姿态的 PoseStamped 话题

目的：掌握发布位姿消息的标准姿势（含 header 与四元数设置）。

// pose_pub.cpp：周期性发布机器人位姿
#include <rclcpp/rclcpp.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#include <chrono>

class PosePub : public rclcpp::Node
{
public:
PosePub() : Node("pose_pub"), yaw_(0.0)
{
pub_ = this->create_publisher<geometry_msgs::msg::PoseStamped>(
"/robot_pose", 10);
timer_ = this->create_wall_timer(
std::chrono::milliseconds(500),
std::bind(&PosePub::timer_cb, this));
}

private:
void timer_cb()
{
// 模拟机器人沿圆形轨迹运动并不断转向
yaw_ += 0.1; // 每 0.5s 转 0.1 rad
double radius = 2.0;
double x = radius * std::cos(yaw_);
double y = radius * std::sin(yaw_);

geometry_msgs::msg::PoseStamped msg;
msg.header.stamp = this->now();
msg.header.frame_id = "map"; // 必须写清楚坐标系！

msg.pose.position.x = x;
msg.pose.position.y = y;
msg.pose.position.z = 0.0;

tf2::Quaternion q;
q.setRPY(0.0, 0.0, yaw_); // 平面运动：只有偏航
msg.pose.orientation = tf2::toMsg(q);

pub_->publish(msg);
RCLCPP_INFO(this->get_logger(),
"pose: (%.2f, %.2f) yaw=%.2f rad", x, y, yaw_);
}

rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr pub_;
rclcpp::TimerBase::SharedPtr timer_;
double yaw_;
};

int main(int argc, char** argv)
{
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<PosePub>());
rclcpp::shutdown();
return 0;
}

观察话题：

ros2 run pose_demo pose_pub &
ros2 topic echo /robot_pose –once

你会看到 header.frame_id: map、位置随时间变化、四元数始终是单位四元数（只有 z/w 分量随 yaw 变化）。

示例 4：Python 发布+订阅位姿并实时转成欧拉角

目的：Python 端完整闭环——发 PoseStamped，订阅端把四元数转回欧拉角显示。

#!/usr/bin/env python3
"""pose_demo.py：发布/订阅位姿，订阅端转欧拉角"""
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import PoseStamped
from tf_transformations import quaternion_from_euler, euler_from_quaternion
import math

class PosePublisher(Node):
def __init__(self):
super().__init__('pose_publisher')
self.pub = self.create_publisher(PoseStamped, '/pose_demo', 10)
self.timer = self.create_timer(1.0, self.timer_cb)
self.yaw = 0.0

def timer_cb(self):
self.yaw += 0.3
msg = PoseStamped()
msg.header.stamp = self.get_clock().now().to_msg()
msg.header.frame_id = 'map'
msg.pose.position.x = 1.0 * math.cos(self.yaw)
msg.pose.position.y = 1.0 * math.sin(self.yaw)
q = quaternion_from_euler(0.0, 0.0, self.yaw) # [x,y,z,w]
msg.pose.orientation.x = q[0]
msg.pose.orientation.y = q[1]
msg.pose.orientation.z = q[2]
msg.pose.orientation.w = q[3]
self.pub.publish(msg)

class PoseSubscriber(Node):
def __init__(self):
super().__init__('pose_subscriber')
self.sub = self.create_subscription(
PoseStamped, '/pose_demo', self.cb, 10)

def cb(self, msg):
q = [msg.pose.orientation.x, msg.pose.orientation.y,
msg.pose.orientation.z, msg.pose.orientation.w]
roll, pitch, yaw = euler_from_quaternion(q)
self.get_logger().info(
'收到位姿: pos=(%.2f, %.2f, %.2f) yaw=%.1f deg (frame=%s)',
msg.pose.position.x, msg.pose.position.y, msg.pose.position.z,
math.degrees(yaw), msg.header.frame_id)

def main(args=None):
rclpy.init(args=args)
pub = PosePublisher()
sub = PoseSubscriber()
executor = rclpy.executors.MultiThreadedExecutor()
executor.add_node(pub)
executor.add_node(sub)
try:
executor.spin()
finally:
executor.shutdown()
pub.destroy_node()
sub.destroy_node()
rclpy.shutdown()

if __name__ == '__main__':
main()

运行：

python3 pose_demo.py

日志示例：

[INFO] 收到位姿: pos=(1.00, 0.00, 0.00) yaw=0.0 deg (frame=map)
[INFO] 收到位姿: pos=(0.96, 0.30, 0.00) yaw=17.2 deg (frame=map)
[INFO] 收到位姿: pos=(0.83, 0.56, 0.00) yaw=34.4 deg (frame=map)

注意 pos=(1.00, 0.00) 时 yaw=0.0：机器人朝向 x 轴正方向；当它走到 (0.96, 0.30) 时 yaw≈17.2°，朝向始终与运动方向一致——这正是"位姿"中位置与姿态必须配套的直观体现。

六、位姿与坐标变换的实际应用

6.1 底盘位姿估计（odometry）

轮式机器人通过轮速积分得到里程计位姿：每个控制周期更新一次 (x, y, yaw)。你会发现它的发布代码和示例 3 几乎一样——所有里程计话题 /odom 的本质就是"不断发布的 PoseStamped"（外加速度）。

6.2 传感器坐标标定（extrinsic calibration）

激光雷达/相机装到机器人上时，需要标定"传感器坐标系相对机器人基座的变换"。标定结果就是一个 TransformStamped（4×4 矩阵）。有了它，雷达扫到的每个点都能变换到机器人坐标系，再变换到地图坐标系——这就是建图的第一步。

6.3 导航与规划中的位姿

• AMCL 定位：输出 PoseWithCovarianceStamped——不仅给位姿，还给"多不自信"（协方差）。
• 路径规划：起点和终点都是位姿；规划出的路径是一串位姿点。
• 机械臂（第 35-40 篇）：逆运动学求解的"目标"就是一个位姿。

一句话：位姿是机器人的"身份证"，坐标变换是机器人的"翻译官"。两者是所有上层模块的公共地基。

七、常见错误与排障

八、小结与下一篇预告

本篇核心：

下一篇进入 tf2 坐标变换详解（C++/Python）：在 ROS2 中如何广播（broadcast）与监听（lookup）坐标变换、tf2_ros 的 Buffer 与 TransformListener 用法、如何发布静态变换、以及"变换不可用"的常见排障。位姿是数学，tf2 是把数学变成机器可用的管道。