今回から数回に分けて、今の箱庭ドローン・シミュレータについて説明したいと思います。
今回のテーマはドローンの機体です。
Unityで作ったドローンの機体
これ、箱庭のUnity環境で作ったドローンの機体です。

全部、Unityのプリミティブなオブジェクト(キューブ等)で作りました。
機体やプロペラの色もUnity標準のものを組み合わせて作りました。
ドローンを作るときの注意ポイントとして座標系がありますので、Unityの座標系を説明しておきます。

Unityの座標系は左手系です。そして、以下のようにドローンの機体と座標系を対応づけて3Dモデルを配置します。
- ドローンが前進する方向が z 軸。
- ドローンの右方向が x 軸。
- ドローンの上昇方向が y 軸。
ドローン中心部から、等距離で4つのローターを配置しています。
今回のドローンでは、計算を単純化できるように、各ローターは座標系の軸方向に配置しています。
機体の物理モデル
実はこの機体、以下の論文を参考にさせていただきました。
座標系は、Unityとは違い、右手座標系です。

式9が、ローター回転数と機体の推力、トルク(ロール、ピッチ、ヨー)の関係式です。

式12が、機体の運動方程式です。

箱庭のシミュレーション環境では、Unityの物理エンジンを利用せずに、ドローンの運動方式を数値計算して、Unity上で再現する方式を採用しています。
これにより、より正確かつ制御された環境でドローンの挙動をシミュレートし、理論モデルと実際の動作の間の一致をテストすることができます。
なお、今回は、非常に単純化して、オイラー法で式12を解いて、C言語でプログラム化しています。
詳細を知りたい方は、こちらのコードをご参照ください。
https://github.com/toppers/hakoniwa-px4sim/blob/main/hakoniwa/src/hako/runner/drone/drone_phys.cpp
コード断片:推力の計算
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軸は位置が0で止まるようにしています)
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;
Unityと物理モデルを繋げるものは?
ここまでで、Unityとドローン物理モデルの説明は以上です。
さて、これらが揃うと何ができるでしょうか?
ドローンの物理モデルは、ローターの回転数を入力すると、位置と姿勢を出力してくれます。
一方で、Unity側は、位置と姿勢を入力すると、ドローンの機体をリアルに可視化してくれます。
しかしです。UnityのプログラムとC言語のプログラムをどうやって繋げれば良いでしょうか?
これらのプログラムは、それぞれ独立しており、互いに連携させることは実は難しいのです。
そこで登場するのが、「箱庭」というシミュレーション環境です。
箱庭はシミュレーションハブであり、Unityというシミュレータと物理モデルを数値計算するC言語のシミュレータを簡単に連携させることができるのです。

今日はここまでにしておきます。またお会いしましょう〜〜。
コメントを残す