2. 数学基础回顾（上）：刚体运动描述、旋转矩阵、欧拉角

做机器人仿真，说白了就是跟空间里的位置和姿态打交道。你想想看，一个机械臂要抓杯子，它得先知道杯子在哪，自己的手爪朝哪个方向。这些信息，全靠数学来描述。

我个人习惯把这一章叫做「地基」。地基打不牢，后面建高楼肯定歪。我记得刚入行那会儿，觉得旋转矩阵不就是个3x3的表格嘛，有啥好学的？结果第一次做逆解算，算出来的角度死活不对，折腾了两天...后来才发现是旋转顺序搞反了。嗯，从那以后我再也不敢小看这些基础了。

2.1 刚体运动的基本概念

什么叫刚体？就是不会变形的物体。机械臂的连杆、机器人的基座，我们都当成刚体处理。刚体在空间里有6个自由度——3个平移自由度（x, y, z），3个旋转自由度（绕x, y, z轴转）。

描述刚体运动，我们需要两个东西：

• 位置：用一个三维向量 p = [x, y, z]^T 表示
• 姿态：用旋转矩阵 R 或欧拉角表示

把位置和姿态合在一起，就构成了刚体的位姿。我在项目中经常用一个4x4的齐次变换矩阵来同时表示这两者，后面会详细讲。

2.2 旋转矩阵

旋转矩阵是描述姿态最直接的方式。它是一个3x3的正交矩阵，行列式为+1。说白了，就是一组标准正交基，告诉你刚体的三个轴在参考坐标系里指向哪。

绕x轴旋转θ角的旋转矩阵长这样：

R_x(θ) = [1      0       0    ]
         [0   cosθ   -sinθ   ]
         [0   sinθ    cosθ   ]

绕y轴和z轴类似：

R_y(θ) = [ cosθ   0   sinθ   ]
         [ 0      1   0      ]
         [-sinθ   0   cosθ   ]

R_z(θ) = [cosθ  -sinθ   0    ]
         [sinθ   cosθ   0    ]
         [0      0      1    ]

旋转矩阵有两个重要性质：

• 正交性：R^T = R^{-1}，即转置等于逆。这意味着求逆特别快。
• 组合性：连续旋转可以用矩阵乘法表示。R_total = R_2 · R_1

注意！矩阵乘法不满足交换律。R_2 · R_1 和 R_1 · R_2 结果不同。我曾经在调试一个六轴机器人时，把旋转顺序搞反了，结果末端执行器直接飞到天花板上去...嗯，从那以后我每次写旋转都会先确认顺序。

2.3 欧拉角

旋转矩阵虽然数学上很漂亮，但9个参数太冗余了。人眼一看，根本不知道这个矩阵对应什么姿态。欧拉角就直观多了——用三个角度来描述旋转。

常见的欧拉角约定有：

• ZYX顺序（也叫RPY角）：先绕z轴转（偏航yaw），再绕y轴转（俯仰pitch），最后绕x轴转（横滚roll）。这是机器人领域最常用的。
• ZYZ顺序：常用于理论推导。

从欧拉角到旋转矩阵的转换（ZYX顺序）：

R = R_z(yaw) · R_y(pitch) · R_x(roll)

展开后：

R = [cos(y)cos(p)  cos(y)sin(p)sin(r)-sin(y)cos(r)  cos(y)sin(p)cos(r)+sin(y)sin(r)]
    [sin(y)cos(p)  sin(y)sin(p)sin(r)+cos(y)cos(r)  sin(y)sin(p)cos(r)-cos(y)sin(r)]
    [-sin(p)       cos(p)sin(r)                      cos(p)cos(r)                   ]

其中 y = yaw, p = pitch, r = roll。

从旋转矩阵反算欧拉角：

pitch = asin(-R[2][0])
yaw   = atan2(R[1][0], R[0][0])
roll  = atan2(R[2][1], R[2][2])

注意atan2函数，它能处理四个象限的角度，比atan安全得多。

2.4 实际应用中的选择

表示方法	优点	缺点	我常用的场景
旋转矩阵	数学性质好，无奇点	9个参数冗余	底层计算、坐标变换
欧拉角	直观，易理解	有万向锁，插值困难	人机交互、示教器显示
四元数	无奇点，插值平滑	不够直观	姿态控制、轨迹规划

我个人习惯是：在代码内部用旋转矩阵或四元数做计算，只在界面上显示给用户看时才转成欧拉角。这样既保证了计算稳定性，又照顾了人的直观感受。

好了，这一章的内容就这些。旋转矩阵和欧拉角是描述刚体姿态的两把刷子，各有各的脾气。下一章我们会继续聊齐次变换矩阵和四元数，把整个数学工具箱补齐。

---

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