In [14]:
# plot solution
tm = np.linspace(0,tf.value[0],nt)
plt.figure(1)
plt.plot(tm,x.value,'k-',label=r'$x$')
plt.plot(tm,v.value,'g-',label=r'$v$')
plt.plot(tm,av.value,'r--',label=r'$a_v$')
plt.legend(loc='best')
plt.xlabel('Time')
plt.show()
In this notebook we will be implementing a simple trajectory optimisation model, using Python and the GEKKO solver.
GEKKO is an optimisation framework, much like PuLP. In contrast to PuLP, however, it is capable of handling differential algebraic equations, such as the ones that we encounter in optimal control problems that focus on the determination of optimal trajectories.
In the example that we cover in this notebook, we can examine how to apply such models in order to plan the 1-D and 2-D trajectories for a vehicle that moves in a line or in a plane, respectively.
Before we proceed, we ensure that the packages that we need are installed. You can notice in the line below the following werkzeug==1.0.0. This means that we are asking pip to install a very specific version of the werkzeug library, which is required in order for GEKKO to run correctly.
!pip install gekko matplotlib numpy werkzeug==1.0.0
Requirement already satisfied: gekko in c:\users\jose\anaconda3\envs\ci370\lib\site-packages (0.2.8) Requirement already satisfied: matplotlib in c:\users\jose\anaconda3\envs\ci370\lib\site-packages (3.3.1) Requirement already satisfied: numpy in c:\users\jose\anaconda3\envs\ci370\lib\site-packages (1.19.1) Requirement already satisfied: werkzeug==1.0.0 in c:\users\jose\anaconda3\envs\ci370\lib\site-packages (1.0.0) Requirement already satisfied: flask-cors in c:\users\jose\anaconda3\envs\ci370\lib\site-packages (from gekko) (3.0.9) Requirement already satisfied: flask in c:\users\jose\anaconda3\envs\ci370\lib\site-packages (from gekko) (1.1.2) Requirement already satisfied: kiwisolver>=1.0.1 in c:\users\jose\anaconda3\envs\ci370\lib\site-packages (from matplotlib) (1.2.0) Requirement already satisfied: pyparsing!=2.0.4,!=2.1.2,!=2.1.6,>=2.0.3 in c:\users\jose\anaconda3\envs\ci370\lib\site-packages (from matplotlib) (2.4.7) Requirement already satisfied: python-dateutil>=2.1 in c:\users\jose\anaconda3\envs\ci370\lib\site-packages (from matplotlib) (2.8.1) Requirement already satisfied: cycler>=0.10 in c:\users\jose\anaconda3\envs\ci370\lib\site-packages (from matplotlib) (0.10.0) Requirement already satisfied: pillow>=6.2.0 in c:\users\jose\anaconda3\envs\ci370\lib\site-packages (from matplotlib) (8.0.0) Requirement already satisfied: certifi>=2020.06.20 in c:\users\jose\anaconda3\envs\ci370\lib\site-packages (from matplotlib) (2020.6.20) Requirement already satisfied: Six in c:\users\jose\anaconda3\envs\ci370\lib\site-packages (from flask-cors->gekko) (1.15.0) Requirement already satisfied: Jinja2>=2.10.1 in c:\users\jose\anaconda3\envs\ci370\lib\site-packages (from flask->gekko) (2.11.2) Requirement already satisfied: itsdangerous>=0.24 in c:\users\jose\anaconda3\envs\ci370\lib\site-packages (from flask->gekko) (1.1.0) Requirement already satisfied: click>=5.1 in c:\users\jose\anaconda3\envs\ci370\lib\site-packages (from flask->gekko) (7.1.2) Requirement already satisfied: MarkupSafe>=0.23 in c:\users\jose\anaconda3\envs\ci370\lib\site-packages (from Jinja2>=2.10.1->flask->gekko) (1.1.1)
from gekko import GEKKO
import numpy as np
import matplotlib.pyplot as plt
The following equations describe a simple model of a car moving in one dimension
\begin{align} \frac{dx}{dt} & = v \\ \frac{dv}{dt} & = a_v \\ \end{align}where $x$ denotes the position of the vehicle, $v$ is velocity. The car responds to acceleration input $a_v$.
The path planning problem is to find values $a_v(t)$ on an interval $0 \leq t \leq t_f$ which moves the car from an initial condition $\left[x(0), v(0), u(0)\right]$ to a specified final condition $\left[x(t_f), v(t_f), u(t_f)\right]$. The desired control inputs minimizes the duration of the trip:
\begin{align} J = \min t_f \end{align}subject to the following limits $v$.
| variable | bound |
|---|---|
| $v$ | 30 $m/s$ |
| $-v$ | 4 $m/s$ |
| $a_v$ | 2 $m/s^2$ |
| $-a_v$ | 2 $m/s^2$ |
We can implement now these parameters using Python variables
v_max = 30 # maximum velocity
v_min = -4 # minimum velocity
av_max = 2 # maximum acceleration
av_min = -2 # minimum acceleration (ie. maximum deceleration)
x_s = 0 # starting position
x_f = 10 # ending position
v_bound = 0 # velocity in the start and end of the journey
We can then initialize an empty GEKKO model
m = GEKKO()
At this point, we need to generate a time vector, which would hold all the time increments that we are going to need when solving this model.
For the purposes of this excercise, let's assume that we need 501 steps between 0 and 1. We can implement such a vector using the np.linspace() command provided by NumPy. We assign the output into the time-dimension of our model.
nt = 501
m.time = np.linspace(0,1,nt)
We can then generate the state variables for the posiiton of the vehicle, and the velocity. When creating these variables, we can assign the initial value using the value parameter, and the lower/upper boundary values using the lb and ub parameters, respectively.
x = m.Var(value=0.0)
v = m.Var(value=0.0, lb=v_min, ub=v_max)
We can now generate the control variables in a similar fashion. First, we create a Manipulated Variable (MV) that controls the vehicle acceleration. MV means that GEKKO can change its value at every time step.
av = m.MV(value=0, lb=av_min,ub=av_max)
av.STATUS = 1
Since we created a time vector m.time between 0 and 1, we need to create a scaling factor tf that documents the final time of the trajectory. FV means that the control variable will be fixed for all the vector m.time.
tf = m.FV(value=1.0,lb=0.1,ub=100.0)
tf.STATUS = 1
Now that we have created the necessary state and control variables, we can start generating the system dynamics. Note that, because our time vector m.time ranges from 0 to 1, we must multiply our control variables by the final time so that the values are scaled appropriately.
m.Equation(x.dt()==v*tf)
m.Equation(v.dt()==av*tf)
<gekko.gekko.EquationObj at 0x1c20f621100>
We also add the boundary and path constraints
# path constraint
m.Equation(x >= 0)
# boundary constraints
m.fix(x, pos=len(m.time)-1,val=10.0) # vehicle must arrive at x=10
m.fix(v, pos=len(m.time)-1,val=0.0) # vehicle must come to a full stop
We state the objective - to minimise the trajectory time
# objective - minimise the travel time
m.Obj(tf)
Finally we solve the model (IMODE = 6 means that a non-linear solver is used)
# solve
m.options.IMODE = 6
m.solve(disp=False)
# print final travel time
print('Final Time: ' + str(tf.value[0]))
Final Time: 4.4811073139
# plot solution
tm = np.linspace(0,tf.value[0],nt)
plt.figure(1)
plt.plot(tm,x.value,'k-',label=r'$x$')
plt.plot(tm,v.value,'g-',label=r'$v$')
plt.plot(tm,av.value,'r--',label=r'$a_v$')
plt.legend(loc='best')
plt.xlabel('Time')
plt.show()
The following equations describe a simple model of a car moving in two dimensions:
\begin{align} \frac{dx}{dt} & = v \cdot cos(\theta) \\ \frac{dy}{dt} & = v \cdot sin(\theta) \\ \frac{dv}{dt} & = a_v \\ \frac{d\theta}{dt} & = \phi \\ \end{align}where $x$ denotes the position of the vehicle, $v$ is velocity, $\theta$ is the heading of the vehicle to the horizontal. The car responds to acceleration input $a_v$ and and steering input $\phi$. We ignore lateral acceleration (or lateral "g" force) on the car for the purposes of this exercise.
The path planning problem is to find values $a_v(t)$ and $\phi(t)$ on an interval $0 \leq t \leq t_f$ which steer the car from an initial condition $\left[x(0), y(0), \theta(0), v(0)\right]$ to a specified final condition $\left[x(t_f), y(t_f), \theta(t_f), v(t_f)\right]$. The desired control inputs minimizes the duration of the trip:
\begin{align} J = \min t_f \end{align}subject to the following limits $v$.
| variable | bound | ||
|---|---|---|---|
| $v$ | 30 $m/s$ | ||
| $-v$ | 4 $m/s$ | ||
| $a_v$ | 2 $m/s^2$ | ||
| $-a_v$ | 2 $m/s^2$ | ||
| $ | \phi | $ | 0.7 $rad$ |
phi_max = 0.7
# initialize GEKKO
m = GEKKO()
nt = 501
m.time = np.linspace(0,1,nt)
# Variables
x = m.Var(value=0)
y = m.Var(value=0)
v = m.Var(value=0, lb=v_min, ub=v_max)
th = m.Var(value=0, lb=-np.pi/2, ub=np.pi/2) # bound heading to -pi/2 and pi/2
# optimize final time
tf = m.FV(value=1.0,lb=0.1,ub=100.0)
tf.STATUS = 1
# control changes every time period
av = m.MV(value=0, lb=-2,ub=2)
av.STATUS = 1
phi = m.MV(value=0, lb=-phi_max, ub=phi_max)
phi.STATUS = 1
# define the ODEs that describe the movement of the vehicle
m.Equation(x.dt()==v*m.cos(th)*tf)
m.Equation(y.dt()==v*m.sin(th)*tf)
m.Equation(v.dt()==av*tf)
m.Equation(th.dt()==phi*tf)
# define path constraints
m.Equation(x >= 0)
m.Equation(y >= 0)
# define voundary constraints
m.fix(x, pos=len(m.time)-1,val=10.0)
m.fix(y, pos=len(m.time)-1,val=10.0)
m.fix(v, pos=len(m.time)-1,val=0)
m.fix(th, pos=len(m.time)-1,val=0)
# Objective function
m.Obj(tf)
# Solve
m.options.IMODE = 6
m.solve(disp=False) # set to True to view solver logs
#Presentation of results
print('Final Time: ' + str(tf.value[0]))
Final Time: 5.3560871478
#Plot solution
tm = np.linspace(0,tf.value[0],nt)
plt.figure(1)
plt.plot(tm,x.value,'k-',label=r'$x$')
plt.plot(tm,y.value,'b-',label=r'$y$')
plt.plot(tm,v.value,'g-',label=r'$v$')
plt.plot(tm,th.value,'m-',label=r'$\theta$')
plt.plot(tm,av.value,'r--',label=r'$a_v$')
plt.plot(tm,phi.value,'y--',label=r'$\phi$')
plt.legend(loc='best')
plt.xlabel('Time')
plt.show()
plt.figure(1)
plt.plot(x.value,y.value,'k-',label=r'vehicle')
plt.legend(loc='best')
plt.xlabel('x position')
plt.ylabel('y position')
plt.show()
