机器人姿态转换详解
一、姿态表示方法
机器人姿态由位置（XYZ坐标）和方向（旋转参数）组成，常用表示方法如下：
- 欧拉角（ZYX顺序）：绕固定坐标系的Z轴（偏航Yaw）、Y轴（俯仰Pitch）、X轴（滚动Roll）旋转，表示为
[A, B, C]。 - RPY角（XYZ顺序）：绕动坐标系的X轴（Roll）、Y轴（Pitch）、Z轴（Yaw）旋转，表示为
[Rx, Ry, Rz]。 - 旋转矩阵：3×3正交矩阵，描述坐标系间的旋转变换，可通过欧拉角或RPY角计算。
- 四元数：单位四元数
[w, x, y, z]，避免万向节锁，适合插值运算。 - 齐次变换矩阵：4×4矩阵，整合旋转矩阵（3×3）和平移向量（3×1），方便复合变换运算。
二、核心转换公式
-
欧拉角→旋转矩阵（ZYX顺序）：
def euler_to_rot_matrix(roll, pitch, yaw): R_x = np.array([[1, 0, 0], [0, np.cos(roll), -np.sin(roll)], [0, np.sin(roll), np.cos(roll)]]) R_y = np.array([[np.cos(pitch), 0, np.sin(pitch)], [0, 1, 0], [-np.sin(pitch), 0, np.cos(pitch)]]) R_z = np.array([[np.cos(yaw), -np.sin(yaw), 0], [np.sin(yaw), np.cos(yaw), 0], [0, 0, 1]]) return R_z @ R_y @ R_x -
四元数→旋转矩阵：
def quaternion_to_rot_matrix(q): w, x, y, z = q return np.array([ [1-2*y*y-2*z*z, 2*x*y-2*z*w,
