第1章：坐标系与位姿表示
各位同学，大家好。我是你们这门课的老朋友。今天咱们来聊聊机器人控制里最基础、也最绕不开的一个话题——坐标系与位姿表示。
说实话，我刚开始做机器人那会儿，觉得这东西太简单了。不就是个坐标嘛，初中就学过。结果第一次调试AGV小车，它就直直地撞上了墙。为什么？因为我把全局坐标和局部坐标搞混了。嗯，从那以后，我再也不敢小看这个知识点。
1.1 全局坐标系与局部坐标系
先问大家一个问题：你站在操场上，说“向前走10米”。这个“前”是谁的前？是你的前，还是操场的前？
这就是全局坐标系和局部坐标系的核心区别。
- 全局坐标系（World Frame）：固定在地面上的坐标系。比如地图上的经纬度，或者工厂车间里的某个固定墙角。它是“绝对”的参考。
- 局部坐标系（Robot Frame / Body Frame）：固定在机器人身上的坐标系。通常以机器人的中心为原点，车头方向为X轴正方向。它是“相对”的参考。
我个人习惯把全局坐标系想象成“上帝视角”，而局部坐标系是“机器人自己的视角”。
关键点：传感器测得的数据，通常是局部坐标系下的。但路径规划、地图构建，都是在全局坐标系下完成的。所以，你必须在两者之间来回切换。
举个例子。你让机器人“向右转”。这个“右”是相对于机器人自己的右，还是相对于地图的右？如果机器人是倒着开的，那它的“右”和地图的“右”正好相反。你看，不搞清楚这个，机器人不撞墙才怪。
1.2 机器人位姿 (x, y, θ)
好了，现在我们知道需要两个坐标系。那怎么描述机器人在全局坐标系下的状态呢？
答案是：位姿（Pose）。
位姿由三部分组成：
- x：机器人在全局坐标系下的横坐标
- y：机器人在全局坐标系下的纵坐标
- θ：机器人的朝向角，通常指机器人车头方向与全局X轴正方向的夹角
说白了，就是“你在哪”和“你面朝哪”。
我的经验：θ的取值范围，我建议统一用[-π, π]或者[0, 2π)。千万别混着用。我在一个项目里吃过这个亏，角度计算时一会儿用度、一会儿用弧度，最后定位误差大得离谱。从那以后，我所有代码里角度一律用弧度制。
你可能会问：为什么是三个量？因为我们的机器人是在二维平面上运动的。如果是无人机或者水下机器人，那就得加上z、roll、pitch、yaw，变成6自由度。但咱们这门课主要讲地面移动机器人，所以(x, y, θ)就够了。
1.3 齐次变换矩阵
现在问题来了：已知机器人在全局坐标系下的位姿是(x, y, θ)，那机器人局部坐标系下的一个点，在全局坐标系下是多少？
这就引出了我们今天的主角——齐次变换矩阵。
先看公式。假设机器人局部坐标系下有一个点P_local = (x_local, y_local)，它在全局坐标系下的坐标P_global为：
P_global = R(θ) * P_local + T其中：
- R(θ) 是旋转矩阵：[[cosθ, -sinθ], [sinθ, cosθ]]
- T 是平移向量：[x, y]^T
但这样写起来有点麻烦。每次都要先旋转再平移。于是数学家们想了个巧妙的办法——把二维坐标扩展成三维齐次坐标。这样，旋转和平移就可以合并成一个矩阵乘法：
[x_global] [cosθ -sinθ x] [x_local]
[y_global] = [sinθ cosθ y] [y_local]
[1 ] [0 0 1] [1 ]这个3x3的矩阵，就是齐次变换矩阵。我习惯用T表示。
核心思想：齐次变换矩阵把“旋转+平移”打包成了一个操作。你只需要一个矩阵乘法，就能完成坐标系变换。
我记得第一次在代码里实现这个矩阵时，还傻傻地先算旋转再算平移。后来发现直接用齐次矩阵，代码简洁了不止一倍。你想想看，一个矩阵乘法搞定一切，多清爽。
1.4 逆变换
反过来，如果我知道全局坐标系下的点，想求它在机器人局部坐标系下的坐标，怎么办？
很简单，求逆矩阵。
齐次变换矩阵的逆矩阵有一个非常优雅的形式：
T_inv = [R^T, -R^T * T]
[0, 1 ]其中R^T是旋转矩阵的转置。因为旋转矩阵是正交矩阵，所以它的逆等于它的转置。这个性质在代码里非常实用，不用真的去算矩阵求逆，直接转置加个负号就行。
避坑指南：我曾经在逆变换时直接调用了线性代数库的求逆函数。结果因为数值精度问题，求出来的逆矩阵误差很大，导致坐标变换后点都飘了。后来我改成手动计算R^T和-R^T*T，问题就解决了。所以，能用解析解就别用数值解。
1.5 知识体系总览
说了这么多，咱们来画个图，把今天的内容串起来。
这张图把今天的内容都串起来了。你看，全局坐标系和局部坐标系之间，通过齐次变换矩阵T及其逆矩阵T⁻¹来沟通。而机器人位姿(x, y, θ)正是这个变换矩阵的参数。
1.6 小结
今天咱们聊了三个东西：
- 全局坐标系 vs 局部坐标系：一个是上帝视角，一个是机器人视角。搞混了就会撞墙。
- 机器人位姿 (x, y, θ)：描述机器人“在哪”和“面朝哪”。二维平面三个量就够了。
- 齐次变换矩阵：把旋转和平移打包成一个矩阵乘法。简洁、优雅、实用。
说实话，这些内容看起来简单，但它是后面所有控制算法的基础。路径跟踪、避障、SLAM，哪一个都离不开坐标系变换。所以，我建议你花点时间，把今天的内容吃透。最好自己动手写几行代码，实现一下齐次变换矩阵的正变换和逆变换。
嗯，今天就到这儿。下一章咱们聊聊运动学模型，看看机器人到底是怎么动起来的。