ESP32でベクトル制御 ーブラシレスモータ駆動への道9ー
HomeMadeGarbage Advent Calendar 2022 |12日目
“50日後にセンサレスベクトル制御してみたい俺” と銘打って毎日少しずつモータの勉強を進めていたことを先日ブログにて報告させていただきました。
残念ながらセンサレスベクトル制御は達成できませんでしたが、磁気エンコーダで回転をセンシングしたベクトル制御による回転は実現できましたので報告させていただきます。
目次
システム構成
回路構成は以下の通りです。
クローズドループ正弦波駆動の時の構成に電流センサを追加したのみです。
変数調整用にと可変抵抗を繋げていますがここでは使用せず、BlynkレガシーアプリでBLEで変数調整しました。
部品
- ESP32 評価ボード
- プリドライバ IR2302
- ドライバ NMOS 2SK4017
- ブラシレスモータ ポール数:14
- 磁気エンコーダ AS5048
- 電流センサ ACS712
センサありベクトル制御
システムの概要図は以下の通りです。
回転速度を指定してモータを制御します。
磁気エンコーダで回転速度とロータ位置を観測して、3相-2相変換や逆park変換時に使用します。
各PI制御のパラメータをBLEを介してBlynkレガシーアプリで調整して回転速度の応答性を改善しました。
コントローラのESP32はデュアルコアで使用し、以下のようにセンシングと回転制御を分担しました。
- core0:電流センシング、磁気エンコーダからロータ位置と回転速度検出とPI制御、シリアル出力
- core1:3相-2相変換、逆Park-Clarke変換、3次調波加算、PWM出力
動作
指定した回転速度に対して実速度がいい感じで追従しています。
負荷をかけて復帰する際にオーバーシュートが観られますのでパラメータはもう少し改善の余地があるかもしれません。
Arduinoコード
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 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 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249 250 251 252 253 254 255 256 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278 279 280 281 282 283 284 285 286 287 288 289 290 291 292 293 294 295 296 297 298 299 300 301 302 303 304 305 306 307 308 309 310 311 312 313 314 315 316 317 318 319 320 321 322 323 324 325 326 327 328 329 330 331 332 333 334 335 336 337 338 |
#define BLYNK_PRINT Serial #define BLYNK_USE_DIRECT_CONNECT #include <BlynkSimpleEsp32_BLE.h> #include <BLEDevice.h> #include <BLEServer.h> #include <AS5048A.h> char auth[] = "トークン"; AS5048A angleSensor(SS, false); #define periodPin 36 #define ampPin 39 #define uHin 25 #define vHin 26 #define wHin 27 #define uPin 34 #define vPin 35 int uPWMCH = 0; int vPWMCH = 1; int wPWMCH = 2; float Vu, Vv, Vw, Vmin, Vmax, Vzero; int period = 2000, amp = 350; float Vdd = 7.4; //電源電圧[V] unsigned long oldTime = 0, nowTime, loopTime; unsigned long measTime = 0; float rotSpeed; int State = 0; uint16_t rotData, rotDataIni; int rotDataCal, rotDataCalOld, rotDiff; int angleCal; int BottunState = 1; float u, v, w; float uOffset = 0, vOffset = 0; float Iu, Iv, Iw, Ia, Ib, Id, Iq, Id2, Iq2; float Iu_Meas, Iv_Meas, Iw_Meas; int IniOK = 0; float Kp = 0.05, Ki = 0.02; float Kpr = 2.0, Kir = 0.002; float diffPd, diffId = 0.0, diffPq, diffIq = 0.0, diffPr, diffIr = 0.0; float IqTarget; float rotSpeedTarget = 80; //指定回転速度[rad/s] float Div = 512.0; float Vd, Vq, Va, Vb; float theta = 0.0; //Core0 void rotPosition(void *pvParameters) { for (;;){ Blynk.run(); rotData = angleSensor.getRawRotation() >> 5; if(IniOK == 1){ delay(200); rotDataIni = rotData; IniOK = 2; } if(IniOK == 2){ nowTime = micros(); //電流センサ https://amzn.to/3euX3Id VOUT=2.5+0.185*I Iu_Meas = (analogReadMilliVolts(uPin) - uOffset) / 185; //[A] Iv_Meas = (analogReadMilliVolts(vPin) - vOffset) / 185; //[A] Iw_Meas = -Iu_Meas - Iv_Meas; rotDataCal = int(rotData - rotDataIni); if(rotDataCal < 0) rotDataCal += Div; rotDiff = rotDataCal - rotDataCalOld; if(rotDiff < 0){ rotDiff += Div; } if(rotDiff > 10){ rotDataCalOld = rotDataCal; loopTime = nowTime - oldTime; oldTime = nowTime; rotSpeed = rotDiff / Div * 2.0 * PI / (loopTime / 1000000.0); //[rad/s] } diffPr = rotSpeedTarget - rotSpeed; diffIr += diffPr; IqTarget = Kpr / 185.0 * diffPr + Kir / 185.0 * diffIr; IqTarget = constrain(IqTarget, 0, 1.2); if(nowTime - measTime > 50000){ Serial.print(IqTarget); Serial.print(", "); Serial.print(Iq); Serial.print(", "); Serial.print(rotSpeedTarget); Serial.print(", "); Serial.print(rotSpeed); Serial.println(""); measTime = nowTime; } /* Serial.print(Vu); Serial.print(", "); Serial.print(Vv); Serial.print(", "); Serial.print(Vw); Serial.print(", "); Serial.println(""); */ } disableCore0WDT(); } } //ヴァーチャルピンデータ受信 BLYNK_WRITE(V0) { BottunState = param.asInt(); } BLYNK_WRITE(V1) { period = param.asInt(); } BLYNK_WRITE(V2) { Kp = param.asFloat(); } BLYNK_WRITE(V4) { Ki = param.asFloat(); } BLYNK_WRITE(V9) { Kpr = param.asFloat(); } BLYNK_WRITE(V10) { Kir = param.asFloat(); } BLYNK_WRITE(V3) { rotSpeedTarget = param.asFloat(); } BLYNK_WRITE(V5) { diffId = 0; diffIq = 0; diffIr = 0; } void setup() { Serial.begin(2000000); angleSensor.begin(); Blynk.setDeviceName("motor"); Blynk.begin(auth); pinMode(uPin, ANALOG); pinMode(vPin, ANALOG); for(int i = 0; i < 100; i++){ uOffset += analogReadMilliVolts(uPin); vOffset += analogReadMilliVolts(vPin); delay(10); } uOffset = uOffset / 100.0; vOffset = vOffset / 100.0; 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); //回転位置検出 タスク xTaskCreatePinnedToCore( rotPosition , "rotPosition" // A name just for humans , 4096 // This stack size can be checked & adjusted by reading the Stack Highwater , NULL , 3 // Priority, with 3 (configMAX_PRIORITIES - 1) being the highest, and 0 being the lowest. , NULL , 0); for(int i = 0; i < 512; i++){ u = i; if(BottunState){ v = i + 512.0 * 2.0 / 3.0; w = i + 512.0 / 3.0; }else{ w = i + 512.0 * 2.0 / 3.0; v = i + 512.0 / 3.0; } if(u >= 512) u -= 512; if(v >= 512) v -= 512; if(w >= 512) w -= 512; Vu = amp *(1.0 + sin(2.0 * PI / (512.0 / 7.0) * u)) / 2.0; Vv = amp *(1.0 + sin(2.0 * PI / (512.0 / 7.0) * v)) / 2.0; Vw = amp *(1.0 + sin(2.0 * PI / (512.0 / 7.0) * w)) / 2.0; ledcWrite(uPWMCH, int(Vu)); ledcWrite(vPWMCH, int(Vv)); ledcWrite(wPWMCH, int(Vw)); delayMicroseconds(1500); } IniOK = 1; } void loop() { if(IniOK == 2){ //モータ電流 [A] Iu = Iu_Meas; Iv = Iv_Meas; Iw = Iw_Meas; //ロータ位置 (電気角) [rad] theta = rotDataCal / Div * 7.0 * 2.0 * PI; //2相変換 [A] //https://jp.mathworks.com/help/sps/ref/parktransform.html [電力不変の a 相を d 軸に揃える場合は、ブロックは次のように実装します] Id = sqrt(2.0/3.0) * (Iu * cos(theta) + Iv * cos(theta - (2.0/3.0) * PI) + Iw * cos(theta + (2.0/3.0) * PI)); Iq = -sqrt(2.0/3.0) * (Iu * sin(theta) + Iv * sin(theta - (2.0/3.0) * PI) + Iw * sin(theta + (2.0/3.0) * PI)); diffPd = Id; diffId += diffPd; Vd = Kp * diffPd + Ki * diffId; diffPq = Iq - IqTarget; //diffPq = Iq - rotSpeedTarget / 1000.0; diffIq += diffPq; Vq = Kp * diffPq + Ki * diffIq; /* Serial.print(Vd); Serial.print(", "); Serial.print(Vq); Serial.print(", "); */ //逆Park変換 [V] Va = Vd * cos(theta) - Vq*sin(theta); Vb = Vd * sin(theta) + Vq*cos(theta); /* Serial.print(Va); Serial.print(", "); Serial.print(Vd); Serial.print(", "); */ //逆Clarke変換 [V] Vu = sqrt(2.0/3.0) * Va; Vv = sqrt(2.0/3.0) * (Va * -0.5 + Vb * sqrt(3)/2.0); Vw = sqrt(2.0/3.0) * (Va * -0.5 - Vb * sqrt(3)/2.0); //3次調波加算 if(Vu > Vv){ if(Vu > Vw){ Vmax = Vu; if(Vv > Vw){ Vmin = Vw; }else{ Vmin = Vv; } }else{ Vmax = Vw; Vmin = Vv; } }else{ if(Vv > Vw){ Vmax = Vv; if(Vu > Vw){ Vmin = Vw; }else{ Vmin = Vu; } }else{ Vmax = Vw; Vmin = Vu; } } Vzero = (Vmax + Vmin)*0.5; /* MaxとMinの平均値 [V]*/ Vu = constrain(((Vu - Vzero) / Vdd) * 1024 + 512, 0, 1023); Vv = constrain(((Vv - Vzero) / Vdd) * 1024 + 512, 0, 1023); Vw = constrain(((Vw - Vzero) / Vdd) * 1024 + 512, 0, 1023); /* Serial.print(Vu); Serial.print(", "); Serial.print(Vv); Serial.print(", "); Serial.print(Vw); Serial.print(", "); Serial.println(""); */ ledcWrite(uPWMCH, int(Vu)); ledcWrite(vPWMCH, int(Vv)); ledcWrite(wPWMCH, int(Vw)); } } |
最新版のBlynkではBLEが使用できないので、以前のバージョンのBlynkレガシーを使用してBLE通信しています。
疑似正弦波を生成するためにドライバ駆動ピン(IO25~27)はledcWriteを用いて20kHz PWM出力します。分解能は10ビット(0~1023) (L. 194-202)。
起動の冒頭で電流センサの電圧を測ってオフセット値を取得します (L. 182-192)。
またモータをオープンループ正弦波駆動で一周させてモータの初期位置を設定します (L. 215-238)。
モータの回転位置・回転速度検出と電流センシングをデュアルコアのcore0で実施しています。
1周の分解能を14bitから9bit (512)にしています (L. 67)。
電流センサの電圧値から電流を算出します (L. 78-81)。
PI制御で回転速度の誤差からIqのターゲット値を算出 (L. 98-101)。
電流センサとエンコーダ位相のオフセット導出後はベクトル制御で回転し続けます (L. 244-337)。
3相-2相変換、逆Park-Clarke変換、3次調波加算、PWM出力を実施します。
ロータ位置から電気角を算出(使用しているブラシレスモータのロータの極数は14 [7ペア]) (L. 251)。
Blynkアプリで、モータ回転速度rotSpeedTarget、PI制御の各種係数をBLEで受信します (L. 140-172) 。
参考
おわりに
ここではESP32を用いたブラシレスモータのセンサありベクトル制御を楽しみました。
電流を管理して回転が制御できるようになったのは今後の人生において非常に有用であると考えます。
いつかセンサレスベクトル制御にも挑戦したいです。