Quaternion to Yaw Pitch Roll - PullRequest
Новая
       15

Quaternion to Yaw Pitch Roll

0 голосов
/ 16 января 2019

Я пытаюсь преобразовать кватернионы в углы эйлерового тангажа. У меня проблемы с карданным замком. Первая странная вещь, которая возникает, это то, что ошибки уже начинают появляться, когда угол наклона находится в окрестности + -pi / 2. Я думал, что проблемы должны возникать только в точности пи / 2.

Во-вторых, мой код показывает неправильный угол поворота 180 градусов с шагом -90 градусов.

Я пробовал код на этом посте и на этом сайте , но ни один из них не работал. Я также попробовал библиотеку pyquaternion , но эта даже не пытается компенсировать блокировку карданного подвеса. В конце концов я сделал Python-эквивалент этого раздела руководства.


Это сработало лучше, но все же дает вышеуказанные проблемы. Это похоже на проблему, которая должна быть решена 1000 раз, но я не могу точно определить проблему.

Для кватерниона: [0.86169383 0.02081877 -0.5058515 0.03412598] код возвращает правильные углы шага крена рыскания: [0.15911653941132517, -60.832556785346696, -9.335093630495875]

Для кватерниона: [0,81154224 0,01913839 -0,58165337 0,05207959] код возвращает неправильные углы шага поворота рыскания: [-173.53260107524108, -71.09657335881491, 0,0]

Вот мой код: 

    def yaw_pitch_roll(self):
    """Get the equivalent yaw-pitch-roll angles aka. intrinsic Tait-Bryan angles following the z-y'-x'' convention

    Returns:
        yaw:    rotation angle around the z-axis in radians, in the range `[-pi, pi]`
        pitch:  rotation angle around the y'-axis in radians, in the range `[-pi/2, pi/2]`
        roll:   rotation angle around the x''-axis in radians, in the range `[-pi, pi]` 

    The resulting rotation_matrix would be R = R_x(roll) R_y(pitch) R_z(yaw)

    Note: 
        This feature only makes sense when referring to a unit quaternion. Calling this method will implicitly normalise the Quaternion object to a unit quaternion if it is not already one.
    """
    self._normalise()
    qw = self.q[0]
    qx = self.q[1]
    qy = self.q[2]
    qz = self.q[3]


    print(2*(qx*qz-qw*qy), self.q)
    if 2*(qx*qz-qw*qy)>=0.94: #Preventing gimbal lock for north pole
        yaw = np.arctan2(qx*qy-qw*qz,qx*qz+qw*qy)
        roll = 0
    elif 2*(qx*qz-qw*qy)<=-0.94: #Preventing gimbal lock for south pole
        yaw = -np.arctan2(qx*qy-qw*qz,qx*qz+qw*qy)
        roll = 0
    else:
        yaw = np.arctan2(qy*qz + qw*qx,
            1/2 - (qx**2 + qy**2))
        roll = np.arctan2(qx*qy - qw*qz,
            1/2 - (qy**2 + qz**2))
    pitch = np.arcsin(-2*(qx * qz - qw * qy))

    return yaw, pitch, roll
...