最適パラメータチューニング about hakoniwa-px4sim

[kenjihiranabe]

空気摩擦の件は前の最大速度と関連していて、速度が上がってしまう、もしくは急に速度が上がってしまう、というのが不安定さを生むような。うーん。

1. 物理式に根本的なミスがある
2. 制御できるパラメータ領域が特定できてない

の2つが可能性なんやけど、、、2が、物理特性として単体テストできるといいのだけど、例えば、

mass 1kg を力1Nであげてるんだから、速度10m/s 以上には1秒では加速できないよ！

みたいな、drone_dynamics class レベルでの物理と意味的テスト。うーん、正解ラベル作るの難しい。

[kenjihiranabe]

この周期、約 0.4/11 = 36 ms ですね、何か心あたりは？ 何かの物理量から出るのか、px4の制御依存なのか、、、バネが要素がドローン内部にないから、px4側とループになった時に振動してるんだろうな。ダンパー(抵抗)欲しい。
そう言えば、並進には風の抵抗入れたが、回転には入ってないね。本には両方入ってなかった気がするが、、

[tmori]

Inertia, Tr を変えながらTakeoffのテストを実行した結果

result.csv

[tmori]

Inertia, Tr を変えながら右へ移動するテストを実行した結果

result.csv

[kenjihiranabe]

これは赤がスコアが高いんですね？
高いの評価基準はなんでしょう？どっかに記述ありますか？

[tmori]

これは赤がスコアが高いんですね？ 高いの評価基準はなんでしょう？どっかに記述ありますか？

赤が高いスコアですね。

評価基準はこちらです。

https://github.com/toppers/hakoniwa-px4sim/tree/main/px4/auto-test#test-result-evaluator

詳細はリンクされているプログラムを見ていただく必要あります。。

[tmori]

(Inertia, Tr) = (0.02, 0.05) と固定して、BとJrを変えながら右へ移動するテストを実行した結果

• components.thruster.parameterB "${p1}e-09"
• components.thruster.parameterJr "${p2}e-08
• p1=B ==> 0.5 1.0 2.0 5.0 10.0 20.0 30.0 40.0 50.0 60.0 70.0 80.0
• p2=Jr ==> 0.1 0.2 0.5 1.0 1.5 2.0 4.0

result.csv

[tmori]

割と安定しているように見えるパラーメタセット

ベースパラメータからの変更点：

• Inertia: 0.02
• Tr: 0.05
• B: 60.0e-09
• Jr: 1.5e-08
• acc.sampleCount: 1

drone_config.json

[tmori]

QGCを使った比較

テスト対象

比較対象コンフィグは以下。

• ベースコンフィグ
• 先にあげたコンフィグ

テスト内容

1st リリースで見えた以下の現象が発生する操作を実行する

「真後ろに飛行指示を出すと、機体が不安定になる」

[tmori]

テスト結果（ベースコンフィグ）

オペレーション内容

1. 左へ移動
2. 斜め右上へ移動
3. 下へ移動
4. 上へ移動

下から上へ移動する際に方向がおかしくなり、元の場所にもどるまでぐるぐる移動していた。

[tmori]

テスト結果（先にあげたコンフィグ）

ベースコンフィグと同じオペレーションを実行したところ、
下から上への移動は改善したように見える。

ただし、まだ不安定感はぬぐえない。

[tmori]

パラメータを微調整していて、ちょっと気づいたこと

• 空気摩擦が小さいと途端に制御が不安定になる（空気摩擦と他パラメータは依存関係があるような気がした）
• 磁気センサのノイズが大きいと、機体の方向が結構ぶれる（0.3はNGだった）
• 磁気センサのノイズが小さいと、PX4がエラーを出す（0.03より小さくはできない）
• 機体が方向転換する場合、静止状態でないと結構乱れる
• パラメータJrを大きくすると、上記の乱れが緩和された（ような気がする）

[tmori]

微調整した結果

new_drone_config.json

差分：

 % diff old_drone_config.json new_drone_config.json 
39c39
<           "inertia": [ 0.01, 0.01, 0.02 ],
---
>           "inertia": [ 0.02, 0.02, 0.04 ],
45c45
<           "Tr": 0.1,
---
>           "Tr": 0.05,
57,58c57,58
<           "parameterB":  5.0e-09,
<           "parameterJr": 1.0e-08
---
>           "parameterB":  60.0e-09,
>           "parameterJr": 5.0e-08
62,63c62,63
<             "sampleCount": 5,
<             "noise": 0.05
---
>             "sampleCount": 1,
>             "noise": 0.03
71c71
<             "noise": 0.05
---
>             "noise": 0.03

[tmori]

仕切り直し

対象パラメータ範囲をもう少し広げてみたほうが良いかもしれない。

• airFrictionCoefficient
• mass_kg
• Inertia
• Tr
• parameterB
• parameterJr

報酬計算

現状、目的地に到着してふらついてないかどうかだけど、もう少し観点を追加したい。

1. 現行通り目的地到着したかどうか
2. チェックポイントを通過したかどうか
3. 移動距離がチェックポイントを全て通った場合の距離と等しいかどうか

3 があることによって、寄り道してない感を出したい。

パラメータ探索方法

パラメータ数が多いので、全探索は難しそう。
遺伝的アルゴリズムか何かを使って評価してみたい。

その他

テスト実行中に、PX4実行プロセスを停止できない現象があったので、自動ツールの終了処理の見直しが必要。

[tmori]

移動距離のスコアを出してみた

distance_result.csv

[tmori]

目標地点で安定しているかどうかのスコア

averaged_result.csv

[tmori]

パラメータ探索は人間の直感でやった方が良いかもと思って、

1. Unity使って、適当なパラメータの起点を探す。
2. 次に、そのパラメータ近辺を全探索ツール使って自動的に調査してもらう。
   を繰り返し始めたら、警告が出ないパラメータがちらほら出てきた。

移動の軌跡も無駄がない。

ただ、Unityで見ていると、結構期待が高周波で振動しているので、もうちょっと良いところがないか探すことにしました。

パラメータセット：

{
  "simulation": {
    "lockstep": true,
    "timeStep": 0.003,
    "logOutputDirectory": "./",
    "logOutput": {
      "sensors": {
          "acc": true,
          "gyro": true,
          "mag": true,
          "baro": true,
          "gps": true    
      },
      "mavlink": {
          "hil_sensor": true,
          "hil_gps": true,
          "hil_actuator_controls": true
      }
    },
    "mavlink_tx_period_msec": {
      "hil_sensor": 3,
      "hil_gps": 3
    },
    "location": {
      "latitude": 47.641468,
      "longitude": -122.140165,
      "altitude": 121.321,
      "magneticField": {
        "intensity_nT": 53045.1,
        "declination_deg": 15.306,
        "inclination_deg": 68.984
      }  
    }
  },
  "components": {
      "droneDynamics": {
          "physicsEquation": "BodyFrame",
          "airFrictionCoefficient": 0.1,
          "inertia": [ 0.002, 0.002, 0.004 ],
          "mass_kg": 0.3,
          "position_meter": [ 0, 0, 0 ],
          "angle_degree": [ 0, 0, 90 ]
        },
        "rotor": {
          "Tr": 0.05,
          "Kr": 8000,
          "rpmMax": 8000
        },
        "thruster": {
          "rotorPositions": [
            {"position": [  0.3,  0.3, 0 ], "rotationDirection":  1.0 },
            {"position": [ -0.3, -0.3, 0 ], "rotationDirection":  1.0 },
            {"position": [  0.3, -0.3, 0 ], "rotationDirection": -1.0 },
            {"position": [ -0.3,  0.3, 0 ], "rotationDirection": -1.0 }
            ],
          "HoveringRpm": 4000,
          "parameterB":  60.0e-09,
          "parameterJr": 5.0e-06
        },
        "sensors": {
          "acc": {
            "sampleCount": 1,
            "noise": 0.03
          },
          "gyro": {
            "sampleCount": 1,
            "noise": 0.01
          },
          "mag": {
            "sampleCount": 1,
            "noise": 0.03
          },
          "baro": {
            "sampleCount": 1,
            "noise": 0.005
          },
          "gps": {
            "sampleCount": 1,
            "noise": 0
          }
        }
  }
}

[tmori]

移動距離の結果

distance_result.csv

[tmori]

最終地点での安定評価

averaged_result.csv

[tmori]

パラメータ探索した結果を一通り見てみたけれど、一番、安定飛行するのは最初に見つけたパラメータ。

そして、そのパラメータでは、ヨー角が小刻みに振動している。。こういう制御って・・

[tmori]

振動現象は謎すぎるので、振動しなさそうなパラメータからまた探索を始めます（自動化できたのでこの点は良いですね）。

ベースパラメータ：

{
  "simulation": {
    "lockstep": true,
    "timeStep": 0.003,
    "logOutputDirectory": "/Users/tmori/project/oss/hakoniwa-px4sim/px4/auto-test/test_resource/logs/",
    "logOutput": {
      "sensors": {
          "acc": true,
          "gyro": true,
          "mag": true,
          "baro": true,
          "gps": true    
      },
      "mavlink": {
          "hil_sensor": true,
          "hil_gps": true,
          "hil_actuator_controls": true
      }
    },
    "mavlink_tx_period_msec": {
      "hil_sensor": 3,
      "hil_gps": 3
    },
    "location": {
      "latitude": 47.641468,
      "longitude": -122.140165,
      "altitude": 121.321,
      "magneticField": {
        "intensity_nT": 53045.1,
        "declination_deg": 15.306,
        "inclination_deg": 68.984
      }  
    }
  },
  "components": {
      "droneDynamics": {
          "physicsEquation": "BodyFrame",
          "airFrictionCoefficient": 0.01,
          "inertia": [ 0.01, 0.01, 0.02 ],
          "mass_kg": 1,
          "position_meter": [ 0, 0, 0 ],
          "angle_degree": [ 0, 0, 90 ]
        },
        "rotor": {
          "Tr": 0.1,
          "Kr": 8000,
          "rpmMax": 8000
        },
        "thruster": {
          "rotorPositions": [
            {"position": [  0.3,  0.3, 0 ], "rotationDirection":  1.0 },
            {"position": [ -0.3, -0.3, 0 ], "rotationDirection":  1.0 },
            {"position": [  0.3, -0.3, 0 ], "rotationDirection": -1.0 },
            {"position": [ -0.3,  0.3, 0 ], "rotationDirection": -1.0 }
            ],
          "HoveringRpm": 4000,
          "parameterB":  5.0e-09,
          "parameterJr": 1.0e-08
        },
        "sensors": {
          "acc": {
            "sampleCount": 1,
            "noise": 0.03
          },
          "gyro": {
            "sampleCount": 1,
            "noise": 0.01
          },
          "mag": {
            "sampleCount": 1,
            "noise": 0.03
          },
          "baro": {
            "sampleCount": 1,
            "noise": 0.005
          },
          "gps": {
            "sampleCount": 1,
            "noise": 0
          }
        }
  }
}

B, Jr を以下の範囲で回します

B：0.5 1.0 2.0 5.0 10.0 20.0　X（e-09)
Jr：1.0 2.0 4.0 8.0 10.0　　　X（e-08）

[tmori]

B=0.5e-09, Jr=8.0e-08

割といい感じに見えたグラフ

あー、姿勢角は積分し続けるから2π超えるのね。。

[tmori]

人回しした結果は、残念ながら、全て全滅でした。。
次は、Tr=0.05にして、同じパラメータ範囲を探索かけます。

[tmori]

探索結果

探索パラメータ

• 固定パラメータ
  • drone_config_base.json
• 可変パラメータ
  • B：0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1.0　X（e-09)
  • Jr：1.0 2.0 4.0 8.0 10.0　　　X（e-08）

探索結果

移動距離

distance_result.csv

目標地点安定性

averaged_result.csv

考察

今回の探索範囲だと、Jrが1.0以下のところが良い結果が得られているように見える。
ただし、結果の再現性がイマイチ。同じパラメータで再度実験を試みたところ、異なる結果が得られた。。（悩ましい

以下、代表的なグラフ：

B=0.4, Jr=1.0

B=1.0, Jr=1.0

[tmori]

次の探索範囲

• 固定パラメータ
  • drone_config_base.json
• 可変パラメータ
  • B：0.4 0.5 0.6 0.7 0.8 0.9 1.0 1.1 1.2 1.3　X（e-09)
  • Jr：0.1 0.2 0.4 0.8 1.0　　　X（e-08）

[tmori]

探索結果

探索結果

移動距離

averaged_result.csv

目標地点安定性

distance_result.csv

考察

• 評価関数としては、移動距離が最小になるものが良い。最終目的地だけだと途中で変なところに行った場合でも点数高くなる
• パラメータ範囲として、すごく狭い領域でしか高得点がでない。別の次元（パラメータ）を変えることでこの領域を広げられそうな気がした。

以下、代表的なグラフ：

B=0.7, Jr=0.1

B=0.9, Jr=0.2

[tmori]

ローター位置の半径が大きすぎるのではないか？

[tmori]

単体テストとして、ドローン力学モデルのテストでチェックした方が良いかも