Hakonwa-Quaternion

2025/5/31 箱庭祭り at 福井
箱庭Droneのプラント側実装の回転表現について。

Transcript

1. ドローンの 回転 Quaternion の話 平鍋健児 2025/5/31
4. ୋঊ্໺͞Μ
5. ドローンの構造(クアッドコプタ) 15 • クアッドコプタとは • ロータ４個で機体の推⼒とトルクを発⽣させて⾶⾏する • ロータとは • プロペラを回転させるためのモーター • ドローンの推⼒・トルクを発⽣させる • ロータの特性として、回転⽅向と最⼤回転数などがある • 今回の解説で使う機体 • 左右・前後を対象としています • ロータ１と２が逆回転（CCW） • ロータ３と４が正回転（CW)
6. ドローンの並進運動 • 垂直⽅向移動 • ４つのローターを同じ回転速度で回転させることで上昇⽅向の推⼒を発⽣させます推⼒ 推⼒＞質量✖重⼒︓上昇 推⼒＝質量✖重⼒︓ホバリング 推⼒＜質量✖重⼒︓下降 • 推⼒はローターの回転速度の２乗に⽐例します • ⽔平⽅向移動 • 左右⽅向 • 機体を左側に傾けると左⽅向へ移動します • 機体を右側に傾けると右⽅向へ移動します • 前後⽅向 • 機体を前側に傾けると前⽅向へ移動します • 機体を後側に傾けると後⽅向へ移動します 16
7. ドローンの回転運動 • Roll/Pitch/Yawの⽤語 17 • Roll/Pitch⽅向の回転 • 左右・前後の推⼒の差がトルクになる
8. ドローンの回転運動 18 • Yaw⽅向の回転 • ロータ回転⽅向と逆トルクが発⽣する • 反トルクの差がYaw⽅向の回転トルクを⽣む 反トルク発⽣の様⼦︓ ⾚⽮印の反トルクが⼤き いので、その⽅向に機体 が回転する
9. やばい、これ好き❤ 以降、詳しくはこちらを参照。 https://github.com/toppers/hakoniwa- px4sim/blob/main/drone_physics/README-ja.md
10. 座標変換 Ground Body Ground Body (⽅向余弦⾏列 DCM: Direction Cosine Matrix)
11. Ground Frame ニュートンの運動⽅程式 オイラーの運動⽅程式（このまま解くのは複雑） Body Frame コリオリの⼒． 遠⼼⼒は重⼼に原点を取って いるので発⽣しない． ジャイロ効果 運動⽅程式 位置が⼊っていないので， 原点を⼀致させて考える． 地上座標系は Vehicle Carried NED: (North-East-Down) として、並進の慣性⼒は扱わない
12. お世話になった教科書 • 書籍購⼊、勉強会 • 式だけでは、コーディングできない （分かっていないと単体テストできない） • 数式の体感理解が必要
13. お世話になったサイト • StampFly の こうへいさん • すごい詳細度とコード https://www.docswell.com/s/Kouhei_Ito/KDVNVK-2022-06-15-193343
14. 編み出した体感理解... 𝝍 psi ... z軸 𝜽 theta ... y軸 𝝓 roll ... x軸 とにかく体を傾けながら考える。微⼩変化時の影響（プラスマイナス） などは、これで体に叩き込む。変数記号も⼀緒に、 「体感⇦⇨コーディング」 を直結させる。
15. ∫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⾃由度なので，全部⾃由にはならない． 例1：ヨー⾓は⾼度を下げずに変えられるが，ピッチ⾓を変えると⾼度 が⼀旦下がる． 例2：斜め姿勢状態では静⽌できない。
17. #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++版)
18. 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 のヨー⾓のバグ 実装レベルの確認テスト
19. 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 かけまくる 引数は，型名と合わせ技で 意味を伝える名前 （ソフトウェアエンジニア向け）
20. 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}; } 教科書の式番号 最も⽬レビューしやすいように できるだけ数式そのまま． （制御エンジニア向け）
21. ドローンの 回転 Quaternion の話 ここから本題
23. Gimbal Lock（ジンバルロック）... ギンバル https://youtu.be/YwG4F-16Tno?si=jQ1K7X25A3QVGuCL&t=82
24. https://youtu.be/OmCzZ-D8Wdk?si=_eu0ATN6icMhp6nM Gimbal Lock and Apollo 13
25. https://www.nasa.gov/history/alsj/e-1344.htm nasa.gov の⽂書 • 許容ピッチ⾓は80度以上くらい。 • 4つ⽬のジンバルを追加する⼿法も検討された。 • Quarternion が採⽤されるのは、Apollo 計画終了後 Apollo1-17の後(1972)
27. 𝝍 psi ... z軸 𝜽 theta ... y軸 𝝓 phi ... x軸
28. https://youtu.be/zc8b2Jo7mno?si=vuuOrYMc4t8ASToX
29. Gimbal Lock • どんな直⾏座標系(x,y,z)でもよいが、肝は、 • 2番⽬の回転が90°になるとき、1番⽬の回転と3番⽬の回転が同じ⽅ 向になる（特異姿勢）。僕達のネーミングでは、θ=90°のとき • 特異姿勢は2つ異なるのオイラー⾓表現できる（⾃由度重複） • 特異姿勢において、オイラー⾓で偏微分すると、 ある⽅向には回転できなくなる。→制御できない • よくある間違い ❌オイラー⾓には表せない姿勢がある 🙆3次元の姿勢はすべてオイラー⾓で表せる。 ❌物理的に不連続である 🙆純粋に「数学的表現のクセ」 https://qiita.com/Arihi/items/4b306feb3d9e6cd93204
32. そこで、Quaternion(クオータニオン) • Hamilton が⾒つけた4元数（複素数の拡張） • 4つの実数からなる。1つの実部（q0)と3つの虚部（q1,q2,q3） 𝒒 = 𝑞! + 𝑞" 𝒊 + 𝑞# 𝒋 +𝑞$ 𝒌 𝒊# = 𝒋#= 𝒌# = −1 𝒊𝒋 = 𝒌, 𝒋𝒌 = 𝒊, 𝒌𝒊 = 𝒋 𝒊𝒋𝒌 = −𝟏
33. https://github.com/toppers/hakoniwa-px4sim/blob/main/drone_physics/README-ja.md
36. 🔥 🙆
37. 宙返りにチャレンジ
38. https://x.com/kouhei_kanazawa/status/1 896399825983467835
39. https://x.com/kanetugu2020/status/1897064961777844320 https://x.com/kouhei_kanazawa/status/1900016421192819060
41. 仲間と 夢を︕
42. この機会に線形代数， 学び直しませんか？ https://github.com/kenjihiranabe/The-Art-of-Linear-Algebra/blob/main/The-Art-of-Linear-Algebra-j.pdf ビジュアル線形代数 Yann LeCun 先⽣も retweet ❤