第二节 坐标系与位姿描述：空间描述、位置与姿态的数学表示、齐次变换矩阵
各位同学，咱们今天聊点硬核的——坐标系与位姿描述。
说实话，我刚开始做机器人那会儿，觉得这玩意儿不就是个坐标嘛，有啥好学的？结果第一次调机械臂就栽了跟头。明明算好的位置，机器人就是抓不到目标。后来才发现，是坐标系搞混了。嗯，从那以后，我再也不敢小看这一章了。
一、空间描述：你站在哪，决定了你看到什么
先问大家一个问题：你说「杯子在桌子上」，这个描述成立的前提是什么？
对，你得先定义「桌子」在哪。机器人也一样。我们描述一个物体的位置，必须基于某个参考系。这个参考系，就是坐标系。
在机器人学里，常用的坐标系有这么几种：
- 世界坐标系：固定在地面上的全局参考系。相当于「绝对位置」。
- 基坐标系：固定在机器人基座上的坐标系。我习惯把它当作机器人的「家」。
- 工具坐标系：固定在机器人末端执行器上的坐标系。比如焊枪尖、夹爪中心。
- 用户坐标系：你自定义的坐标系。比如工作台的一个角落。
说白了，坐标系就是你的「视角」。视角不同，同一个点的坐标值就不同。但点本身没变，变的是描述方式。
二、位置的数学表示：一个向量搞定
位置，就是空间中的一个点。用三维向量表示：
P = [x, y, z]^T比如，点P在世界坐标系下的位置是 (1.0, 2.5, 0.3) 米。就这么简单。
但要注意：这个向量必须明确是相对于哪个坐标系的。否则就是耍流氓。
三、姿态的数学表示：光有位置不够
位置只告诉你在哪，没告诉你怎么「朝向」。比如一个杯子，你可以正着放，也可以倒着放。这就是姿态。
描述姿态，常用的方法有三种：
| 方法 | 优点 | 缺点 |
|---|---|---|
| 旋转矩阵 (3x3) | 直观，容易做变换 | 9个参数，有冗余 |
| 欧拉角 (RPY) | 参数少，容易理解 | 有万向锁问题 |
| 四元数 | 无奇点，插值平滑 | 不太直观 |
我个人最常用的是旋转矩阵。为什么？因为做矩阵乘法方便。你想想看，两个坐标系之间的变换，本质上就是一次矩阵运算。
旋转矩阵 R 是一个 3x3 的正交矩阵，满足 R^T R = I，且 det(R) = 1。它的每一列，就是原坐标系各轴在新坐标系中的投影。
R = [ r11 r12 r13
r21 r22 r23
r31 r32 r33 ]举个例子，绕Z轴旋转θ角：
Rz(θ) = [ cosθ -sinθ 0
sinθ cosθ 0
0 0 1 ]四、齐次变换矩阵：位置+姿态，打包带走
好了，现在我们有位置向量 P 和旋转矩阵 R。能不能把它们合在一起？
当然可以。这就是齐次变换矩阵 T：
T = [ R P
0 1 ]它是一个 4x4 的矩阵。左上角3x3是旋转矩阵，右上角3x1是位置向量，最后一行是 [0 0 0 1]。
为什么叫「齐次」？因为我们在三维坐标后面加了一个1，变成了齐次坐标。这样做的好处是：一次矩阵乘法，就能同时完成旋转和平移。
你想想看，如果没有齐次变换矩阵，你得先旋转再平移，两步操作。有了它，一步到位。
# 伪代码示例：齐次变换
T = [[R, P],
[0, 1]]
# 点P在坐标系A下的坐标
p_A = [x, y, z, 1]^T
# 变换到坐标系B
p_B = T * p_A五、知识体系结构图
下面这张图，是我自己梳理的本章知识脉络。你看一眼，就能明白各个概念之间的关系。
六、实战中的一点体会
最后，跟大家分享一个我自己的经验。
做机器人控制，坐标系变换是基本功。但很多人容易犯一个错：只关注位置，忽略了姿态。比如，你让机器人去抓一个螺丝，位置算对了，但姿态没调好，夹爪一碰就把螺丝碰倒了。
我建议，每次做坐标变换时，都问自己三个问题：
- 这个点相对于哪个坐标系？
- 这个姿态是用什么方式表示的？
- 变换矩阵的乘法顺序对吗？（先旋转后平移，顺序不能乱）
这三个问题想清楚了，坐标系这块基本就不会出大问题。
公众号:蓝海资料掘金营，微信deep3321