Optimal Control Problem with nonlinear equations

Question

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

Optimal Control Problem with nonlinear equations

Answers (1)

Related Questions