前回、Unity＋Python＋箱庭で自作ドローンを動かしてみる！では、Unityで作成したドローンをPythonで制御しました。
ただ、この方法にはいくつか課題があります。
- 物理モデルの精度
- Unityの物理エンジンはシンプルなシミュレーションには適していますが、高度なドローンの飛行動作を再現するには限界があります。
- リアルタイム制御とパフォーマンス
- Pythonは手軽に使える言語で、様々なライブラリを使ってスピーディに試作することがができますが、リアルタイム性や計算パフォーマンスの面で課題があります。特に複雑な計算が必要な場合、遅延やパフォーマンスの問題が生じる可能性があります。また、組み込みOS上でPythonプログラムを動かすことは至難の技だと言えます。
現在検討中のドローン・シミュレーション構成
そこで、こういう構成のドローン・シミュレータを検討してみました。
ドローン物理モデル(C/C++)
ドローンの物理モデルは、数式をベースにオイラー法やルンゲクッタ法などで数値計算できるようにします。こうすることで、必要な精度のシミュレーションが可能になります。
ドローン制御モデル(C/C++)
ドローンの制御モデルもC/C++言語で作成します。C/C++言語で作成しておけば、組み込みOSへの適用も容易になると思いますし、PX4への適用も可能になります。
MATLAB/Simulink
上記の物理モデルと制御モデルは、モデルベース開発として、MATLAB/Simulinkで作成することができます。
さらに、エンベデッドコーダーを使用すれば、物理モデルと制御モデルを、Cコードとして生成することができます。
外部環境/ビジュライズ(Unity)
Unity側では、ドローンの物理モデルが出力する「位置・姿勢情報」を入力してビジュアライズします。また、ドローンの外乱等のイベントはUnity上でシナリオ化して発生させます。
詳細アーキテクチャ
これらの内容をより具体的にアーキテクチャ検討したものがこちらです。
中核となるのはドローン物理モデルとドローン制御モデルです。これらはC/C++で開発されます。
ドローン制御モデルは、PX4のAPIで開発されますので、箱庭のシミュレーション環境で動作チェックできたものを、そのまま Pixhawk上でデプロイし、動作確認ができる所が優れています。
物理モデルはドローンの動的な挙動を数値計算し、より精度の高いシミュレーションが可能となっています。
シミュレーションの世界では、PX4 は SITL (Software In The Loop) 環境上で実行されます。
Unityはこのシステムのビジュアライゼーションと外部環境を担当しています。シミュレーションからの「位置・姿勢情報」はUnityに送られ、リアルタイムでの視覚フィードバックが得られます。また、Unity内で発生させた外乱やその他のイベントは、物理モデル側にフィードバックされます。
このアーキテクチャにより、開発からテスト、デプロイまでの一貫したフローが実現し、PX4フライトスタックを使用する開発者にとって、現実の条件下でのドローンの挙動を正確に評価することが可能になります。
現在の開発状況
箱庭ラボでは、このドローン・シミュレータを、TOPPERS/箱庭ベースで、誰でも使えるように、オープンソースとして開発しています。
現時点で、以下の対応ができ、その成果を一般公開しました。
- ドローン物理モデルをC言語で実装
- ドローン制御モデルをC言語で実装
- Unity上でのビジュアライズ
- ドローン制御モデルを PX4 on SITL に組み込んで実行
ドローン物理モデルをC言語で実装
ドローン物理モデルは、以下の数式をオイラー法で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軸の速度と位置の計算
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軸の速度と位置の計算
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軸は位置が０で止まるようにしています）
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軸の角速度と角度φの計算
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軸の角速度と角度θの計算
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軸の角速度と角度ψの計算
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標準のものを組み合わせて作りました。
ドローン中心部から、等距離で４つのローターを配置しています。今回のドローンでは、計算を単純化できるように、各ローターは座標系の軸方向に配置しています。
ドローン制御モデルを PX4 on SITL に組み込んで実行
こちらをご参照ください。
PX4と接続可能な箱庭ドローン・シミュレータのファーストリリースしました！
公開中の github URL
以下で一般公開中です！
箱庭ラボの開発風景
これらの取り組み風景をもっと詳しく知りたい方は、以下をご覧ください。
-
箱庭ラボの開発風景
- PX4 を箱庭に組み込むための悪戦苦闘ぶりが記録されています
- この開発を通して、PX4やMAVLINKの理解が深まり、箱庭への適用が可能となりました
-
箱庭で作ったドローン機体を見てみよう！
- ドローンの機体の物理モデルの説明およびUnityでのモデル化について詳しく説明しています
-
箱庭で作ったドローン機体の制御方法について考えてみよう
- ドローンがどのような原理で動くのかを数式をベースに説明しています
-
ドローン・シミュレーションを実現する箱庭の内部設計を解説します
- 箱庭のドローン・シミュレータの仕組みについて解説しています
-
ちょっと脱線します
- PXとの連携方法について作戦練り直しました。どういう作戦か興味ある方ぜひご覧ください。
- とうとう箱庭のPX4連携で、ドローン・ホバリングと周回飛行に成功しました！（第一段階の技術ハードルを突破）
- PX4と接続可能な箱庭ドローン・シミュレータのファーストリリースしました！
- ニューヨーク上空をPS4コントローラで操縦！PX4連携可能な箱庭ドローンシミュレータの実力
- C言語で作成したドローン物理モデルをPID制御でサクサクっと動作確認してみよう
- 熟練Unityエンジニアから見た箱庭ドローンシミュレータの魅力と将来ビジョン
-
ドローン動力学の数学と実装
- 最新のドローン動力学の解説が出ました！！
- 箱庭ドローン・シミュレータの機能アップデート版v1.1.0をリリースしました！
-
箱庭チュートリアル会 #9 箱庭ドローンシミュレータを徹底解剖！する会
- 箱庭ドローンシミュレータの内部構造を理解したい方には必見のイベントです！
- 生成AIコンテスト！AIが作成した制御プログラムを箱庭ドローンシミュレータで走らせて比較してみよう！！
デモ
最後に、デモ動画をお見せします。
画面左側が箱庭環境です。画面右側が、Unity環境です。
箱庭環境で箱庭を起動し、UnityのSTARTボタンでシミュレーション開始します。
シミュレーション開始すると、上昇の推力が与えられて、地上６mまで浮上してホバリングします。
- キーボードで、左に移動するコマンド（j）を発行すると、機体が左に傾いて、左に移動します。
- 逆に、キーボードで、右に移動するコマンド（l）を発行すると、機体が右に傾いて、右に移動します。
今度は、視点を少し変えて、前後方向です。
- キーボードで、前に移動するコマンド（i）を発行すると、機体が前に傾いて、前に移動します。
- キーボードで、後ろに移動するコマンド（m）を発行すると、機体が後ろに傾いて、後ろに移動します。
最後は、ヨー方向です。
- キーボードで、左回転するコマンド（f）を発行すると、機体が左回転します。（ちょっとオーバーな動きですが・・）
- キーボードで、右回転するコマンド（g）を発行すると、機体が右回転します。
これで、機体の動きを箱庭のシミュレーションで確認できました！