第二章坐标系与位姿表示：全局坐标系与局部坐标系、欧拉角与四元数、齐次变换矩阵

做轨迹跟踪，第一件事不是调PID参数，而是搞清楚你的机器人现在在哪儿、要往哪儿去。说白了，就是坐标系和位姿表示的问题。我见过太多新手一上来就调参数，结果机器人满屋子乱窜，最后发现是坐标系搞反了——嗯，我自己也干过这种事。

2.1 全局坐标系与局部坐标系

先聊个最基础的概念。全局坐标系，也叫世界坐标系，就是整个场景的“参考系”。比如你在一张地图上标出原点(0,0,0)，所有东西的位置都相对于这个原点来描述。局部坐标系呢，是附着在机器人身上的坐标系，随着机器人移动而移动。

举个例子。你站在房间里，房间的墙角是全局原点。你往前走一步，你的身体坐标系也跟着往前挪了。这时候，你手里的杯子在“你的坐标系”里位置没变，但在“房间坐标系”里位置变了。

我在项目里遇到过最典型的坑：用激光雷达做SLAM时，雷达数据是在雷达自己的局部坐标系下的，但控制指令需要全局坐标。如果你忘了做坐标系转换，机器人会以为障碍物在它左边，实际上在右边——撞墙是迟早的事。

核心要点：

全局坐标系：固定不变，用于描述整个环境
局部坐标系：随机器人运动，用于描述传感器数据和自身状态
两者之间需要做坐标变换，这是轨迹跟踪的基础

为什么需要区分这两个坐标系？你想想看，机器人身上的传感器（比如IMU、轮式编码器）测量的都是相对于机器人自身的数据。但你要规划路径、避开障碍物，必须知道机器人在全局地图中的位置。所以，坐标系转换是绕不开的一步。

2.2 欧拉角与四元数

好，现在我们知道机器人有位置（x,y,z），还有姿态——也就是它朝哪个方向看。描述姿态，最直观的方式就是欧拉角。

欧拉角用三个角度表示：偏航角（Yaw）、俯仰角（Pitch）、横滚角（Roll）。你可以想象一架飞机：偏航是机头左右转，俯仰是机头上下抬，横滚是机身左右倾斜。很直观，对吧？

但是，欧拉角有个致命问题——万向锁。当俯仰角达到±90度时，偏航和横滚会变得无法区分。我在做四轴飞行器的时候遇到过这个问题，飞控突然失控，飞机直接翻了个跟头。后来查日志，发现就是欧拉角导致的万向锁。

避坑指南：

我曾经在无人机项目里直接用欧拉角做姿态插值，结果在俯仰角接近90度时，轨迹出现剧烈抖动。后来换成四元数，问题立刻解决。所以，如果你做的是全向移动机器人或飞行器，建议直接上四元数。

四元数是什么？说白了，它是一个四维复数，用四个数(q0,q1,q2,q3)来表示旋转。它没有万向锁问题，而且插值平滑。缺点是不直观——你很难直接看出机器人朝哪个方向。

我个人习惯：调试阶段用欧拉角看数据，因为一眼就能看出问题。但在代码里，全部用四元数做运算。最后输出控制指令时，再转回欧拉角给用户看。

来个代码示例，展示一下ROS里怎么转换：

# Python示例：欧拉角与四元数互转
import tf.transformations as tf

# 欧拉角转四元数（单位：弧度）
roll = 0.1
pitch = 0.2
yaw = 0.3
quat = tf.quaternion_from_euler(roll, pitch, yaw)
print("四元数:", quat)  # [q0, q1, q2, q3]

# 四元数转欧拉角
euler = tf.euler_from_quaternion(quat)
print("欧拉角:", euler)  # [roll, pitch, yaw]

小技巧：

ROS里用tf库做转换时，注意角度单位是弧度。我见过有人传了度数进去，结果机器人转了半圈就停了——因为90度被当成了90弧度。

2.3 齐次变换矩阵

有了位置和姿态，怎么把它们组合在一起？齐次变换矩阵就是干这个的。它是一个4x4的矩阵，左上角3x3是旋转矩阵（描述姿态），右上角3x1是平移向量（描述位置），最后一行是[0,0,0,1]。

为什么叫“齐次”？因为用这个矩阵，你可以把旋转和平移写成一个矩阵乘法，一次搞定。比如，要把局部坐标系下的点p_local转换到全局坐标系：

p_global = T * p_local

其中T就是齐次变换矩阵。p_local是齐次坐标，也就是在(x,y,z)后面加个1，变成(x,y,z,1)。

我在做机械臂轨迹跟踪时，经常需要串联多个变换。比如：

基座到肩关节的变换
肩关节到肘关节的变换
肘关节到腕关节的变换

把这些矩阵乘起来，就能得到末端执行器在全局坐标系下的位姿。这就是所谓的“正运动学”。

齐次变换矩阵的组成：

部分	维度	含义
旋转矩阵R	3x3	描述姿态（由四元数或欧拉角得到）
平移向量t	3x1	描述位置（x,y,z）
最后一行	1x4	固定为[0,0,0,1]

来个实际例子。假设机器人局部坐标系下有个点(1,0,0)，机器人自己在全局坐标系下的位置是(2,3,0)，朝向是0度。那么齐次变换矩阵就是：

import numpy as np

# 旋转矩阵（这里假设朝向0度，即单位矩阵）
R = np.eye(3)
# 平移向量
t = np.array([[2], [3], [0]])
# 构建齐次变换矩阵
T = np.vstack([np.hstack([R, t]), [0, 0, 0, 1]])

# 局部坐标点（齐次形式）
p_local = np.array([[1], [0], [0], [1]])
# 转换到全局坐标
p_global = T @ p_local
print("全局坐标:", p_global[:3].flatten())  # [3, 3, 0]

你看，局部坐标(1,0,0)加上机器人位置(2,3,0)，得到全局坐标(3,3,0)。很简单，对吧？

注意：

矩阵乘法不满足交换律。先旋转再平移，和先平移再旋转，结果完全不同。我见过有人把顺序搞反，结果机器人定位误差直接差了半米。

2.4 知识体系总览

为了让你更直观地理解这一章的内容，我画了一张图。它展示了坐标系、位姿表示和变换矩阵之间的关系：

这张图把本章的核心逻辑串起来了。全局坐标系和局部坐标系之间通过坐标变换连接，而位姿表示（欧拉角、四元数）最终都汇入齐次变换矩阵，用于实际计算。

好了，这一章就到这里。坐标系和位姿表示是轨迹跟踪的基石，搞不懂这个，后面调参就是瞎调。我个人建议你动手写几个小例子，比如把局部坐标点转换到全局坐标，感受一下矩阵乘法的威力。

可可精品资料库