I am trying to implement a lateral controller for an autonomous vehicle defined by a lateral dynamic model.Well, my problem is that the CVs don't reach the desired reference or target point set by SP. I am using the following equations of motion and objective function. I am using a semi empirical formula (pacejka) to calculate tire forces donated by Fyf Fyr. Here are the equations of motion and objective function. Thanks in advance.
from gekko import GEKKO
import numpy as np
import matplotlib.pyplot as plt
import time
import math
#%% NMPC model
T = 5
nt = 51
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=-.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.40, ub=0.40 )
#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 = 0
#MV tuning
steering.STATUS = 1
steering.DCOST = 0.01
#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
print('Solver starts ...')
t = time.time()
m.solve(disp=True)
print('Solver took ', time.time() - t, 'seconds')
plt.figure()
plt.subplot(4,1,1)
plt.plot(m.time,steering.value,'b-',LineWidth=2)
plt.ylabel('steering wheel')
plt.subplot(4,1,2)
plt.plot(m.time,ypos.value,'r--',LineWidth=2)
plt.ylabel('y-point')
plt.subplot(4,1,3)
plt.plot(m.time,psipos.value,'r--',LineWidth=2)
plt.ylabel('yaw angle')
plt.xlabel('time')
plt.show()
For a reference trajectory, you need to include the time constant
TAUfor how fast to reach the setpoint.There is additional information on tuning an MPC application in the Dynamic Optimization exercises.
One other correction that you need is the
-1.0inFfy = 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 theypossetpoint that has a higher weight. You may need anotherMVto control bothyposandpsipos. Otherwise, you may consider opening thesteeringbounds 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.