Implementation of a Lateral contoller using non linear model predictive control in GEKKO

For a reference trajectory, you need to include the time constant TAU for how fast to reach the setpoint.

ypos.TAU = 1.5
psipos.TAU = 1.5

There is additional information on tuning an MPC application in the Dynamic Optimization exercises.

One other correction that you need is the -1.0 in Ffy = m.Var(value=0.0, lb=-1.0*maxF, ub=maxF). Otherwise, it can never reach the setpoint. It appears that both setpoints cannot be reached so it preferentially tries to meet the ypos setpoint that has a higher weight. You may need another MV to control both ypos and psipos. Otherwise, you may consider opening the steering bounds to see if it can find a better solution with fewer restrictions. I also set the end time to 10 with 101 points because it needed additional time to stabilize to the new setpoint.

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()