49 lines
1.3 KiB
Python
49 lines
1.3 KiB
Python
import numpy as np
|
|
import matplotlib.pyplot as plt
|
|
from scipy.optimize import minimize
|
|
|
|
# 定义系统参数
|
|
current_position = 0
|
|
target_position = 50
|
|
max_drive = 10
|
|
min_drive = -10
|
|
period = 0.1 # 100ms
|
|
|
|
# 定义MPC优化目标函数
|
|
def mpc_objective(drive_sequence, *args):
|
|
current_position, target_position, period = args
|
|
position = current_position
|
|
total_error = 0
|
|
for drive in drive_sequence:
|
|
position += drive * period
|
|
total_error += (position - target_position) ** 2
|
|
return total_error
|
|
|
|
# 初始驱动序列
|
|
initial_drive_sequence = np.zeros(50)
|
|
|
|
# 约束条件
|
|
bounds = [(min_drive, max_drive) for _ in range(len(initial_drive_sequence))]
|
|
|
|
# 优化驱动序列
|
|
result = minimize(mpc_objective, initial_drive_sequence, args=(current_position, target_position, period), bounds=bounds)
|
|
|
|
# 获取优化后的驱动序列
|
|
optimal_drive_sequence = result.x
|
|
|
|
# 计算轨迹
|
|
trajectory = [current_position]
|
|
position = current_position
|
|
for drive in optimal_drive_sequence:
|
|
position += drive * period
|
|
trajectory.append(position)
|
|
|
|
# 绘制轨迹图
|
|
plt.plot(trajectory, label='Trajectory')
|
|
plt.axhline(y=target_position, color='r', linestyle='--', label='Target Position')
|
|
plt.xlabel('Time Step')
|
|
plt.ylabel('Position')
|
|
plt.legend()
|
|
plt.title('MPC Trajectory')
|
|
plt.show()
|