高度制御の理論的導出 #328
Comments
注意：この情報は間違っているので、このスレッドの後を参照。補足情報
|
プラント全体ローターの伝達関数高度運動の伝達関数プラント全体の伝達関数 |
制御側 |
各種パラメータ以下を前提とする
|
L(s) を求める |
閉ループ特性方程式 1+𝐿(𝑠)を求める展開された分子を整理した結果は次の通りです： |
|
うーん、そもそも不安定であることが判明してしまった。まいった。 |
|
わかった！NED座標系だから、PID制御パラメータは全部マイナスにしないといけないということか！ |
調整後のパラメータ{
"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
}
} |
閉ループ伝達関数の極左反平面！ 
|
ステップ応答
|
ボード線図と位相線図
|
箱庭での実験結果このパラメータを使ってもうまくいかなかった。 以下にしたら良い感じだった。 箱庭の制御は高度を正負反転しているので、正の値が正しい 
|
このギャップがどこからくるものなのか？
|
検証のためにミキサーなしでやってみようプラント制御結果伝達関数ベースで導出したパラメータを箱庭ドローンシミュレータに設定したが同じ動きにならなかった。 うーん、やっぱり、根本的なところで何かが違う気がするなー。こまった |
|
あれ？
ですが、これ違ってませんか？ なので、 |
|
|
|
とりあえず、全体の流れを整理して、式をもう一度作ってみた。
|
|
野波先生の 式(2.92) と上記 P(s) は一致するので、よしとする。 |
|
zが下向きの時、u=duty や Ωを上げる方向なので、Kp はマイナス値になるんですね。 |
|
手計算した結果、この式と全く一緒になりました！ |
|
いろいろ Kp, Ki, Kd を触ってみて、再度、理論的に分かったこと。
Kp = -1, Ki=0, Kd = -10 で位相余裕を確保してみた。 
|
箱庭で実験した結果です。測定条件
測定結果Kp=-1, Kd=-1目標高度に到達しましたが、整定時間が長いのと、結構振動しますね。。 
Kp=-1, Kd=-10目標高度に到達しました。整定時間も短くなりましたが、まだ振動しますね。。 
|
|
ちなみに、自分の方で箱庭用にパラメータ調整した結果はこんな感じです。
これに対して、Ki = 0.0にしてみたところ、結果は、ほぼ同じですね。 |
|
ありがとう！ Kp=-1, Ki=0, Kd=-1 で線形と箱庭で比較。 結構線形モデルと挙動が違うなぁ。線形だとダンパが効くけど、箱庭では効いていない。 |
空気抵抗係数は 0.05 で、一致させていますね。 箱庭のパラメータ： |
|
ありがとう。なるほど。そうなると、そもそも線形化したモデルは、箱庭のモデルから乖離している気がするなぁ。。。どこか計算間違っているのかなぁ。二人とも同じ答えになったしなぁ。。。どう詰めていくのかが今は不明です。 |
|
ここで、箱庭のパラメータと合わせている、ということでしたが、 たしか、機体側にもこのパラメータなかったでしたっけ？（確か、x, y, z の３方向で別々に設定していたはず） |
こちらは、機体側のパラメータになりますね。x, y, z の3方向で別々の空気摩擦抵抗設定できるようにする話はまだ未対応ですね。。 |
|
ここは、このコンフィグパラメータを読んでいるということであってますか？ |
はい、あってます。 |
|
箱庭のPID制御プログラム、よくみてみたら誤りがありました。 double calculate(double sp, double input) {
target = sp;
double error = target - input;
integral += error * delta_time;
double derivative = (first_time) ? 0.0 : error - prev_error;
first_time = false;
prev_error = error;
return Kp * error + Ki * integral + Kd * derivative;
}PID制御式(連続時間)： PID制御式(離散時間)： Kdのところ、Δtで割らないとダメですね。。 - double derivative = (first_time) ? 0.0 : error - prev_error;
+ double derivative = (first_time) ? 0.0 : (error - prev_error) / delta_time; |
さらに制御周期は連続系と同じようにシミュレーション周期と一致させてみたら振動が止まりました。ちなみにミキサーエラーはなしでした（Ω^2はマイナスになってない） 
|
Kp=1.0, Kd=0.3の結果Kdが連続系と少しずれてい流？
|
throttle_gainのパラメータが悪さしていました。PID制御で決めた値は以下のように推力に変換されます。 double run_thrust_control(double power) {
return (this->mass * this->gravity) + (this->throttle_gain * power);
}
ここで、 結果： PIDパラメータ： グラフとしては同じようにオーバーシュートしてきました。ただ、応答時間は約10倍くらい遅いのが気になるところ。。 |
ミキサーの種類ミキサーは、 |
伝達関数とシミュレーションの条件を変えてみる伝達関数側 ：ローターを外して、直接、推力を与える 結果、ほぼ同じ振る舞いで、同じ時間スケールに見える。 伝達関数側のステップ応答
シミュレータの応答
|
考察伝達関数側のモデルが、シミュレータの側のものと一致していない可能性があるのでは？ |
シミュレータの処理の流れ（高度制御）
|
ロータの処理double rotor_omega_acceleration(
double Kr /* gain constant */,
double Tr /* time constant */,
double omega, /* in rpm */
double duty_rate /* 0-1 of PWM */)
{
/**
* See Nonami's book (2.48)
* G(s) = Kr/(Tr s + 1).
*/
return (Kr * duty_rate - omega) / Tr;
} |
スラスタの処理double rotor_thrust(
double A, /* the A parameter in Trust = A*(Omega)^2 */
double omega /* in rpm */ )
{
/**
* Nonami's book (2.50)
* T = A * (Omega)^2
*/
return A * (omega * omega);
} |
ミキサー処理 // linearized version
PwmDuty run_linear(double mass, double thrust, double torque_x, double torque_y, double torque_z)
{
const double m = mass; // pass through mass from caller
const int N = ROTOR_NUM;
const double g = GRAVITY;
const double A = param_A;
const double T0 = m*g; // equibilium
const double omega0 = std::sqrt(T0/(N*A)); // equibilium
const double delta_T = thrust - T0;
const double Kr = param_Kr;
glm::dvec4 delta_U = glm::dvec4(delta_T, torque_x, torque_y, torque_z);
glm::dvec4 delta_omega_times_omega0 = 0.5 * MA_inv * delta_U;
glm::dvec4 delta_omega = delta_omega_times_omega0/omega0;
glm::dvec4 omega = omega0 + delta_omega;
PwmDuty duty = {};
for (int i = 0; i < ROTOR_NUM; i++) {
if (omega[i] < 0) {
if (mixer_info.enableErrorLog) {
std::cout << "ERROR: thrust:" << thrust << " tx: " << torque_x << " ty: " << torque_y << " tz: " << torque_z << std::endl;
std::cout << "ERROR: Invalid caluculation of Omega to duty because of omega("<< i << ") is minus value(" << omega[i] << ")..." << std::endl;
}
omega[i] = 0.0;
}
else {
duty.d[i] = omega[i] / Kr;
}
if (mixer_info.enableDebugLog) {
std::cout << "Motor " << i << ": Omega = " << omega[i] << ", PWM Duty = " << duty.d[i] << std::endl;
}
}
return duty;
} |
ブロック線図をもう一度
|
|
抜けてないと思う。位置だけなら、ミックス行列かけても単にN倍なので。 |
プログラムからMixerの伝達関数を求めてみるトルクは０と考えてよくて、 そうすると、こうなるのでは？ |
レッツとらい！プログラムの都合上、Controller側にMixerかけないので、Plant側に書いた。 {
"plants": [
{
"comment": "Mixer",
"num": [
"1"
],
"den": [
"2 * N * A * W0 * Kr"
]
},
{
"comment": "Rotor",
"num": [
"2 * N * A * W0 * Kr"
],
"den": [
"Tr",
"1"
]
},
{
"comment": "Altitude",
"num": [
"-1"
],
"den": [
"m",
"d",
"0"
]
}
],
"controller": {
"num": [
"Kd",
"Kp",
"Ki"
],
"den": [
"1",
"0"
]
},
"constants": {
"Kp": -1,
"Ki": 0.0,
"Kd": -1,
"m": 0.71,
"g": 9.81,
"N": 4,
"d": 0.05,
"A": 8.3e-7,
"Tr": 1.93e-02,
"Kr": 2896,
"W0": 1448,
"U0": "math.sqrt( (m * g) / (N * A * (Kr**2)) )",
"Wc": 1002,
"PM": 90
}
}
|
伝達関数モデルのステップ応答結果ボード線図： ステップ応答： |
箱庭ドローンシミュレータの結果
|
|
おー、ほぼ理論通りの結果になった！！ |
|
ログリプレイしてみた。 2024-09-22.13.02.45.mov |
パラメータ調整してみる。
考察理論ベースでの考察として、
実際、箱庭ドローンシミュレータでシミュレーションしてみると、
トライ理論ベースでパラメータ調整してみた。
位相余裕はほぼそのままで、カットオフ周波数は3倍近くなった！ ステップ応答
箱庭ドローンシミュレータで実験してみる。オーバーシュートがなくなって、整定時間が１秒近く短縮した。
リプレイ結果： 2024-09-22.13.43.21.mov |
|
いい感じですね。理論とシミュレーションが合うのが気持ちいい。最初はカットオプが小さくて、ちょっと反応鈍いと思ったので、徐々に早めて、それでも位相余裕保てていて、とてもいいと思いました。 後で議論したいのは、
これで「抜けている？」のところは、「抜けていない」と重む。
としておき、「PID の出力を ΔT でなく ΔU 」とすれば、吸収できてしまうと思う。つまりPIDの入力は z 残差、出力は ΔUとする。としてしまえば、一気に１つのPIDで、左のミキサーなしで解決可能。 もしかして、ミキサーありなしで共通のPIDを使いたい、ということであれば、そんな設計になるのかと思う。（でも分母分子がキャンセルするだけで、矛盾してしまわないか不安）。つまりミキサーを通す、というのは「プラント都合」なので、そのまま（プラントの入出力に従って）PIDすればよいと思う。 |
|
理解しました！ あとは、この辺りの線形モデル化のドキュメントで説明ですね。 |
|
こちらで管理するので、一旦、クローズ |
設計メモ
ブロック線図
ローター側
ここで、$U(s)$ は、平衡状態からの差分で考える。
ここから、伝達関数を求めるとこうなる。
ちなみに、 $T(s) $ はこうなる。
高度運動
z軸はNED座標系。
対象の運動方程式：
ここで$u(t) = T_0 + \Delta T(t)$ として、 $T_0$ の項は、重力項を打ち消すものとする。
ラプラス変換:
両辺に$ms$ をかけて、右辺の $Z(s)$ を左辺に移動してまとめる。
The text was updated successfully, but these errors were encountered: