箱庭で作ったドローン機体を見てみよう！

今回から数回に分けて、今の箱庭ドローン・シミュレータについて説明したいと思います。

今回のテーマはドローンの機体です。

Unityで作ったドローンの機体

これ、箱庭のUnity環境で作ったドローンの機体です。

全部、Unityのプリミティブなオブジェクト（キューブ等）で作りました。

機体やプロペラの色もUnity標準のものを組み合わせて作りました。

ドローンを作るときの注意ポイントとして座標系がありますので、Unityの座標系を説明しておきます。

Unityの座標系は左手系です。そして、以下のようにドローンの機体と座標系を対応づけて３Dモデルを配置します。

ドローンが前進する方向が z 軸。
ドローンの右方向が x 軸。
ドローンの上昇方向が y 軸。

ドローン中心部から、等距離で４つのローターを配置しています。

今回のドローンでは、計算を単純化できるように、各ローターは座標系の軸方向に配置しています。

機体の物理モデル

実はこの機体、以下の論文を参考にさせていただきました。

小型無人航空機の厳密・簡易なモデリングとモデルベース制御

座標系は、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軸は位置が０で止まるようにしています）

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;