2. 空间描述与变换：位置与姿态的描述、坐标变换、齐次变换矩阵
好，咱们正式开始聊机器人学里最基础、也最绕不开的一块内容——空间描述与变换。
你想想看，机器人要抓个杯子，它得知道杯子在哪儿、自己的手在哪儿、手该怎么转才能对准杯子。这些「在哪儿」「怎么转」的背后，就是位置和姿态的描述问题。说白了，就是怎么用数学语言告诉机器人：「目标在这里，你往那个方向走，再转个角度，就到了。」
我个人习惯把这一章看作整个机器人学的「坐标系世界观」。你后面学运动学、动力学、轨迹规划，全得靠这套语言。所以咱们慢慢来，把地基打牢。
2.1 位置与姿态的描述
先聊最简单的——位置。
一个点 P 在三维空间里，用三个坐标就能搞定：
P = [x, y, z]ᵀ这没什么好说的。但问题来了：这个坐标是相对于谁的？
嗯，这里要注意——没有的位置，只有相对的位置。你说「杯子在桌子中央」，那是相对于桌子说的。你说「杯子在我右手边 30 厘米」，那是相对于你自己的身体说的。所以，我们得先定一个参考坐标系。
我一般用 {A} 表示世界坐标系，{B} 表示机器人基座坐标系，{C} 表示工具坐标系。这样写公式的时候，一眼就能看出这个向量是相对于谁的。
位置向量表示法：
AP = [px, py, pz]ᵀ —— 表示点 P 在坐标系 {A} 中的位置。
接下来是姿态。这个稍微绕一点。
位置告诉你「在哪儿」，姿态告诉你「朝哪儿」。一个刚体在空间里，不光有位置，还有朝向。比如一个扳手，你把它平放和竖放，位置可能一样，但姿态完全不同。
描述姿态最直观的方式是旋转矩阵。假设我们有两个坐标系 {A} 和 {B}，{B} 相对于 {A} 的姿态，可以用一个 3×3 的矩阵来表示：
ABR = [ AXB , AYB , AZB ]这个矩阵的每一列，就是 {B} 的三个坐标轴在 {A} 中的方向向量。说白了，你把 {B} 的 x 轴、y 轴、z 轴在 {A} 里怎么投影的写出来，就是一个旋转矩阵。
小技巧：旋转矩阵有个好记的性质——它的逆等于它的转置。也就是说 ABR⁻¹ = ABRᵀ。这在做坐标变换的时候能省不少事。我当年第一次推导雅可比矩阵时，就是靠这个性质简化了一大堆计算。
那位置和姿态合在一起，就构成了一个刚体的位姿（Pose）。一个完整的位姿描述是：
{B} = { ABR , APB }其中 APB 是 {B} 的原点在 {A} 中的位置。
2.2 坐标变换
有了位置和姿态的描述，接下来就是怎么把一个点从一个坐标系变到另一个坐标系。这就是坐标变换。
最常见的场景：你用一个相机（坐标系 {C}）看到了一个螺丝的位置，但机器人（坐标系 {R}）要去抓它。你得把相机看到的坐标，换算成机器人能理解的坐标。
坐标变换分两步：
- 旋转：先把点绕着原点转，对齐两个坐标系的朝向。
- 平移：再沿着坐标轴移动，对齐两个坐标系的原点。
公式长这样：
AP = ABR · BP + APB我来拆解一下：
- BP 是点在 {B} 里的坐标。
- 先乘旋转矩阵 ABR，把方向对齐到 {A}。
- 再加上平移向量 APB，把原点对齐。
- 结果 AP 就是点在 {A} 里的坐标。
我曾经在一个项目里犯过低级错误——把旋转和平移的顺序搞反了。先平移再旋转，结果算出来的位置差了十万八千里。那次调试花了我整整一个下午。所以记住：先旋转，后平移。顺序不能乱。
注意：旋转矩阵的乘法不满足交换律。也就是说 ABR · BP 和 BP · ABR 是完全不同的东西。前者是旋转向量，后者是旋转坐标轴——别搞混了。
2.3 齐次变换矩阵
刚才的公式 AP = ABR · BP + APB 虽然清晰，但写起来有点啰嗦。而且如果你有一连串的变换（比如从相机到机械臂末端，再到基座，再到世界），就得反复做旋转+平移，公式会变得很长。
所以，聪明的前辈们想了个办法——齐次变换矩阵。
把旋转矩阵和平移向量塞进一个 4×4 的矩阵里：
ABT = [ ABR APB ]
[ 0 1 ]最后一行是 [0 0 0 1]，这是齐次坐标的固定格式。
同时，把三维点 P 扩展成四维齐次坐标：
BP̃ = [ BP ; 1 ]这样，坐标变换就变成了一个简单的矩阵乘法：
AP̃ = ABT · BP̃你看，旋转和平移被合并成一步了。简洁、优雅、好算。
为什么叫「齐次」？
因为引入了一个额外的维度（第4维），让原本非线性的旋转+平移变成了线性变换。这样你就可以用矩阵乘法串联任意多个变换了。比如从工具坐标系到世界坐标系：
WT = WBT · BET · ETT
三个矩阵一乘，一步到位。
齐次变换矩阵还有一个特别实用的性质——求逆。如果你知道 {B} 相对于 {A} 的变换，想求 {A} 相对于 {B} 的变换，不用重新算，直接对矩阵求逆就行：
BAT = (ABT)⁻¹而且这个逆有解析形式：
BAT = [ (ABR)ᵀ -(ABR)ᵀ · APB ]
[ 0 1 ]嗯，这里要注意——旋转矩阵的逆就是它的转置，所以计算量很小。我写代码的时候，从来不会去调通用的矩阵求逆库，直接用这个公式手写，又快又准。
2.4 本章知识体系
下面这张图把本章的核心逻辑串起来了。你可以把它当作一个「坐标系世界观」的地图：
从图上你能看到，整个知识体系是从「位置」和「姿态」这两个基本概念出发，汇聚成「位姿」，然后通过「坐标变换」把不同坐标系联系起来，最后用「齐次变换矩阵」这个工具统一表达。底部的三个应用方向，就是你在后续章节里会反复用到的地方。
2.5 实用技巧与避坑
最后，分享几个我这些年积累下来的实用经验：
写代码时的建议：
- 用结构体或类把旋转矩阵、平移向量、齐次变换矩阵封装好，别裸着用数组。我见过太多人因为索引搞错而出 bug。
- 每次做变换前，先确认当前坐标系的定义。左手系还是右手系？x 轴朝哪？y 轴朝哪？这些不统一，后面全白算。
- 调试时，把变换矩阵打印出来，手动验证一下单位向量变换后长度是否为 1。如果变了，说明旋转矩阵有问题。
我曾经踩过的坑：
有一次做手眼标定，相机和机械臂之间的变换怎么算都不对。折腾了两天，最后发现是相机坐标系是「z 轴朝前」，而机械臂坐标系是「z 轴朝上」。两个坐标系的定义不一样，直接套公式当然出错。所以——拿到任何传感器或机器人的坐标系定义，第一件事就是确认它的轴向和方向。别想当然。