Hakoniwa, Mori-san, and me... Drone Simulation and Math

箱庭まつり #2 での発表

Transcript

1. 箱庭と 森さんと 私 箱庭祭り 2024/12/08 平鍋健児
2. 森さんと私 • 最初は、⾒守り • ちょっとほおっておきすぎかな、悩みもありそう • 週⼀回、対⾯の機会をもつことに...
3. 社⻑室での 最初の相談事項
4. ずっぽり巻き込まれた... • 式を⾒ながら、アドバイス。 • ちょっと式を変えたりして、触ってみた。 • 動かない... （⼀⽂字 sin/cos, ψ/θ/φ, +/- 間違っても全く⾶ばない）。 • これ、ぼくがやってもいいかな︖
5. やばい、これ好き❤
6. 座標変換 Ground Body Ground Body (⽅向余弦⾏列 DCM: Direction Cosine Matrix)
7. Ground Frame ニュートンの運動⽅程式 オイラーの運動⽅程式（このまま解くのは複雑） Body Frame 位置が⼊っていないので， 原点を⼀致させて考える． (Vehcle Carried NED) 並進の慣性⼒は扱わない コリオリの⼒． 遠⼼⼒は重⼼に原点を取って いるので発⽣しない． ジャイロ効果 運動⽅程式
8. 運動⽅程式 Body Frame ニュートンの運動⽅程式 オイラーの運動⽅程式
9. やってみるか... • 書籍購⼊、勉強会 • 式だけでは、コーディングできない （分かっていないと単体テストできない） • 数式の体感理解が必要
11. 編み出した体感理解... 𝝍 psi ... z軸 𝜽 theta ... y軸 𝝓 roll ... x軸 とにかく体を傾けながら考える。微⼩変化時の影響（プラスマイナス） などは、これで体に叩き込む。変数記号も⼀緒に、 「体感⇦⇨コーディング」 を直結させる。
12. ∫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⾃由度なので，全部⾃由にはならない． 例：ヨー⾓は⾼度を下げずに変えられるが，ピッチ⾓を変えると ⾼度が⼀旦下がる．
14. #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++版)
15. 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 のヨー⾓のバグ 実装レベルの確認テスト
16. 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 かけまくる 引数は，型名と合わせ技で 意味を伝える名前 （ソフトウェアエンジニア向け）
17. 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}; } 教科書の式番号 最も⽬レビューしやすいように できるだけ数式そのまま． （制御エンジニア向け）
18. 次は制御 • 書籍購⼊、勉強会 • 式だけでは、コーディングできない • 数学がすごく難（ラプラス変換から） • 数式とのシミュレーションの⼀致が必要
22. 理論と計算の突合は sympy, control 便利 https://colab.research.google.com/drive/15e1ReQwaLIpS8Avl65JudQ-kr-RbY1vy 物理記号を綺麗TeX表⽰ 実際の数値を代⼊しての 周波数応答も⼀発。 ω0 = 1/T 機体の時定数 10秒 ω0 = 1/T ロータの時定数 0.1秒
23. 発表してきた︕
24. ん︖また何か呼ばれた気がする... なんか数学的な命題としては⽢い。。。 うまく命題化できれば、証明できそう。
25. やっていることは、概念の定 義と記号化のみ。 その記号で更新規則を明確化 し、その場合の保存則として 数学的帰納法で証明する。
27. 命題が記号化できた瞬間に解けた︕ （⼤したことは⾔っていない。普通 の直感を形式化たしのみ。） 森さんの直感正しい︕
29. 最近もっとも苦労した話 この return 1⾏のコードを 書くのに⼀週間かかった。 バッテリ対応ローターの 微分⽅程式。 これによって電流計算で き、バッテリーの減りも モデル化できる。 https://github.com/toppers/hakoniwa-px4sim/issues/386
30. やりたいことの全体構成
31. こうへいさんの神資料：https://www.docswell.com/s/Kouhei_Ito/KDVNVK-2022-06-15-193343#p2
32. 写経して、現在のコンフィグパラメータとの対 応を調べる。 実パラメータ値を⼊れて、想定どおりか確かめ る。
33. バッテリーのモデル（読み込み元データ） バッテリーのモデル（実装時） バッテリーモデル側は森さんが製品デー タプロットを⼊⼒できるように改変
35. しかし、まだ1分しか⾶ばない︕ とりあえず先送り。容量がたくさんあると仮定して⾶ばしている。 この計算を確かめてくれる３⼈ ⽬を募集しています。 解けたらぼくが何かおごりま す。 https://docs.google.com/spreadsheets/d/1JACdNpwAYr-OUya5DmPa8r_1QfsI-4M4G-kJ_zLCvQ0/edit?gid=156552135#gid=156552135
36. 今⽇来る電⾞ で書いた コード E→Q 1つに決まる。 Q→E θ = 90°付近で複数の候補。 変換先が不連続の可能性。 Quaternion と Euler の 変換。 （まだコミットしてない。）
37. さっき通ったテスト これで、競技ドローンも⾶ばせるようになるでしょう︕ テストコードの⼀部
38. ぼくと森さん • 最初は、⾒守り • ちょっとほおっておきすぎかな、悩みもありそう • 週⼀回、対⾯の機会をもつことに... • どんどん巻き込まれていきました。うれしかったです。 • 最後に、始まる前のエピソード０。（社⻑としての仕事）
39. 最初の企画書
40. ロゴも完成︕
41. 森さんのモチベーションとパワー • 圧倒的コーディングパワー • 圧倒的アーキテクティングセンス • 協調的⼈柄と、引きつけ型巻き込み⼒ • ほっておいても、どんどん進む。 •夢を形に •形を夢に
42. 仲間と 夢を︕
43. この機会に線形代数， 学び直しませんか？ https://github.com/kenjihiranabe/The-Art-of-Linear-Algebra/blob/main/The-Art-of-Linear-Algebra-j.pdf