解析力学と制御工学

---

【第1章：力学系の記述と制御の基礎】

節	内容
1.1	ニュートン力学・質点系・剛体運動の基礎（制御対象の物理モデル）
1.2	解析力学の視点：ラグランジュの定式化と制約系の記述
1.3	システム制御工学の概要：制御対象・センサ・アクチュエータ・フィードバック
1.4	制御対象の運動方程式と状態空間モデルの導出（1次・2次遅れ系）

import numpy as np
import matplotlib.pyplot as plt

# -------------------- Parameters --------------------
t = np.linspace(0, 10, 500)
m = 1.0                 # 質量 [kg]
k = 1.0                 # ばね定数 [N/m]
b = 0.5                 # 減衰係数 [Ns/m]
F = 1.0                 # 入力（外力） [N]
tau = 2.0               # 一次遅れの時定数
K = 1.0                 # ゲイン

# -------------------- 1.1 ニュートン力学（質点＋ばね＋ダンパ） --------------------
# 2次遅れ系: m*x'' + b*x' + k*x = F
# 状態空間モデル（解析解の代わりに数値解）
omega_n = np.sqrt(k / m)
zeta = b / (2 * np.sqrt(k * m))

x1 = F / k * (1 - np.exp(-zeta * omega_n * t) * (
    np.cos(omega_n * np.sqrt(1 - zeta**2) * t) +
    (zeta / np.sqrt(1 - zeta**2)) * np.sin(omega_n * np.sqrt(1 - zeta**2) * t)
))

# -------------------- 1.2 ラグランジュ定式化と同じ結果 --------------------
# L = T - V = (1/2)m*x'^2 - (1/2)k*x^2 → Euler-Lagrange equation → 同じ方程式

# -------------------- 1.3 一次遅れ系（1st-order lag） --------------------
# tau * y' + y = Ku
y_lag = K * F * (1 - np.exp(-t / tau))

# -------------------- Plot --------------------
fig, ax = plt.subplots(1, 2, figsize=(14, 5))

# 1.1 & 1.2: 二階系（ばね-ダンパ系）
ax[0].plot(t, x1, label="Mass-Spring-Damper (2nd order)")
ax[0].set_title("1.1 / 1.2: Newtonian & Lagrangian Mechanics")
ax[0].set_xlabel("Time [s]")
ax[0].set_ylabel("Displacement x(t) [m]")
ax[0].legend()
ax[0].grid(True)

# 1.3: 一次遅れ系
ax[1].plot(t, y_lag, label="1st-order Lag System", color='orange')
ax[1].set_title("1.3: First-order Lag Control System")
ax[1].set_xlabel("Time [s]")
ax[1].set_ylabel("Output y(t)")
ax[1].legend()
ax[1].grid(True)

plt.tight_layout()
plt.show()

---

【第2章：ラグランジアン系と状態空間】

節	内容
2.1	ラグランジアンの定義とオイラー＝ラグランジュ方程式の導出
2.2	状態変数としての $q, \dot{q}$ と制御可能性
2.3	電気回路・バネ質量ダンパ系のラグランジュ定式化
2.4	1自由度・多自由度系の状態空間表現との一致と変換

import numpy as np
import matplotlib.pyplot as plt

# -------------------- 時間変数とパラメータ --------------------
t = np.linspace(0, 10, 500)
m = 1.0     # mass [kg]
k = 2.0     # spring constant [N/m]
b = 0.5     # damping coefficient [Ns/m]
F = 1.0     # external force [N]

# -------------------- 2.1: ラグランジアン定義と導出（理論） --------------------
# L = T - V = (1/2)*m*q_dot^2 - (1/2)*k*q^2
# → Euler-Lagrange: d/dt(∂L/∂q_dot) - ∂L/∂q = -b*q_dot + F (非保存項加えると制御可能)

# -------------------- 2.2: 状態変数としての q, q_dot --------------------
# 定義： x1 = q, x2 = q_dot とすると状態空間系：
# dx1/dt = x2
# dx2/dt = (1/m)(-k*x1 - b*x2 + F)

# -------------------- 2.3: バネ質量ダンパ系の数値解 --------------------
omega_n = np.sqrt(k / m)
zeta = b / (2 * np.sqrt(k * m))
omega_d = omega_n * np.sqrt(1 - zeta**2)

# 応答（ゼロ初期条件、定常応答まで含む）
x1 = F / k * (1 - np.exp(-zeta * omega_n * t) * (
    np.cos(omega_d * t) + (zeta / np.sqrt(1 - zeta**2)) * np.sin(omega_d * t)
))
x2 = np.gradient(x1, t)  # x1の時間微分（速度）

# -------------------- 2.4: 状態空間ベクトルと一致 --------------------
# x = [x1, x2] として、状態空間の軌道描写

# -------------------- プロット --------------------
fig, axes = plt.subplots(1, 3, figsize=(18, 5))

# x1: 位置（q）
axes[0].plot(t, x1, label="Position $q(t)$")
axes[0].set_title("2.1–2.3: Position from Lagrangian")
axes[0].set_xlabel("Time [s]")
axes[0].set_ylabel("q(t)")
axes[0].legend()
axes[0].grid(True)

# x2: 速度（q_dot）
axes[1].plot(t, x2, label="Velocity $\dot{q}(t)$", color="orange")
axes[1].set_title("2.2: Generalized Velocity")
axes[1].set_xlabel("Time [s]")
axes[1].set_ylabel("dq/dt")
axes[1].legend()
axes[1].grid(True)

# 状態空間軌道（q vs q_dot）
axes[2].plot(x1, x2, label="Phase Space (q, dq/dt)", color="green")
axes[2].set_title("2.4: State-Space Orbit")
axes[2].set_xlabel("q(t)")
axes[2].set_ylabel("dq/dt")
axes[2].legend()
axes[2].grid(True)

plt.tight_layout()
plt.show()

---

【第3章：ハミルトン形式とエネルギー制御】

節	内容
3.1	ハミルトニアンの定義と正準方程式
3.2	エネルギー保存系と非保存系（制御系）との関係
3.3	シンプレクティック構造と制御系の位相空間解析
3.4	ハミルトン構造に基づく力学系の可観測性と安定性解析

import numpy as np
import matplotlib.pyplot as plt
from scipy.integrate import solve_ivp
from scipy.linalg import solve_continuous_are, eigvals

# -------------------- 基本パラメータ --------------------
m = 1.0
k = 1.0
b = 0.5
F = 1.0
A = np.array([[0, 1/m], [-k, 0]])
B = np.array([[0], [1]])
C = np.array([[1, 0]])
Q = np.eye(2)
R = np.array([[1]])

# -------------------- Hamiltonian 系（保存系） --------------------
def hamiltonian_dynamics(t, z):
    q, p = z
    dqdt = p / m
    dpdt = -k * q
    return [dqdt, dpdt]

z0 = [1.0, 0.0]
t_eval = np.linspace(0, 10, 500)
sol_H = solve_ivp(hamiltonian_dynamics, [0, 10], z0, t_eval=t_eval)
q_H, p_H = sol_H.y
H_vals = p_H**2 / (2 * m) + 0.5 * k * q_H**2

# -------------------- LQR 最適制御系 --------------------
P = solve_continuous_are(A, B, Q, R)
K = np.linalg.inv(R) @ B.T @ P

def lqr_dynamics(t, x):
    u = -K @ x
    dxdt = A @ x + B @ u
    return dxdt.flatten()

x0 = np.array([1.0, 0.0])
sol_LQR = solve_ivp(lqr_dynamics, [0, 10], x0, t_eval=t_eval)
x1_LQR, x2_LQR = sol_LQR.y
u_LQR = -K @ sol_LQR.y

# -------------------- 可観測性と安定性解析 --------------------
O = np.vstack([C, C @ A])
rank_O = np.linalg.matrix_rank(O)
eigenvals_A = eigvals(A)

# -------------------- 合体プロット --------------------
fig, axes = plt.subplots(2, 3, figsize=(18, 10))

# 1. ハミルトン系の時間発展
axes[0, 0].plot(t_eval, q_H, label="q(t)")
axes[0, 0].plot(t_eval, p_H, label="p(t)")
axes[0, 0].set_title("Hamiltonian Dynamics (q, p)")
axes[0, 0].set_xlabel("Time [s]")
axes[0, 0].set_ylabel("State")
axes[0, 0].legend()
axes[0, 0].grid(True)

# 2. エネルギー保存
axes[0, 1].plot(t_eval, H_vals, label="H(t)", color="orange")
axes[0, 1].set_title("Hamiltonian Energy")
axes[0, 1].set_xlabel("Time [s]")
axes[0, 1].set_ylabel("Energy")
axes[0, 1].legend()
axes[0, 1].grid(True)

# 3. 位相空間（q, p）
axes[0, 2].plot(q_H, p_H, label="Phase Space", color="green")
axes[0, 2].set_title("Hamiltonian Phase Space")
axes[0, 2].set_xlabel("q")
axes[0, 2].set_ylabel("p")
axes[0, 2].legend()
axes[0, 2].grid(True)

# 4. LQR 軌道
axes[1, 0].plot(t_eval, x1_LQR, label="x1(t)")
axes[1, 0].plot(t_eval, x2_LQR, label="x2(t)")
axes[1, 0].set_title("LQR Optimal State Trajectory")
axes[1, 0].set_xlabel("Time [s]")
axes[1, 0].set_ylabel("State")
axes[1, 0].legend()
axes[1, 0].grid(True)

# 5. LQR 制御入力
axes[1, 1].plot(t_eval, u_LQR[0], label="u(t)", color="purple")
axes[1, 1].set_title("LQR Control Input")
axes[1, 1].set_xlabel("Time [s]")
axes[1, 1].set_ylabel("Control")
axes[1, 1].legend()
axes[1, 1].grid(True)

# 6. 固有値解析（安定性と可観測性）
axes[1, 2].axhline(0, color='gray', linestyle='--')
axes[1, 2].plot(np.real(eigenvals_A), np.imag(eigenvals_A), 'ro', label='Eigenvalues')
axes[1, 2].set_title(f"Eigenvalue Analysis\nObservability Rank = {rank_O}")
axes[1, 2].set_xlabel("Real Part")
axes[1, 2].set_ylabel("Imaginary Part")
axes[1, 2].legend()
axes[1, 2].grid(True)

plt.tight_layout()
plt.show()

---

【第4章：変分原理と最適制御】

節	内容
4.1	変分法による制御問題の定式化（最小作用・最小エネルギー）
4.2	ポンテャギンの最大原理とハミルトン＝ヤコビ＝ベルマン（HJB）方程式
4.3	LQR（線形二次レギュレータ）と作用汎関数の関係
4.4	スタートアップ時間最小・燃料最小の最適制御軌道の解析例

# 修正: np.gradient の第2引数 t_eval は1次元の長さが一致する必要があるため省略し、デフォルト間隔で使用

import numpy as np
import matplotlib.pyplot as plt
from scipy.integrate import solve_ivp
from scipy.linalg import solve_continuous_are

# -------------------- 基本パラメータ --------------------
A = np.array([[0, 1], [0, 0]])
B = np.array([[0], [1]])
Q = np.eye(2)
R = np.array([[1]])
x0 = np.array([1.0, 0.0])
t_eval = np.linspace(0, 10, 500)

# -------------------- 4.1: LQR最適制御問題（最小エネルギー） --------------------
P = solve_continuous_are(A, B, Q, R)
K = np.linalg.inv(R) @ B.T @ P

def lqr_dynamics(t, x):
    u = -K @ x
    return (A @ x + B @ u).flatten()

sol_lqr = solve_ivp(lqr_dynamics, [0, 10], x0, t_eval=t_eval)
x1_lqr, x2_lqr = sol_lqr.y
u_lqr = -K @ sol_lqr.y

# -------------------- 4.2: HJBに対応する値関数とその時間微分 --------------------
V = np.einsum('ij,jk,ik->i', sol_lqr.y.T, P, sol_lqr.y.T)  # V(x) = xᵀPx
Vdot = np.gradient(V)  # 修正済み

# -------------------- 4.4: Bang-bang制御（最短時間） --------------------
umax = 1.0

def bang_bang_dynamics(t, x):
    u = -umax if x[0] > 0 else umax
    return (A @ x + B.flatten() * u)

sol_bb = solve_ivp(bang_bang_dynamics, [0, 10], x0, t_eval=t_eval)
x1_bb, x2_bb = sol_bb.y
u_bb = np.where(x1_bb > 0, -umax, umax)

# -------------------- プロット --------------------
fig, axes = plt.subplots(2, 3, figsize=(18, 10))

axes[0, 0].plot(t_eval, x1_lqr, label="x1(t)")
axes[0, 0].plot(t_eval, x2_lqr, label="x2(t)")
axes[0, 0].set_title("4.1 LQR: Minimum Energy Trajectory")
axes[0, 0].set_xlabel("Time [s]")
axes[0, 0].legend()
axes[0, 0].grid(True)

axes[0, 1].plot(t_eval, u_lqr[0], label="u(t)", color='orange')
axes[0, 1].set_title("4.1 LQR: Optimal Control Input")
axes[0, 1].set_xlabel("Time [s]")
axes[0, 1].legend()
axes[0, 1].grid(True)

axes[0, 2].plot(t_eval, V, label="Value Function V(x)", color='green')
axes[0, 2].plot(t_eval, Vdot, label="dV/dt", linestyle='--', color='red')
axes[0, 2].set_title("4.2 HJB: Value Function & dV/dt")
axes[0, 2].set_xlabel("Time [s]")
axes[0, 2].legend()
axes[0, 2].grid(True)

axes[1, 0].plot(t_eval, x1_bb, label="x1(t)")
axes[1, 0].plot(t_eval, x2_bb, label="x2(t)")
axes[1, 0].set_title("4.4 Bang-Bang: Min-Time Trajectory")
axes[1, 0].set_xlabel("Time [s]")
axes[1, 0].legend()
axes[1, 0].grid(True)

axes[1, 1].plot(t_eval, u_bb, label="u(t)", color='purple')
axes[1, 1].set_title("4.4 Bang-Bang: Control Input")
axes[1, 1].set_xlabel("Time [s]")
axes[1, 1].legend()
axes[1, 1].grid(True)

axes[1, 2].plot(x1_lqr, x2_lqr, label="LQR", color='blue')
axes[1, 2].plot(x1_bb, x2_bb, label="Bang-Bang", color='red', linestyle='--')
axes[1, 2].set_title("4.3–4.4: Phase Space Comparison")
axes[1, 2].set_xlabel("x1")
axes[1, 2].set_ylabel("x2")
axes[1, 2].legend()
axes[1, 2].grid(True)

plt.tight_layout()
plt.show()

---

【第5章：制御系の安定性と力学的視点】

節	内容
5.1	Lyapunov関数とラグランジアンとの関係
5.2	受動性（Passivity）とエネルギー関数による安定性
5.3	ハミルトン系の安定解析とベクトル場の幾何構造
5.4	モデル予測制御（MPC）におけるラグランジュ乗数の意味

import numpy as np
import matplotlib.pyplot as plt
from scipy.integrate import solve_ivp

# -------------------- 5.1 Lyapunov関数とラグランジアン --------------------
# 系: dq/dt = p/m, dp/dt = -kq（保存系）
m = 1.0
k = 1.0

def hamiltonian_dynamics(t, z):
    q, p = z
    dqdt = p / m
    dpdt = -k * q
    return [dqdt, dpdt]

# Lyapunov関数としてのエネルギー（L = T - V）
def energy_function(q, p):
    return p**2 / (2 * m) + 0.5 * k * q**2

# -------------------- 5.2 Passivity 系（減衰あり） --------------------
# dq/dt = p/m, dp/dt = -kq - b*p（非保存系）
b = 0.2

def passive_dynamics(t, z):
    q, p = z
    dqdt = p / m
    dpdt = -k * q - b * p
    return [dqdt, dpdt]

# -------------------- 5.3 ハミルトンベクトル場と位相軌道 --------------------
z0 = [1.0, 0.0]
t_eval = np.linspace(0, 20, 800)

sol_conservative = solve_ivp(hamiltonian_dynamics, [0, 20], z0, t_eval=t_eval)
sol_dissipative = solve_ivp(passive_dynamics, [0, 20], z0, t_eval=t_eval)

q_cons, p_cons = sol_conservative.y
q_diss, p_diss = sol_dissipative.y

H_cons = energy_function(q_cons, p_cons)
H_diss = energy_function(q_diss, p_diss)

# -------------------- 5.4 MPCとラグランジュ乗数の直感（図示） --------------------
# 最適解が制約に張り付く場合のラグランジュ乗数の意味（速度制約 u in [-1, 1]）

u_max = 1.0
def mpc_dynamics(t, x):
    u = -u_max if x[0] > 0 else u_max
    return [x[1], u]

x0 = [1.0, 0.0]
sol_mpc = solve_ivp(mpc_dynamics, [0, 10], x0, t_eval=np.linspace(0, 10, 500))
x1_mpc, x2_mpc = sol_mpc.y
u_mpc = np.where(x1_mpc > 0, -u_max, u_max)
lagrange_multiplier = np.abs(u_mpc)  # 速度制約が有効な区間では乗数≠0

# -------------------- Plot --------------------
fig, axes = plt.subplots(2, 3, figsize=(18, 10))

# Conserved vs Dissipated Energy
axes[0, 0].plot(t_eval, H_cons, label="Conservative")
axes[0, 0].plot(t_eval, H_diss, label="Dissipative", linestyle='--')
axes[0, 0].set_title("5.1–5.2: Energy as Lyapunov / Passivity")
axes[0, 0].set_xlabel("Time [s]")
axes[0, 0].set_ylabel("Energy")
axes[0, 0].legend()
axes[0, 0].grid(True)

# Phase Space Comparison
axes[0, 1].plot(q_cons, p_cons, label="Conservative")
axes[0, 1].plot(q_diss, p_diss, label="Dissipative", linestyle='--')
axes[0, 1].set_title("5.3: Phase Space Structure")
axes[0, 1].set_xlabel("q")
axes[0, 1].set_ylabel("p")
axes[0, 1].legend()
axes[0, 1].grid(True)

# Vector Field of Conservative System
Q, P = np.meshgrid(np.linspace(-2, 2, 20), np.linspace(-2, 2, 20))
dQ = P / m
dP = -k * Q
axes[0, 2].quiver(Q, P, dQ, dP)
axes[0, 2].set_title("5.3: Hamiltonian Vector Field")
axes[0, 2].set_xlabel("q")
axes[0, 2].set_ylabel("p")
axes[0, 2].grid(True)

# MPC States
axes[1, 0].plot(sol_mpc.t, x1_mpc, label="x1(t)")
axes[1, 0].plot(sol_mpc.t, x2_mpc, label="x2(t)")
axes[1, 0].set_title("5.4 MPC: State Trajectory")
axes[1, 0].set_xlabel("Time [s]")
axes[1, 0].legend()
axes[1, 0].grid(True)

# MPC Control Input
axes[1, 1].plot(sol_mpc.t, u_mpc, label="u(t)", color='purple')
axes[1, 1].set_title("5.4 MPC: Control Input")
axes[1, 1].set_xlabel("Time [s]")
axes[1, 1].legend()
axes[1, 1].grid(True)

# Lagrange Multiplier Intuition
axes[1, 2].plot(sol_mpc.t, lagrange_multiplier, label="|λ(t)| (Active Constraint)", color='green')
axes[1, 2].set_title("5.4: Lagrange Multiplier Activity")
axes[1, 2].set_xlabel("Time [s]")
axes[1, 2].legend()
axes[1, 2].grid(True)

plt.tight_layout()
plt.show()