現状のPython APIで駆動するフライト制御をもっとキビキビ動くようにしたい

[tmori]

課題

水平移動が極めて遅い。目的地に到着するのに１００秒程度かかってしまう。少なくとも１０倍程度にしたい。

目指したいこと

水平移動時のプラントモデルの伝達関数をある程度同定した形で、過渡特性のパラメータをベースにして、理論的にパラメータを求めたい。

参考情報

• 制御工学同演習第十回　安定性２，制御系の過渡特性１
• 制御工学同演習第十一回　制御系の過渡特性２，定常特性１
• 制御工学同演習第十ニ回　定常特性２，制御系設計仕様，制御系設計法

[tmori]

計測結果

高さの推移

テイクオフ：

takeoff指示から、１秒程度で目標の高さでホバリングしているので、ここは問題ない。

着地：

着地は、１０秒くらいかかっている。若干遅い。 ただ、水平移動に比べたら、そんなに気にならないので、一旦、放置する。

[tmori]

計測結果

水平移動の推移

• 目標地点(X=5、Y＝０)に到達するのに約70秒かかっている。
• オーバーシュートが1mを超えている
• 立ち上がりに10秒かかっている
• オーバーシュートしてから位置補正に60秒もかかっている。。

[tmori]

プラントモデル

$$ \begin{array}{llll} \dot{u_e} = &-\frac{T}{m}(\cos{\phi}\sin{\theta}\cos{\psi} + \sin{\psi}\sin{\phi}) & &- \frac{d}{m}u_e \ \dot{v_e} = &-\frac{T}{m}(\cos{\phi}\sin{\theta}\sin{\phi} - \sin{\phi}\cos{\psi}) & &-\frac{d}{m}v_e \ \dot{w_e} = &-\frac{T}{m}(\sin{\phi}\cos{\theta}) &+g &-\frac{d}{m}w_e \end{array} $$

[tmori]

ひとまず、前進の水平移動のみを想定したいので、 $\dot{u_e}$ のみに着目。 $\phi$ = 0°、 $\theta$ = $\omega t$ ° 、 $\psi$ = 0° とおく。

[tmori]

代入して、速度を位置の表現にすると：

$\ddot{x} = -\frac{u(t)}{m}\sin{\omega t} - \frac{d}{m}\dot{x}$

ラプラス変換しやすいようにすると：

$\ddot{x} = -\frac{u(t)}{m} \sin{\omega t} u_s(t) - \frac{d}{m}\dot{x}$

[tmori]

ラプラス変換したもの

$s^{2}X(s) = -\frac{U(s)}{m} \frac{\omega}{s^{2}+\omega^{2}} - \frac{d}{m}sX(s)$

[tmori]

$U(s)$ は、推力だけど、現状、垂直方向のPID制御でしかコントロールできてない。 水平移動時にも推力をコントロールできるようにしなければならないのではないか。

https://github.com/toppers/hakoniwa-px4sim/issues/169

[tmori]

• θ=wtではなくて、θのままにしないといけない。
• θについては、オイラー角の変化率から決める。でも、難しそう。（p, q, rの運動方程式から考える必要あり）

[kenjihiranabe]

ここに、すごいいい記事みつけました。PX4の位置制御の方法です。 https://qiita.com/daisuke-iwakura/items/a9257b3f0f75659c3d09

[kenjihiranabe]

今の森さん側の作りを変えずに、ざっくり要点を言うと、

• 位置目標だけでなく、速度目標を持ち、速度をフィードバック制御。
• 推力は、上下移動したくないので、上向き成分を重力との釣り合い値T0とすると、T0 = Tcos(θ) となるように Tとθ を制御。残りの成分 Tsin(θ) を水平移動に使う。

[tmori]

入力のインタフェースをラジコンにする方法もあり。

[tmori]

アーキテクチャ：

[tmori]

[tmori]

[tmori]

[tmori]

作業タスク出し：

• [x] 機体プロキシのインタフェース設計 [4/21]
• [x] 機体プロキシのPDUデータ仕様定義 [4/22]
• [x] 機体プロキシのPDUデータ作成 [4/22]
• [x] 機体プロキシの再設計 [4/22]
• [x] 機体プロキシの作成 [4/23]
• [x] 外部制御：姿勢制御作成 [4/23]
• [x] 外部制御：角速度制御作成 [4/23]
• [x] 外部制御：スロットルゲイン作成 [4/23]
• [x] Python側：PS4コントローラ機能作成 [4/23]
• [x] Python側：ラジコンコントローラ用Python API 作成 [4/23]
• [x] 結合テスト（Mixerなし） [4/23]
• [ ] Mixer作成 [ペンディング]
• [ ] 結合テスト（Mixerあり） [ペンディング]
• [x] 全体設計 [4/24]
• [x] 高度制御器 [4/25]
• [x] 速度制御器(x) [4/25]
• [x] 速度制御器(y) [4/25]
• [x] 位置制御 [4/26]
• [x] 方位制御器 [4/26]
• [x] ウェイポイントシーケンサー [4/28]
• [ ] 遅延機能の追加 [4/29]
• [x] ラジコン目標値設定機能作成
• [x] スロットル自動補正対応
• [x] 荷物の着脱APIの追加
• [x] カメラ撮影APIの追加
• [ ] bluetooth対応

[tmori]

この辺りのクラス設計を見直したい。

[tmori]

リファクタリングの方向性、必要性を考える

まずは現行のクラスの役割を一言で言えるか

• DroneControlProxyManager
  • 以下を一括して実行する
  • 箱庭のシミュレーション制御
  • Python側からの要求に応える
  • ドローンのシミュレーションを実行
• DroneControlProxy
  • Python側からの要求に応える
• AircraftSystemTaskManager → AirCraftModuleSimulator
  • 箱庭のシミュレーション制御
  • 外部モジュール管理
  • DONE:外部モジュールのcontextをインクルードしているのはなんとかしたい
• AirCraftManager
  • 複数機体の管理
• AircraftSystemContainer　→ AirCraftModule
  • １個の機体の管理

[tmori]

Mixerの仕様検討

• 野波先生の教科書の 式 (2.63) をベースにして、Ωを計算して、そこからDを求める
• ただし、Jrの項の計算は厄介なので、省略する
• 平鍋ちょこっと修正（9/4/2024)

基本式

• U = (T, τφ, τθ, τψ)
• (Ω^2) = (Ω1^2, Ω2^2, Ω3^2, Ω4^2)
• Ti = A Ωi^2
• T = ΣTi
• τψi = BΩi^2
• τψ = Σ(ccwi) * τψi
• (τφ, τθ, 0) = Σ -(rxi, ryi, 0) x (0, 0, AΩi^2) = Σ -(ryi AΩi^2, rx AΩi^2, 0)
• U = A(Ω^2) ---> この連立一時方程式をΩ^2について解くことになる。しかし、Ωi^2 < 0 なる解が出てしまうことがある。線形化を試してみる。すなわち、ΔU = 2 A [Ω0*ΔΩ] を解く。これだと、ΔΩ　が負で出ても、Ω0からのプラスマイナスで不安定にならないと予想。
• Ωi(s) = (Kr/(sTr+1)) Di(s) ---> `つり合い時に、Ωi0 = Kr Di0 ... (1)となる。その周辺にて、ΔΩ =Kr ΔD ...(2)として求める。ただし、1次式なので、(1)+(2) で Ωi=KrDi と考えて良い。これは、「モーターがステップ応答に対して瞬時に目標値に達する」と仮定していることと同値（s=0 としている）。

[tmori]

行列での表現

※上記で Kr は省く（Ω->D計算の時に後で割る）。

• U = A M [Ω^2]
• 線形化して、ΔU = 2 A M [Ω0 ΔΩ] を解く。
• U = ΔU + U0 ... U0 はホバーもしくは低速飛行定常位置。（ホバーなら、U0 = [mg, 0, 0, 0]）
• Ω = ΔΩ + Ω0 ... Ω0 はホバーもしくは低速飛行定常位置。（ホバーなら、Ω0 = √(mg/NA) , A はロータの定数, Nはロータ数)

[tmori]

PWMの計算方法

(9/5/2024 平鍋修正)

[Ω^2] = A^-1 M^-1 U
から、Ωi^2 を求める。
Ωi^2の結果からΩiを求める。±√となるが、PWM値は0-1なので、プラスのみ使用する。
ここで、Ωi = Kr Di　なので、
Di = Ωi/Kr とする

上記方針だと、Ωi^2 < 0 なる解が出ることがある（逆回転トラストを発生させて、急激に回転させようとする）。そこで、線形化した解を用いて、ホバー時回転数の -50% を下限とするなどをトライ。

[Ωi0 ΔΩi] = (1/2) A^-1 M^-1 ΔU
から、ΔΩi を求める。
Ωi = Kr Di　の近似（一時遅れなしに直でステップ応答する近似）を使い、Di = Ωi/Kr とする

[tmori]

上位制御部の設計

３段階に分けて実装する。

1. 高度制御、速度制御を実装して、ラジコン操作で期待した位置、速度で移動することを確認する
2. 位置制御、方位制御を実装して、Python API で、期待した位置へ移動することを確認する
3. ウェイポイントシーケンサを実装して、Python API で、期待した位置へ移動することを確認する

[tmori]

Mixerはまだ未完成ではありますが、今後はこちらで管理します。

https://github.com/toppers/hakoniwa-px4sim/issues/205

[kenjihiranabe]

https://github.com/toppers/hakoniwa-px4sim/issues/178#issuecomment-2073704541

PWMの計算方法

Ω = A^-1 M^-1 U から、Ωi^2 を求める。 Ωi^2の結果からΩiを求める。±√となるが、PWM値は0-1なので、プラスのみ使用する。

ここで、Ωi = Kr Di　なので、

Di = Ωi/Kr

となる

上記は、ラプラス変換での式、Di(s) = (Ktr/(1+s Tr)) * Ωi （式2.48)なので、線形化が必要。 再考中。