Problem on Obstacle Avoidance using GEKKO (No solution found, and solution exists)

user_15446262 0
Viewed 47

The original question can be found here.

I am a beginner at using this package, and most of the code given below has been borrowed from the examples given in the official documentation for GEKKO.

Usual imports:

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

Initializing the model and defining the time points:

m = GEKKO() # initialize gekko
nt = 501
m.time = np.linspace(0.0,1.0,nt)
t = m.Param(value=m.time)

p = np.zeros(nt) # mark final time point
p[-1] = 1.0
final = m.Param(value=p)

Defining the state and control variables:

x = m.Var(value=0.0)
y = m.Var(value=0.0)
z = m.Var(value=0.0)

V = m.FV(value=2.0, lb=0.0, ub=100.0)
V.STATUS = 1

theta = m.CV(1.0)
theta.STATUS = 1
theta.SPHI = 0
theta.SPLO = 6.28

V was chosen as a Fixed Variable since the problem statement mentions that:

V is a constant scalar speed.

theta has been assumed to be in [0,2*pi] since it hasn't been directly mentioned in the problem statement.

z is used as a proxy variable in order to implement the integral objective.

The constraint equations are defined:

m.Equation(x.dt()==V*m.cos(theta))
m.Equation(y.dt()==V*m.sin(theta))
m.Equation(x*final==1.2)
m.Equation(y*final==1.6)
m.Equation(((x-0.4)**2)+((y-0.5)**2)>=0.1)
m.Equation(((x-0.8)**2)+((y-1.5)**2)>=0.1)

Defining the variables needed in order to obtain 2nd order derivatives:

dx = m.Var(value = 0.0)
dy = m.Var(value = 0.0)
ddx = m.Var(value = 0.0)
ddy = m.Var(value = 0.0)

Corresponding equations to make this work:

m.Equation(dx==x.dt())
m.Equation(dy==y.dt())
m.Equation(ddx==dx.dt())
m.Equation(ddy==dy.dt())

Setting the proxy variable's derivatives as the objective inside the integral and solving:

m.Equation(z.dt()==(ddx**2)+(ddy**2))
m.Obj(z*final)

m.options.IMODE = 6 # optimal control mode
m.solve(disp=True)

I am not completely sure if the way I have coded the given question is correct. As the given link shows, a solution to the problem exists. I have tried using APOPT and IPOPT to solve this problem, and both solvers are unable to find a solution. COLD_START cannot be used since DOF < 0. Any help with this would be greatly appreciated!

gekko
1 Answers

A problem with the model is the two terminal constraints because every point except the final point (where final=1) is 0=1.2 and 0=1.6.

m.Equation(x*final==1.2)
m.Equation(y*final==1.6)

An alternative form is needed.

m.Equation((x-1.2)*final==0)
m.Equation((y-1.6)*final==0)

Below is a feasible solution but not optimized.

path planning

import numpy as np
from gekko import GEKKO

m = GEKKO()
nt = 51
m.time = np.linspace(0,1,nt)

# mark final time point
p = np.zeros(nt); p[-1] = 1 
final = m.Param(p)

# define theta as a Variable
#theta = m.Var(value=np.pi/2.0,lb=0,ub=2*np.pi)

# define theta as an MV
theta = m.MV(value=0,lb=0,ub=2*np.pi)
theta.STATUS = 1; theta.DCOST = 0

V = m.FV(value=2.15,lb=0,ub=10) # velocity
#V.STATUS = 1

x,y,dx,dy,ddx,ddy = m.Array(m.Var,6)

m.Equation(x.dt()==V*m.cos(theta))
m.Equation(y.dt()==V*m.sin(theta))
m.Equation(((x-0.4)**2)+((y-0.5)**2)>=0.1)
m.Equation(((x-0.8)**2)+((y-1.5)**2)>=0.1)

m.Equation(dx==x.dt())
m.Equation(dy==y.dt())
m.Equation(ddx==dx.dt())
m.Equation(ddy==dy.dt())

# alternative 1 terminal constraint
m.Minimize(1e4*(x-1.2)**2)
m.Minimize(1e4*(y-1.6)**2)

# alternative 2 terminal constraint
#m.fix_final(x,1.2)
#m.fix_final(y,1.6)

# alternative 3 terminal constraint
#m.Equation((x-1.2)*final==0)
#m.Equation((y-1.6)*final==0)

# objective
m.Minimize(m.integral((ddx**2)+(ddy**2))*final)

m.options.SOLVER = 3
m.options.MAX_ITER = 1000
m.options.NODES = 3
m.options.IMODE = 6 # optimal control mode
m.solve(disp=True)

import matplotlib.pyplot as plt
plt.figure(figsize=(5,5))
th = np.linspace(0,2*np.pi,500)
x1 = np.sqrt(0.1)*np.cos(th)+0.4
y1 = np.sqrt(0.1)*np.sin(th)+0.5
x2 = np.sqrt(0.1)*np.cos(th)+0.8
y2 = np.sqrt(0.1)*np.sin(th)+1.5
plt.plot(x1,y1,'r',x2,y2,'r');
plt.plot(x.value,y.value,'b-o',markersize=2)
plt.plot([1.2],[1.6],'o',color='orange')
plt.xlabel('x'); plt.xlim([0,2.0])
plt.ylabel('y'); plt.ylim([0,2.0])
plt.title('Obstacle avoidance state variables, Speed = %2.4g'%V.value[0])

IPOPT isn't able to solve the problem where V.STATUS=1 (calculated). I tried a couple other terminal constraints but the solvers (APOPT and IPOPT) are unsuccessful.

user_23669410
Related