1
0

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?

Day 16

STM32×AIで「3軸倒立振子」を作る25日間（ひとりアドカレ） Advent Calendar 2025

@RamTuckey

STM32と自作ドライバで挑むFOC実装

1
Posted at

前回の記事では、電流センサ（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 (ジンバルモーター)

PXL_20251215_220146200.jpg

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$（磁束引張）として消費され、モーターは回りません。

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

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

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

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

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

その結果がこちらです。

image.png

  • 横軸: 電気角 [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

1
0
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
1
0

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?