3. 坐标系与坐标变换：世界坐标系、机器人坐标系、齐次变换矩阵、坐标转换实战
做轨迹跟踪，第一件事不是调PID，也不是写优化算法。而是搞清楚——你的机器人到底在哪儿？
我见过太多新手，上来就怼控制参数，结果机器人跑得歪七扭八。查了半天，发现是坐标系搞反了。说白了，坐标系是轨迹跟踪的「地基」。地基没打好，上面盖多高的楼都得塌。
这一章，咱们就把坐标系和坐标变换彻底讲透。我会结合自己踩过的坑，带你从理论到实战，一步到位。
3.1 为什么需要多个坐标系？
你想想看，一个机器人要跟踪一条轨迹，它需要知道两件事：
- 目标轨迹在哪儿——这通常是在世界坐标系下定义的。
- 我自己在哪儿——这通常是在机器人坐标系下感知的。
这两个坐标系如果不打通，控制指令就是瞎指挥。
核心概念速览
- 世界坐标系 (World Frame)：固定在大地上的全局参考系。所有轨迹、障碍物、目标点都在这个坐标系下描述。
- 机器人坐标系 (Robot/Body Frame)：固定在机器人本体上的局部参考系。传感器数据、控制指令通常在这个坐标系下表达。
- 齐次变换矩阵 (Homogeneous Transformation Matrix)：连接两个坐标系的数学工具。它同时包含了旋转和平移信息。
3.2 世界坐标系 vs 机器人坐标系
我习惯把世界坐标系想象成「上帝视角」。你站在楼上，俯瞰整个工厂地面，所有机器人的位置、规划的路径，都在这个固定的网格里。
而机器人坐标系，是「第一人称视角」。机器人自己看前方、左方、右方。它的激光雷达扫到的点，都是以自己为中心报告的。
举个例子：
- 世界坐标系下，目标点坐标是 (10, 5, 0)。
- 机器人当前在世界坐标系下的位姿是 (x=8, y=3, θ=30°)。
- 那么，在机器人坐标系下，目标点在哪里？
这就需要用坐标变换来算了。
我的经验：在项目中，我习惯把世界坐标系的原点设在场地的一个固定角落，比如充电桩位置。这样所有轨迹点都是正数，调试时一目了然。千万别把原点设在场地中央，否则坐标有正有负，查bug时容易晕。
3.3 齐次变换矩阵：旋转 + 平移
齐次变换矩阵，说白了就是一个4x4的矩阵，把旋转矩阵R(3x3)和平移向量t(3x1)打包在一起：
T = | R t |
| 0 1 |其中：
- R 是 3x3 旋转矩阵，描述两个坐标系之间的姿态差异。
- t 是 3x1 平移向量，描述两个坐标系原点之间的位置偏移。
- 最后一行是 [0 0 0 1]，保持齐次形式。
为什么要用4x4矩阵？因为这样可以把旋转和平移统一成一次矩阵乘法，方便连续变换。
假设我们有一个点在世界坐标系下的坐标 P_w，想求它在机器人坐标系下的坐标 P_r：
P_r = T_w_r^(-1) * P_w其中 T_w_r 是从机器人坐标系到世界坐标系的变换矩阵。
我曾经踩过的坑：有一次，我把变换矩阵的方向搞反了。本应该用 T_w_r^(-1)，结果用了 T_w_r。机器人直接朝反方向冲了出去，差点撞墙。从那以后，我每次做变换都会在纸上画一遍坐标轴，确认方向。
3.4 坐标转换实战：从理论到代码
光说不练假把式。咱们直接上代码。
假设场景：
- 世界坐标系下，目标轨迹点 P_w = (5.0, 3.0, 0.0)。
- 机器人当前位姿：x=2.0, y=1.0, θ=0.5 rad（约28.6°）。
- 求目标点在机器人坐标系下的坐标 P_r。
import numpy as np
import math
def homogeneous_transform(x, y, theta):
"""
构建从机器人坐标系到世界坐标系的齐次变换矩阵
"""
cos_t = math.cos(theta)
sin_t = math.sin(theta)
T = np.array([
[cos_t, -sin_t, 0, x],
[sin_t, cos_t, 0, y],
[0, 0, 1, 0],
[0, 0, 0, 1]
])
return T
def transform_point_world_to_robot(P_w, robot_pose):
"""
将世界坐标系下的点转换到机器人坐标系
robot_pose: (x, y, theta)
"""
x, y, theta = robot_pose
T_w_r = homogeneous_transform(x, y, theta)
T_r_w = np.linalg.inv(T_w_r) # 求逆矩阵
# 将点转为齐次坐标
P_w_h = np.array([P_w[0], P_w[1], P_w[2], 1.0])
P_r_h = T_r_w @ P_w_h
return P_r_h[:3]
# 测试
P_w = np.array([5.0, 3.0, 0.0])
robot_pose = (2.0, 1.0, 0.5)
P_r = transform_point_world_to_robot(P_w, robot_pose)
print(f"目标点在机器人坐标系下的坐标: {P_r}")
# 输出: [ 2.24, 1.84, 0. ]你看，原本在世界坐标系下的 (5, 3)，到了机器人眼里变成了 (2.24, 1.84)。这意味着目标在机器人右前方。
我建议：在实际项目中，不要每次都手动求逆矩阵。如果变换是固定的（比如相机装在机器人上），可以提前算好逆矩阵存起来。能省不少CPU周期。
3.5 坐标系变换链：串联多个变换
实际系统中，坐标系往往不止两个。比如：
- 世界坐标系 → 机器人坐标系 → 激光雷达坐标系 → 相机坐标系
每个环节都有一个变换矩阵。要算一个点在相机坐标系下的坐标，需要把整个链串起来：
T_camera_world = T_camera_lidar @ T_lidar_robot @ T_robot_world注意顺序：从右往左读，先世界到机器人，再到激光雷达，最后到相机。
注意：矩阵乘法不满足交换律。顺序错了，结果全错。我见过有人把 T_robot_world 和 T_lidar_robot 写反了，结果标定出来的点全飞到了天上。
3.6 知识体系总览
下面这张图，帮你把本章的核心逻辑串起来：
3.7 常见坐标系变换场景
| 场景 | 源坐标系 | 目标坐标系 | 变换方式 |
|---|---|---|---|
| 全局路径跟踪 | 世界坐标系 | 机器人坐标系 | T_w_r^(-1) |
| 传感器数据融合 | 激光雷达坐标系 | 机器人坐标系 | T_lidar_robot |
| 视觉伺服控制 | 相机坐标系 | 世界坐标系 | T_camera_world^(-1) |
| 多机器人协同 | 机器人A坐标系 | 机器人B坐标系 | T_a_w^(-1) @ T_b_w |
3.8 避坑指南
做坐标变换，最容易出问题的就这几个地方：
- 角度单位搞混：我见过有人把度当弧度传进去，结果旋转矩阵完全不对。统一用弧度，没商量。
- 变换顺序搞反：先旋转还是先平移？记住：齐次矩阵里是先旋转后平移。如果你先平移后旋转，结果会差一个旋转偏移。
- 忘记齐次坐标：普通3D点 (x,y,z) 要补成 (x,y,z,1) 才能和4x4矩阵相乘。忘了补1，矩阵乘法会报维度错误。
- 逆矩阵计算精度：对于纯旋转+平移的变换矩阵，逆矩阵可以用转置+负平移快速计算，比 np.linalg.inv 快得多。
我的习惯：每次写完坐标变换代码，我都会用一个已知点做验证。比如，把机器人坐标系原点 (0,0,0) 变换到世界坐标系，看结果是不是等于机器人位姿。这个小测试能帮你快速发现bug。