如何用scipy.spatial.transform.Rotation实现6DoF车辆仿真的坐标变换？

方案1：用scipy.spatial.transform.Rotation（推荐） #

这是scipy专门用来处理旋转变换的模块，完全能替代你手写的变换矩阵，还自动处理了俯仰角奇异这类问题，不用你自己额外做判断。

步骤很简单：

• 先根据你的欧拉角旋转顺序（比如你图里的Z-Y-X偏航-俯仰-滚转）创建Rotation实例：

  from scipy.spatial.transform import Rotation as R
  
  # yaw(ψ)、pitch(θ)、roll(φ)为全局到车体的欧拉角，degrees=False表示用弧度
  rot = R.from_euler('zyx', [yaw, pitch, roll], degrees=False)
  

• 全局→车体变换：直接用旋转矩阵的逆（正交矩阵的逆等于转置）作用于向量，代码里可以写rot.inv().apply(global_torque_or_omega)，或者更高效的rot.T.apply(global_torque_or_omega)
• 车体→全局变换：直接调用rot.apply(body_torque_or_omega)就行

这个模块封装了所有旋转相关的细节，支持四元数、旋转矩阵、轴角等多种表示，数值稳定性比手写矩阵好得多，完全不用你操心奇异情况的处理。

方案2：用scipy.linalg辅助构造变换矩阵 #

如果你更习惯手动操作矩阵，也可以用scipy的线性代数模块来简化实现：

• 用scipy.linalg的函数构造你需要的变换矩阵，比如Z-Y-X顺序的旋转矩阵：

  import numpy as np
  from scipy.linalg import inv
  
  def get_rot_matrix(yaw, pitch, roll):
      cy, sy = np.cos(yaw), np.sin(yaw)
      cp, sp = np.cos(pitch), np.sin(pitch)
      cr, sr = np.cos(roll), np.sin(roll)
      return np.array([
          [cy*cp, cy*sp*sr - sy*cr, cy*sp*cr + sy*sr],
          [sy*cp, sy*sp*sr + cy*cr, sy*sp*cr - cy*sr],
          [-sp, cp*sr, cp*cr]
      ])
  
  rot_mat = get_rot_matrix(yaw, pitch, roll)
  # 全局转车体：正交矩阵逆等于转置，直接用转置相乘
  body_vec = rot_mat.T @ global_vec
  # 车体转全局：直接用原矩阵相乘
  global_vec = rot_mat @ body_vec
  

  这种方式和你原来的实现逻辑一致，scipy的inv函数能稳定处理矩阵求逆，但奇异情况的判断还是需要你自己补充。

注意事项 #

• 优先选第一种方案，Rotation模块是scipy官方推荐的旋转处理工具，比手写矩阵更可靠，也省掉了很多维护工作。
• 一定要注意欧拉角的旋转顺序必须和你原来的变换矩阵完全匹配，否则变换结果会完全错误——这是坐标系变换里最容易踩的坑。

内容的提问来源于stack exchange，提问作者Artur