Upgrade to Pro — share decks privately, control downloads, hide ads and more …

Math, Physics, and Dynamics of Drone in Hakoniwa

Math, Physics, and Dynamics of Drone in Hakoniwa

箱庭ラボで開発している,ドローンシミュレータの数学・物理・動力学の解説資料です.

詳しくはこちらです.
https://github.com/toppers/hakoniwa-px4sim/blob/main/drone_physics/README-ja.md

プロジェクトの全体像(箱庭ドローンシミュレータ)はこちら.
https://github.com/toppers/hakoniwa-px4sim/blob/main/README-ja.md

Kenji Hiranabe

February 12, 2024
Tweet

More Decks by Kenji Hiranabe

Other Decks in Programming

Transcript

  1. 本⽇のお品書き • 座標系の話(数学) • 運動⽅程式の話(物理) • 機体の物理モデル全体 • プラントとコントローラの話(制御) •

    ソフトウェア設計の話 • その他雑感など... 詳しい説明はこちらから: https://github.com/toppers/hakoniwa-px4sim/blob/main/drone_physics/README-ja.md
  2. Ground Frame 地上座標 x (front) (right) y z (down) Body

    Frame 機体座標 θ: theta(pitch) φ: phi(roll) xe ye ze 回転⽅向はすべて軸+に対して右ネジ 地上枠を機体枠へ,ψ, θ, φの順に回して重ねる=姿勢 ψ: psi(yaw) θ φ v=(u, v, w) ω=(p, q, r) 速度 ⾓速度 原点を Body Frame の 原点に重ねる("Vehicle Carried NED") (north) (east) (down) NED FRD O=Oe Oe
  3. Ground Frame ニュートンの運動⽅程式 オイラーの運動⽅程式(このまま解くのは複雑) Body Frame 位置が⼊っていないので, 原点を⼀致させて考える. (Vehcle Carried

    NED) 並進の慣性⼒は扱わない コリオリの⼒. 遠⼼⼒は重⼼に原点を取って いるので発⽣しない. ジャイロ効果 運動⽅程式
  4. ∫dt (ue , ve , we ) (φ', θ', ψ')

    地上座標での速度と姿勢変化 そのために.. 最終⽬標 (x, y, z) (φ, θ, ψ) 地上座標での機体の位置と姿勢 プラントのモデル ∫dt 機体座標での加速度と⾓加速度 (u', v', w') (p', q', r') そのために.. T τ 4ロータによる推⼒とトルク コントロール (c1 , c2 , c3 , c4 ) dynamics transformation そのために.. 機体座標での速度と⾓速度 (u, v, w ) (p, q, r ) Note: 4⼊⼒6⾃由度なので,全部⾃由にはならない. 例:ヨー⾓は⾼度を下げずに変えられるが,ピッチ⾓ を変えると⾼度が⼀旦下がる.
  5. ⾶⾏計画と安定⾶⾏制御 位置 姿勢 位置 姿勢 加速度,位置,姿勢,軌跡 gyro, mag, acc, vel,

    GPS コントロール Commander Navigator (c1 , c2 , c3 , c4 ) Position/ Attitude Controller ここへ⾏って! ここにいます ⾶⾏計画(外ループ) 安定⾶⾏制御(内ループ)
  6. ソフトウェア的な話 • ⼀⽂字の間違い(+/- とか sin/cos とか)でコンパイルエラーなしに動作時バ グとなるので「レビュー容易性」「テスト容易性」が肝. 1. 数式とコード 数式が命.コード上,もっとも⾒やすく,レビューしやすくした.

    2. クラスでなく関数の集合 テスト容易性のため,このレベルでは状態を持ちたくなかった. 3. 単体テスト 本体と切り離して単体テストできる環境を最初から作り,内部assert,外部 assert(ユ ニットテスト)しまくった. 4. アルゴリズムの突合 機体と地上,別のアルゴリズムで計算できる場合,両⽅作って答え合わせをした.
  7. #include <iostream> #include "drone_physics.hpp" int main() { // このライブラリの名前空間 using

    namespace hako::drone_physics; // 機体座標系を Euler ⾓で指定 VectorType frame = {0, 0, M_PI/2}; VelocityType body_velocity = {100, 200, 300}; // 機体座標系から地上座標系への速度変換 VelocityType ground_velocity = ground_vector_from_body(body_velocity, frame); // x,y,z 座標を取り出す auto [u, v, w] = ground_velocity; std::cout << "u = " << u << ", v = " << v << ", w = " << w << std::endl; // output: u = 200, v = -100, w = 300 // 逆変換して戻す VelocityType body_velocity2 = body_vector_from_ground( {u, v, w}, {0, 0, M_PI/2}); auto [u2, v2, w2] = body_velocity2; std::cout << "u2 = " << u2 << ", v2 = " << v2 << ", w2 = " << w2 << std::endl; // output: u2 = 100, v2 = 200, w2 = 300, back again. } Hello World(C++版)
  8. 1. ユニットテスト drone_physics % ./utest -------start unit test------- test_frame_all_unit_vectors_with_angle0... PASS

    test_frame_all_unit_vectors_with_some_angles... PASS test_frame_matrix_is_unitary... PASS ... -------all standard test PASSSED!!---- test_issue_89_yaw_angle_bug... PASS ... -------all bug issue test PASSSED!!---- All(4667)asserts passed. 実数が⼊⼒なので,網を掛けて assert ⽂ 70,1回のbuildで 5,000 くらい,それでも安⼼できる. バグがでると再現テスト追加 これは,issue 89 のヨー⾓のバグ 実装レベルの確認テスト
  9. 2. 数式とコード(1/2) /* acceleration in body frame based on mV'+

    w x mV = F ... eq.(1.136),(2.31)*/ AccelerationType acceleration_in_body_frame( const VelocityType& body_velocity, const EulerType& angle, const AngularVelocityType& body_angular_velocity, double thrust, double mass /* 0 is not allowed */, double gravity, /* usually 9.8 > 0*/ double drag, /* air friction of 1-st order(-d1*v) counter to velocity */ { assert(!is_zero(mass)); using std::sin; using std::cos; const auto c_phi = cos(angle.phi), s_phi = sin(angle.phi), c_theta = cos(angle.theta), s_theta = sin(angle.theta); const auto [u, v, w] = body_velocity; const auto [p, q, r] = body_angular_velocity; const auto T = thrust; const auto m = mass; const auto g = gravity; //... 引数を数式で使われている 変数⽂字に全部バラす 内部でも assert かけまくる 引数は,型名と合わせ技で 意味を伝える名前 (ソフトウェアエンジニア向け)
  10. 2. 数式とコード /* * See nonami's book eq.(1.136).(2.31) * Colioris's

    force is (p, q, r) x (u, v, w). * * Difference with the 'ground' version is that * (1) 'g' is broken down to x, y, z components. * (2) T is only relavant to z-axis. * (3) Coriolis force(using uvw,pqr) IS needed(because the body frame is rotating!) */ /*****************************************************************/ double dot_u = - g * s_theta - (q*w - r*v) - d/m * u; double dot_v = + g * c_theta * s_phi - (r*u - p*w) - d/m * v; double dot_w = -T/m + g * c_theta * c_phi - (p*v - q*u) - d/m * w; /*****************************************************************/ return {dot_u, dot_v, dot_w}; } 教科書の式番号 最も⽬レビューしやすいように できるだけ数式そのまま. (制御エンジニア向け)
  11. その他感想など • グラフ化はLLM 活⽤で open interpreter と会話してグラフ化 • コードもドキュメンントも Copilot

    で⽣産性爆上がり(体感3倍) ドキュメントは(どんどん量は書けるが)冗⻑になりがち注意. • アルゴリズムよりも機体パラメータ合わせ重要!!! そもそも物理的に制御できない,⼤きさやイナーシャとトルクのバランスなど なかなか安定して⾶ばずに⼀番苦労した 森さんが,パラメータ探索のためのコードを書いた • C⾔語 I/F もあります. • MATLAB 実装と差し替え可能. • README.ja
  12. MATLAB インターフェイス Thanks ! > 三浦功也 @MathWorks 😁 ⼊⼒構造体の型 出⼒構造体の型

    typedef struct mi_drone_acceleration_in_t { double phi; /* euler angle x */ double theta; /* euler angle y */ double psi; /* euler angle z */ double u; /* velocity x */ double v; /* velocity y */ double w; /* velocity z */ double p; /* anglular velocity x */ double q; /* anglular velocity y */ double r; /* anglular velocity z */ /* force and torque */ double thrust; double torque_x; double torque_y; double torque_z; /* other constants */ double mass; double Ixx; double Iyy; double Izz; double gravity; double drag; } mi_drone_acceleration_in_t; typedef struct mi_drone_acceleration_out_t { double du; /* dot u = dotdot x = x - acceleration */ double dv; /* dot v = dotdot y = y - acceleration */ double dw; /* dot w = dotdot z = z - acceleration */ double dp; /* dot p = x coordinate of angular vector acceleration */ double dq; /* dot q = y coordinate of angular vector acceleration */ double dr; /* dot r = z coordinate of angular vector acceleration */ } mi_drone_acceleration_out_t; 実装
  13. 参考⽂献 • ドローン⼯学⼊⾨(野波健蔵博⼠) • ⼩型無⼈航空機の厳密・簡易なモデリングとモデルベース制御(野波健蔵博⼠) • Drone control Lecture(Seongheon Lee)

    • 剛体の回転運動を⽀配するオイラーの運動⽅程式(スカイ技術研究所ブログ) • オイラー⾓とは?定義と性質、回転⾏列・⾓速度ベクトルとの関係(スカイ技術研 究所ブログ) • ⾶⾏⼒学における機体座標系の定義(@mtk_birdman) • 「マルチコプタの運動と制御」基礎のきそ(伊藤恒平) • 線形代数の可視化(@kenjihiranabe) • Euler Angles and the Euler Rotation sequence(Christopher Lum), [YouTube]