机器人运动学基础：位姿描述、齐次变换矩阵、正逆运动学
各位同学，欢迎来到第二章。这一章是机器人学的“地基”。
说实话，我见过太多工程师，算法调得飞起，结果机器人撞了。一查，原来是运动学算错了。嗯，这章内容虽然基础，但你要是搞混了，后面所有轨迹规划都是空中楼阁。
2.1 位姿描述：机器人到底在哪儿？
先问个问题：你怎么告诉别人“杯子在桌子上”？
你肯定会说“杯子在桌子中心偏左10厘米，朝东放着”。这里面其实包含了两层信息：位置和姿态。合起来，就叫位姿。
核心概念：
- 位置：用三维坐标 (x, y, z) 表示。说白了，就是空间里的一个点。
- 姿态：用旋转矩阵、欧拉角或四元数表示。描述的是“它朝哪个方向”。
我个人习惯用 旋转矩阵 来描述姿态。为什么？因为它直观，而且做矩阵乘法时不容易出错。虽然它占内存（9个元素），但计算稳定。
我的小经验：
在项目里，如果你只是做简单的视觉抓取，用欧拉角（RPY角）就够了。但如果你要做复杂的插补或滤波，我建议你转成四元数。为什么？欧拉角有万向锁问题，我曾经在调一个六轴机械臂时，因为没注意这个，导致手腕在某个角度突然“卡死”了一下。嗯，从那以后，我只要涉及连续旋转，必用四元数。
2.2 齐次变换矩阵：把位置和姿态打包
你想想看，如果每次都要分开算位置和姿态，多麻烦？
所以，数学家发明了齐次变换矩阵。它是一个 4x4 的矩阵，把旋转和位移打包在一起。
# 齐次变换矩阵 T 的结构
T = | R p |
| 0 1 |
# R: 3x3 旋转矩阵
# p: 3x1 位置向量
# 最后一行是 [0, 0, 0, 1]这个矩阵有什么用？
- 描述位姿：一个矩阵，搞定一个坐标系相对于另一个坐标系的关系。
- 坐标变换：把一个点从一个坐标系，变换到另一个坐标系。
- 链式乘法：把多个变换连起来。比如：基座 → 关节1 → 关节2 → 末端。
避坑指南：
我曾经在写代码时，把变换矩阵的乘法顺序搞反了。记住：左乘是相对于固定坐标系变换，右乘是相对于当前坐标系变换。搞反了，机器人会跑到天上去。
2.3 正运动学：已知关节角度，求末端位姿
正运动学，说白了就是“已知关节怎么动，求手在哪儿”。
这是最简单的。你只要把每个关节的齐次变换矩阵乘起来，就得到了末端相对于基座的位姿。
# 正运动学公式
T_0_n = T_0_1 * T_1_2 * ... * T_(n-1)_n
# 其中 T_(i-1)_i 是第 i 个关节的变换矩阵我记得刚入行时，觉得这步太简单了，不就是矩阵乘法吗？结果在写代码时，忘了考虑关节的偏移量（DH参数中的d和a），算出来的位置差了10厘米。嗯，细节决定成败。
正运动学的特点：
- 唯一解：给定一组关节角度，末端位姿是确定的。
- 计算简单：就是矩阵连乘。
- 用于仿真和验证：我们经常用正运动学来验证逆运动学算得对不对。
2.4 逆运动学：已知末端位姿，求关节角度
这才是真正的难点。你想让机器人去抓一个杯子，你知道杯子在哪儿（末端位姿），但你需要算出每个关节该转多少度。
逆运动学，就是解决这个问题的。
逆运动学的坑：
- 多解：同一个末端位姿，可能有多种关节角度组合（比如“肘部朝上”和“肘部朝下”）。
- 无解：目标点太远，机器人够不着。
- 奇异点：在某些姿态下，关节速度会变得无穷大，机器人会“抖”。
我曾经在一个项目中，因为没处理奇异点，机器人在执行轨迹时突然加速，差点把工件甩飞。从那以后，我每次做逆运动学求解，都会先检查雅可比矩阵的行列式，看看是不是接近0。
我的建议：
对于六轴机器人，如果追求实时性，用解析法（如果有封闭解）。如果机器人构型复杂，或者你懒得推导公式，用数值法（比如迭代法）。但数值法要注意初值选择，否则可能收敛到错误的解。
2.5 知识体系总览
下面这张图，是我自己总结的运动学知识框架。你把它记住了，后面学轨迹规划会轻松很多。
2.6 本章小结
这一章，我们讲了三个核心概念：
- 位姿描述：用位置和姿态描述一个物体在空间中的状态。
- 齐次变换矩阵：把旋转和平移打包，方便计算。
- 正/逆运动学：正运动学简单唯一，逆运动学复杂多解。
嗯，这些概念你可能觉得抽象。但相信我，等你开始写轨迹规划代码时，你会发现，所有的算法最终都会落到这些矩阵运算上。把基础打牢，后面才能飞得起来。