2014年6月27日金曜日

ライントレーサのPID制御に挑戦



1年半前にライントレーサを作った記事を書きました。
それを撮影したyoutube動画
センサ4つを使い、3段階の条件分岐(センター、左右寄り、左右に外れた)でモーターの出力を制御。
ON/OFF制御と比例制御の間でしょうか。
車体先端を左右に振りながらの走行でいまいち。
当時の記事でPID制御があることには触れていましたが、こちらの記事での実験で自分にもPIDライブラリを使えそうなので、ライントレーサのPID制御に挑戦です。

PID制御とは 制御工学におけるフィードバック制御の一種であり、入力値の制御を出力値と目標値との偏差、その積分、および微分の3つの要素によって行う方法のことである。(wikipedia)

参考:ETロボコンではじめるシステム制御(3):オンオフ制御の欠点を補う「PID制御」とは? (1/2)

車体の基本は当時と同じもの。
TAMIYAのツインモーターギアボックス(FA-130モーター2個付き)
ユニバーサルプレート(2枚セット)
スリムタイヤセット(55mm経)




ボールキャスターは滑りが悪かったり、ビニールテープの段差に引っかかったりしたので、DIYショップにある小型のゴムタイヤキャスターに交換。
ネジ穴がユニバーサルプレートの穴の位置にちょうど合いました。

センサはフォトリフレクタLBR-127HLD x2を前輪と後輪の間に配置。

間隔は、ビニールテープ幅に両方が入るぐらい。
キャスターの穴の位置は合っているが、中心からずれてしまった。
その後、斜めに取り付けることにより、中心に合わせました。
モータードライバはTA7291P x2をシールド化したもの。
シリアル通信の無線化のためにSBDBT(3.3V)を使っています。

arduino以外の回路図
※USBでスケッチをアップロードするとき、SBDBTのTXとarduinoのRX(D0)の接続を切らないといけません。
※各モーター端子間にコンデンサを入れるほうがいいかもしれません

PIDライブラリはこちらからダウンロード
センサ2個の出力をグラフにするとこんなイメージ。

上が進行方向
黄色い四角がセンサで、黒ラインとの位置関係と出力値
赤:センサ0の出力値 val_sensor[0]
緑:センサ1の出力値 val_sensor[1]
水:合計値val_sum

ライン中心付近では合計値が最大になるので、arduinoの起動・リセット時にsetup()内でそれを目標値(Setpoint)と設定します。

ライン中心から逸れると合計値は減っていきますが、それだけでは左右どちらに逸れたかわかりません。
そこでセンサの値の差val_diffの正負で左右を判断、f_rightのフラグを立てます。

現在のセンサ合計値をInputに入れ、Compute()するとPIDライブラリが計算し、結果がOutputに格納されています。
大きく逸れると、大きい値が出力される。
SetOutputLimits(0, power)で出力値の制限。(デフォルトはPWM用に0~255)
powerはモーターの最大出力(%)で、ラインの右側にそれた場合、左側のタイヤの出力を弱めるために、power - Output とし、ライン中心に向かいます。

Kp、Ki、Kdの値で、比例制御、積分制御、微分制御の効き具合が変わってくるのですが、実際に走らせながら設定変更できるようにしています。
シリアルモニタなどから以下を入力。qqqqqqqqqqなら10回分のqコマンド。
q、a: Kpの増減
w、s: Kiの増減
e、d: Kdの増減
r、f: モーター出力の増減
z: シリアルモニタ出力on/off
x: モーターon/off

参考:ETロボコンではじめるシステム制御(4):滑らかで安定したライントレースを実現する (1/2)


SetSampleTime(10)とし、デフォルトの100msから変更。


動画の最初はKp=0.1、Ki=0、Kd=0でP制御から始まり、走らせながら0.01単位で調整。
各パラメータは走行速度によっても最適値は変わるようです。
arduino起動・リセット時にセンサの読み取りをするので、LED点灯3秒間の間に動画のように動かしてください。

PID制御がうまくいくとカーブもスムーズに走行できているのがわかると思います。
直線部分で直進しないのは、なぜなんでしょう。
そのため、カーブに入るタイミングによって大きくコースから外れてしまいます。
3つのパラメータだけの問題で、改良の余地はたくさんありそう。



// ライントレーサー PID制御 by cranberry
//                 with PID library
// http://cranberrytree.blogspot.jp/
//                           2014/6/27

// PID制御
// P Proportinal 比例
// I Integral 積分
// D Derivative 微分

/*
TA7291P
1 GND
2 OUT1   motor
3 NC
4 Vref   10KΩ+Vs    //10KΩ経由でモーター電源+  VrefにPWMを入力するのではなく
5 IN1    PWM or low  //IN1,IN2どちらかにPWMを入力 残りはLOW 回転方向が変わる
6 IN2    PWM or low  //VrefにPWM100%入れても出力がPWMの電圧に制限されてしまう
7 Vcc    logic power 4.5-20V
8 Vs     motor power 0-20V
9 NC
10 OUT2  motor
*/

#include <PID_v1.h>             //http://playground.arduino.cc/Code/PIDLibrary
double Setpoint, Input, Output;
double kp = 0.1;
double ki = 0.0;
double kd = 0.0;
PID myPID(&Input, &Output, &Setpoint, kp, ki, kd, DIRECT);

boolean cwccw;
byte pin_sensor[2] = {A0,A1};  //フォトリフレクタ入力ピン
int val_sensor[2] = {0,0};     //フォトリフレクタ読み取り値
byte pin_sig[2][2] = {         //signal to IN1,IN2
  {10,9},                      //motor0 : pin10,9 PWM+LOW or LOW+PWM → CW or CCW
  {5,6}                        //motor1 : pin 5,6 PWM+LOW or LOW+PWM → CW or CCW
};

boolean f_right;           //ラインの右側を走っているか ラインから完全に外れた時のために記憶
int power = 40;            //モーター出力最大値(%)
int val_max;               //センサ最大合計値
int val_min = 2046;        //センサ最小合計値
boolean monitor = true;    //シリアルモニタ出力on/off
boolean m_stop = false;    //モーターon/off

#define led_0 2
#define led_1 3

void setup() {
  Serial.begin(115200);
  pinMode(pin_sensor[0], INPUT);
  pinMode(pin_sensor[1], INPUT);
  pinMode(pin_sig[0][0], OUTPUT);       //モータードライバ用ピン
  pinMode(pin_sig[0][1], OUTPUT);       //モータードライバ用ピン
  pinMode(pin_sig[1][0], OUTPUT);       //モータードライバ用ピン
  pinMode(pin_sig[1][1], OUTPUT);       //モータードライバ用ピン
  pinMode(led_0, OUTPUT);               //LED
  pinMode(led_1, OUTPUT);
  digitalWrite(led_0, HIGH);
  digitalWrite(led_1, HIGH);

  long st = millis();
  while(millis() - st < 3000){
    val_sensor[0] = analogRead(pin_sensor[0]);
    val_sensor[1] = analogRead(pin_sensor[1]);
    if(val_sensor[0] + val_sensor[1] < val_min){
      val_min = val_sensor[0] + val_sensor[1];   //反射が多い
    }
    if(val_sensor[0] + val_sensor[1] > val_max){
      val_max = val_sensor[0] + val_sensor[1];   //反射が少ない
    }
  }
  Serial.println("");
  Serial.println(val_min);
  Serial.println(val_max);

  delay(2000);
  digitalWrite(led_0, LOW);
  digitalWrite(led_1, LOW);

  myPID.SetOutputLimits(0, power);
  myPID.SetSampleTime(10);
  Input = 0;
  Setpoint = val_max;
  myPID.SetMode(AUTOMATIC);
/*
  //モーターの配線確認用
  motor(0, 50, false);       //モーター0:左 前進
  delay(1000);
  motor_stop(0,0);
  motor(1, 50, false);       //モーター1:右 前進
  delay(1000);
  motor_stop(1,0);
  delay(1000);
*/
}

void loop() {
  if(Serial.available()){
    ser_rcv();
    myPID.SetTunings(kp, ki, kd);
    myPID.SetOutputLimits(0, power);
  }
  for(byte i=0;i < 2;i++){                              //フォトリフレクタセンサー入力
    val_sensor[i] = analogRead(pin_sensor[i]);
  }
  int val_diff = val_sensor[0] - val_sensor[1];
  int val_sum = val_sensor[0] + val_sensor[1];
  Input = val_sum;
  myPID.Compute();

  if(val_sum > val_max / 2 && val_diff > 0){
    f_right = true;
  }
  else if(val_sum > val_max / 2 && val_diff <= 0){
    f_right = false;
  }
  if(f_right == true){                                  //右にずれた
    digitalWrite(led_0, HIGH);
    motor(0, power - Output, false);                    //左側の出力を下げる
    motor(1, power, false);                             //右側は標準出力
    digitalWrite(led_0, LOW);
  }
  else{                                                 //左にずれた
    digitalWrite(led_1, HIGH);
    motor(0, power, false);                             //左側は標準出力
    motor(1, power - Output, false);                    //右側の出力を下げる
    digitalWrite(led_1, LOW);
  }
  if(monitor == true){
    Serial.print(val_sensor[0]);
    Serial.print("\t");
    Serial.print(val_sensor[1]);
    Serial.print("\t");
    Serial.print(val_diff);             //val_sensor[0] - val_sensor[1]
    Serial.print("\t");
    Serial.print(val_sum);              //val_sensor[0] + val_sensor[1]
    Serial.print("\t");
    Serial.print(f_right);
    Serial.print("\t");
    Serial.print(power);                //モーター最大出力(%)
    Serial.print("\t");
    Serial.print(Output);               //0 ~ モーター最大出力
    Serial.print("\t");
    Serial.print(power - Output);       //カーブ内側タイヤのモーター出力
    Serial.print("\t");
    Serial.print(kp, 2);                //Kp
    Serial.print("\t");
    Serial.print(ki, 2);                //Ki
    Serial.print("\t");
    Serial.print(kd, 2);                //Kd
    Serial.print("\t");
    Serial.println("");
  }
}

void motor(byte num, byte pw, boolean cw){
  pw = map(pw, 0, 100, 0, 255);
  if(m_stop == false){
    if(cwccw != cw){
      digitalWrite(pin_sig[num][0],LOW);
      digitalWrite(pin_sig[num][1],LOW);
      delay(1);
    }
    if(cw == true){
      cwccw = true;
      analogWrite(pin_sig[num][0], pw);
      analogWrite(pin_sig[num][1], 0);
    }
    else{
      cwccw = false;
      analogWrite(pin_sig[num][0], 0);
      analogWrite(pin_sig[num][1], pw);
    }
  }
  else{
    motor_stop(0, 0);
    motor_stop(1, 0);
  }
}

void motor_stop(byte num,int dur){
  digitalWrite(pin_sig[num][0],LOW);
  digitalWrite(pin_sig[num][1],LOW);
  if(dur >0){
    delay(dur);
  }
}

void ser_rcv(){
  float k_step = 0.01;
  char c = Serial.read();
  if(c == 'q'){              //Kp +
    kp += k_step;
  }
  else if(c == 'a'){         //Kp -
    kp -= k_step;
    if(kp < 0){
      kp = 0;
    }
  }
  else if(c == 'w'){         //Ki +
    ki += k_step;
  }
  else if(c == 's'){         //Ki -
    ki -= k_step;
    if(ki < 0){
      ki = 0;
    }
  }
  else if(c == 'e'){         //Kd +
    kd += k_step;
  }
  else if(c == 'd'){         //Kd -
    kd -= k_step;
    if(kd < 0){
      kd = 0;
    }
  }
  else if(c == 'r'){         //motor power +
    power += 5;
    if(power > 100){
      power = 100;
    }
  }
  else if(c == 'f'){         //motor power -
    power -= 5;
    if(power < 0){
      power = 0;
    }
  }
  else if(c == 'z'){         //serial monitor output on/off
    monitor = !monitor;
  }
  else if(c == 'x'){         //motor on/off
    m_stop = !m_stop;
  }
}




ノートPCにxubuntuが入っているのですが、CPUが古くてもwindowsに比べたらコンパイル早いです。

環境:arduinoUNO、arduinoIDE1.0.5、win7(64)



0 件のコメント:

コメントを投稿