STM32でPD制御——サーボ・BNO055・printfを全部つないで「傾いたら戻る」を作った話

はじめに

このシリーズでは、STM32（Nucleo-L432KC）で一つずつ部品を動かしてきました。

PWMでサーボを動かす（PSC/ARR/CCRで1µs刻み・50Hz）
BNO055をI2Cで読む（2段階読み取り・LSB/MSB）
printfでマイコンの値をPCに出す（_writeのリターゲット）

今回はこの3つを合体させます。ゴールは「基板を手で傾けると、サーボがそれを打ち消す方向に動く」——つまり姿勢を水平に保つPD制御です。

ただ、実際に動かすまでに一箇所どうしても越えられない壁がありました。「D項（微分）をどう作るか」です。素直に「角度を時間で微分」する方法でやったら誤差が大きくて、サーボがガタガタ震える。最終的にキネマティクス方程式で角速度を算出することで解決したのですが、そこに辿り着くまでが長かった。この記事はその顛末込みで書きます。

動作イメージ：基板を傾ける → BNO055がピッチ角θとその角速度θ̇を返す → PDで指令値を計算 → サーボが逆方向に動く

前提知識：なぜP制御だけだと足りないのか

まず「P制御」から。ピッチ角θを0（水平）に戻したいなら、一番素直なのは

指令 = k_p × θ

傾いた分だけ比例して戻す。これがP（比例）制御です。

ところがP制御だけだと、行き過ぎて戻り過ぎる（オーバーシュート）。振り子みたいに水平の周りで揺れ続けたり、ゲインを上げると発振したりします。理由は単純で、「今どれだけ傾いているか」しか見ておらず、「今どっち向きに、どれだけの速さで動いているか」を無視しているから。

そこでD（微分）項を足します。

指令 = k_p × θ  +  k_d × θ̇

θ̇（角速度）は「これから傾きが増えるのか減るのか」の情報です。速く倒れつつあるなら強めにブレーキ、もう戻りつつあるなら弱める。Dは"予測して先にブレーキをかける"係数だと思うとしっくりきます。この記事の実装では、

k_p = 1
k_d = 0.2

を使いました（後で理由を書きます）。

I項（積分）は今回は入れていません。まずPDで挙動を掴むのが目的だったのと、姿勢を"だいたい水平"に保つだけなら定常偏差はそこまで気にならなかったためです。

全体の構成：誰が何をするか

今回は役割をタイマで分けました。

役割	使うもの	周期
センサ読み取り＋PD演算	while(1)ループ内でI2C通信	約30ms（I2C時間込みで揺れる）
サーボへのCCR書き込み	TIM7の割り込み	きっちり50Hz（20ms）
サーボPWM出力	TIM15 CH2	50Hz（20ms周期）
値のログ出力	USART2 → printf	ループごと

ポイントは、「計算するループ」と「サーボに指令を渡すタイミング」を分けたことです。while(1)はI2C通信の時間でどうしても周期がブレます。でもサーボへの書き込みをTIM7の割り込み（正確な50Hz）に任せておけば、出力側は一定リズムを保てます。

「じゃあ周期がブレたらD項の計算がおかしくなるのでは？」——ここが今回の肝で、後の「つまずきポイント」で回収します。

設定：CubeMXでやること

クロック（これが全ての土台）

このプロジェクトのSYSCLKは42MHzです（MSI 4MHz → PLL ×21 ÷2）。APBの分周は1なので、タイマのクロックも42MHz。

なぜ42MHzかというと、この後のプリスケーラが気持ちよく決まるからです。

TIM7（制御ループの時計）

基本タイマTIM7を「一定周期の割り込み」として使います。

Prescaler：42 - 1
Counter Period（ARR）：20000 - 1
Mode：Activated（One Pulse Modeはオフ）

計算するとこうなります。

計数クロック = 42MHz ÷ (41 + 1) = 42MHz ÷ 42 = 1MHz  → 1カウント = 1µs
オーバーフロー周期 = 20000カウント × 1µs = 20000µs = 20ms
割り込み周波数 = 1 ÷ 0.02s = 50Hz

プリスケーラを42-1にした瞬間、「1カウント＝1µs」になるのが42MHzの美味しいところ。µs単位でそのまま考えられます。NVICでTIM7のグローバル割り込みを有効化するのも忘れずに。

サーボへのPWMはTIM15のCH2から出します。TIM7と同じ設定で、

Prescaler：42 - 1（→ 1µs刻み）
Period（ARR）：20000 - 1（→ 20ms周期 = 50Hz）

CCRの値がそのままµsになります。

1000 → 1.0ms（下限）
1500 → 1.5ms（中央）
2000 → 2.0ms（上限）

シリーズ1回目でやったサーボ制御そのままの世界観です。

I2C1（BNO055）とUSART2（printf）

I2C1：7bitアドレス、プルアップ有効（BNO055のアドレスは0b0101000）
USART2：115200bps（printf用。_writeのリターゲットとTeraTerm側のボーレート一致は2回目・3回目の記事の通り）

コード

1. サーボへの書き込みはTIM7の割り込みで

計算結果ccr（float）をグローバルに持っておき、TIM7が発火するたびにサーボへ渡します。ここで**飽和（クランプ）**も入れて、サーボの可動範囲を超える指令が来ても1000〜2000µsに収めます。

static float ccr = 0.0f;

void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
    if (htim == &htim7) {
        if (ccr > 2000) {
            __HAL_TIM_SET_COMPARE(&htim15, TIM_CHANNEL_2, 2000);
        } else if (ccr < 1000) {
            __HAL_TIM_SET_COMPARE(&htim15, TIM_CHANNEL_2, 1000);
        } else {
            __HAL_TIM_SET_COMPARE(&htim15, TIM_CHANNEL_2, (int)ccr);
        }
    }
}

飽和処理はサーボを守るために地味に大事。PDの計算が暴れても、物理的にありえない指令はここで止まります。

2. 起動処理

setbuf(stdout, NULL);          // printfをバッファリングせず即出力（3回目の記事参照）

HAL_TIM_Base_Start_IT(&htim7); // TIM7割り込みスタート（= 50Hzの心拍）
HAL_TIM_PWM_Start(&htim15, TIM_CHANNEL_2);

HAL_Delay(850);                // BNO055の起動待ち（ここは経験則）
uint16_t DevAddress_BNO055 = 0b0101000 << 1;  // 7bit→8bitへ左シフト

// OPR_MODE(0x3D) を IMU モード(0x08) に設定
uint8_t opr_mode_IMU[2] = {0x3D, 0b00001000};
HAL_Delay(850);
HAL_I2C_Master_Transmit(&hi2c1, DevAddress_BNO055, opr_mode_IMU, 2, 100);
HAL_Delay(100);

printf("%10s,%10s,%10s\r\n", "theta", "theta_dot", "theta_s"); // CSVヘッダ

BNO055は**IMUモード（0x08）**にしました。加速度＋ジャイロのフュージョンで相対姿勢を出すモードで、地磁気を使わないのでキャリブレーションが楽。室内で机の上、というこの用途に向いています。

3. メインループ：角度とその微分を取る

ここが本題です。ピッチ角θと、その角速度θ̇を用意します。

float k_p = 1.0f;
float k_d = 0.2f;
float theta_s = 0.0f;
float euler_pitch, dot_euler_pitch;

while (1)
{
    HAL_Delay(20);

    // --- オイラー角ほか（レジスタ0x1Aから26バイト読む）---
    tx[0] = 0x1A;
    HAL_I2C_Master_Transmit(&hi2c1, DevAddress_BNO055, tx, 1, 100);
    HAL_I2C_Master_Receive(&hi2c1, DevAddress_BNO055, rx, 26, 100);

    binary_euler_roll  = ((int16_t)rx[3] << 8) | (int16_t)rx[2];
    binary_euler_pitch = ((int16_t)rx[5] << 8) | (int16_t)rx[4];

    real_euler_roll = (float)binary_euler_roll / 16.0f;   // [degree]
    euler_pitch     = (float)binary_euler_pitch / 900.0f; // [rad]  ← 900 LSB = 1 rad

    // --- ジャイロ（レジスタ0x14から6バイト読む）---
    tx_g[0] = 0x14;
    HAL_I2C_Master_Transmit(&hi2c1, DevAddress_BNO055, tx_g, 1, 100);
    HAL_I2C_Master_Receive(&hi2c1, DevAddress_BNO055, rx_g, 6, 100);

    binary_gyr_x = ((int16_t)rx_g[1] << 8) | (int16_t)rx_g[0];
    binary_gyr_y = ((int16_t)rx_g[3] << 8) | (int16_t)rx_g[2];
    binary_gyr_z = ((int16_t)rx_g[5] << 8) | (int16_t)rx_g[4];

    real_gyr_x = (float)binary_gyr_x / 900.0f;  // [rad/s] ← 900 LSB = 1 rad/s
    real_gyr_y = (float)binary_gyr_y / 900.0f;
    real_gyr_z = (float)binary_gyr_z / 900.0f;

    // 機体座標へ軸を組み替え
    float x_aircraft_gyr =  real_gyr_y;
    float y_aircraft_gyr =  real_gyr_x;
    float z_aircraft_gyr = -real_gyr_z;

    // ★ ピッチ角速度をジャイロから作る（数値微分ではなく）
    dot_euler_pitch = cos(real_euler_roll) * y_aircraft_gyr
                    - sin(real_euler_roll) * z_aircraft_gyr;

    // --- PD制御則 ---
    theta_s = k_p * euler_pitch + k_d * dot_euler_pitch;   // [rad]

    // --- rad → CCR[µs] へ変換 ---
    ccr = (1000.0f / 180.0f) * (theta_s * 180.0f / M_PI) + 1500.0f;

    // --- ログ（3回目の記事のprintf） ---
    printf("%10.4f,%10.4f,%10.4f\r\n", euler_pitch, dot_euler_pitch, theta_s);

    HAL_Delay(10);
}

BNO055のLSB換算はデータシートの値をそのまま使うのが確実です。ラジアン出力は900 LSB = 1 rad、度出力は16 LSB = 1°。想像で係数を決めると必ずズレます。

単位変換の裏取り：rad が µs になるまで

theta_s（ラジアン）をサーボのパルス幅（µs）に直す一行、ここは実機の値で検算しておきます。

ccr = (1000.0f / 180.0f) * (theta_s * 180.0f / M_PI) + 1500.0f;

theta_s * 180.0f / M_PI … rad → deg
× (1000/180) … 180°を1000µsに対応させる
+ 1500 … 中央（1.5ms）を基準にする

途中の180は約分できて、実質こうです。

ccr = 1500 + 1000 × theta_s[rad] / π

具体的な数字で確かめます。仮に基板が約11.5°（= 0.2 rad）傾いていて、ほぼ静止（θ̇≈0）なら、

theta_s = 1×0.2 + 0.2×0 = 0.2 [rad]
ccr = 1500 + 1000 × 0.2 / 3.1416 ≒ 1500 + 63.7 ≒ 1564 [µs]

→ 中央から約64µsだけサーボが動く。傾きに対して素直で、いきなり端まで振れたりしない、ちょうどいい効き方です。k_p=1・k_d=0.2はこの「効き過ぎず、でも戻る」感覚から実機で詰めた値です。

つまずきポイント

① 角度を時間で微分したら誤差が大きすぎた → キネマティクス方程式で解決（今回の本丸）

最初、D項のθ̇はこう作っていました。前回の角度との差を時間で割る、いわゆる数値微分です。

// これで痛い目を見た
dot_euler_pitch = (now_euler_pitch - previous_euler_pitch) / (20.0f / 1000.0f);

理屈は正しい。でも実機では誤差が大きく、サーボが細かくガタガタ震える。原因は主に2つ。

角度センサ値の小さなノイズや量子化が、差分（微分）で一気に増幅される。差分は「小さな揺れ」を「大きな速度」に化けさせる
割る分母Δtが、I2C通信の時間でループごとに揺れる。分母が一定でないぶん誤差が乗る

解決策は、差分をやめてキネマティクス方程式（運動学式）でオイラー角の角速度を算出することでした。ここが今回の一番の学びです。

ポイントは、「機体がジャイロで測る角速度（機体座標のp, q, r）」と「オイラー角の変化率 θ̇」は同じものではない、ということ。姿勢が傾いていると、両者はroll角φを介して次の関係で結ばれます（ピッチ角速度の運動学式）。

θ̇ = q・cos(φ) − r・sin(φ)
      q：機体のピッチ角速度   r：機体のヨー角速度   φ：roll角

これをそのままコードにするとこうなります。差分（Δθ/Δt）は一切使いません。

// キネマティクス方程式で θ̇ を算出（q=y_aircraft_gyr, r=z_aircraft_gyr）
dot_euler_pitch = cos(real_euler_roll) * y_aircraft_gyr
                - sin(real_euler_roll) * z_aircraft_gyr;

これに変えた瞬間、震えが止まりました。「D項＝角度を微分」と機械的に覚えていたのが敗因で、**"角速度は測れる量なのだから、微分せず運動学から組み立てればいい"**というのが腹落ちした瞬間でした。

副産物として、ループ周期のブレも問題にならなくなりました。この式はΔtを含まない（差分ではなく、その瞬間のジャイロ値から直接計算する）ので、while(1)がI2C通信で多少もたついても値が素直なまま。これが「計算ループとサーボ更新を分けても平気」な理由です。

② rollの単位——degのままcos()に入れていた

上の式、cos(real_euler_roll)のreal_euler_rollは/16で計算しているので度です。でもC言語のcos()はラジアンを期待します。厳密にはここで変換が必要。

今回は「机の上で水平付近を保つ」用途で、rollがほぼ0°。cos(0付近)≈1・sin(0付近)≈0なので実害が出ず動いてしまいましたが、大きく傾ける使い方をするならreal_euler_roll * M_PI / 180.0fにすべき箇所です。"たまたま動いている"は"正しい"とは違う、という自戒として残しておきます。

③ サーボの飽和を忘れない

PDの計算はいくらでも大きな値を吐きます。そのままCCRに入れるとサーボの可動範囲外を叩いて壊れかねない。TIM7割り込みの中で1000〜2000µsにクランプしておくのが安全です（コード1参照）。

④ BNO055の起動待ち

電源投入直後にI2Cを叩いても応答しません。HAL_Delay(850)で起動を待ってからモード設定・読み取りに入ります。ここを削ると「たまに立ち上がらない」不安定さの原因になります。

応用・実践：ログを取って挙動を見る

3回目の記事のprintfが、ここで効いてきます。毎ループtheta, theta_dot, theta_sをCSV形式で吐いているので、TeraTermのログをそのままExcelやPythonに貼れば応答が可視化できます。出力フォーマットはこう。

printf("%10s,%10s,%10s\r\n", "theta", "theta_dot", "theta_s");        // ヘッダ
printf("%10.4f,%10.4f,%10.4f\r\n", euler_pitch, dot_euler_pitch, theta_s); // 各行

     theta, theta_dot,   theta_s
     ...（TeraTermのログをここに貼る）...

TODO（自分用）：実際に傾けたときのログを取って、上に実データを貼る／時系列グラフ画像を追加する。
傾け始めにθが立ち上がり、θ̇がそれを先読みしてtheta_sにブレーキをかける様子が見えるはず。k_dを0にした場合と比べると、D項の仕事が数字で見えて面白い。

次の一歩

I項を足してPIDへ：定常偏差（微妙に傾いたまま止まる）が気になったら積分項を追加
制御演算もTIM7割り込みの中へ：今はループで計算しているが、センサ読みも含めて割り込みに寄せると周期がさらに安定する
ゲインの詰め方：k_pを上げて発振する直前を探り、k_dで抑える、という順で追い込むと感覚が掴みやすい

まとめ

PDは「今の傾き（P）」＋「傾きの変化の速さ（D）」で先読みブレーキをかける制御
D項は角度を時間で微分せず、キネマティクス方程式（θ̇ = q·cosφ − r·sinφ）で角速度を算出するとノイズ・誤差に強い（今回の一番の学び）
計算ループとサーボ更新（TIM7の50Hz割り込み）を分けると、周期のブレに強くなる。運動学式のθ̇はΔtを含まないので相性が良い
単位換算（LSB、rad↔deg、rad↔µs）は必ず実機の値・データシートで裏取りする
飽和・起動待ちのような「地味な保険」が安定動作を支える

サーボ・センサ・printfと積み上げてきたものが、初めて一つの「動く制御」になりました。手で傾けて、ちゃんと戻ろうとするのを見た瞬間はやっぱり嬉しいです。

この記事のシリーズ

STM32のPWMでサーボを動かす
STM32でBNO055をI2C接続
STM32のprintfでマイコンの値をPCに出す
STM32でPD制御（本記事）