search

toppers / hakoniwa-px4sim

PX4-compatible drone simulation with physics-based modeling in C, visualizations via game engines, headless operation, and automated test scenarios. Supports external parameterization and MATLAB/Simulink integration.
40 stars 12 forks source link

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

Open tmori opened 5 months ago

tmori commented 5 months ago

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

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

制御側の式

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

設計仕様

参考： スクリーンショット 2024-06-11 9 30 05

kenjihiranabe commented 5 months ago

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

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

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

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

（今日はここまで）

箱庭-25 箱庭-26

tmori commented 5 months ago

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

tmori commented 5 months ago

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

tmori commented 5 months ago

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

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

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

スクリーンショット 2024-06-12 6 38 09
tmori commented 5 months ago

グラフ表示のコマンド

python3 python/hako_TimelineAnalyzer.py --columns  Z --diff ./drone_log3/drone_dynamics.csv
kenjihiranabe commented 5 months ago

そうなのよー。うまく整定する解は、定常偏差1m以内にはないみたい。

2024年6月12日(水) 6:33 Takashi Mori @.***>:

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

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 https://github.com/toppers/hakoniwa-px4sim/issues/231#issuecomment-2161634666, or unsubscribe https://github.com/notifications/unsubscribe-auth/AAILCJJCLVRPGGWEY6CMADDZG5ULTAVCNFSM6AAAAABJDINPBKVHI2DSMVQWIX3LMV43OSLTON2WKQ3PNVWWK3TUHMZDCNRRGYZTINRWGY . You are receiving this because you commented.Message ID: @.***>

tmori commented 5 months ago

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

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 commented 5 months ago

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

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 24 44

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

https://github.com/toppers/hakoniwa-px4sim/assets/164193/397d2862-afce-43ba-9366-6eeded19259d

kenjihiranabe commented 5 months ago

計算間違っていたので、貼り直します。 箱庭-25 箱庭-26 計算のワークシートも貼ります。 https://docs.google.com/spreadsheets/d/1JACdNpwAYr-OUya5DmPa8r_1QfsI-4M4G-kJ_zLCvQ0/edit?usp=sharing

tmori commented 5 months ago

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

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