開発版ドキュメント · 0.2.3 30c2f8ba · 入門例の検証対象 0.2.3 · 版情報 · 既知の制限

ケーススタディのフロー:

6自由度アクティブ防振装置のアクティブダンピング (非コロケート配置)#

このチュートリアルでは、gwexpy と python-control を組み合わせて、多自由度（MIMO）システムのシミュレーション、システム同定、そして制御系設計を行う一連の流れを実演します。

シナリオ: 3つの足で支持された正三角形の防振台（6自由度剛体）を想定します。 今回は、アクチュエータとセンサーの位置が異なる（非コロケート） 構成を扱います。

  • アクチュエータ: 3つの足の位置（支持点）に設置 (\(0^\circ, 120^\circ, 240^\circ\))

  • センサー: 足と足の中間地点に設置 (\(60^\circ, 180^\circ, 300^\circ\))

このように入出力位置がずれている場合、単純な各軸独立制御ではうまくいかないため、モード空間（Modal Space） での制御器設計を行います。

ステップ:

  1. 物理モデルの構築: センサー・アクチュエータ配置を考慮した状態空間モデルを作成します。

  2. MIMO伝達関数の測定: FrequencySeriesMatrix で非対角成分を含む伝達関数行列を可視化します。

  3. システム同定: 物理パラメータと幾何学配置から制御用モデルを定義します。

  4. ダンピング制御器の設計: センサー信号をモード座標に変換し、モードごとにダンピングをかける「モード制御」を設計します。

  5. 閉ループ検証: インパルス応答とASD比較により制振性能を確認します。

import warnings


import control
import matplotlib.pyplot as plt
import numpy as np
from scipy import signal

from gwexpy import TimeSeries, TimeSeriesDict
from gwexpy.frequencyseries import FrequencySeriesMatrix

1. 6自由度アクティブ防振装置のシミュレーション (Plant Model)#

剛体の運動方程式 \(M \ddot{q} + C \dot{q} + K q = F\) を定義します。 ここでは垂直3自由度（\(z, \theta_x, \theta_y\)）に焦点を当てます。

座標変換: モード座標 \(q = [z, \theta_x, \theta_y]^T\) に対し、

  • アクチュエータ物理座標 \(p_{act}\): \(0^\circ, 120^\circ, 240^\circ\)

  • センサー物理座標 \(p_{sen}\): \(60^\circ, 180^\circ, 300^\circ\)

それぞれの変換行列 \(T_{act}, T_{sen}\) を定義し、状態空間モデルに組み込みます。

# Physical parameters for the rigid-body modes we want to damp: one vertical translation and two tilts.
m = 100.0  # mass [kg]
I_x = 20.0  # moment of inertia [kg m^2]
I_y = 20.0

# Springs and dampers act at the leg locations, so their stiffness and loss enter through actuator-space geometry.
k_leg = 2000.0  # spring constant [N/m]
c_leg = 0.20  # damping coefficient [N s/m]

# Geometry sets how vertical motion mixes into pitch/roll; that lever arm is what creates modal cross-coupling.
R = 0.5  # [m]

# Define actuator and sensor placement separately so we can study the non-collocated case.
deg2rad = np.pi / 180.0
# Actuators push at the support legs.
angles_act = np.array([0, 120, 240]) * deg2rad
# Sensors are intentionally offset from the actuators, which is why a naive diagonal controller would mix modes.
angles_sen = np.array([60, 180, 300]) * deg2rad


# Build the geometry matrix that maps rigid-body modal coordinates into physical sensor/actuator displacements.
# Each row says how one leg/sensor displacement is produced by vertical motion plus pitch/roll about the platform center.
def make_transform_matrix(angles, radius):
    T = np.zeros((3, 3))
    for i, ang in enumerate(angles):
        T[i, 0] = 1.0
        T[i, 1] = radius * np.sin(ang)
        T[i, 2] = -radius * np.cos(ang)
    return T


T_act = make_transform_matrix(angles_act, R)
T_sen = make_transform_matrix(angles_sen, R)

print("Actuator Transform Matrix T_act:")
print(np.round(T_act, 2))
print("Sensor Transform Matrix T_sen:")
print(np.round(T_sen, 2))

# Mass/inertia stay diagonal in modal coordinates, which makes the physical rigid-body modes easy to interpret.
M_modal = np.diag([m, I_x, I_y])

# Stiffness and damping start in leg space because that is where the hardware is attached.
K_phys = np.diag([k_leg, k_leg, k_leg])
C_phys = np.diag([c_leg, c_leg, c_leg])

# Project stiffness/damping into modal space so translation and rotations can be controlled as separate physical modes.
# Use the actuator geometry here because the restoring forces are applied at the legs, not at the sensor locations.
K_modal = T_act.T @ K_phys @ T_act
C_modal = T_act.T @ C_phys @ T_act

# Build the plant in state space so open-loop and closed-loop motion can be compared with the same model.
# x = [q, q_dot]^T
A_sys = np.block(
    [
        [np.zeros((3, 3)), np.eye(3)],
        [-np.linalg.inv(M_modal) @ K_modal, -np.linalg.inv(M_modal) @ C_modal],
    ]
)
B_sys_modal = np.block([[np.zeros((3, 3))], [np.linalg.inv(M_modal)]])
C_sys_modal = np.block([np.eye(3), np.zeros((3, 3))])
D_sys = np.zeros((3, 3))

# Convert between modal and physical coordinates because hardware talks in leg forces and sensor displacements, not pure modes.
# Input: Actuator forces u at leg locations -> generalized modal force Q = T_act.T * u
# Output: Sensor displacements y at sensor locations -> y = T_sen * q
B_sys = B_sys_modal @ T_act.T
C_sys = T_sen @ C_sys_modal

sys = control.StateSpace(
    A_sys,
    B_sys,
    C_sys,
    D_sys,
    inputs=["ACT1", "ACT2", "ACT3"],
    outputs=["SEN1", "SEN2", "SEN3"],
)
print(sys)

Actuator Transform Matrix T_act:
[[ 1.    0.   -0.5 ]
 [ 1.    0.43  0.25]
 [ 1.   -0.43  0.25]]
Sensor Transform Matrix T_sen:
[[ 1.    0.43 -0.25]
 [ 1.    0.    0.5 ]
 [ 1.   -0.43 -0.25]]
<StateSpace>: sys[0]
Inputs (3): ['ACT1', 'ACT2', 'ACT3']
Outputs (3): ['SEN1', 'SEN2', 'SEN3']
States (6): ['x[0]', 'x[1]', 'x[2]', 'x[3]', 'x[4]', 'x[5]']

A = [[ 0.00000000e+00  0.00000000e+00  0.00000000e+00
       1.00000000e+00  0.00000000e+00  0.00000000e+00]
     [ 0.00000000e+00  0.00000000e+00  0.00000000e+00
       0.00000000e+00  1.00000000e+00  0.00000000e+00]
     [ 0.00000000e+00  0.00000000e+00  0.00000000e+00
       0.00000000e+00  0.00000000e+00  1.00000000e+00]
     [-6.00000000e+01 -3.33955086e-15 -2.16715534e-15
      -6.00000000e-03 -3.68259567e-19 -2.35922393e-19]
     [-1.70530257e-14 -3.75000000e+01  9.61481343e-15
      -2.08166817e-18 -3.75000000e-03  9.61481343e-19]
     [-1.13686838e-14  9.93438680e-15 -3.75000000e+01
      -1.04083409e-18  9.61481343e-19 -3.75000000e-03]]

B = [[ 0.          0.          0.        ]
     [ 0.          0.          0.        ]
     [ 0.          0.          0.        ]
     [ 0.01        0.01        0.01      ]
     [ 0.          0.02165064 -0.02165064]
     [-0.025       0.0125      0.0125    ]]

C = [[ 1.00000000e+00  4.33012702e-01 -2.50000000e-01
       0.00000000e+00  0.00000000e+00  0.00000000e+00]
     [ 1.00000000e+00  6.12323400e-17  5.00000000e-01
       0.00000000e+00  0.00000000e+00  0.00000000e+00]
     [ 1.00000000e+00 -4.33012702e-01 -2.50000000e-01
       0.00000000e+00  0.00000000e+00  0.00000000e+00]]

D = [[0. 0. 0.]
     [0. 0. 0.]
     [0. 0. 0.]]

2. MIMO伝達関数の測定#

センサーとアクチュエータの位置が異なるため、対角成分（ACT1->SEN1）だけでなく、非対角成分にも強い応答が現れるはずです。

fs = 64
duration = 1024.0
t = np.arange(0, duration, 1 / fs)
n_samples = len(t)

# Drive all three actuators with broadband excitation so every mode is visible in the measured MIMO transfer matrix.
np.random.seed(0)
u_data = np.random.normal(0, 1.0, (3, n_samples))

# Simulate the plant response to that broadband drive, mimicking a system-identification experiment.
time_response = control.forced_response(sys, T=t, U=u_data)

# Wrap channels as TimeSeriesDict so transfer estimation uses the same workflow as measured detector data.
tsd_input = TimeSeriesDict(
    {
        f"ACT{i + 1}": TimeSeries(
            u_data[i], t0=0, sample_rate=fs, name=f"Actuator {i + 1}", unit="N"
        )
        for i in range(3)
    }
)

tsd_output = TimeSeriesDict.from_control(time_response, unit="m")
for i in range(3):
    tsd_output[f"SEN{i + 1}"].name = f"Sensor {i + 1}"

# Estimate the full 3x3 transfer matrix to reveal how each actuator leaks into every sensor in the non-collocated layout.
tfs = [
    [
        tsd_input[f"ACT{j + 1}"].transfer_function(
            tsd_output[f"SEN{i + 1}"], fftlength=5
        )
        for j in range(3)
    ]
    for i in range(3)
        ]

tf_matrix = FrequencySeriesMatrix(
    [[tf.value for tf in row] for row in tfs], frequencies=tfs[0][0].frequencies
)

tf_data = tf_matrix.value
freqs = tf_matrix.frequencies

# Plot the matrix magnitude to see both the dominant resonant paths and the off-diagonal cross-couplings that modal control must suppress.
tf_matrix.abs().plot(figsize=(12, 10), xscale="log", yscale="log").suptitle(
    "MIMO Transfer Function Matrix (Non-collocated)\n"
)
plt.tight_layout()
plt.show()
../../_images/df15688c2a1a2ae028f26180ac2b45f4fcdb5b7e41305f1b379cb7ff24d47abd.png

5 & 6. 閉ループ検証 (インパルス応答 & ASD)#

構築したモード制御系を用いて、閉ループシミュレーションを行います。 アクチュエータ1へのインパルス外乱、および全軸への定常地面振動に対する応答を確認します。

# Close the loop and compare it with open loop to see whether modal damping reduces the resonant motion.
sys_cl = control.feedback(sys, K_mimo, sign=-1)

# --- Impulse response shows how quickly a localized actuator kick rings down with and without modal damping. ---
u_impulse = np.zeros((3, n_samples))
u_impulse[0, 100] = 100.0 * fs  # Impulse on actuator 1

resp_ol = control.forced_response(sys, T=t, U=u_impulse)
resp_cl = control.forced_response(sys_cl, T=t, U=u_impulse)

fig, axes = plt.subplots(3, 1, figsize=(10, 8), sharex=True)
for i in range(3):
    ax = axes[i]
    ax.plot(t, resp_ol.outputs[i], label="Open Loop", alpha=0.6)
    ax.plot(t, resp_cl.outputs[i], label="Closed Loop (Modal Control)", linewidth=2)
    ax.set_ylabel(f"SEN{i + 1}")
    ax.grid(True)
    if i == 0:
        ax.legend(loc="upper right")
axes[2].set_xlabel("Time [s]")
axes[0].set_title("Impulse Response Comparison (Impulse on ACT1)")
plt.show()

# --- ASD comparison asks whether the controller lowers the disturbance-driven motion across the resonance band. ---
# Use low-frequency ground-like disturbance so the rigid-body resonance is excited in the same band the controller is meant to suppress.
np.random.seed(42)
wn = np.random.normal(0, 1.0, (3, n_samples))
b, a = signal.butter(1, 5.0, fs=fs, btype="low")
dist = signal.lfilter(b, a, wn) * 50.0

resp_ol_noise = control.forced_response(sys, T=t, U=dist)
resp_cl_noise = control.forced_response(sys_cl, T=t, U=dist)

tsd_ol = TimeSeriesDict.from_control(resp_ol_noise)
for i in range(3):
    tsd_ol[list(tsd_ol.keys())[i]].name = f"SEN{i + 1}"
tsd_ol = TimeSeriesDict({ts.name: ts for ts in tsd_ol.values()})
tsd_cl = TimeSeriesDict.from_control(resp_cl_noise)
for i in range(3):
    tsd_cl[list(tsd_cl.keys())[i]].name = f"SEN{i + 1}"
tsd_cl = TimeSeriesDict({ts.name: ts for ts in tsd_cl.values()})

fig, axes = plt.subplots(1, 3, figsize=(18, 5), sharey=True)
for i in range(3):
    ax = axes[i]
    asd_ol = (
        tsd_ol[f"SEN{i + 1}"].asd(fftlength=16, overlap=8, method="welch").crop(0.1, 10)
    )
    asd_cl = (
        tsd_cl[f"SEN{i + 1}"].asd(fftlength=16, overlap=8, method="welch").crop(0.1, 10)
    )

    ax.loglog(asd_ol, label="Open Loop", alpha=0.8)
    ax.loglog(asd_cl, label="Closed Loop", linewidth=2)
    ax.set_title(f"Sensor {i + 1} ASD")
    ax.set_xlabel("Frequency [Hz]")
    ax.grid(True, which="both", alpha=0.5)
    if i == 0:
        ax.set_ylabel(r"ASD [$\mathrm{m}/\sqrt{\mathrm{Hz}}$]")
        ax.legend()
plt.tight_layout()
plt.show()

# Even with non-collocated sensors and actuators, the modal transforms let us damp the physical rigid-body modes instead of fighting geometry-induced cross-coupling directly in sensor space.
../../_images/3cb375f6741bcc30a7f40fa5e2782c355803692b09e73a030e8b1f71d4dd206d.png ../../_images/7f855bd25a24c1537a7ca0841a22b0a0ee1d08bfe5caf694964cadc8c6ce6574.png