# Rotary Inverted Pendulum (Furuta Pendulum) — Interactive Simulation 回転型倒立振子 (Furuta pendulum) をブラウザ上で動かせるインタラクティブなシミュレーションです。 **振り上げ (swing-up) → 倒立維持 → 外乱からの復帰** を、制御則を切り替えながら観察できます。ビルド不要の静的サイト (ES Modules + Canvas 2D)。依存パッケージも CDN もありません。 ![デモ: 吊り下がり状態から振り上げ、LQR で倒立維持、外乱を受けて再度振り上げて復帰する様子](docs/demo.gif) ![全体のスクリーンショット](docs/screenshot.png)

モバイル表示

縦 1 カラムに積み替わり、横スクロールは出ません (390 / 360 / 768 / 1180 / 1920 px で確認)。モバイル表示

## 動かす ```sh python3 -m http.server 8000 # → http://localhost:8000/ ``` ES Modules を使っているので `file://` では動きません。上記のように HTTP で配信してください。テスト (ブラウザ不要): ```sh node --test "test/*.test.mjs" ``` ## 何ができるか | 操作 | 内容 | | --- | --- | | 制御則の切替 | 制御なし / PID / LQR / Swing-up → LQR | | ドラッグ | 振子を横から突く (外乱トルク)。ロバスト性の確認用 | | ⚡ 外乱を与える | 振り上げエネルギーの 25% 相当のインパルスを振子に加える | | ← → キー | アームに手動トルク | | Space / R | 一時停止 / リセット | | スライダ | アーム長・振子長・質量・粘性摩擦・トルク上限、LQR の Q/R、PID ゲイン、再生速度、物理ステップ幅 | 状態量 (θ, α, θ̇, α̇) と制御入力 u は画面下部にリアルタイムでプロットされます。 ## 運動方程式 ### 座標系 - **θ**: 回転アームの角度 (鉛直な z 軸まわり) - **α**: 振子の角度。**α = 0 で倒立**、α = π で吊り下がり。回転軸はアーム方向 ê_r アーム方向を ê_r = (cosθ, sinθ, 0)、それに垂直な水平方向を ê_θ = (−sinθ, cosθ, 0) とすると、振子重心 (支点から l = L_p/2) の位置は ``` p = L_r ê_r + l ( sinα ê_θ + cosα ẑ ) ``` ### ラグランジアン速度を計算すると ``` |ṗ|² = (L_r² + l² sin²α) θ̇² + 2 L_r l cosα · θ̇ α̇ + l² α̇² ``` となり、振子の角速度 ω = θ̇ ẑ + α̇ ê_r を主軸に分解して細長い棒の近似 (軸まわり慣性 ≒ 0) を使うと回転運動エネルギーは ½ J_p (θ̇² sin²α + α̇²)。まとめて ``` J₀ = J_r + m_p L_r² … アーム側の実効慣性 Ĵ = J_p + m_p l² = m_p L_p²/3 … 振子の支点まわり慣性 T = ½ (J₀ + Ĵ sin²α) θ̇² + m_p L_r l cosα · θ̇ α̇ + ½ Ĵ α̇² V = m_p g l cosα (α = 0 で最大) ``` ### Euler–Lagrange ``` (1) (J₀ + Ĵ sin²α) θ̈ + m_p L_r l cosα · α̈ + 2 Ĵ sinα cosα · θ̇ α̇ − m_p L_r l sinα · α̇² = τ − b_r θ̇ (2) m_p L_r l cosα · θ̈ + Ĵ α̈ − Ĵ sinα cosα · θ̇² − m_p g l sinα = − b_p α̇ ``` 慣性行列は `M₁₁ = J₀ + Ĵ sin²α`, `M₁₂ = M₂₁ = m_p L_r l cosα`, `M₂₂ = Ĵ`。実装は [`src/furuta.js`](src/furuta.js) にあり、式番号とコードが対応するようコメントを付けています。数値積分は固定ステップの古典的 **4 次 Runge–Kutta**。制御入力はステップ内で一定 (zero-order hold)。既定の物理ステップは 1 ms で、描画 1 フレーム (60fps) あたり複数回サブステップを回します。摩擦と入力がゼロなら全エネルギーが相対誤差 1e-9 未満で保存することをテストで確認しています。参考文献: B. Cazzolato & Z. Prime, *On the Dynamics of the Furuta Pendulum*, Journal of Control Science and Engineering, 2011. [doi:10.1155/2011/528341](https://doi.org/10.1155/2011/528341) ### 倒立点まわりの線形化 sinα ≈ α, cosα ≈ 1, 速度の 2 次項を無視すると `ẋ = A x + B u` (x = [θ, α, θ̇, α̇]) が得られます。 `det = J₀Ĵ − (m_p L_r l)²` として ``` A = [ 0 0 1 0 ] [ 0 0 0 1 ] [ 0 −m_p L_r l·m_p g l/det −Ĵ b_r/det m_p L_r l·b_p/det ] [ 0 J₀·m_p g l/det m_p L_r l·b_r/det −J₀ b_p/det ] B = [ 0, 0, Ĵ/det, −m_p L_r l/det ]ᵀ ``` ## 制御則 ### 1. 制御なし入力ゼロ。振子は摩擦で減衰しながら吊り下がり位置へ収束します (モデルの妥当性確認用)。 ### 2. PID 倒立点近傍で α を 0 に戻す PID に、アームが流れていかないよう弱い外側ループを足したもの: ``` u = Kp·α + Ki·∫α dt + Kd·α̇ + Karm·(θ + θ̇) ``` 符号に注意が必要です。線形化モデルでは **∂α̈/∂u < 0** (正のトルクでアームが正方向に動くと、振子は相対的に負方向へ倒れる) なので、α を 0 に戻すゲインは「正」になります。積分項はアンチワインドアップのためクランプしています。 ### 3. LQR 倒立点まわりの線形化モデルに対して ``` minimize ∫ (xᵀQx + uᵀRu) dt → u = −K x, K = R⁻¹BᵀP ``` の連続時間代数リカッチ方程式 (CARE) ``` AᵀP + PA − PBR⁻¹BᵀP + Q = 0 ``` を**ブラウザ内で数値的に解いています** ([`src/lqr.js`](src/lqr.js))。事前計算した定数ではないので、 Q/R スライダを動かすとゲインが即座に再計算されます。手順は 2 段階: 1. **微分リカッチ方程式の積分** — `dP/dτ = AᵀP + PA − PBR⁻¹BᵀP + Q` を `P(0) = 0` から RK4 で定常解まで積分する。(A,B) 可安定・(A,Q) 可検出なら単調に P∞ へ収束するので、安定化ゲインの初期値が手に入る。 2. **Newton–Kleinman 反復** — そこから `(A − BK_i)ᵀP_{i+1} + P_{i+1}(A − BK_i) + Q + K_iᵀRK_i = 0` を解いて 2 次収束させる。各反復は Lyapunov 方程式 1 本で、Kronecker 展開して 16×16 の線形方程式を部分ピボット付きガウス消去で直接解く ([`src/linalg.js`](src/linalg.js))。既定パラメータでの解は残差 ‖AᵀP + PA − PBR⁻¹BᵀP + Q‖∞ ≈ 3e-11、K ≈ [−1.00, −14.52, −1.04, −2.10]。残差は画面右のパネルに常時表示しています。 2 回目以降は前回のゲインをウォームスタートに使って手順 1 を丸ごと省略するので、スライダをドラッグしても再計算は 10 ms 程度です。 **θ は畳んでフィードバックします。** θ はラグランジアンに現れない循環座標 (M も V も θ に依存しない) なので θ と θ+2π は同じ物理状態です。畳まないと、振り上げでアームが何回転かしたあと LQR に切り替わった瞬間、「θ を 0 に戻す」ために全力トルクが出て振子を倒してしまいます。 ### 4. エナジーベーススイングアップ → LQR 振子のエネルギーを、倒立静止を 0 として ``` E = ½ Ĵ α̇² + m_p g l (cosα − 1) (吊り下がり静止で E = −2 m_p g l) ``` と定義します。式 (2) から θ̇² 項と摩擦を無視すると ``` dE/dt ≈ − m_p L_r l · α̇ cosα · θ̈ ``` なので、アームの角加速度指令を **θ̈_d = k_E (E − E_ref) · sign(α̇ cosα)** と選べば `dE/dt = −m_p L_r l k_E |E − E_ref| |α̇ cosα| ≥ 0` となり、E は単調に E_ref へ近づきます (Åström–Furuta 型)。エネルギーが余っているときは符号が自然に反転して、今度は抜き取ります。実装上のポイント ([`src/controllers.js`](src/controllers.js)): - **角加速度指令はトルクへ厳密に逆算する**。`τ ≈ J₀ θ̈_d` という近似では、振子の反作用項 M₁₂ α̈ が指令トルクより大きくなり (真下付近で |M₁₂ α̈| ≫ |J₀ θ̈_d|)、実際のアーム加速度が指令と別物になって振り上がりません。式 (1)(2) を θ̈ = θ̈_d について解いた計算トルク法を使っています。 - **E_ref は 0 ではなく E_swing の 7% 上**に置きます。ちょうど 0 を狙うと摩擦による定常誤差の分だけエネルギーが不足し、折り返し点が倒立点の手前 (|α| ≒ 0.45 rad) で止まってハンドオーバー条件に永久に入れません。少し余らせて倒立点を「通過」させます。 - **アーム抑制項とアーム速度ガバナ**。アームが速く回りすぎるとコリオリ項 2Ĵ sinα cosα θ̇α̇ が τ_max を超えて計算トルク法が成立しなくなり、遠心力項が振子を励振し続けて捕まらなくなります。 θ̇ が上限に近づくほど、加速度の予算をエネルギー項から抑制項へ移します。 - **ゲインはすべて無次元化**してあります (エネルギーは E_swing = 2 m_p g l、加速度は τ_max/J₀、時間は ω_n = √(m_p g l/Ĵ) で正規化)。長さ・質量・トルク上限を変えても同じ挙動になります。ハンドオーバーは `|α| < 0.5 rad` かつ `|E| < 0.15 E_swing` で LQR へ。後者は「ホモクリニック軌道 (ちょうど倒立点に漸近する軌道) の近くにいるか」を見ています。外乱で `|α| > 0.8 rad` になったら振り上げに戻ります。 ### トルクが足りないときアーム先端が出せる加速度 (τ_max/J₀ · L_r) が重力加速度をかなり下回ると、原理的に振り上げられません。その場合はパネルに警告を出します。これはバグではなく、アクチュエータの能力不足という物理的な事実です。 ## ファイル構成 ``` index.html 画面とコントロールの定義 style.css レイアウト / テーマ (レスポンシブ) src/ furuta.js 運動方程式・RK4・エネルギー・線形化 lqr.js CARE の数値解 (DRE 積分 → Newton–Kleinman) linalg.js 行列演算・ガウス消去・Lyapunov 方程式 controllers.js 制御なし / PID / LQR / エナジースイングアップ renderer.js 等角投影の 3D 風ビュー + 上面図 scope.js 状態量と制御入力の時系列プロット main.js UI 組み立てとシミュレーションループ test/sim.test.mjs ヘッドレス回帰テスト ``` ## テストでカバーしていること `node --test "test/*.test.mjs"` で以下を確認しています。 - Lyapunov 方程式ソルバが `AᵀP + PA + Q = 0` を残差 1e-10 未満で満たす - CARE の解の残差が 1e-8 未満で、閉ループ A − BK が安定 - ウォームスタートの有無で同じ解に収束し、かつ DRE の積分が省略されている - 摩擦・入力ゼロで全エネルギーが保存する (RK4 の検証) - 線形化モデルが倒立点近傍で非線形モデルと一致する - 制御なしで吊り下がり位置へ収束する - PID / LQR が倒立点近傍の初期ずれから復帰する - **品質基準**: 吊り下がり静止 → 振り上げ → 倒立維持 → 外乱 (振り上げエネルギーの 30% 相当) → 復帰 - 上記のスイングアップが 10 通りのパラメータ変更に追従する - トルク不足の設定では振り上がらず、かつアームが暴走しない ## GitHub Pages 相対パスのみで構成しているので、リポジトリに置いてそのまま公開できます。ビルドステップは不要です。