車輪速度制御モデル (wheel.m) の全体アーキテクチャ Slide 1 / 6

RDS 車輪速度制御モデル (wheel.m) 構造解説

実機同定伝達関数・状態空間表現・PID/FF制御器・エンコーダ量子化・2次LPF速度推定の全容

目標速度 v_ref [rad/s] + - 偏差 e 【Slide 4】 コントローラ PID + FF制御 飽和制限 ±3198 PWM a_pwm 【Slide 2】 モータプラント x' = Ax + Bu x=[x_enc; v_mot] 【Slide 3】 出力 & 量子化 round(C x) x_enc [pulse] 【Slide 5】速度推定処理 後退差分 (Δ/Ts) ＋ 60Hz 2次LPF v_fb [rad/s] (フィードバック速度)
モータ同定モデルと状態空間シグナルフロー ($\dot{x} = Ax + Bu$) Slide 2 / 6

実機同定モデル (`ff_coeff`) と連続系変換

離散時間 1次遅れ伝達関数（サンプリング時間 $T_s = 0.001$ s）：

$$v_{\mathrm{mot}}[k] = 0.9504 v_{\mathrm{mot}}[k-1] + 0.01039 a_{\mathrm{pwm}}[k]$$

極 $a_m$ 及びゲイン $b_m$ の導出：

$$e^{a_m T_s} = 0.9504 \implies a_m = \frac{\ln(0.9504)}{0.001} \approx -50.8924 \text{ rad/s}$$ $$b_m = \frac{0.01039}{1 - 0.9504} \cdot (-a_m) \approx 10.6605 \text{ rad/(s}^2 \cdot \text{count)}$$

状態変数 $x(t) = \begin{bmatrix} x_{\mathrm{enc}}(t) \\ v_{\mathrm{mot}}(t) \end{bmatrix}$ の連続時間状態方程式：

$$\dot{x}(t) = \begin{bmatrix} 0 & K_{\mathrm{enc}} \\ 0 & a_m \end{bmatrix} x(t) + \begin{bmatrix} 0 \\ b_m \end{bmatrix} a_{\mathrm{pwm}}(t)$$

※ $K_{\mathrm{enc}} = \frac{8192}{2\pi} \approx 1303.797 \text{ pulse/rad}$

モータ＆エンコーダ 内部シグナルフロー図

入力 a_pwm [count] b_m + v'mot ∫ dt v_mot [rad/s] a_m + K_enc ∫ dt x_enc [pulse] 💡 1次遅れ系モータ速度 $v_{\mathrm{mot}}$ に積分器を直列接続した2次状態空間系
出力方程式 ($y = Cx + d$) とエンコーダパルス量子化ノイズ Slide 3 / 6

出力方程式 $y = Cx + d$ と厳密離散化

観測量 $y$ はエンコーダ累積パルス $x_{\mathrm{enc}}$ のみを取り出します：

$$C = \begin{bmatrix} 1 & 0 \end{bmatrix}, \quad d = 0$$ $$y[k] = C x[k] + d = x_{\mathrm{enc}}[k]$$

ZOH（0次ホールド）厳密離散化行列：

$$A_d = e^{A T_s} = \begin{bmatrix} 1 & K_{\mathrm{enc}} \frac{1 - e^{a_m T_s}}{-a_m} \\ 0 & e^{a_m T_s} \end{bmatrix}$$ $$B_d = \int_0^{T_s} e^{A \tau} B d\tau = \begin{bmatrix} K_{\mathrm{enc}} \frac{b_m}{-a_m} (T_s - \frac{1 - e^{a_m T_s}}{-a_m}) \\ \frac{b_m}{-a_m} (1 - e^{a_m T_s}) \end{bmatrix}$$

💡 パルス離散化ノイズのメカニズム

実機エンコーダは整数ステップでしか位置を測れません．微分（後退差分）時にこれが激しい離散化ノイズ（ステップ状スパイク）を生みます．

エンコーダの構造とパルス量子化 (丸め) 概念図

センサ 光学式エンコーダ 8192 pulse/rev 位置パルスの量子化波形 時間 t パルス 1 2 3 真値 x(t) round(x) ⚠️ 整数化 `round()` が生む微分ノイズ： サンプリング $T_s=1\text{ms}$ で階段状にパルスが 1 変化するだけで， 速度換算では $\Delta x / T_s = 1 / 0.001 = 1000 \text{ pulse/s}$ の巨大なスパイクになります．
コントローラ設計（PID 制御 ＋ モデル逆フィードフォワード制御） Slide 4 / 6

PID フィードバック制御部 (`pid.hpp`)

ゲイン定数：$K_p = 2.0, \quad K_i = 200.0, \quad K_d = 0.0$

$$e[k] = v_{\mathrm{ref}}[k] - v_{\mathrm{fb}}[k]$$ $$u_{\mathrm{pid}}[k] = K_p e[k] + K_i \sum T_s e[k]$$

🔄 アンチワインドアップ (Anti-windup)

目標速度の符号反転時（$v_{\mathrm{ref}}[k-1] \cdot v_{\mathrm{ref}}[k] < 0$）に積分値を 0 リセットし、逆転時の急激なオーバーシュートを抑制．

FF (Feed-Forward) 制御部 ＆ 飽和処理

同定モデル逆伝達関数（$1/G_m$）＋ 120Hz 1次LPF：

$$u_{\mathrm{ff}}[k] = \text{LPF}_{120\mathrm{Hz}} \left( \frac{v_{\mathrm{ref}}[k] - 0.9504 v_{\mathrm{ref}}[k-1]}{0.01039} \right)$$

全制御入力の飽和リミッタ処理:

$$a_{\mathrm{pwm}}[k] = \text{clamp}(u_{\mathrm{pid}}[k] + u_{\mathrm{ff}}[k], -3198, 3198)$$

コントローラ内部回路・詳細ブロックダイアグラム

目標速度 v_ref [rad/s] FF 制御器（同定モデル逆伝達関数） G_m^{-1}(z) ＋ 120Hz 1次LPF u_ff [count] + - v_fb (推定速度) 偏差 e P制御 : × K_p I制御 : K_i ∫ dt 【符号反転時 積分Reset】 + u_pid [count] Σ 飽和リミッタ ±3198 count a_pwm [PWM入力]
エンコーダ速度推定処理（後退差分 ＋ 60Hz 2次バターワース LPF） Slide 5 / 6

速度推定アルゴリズムと 60Hz LPF 係数

Step 1: 後退差分によるパルスレート計算

$$u_{\mathrm{lpf}}[k] = \frac{x_{\mathrm{enc}}[k] - x_{\mathrm{enc}}[k-1]}{T_s} \quad [\text{pulse/s}]$$

Step 2: 2次 IIR フィルタ (`lpf_2nd_coeff_60hz`)

$$\begin{aligned} y_{\mathrm{lpf}}[k] &= b_0 u_{\mathrm{lpf}}[k] + b_1 u_{\mathrm{lpf}}[k-1] + b_2 u_{\mathrm{lpf}}[k-2] \\ &\quad - a_1 y_{\mathrm{lpf}}[k-1] - a_2 y_{\mathrm{lpf}}[k-2] \end{aligned}$$

係数：$a_1 = -1.475480, a_2 = 0.586920, b_0 = b_2 = 0.027860, b_1 = 0.055720$

Step 3: 単位換算（パルス速度 $\to$ 角速度）

$$v_{\mathrm{fb}}[k] = K_{\mathrm{p2r}} \cdot y_{\mathrm{lpf}}[k] = \frac{2\pi}{8192} \cdot y_{\mathrm{lpf}}[k] \quad [\text{rad/s}]$$

速度推定パイプラインとフィルタ波形比較

① 後退差分 Δx_enc / Ts ② 60Hz 2次LPF 高周波ノイズ除去 ③ 単位変換 × 2π/8192 [rad/s] 差分スパイクノイズ vs LPF 平滑化波形 時間 t 速度 差分値 (ノイズ大) LPF推定値 v_fb 🎯 カットオフ 60Hz 2次バターワースの絶妙な効果： パルス離散化の超高周波スパイクをカットしつつ，制御遅延（位相遅れ）を最小限に抑えます．
【車輪回転アニメーション機能】`wheel.m` シミュレータ Slide 6 / 6

シミュレータ設定

⚙️ リアルタイム車輪モニタ

推定速度 $v_{fb}$:
0.0 rad/s

モータ回転数:
0.0 rpm

制御入力 $a_{pwm}$:
0 count

エンコーダ位置:
0 pulse