RustでPID制御を書いてみる。微分先行型PID制御の入力値を確認してみる
「RustでPID制御を書いてみる」です。
このシリーズでは、南裕樹様が書かれた『Pythonによる制御工学入門』を参考&題材にして作成させていただいております。
前回は、PID制御の応用形である2自由度制御、具体的には微分先行型PID制御(P-ID制御)を書いてみました。
今回は、微分先行型PID制御(P-ID制御)にすることによって入力値がどうなっているかを確認していみます。
具体的には『Pythonによる制御工学入門』内の図5.21左(161ページ)のグラフを描いていきます。
さて、前回、微分先行型PID制御(P-ID制御)は、1自由度のPID制御の「操作量が急変する」ことの課題を改善したものだと書きました。ではどう変わったのかを見ていきたいと思います。
コードは下記です。
use plotters::prelude::*; struct PIDController { sv: f64, // 目標値 kp: f64, // 比例項のゲイン kd: f64, // 微分項のゲイン ki: f64, // 積分項のゲイン integrator: f64, // 積算器 dt: f64, // 時間の刻み幅 err_prev: f64, // 1時点前の"目標値-測定値"" pv_prev: f64, // 1時点前の"目標値-測定値"" } impl PIDController { fn new(dt: f64) -> PIDController { return PIDController { sv: 30.0, kp: 2.0, kd: 0.1, ki: 10.0, dt: dt, err_prev: 0.0, integrator: 0.0, pv_prev: 0.0, }; } /// 1自由度PIDにより次の入力値を求める fn calc_1pid(&mut self, pv: f64) -> f64 { let term_p = self.kp * (self.sv - pv); let term_d = (self.kd / self.dt) * ((self.sv - pv) - self.err_prev); let term_i = self.ki * self.dt * self.integrator; let mv = term_p + term_i + term_d; self.integrator += self.sv - pv; self.err_prev = self.sv - pv; return mv; } /// 微分先行型PIDにより次の入力値を求める fn calc_2pid(&mut self, pv: f64) -> f64 { let term_p = self.kp * (self.sv - pv); let term_d = (self.kd / self.dt) * (-1.0 * (pv - self.pv_prev)); let term_i = self.ki * self.dt * self.integrator; let mv = term_p + term_i + term_d; self.integrator += self.sv - pv; self.err_prev = self.sv - pv; self.pv_prev = pv; return mv; } } struct Plant { h: f64, u: f64, y: f64, v: f64, } impl Plant { fn new(h: f64) -> Plant { return Plant { h, u: 0.0, y: 0.0, v: 0.0, }; } fn get_state(&self) -> (f64, f64) { return (self.y, self.v); } fn set_input(&mut self, val: f64) { self.u = val; return; } fn step(&mut self) { // 「Pythonによる制御工学入門」p.60, 式(3.6)より // dx/dt = v // dv/dt = (1/J) * (u - (mu * v) - (M * g * l * y)) // となる。これらを用いてオイラー法により微小時間後の状態を求める // なお、数値は上記本に記載されているものを用いている let dv_dt = (self.u - (0.015 * self.v) - (0.5 * 9.81 * 0.2 * self.y)) / 0.01; self.y = self.y + (self.v * self.h); self.v = self.v + (dv_dt * self.h); return; } } fn main() { // グラフの基本的な設定をする let rt = BitMapBackend::new("fig.png", (640, 480)).into_drawing_area(); rt.fill(&WHITE).unwrap(); // 背景を白にする let mut ch = ChartBuilder::on(&rt) .margin(25) .x_label_area_size(25) .y_label_area_size(35) .build_cartesian_2d( 0.0..0.5, // x軸の範囲 -1.0..12.0, // y軸の範囲 ) .unwrap(); ch.configure_mesh().draw().unwrap(); // グラフ内にグリッド線を描画 // グラフに描画するデータの生成 let mut t_arr: Vec<f64> = Vec::new(); let mut u_1pid_arr: Vec<f64> = Vec::new(); let mut u_2pid_arr: Vec<f64> = Vec::new(); let t_max: f64 = 2.0; // tの最大値 let points: u64 = 1_000_000; // 計算する点数。精度を確保するために多くしている let mut t: f64 = 0.0; let h = t_max / (points as f64); // 刻み幅 //積分ゲイン0で動かすモデルを作る let mut plant_1pid = Plant::new(h); let mut pid_1pid = PIDController::new(h); //積分ゲイン5で動かすモデルを作る let mut plant_2pid = Plant::new(h); let mut pid_2pid = PIDController::new(h); for _ in 0..points { // 制御対象の状態を取得する let state_1pid = plant_1pid.get_state(); let state_2pid = plant_2pid.get_state(); t_arr.push(t); u_1pid_arr.push(plant_1pid.u / pid_1pid.sv); u_2pid_arr.push(plant_2pid.u / pid_1pid.sv); // 次の入力値を計算し設定する plant_1pid.set_input(pid_1pid.calc_1pid(state_1pid.0)); plant_2pid.set_input(pid_2pid.calc_2pid(state_2pid.0)); // 次の状態を計算する plant_1pid.step(); plant_2pid.step(); t += h; } // データをグラフに描画する(1つ目) let line_1pid = LineSeries::new( t_arr.iter().zip(u_1pid_arr.iter()).map(|(x, y)| (*x, *y)), &RED, ); ch.draw_series(line_1pid) .unwrap() .label("pid") .legend(|(x, y)| PathElement::new(vec![(x, y), (x + 20, y)], &RED)); // データをグラフに描画する(2つ目) let line_2pid = LineSeries::new( t_arr.iter().zip(u_2pid_arr.iter()).map(|(x, y)| (*x, *y)), &BLUE, ); ch.draw_series(line_2pid) .unwrap() .label("pi-d") .legend(|(x, y)| PathElement::new(vec![(x, y), (x + 20, y)], &BLUE)); // 凡例を書く ch.configure_series_labels() .background_style(&WHITE.mix(0.8)) .border_style(&BLACK) .draw() .unwrap(); return; }
前回からの差分は、これまで位置に着目してデータ生成、グラフ描画していたところを入力に置き換えただけで、ほとんど変更しておりません。
では、このコードでどののようなグラフが作成されるでしょうか。 少し見辛いグラフになってしまいましたが、グラフの左端に注目してください。赤線の1自由度pid制御は左端(≒目標が急に変わるタイミング※)で10.0より上の所まで上がってから急激に1.0近くに下がっているかと思います。それに対して青線の微分先行型pid制御では、左端で1.0に上がっているだけで、赤線のような上がり方はしておりません。
※厳密には”目標値が急変した時"を示しているわけではない(最初は動いてない状態なので目標値も何もない)のですが、ほぼ同じことですので"≒"としました。
左端で一瞬上昇しているのは、シミュレーションの仕方(PID制御の計算を行う時間周期)に依る所もあります。今回のコードでは非常に細かい時間間隔にしているために本当に一瞬だけ入力が立ち上がっていますが、例えばPID制御の計算を行う時間間隔が長ければ、その分だけ急変した(この例では非常に大きな)入力値になる時間が増えていきます。
これで微分先行型PID制御(P-ID制御)によって、1自由度のPID制御の「操作量が急変する」ことが改善されたのが示せたかと思います。
今回はここまで。