File size: 5,559 Bytes
573eba1 | 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 | """Next Balance: hold a vertical lift platform at its target while the payload and friction change.
Simulation only. Plant (semi-implicit Euler, dt = 0.01 s):
m(t) * (z'' + g) = u(t - delay) - c(t) * z' - F(t) * tanh(z' / 0.01)
Sensor: z + Gaussian noise. Actuator force is limited to [0, U_MAX].
Both controllers use the same alpha-beta state estimator and know the nominal payload only.
fixed PID : u = Kp e + Ki I - Kd v + m_nominal g
adaptive : recursive least squares (forgetting factor) estimates [m, c, F] from filtered
signals; u = m_hat (wn^2 e - 2 zeta wn v + ki I + g) + c_hat v
(F_hat is estimated to keep m_hat unbiased but not fed back: it chatters with noisy v)
The same code is ported line by line to web/core.js; parity is tested on fixed scenarios.
"""
import math
from nlrt import Mulberry32
DT, G, U_MAX, M_PLATFORM, EPS = 0.01, 9.81, 400.0, 2.0, 0.01
ALPHA, BETA = 0.3, 0.05
Z_REF, T_STEP, T_RAMP = 0.5, 0.5, 1.0
def scenario(seed, stress=False):
r = Mulberry32(seed)
s = {"payload0": r.uniform(1, 4), "t_load": r.uniform(3.5, 4.5),
"dm": r.uniform(-1, 10 if stress else 6), "c0": r.uniform(2, 5), "F0": r.uniform(1, 3),
"t_fric": r.uniform(6.5, 7.5), "fric_gain": r.uniform(1, 6 if stress else 4),
"delay": r.randint(8 if stress else 6), "noise": r.uniform(0, 0.006 if stress else 0.003),
"T": 12.0, "seed": seed}
return s
def simulate(sc, ctrl, params, record_every=1):
rng = Mulberry32(sc["seed"] * 31 + 7)
m_nom = M_PLATFORM + sc["payload0"]
n = int(round(sc["T"] / DT))
z = v = 0.0
zh = vh = 0.0
integ = 0.0
queue = [m_nom * G] * (sc["delay"] + 1) # commands in flight; start at hover force
# adaptive state: RLS + state-variable filters
theta = [m_nom, 3.0, 2.0]
P = [[10.0, 0, 0], [0, 10.0, 0], [0, 0, 10.0]]
fz = [0.0, 0.0]
fu = [m_nom * G, 0.0]
ft = [0.0, 0.0]
w = params.get("wf", 20.0)
lam = params.get("lam", 0.995)
t_hist, z_hist, u_hist, m_hist = [], [], [], []
energy = 0.0
for k in range(n):
t = k * DT
m = M_PLATFORM + sc["payload0"] + (sc["dm"] if t >= sc["t_load"] else 0.0)
g_f = sc["fric_gain"] if t >= sc["t_fric"] else 1.0
c, F = sc["c0"] * g_f, sc["F0"] * g_f
ref = Z_REF * min(1.0, max(0.0, (t - T_STEP) / T_RAMP))
# sensor and estimator
meas = z + sc["noise"] * rng.normal()
zp = zh + vh * DT
res = meas - zp
zh = zp + ALPHA * res
vh = vh + (BETA / DT) * res
e = ref - zh
integ = max(-params.get("imax", 0.5), min(params.get("imax", 0.5), integ + e * DT))
if ctrl == "pid":
u = params["kp"] * e + params["ki"] * integ - params["kd"] * vh + m_nom * G
else:
u_applied = queue[0]
tv = math.tanh(vh / EPS)
# state-variable filters H(s) = w^2 / (s + w)^2 on measured z, applied force and friction regressor
az = w * w * (meas - fz[0]) - 2 * w * fz[1]
au = w * w * (u_applied - fu[0]) - 2 * w * fu[1]
at = w * w * (tv - ft[0]) - 2 * w * ft[1]
fz = [fz[0] + DT * fz[1], fz[1] + DT * az]
fu = [fu[0] + DT * fu[1], fu[1] + DT * au]
ft = [ft[0] + DT * ft[1], ft[1] + DT * at]
phi = [az + G, fz[1], ft[0]]
y = fu[0]
Pphi = [sum(P[i][j] * phi[j] for j in range(3)) for i in range(3)]
den = lam + sum(phi[i] * Pphi[i] for i in range(3))
K = [Pphi[i] / den for i in range(3)]
err = y - sum(phi[i] * theta[i] for i in range(3))
theta = [theta[i] + K[i] * err for i in range(3)]
P = [[(P[i][j] - K[i] * Pphi[j]) / lam for j in range(3)] for i in range(3)]
theta = [min(30.0, max(0.5, theta[0])), min(60.0, max(0.0, theta[1])), min(60.0, max(0.0, theta[2]))]
wn, zeta = params["wn"], params["zeta"]
acmd = wn * wn * e - 2 * zeta * wn * vh + params["ki_a"] * integ
u = theta[0] * (acmd + G) + theta[1] * vh
u = max(0.0, min(U_MAX, u))
queue.append(u)
u_act = queue.pop(0)
# plant
a = (u_act - c * v - F * math.tanh(v / EPS)) / m - G
v = v + a * DT
z = z + v * DT
energy += u_act * u_act * DT
if k % record_every == 0:
t_hist.append(round(t, 4))
z_hist.append(z)
u_hist.append(u_act)
m_hist.append(theta[0] if ctrl != "pid" else m_nom)
return {"t": t_hist, "z": z_hist, "u": u_hist, "m_hat": m_hist, "energy": energy}
def metrics(sc, tr, band=0.01, record_every=1):
t, z = tr["t"], tr["z"]
iae = sum(abs(z[i] - Z_REF * min(1.0, max(0.0, (t[i] - T_STEP) / T_RAMP))) for i in range(len(t))) * DT * record_every
after = [i for i in range(len(t)) if t[i] >= sc["t_load"]]
max_dev = max(abs(z[i] - Z_REF) for i in after)
last_out = None
for i in after:
if abs(z[i] - Z_REF) > band:
last_out = t[i]
settled = last_out is None or last_out < t[-1] - 0.5
settle = 0.0 if last_out is None else last_out - sc["t_load"]
step_idx = [i for i in range(len(t)) if T_STEP <= t[i] < sc["t_load"]]
overshoot = max(0.0, max(z[i] for i in step_idx) - Z_REF) if step_idx else 0.0
return {"max_deviation_m": max_dev, "settling_time_s": settle, "settled": settled,
"control_energy_kN2s": tr["energy"] / 1e6, "step_overshoot_m": overshoot,
"final_error_m": abs(z[-1] - Z_REF), "iae_ms": iae}
|