1

我在这里做错了什么？

我有一个使用旋转矩阵和 NumPy 旋转欧拉角输入的函数。这被证明是正确的，因为它目前正在无人机上运行，​​这些无人机在其飞行控制回路中实时使用它。

我正在尝试使用 SciPy Rotation 来实现相同的功能。当我这样做时，结果是乱码，但是如果我似乎没有理由将 inv() 放在那里，它会给出正确的答案。东西放哪儿了？

VF、BUL 和 VTOL 只是不同的参考系。我从 yaml 加载旋转矩阵。我的输入 (vehicle_rph) 是在 VF 框架中测量的向量时间序列数组，但我需要知道 VTOL 框架中的答案。yaml 中的所有内容都与 BUL 进行比较，因此为了从 VF 到达 VTOL，如果必须通过 VF -> BUL -> VTOL

下面我做了两次——一次在 NumPy 中，一次在 SciPy 中。如您所见，当我将随机 inv() 放在那里时，NumPy（被证明是正确的）与 SciPy 匹配。如果你能告诉我为什么 Z 轴略有不同，那么加分，但它已经足够接近我的需要了。

提前致谢！！

输入：

#using numpy arrays
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)

#using scipy rotations
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'Using numpy:\n{R_VTOL_from_VF}')
print('')
print(f'Using 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'sp reg: {vehicle_rph_vtol_sp[0]}')
print(f'sp inv: {vehicle_rph_vtol_sp_inv[0]}')
print(f'np reg: {vehicle_rph_vtol_np[0]}')

输出：

Rotation matrix using NumPy:
[[ 0.52205926  0.          0.85290921]
 [ 0.          1.          0.        ]
 [-0.85290921  0.          0.52205926]]

Rotation matrix using SciPy:
[[ 0.52205926  0.          0.85290921]
 [-0.          1.          0.        ]
 [-0.85290921 -0.          0.52205926]]

sp reg: [-3.11319763  1.13894171 -1.90741245]
sp inv: [-0.01189337 -0.04056495  1.25948718]
np reg: [-0.01189337 -0.04056495  1.29595719]
4

0 回答 0