带有 GEKKO 的轨迹规划器无法处理给定的目标速度

Question

I have set up a Trajectory Planner for a vehicle with GEKKO, so basically i used a kinematic single-track model, which in nonlinear. It all works fine until i get to the part, when i give a goal velocity that is not equal to 0. I can give all other goal states (x-position, y-position, steering angle and yaw angle) without problems, but if i give a goal velocity, the optimizer exits with the following code:

Converged to a point of local infeasibility. Problem may be infeasible.

I also tried init- and goal-states combinations, that should be totally feasible, for example

startstate = [0.0, 0.0, 0.0, 0.0, 0.0]
finalstate = [10.0, 0.0, 0.0, 2.0, 0.0]

but I still have the same problem.

Does anyone have a clue, what might cause this problem?

The executable code is given below and thanks in advance!!

# Imports
import numpy as np
from gekko import GEKKO
import matplotlib.pyplot as plt

m = GEKKO() # initialize the model

# Set the constants
lwb = 2.471 # wheelbase of vehicle 3
amax = 11.5 # maximum acceleration of vehicle 3
mindelta = -1.023 # minimum steering angle
maxdelta = 1.023 # maximum steering angle
mindeltav = -0.4 # minimum steering velocity
maxdeltav = 0.4 # maximum steering velocity
minv = -11.2 # minimum velocity
maxv = 41.7 #maximum velocity

lwba = m.Const(value=lwb)
amaxa = m.Const(value=amax)
mindeltaa = m.Const(value=mindelta)
maxdeltaa = m.Const(value=maxdelta)
mindeltava = m.Const(value=mindeltav)
maxdeltava = m.Const(value=maxdeltav)
minva = m.Const(value=minv)
maxva = m.Const(value=maxv)

# Set time
startt = 0.0 # select start time of the simulation
endt = 10.0 # select end time of the simulation
dt = 100 # select discretization of the simulation
m.time = np.linspace(startt, endt, dt) # set the time (from start to endt in dt steps)
finalt = int(dt*endt/(endt-startt))-1 # compute the discretization step belonging to the goalt

# Set initial and final state
startstate = [1.0, 1.0, -0.2, 1.0, -0.3] # [sx, sy, delta, v, psi]
finalstate = [10.0, 8.0, 0.3, 0.0, 2.0] # [sx, sy, delta, v, psi]

# Create the state variables
# x1 = sx (position in x-direction)
# x2 = sy (position in y-direction)
# x3 = delta (steering angle)
# x4 = v (velocity in x-direction)
# x5 = psi (heading)

sxa = m.SV(value=startstate[0])
sya = m.SV(value=startstate[1])
deltaa = m.SV(value=startstate[2], lb=mindeltaa, ub=maxdeltaa)
va = m.SV(value=startstate[3], lb=minva, ub=maxva)
psia = m.SV(value=startstate[4])

# Create the input variables
# u1 = vdelta (velocity of steering angle)
# u2 = longa (longitudinal acceleration)

vdeltaa = m.CV(value=0, lb=mindeltava, ub=maxdeltava)
longaa = m.CV(value=0)

# Define the state space model
# differential equations
m.Equation(sxa.dt() == va * m.cos(psia))
m.Equation(sya.dt() == va * m.sin(psia))
m.Equation(deltaa.dt() == vdeltaa)
m.Equation(va.dt() == longaa)
m.Equation(psia.dt() == (va/lwba)*m.tan(deltaa))

# Add constraint
# Friction circle
m.Equation(m.sqrt(longaa**2+(va*psia.dt())**2) <= amax)

# Add Objectives
m.Obj(1*vdeltaa**2) # minimize steering velocity
m.Obj(1*longaa**2) # minimize longitudinal acceleration

# Fix the final values
m.fix(sxa, pos = finalt, val=finalstate[0])
m.fix(sya, pos = finalt, val=finalstate[1])
m.fix(deltaa, pos = finalt, val=finalstate[2])
m.fix(va, pos = finalt, val=finalstate[3])
m.fix(psia, pos = finalt, val=finalstate[4])

# Solve
m.options.IMODE = 6 # MPC
m.solve()

# Plot trajectory
plt.plot(sxa.value, sya.value)
plt.axis('equal')
plt.title('Trajectory')
plt.show()

# Plot state variables
plt.figure()
plt.suptitle('State Variables', fontsize=16)
# plot steering angle
plt.subplot(131)
plt.title('Steering Angle/Delta')
plt.plot(m.time, deltaa.value)
# plot velocity
plt.subplot(132)
plt.title('Velocity')
plt.plot(m.time, va.value)
# plot yaw angle
plt.subplot(133)
plt.title('Orientation/Psi')
plt.plot(m.time, psia.value)
plt.subplots_adjust(wspace=0.5)
plt.show()

# plot input
plt.figure()
plt.suptitle('Input', fontsize=16)
plt.subplot(121)
plt.title('Velocity of Steering Angle/v delta')
plt.plot(m.time, vdeltaa.value)
plt.subplot(122)
plt.title('Longitudinal Acceleration/long a')
plt.subplots_adjust(wspace=0.5)
plt.plot(m.time, longaa.value)
plt.show()

Answer 1

The issue is that when you specify a state variable at the end, it also sets the derivative value to zero. One work-around that may also improve your convergence performance is to include a change in the final constraint:

# Fix the final values
#m.fix(sxa, pos = finalt, val=finalstate[0])
#m.fix(sya, pos = finalt, val=finalstate[1])
#m.fix(deltaa, pos = finalt, val=finalstate[2])
#m.fix(va, pos = finalt, val=finalstate[3])
#m.fix(psia, pos = finalt, val=finalstate[4])

p = np.zeros(len(m.time))
p[finalt] = 1000
final = m.Param(p)
m.Minimize(final*(sxa-finalstate[0])**2)
m.Minimize(final*(sya-finalstate[1])**2)
m.Minimize(final*(deltaa-finalstate[2])**2)
m.Minimize(final*(va-finalstate[3])**2)
m.Minimize(final*(psia-finalstate[4])**2)

This helps the solver converge faster and avoid infeasibilities by minimizing the deviation from the final conditions instead of including them as hard constraints. The disadvantage of this approach is that it sometimes conflicts with the other objectives.

Here is the full code with the simple case that was formerly infeasible:

# Imports
import numpy as np
from gekko import GEKKO
import matplotlib.pyplot as plt

m = GEKKO() # initialize the model

# Set the constants
lwb = 2.471 # wheelbase of vehicle 3
amax = 11.5 # maximum acceleration of vehicle 3
mindelta = -1.023 # minimum steering angle
maxdelta = 1.023 # maximum steering angle
mindeltav = -0.4 # minimum steering velocity
maxdeltav = 0.4 # maximum steering velocity
minv = -11.2 # minimum velocity
maxv = 41.7 #maximum velocity

lwba = m.Const(value=lwb)
amaxa = m.Const(value=amax)
mindeltaa = m.Const(value=mindelta)
maxdeltaa = m.Const(value=maxdelta)
mindeltava = m.Const(value=mindeltav)
maxdeltava = m.Const(value=maxdeltav)
minva = m.Const(value=minv)
maxva = m.Const(value=maxv)

# Set time
startt = 0.0 # select start time of the simulation
endt = 10.0 # select end time of the simulation
dt = 100 # select discretization of the simulation
m.time = np.linspace(startt, endt, dt) # set the time (from start to endt in dt steps)
finalt = int(dt*endt/(endt-startt))-1 # compute the discretization step belonging to the goalt

# Set initial and final state
startstate = [0.0, 0.0, 0.0, 0.0, 0.0]
finalstate = [10.0, 0.0, 0.0, 2.0, 0.0]
#startstate = [1.0, 1.0, -0.2, 1.0, -0.3] # [sx, sy, delta, v, psi]
#finalstate = [10.0, 8.0, 0.3, 0.0, 2.0] # [sx, sy, delta, v, psi]

# Create the state variables
# x1 = sx (position in x-direction)
# x2 = sy (position in y-direction)
# x3 = delta (steering angle)
# x4 = v (velocity in x-direction)
# x5 = psi (heading)

sxa = m.SV(value=startstate[0])
sya = m.SV(value=startstate[1])
deltaa = m.SV(value=startstate[2], lb=mindeltaa, ub=maxdeltaa)
va = m.SV(value=startstate[3], lb=minva, ub=maxva)
psia = m.SV(value=startstate[4])

# Create the input variables
# u1 = vdelta (velocity of steering angle)
# u2 = longa (longitudinal acceleration)

vdeltaa = m.CV(value=0, lb=mindeltava, ub=maxdeltava)
longaa = m.CV(value=0)

# Define the state space model
# differential equations
m.Equation(sxa.dt() == va * m.cos(psia))
m.Equation(sya.dt() == va * m.sin(psia))
m.Equation(deltaa.dt() == vdeltaa)
m.Equation(va.dt() == longaa)
m.Equation(psia.dt() == (va/lwba)*m.tan(deltaa))

# Add constraint
# Friction circle
m.Equation(m.sqrt(longaa**2+(va*psia.dt())**2) <= amax)

# Add Objectives
m.Obj(1*vdeltaa**2) # minimize steering velocity
m.Obj(1*longaa**2) # minimize longitudinal acceleration

# Fix the final values
#m.fix(sxa, pos = finalt, val=finalstate[0])
#m.fix(sya, pos = finalt, val=finalstate[1])
#m.fix(deltaa, pos = finalt, val=finalstate[2])
#m.fix(va, pos = finalt, val=finalstate[3])
#m.fix(psia, pos = finalt, val=finalstate[4])

p = np.zeros(len(m.time))
p[finalt] = 1000
final = m.Param(p)
m.Minimize(final*(sxa-finalstate[0])**2)
m.Minimize(final*(sya-finalstate[1])**2)
m.Minimize(final*(deltaa-finalstate[2])**2)
m.Minimize(final*(va-finalstate[3])**2)
m.Minimize(final*(psia-finalstate[4])**2)

# Solve
m.options.IMODE = 6 # MPC
m.solve()

# Plot trajectory
plt.figure()
plt.plot(sxa.value, sya.value)
plt.axis('equal')
plt.title('Trajectory')
plt.savefig('Vehicle_traj.png')

# Plot state variables
plt.figure()
plt.suptitle('State Variables', fontsize=16)
# plot steering angle
plt.subplot(131)
plt.title('Steering Angle/Delta')
plt.plot(m.time, deltaa.value)
# plot velocity
plt.subplot(132)
plt.title('Velocity')
plt.plot(m.time, va.value)
# plot yaw angle
plt.subplot(133)
plt.title('Orientation/Psi')
plt.plot(m.time, psia.value)
plt.subplots_adjust(wspace=0.5)
plt.savefig('Vehicle_states.png')

# plot input
plt.figure()
plt.suptitle('Input', fontsize=16)
plt.subplot(121)
plt.title('Velocity of Steering Angle/v delta')
plt.plot(m.time, vdeltaa.value)
plt.subplot(122)
plt.title('Longitudinal Acceleration/long a')
plt.subplots_adjust(wspace=0.5)
plt.plot(m.time, longaa.value)
plt.savefig('Vehicle_input.png')
plt.show()

There are additional methods to enforce terminal constraints, including another option to have a hard constraint with an FV, if that is required by your problem. The issue of m.fix() incorrectly setting the derivative to zero is reported as an issue in the Gekko Github repository .