How can I optimise a trajectory to pass through waypoints in GEKKO?

Question

My goal is to calculate the optimal trajectory of a fixed wing drone so that it reaches a series of specified x coordinates (eg. [150, 0]).

I have tried a couple of approaches to enforce the waypoint constraints. When optimising only to a single waypoint I was successful with both:

m.fix_final(x, val = 150.0)

Which also fixes the derivative of x to 0,:

m.Minimize(final*10**5*(x - 150)**2)

Which worked but didn't seem extensible to the multi-waypoint case since the time at which waypoints are reached makes specifying which node should minimise the distance to the waypoint difficult, and:

waypoint = m.Var(value=0)
m.Equation(waypoint ==  m.if3(x - 150, 0, 1))
m.fix_final(waypoint, val=1)

Which leaves the derivative free but is clunky (I also don't really understand how the optimiser handles this and it seemed very sensitive to initial conditions).

All approaches failed when extending to 2 waypoints, for my current attempt I am inspired by this approach.

import numpy as np
import matplotlib.pyplot as plt
from gekko import GEKKO
from mpl_toolkits.mplot3d import Axes3D
import matplotlib.pyplot as plt

waypoints = [150, 0]

# create GEKKO model
m = GEKKO(remote=False)

num_time_steps = 31
# scale 0-1 time with tf
m.time = np.linspace(0,1,num_time_steps)

# options
m.options.NODES = 4 # collocation nodes
m.options.SOLVER = 1 # solver APOPT # 3 # solver (IPOPT)
m.options.IMODE = 6
m.options.MAX_ITER = 1000
m.options.MV_TYPE = 0
m.options.DIAGLEVEL = 1

# final time
tf = [m.FV(value=3.0, lb = 0.1) for i in range(len(waypoints))] # lb on time to prevent time-travel

# Manipulated Variables
CL = [m.MV(value=1, lb = 0, ub = 10) for i in range(len(waypoints))] # lift coefficient
phi = [m.MV(value=0, lb = np.deg2rad(-45), ub = np.deg2rad(45)) for i in range(len(waypoints))] # roll angle

# set STATUS to 1 to allow optimizer to change
for i in range(len(waypoints)): 
    tf[i].STATUS = 1
    CL[i].STATUS = 1
    phi_dot[i].STATUS = 1

mass = m.Const(value=9.5)
g = m.Const(value=9.81)
CD0 = m.Const(value=0.01)
rho = m.Const(value=1.225)
S = m.Const(value=0.65)
f_max = m.Const(value=40)

# State Variables
V = [m.Var(value=100, lb = 0, ub = 200, fixed_initial=False) for i in range(len(waypoints))] # velocity
psi = [m.Var(value=0, fixed_initial=False) for i in range(len(waypoints))] # heading angle
gamma = [m.Var(value=0, lb = np.deg2rad(-10), ub = np.deg2rad(10), fixed_initial=False) for i in range(len(waypoints))] # flight path angle

x = [m.Var(value=0, fixed_initial=False) for i in range(len(waypoints))] # x position
y = [m.Var(value=0, fixed_initial=False) for i in range(len(waypoints))] # y position
z = [m.Var(value=0, fixed_initial=False) for i in range(len(waypoints))] # z position


# specify initial conditions
m.fix_initial(x[0], val=0.0)
m.fix_initial(y[0], val=0.0)
m.fix_initial(z[0], val=150.0)
m.fix_initial(V[0], val=60.0)
m.fix_initial(psi[0], val=0.0)
m.fix_initial(gamma[0], val=0.0)

# Aerodynamic Model
k = m.Intermediate(4 * f_max ** 2 * CD0)
CD = [m.Intermediate(CD0 + CL[i] ** 2 / k) for i in range(len(waypoints))]

D = [m.Intermediate(0.5 * rho * V[i] ** 2 * S * CD[i]) for i in range(len(waypoints))]
L = [m.Intermediate(0.5 * rho * V[i] ** 2 * S * CL[i]) for i in range(len(waypoints))]

for i in range(len(waypoints)):
    # differential equations scaled by tf
    m.Equation(V[i].dt()==tf[i]*(- D[i] / mass - g*m.sin(gamma[i])))
    m.Equation(gamma[i].dt()== tf[i] * (L[i] * m.cos(phi[i]) - mass * g * m.cos(gamma[i])) / (mass * V[i]))
    m.Equation(psi[i].dt()==tf[i]*(L[i] * m.sin(phi[i]) + m.cos(psi[i])) / (mass * V[i] * m.cos(gamma[i])))
    m.Equation(x[i].dt()==tf[i]*(V[i] * m.cos(gamma[i]) * m.cos(psi[i])))
    m.Equation(y[i].dt()==tf[i]*(V[i] * m.cos(gamma[i]) * m.sin(psi[i])))
    m.Equation(z[i].dt()==tf[i]*(V[i] * m.sin(gamma[i])))

for i in range(len(waypoints) - 1):
        m.Connection(phi[i+1], phi[i], pos1 = 1, pos2 = 'end', node1 = 1, node2 = 'end')
        m.Connection(x[i+1], x[i], pos1 = 1, pos2 = 'end', node1 = 1, node2 = 'end')
        m.Connection(y[i+1], y[i], pos1 = 1, pos2 = 'end', node1 = 1, node2 = 'end')
        m.Connection(z[i+1], z[i], pos1 = 1, pos2 = 'end', node1 = 1, node2 = 'end')
        m.Connection(psi[i+1], psi[i], pos1 = 1, pos2 = 'end', node1 = 1, node2 = 'end')
        m.Connection(gamma[i+1], gamma[i], pos1 = 1, pos2 = 'end', node1 = 1, node2 = 'end')
        m.Connection(V[i+1], V[i], pos1 = 1, pos2 = 'end', node1 = 1, node2 = 'end')

        m.Connection(phi[i+1],'calculated', pos1=1, node1=1)
        m.Connection(x[i+1],'calculated', pos1=1, node1=1)
        m.Connection(y[i+1],'calculated', pos1=1, node1=1)
        m.Connection(z[i+1],'calculated', pos1=1, node1=1)
        m.Connection(psi[i+1],'calculated', pos1=1, node1=1)
        m.Connection(gamma[i+1],'calculated', pos1=1, node1=1)
        m.Connection(V[i+1],'calculated', pos1=1, node1=1)


f = np.zeros(num_time_steps); f[-1]=1; final=m.Param(f)

# minimize final time while meeting waypoints
for i in range(len(waypoints)):
        m.Minimize(final*10**5*(x[i] - waypoints[i])**2)

m.Minimize((m.sum(tf))**2)


m.solve()

This approach decides it can't reach both waypoints and just tries to position both phase endpoints as close to the midpoint between the two goals as possible.

Is there a recommended way to approach this type of multiple waypoint problem? (/ is there a silly mistake in my dynamics somewhere that prohibits turning?)

Thank you in advance for any help.

Edit:

Playing around with initial conditions and the mass of the aircraft now allows for turning and thus the meeting of all the waypoints (added a further waypoint in the image below). I need to verify the results but this approach seems to work. If there is a better/ more principled approach please let me know.

Answer 1

It looks like you found something that works for the solution. The choice of soft constraints (objective penalty) and hard constraints (fixed values, terminal constraints) is often problem-dependent. A combination of the two can also help improve convergence.

Two related problems that may be helpful:

• Inverted Pendulum: See Response with Different Weights
• Combined Scheduling and Control: See Section 8.3 for Code and Results in Gekko Paper

for waypoint constraints.