2、运动控制基础（一）：坐标系与位姿描述

各位同学，欢迎来到运动控制的第一课。说实话，我刚入行那会儿，觉得坐标系这玩意儿太简单了，不就是XYZ嘛。结果第一次调机械臂，工具坐标系设错了，直接撞了工件。嗯，从那以后我再也不敢小看这些基础概念了。

今天咱们要聊的，是运动控制的「世界观」——坐标系与位姿描述。你想想看，一个机器人要动起来，首先得知道「我在哪」、「我要去哪」。这背后全靠坐标系和位姿来支撑。

2.1 世界坐标系 vs 工具坐标系

先说世界坐标系。说白了，这就是一个「绝对参考系」。我习惯把世界坐标系原点设在机器人底座中心，Z轴朝上，X轴朝前。这样整个工作空间里的位置都有了一个统一的度量标准。

但问题来了——机器人末端夹着工具，你关心的是工具尖端的运动，而不是机器人法兰盘。这时候就需要工具坐标系（TCP，Tool Center Point）。

我在项目中遇到过最典型的坑：用六轴机器人拧螺丝，工具坐标系没标定，结果每次换刀片都要重新调点位。后来我学乖了，提前用四点法标定好TCP，一劳永逸。

坐标系类型	特点	典型应用
世界坐标系	固定不变，全局参考	路径规划、多机器人协同
工具坐标系	随工具变化，局部参考	焊接、抓取、装配
工件坐标系	依附于工件	流水线作业、视觉引导

2.2 位姿描述：位置 + 姿态

一个刚体在空间中的状态，用「位姿」来描述。位姿 = 位置（3个自由度）+ 姿态（3个自由度），一共6个自由度。

位置好理解，就是XYZ坐标。但姿态怎么描述？这里就有讲究了。常用的有三种方式：

• 旋转矩阵：3x3矩阵，9个参数，冗余大，计算方便
• 欧拉角：3个角度，直观但容易死锁
• 四元数：4个参数，无奇点，插值平滑

我个人最常用的是四元数，原因后面会讲。但先说说欧拉角，因为很多工程师一开始都爱用它。

2.3 欧拉角：直观但危险

欧拉角用三个旋转角度来描述姿态，比如绕Z轴转α，再绕Y轴转β，再绕X轴转γ。听起来很直观对吧？

但有个大坑——万向锁（Gimbal Lock）。当β接近±90°时，α和γ的旋转轴会重合，丢失一个自由度。我曾经在调试一个云台时，俯仰角到了85°，结果航向角突然失控乱转。查了半天，就是万向锁搞的鬼。

欧拉角的另一个问题是：旋转顺序不同，结果不同。常见的顺序有ZYX、ZYZ、XYZ等。我建议统一用ZYX（先绕Z，再绕Y，最后绕X），这是工业机器人最常用的约定。

2.4 四元数：优雅的解决方案

四元数是什么？说白了就是一个超复数：q = w + xi + yj + zk，其中w是实部，x、y、z是虚部。约束条件：w² + x² + y² + z² = 1。

为什么我偏爱四元数？三个理由：

1. 无奇点：不会出现万向锁，任意姿态都能平滑表示
2. 插值平滑：用球面线性插值（SLERP），旋转过渡自然
3. 计算高效：组合旋转只需一次乘法，比矩阵快

来看一个实际例子。假设你要让机械臂从姿态A转到姿态B，用欧拉角插值会这样：

// 欧拉角插值（容易出问题）
float alpha = lerp(alpha1, alpha2, t);
float beta  = lerp(beta1,  beta2,  t);
float gamma = lerp(gamma1, gamma2, t);
// 问题：中间姿态可能不自然，甚至出现翻转

用四元数就优雅多了：

// 四元数SLERP插值
Quaternion q_start, q_end;
float t = 0.5;  // 插值系数
Quaternion q_mid = slerp(q_start, q_end, t);
// 结果：最短路径旋转，平滑自然

2.5 刚体运动学基础

刚体运动学，说白了就是研究「物体怎么动」而不考虑「为什么动」。核心概念就两个：

• 平移：位置变化，用向量表示
• 旋转：姿态变化，用旋转矩阵或四元数表示

一个刚体的完整运动，可以分解为：先旋转，再平移。用齐次变换矩阵表示：

| R   t |
| 0   1 |

其中 R 是 3x3 旋转矩阵
      t 是 3x1 平移向量

我在做AGV导航时，经常用齐次矩阵来拼接不同传感器数据。比如激光雷达的局部坐标系，要转换到世界坐标系，就是左乘一个齐次变换矩阵。

刚体运动学还有一个重要概念——速度。线速度v和角速度ω。注意：角速度不是欧拉角的导数！这个坑我踩过。角速度是瞬时旋转轴的方向和大小，而欧拉角导数需要经过复杂的雅可比矩阵转换。

2.6 知识体系总览

下面这张图，是我自己总结的运动控制坐标系知识体系。建议你保存下来，每次调试前看一眼。

2.7 实战建议

最后，给你几个我踩坑换来的建议：

• 坐标系命名要规范：我习惯用 {W}、{T}、{O} 分别表示世界、工具、工件坐标系。代码里也保持一致，避免混淆。
• 四元数一定要归一化：浮点运算会导致四元数偏离单位球面，每隔几步就做一次归一化，否则姿态会漂移。
• 调试时先验证旋转：我每次写运动学代码，都会先单独测试旋转部分。用一个已知姿态输入，看输出是否正确。平移一般不会错，旋转才是bug重灾区。

---