PID制御の勉強ができる環境を作る

[tmori]

目的

箱庭ドローンシミュレータを制御の勉強として利用できるようにしたい。 そして、参考資料にあるように、制御の競技会を同時実施できるようにする！

参考： https://www.youtube.com/watch?v=y-C4AId2Za8&t=1840s

[tmori]

構成

PID制御側は、MATLAB／Simulinkでモデリング＆コード生成しても良いし、手コーディングして良い。 機体の動きは、Unityでリアルで見れるようにする。

[tmori]

課題など

• PID制御プログラムをモジュールとして組み込めるプラットフォームの検討が必要
• PID制御プログラムは、箱庭とは別の環境でビルドできる必要がある
• なんなら、MATLABで作ってコード生成してもらったものを組み込めるようにしたい
• 制作した制御プログラムはPX4に組み込むこともできるようにしたい
• なんなら、それを実機でそのまま使えたら素敵だ。

[tmori]

PID制御プログラムのモジュール化方法について

• 案1. ダイナミックローディングできるようにする
• 案2. スタティックリンクする？

案1でしょ。

作戦

1. 案1のビルド環境を別のリポジトリで管理する。
2. ビルド環境は dockerか何かで簡便な方法を採用する
3. MATLABも視野に入れる

[tmori]

詳細設計

[tmori]

実装ステップ

• [x] drone と controllerを接続して実行できるようにする
• [x] Unityと連携できるようにする
• [x] ローダーを作る
• [x] 新しいリポジトリ現行リポジトリでcontrollerを作成できるようにする

[tmori]

残作業

• [x] コンフィグファイルの設定がないケースのエラーハンドリング
• [x] ローダブルモジュールがない場合のエラーハンドリング
• [x] PID制御課題のREADMEを作る
• [x] PID制御プログラム実行方法のREADMEを作る
• [x] MATLAB連携に向けた対応内容を検討する
• [x] その他あれば

MATLAB連携に向けた検討はこちらに移管する。

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

[tmori]

最後の作業

この環境を利用して、現行の物理モデルのホバリングを理論的に検討したPID制御モデルでコントロールできるようにして、箱庭ラボのブログで紹介する。

• [x] 平鍋さんの２次伝達関数の理解
• [x] 伝達関数から理論的にPIDの値を求めるやり方がないか調べる
• [ ] プログラム実装して複数のパラメータで同時に検証してみる。
• [ ] サンプルプログラムを自動作成するツールを作る。
• [ ] 一般公開する
• [ ] 箱庭ラボのブログで紹介する

[tmori]

平鍋さんの２次伝達関数の理解

対象の運動方程式：

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

ここで、目標値との誤差の方程式にすると以下となる。

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

$$ e(t) = R - z(t) $$

$R$ はホバリングする目標値(固定値)

これをラプラス変換するとこうなって、

$$ -s^{2} E(s) = -\frac{U(s)}{m} + \frac{g}{s} + d s \frac{E(s)}{m} $$

伝達関数にするとこうなって、２次遅れの伝達関数で表現できない。

$$ Gp(s) = \frac{E(s)}{U(s)} = \frac{1}{m} / (s^{2} + \frac{d}{m} s + \frac{g}{s}) $$

[tmori]

伝達関数から理論的にPIDの値を求めるやり方がないか調べる

これではないか？

https://tajimarobotics.com/pid-stability-pid-control/

[kenjihiranabe]

これをラプラス変換するとこうなって、

$$ -s^{2} E(s) = -U(s)/m + g E(s) + d s E(s) / m $$

gE(s)は、g/s かな。そして、純粋に線形にならないので、E/U が解けない。

[tmori]

なるほど。２次遅れの伝達関数にして、理論的な方向で攻めようかと思いましたが、難しそうですね。

[kenjihiranabe]

僕の方策は、T/m - g をT' としてT' をコントロール可能な変数と捉えて線形化しました。(スライドの計算メモのページ)

[tmori]

なるほど！

[tmori]

やっと理解できました。P制御と繋げた微分方程式作って、２次の伝達関数にしているのか！

https://speakerdeck.com/hiranabe/math-physics-and-dynamics-of-drone-in-hakoniwa?slide=13

[tmori]

まずは、P制御をベースにして、考えていきます。

[tmori]

プラント側

対象の運動方程式：

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

これをラプラス変換するとこうなる。

$s^{2}Z(s) = -\frac{U(s)}{m} + \frac{g}{s} - \frac{d}{m} s Z(s)$

[tmori]

P制御側

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

これをラプラス変換すると、

$U(s) = K_p ( \frac{R}{s} - Z(s) )$

[tmori]

プラント側のU(s) にP制御の式をラプラス関数で代入

$s^{2}Z(s) = -\frac{K_p ( \frac{R}{s} - Z(s) ) }{m} + \frac{g}{s} - \frac{d}{m} s Z(s)$

左辺に微分項、右辺を積分項にしてみる。

$s^{2}Z(s) + \frac{d}{m} s Z(s) + \frac{K_p}{m} Z(s) = (g -\frac{K_p R}{m}) \frac{1}{s}$

ここからZ(s)を求めるとこうなる。

$Z(s) = \frac{(g -\frac{K_p R}{m})}{s(s^{2} + \frac{d}{m} s + \frac{K_p}{m})}$

なので、積分要素と２次の遅れ要素の直列接続とみなせる

$Z(s) = \frac{1}{s} \frac{(g -\frac{K_p R}{m})}{s^{2} + \frac{d}{m} s + \frac{K_p}{m}}$

変形すると、

$Z(s) = \frac{m}{K_p}(g -\frac{K_p R}{m}) \frac{1}{s} \frac{\frac{K_p}{m}}{s^{2} + \frac{d}{m} s + \frac{K_p}{m}}$

ここで、

$\omega_n^{2} = \frac{K_p}{m}$

$\zeta = \frac{d}{2\sqrt{mK_p}}$

とおくと、こうなる。

$Z(s) = \frac{(g -\omega_n^{2} R)}{\omega_n^{2}} \frac{1}{s} \frac{\omega_n^{2}}{s^{2} + 2 \zeta \omega_n s + \omega_n^{2}}$

[tmori]

P制御とプラントモデルを含めた伝達関数の解釈

1/s は単位ステップ関数 $u_s(t)$ のラプラス変換 $U(s)$ なので、単位ステップ信号に対する伝達関数として表現すると、こうなるので、２次遅れ要素の特性から Kp値の妥当な値を推測することが可能になるはず。

$$ D(s) = \frac{Z(s)}{U(s)} = \frac{(g -\omega_n^{2} R)}{\omega_n^{2}} \frac{\omega_n^{2}}{s^{2} + 2 \zeta \omega_n s + \omega_n^{2}} $$

[tmori]

２次遅れ要素の特徴

• $\omega_n^{2} = \frac{K_p}{m}$

• $\zeta = \frac{d}{2\sqrt{mK_p}}$

• $d$は、定数（０より大きい値）

• [ ] 過制動($\zeta > 1$)

• [ ] 臨界制動($\zeta = 1$)

• [ ] 不足制動($0 < \zeta < 1$)

• [x] 持続振動($\zeta = 0$) d=0のケースなので対象外とする。

[tmori]

特徴とパラメータ候補

特徴	条件	Kp値
過制動	$K_p < \frac{d^{2}}{4m}$	TODO
臨界制動	$K_p = \frac{d^{2}}{4m}$	TODO
不足制動	$K_p > \frac{d^{2}}{4m}$	TODO

うーん、ダメだ。 $K_p$ 値が果てしなく小さくなる。。

[tmori]

再考：P制御側

重力を定常的に与えないと、相当不安定になるのではないか。 z軸はNED座標系。Rは正の値で指定する。

$u(t) = K_p ( z(t) + R ) + m g$

これをラプラス変換すると、

$U(s) = K_p ( Z(s) + \frac{R}{s} ) + \frac{m g}{s}$

うーん、 $u(t)$ が負にならないような考慮が必要であることに気づいた。

[tmori]

再考：プラント側のU(s) にP制御の式をラプラス関数で代入

$s^{2}Z(s) = -\frac{ K_p ( Z(s) + \frac{R}{s}) + \frac{mg}{s} }{m} + \frac{g}{s} - \frac{d}{m} s Z(s)$

左辺に微分項、右辺を積分項にしてみる。

$s^{2}Z(s) + \frac{d}{m} s Z(s) + \frac{K_p}{m} Z(s) = ( -\frac{K_p R}{m}) \frac{1}{s}$

ここからZ(s)を求めるとこうなる。

$Z(s) = \frac{( -\frac{K_p R}{m})}{s(s^{2} + \frac{d}{m} s + \frac{K_p}{m})}$

なので、積分要素と２次の遅れ要素の直列接続とみなせる

$Z(s) = \frac{1}{s} \frac{( -\frac{K_p R}{m})}{s^{2} + \frac{d}{m} s + \frac{K_p}{m}}$

変形すると、

$Z(s) = \frac{m}{K_p}( -\frac{K_p R}{m}) \frac{1}{s} \frac{\frac{K_p}{m}}{s^{2} + \frac{d}{m} s + \frac{K_p}{m}}$

ここで、

$\omega_n^{2} = \frac{K_p}{m}$

$\zeta = \frac{d}{2\sqrt{mK_p}}$

とおくと、こうなる。

$Z(s) = - R \frac{1}{s} \frac{\omega_n^{2}}{s^{2} + 2 \zeta \omega_n s + \omega_n^{2}}$

[kenjihiranabe]

この三次式まで、僕の結果と整合します。この後、逆ラプラス変換して時間領域の式が出せています。計算メモのページ。

[kenjihiranabe]

この3次式まで僕の手計算と整合しています。 この後逆ラプラスして時間領域で式をだして、グラフ書きました。

[tmori]

ありがとうございます！ $\omega_n$ と $\zeta$ の値が違うようですが、同じ感じですね。

ここから、 $\zeta$ の範囲から $K_p$ の代表値を決めて、実際にシミュレーションしてみようと思っています。

２次遅れ要素の特徴

• $\omega_n^{2} = \frac{K_p}{m}$

• $\zeta = \frac{d}{2\sqrt{mK_p}}$

• $d$は、定数（０より大きい値）

• [ ] 過制動($\zeta > 1$)

• [ ] 臨界制動($\zeta = 1$)

• [ ] 不足制動($0 < \zeta < 1$)

• [x] 持続振動($\zeta = 0$) d=0のケースなので対象外とする。

特徴	条件	備考
過制動($\zeta > 1$)	$K_p < \frac{d^{2}}{4m}$	現実的にありえない？
臨界制動($\zeta = 1$)	$K_p = \frac{d^{2}}{4m}$	現実的にありえない？
不足制動($0 < \zeta < 1$)	$K_p > \frac{d^{2}}{4m}$	$K_p$が大きくなると $\zeta$ は０に近づく

機体パラメータ値から求める。

d= $10^{-4}$ m= $10^{-1}$ $K_p=\frac{10^{-8}}{4*10^{-1}}=\frac{10^{-7}}{4}$

[kenjihiranabe]

ここからは、方針(オーバーシュートx%までとか、整定時間何秒にしたいとか、外乱に強くしたいとか)で決めていくのだと思う。

[tmori]

試してみた(その１)

目標値は -10m $K_p$ = (1/4.0)*1.0e-05 $\zeta$=0.1

[tmori]

試してみた(その２)

目標値は -10m $K_p$ = (1/4.0)*1.0e-04 $\zeta$=0.0316227766

[tmori]

試してみた(その３)

目標値は -10m $K_p$ = (1/4.0)*1.0e-00 $\zeta$=0.000316227766

[tmori]

所感

• $\zeta$ が０に近づくに連れて、持続振動になることが再現された。
• $\zeta$ が１に近づくと、不足振動になり、振動数およびオーバーシュート量が減るが、収束時間は非常に長い。約３時間かかる。

ここでD制御が必要なのか？

[tmori]

PD 制御

z軸はNED座標系。Rは正の値で指定する。

$u(t) = K_p ( z(t) + R ) + m g + K_d ( \dot{z(t) + R} ) $

Rは定数なので、こうなる。

$u(t) = K_p ( z(t) + R ) + m g + K_d \dot{z(t)} $

これをラプラス変換すると、

$U(s) = K_p ( Z(s) + \frac{R}{s} ) + \frac{m g}{s} + K_d s Z(s) $

[tmori]

プラント側のU(s) にPD制御の式をラプラス関数で代入

$s^{2}Z(s) = -\frac{ K_p ( Z(s) + \frac{R}{s}) + \frac{mg}{s} + K_d s Z(s) }{m} + \frac{g}{s} - \frac{d}{m} s Z(s)$

左辺に微分項、右辺を積分項にしてみる。

$s^{2}Z(s) + \frac{d}{m} s Z(s) + K_d s Z(s) + \frac{K_p}{m} Z(s) = ( -\frac{K_p R}{m}) \frac{1}{s}$

ここからZ(s)を求めるとこうなる。

$Z(s) = \frac{( -\frac{K_p R}{m})}{s(s^{2} + (\frac{d}{m} + K_d ) s + \frac{K_p}{m})}$

なので、積分要素と２次の遅れ要素の直列接続とみなせる

$Z(s) = \frac{1}{s} \frac{( -\frac{K_p R}{m})}{s^{2} + (\frac{d}{m} + K_d) s + \frac{K_p}{m}}$

変形すると、

$Z(s) = \frac{m}{K_p}( -\frac{K_p R}{m}) \frac{1}{s} \frac{\frac{K_p}{m}}{s^{2} + \frac{d}{m} s + \frac{K_p}{m}}$

ここで、

$\omega_n^{2} = \frac{K_p}{m}$

$\zeta = \frac{d + m K_d}{2\sqrt{mK_p}}$

とおくと、こうなる。

$Z(s) = - R \frac{1}{s} \frac{\omega_n^{2}}{s^{2} + 2 \zeta \omega_n s + \omega_n^{2}}$

[tmori]

PDパラメータ探索の方針

$\zeta$ が 0.707 が最適な値だと仮定して、 $K_p$ と $K_d$ のバリエーションを探索してみるとどうだろうか。

[kenjihiranabe]

現在、10m でホバリングしていない事に注意。グラフのvalue は、R-g/wn^2 (→10)の値であり。P制御だけでは定常位置偏差 g/wn^2 がずっと出てしまう。→I制御が必要、という流れ。

[tmori]

ζ=0.707 のPD制御結果（その１）

Kp = 0.25 Kd = 2.234730306

１秒くらいで目標値に収束しました！ 収束した数値は10mになってますが、おそらくシミュレーション誤差と思われます。

drone_dynamics.csv

[tmori]

ζ=0.707 のPD制御結果（その２）

Kp = 0.0000025 Kd = 0.00607

こちらはKpが小さいので収束に時間がかかっていますし、収束値は10mになっていません。

drone_dynamics.csv

[tmori]

現在、10m でホバリングしていない事に注意。グラフのvalue は、R-g/wn^2 (→10)の値であり。P制御だけでは定常位置偏差 g/wn^2 がずっと出てしまう。→I制御が必要、という流れ。

コメントありがとうございます！ Kiのほうも同様に数式化できないかやってみます。

あと、オーバーシュート値とオーバーシュートに達する時間も解析的に求められると評価がしやすいですよね。

[tmori]

PID 制御

z軸はNED座標系。Rは正の値で指定する。

$u(t) = K_p ( z(t) + R ) + m g + K_d ( \dot{z(t) + R} ) + K_i \int_0^t ( z(\tau) + R ) d\tau $

Rは定数なので、こうなる。

$u(t) = K_p ( z(t) + R ) + m g + K_d \dot{z(t)} + K_i R t + K_i \int_0^t z(\tau) d\tau $

これをラプラス変換すると、

$U(s) = K_p ( Z(s) + \frac{R}{s} ) + \frac{m g}{s} + K_d s Z(s) + K_i ( \frac{R}{s^{2}} + \frac{Z(s)}{s} )$

[tmori]

プラント側のU(s) にPID制御の式をラプラス関数で代入

$s^{2}Z(s) = -\frac{ K_p ( Z(s) + \frac{R}{s}) + \frac{mg}{s} + K_d s Z(s) + K_i ( \frac{R}{s^{2}} + \frac{Z(s)}{s} )}{m} + \frac{g}{s} - \frac{d}{m} s Z(s)$

左辺に $Z(s)$ 項、右辺を $s$ 項にしてみる。

$s^{2}Z(s) + \frac{d}{m} s Z(s) + K_d s Z(s) + \frac{K_p}{m} Z(s) + K_i \frac{Z(s)}{s} = ( -\frac{K_p R}{m}) \frac{1}{s} - \frac{K_i R}{s^{2}}$

[tmori]

Kiが入ると、２次遅れにならないので、これまでの解き方では難しそう。

[tmori]

現時点の課題まとめと今後の方針

1. PD制御の計算結果が怪しいかもしれない
2. I制御を含めた解析は難しそう

I制御の解析は一旦ペンディングします。 PD制御の計算結果は再度レビューします。

その上で、PD制御のパラメータをいくつか決めて、I制御パラメータを微調整する探索をやって、Unityでビジュアライズするところをゴールにしようと思います。

[kenjihiranabe]

例の制御ドローンレース，でも，PD 制御までです． 例の講義の秀逸なところは，早く収束するように攻めると，外乱に弱くなる．Kp, Kd を決める目安としては，「位相余裕」と「ゲイン余裕」を見ている．

[kenjihiranabe]

森さんのやり方だと，一巡伝達関数でなく，閉ループのステップ応答を直接求めるようになっていて，制御の通常コースの，

1. P(s) を与えて，C(S) を設計する課題． 2.そのために，「一巡伝達関数＝C(s)P(s)」が分かると閉ループは「W=CP/(1+CP)」である．
2. W の条件（外乱につよい，すばやく収束する，などなど）がC(S)設計への要求となる．
3. C(S)設計のために，「WでなくCPの特性を見て，要求を満たすCを設計」（CPのボード線図での位相余裕とゲイン余裕，ナイキスト線図上の，(-1,0)点と軌跡の関係）

という手順が使えなくて，教材としては制御の教科書にうまく乗らないのがちょっと残念に思いました．何かうまくできるといいのだけど．

[tmori]

説明ありがとございます！ まだまだ勉強不足なもので、フィードバック制御側の理解を進めたいと思います。