3. 位姿描述：位置(x,y)与朝向角θ，齐次坐标与变换矩阵

各位同学，今天我们聊一个机器人学里最基础、也最绕不开的话题——位姿描述。

说白了，就是回答两个问题：「我的机器人在哪？」 和 「它脸朝哪边？」

你可能会觉得，这有什么难的？不就是坐标加个角度吗？嗯，没错，但实际工程里，坑全藏在「怎么算」和「怎么组织数据」里。我当年刚入行时，就因为变换矩阵的顺序搞反了，让一台AGV在仓库里原地转了三圈，差点撞货架。从那以后，我对位姿描述就格外较真。

3.1 从二维平面说起：位置与朝向

我们先从最简单的场景开始——平面移动机器人，比如扫地机器人、AGV小车。

在二维平面上，一个刚体的位姿（Pose）可以用三个参数唯一确定：

• x：横坐标，单位米（或毫米）
• y：纵坐标，单位米（或毫米）
• θ：朝向角，单位弧度（或度），通常定义为与x轴正方向的夹角

我习惯把这三个参数写成一个向量：[x, y, θ]ᵀ。这看起来简单，但有个细节要注意：朝向角θ是有范围的。我个人建议统一用 [-π, π) 或 [0, 2π)，千万别混用，否则角度换算时你会疯掉。

举个例子：假设你的机器人位于 (1.0, 2.0) 处，朝向角为 30°（即 π/6 rad）。那么它的位姿就是：

x = 1.0 m
y = 2.0 m
θ = 30° (或 0.5236 rad)

你想想看，如果只告诉位置 (1.0, 2.0)，机器人知道自己在哪，但它不知道往哪走。这就是为什么位姿描述必须包含朝向。

3.2 齐次坐标：为什么要多一个1？

好，现在问题来了。我们想把一个点 (x, y) 先平移，再旋转，怎么办？

用常规的笛卡尔坐标，平移和旋转是两种不同的运算——平移是加法，旋转是乘法。混在一起算很麻烦。有没有一种方法，能把它们统一成矩阵乘法？

有，这就是齐次坐标。

说白了，齐次坐标就是在二维坐标后面加一个「1」，变成 (x, y, 1)。这样，一个2D点就变成了3D向量。为什么这么做？因为这样我们就可以用一个 3×3 的矩阵，同时表达旋转和平移。

举个例子，点 P 在笛卡尔坐标下是 (2, 3)，齐次坐标就是 (2, 3, 1)。

3.3 变换矩阵：旋转+平移，一网打尽

有了齐次坐标，我们就可以定义齐次变换矩阵了。对于二维平面，它是一个 3×3 的矩阵：

| cosθ  -sinθ  tx |
| sinθ   cosθ  ty |
|  0      0    1  |

其中：

• cosθ, sinθ 负责旋转
• tx, ty 负责平移
• 最后一行永远是 [0, 0, 1]，这是齐次坐标的固定格式

这个矩阵的作用是：把一个点从一个坐标系，变换到另一个坐标系。

我记得有一次做多传感器融合，需要把激光雷达的数据转换到机器人底盘坐标系。当时我直接用这个矩阵左乘点云，一行代码搞定。同事还在手算三角函数，我已经跑完数据了。嗯，这就是矩阵的力量。

3.4 实战：用变换矩阵描述机器人位姿

现在我们把前面学的串起来。

假设机器人的位姿是 (x=1.0, y=2.0, θ=30°)。那么它的变换矩阵就是：

T = | cos30°  -sin30°  1.0 |
    | sin30°   cos30°  2.0 |
    |   0        0      1   |

代入数值（cos30°≈0.866, sin30°=0.5）：

T = | 0.866  -0.5  1.0 |
    | 0.5    0.866 2.0 |
    | 0      0     1   |

这个矩阵 T 就完整描述了机器人的位姿。如果你想问：「机器人坐标系里的一个点 (0.5, 0.3)，在世界坐标系里是多少？」

很简单，用 T 左乘这个点的齐次坐标：

P_world = T * [0.5, 0.3, 1]ᵀ

算出来就是世界坐标下的位置。这就是变换矩阵的威力——一次乘法，完成旋转+平移。

3.5 逆变换：从世界坐标回到机器人坐标

有正向就有逆向。如果我们知道世界坐标下的点，想求它在机器人坐标系下的坐标，就需要逆变换矩阵。

对于齐次变换矩阵，求逆有公式：

T_inv = | Rᵀ   -Rᵀ * t |
        | 0      1     |

其中 R 是旋转矩阵（2×2），t 是平移向量（2×1）。

说白了，就是旋转矩阵转置，平移向量取反再旋转。这个公式我建议你背下来，因为实际工程中经常要用到。

3.6 知识体系总览

为了让你更直观地理解本章的知识结构，我画了一张图：

这张图展示了本章的核心逻辑：从位姿的三个参数出发，引出位置和姿态，再通过齐次坐标和变换矩阵，最终实现坐标变换。你顺着箭头看，思路就很清晰了。

3.7 总结与实用建议

好了，我们来收个尾。

这一章我们讲了：

• 位姿 = 位置 (x,y) + 朝向角 θ
• 齐次坐标就是在坐标后面加个1，让平移变成乘法
• 变换矩阵 T 同时包含旋转和平移，一次乘法搞定坐标变换
• 逆变换用于从目标坐标系回到源坐标系

我曾经在一个项目中，因为偷懒用了 (x,y,θ) 向量直接做运算，结果在串联多个坐标系时，代码越写越乱，最后不得不重构。从那以后，我所有机器人项目都强制使用齐次变换矩阵。你想想看，一个矩阵搞定所有，多清爽。

---

公众号:蓝海资料掘金营，微信deep3321