ローターとthrusterをjmavsimの実装に合わせてみる about hakoniwa-px4sim

[kenjihiranabe]

微分方程式を除いて，線形に回転数とThrustをリンクさせるモデルです．

37d4ef4

thrust_dynamics.hpp の body_thrust と body_torque の呼び出しをそれぞれ，body_thrust_linear, body_torque_linear に変更してください．また，Kr, Tr はそのまま，A,B の値を回転数分の１（1/8000） くらいにして呼び出してください．新しくパラメータを A2, B2 とかコンフィグ追加がいいと思います．
パラメータ名も変更してコミットしました．

[kenjihiranabe]

ありがとう。こちらはKr は利用前提で作りました。(ロータの微分方程式は変えていない)。パラメータとして必要です。

[kenjihiranabe]

8000分の1の話は、2乗を1乗-linearにしたのでだいたい、標準回転数分くらいズレるだろう、の予想の数値です。

[tmori]

rotor_omega_accelerationは、そのまま利用できることをやっと理解できました。

https://github.com/PX4/jMAVSim/blob/66b764ada522893c05224950aa6268c809f8e48a/src/me/drton/jmavsim/Rotor.java#L22

の指数関数部分をテーラー展開して１次微分の項だけ取れば、こちらの数式と一致するということですね。

[kenjihiranabe]

そうです！ また、今の定数Krがそれなりに感覚ができてるので崩さない方がいいかと思いそうしてます。jmav の式にするのは簡単ですので、必要なら一回まわしてから、と思いました。 2024年1月10日(水) 9:34 Takashi Mori ***@***.***>:

…

rotor_omega_accelerationは、そのまま利用できることをやっと理解できました。 https://github.com/PX4/jMAVSim/blob/66b764ada522893c05224950aa6268c809f8e48a/src/me/drton/jmavsim/Rotor.java#L22 の指数関数部分をテーラー展開して１次微分の項だけ取れば、こちらの数式と一致するということですね。 — Reply to this email directly, view it on GitHub <#125 (comment)>, or unsubscribe <https://github.com/notifications/unsubscribe-auth/AAILCJIZA4J3J5CBC3ZX44DYNXOZJAVCNFSM6AAAAABBSSDGNGVHI2DSMVQWIX3LMV43OSLTON2WKQ3PNVWWK3TUHMYTQOBUGAYDQOBRGY> . You are receiving this because you commented.Message ID: ***@***.***>

[tmori]

A2 の求め方

理想状態で、ホバリングするT_0：

dot_w = -T/m + g = 0
→
T_0=g*m

ホバリングするlinearのTは、
T_0=4 * A2 * w_0

なので、A2は、
A2 = T_0 / (4 * w_0)
→
A2 = g*m / (4 * w_0)
となる。

g=9.81, m=0.1kg, w_0=4000rpmとした場合、
A2=0.0000613125

[kenjihiranabe]

これ，PID でうまく行っているので，プラント側はクローズでどうでしょう？

[tmori]

linear用のパラメータ仕様を検討しました。

rotor

パラメータ	利用有無	linearの設定値	備考
Tr	O	0.1	そのまま利用
Kr	O	8000	そのまま利用
rpmMax	x	-	利用しない（あっても無視する）
vendor_name	O	linear	新規追加、"nonlinear" or "linear"

thruster

パラメータ	利用有無	linearの設定値	備考
rotorPositions	O	現行通り	そのまま利用
HoveringRpm	x	-	利用しない（あっても無視する）
parameterB	x	-	利用しない（あっても無視する）
parameterJr	x	-	利用しない（あっても無視する）
parameterB_linear	O	1/8000	新規追加
parameterA_linear	O	1?	新規追加
vendor_name	O	linear	新規追加、"nonlinear" or "linear"

[tmori]

ありがとう。こちらはKr は利用前提で作りました。(ロータの微分方程式は変えていない)。パラメータとして必要です。

了解です！修正しました。

[tmori]

8000分の1の話は、2乗を1乗-linearにしたのでだいたい、標準回転数分くらいズレるだろう、の予想の数値です。

了解です。設定値も追加しました。

[tmori]

コンフィグ

      "droneDynamics": {
          "physicsEquation": "BodyFrame",
          "collision_detection": false,
          "airFrictionCoefficient": 0.0001,
          "inertia": [ 0.0000625, 0.00003125, 0.00009375 ],
          "mass_kg": 0.1,
          "body_size": [ 0.1, 0.1, 0.01 ],
          "position_meter": [ 0, 0, 0 ],
          "angle_degree": [ 0, 0, 0 ]
        },
        "rotor": {
          "Tr": 0.1,
          "Kr": 6000,
          "rpmMax": 8000
        },
        "thruster": {
          "vendor": "linear",
          "rotorPositions": [
            {"position": [  0.05,  0.05, 0 ], "rotationDirection":  1.0 },
            {"position": [ -0.05, -0.05, 0 ], "rotationDirection":  1.0 },
            {"position": [  0.05, -0.05, 0 ], "rotationDirection": -1.0 },
            {"position": [ -0.05,  0.05, 0 ], "rotationDirection": -1.0 }
            ],
          "parameterA_linear": 0.0000613125,
          "parameterB_linear": 0.000001666666667
        },

[tmori]

調整結果

B2ですが、1/Kr だと、機体の振動が激しく、途中で変な動きになりました。
なので、0.01/Krにしたところ、安定して移動することを確認しました。
B2は、小さい方が安定することがわかってきましたので、どのあたりが良さげなパラメータになりそうな調査してみます。

[kenjihiranabe]

僕の目算は、前のBの値の1/8000です。 2024年1月10日(水) 13:04 Takashi Mori ***@***.***>:

…

調整結果 B2ですが、1/Kr だと、機体の振動が激しく、途中で変な動きになりました。 なので、0.01/Krにしたところ、安定して移動することを確認しました。 B2は、小さい方が安定することがわかってきましたので、どのあたりが良さげなパラメータになりそうな調査してみます。 — Reply to this email directly, view it on GitHub <#125 (comment)>, or unsubscribe <https://github.com/notifications/unsubscribe-auth/AAILCJMDVGKFTKOSGJCNYQTYNYHMZAVCNFSM6AAAAABBSSDGNGVHI2DSMVQWIX3LMV43OSLTON2WKQ3PNVWWK3TUHMYTQOBUGE2TONJYGY> . You are receiving this because you commented.Message ID: ***@***.***>

[tmori]

では、このパラメータセットを試してみます。

Kr=8000
B2=1/8000

[kenjihiranabe]

あ，ごめんなさい，前動いていた parameterB * (1/8000) => parameterB_linear の意味です．

[tmori]

あ，ごめんなさい，前動いていた parameterB * (1/8000) => parameterB_linear の意味です．

はい、それで実施してみました。が、、姿勢角の値が発散しちゃいましたね。。

(Rx, Ry, Rz) = (inf,nan,inf)

[kenjihiranabe]

Kr = 前動いていた Kr の値で．

[tmori]

Kr = 前動いていた Kr の値で．

ということは、
Kr=6000
parameterB_linear=1/8000
ですかね？

[tmori]

30.0e-10,

[tmori]

30.0e-10 x 8000 の値を使ってみます。

[tmori]

パラメータ設定の目安

前回安定していたパラメータセット：#117 (comment)

B=3.00E-10

• Kr: 8000（固定）
• Aは固定(rpm4000でホバリング）
• Tr: 00.5-0.15 (2-20倍あればOK)
• B_linearは、前回の値の8000倍くらい: 2.40E-06

上記で、以下のパラメータセットだとシミュレーション動きました。

Kr=8000
Tr=0.05
A2=0.0000613125
B2=2.40E-06

[tmori]

ベースパラメータ

{
  "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",
          "collision_detection": false,
          "airFrictionCoefficient": 0.0001,
          "inertia": [ 0.0000625, 0.00003125, 0.00009375 ],
          "mass_kg": 0.1,
          "body_size": [ 0.1, 0.1, 0.01 ],
          "position_meter": [ 0, 0, 0 ],
          "angle_degree": [ 0, 0, 0 ]
        },
        "rotor": {
          "Tr": 0.05,
          "Kr": 8000,
          "rpmMax": 8000
        },
        "thruster": {
          "vendor": "linear",
          "rotorPositions": [
            {"position": [  0.05,  0.05, 0 ], "rotationDirection":  1.0 },
            {"position": [ -0.05, -0.05, 0 ], "rotationDirection":  1.0 },
            {"position": [  0.05, -0.05, 0 ], "rotationDirection": -1.0 },
            {"position": [ -0.05,  0.05, 0 ], "rotationDirection": -1.0 }
            ],
            "parameterA_linear": 0.0000613125,
            "parameterB_linear":  2.40E-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.01
          },
          "gps": {
            "sampleCount": 1,
            "noise": 0
          }
        }
  }
}

[tmori]

パラメータ探索範囲

Tr = 0.05 0.07 0.1 0.12 0.15
B_linear: 0.01E-06 0.1E-06 1.0E-06 2.0E-06 2.4E-06

[tmori]

探索結果

distance_result.csv

[tmori]

一番良い結果

Tr=0.12
B2=2.0e-06

元々、B2=2.4e-06 だったので、良い線いっているのでは？
と思ったけど、振動するケースだった。

[tmori]

２番目に良い結果

Tr=0.05
B2=2.0e-06

[tmori]

所感

linearにしても、全体的に高得点は出なかった。
あと、コントローラで操作してみたが、目的の方向への移動に対して、横揺れが大きく、操作しづらい感があった。
これは nonlinearでも同様にあった。

もう少しキビキビ動くパラメータを見つけたい。

ひとまず、以下あたりをさらに探索してみる。

Tr: 0.08 0.09 0.11
B2: 1.1 1.2 1.3 1.4 1.5

[kenjihiranabe]

今日は手を出せないですが、気になるなら、そちらで例のロータの一次微分式、exp バージョンに変えて、感触試してみてみてもいいよ。
あと、並進のdrag ですが、vでなくてv^2 を使ってるモデルをネットで発見しました。でもこれも良くなる確信がぜんぜんないや。
姿勢角度の積算に、勘違いがあったので、これ直したらもしかしたら、うまく行くかも。

[tmori]

今日は手を出せないですが、気になるなら、そちらで例のロータの一次微分式、exp バージョンに変えて、感触試してみてみてもいいよ。

了解です。考えてみます。

あと、並進のdrag ですが、vでなくてv^2 を使ってるモデルをネットで発見しました。でもこれも良くなる確信がぜんぜんないや。 姿勢角度の積算に、勘違いがあったので、これ直したらもしかしたら、うまく行くかも。

これは、期待大ですね。

[tmori]

最新の探索結果。新しい良い結果はありませんでしたね。

distance_result.csv

[tmori]

ちょっと手詰まり感ありますが、いくつか試してみます。

1. 機能追加：jmavsim実装のローター機能を作って差し替えできるようにする
2. 機能改善：A2のパラメータはHoveringRpmで決まるので、A2のパラメータは削除して自動計算する

[tmori]

jmavsimの修正しました。試してみましたが、みた感じ、同じ動きに見えましたが、念の為パラメータ探索を昨日と同じ範囲でやってみます。

0777e75

[tmori]

いいと思います！