12末リリースに向けた作業リスト

[tmori]

ゴールイメージ

背景

toppers/hakoniwa-px4sim#328

方向性

まずは高度速度制御で一巡できるようにする

作業リスト

作業リスト

• プラント側のパラメータをFixする #26
• シミュレーション精度をパラメータで指定できるようにしたい #27
• プラント側：理論とシミュレーション結果が一致するかチェックする(ステップ応答) #28
• プラント側：理論とシミュレーション結果が一致するかチェックする(ボード線図)
• プラント側：理論とシミュレーション結果が一致するかチェックする(位相線図)
• 制御側：理論とシミュレーション結果の一致性をチェックするためのPIDパラメータセットを決める
• 制御側：理論とシミュレーション結果が一致するかチェックする(ステップ応答) #29
• 制御側：理論とシミュレーション結果が一致するかチェックする(ボード線図)
• 制御側：理論とシミュレーション結果が一致するかチェックする(位相線図)
• 極再配置のやり方を調べる #30
• うまくいかない体験をするためのPIDパラメータを何個か作る
• うまくいかない体験をシミュレータで確認するための手順を作る
• 線形モデル化して解析する手法の説明をする(Google Colab ベース)
• 極配置法による解析手法の説明をする(Google Colab ベース)
• シミュレータとして見せる機能を整理する（ステップ応答、ログリプレイくらい？）

[tmori]

プラント側の結果：

横軸は rad/s っす。

パラメータ

{
    "plants": [
        {
            "comment": "Rotor",
            "num": [
                "2 * N * A * W0 * Kr"
            ],
            "den": [
                "Tr",
                "1"
            ]
        },
        {
            "comment": "Speed Altitude",
            "num": [
                "1"
            ],
            "den": [
                "m",
                "d"
            ]
        }
    ],
    "controllers": [],
    "constants": {
        "m": 0.71,
        "g": 9.81,
        "N": 4,
        "d": 0.05,
        "A": 8.3e-07,
        "Tr": 0.046,
        "Kr": 2896,
        "W0": 1448,
        "Ix": 0.0061,
        "Iy": 0.00653,
        "Iz": 0.0116
    }
}

[tmori]

制御側の結果

横軸は rad/s っす。

パラメータ

{
    "plants": [
        {
            "comment": "Rotor1",
            "num": [
                "1"
            ],
            "den": [
                "Tr",
                "1"
            ]
        },
        {
            "comment": "Speed Altitude",
            "num": [
                "1"
            ],
            "den": [
                "m",
                "d"
            ]
        }
    ],
    "controllers": [
        {
            "num": [
                "VAlt_Kd",
                "VAlt_Kp",
                "VAlt_Ki"
            ],
            "den": [
                "1",
                "0"
            ]
        }
    ],
    "constants": {
        "m": 0.71,
        "g": 9.81,
        "N": 4,
        "d": 0.05,
        "A": 8.3e-07,
        "Tr": 0.046,
        "Kr": 2896,
        "W0": 1448,
        "Ix": 0.0061,
        "Iy": 0.00653,
        "Iz": 0.0116,
        "VAlt_Kp": 1.0,
        "VAlt_Ki": 0.1,
        "VAlt_Kd": 0.1,
    }
}

[kenjihiranabe]

僕が手プロットしたときは、高周波は測定値が上振れしたが、今度は下振れしてますね。シミュレーションの解像度上げたのと、どちらにしてもシミュレーション周波数が、10^3なので限界なんでしょうね。

[tmori]

プラント側：理論とシミュレーション結果が一致するかチェックする(ステップ応答)

パラメータは同じ。

理論ベース

Overshoot: 28.03% at 0.12 seconds
Steady-State Value: 1.00
Steady-State Error: 0.00
Rise Time (10%-90%): 0.04 seconds
Delay Time (50%): 0.04 seconds

[tmori]

あー、プラントのシミュレーションは入力与えるだけで、フィードバックはない。
なので、そもそも閉ループ伝達関数の一致性確認はできないことに気づいた。

[tmori]

ステップ応答については、プラントはスキップします。

[tmori]

制御側：理論とシミュレーション結果が一致するかチェックする(ステップ応答)

パラメータは同じ。

理論ベース

Overshoot: 1.41% at 5.27 seconds
Steady-State Value: 1.01
Steady-State Error: -0.01
Rise Time (10%-90%): 1.55 seconds
Delay Time (50%): 0.50 seconds
Settling Time (within ±5%): 5.49 seconds

[tmori]

シミュレーションの方は、目標速度を1m/secに設定して実行したところ、そもそも浮上しない。。

[tmori]

浮上しないのではなくて、極端に遅いのか。。

[tmori]

あ、理論ベースの数式はホバリングしている状態だった。。
そっか、シミュレーションでやる場合は、まずはホバリング状態作らないといけないのか。。

[tmori]

最新の評価ツールは、、こういうの対応出来る！

    "signals": {
      "step1": {
        "type": "step",
        "parameters": {
          "comment": "target speed z : 1 m/sec",
          "offsets": [ 1 ]
        }
      },
      "step2": {
        "type": "step",
        "parameters": {
          "comment": "target speed z : 0 m/sec",
          "offsets": [ 0 ]
        }
      },
      "step3": {
        "type": "step",
        "parameters": {
          "comment": "target speed z : 1 m/sec",
          "offsets": [ 1 ]
        }
      }
    },
    "signal_input_timings": [
      {
        "name": "step1",
        "duration_sec": 100.0
      },
      {
        "name": "step2",
        "duration_sec": 100.0
      },
      {
        "name": "step3",
        "duration_sec": 100.0
      }
    ]

[tmori]

実験結果：

１００秒間上昇させた後、次の100秒は目標速度を０にしてホバリングさせ、そこから評価を始めるという技です。

Z軸方向の変化

Vzの変化

これを拡大すると、こうなって、なんか理論と一致しているような気がしてきたぞっと。

[tmori]

理論値を100秒間観測

Overshoot: 1.40% at 5.05 seconds
Steady-State Value: 1.00
Steady-State Error: -0.00
Rise Time: Not Defined
Delay Time (50%): 1.01 seconds
Settling Time (within ±5%): 100.00 seconds

[tmori]

シミュレーション結果の方

OK c(Steady state value)  : 1.000   (Target: 1.0±0.010 m)
OK T_r(Rise time)         : 1.562 s (Target: ≤ 2.000 s)
OK T_d(Delay time)        : 0.567 s (Target: ≤ 1.010 s)
OK O_s(Maximum overshoot) : 0.021   (Target: ≤ 0.040 m)
OK T_s(5% settling time)  : 2.072 s (Target: ≤ 5.490 s)

[tmori]

プラントのステップ応答をもう一度（開ループでやってみる）

理論ベース

Overshoot: 55539.62% at 100.00 seconds
Steady-State Value: 556.40
Steady-State Error: -555.40
Rise Time: Not Defined
Delay Time (50%): 1.01 seconds
Settling Time (within ±5%): 100.00 seconds

実験結果

[tmori]

時間スケールは一緒だけど、大きさは違うな。。うーん。

[tmori]

わかってきた。

理論ベースの伝達関数は、

Vz(s)/ΔD(s)

だった。

ΔD(s) は、ホバリングするデューティ値(0.5)からの微小変化分。

これに対して、シミュレーションでは、直接デューティを与えているから、

Vz(s)/D(s)

をみていることになる。

ボード線図や位相線図の計測する場合は、ホバリング値に対する変化量をみているから、問題ないけれど、ステップ応答だとこれが成り立たないのか。。

[tmori]

理論ベースのプラントの方程式を出してみる

Vzの運動方程式

$\dot{V_z} = -\frac{T(t)}{m} + g - \frac{d}{m}V_z$

ローターの運動方程式

$\dot{\Omega} = \frac{K_r d(t) - \Omega(t)}{Tr}$

スラスタの式

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

[tmori]

理論ベースのプラントのラプラス変換結果を出してみる

Vz

$sV_z(s)= - \frac{T(s)}{m} + \frac{g}{s} - \frac{d}{m}V_z(s)$

ローター

$s\Omega(s) = \frac{Kr D(s) - \Omega(s)}{Tr}$

$\Omega(s) = \frac{Kr}{Tr s + 1}D{s}$

スラスタ

$T(s) = A \Omega(s)^2$

[tmori]

と、ここまできて、非線形だから伝達関数にならないことに気づいた。。

[tmori]

漸近する値のチェックをする

微分方程式ベースで漸近値を出してみる。

初期値：Vz(t) = 0, T(0)
に対して、 $T_{max}$ を与えることを考える（今回の実験と同じ状況）。

ここで、

$T_{max} = A\Omega_{max}^2$
$T_0 = A\Omega_{0}^2 = m g$
$\Omega_{max} = 2 \Omega_0$ ... ホバリング回転数の２倍としている。
$T_{max} = 4 T_0$

なので、Vzの運動方程式

$\dot{V_z} = -\frac{T(t)}{m} + g - \frac{d}{m}V_z$

は、入力を与えたところから以下のように表せる。

$\dot{V_z} = -\frac{4 T_{0}}{m} + g - \frac{d}{m}V_z$

$\dot{V_z} = -\frac{4 mg}{m} + g - \frac{d}{m}V_z$

$\dot{V_z} = -3 g - \frac{d}{m}V_z$

ラプラス変換する

$sV_z(s) = -\frac{3g}{s} - \frac{d}{m}V_z(s)$

$V_z(s) = - \frac{-3mg}{s(ms + d)}$

留数計算

$V_z(s) = \frac{A}{s} + \frac{B}{ms + d}$

とすると、

$A = \frac{-3mg}{d}$
$B = \frac{3m^2 g}{d}$

[tmori]

逆ラプラス変換する

$V_z(t) = A u_s(t) + \frac{B}{m} e^{-\frac{d}{m} t} u_s(t)$

展開すると、

$V_z(t) = -\frac{3mg}{d} u_s(t) + \frac{3mg}{d} e^{-\frac{d}{m} t} u_s(t)$

となるので、

第二項は時間が経つと０になり、漸近する値は、

$-\frac{3mg}{d}$

となる。

ここで、m=0.71, g = 9.81, d = 0.05 としているので、代入すると、

-417.906

となるので、シミュレーション結果と一致する。

また、時定数は、第二項で決まるため、 $-d/m$ となり

14.2

秒となる。

[tmori]

プラントモデルの過渡特性は、伝達関数ベースで求めることは困難な気がしてきたし、これ以上やると沼に落ちそうなので、一旦、ペンディング。まとめに入る。