倒立振子

t θ x レールの端

振り上げ

qx 位置の重み

qθ 角度の重み

R 入力の重み

教材

m 振子の質量

l 重心までの長さ

M 台車の質量

L レールの長さ

Vmax 電圧の上限

Fmax 力の上限

制御周期

fv 速度のフィルタ

直近 10 s

CH1 θ /divH /divTRIG AUTO

CH1 F /divH /divTRIG AUTO

整定時間 t_s
振り上げの時間 t_up
最大の力 |F|_max
台車の最大の変位
開ループの不安定な極 p₊
閉ループの極
状態フィードバックのゲイン K

s 平面

開ループ閉ループ/div

動作原理と式

台車と振子

レールの上を動く台車に軸を付け、振子を下げます。ラグランジュの方程式から、次の運動方程式が出ます[1]。

$M$: 台車の質量
$m$: 振子の質量
$l$: 軸から振子の重心まで
$J$: 振子の重心まわりの慣性モーメント
$x$: 台車の位置。右が正
$θ$: 振子の角度。真上から測り、先端が右へ傾くと正
$F$: 台車を押す力
$b_{c}, F_{c}$: 台車の粘性摩擦とクーロン摩擦
$b_{p}$: 軸の粘性摩擦

(M_{e} + m) \ddot{x} + m l \cos θ \ddot{θ} - m l {\dot{θ}}^{2} \sin θ = F - b_{c} \dot{x} - F_{c} sgn \dot{x}

m l \cos θ \ddot{x} + (J + m l^{2}) \ddot{θ} - m g l \sin θ = - b_{p} \dot{θ}

駆動

台車は DC モータが減速の歯車とピニオンで動かします。巻線のインダクタンスを無視すると、電圧 $V$ から力 $F$ は下の式になります。

逆起電力は、速さに比例する制動として働く
回転子の慣性は、台車の質量に足した等価質量 $M_{e}$ として入る

F = α V - β \dot{x}

α = \frac{K_{g} k_{t}}{R_{m} r}

β = \frac{K_{g}^{2} k_{t}^{2}}{R_{m} r^{2}}

M_{e} = M + \frac{K_{g}^{2} J_{m}}{r^{2}}

入力 $u$ は電圧か力です。力で駆動するときは $F = u$ です。どちらも、入力は上限で切ります。

線形の模型

真上（ $θ = 0$ ）の近くで $\sin θ \approx θ$ 、 $\cos θ \approx 1$ とし、 ${\dot{θ}}^{2}$ とクーロン摩擦を落とします。すると、状態 $x = {[x θ \dot{x} \dot{θ}]}^{T}$ の線形の模型になります[1]。

\dot{x} = A x + B u

A = [\begin{matrix} 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \\ 0 & - \frac{m^{2} l^{2} g}{D} & - \frac{(J + m l^{2}) b}{D} & \frac{m l b_{p}}{D} \\ 0 & \frac{(M_{e} + m) m g l}{D} & \frac{m l b}{D} & - \frac{(M_{e} + m) b_{p}}{D} \end{matrix}]

B = [\begin{matrix} 0 \\ 0 \\ \frac{(J + m l^{2}) α}{D} \\ - \frac{m l α}{D} \end{matrix}]

D = (M_{e} + m) (J + m l^{2}) - m^{2} l^{2}

b = β + b_{c}

p_{+} \approx \sqrt{\frac{(M_{e} + m) m g l}{D}}

$A$ の固有値のうち正の 1 つ $p_{+}$ が、振子が倒れていく速さです。これが開ループの不安定な極です。台車が振子よりずっと重く $J = 0$ なら、 $\sqrt{g / l}$ に近づきます。固有値は、ヘッセンベルク化と QR 法で求めます[2]。

状態フィードバック

入力 1 つで 4 つの状態をすべて動かせる（可制御）のは、可制御性行列 $C$ の階数が 4 のときです[1]。振子と台車ではいつも可制御です。そのため、状態フィードバック $u = - K x$ によって閉ループの極、つまり $A - B K$ の固有値をどこへでも置けます。

状態フィードバック

u = - K x

C = [B A B A^{2} B A^{3} B]

極配置

固有角周波数 $ω_{i}$ と減衰比 $ζ_{i}$ の 2 組で、4 つの極を決めます。その特性多項式 $φ (s)$ から、アッカーマンの式でゲインを求めます[3]。

φ (s) = \prod_{i = 1}^{2} (s^{2} + 2 ζ_{i} ω_{i} s + ω_{i}^{2})

K = [0 0 0 1] C^{- 1} φ (A)

LQR

LQR は、状態のずれと入力の大きさの重み付きの和 $𝒥$ を最小にするゲインです[1][4]。重み $Q = diag (q_{x}, q_{θ}, q_{\dot{x}}, q_{\dot{θ}})$ と $R$ から、代数リカッチ方程式の解 $P$ を求めます。

𝒥 = \int_{0}^{\infty} (x^{T} Q x + R u^{2}) d t

A^{T} P + P A - P B R^{- 1} B^{T} P + Q = 0

K = R^{- 1} B^{T} P

リカッチ方程式の解き方

ハミルトン行列 $H$ の符号関数 $W$ を使って解きます[5][6]。

ハミルトン行列 $H$
符号関数 $W$ 行列式で尺度を合わせたニュートン法
解 $P$ $W$ を 4 行ずつに分けた区画 $W_{ij}$ の連立方程式を最小二乗で解く

H = [\begin{matrix} A & - B R^{- 1} B^{T} \\ - Q & - A^{T} \end{matrix}]

Z_{k + 1} = \frac{1}{2} (\frac{Z_{k}}{c_{k}} + c_{k} Z_{k}^{- 1})

c_{k} = {| \det Z_{k} |}^{1 / 8}

[\begin{matrix} W_{12} \\ W_{22} + I \end{matrix}] P = - [\begin{matrix} W_{11} + I \\ W_{21} \end{matrix}]

PID

PID は 2 つのループを重ねます。

外の位置のループ: 台車の位置のずれから、振子の角度の目標 $θ_{r}$ を作る。 $θ_{r}$ は ±10° で切る
内の角度のループ: 振子をその角度に保つ入力を出す

台車を右へ運ぶには、まず振子を右へ傾ける必要があるからです。初めに台車が逆へ動く、不安定な零点の性質です。積分がなければ、これは状態フィードバックと同じ形です。

PID の 2 つのループ。外の位置のループが角度の目標

θ_{r}

を作り、内の角度のループが入力

u

を出す

θ_{r} = K_{Px} e_{x} + K_{Ix} \int e_{x} d t - K_{Dx} \hat{\dot{x}}

e_{x} = - \hat{x}

u = K_{P θ} (\hat{θ} - θ_{r}) + K_{I θ} \int (\hat{θ} - θ_{r}) d t + K_{D θ} \hat{\dot{θ}}

K = - [K_{P θ} K_{Px} K_{P θ} K_{P θ} K_{Dx} K_{D θ}] （積分なし）

今の値を代入した式

振り上げ

振り上げは Åström・Furuta のエネルギー法です[7]。振子のエネルギー $E$ は、台車の加速度 $\ddot{x}$ を通してだけ変わります。 $\dot{θ} \cos θ$ と逆の向きに加速すればエネルギーが増え、目標に近づくと加速をやめます。

$E$ は、直立して止まっていると 0、真下で止まっていると $- 2 m g l$ です。

E = \frac{1}{2} (J + m l^{2}) {\dot{θ}}^{2} + m g l (\cos θ - 1)

\frac{d E}{d t} = - m l \dot{θ} \cos θ \ddot{x} - b_{p} {\dot{θ}}^{2}

このツールで足したこと

この式に、このツールで次の 4 つを足しています。

目標を直立より少し上の $E_{0} = 0.05 m g l$ にする
$s = (\dot{θ} / ω_{0}) \cos θ$ が小さい間、つまり直立の近くでは、加速を $| s |$ に比例して弱める。下限は 0.2
台車をレールの中央へ寄せる項を足す。固有角周波数は 1.5 rad/s
加速度から入力へ直すときに、逆起電力と台車の摩擦を補う

\ddot{x} = {sat}_{a_{\max}} (k g (\frac{E}{m g l} - 0.05) sgn s \min (\max (| s |, 0.2), 1) - ω_{c}^{2} \hat{x} - 2 ω_{c} \hat{\dot{x}})

安定化との切り替え

振り上げ
真上から $θ_{sw}$ の内側に入る安定化の制御へ切り替える
安定化
$\max (θ_{sw} + 10 °, 30 °)$ の外へ出る倒れたとみなし、振り上げに戻る

振り上げないときは、入力を 0 にします。

計測と制御周期

制御は実機と同じく離散時間で動きます。制御周期 $T_{s}$ ごとに、次の順に処理します。

角度と位置を読むエンコーダの 1 カウントの刻みに丸め、雑音を足す
速度を推定する差分から
入力を決める
入力を保つ次の周期まで

速度の推定は、微分に 2 次の低域通過フィルタを掛けたもの $H (s)$ を、双一次変換で離散化しています。減衰比 $ζ$ は 0.9 です。一定の速さを正しく読むよう、周波数を合わせる補正はしません。

\hat{θ} = Δ θ round \frac{θ + π + n_{θ}}{Δ θ} - π

H (s) = \frac{ω_{f}^{2} s}{s^{2} + 2 ζ ω_{f} s + ω_{f}^{2}}

s \leftarrow \frac{2}{T_{s}} \frac{1 - z^{- 1}}{1 + z^{- 1}}

制御器は連続時間の線形の模型で設計します。そのため、制御周期が長いほど、またフィルタの遅れが大きいほど、設計した閉ループの極からずれて、ついには倒れます。

運動の計算

解き方: 0.25 ms 刻みの 4 次のルンゲ・クッタ法
クーロン摩擦: $\tanh (\dot{x} / 0.2 mm/s)$ で滑らかにする
レールの端: 台車が当たると、反発係数 0.2 で跳ね返る。そのとき台車が受ける撃力で、振子の角速度も変わる

押したとき

図やボタンで押したときは、押した点にかかる水平の力積 $P$ から、質量行列 $M (θ)$ を通して速度の変化を求めます。 $r$ は軸から押した点までです。台車を押したときは、右辺の第 2 成分が 0 です。

M (θ) Δ [\begin{matrix} \dot{x} \\ \dot{θ} \end{matrix}] = [\begin{matrix} P \\ P r \cos θ \end{matrix}]

M (θ) = [\begin{matrix} M_{e} + m & m l \cos θ \\ m l \cos θ & J + m l^{2} \end{matrix}]

整定時間

出来事から、角度 ±1° と位置 ±10 mm の範囲に入って 1 s 留まるまでの時間です。出来事は、振り上げて切り替えた、押した、初めの状態から離した、のどれかです。

教材の値

教材の値は、次の 2 つから取りました。

Quanser の IP02 と SIP の設定ファイル[8]: 台車・おもり・粘性摩擦・モータ・歯車・ピニオン・台車が動ける範囲 814 mm・エンコーダの分解能・D/A の上限 10 V・振子・LQR の重みの初期値
Control Tutorials for MATLAB and Simulink の Inverted Pendulum: System Modeling[9]

おもりを載せた台車・粘性摩擦・モータ・歯車・ピニオン・長い振子の値は、Bates の論文の付録の表 1 にもあります[10]。ただし、振子の慣性モーメントだけは表 1 と異なります。

このツールで決めたこと

振り上げに足した 4 つの工夫とその値
クーロン摩擦の初期値
速度のフィルタの遮断周波数の初期値。Quanser の SIP の設定は 10 Hz
PID と極配置の初期値
レールの端の反発係数
押す強さ

出典

台車と振子の模型、可制御性、状態フィードバック、LQRK. J. Åström and R. M. Murray, Feedback Systems: An Introduction for Scientists and Engineers, 2nd ed., Princeton University Press, 2021
固有値を求めるヘッセンベルク化と QR 法W. H. Press et al., Numerical Recipes in C, 2nd ed., Cambridge University Press, 1992, 11.5・11.6 節
アッカーマンの式、極配置K. Ogata, Modern Control Engineering, 5th ed., Prentice Hall, 2010
LQRR. E. Kalman, “Contributions to the theory of optimal control,” Boletín de la Sociedad Matemática Mexicana, vol. 5, pp. 102–119, 1960
符号関数によるリカッチ方程式の解法J. D. Roberts, “Linear model reduction and solution of the algebraic Riccati equation by use of the sign function,” Int. J. Control, vol. 32, no. 4, pp. 677–687, 1980
符号関数によるリカッチ方程式の解法R. Byers, “Solving the algebraic Riccati equation with the matrix sign function,” Linear Algebra Appl., vol. 85, pp. 267–279, 1987
振り上げのエネルギー法K. J. Åström and K. Furuta, “Swinging up a pendulum by energy control,” Automatica, vol. 36, no. 2, pp. 287–295, 2000
教材の値Quanser Consulting, IP02 と SIP の設定ファイル（config_ip02.m・config_sp.m・setup_ip02_sip.m）
教材の値Control Tutorials for MATLAB and Simulink, Inverted Pendulum: System Modeling, University of Michigan ほか
教材の値と同じ値（付録の表 1）D. Bates, “A Hybrid Approach for Reinforcement Learning Using Virtual Policy Gradient for Balancing an Inverted Pendulum,” arXiv:2102.08362, 2021

電子回路

制御

デジタルフィルタ

信号・通信

音響

画像・光学

SVG

倒立振子

動作原理と式

台車と振子

駆動

線形の模型

状態フィードバック

極配置

LQR

リカッチ方程式の解き方

PID

今の値を代入した式

振り上げ

このツールで足したこと

安定化との切り替え

計測と制御周期

運動の計算

押したとき

整定時間

教材の値

このツールで決めたこと

出典

関連ツール

倒立振子

台車と振子

制御

振子と台車

駆動

計測と制御周期

応答

計算結果

極

動作原理と式

台車と振子

駆動

線形の模型

状態フィードバック

極配置

LQR

リカッチ方程式の解き方

PID

今の値を代入した式

振り上げ

このツールで足したこと

安定化との切り替え

計測と制御周期

運動の計算

押したとき

整定時間

教材の値

このツールで決めたこと

出典

関連ツール

PID制御

カルマンフィルタ

タイマIC 555

受動素子組み合わせ計算機

双2次フィルタ

IIR フィルタ

JJY シミュレータ

JJY デコーダ

FIR フィルタ

オシロスコープ

スペクトラムアナライザ

FM音源

ノイズジェネレータ

ギター音響モデル

ピアノ音響モデル

ヴァイオリン音響モデル

トランペット音響モデル

SVG テーマスキーマチェッカー

極と零点

抵抗カラーコード

PCBコイル

JPEG

オルガン音響モデル

エンジン音響モデル

カメラレンズ

ハロ（暈）

フォーカシングマスク

日食

チューナー