器（うつわ）に魂を入れる：PyBulletで実装するPID制御【Day 5】

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

1. はじめに：ただの箱からロボットへ

昨日の [Day 4] では、Fusion 360で設計した筐体を物理シミュレータ (PyBullet) の中に「デジタルツイン」として召喚することに成功しました。
しかし、今のロボットは物理法則に従って 「ただ倒れるだけ」 の存在です。

Day 5 の今日は、このロボットに 「制御（魂）」 を吹き込みます。
制御工学の基本にして王道である PID制御 をPythonで実装し、重力に逆らって直立させます。

2. 制御の戦略：離散時間PID制御の実装

倒立振子の力学

倒立振子の原理はシンプルです。
「右に倒れそうになったら、ホイールを右に回す」。これだけです。
ホイールを加速させると、その反作用（Reaction Torque）でボディには逆向き（左向き）の力が働き、姿勢が戻ります。

連続時間から離散時間へ

教科書的なPID制御の式は連続時間で書かれますが、コンピュータ（およびシミュレータ）で扱うには離散化する必要があります。

連続時間のPID:
$$u(t) = K_p e(t) + K_i \int e(t) dt + K_d \frac{d e(t)}{dt}$$

離散時間のPID (今回の実装):
シミュレーションのステップ時間 dt (約4ms) を考慮し、以下のように実装します。

$$u[k] = K_p e[k] + K_i \sum_{j=0}^{k} e[j] \Delta t + K_d \frac{e[k] - e[k-1]}{\Delta t}$$

• $e[k]$: 現在の角度偏差 (目標角度 - 現在角度)
• $K_p$ (Proportional): バネ係数のような働き。傾きに比例して戻す力。
• $K_d$ (Derivative): ダンパ係数のような働き。倒れる速度に比例してブレーキを掛ける力。
• $K_i$ (Integral): 定常偏差を消す力。（※今回は高速な姿勢制御であり、ワインドアップ現象による暴走を防ぐため $K_i \approx 0$ とします）

3. 実装：PythonによるPIDコントローラ

PyBulletはPythonで制御ループを直接記述できるのが強みです。
今回は、汎用的なPIDクラスに加え、リアルタイムでゲイン調整ができるGUI や ログ保存機能 を備えたコードを実装しました。

コントローラクラス (PIDController)

まずは制御の中核となるクラスです。
ポイントは 「角度の正規化」 です。物理シミュレータでは角度が $-\pi$ から $+\pi$ に飛ぶことがあり、その瞬間に微分項が暴走するのを防ぐ処理を入れています。

class PIDController:
    def __init__(self, kp, ki, kd, target=0):
        self.kp = kp
        self.ki = ki
        self.kd = kd
        self.target = target
        self.prev_error = 0
        self.integral = 0
        
    def compute(self, current_value, dt):
        error = self.target - current_value
        
        # ★重要テクニック：角度の正規化 (-pi ~ +pi)
        # これがないと、180度付近で数値が不連続になり暴走する
        while error > math.pi:
            error -= 2 * math.pi
        while error < -math.pi:
            error += 2 * math.pi
            
        p_term = self.kp * error
        self.integral += error * dt
        i_term = self.ki * self.integral
        
        # 微分項（D制御）
        if dt > 0:
            derivative = (error - self.prev_error) / dt
        else:
            derivative = 0
        d_term = self.kd * derivative
        
        self.prev_error = error
        return p_term + i_term + d_term

    def reset(self):
        self.prev_error = 0
        self.integral = 0

メイン処理：頂点倒立への挑戦

Cubliの醍醐味である 「頂点での倒立（Corner Balancing）」 を目指すため、目標角度を特殊な値に設定しています。

• deg_roll = 135.0 (または45度)
• deg_pitch = 35.26 (※ $\tan^{-1}(1/\sqrt{2})$ )

この角度こそが、立方体の対角線が鉛直になり、重心が最も高い位置に来る「頂点立ち」の姿勢です。

# ==============================================================
# メイン処理
# ==============================================================
def main():
    physicsClient = p.connect(p.GUI)
    p.setAdditionalSearchPath(pybullet_data.getDataPath())
    p.setGravity(0, 0, -9.81)
    
    planeId = p.loadURDF("plane.urdf")
    
    # -----------------------------------------------------------
    # ★角度設定
    # -----------------------------------------------------------
    # 現物合わせした「正解の角度」
    deg_roll  = 135.0
    deg_pitch = 35.3
    target_yaw = 0
    
    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.3, cameraYaw=45, cameraPitch=-15, cameraTargetPosition=[0,0,0.1])

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

    # ------------------------------------------------
    # GUIスライダー
    # ------------------------------------------------
    # Kp=1.5, Kd=0.03 あたりが良い初期値かもしれません
    sld_kp = p.addUserDebugParameter("Kp (P Gain)", 0, 5.0, 1.5)
    sld_kd = p.addUserDebugParameter("Kd (D Gain)", 0, 0.1, 0.03)
    # sld_ki = p.addUserDebugParameter("Ki (I Gain)", 0, 0.2, 0.0)
    
    btn_start = p.addUserDebugParameter(">>> START CONTROL <<<", 1, 0, 0)
    btn_reset = p.addUserDebugParameter("Reset Robot", 1, 0, 0)
    
    prev_start_val = p.readUserDebugParameter(btn_start)
    prev_reset_val = p.readUserDebugParameter(btn_reset)

    is_running = False

    pid_x = PIDController(0, 0, 0, target=target_roll)
    pid_y = PIDController(0, 0, 0, target=target_pitch)
    pid_z = PIDController(0, 0, 0, target=0)

    print("Ready! Press 'START CONTROL' button in GUI.")
    print("Press 'x', 'X', 'y', 'Y' to apply disturbance force.")

    # ★ログファイルの準備 (リアルタイム書き込みモード)
    log_file = open("sim_log.csv", "w")
    log_file.write("Time,Roll,Pitch,Yaw,ErrR,ErrP,ErrY,TrqX,TrqY,TrqZ,VelX,VelY,VelZ\n")
    start_time = 0

    last_time = time.time()
    
    try:
        while True:
            # キーボード入力
            keys = p.getKeyboardEvents()

            # GUIボタン監視
            curr_start_val = p.readUserDebugParameter(btn_start)
            curr_reset_val = p.readUserDebugParameter(btn_reset)

            if curr_start_val != prev_start_val:
                prev_start_val = curr_start_val
                is_running = True
                start_time = time.time() # ログ用の時間リセット
                print("Control Started!")

            if curr_reset_val != prev_reset_val:
                prev_reset_val = curr_reset_val
                is_running = False 
                p.resetBasePositionAndOrientation(boxId, startPos, startOrientation)
                p.resetBaseVelocity(boxId, [0,0,0], [0,0,0])
                p.resetJointState(boxId, 0, targetValue=0, targetVelocity=0)
                p.resetJointState(boxId, 1, targetValue=0, targetVelocity=0)
                p.resetJointState(boxId, 2, targetValue=0, targetVelocity=0)
                pid_x.reset()
                pid_y.reset()
                pid_z.reset()
                print("Reset & Paused.")

            if is_running:
                current_time = time.time()
                dt = current_time - last_time
                if dt < 1./240.: dt = 1./240.
                last_time = current_time

                # ------------------------------------------------
                # ★外乱入力 (4方向に対応)
                # ------------------------------------------------
                force_mag = 25.0 # 力の強さ (N) 
                
                # X (横)
                if ord('i') in keys and keys[ord('i')] & p.KEY_WAS_TRIGGERED: # +X方向
                    print(">>> Push +X")
                    p.applyExternalForce(boxId, -1, forceObj=[force_mag, 0, 0], posObj=[0, 0, 0.1], flags=p.LINK_FRAME)
                

                if ord('k') in keys and keys[ord('k')] & p.KEY_WAS_TRIGGERED: # -X方向
                    print(">>> Push -X")
                    p.applyExternalForce(boxId, -1, forceObj=[-force_mag, 0, 0], posObj=[0, 0, 0.1], flags=p.LINK_FRAME)

                # Y (奥)
                if ord('j') in keys and keys[ord('j')] & p.KEY_WAS_TRIGGERED: # +Y方向
                    print(">>> Push +Y")
                    p.applyExternalForce(boxId, -1, forceObj=[0, force_mag, 0], posObj=[0, 0, 0.1], flags=p.LINK_FRAME)
                
                if ord('m') in keys and keys[ord('m')] & p.KEY_WAS_TRIGGERED: # -Y方向
                    print(">>> Push -Y")
                    p.applyExternalForce(boxId, -1, forceObj=[0, -force_mag, 0], posObj=[0, 0, 0.1], flags=p.LINK_FRAME)

                # 姿勢取得
                pos, quat = p.getBasePositionAndOrientation(boxId)
                r, p_angle, y = p.getEulerFromQuaternion(quat)

                # ゲイン読み取り
                kp_val = p.readUserDebugParameter(sld_kp)
                kd_val = p.readUserDebugParameter(sld_kd)
                ki_val = 0.0 # Kiはゼロ固定
                
                for pid in [pid_x, pid_y, pid_z]:
                    pid.kp, pid.ki, pid.kd = kp_val, ki_val, kd_val

                # PID計算 & トルク指令
                torque_x = -1 * pid_x.compute(r, dt)
                torque_y = -1 * pid_y.compute(p_angle, dt) * 2.0 
                torque_z = -1 * pid_z.compute(y, dt)

                max_torque = 0.3
                torque_x = max(-max_torque, min(max_torque, torque_x))
                torque_y = max(-max_torque, min(max_torque, torque_y))
                torque_z = max(-max_torque, min(max_torque, torque_z))

                p.setJointMotorControl2(boxId, 0, p.TORQUE_CONTROL, force=torque_x)
                p.setJointMotorControl2(boxId, 1, p.TORQUE_CONTROL, force=torque_y)
                p.setJointMotorControl2(boxId, 2, p.TORQUE_CONTROL, force=torque_z)

                # ------------------------------------------------
                # ログ保存 & 表示
                # ------------------------------------------------
                vel_x = p.getJointState(boxId, 0)[1]
                vel_y = p.getJointState(boxId, 1)[1]
                vel_z = p.getJointState(boxId, 2)[1]
                
                err_r = (target_roll - r) * 180/math.pi
                err_p = (target_pitch - p_angle) * 180/math.pi
                err_y = (target_yaw - y) * 180/math.pi

                # コンソール表示
                print(f"Err(deg) R:{err_r:5.1f} P:{err_p:5.1f} Y:{err_y:5.1f} | "
                      f"Vel(rad/s) X:{vel_x:5.1f} Y:{vel_y:5.1f} Z:{vel_z:5.1f}")

                # ★CSV書き込み (リアルタイム)
                elapsed_time = current_time - start_time
                log_row = f"{elapsed_time:.3f},{r:.3f},{p_angle:.3f},{y:.3f},{err_r:.3f},{err_p:.3f},{err_y:.3f},{torque_x:.3f},{torque_y:.3f},{torque_z:.3f},{vel_x:.3f},{vel_y:.3f},{vel_z:.3f}\n"
                log_file.write(log_row)

                p.stepSimulation()

            else:
                # 停止中
                p.resetBasePositionAndOrientation(boxId, startPos, startOrientation)
                p.resetBaseVelocity(boxId, [0,0,0], [0,0,0])
                p.resetJointState(boxId, 0, targetValue=0, targetVelocity=0)
                p.resetJointState(boxId, 1, targetValue=0, targetVelocity=0)
                p.resetJointState(boxId, 2, targetValue=0, targetVelocity=0)
                last_time = time.time()

            time.sleep(1./240.)

    except KeyboardInterrupt:
        print("\nSimulation stopped by user.")
    finally:
        # 終了時にファイルを閉じる
        if not log_file.closed:
            log_file.close()
            print("Log file saved to 'sim_log.csv'.")
        p.disconnect()

if __name__ == "__main__":
    main()

4. 苦闘の記録：正帰還ループ（自殺モード）の罠

実装していざ動かしてみると、「誤差はゼロ付近なのに、ホイールが無限に加速して転倒する」 という現象に悩まされました。

原因：符号の逆転

原因は、制御入力の符号ミスによる 「正のフィードバック（Positive Feedback）」 でした。
物理シミュレータでは、モータの回転方向とトルクの正負定義が直感と逆になることがよくあります。

今回は実験の結果、PID出力に -1 を掛ける ことで安定しました。

# 修正前：倒れる方向へ加速（NG）
torque = pid.compute(...) 

# 修正後：倒れるのを食い止める（OK）
torque = -1 * pid.compute(...)

5. 成果：キー操作による外乱テスト

調整の結果、Kp=1.5, Kd=0.03 付近で、あの不安定な「頂点立ち」が見事に安定しました。

安定性をテストするために、キーボードの I, J, K, M キー で前後左右から「外乱（衝撃）」を与えてみます。

(※ここに、キー操作で叩かれても持ち直すGIF動画を貼る)

• I / K キー: X軸方向への衝撃 (+X / -X)
• J / M キー: Y軸方向への衝撃 (+Y / -Y)

p.applyExternalForce 関数で 25.0 N もの力を加えても、瞬時にホイールが唸りを上げて姿勢を復元します。
これこそが制御の力、ロボットに魂が宿った瞬間です。

6. リアルタイム・ログ解析によるチューニング

感覚による調整だけでなく、データを可視化するためにCSV書き出し機能も実装しました。

Time,Roll,Pitch,Yaw,ErrR,ErrP,ErrY,TrqX,TrqY,TrqZ...
1.002,2.356,0.615,0.000,0.001,0.002,0.000,-0.050...

これをExcelでグラフ化すると、「外乱が入った瞬間にP項（復元力）が立ち上がり、その後D項（ブレーキ）が効いてオーバーシュートを防ぎながら収束していく様子」が手に取るようにわかります。
実際に外乱が加えられているときの動画とその時の姿勢グラフと制御しているモータトルクのグラフが以下になります。

7. まとめと次回予告

本日の成果

• PythonでPIDコントローラをフルスクラッチ実装しました。
• GUIスライダーによるリアルタイムゲイン調整機能を入れ、効率的にパラメータ探索を行いました。
• 難易度の高い 頂点倒立（3軸バランス） に成功しました。

次回予告 [Day 6]

PID制御は直感的で強力ですが、「3軸が干渉し合う複雑な姿勢」 では、軸ごとに独立したPID制御を行う限界が見えてきました。
（実際、Y軸だけゲインを2倍にするなどのその場しのぎ調整が必要でした）

明日は、数学の力で最適なゲインを全軸まとめて自動算出する 「LQR (線形2次レギュレータ) 制御」 に挑戦します。
状態方程式 $\dot{x} = Ax + Bu$ を導出し、現代制御理論の力でPIDを超える安定性を目指します。

---

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