RDS 車輪速度制御モデル (wheel.m) の理論と演習 Slide 1 / 6

車輪速度制御モデル (wheel.m) 解説＆演習ガイド

InternalSoftwareE2023 の同定モデル・状態空間表現・PID/FF制御・エンコーダ速度推定・車輪回転視覚化

目標速度 v_ref [rad/s] + - 偏差 e コントローラ PID + FF制御 飽和制限 ±3198 a_pwm モータプラント x' = Ax + Bu x = [x_enc; v_mot] y = Cx + d C=[1 0], d=0 x_enc [pulse] 速度推定処理 後退差分 + 60Hz LPF v_fb [rad/s] (FB信号)
モータ同定モデルと状態空間表現 ($\dot{x} = Ax + Bu$) Slide 2 / 6

RDS 実機同定モデル (`ff_coeff`)

RDS コード内の同定伝達関数（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}$（位置 [pulse]，モータ角速度 [rad/s]）：

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

ここで $K_{\mathrm{enc}} = \dfrac{\text{MOT\_PLS\_PER\_1REV}}{2\pi} \approx 1303.797$ [pulse/rad]．

出力方程式 ($y = Cx + d$) とエンコーダパルス処理 Slide 3 / 6

出力方程式 $y = Cx + d$

観測量 $y = x_{\mathrm{enc}}$ [pulse] を直接取り出す出力行列：

$$C = \begin{bmatrix} 1 & 0 \end{bmatrix}, \quad d = 0$$ $$y(t) = C x(t) + d = x_{\mathrm{enc}}(t) \quad [\text{pulse}]$$

離散化（ZOH厳密離散化）による更新式：

$$x[k+1] = A_d x[k] + B_d a_{\mathrm{pwm}}[k]$$

エンコーダのパルス量子化 (離散化ノイズ)

実機エンコーダは 32bit 整数パルスを離散的にカウントするため：

$$x_{\mathrm{enc, meas}}[k] = \text{round}(x_{\mathrm{enc}}[k])$$

💡 実機再現の重要性

パルスを整数化することで，低速時に後退差分を取った際の段階的なステップノイズ（離散化ノイズ）が再現され，フィルタ処理の重要性が視覚化されます．

コントローラ設計（PID 制御 + FF 制御項） Slide 4 / 6

PID コントローラ（`pid.hpp` 準拠）

ゲイン定数：$K_p = 2.0, K_i = 200.0, 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] + K_d \frac{e[k] - e[k-1]}{T_s}$$

Anti-windup: 目標速度の符号反転（$v_{\mathrm{ref}}[k-1] \cdot v_{\mathrm{ref}}[k] < 0$）時に積分値を 0 リセット．

FF (Feed-Forward) 制御項と飽和処理

RDS 実機の FF 制御（同定モデル逆フィルタ + 120Hz 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)$$
微分器 + 60Hz 2次 LPF による速度推定 Slide 5 / 6

後退差分によるパルスレート換算

エンコーダ位置パルス差分からパルス速度 [pulse/s] へ変換：

$$\Delta x_{\mathrm{enc}}[k] = x_{\mathrm{enc, meas}}[k] - x_{\mathrm{enc, meas}}[k-1]$$ $$u_{\mathrm{lpf}}[k] = \Delta x_{\mathrm{enc}}[k] \times 1000.0 \quad [\text{pulse/s}]$$

60Hz 2次バターワース LPF 差分方程式

`z_transform.hpp` 内の `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$

単位変換:

$$v_{\mathrm{fb}}[k] = K_{\mathrm{p2r}} \times y_{\mathrm{lpf}}[k] \quad [\text{rad/s}]$$

※ $K_{\mathrm{p2r}} = \text{WHL\_PULSESEC\_TO\_MOT\_ANGVEL} = \frac{2\pi}{\text{MOT\_PLS\_PER\_1REV}}$ [rad/pulse]

【車輪回転アニメーション機能】`wheel.m` シミュレータ Slide 6 / 6

シミュレータ設定

リアルタイム車輪モニタ

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

モータ回転数: 0.0 rpm

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

エンコーダ位置: 0 pulse