自動ホワイトボードクリーナー

概要

私は、今まで壁を走るロボット「うおーるぼっと」シリーズを開発してきました。
うおーるぼっとは、ホワイトボードに貼り付けてリモコンやスマートフォンで動かしたり、ペンで描いたコースに触れないように動かしたりする楽しむためのロボットです。

うおーるぼっとはホワイトボードを走れるので、折角ならばホワイトボードに書いたものを自動的に消せるようにしたいと思いました。
自動的に消せるホワイトボードはあるものの、専用のものだったり、故障が多かったりと一般的にはなかなか見かけません。
うおーるぼっとであれば、手軽にホワイトボードに貼り付けて使えるので、一般的なホワイトボードでも後づけで利用できます。
今回は、Spresenseを使用し、音声認識や画像認識を使いつつ試作を作ってみました。

構成

構成は以下のとおりです。

センサーとして、Spresense用のHDRカメラボード、コンデンサマイク、三軸の加速度センサーを使用してます。
HDRカメラは、ホワイトボード上に書かれたものやホワイトボードの端を検出するのに使用しています。
コンデンサマイクは、音声のキーワードで動作開始ができるように搭載しました。
三軸の加速度センサーは向いている方向がわかるように搭載しています。垂直に張り付いているので、重力加速度を検出するだけで方向がわかります。

部品

機能確認

姿勢制御

垂直に走行する場合、重力加速度を検出すれば、向いてる方向がわかります。
そのため、加速度センサを使って、上方向、右方向、左方向、下方向への正確に進めるようにコントロールします。
加速度センサは、SPRESENSEにI2Cで接続できるMMA8451を使用しました。
拡張ボードを使用したので、電圧は3.3Vベースで信号線とともに接続しています。

次の動画は、姿勢制御の確認です。ホワイトボードを回転させても常に右方向を向き続けます。

画像認識

取り付けられたカメラにより、ホワイトボードの端や書かれたものを認識して制御します。

キーワード音声認識

「開始」と「終了」という音声のキーワードで動作を開始・停止できるようにしました。
キーワードの音声と環境音を検出して、キーワードのパターンを区別しています。

動作

組み合わせた動作の様子です

ソースコード

画像認識と音声認識

メインコアのコード
Main.ino

#if (SUBCORE != 1)
#error "Core selection is wrong!!"
#endif

#include <MP.h>
#include <Servo.h>
#include <Wire.h>
#include <Adafruit_MMA8451.h>
#include <Adafruit_Sensor.h>

// サーボの停止位置の値
// 個体差で中心値を微調整
#define SERVO1_CENTRE_VALUE 93
#define SERVO2_CENTRE_VALUE 90

// サーボの最大スピード
#define SERVO_MAX_SPEED     40

enum Mode {
  MODE_STOP,
  MODE_RIGHT_RUN,
  MODE_LEFT_RUN,
  MODE_UP_RUN,
  MODE_DOWN_RUN
};

Servo servo1;
Servo servo2;
Adafruit_MMA8451 mma = Adafruit_MMA8451();

Mode currentMode = MODE_STOP;

void setServo(int16_t right,int16_t left) {
  int16_t servo1_speed = right;
  int16_t servo2_speed = left;

  if (servo1_speed > SERVO_MAX_SPEED)  servo1_speed = SERVO_MAX_SPEED;
  if (servo2_speed > SERVO_MAX_SPEED)  servo2_speed = SERVO_MAX_SPEED;

  servo1.write(SERVO1_CENTRE_VALUE+servo1_speed);
  servo2.write(SERVO2_CENTRE_VALUE-servo2_speed);
}

void setup() {
  MP.begin();
  Serial.begin(115200);
  servo1.attach(6);
  servo2.attach(5);
  mma.begin();
  mma.setRange(MMA8451_RANGE_2_G);
}

void loop() {
  int ret;
  int *buff;
  
  ret = MP.Recv(&msgid, &buff);
  if (ret >= 0) {
    currentMode = buff;
  }
  mma.read();
  if (currentMode != MODE_STOP && mma.z > -3000 && mma.z < 3000){
    int16_t position = 0;
    if (currentMode == MODE_RIGHT_RUN || currentMode == MODE_LEFT_RUN)  position = mma.y;
    if (currentMode == MODE_UP_RUN || currentMode == MODE_DOWN_RUN)  position = mma.x;
    int target = 0;         // 目標値（Yを0に合わせる）
    float Kp = 0.05;        // 比例ゲイン（調整可能）
    int controlSignal = Kp * (position - target); // 制御信号を計算
    // 制御信号をm1とm2に反映
    int16_t m1 = constrain(-controlSignal, -SERVO_MAX_SPEED, SERVO_MAX_SPEED); // m1の範囲を制限

    if (m1 > 7 || m1 < -7) {
      int16_t m2 = -m1; // m2はm1と逆向きに設定
      if (currentMode == MODE_RIGHT_RUN || currentMode == MODE_UP_RUN)  setServo(m1,m2);
      if (currentMode == MODE_LEFT_RUN || currentMode == MODE_DOWN_RUN)  setServo(m2,m1);
    } else {
      setServo(-SERVO_MAX_SPEED,-SERVO_MAX_SPEED);
    }
  } else {
    setServo(0,0);
  }
  Serial.println();
  delay(100);
}

足回りの制御

サブコアのコード
Sub.ino

#if (SUBCORE != 1)
#error "Core selection is wrong!!"
#endif

#include <MP.h>
#include <Servo.h>
#include <Wire.h>
#include <Adafruit_MMA8451.h>
#include <Adafruit_Sensor.h>

// サーボの停止位置の値
// 個体差で中心値を微調整
#define SERVO1_CENTRE_VALUE 93
#define SERVO2_CENTRE_VALUE 90

// サーボの最大スピード
#define SERVO_MAX_SPEED     40

enum Mode {
  MODE_STOP,
  MODE_RIGHT_RUN,
  MODE_LEFT_RUN,
  MODE_UP_RUN,
  MODE_DOWN_RUN
};

Servo servo1;
Servo servo2;
Adafruit_MMA8451 mma = Adafruit_MMA8451();

Mode currentMode = MODE_STOP;

void setServo(int16_t right,int16_t left) {
  int16_t servo1_speed = right;
  int16_t servo2_speed = left;

  if (servo1_speed > SERVO_MAX_SPEED)  servo1_speed = SERVO_MAX_SPEED;
  if (servo2_speed > SERVO_MAX_SPEED)  servo2_speed = SERVO_MAX_SPEED;

  servo1.write(SERVO1_CENTRE_VALUE+servo1_speed);
  servo2.write(SERVO2_CENTRE_VALUE-servo2_speed);
}

void setup() {
  MP.begin();
  Serial.begin(115200);
  servo1.attach(6);
  servo2.attach(5);
  mma.begin();
  mma.setRange(MMA8451_RANGE_2_G);
}

void loop() {
  int ret;
  int *buff;
  
  ret = MP.Recv(&msgid, &buff);
  if (ret >= 0) {
    currentMode = buff;
  }
  mma.read();
  if (currentMode != MODE_STOP && mma.z > -3000 && mma.z < 3000){
    int16_t position = 0;
    if (currentMode == MODE_RIGHT_RUN || currentMode == MODE_LEFT_RUN)  position = mma.y;
    if (currentMode == MODE_UP_RUN || currentMode == MODE_DOWN_RUN)  position = mma.x;
    int target = 0;         // 目標値（Yを0に合わせる）
    float Kp = 0.05;        // 比例ゲイン（調整可能）
    int controlSignal = Kp * (position - target); // 制御信号を計算
    // 制御信号をm1とm2に反映
    int16_t m1 = constrain(-controlSignal, -SERVO_MAX_SPEED, SERVO_MAX_SPEED); // m1の範囲を制限

    if (m1 > 7 || m1 < -7) {
      int16_t m2 = -m1; // m2はm1と逆向きに設定
      if (currentMode == MODE_RIGHT_RUN || currentMode == MODE_UP_RUN)  setServo(m1,m2);
      if (currentMode == MODE_LEFT_RUN || currentMode == MODE_DOWN_RUN)  setServo(m2,m1);
    } else {
      setServo(-SERVO_MAX_SPEED,-SERVO_MAX_SPEED);
    }
  } else {
    setServo(0,0);
  }
  Serial.println();
  delay(100);
}

最後に

電源にモバイルバッテリーを利用したため、重量が重く、自重による負荷でなかなか水平に直進できませんでした。
そのため、姿勢制御が過剰に働いてしまうことが多かったので、バッテリーを見直した方が良いかもしれません。
あと、サーボモーターも必要以上にパワーがあるものだったので、これも変更すれば軽量化できそうです。

画像認識も誤認が多いので、もうちょっと作りこんでいきたいと思っています。