Notebook 7.1 - Introduction to Trajectory Optimisation in Python¶

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.

1. Mathematical model - 1D vehicle movement¶

The following equations describe a simple model of a car moving in one dimension

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\left(t\right)$ on an interval $0\le t\le t_f$ which moves the car from an initial condition $\left[x\right(0),v(0),u(0\left)\right]$ to a specified final condition $\left[x\right(t_f),v(t_f),u(t_f\left)\right]$. The desired control inputs minimizes the duration of the trip:

subject to the following limits $v$.

We can implement now these parameters using Python variables

We can then initialize an empty GEKKO model

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.

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.

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.

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.

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.

We also add the boundary and path constraints

We state the objective - to minimise the trajectory time

Finally we solve the model (IMODE = 6 means that a non-linear solver is used)

Plots¶

2. Mathematical model - 2D Vehicle Movement¶

The following equations describe a simple model of a car moving in two dimensions:

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 $\varphi$. 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\left(t\right)$ and $\varphi \left(t\right)$ on an interval $0\le t\le t_f$ which steer the car from an initial condition $\left[x\right(0),y(0),\theta (0),v(0\left)\right]$ to a specified final condition $\left[x\right(t_f),y(t_f),\theta (t_f),v(t_f\left)\right]$. The desired control inputs minimizes the duration of the trip:

subject to the following limits $v$.