古典制御理論を活用して、ドローンのテイクオフ→ホバリングまでをP制御できるようにする

[tmori]

ドローンのプラント側の運動方程式

$\ddot{z} = -\frac{u(t)}{m} + g - \frac{d}{m} \dot{z}$

• 座標系：NED
• z：ドローンの縦方向の位置（下方向がプラス、上方向がマイナス）
• u(t)：入力（後述の制御側の式を参照）
• 各パラメータの値：
  • $m = 0.1 kg$
  • $d = 0.0001$
  • $g = 9.81 m/s²$

制御側の式

$u(t) = K_p ( R - z(t) )$

• $R$ : 目標値
  • -10m
• $K_p$：比例ゲイン

設計仕様

• 速応性
  • 立ち上がり時間： $T_r$
    • 定常値(c)の10%から90%に達する時間：10秒以内(TODO)
  • 遅れ時間： $T_d$
    • 定常値(c)の50%に達する時間：5秒以内(TODO)
• 減衰性
  • 最大オーバーシュート量： $O_s$
    • 定常値を超えた最大値：1m以内
• 定常特性
  • 5%整定時間： $T_s$
    • 定常値（c）の±5%の範囲に落ち着く時間：20秒以内(TODO)
  • 定常値(c)：目標値の1%以内(±0.1m)

参考：

[kenjihiranabe]

ぼくの手計算を貼っておきます。基本的に、閉ループは2次系になり、2次系のステップ応答（3次系、1型）を解くことになります。（森さんの数式とは、 $z=-z$ で逆方向です。また、 $R=z_d$ と書いています。）

最初は、１つの設計定数で求まるかと思っていたのですが、勘違いで、いくつかの合わせ技不等式になりました。

1. $\omega_n$ の1変数方程式として定常偏差（定常特性）の式が使えたので、そこから、 $\omega_n$ を求め、
2. $\zeta$ の1変数方程式として、オーバーシュート量（減衰性）が使えたので、そこから、 $\zeta$ を求めた。

その結果、定常偏差を10%にするにはゲインを上げないといけないのですが、そうすると、 $d$ が小さいために、 $\zeta$ がオーバーシュト量をどうしても超えてしまうことが分かりました。

（今日はここまで）

[tmori]

ありがとうございます！
解析いただいたパラメータで動かせる環境を作ってみますね。

[tmori]

シミュレーション環境として準備するもの

• 設定パラメータを外部入力ファイルとして読み込みシミュレーション実行できる機能

• シミュレーション実行結果に対して、制御仕様通りであることを自動判定する機能

• シミュレーション結果をグラフ表示する機能

• 他に何かあれば適宜検討予定

[tmori]

Kp=10 を適用した時の結果

• 対象プログラム：
  • https://github.com/toppers/hakoniwa-px4sim/blob/main/drone_control/workspace/DroneAvator/hako_module_drone_controller_impl.cpp
• 実行結果：収束しない。。

空気摩擦係数を大きくした(d=0.02)結果

空気摩擦が小さすぎて、Kpだけでは収束しないという結果が得られました。

[tmori]

グラフ表示のコマンド

python3 python/hako_TimelineAnalyzer.py --columns  Z --diff ./drone_log3/drone_dynamics.csv

[kenjihiranabe]

そうなのよー。うまく整定する解は、定常偏差1m以内にはないみたい。 2024年6月12日(水) 6:33 Takashi Mori ***@***.***>:

…

Kp=10 を適用した時の結果 - 対象プログラム： - https://github.com/toppers/hakoniwa-px4sim/blob/main/drone_control/workspace/DroneAvator/hako_module_drone_controller_impl.cpp - 実行結果：収束しない。。 2024-06-12.6.32.49.png (view on web) <https://github.com/toppers/hakoniwa-px4sim/assets/164193/f4fcbb4a-0f65-4f03-bbff-ca8f3cc03ecf> — Reply to this email directly, view it on GitHub <#231 (comment)>, or unsubscribe <https://github.com/notifications/unsubscribe-auth/AAILCJJCLVRPGGWEY6CMADDZG5ULTAVCNFSM6AAAAABJDINPBKVHI2DSMVQWIX3LMV43OSLTON2WKQ3PNVWWK3TUHMZDCNRRGYZTINRWGY> . You are receiving this because you commented.Message ID: ***@***.***>

[tmori]

制御仕様の各データを出力するツール作りました

python3 python/control_evaluate.py ./drone_log3/drone_dynamics.csv

出力結果(例)：

OK c(Steady state value)  : 9.902 m (Target: 10.0±0.100 m)
OK T_r(Rise time)         : 0.102 s (Target: ≤ 10.000 s)
OK T_d(Delay time)        : 0.102 s (Target: ≤ 5.000 s)
NG O_s(Maximum overshoot) : 9.597 m (Target: ≤ 1.000 m)
NG T_s(5% settling time)  : 29.853 s (Target: ≤ 20.000 s)

[tmori]

パラメータチューニング結果

P制御だけでは制御仕様を満たせないことがわかったので、PID制御に切り替えて試行錯誤した結果です。

DroneAvator_Kp=5.5
DroneAvator_Ki=0.5
DroneAvator_Kd=4.5

結果：

OK c(Steady state value)  : 9.914 m (Target: 10.0±0.100 m)
OK T_r(Rise time)         : 0.042 s (Target: ≤ 10.000 s)
OK T_d(Delay time)        : 0.012 s (Target: ≤ 5.000 s)
OK O_s(Maximum overshoot) : 0.256 m (Target: ≤ 1.000 m)
OK T_s(5% settling time)  : 0.051 s (Target: ≤ 20.000 s)

グラフ：

一番右の機体が調整したもの

2024-06-12.9.25.49.mov

[kenjihiranabe]

計算間違っていたので、貼り直します。


計算のワークシートも貼ります。
https://docs.google.com/spreadsheets/d/1JACdNpwAYr-OUya5DmPa8r_1QfsI-4M4G-kJ_zLCvQ0/edit?usp=sharing

[tmori]

P制御だけでは両立しないというのが理論的にわかったのは面白い結果だと思いました。

ここからPD制御とPID制御へと移る動機づけになると思いますが、理論的に導出できなくなるところでもあるのでツライですね。。