高度制御の理論的導出

[tmori]

設計メモ

• ミキサーは入れないでおく。
  • 変換できるという前提をおく。
  • 仮に変換でマイナス値になった場合、０にするというのを入れると、不連続なるので理論的な解析が難しい。

ブロック線図

ローター側

$\dot{\Omega}(t) = (K_r u(t) - \Omega(t))/ T_r$

• $K_r$ - ローターのゲイン定数
• $T_r$ - ローターの時定数
• $u(t)$ - ローターのデューティー比 ($0.0 \le d(t) \le 1.0$)

$\Omega(s)/U(s) = G(s) = K_r/(T_r s + 1)$

$T= N A \Omega^{2}$

$N$ はローター数で、今回は４。

ここで、 $U(s)$ は、平衡状態からの差分で考える。

$U(s) = U_0 + \Delta U(s)$

ここから、伝達関数を求めるとこうなる。

ちなみに、 $T(s) $ はこうなる。

$T(s) = T_0 + \Delta T(s)$

高度運動

z軸はNED座標系。

対象の運動方程式：

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

ここで $u(t) = T_0 + \Delta T(t)$ として、 $T_0$ の項は、重力項を打ち消すものとする。

ラプラス変換:

$s^{2}Z(s) = -\frac{\Delta T(s)}{m} - \frac{d}{m} s Z(s)$

$Z(s) = -\frac{\Delta T(s)}{ms^{2}} - \frac{d}{ms} Z(s)$

両辺に $ms$ をかけて、右辺の $Z(s)$ を左辺に移動してまとめる。

$(ms + d) Z(s) = -\frac{\Delta T(s)}{s^{1}} $

$Z(s) = -\frac{ \Delta T(s)}{s(ms + d)}$

[tmori]

注意：この情報は間違っているので、このスレッドの後を参照。

補足情報

$( \Delta T(s) / \Delta U(s) )$ の導出:

線形化では、動作点 $( U_0 )$ に対する入力の小さな変動 $( \Delta U(s) )$ に対するトルクの変動 $( \Delta T(s) )$ を考えます。具体的には、次のように展開されます：

1. 元の関係式:
   $T(s) = N A \Omega(s)^2 = N A \left( \frac{K_r}{T_r s + 1} U(s) \right)^2$

2. 動作点周りでの摂動：
   入力 $( U(s) )$ を定常値 $( U_0 )$ とその変化 $( \Delta U(s) )$ に分けます：
   $U(s) = U_0 + \Delta U(s)$
   このとき、 $( U(s)^2 )$ は次のように展開されます：
   $U(s)^2 = (U_0 + \Delta U(s))^2 = U_0^2 + 2 U_0 \Delta U(s) + \Delta U(s)^2$
   ここで、高次の小さな変動項 $( \Delta U(s)^2 )$ は無視できるため、次のように近似できます：
   $U(s)^2 \approx U_0^2 + 2 U_0 \Delta U(s)$

3. トルクの変動 $( \Delta T(s) )$ を求める：
   これにより、トルク $( T(s) )$ の変動は次のように表されます：
   $T(s) = N A \left( \frac{K_r}{T_r s + 1} \right)^2 (U_0^2 + 2 U_0 \Delta U(s))$
   したがって、トルクの変動 $( \Delta T(s) ) $ は次の形になります：
   $\Delta T(s) \approx 2 N A U_0 \cdot \left( \frac{K_r}{T_r s + 1} \right)^2 \Delta U(s)$

4. 線形化された伝達関数 $( \frac{\Delta T(s)}{\Delta U(s)} )$ ：
   動作点周りの小さな摂動に対するトルクの変動に対して、伝達関数は次のように表されます：
   $\frac{\Delta T(s)}{\Delta U(s)} = 2 N A U_0 \cdot \frac{K_r^2}{(T_r s + 1)^2}$

結論：

線形化された摂動系の伝達関数 $( \frac{\Delta T(s)}{\Delta U(s)} )$ は次のようになります：

$\frac{\Delta T(s)}{\Delta U(s)} = 2 N A U_0 \cdot \frac{K_r^2}{(T_r s + 1)^2}$

[tmori]

プラント全体

ローターの伝達関数

$\frac{\Delta T(s)}{\Delta U(s)} = 2 N A U_0 \cdot \frac{K_r^2}{(T_r s + 1)^2}$

高度運動の伝達関数

$\frac{Z(s)}{\Delta T(s)} = -\frac{1}{s(ms + d)}$

プラント全体の伝達関数

$\frac{Z(s)}{\Delta U(s)} = -\frac{2 N A U_0 K_r^2}{s(ms + d)(T_r s + 1)^2}$

[tmori]

制御側

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

[tmori]

各種パラメータ

以下を前提とする
#326

• m: 0.71
• N: 4
• Kr: 2896(最大回転数)
• Tr: 1.93e-02
• A: 8.3x10-7
• $U_0$ : $\sqrt{mg/NA Kr^{2}}$
  • ホバリング状態であり、 $T_0$ が、重力と一致する
  • $T_0 = NA K_r^{2} U_0^2$ から、 $U_0$ を求めた

[tmori]

L(s) を求める

$C(s) = K_p + K_i/s + sK_d = \frac{K_d s^{2} + Kp s + Ki}{s}$

$P(s) = -\frac{2 N A U_0 K_r^2}{s(ms + d)(T_r s + 1)^2}$

$L(s) = C(s) P(s) = -\frac{2 N A U_0 K_r^2(K_d s^{2} + Kp s + Ki)}{s^{2}(ms + d)(T_r s + 1)^2}$

[tmori]

閉ループ特性方程式 1+𝐿(𝑠)を求める

$1 + L(s) = \frac{s^{2}(ms + d)(T_r s + 1)^2 - 2 N A U_0 K_r^2(K_d s^{2} + Kp s + Ki)}{s^{2}(ms + d)(T_r s + 1)^2}$

展開された分子を整理した結果は次の通りです：

$\text{分子} = Tr^2 m s^5 + (Tr^2 d + 2 Tr m) s^4 + (2 Tr d + m) s^3 + (d - 2 A K_d K_r^2 N U_0) s^2 - 2 A K_p K_r^2 N U_0 s - 2 A K_i K_r^2 N U_0$

[tmori]

うーん、そもそも不安定であることが判明してしまった。まいった。

[tmori]

わかった！NED座標系だから、PID制御パラメータは全部マイナスにしないといけないということか！

[tmori]

調整後のパラメータ

{
    "plants": [
        {
            "comment": "Rotor",
            "num": [
                "2 * N * A * U0 * Kr**2"
            ],
            "den": [
                "Tr**2",
                "2*Tr",
                "1"
            ]
        },
        {
            "comment": "Altitude",
            "num": [
                "-1"
            ],
            "den": [
                "m",
                "d",
                "0"
            ]
        }
    ],
    "controller": {
        "num": [
            "Kd",
            "Kp",
            "Ki"
        ],
        "den": [
            "1",
            "0"
        ]
    },
    "constants": {
        "Kp": -3.6182108001838924,
        "Ki": -0.1,
        "Kd": -0.5599136193021486,
        "m": 0.71,
        "g": 9.81,
        "N": 4,
        "d": 0.05,
        "A": 8.3e-7,
        "Tr": 1.93e-02,
        "Kr": 2896,
        "U0": "math.sqrt( (m * g) / (N * A * (Kr**2)) )",
        "Wc": 20,
        "PM": 30
    }    
}

[tmori]

閉ループ伝達関数の極

左反平面！

システムの極: [-7.81364819e+01 +0.j         -8.31995767e+00+21.83536742j
 -8.31995767e+00-21.83536742j -8.89321092e+00 +0.j

[tmori]

ステップ応答

[tmori]

ボード線図と位相線図

ゲイン余裕 (Gain Margin): 11.013343356723455 dB
位相余裕 (Phase Margin): 30.009422184890894 degrees
ゲイン余裕発生周波数: 44.97084634065474 rad/s
位相余裕発生周波数: 20.059957375420947 rad/s

[tmori]

箱庭での実験結果

このパラメータを使ってもうまくいかなかった。
振動してしまう。

以下にしたら良い感じだった。

PID_PARM_ALT_Kp 1.0
PID_PARM_ALT_Ki 0.10
PID_PARM_ALT_Kd 50.0

箱庭の制御は高度を正負反転しているので、正の値が正しい

[tmori]

このギャップがどこからくるものなのか？

• ミキサーの不連続性による影響もありそう。結構、推力をマイナスにしようとしている
• そもそも推力値が物理的な上限、下限に入ってない可能性もあるか。。
• 離散時間での誤差もありそう
• 制御周期の問題もあるかも

[tmori]

検証のためにミキサーなしでやってみよう

プラント

$Z(s) = -\frac{ \Delta T(s)}{s(ms + d)}$

$T(s) = T_0 + \Delta T(s)$

$T_0 = mg$

制御

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

結果

伝達関数ベースで導出したパラメータを箱庭ドローンシミュレータに設定したが同じ動きにならなかった。

うーん、やっぱり、根本的なところで何かが違う気がするなー。こまった

[kenjihiranabe]

あれ？

1. 元の関係式:
   $T(s) = N A \Omega(s)^2 = N A \left( \frac{K_r}{T_r s + 1} U(s) \right)^2$

ですが、これ違ってませんか？

$T(t) = N A \Omega(t)^2$

なので、 $f(t)^2$ をラプラス変換する必要があって（ $\Omega(t)^2$ ）、これは不能です。ですが、 $t$ の領域で $U_0$ と $T_0$ のまわりで線形化することはできて、そこからラプラス変換ですね。カンファレンス聴きながら、もう一回式を追ってみます。

[kenjihiranabe]

#328 (comment)

$\Delta T(s)/\Delta U(s)$ は通常の一時遅れでした。(U->Ω->T)

$\Delta T(s)/\Delta U(s) = 2 \Omega_0 N A \frac{K_r}{Tr s + 1}$

[kenjihiranabe]

とりあえず、全体の流れを整理して、式をもう一度作ってみた。

• u でなくΔu を使う（そうしないと、ΔΩが作れない）-> U0 を足さない
• Lの先頭のマイナスが気になる。z座標系をにしたときに、L(s)+1 = 0 の極が異なってしまう（まだどこかミスしている）。
  

[kenjihiranabe]

野波先生の 式(2.92) と上記 P(s) は一致するので、よしとする。

[kenjihiranabe]

zが下向きの時、u=duty や Ωを上げる方向なので、Kp はマイナス値になるんですね。

[kenjihiranabe]

手計算した結果、この式と全く一緒になりました！
https://colab.research.google.com/drive/14hSlp1h4ffHOOTq3G3upaA6x1kknOPqh

[kenjihiranabe]

いろいろ Kp, Ki, Kd を触ってみて、再度、理論的に分かったこと。

• Ki は不要。高度の運動方程式に 1/s があるので、「内部モデル原理」により、誤差は必ず 0 になる。
• Kd は安定化のために必ず必要。ないと位相が -180 を超えてしまう。

Kp = -1, Ki=0, Kd = -10 で位相余裕を確保してみた。