2. 坐标系与变换：全局坐标系、局部坐标系、齐次变换矩阵、坐标转换实战

做目标点跟踪，说白了就是回答一个问题：“目标到底在哪儿？”

但这个问题没那么简单。你站在地面上看目标，和机器人自己“眼睛”里看到的目标，位置是完全不同的。我刚开始做这个项目时，就吃过这个亏——明明传感器数据都对，但机器人就是抓不到目标。后来才发现，是坐标系搞混了。

嗯，这节课我们就来彻底理清坐标系这件事。

2.1 全局坐标系 vs 局部坐标系

先讲个最直观的例子。你站在操场上，问“国旗在哪儿？”你会说“在我正前方20米”。这个“我”，就是你的局部坐标系。但如果用GPS定位，国旗的坐标是(116.4°E, 39.9°N)，这就是全局坐标系。

在机器人系统里，全局坐标系通常固定在地面或世界原点，我们叫它世界坐标系。局部坐标系则跟着机器人、传感器或目标物体一起动。

举个例子，你车上的激光雷达检测到前方5米有个障碍物。这个“前方5米”是雷达自己的局部坐标系。但你要知道这个障碍物在地图上的绝对位置，就得把雷达坐标转换到车体坐标，再转换到世界坐标。

2.2 齐次变换矩阵

坐标系之间的转换，靠的就是齐次变换矩阵。它是个4x4的矩阵，同时包含了旋转和平移信息。

为什么用4x4？因为3x3的旋转矩阵只能处理旋转，平移得单独加。齐次变换矩阵把旋转和平移塞进一个矩阵里，用起来特别方便。

它的标准形式长这样：

T = [ R   t ]
    [ 0   1 ]

其中：

• R：3x3旋转矩阵
• t：3x1平移向量
• 0：1x3零向量
• 1：标量1

用齐次坐标表示一个点，就是在三维坐标(x,y,z)后面加个1，变成(x,y,z,1)。这样，矩阵乘法就能同时完成旋转和平移。

2.3 坐标转换实战

光说不练假把式。我们来看一个实际例子。

假设有个机器人，它的激光雷达检测到目标点在雷达坐标系下的位置是(2, 3, 0)。雷达安装在机器人前方1米、高度0.5米处，且雷达坐标系相对于机器人坐标系绕Z轴旋转了30度。

我们要把目标点从雷达坐标系转换到机器人坐标系。

步骤：

1. 构建雷达到机器人的齐次变换矩阵
2. 将目标点表示为齐次坐标
3. 做矩阵乘法

用Python实现：

import numpy as np
import math

# 目标点在雷达坐标系下的位置
p_radar = np.array([2, 3, 0, 1])

# 旋转角度（绕Z轴）
theta = math.radians(30)

# 构建旋转矩阵
R = np.array([
    [math.cos(theta), -math.sin(theta), 0],
    [math.sin(theta),  math.cos(theta), 0],
    [0,                0,               1]
])

# 平移向量（雷达在机器人坐标系中的位置）
t = np.array([1, 0, 0.5])

# 构建齐次变换矩阵
T_radar_to_robot = np.eye(4)
T_radar_to_robot[:3, :3] = R
T_radar_to_robot[:3, 3] = t

# 坐标转换
p_robot = T_radar_to_robot @ p_radar

print(f"目标点在机器人坐标系下的位置：{p_robot[:3]}")
# 输出：[2.732, 2.598, 0.5]

你看，就这么简单。但实际项目中，往往有多个坐标系串联：雷达→车体→世界。这时候就需要把多个变换矩阵乘在一起。

2.4 知识体系总览

下面这张图，帮你把本章的核心逻辑串起来：

这张图想表达的是：全局坐标系和局部坐标系之间，通过齐次变换矩阵建立联系。而实战中，我们就是靠矩阵乘法，把目标点在不同坐标系之间来回转换。

2.5 小结

这一章我们聊了：

• 全局坐标系和局部坐标系的区别
• 齐次变换矩阵的结构和用法
• 一个完整的坐标转换Python例子

说白了，坐标系转换就是“换个角度看问题”。你站在不同坐标系下，看到的目标位置不一样，但通过变换矩阵，你能把这些视角统一起来。

我建议你课后自己动手写几个例子，比如把目标点从世界坐标转到相机坐标。多练几次，你就能找到感觉。