はじめに

ビジネス統括本部のイチノです。最近はドローンの開発に取り組んでいます。ドローンを制御するソフトウェアにPX4やArduPilotなどがあります。これらのソフトウェアでは、PID制御を組み合わせた制御システムを実装しています*1 *2 。個人的に、簡易でプロペラモータをPID制御できるものが作れないかと考えていました。今回は、簡易のプロペラモータを使ったPID制御システムを実装したので紹介します。

プロペラモータ制御システムの紹介

実装したPID制御システムの概略図です。

板の片方を台にテープで固定し、もう片方にプロペラモータをつけます。プロペラーモータを回し、板を水平に維持することが制御システムの目標です。水平から板がどれくらい傾いているかをθと定義し、θに応じてプロペラモータの回転数を制御しています。プロペラモータの回転数を調整する方法としてPID制御を用いています。PID制御についての説明は割愛します。YouTubeなどをご参照くだいさい。 Pythonで学ぶ制御工学 Part2

システムを構成する部品の一部です。銅線やギボシ端子などの細かい部品は省いています。

プロペラ + モータ + ESC セット
- モータ、プロペラと、ESCのセットだったら類似品でも良いです。
M5Stack Gray（9軸IMU搭載） - スイッチサイエンス
- 板の傾きを手軽に取れるセンサーとして地磁気を内蔵するマイコンが良かったので、 M5Stack Grayを選択しています。
電源装置
- 電源は、電圧7.4V、最大電流4Aを供給できれば良いです。
板
- 加工しやすそうな木の板で良いです。参考までに利用した板は、縦横厚みがそれぞれ4x50x0.6[cm]です。

M5Stack の開発環境を導入しています。導入には、以下のドキュメントを参照しています。

docs.m5stack.com

参考用に M5Stack へ書き込んだプログラムのソースコードを抜粋して掲載します。

#define M5STACK_MPU6886 // MPU6886 センサーを利用する場合のマクロ
#include <M5Stack.h>

const int PWM_PIN = 2;  // ESCへのPWM信号出力ピン番号
const int PWM_CH = 0;   // PWM のチャンネル
const int PWM_HZ = 490; // 今回使用したESC の PWM 周波数は 490[Hz] で利用可能
const int PWM_BIT = 16; // PWM の分解能 16bit (0〜65535)

const float TARGET_DEG = 0.0; // 目標角度。単位はdegree。

// PID パラメータ
const float KP = 100.0;
const float KI = 10.0;
const float KD = 5.0;
const float throttle = 65535 * 0.65;

float get_seconds() {
  return micros() / 1000000.0;
}

...

void setup() {
  M5.begin();
  M5.IMU.Init();

  ledcSetup(PWM_CH, PWM_HZ, PWM_BIT);
  ledcAttachPin(PWM_PIN, PWM_CH);
  ...
}

void loop() {
  M5.update();
  M5.IMU.getAhrsData(&pitch_deg, &roll_deg, &yaw_deg);
  ...

  sec = get_seconds();
  dt = sec - prev_sec;
  float e = TARGET_DEG - roll_deg;

  sum_errors += e * dt;

  p = KP * e;
  i = KI * sum_errors;
  d = KD * (e - prev_error) / dt;

  prev_error = e;
  prev_sec = sec;

  float pid = p + i + d;
  duty_rate = throttle + pid;
  duty_rate = constrain(duty_rate, MIN_DUTY_RATE, MAX_DUTY_RATE);  // 制御不能にならないよう制約を設定

  ledcWrite(PWM_CH, duty_rate);

  ...
}