ドローンの制御をする際には以下が必要になります．
- 姿勢角の計測
- PID制御
- 2のモータへの配分
1〜3の順番で以下にまとめていきます．
姿勢角の計測
この記事で姿勢角の計測（BMX055の動かし方）についてはまとめているので，そちらが知りたければ参照ください．Arduinoではなく，esp32を本記事では使っていますが，配線の仕方を以下のように変えれば問題なく使えました．
| （arduinoの場合） | esp32 | BMX055 |
| 5V | 3.3V | Vcc |
| GND | GND | GND |
| A4 | D21 | SDA |
| A5 | D22 | SCL |
PID制御
PID制御は簡単に言うと，誤差の比例項（P）と積分項（I）と微分項（D）をフィードバックする制御です．PID制御に関する詳解は別のサイトにおまかせするとして，どのようにPID制御値を決定するかをコードベースで書いていきます．
制御量はそれぞれの項にゲインをかけて定めます．積分項の値は，誤差の積分となっているため大きくなりやすいです．そのため，積分項のゲインは小さくするのが一般的です．
制御量 = - Kp * 比例項 - Ki * 積分項 - Kd * 微分項
ゲインはヘッダーファイルで定めています．
// Gain: roll, pitch, yaw
float Kp[3] = {100.0f, 100.0f, 100.0f};
float Ki[3] = { 0.1f, 0.1f, 0.1f};
float Kd[3] = { 1.0f, 1.0f, 1.0f}ロール・ピッチ・ヨー角の比例項・積分項・微分項を格納する配列，PID制御値を格納する配列を用意します．
// Measured value: roll, pitch, yaw
float rpy_p[3] = {0.0f, 0.0f, 0.0f};
float rpy_i[3] = {0.0f, 0.0f, 0.0f};
float rpy_d[3] = {0.0f, 0.0f, 0.0f};
float rpy_pre[3] = {0.0f, 0.0f, 0.0f};
// Calculated value: roll, pitch, yaw
float pid_rpy[3] = {0.0f, 0.0f, 0.0f}ロール・ピッチ・ヨー角のデータを取得し，この値から比例項・積分項・微分項を算出します．積分項は単純に足し算，微分項は前進差分近似によって求めています．
void PID::calculate_rpy(float data[3]) {
for (int i=0; i<3; i++) {
rpy_p[i] = data[i];
rpy_i[i] += rpy_p[i];
rpy_d[i] = (rpy_p[i] - rpy_pre[i]) / ((float)SAMPLING_TIME_MS/1000);
rpy_pre[i] = rpy_p[i];
}
}制御量を求めます．モータへのPID制御値の配分の際に符号を決定するため，ここでは正の値としています．
void PID::calculate_pid(float data[3]) {
calculate_rpy();
for (int i=0; i<3; i++) {
pid_rpy[i] = Kp[i]*rpy_p[i] + Ki[i]*rpy_i[i] + Kd[i]*rpy_d[i];
}
}モータへのPID制御値の配分
ドローンの模式図を以下で示します．図中の○はプロペラを表していて，○の中の矢印はロータの回転方向を表しています．
たとえば，x軸正の方向に機体が傾いたときは，下図の青色のモータの回転量を大きくする必要があります．
また，機体がz軸周りに正の回転をした場合，下図の青色のモータの回転を強めます．そうすることで時計回りに反トルクが発生し，機体がニュートラルな姿勢に戻ります．ここで，モータの回転の方向と逆向きに反トルクが発生することに注意します．
これをソースコードにすると以下のようになります．m_pid_cmd[i]はmotor i+1の制御量をそれぞれ表しています．x軸正の向きに傾いたときにmotor 3とmotor 4の制御量が大きくなるようにプログラムされていることがわかりますね．z軸正の向きに機体が回転したときは，motor 1とmotor 3の回転が強まることがわかるかと思います．
m_pid_cmd[0] = - pid_data[0] + pid_data[1] + pid_data[2];
m_pid_cmd[1] = - pid_data[0] - pid_data[1] - pid_data[2];
m_pid_cmd[2] = + pid_data[0] - pid_data[1] + pid_data[2];
m_pid_cmd[3] = + pid_data[0] + pid_data[1] - pid_data[2];最後に，各モータに割り振られた制御値をスケーリングします．ledcWrite()関数で制御するため，0〜255までの値にスケーリングします．
#define PID_MAX 2000
for (int i=0; i<4; i++) {
limit_command(m_pid_cmd[i], 0, PID_MAX);
m_pid_cmd[i] = 255*m_pid_cmd[i]/PID_MAX;
};
void Motor::limit_command(int &cmd, int min, int max) {
if (cmd > max) { cmd = max; }
if (cmd < min) { cmd = min; }
}コードを部分的に切り取っているだけなのでインデントがおかしい等の読みにくさはご容赦ください．フルバージョンはgitに載せています．
配線
配線は以下のように行いました．
| 電源 | esp32 | LED | BMX055 |
| D12 | LED1 -> 220 Ω抵抗 -> GND | ||
| D13 | LED2 -> 220 Ω抵抗 -> GND | ||
| D14 | LED3 -> 220 Ω抵抗 -> GND | ||
| D15 | LED4 -> 220 Ω抵抗 -> GND | ||
| +端子 | Vin | ||
| -端子 | GND | GND | |
| 3.3V | Vcc | ||
| D21 | SDA | ||
| D22 | SCL |
動画
傾いた方向のLEDが強く光っている様子が確認できるかと思います．
コメント