SciPy Rotation仅调用inv()时输出正确结果，问题排查求助

问题描述 #

我有一个通过旋转矩阵和NumPy对欧拉角输入执行旋转的函数，该函数已在多架无人机的实时飞行控制环路中运行，正确性得到验证。
我尝试使用SciPy Rotation实现相同功能，直接编写代码得到的输出完全异常，但无理由添加inv()调用后却能输出正确结果，请问问题出在哪里？
VF、BUL和VTOL为不同的参考坐标系，我从yaml文件加载旋转矩阵。我的输入vehicle_rph是VF坐标系下测得的向量时序数组，需要得到其在VTOL坐标系下的对应值。yaml中所有矩阵均以BUL为参考基准，因此要实现VF到VTOL的转换，需要走VF→BUL→VTOL的路径。
以下我分别用NumPy和SciPy各实现了一次转换流程，可以看到经验证正确的NumPy输出，仅与我额外添加了inv()的SciPy输出匹配。如果能告知Z轴结果存在微小差异的原因会额外感谢，目前该偏差已满足我的使用需求。 #

输入代码 #

# 使用NumPy数组实现
R_VF_from_BUL = frames['frames']['VF']['R_vf_from_bul']
R_VTOL_from_BUL = frames['frames']['VTOL-B']['R_vtolb_from_bul']
R_BUL_from_VF = np.transpose(R_VF_from_BUL)
R_VTOL_from_VF = np.matmul(R_VTOL_from_BUL, R_BUL_from_VF)

# 使用SciPy Rotation实现
r_vf_from_bul = R.from_matrix(frames['frames']['VF']['R_vf_from_bul'])
r_vtol_from_bul = R.from_matrix(frames['frames']['VTOL-B']['R_vtolb_from_bul'])
r_bul_from_vf = r_vf_from_bul.inv()
r_vtol_from_vf = r_vtol_from_bul*r_bul_from_vf

print(f'NumPy计算结果:\n{R_VTOL_from_VF}')
print('')
print(f'SciPy计算结果:\n{r_vtol_from_vf.as_matrix()}')

vehicle_rph_vtol_sp = (r_vtol_from_vf*R.from_euler('XYZ', vehicle_rph)).as_euler('XYZ')
vehicle_rph_vtol_sp_inv = (r_vtol_from_vf.inv()*R.from_euler('XYZ', vehicle_rph)).as_euler('XYZ')
vehicle_rph_vtol_np = transform_euler_angles_signal_to_vtol_from_vf_frame(vehicle_rph)

print('')
print(f'常规SciPy输出: {vehicle_rph_vtol_sp[0]}')
print(f'加inv()的SciPy输出: {vehicle_rph_vtol_sp_inv[0]}')
print(f'NumPy基准输出: {vehicle_rph_vtol_np[0]}')

输出结果 #

NumPy计算的旋转矩阵:
[[ 0.52205926  0.          0.85290921]
 [ 0.          1.          0.        ]
 [-0.85290921  0.          0.52205926]]

SciPy计算的旋转矩阵:
[[ 0.52205926  0.          0.85290921]
 [-0.          1.          0.        ]
 [-0.85290921 -0.          0.52205926]]

常规SciPy输出: [-3.11319763  1.13894171 -1.90741245]
加inv()的SciPy输出: [-0.01189337 -0.04056495  1.25948718]
NumPy基准输出: [-0.01189337 -0.04056495  1.29595719]

问题解答 #

1. 为什么加inv()后输出才正确 #

你混淆了向量坐标系转换和姿态参考系转换的旋转方向：
你通过NumPy计算得到的R_VTOL_from_VF矩阵，作用是将VF坐标系下的向量坐标转换为VTOL坐标系下的坐标，运算逻辑为v_VTOL = R_VTOL_from_VF @ v_VF。
但你后续处理的vehicle_rph是欧拉角表示的姿态，本质是机体相对于VF参考系的旋转变换。当你需要把这个姿态的参考系从VF切换到VTOL时，所需的旋转变换就是R_VTOL_from_VF的逆（旋转矩阵是正交矩阵，逆等于转置），这就是你额外加inv()后结果能和NumPy输出匹配的根本原因。
补充：你构造r_vtol_from_vf的逻辑完全正确，输出的旋转矩阵和NumPy计算结果一致，问题出在对姿态做坐标系转换时的旋转方向搞反了。

2. Z轴结果存在微小差异的原因 #

这个差异来自欧拉角旋转约定的不一致：
SciPy的R.from_euler('XYZ', value)中大写的XYZ对应的是**内旋（intrinsic）顺序，即每次旋转都绕上一次旋转后的机体轴转动；而你自己实现的NumPy版本函数用的是外旋（extrinsic）**顺序，每次旋转都绕固定的参考系轴转动。两者的旋转顺序约定不同，最终解算出来的欧拉角就会出现小范围的偏差。你可以把SciPy调用里的'XYZ'改为小写'xyz'，就可以对齐两个实现的欧拉角约定，消除Z轴的数值差异。 #

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