HomeMadeGarbage Advent Calendar 2022 ｜16日目

前回はクローズドループ正弦波駆動によるブラシレスモータの制御に再挑戦しました。

ここでは正弦波駆動を応用して色々試しましたので報告します。

ブラシレスモータの回転位置を指定

前回と全く同じシステム構成で可変抵抗でモータの回転位置を指定しての制御を実施しました。

ブラシレスモータをサーボモータのようにアレする#ブラシレスモータ駆動への道 pic.twitter.com/8kqTDh6kAQ

— HomeMadeGarbage (@H0meMadeGarbage) December 10, 2022

可変抵抗での指定角度と磁気エンコーダによる測定角度の誤差をPI制御して正弦波の波高値にして、位相は測定角度を電気角に変換して使用し制御しています。

ブラシレスモータをサーボ感覚で動かせるようになりました。

可変抵抗を6軸 IMUセンサに変えて制御してみました。

ブラシレスモータをIMUで動かしてよい#ブラシレスモータ駆動への道 pic.twitter.com/Mr0vrfapYR

— HomeMadeGarbage (@H0meMadeGarbage) December 10, 2022

前出の可変抵抗によるモータ回転位置指定をIMUの傾き角度に変えただけですが、何かができそうな感じがにわかにあふれ出しました。

KKHMF 2個 MPU-6050 6DOF GY-521 MPU6050 3軸ジャイロスコープ + 加速度センサーモジュール Arduino用

おかもちリベンジ

以前製作したおかもちを本システムをベースに作ってみたいと思います。

台にIMUつけてブラシレスモータ制御
ブラシレスモータのエンコーダ使わずに素直に回転制御だけ

回せればここまでできるもんなんだな。
コレはまた一つおりこうさんになったわ#PD制御 #M5Stack #MPU6050 pic.twitter.com/TTIo52KzTs

— HomeMadeGarbage (@H0meMadeGarbage) March 5, 2022

PI制御

磁気エンコーダを排除して、IMUによる傾き角度を使用して板が平行になるようにブラシレスモータを制御しました。

IMUをエンコーダ代わりに正弦波駆動
位相はIMUの角度をPI制御で導出
振幅は一定

さらにPID制御にして振幅も可変にして
俺なりのおかもちをもちたい#ブラシレスモータ駆動への道 pic.twitter.com/bdM3tjLuNR

— HomeMadeGarbage (@H0meMadeGarbage) December 11, 2022

IMUセンサは板の下側に設置して、板が平行になる(0°)ようにIMUの角度をPI制御した値を正弦波の位相としました。
正弦波の位相は一定 (約1V)としました。

なかなか良い動きを実現できています。

PID制御

さらにIMUによる角速度も使用してPID制御にしてみました。振幅は一定。

IMUをエンコーダ代わりに正弦波駆動
位相はIMUの角度をPIからPID制御に変更
よりヌルヌルした。

次は振幅を可変にしてみたい#ブラシレスモータ駆動への道 pic.twitter.com/uVGodgckF3

— HomeMadeGarbage (@H0meMadeGarbage) December 12, 2022

制御がよりヌルヌルして外乱に迅速に反応できるようになりました。

PID制御振幅可変

次に正弦波駆動の位相のPID制御に加えて振幅をIMUによる傾きのP制御で導出するようにしました。

傾きが大きくなるほど振幅が大きくなってモータへの電流が増えます。

IMUをエンコーダ代わりに正弦波駆動
位相はIMUの角度をPID制御
振幅はIMUの角度のP制御で力強さを演出#ブラシレスモータ駆動への道 pic.twitter.com/Xv8lMfPmJ3

— HomeMadeGarbage (@H0meMadeGarbage) December 12, 2022

動画ではわかりにくいですが、振幅可変によって応答性が少し改善し板の平行保持力が向上しました。

板の保持力は振幅を常に最大(7.4/2 V)にすれば強くなりますが、大きな電流がずっと流れ続けてもったいないので振幅を傾きに応じて可変としました。

Arduinoコード

位相をPID制御で振幅をP制御で生成するおかもちのコードを以下に記します。

#include <Kalman.h>
#include "I2Cdev.h"
#include "MPU6050.h"
#include <Wire.h>


#define uHin 25
#define vHin 26
#define wHin 27

int uPWMCH = 0;
int vPWMCH = 1;
int wPWMCH = 2;

float twoPI = 2.0 * PI;
float onePI = PI;

float Vu, Vv, Vw;
float amp = 150.0, amp2, offset = 1.3;

unsigned long oldTime = 0, loopTime, nowTime;
float dt;

uint16_t rotData, rotDataIni;

float thetaM = 0.0;
int i = 0;

float Kp = 60.0, Ki = 0.015, Kd = 0.05, Kpa = 40.0;
float diffI = 0.0;

MPU6050 mpu;

int16_t ax, ay, az;
int16_t gx, gy, gz;

float accX = 0, accY = 0, accZ = 0;
float gyroX = 0, gyroY = 0, gyroZ = 0;

float theta = 0.0;
float theta_dot = 0.0;

Kalman kalman;
float kalAngle, kalAngleDot;


//加速度センサから傾きデータ取得 [deg]
void get_theta() {
  mpu.getAcceleration(&ax, &ay, &az);
  accX = ax / 16384.0;
  accY = ay / 16384.0;
  accZ = az / 16384.0;
  
  //傾斜角導出 単位はdeg
  theta  = atan2(-1.0 * accY , -1.0 * accZ) * 180.0/PI;
}

//角速度取得
void get_gyro_data() {
  mpu.getRotation(&gx, &gy, &gz);
  gyroY = gy / 131.072;

  theta_dot = gyroX;
}

//Core0
void rotPosition(void *pvParameters) {
  Wire.begin();
  mpu.initialize();
  
  get_theta();
  kalman.setAngle(theta);
  
  for (;;){  
    nowTime = micros();
    loopTime = nowTime - oldTime;
    oldTime = nowTime;
    
    dt = (float)loopTime / 1000000.0; //sec

    get_theta();
    get_gyro_data();
      
    //カルマンフィルタ 姿勢 傾き
    kalAngle = kalman.getAngle(theta, theta_dot, dt);
    
    //カルマンフィルタ 姿勢 角速度
    kalAngleDot = kalman.getRate();
   

    Serial.print(amp2);
    Serial.print(", ");
    Serial.print(kalAngle);
    Serial.print(", ");
    Serial.println(thetaM);
    
    disableCore0WDT();
  }
}


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

  ledcSetup(uPWMCH, 20000, 10);
  ledcAttachPin(uHin, uPWMCH);
  ledcWrite(uPWMCH, 0);
  ledcSetup(vPWMCH, 20000, 10);
  ledcAttachPin(vHin, vPWMCH);
  ledcWrite(vPWMCH, 0);
  ledcSetup(wPWMCH, 20000, 10);
  ledcAttachPin(wHin, wPWMCH);
  ledcWrite(wPWMCH, 0);


  //core0
  xTaskCreatePinnedToCore(
    rotPosition
    ,  "rotPosition"   // A name just for humans
    ,  4096  // This stack size can be checked & adjusted by reading the Stack Highwater
    ,  NULL
    ,  1  // Priority, with 3 (configMAX_PRIORITIES - 1) being the highest, and 0 being the lowest.
    ,  NULL 
    ,  0);
}

void loop() {
  diffI += kalAngle / 180.0;
  thetaM = Kp * kalAngle / 180.0 + Ki * diffI + Kd * kalAngleDot;

  amp2 = amp + Kpa *  fabs(kalAngle);
  amp2 = constrain(amp2, 0.0, 512);
  
  
  Vu = amp2 * sin(thetaM + offset);
  Vv = amp2 * sin(thetaM + offset + twoPI / 3.0);
  Vw = amp2 * sin(thetaM + offset + 2.0 * twoPI / 3.0);
      
  ledcWrite(uPWMCH, int(Vu) + 511);
  ledcWrite(vPWMCH, int(Vv) + 511);
  ledcWrite(wPWMCH, int(Vw) + 511);
}

100

101

102

103

104

105

106

107

108

109

110

111

112

113

114

115

116

117

118

119

120

121

122

123

124

125

126

127

128

129

130

131

132

133

134

135

136

137

138

139

140

141

142

#include <Kalman.h>

#include "I2Cdev.h"

#include "MPU6050.h"

#include <Wire.h>

#define uHin 25

#define vHin 26

#define wHin 27

int uPWMCH = 0;

int vPWMCH = 1;

int wPWMCH = 2;

float twoPI = 2.0 * PI;

float onePI = PI;

float Vu, Vv, Vw;

float amp = 150.0, amp2, offset = 1.3;

unsigned long oldTime = 0, loopTime, nowTime;

float dt;

uint16_t rotData, rotDataIni;

float thetaM = 0.0;

int i = 0;

float Kp = 60.0, Ki = 0.015, Kd = 0.05, Kpa = 40.0;

float diffI = 0.0;

MPU6050 mpu;

int16_t ax, ay, az;

int16_t gx, gy, gz;

float accX = 0, accY = 0, accZ = 0;

float gyroX = 0, gyroY = 0, gyroZ = 0;

float theta = 0.0;

float theta_dot = 0.0;

Kalman kalman;

float kalAngle, kalAngleDot;

//加速度センサから傾きデータ取得 [deg]

void get_theta() {

mpu.getAcceleration(&ax, &ay, &az);

accX = ax / 16384.0;

accY = ay / 16384.0;

accZ = az / 16384.0;

//傾斜角導出単位はdeg

theta = atan2(-1.0 * accY , -1.0 * accZ) * 180.0/PI;

}

//角速度取得

void get_gyro_data() {

mpu.getRotation(&gx, &gy, &gz);

gyroY = gy / 131.072;

theta_dot = gyroX;

}

//Core0

void rotPosition(void *pvParameters) {

Wire.begin();

mpu.initialize();

get_theta();

kalman.setAngle(theta);

for (;;){

nowTime = micros();

loopTime = nowTime - oldTime;

oldTime = nowTime;

dt = (float)loopTime / 1000000.0; //sec

get_theta();

get_gyro_data();

//カルマンフィルタ姿勢傾き

kalAngle = kalman.getAngle(theta, theta_dot, dt);

//カルマンフィルタ姿勢角速度

kalAngleDot = kalman.getRate();

Serial.print(amp2);

Serial.print(", ");

Serial.print(kalAngle);

Serial.print(", ");

Serial.println(thetaM);

disableCore0WDT();

}

void setup() {

Serial.begin(115200);

ledcSetup(uPWMCH, 20000, 10);

ledcAttachPin(uHin, uPWMCH);

ledcWrite(uPWMCH, 0);

ledcSetup(vPWMCH, 20000, 10);

ledcAttachPin(vHin, vPWMCH);

ledcWrite(vPWMCH, 0);

ledcSetup(wPWMCH, 20000, 10);

ledcAttachPin(wHin, wPWMCH);

ledcWrite(wPWMCH, 0);

//core0

xTaskCreatePinnedToCore(

rotPosition

, "rotPosition" // A name just for humans

, 4096 // This stack size can be checked & adjusted by reading the Stack Highwater

, NULL

, 1 // Priority, with 3 (configMAX_PRIORITIES - 1) being the highest, and 0 being the lowest.

, NULL

, 0);

}

void loop() {

diffI += kalAngle / 180.0;

thetaM = Kp * kalAngle / 180.0 + Ki * diffI + Kd * kalAngleDot;

amp2 = amp + Kpa * fabs(kalAngle);

amp2 = constrain(amp2, 0.0, 512);

Vu = amp2 * sin(thetaM + offset);

Vv = amp2 * sin(thetaM + offset + twoPI / 3.0);

Vw = amp2 * sin(thetaM + offset + 2.0 * twoPI / 3.0);

ledcWrite(uPWMCH, int(Vu) + 511);

ledcWrite(vPWMCH, int(Vv) + 511);

ledcWrite(wPWMCH, int(Vw) + 511);

}

疑似正弦波を生成するためにドライバ駆動ピン(IO25～27)はledcWriteを用いて20kHz PWM出力します。分解能は10ビット(0～1023) (L. 105-113)。

ESP32はデュアルコアで使用しcore0でIMUセンサMPU6050のカルマンフィルタを通した傾きと角速度をセンシングします (L. 67-99)。

core1で正弦波駆動でブラシレスモータを回します。
IMUの傾きと角速度を用いてPID制御で正弦波の位相thetaMを導出します (L. 128, 129)。
IMUの傾きからP制御で正弦波の振幅amp2を導出します (L. 131, 132)。

導出された正弦波をPWM変換してドライバを駆動します (L. 139-141)。

動作

改めて位相をPID制御で振幅をP制御で生成するおかもち動作。

配線が邪魔なので改めてコントローラを選定しドライバも小型化しておかもちを完成させたいと思います。

使用したモータのトルクが大きくないので、重いものを載せれるようにギアの勉強もしたほうが良いかもしれません。

「正弦波駆動の応用　ーブラシレスモータ駆動への道11ー」への1件のフィードバック

匿名より:

2022年12月17日 1:27 AM

色々な記事を楽しく見ました。

ラダー図のソフトがArduinoIDEに加わったそうです。　詳細は下記
Arduino PLC IDE Boost production and building automation with your own Industry 4.0 control system
https://www.arduino.cc/pro/software-plc-ide

返信

コメントはこちらからコメントをキャンセル

This site uses Akismet to reduce spam. Learn how your comment data is processed.

ブラシレスモータの回転位置を指定

おかもちリベンジ

PI制御

PID制御

PID制御 振幅可変

Arduinoコード

動作

関連記事

「正弦波駆動の応用 ーブラシレスモータ駆動への道11ー」への1件のフィードバック

コメントはこちらから コメントをキャンセル

PID制御振幅可変

「正弦波駆動の応用　ーブラシレスモータ駆動への道11ー」への1件のフィードバック

コメントはこちらからコメントをキャンセル