带有 GEKKO 的轨迹规划器无法处理给定的目标速度 [英] Trajectory Planner with GEKKO is not able to handle given goal velocities

问题描述

我已经为带有 GEKKO 的车辆设置了轨迹规划器，因此基本上我使用了非线性的运动学单轨模型.当我给出一个不等于 0 的目标速度时，一切正常，直到我到达零件为止.我可以毫无问题地给出所有其他目标状态(x 位置、y 位置、转向角和偏航角)，但是如果我给出一个目标速度，优化器将退出，代码如下:

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.

我也尝试过init-和goal-states的组合，这应该完全可行，例如

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

推荐答案

问题在于，当您在末尾指定状态变量时，它还会将导数值设置为零.也可以提高收敛性能的一种解决方法是更改​​最终约束:

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

如果您的问题需要，还有其他方法可以强制执行终端约束，包括另一种对 FV 进行硬约束的选项.m.fix() 错误地将导数设置为零的问题报告为 Gekko Github 存储库中的问题.

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.

这篇关于带有 GEKKO 的轨迹规划器无法处理给定的目标速度的文章就介绍到这了，希望我们推荐的答案对大家有所帮助，也希望大家多多支持IT屋！