对于参考轨迹，您需要包括时间常数TAU for 达到设定点的速度有多快.
ypos.TAU = 1.5
psipos.TAU = 1.5
还有关于以下的附加信息在动态优化练习中调整 MPC 应用程序 https://apmonitor.com/do/index.php/Main/ControllerTuning.
您需要的另一项更正是-1.0 in Ffy = m.Var(value=0.0, lb=-1.0*maxF, ub=maxF)。否则，它永远无法达到设定点。看来两个设定点都无法达到，因此它优先尝试满足ypos具有较高权重的设定点。您可能需要另一个MV来控制两者ypos and psipos。否则，您可以考虑打开steering看看是否能找到限制更少、更好的解决方案。我还将结束时间设置为 10，即 101 点，因为需要额外的时间才能稳定到新的设定点。
from gekko import GEKKO
import numpy as np
import matplotlib.pyplot as plt
import time
import math
#%% NMPC model
T = 10
nt = 101
m = GEKKO(remote=False)
m.time = np.linspace(0,T,nt)
#Model Parameters
X_speed = m.Param(value=10.0)
mass=m.Param(value=1611.0)
c=m.Param(value=1.351)
b=m.Param(value=1.5242)
Iz=m.Param(value=3048.1)
Cyf=m.Param(value=1.30)
Dyf=m.Param(value=3449.94238709)
Byf=m.Param(value=0.223771457713)
Eyf=m.Param(value=-0.6077272729)
Cyr=m.Param(value=1.30)
Dyr=m.Param(value=3846.47835351)
Byr=m.Param(value=0.207969093485)
Eyr=m.Param(value=-0.7755647971)
#Variables
slip_angle_front_tire = m.Var(value=0.0, lb=-10.0, ub=14.0 )
slip_angle_rear_tire = m.Var(value=0.0, lb=-10.0, ub=14.0 )
phi_f = m.Var(value=0.0)
phi_r = m.Var(value=0.0)
maxF = 5000
Ffy = m.Var(value=0.0, lb=-1.0*maxF, ub=maxF )
Fry = m.Var(value=0.0, lb=-1.0*maxF, ub=maxF )
xpos = m.Var(value=0.0)
dy = m.Var(value=0.0)
dpsi = m.Var(value=0.0)
#MV
steering = m.MV(value=0, lb=-0.4, ub=0.4 )
#CV
ypos = m.CV(value=0.0 ,lb =-200.0,ub=200.0 )
psipos = m.CV(value=0.0,lb=-3.5,ub=3.5)
#Equations
m.Equation(ypos.dt() == dy)
m.Equation(psipos.dt() == dpsi)
m.Equation(slip_angle_front_tire == steering - m.atan( (dy+b*dpsi)/X_speed ) )
m.Equation(slip_angle_rear_tire == -1.0*m.atan( (dy-c*dpsi) / X_speed))
m.Equation(phi_f == (1-Eyf)*(slip_angle_front_tire) + (Eyf/Byf)*(m.atan(Byf*slip_angle_front_tire) ) )
m.Equation(phi_r == (1-Eyr)*(slip_angle_rear_tire) + (Eyr/Byr)*(m.atan(Byr*slip_angle_rear_tire) ) )
m.Equation(Ffy == (Dyf*( m.sin(Cyf*m.atan(Byf*phi_f ) ) ) ) *2.0 )
m.Equation(Fry == (Dyr*( m.sin(Cyr*m.atan(Byr*phi_r ) ) ) ) *2.0 )
m.Equation(mass*dy.dt() == (Ffy*m.cos(steering) ) + (Fry) - (X_speed*dpsi*mass) )
m.Equation(dpsi.dt()*Iz == ( b*Ffy*m.cos(steering) ) - ( c*Fry) )
#Global options
m.options.IMODE = 6 #MPC
m.options.CV_TYPE = 2
m.options.MV_TYPE = 1
#MV tuning
steering.STATUS = 1
steering.DCOST = 0.1
#CV Tuning
ypos.STATUS = 1
psipos.STATUS = 1
ypos.TR_INIT = 2
psipos.TR_INIT = 2
ypos.WSP = 100
psipos.WSP = 10
ypos.SP = 9.2
psipos.SP = 1.5
ypos.TAU = 1.5
psipos.TAU = 1.5
print('Solver starts ...')
t = time.time()
m.solve(disp=True)
print('Solver took ', time.time() - t, 'seconds')
plt.figure()
plt.subplot(3,1,1)
plt.plot(m.time,steering.value,'b-',LineWidth=2)
plt.ylabel('steering wheel')
plt.subplot(3,1,2)
plt.plot([0,10],[9.2,9.2],'k-')
plt.plot(m.time,ypos.value,'r--',LineWidth=2)
plt.ylabel('y-point')
plt.subplot(3,1,3)
plt.plot([0,10],[1.5,1.5],'k-')
plt.plot(m.time,psipos.value,'g:',LineWidth=2)
plt.ylabel('yaw angle')
plt.xlabel('time')
plt.show()
