[制御工学教材] PID制御パラメータの理論的導出方法の検討

[tmori]

タスクリスト

• 高度制御の理論的導出 #328
• 角速度制御の理論的導出
• 水平角度制御の理論的導出
• ヨー角制御の理論的導出
• 水平移動速度制御の理論的導出
• 水平位置制御の理論的導出

古典制御での特性メモ

（参照元：https://www.docswell.com/s/Kouhei_Ito/KDVNVK-2022-06-15-193343）

[tmori]

PID制御パラメータを決める手順

1. 位相余裕PMとゲイン交差周波数ωcを決める
   2.位相余裕PMの時のφmを計算する
2. 制御対象の周波数伝達関数を求める
3. 制御対象の周波数伝達関数からのωcの時の実部uと虚部vの値を求める
4. Kp計算式で比例ゲインを求める
5. 積分時間を適当に決めてTd計算式から微分時間を求める
6. 求めたゲインで以下を見る。
   • 周波数応答
   • ステップ応答（オーバーシュート量や整定時間などチェック）
   • インパルス応答（インパルス的な出力が出ないかチェック）
   • 外乱からの応答（外乱影響受けないかチェック）

[tmori]

伝達関数

開ループ伝達関数

$L(s)=C(s)P(s)$

閉ループ伝達関数

$W(s)=\frac{L(s)}{1 + L(s)}$

定常偏差と外乱

$y(s) = \frac{L(s)}{1 + L(s)} r(s) + \frac{P(s)}{1 + L(s)} d(s) $

$e(s) = r(s) - y(s)$

$y(s) を代入すると、$

$E(s) = \frac{1}{1 + L(s)} r(s) - \frac{P(s)}{1 + L(s)} d(s) $

定常偏差: $d(s)$ を0 にして見る

$E_p(s) = \frac{1}{1 + L(s)} r(s) $

外乱による偏差: $r(s)$ を 0 にして見る

$E_d(s) = \frac{P(s)}{1 + L(s)} d(s) $

最終値の定理: 単位ステップ応答 $u(s) = 1/s$ で見る

$\lim_{s \to 0} s E(s) (1/s) = \lim_{s \to 0} E(s)$

[tmori]

箱庭ドローンの角速度制御に関する情報

ブロック線図

ローター

伝達関数（一次遅れ）：ほんとは、プロペラ毎ではあるが、以下と仮定した

$G_{P_r}(s) = \tau_\phi(s) / u(s) = 1 / (T_s s + 1)$

角速度運動

$\dot{p} = (\tau_\phi -qr(I_z -I_y))/I_x$

$p$ 方向にだけ水平移動するならば、 $q$ と $r$ はゼロと見做せるので、以下となる。

$\dot{p} = \tau_\phi /I_x$

伝達関数：

$G_{P_p}(s) = p(s)/ \tau_\phi(s) = 1 /sI_x $

プラントの伝達関数

$P(s) = G_{P_r} G_{P_p} = \frac{1}{sI_x(T_ss + 1)} $

PID制御

$C(s) = K_p + K_i/s + sK_d$

[tmori]

箱庭ドローンの角速度制御に関する基本情報

開ループ伝達関数

$L(s) = C(s) P(s) = (K_p + K_i/s + sK_d) ( 1 / (T_s s + 1)) (1 /sI_x)$

$L(s) =\frac{K_ds^2 + K_ps + K_i}{I_xT_ss^3 + I_xs^2}$

閉ループ伝達関数

$W(s) = \frac{K_ds^2 + K_ps + K_i}{I_xT_ss^3 + (I_x+K_d)s^2 + sK_p + K_i}$

定常偏差の伝達関数

$G_{E_p}(s) = \frac{I_xT_ss^3 + I_xs^2}{I_xT_ss^3 + (I_x+K_d)s^2 + sK_p + K_i} $

外乱偏差の伝達関数

$G_{E_d}(s) = \frac{s}{I_xT_ss^3 + (I_x+K_d)s^2 + sK_p + K_i} $

[tmori]

箱庭ドローンの機体パラメータ

• $T_s$ : 0.2
• $I_x$: 0.0000625

[tmori]

プラントモデルのボード線図と位相線図

ゲイン余裕: None dB
位相余裕: 1.0153054674234738 degrees
ゲイン余裕発生周波数: 282.8245593007365 rad/s
位相余裕発生周波数: None rad/s

ここから、バンド幅( $\omega_b$ ) が、約282 rad/s であることがわかった。
サンプリング角周波数 $\omega_s$ は、そのバンド幅 $\omega_b$ の 10倍から100倍程度と言われている。

• 10 倍：2.2msec
• 100倍：0.22msec

なので、シミュレーション周期は、現状 3msec はちょっと大きい、100倍は無理として、1msec周期くらいにすべきではないか？？

[tmori]

P制御結果

{
    "numerator": [
        "Kd",
        "Kp",
        "Ki"
    ],
    "denominator": [
        "Ts * Ix",
        "Ix + Kd",
        "Kp",
        "Ki"
    ],
    "constants": {
        "Kp": 0.001,
        "Ki": 0,
        "Kd": 0,
        "Ts": 0.2,
        "Ix": 0.0000625
    }
}

L(s):

ゲイン余裕: None dB
位相余裕: 31.425862388825465 degrees
ゲイン余裕発生周波数: 8.274796325994506 rad/s
位相余裕発生周波数: None rad/s

W(s):
ステップ応答：

インパルス応答：

外乱：

[tmori]

PI制御

{
    "numerator": [
        "Kd",
        "Kp",
        "Ki"
    ],
    "denominator": [
        "Ts * Ix",
        "Ix",
        "0",
        "0"
    ],
    "constants": {
        "Kp": 0.001,
        "Ki": 0.002,
        "Kd": 0,
        "Ts": 0.2,
        "Ix": 0.0000625
    }
}

L(s):

ゲイン余裕: None dB
位相余裕: 17.521725394893195 degrees
ゲイン余裕発生周波数: 8.407008430994608 rad/s
位相余裕発生周波数: None rad/s

W(s):

ステップ応答:

インパルス応答：

外乱：

I制御を入れることで、外乱の影響が消えた！

[tmori]

わかったこと

• 位相余裕が大きくなるとオーバーシュートが減少するけど、バンド幅が減少し速応性が落ちる

次やること

まずは、こうへいさんの資料をベースにして、PIDパラメータを求める手順を写経していく。

• 比例ゲインを増やした時のバンド幅、位相余裕、ゲイン余裕の変化をCSVファイル化して、グラフ化できるようにする
• 積分時間を変えた場合のグラフ化をする
• 位相余裕とバンド幅を決める
• バンド幅から制御対象のuとvを求めるための計算式を導出する
• 積分時間を決めて、Kdを求める
• Kp、Ki、Kdで、周波数応答、ステップ応答、インパルス応答、外乱応答をチェックする

これができるようになったら、箱庭ドローンシミュレータの物理モデルをこうへいさんの資料ベースのものに変更して、角速度制御と角度制御を組み込み、ラジコン操作可能になるかをチェックする。

合わせ技で、ミキサー対応もやってみたい。

まだわかってないこと

• 角度制御のプラントモデルはどう設計するのか？角速度制御のL(S)も含めるのか？単純化するのか？？
  • こうへいさんの資料p29 で、各速度制御とプラント含めた伝達関数で同様に設計するみたい
  • でもこの勢いで、速度制御が水平位置制御やりだすと大変なことになりそう。。
• ミキサーで回転速度が負になる場合は、一般的に、どう考えるのか？

[tmori]

KpとKdを導出する式

PID制御側

$C(s) = K_p + K_i/s + s K_d$

$C(j\omega) = K_p -j \frac{(K_i - K_d \omega^2)}{\omega}$

$\alpha = K_p$
$\beta = - \frac{K_i - K_d \omega^2}{\omega}$

プラント側

$P(j\omega) = u + j v$

開ループ伝達関数

$L(j\omega) = C(j\omega) P(j\omega)$
$= (u \alpha - v \beta) + j (u \beta + v \alpha) $

$A = u \alpha - v \beta$
$B = u \beta + v \alpha$

位相余裕 PM とゲイン交差周波数 $\omega_c$ を与えて導く

ゲイン交差周波数の時の位相 $\phi_m$ :
$\phi_m = PM - \pi$

$\omega = \omega_c$ の時、ゲインは１になるので、以下の式が成り立つ。

$A^2 + B^2 = 1$

また、 $phi_m$ は、以下のように書ける。

$\tan\phi_m = B/A$

$B = A \tan\phi_m$ なので、

$A^2( 1 + \tan^2\phi_m) = 1$

となる。ここで、 $1 + \tan^2\phi = 1/\cos^2\phi$ より、

$A = \cos\phi_m$
$B = \sin\phi_m$

となる。

[tmori]

続き

開ループ伝達関数に先の $A, B$ の関係式を代入すると、

$A = u \alpha - v \beta = \cos\phi_m$
$B = u \beta + v \alpha = \sin\phi_m$

となり、さらに、PID制御の $\alpha, \beta$ を代入するとこうなる。

$u K_p - v (- \frac{K_i - K_d \omega^2}{\omega}) = \cos\phi_m$ ... (1)
$v K_p + u (- \frac{K_i - K_d \omega^2}{\omega}) = \sin\phi_m$ ... (2)

ここから、 $K_p$ を求める。

$u * (1) + v * (2)$ を実施すると、 $K_p$ は以下になります。

$K_p = \frac{u\cos\phi_m + v\sin\phi_m}{u^2 + v^2}$

そして、(1) 式に $K_p$ を代入して、 $K_d$ を求めると以下になります。

$K_d = \frac{K_i}{\omega_c^2} - \frac{v\cos\phi_m + u\sin\phi_m}{\omega_c(u^2 + v^2)}$

Kd間違っているかもしれない。ChatGPTの答え。

[tmori]

プラントモデルの u, v を求める

$P(s) = G_{P_r} G_{P_p} = \frac{1}{sI_x(T_ss + 1)} $

なので、

$P(j\omega) =\frac{I_xT_s\omega_c^2 + j I_x\omega_c}{(\omega_cI_x)^2(1-(T_s\omega_c)^2)} $

$u =I_x \frac{T_s\omega_c^2}{k}$
$v =I_x \frac{\omega_c}{k}$

$k = (\omega_cI_x)^2 (1-(T_s\omega_c)^2)$

上記式誤ってました。

[tmori]

ツールの使い方

cd hakoniwa

プラントのボード線図と位相線図を見る

定義ファイル：ps.json

python python/bode_analyze/analyze.py python/bode_analyze/ps.json

開ループ伝達関数

定義ファイル：ls.json

python python/bode_analyze/analyze.py python/bode_analyze/ls.json

閉ループ伝達関数のステップ応答などを見る

定義ファイル：ws.json

ステップ応答：

python python/bode_analyze/analyze.py python/bode_analyze/ws.json  --response step

インパルス応答：

python python/bode_analyze/analyze.py python/bode_analyze/ws.json  --response impulse

外乱応答を見る

定義ファイル：eds.json

python python/bode_analyze/analyze.py python/bode_analyze/eds.json  --response step

[tmori]

気づき

Unityなしで制御モデルの評価できる箱庭なら、伝達関数ベースでPIDパラメータ決めたら、即、評価してチェックできるな。

こんだけ。

 bash eval-ctrl.bash -1 X:10 Y:0 S:5

OK c(Steady state value)  : 10.003   (Target: 10±0.100 m)
OK T_r(Rise time)         : 3.261 s (Target: ≤ 10.000 s)
OK T_d(Delay time)        : 3.210 s (Target: ≤ 5.000 s)
OK O_s(Maximum overshoot) : 0.350   (Target: ≤ 1.000 m)
OK T_s(5% settling time)  : 4.854 s (Target: ≤ 20.000 s)

[tmori]

角度制御をどうするか

• 角速度制御のPIDパラメータを決める
• 角速度制御のW(s) を導出する
• C2(s) -> 角速度制御のW(s) -> 1/s -> 角度

[tmori]

角速度制御のPIDパラメータを決める。

ここから、こうへいさんの資料をベースにした機体パラメータを採用します。

• Kp = 1.2
• Ki = 60.0
• Kd = 0.0067

L(s) のPIパラメータ調査

L(s) のボード線図と位相線図

ゲイン余裕 (Gain Margin): inf dB
位相余裕 (Phase Margin): 28.667827092862098 degrees
ゲイン余裕発生周波数: nan rad/s
位相余裕発生周波数: 94.54368192713332 rad/s

W(s) のステップ応答とインパルス応答

Ed(s) のステップ応答

[tmori]

角速度制御のW(s)を求める

(0.0067*s**2 + 1.2*s + 60.0)/(0.00011773*s**3 + 0.0128*s**2 + 1.2*s + 60.0)

L(s) について、C(s) と 既存のG(s)と新規P(s)を掛けて、新しい伝達関数を作るプログラムが必要。
そのL(s)から、W(s)を求めるプログラムが必要。

これが、できれば、順番に階層構造の制御パラメータ解析を積み上げていくことができる。