Finding a Trajectory Given Model Dynamics Using Optimal Control | Combine

Assume that we have a mass. Its purpose in life is to move from one point to another in a two-dimensional space. It can do this by applying a force in any direction as long as the magnitude of the vector is limited.

The mass is obliged to visit two points on the way while it is not allowed to violate the laws of motion.

The mass has a maximum of 50 seconds to fulfill its task as quickly as possible while the total energy consumed is minimized. The laws of motion of the mass are included as constraints when the optimum is defined as:

$$\min_{u_x(t),\,u_y(t)} w\,\underbrace{e(t_f)}_{\text{energy}} + (1-w) \underbrace{t_f}_{\text{final time}}$$

$$\begin{array}{rcll} \text{such that} & & \\ t_f & \leq & 50 & \text{final time} \\ u_x^2(t) + u_y^2(t) & \leq & 1 & \text{maximum force} \\ x\left(\frac{1}{3}t_f\right) & = & 0 & \text{first waypoint} \\ y\left(\frac{1}{3}t_f\right) & = & 1 & \\ x\left(\frac{2}{3}t_f\right) & = & 1 & \text{second waypoint} \\ y\left(\frac{2}{3}t_f\right) & = & 0 & \\ \dot{x}(t) & = & v_x(t) & \text{equations of motion} \\ \dot{v}_x(t) & = & \frac{1}{m} u_x(t) & \\ \dot{y}(t) & = & v_y(t) & \\ \dot{v}_y(t) & = & \frac{1}{m} u_y(t) & \\ \dot{e}(t) & = & u_x^2(t) + u_y^2(t) & \\ \end{array}$$

Since we have two goals working against each other, the parameter $$w$$ is used to weight the importance of the two terms. There is a trade-off.

This problem can be solved using Pontryagin’s Principle. In this case we are using the numerical solver ACADO and the problem is solved for $$w \in \left\{ 0.0,\,0.1,\,0.2,\,\dots,\,1.0\right\}$$.

The set of trajectories for different $$w$$ become

where the widest trajectory minimizes the time (high velocity) and the trajectory with the tightest turn minimizes the energy (low velocity).
The Pareto Front is

Here we see that a good trade-off might be somewhere in the region between 25 and 30 seconds since the energy does not change much for a change in duration and vice versa.

Optimal control is incredibly powerful, but it can also be quite difficult to solve complex problems. The formulation of the model needs to be correct and the constraints must be formulated such that a solution exists. For complex problems, the solver needs to be given a good initial guess of the solution, otherwise, it might fail to find a feasible solution at all.