Discretized Optimal Trajectory, Problem-Based

This example shows how to solve a discretized optimal trajectory problem using the problem-based approach. The trajectory has constant gravity, limits on the applied force, and no air resistance. The solution process is to solve for the optimal trajectory over a fixed time $$T$$, and use that solution to find the optimal $$T$$, meaning the time that minimizes the cost. The final section shows how to include air resistance.

Compared to a nondiscretized optimization, such as the example Fit ODE Parameters Using Optimization Variables, the discretized version is not as accurate at solving an ordinary differential equation (ODE). However, the discretized version is not affected by noise in the variable-step ODE solver, as described in Optimizing a Simulation or Ordinary Differential Equation. The discretized version can also be easier to customize, and is straightforward to model. Finally, the discretized version can take advantage of automatic differentiation in the optimization process; see Effect of Automatic Differentiation in Problem-Based Optimization.

Problem Description

The problem is to move an object from position $$p_0$$ at time $$0$$ to position $$p_F$$ at time $$T$$ using a controlled jet. Represent position as a vector $$p(t)$$, velocity as a vector $$v(t)$$, and applied acceleration as a vector $$a(t)$$. In continuous time, the equations of motion, including the force of gravity, are

$$\frac{dp}{dt}=v(t)$$

$$\frac{dv}{dt}=a(t)+g*[0,0,-1]$$.

Solve the problem by discretizing time. Divide time into $$N$$ equal intervals of size $$t=T/N$$. The position at time step $$i$$ is a vector $$p(i)$$, the velocity is a vector $$v(i)$$, and the applied acceleration is a vector $$a(i)$$. You can make a set of equations that represent an ODE model fairly accurately. Some approximate equations of motion are:

$$v(i)=v(i-1)+t(a(i-1)+g)$$

$$\begin{array}{l}p(i)=p(i-1)+t\left(\frac{v(i-1)+v(i)}{2}\right)\\ =p(i-1)+tv(i-1)+t^2\frac{a(i-1)+g}{2}.\end{array}$$

The preceding equations integrate velocity using a two-point (trapezoidal rule) approximation, and integrate acceleration using a one-point (Euler) approximation. This integration scheme gives simple equations: the position and velocity at step $$i$$ depend only on the position, velocity, and acceleration at step $$i-1$$. The equations are also easy to modify for air resistance.

The boundary conditions are $$p(1)=p_0$$, $$p(N)=p_F$$, and $$v(1)=v(N)=[0,0,0]$$. Set the initial and final positions.

p0 = [0 0 0];
pF = [5 10 3];

The cost of using the jet to create force $$a$$ for time $$t$$ is $$\Vert a\Vert t$$. The total cost is the sum of the norms of the accelerations times $$t$$:

$$Cost=\sum _{i=1}^{N-1}\Vert a(i)\Vert t.$$

To convert this cost to a linear cost in optimization variables, create variables $$s(i)$$ and create associated second-order cone constraints:

$$\begin{array}{l}Cost=\sum _{i=1}^{N-1}s(i)t\\ \Vert a(i)\Vert \le s(i).\end{array}$$

Impose additional constraints that the norm of the acceleration is bounded by a constant Amax for all time steps:

$$\Vert a(i)\Vert \le Amax.$$

These constraints are also second-order cone constraints. Because the constraints are linear or second-order cone constraints and the objective function is linear, solve calls the coneprog solver to solve the problem.

The following code creates an optimization problem for a fixed time T. The code incorporates the equations of motion as problem constraints. You can access the setupproblem.m function file by running this example using the live script. The function includes an air resistance argument; set air = true for a model with air resistance. For the definition of air resistance, see the section Include Air Resistance.

type setupproblem

function trajectoryprob = setupproblem(T,air)
    if nargin == 1
        air = false;
    end
    N = 50;
    g = [0 0 -9.81];
    p0 = [0 0 0];
    pF = [5 10 3];
    Amax = 25;
    t = T/N;
    p = optimvar("p",N,3);
    v = optimvar("v",N,3);
    a = optimvar("a",N-1,3,"LowerBound",-Amax,"UpperBound",Amax);
    trajectoryprob = optimproblem;
    s = optimvar("s",N-1,"LowerBound",0,"UpperBound",3*Amax);
    trajectoryprob.Objective = sum(s)*t;
    scons = optimconstr(N-1);
    for i = 1:(N-1)
        scons(i) = norm(a(i,:)) <= s(i);
    end
    acons = optimconstr(N-1);
    for i = 1:(N-1)
        acons(i) = norm(a(i,:)) <= Amax;
    end
    vcons = optimconstr(N+1,3);
    vcons(1,:) = v(1,:) == [0 0 0];
    if air
        vcons(2:N,:) = v(2:N,:) == v(1:(N-1),:)*exp(-t) + t*(a(1:(N-1),:) + repmat(g,N-1,1));
    else
        vcons(2:N,:) = v(2:N,:) == v(1:(N-1),:) + t*(a(1:(N-1),:) + repmat(g,N-1,1));
    end
    vcons(N+1,:) = v(N,:) == [0 0 0];
    pcons = optimconstr(N+1,3);
    pcons(1,:) = p(1,:) == p0;
    if air
        pcons(2:N,:) = p(2:N,:) == p(1:(N-1),:) + t*(1+exp(-t))/2*v(1:(N-1),:) + t^2/2*(a(1:(N-1),:) + repmat(g,N-1,1));
    else
        pcons(2:N,:) = p(2:N,:) == p(1:(N-1),:) + t*v(1:(N-1),:) + t^2/2*(a(1:(N-1),:) + repmat(g,N-1,1));
    end
    pcons((N+1),:) = p(N,:) == pF;
    trajectoryprob.Constraints.acons = acons;
    trajectoryprob.Constraints.scons = scons;
    trajectoryprob.Constraints.vcons = vcons;
    trajectoryprob.Constraints.pcons = pcons;
end

Solve Problem for T = 20

Create and solve a trajectory problem for time $$T=20$$.

trajprob = setupproblem(20);
[sol,fval,eflag,output] = solve(trajprob)

Solving problem using coneprog.
Optimal solution found.

sol = struct with fields:
    a: [49x3 double]
    p: [50x3 double]
    s: [49x1 double]
    v: [50x3 double]

fval = 192.2989

eflag = 
    OptimalSolution

output = struct with fields:
           iterations: 8
    primalfeasibility: 3.2932e-07
      dualfeasibility: 2.9508e-07
           dualitygap: 1.7343e-08
            algorithm: 'interior-point'
         linearsolver: 'prodchol'
              message: 'Optimal solution found.'
               solver: 'coneprog'

Plot the trajectory and norm of the acceleration by calling the plottrajandaccel helper function shown at the end of this example.

plottrajandaccel(sol,p0,pF)

The acceleration is near that of gravity (9.81) for all times except those near the end, when the acceleration dips a bit.

Find Minimal Cost

What time $$T$$ causes the cost to be minimal? For too small a time, such as $$T=1$$, the problem is infeasible, meaning it has no solution.

myprob = setupproblem(1);
[solm,fvalm,eflagm,outputm] = solve(myprob);

Solving problem using coneprog.
Problem is infeasible.

Time 1.5 gives a feasible problem.

myprob = setupproblem(1.5);
[solm,fvalm,eflagm,outputm] = solve(myprob);

Solving problem using coneprog.
Optimal solution found.

The tomin helper function at the end of this example sets up a problem for time T and then calls solve to calculate the cost of the solution. Call fminbnd on tomin to find the optimal time (lowest cost possible) in the interval $$1.5\le T\le 10$$.

[Tmin,Fmin] = fminbnd(@(T)tomin(T,false),1.5,10)

Tmin = 1.9499

Fmin = 24.6102

Obtain the trajectory for the optimal time Tmin.

minprob = setupproblem(Tmin);
sol = solve(minprob);

Solving problem using coneprog.
Optimal solution found.

Plot the minimizing trajectory and acceleration.

plottrajandaccel(sol,p0,pF)

The minimizing solution is nearly a "bang-bang" solution: the acceleration is either maximal or zero for all but two values.

Plot Nonminimizing Trajectories

Plot the trajectories for a variety of times.

figure
hold on
options = optimoptions("coneprog","Display","none");
for i = 1:10
    T = 2*i;
    prob = setupproblem(T);
    sol = solve(prob,"Options",options);
    psol = sol.p;
    plot3(psol(:,1),psol(:,2),psol(:,3),'rx',"Color",[256-25*i 20*i 25*i]/256)
end
view([18 -10])
xlabel("x")
ylabel("y")
zlabel("z")
legend("T = 2","T = 4","T = 6","T = 8","T = 10","T = 12","T = 14","T = 16","T = 18","T = 20")
hold off

The shortest time (2) has a nearly direct trajectory on this scale. The intermediate times have large variations from a direct path. The largest time (20) also has a nearly direct path.

Include Air Resistance

Change the model dynamics to include air resistance. Linear air resistance changes the velocity by a factor $$\exp (-t)$$ after time $$t$$. The equations of motion become

$$v(i)=v(i-1)\exp (-t)+t(a(i-1)+g)$$

$$\begin{array}{l}p(i)=p(i-1)+t\left(\frac{v(i-1)+v(i)}{2}\right)\\ =p(i-1)+t\left(\frac{1+\exp (-t)}{2}\right)v(i-1)+t^2\frac{a(i-1)+g}{2}.\end{array}$$

The problem formulation in the setupproblem(T,air) function for air = true has factors of exp(-t) in both the line defining the velocity constraint and the line defining the position constraint:

vcons(2:N,:) = v(2:N,:) == v(1:(N-1),:)*exp(-t) + t*(a(1:(N-1),:) + repmat(g,N-1,1));
pcons(2:N,:) = p(2:N,:) == p(1:(N-1),:) + t*(1+exp(-t))/2*v(1:(N-1),:) + t^2/2*(a(1:(N-1),:) + repmat(g,N-1,1));

Find the optimal time for the problem with air resistance.

[Tmin2,Fmin2] = fminbnd(@(T)tomin(T,true),1.5,10)

Tmin2 = 1.9396

Fmin2 = 28.7967

The optimal time is only slightly lower than the time in the problem without air resistance (1.94 instead of 1.95), but the cost Fmin is about 17% higher (28.8 instead of 24.6).

compartable = table([Tmin;Tmin2],[Fmin;Fmin2],'VariableNames',["Time" "Cost"],'RowNames',["Original" "Air Resistance"])

compartable=2×2 table
                       Time      Cost 
                      ______    ______

    Original          1.9499     24.61
    Air Resistance    1.9396    28.797

Plot the trajectory and acceleration for the optimal solution with air resistance.

minprob = setupproblem(Tmin2,true);
sol = solve(minprob);

Solving problem using coneprog.
Optimal solution found.

plottrajandaccel(sol,p0,pF)

With air resistance, the time of zero acceleration shifts to a later time step and is shorter. However, the solution is still nearly "bang-bang."

Helper Functions

This code creates the plottrajandaccel helper function.

function plottrajandaccel(sol,p0,pF)
figure
psol = sol.p;
plot3(psol(:,1),psol(:,2),psol(:,3),'rx')
hold on
plot3(p0(1),p0(2),p0(3),'ks')
plot3(pF(1),pF(2),pF(3),'bo')
hold off
view([18 -10])
xlabel("x")
ylabel("y")
zlabel("z")
legend("Steps","Initial Point","Final Point")
figure
asolm = sol.a;
nasolm = sqrt(sum(asolm.^2,2));
plot(nasolm,"rx")
xlabel("Time step")
ylabel("Norm(acceleration)")
end

This code creates the tomin helper function.

function F = tomin(T,air)
    if nargin == 1
        air = false;
    end
    problem = setupproblem(T,air);
    opts = optimoptions("coneprog","Display","none");
    [~,F] = solve(problem,"Options",opts);
end