ロボカップジュニア サッカーの試合で勝つために最も重要なことはアウトオブバウンズ対策です。アウトオブバウンズの少ない方のチームが勝つといっても過言ではありません。今回はアウトオブバウンズ対策の根幹となるラインセンサー技術を、CIAO Tezukayamaの2019機をベースに紹介します。

2019機についてはこちらでも紹介しています。


2019機のラインセンサー には次の特徴があります。

• すべてのセンサーを個別に監視している
• 閾値を短時間で設定できる
• 2進数を利用して大量のデータを簡潔に扱える
• ノイズ、通信エラー対策を施している
• LEDを消灯することが出来る

2019機では、ラインを踏んだときにライン上をトレースすることで無駄な動作を省き最短経路でボールを追いかけることが目標でした。そのためには正確なラインの位置を把握する必要があります。距離センサーを用いて壁までの距離を計測することで間接的にラインの位置を推定することもできますが、直接ラインを検知するラインセンサーを細かく配置する方がより信頼性の高いデータが得られると考えました。その他にもより正確なデータが得られるよう工夫しています。

センサー回路

2019機で用いているラインセンサーの回路です。最も一般的でシンプルなものだと思います。LEDとフォトトランジスタを組み合わせて、反射した光の明るさを測定するタイプのものです。各センサーの出力をまとめて１つの入力ポートで監視したり、可変抵抗で閾値を設定してデジタル入力ポートで監視したりするといったマイコンの入力ポートを節約するような工夫はしていません。

センサー値の取得と閾値の設定

各センサーをそれぞれマイコンのアナログ入力ポートに接続し個別で読み込んでいるので、かなりの数のアナログ入力ポートが必要になります。2019機ではアナログ入力ポートを多く持つATtiny828を4つ用い、それぞれ約12個のセンサーを監視させています。
ノイズ対策として、ソフトウェア上でローパスフィルタを通しています。ローパスフィルタは、 前回値×定数1 + 計測値×定数2 (ただし、定数1 + 定数2 = 1) を今回の値とすることで急激な変化を抑える働きがあります。
ラインセンサーで正確なデータを取得するには閾値を設定する必要があります。手動で正確な閾値を個別に設定するには時間がかかるので、閾値の設定を自動化しました。試合前にロボットをライン上を通過させてセンサー値の最大値と最小値を取得し、その間の数値を閾値として設定します。この手法は全てのセンサーを個別に読むことで簡単に実現しています。

データの取り扱い

マイコン間で大量のセンサーのデータをやり取りするので、簡潔にデータを扱えるように2進数を使ってすべてのセンサーのデータを１つの数値にまとめて取り扱います。
12桁の2進数として表し、各ビットに各センサーを割り当てます。2進数ですが１つの数値なので簡単に扱うことができます。値を代入したり読み出すときにはビット演算子を使うと簡単です。

マイコン間の通信

スレーブがセンサーを監視しているのでマスターにデータを送信する必要があります。通信方式はSPIで、一度に送ることができるデータは8ビットなので12ビットを上位4ビットと下位8ビットに分けて送信します。SPI通信では、スレーブからマスターにデータを送信すると同時にマスターからスレーブにデータを送ることができるので無駄のないようにやり取りをします。 アドレスは、かつて接続不良による通信エラーやセンサーの接続ミスを検出するために用いていましたが、2019機では使用していません。

通信エラー対策

通信エラーによる意図しないデータを無視するために、今回を含めた過去３回分のデータを比較して正しいデータを取り出しています。過去３回のうち2回以上" 1 "であれば" 1 "、そうでなければ" 0 "とします。ただし、このやり方では最新のデータが反映されるのが1周期分だけ遅れることになるので、1周期を出来る限り短くするなどの対策をしなければラインに反応しきれなくなる可能性もあります。また、前々回が" 0 "かつ前回が" 1 "かつ今回が" 0 "というデータが正しいデータであった場合では正しいデータまでも無視してしまうことになるので、スレーブ側で前々回が" 0 "かつ前回が" 1 "かつ今回が" 0 "であれば" 0 "ではなく" 1 "を送信するようにしています。つまり、マスターは連続しない" 1 "を受信することはあり得ないということです。

ライントレースの考え方

ボールを追いかけるために必要な速度のx成分と、マシンがライン上の目標とする位置に近づくのに必要な速度のx成分を比較して、どのような速度で移動するのが正しいのかを判断します。 大まかに次の４つの場合を考えてみます。


それぞれの場合において ボールを追いかけること と 目標に近づくこと のどちらを優先するべきかを考慮した上で最終的な速度も表記しました。これらを満たすように速度を決定するロジックを組み立てます。すべて場合分けするのもいいですが、プログラムはできるだけ短くしたいので数値の大小関係に注目してシンプルに実装しました。
最終的には、ボールを追いかけるために必要な速度のy成分 と 決定されたx軸方向の速度 を合成して移動する速度の向きと大きさを計算します。

マスター側の通信プログラムの一部です。通信部分以外の細かな処理は省略してあります。
mbedのプログラムです。

void Line::getValue(){
    if(lifted){
        for(int i=0; i<4 ; i++){
            ss[i] = 0;
            spi.write(6);
            ss[i] = 1;
            value[i] = 0;
        }
    }
    else{
        for(int i=0; i<4; i++){
            ss[i] = 0;
            spi.write(2);
            wait_us(100);
            value[i] = spi.write(3);
            wait_us(100);
            value[i] |= spi.write(1) << 8;
            ss[i] = 1;
            if((value[i] & 0b000011111111) == 100){
                ss[i] = 0;
                spi.write(2);
                wait_us(100);
                value[i] = spi.write(3);
                wait_us(100);
                value[i] |= spi.write(1) << 8;
                ss[i] = 1;
                if((value[i] & 0b000011111111)  == 100)
                    value[i] = 0;
            }
        }
    }
    
    for(int i = 0; i < 4; i++){
        l[i][index] = value[i];
        value[i] = (l[i][0] & l[i][1] | l[i][2]) & (l[i][0] | l[i][1] & l[i][2]);
    }
    index++;
    index %= 3;

    if(value[0] & 0b000000010000){
        //    後ろから5番目のビットが1のとき実行される
    }
}

スレーブ側の全プログラムです。一部実際に使用したものと異なる部分があります。
Arduinoのプログラムです。
ちなみにATtiny828は、データシートのエラッタに記載されているようにポートピンPD3(SPI通信でSSとして用いる)はウォッチドッグタイマを有効にしなければ正常に動作しません。次のプログラムでは25行目と40行目で対処しています。

#include <EEPROM.h>
#include <Adafruit_NeoPixel.h>

#define Address  100

Adafruit_NeoPixel pixels = Adafruit_NeoPixel(10, 17, NEO_GRB + NEO_KHZ800);

const int pins[12] = {21, 20, 23, 22, 2, 3, 4, 5, 0, 1, 6, 7};
const int eeprom_offset = 0;
const float threshould_ratio = 0.6;

float value[12];
int mode = 1;
int calibration_max[12];
int calibration_min[12];
int threshould[12];
int send;
int line;
bool led_status, led_set;


void setup() {
  
  pinMode(25, OUTPUT);  //MISO設定
  WDTCSR = 0b01100001;
  SPCR = 0b11000000;    //SPI設定
  
  pixels.begin();
  for (int i = 0; i < 10; i++) {
    pixels.setPixelColor(i, pixels.Color(0, 150, 0));
  }
  pixels.show();

  for (int i = 0; i < 12; i++) {
    threshould[i] = EEPROM.read(i + eeprom_offset) * 8;
  }
}

void loop() {
  __asm__ __volatile__ ("wdr");
  
  getValues();
  
  if (led_status != led_set) {
    for (int i = 0; i < 10; i++) {
      pixels.setPixelColor(i, pixels.Color(0, led_set ? 150 : 0, 0));
    }
    pixels.show();
    led_status = led_set;
  }
  
  switch (mode) {
    case 1: //計測
      if (value[0] > threshould[0])
        line |= 0b000000000001;
      if (value[1] > threshould[1])
        line |= 0b000000000010;
      if (value[2] > threshould[2])
        line |= 0b000000000100;
      if (value[3] > threshould[3])
        line |= 0b000000001000;
      if (value[4] > threshould[4])
        line |= 0b000000010000;
      if (value[5] > threshould[5])
        line |= 0b000000100000;
      if (value[6] > threshould[6])
        line |= 0b000001000000;
      if (value[7] > threshould[7])
        line |= 0b000010000000;
      if (value[8] > threshould[8])
        line |= 0b000100000000;
      if (value[9] > threshould[9])
        line |= 0b001000000000;
      if (value[10] > threshould[10])
        line |= 0b010000000000;
      if (value[11] > threshould[11])
        line |= 0b100000000000;
    case 2:
    case 3:   //転送待機
      break;
    case 4:   //キャリブレーション
      for (int i = 0; i < 12; i++) {
        calibration_max[i] = max(value[i], calibration_max[i]);
        calibration_min[i] = min(value[i], calibration_min[i]);
        threshould[i] = calibration_max[i] * threshould_ratio + calibration_min[i] * (1 - threshould_ratio);
        threshould[i] &= 0b1111111000;
      }
      break;
    case 7:   //起動時イルミネーション
      for (int i = 0; i < 10; i++) {
        pixels.setPixelColor(i, pixels.Color(0, 0, 0));
      }
      for (int j = 1; j < 20; j++) {
        for (int i = 0; i < 10; i++) {
          pixels.setPixelColor(i, pixels.Color(j * 13, j * 13, j * 13));
        }
        pixels.show();
        delay(25);
      }
      delay(500);

      for (int j = 51; j > 10; j--) {
        for (int i = 0; i < 10; i++) {
          pixels.setPixelColor(i, pixels.Color(j * 5, j * 5, j * 5));
        }
        pixels.show();
        delay(15);
      }
      for (int j = 10; j < 52; j++) {
        for (int i = 0; i < 10; i++) {
          pixels.setPixelColor(i, pixels.Color(j * 5, j * 5, j * 5));
        }
        pixels.show();
        delay(15);
      }
      delay(500);

      for (int j = 0; j < 52; j++) {
        for (int i = 0; i < 10; i++) {
          pixels.setPixelColor(i, pixels.Color(255 - j * 5, 255 - j, 255 - j * 5));
        }
        pixels.show();
        delay(10);
      }

      for (int i = 0; i < 10; i++) {
        pixels.setPixelColor(i, pixels.Color(0, 200, 0));
      }
      pixels.show();
      mode = 0;
      break;
    default:  //待機モード
      send = line & 0b000011111111;
      SPDR = Address;
      break;
  }
}


void getValues() {
  const float ratio = 0.3;
  for (int i = 0; i < 12; i++)
    value[i] = value[i] * (1 - ratio) + analogRead(pins[i]) * ratio;    //ローパスフィルター
}

int lastvalue[3];
ISR (SPI_STC_vect) {
  mode = SPDR;
  switch (mode) {
    case 2:   //下位データ転送
      led_set = true;
      lastvalue[0] = lastvalue[1];
      lastvalue[1] = lastvalue[2];
      lastvalue[2] = line;
      line = ~lastvalue[0] & lastvalue[1] | lastvalue[2];
      send = line & 0b000011111111;
      SPDR = send;
      break;
    case 3:   //上位データ転送
      send = line >> 8;
      line = 0;
      SPDR = send;
      break;
    case 4:   //キャリブレーション開始
      for (int i = 0; i < 12; i++) {
        calibration_max[i] = 0;
        calibration_min[i] = 0;
        threshould[i] = 0;
      }
      break;
    case 5:   //キャリブレーション終了
      for (int i = 0; i < 12; i++) {
        EEPROM.write(i + eeprom_offset, threshould[i] / 8);
      }
      mode = 1;
      SPDR = Address;
      break;
    case 6:   //LED消灯
      led_set = false;
      SPDR = 0;
      break;
    default:
      SPDR = Address;
      break;
  }
}

マスター側のライントレースのプログラムの一部です。ライントレース以外の部分は省略しています。

Move Offense::process_line(Move move, Sensors sensors){
    getDetected(sensors);
    
    int movex, movey;

    movex = speed * move.power * sin(move.angle);
    movey = speed * move.power * cos(move.angle);
    
    if (detected[FRONT] == 1) {
        int var = sensors.line.coordinateY;
        if (var == None)
            var = -36;
        int yMax = line.pid[FRONT].getPID(var, 15, interval) * speed;
        movey = constrain(movey, -200, yMax);
    }
    if (detected[RIGHT] == 1) {
        int var = sensors.line.coordinateX;
        if (var == None)
            var = -36;
        int xMax = line.pid[RIGHT].getPID(var, 15, interval) * speed;
        movex = constrain(movex, -200, xMax);
    }
    if (detected[BACK] == 1) {
        int var = sensors.line.coordinateY;
        if (var == None)
            var = 36;
        int yMin = line.pid[BACK].getPID(var, -15, interval) * speed;
        movey = constrain(movey, yMin, 200);
    }
    if (detected[LEFT] == 1) {
        int var = sensors.line.coordinateX;
        if (var == None)
            var = 36;
        int xMin = line.pid[LEFT].getPID(var, -15, interval) * speed;
        movex = constrain(movex, xMin, 200);
    }
    
    move.angle = atan2(movex, movey);
    move.power = (float)sqrt(movex * movex + movey * movey) / speed;
    
    return move;
}

さいごに

2019機では、ラインセンサー周りが原因でアウトオブバウンズしてしまうことはかなり少なくなっていましたが、それでもまだまだ完璧ではなかったと思います。何度も言いますが、アウトオブバウンズを減らすだけでより良い結果が得られます。アウトオブバウンズを減らすだけでよりサッカーらしい試合ができるようになると思います。今回紹介した技術はセンサーの配置や種類に関係なく応用できるものもあると思うので、ぜひ参考にしてもらえると嬉しいです。