toppers
Mixer作成
原因が一個わかりました。
M_inv * M が I になりませんでした。 原因は、精度が float だったため。double制度にしたら、Iになりました。
でも、まだ、ASSERTで引っかかっています。。
ASSERTの発生条件：
double thrust = 1.0;
double torque_x = 0.1;
double torque_y = 0.0;
double torque_z = 0.0;
エラー：
ERROR: Invalid caluculation of Omega to duty because of omega^2(0) is minus value(-1.63099e+07)...
Motor 0: Omega^2 = 0, PWM Duty = 0
Motor 1: Omega^2 = 4.89297e+07, PWM Duty = 0.874372
Motor 2: Omega^2 = 4.89297e+07, PWM Duty = 0.874372
ERROR: thrust:1 tx: 0.1 ty: 0 tz: 0
ERROR: Invalid caluculation of Omega to duty because of omega^2(3) is minus value(-1.63099e+07)...
成功パターン
double thrust = 1.0;
double torque_x = 0.01;
double torque_y = 0.0;
double torque_z = 0.0;
Motor 0: Omega^2 = 1.30479e+07, PWM Duty = 0.451524
Motor 1: Omega^2 = 1.95719e+07, PWM Duty = 0.553002
Motor 2: Omega^2 = 1.95719e+07, PWM Duty = 0.553002
Motor 3: Omega^2 = 1.30479e+07, PWM Duty = 0.451524
wolfram との比較結果
glm:
Matrix M_inv:
0.25 -5 5 0.25
0.25 5 -5 0.25
0.25 5 5 -0.25
0.25 -5 -5 -0.25
wolfram
0.0004 -0.008 0.008 0.0004
-1.6e-05 0.008 -0.008 0.0004
-1.6e-05 0.008 0.008 -0.0004
-1.6e-05 -0.008 -0.008 -0.0004
計算ミスっているかも。。
Ω^2が負の値になる件について
- まず逆行列計算によって負の値になるのは数式の誤りではない。
- 与えている推力とトルクが物理的にあり得ない値にしてしまっているからと思われる
- 一方で、物理的にあり得る範囲で推力とトルクを計算することは難しい。
- どうしたら良いか？
案1.
推力とトルクの上限・下限を設定する。
- 推力：
- 上限：2 * T0(ホバリングする推力)
- 下限：0
- トルク：
- 上限：+(イナーシャの値)
- 下限：-(イナーシャの値)
今回わかった問題は以下でコミット：
https://github.com/toppers/hakoniwa-px4sim/commit/ec5085fccbc3da1510861f04fa37804365499bb4
- Wolfram との差分は、1列目の符号を除いて、ぴったり、Wolf*(2500/4) = glm に見える。
- Ω^2 < 0 という値がでることは、モータ逆回転（ccw=-ccw）を意味している（より強いトルクが必要となるとき）。
Ω^2 < 0 という値がでることは、モータ逆回転（ccw=-ccw）を意味している（より強いトルクが必要となるとき）。
はい、その通りで、実験で、そのトルクを出すのに推力を強くしたらどうかなと思って、大きくしたらマイナスにならなくなりました。 ただ、どれだけあげれば良いのかはちょっとわからずです。
Wolfram との差分は、1列目の符号を除いて、ぴったり、Wolf*(2500/4) = glm に見える。
小数点になっていたので、見逃していました。確かに、値は一緒でした。
- 原因がわかったので、一旦、Wolfram は考えなくてよいとする。
- Ω^2 < 0 の解が出たときの対処方は、よくわからない。確かに T を大きく設定しなおすのはありかも。。。
- [T, τ] が出せない値である、と解釈すると、一旦制御不可エラーで返して、上位の状態に応じて目標値を緩めに設定する（p,q,rを上げすぎず徐々に上げるなど？）
＞ちょっと難しいので、結論としては、Ω^2が負になる場合は、０として
これをやると、（本来は一次遅れなのだけど今回は直値計算なので）一瞬でモーターがストップすることになり、それでダメなんじゃないか。。。？現状維持、くらいならなんとかなるかも。もし現状維持ためすなら、前回値を持っておいて（あるいは取得して）一次遅れ計算もしてしまうのもありか。いっぺんにやると良くなさそうなので、もう少し考える。