ちょっと脱線します

前回のブログでは、C言語で作成した箱庭のドローン物理モデルと制御モデルを連携させてシミュレーションするための仕組みを説明しました。

本来であれば、今回からPX4との連携方法について説明していく予定だったのですが、ちょっと脱線したいと思います。

脱線したいと思ったキッカケ

実は、ドローン・シミュレータの開発、2023年10月から始めております。

当初は、１ヶ月くらいでプロトタイプ版ができると思っていたのですが、すでに２ヶ月過ぎてしまいました。

『すみません、ドローン・シミュレータ開発、完全に甘く見ていました。(> <)』

開発に時間がかかっている原因は、PX4とやり取りする通信データの仕様がわからないことに尽きます。

これまで、さまざまなパターンのデータを試しているのですが、何が正解かわからず、デバッグの繰り返しが延々と続いていました。当然、この作業を通して、ドローンの物理運動方程式やセンサー/アクチュエータ情報の理解は進んではいますが、ちょっとキリがないので、作戦を変えることにしました。

その作戦とは、以下のものです。

PX4の通信データ仕様を仮設で良いので、全てを明らかにする（単位や座標系など）
上記仕様に基づいて、プロト実装を行い、うまくいかないことを確認する
うまくいかないことを素直にPX4コミュニティに問い合わせする

この作戦を思いついたのは、正直、苦し紛れなのですが・・・、案外、良いアイディアだなと気づきました。

箱庭ラボはオープンソース・コミュニティとの連携も目指しています。

なので、この作戦が成功した暁には、「PX4コミュニティと繋がるきっかけになる」し、それでいて「問題の解決もできちゃう」ので、一石二鳥の効果が期待できるです。

当然、準備に時間はかかりますが、腰を据えてやっていこうという気持ちに切り替わりました（転んでもタダでは起きません）。

PX4の通信データの仕様とアーキテクチャ整理

まず、通信データの仕様を整理するために、AirSim と PX4 との間の通信データのやり取りをハッキングして、データ解析しました。その結果、制御に関係するものは、以下の３つのMAVLinkメッセージのみであることがわかりました。

HIL_SENSOR
HIL_GPS
HIL_ACTUATOR_CONTROLS

詳細な調査結果はこちらです。そして、この通信データと物理モデルとの間のデータ変換のアーキテクチャを検討しました（下図）。

詳細は、以下のURLで全て公開しています（現時点では、仮説です。また、PX4コミュニティとの対話に向けて全て英語です）。

https://github.com/toppers/hakoniwa-px4sim/tree/main/docs/phys_specs

まず、左側にPX4があります。PX4は、HIL_SENSORとHIL_GPSのデータからドローンの物理モデルの制御を行い、HIL_ACTUATOR_CONTROLSを出力します。物理モデル側では、HIL_ACTUATOR_CONTROLSのcontrolsのデータを４個受けます。それぞれ、ロータのPWMのデューティ比です。

この値をドローンに対する推力とトルクに変換し、物理運動方程式を使って、ドローンの位置・姿勢等を出力します。

ただし、これらの値は、そのままPX4に入力するわけにはいかないのです。HIL_SENSOR、HIL_GPSのデータ仕様への変換が必要となります。MAVLinkのメッセージ仕様については公式サイトを参照すればある程度は情報があるのですが、少し抽象度が高い記載があるところもあり、PX4向けに具体的にどのような値を設定すべきか悩むところが多いのです。

そこで、ある程度、コンポーネント分割（GPSやGyroなど）して、コンポーネント毎にデータの入出力仕様を明らかにして、データ変換方法を定義しました（下図）。リンク先に全ての仕様が記載されています。

とはいえ、まだまだ曖昧な記載があったり、検討の余地が残っていますが、少しずつ整理していきたいと思っています。ちなみに、現時点では、DroneDynamicsの出力は地上座標系になっていますが、機体座標系にした方がすっきりすると思われ、今後、修正していく予定です。

箱庭のメリットを活かした開発方法を考えました

上記で整理した物理モデルのプロトタイプ実装は容易ですが、悩ましいのはデバッグ方法です。

これまでエイヤで全部実装した後に、PX4と接続して、機体の動きをUnityで確認するという方法で動作チェックしていました（ビッグバンテストです）。しかし、このやり方は不味かったです。さまざまなデータが複雑に絡む今回のようなケースでは、どこが正しくて、どこが間違っているのか検討するのがすごく難しいのです。

そこで、今回のプロトタイプ実装では、仕様の整理が終わったものから順番に、その仕様通り動作しているかどうかチェックして積み上げていく方式を取ることにしました。特に、ドローンの開発の場合は、時系列データをグラフ化してチェックすることはとても重要だとわかりましたので、そのための環境整備が重要だと気づきました。

そこで、その環境づくりとして、箱庭の機能拡張をやってやろという思いに駆られました（転んでもタダでは起きません）。

すなわち、各コンポーネント間の入出力データを箱庭のPDUデータとして外出しして、Pythonプログラムで解析できるようにするのです（下図）。