Optimal and Robust Control

Trajectory Optimization for a Pendulum

Find a trajectory (both states and controls) getting a pendulum modeled by

from \(x_1(0)=0, x_2(0)=0\) to \(x_1(T_\mathrm{f})=\pi, x_2(T_\mathrm{f})=0\) in the shortest possible time \(T_\mathrm{f}\). Parameters of the model are \(a_1 = 9.81\), \(a_2 = 0.1\) and \(a_3 = 1\) and the control \(u(t)\) is limited to \(|u(t)|<2.75\). Use direct collocation method to solve this problem. Discretize the control horizon by \(N=50\) equidistantly spaced points.

As you have realized during the seminar, the direct multiple shooting method is very easy to implement but, unfortunately, may require a large number of discretization points to provide accurate enough trajectories. In this homework assignment, your goal essentially is to take the code from the seminar, modify the objective function and replace the direct multiple shooting method by a direct collocation method.

Besides the references given in the section "Recommended reading", yet another accessible (meaning easier to grasp) exposition can be found in this blog post; check it out if you have not had enough.

Implement your solution as a function with the header of the following form:

function [ t_star, x_star, u_star, Tf_star ] = hw8_cvutID()

where cvutID is your KOS username, t_star is the time vector of the trajectory, x_star is the state trajectory and u_star is the control trajectory. 

If you do not want to write the whole code from scratch, you may base your solution on the attached file. You can submit only one m-file. If you need more functions, you can use nested functions (for details, see this).