gekkompclateral

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

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

  • 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.

    optimal control

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