3軸リアクションホイール倒立振子を作るアドカレの17日目です。
1. はじめに：電圧制御から電流制御へ
Day 16では、SVPWM（空間ベクトル変調）を用いた「電圧制御（Open-Loop FOC）」の実装に成功しました。これによりモーターは静粛かつ滑らかに回転するようになりましたが、これはあくまで指令した電圧ベクトルを出力しているに過ぎません。
倒立振子のように、外乱（傾き）に対して瞬発的に姿勢を維持するシステムには、「必要なだけの力（トルク）」 を正確に出力する能力が求められます。モーターにおけるトルクは電流に比例するため（$T = K_t \cdot I$）、電圧ではなく電流を直接制御する必要があります。
本日は、Day 16で構築したシステムに 「電流フィードバックループ（PI制御）」 を実装し、FOC（ベクトル制御）を完成させます。さらにその応用として、プログラムによってモーターの挙動を物理的なバネのように振る舞わせる 「バーチャルスプリング（電子バネ）」 の実験を行います。
2. 実装の壁：1Aの異常電流と物理的解決
電流制御ループを閉じた（フィードバックを開始した）直後、システムは予期せぬ挙動を示しました。
-
現象:
- 無負荷状態であるにもかかわらず、電源装置の電流表示が 1A を超える。
- ローターを手で止めようとすると、逆に回転を加速させるような力が働く（正帰還動作）。
-
原因分析:
- Day 16の実装では、回転方向の不整合を解消するために、プログラム上で角度計算に
2PI - angleのような補正を行っていました。 - しかし、電流フィードバックにおいては、「電気角の回転方向」と「物理的なトルク発生方向（3相の相順）」の不一致 は致命的です。制御器が「誤差を減らそう」として出力した電流が、逆に「誤差を増やす」方向にトルクを発生させてしまい、制御が発散していました。
- Day 16の実装では、回転方向の不整合を解消するために、プログラム上で角度計算に
解決策：物理配線の修正
ソフトウェアでの小手先の修正（符号反転など）は、さらなる混乱を招くため却下しました。根本的な解決策として、インバータとモーター間の配線（U相とV相）を物理的に入れ替えました。
これにより、モーターの回転方向が物理的に反転し、以下の成果が得られました。
- プログラム上の角度計算が
Theta = Raw_Angle（補正なし）で整合するようになった。 - アライメント（電気角0度合わせ）が正常に機能するようになった。
- 結果として、無負荷時の消費電流は 0.1A以下 に激減し、制御系が安定した。
3. バーチャルスプリング（電子バネ）の実装
電流制御（トルク制御）が正しく機能していることを実証するため、モーターを「仮想的なバネ」として動作させるプログラムを実装します。
制御則は、物理学におけるフックの法則 $F = -kx$ をそのまま適用します。ここでは、変位 $x$ を「角度偏差」、力 $F$ を「目標電流 $I_q$」に置き換えます。
void HAL_ADCEx_InjectedConvCpltCallback(ADC_HandleTypeDef *hadc)
{
// ADC1以外からの呼び出しや、FOC無効時は何もしない
if (hadc->Instance != ADC1 || !foc_enable) return;
// 1. 電流取得 & [A]変換
float Iu_l = ((float)ADC1->JDR1 - offset_u) * (3.3f / 4095.0f) / DRV8311_GAIN;
float Iv_l = ((float)ADC1->JDR2 - offset_v) * (3.3f / 4095.0f) / DRV8311_GAIN;
float Iw_l = ((float)ADC1->JDR3 - offset_w) * (3.3f / 4095.0f) / DRV8311_GAIN;
//float Iu_l = (offset_u - (float)ADC1->JDR1) * (3.3f / 4095.0f) / DRV8311_GAIN;
//float Iv_l = (offset_v - (float)ADC1->JDR2) * (3.3f / 4095.0f) / DRV8311_GAIN;
//float Iw_l = (offset_w - (float)ADC1->JDR3) * (3.3f / 4095.0f) / DRV8311_GAIN;
// オフセット微調整 (3相合計が0になるように)
float Iavg = (Iu_l + Iv_l + Iw_l) / 3.0f;
Iu_l -= Iavg;
Iv_l -= Iavg;
Iw_l -= Iavg;
// グローバル変数へ反映(ログ用)
g_Iu = Iu_l; g_Iv = Iv_l; g_Iw = Iw_l;
// 2. 座標変換 (Clarke & Park)
// メインループで更新された最新の角度を使う
float th = g_theta_elec;
float c = cosf(th);
float s = sinf(th);
float I_alpha = Iu_l;
float I_beta = (Iu_l + 2.0f * Iv_l) * 0.57735f; // 1/sqrt(3)
float Id_l = I_alpha * c + I_beta * s;
float Iq_l = -I_alpha * s + I_beta * c;
g_Id = Id_l; g_Iq = Iq_l;
// ① 目標は「累積角度 0」 (起動した場所)
float target = 0.0f;
// ② ズレを計算 (単純な引き算)
// ★重要: ここで if(> PI)... のような正規化をしてはいけません！
// 遠くに行けば行くほど値を大きくするためです。
float angle_err = target - g_total_angle;
// ③ バネ定数 (K)
float spring_k = 0.5f; // 好みで調整
// ④ 目標電流
float Iq_ref = spring_k * angle_err;
// ★重要: 安全リミットを広げる
// 遠くまで回した時に強い力を出すため、モーター定格付近まで許可
if (Iq_ref > 1.5f) Iq_ref = 1.5f;
if (Iq_ref < -1.5f) Iq_ref = -1.5f;
// ===== ここから下は「電流PI制御」 (目標に追従させる処理) =====
// --- d軸 (目標 0A: 常に0にする) ---
float Id_err = 0.0f - Id_l;
Id_err_int += Id_err * CONTROL_DT;
// ... (d軸の積分リミットや計算はそのまま) ...
float limit_i = VOLTAGE_LIMIT / KI_GAIN; // 定義がない場合は直接数値を書いてもOK
Id_err_int = clampf(Id_err_int, -limit_i, limit_i);
float Vd_l = KP_GAIN * Id_err + KI_GAIN * Id_err_int;
// --- q軸 (目標 Iq_ref: さっき計算したバネの力) ---
float Iq_err = Iq_ref - Iq_l; // 上で作った Iq_ref を使用
// ★ここを変更: IQ_TARGET ではなく、計算した Iq_ref を使う
//float Iq_err = Iq_ref - Iq_l;
Iq_err_int += Iq_err * CONTROL_DT;
Iq_err_int = clampf(Iq_err_int, -limit_i, limit_i);
float Vq_l = KP_GAIN * Iq_err + KI_GAIN * Iq_err_int;
// 4. 逆変換 & SVPWM
float Valpha = Vd_l * c - Vq_l * s;
float Vbeta = Vd_l * s + Vq_l * c;
float U_ref = Valpha;
float V_ref = (-0.5f * Valpha) + (0.866f * Vbeta);
float W_ref = (-0.5f * Valpha) - (0.866f * Vbeta);
// ゼロ相注入 (SVPWM)
float v_max = U_ref;
if(V_ref > v_max) v_max = V_ref;
if(W_ref > v_max) v_max = W_ref;
float v_min = U_ref;
if(V_ref < v_min) v_min = V_ref;
if(W_ref < v_min) v_min = W_ref;
float v_zero = -0.5f * (v_max + v_min);
U_ref += v_zero;
V_ref += v_zero;
W_ref += v_zero;
// 5. Duty計算 & PWM更新
float du = (U_ref / V_REF) + 0.5f;
float dv = (V_ref / V_REF) + 0.5f;
float dw = (W_ref / V_REF) + 0.5f;
du = clampf(du, 0.05f, 0.95f);
dv = clampf(dv, 0.05f, 0.95f);
dw = clampf(dw, 0.05f, 0.95f);
g_duty_u = du; // ログ用
// Mode 2 なので (1-duty)
TIM1->CCR1 = (uint32_t)((1.0f - du) * PWM_PERIOD);
TIM1->CCR2 = (uint32_t)((1.0f - dv) * PWM_PERIOD);
TIM1->CCR3 = (uint32_t)((1.0f - dw) * PWM_PERIOD);
}
この数行のコードにより、モーターは単なる回転体から、外力に対して復元力を持つ 「弾性体」 へと性質を変えます。
4. 動作検証：ログデータが語る「電子バネ」の挙動
実装したプログラムをSTM32に書き込み、実際にモーター軸を手で弾いた際の挙動を検証しました。
実際の動作
データ解析
動作中の内部データをシリアル通信で取得し、グラフ化しました。
- 青線 (Electrical Angle): 手で加えられた外乱による角度変化。
- 緑線 (Iq / Torque Current): モーターが発生させるトルク電流。
- オレンジ線 (Id / Flux Current): 磁束成分電流（制御目標は0）。
このグラフから、電流制御が機能していることを示す3つの特徴が読み取れます。
-
角度変化への即応性 (青線 vs 緑線)
青線（角度）が大きく変化した瞬間、即座に緑線（$I_q$）が逆方向にスパイクしています。これは、「位置ズレを検知し、復元トルクを発生させる」 というフィードバック制御が遅延なく機能していることを示します。 -
静止時の省エネルギー性
外乱がない区間（グラフの平坦部）では、緑線（$I_q$）はほぼ 0A に収束しています。
電圧制御で位置を保持する場合、常に一定の電圧を印加し続ける必要がありますが、電流制御では 「負荷がない時は電流を流さない（トルクゼロ）」 という効率的な動作が自動的に実現されています。 -
無効電流の抑制 (オレンジ線)
急激な負荷変動時でも、オレンジ線（$Id$）は0付近に制御されています。PI制御器が $V_d$ を適切に調整し、トルクに寄与しない無駄な電流（無効電流）を抑制している証拠です。これが、1Aの異常発熱を防ぐ鍵となっています。
5. 電圧制御との決定的な違い：安全性とトルクの質
「バネのような動き」自体は、Day 16の電圧制御でも位置フィードバックを行えば模擬することは可能です。しかし、電流制御（FOC） には決定的な優位性があります。
-
電圧制御の限界（リスク）:
電圧制御で位置を保持している際、外力によって無理やり軸を固定（ロック）すると、モーターの逆起電力が消失します。その結果、巻線抵抗のみで決まる大電流（$I = V/R$）が流れ続け、最悪の場合モーターやドライバが焼損します。 -
電流制御の利点（安全性）:
電流制御では、フィードバックループが常に電流値を監視しています。手で軸を完全にロックしても、制御器は 「指定された電流値（例：0.5A）以上は流さない」 ように電圧を自動的に下げます（$V = I_{ref} \times R$）。
実際、実験中に手で強くひねってモーターを停止させても、電源装置の電流値は設定範囲内に収まっており、システムは安全に稼働し続けました。
倒立振子のように、常に予測不能な外乱や負荷変動にさらされるロボットにおいて、この 「トルクのリミッター機能」 と 「必要な時だけ力を出す特性」 は不可欠です。
6. まとめと次回予告
Day 17では、FOCの核心である 「電流フィードバック制御」 を実装し、モータードライバ開発の山場を越えました。
- 物理配線の修正: ソフトウェアでの対症療法ではなく、ハードウェアの整合性を取ることで制御を安定化させました。
- 電子バネの実装: プログラム（フックの法則）によって、モーターの物理的特性を書き換える実験に成功しました。
- 安全性: 電流制御ループにより、過負荷時でもシステムを保護できることを確認しました。
今回実装した「電子バネ」は、角度偏差に応じて復元力を発生させました。この「角度偏差」を「倒立振子の傾き」に置き換えれば、それはそのまま 倒立制御 になります。
強靭な「筋肉（FOCドライバ）」は完成しました。次に必要なのは、傾きを感じ取る「三半規管（センサ）」です。
次回予告：
次回は、高性能6軸IMUセンサ 「ICM-42688」 を搭載し、STM32とのSPI通信の実装に挑みます。高速かつ低ノイズに「傾き」を読み取れるようになった時、この電子バネは「倒立するためのバネ」へと進化します。