1. はじめに

想定読者

古典力学をある程度理解しており，シミュレーションに興味がある人
PID制御に興味がある人
倒立振り子をマイコンで作成しようと思いつつ，材料を揃えるのをめんどくさがっている人

本記事の方針

台車に取り付けられた倒立振り子の運動方程式を導出し，PC上でシミュレーションします
PID制御（今回はPD制御）によって倒立振り子が倒れないように制御します
環境構築が簡単かつ，科学分野で広く使用されているPythonを用いて実装します．

何を作るか

倒立振り子を自由に操作できるようになります．

背景

紙面上で導いた運動方程式、動かしてみたくない？？？
ということで、今回は「倒立振り子（Inverted Pendulum）」という古典的かつ直感的な題材を用い、運動方程式の導出から数値シミュレーション、制御実装まで一通りやってみます。傘の柄の先を手のひらで支えてバランスを取る遊び、したことありませんか？それです。

PID制御を可視化してみたくない？？？
せっかくなので、手動ではなく自動でバランスを取るようにPD制御も実装し、視覚的に確認できるようにします。安定性解析などの細かい理論は脇に置いて、まずは制御できることを優先します。

ぜひ，コードをダウンロードし手元で実行してみてください．

コードの取得

以下のコマンドでコードをクローンできます：

git clone https://github.com/kitsune8848/002_PD_InvertedPendulum.git

2. 倒立振り子の運動方程式を導出する

モデルと変数

次のような倒立振り子モデルを扱います（図参照）。

各パラメータは以下の通りです：

台車の位置：$x$
振子の角度：$\theta$（鉛直上向きを $0$ とし、時計回りに正）
振子の長さ：$l$（回転中心から重心までの長さ）
台車の質量：$M$
振子の質量：$m$
重力加速度：$g$
台車への外力：$u$（この力を制御する）

ラグランジアンによる導出

運動方程式はラグランジュ方程式により以下のように求めます：

$$
\frac{d}{dt} \left( \frac{\partial L}{\partial \dot{q}_i} \right) - \frac{\partial L}{\partial q_i} = Q_i \tag1
$$

一般化座標：$q_i = x, \theta$
一般化外力：$Q_x = u$, $Q_\theta = 0$
ラグランジアン $L = T - V$

ラグランジュ方程式をご存知でない方は解析力学をあたってください．面白いですよ．

重心位置と速度

$$
x_p = x + l\sin\theta, \quad y_p = -l\cos\theta \tag{2}
$$

$$
\dot{x}_p = \dot{x} + l\cos\theta\cdot \dot{\theta}, \quad \dot{y}_p = l\sin\theta\cdot \dot{\theta}\tag{3}
$$

運動エネルギーと位置エネルギー

$$
T = \frac{1}{2}(M + m)\dot{x}^2 + ml\cos\theta \cdot \dot{x}\dot{\theta} + \frac{1}{2}ml^2\dot{\theta}^2 \tag{4}
$$

$$
V = -mgl\cos\theta \tag{5}
$$

したがって、

$$
L = \frac{1}{2}(M + m)\dot{x}^2 + ml\cos\theta \cdot \dot{x}\dot{\theta} + \frac{1}{2}ml^2\dot{\theta}^2 + mgl\cos\theta\tag{6}
$$

ラグランジュ方程式の適用

$x$ に関する方程式：

$$
(M + m)\ddot{x} + ml\cos\theta \cdot \ddot{\theta} - ml\dot{\theta}^2 \sin\theta = u \tag{7}
$$

$\theta$ に関する方程式：

$$
ml\cos\theta \cdot \ddot{x} + ml^2\ddot{\theta} - mgl\sin\theta = 0 \tag{8}
$$

加速度の導出

これらを連立して解くと、次のように書けます：

$$
\ddot{\theta} = \frac{g \sin\theta - \cos\theta \cdot \left( \frac{u + ml\dot{\theta}^2 \sin\theta}{M + m} \right)}{l \left( \frac{4}{3} - \frac{m\cos^2\theta}{M + m} \right)}\tag{9}
$$

$$
\ddot{x} = \frac{u + ml\dot{\theta}^2 \sin\theta - ml\ddot{\theta}\cos\theta}{M + m}\tag{10}
$$

状態変数の定義

上の2つの式を1階微分で次のように表します．

\frac{d}{dt}
\begin{bmatrix}
x \\
\dot{x} \\
\theta \\
\dot{\theta}
\end{bmatrix}
=
\begin{bmatrix}
\dot{x} \\
f_1(x, \dot{x}, \theta, \dot{\theta}, u) \\
\dot{\theta} \\
f_2(x, \dot{x}, \theta, \dot{\theta}, u)
\end{bmatrix}\tag{11}

ここで：

$f_1$ は $x$ 方向の加速度 $\ddot{x}$
$f_2$ は $\theta$ 方向の加速度 $\ddot{\theta}$

f_1(x, \dot{x}, \theta, \dot{\theta}, u) = \frac{u + ml\dot{\theta}^2 \sin\theta - ml\ddot{\theta}\cos\theta}{M + m}\tag{13}

f_2(x, \dot{x}, \theta, \dot{\theta}, u)= \frac{g \sin\theta - \cos\theta \cdot \left( \frac{u + ml\dot{\theta}^2 \sin\theta}{M + m} \right)}{l \left( \frac{4}{3} - \frac{m\cos^2\theta}{M + m} \right)}\tag{12}

3. 数値計算：Runge-Kutta法（4次）

上記の状態方程式を数値的に時間発展させるために、4次のRunge-Kutta法を使用します。

この方法では、次のように状態量を時間発展させます：

\begin{aligned}
k_1 &= f(t_n, y_n) \\
k_2 &= f\left(t_n + \frac{h}{2}, \, y_n + \frac{h}{2}k_1 \right) \\
k_3 &= f\left(t_n + \frac{h}{2}, \, y_n + \frac{h}{2}k_2 \right) \\
k_4 &= f\left(t_n + h, \, y_n + hk_3 \right) \\
\\
y_{n+1} &= y_n + \frac{h}{6}(k_1 + 2k_2 + 2k_3 + k_4)
\end{aligned}

ここで：

$h$ は時間刻み幅（例：$h = 0.01$）
$f(t, y)$ は状態量 $y$ に対する時間微分（状態空間モデルに基づく）

この数値積分法を用いることで、非線形な倒立振子の運動を高精度にシミュレートできます。

導出方法についてはこちらで詳しく説明されています．

4. 倒立振り子モデルの実装

実装と言っても肝心の部分は簡単です．
今外部から力uを食えた時の各物理量を計算するupdate_stateメソッドを見てみましょう．

式(11)を求めているのが内部関数derivativesです．

update_stateではderivatives関数を使用して4次のRunge-Kutta法を解いていますね．

python inverted_pendulum.py

class InvertedPendulum:
    """
    倒立振子モデルのシミュレーションクラス。
    Runge-Kutta法（4次）を用いて運動方程式を時間発展させる。
    """

    # 定数
    M = 0.4                      # 台車の質量 [kg]
    m = 0.05                     # 振子の質量 [kg]
    l = 0.15                     # 振子の長さ（回転中心から重心まで）[m]
    g = 9.8                      # 重力加速度 [m/s^2]
    max_disturbance_u = 0.01    # 外乱の最大値
    t_num = 10                  # 1ステップあたりの微小刻み数（精度向上用）

    def update_state(self, external_force: float):
        """
        指定した外力（+ノイズ）を加えて、状態を更新する
        現在の状態を Runge-Kutta 法（4次）で時間発展させる
       
        Parameters:
        - external_force (float): 外部から加える台車への力

        Returns:
        - (theta, theta_dot): 更新後の角度と角速度
        """

        def derivatives(state, u):
            """
            状態の導関数（加速度）を計算する内部関数

            Parameters:
            - state: 現在の状態ベクトル [x, x_dot, theta, theta_dot]
            - u: 外部からの力

            Returns:
            - 微分値ベクトル [x_dot, x_acc, theta_dot, theta_acc]
            """
            x, x_dot, theta, theta_dot = state
            cost = math.cos(theta)
            sint = math.sin(theta)
            total_mass = self.M + self.m
            ml = self.m * self.l

            # θ方向の加速度
            temp = (u + ml * theta_dot**2 * sint) / total_mass
            theta_acc = (self.g * sint - cost * temp) / (
                self.l * (4.0 / 3.0 - (self.m * cost**2) / total_mass)
            )

            # x方向の加速度
            x_acc = temp - (ml * theta_acc * cost) / total_mass

            return np.array([x_dot, x_acc, theta_dot, theta_acc])

        self.u = external_force

        if self.noisy:
            disturbance = np.random.uniform(-self.max_disturbance_u, self.max_disturbance_u)
            self.u += disturbance

        # 現在の状態ベクトル
        state = np.array([self.x, self.x_dot, self.theta, self.theta_dot])
        u = self.u

        # Runge-Kutta 4次法で積分
        for _ in range(self.t_num):
            dt = self.dt_small
            k1 = derivatives(state, u)
            k2 = derivatives(state + 0.5 * dt * k1, u)
            k3 = derivatives(state + 0.5 * dt * k2, u)
            k4 = derivatives(state + dt * k3, u)

            state += (dt / 6.0) * (k1 + 2*k2 + 2*k3 + k4)

        # 状態更新
        self.x, self.x_dot, self.theta, self.theta_dot = state
        
        return SensorValues(*state, external_force)

5. PD制御の実装

PID制御について知らない方はこちらを参照ください．

PD制御は、次の2つの要素を使ってシステムを制御する方法です：

P（比例）制御：目標と現在のズレ（誤差）に比例した出力
→ 誤差が大きいほど強く戻そうとする
D（微分）制御：誤差の変化の速さ（≒速度）に比例した出力
→ 急な変化を抑えて滑らかに制御する

これらのズレに係数をかけてたしあわせた大きさの外力を加えます．
とても単純ですね．

python control_plant_manager.py

class ControlPlantManager:
    """
    倒立振子のインタラクティブシミュレーションを実行するクラス。
    ユーザー入力または自動制御により、倒立振子を制御します。
    """

    def _auto_control(self):
        """
        簡易PD制御に基づいた自動バランス制御。

        Returns:
            tuple: (x, x_dot, theta, theta_dot)
        """

        # 現在の状態を取得
        theta = self.plant.theta
        theta_dot = self.plant.theta_dot
        x_dot = self.plant.x_dot

        #目的の値との差を見る 
        #今回は全て0が目的の値．冗長だがわかりやすいように書いておく
        error_theta = 0 - wrap_to_pi(theta)# 角度を -π 〜 π に正規化
        error_theta_dot =  0 - theta_dot 
        error_x_dot = 0 - x_dot 

        # 単純なPD制御（I項は省略）
        force = (
            -30.0 * error_theta    # P項：倒立維持
            -3.0 * theta_dot       # D項：角速度抑制
            -1.0 * x_dot           # 台車速度の減衰
        )

        # 外力制限（過大な制御を防ぐ）
        force = max(min(force, 1), -1)

        return self.plant.update_state(force)

6. 完成したもの

キー操作で倒立振り子をシミュレーションできるソフトを作りました．
上のモデル以外は特に重要ではないので説明を省きます．

操作方法

a: PD制御開始
←: 左方向に力を加える
→: 右方向に力を加える
↑: ストップする

7. 最後に

倒立振り子をシミュレーションできるようになりました．
要は運動方程式を解いて，(11)の形にして4次のRunge-Kutta法を適用すれば大抵のものはシミュレーションできます．

ちなみにですが，実はPD制御は倒立振り子の制御に向いていなかったりします．厳密に言うと$\theta$が15度ほどなら適していますが，真下で静止した状態で振り上げるのには向いていません．これも制御工学的に説明できるものですので，興味のある方は調べてみてください．

さらにちなむと，今回は簡単のため，物理モデルから速度や位置，各速度や角度といったものを取得しましたが，実際のセンサでは加速度しか取得できないです．それらを積分して速度や位置を求める必要があるのですが，ノイズが絡んできたりして，一筋縄ではいきません．
ぜひ，実際に倒立振り子作ってみてくださいね．