📚 课程目录第3章坐标变换基础

3. 坐标变换基础：坐标系、平移、旋转、欧拉角与四元数

3.1 核心原理：从“世界描述”到“自身描述”

在ROS2中，TF（Transform）坐标变换的本质是回答一个基本问题：机器人身上的某个点或某个坐标系，在另一个坐标系下应该位于何处？ 这要求我们首先建立严格的坐标系概念。一个坐标系由原点位置和三个正交轴（X、Y、Z）的方向唯一确定。在机器人学中，我们通常使用右手定则定义坐标系。

坐标变换包含两种基本操作：平移（Translation）和旋转（Rotation）。平移描述了两个坐标系原点之间的位移矢量；旋转则描述了两个坐标系各轴之间的指向差异。任何复杂的坐标变换，都可以分解为一次平移和一次旋转的组合。在ROS2中，这两部分被封装在 geometry_msgs/msg/TransformStamped 消息中，其中 translation 字段表示平移矢量，rotation 字段表示旋转（以四元数形式存储）。

关键理解：TF树中的每一个“边”都代表一个父坐标系到子坐标系的变换关系。这个变换关系是静态的（如机器人底盘到激光雷达）或动态的（如底盘到移动的机械臂末端）。变换关系一旦确定，任意点在不同坐标系下的坐标就可以通过矩阵运算互相转换。

下图展示了机器人中常见的坐标系关系：map（地图）、odom（里程计）、base_link（机器人基座）以及传感器坐标系 laser（激光雷达）。它们之间通过平移和旋转连接，构成一个有向无环图（DAG），即TF树。

3.2 方法流程：欧拉角与四元数的转换

旋转的表达方式主要有三种：旋转矩阵、欧拉角和四元数。ROS2中TF消息内部使用四元数，但人机交互时欧拉角更直观。理解两者的转换是使用TF的必备技能。

欧拉角按照旋转顺序（如ZYX）分解为绕各轴的旋转角度。但欧拉角存在万向锁问题，且旋转顺序不同会导致结果不同。因此ROS2中推荐使用四元数进行运算和存储。

四元数由一个实部（w）和三个虚部（x, y, z）组成，表示绕某个单位轴旋转一定角度。其数学形式为：

q = w + xi + yj + zk
其中 w = cos(θ/2)
     x = ax * sin(θ/2)
     y = ay * sin(θ/2)
     z = az * sin(θ/2)
(ax, ay, az) 为单位旋转轴，θ 为旋转角度

在ROS2中，可以使用 tf2_geometry_msgs 和 tf2 库进行转换。以下是C++和Python中常用的转换方法：

实操要点：在编写代码时，务必明确欧拉角的旋转顺序。ROS2中默认使用 ZYX 顺序（即先绕Z轴偏航yaw，再绕Y轴俯仰pitch，最后绕X轴横滚roll）。使用 tf2::Quaternion::setRPY() 时，参数顺序为 (roll, pitch, yaw)。

// C++ 示例：欧拉角转四元数
#include <tf2/LinearMath/Quaternion.h>
tf2::Quaternion q;
q.setRPY(roll, pitch, yaw);  // 注意顺序：roll(X), pitch(Y), yaw(Z)
q.normalize();

// C++ 示例：四元数转欧拉角
tf2::Matrix3x3 m(q);
double roll, pitch, yaw;
m.getRPY(roll, pitch, yaw);

# Python 示例：欧拉角转四元数
from tf_transformations import quaternion_from_euler
q = quaternion_from_euler(roll, pitch, yaw)  # 返回 [x, y, z, w]

# Python 示例：四元数转欧拉角
from tf_transformations import euler_from_quaternion
roll, pitch, yaw = euler_from_quaternion([x, y, z, w])

3.3 案例/场景：机械臂末端位姿计算

假设一个移动机器人搭载了6自由度机械臂。我们需要计算机械臂末端（tool0）在map坐标系下的位置。这需要经过多级TF变换：

map → odom：通过AMCL定位模块发布，提供全局定位修正。
odom → base_link：由轮式里程计发布，高频更新。
base_link → arm_base：机械臂安装座相对机器人本体的固定变换（URDF中定义）。
arm_base → tool0：由机械臂控制器发布，随关节角度变化。

使用 tf2_ros::Buffer 的 lookupTransform() 接口，可以一次性获取从 tool0 到 map 的完整变换：

// C++ 示例：查询变换
geometry_msgs::msg::TransformStamped transform;
try {
    transform = tf_buffer->lookupTransform(
        "map",        // 目标坐标系
        "tool0",      // 源坐标系
        rclcpp::Time(0)  // 最近可用时间
    );
} catch (tf2::TransformException &ex) {
    RCLCPP_ERROR(rclcpp::get_logger("example"), "变换查询失败: %s", ex.what());
}

查询得到的 transform.transform.translation 即为末端在map系下的位置，transform.transform.rotation 为姿态四元数。若需要欧拉角，可进一步转换。

坐标系对	发布者	频率	类型
map → odom	AMCL节点	低频（10Hz）	动态
odom → base_link	轮式里程计节点	高频（50-100Hz）	动态
base_link → arm_base	URDF + robot_state_publisher	静态	固定
arm_base → tool0	机械臂驱动节点	关节状态频率	动态

3.4 误区/复盘：常见错误与规避

常见误区一：混淆旋转顺序。 在调用 setRPY() 或 quaternion_from_euler() 时，参数顺序是 (roll, pitch, yaw)，即绕X、Y、Z轴的顺序。如果误传为 (yaw, pitch, roll)，会导致姿态完全错误。建议在代码注释中明确标注。

常见误区二：忘记归一化四元数。 四元数必须满足 x² + y² + z² + w² = 1。在手动构造或连续乘法后，数值误差可能导致模长偏离1，从而引起旋转失真。每次构造后应调用 normalize() 或 normalized()。

常见误区三：忽略时间戳同步。 TF查询时使用 rclcpp::Time(0) 表示获取最近数据，但在高动态场景下，不同传感器的时间戳可能不一致。应使用 waitForTransform() 等待有效变换，或使用消息过滤器 tf2_ros::MessageFilter 进行时间同步。

复盘建议：在调试TF问题时，使用 tf2_echo map tool0 命令行工具实时查看变换结果。若输出显示“Lookup would require extrapolation into the past”，说明时间戳不同步；若显示“Could not find transform”，则检查TF树是否完整发布。养成先运行 rqt_tf_tree 查看TF树拓扑的习惯，能快速定位缺失的坐标系。

最后，务必记住：四元数是ROS2中旋转的“通用语言”，欧拉角仅用于人机交互和调试。在编写发布TF的代码时，始终使用四元数进行存储和传输；在需要可视化或人工理解时，再转换为欧拉角。掌握这一原则，可以避免绝大多数坐标变换相关的隐蔽错误。