四元数衍生的旋转矩阵问题

如何解决四元数衍生的旋转矩阵问题

我的问题很简单：我有一个设备，我可以从中读取线性加速度和表示他的方向的四元数。我需要将重力分量添加到这些线性加速度中。 我通过采用四元数找到了这个问题的解决方案，将其转换为旋转矩阵，然后在旋转矩阵和我希望添加的重力向量之间进行矩阵乘积：[0,10]。从结果向量中，我将每个分量添加到线性加速度向量中。为了创建旋转矩阵，我遵循了 this link。 该设备具有 RH 坐标系：

python代码如下：

        q_init = Quaternion(data.pose.orientation.w,data.pose.orientation.x,data.pose.orientation.y,data.pose.orientation.z)

        q = q_init.elements

        q0 = q[0]
        q1 = q[1]
        q2 = q[2]
        q3 = q[3]
        #i create the quaternion-derived rotation matrix

        r00 = 1 - 2*(q2*q2 + q3*q3)
        r01 = 2*(q1*q2 - q3*q0) 
        r02 = 2*(q1*q3 + q2*q0) 

        r10 = 2*(q1*q2 + q3*q0) 
        r11 = 1 - 2*(q1*q1 + q3*q3)
        r12 = 2*(q2*q3 - q1*q0)  

        r20 = 2*(q1*q3 - q2*q0)  
        r21 = 2*(q2*q3 + q1*q0) 
        r22 = 1 - 2*(q1*q1 + q2*q2)

        rot_matrix = np.array([[r00,r01,r02],[r10,r11,r12],[r20,r21,r22]])
        
        g_vect = np.array([[0],[0],[10]])

        g_rot = np.dot(rot_matrix,g_vect)

        if len(self.AccX) != 0:
            
            self.Output[3] = (self.AccX[-1] + g_rot[0,0])
            self.Output[4] = (self.AccY[-1] + g_rot[1,0])
            self.Output[5] = (self.AccZ[-1] + g_rot[2,0])

使用此代码，如果我保持静止，我会得到 Acceleration_with_Gravity (AccG) 向量 [0,10]，这很好，并且与其他设备相同。如果我俯仰 90 度，我会得到 [10,0]，这很好。如果我滚动 90 度。 -> [0,10,0]，很好。

当我第一次俯仰 90 度时出现问题。然后滚动 90 度：

如果我绕 Y Ax 顺时针旋转 90 度，然后绕 X 轴顺时针旋转我的设备，它应该为我旋转的每个角度提供 [10,0] 但是相反，一旦我开始旋转设备，斧头似乎在 Y 上发生了变化（显然是负的），它给了我 [0,-10,0] 而不是常数 [10,0]！

解释图片/视频：

移动：link

旋转视频：Link

我不知道问题出在哪里，我尝试使用其他 python 库，只使用四元数但问题似乎仍然存在，并且旋转矩阵取自维基百科，我也尝试用其他替代方法更改它相同的旋转矩阵，但问题仍然存在。

非常感谢您的帮助。