現代制御の勝利：LQRで導く「数学的最適解」の実力【Day 6】

3軸リアクションホイール倒立振子を作るアドカレ の6日目です。

1. はじめに：PIDの限界と現代制御への期待

前回の [Day 5] では、古典制御の代表格である PID制御 を用いて倒立に成功しました。
しかし、その挙動には以下の課題が残りました。

• パラメータ調整の泥沼: $K_p, K_d$ の最適値を手探りで探す必要があり、根気と経験則に依存する。
• 多変数間の干渉: 「角度を直したい」と「ホイールを止めたい」という2つの目的がケンカし、ホイールが暴走しやすい。

本日は、現代制御理論の王道 LQR (Linear Quadratic Regulator: 線形2次レギュレータ) を実装します。
ロボットの物理モデルを数式化し、数学的に**「最も効率的（最適）」なゲインを自動算出する**アプローチです。

2. 状態空間モデルの構築 (Modeling)

LQRを適用するためには、ロボットの運動を 状態方程式 $\dot{x} = Ax + Bu$ で記述する必要があります。
ここを疎かにすると現代制御は絶対にうまくいきません。真面目に運動方程式を導出します。

2.1 物理変数の定義

Fusion 360 の設計データから抽出したパラメータです。

変数	定義	値
$m$	システム全体の質量 ($m_b + m_w$)	0.5 kg
$L$	接地点から重心までの距離	0.0866 m
$I_b$	ボディの慣性モーメント (支点周り)	$0.0006, \mathrm{kg \cdot m^2}$
$I_w$	フライホイールの慣性モーメント	$0.0001, \mathrm{kg \cdot m^2}$
$g$	重力加速度	$9.81, \mathrm{m/s^2}$
$u$	モータへの入力トルク	$\mathrm{N \cdot m}$

2.2 運動方程式の導出

今回のシステムは「倒立振子（ボディ）」と「回転体（ホイール）」の連成系です。
直立状態近傍での微小角 $\theta$ において $\sin\theta \approx \theta$ と近似し、線形化を行います。

① ボディの回転運動:
重力による「倒れるモーメント」と、モータの「反作用トルク $-u$」が作用します。
運動方程式は以下のようになります。
$$I_b \ddot{\theta} = mgL \theta - u$$
これを変形して、角加速度 $\ddot{\theta}$ を求めます：
$$\ddot{\theta} = \frac{mgL}{I_b}\theta - \frac{1}{I_b}u \quad \cdots (1)$$

② ホイールの回転運動:
モータトルク $u$ は、ホイールの回転加速に使われます。ただし、ホイール自体もボディに乗って揺れているため、空間に対する絶対的な回転加速度は $(\ddot{\theta} + \ddot{\phi})$ となります。
$$u = I_w (\ddot{\theta} + \ddot{\phi})$$
ここで $\phi$ はホイールの回転角です。これを $\ddot{\phi}$ について解きます。
$$\ddot{\phi} = \frac{u}{I_w} - \ddot{\theta}$$
ここに式(1)を代入して $\ddot{\theta}$ を消去し、整理すると：
$$\ddot{\phi} = -\frac{mgL}{I_b}\theta + \left( \frac{1}{I_w} + \frac{1}{I_b} \right) u \quad \cdots (2)$$

2.3 状態方程式への変換

制御したい状態ベクトルを $x = [\theta, \dot{\theta}, \dot{\phi}]^T$ （角度、角速度、ホイール角速度）と定義します。
式(1), (2)を行列形式 $\dot{x} = Ax + Bu$ にまとめます。

\frac{d}{dt} \begin{bmatrix} \theta \\ \dot{\theta} \\ \dot{\phi} \end{bmatrix} =
\underbrace{\begin{bmatrix} 0 & 1 & 0 \\ \frac{mgL}{I_b} & 0 & 0 \\ -\frac{mgL}{I_b} & 0 & 0 \end{bmatrix}}_{A}
\begin{bmatrix} \theta \\ \dot{\theta} \\ \dot{\phi} \end{bmatrix} +
\underbrace{\begin{bmatrix} 0 \\ -\frac{1}{I_b} \\ \frac{I_b+I_w}{I_b I_w} \end{bmatrix}}_{B} u

これが今回作成したCubliの　「物理的な設計図（状態空間モデル）」　です。
$A$ 行列の要素を見れば、「角度が開くと、重力でどれくらいの加速度で倒れようとするか（不安定性）」が数値化されています。

3. 実装：リカッチ方程式による最適ゲイン導出

モデルができれば、あとは数学の出番です。
LQRの目的は、以下の評価関数（コスト関数） $J$ を最小化するような入力 $u = -Kx$ を見つけることです。

$$J = \int_{0}^{\infty} (x^T Q x + u^T R u) dt$$

• $Q$ 行列: 状態のズレに対するペナルティ。「角度のズレ」を許さないのか、「ホイール速度」を許さないのか、設計者の意思をここに込めます。
• $R$ 行列: 入力に対するペナルティ。「省エネで動け（大きく）」か、「電力無視で全力で耐えろ（小さく）」かを決めます。

最適ゲイン K の算出プロセス

数学的に、$J$ を最小化するフィードバックゲイン $K$ は、以下の手順で一意に求まります。

1. 代数リカッチ方程式 (ARE) を解く:
   未知行列 $P$ についての方程式を解きます。
   $$A^T P + P A - P B R^{-1} B^T P + Q = 0$$
2. ゲイン $K$ を計算する:
   求まった $P$ を使って決定します。
   $$K = R^{-1} B^T P$$

これを手計算で解くのは困難ですが、Pythonなら scipy.linalg.solve_continuous_are 関数を使って一瞬です。

Pythonコード解説 (LQRControllerクラス)

ソースコード内の LQRController クラスの核心部分です。

class LQRController:
    def __init__(self):
        # 1. 物理パラメータ (Fusion 360より)
        self.Ib = 0.0006
        self.Iw = 0.0001
        self.L = 0.0866 
        self.m = 0.5 
        self.g = 9.81
        self.MGL = self.m * self.g * self.L

        # 2. システム行列 A, B の構築 (数式通り！)
        # A matrix
        a21 = self.MGL / self.Ib       # d(theta_dot)/d(theta)
        a31 = -(self.MGL / self.Ib)    # d(phi_dot)/d(theta)
        self.A = np.array([
            [0, 1, 0],
            [a21, 0, 0],
            [a31, 0, 0]
        ])

        # B matrix
        b2 = -1 / self.Ib              # d(theta_dot)/d(u)
        b3 = (self.Ib + self.Iw) / (self.Ib * self.Iw) # d(phi_dot)/d(u)
        self.B = np.array([
            [0],
            [b2],
            [b3]
        ])

        # 3. 重み行列 Q, R の設計 (ここがチューニングポイント)
        # diag([角度, 角速度, ホイール速度])
        # 角度偏差(100.0)は絶対に許さない。ホイール速度(0.001)はある程度許容する。
        self.Q = np.diag([100.0, 1.0, 0.001]) 
        self.R = np.array([[1.0]])

        # 4. リカッチ方程式を解いて最適ゲインKを導出
        if HAS_SCIPY:
            # solve_continuous_are が A^T P + PA ... = 0 を解いてくれる
            P = solve_continuous_are(self.A, self.B, self.Q, self.R)
            
            # K = R^-1 B^T P
            self.K = np.linalg.inv(self.R) @ self.B.T @ P
            
            print(f">>> Calculated LQR Gain K: {self.K}")
            # 出力例: [[ -48.23,  -6.84,  -0.031 ]]
        else:
            # Scipyがない環境用のフォールバック
            self.K = np.array([[ -45.0, -5.0, -0.05 ]])

4. なぜLQRはPIDより優れているのか？

算出されたゲイン K = [-48.23, -6.84, -0.031] を見てください。
これは、PID制御で言うところの以下に対応します。

• $K_p \approx 48.23$ （角度への比例項）
• $K_d \approx 6.84$ （角速度への微分項）
• $K_{wheel} \approx 0.031$ （ホイール速度へのフィードバック）

この「第3の項」が決定的な違いです。
PID制御では「角度」しか見ていないため、姿勢を直すためにホイールを無限に加速させてしまい、最終的にモータの限界を超えて転倒します。

一方 LQR は、「姿勢を直したいけど、ホイール速度も上げすぎたくない」 というトレードオフを数式 ($Q$ 行列) で考慮してゲインを決めています。
そのため、「倒れそうなときは加速して耐えるが、余裕ができたらゆっくり減速してホイール速度を回収する」 という、人間業では調整不可能な高度な制御を自動で行ってくれるのです。

5. 実験結果と実装コード

論より証拠。まずは、算出したLQRゲインで実際にロボットが制御されている様子をご覧ください。

5.1 実験動画：数学的最適解の挙動

動画の注目ポイント：
画面左上のグラフ（Realtime Plotter）を見てください。
キーボード操作で 15N もの外乱（衝撃）を与えていますが、角度誤差（赤・青のライン）が オーバーシュート（行き過ぎ）することなく、短時間収束 しています。

これが数学的に導かれた最適解、LQRの実力です。

5.2 実装ソースコード (sim_lqr.py)

この挙動を実現している全ソースコードです。
「リアルタイムグラフ描画」と「GUIによるゲイン微調整 (HUD)」も実装し、シミュレーションならではの検証環境として仕上げました。

import pybullet as p
import pybullet_data
import time
import math
import numpy as np
import matplotlib.pyplot as plt
try:
    from scipy.linalg import solve_continuous_are
    HAS_SCIPY = True
except ImportError:
    HAS_SCIPY = False
    print("!!! Scipy not found. Using default gains. !!!")
class RealtimePlotter:
    def __init__(self):
        # グラフの初期設定
        self.fig, self.ax = plt.subplots()
        self.ax.set_title("Roll / Pitch Error Monitor")
        self.ax.set_xlabel("Time (s)")
        self.ax.set_ylabel("Angle Error (deg)")
        self.ax.grid(True)
        
        # データを格納するリスト (最大100点など)
        self.max_len = 200
        self.times = [0] * self.max_len
        self.err_r = [0] * self.max_len
        self.err_p = [0] * self.max_len
        
        # 線オブジェクトの初期化
        self.line_r, = self.ax.plot(self.times, self.err_r, label="Roll Err", color="red")
        self.line_p, = self.ax.plot(self.times, self.err_p, label="Pitch Err", color="blue")
        self.ax.legend(loc='upper right')
        
        # Y軸の範囲を固定 (これがないとスケール調整で重くなる)
        self.ax.set_ylim(-1, 1) 

        # 非ブロッキングモードにするおまじない
        plt.ion()
        plt.show()

    def update(self, current_time, roll_err_deg, pitch_err_deg):
        # データの更新 (古いものを捨てて新しいものを追加)
        self.times.append(current_time)
        self.err_r.append(roll_err_deg)
        self.err_p.append(pitch_err_deg)
        
        self.times = self.times[-self.max_len:]
        self.err_r = self.err_r[-self.max_len:]
        self.err_p = self.err_p[-self.max_len:]

        # グラフの再描画 (高速化のため set_data を使用)
        self.line_r.set_data(range(len(self.times)), self.err_r)
        self.line_p.set_data(range(len(self.times)), self.err_p)
        
        # X軸の範囲をスライド
        self.ax.set_xlim(0, len(self.times))
        
        # 描画イベントの処理 (0.001秒だけ待って更新)
        plt.pause(0.001)
        
class LQRController:
    def __init__(self):
        self.Ib = 0.0006
        self.Iw = 0.0001
        self.L = 0.0866 
        self.m = 0.5 
        self.g = 9.81
        self.MGL = self.m * self.g * self.L

        a21 = self.MGL / self.Ib
        a31 = -(self.MGL / self.Ib)
        self.A = np.array([[0, 1, 0],[a21, 0, 0],[a31, 0, 0]])
        b2 = -1 / self.Ib
        b3 = (self.Ib + self.Iw) / (self.Ib * self.Iw)
        self.B = np.array([[0],[b2],[b3]])
        self.Q = np.diag([100.0, 1.0, 0.001]) 
        self.R = np.array([[1.0]])

        if HAS_SCIPY:
            P = solve_continuous_are(self.A, self.B, self.Q, self.R)
            self.K = np.linalg.inv(self.R) @ self.B.T @ P
            print(f">>> LQR Gain K: {self.K}")
        else:
            self.K = np.array([[ -45.0, -5.0, -0.05 ]])

    def get_components(self, angle_err, body_vel, wheel_vel):
        u_angle = -self.K[0, 0] * angle_err
        u_gyro  = -self.K[0, 1] * body_vel
        u_wheel = -self.K[0, 2] * wheel_vel
        return u_angle, u_gyro, u_wheel

def main():
    physicsClient = p.connect(p.GUI)
    p.setAdditionalSearchPath(pybullet_data.getDataPath())
    p.setGravity(0, 0, -9.81)
    
    # ★軽量化設定: 影を消す
    p.configureDebugVisualizer(p.COV_ENABLE_SHADOWS, 0)
    
    planeId = p.loadURDF("plane.urdf")
    
    deg_roll  = 135.0
    deg_pitch = 35.3
    target_pitch = deg_pitch * (math.pi / 180.0)
    target_roll  = deg_roll  * (math.pi / 180.0)
    
    startPos = [0, 0, 0.09] 
    startOrientation = p.getQuaternionFromEuler([target_roll, target_pitch, 0])
    
    boxId = p.loadURDF("my_cubli.urdf", startPos, startOrientation, globalScaling=1.0)
    p.changeDynamics(boxId, -1, lateralFriction=1.0, spinningFriction=0.1)

    p.resetDebugVisualizerCamera(cameraDistance=0.4, cameraYaw=45, cameraPitch=-10, cameraTargetPosition=[0,0,0.1])

    for i in range(3):
        p.setJointMotorControl2(boxId, i, p.VELOCITY_CONTROL, force=0)

    # ------------------------------------------------
    # GUI
    # ------------------------------------------------
    params = [1.03, -0.004, -0.002] # k1, k2, k3 初期値
    param_names = ["Angle (k1)", "Gyro (k2) ", "Wheel (k3)"]
    selected_idx = 0 

    btn_start = p.addUserDebugParameter(">>> START LQR <<<", 1, 0, 0)
    btn_reset = p.addUserDebugParameter("Reset Robot", 1, 0, 0)
    
    p.addUserDebugParameter("--- SELECT ---", 1, 0, 0)
    btn_sel_1 = p.addUserDebugParameter("[1] Select Angle", 1, 0, 0)
    btn_sel_2 = p.addUserDebugParameter("[2] Select Gyro", 1, 0, 0)
    btn_sel_3 = p.addUserDebugParameter("[3] Select Wheel", 1, 0, 0)
    
    p.addUserDebugParameter("--- ADJUST ---", 1, 0, 0)
    btn_inc_coarse = p.addUserDebugParameter("[++] Up 0.01", 1, 0, 0)
    btn_dec_coarse = p.addUserDebugParameter("[--] Down 0.01", 1, 0, 0)
    btn_inc_fine   = p.addUserDebugParameter("[+] Up 0.001", 1, 0, 0)
    btn_dec_fine   = p.addUserDebugParameter("[-] Down 0.001", 1, 0, 0)

    btn_ids = [btn_start, btn_reset, btn_sel_1, btn_sel_2, btn_sel_3, 
               btn_inc_coarse, btn_dec_coarse, btn_inc_fine, btn_dec_fine]
    prev_btn_states = [p.readUserDebugParameter(i) for i in btn_ids]

    is_running = False
    
    # テキストID管理
    text_ids = [-1, -1, -1, -1]
    
    # ★HUD更新関数 (必要なときだけ呼ぶ)
    def update_hud():
        base_pos = [0, 0, 0.4] 
        line_height = 0.05
        lines = ["=== LQR TUNING ==="]
        for i in range(3):
            prefix = ">> " if i == selected_idx else "   "
            lines.append(f"{prefix}{param_names[i]}: {params[i]:.4f}")

        for i, line_str in enumerate(lines):
            pos = [base_pos[0], base_pos[1], base_pos[2] - (i * line_height)]
            color = [0, 0, 0]
            if i == 0: color = [0, 0, 1]
            elif (i-1) == selected_idx: color = [1, 0, 0]
            text_ids[i] = p.addUserDebugText(line_str, pos, textColorRGB=color, textSize=1.5, replaceItemUniqueId=text_ids[i])

    try:
        lqr = LQRController()
    except:
        pass

    # 初回描画
    update_hud()
    print("Ready!")

    last_time = time.time()
    # ★グラフ表示の準備
    try:
        plotter = RealtimePlotter()
    except:
        print("Matplotlib error. Graph disabled.")
        plotter = None

    frame_count = 0 # 間引き用カウンタ
    while True:
        keys = p.getKeyboardEvents()
        curr_btn_states = [p.readUserDebugParameter(i) for i in btn_ids]
        
        # 値が変更されたかどうかのフラグ
        need_update_hud = False

        # 選択
        if curr_btn_states[2] != prev_btn_states[2]: selected_idx = 0; need_update_hud = True
        if curr_btn_states[3] != prev_btn_states[3]: selected_idx = 1; need_update_hud = True
        if curr_btn_states[4] != prev_btn_states[4]: selected_idx = 2; need_update_hud = True
        
        if ord('1') in keys and keys[ord('1')] & p.KEY_WAS_TRIGGERED: selected_idx = 0; need_update_hud = True
        if ord('2') in keys and keys[ord('2')] & p.KEY_WAS_TRIGGERED: selected_idx = 1; need_update_hud = True
        if ord('3') in keys and keys[ord('3')] & p.KEY_WAS_TRIGGERED: selected_idx = 2; need_update_hud = True

        # 増減
        delta = 0.0
        if curr_btn_states[5] != prev_btn_states[5]: delta = 0.01
        if curr_btn_states[6] != prev_btn_states[6]: delta = -0.01
        if curr_btn_states[7] != prev_btn_states[7]: delta = 0.001
        if curr_btn_states[8] != prev_btn_states[8]: delta = -0.001
        
        if p.B3G_UP_ARROW in keys and keys[p.B3G_UP_ARROW] & p.KEY_WAS_TRIGGERED:   delta = 0.01
        if p.B3G_DOWN_ARROW in keys and keys[p.B3G_DOWN_ARROW] & p.KEY_WAS_TRIGGERED: delta = -0.01
        if p.B3G_RIGHT_ARROW in keys and keys[p.B3G_RIGHT_ARROW] & p.KEY_WAS_TRIGGERED: delta = 0.001
        if p.B3G_LEFT_ARROW in keys and keys[p.B3G_LEFT_ARROW] & p.KEY_WAS_TRIGGERED:  delta = -0.001
        
        if delta != 0.0:
            params[selected_idx] += delta
            need_update_hud = True

        # START / RESET
        if curr_btn_states[0] != prev_btn_states[0]:
            is_running = True
            print("LQR Started!")
        
        if curr_btn_states[1] != prev_btn_states[1]:
            is_running = False
            p.resetBasePositionAndOrientation(boxId, startPos, startOrientation)
            p.resetBaseVelocity(boxId, [0,0,0], [0,0,0])
            for i in range(3): p.resetJointState(boxId, i, 0, 0)
            print("Reset.")

        prev_btn_states = curr_btn_states

        # ★変更があったときだけ描画更新 (これで軽くなる！)
        if need_update_hud:
            update_hud()

        # 制御ループ
        if is_running:
            current_time = time.time()
            dt = current_time - last_time
            if dt < 1./240.: dt = 1./240.
            last_time = current_time

            pos, quat = p.getBasePositionAndOrientation(boxId)
            r, p_angle, y = p.getEulerFromQuaternion(quat)
            lin_v_world, ang_v_world = p.getBaseVelocity(boxId)
            _, inv_orient = p.invertTransform([0,0,0], quat)
            _, ang_v_local = p.multiplyTransforms([0,0,0], inv_orient, ang_v_world, [0,0,0,1])
            vel_x, vel_y, vel_z = [p.getJointState(boxId, i)[1] for i in range(3)]

            err_r = r - target_roll
            err_p = p_angle - target_pitch
            err_y = y 
            for err in [err_r, err_p, err_y]:
                while err > math.pi: err -= 2*math.pi
                while err < -math.pi: err += 2*math.pi

            s_angle, s_gyro, s_wheel = params

            ux_a, ux_g, ux_w = lqr.get_components(err_r, ang_v_local[0], vel_x)
            uy_a, uy_g, uy_w = lqr.get_components(err_p, ang_v_local[1], vel_y)
            uz_a, uz_g, uz_w = lqr.get_components(err_y, ang_v_local[2], vel_z)

            torque_x = (s_angle * ux_a) + (s_gyro * ux_g) + (s_wheel * ux_w)
            torque_y = ((s_angle * uy_a) + (s_gyro * uy_g) + (s_wheel * uy_w)) * 2.0
            torque_z = (s_angle * uz_a) + (s_gyro * uz_g) + (s_wheel * uz_w)

            limit = 0.5
            p.setJointMotorControl2(boxId, 0, p.TORQUE_CONTROL, force=max(-limit, min(limit, torque_x)))
            p.setJointMotorControl2(boxId, 1, p.TORQUE_CONTROL, force=max(-limit, min(limit, torque_y)))
            p.setJointMotorControl2(boxId, 2, p.TORQUE_CONTROL, force=max(-limit, min(limit, torque_z)))

            # 外乱 (WASD)
            force_mag = 15.0
            if ord('i') in keys and keys[ord('i')] & p.KEY_WAS_TRIGGERED: p.applyExternalForce(boxId, -1, [force_mag, 0, 0], [0, 0, 0.1], p.LINK_FRAME)
            if ord('j') in keys and keys[ord('j')] & p.KEY_WAS_TRIGGERED: p.applyExternalForce(boxId, -1, [-force_mag, 0, 0], [0, 0, 0.1], p.LINK_FRAME)
            if ord('k') in keys and keys[ord('k')] & p.KEY_WAS_TRIGGERED: p.applyExternalForce(boxId, -1, [0, force_mag, 0], [0, 0, 0.1], p.LINK_FRAME)
            if ord('m') in keys and keys[ord('m')] & p.KEY_WAS_TRIGGERED: p.applyExternalForce(boxId, -1, [0, -force_mag, 0], [0, 0, 0.1], p.LINK_FRAME)

            # ------------------------------------------------
            # ★グラフ更新 (5フレームに1回だけ描画)
            # ------------------------------------------------
            frame_count += 1
            if frame_count % 10 == 0 and plotter is not None:
                # 誤差を計算 (度数法)
                deg_err_r = (target_roll - r) * 180/math.pi
                deg_err_p = (target_pitch - p_angle) * 180/math.pi
                
                # 角度正規化 (-180 ~ 180) 表示用
                while deg_err_r > 180: deg_err_r -= 360
                while deg_err_r < -180: deg_err_r += 360
                while deg_err_p > 180: deg_err_p -= 360
                while deg_err_p < -180: deg_err_p += 360

                # グラフ更新
                plotter.update(current_time, deg_err_r, deg_err_p)

            p.stepSimulation()
        else:
            p.resetBasePositionAndOrientation(boxId, startPos, startOrientation)
            p.resetBaseVelocity(boxId, [0,0,0], [0,0,0])
            for i in range(3): p.resetJointState(boxId, i, 0, 0)
            

        time.sleep(1./240.)

if __name__ == "__main__":
    main()

6. まとめと次回予告

本日の検証により、「正確な物理モデルさえあれば、LQRは調整不要で最強の性能を発揮する」 ことが実証されました。
起動した瞬間、まるで彫像のように静止する姿は感動的です。

しかし、これはあくまで「モデルが完璧」なシミュレーションの中の話です。
次回 [Day 7] では、現実世界の不確実性（摩擦、バックラッシュ、ノイズ）に立ち向かうため、モデルに依存しない 「深層強化学習 (Deep Reinforcement Learning)」 に挑戦します。

数学モデルの限界を、AIは超えることができるのか？ お楽しみに。

アドベントカレンダー参加中
STM32×AIで「3軸倒立振子」を作る25日間（ひとりアドカレ）Advent Calendar 2025