RustでPID制御を書いてみる。微分先行型PID制御を書いてみる

「RustでPID制御を書いてみる」です。

このシリーズでは、南裕樹様が書かれた『Pythonによる制御工学入門』を参考＆題材にして作成させていただいております。

前回は、PID制御の外乱抑制性能、つまり外からの余計のノイズが入力された時にどれだけ抑制してくれるかを見てみました。

今回は、PID制御の応用形である2自由度制御、具体的には微分先行型PID制御（P-ID制御）を書いていきます。

具体的には『Pythonによる制御工学入門』内の図5.19右（160ページ）の3つの線が書かれたグラフを描いていきます。

さて、前回と前々回で記事に取り上げたPID制御ですが、目標値に向かって良い感じに制御をしてくれるのですが、難点もあり、それは”目標値が急変した時（例えばステップ信号）に微分項の影響によって操作量が急変すること”です。落ち着いて考えてみると、微分項があるので目標が急に変わると、それに合わせて微分項が大きくなるのは当然かもしれません。

操作量が急変することはシミュレーションをしている上では大した問題とは思えないかもしれませんが、実際の制御対象（モノ）に対して急激に変化する信号（入力）を加えると、その制御対象物に負荷を与えることになり、寿命や品質などに悪い影響を与えることもあります。なので出来れば操作量は急変してほしくありません。

さて、では操作量が急変することを改善するための微分先行型PID制御（P-ID制御）とはどのようなものでしょうか。コードは下記になります。

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..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_1pid_arr: Vec<f64> = Vec::new();
    let mut x_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);
        x_1pid_arr.push(state_1pid.0);
        x_2pid_arr.push(state_2pid.0);

        // 次の入力値を計算し設定する
        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(x_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(x_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;
}

コードでfn calc_2pid(&mut self, pv: f64) -> f64 {から始まる関数の部分があると思いますが、これが微分先行型PID制御の処理の部分です。（関数名が分かりづらくて恐縮ですが、2自由度の微分先行型（P-ID制御）ということで2pidと書いてます）

この関数内の微分項の計算を見てみると

        let term_d = (self.kd / self.dt) * (-1.0 * (pv - self.pv_prev));

と、1自由度の時は”目標値と測定値との差”を用いていましたが、今回の微分先行型では"測定値"から計算しており、こうすることで目標値が急激に変わったとしても計算に影響がでないようにしています。

さて、このコードで下記のようなグラフが作成されます。 制御対象の出力結果は、1自由度のPIDでも微分先行型でもあまり変化はないように見えます。

今回はここまでです。次回は、入力が2つでどう変わったのかを見ていきたいと思います。