Arduinoとフォトリフレクタ６個でPID制御を使ったラインフォロワーロボットの製作

　　完成ビデオ

　次の動画は、A3の厚紙６枚に２cm幅のラインを作り、その上をトレースしているビデオです。

　　完成写真

　タイヤが厚紙上で空回転するのでゴムバンドを付けています。

操作方法は、電源スイッチ投入後、キャリブレーションスイッチを押すとArduino 上のLED (通常はピン 13) がオンになります。
　その後、Arduino上のLEDが消えるまでの間に、ロボットを手動で動かし、ラインを横切ってセンサーをスライドさせます。Arduino 上のLEDが点灯する時間は約１０秒で、その間にキャリブレーションします。
　キャリブレーションが終了したら、ロボットをライン上に置き、スタートスイッチを押してロボットを起動させます。

ページの先頭へ

　　使用部品

　ほとんどの主要な部品はAliExpressで購入したものです。
＊フォトリフレクタLBR127HLD ６個及び基板
＊Arduino NANO
＊モータードライバーDRV8835
＊ギヤードモーター及びタイヤ２セット
＊4800mAh充電式3.7Vリチウムイオン16340バッテリー
＊5V、3.3V出力充電式16340リチウム電池用シールドモジュール
＊006P形7.2Vニッケル水素充電池
＊キャスター１個
＊プッシュスイッチ２個及びプッシュロックスイッチ１個
＊ユニバーサル基板
＊抵抗、トランジスタ2SC2655
＊ネジ類

ページの先頭へ

　　回路図

　次に、回路図を表示します。
　QTRSensorsライブラリーを使用する場合、本来ならばPololuのQTR-8RC(又はQRC-8A)フォトリフレクタアレイを使用するところですが、フォトリフレクタLBR127HLDが手持ちにあったので、フォトリフレクタアレイを自作しました。

　また、電源は、Arduinoとフォトリフレクタアレイは５Ｖ電源を使用し、モーター用の電源は、 006P形7.2Vニッケル水素充電池を使用し、２つに分けました。

ページの先頭へ

　　プログラム

　以下がプログラムです。

						
#include <QTRSensors.h>

QTRSensors qtr;
const uint8_t SensorCount = 6;
uint16_t sensorValues[SensorCount];

//Kp,Ki,Kd値は現状に応じて調整、設定します。
float Kp = 0;
float Ki = 0;
float Kd = 0;
int P;
int I;
int D;

int lastError = 0;
boolean onoff = false;

const uint8_t maxspeeda = 150;
const uint8_t maxspeedb = 150;
const uint8_t basespeeda = 100;
const uint8_t basespeedb = 100;

int mode = 8;
int aphase = 9;
int aenbl = 6;
int bphase = 5;
int benbl = 3;

int SWcalibrate = 4;
int SWstart = 2;

void setup() {
  Serial.begin(9600);
  qtr.setTypeAnalog();
  qtr.setSensorPins((const uint8_t[]){A0, A1, A2, A3, A4, A5}, SensorCount);
  qtr.setEmitterPin(7);//LEDON PIN

  pinMode(mode, OUTPUT);
  pinMode(aphase, OUTPUT);
  pinMode(aenbl, OUTPUT);
  pinMode(bphase, OUTPUT);
  pinMode(benbl, OUTPUT);
  digitalWrite(mode, HIGH);

  delay(500);
  pinMode(LED_BUILTIN, OUTPUT);

  boolean Ok = false;
  while (Ok == false) {
    if(digitalRead(SWcalibrate) == HIGH) {
      calibration();
      Ok = true;
    }
  }
  forward_brake(0, 0);
}

void calibration() {
  digitalWrite(LED_BUILTIN, HIGH);
  for (uint16_t i = 0; i < 400; i++)
  {
    qtr.calibrate();
  }
  digitalWrite(LED_BUILTIN, LOW);
}

void loop() {
  if(digitalRead(SWstart) == HIGH) {
    onoff =! onoff;
    if(onoff = true) {
      delay(1000);
    }
    else {
      delay(50);
    }
  }
  if (onoff == true) {
    PID_control();
  }
  else {
    forward_brake(0,0);
  }
}

void forward_brake(int posa, int posb) {
  digitalWrite(aphase, LOW);
  digitalWrite(bphase, HIGH);
  analogWrite(aenbl, posa);
  analogWrite(benbl, posb);
}

void PID_control() {
  uint16_t position = qtr.readLineBlack(sensorValues);
  int error = position - 2500;

  P = error;
  I = I + error;
  D = error - lastError;
  lastError = error;
  int motorspeed = P*Kp + I*Ki + D*Kd;
  
  int motorspeeda = basespeeda + motorspeed;
  int motorspeedb = basespeedb - motorspeed;
  
  if (motorspeeda > maxspeeda) {
    motorspeeda = maxspeeda;
  }
  if (motorspeedb > maxspeedb) {
    motorspeedb = maxspeedb;
  }
  if (motorspeeda < 0) {
    motorspeeda = 0;
  }
  if (motorspeedb < 0) {
    motorspeedb = 0;
  } 
  
  forward_brake(motorspeeda, motorspeedb);
}

ページの先頭へ

　　プログラムの概略

１．QTRSensorsライブラリーをArduino IDEにインストールします。
「ツール」、「ライブラリの管理」をクリックし、「ライブラリマネージャ」でQTRSensorsをインストールします。

２．「スケッチ」、「ライブラリをインクルード」をクリックして、インストールしたQTRSensorsをインクルードします。
#include <QTRSensors.h>

３．QTRSensorsでは、黒ライン上にある６個のセンサーの position の値を1000単位で次のようにしています。

　例えば、A2に接続されたセンサーが黒ライン上にある場合は、position=2000 となります。
uint16_t position = qtr.readLineBlack(sensorValues);　で黒ライン上にあるセンサーのpositionの値を取得し、
int error = position - 2500;　から中央値2500との差の±でerrorの値を決めています。

４．QTRSensorsのキャリブレーションについて　セットアップ段階では、センサーを 10 秒間キャリブレーションし、キャリブレーション中に Arduino 上の LED (通常はピン 13) をオンにします。
　この10 秒間に、各センサーがラインの暗さと地面の明るさを読み取ることができるように、キャリブレーションフェーズ中にラインを横切ってセンサーをスライドさせる必要があります。
　このようにセンサーを、最も明るい測定値と暗い測定値に遭遇させる必要があります。キャリブレーションされたセンサー値を読み取り、それらを使用してラインの位置を推定します。

５．次のプログラムで、キャリブレーションフェーズ中にラインを横切ってセンサーをスライドさせ、各センサーの読み取り値をシリアルモニターで確認してください。

	
#include <QTRSensors.h>

QTRSensors qtr;

const uint8_t SensorCount = 6;
uint16_t sensorValues[SensorCount];

void setup()
{
  qtr.setTypeAnalog();
  qtr.setSensorPins((const uint8_t[]){A0, A1, A2, A3, A4, A5}, SensorCount);
  qtr.setEmitterPin(7);

  delay(500);
  pinMode(LED_BUILTIN, OUTPUT);
  digitalWrite(LED_BUILTIN, HIGH);
  
  for (uint16_t i = 0; i < 400; i++)
  {
    qtr.calibrate();
  }
  digitalWrite(LED_BUILTIN, LOW);
  
  Serial.begin(9600);
  for (uint8_t i = 0; i < SensorCount; i++)
  {
    Serial.print(qtr.calibrationOn.minimum[i]);
    Serial.print(' ');
  }
  Serial.println();
  
  for (uint8_t i = 0; i < SensorCount; i++)
  {
    Serial.print(qtr.calibrationOn.maximum[i]);
    Serial.print(' ');
  }
  Serial.println();
  Serial.println();
  delay(1000);
}

void loop()
{
  uint16_t position = qtr.readLineBlack(sensorValues);
  for (uint8_t i = 0; i < SensorCount; i++)
  {
    Serial.print(sensorValues[i]);
    Serial.print('\t');
  }
  Serial.println(position);

  delay(250);
}

ページの先頭へ

　　PID制御とその調整

１．PID制御について
　基本的には、センサーアレイの設定値（ラインの理想的な位置）とラインの現在の位置を比較します。得られたエラーは、ロボットがそれぞれ右に曲がるか左に曲がるかを決定します。
　コードを改善する目的は、ラインフォロワーを、振動したりラインから外れたりすることなく、どのトラックでも高速で移動するロボットにすることです。
　最良の解決策は、ロボットを軌道に乗せるのに役立つPID制御（下図）です。

　これはフィードバックベースの制御システムであり、（設定値とセンサーアレイの実際の値の間の）誤差を継続的に計算し、比例（P）、積分（I）、微分（D）の3つの制御項に基づいて補正を行います。

　これらの3つの制御項を合計して、PIDコントローラーの出力（この場合はモーター速度）を計算します。各用語には独自の目的があり、ロボットが線をたどる方法に異なる影響を与えます。それらは、互いに乗算される定数を伴います。結局、モーター速度に値を割り当てるコード行は次のとおりです。
int motorspeed = P*Kp + I*Ki + D*Kd;

　フォロワーラインコードを完成させるには、各制御項の定数を見つけることが最も重要な部分です。それらは任意の値を持つことができます。たとえば、私のロボットでは、Kpを0.12、Kiを0.002、 Kdを0.08としました。値の変更は、Arduino IDE上で手動で行うか、Bluetoothモジュールを追加することにより直接これらの値を調整できます。

（１）比例項は、設定値と現在の位置の間の誤差に正比例する値を示します。ロボットがカーブを描く方法やライン上を移動する方法を直接制御します。
　線が中央にあるほど、ロボットは直進します。線がセンサーの端に近いほど、回転が速くなります。ただし、ロボットが振動するため、この定数だけを使用するだけでは不十分です。
　比例定数（Kp）は、モーターの速度感度を設定します。次のコード行は、比例項の設定であり、それぞれの定数を掛けることはありません。
int P = error;

（２）積分項は、（プログラムが開始されてからの）一定期間のエラーの合計です。
　結果は、ロボットがトラック上でどれだけ修正されなければならなかったかを示す値です。エラーが0の場合、それは増加の必要がなく、ロボットが正しい経路にあることを意味します。
　その後、積分ゲイン（Ki）を掛けます。積分項は、ラインフォロワーロボットが曲線をよりスムーズに取るのに役立ち、ロボットがラインの中心に来るのを速くします。
int I = I + error;

（３）微分項は、センサーアレイの最後の値と実際の値の差です。
　これにより、制御システムはエラーの変化に対してより強力に反応し、ロボットをより短時間で安定させることができます。また、微分ゲイン（Kd）を掛けることができ、ロボットが突然タイトなカーブを描くのに役立ちます。
int D = error - lasterror;

（４）ただし、ロボットの制御に1つの用語しか使用されていない場合、正しく機能しません。
軌道に乗ったままですが、振動します。各用語にはそれぞれの役割があり、組み合わせると、調和して脱線することなく高速でラインを追跡できるロボットになります。

　ラインを追跡するためのアルゴリズムは、実際の高速フィードバックを提供するため、 PID制御を使用すると簡単です。
　このコントローラーで手動で行うのが難しいのは、各制御項の定数（Kp、Ki、およびKd）を設定することだけです。値が見つかったら、ロボットは、ロボットの物理的制限内で追跡できる任意のタイプのラインを追跡することができます。

２．各項の設定値を決定する試行錯誤の方法
　PIDコントローラーのゲインは、試行錯誤の方法で取得できます。各ゲインパラメータの重要性を理解すると、この方法は比較的簡単になります。

（１）この方法では、KiとKdの項が最初にゼロに設定され、ロボットが振動/ぐらつき始めるまで比例ゲインKpを増加します。

（２）比例ゲインが大きくなると、ロボットは速くなりますが、不安定になり、ロボットがラインを逃してしまう可能性があるため、注意が必要です。目標は、ロボットが非常にぐらついている場合でも、ロボットがラインに追従するようにすることです。

（３）ロボットがオーバーシュートしてラインを失った場合は、Kp値を下げてください。ロボットがターン（ターンの曲率は、私のロボットの場合約１１cm以上）をナビゲートできない場合、または動きが遅いと思われる場合は、Kp値を増やします。

（４）Kpが所望の高速応答を得るように設定されているとき、振動を停止するようにするため積分ゲインKiを増加させます。積分項は定常状態の誤差を減らしますが、オーバーシュートを増やします。

（５）高速システムでは、変更にすぐに対応できるように、ある程度のオーバーシュートが常に必要です。積分項は、最小の定常状態誤差を達成するために微調整されます。

（６）次にKpとKiが、最小限の定常偏差と、速いスピードでラインを取得するように設定されているとき、ロボットは、その設定点に素早く戻るまで、微分項Kdを増加させていきます。

（７）微分項を増やすと、オーバーシュートが減少し、安定性のあるゲインが高くなりますが、システムはノイズに非常に敏感になります。

ページの先頭へ

ラインフォロワーロボットの製作３

Arduino NANOでラインフォロワーロボット３号機を製作しました。

Arduino NANOでQTRSensorsライブラリーを使用したラインフォロワーロボット３号機を製作

完成ビデオ

完成写真

使用部品

回路図

プログラム

プログラムの概略

PID制御とその調整

目次