# 用最佳化自動調 PID：受控體同 PID 章的 G(s) = 1/(s+1)³，目標是讓單位步階響應的 ITAE = ∫ t|e(t)| dt 最小 # 比較 Ziegler–Nichols、隨機搜尋、CMA-ES、貝氏最佳化；每個方法跑 5 個亂數種子。每「評估」一次 = 做一次閉迴路步階實驗 import json import math from pathlib import Path import numpy as np from scipy.linalg import expm from scipy.stats import norm # ---------- 受控體與數位 PID ---------- DT, T_END, U_MAX, TF = 0.01, 20.0, 10.0, 0.05 A = np.array([[0, 1, 0], [0, 0, 1], [-1, -3, -3]], float) # 1/(s+1)³ 的可控標準式 M = expm(np.block([[A, np.array([[0], [0], [1]])], [np.zeros((1, 4))]]) * DT) AD, BD = M[:3, :3], M[:3, 3] # 零階保持離散化 def pid_sim(kp, ki, kd): """取樣 0.01 s；微分作用在量測值（一階濾波 0.05 s）；|u| ≤ 10，飽和時停止積分""" n = int(round(T_END / DT)); x, I, D, y_old = np.zeros(3), 0.0, 0.0, 0.0 ys = np.empty(n + 1) for k in range(n + 1): y = x[0]; e = 1.0 - y D = (TF * D - (y - y_old)) / (TF + DT) raw = kp * e + ki * (I + e * DT) + kd * D u = min(max(raw, -U_MAX), U_MAX) if u == raw: I += e * DT ys[k], y_old = y, y x = AD @ x + BD * u return np.arange(n + 1) * DT, ys def itae(kp, ki, kd): t, y = pid_sim(kp, ki, kd) return float(np.sum(t * np.abs(1.0 - y)) * DT) LO, HI = np.array([-1.0, -2.0, -2.0]), np.array([1.5, 1.0, 1.0]) # log10 Kp、Ki、Kd 的範圍 gains = lambda z: tuple(10 ** (LO + (HI - LO) * np.asarray(z, float))) # 正規化座標 [0, 1]³ → 增益 cost = lambda z: itae(*gains(z)) # ---------- CMA-ES（同 cmaes_benchmark.py）---------- class CMAES: def __init__(self, x0, sigma0, rng): n = self.n = len(x0) self.lam = 4 + int(3 * math.log(n)); self.mu = self.lam // 2 w = np.array([math.log(self.mu + 0.5) - math.log(i + 1) for i in range(self.mu)]) self.w = w / w.sum(); me = self.mueff = 1 / np.sum(self.w ** 2) self.cs = (me + 2) / (n + me + 5); self.ds = 1 + 2 * max(0.0, math.sqrt((me - 1) / (n + 1)) - 1) + self.cs self.cc = (4 + me / n) / (n + 4 + 2 * me / n); self.c1 = 2 / ((n + 1.3) ** 2 + me) self.cmu = min(1 - self.c1, 2 * (me - 2 + 1 / me) / ((n + 2) ** 2 + me)) self.chin = math.sqrt(n) * (1 - 1 / (4 * n) + 1 / (21 * n * n)) self.m, self.sigma, self.rng = np.array(x0, float), sigma0, rng self.C, self.ps, self.pc, self.g, self.D, self.B = np.eye(n), np.zeros(n), np.zeros(n), 0, np.ones(n), np.eye(n) def ask(self): Z = np.array([[float(self.rng.normal()) for _ in range(self.n)] for _ in range(self.lam)]) self.Y = Z @ np.diag(self.D) @ self.B.T return self.m + self.sigma * self.Y def tell(self, fits): n, ys = self.n, self.Y[np.argsort(fits, kind="stable")[:self.mu]] yw = self.w @ ys; self.m = self.m + self.sigma * yw self.ps = (1 - self.cs) * self.ps + math.sqrt(self.cs * (2 - self.cs) * self.mueff) * (self.B @ ((self.B.T @ yw) / self.D)) self.g += 1 hs = float(np.linalg.norm(self.ps) / math.sqrt(1 - (1 - self.cs) ** (2 * self.g)) < (1.4 + 2 / (n + 1)) * self.chin) self.pc = (1 - self.cc) * self.pc + hs * math.sqrt(self.cc * (2 - self.cc) * self.mueff) * yw self.C = ((1 - self.c1 - self.cmu) * self.C + self.c1 * (np.outer(self.pc, self.pc) + (1 - hs) * self.cc * (2 - self.cc) * self.C) + self.cmu * sum(wi * np.outer(y, y) for wi, y in zip(self.w, ys))) self.C = (self.C + self.C.T) / 2 self.sigma *= math.exp((self.cs / self.ds) * (np.linalg.norm(self.ps) / self.chin - 1)) d2, self.B = np.linalg.eigh(self.C); self.D = np.sqrt(np.maximum(d2, 1e-300)) def run_cma(budget, seed): """從範圍中央出發；超出 [0, 1] 的樣本在邊界上評估並加懲罰""" rng = np.random.default_rng(100 + seed); es = CMAES(np.full(3, 0.5), 0.3, rng); F, XS = [], [] while len(F) + es.lam <= budget: X = es.ask(); Xc = np.clip(X, 0, 1) fits = [cost(xc) for xc in Xc]; F += fits; XS += list(Xc) es.tell(np.array([f + 1e3 * float(np.sum((x - xc) ** 2)) for f, x, xc in zip(fits, X, Xc)])) return F, XS # ---------- 貝氏最佳化：高斯過程（Matérn 5/2）＋期望改善量 ---------- def matern52(P, Q, ell): d = np.sqrt(np.maximum(((P[:, None, :] - Q[None, :, :]) ** 2).sum(-1), 0.0)) / ell return (1 + math.sqrt(5) * d + 5 * d * d / 3) * np.exp(-math.sqrt(5) * d) def gp_fit(X, y, noise=1e-6): """y 標準化；長度尺度 ℓ 取邊際概似最大的格點值""" mu, sd = y.mean(), y.std() or 1.0; t = (y - mu) / sd; best = None for ell in (0.05, 0.08, 0.12, 0.18, 0.25, 0.35, 0.5, 0.7, 1.0): Lc = np.linalg.cholesky(matern52(X, X, ell) + noise * np.eye(len(X))) a = np.linalg.solve(Lc.T, np.linalg.solve(Lc, t)) ll = -0.5 * t @ a - np.log(np.diag(Lc)).sum() if best is None or ll > best[0]: best = (ll, ell, Lc, a) return X, mu, sd, best[1], best[2], best[3] def gp_predict(g, P): X, mu, sd, ell, Lc, a = g Ks = matern52(P, X, ell); v = np.linalg.solve(Lc, Ks.T) return mu + sd * (Ks @ a), sd * np.sqrt(np.maximum(1.0 - (v * v).sum(0), 1e-12)) def ei(m, s, best, xi=0.01): z = (best - xi - m) / s return (best - xi - m) * norm.cdf(z) + s * norm.pdf(z) def run_bo(budget, seed, n_init=5): """先隨機 5 點，之後每次：對 log(ITAE) 建 GP → 找 EI 最大的點 → 做一次實驗""" rng = np.random.default_rng(seed) X = list(rng.random((n_init, 3))); F = [cost(x) for x in X] while len(X) < budget: y = np.log(F); g = gp_fit(np.array(X), y) C = rng.random((4000, 3)); m, s = gp_predict(g, C); e = ei(m, s, y.min()) L = np.clip(np.repeat(C[np.argsort(-e)[:5]], 200, 0) + rng.normal(0, 0.03, (1000, 3)), 0, 1) m2, s2 = gp_predict(g, L); e2 = ei(m2, s2, y.min()) x = L[int(np.argmax(e2))] if e2.max() > e.max() else C[int(np.argmax(e))] X.append(x); F.append(cost(x)) return F, X # ---------- 比較 ---------- def info(g): t, y = pid_sim(*g) out = np.where(np.abs(y - 1) > 0.02)[0] return max(0.0, (y.max() - 1) * 100), t[out[-1] + 1] Tu = 2 * math.pi / math.sqrt(3); zn = (4.8, 4.8 / (Tu / 2), 4.8 * Tu / 8) # Ziegler–Nichols 臨界增益法（Ku = 8） os_, ts = info(zn) print(f"Ziegler–Nichols：Kp={zn[0]:.2f}, Ki={zn[1]:.2f}, Kd={zn[2]:.2f}，ITAE {itae(*zn):.2f}，超越 {os_:.1f}%，安定時間 {ts:.2f} s") def run_random(budget, seed): X = list(np.random.default_rng(50 + seed).random((budget, 3))) return [cost(x) for x in X], X methods = {"隨機搜尋 30 次": lambda s: run_random(30, s), "CMA-ES 28 次": lambda s: run_cma(30, s), # CMA-ES 每代 7 個，4 代 = 28 次 "貝氏最佳化 30 次": lambda s: run_bo(30, s), "CMA-ES 294 次": lambda s: run_cma(300, s)} out = {"zn": zn} for name, run in methods.items(): runs = [run(s) for s in range(5)] bests = [min(F) for F, _ in runs] print(f"{name:10s}：最好的 ITAE 中位數 {np.median(bests):.3f}（5 個種子 {min(bests):.3f}～{max(bests):.3f}）") out[name] = {"best": bests, "curves": [np.minimum.accumulate(F).tolist() for F, _ in runs], "gains": [gains(X[int(np.argmin(F))]) for F, X in runs]} for name in ("貝氏最佳化 30 次", "CMA-ES 294 次"): # 中位數那個種子的結果 k = int(np.argsort(out[name]["best"])[2]); g = out[name]["gains"][k] os_, ts = info(g) print(f"{name}（中位數的種子）：Kp={g[0]:.2f}, Ki={g[1]:.2f}, Kd={g[2]:.2f}，ITAE {itae(*g):.3f}，超越 {os_:.1f}%，安定時間 {ts:.2f} s") Path(__file__).with_name("pid_tuning.json").write_text(json.dumps(out, ensure_ascii=False), encoding="utf-8")