C言語で作ったドローン物理モデルをUnityでビジュアライズ＆制御する！TOPPERS/箱庭のドローン・シミュレーション構想と現在の開発状況

前回、[Unity＋Python＋箱庭で自作ドローンを動かしてみる！](https://qiita.com/kanetugu2018/items/cacbc0b3632e070fb777)では、Unityで作成したドローンをPythonで制御しました。

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/244147/566c84e3-1e88-0786-2f19-e290f9cb695f.png)

ただ、この方法にはいくつか課題があります。

* 物理モデルの精度
  * Unityの物理エンジンはシンプルなシミュレーションには適していますが、高度なドローンの飛行動作を再現するには限界があります。
* リアルタイム制御とパフォーマンス
  * Pythonは手軽に使える言語で、様々なライブラリを使ってスピーディに試作することがができますが、リアルタイム性や計算パフォーマンスの面で課題があります。特に複雑な計算が必要な場合、遅延やパフォーマンスの問題が生じる可能性があります。また、組み込みOS上でPythonプログラムを動かすことは至難の技だと言えます。

# 現在検討中のドローン・シミュレーション構成

そこで、こういう構成のドローン・シミュレータを検討してみました。

![スクリーンショット 2023-11-12 18.14.54.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/244147/c7ba5e19-d662-777a-9430-f72533fb4c18.png)

## ドローン物理モデル(C/C++)

ドローンの物理モデルは、数式をベースにオイラー法やルンゲクッタ法などで数値計算できるようにします。こうすることで、必要な精度のシミュレーションが可能になります。


## ドローン制御モデル(C/C++)

ドローンの制御モデルもC/C++言語で作成します。C/C++言語で作成しておけば、組み込みOSへの適用も容易になると思いますし、[PX4](https://docs.px4.io/main/en/)への適用も可能になります。

## MATLAB/Simulink

上記の物理モデルと制御モデルは、モデルベース開発として、MATLAB/Simulinkで作成することができます。

さらに、エンベデッドコーダーを使用すれば、物理モデルと制御モデルを、Cコードとして生成することができます。


## 外部環境/ビジュライズ(Unity)
Unity側では、ドローンの物理モデルが出力する「位置・姿勢情報」を入力してビジュアライズします。また、ドローンの外乱等のイベントはUnity上でシナリオ化して発生させます。

# 詳細アーキテクチャ

これらの内容をより具体的にアーキテクチャ検討したものがこちらです。

![スクリーンショット 2023-11-12 18.52.56.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/244147/83415a52-374d-dd94-ce46-5a08b2faf6aa.png)

中核となるのはドローン物理モデルとドローン制御モデルです。これらはC/C++で開発されます。

ドローン制御モデルは、PX4のAPIで開発されますので、箱庭のシミュレーション環境で動作チェックできたものを、そのまま Pixhawk上でデプロイし、動作確認ができる所が優れています。

物理モデルはドローンの動的な挙動を数値計算し、より精度の高いシミュレーションが可能となっています。

シミュレーションの世界では、PX4 は [SITL (Software In The Loop)](https://docs.px4.io/main/en/simulation/) 環境上で実行されます。

Unityはこのシステムのビジュアライゼーションと外部環境を担当しています。シミュレーションからの「位置・姿勢情報」はUnityに送られ、リアルタイムでの視覚フィードバックが得られます。また、Unity内で発生させた外乱やその他のイベントは、物理モデル側にフィードバックされます。

このアーキテクチャにより、開発からテスト、デプロイまでの一貫したフローが実現し、PX4フライトスタックを使用する開発者にとって、現実の条件下でのドローンの挙動を正確に評価することが可能になります。

# 現在の開発状況

[箱庭ラボ](https://hakoniwa-lab.net/)では、このドローン・シミュレータを、TOPPERS/箱庭ベースで、誰でも使えるように、オープンソースとして開発しています。

現時点で、以下の対応ができ、その成果を一般公開しました。

1. ドローン物理モデルをC言語で実装
1. ドローン制御モデルをC言語で実装
1. Unity上でのビジュアライズ
1. ドローン制御モデルを PX4 on SITL に組み込んで実行

## ドローン物理モデルをC言語で実装

ドローン物理モデルは、以下の数式をオイラー法でC言語にしました。

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/244147/50cedce0-07da-0a15-673c-b2eafd674dcc.png)

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/244147/61a79f46-6e45-5314-d3e1-bace4d19a69f.png)

コード断片：推力の計算

```c
double u = \
       phys.param.p * ( propeller.w[0] * propeller.w[0]) \
     + phys.param.p * ( propeller.w[1] * propeller.w[1]) \
     + phys.param.p * ( propeller.w[2] * propeller.w[2]) \
     + phys.param.p * ( propeller.w[3] * propeller.w[3]) \
     ;
```

コード断片：X軸の速度と位置の計算

```c
phys.next.vec.x = ( phys.delta_t /  phys.param.m ) * u 
                * 
                  ( 
                      cos(phys.current.rot.x)
                    * sin(phys.current.rot.y)
                    * cos(phys.current.rot.z)
                    +
                      sin(phys.current.rot.x)
                    * sin(phys.current.rot.z) 
                  ) 
                  + phys.current.vec.x;

phys.next.pos.x = (phys.current.vec.x * phys.delta_t) + phys.current.pos.x;
```

コード断片：Y軸の速度と位置の計算

```c
phys.next.vec.y = ( phys.delta_t /  phys.param.m ) * u 
                * 
                  ( 
                      cos(phys.current.rot.x) 
                    * sin(phys.current.rot.y) 
                    * sin(phys.current.rot.z) 
                    - 
                      sin(phys.current.rot.x) 
                    * cos(phys.current.rot.z) 
                  ) 
                  + phys.current.vec.y;

phys.next.pos.y = (phys.current.vec.y * phys.delta_t) + phys.current.pos.y;
```

コード断片：Z軸の速度と位置の計算（境界条件として、Z軸は位置が０で止まるようにしています）

```c
phys.next.vec.z = ( phys.delta_t /  phys.param.m ) * u 
                * 
                  ( 
                      cos(phys.current.rot.y) 
                    * cos(phys.current.rot.x) 
                  ) 
                  - (phys.param.gravity * phys.delta_t )
                  + phys.current.vec.z;

phys.next.pos.z = (phys.current.vec.z * phys.delta_t) + phys.current.pos.z;
/*
 * 境界条件：地面から下には落ちない
 */
if (phys.next.pos.z < 0) {
  phys.next.pos.z = 0;
  phys.next.vec.z = 0;
}
```

コード断片：X軸の角速度と角度φの計算

```c
double torque_phi = - phys.param.l * phys.param.p * propeller.w[1] * propeller.w[1]
                    + phys.param.l * phys.param.p * propeller.w[3] * propeller.w[3];
phys.next.rot_vec.x = torque_phi * phys.delta_t + phys.current.rot_vec.x;
phys.next.rot.x     = (phys.current.rot_vec.x * phys.delta_t) + phys.current.rot.x;
```

コード断片：Y軸の角速度と角度θの計算

```c
double torque_theta = - phys.param.l * phys.param.p * propeller.w[0] * propeller.w[0]
                      + phys.param.l * phys.param.p * propeller.w[2] * propeller.w[2];
phys.next.rot_vec.y = torque_theta * phys.delta_t + phys.current.rot_vec.y;
phys.next.rot.y     = (phys.current.rot_vec.y * phys.delta_t) + phys.current.rot.y;
```

コード断片：Z軸の角速度と角度ψの計算

```c
double torque_psi   = phys.param.k * propeller.w[0] * propeller.w[0]
                    - phys.param.k * propeller.w[1] * propeller.w[1]
                    + phys.param.k * propeller.w[2] * propeller.w[2]
                    - phys.param.k * propeller.w[3] * propeller.w[3];

phys.next.rot_vec.z = torque_psi * phys.delta_t + phys.current.rot_vec.z;
phys.next.rot.z     = (phys.current.rot_vec.z * phys.delta_t) + phys.current.rot.z;
```


## ドローン制御モデルをC言語で実装

制御モデルの方は、Z軸の位置と、ロール、ピッチ、ヨーの姿勢をPID制御しています。詳細は、箱庭ラボのブログで説明予定です。

## Unity上でのビジュアライズ

今回Unityで実現したドローンは、全部、Unityのプリミティブなオブジェクト（キューブ等）で作りました。機体やプロペラの色もUnity標準のものを組み合わせて作りました。

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/244147/b418558d-bd04-1cbd-6176-343d7a8a87b5.png)

ドローン中心部から、等距離で４つのローターを配置しています。今回のドローンでは、計算を単純化できるように、各ローターは座標系の軸方向に配置しています。

## ドローン制御モデルを PX4 on SITL に組み込んで実行

こちらをご参照ください。

[PX4と接続可能な箱庭ドローン・シミュレータのファーストリリースしました！](https://hakoniwa-lab.net/2023/12/29/news-03/)

## 公開中の github URL

以下で一般公開中です！

https://github.com/toppers/hakoniwa-px4sim

## 箱庭ラボの開発風景

これらの取り組み風景をもっと詳しく知りたい方は、以下をご覧ください。

* [箱庭ラボの開発風景](https://hakoniwa-lab.net/2023/10/16/blog-dev-scene/)
  * PX4 を箱庭に組み込むための悪戦苦闘ぶりが記録されています
  * この開発を通して、PX4やMAVLINKの理解が深まり、箱庭への適用が可能となりました
* [箱庭で作ったドローン機体を見てみよう！](https://hakoniwa-lab.net/2023/11/11/drone-01/)
  * ドローンの機体の物理モデルの説明およびUnityでのモデル化について詳しく説明しています
* [箱庭で作ったドローン機体の制御方法について考えてみよう](https://hakoniwa-lab.net/2023/11/12/drone-02/)
  * ドローンがどのような原理で動くのかを数式をベースに説明しています
* [ドローン・シミュレーションを実現する箱庭の内部設計を解説します](https://hakoniwa-lab.net/2023/11/26/drone-03/)
  * 箱庭のドローン・シミュレータの仕組みについて解説しています
* [ちょっと脱線します](https://hakoniwa-lab.net/2023/12/03/drone-04/)
  * PXとの連携方法について作戦練り直しました。どういう作戦か興味ある方ぜひご覧ください。
* [とうとう箱庭のPX4連携で、ドローン・ホバリングと周回飛行に成功しました！（第一段階の技術ハードルを突破）](https://hakoniwa-lab.net/2023/12/24/drone-05/)
* [PX4と接続可能な箱庭ドローン・シミュレータのファーストリリースしました！](https://hakoniwa-lab.net/2023/12/29/news-03/)
* [ニューヨーク上空をPS4コントローラで操縦！PX4連携可能な箱庭ドローンシミュレータの実力](https://qiita.com/kanetugu2018/items/11f9c90aa2711eba5f7f)
* [C言語で作成したドローン物理モデルをPID制御でサクサクっと動作確認してみよう](https://hakoniwa-lab.net/2024/01/13/drone-06/)
* [熟練Unityエンジニアから見た箱庭ドローンシミュレータの魅力と将来ビジョン](https://hakoniwa-lab.net/2024/01/19/drone-07/)
* [ドローン動力学の数学と実装](https://qiita.com/kenjihiranabe/items/498418ceb8a80f8242d0)
  * 最新のドローン動力学の解説が出ました！！
* [箱庭ドローン・シミュレータの機能アップデート版v1.1.0をリリースしました！](https://hakoniwa-lab.net/2024/01/26/news-04/)
* [箱庭チュートリアル会 #9 箱庭ドローンシミュレータを徹底解剖！する会](https://hakoniwa.connpass.com/event/308790/)
  * 箱庭ドローンシミュレータの内部構造を理解したい方には必見のイベントです！
* [生成AIコンテスト！AIが作成した制御プログラムを箱庭ドローンシミュレータで走らせて比較してみよう！！](https://qiita.com/kanetugu2018/items/4e05af77f0cc053ea907)

# デモ

最後に、デモ動画をお見せします。

画面左側が箱庭環境です。画面右側が、Unity環境です。
箱庭環境で箱庭を起動し、UnityのSTARTボタンでシミュレーション開始します。

https://twitter.com/kanetugu2020/status/1723639630434845011

シミュレーション開始すると、上昇の推力が与えられて、地上６mまで浮上してホバリングします。

* キーボードで、左に移動するコマンド（j）を発行すると、機体が左に傾いて、左に移動します。
* 逆に、キーボードで、右に移動するコマンド（l）を発行すると、機体が右に傾いて、右に移動します。

今度は、視点を少し変えて、前後方向です。

* キーボードで、前に移動するコマンド（i）を発行すると、機体が前に傾いて、前に移動します。
* キーボードで、後ろに移動するコマンド（m）を発行すると、機体が後ろに傾いて、後ろに移動します。

最後は、ヨー方向です。

* キーボードで、左回転するコマンド（f）を発行すると、機体が左回転します。（ちょっとオーバーな動きですが・・）
* キーボードで、右回転するコマンド（g）を発行すると、機体が右回転します。

これで、機体の動きを箱庭のシミュレーションで確認できました！