第二章：坐标系与坐标变换——机器人眼中的世界

大家好，欢迎来到第二章。

说实话，坐标系和坐标变换，是很多初学者觉得「枯燥」的部分。但我得说，这恰恰是整个导航系统里最核心的基石。你想想看，机器人要导航，首先得知道「我在哪」、「目标在哪」、「障碍物在哪」。这三个问题，本质上就是坐标系之间的转换问题。

我在做第一个机器人项目时，就吃过坐标变换的亏。当时小车明明避开了障碍物，但传感器数据就是不对。查了两天，最后发现是激光雷达的安装角度没补偿——说白了，就是坐标系没对齐。从那以后，我再也不敢小看这部分内容了。

2.1 机器人常用坐标系

在机器人世界里，我们通常用三个坐标系来描述一切。嗯，其实不止三个，但最核心的就这三个。

2.1.1 世界坐标系（World Frame）

世界坐标系是绝对参考系。你可以把它想象成地图上的经纬度。一旦定义好，它就固定不动了。所有全局规划、定位，都是基于这个坐标系。

我个人习惯用 map 或 world 作为世界坐标系的名字。在ROS里，map 帧通常是固定的，原点一般设在机器人启动位置或地图的某个角落。

2.1.2 机器人坐标系（Base Frame）

机器人坐标系是跟着机器人走的。通常叫 base_link 或 base_footprint。原点一般在机器人中心在地面的投影点。

为什么要单独搞一个机器人坐标系？因为控制指令、速度命令都是相对于机器人自身的。比如「向前走0.5米」，这个「前」就是机器人坐标系下的前。

2.1.3 传感器坐标系（Sensor Frame）

每个传感器都有自己的坐标系。激光雷达、摄像头、IMU……它们安装的位置和朝向都不一样。

举个例子：激光雷达装在机器人前方，它的坐标系原点就在雷达中心。如果雷达测到前方1米有障碍物，这个「前方1米」是相对于雷达自己的坐标系说的。但机器人需要知道这个障碍物相对于自己（base_link）的位置，这就需要进行坐标变换。

核心要点： 所有传感器数据最终都要转换到机器人坐标系或世界坐标系下，才能被统一使用。

2.2 欧拉角与四元数——旋转的两种表达

说到坐标变换，就绕不开旋转的表达。我见过太多人在欧拉角和四元数上翻车了。我自己也翻过，嗯，还不止一次。

2.2.1 欧拉角

欧拉角用三个角度来描述旋转：绕X轴转（roll，翻滚角）、绕Y轴转（pitch，俯仰角）、绕Z轴转（yaw，偏航角）。

听起来很直观对吧？但问题来了——万向锁。

我曾经在调试一个机械臂时，遇到了万向锁。当时机械臂突然「卡住」了，怎么动都不对。查了半天，发现是欧拉角在某个姿态下丢失了一个自由度。说白了，就是两个旋转轴重合了，导致无法唯一确定姿态。

避坑指南： 我曾经在项目中直接用欧拉角做插值，结果机器人姿态出现了奇怪的抖动。后来改用四元数球面插值（Slerp），问题立刻解决。所以，如果你要做平滑旋转，请用四元数。

2.2.2 四元数

四元数用四个数来表示旋转：一个实部 + 三个虚部。写成 q = w + xi + yj + zk，其中 w 是实部，x, y, z 是虚部。

你可能会问：「为什么非要用四个数？三个数不够吗？」

嗯，三个数（欧拉角）有万向锁问题，而且插值不连续。四元数没有这些问题。虽然它不那么直观，但计算稳定、插值平滑。在ROS里，所有旋转都用四元数表示。

下面是一个Python示例，展示如何在欧拉角和四元数之间转换：

import numpy as np
from scipy.spatial.transform import Rotation as R

# 欧拉角转四元数（ZYX顺序，即yaw-pitch-roll）
euler = [0.3, 0.1, 0.5]  # [roll, pitch, yaw] 弧度
r = R.from_euler('zyx', [euler[2], euler[1], euler[0]], degrees=False)
quat = r.as_quat()  # 返回 [x, y, z, w]
print(f"四元数: x={quat[0]:.3f}, y={quat[1]:.3f}, z={quat[2]:.3f}, w={quat[3]:.3f}")

# 四元数转欧拉角
r2 = R.from_quat(quat)
euler2 = r2.as_euler('zyx', degrees=False)
print(f"欧拉角: roll={euler2[2]:.3f}, pitch={euler2[1]:.3f}, yaw={euler2[0]:.3f}")
小技巧： 在ROS中，可以用 tf.transformations.euler_from_quaternion() 和 tf.transformations.quaternion_from_euler() 直接转换，不需要自己写数学公式。

2.3 TF变换基础

TF（Transform）是ROS里管理坐标变换的框架。说白了，它维护了一棵树，记录了所有坐标系之间的相对位置和姿态。

2.3.1 TF树的结构

TF树是一个单向无环图。根节点通常是 map 或 odom，然后分支出 base_link，再分支出各个传感器坐标系。

下面我用SVG画一张TF树的结构图，帮你直观理解：

TF树结构示意图 map odom base_link laser_frame camera_frame imu_frame 箭头方向表示父→子关系，变换从子到父

2.3.2 发布和监听TF

在ROS里，有两种方式使用TF：发布变换和监听变换。

发布变换：告诉系统「坐标系A相对于坐标系B的位置和姿态」。比如，激光雷达相对于机器人底盘的变换。

import rospy
import tf2_ros
import geometry_msgs.msg

def publish_tf():
    br = tf2_ros.TransformBroadcaster()
    t = geometry_msgs.msg.TransformStamped()
    
    t.header.stamp = rospy.Time.now()
    t.header.frame_id = "base_link"
    t.child_frame_id = "laser_frame"
    
    # 激光雷达安装在机器人前方0.2米，高度0.1米
    t.transform.translation.x = 0.2
    t.transform.translation.y = 0.0
    t.transform.translation.z = 0.1
    
    # 无旋转
    t.transform.rotation.x = 0.0
    t.transform.rotation.y = 0.0
    t.transform.rotation.z = 0.0
    t.transform.rotation.w = 1.0
    
    br.sendTransform(t)

监听变换：查询任意两个坐标系之间的变换。比如，想知道激光雷达检测到的点在世界坐标系下的位置。

import rospy
import tf2_ros

def lookup_tf():
    tf_buffer = tf2_ros.Buffer()
    listener = tf2_ros.TransformListener(tf_buffer)
    
    rospy.sleep(0.5)  # 等待TF树建立
    
    try:
        # 查询 laser_frame 到 map 的变换
        trans = tf_buffer.lookup_transform("map", "laser_frame", rospy.Time(0))
        print(f"激光雷达相对于map的位置: ({trans.transform.translation.x:.2f}, "
              f"{trans.transform.translation.y:.2f}, {trans.transform.translation.z:.2f})")
    except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
            tf2_ros.ExtrapolationException) as e:
        rospy.logerr(f"TF查询失败: {e}")
重要提醒： lookup_transform() 的第一个参数是目标坐标系，第二个参数是源坐标系。它返回的是「从源坐标系到目标坐标系的变换」。我刚开始用的时候经常搞反，结果数据全错了。

2.4 坐标变换的数学本质

说白了，坐标变换就是矩阵乘法。一个点从坐标系A变换到坐标系B，就是乘以一个4x4的齐次变换矩阵。

齐次变换矩阵长这样：

| R   t |
| 0   1 |

其中 R 是3x3的旋转矩阵，t 是3x1的平移向量。

举个例子，把激光雷达检测到的点转换到机器人坐标系：

import numpy as np

# 假设激光雷达检测到点 (1.0, 0.0, 0.0) 在 laser_frame 下
point_laser = np.array([1.0, 0.0, 0.0, 1.0])  # 齐次坐标

# 激光雷达到base_link的变换矩阵（平移(0.2, 0, 0.1)，无旋转）
T_laser_to_base = np.array([
    [1, 0, 0, 0.2],
    [0, 1, 0, 0.0],
    [0, 0, 1, 0.1],
    [0, 0, 0, 1.0]
])

# 变换到base_link坐标系
point_base = T_laser_to_base @ point_laser
print(f"点在base_link下的坐标: ({point_base[0]:.2f}, {point_base[1]:.2f}, {point_base[2]:.2f})")
# 输出: (1.20, 0.00, 0.10)
我的习惯： 在写代码时，我通常用 tf2_geometry_msgs 包里的 do_transform_point() 函数，它直接接受 PointStamped 和 TransformStamped，省去手动矩阵运算的麻烦。

2.5 常见坑与避坑指南

  • 坐标系命名不一致：我曾经在一个项目里，有人用 base_link，有人用 base_footprint，结果两个坐标系差了0.05米的高度，导致导航时机器人一直「撞墙」。建议团队统一命名规范。
  • 时间戳不同步：TF变换是有时间戳的。如果你查询变换时用了错误的时间戳，可能会得到过时的数据。我习惯用 rospy.Time(0) 获取最新可用的变换。
  • 忘记发布静态变换：传感器相对于机器人的位置通常是固定的。用 static_transform_publisher 发布一次就好，不需要每帧都发。
避坑指南： 我曾经在调试时发现TF树里出现了循环依赖——两个节点互相指向对方。这会导致TF系统崩溃。记住：TF树必须是单向的，不能有环。

好了，这一章的内容就到这里。坐标系和坐标变换是机器人导航的「语言」，掌握了它，你才能读懂机器人的世界。下一章我们会深入传感器数据处理，到时候这些变换知识会派上大用场。

专注资料整理
← 上一章 📖 返回目录 下一章 →