SimpleFOCでブラシレスモーターを回す
この記事は
学ロボ Advent Calendar 2023
の４日目の記事となります．
目次
自己紹介
電気通信大学 2020年入学のpizac(ぴざっく)と申します．
電通大ロボメカ工房で学ロボ20~23の電装を担当してました．
電通大の回路と制御を全て担当しました。本番ではトラブルで動かなかったのですが、電装系は大幅に進化しました。ちょっと作った基板を自慢しようと思います。(下に続く)#学ロボ
— pizac (@pizac__) 2023年6月4日
はじめに
近年の学ロボではブラシレスモーター(BLDC)を使うチームが増えています．BLDCはアウトローターにできるため，大きなトルクを得ることができます．そのため，ギアヘッドを小さくして小型化＆軽量化ができます．ブラシツキに比べて制御が大変ですが，Odriveなど安価なモータードライバー(MD)があり，誰でも簡単にBLDCを回すことができます．
また，DJI社のロボマスモーターが人気です．これはBLDCとMDがセット販売されていて，CANで電流指令値を送れば電流追従してくれる代物です．電流，速度，位置も返してくれるため，PIDカスケード制御で速度制御や位置制御ができます．
これらの安価で高性能なMDにより，自作MDを作る回路班も減ってきました．
ロボコン自作MD界隈は界隈の消滅の危機に対し，ロボマスモタドラ買ったら負け教という謎の宗教を立ち上げるほどです．
— さばねっこ🐟 (@sabanekko1) 2023年11月28日
界隈の繁栄のため，今回は誰でもできるBLDCのベクトル制御を記事にしました．
ベクトル制御とは，BLDCの制御方法の１つで迷ったらこれを使いましょう．
準備
ベクトル制御にはSimpleFOCを使います．OdriveやVescなど他のオープソースのライブラリに比べて読みやすいので採用しました．
ハードはB-G431B-ESC1を使います．DigikeyやMouserで約3千円で買えます．
マイコン，書き込み機，MDが入っていて，回路図も公開されています．
CANFD＠1Mbpsも搭載されてるため上記のC610/C620を作ることもできます．
回転子の角度を読み取るために，インクリメンタルのロータリーエンコーダーAMT-102-Vを使います．
ブラシレスモーターにはC5055-400KVを使います．PMSMであればなんでも構わないです．
OSはUbuntu 22.04ですが，Windowsでも大丈夫だと思います．
配線
エンコーダーの信号線などをはんだ付けします．この作業が一番大変です．
コツは，導線と基板にはんだを先に塗っておき，繋げるイメージです．
Lチカ
VSCodeに拡張機能のPlatformIO IDEとSerial Monitorを入れます．
ボードを指定してプロジェクトを作成します．
src/main.cppを書き換えます．
https://github.com/pizacl/B-G431B-ESC1/blob/main/test/L_chika.cpp
platformio.iniで設定を追加します．
https://github.com/pizacl/B-G431B-ESC1/blob/main/platformio.ini
下の水色のバーの"→"で書き込みます．
LEDが点滅すれば成功です．
エンコーダー読み取り
src/main.cppを書き換えます．
https://github.com/pizacl/B-G431B-ESC1/blob/main/test/Encoder.cpp
Serial Monitorでシリアル通信します．
モーターをぐりぐり動かして角度が変わったら成功です．
ベクトル制御
src/main.cppを書き換えます．
https://github.com/pizacl/B-G431B-ESC1/blob/main/src/main.cpp
シリアルモニターでBaud rateを115200 bpsに設定します．
T100と入力してEnterを押します．これは，100 rad/sで回す指示です．
ロボマスモタドラ買ったら負け教を広めるため、誰でもできるブラシレスモーターのベクトル制御を書く pic.twitter.com/h6PWZOwkRr
— pizac (@pizac__) 2023年12月1日
BLDCが回れば成功です．
ソースコードの解説
上から解説します．
VSCodeはCtrl+クリックで実装部分に飛べます．内部実装を読み解くと理解が深まります．
BLDCMotor motor = BLDCMotor(7);
モーターの極数/2を設定します．極数はデータシートに書いてあります．モーター名のP14とかP36とかは14極，36極という意味です．
Encoder encoder = Encoder(A_HALL1, A_HALL2, 2048, A_HALL3);
エンコーダーのA, B, Zのピンと，ppr(解像度)を設定します．
SimpleFOCでは他に磁気センサやホールセンサを選べます．
void doTarget(char* cmd) { command.motion(&motor, cmd); }
T100とか送った際に呼ばれる関数です．内部ではmotor.targetを変えてます．
motor.targetに向かって速度制御や位置制御します．
driver.voltage_power_supply = 12;
駆動用の電源電圧です．今回は12 Vを使用しました．
motor.voltage_sensor_align = 1;
電気角(motor.electrical_angle)の原点とモーターの原点を合わせる時の電圧です．
電圧を大きくするとMDが燃える可能性があります．
BLDCを回す際にUVW相に順番に電流を流しますが，１回転する間に何回か往復します．電気角とは，その１周内の角度です．つまり0から2πを往復します．
motor.velocity_index_search = 3;
エンコーダーのZ相で原点出しを行います．角度(motor.shaft_angle)の原点です．
motor.voltage_limit = 6;
電圧はSin波で入力しますが，その最大値です．
例えば，停止してる状態は全ての相に半分の3 Vがかかります．回転時は0 Vから6 Vまで動かして交流を作ります．
モーターはこの電圧と逆起電力がつりあう速度まで回ります．つまり，この値を電源電圧ギリギリまで上げると最大速度が上がります．同じにするのは回路的に良くないです．
motor.controller = MotionControlType::velocity;
位置センサを使わない強制転流，トルク制御，速度制御，位置制御を選べます．
motor.torque_controller = TorqueControlType::foc_current;
電流制御するかどうかです．
その後にPID関係がいろいろとありますが，これはPIDループが重なっているカスケード制御のためです．位置制御の場合３つのPIDループが重なっています．
motor.PID_current_q.P = motor.PID_current_d.P = 0.1;
motor.PID_current_q.I = motor.PID_current_d.I = 10;
電流のPIゲインです．ベクトル制御は回転に影響するq軸電流を追従して，影響しないd軸電流を0にして効率を上げる方法です．基本的にq軸とd軸は同じゲインでいいです．
このゲインはモーターの抵抗とインダクタンスから計算できますが，今回は適当に入れました．
motor.PID_velocity.P = 0.5;
motor.PID_velocity.I = 1;
motor.PID_velocity.output_ramp = 1000;
motor.LPF_velocity.Tf = 0.01;
速度のPIゲインです．
output_rampは台形に電流を入力する際の傾きです．低くすれば緩やかにトルク(加速度)が変動します．
LPF_velocity.Tfは速度にかかるローパスフィルタ(LPF)の時定数です．速度は角度の微分なので高周波成分が増幅されます．これを除くためにLPFを入れます．時定数を大きくすると追従性能が落ちるため，なるべく低い方が良いです．
motor.P_angle.P = 20;
位置のPゲインです．大きくすると素早く追従しますが発振します．
motor.init();
ドライバーの初期化がうまくいったか，パラメーターが正しいかどうかを調べます．
motor.initFOC();
原点出しやエンコーダーとモーターの回転の方向を合わせたりします．
途中にcurrentSense.skip_align = true;とありましたが，電流センサの位置を特定するのをスキップしてます．
motor.move();
速度と位置のPIDを更新します．電流に比べて遅いためmotor.motion_downsampleで制御周期を遅くできます．
motor.loopFOC();
電流のPIを更新して，電圧入力を行います．
おわりに
BLDCのベクトル制御についてなんとなくわかったと思います．
よりベクトル制御を理解するにはこの本がおすすめです．
あとは回路ですが，とりあえずB-G431B-ESC1がどういう構造なのかを勉強して，違うICでできないか考えれば良いと思います．また，Odrive v3.6や，ゲートドライバの評価ボードの回路図を参考にしましょう．ブラシツキMDが作れれば，そこまで難しくないと思います．
もしお役に立てたなら，電通大ロボメカ工房の可愛い後輩達と仲良くしていただけると幸いです．https://x.com/UECrmfNHK
それでは，良きBLDCMD自作ライフを．
