プラント物理モデルを、制御のために線形化して行列で表現してみたい。 about hakoniwa-px4sim

[kenjihiranabe]

@tmori さん、第一バージョンとして、ホバー位置で線形化して、dx/dt = Ax +Bu の形で動く物理モデルを作りました。
インターフェイスは matlab i/f を使っています。ソースコードは１本のみ。

matlab で mi_drone_acceleration() 関数を呼び出している位置で、drone_acceleration_by_linear_at_hover() 関数を呼び出してください。並進と回転のごくごく簡単なテストをして、既存の物理モデルと大きな乖離がないかだけ確かめました。

CMakeList.txt も書き換えていますので、注意してください。本体とのリンク方法はよくわからなかったので、加えていません。
以下、コミット分です。
d6c16d2

[kenjihiranabe]

ありがとう！おどろくほど似てますね。びっくり。結局フィードバック制御さえしていれば、おおまかな線形化と変わらないんですね。

[kenjihiranabe]

$u,v,w$ は $0$ に近くないが、 $p,q,r$ を近似的に $0$ にすると上記は消える。強く回転していない、という仮定にすることは妥当なのか、判別がつかない。

[kenjihiranabe]

線形化するには、２つの方針がありそう。

1. cos θ =1, sin θ = θ （角度の傾きはほんのちょっと）、また、(p, q, r) = 0（ 回転していない）として x' = Ax + Bu を作る。これは、結果的にコリオリ力、ジャイロ効果を無視することになる。（おそらく金沢の赤坂先生はこの方法か？証拠：d(φ, θ, ψ)/dt と(p, q, r）の関係する行列Aの成分が、単位行列になっているので、少なくとも角度についてはその方針。）
2. 水平飛行定常状態(p=p0, q=q0, r=r0, θ=θ0...)からの差分Δp, Δq, Δr, Δθ、で立式する。結果としては、x=x0+Δx として計算出力することになる。
   https://www.docswell.com/s/Kouhei_Ito/KDVNVK-2022-06-15-193343#p4

本当は、2でやるのが正確。まずは、1と赤坂先生モデルでやって比較してみるか（これはすぐできる）。

[kenjihiranabe]

検討中。1 の場合、ほとんど何も加工しないで、thrust と torque を それぞれ w, (φ,θ, ψ) の時間微分に伝えるだけのシンプルなもでるになった。（しかし赤坂先生の行列とはずれている）
２の場合の導出を手で行なった。両方あっていると思われる。
ここに、計算シートを置いておく。
計算したシート

シート現物
https://docs.google.com/spreadsheets/d/1cSYqam0Fsw-jFBFTIRoHwVEYEov8a7Phhk8fHwuOqeA/edit?usp=sharing

計算過程

drone_linearization.pdf

[tmori]

ひとまず、こんな感じでリンクできるようにしました。

DRONE_SIMPLE_PHYSICSの定義を有効化してもらえればリンクされます。（デフォルトは従来のもの）

b04f4dd

[tmori]

ログを取りました。

• 従来のもの
  • drone_log_real.zip
• 線形モデルのもの
  • drone_log_simple.zip

[tmori]

Z軸

従来

線形モデル

[tmori]

ロール

従来

線形モデル

[tmori]

ピッチ

従来

線形モデル

[tmori]

ヨー

従来

線形モデル

[tmori]

全体的にほぼ同じですが、ロールがちょっと違いますね。