Tomofiles Note

ドローンとインターネット、そして人との関係を考えるソフトウェアエンジニアのアウトプットブログ

MAVSDKでSITLのドローンからテレメトリーを取得してCesiumJSにアニメーション表示してみた 〜Part 3 座標系解決編〜

こんにちは。

Part1、2でMAVSDKで取得したテレメトリーをCesiumJSに表示するところまで試してみました。

tomofiles.hatenablog.com
tomofiles.hatenablog.com

その時、3Dモデルの表示のされ方が少し変だったと思いますが、以下の記事でその原因を究明しました。

tomofiles.hatenablog.com

今回は、この座標系の知識を利用して、Part1で紹介したmavsdk_tryを改修してみたいと思います。


まず、前回ネットからコピペして利用したロジックを以下に再掲します。

// mavsdk_try/static/App.js #31-47
            var quatlocal = new Cesium.Quaternion(
                czml.orientation.unitQuaternion[1],
                czml.orientation.unitQuaternion[2],
                czml.orientation.unitQuaternion[3],
                czml.orientation.unitQuaternion[4])
            var pos = Cesium.Cartesian3.fromDegrees(
                czml.position.cartographicDegrees[1],
                czml.position.cartographicDegrees[2],
                czml.position.cartographicDegrees[3]);
            var mtx4 = Cesium.Transforms.eastNorthUpToFixedFrame(pos); // ★
            var mtx3 = Cesium.Matrix4.getMatrix3(mtx4, new Cesium.Matrix3());
            var base = Cesium.Quaternion.fromRotationMatrix(mtx3)
            var quat = Cesium.Quaternion.multiply(base, quatlocal, new Cesium.Quaternion()) // ★
            czml.orientation.unitQuaternion[1] = quat.x;
            czml.orientation.unitQuaternion[2] = quat.y;
            czml.orientation.unitQuaternion[3] = quat.z;
            czml.orientation.unitQuaternion[4] = quat.w;

★を新たに付与しました。

★ ENUとNED

前回の"座標系とCesiumJS"の記事で説明しましたが、ENUフレームとNEDフレームの考慮がこのロジックに不足しています。
当該座標(測地座標の緯度、経度、高度)におけるグローバルな回転を★で計算していますが、ENUフレームを利用しています。しかし、MAVSDKから取得できるテレメトリーがNEDフレームのため、フレームが合っていません。その辺の考慮がないため、3Dモデルの表示が変なのです。

解決策としては、原点のフレームをCesium.Transforms.northEastDownToFixedFrameを使用してNEDフレームとして計算するか、NED→ENUのフレーム変換をしてあげる必要があります。
ただ、前回の記事で書きましたが、NEDフレームは3Dモデルの表示に影響を与えます。そのため、CesiumJSでの表示ではENUフレームで統一しておいたほうが、色々問題が少なそうなので、"表示のために"ENUフレームに変換する方式を取りましょう。

フレーム変換についても、前回の記事で紹介しました。
ENUとNEDは、xとyが逆となり、zが正負逆になり、90度回転させることで、変換出来ます。

https://stackoverflow.com/questions/53169107/quaternion-frame-conversions-ned-enu-eun

そのため、mavsdk_tryにもそのロジックを追加し、フレーム変換します。
上記、もろもろの考慮を追加した、新しいロジックは以下となります。

// mavsdk_cesium/static/App.js #43-69
        // 地球固定座標での回転を計算
        var pos = Cesium.Cartesian3.fromDegrees(
            telemetry.position.cartographicDegrees[1],
            telemetry.position.cartographicDegrees[2],
            telemetry.position.cartographicDegrees[3]);
        var mtx4 = Cesium.Transforms.eastNorthUpToFixedFrame(pos);
        var mtx3 = Cesium.Matrix4.getMatrix3(mtx4, new Cesium.Matrix3());
        var base = Cesium.Quaternion.fromRotationMatrix(mtx3);
        // ローカル座標での回転を計算(NED→ENU)
        var quatlocal = new Cesium.Quaternion(
            telemetry.orientation.unitQuaternion[2], // ★
            telemetry.orientation.unitQuaternion[1], // ★
            -telemetry.orientation.unitQuaternion[3], // ★
            telemetry.orientation.unitQuaternion[4]);
        var quat90 = Cesium.Quaternion.fromAxisAngle(
            new Cesium.Cartesian3(0, 0, 1),
            Cesium.Math.toRadians(90)
        );
        var quatlocal = Cesium.Quaternion.multiply(quatlocal, quat90, new Cesium.Quaternion()); // ★
        // 回転を掛け合わせる
        var quat = Cesium.Quaternion.multiply(base, quatlocal, new Cesium.Quaternion());

★の辺りが、x、y、zの軸の入れ替えと90度の回転を行っている箇所です。
x、y、zの軸の入れ替えは、クォータニオンの要素を入れ替えるだけなので、非常に簡単ですね。90度の回転も、z軸に対する90度の回転を行ったクォータニオンを生成して、ローカルなクォータニオンと掛けることで、実現できます。

以下に、上記修正を加えたmavsdk_cesiumというリポジトリーをGithubに公開しました。

github.com

最後に

比較動画を撮りました。
左が改修後、右が改修前です。
明らかにドローンの3Dモデルの姿勢が変だったのがわかると思います。

streamable.com

※後半、動画がカクカクしてますが、キャプチャソフトのせいなので無視してください。

◆Previous Part
tomofiles.hatenablog.com