坐标系与坐标变换：全局坐标系、局部坐标系、齐次坐标与变换矩阵

做机器人路径规划，说白了就是回答三个问题：我在哪？我要去哪？怎么去？ 而这三个问题的核心，都绕不开坐标系与坐标变换。

我记得刚入行那会儿，带我的老工程师跟我说过一句话：「搞不懂坐标系变换，你写的路径规划代码就是一堆废纸。」当时我不信，直到有一次在项目里，因为一个旋转矩阵的方向搞反了，机器人直接撞上了货架……嗯，从那以后，我再也不敢轻视这个基础了。

今天我们就来聊聊坐标系这件事。我会尽量用我自己的经验，帮你把这些概念串起来。

1. 全局坐标系 vs 局部坐标系

先问个问题：你站在房间里，说「杯子在我右边30厘米处」。这个「右边」是相对于谁的？

如果你面向北，右边是东。如果你面向南，右边是西。同一个杯子，位置描述完全不一样。这就是坐标系的意义——它定义了「参考基准」。

• 全局坐标系（World Frame）：固定不变的参考系。比如地图上的经纬度，或者工厂里贴在墙上的那个原点标记。所有物体的绝对位置，最终都要映射到全局坐标系下。
• 局部坐标系（Local Frame）：附着在某个运动物体上的坐标系。比如机器人身上的激光雷达、摄像头、机械臂末端，都有自己的局部坐标系。

我个人习惯把全局坐标系想象成「上帝视角」，局部坐标系想象成「第一人称视角」。路径规划时，传感器数据都是在局部坐标系下采集的，但规划路径必须在全局坐标系下进行。所以，坐标变换就是连接这两个视角的桥梁。

2. 齐次坐标：为什么需要它？

你可能会问：平移和旋转，不就是加法和乘法吗？为什么还要搞个「齐次坐标」出来？

我刚开始学的时候也有这个疑问。直到我写代码时发现：平移是加法，旋转是乘法，两者没法统一成一个矩阵运算。每次变换都要先旋转再平移，写起来麻烦，还容易出错。

齐次坐标的妙处：它给二维坐标加了一个维度（变成3维），给三维坐标加了一个维度（变成4维）。这样一来，平移和旋转就可以合并成一个矩阵乘法了。

举个例子，二维空间中的点 (x, y)，齐次坐标表示为 (x, y, 1)。

# 二维齐次坐标示例
# 普通坐标 (2, 3) → 齐次坐标 (2, 3, 1)
point_2d = [2, 3]
point_homogeneous = [2, 3, 1]  # 加了个1

为什么加个1？因为这样平移矩阵就能写成乘法形式：

# 平移矩阵（二维齐次形式）
# 想将点 (x, y) 平移 (dx, dy)
# 变换矩阵 T 为：
# [1, 0, dx]
# [0, 1, dy]
# [0, 0, 1 ]

# 变换结果 = T * [x, y, 1]^T

你看，平移变成了矩阵乘法。旋转矩阵也是乘法。两者一结合，一个矩阵就能搞定旋转+平移。这就是齐次坐标存在的意义。

3. 变换矩阵：旋转 + 平移 = 一个矩阵

变换矩阵 T 的结构其实很简单。以三维空间为例：

# 三维变换矩阵 T (4x4)
# T = [ R    t ]
#     [ 0^T  1 ]
#
# 其中：
# R 是 3x3 旋转矩阵
# t 是 3x1 平移向量
# 0^T 是 1x3 零向量
# 右下角是 1

# 示例：绕Z轴旋转45度，再平移 (1, 2, 3)
import numpy as np

theta = np.pi / 4  # 45度
R = np.array([
    [np.cos(theta), -np.sin(theta), 0],
    [np.sin(theta),  np.cos(theta), 0],
    [0,              0,             1]
])
t = np.array([1, 2, 3])

T = np.eye(4)
T[:3, :3] = R
T[:3, 3] = t

print("变换矩阵 T:")
print(T)

输出结果：

变换矩阵 T:
[[ 0.7071 -0.7071  0.      1.    ]
 [ 0.7071  0.7071  0.      2.    ]
 [ 0.      0.      1.      3.    ]
 [ 0.      0.      0.      1.    ]]

有了这个 T 矩阵，你就可以把局部坐标系下的任意点 p_local 变换到全局坐标系：

p_global = T @ p_local  # 矩阵乘法

4. 知识体系结构图

下面这张图，是我自己梳理的坐标系与变换的知识脉络。你可以把它当作一个「思维导图」来用：

5. 实战中的链式变换

实际项目中，很少只有一个变换。比如一个移动机器人，它的传感器链路可能是这样的：

1. 激光雷达扫描到障碍物，得到的是 激光雷达坐标系 下的点。
2. 激光雷达装在机器人上，需要变换到 机器人基座坐标系。
3. 机器人在地图上移动，需要变换到 全局地图坐标系。

这就是链式变换：

# 链式变换示例
# T_map_base: 机器人基座 → 全局地图
# T_base_lidar: 激光雷达 → 机器人基座
# p_lidar: 激光雷达检测到的点

# 最终全局坐标
p_map = T_map_base @ T_base_lidar @ p_lidar

# 或者合并成一个变换矩阵
T_map_lidar = T_map_base @ T_base_lidar
p_map = T_map_lidar @ p_lidar

我个人习惯把这种链式变换写成「从右往左读」的方式：先经过激光雷达变换，再经过基座变换，最后到地图。这样逻辑清晰，不容易搞混。

6. 总结一下

坐标系与坐标变换，说白了就是一套「翻译规则」。把局部感知翻译成全局理解，把传感器数据翻译成规划指令。

• 全局坐标系：固定的参考基准，用于路径规划和地图构建。
• 局部坐标系：随机器人运动，用于传感器数据采集。
• 齐次坐标：用多一个维度，把平移和旋转统一成矩阵乘法。
• 变换矩阵：旋转+平移的打包工具，支持链式运算。

嗯，这些概念看起来简单，但真正写代码时，每一个细节都可能让你栽跟头。我建议你动手写几个小例子，比如把一个点从激光雷达坐标系变换到全局地图，跑通了，才算真正理解。

---