4、机器人运动学基础：刚体运动描述、旋转矩阵与四元数、坐标变换（TF2）原理

各位同学，欢迎来到运动学基础这一章。

说实话，这部分内容看起来有点数学，但它是整个机器人控制的基石。你想想看，机器人要抓个杯子，首先得知道杯子在哪儿，自己的手在哪儿，对吧？这背后就是刚体运动描述和坐标变换。

我个人习惯把这一章叫做“机器人的世界观”。搞懂了它，你写出来的代码才靠谱。

4.1 刚体运动描述：位置与姿态

一个刚体在三维空间里，有6个自由度。3个是位置（x, y, z），3个是姿态（绕x, y, z轴的旋转）。

位置很好理解，就是坐标。姿态呢？说白了就是“它朝哪边看”。

描述姿态，我们常用的有三种方式：旋转矩阵、欧拉角、四元数。我一个个说。

4.1.1 旋转矩阵

旋转矩阵是一个3x3的矩阵。它能把一个向量从一个坐标系，转到另一个坐标系。

比如，我有一个点P，在坐标系A里的坐标是P_A。我想知道它在坐标系B里的坐标P_B。如果我知道坐标系B相对于A的旋转矩阵R_BA，那直接乘就行：

P_B = R_BA * P_A

旋转矩阵有个特点：它是正交矩阵，行列式为1。这意味着它不会拉伸或压缩向量，只会旋转。

核心要点：

旋转矩阵有9个元素，但只有3个自由度（冗余）。
它没有万向锁问题，但计算量大。
组合多个旋转时，矩阵乘法顺序很重要。

我在项目中遇到过一个问题：用旋转矩阵做连续旋转，结果数值误差越来越大，矩阵慢慢变得不正交了。嗯，这里要注意，长时间运行后，最好做一次正交化处理。

4.1.2 欧拉角

欧拉角用三个角度来描述旋转：绕x轴转（roll）、绕y轴转（pitch）、绕z轴转（yaw）。

直观，好理解。但有个致命问题——万向锁。

避坑指南：

我曾经在做一个机械臂的逆解时，用了欧拉角。结果在pitch接近90度时，roll和yaw突然变得无法区分，机械臂直接乱跳。这就是万向锁。从那以后，我在内部计算中再也不敢用欧拉角了。

ROS2里，欧拉角主要用于人机交互的输入输出。内部计算，我建议用四元数。

4.1.3 四元数

四元数是个很神奇的东西。它用四个数（w, x, y, z）来表示旋转。

为什么用四个数？因为它避免了万向锁，而且插值平滑。

四元数的基本形式：

q = w + x*i + y*j + z*k

其中，i² = j² = k² = ijk = -1。

单位四元数表示旋转。w是实部，代表旋转角的一半的余弦。x, y, z是虚部，代表旋转轴的方向。

我的小技巧：

ROS2里，四元数用 geometry_msgs::msg::Quaternion 表示。你不需要手算四元数，直接用TF2的API转换就行。但你要理解它的含义，不然调试时一脸懵。

四元数乘法：

q1 * q2 表示先做q2旋转，再做q1旋转

注意顺序！和矩阵乘法一样，不满足交换律。

4.2 坐标变换（TF2）原理

好了，数学基础有了。现在说说ROS2里怎么用——TF2。

TF2是ROS2的坐标变换库。它维护了一棵“坐标变换树”。

举个例子：

世界坐标系：map
机器人基座：base_link
激光雷达：laser
摄像头：camera

这些坐标系之间，通过TF2发布变换关系。你只需要知道两个坐标系的名字，就能随时获取它们之间的变换。

// 获取 laser 到 base_link 的变换
geometry_msgs::msg::TransformStamped t;
t = tf_buffer->lookupTransform("base_link", "laser", rclcpp::Time(0));

rclcpp::Time(0) 表示获取最新的一帧变换。

TF2的核心概念：

广播器（Broadcaster）：发布坐标系之间的变换关系。
监听器（Listener）：接收并缓存所有变换关系。
变换树（Transform Tree）：所有坐标系构成一棵树，根节点通常是map或odom。
时间戳：每个变换都有时间戳，支持历史查询。

我个人习惯，在机器人启动时，就把所有固定坐标系（比如base_link到laser）的变换发布好。动态的（比如机械臂末端）则实时发布。

4.2.1 TF2的工作流程

下面这张图，是我自己总结的TF2工作流程。你看一遍就明白了。

流程很简单：

传感器节点（比如激光雷达驱动）计算出laser到base_link的变换。
它通过TF2广播器发布这个变换。
TF2监听器收到后，存入缓存（Buffer）。
用户节点（比如导航模块）从缓存中查询需要的变换。

你看，整个过程是解耦的。每个节点只关心自己知道的变换，不用管全局。

4.2.2 静态变换与动态变换

类型	特点	示例	发布方式
静态变换	固定不变	base_link → laser	static_transform_publisher
动态变换	随时间变化	odom → base_link	TransformBroadcaster

静态变换，我建议用launch文件里的static_transform_publisher。省事，而且不会因为节点重启而丢失。

# 在launch文件中声明静态变换
static_transform_publisher x y z yaw pitch roll frame_id child_frame_id

动态变换，比如里程计到基座的变换，就得在代码里实时发布了。

我的经验：

TF2的缓存默认保留10秒的历史数据。如果你需要查询历史变换，记得设置合适的缓存时间。我曾经调试一个延时问题，查了半天才发现是缓存时间太短，历史数据被清掉了。

4.3 旋转矩阵与四元数的互转

在实际开发中，你经常需要在旋转矩阵和四元数之间转换。ROS2的tf2提供了现成的函数。

#include <tf2/LinearMath/Quaternion.h>
#include <tf2/LinearMath/Matrix3x3.h>

// 四元数转旋转矩阵
tf2::Quaternion q(0.707, 0.0, 0.0, 0.707);
tf2::Matrix3x3 m(q);

// 旋转矩阵转四元数
tf2::Matrix3x3 m2;
m2.setRPY(1.57, 0.0, 0.0);  // roll=90度
tf2::Quaternion q2;
m2.getRotation(q2);

我个人习惯，在代码内部统一用四元数。只在需要显示给用户看时，才转成欧拉角。

注意：

四元数有“双倍覆盖”特性。q和-q表示同一个旋转。这在插值时要注意，不然可能走远路。

好了，这一章的内容就这些。运动学基础是后面所有控制算法的基础。你花时间搞懂它，后面写代码会顺畅很多。