ドローン・シミュレーションを実現する箱庭の内部設計を解説します

前回のブログでは、箱庭ドローン・シミュレータの概要を紹介しました。今回は、その中でも箱庭ファン必見？の内部設計に焦点を当ててみましょう。

ドローン制御モデルと物理モデルの連携

まず、箱庭ドローン・シミュレータの処理は下図のようになっています。

箱庭ドローン・シミュレータの重要な要素は、ドローンの制御モデルと物理モデルの連携です。ドローン制御モデルは、C言語の関数 drone_control_run() によって実装され、一方で物理モデルは drone_run() 関数で表現されています。そして、これらのモデルは my_task() というコールバック関数に統合され、シームレスな連携が実現されています。

また、ドローンの制御側と物理側は制御変数と観測変数で相互に連携しています。例えば、Z軸方向のドローンの位置制御を例にすると、下図のようなイメージになります。

今回の場合、ドローン制御モデル側では、目標位置として7.5mでホバリングするPID制御を実装しています。この際、ドローン物理モデル側から観測変数として、ドローンの位置Zを入力し、目標位置とのズレからPID計算し、ドローンのZ軸方向の推力計算を行いドローン制御モデル側にThrustとして出力するのです。

タイミングの重要性

シミュレーションのリアルな体験を提供するためには、タイミングが非常に重要です。ここで注目すべきは、hako_sim_runner() 関数が箱庭のアセットAPIの hako_asset_runner_step() を呼び出すことを繰り返すだけで、それによって my_task() がコールバックされる仕組みです。このタイミングは、箱庭のシミュレーション時間が進む制御周期に依存しており、今回は1msec毎に呼び出されるように設定されています。

ソースコードの一部

hako_sim_runner.cpp のhako_sim_runner()のコード断片は以下の通りで、この簡潔なコードが、箱庭ドローン・シミュレータの根幹を成す仕組みを示しています。

//コールバック関数の定義
static hako_asset_runner_callback_t my_callbacks = {
    my_setup,   // setup
    NULL,   // write_initial_pdu_data
    my_task,   // do_task
    my_reset    // reset
};
void *hako_sim_runner(void *argp)
{
    //コールバック関数を登録
    hako_asset_runner_register_callback(&my_callbacks);
    
    //箱庭アセットとしての初期化処理を実行
    hako_sim_control.arg = static_cast<HakoSimRunnerArgType*>(argp);
    if (hako_asset_runner_init(hako_sim_control.arg->asset_name, hako_sim_control.arg->config_path, hako_sim_control.arg->delta_time_msec * 1000) == false) {
        std::cerr << "ERROR: " << "hako_asset_runner_init() error" << std::endl;
        return nullptr;
    }
    while (true) {
        //シミュレーション時間をリセット
        hako_sim_control.asset_time = 0;
        std::cout << "INFO: start simulation" << std::endl;
        //シミュレーションループ
        while (true) {
            //シミュレーション時間を進める
            //箱庭側でシミュレーション時間を進めて良いという判断になった場合、
            //my_task()コールバック関数が呼び出される
            if (hako_asset_runner_step(1) == false) {
                std::cout << "INFO: stopped simulation" << std::endl;
                break;
            }
            else {
                //シミュレーション時間を進めることができた場合は、箱庭アセット側も時間を進める
                hako_sim_control.asset_time += HAKO_PX4_RUNNER_MASTER_DELTA_USEC;
                hako_asset_time = hako_sim_control.asset_time;
                usleep(hako_sim_control.arg->delta_time_msec * 200);
            }
            //std::cout << "STEP" << std::endl;
        }
        //リセット発生した場合は最初からやり直す。
    }
    std::cout << "INFO: end simulation" << std::endl;
    hako_asset_runner_fin();

    return nullptr;
}

また、my_task()側のコード断片は以下の通りです。

static void my_task()
{
    //ドローン物理モデルの処理を実行します
    drone_run(drone_propeller, drone_phys);

    //Unity側にドローンの位置と姿勢情報を通知します
    do_io_write();

    //ドローン制御モデルの処理を実行します
    drone_control_run(drone_ctrl);
    //制御モデルのアウトプット(drone_ctrl.signal)を
    //ドローン物理モデルの入力データ(drone_propeller)に変換します
    convert2RotationRate(drone_ctrl.signal, drone_phys, drone_propeller);
    return;
}

まとめ

内部設計において、ドローン制御モデルと物理モデルの連携、適切なタイミングでのコールバック、そしてシンプルかつ効率的なコード構造が、箱庭ドローン・シミュレータの優れた特徴になっています。

次の野望

ここまでの説明には、PX4は登場しませんでした（絶賛開発中なので・・）。次回以降では、PX4と箱庭の連携について説明したいと思います。乞うご期待ください！

PX4の補足説明

PX4の主な特徴は以下の通りで、大変優れたプラットフォームです。箱庭ドローン・シミュレータがPX4と繋がるようになれば、PX4ユーザにも箱庭を広げるチャンスになると考えております。

モジュール式のアーキテクチャ:
- PX4はモジュール式で設計されており、異なるセンサーやアクチュエータに容易に対応できます。これにより、ドローンの設計者やオペレーターは、様々な構成のドローンに対応する柔軟性を持っています。
広範なサポート:
- PX4は、多くのハードウェアプラットフォームや飛行制御システムでサポートされており、幅広い用途やプロジェクトに適用可能です。
コミュニティによる開発とサポート:
- このプロジェクトは活発なコミュニティによって支えられており、定期的にアップデートが行われ、新しい機能が追加されています（箱庭もこうなりたい）。
高度な機能:
- PX4には、自動離着陸、ミッションプランニング、GPSによるナビゲーションなど、先進的な飛行機能が含まれています。
シミュレーション環境の統合:
- PX4はシミュレーション環境との連携が可能で、実際の飛行前にドローンの動作を安全にテストすることができます。