Для эталонной траектории, необходимо включить постоянное время TAU для как быстро достичь заданного значение .
ypos.TAU = 1.5
psipos.TAU = 1.5
Существует дополнительная информация о настройке приложение MP C в Dynami c Упражнения по оптимизации .
Еще одно исправление, которое вам необходимо, - это -1.0 в Ffy = m.Var(value=0.0, lb=-1.0*maxF, ub=maxF). В противном случае он никогда не сможет достичь заданного значения. Похоже, что обе уставки не могут быть достигнуты, поэтому он предпочтительно пытается соответствовать уставке ypos, которая имеет больший вес. Вам может понадобиться еще один MV для управления как ypos, так и 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()