基于Scipy四元数的ROV六自由度旋转轴一致性问题咨询

背景与已解决问题 #

我正在开发一款简易ROV模拟器，计划用scipy的四元数实现旋转逻辑，替代之前手动用三角函数构造旋转矩阵的方案（即通用3D旋转矩阵实现方式）。

此前已解决一个问题：代码初期运行正常，但多次变换后旋转不再绕本体轴进行，推测是手动构造矩阵的误差累积导致，改用scipy四元数后该问题解决。

当前待解决问题 #

当前存在旋转轴不一致的问题：偏航（yaw）旋转围绕全局Z轴进行，而横滚（roll）、俯仰（pitch）围绕ROV本体X、Y轴进行，如何实现所有旋转都围绕本体轴进行，保证旋转轴的一致性？

实现代码 #

import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
from matplotlib.widgets import Slider
import numpy as np
from scipy.spatial.transform import Rotation

class RovTemp(object):
    def __init__(self):
        # 全局坐标系下的旋转矩阵
        self.vehicleAxes = np.eye(3)
        # 当前横滚、俯仰、偏航角度
        self.rotation_angles = np.zeros(3)
        # 拆解本体坐标系的三个单位向量，方便调用
        self.iHat, self.jHat, self.kHat = self.getCoordSystem()

    def getCoordSystem(self):
        # 返回iHat, jHat, kHat
        return self.vehicleAxes.T

    def computeRollPitchYaw(self):
        # 计算全局坐标系下的横滚、俯仰、偏航角度
        roll = np.arctan2(self.kHat[1], self.kHat[2])
        pitch = np.arctan2(-self.kHat[0], np.sqrt(self.kHat[1]**2 + self.kHat[2]**2))
        yaw = np.arctan2(self.jHat[0], self.iHat[0])
        return np.array([roll, pitch, yaw])

    def updateMovingCoordSystem(self, rotation_angles):
        # 存储当前姿态角度
        self.rotation_angles = rotation_angles
        # 从(横滚、俯仰、偏航)角度创建四元数并转换为旋转矩阵
        self.vehicleAxes = Rotation.from_euler('xyz', rotation_angles, degrees=False).as_matrix()
        # 更新本体坐标系向量
        self.iHat, self.jHat, self.kHat = self.getCoordSystem()

rov = RovTemp()

# 绘制姿态
fig  = plt.figure()
ax = fig.add_subplot(projection='3d')
ax.set_xlabel("x")
ax.set_ylabel("y")
ax.set_zlabel("z")
ax.set_aspect("equal")
plt.subplots_adjust(top=0.95, bottom=0.15)
lim = 0.5
ax.set_xlim((-lim, lim))
ax.set_ylim((-lim, lim))
ax.set_zlim((-lim, lim))

def plotCoordSystem(ax, iHat, jHat, kHat, x0=np.zeros(3), ds=0.45, ls="-"):
    x1 = x0 + iHat*ds
    x2 = x0 + jHat*ds
    x3 = x0 + kHat*ds
    lns = ax.plot([x0[0], x1[0]], [x0[1], x1[1]], [x0[2], x1[2]], "r", ls=ls, lw=2)
    lns += ax.plot([x0[0], x2[0]], [x0[1], x2[1]], [x0[2], x2[2]], "g", ls=ls, lw=2)
    lns += ax.plot([x0[0], x3[0]], [x0[1], x3[1]], [x0[2], x3[2]], "b", ls=ls, lw=2)
    return lns

# 绘制两次坐标系：一次作为参考（虚线），一次用于更新（实线）
plotCoordSystem(ax, rov.iHat, rov.jHat, rov.kHat, ls="--")
lns = plotCoordSystem(ax, rov.iHat, rov.jHat, rov.kHat)

sldr_ax1 = fig.add_axes([0.15, 0.01, 0.7, 0.025])
sldr_ax2 = fig.add_axes([0.15, 0.05, 0.7, 0.025])
sldr_ax3 = fig.add_axes([0.15, 0.09, 0.7, 0.025])
sldrLim = 180
sldr1 = Slider(sldr_ax1, 'phi', -sldrLim, sldrLim, valinit=0, valfmt="%.1f deg")
sldr2 = Slider(sldr_ax2, 'theta', -sldrLim, sldrLim, valinit=0, valfmt="%.1f deg")
sldr3 = Slider(sldr_ax3, 'psi', -sldrLim, sldrLim, valinit=0, valfmt="%.1f deg")

def onChanged(val):
    global rov, lns, ax
    angles = np.array([sldr1.val, sldr2.val, sldr3.val])/180.*np.pi
    rov.updateMovingCoordSystem(angles)
    for l in lns:
        l.remove()
    lns = plotCoordSystem(ax, rov.iHat, rov.jHat, rov.kHat)
    ax.set_title(
        "roll, pitch, yaw = "+", ".join(['{:.1f} deg'.format(v) for v in rov.computeRollPitchYaw()/np.pi*180.]))
    return lns

sldr1.on_changed(onChanged)
sldr2.on_changed(onChanged)
sldr3.on_changed(onChanged)

plt.show()

问题现象 #

滑块设置的旋转角度与程序计算并显示的角度不一致，如图所示：

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