机器人姿态转换详解：python和c++案例

机器人姿态转换详解

一、姿态表示方法

机器人姿态由位置（XYZ坐标）和方向（旋转参数）组成，常用表示方法如下：

1. 欧拉角（ZYX顺序）：绕固定坐标系的Z轴（偏航Yaw）、Y轴（俯仰Pitch）、X轴（滚动Roll）旋转，表示为[A, B, C]。
2. RPY角（XYZ顺序）：绕动坐标系的X轴（Roll）、Y轴（Pitch）、Z轴（Yaw）旋转，表示为[Rx, Ry, Rz]。
3. 旋转矩阵：3×3正交矩阵，描述坐标系间的旋转变换，可通过欧拉角或RPY角计算。
4. 四元数：单位四元数[w, x, y, z]，避免万向节锁，适合插值运算。
5. 齐次变换矩阵：4×4矩阵，整合旋转矩阵（3×3）和平移向量（3×1），方便复合变换运算。

二、核心转换公式

6. 欧拉角→旋转矩阵（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
   

7. 四元数→旋转矩阵：

   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,