web-dev-qa-db-ja.com

クォータニオンからヨーを抽出する

回転クォータニオンがあり、上軸（ヨー）の周りの回転角度を抽出したい。私はXNAを使用していますが、これについては組み込みの関数はありません。これを行う最良の方法は何ですか？

助けてくれてありがとう、Venatu

mathxnaquaternions
27
Venatu

回転の四元数表現は、軸と角度の変化です。つまり、軸の周りにrラジアンx、y、zで回転すると、四元数qは：

q[0] = cos(r/2);
q[1] = sin(r/2)*x;
q[2] = sin(r/2)*y;
q[3] = sin(r/2)*z;

y軸の周りを回転するだけのクォータニオンを作成する場合は、x軸とz軸をゼロにしてから、四元数：

q[1] = 0;
q[3] = 0;
double mag = sqrt(q[0]*q[0] + q[2]*q[2]);
q[0] /= mag;
q[2] /= mag;

結果の角度が必要な場合：

double ang = 2*acos(q[0]);

これは、クォータニオン表現が格納されていることを前提としています：w、x、y、z。 q [0]とq [2]の両方がゼロまたはそれに近い場合、結果の四元数は{1,0,0,0}になります。

32
JCooper

クォータニオンqを指定すると、次のようにロール、ピッチ、ヨーを計算できます。

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);

これは、xyzオーダーの固有のテイトブライアンローテーションに適合します。他のローテーション順序では、外部および適切なオイラーローテーションの他の変換を使用する必要があります。

25
thewhiteambit

変換四元数からオイラーへ

ヨー、ピッチ、ロールが任意の回転に適していないことをご存知でしょうか。オイラー角は、特異点（上記のリンクを参照）と不安定性の影響を受けます。デビッドサックスのプレゼンテーションの38:25をご覧ください

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

幸運を！

4
Ali

注：以下のコードを Wikipediaの方程式 プラス Pixhawkのドキュメント に対して検証し、それが正しい。

ドローン/航空を使用している場合、以下はコードです（ DJI SDK から直接取得）。ここで、q0、q1、q2、q3はそれぞれ、クォータニオンのw、x、y、zコンポーネントに対応します。また、一部の文献では、ヨー、ピッチ、ロールをそれぞれヘディング、姿勢、バンクと呼ぶ場合があることに注意してください。

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));

3つすべてを計算する必要がある場合は、次の関数を使用することにより、一般的な用語の再計算を回避できます。

//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;
}

固有ライブラリに関する注意

Eigenライブラリを使用している場合、この変換を行う別の方法がありますが、これは上記の直接コードほど最適化されていない可能性があります。

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

クォータニオンは、3Dベクトルコンポーネントとスカラーコンポーネントの2つのコンポーネントで構成されます。

クォータニオンのベクトルコンポーネントは、各軸の周りの独立した回転を表すため、ベクトルコンポーネントのxコンポーネントとyコンポーネントをゼロにして、zコンポーネントをそのままにするだけで、ベクトルを解決できます。期間：

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

スカラー項は回転の大きさを表します。単位四元数（姿勢を表すために使用されるものなど）の場合、四元数全体の大きさが1でなければなりません。したがって、スカラー項は次のように解決できます。

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

Qxとqyはゼロなので、スカラー成分は

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

したがって、ヨーを表す完全な四元数は、

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