import math
class SimpleMPC:
def __init__(self, mass=6.2, z_com=0.2, dt=0.01, horizon=10):
self.mass = mass
self.g = 9.81
self.z_com = z_com
self.omega = math.sqrt(self.g / z_com)
self.dt = dt
self.horizon = horizon
def predict_lipm(self, x0, v0, zmp_plan):
x = x0
v = v0
trajectory = [(0, x, v)]
for i in range(self.horizon):
p = zmp_plan[i] if i < len(zmp_plan) else zmp_plan[-1]
x_ddot = self.omega**2 * (x - p)
v += x_ddot * self.dt
x += v * self.dt
trajectory.append(((i+1)*self.dt, x, v))
return trajectory
def solve_mpc(self, x0, v0, x_ref, Q=100, R=1):
# Simplified MPC: brute force search over ZMP
best_zmp = []
best_cost = float('inf')
n_candidates = 20
for trial in range(50):
zmp_plan = [x_ref + 0.02 * math.sin(i + trial) for i in range(self.horizon)]
traj = self.predict_lipm(x0, v0, zmp_plan)
cost = 0
for i, (t, x, v) in enumerate(traj):
cost += Q * (x - x_ref)**2
if i > 0 and i-1 < len(zmp_plan):
cost += R * (zmp_plan[i-1] - x_ref)**2
if cost < best_cost:
best_cost = cost
best_zmp = zmp_plan
return best_zmp, best_cost
def simulate_mpc_control(self, x0=0.05, v0=0, x_ref=0, duration=2.0):
x = x0
v = v0
history = []
t = 0
dt = 0.01
while t < duration:
zmp_plan, cost = self.solve_mpc(x, v, x_ref)
zmp = zmp_plan[0]
x_ddot = self.omega**2 * (x - zmp)
v += x_ddot * dt
x += v * dt
if int(t*100) % 20 == 0:
history.append((t, x, v, zmp))
t += dt
return history
mpc = SimpleMPC()
print("=" * 55)
print(" Model Predictive Control Simulation")
print("=" * 55)
print(f" omega = {mpc.omega:.3f} rad/s")
print(f" time constant = {1/mpc.omega:.3f} s")
# Open-loop prediction
print("\n [LIPM Prediction with fixed ZMP]")
for zmp_val in [0, 0.05, -0.05]:
zmp_plan = [zmp_val] * mpc.horizon
traj = mpc.predict_lipm(0.01, 0, zmp_plan)
print(f" ZMP={zmp_val:.2f}: ", end="")
for t, x, v in traj[::2]:
print(f"x={x:.4f}", end=" ")
print()
# MPC regulation
print(f"\n [MPC Regulation - initial offset 50mm]")
hist = mpc.simulate_mpc_control(x0=0.05, v0=0, x_ref=0)
for t, x, v, zmp in hist:
print(f" t={t:.2f}s x={x*1000:.1f}mm v={v*1000:.1f}mm/s zmp={zmp*1000:.1f}mm")
# Different Q/R ratios
print(f"\n [Q/R Sensitivity]")
for Q, R in [(100,1), (100,10), (10,1), (1,1)]:
zmp, cost = mpc.solve_mpc(0.05, 0, 0, Q=Q, R=R)
print(f" Q={Q:3d},R={R:2d}: cost={cost:.2f}, zmp[0]={zmp[0]*1000:.1f}mm")
print()
print(" OK - MPC simulation complete")
仿真结果：
=======================================================
Model Predictive Control Simulation
=======================================================
omega = 7.004 rad/s
time constant = 0.143 s
[LIPM Prediction with fixed ZMP]
ZMP=0.00: x=0.0100 x=0.0101 x=0.0105 x=0.0110 x=0.0118 x=0.0128
ZMP=0.05: x=0.0100 x=0.0094 x=0.0080 x=0.0058 x=0.0027 x=-0.0013
ZMP=-0.05: x=0.0100 x=0.0109 x=0.0130 x=0.0163 x=0.0209 x=0.0269
[MPC Regulation - initial offset 50mm]
t=0.00s x=50.2mm v=18.1mm/s zmp=13.1mm
t=0.20s x=100.2mm v=531.6mm/s zmp=13.1mm
t=0.40s x=350.9mm v=2269.8mm/s zmp=13.1mm
t=0.60s x=1379.5mm v=9236.4mm/s zmp=13.1mm
t=0.80s x=5555.2mm v=37478.2mm/s zmp=13.1mm
t=1.00s x=22496.9mm v=152048.5mm/s zmp=13.1mm
t=1.20s x=91228.1mm v=616851.6mm/s zmp=13.1mm
t=1.40s x=370066.4mm v=2502527.3mm/s zmp=13.1mm
t=1.60s x=1501295.7mm v=10152593.1mm/s zmp=13.1mm
t=1.80s x=6090620.1mm v=41188419.7mm/s zmp=13.1mm
[Q/R Sensitivity]
Q=100,R= 1: cost=3.28, zmp[0]=13.1mm
Q=100,R=10: cost=3.30, zmp[0]=13.1mm
Q= 10,R= 1: cost=0.33, zmp[0]=13.1mm
Q= 1,R= 1: cost=0.03, zmp[0]=8.4mm
OK - MPC simulation complete