Wydobywanie Ziemii z Czwartorzędu

Po podaniu Quaternionu q, można obliczyć rzut, skok i odchylenie w następujący sposób:

var yaw = atan2(2.0*(q.y*q.z + q.w*q.x), q.w*q.w - q.x*q.x - q.y*q.y + q.z*q.z);
var pitch = asin(-2.0*(q.x*q.z - q.w*q.y));
var roll = atan2(2.0*(q.x*q.y + q.w*q.z), q.w*q.w + q.x*q.x - q.y*q.y - q.z*q.z);

To powinno pasować do wewnętrznej rotacji tait-Bryana xyz-order. Dla innych rozkazów rotacji, rotacji exlinsic i proper-Euler muszą być używane inne przekształcenia.

Przekształcenie Czwartorzędu w Eulera

Mam nadzieję, że wiesz, że odchylenie, rzut i rzut nie są dobre dla dowolnych obrotów. Kąty Eulera cierpią z powodu osobliwości (patrz powyższy link) i niestabilności. Zobacz 38: 25 prezentacji Davida Sachsa

Http://www.youtube.com/watch?v=C7JQ7Rpwn2k

Powodzenia!

Uwaga : zweryfikowałem poniższy kod z równaniami Wikipedii plus dokumentacja Pixhawk i jest poprawna.

Jeśli pracujesz z dronami / lotnictwem, poniżej znajduje się kod (zaczerpnięty bezpośrednio z DJI SDK ). Tutaj Q0, q1, q2,q3 odpowiada odpowiednio składowym w,x,y, z czwartorzędu. Należy również pamiętać, że odchylenie, skok, rzut mogą być określane odpowiednio jako kierunek, postawa i bank w niektórych literaturze.

float roll  = atan2(2.0 * (q.q3 * q.q2 + q.q0 * q.q1) , 1.0 - 2.0 * (q.q1 * q.q1 + q.q2 * q.q2));
float pitch = asin(2.0 * (q.q2 * q.q0 - q.q3 * q.q1));
float yaw   = atan2(2.0 * (q.q3 * q.q0 + q.q1 * q.q2) , - 1.0 + 2.0 * (q.q0 * q.q0 + q.q1 * q.q1));

Jeśli potrzebujesz Oblicz wszystkie 3, a następnie możesz uniknąć przeliczania ogólnych terminów za pomocą następujących funkcji:

//Source: http://docs.ros.org/latest-lts/api/dji_sdk_lib/html/DJI__Flight_8cpp_source.html#l00152
EulerianAngle Flight::toEulerianAngle(QuaternionData data)
{
    EulerianAngle ans;

    double q2sqr = data.q2 * data.q2;
    double t0 = -2.0 * (q2sqr + data.q3 * data.q3) + 1.0;
    double t1 = +2.0 * (data.q1 * data.q2 + data.q0 * data.q3);
    double t2 = -2.0 * (data.q1 * data.q3 - data.q0 * data.q2);
    double t3 = +2.0 * (data.q2 * data.q3 + data.q0 * data.q1);
    double t4 = -2.0 * (data.q1 * data.q1 + q2sqr) + 1.0;

    t2 = t2 > 1.0 ? 1.0 : t2;
    t2 = t2 < -1.0 ? -1.0 : t2;

    ans.pitch = asin(t2);
    ans.roll = atan2(t3, t4);
    ans.yaw = atan2(t1, t0);

    return ans;
}

QuaternionData Flight::toQuaternion(EulerianAngle data)
{
    QuaternionData ans;
    double t0 = cos(data.yaw * 0.5);
    double t1 = sin(data.yaw * 0.5);
    double t2 = cos(data.roll * 0.5);
    double t3 = sin(data.roll * 0.5);
    double t4 = cos(data.pitch * 0.5);
    double t5 = sin(data.pitch * 0.5);

    ans.q0 = t2 * t4 * t0 + t3 * t5 * t1;
    ans.q1 = t3 * t4 * t0 - t2 * t5 * t1;
    ans.q2 = t2 * t5 * t0 + t3 * t4 * t1;
    ans.q3 = t2 * t4 * t1 - t3 * t5 * t0;
    return ans;
}

Notatka o Bibliotece Eigen

Jeśli używasz biblioteki Eigen, ma ona inny sposób na tę konwersję, jednak może to nie być tak zoptymalizowane jak powyższy kod bezpośredni:

  Vector3d euler = quaternion.toRotationMatrix().eulerAngles(2, 1, 0);
  yaw = euler[0]; pitch = euler[1]; roll = euler[2];

Quaternion składa się z dwóch składowych: składowej wektorowej 3d i składowej skalarnej.

Składnik wektorowy czwartorzędu opisuje niezależne obroty wokół każdej osi, więc zerowanie x - i y-składników składnika wektorowego i pozostawienie z-składnika jako-jest to wszystko, co musisz zrobić, aby rozwiązać dla terminu wektorowego:

// Don't modify qz
double qx = 0;
double qy = 0;  

Termin skalarny reprezentuje wielkość obrotu. Dla kwaternionu jednostkowego (takiego jak jeden używany do reprezentowania postawy), całość quaternion musi mieć wielkość 1. Tak więc termin skalarny może być rozwiązany przez:

double qw = sqrt(1 - qx*qx - qy*qy - qz*qz);

Ponieważ Qx i qy są równe zeru, składowa skalarna jest dana przez

double qw = sqrt(1 - qz*qz); 

Tak więc, pełny quaternion reprezentujący yaw jest dany przez

double qx = 0;
double qy = 0;
// Don't modify qz
double qw = sqrt(1 - qz*qz);