RustでPID制御を書いてみる。P制御を実装する
前回までで、制御モデルの数値計算を行い、その結果を図にするところまで行いました。
今回はようやくPID制御の実装をはじめていきたいと思います。
さて、このシリーズの最初に、南裕樹様が書かれた『Pythonによる制御工学入門』を参考にさせていただいてると書きました。
今回はその5.2.1(144ページ)の内容、比例成分のみを使ったP制御(比例制御)の実装をします。
比例制御の処理は、目標値と現在値との差に比例した操作量を算出するというシンプルな内容です。
全体のコードは下記です。
use plotters::prelude::*; struct PIDController { sv: f64, // 目標値 kp: f64, // 比例項のゲイン } impl PIDController { fn new() -> PIDController { return PIDController { sv: 0.0, kp: 0.0 }; } /// 目標値を設定する fn set_reference(&mut self, val: f64) { self.sv = val; return; } /// 比例ゲインを設定する fn set_proportional_gain(&mut self, val: f64) { self.kp = val; return; } /// 次の入力値を求める fn calc(&mut self, pv: f64) -> f64 { let term_p = self.kp * (self.sv - pv); let mv = term_p; 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..2.0, // x軸の範囲 -0.1..50.0, // y軸の範囲 ) .unwrap(); ch.configure_mesh().draw().unwrap(); // グラフ内にグリッド線を描画 // グラフに描画するデータの生成 let mut t_arr: Vec<f64> = Vec::new(); let mut x_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); // 刻み幅 let mut plant = Plant::new(h); let mut pid = PIDController::new(); pid.set_reference(30.0); // 制御器の目標値を設定する pid.set_proportional_gain(2.0); // 制御器の比例ゲインを設定する for _ in 0..points { // 制御対象の状態を取得する let state = plant.get_state(); t_arr.push(t); x_arr.push(state.0); // 次の入力値を計算し設定する let next = pid.calc(state.0); plant.set_input(next); // 次の状態を計算する plant.step(); t += h; } // データをグラフに描画する(1つ目) let line = LineSeries::new(t_arr.iter().zip(x_arr.iter()).map(|(x, y)| (*x, *y)), &RED); ch.draw_series(line) .unwrap() .label("x") .legend(|(x, y)| PathElement::new(vec![(x, y), (x + 20, y)], &RED)); // 凡例を書く ch.configure_series_labels() .background_style(&WHITE.mix(0.8)) .border_style(&BLACK) .draw() .unwrap(); return; }
まず見ていただきたいのは、main関数のfor文(各時点での状態を計算している部分)です。
let state = plant.get_state();
最初に制御モデルの状態(位置)を取得します。実際の制御ですとここでセンサなどの値を読み取ります。
次に、数行進んで
let next = pid.calc(state.0);
によって次の制御モデルへの入力値を計算しています。pid_calc
では、今の状態(位置)を引数にして実行すると、それを元にP制御を行い、その結果を返してくれます。
そして、
plant.set_input(next);
によって制御モデルへ次の入力値を指示しています。
数値計算で制御のシミュレーションをする際や組込みなど実際の制御ではこのように書くことも多いのかなと思います。
さて、上記のコードを実行すると下の図のようになります。
今回は比例ゲインを2.0にしましたが、『Pythonによる制御工学入門』の図5.7(145ページ)のKp=2の線と一致しているかと思います。
今回はここまで。