環境

以下の部品を使用しました。

種類	選定品	@値段	個数
タイヤ	38mmオムニホイール	665円	3
ハブ	4mmハブ	270円	3
モーター	GA370 6V 280rpm	1200円	3
モータードライバ	L298P	110円	2
マイコンボード	arduino mega	5800円	1

概要

ここではモーターを速度制御します。上記の部品を組んでモーターの速度制御をします。
モーターに制御できる変数は印加電圧だけですが、速度値をみながらうまく印加電圧を操作することで目標の速度にすることを速度制御と呼びます。

電気的接続

モータードライバシールドの設計

arduino megaとモーターをつなげるためのシールドを製作します。回路図は以下になります。モータードライバ1つで2chあります。

| Pin名 | I/O 接続 | 説明 |
|:-:|:-:|:-:|:-:|
| MOT0_A | -> MD | 回転極性+ |
| MOT0_B | -> MD | 回転極性- |
| MOT0_PWM | -> MD | 回転強さ |
| MOT0_CRNT | <- MD | モーター電流値 |
| ENC0_A | <- ENC | コンコーダーA相 |
| ENC0_B | <- ENC | コンコーダーB相 |

電源

電源は5Vです。Arduinoからも5Vが供給されていますがこれはArduinoに刺さったUSB経由の給電なのであまり電流をとれません。この電源とは別にシールド上のUSBコネクタから5Vを取ります。GNDはArduinoと共通にします。
今回は電源としてUSBモバイルバッテリーを使用しました。

モーター制御アルゴリズム

今回の構成ではモーターへの制御入力はPWMのデューティー比でこれはモーターの力と比例します。¹

またエンコーダーからはモーターの速度が出力されています。今回はモーターに速度を指示したいのでこのエンコーダーの出力をフィードバックして制御します。今回はFF+PI制御を行います。時刻tでのエンコーダーのモーターの測定速度を$\omega(t)$、指示速度を$\omega^*(t)$とするとFF+PI制御の出力$D$は

$$D(t)= K_f \omega^(t) - K_p (\omega(t)-\omega^(t)) - K_i \int_0^t (\omega(t)-\omega^*(t))dt$$

となります。$K_f, K_p, K_i$の3つはパラメーターでうまく動くように値を合わせます。²

モーターをデューティー比を指定して回す、エンコーダーの情報を読む

FB制御とかはせずにデューティー比を指定して回します。

test1.ino

# define MOT0_A 32
# define MOT0_B 33
# define MOT0_P 5
# define ENC0_A 2
# define ENC0_B 22

void setMotor(float value){
  if(value > 0){
    digitalWrite(MOT0_A, HIGH);
    digitalWrite(MOT0_B, LOW);
    analogWrite(MOT0_P, (int)(value*255));
  }
  else if(value < 0){
    digitalWrite(MOT0_A, LOW);
    digitalWrite(MOT0_B, HIGH);
    analogWrite(MOT0_P, (int)(-value*255));
  }
  else{
    digitalWrite(MOT0_A, LOW);
    digitalWrite(MOT0_B, LOW);
    analogWrite(MOT0_P, 0);
  }
}

volatile int count = 0;
void readEncoder() {
  bool sa = digitalRead(ENC0_A);
  bool sb = digitalRead(ENC0_B);
  if(sa == sb)count++;
  else count--;
}

void setup() {
  Serial.begin(9600);

  //for Motor0
  pinMode(MOT0_A, OUTPUT);
  pinMode(MOT0_B, OUTPUT);
  pinMode(MOT0_P, OUTPUT);
  pinMode(ENC0_A, INPUT_PULLUP);
  pinMode(ENC0_B, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(ENC0_A), readEncoder, CHANGE);
}

void loop() {
  if (Serial.available() > 0) {
    int incomingByte = Serial.read();
    if(incomingByte == '1') setMotor(-1.0);
    else if(incomingByte == '2') setMotor(-0.75);
    else if(incomingByte == '3') setMotor(-0.5);
    else if(incomingByte == '4') setMotor(-0.25);
    else if(incomingByte == '5') setMotor(0.0);
    else if(incomingByte == '6') setMotor(0.25);
    else if(incomingByte == '7') setMotor(0.5);
    else if(incomingByte == '8') setMotor(0.75);
    else if(incomingByte == '9') setMotor(1.0);
  }

  static int last_count = 0;
  int speed_count = count - last_count;
  last_count = count;
  
  Serial.print("pos: ");
  Serial.print(count);
  Serial.print(", vel: ");
  Serial.println(speed_count);
  delay(50);
}

setMotor(float value)をvalue=[-1.0, 1.0]の範囲で呼ぶとその値に応じたPWMが設定されて、モーターが回ります。
エンコーダーは今回はENC0_Aが遷移したときのみカウントします。attachInterrupt(digitalPinToInterrupt(ENC0_A), readEncoder, CHANGE);でENC0_Aが遷移した時に関数が呼ばれるようにします。この関数の中でENC0_AとENC0_Bの値を比較してカウントアップ・カウントダウンします。

モーターを速度制御する

きちんと制御するには、本来は運動のダイナミクスを解いたり電流フィードバックをしたりすることが必要ですが、今回は簡易化のため速度でPWMをフィードバックする制御系を組みます。また制御方式はFF+PI制御とします。この方式でもそこそこチューニングをすればそこそこモーターを速度制御することができます。

test2.ino

# define MOT0_A 32
# define MOT0_B 33
# define MOT0_P 5
# define ENC0_A 2
# define ENC0_B 22

void setMotor(float value){
  
  if(value > 0){
    digitalWrite(MOT0_A, HIGH);
    digitalWrite(MOT0_B, LOW);
    analogWrite(MOT0_P, min(max((int)(value*256), 0), 255));
  }
  else if(value < 0){
    digitalWrite(MOT0_A, LOW);
    digitalWrite(MOT0_B, HIGH);
    analogWrite(MOT0_P, min(max((int)(-value*256), 0), 255));
  }
  else{
    digitalWrite(MOT0_A, LOW);
    digitalWrite(MOT0_B, LOW);
    analogWrite(MOT0_P, 0);
  }
}

volatile int count = 0;
void readEncoder() {
  bool sa = digitalRead(ENC0_A);
  bool sb = digitalRead(ENC0_B);
  if(sa == sb)count++;
  else count--;
}

void setup() {
  Serial.begin(9600);

  //for Motor0
  pinMode(MOT0_A, OUTPUT);
  pinMode(MOT0_B, OUTPUT);
  pinMode(MOT0_P, OUTPUT);
  pinMode(ENC0_A, INPUT_PULLUP);
  pinMode(ENC0_B, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(ENC0_A), readEncoder, CHANGE);
}

void loop() {
  static int target = 0;

  if (Serial.available() > 0) {
    int incomingByte = Serial.read();
    if(incomingByte == '1') target = -40;
    else if(incomingByte == '2') target = -30;
    else if(incomingByte == '3') target = -20;
    else if(incomingByte == '4') target = -10;
    else if(incomingByte == '5') target = 0;
    else if(incomingByte == '6') target = 10;
    else if(incomingByte == '7') target = 20;
    else if(incomingByte == '8') target = 30;
    else if(incomingByte == '9') target = 40;
  }

  static int last_count = 0;
  int speed_count = count - last_count;
  last_count = count;

  float diff = (speed_count - target);

  static float iterm = 0;
  float dt =0.050;
  iterm += diff * dt;

  float Kf = 0.02;
  float Kp = 0.03;
  float Ki = 0.07;
  float value = Kf * target - Kp * diff - Ki * iterm;
  setMotor(value);

  if(target == 0 && speed_count == 0)iterm *= 0.5;
  
  Serial.print("encoder: ");
  Serial.println(speed_count);
  delay(50);
}

FF+PI制御のメインはfloat value = Kf * target - Kp * diff - Ki * iterm;の部分です。

Kf * targetはFF項です。速度指令値に比例した出力をします。事前にこれぐらいのPWMを入れたらこれぐらいの速さになるというのを計っておいてそれでKfの値を決めます。
- Kp * diffはP項です。これが大きいほど反応が良くなりますが、大きすぎると振動してしまいます。振動しない程度に上げます。
- Ki * itermはI項です。これは観測値が指令値に近づいてくると徐々にP項が小さくなることを補うためにあります。Ki=Kp/（時定数）とするのが良いです。

参考

フィードバック制御とフィードフォワード制御

目次ページへのリンク

ROS講座の目次へのリンク

本当は違いますが、デューティー比を上げると力は増えます。非線形なところはPI制御が補ってくれます。 ↩
これらの値をチューニングする有名な手法もあります。 ↩