从旋转矩阵到齐次变换：机器人空间运动的数学语言

1. 刚体位姿描述从坐标系到旋转矩阵想象你桌上放着一部手机。要精确描述它的摆放状态我们需要两个关键信息位置放在桌子的哪个位置和姿态是横放、竖放还是斜着放。这就是机器人学中的位姿描述问题。最常用的方法是在手机运动刚体上建立一个坐标系{B}在桌子固定参考系上建立坐标系{A}。通过描述{B}相对于{A}的状态就能完整表达手机的位姿位置描述用坐标系{B}的原点在{A}中的坐标表示比如(2,3,0)表示手机中心距离桌子参考点向右2单位、向前3单位。姿态描述用旋转矩阵表示。这个3×3矩阵的每一列都是{B}的X/Y/Z轴在{A}中的方向向量。例如手机竖放时它的Y轴可能指向桌子的Z轴方向。绕单轴旋转的矩阵特别简单# 绕X轴旋转θ角的旋转矩阵 Rx [[1, 0, 0], [0, cosθ, -sinθ], [0, sinθ, cosθ]]这种矩阵有个重要特性正交性。意味着它的逆矩阵等于转置矩阵这在计算坐标变换时非常方便。2. 坐标变换当位置和姿态同时变化实际场景中物体往往既有平移又有旋转。假设手机先旋转了一定角度又被平移到了桌面另一个位置这时如何计算手机表面某点在桌面坐标系中的坐标2.1 分步处理法传统方法是分两步计算旋转用旋转矩阵处理方向变化P_{rotated} R \cdot P_{local}平移加上位置偏移量P_{world} P_{rotated} P但这种方法在连续变换时会变得繁琐。比如机械臂有多个关节时每增加一个关节就需要多一次旋转和平移运算。2.2 齐次坐标的魔法数学家们想出了个聪明办法齐次坐标。通过增加一个维度把旋转和平移合并成一个4×4矩阵T [[r11, r12, r13, px], [r21, r22, r23, py], [r31, r32, r33, pz], [0, 0, 0, 1]]其中左上角3×3是旋转矩阵右上角3×1是平移向量。这样复杂的变换可以用一次矩阵乘法完成P_{world} T \cdot P_{local}实测案例假设手机旋转90度后平移到(5,4,0)屏幕上某点局部坐标(3,2,0)的世界坐标计算import numpy as np T np.array([[0,-1,0,5], [1,0,0,4], [0,0,1,0], [0,0,0,1]]) P_local np.array([3,2,0,1]) # 齐次坐标 P_world T P_local # 输出[3,7,0,1]3. 齐次变换的进阶应用3.1 运动合成的两种方式齐次变换矩阵相乘时顺序不同会导致完全不同的物理意义相对固定坐标系新运动是相对于原始参考系的。这时要从右往左乘矩阵就像穿衣服时先内衣再外套。T_{total} T_{move2} \cdot T_{move1}相对运动坐标系新运动是相对于物体当前位姿的。这时要从左往右乘矩阵就像先戴眼镜再调整眼镜腿。T_{total} T_{move1} \cdot T_{move2}3.2 逆变换换个视角看问题已知{B}相对于{A}的变换矩阵如何求{A}相对于{B}的变换不需要复杂求逆利用旋转矩阵的正交性可以直接构造T_B^A \begin{bmatrix} R_A^B -R_A^B \cdot P \\ 0 1 \end{bmatrix}这在机器人视觉中特别有用。比如已知相机相对于机械臂的位姿就能快速计算机械臂在相机视角中的位置。4. 欧拉角直观但危险的姿态描述虽然旋转矩阵很强大但人类更习惯用角度思考。欧拉角用三个绕轴旋转的角度来描述姿态比如无人机控制中的偏航(Yaw)左右转头俯仰(Pitch)上下点头滚转(Roll)左右歪头# ZYX欧拉角转旋转矩阵 def euler_to_matrix(yaw, pitch, roll): Rz [[cos(yaw), -sin(yaw), 0], [sin(yaw), cos(yaw), 0], [0, 0, 1]] Ry [[cos(pitch), 0, sin(pitch)], [0, 1, 0], [-sin(pitch), 0, cos(pitch)]] Rx [[1, 0, 0], [0, cos(roll), -sin(roll)], [0, sin(roll), cos(roll)]] return Rz Ry Rx但欧拉角有个致命缺陷——万向节死锁。当俯仰角为±90度时偏航和滚转会失去独立性。这就是为什么航天器姿态控制要慎用欧拉角。5. 旋转变换通式绕任意轴旋转现实中的旋转往往不限于坐标轴。通过罗德里格斯公式可以建立绕空间任意单位向量k旋转θ角的通用旋转矩阵R I sinθ[k]_× (1-cosθ)[k]_×^2其中[k]_×是向量k的叉积矩阵。这个公式在机器人轨迹规划中非常实用比如让机械臂末端沿特定轴旋转。我在实际项目中就遇到过这样的情况需要让机械臂绕工具坐标系的一个斜轴旋转15度。直接用这个公式比分解成欧拉角要稳定可靠得多。