前回の記事では、電流センサ（ADC）と磁気エンコーダ（AS5600）の実装を行いました。これにより、システムはローターの回転位置を知る「目」と、負荷を感じ取る「神経」を獲得しました。

今回は、これらを統合し、ブラシレスモーターを駆動する FOC（Field Oriented Control：ベクトル制御） の実装を行います。
結論から述べると、FOCの実装によりモーターの駆動音は劇的に変化しました。これまで矩形波駆動で生じていた振動音や唸りが消え、無音で滑らかに回転する様子は、倒立振子の姿勢制御に必要な「高精度なトルク制御」の実現を示唆しています。

本記事では、STM32G4と自作ドライバ（DRV8311H）を用いたFOC実装の詳細と、開発中に遭遇した技術的課題について解説します。

1. なぜ「ただ回す」だけでは不十分なのか

倒立振子の制御において、モータードライバに求められる要件は「高トルク」よりも「低速域での滑らかさ」と「応答性」です。

従来のラジコンESCなどで採用される「矩形波駆動（6ステップ駆動）」は、120度ごとに通電相を切り替える方式です。この方式は実装が容易ですが、転流（Commutation）の瞬間にトルク変動（トルクリップル）が発生します。
倒立振子は、垂直付近での微細なバランス制御（ゼロ速度周辺での正負のトルク切り替え）を連続的に行います。矩形波駆動による振動は、制御ループにとって外乱ノイズとなり、安定性を著しく損なう原因となります。

したがって、回転子の角度に合わせて正弦波状の電流を流し、常に理想的な磁束ベクトルを生成する FOC（ベクトル制御） および SVPWM（空間ベクトル変調） の実装が不可欠となります。

2. システム構成

ハードウェア構成は前回までと同様です。

マイコン: STM32G474RET6 (Nucleo-64)
ゲートドライバ: DRV8311H (3相PWM入力、電流センスアンプ内蔵)
角度センサ: AS5600 (I2C接続磁気エンコーダ)
モーター: GBM2804 (ジンバルモーター)

3. FOCアルゴリズムの実装

今回実装したファームウェアの制御フローは、以下の3ステップで構成されています。現在は電流PI制御の調整前段階として、電圧指令値を直接指定する「電圧制御モード（Open-Loop FOC）」で動作検証を行っています。

3.1 角度取得と電気角への変換

まず、I2C経由でAS5600からローターの機械角 $\theta_m$ を読み取ります。
FOCの計算には、磁極の位置を示す「電気角 $\theta_e$」が必要です。使用するGBM2804は極対数（Pole Pairs）$P_n = 7$ であるため、以下の関係式で変換します。

$$\theta_e = (P_n \times \theta_m) \mod 2\pi$$

3.2 逆Park変換によるベクトル計算

モーターを回転させるためのトルク成分電圧 $V_q$ と、磁束成分電圧 $V_d$ を設定します。
通常、SPM（表面磁石型）モーターでは $V_d = 0$ とし、効率よくトルクを発生させるため $V_q$ に値を設定します。
これを回転座標系（d-q軸）から固定座標系（$\alpha-\beta$軸）へ変換（逆Park変換）します。

$$
\begin{aligned}
V_\alpha &= V_d \cos\theta_e - V_q \sin\theta_e \
V_\beta &= V_d \sin\theta_e + V_q \cos\theta_e
\end{aligned}
$$

3.3 SVPWM（空間ベクトル変調）生成

算出された $V_\alpha, V_\beta$ を、各相（U, V, W）のPWMデューティ比に変換します。
ここでは単純な正弦波PWM（SPWM）ではなく、第3次高調波を注入した SVPWM を採用しました。これにより、SPWMと比較してDCバス電圧の利用率を約15.5%向上させることができます。

波形生成には、計算負荷の低い「Min-Max法」を用いています。

$$
\begin{aligned}
V_{offset} &= -\frac{1}{2} (\max(V_a, V_b, V_c) + \min(V_a, V_b, V_c)) \
V_{pwm_u} &= V_a + V_{offset}
\end{aligned}
$$

/* USER CODE BEGIN Header */
/**
  * 記事用：SVPWM電圧制御モード (電流PI制御なし)
  * 目的：AS5600の角度に合わせて、SVPWMで滑らかにモーターを回す
  */
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "main.h"
#include <math.h>
#include <stdio.h>

/* --- 設定 --- */
#define POLE_PAIRS 7.0f       // GBM2804: 7極対
#define V_REF 12.0f           // 電源電圧 12V
#define PWM_PERIOD 4250.0f    // ARR値
#define AS5600_ADDR (0x36 << 1)

// ★ここをいじって強さを変える★
// 電圧の強さ (0.0 ~ 12.0)
// 1.0V ~ 3.0V くらいでジンバルモーターは十分回ります
#define TARGET_VOLTAGE 2.0f   

/* --- 変数 --- */
ADC_HandleTypeDef hadc1;
I2C_HandleTypeDef hi2c2;
UART_HandleTypeDef hlpuart1;
TIM_HandleTypeDef htim1;

float Theta_mech = 0.0f;
float Theta_elec = 0.0f;
float zero_offset_mech = 0.0f; // アライメントで取得

// 表示用
float Iu_disp, Iv_disp, Iw_disp; 

/* プロトタイプ宣言 */
void SystemClock_Config(void);
static void MX_GPIO_Init(void);
static void MX_LPUART1_UART_Init(void);
static void MX_TIM1_Init(void);
static void MX_ADC1_Init(void);
static void MX_I2C2_Init(void);

// printf用
int _write(int file, char *ptr, int len) {
  HAL_UART_Transmit(&hlpuart1, (uint8_t *)ptr, len, 100);
  return len;
}

// 角度取得
float AS5600_GetAngle(void) {
    uint8_t raw[2];
    HAL_I2C_Mem_Read(&hi2c2, AS5600_ADDR, 0x0C, I2C_MEMADD_SIZE_8BIT, raw, 2, 10);
    uint16_t val = ((uint16_t)raw[0] << 8) | raw[1];
    return (float)val * (2.0f * M_PI / 4096.0f);
}

// 角度正規化
float normalize_angle(float angle) {
    float a = fmod(angle, 2.0f * M_PI);
    return (a < 0.0f) ? a + 2.0f * M_PI : a;
}

int main(void)
{
  HAL_Init();
  SystemClock_Config();
  MX_GPIO_Init();
  MX_LPUART1_UART_Init();
  MX_TIM1_Init();
  MX_ADC1_Init();
  MX_I2C2_Init();

  printf("=== Motor Open Loop Control Start ===\r\n");

  // PWM開始
  HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_1);
  HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_2);
  HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_3);

  // ADC開始 (電流見るだけ用)
  HAL_ADCEx_Calibration_Start(&hadc1, ADC_SINGLE_ENDED);
  HAL_ADCEx_InjectedStart_IT(&hadc1);

  // --- 1. アライメント（0点合わせ） ---
  printf("Aligning...\r\n");
  // U相に固定 (PWM Mode2なので反転注意)
  TIM1->CCR1 = 2125; // 50%
  TIM1->CCR2 = 4250; // 0%
  TIM1->CCR3 = 4250; // 0%
  HAL_Delay(1000);

  // 0点記録
  // ★重要: 回転方向が逆ならここを (2PI - GetAngle()) にする
  float raw = AS5600_GetAngle();
  zero_offset_mech = 2.0f * M_PI - raw; // 逆回転対応版
  if(zero_offset_mech < 0) zero_offset_mech += 2.0f*M_PI;
  if(zero_offset_mech > 2.0f*M_PI) zero_offset_mech -= 2.0f*M_PI;

  printf("Offset: %.2f rad\r\n", zero_offset_mech);

  // 電圧オフ
  TIM1->CCR1 = 4250; TIM1->CCR2 = 4250; TIM1->CCR3 = 4250;
  HAL_Delay(500);

  int count = 0;

  while (1)
  {
      // 1. 角度取得
      float raw_angle = AS5600_GetAngle();
      Theta_mech = 2.0f * M_PI - raw_angle; // 逆回転対応
      if(Theta_mech < 0) Theta_mech += 2.0f*M_PI;

      Theta_elec = normalize_angle((Theta_mech - zero_offset_mech) * POLE_PAIRS);

      // 2. 電圧ベクトル計算 (電流PI制御ではなく、直接電圧を指定)
      // d軸電圧=0, q軸電圧=TARGET_VOLTAGE (常に回転方向に電圧をかける)
      float Vd = 0.0f;
      float Vq = TARGET_VOLTAGE; 

      // 3. 逆パーク変換 (dq -> alpha,beta)
      float c = cosf(Theta_elec);
      float s = sinf(Theta_elec);
      float Valpha = Vd * c - Vq * s;
      float Vbeta  = Vd * s + Vq * c;

      // 4. SVPWM (alpha,beta -> U,V,W)
      float U_ref = Valpha;
      float V_ref = (-0.5f * Valpha) + (0.866f * Vbeta);
      float W_ref = (-0.5f * Valpha) - (0.866f * Vbeta);

      // 5. Duty計算
      float du = (U_ref / V_REF) + 0.5f;
      float dv = (V_ref / V_REF) + 0.5f;
      float dw = (W_ref / V_REF) + 0.5f;

      // リミット
      if(du>0.95f) du=0.95f; if(du<0.05f) du=0.05f;
      if(dv>0.95f) dv=0.95f; if(dv<0.05f) dv=0.05f;
      if(dw>0.95f) dw=0.95f; if(dw<0.05f) dw=0.05f;

      // 6. 出力 (PWM Mode 2反転)
      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);

      // --- ログ出力 (見て楽しむ用) ---
      count++;
      if(count >= 5000) { // 適当に間引く
          // 電流も一応読んで表示だけする（制御には使わない）
          float ad_u = (float)ADC1->JDR1;
          float ad_v = (float)ADC1->JDR2;
          // 電圧モードなので電流はずれるかもしれないが、波形は見れる
          printf("Th:%.2f Vq:%.1f Duty:%.2f\r\n", Theta_elec, Vq, du);
          count = 0;
      }
  }
}

4. 実装における技術的課題

理論通りにコードを書いても、実機では予期せぬ挙動が発生します。特に以下の2点が大きな障壁となりました。

① PWM Mode 2 の論理反転トラップ

Day 15の記事で解説した通り、ローサイド・シャント抵抗での電流計測タイミングを確保するため、タイマー設定を 「Center Aligned Mode 1」かつ「PWM Mode 2」 に設定していました。

PWM Mode 1: カウンタ値 < CCR で Active (High)
PWM Mode 2: カウンタ値 < CCR で Inactive (Low)

この設定により、CCR（Capture Compare Register）の値と出力デューティの関係が逆転します。
これに気づかず、初期化コードで CCR = 0 （出力オフのつもり）を設定したところ、実際には Duty 100%（フルパワー） が出力される事態となりました。
対策として、ソフトウェア側で CCR = ARR - Target_Duty_Count のように論理を反転させて書き込む処理を追加しました。

② 電気角アライメントの重要性

エンコーダが示す「0度」と、モーターの電気的な「0度（d軸）」は物理的に一致していません。このズレ（オフセット）が正確でないと、$V_q$（トルク）を印加しても $V_d$（磁束引張）として消費され、モーターは回りません。

起動シーケンスとして、以下のアライメント処理を実装しました。

強制的に $V_d$ に電圧を印加し、$V_q = 0$、電気角指令 $\theta = 0$ で固定する。
モーターのローターが磁力で特定の位置（電気角0度）に引き寄せられ、静止するのを待つ。
その時のエンコーダ角度を読み取り、ゼロ点オフセット として保存する。

このシーケンスを経ることで、エンコーダ値と電気角の同期が確立され、スムーズな回転が可能になりました。

5. 動作検証：ログデータが語る「FOCの正しさ」

今回実装した「電圧制御モード（Open-Loop FOC）」が正しく機能しているかを確認するため、シリアル通信で電気角（Theta）と出力Duty比をログ取得し、Excelでグラフ化しました。

その結果がこちらです。

横軸: 電気角 [rad] ($0$ ～ $2\pi$)
縦軸: U相のDuty比 ($0.0$ ～ $1.0$)

この美しい波形は、単なるサイン波ではありません。以下の3つのポイントから、FOCアルゴリズムがハードウェア上で完璧に動作していることが読み取れます。

① 計算通りの振幅（電圧）

今回のテストでは、12V の電源電圧（$V_{bus}$）に対し、q軸電圧（回転させる力）として 2.0V を指令しました。

理論値: PWMの変調度は $2.0V / 12.0V \approx \mathbf{0.166}$ となります。
実測値: グラフの中心（0.5）から、ピークは 0.67、ボトムは 0.33 付近まで振れています。
振幅 $= 0.67 - 0.50 = \mathbf{0.17}$

指令した「2.0V」という数値が、誤差なく正確にPWMのDuty比に変換されていることがわかります。プログラム内の単位変換係数が正しいことの証明です。

② 「馬の鞍」型波形（SVPWMの証）

グラフの頂上付近をよく見ると、きれいな丸い山ではなく、少し平坦に潰れたような形（台形に近い形）をしていることに気づきます。

これは計算ミスや歪みではなく、SVPWM（空間ベクトル変調）特有の 「馬の鞍（Saddle）」波形 です。
単純な正弦波（SPWM）ではなく、あえて3相全体に共通のオフセット電圧（ゼロ相電圧）を注入することで、電源電圧を最大限（純粋なサイン波より約15.5%多く）有効活用する処理が入っています。

この波形が見えるということは、Inverse Park Transform から SVPWM の計算までが教科書通りに機能している決定的な証拠 です。

③ 角度との完全同期

ログ数値を追うと、電気角の変化に対してDuty比が矛盾なく追従しています。

$0$ rad / $2\pi$ rad: Dutyは中心の 0.5（電圧ゼロ）。
$\pi/2$ rad (90度): $\sin$波の頂点付近。
$3\pi/2$ rad (270度): $\sin$波の底付近。

エンコーダで読み取った「物理的な角度」と、ドライバが出力する「電気的な波」が寸分の狂いもなく同期しています。
従来の矩形波駆動（6ステップ）のような「ガクッ」という切り替わりが一切ないこの滑らかな波形こそが、今回のテーマである 「圧倒的な静音性」 の正体です。

6. 実際の動作確認

実際の回転の様子です。

指で負荷をかけても脱調することなく、粘り強く滑らかに回り続けます。耳をすませても回転音はほとんど聞こえません。これこそがFOCの恩恵です。

7. まとめと次回予告

Day 16では、STM32と自作ドライバを用いたFOC（電圧制御モード）の実装に成功しました。
SVPWMによる駆動は極めて静粛かつ滑らかであり、倒立振子の姿勢制御に求められる分解能と応答性を満たす「足回り」が完成しました。

次回の課題:
現在は電圧ベクトルを直接指定して回していますが、倒立振子の姿勢制御器（PIDコントローラ等）が出力するのは「トルク指令値」です。トルクを正確に制御するためには、電流値をフィードバックする 電流PI制御ループ（Current Loop） の実装が必要です。

次回 Day 17 では、今回ログモニタのみに使用した電流値をフィードバック制御に組み込み、目標トルクに対して正確に電流を流す「クローズドループ制御」を完成させます。

アドベントカレンダー参加中
STM32×AIで「3軸倒立振子」を作る25日間（ひとりアドカレ）Advent Calendar 2025