(2020.09.14 がっつり記事を書き直しました)
# PID制御の説明
DCモーターの位置や速度の制御をPIDで行います。
位置制御の場合、DCモータの角度をエンコーダで読み取り、
取得した現在の角度と、目標の角度の差を偏差と呼びますが、
偏差にある一定の値(Kp)をかけたことを比例(P : proportional)と呼びます。
さらに、計算するごとに偏差を足していくことを積分(I : integral)と呼び、
一回前に計算した偏差と、現在計算している偏差の差を微分(D : differential)と呼びます。
PID制御では、これら比例, 積分, 微分に対して、それぞれにある一定の値(Kp, Ki, Kd)をかけて、足し合わせた値をモーターへの入力とします。
つまり、Kp*P + Ki*I + Kd*Dです。
まあ、あとは実際にプログラムを見るのが手っ取り早いかと思います。
# 用意するもの
- Arduino
- DCモータ
- モータドライバ 今回は()を使います。
# まずは位置のP制御
以下のようなプログラムがP制御を行います。
以下の部分、MotorDriverとかEncoderとかいうクラスは存在しないのでエラーがでることに注意してください。
MotorDriver motor_driver(); Encoder encoder();
// 比例ゲインKp float Kp = 0.6; // 目標角度[rad] float target = 3.14; MotorDriver motor_driver(); Encoder encoder(); void setup() { motor_driver.init(); encoder.init(); } void loop() { // エンコーダから現在の角度を取得 float current_rad = encoder.getEncoderRadians(); float err = target - current_rad; float P = Kp * err; motor_driver.setMotorSpeed( P ); }
# 位置のPID制御
float Kp = 0.6; // 比例ゲインKp float Ki = 0.1; // 比例ゲインKi float Kd = 0.1; // 比例ゲインKd float target = 3.14; // 目標角度[rad] MotorDriver motor_driver(); Encoder encoder(); void setup() { motor_driver.init(); encoder.init(); } void loop() { static float integral = 0; static float last_err = 0; static unsigned long last_micros = 0; float current_rad = encoder.getEncoderRadians(); // エンコーダから現在の角度を取得 float err = target - current_rad; // 偏差を計算 float P = Kp * err; // Pを計算 unsigned long current_micros = micros(); // 現在の時間を取得 float dt = ((float)(current_micros - last_micros)) / 1000000.0; // 経過時間を計算 integral += err * dt; // 偏差の積分を計算 float I = Ki * integral; // Iを計算 float diff = (err - last_err) / dt; // 偏差の微分を計算 float D = Kd * diff; // Dを計算 motor_driver.setMotorSpeed( P+I+D ); // PIDでモータを制御 last_err = err; // 現在の偏差を保存 last_micros = current_micros; // 現在の時間を保存 }