PID制御にチャレンジしてみた - すだちキャンパス

こんにちは。
夏休みのイベントで、Arduinoに触る機会があったのでそこでPID制御にチャレンジしてみました。なぜPID制御かというと、お題が「Arduinoを使ってライントレースしよう！」だったからです。ちなみに去年も同じイベントに参加したのですが、その時もライントレース(LEGO)でした。なんで毎年ライントレースなんだ・・・？

ライントレースについて

基本的なライントレースの仕組みは、カラーセンサで黒と白を判別し、それによって各車輪に命令を出すことで制御するといった感じになります。今回は、カラーセンサを二つ用いたので図のような形で行いました。

PID制御とは

比例+積分+微分制御のことです。上で述べたライントレースの仕組みだけだと、動きがカクカクしてあまりスムーズにライントレースを行うことができません。そこでまず出てくるのがP制御です。色の値によって、車輪がどれくらい動くかを調整することになります。
しかし、これだけだと今度は安定せず、ふらつきが大きくなってしまうことがあります。そこで今度はI制御です。誤差を累積して、ある一定以上になると操作量を増やすようにします。
しかし、これでは急カーブなどに対応できません。ここで最後に出てくるのがD制御です。前回と今回の差を見ることで、操作量をあらかじめ補正しておいてカーブに対応します。
・・・というのが今回のライントレースのために調べて得た知識です。ちゃんと本とか読んだ訳ではないのでもしかしたら厳密には間違ってるところがあるかも・・・

まあなんにせよこんな感じになるようにコードを書きました。また、これだけだと急カーブで勢い余ってはみ出してしまうことが多かったので、工夫としてはみ出しそうになったら(=カラーセンサの読み取る値が両方白になったら)バックして戻るようにしました。

書いたコード

#include "ESC2nd.h"

Sensor sensor;
Motor motorR('r');
Motor motorL('l');


int w_min=3500;
float p=0.13;
float i=0.03;
float d=0.15;
float Lzure_old=0;
float Rzure_old=0;
float Lspeed=90;
float Rspeed=90;

void setup() {
  Serial.begin(9600);
  delay(3000);
}

void loop() {
  int value[8];
  sensor.read(value);
  float Lzure = value[1]-w_min;
  float Rzure = value[3]-w_min;

  if(Lzure+Lzure_old<=1000){
    Lspeed = 60 + abs(Lzure)*p + abs(Lzure+Lzure_old)*i + abs(Lzure-Lzure_old)*d;
  }else{
    Lspeed = 60 + abs(Lzure)*p + abs(Lzure-Lzure_old)*d;
  }
  if(Rzure+Rzure_old<=1000){
    Rspeed = 60 + abs(Rzure)*p + abs(Rzure+Rzure_old)*i + abs(Rzure-Rzure_old)*d;
  }else{
    Rspeed = 60 + abs(Rzure)*p + abs(Rzure-Rzure_old)*d;
  }

  Serial.print(Lspeed);
  Serial.print(" ");
  Serial.print(Rspeed);
  Serial.println();

  if(w_min<=value[1]){ //left white
    if(w_min<=value[3]){ //right white
      motorR.drive(95);
      motorL.drive(95);
    }else{ //right black 右折
      motorR.drive(-10);
      motorL.drive(Lspeed);
    }
  }else{ //left black 
    if(w_min<=value[3]){ //right white
      motorR.drive(Rspeed);
      motorL.drive(-10); 
    }else{ //right black 
      motorR.drive(95);
      motorL.drive(95);
    }     
  }

  if(w_min<=value[1] && 4000<=value[2] && w_min<=value[3]){
    motorR.drive(-40);
    motorL.drive(-40);
    if(value[2]<w_min){
      if(value[1]<w_min){
        motorR.drive(10);
        motorL.drive(-30);
      }else if(value[3]<w_min){
        motorR.drive(-30);
        motorL.drive(10);
      }
    }
  }
  
  Lzure_old=Lzure;
  Rzure_old=Rzure;
}

このほかにも、黒と白が入れ替わった場合のライントレースなどもできるように頑張ったのですが、6割くらいの確率でしか成功しなかったので結局本番では使いませんでした(ちょっと残念)。でも予選(個人)1位、本番(チーム)2位だったのでそこそこ満足です。
Arduinoは結構簡単に動いてくれて楽しかったので、また機会があれば触ってみたいですね。