3、运动学基础（一）：刚体在空间中的位姿描述、旋转矩阵、齐次变换矩阵
各位同学，欢迎来到运动学基础的第一讲。
说实话，很多初学者一上来就盯着DH参数、逆解公式猛啃，结果越学越懵。为什么？因为最底层的概念没搞透——刚体在空间里到底怎么描述？
我当年刚入行时也犯过这个错。有一次调试一个六轴协作臂，明明正解算出来位置是对的，但末端就是歪的。查了两天，最后发现是旋转矩阵的乘法顺序搞反了。嗯，从那以后，我再也不敢小看这节内容了。
3.1 刚体在空间中的位姿描述
先问一个问题：一个机械臂的末端执行器，在空间里到底有几个自由度？
答案是6个。3个位置自由度（x, y, z），3个姿态自由度（绕三个轴的旋转）。这6个量合在一起，就叫做位姿。
核心概念：
- 位置：用三维坐标 (x, y, z) 表示，描述刚体上某一点在参考坐标系中的位置。
- 姿态：用旋转矩阵或欧拉角表示，描述刚体相对于参考坐标系的朝向。
- 位姿 = 位置 + 姿态，完整描述刚体在空间中的状态。
我个人习惯把位姿想象成「一个坐标系」。每个刚体都固连一个坐标系，这个坐标系的原点就是位置，坐标轴的指向就是姿态。你想想看，这样理解是不是直观多了？
3.2 旋转矩阵
旋转矩阵是描述姿态最基础的工具。它是一个3×3的正交矩阵，行列式为+1。
为什么用矩阵？因为方便计算。两个坐标系之间的旋转关系，说白了就是一个线性变换。
3.2.1 绕单轴的旋转矩阵
绕x轴旋转角度θ：
R_x(θ) = [1 0 0 ]
[0 cosθ -sinθ ]
[0 sinθ cosθ ]绕y轴旋转角度θ：
R_y(θ) = [ cosθ 0 sinθ ]
[ 0 1 0 ]
[-sinθ 0 cosθ ]绕z轴旋转角度θ：
R_z(θ) = [cosθ -sinθ 0 ]
[sinθ cosθ 0 ]
[0 0 1 ]小技巧：记不住正负号？我有个笨办法——用右手定则。大拇指指向旋转轴正方向，四指弯曲的方向就是正角度方向。绕z轴旋转时，从z轴正向往原点看，x轴转向y轴就是正方向。
3.2.2 旋转矩阵的性质
旋转矩阵有几个重要性质，我在项目中经常用到：
- 正交性：RT = R-1，即转置等于逆。这意味着求逆运算变得极其简单。
- 行列式为+1：保证是纯旋转，没有缩放或镜像。
- 不满足交换律：R1·R2 ≠ R2·R1。这一点特别容易踩坑。
避坑指南：我曾经在写轨迹规划代码时，把两个旋转矩阵的乘法顺序搞反了。结果机械臂末端画出来的圆变成了椭圆。记住：先旋转的矩阵放在右边，后旋转的放在左边。即 R = R后 · R先。
3.3 齐次变换矩阵
旋转矩阵只能描述姿态，不能描述位置。那位置和姿态一起描述怎么办？用齐次变换矩阵。
齐次变换矩阵是一个4×4的矩阵：
T = [R p]
[0 1]其中R是3×3旋转矩阵，p是3×1位置向量，0是1×3零向量。
为什么叫「齐次」？因为引入了第4个维度，把旋转和平移统一到了一个矩阵里。这样，一个点从一个坐标系变换到另一个坐标系，只需要一次矩阵乘法：
P' = T · P其中P是齐次坐标 (x, y, z, 1)T。
3.3.1 齐次变换矩阵的逆
求逆也很方便：
T⁻¹ = [Rᵀ -Rᵀ·p]
[0 1 ]不用去算4×4矩阵的逆，直接用这个公式就行。我在写代码时都是这么干的，省时省力。
3.4 知识体系结构图
下面我用一张SVG图来总结本章的核心逻辑：
3.5 实际应用中的注意事项
讲完了理论，说说实际中怎么用。
| 场景 | 常用方法 | 我的建议 |
|---|---|---|
| 描述末端位姿 | 齐次变换矩阵 | 用4×4矩阵，方便链式乘法 |
| 视觉标定 | 旋转矩阵 + 平移向量 | 注意坐标系定义要统一 |
| 轨迹插补 | 欧拉角或四元数 | 避免万向锁用四元数 |
| 逆解计算 | 旋转矩阵分块 | 利用正交性简化运算 |
经验之谈：写代码时，我建议把所有旋转矩阵和齐次变换矩阵封装成类。这样不容易搞混坐标系。我自己的库里就有一个Transform类，包含R和p，重载了乘法运算符，用起来特别顺手。
好了，这一讲就到这里。刚体位姿描述是运动学的基石，后面的正解、逆解、雅可比矩阵全都建立在这个基础上。把旋转矩阵和齐次变换矩阵搞透，后面的路就好走了。
本章要点回顾：
- 位姿 = 位置 (3) + 姿态 (3) = 6自由度
- 旋转矩阵是正交矩阵，RT = R-1
- 齐次变换矩阵统一描述旋转和平移
- 矩阵乘法顺序不可交换，先旋转的放右边
可可精品资料库