import numpy as npfrom scipy.optimizeimport minimizefrom tqdm import tqdmimport matplotlib.pyplotas pltimport seaborn as sns
现在，让我们定义在问题陈述中发现的一些系统参数和其他基本常数。
# System parameterseta = 0.9 # Efficiency of the motorC_rr = 0.01 # Rolling resistance coefficientm = 500 # Mass of the vehicle (kg)g = 9.8 # Gravitational acceleration (m/s^2)rho = 1.2 # Air density (kg/m^3)C_d = 0.15 # Drag coefficientA_ref = 1 # Reference area (m^2)dx = 10 # Distance step (m)dxc = 2 # Distance step (m) for simulation of continuous systemxfinal = 20000 # in metresxpreview = 500 # in metres# Initial conditionsv0 = 15 # in m/sE_bat0 = 200000 # in Jx0 = 0 # in mt0 = 0 # s# MPC parametersNp = 50 # Prediction horizonNc = 50 # Control horizonsim_steps = int(xfinal/dx) # Total simulation steps (20 km / 10 m) = 2000 steps# Constraintsv_min, v_max = 0 , 30 # m/sE_bat_min, E_bat_max = 1000, 200000
我们制作一个函数来输出空间中太阳轮廓的值，如下所示：
# Solar power functiondef solar_power(x):if x>=0 and x <= 5000: # 0-5 kmreturn1000elif x>5000 and x <= 10000: # 5-10 kmreturn800elif x>10000 and x <= 15000: # 10-15 kmreturn1200else: # 15-20 kmreturn1000
# System dynamicsdefsystem_dynamics(X, a, P_sun): E_bat, v = X # State variables dE_bat = P_sun - (v/eta) * (m*a + C_rr*m*g + 0.5*rho*v**2*C_d*A_ref) dv = areturn np.array([dE_bat, dv]) # Return the derivative of the state
# refined rk4 method for state updatedef rk4_update(X, a, dx):E_bat, v, t, x = XP_sun = solar_power(x)# Update velocity using the equation of motion: v^2 = v_0^2 + 2aΔxv_next = v + a*dx/v #can also use the other kinematic approximation = np.sqrt(v**2 + 2*a*dx + eps)# Calculate average velocity over the stepv_avg = (v + v_next) / 2# Update battery energy using RK4k1
= P_sun - (v/eta) * (m*a + C_rr*m*g + 0.5*rho*v**2*C_d*A_ref)k2 = P_sun - (v_avg/eta) * (m*a + C_rr*m*g + 0.5*rho*v_avg**2*C_d*A_ref)k3 = P_sun - (v_avg/eta) * (m*a + C_rr*m*g + 0.5*rho*v_avg**2*C_d*A_ref)k4 = P_sun - (v_next/eta) * (m*a + C_rr*m*g + 0.5*rho*v_next**2*C_d*A_ref)E_bat_next = E_bat + (dx/v_avg) * (k1 + 2*k2 + 2*k3 + k4) / 6# Update timet_next = t + 2*dx / (v + v_next)# Update positionx_next = x + dxreturn np.array([E_bat_next, v_next, t_next, x_next])
4 成本函数和约束
现在，我们定义成本函数（我们希望使用 MPC 控制输入进行优化），如问题所示：
其中 k 是外部模拟循环的索引，其中在每个 k 处，MPC 预测接下来 Np 个步骤的轨迹，因此该步骤 k 的运行成本是遍历该预测范围的预期时间以及预测范围的每个步骤 i 处的预测控制输出（i 是内部循环的索引）。
# Cost function for MPCdefcost_function(a, X): E_bat, v, t, x = X # extract the state variables t_start = t J = 0for i inrange(Np): X = rk4_update([E_bat, v, t, x], a[i], dx) # simulate the system and get the next state E_bat, v, t, x = X # extract the state variables J += 10 * a[i]**2# add penalty for control signal J += (t - t_start) # add penalty for time takenreturn J
非线性约束可以定义如下：
# Nonlinear constraints for optimizationdefnlcon(a, X): E_bat, v, t, x = X # extract the state variables c = []# Np is the prediction horizonfor i inrange(Np): X = rk4_update([E_bat, v, t, x], a[i], dx) # simulate the system and get the next state E_bat, v, t, x = X # extract the state variables c.extend([v_min - v, v - v_max, E_bat_min - E_bat, E_bat - E_bat_max]) # add constraintsreturn np.array(c)
注意，速度以米/秒为单位，电池能量以焦耳为单位。另请注意，约束必须在每个时间步应用，因此您可以看到，在上面代码段的 for 循环中，对于迭代索引 i 的每个值，都会附加约束。
# Nonlinear constraints for optimizationdefnlcon(a, X): E_bat, v, t, x = X # extract the state variables c = []# Np is the prediction horizonfor i inrange(Np): X = rk4_update([E_bat, v, t, x], a[i], dx) # simulate the system and get the next state E_bat, v, t, x = X # extract the state variables c.extend([v_min - v, v - v_max, E_bat_min - E_bat, E_bat - E_bat_max]) # add constraintsreturn np.array(c)# Main simulation loopx_array = [x0]v_array = [v0]E_bat_array = [E_bat0]t_array = [t0]a_array = []for k in tqdm(range(sim_steps - 1)): X = np.array([E_bat_array[k], v_array[k], t_array[k], x_array[k]])# Solve MPC optimization problem a_init = np.zeros(Np) # initial guess for control signal bounds = [(-10, 10)] * Np cons = {'type': 'ineq', 'fun': lambda a: -nlcon(a, X)} # nonlinear constraints result = minimize(lambda a: cost_function(a, X), a_init, method='SLSQP', bounds=bounds, constraints=cons) # optimization a = result.x[0] a_array.append(a) # only store the first control signal# continous system simulation# Update state using RK4 method - using smaller step size for inner loop ac = np.ones(int(dx/dxc)) * afor jj inrange(int(dx/dxc)): X_next = rk4_update(X, ac[jj], dxc) X = X_next#X_next = rk4_update(X, a, dxc) # normal - using same step as outer loop E_bat_array.append(X_next[0]) v_array.append(X_next[1]) t_array.append(X_next[2]) x_array.append(X_next[3])# Break if we've reached the end of the raceif x_array[-1] >= xfinal:break
6 绘图
我们绘制控制信号输入以及输出状态变量：
# Plot results# make beautiful plots using seabornsns.set_context("talk")sns.set_style("whitegrid")sns.set_palette("husl")fig, axs = plt.subplots(3, 1, figsize=(12, 8))axs[0].plot(np.array(x_array[:-1])/1000, v_array[:-1])axs[0].set_xlabel('Position (km)')axs[0].set_ylabel('Velocity (m/s)')axs[0].set_title('Velocity Profile')axs[1].plot(np.array(x_array[:-1])/1000, np.array(E_bat_array[:-1])/1000)axs[1].set_xlabel('Position (km)')axs[1].set_ylabel('Battery State of Charge (kJ)')axs[1].set_title('Battery State of Charge')axs[2].plot(np.array(t_array[:-1]), a_array)axs[2].set_xlabel('Position (km)')axs[2].set_ylabel('Acceleration (m/s^2)')axs[2].set_title('Acceleration Profile')plt.tight_layout()plt.show()print(f"Race completion time: {t_array[-1]:.2f} seconds")
