I am trying to minimize the vehicle's fuel consumption with safe constraints by using optimal control. There is a safe distance constraint
p.Equation(p.s - p.sLead + p.dmin + p.v * 0.5 <= 0)
and a moving constraint for distance
p.Equation(goal - p.final * p.s <= 0)
When I add the two constraints at the same time, the result shows
EXIT: Converged to a point of local infeasibility. Problem may be infeasible.
An error occured.
The error code is 2
When I remove the moving constraint for distance, the result seems meaningless, all the variables equal to the initial value. I am stuck with how to express the two constraints correctly in Gekko, please help me to check, the problem may find the local minimal successful solution?
Here is my code:
import numpy as np
from gekko import GEKKO
# Parameters
M, Av, Cd, rho, mu, g = 1200, 2.5, 0.32, 1.184, 0.013, 9.81
b0, b1, b2, b3 = 0.1569, 2.4508 * 10e-2, -7.415 * 10e-4, 5.975 * 10e-5
c0, c1, c2 = 0.07224, 9.681 * 10e-2, 1.075 * 10e-3
tf = 20
time = np.linspace(0, tf - 1, tf)
Ts = 1 # sample time
# Initial conditions of the lead car
v0Lead = 10 # m/s
s0Lead = 40 # m
vLead = v0Lead - 3.2 * np.sin(0.3 * time) + 0.2 * np.random.rand(len(time))
sLead = np.zeros(len(time))
sLead[0] = s0Lead
for i in range(1, len(time)):
sLead[i] = sLead[i - 1] + Ts * vLead[i]
uMin, uMax, vMin, vMax = -2, 3, 0, 18
# Final goal to cross the red lights
goal0 = 200
# Practical gap
goalGap = 5
goal = goal0 + goalGap
dMin = 5
v0 = 5
s0 = 5
u0 = (0.5 / M) * Cd * rho * Av * (v0 ** 2) + mu * g
uHost = np.ones(tf) * u0
uHost[-1] = 0
vHost = np.ones(tf) * v0
sHost = s0 + vHost * np.linspace(0, tf - 1, tf)
def modelopt(dMin, v0, s0, t_cons):
# MODEL
p = GEKKO(remote=False)
# parametre
p.time = time
'''
final = np.zeros(tf)
index = int(t_cons - 1)
final[index] = 1
p.final = p.Param(value=final)
'''
p.dmin = p.Param(value=dMin, name='dmin')
p.u = p.MV(value=u0, lb=uMin, ub=uMax, name='Tractive Force')
p.u.STATUS = 1
p.u.DCOST = 0 # Delta cost penalty for MV movement
p.u.DMAX = 20
p.sLead = p.FV(value=sLead, name="preceding distance")
p.a = p.CV(name="acceleration")
p.v = p.CV(value=v0, lb=vMin, ub=vMax, name="velocity")
p.s = p.SV(value=s0, name="distance")
#p.s.STATUS = 1
# S_final = p.s.value[-1]
p.obj = p.Var(name="objective")
p.Equation(p.s.dt() == p.v)
p.Equation(p.v.dt() == p.a)
p.Equation(p.a == (-1 / (2 * M)) * Cd * rho * Av * p.v ** 2 - mu * g + p.u)
# p.Equation(goal - p.final * p.s <= 0)
p.Equation(p.s - p.sLead + p.dmin + p.v * 0.5 <= 0)
p.Equation(p.obj == p.if2(p.a, (b0 + b1 * p.v + b2 * (p.v ** 2) + b3 * (p.v ** 3)) / p.v,
(b0 + b1 * p.v + b2 * (p.v ** 2) + b3 * (p.v ** 3) +
p.a * (c0 + c1 * p.v + c2 * (p.v ** 2))) / p.v))
p.Minimize(p.sum([p.obj]))
p.options.IMODE = 6
try:
p.solve(disp=True)
except:
print('Not successful')
p.open_folder()
from gekko.apm import get_file
print(p.server)
print(p.model_name)
f = get_file(p.server, p.model_name, 'infeasibilities.txt')
f = f.decode().replace('\r', '')
with open('infeasibilities.txt', 'w') as fl:
fl.write(str(f))
uHost = p.u.value[1]
vHost = p.v.value[1]
sHost = p.s.value[1]
return uHost, vHost, sHost
vHost = np.ones(tf) * v0
sHost = s0 + vHost * time
uControl = np.zeros(tf - 1)
vControl = np.zeros(tf)
vControl[0] = v0
sControl = np.zeros(tf)
sControl[0] = sHost[0]
goal_cons = np.zeros(tf)
t_cons = np.zeros(tf)
goal_cons[0] = goal
t_cons[0] = tf
for t in range(tf - 1):
uControl[t], vControl[t + 1], sControl[t + 1] = modelopt(dMin, vControl[t], sHost, t_cons[t])
sHost = sControl[t + 1] + vControl[t + 1] * np.linspace(0, tf - 1, tf)
goal_cons[t + 1] = goal - sControl[t + 1]
t_cons[t + 1] = t_cons[t] - 1