Discretized Optimal Trajectory, Problem-Based

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{dv}{dt}=a(t)+g*[0,0,-1]$$.

The jet applies an acceleration $$a(t)$$. The resulting acceleration has an added term for gravity.

The maximum acceleration the jet can apply is $$A_{max}=25$$.

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

The preceding equations integrate velocity using a two-point (trapezoidal rule) approximation, and integrate acceleration using a one-point (Euler) approximation. If you regard the acceleration $$a(i)$$ as applying to the center of time interval $$i$$, the integration scheme is a midpoint rule for acceleration. The overall 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(0)=p_0$$, $$p(N)=p_F$$, and $$v(0)=v(N)=[0,0,0]$$. Set the initial and final positions.

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

MATLAB® indices start at 1, not 0. For easier indexing, specify the number of intervals as $$N-1$$, and have the indices of position and velocity go from $$1$$ through $$N$$ instead of $$0$$ through $$N$$. With this indexing, the acceleration has indices $$1$$ through $$N-1$$.

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$$.

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

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

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 setupproblem1 helper function at the end of this example creates an optimization problem for a fixed time T. The code incorporates the equations of motion as problem constraints. 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.

The applied acceleration $$a$$ is the main optimization variable in the problem. As Vanderbei [1] suggests, the problem does not need to take velocity v and position p as optimization variables. You can derive their values from the equations of motion and the values of the applied acceleration $$a$$. For N steps, $$a$$ has dimensions N – 1 by 3. The problem also includes the vector variable s as an optimization variable that allows for a linear objective function.

Solve Problem for T = 20

Create and solve a trajectory problem for time $$T=20$$. For reliability in this numerically sensitive problem, set the optimality tolerance to 1e-9.

[trajprob,p] = setupproblem1(20);

What is the default solver for this problem?

defaultsolver = solvers(trajprob)

defaultsolver = 
"coneprog"

Set the options for the coneprog solver and solve the problem.

opts = optimoptions(defaultsolver,Display="none",OptimalityTolerance=1e-9);
[sol,fval,eflag,output] = solve(trajprob,Options=opts)

sol = struct with fields:
    a: [49×3 double]
    s: [49×1 double]

fval = 192.2812

eflag = 
    OptimalSolution

output = struct with fields:
           iterations: 12
    primalfeasibility: 4.4823e-10
      dualfeasibility: 3.3916e-09
           dualitygap: 3.2421e-11
            algorithm: 'interior-point'
         linearsolver: 'augmented'
              message: 'Optimal solution found.'
               solver: 'coneprog'

Plot the trajectory and norm of the acceleration by calling the plottrajandaccel helper function.

N = 50;
plottrajandaccel(sol,p,p0,pF)

The acceleration is nearly maximal at the beginning and end periods, and nearly zero in the middle. This type of solution, where acceleration is either maximal or 0, is called "bang-bang."

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 = setupproblem1(1);
opts.Display = "final"; % To see the solution status
[solm,fvalm,eflagm,outputm] = solve(myprob,Options=opts);

Solving problem using coneprog.
Problem is infeasible.

Time 1.5 gives a feasible problem.

myprob = setupproblem1(1.5);
[solm,fvalm,eflagm,outputm] = solve(myprob,Options=opts);

Solving problem using coneprog.
Optimal solution found.

The tomin helper function 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.9517

Fmin = 24.6095

Obtain the trajectory for the optimal time Tmin.

[minprob,p] = setupproblem1(Tmin);
sol = solve(minprob,Options=opts);

Solving problem using coneprog.
Optimal solution found.

Plot the minimizing trajectory and acceleration.

plottrajandaccel(sol,p,p0,pF)

The minimizing solution is closer to 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",OptimalityTolerance=1e-9);
g = [0 0 -9.81];
for i = 1:10
    T = 2*i;
    t = T/N;
    prob = setupproblem1(T);
    sol = solve(prob,"Options",options);
    asol = sol.a;
    vsol = cumsum([[0 0 0];t*(asol+repmat(g,size(asol,1),1))]);
    N = size(vsol,1);
    np = 2:N;
    n0 = 1:(N-1);
    psol = cumsum([p0;t*(vsol(np,:) + vsol(n0,:))/2]);
    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. Longer times have increasingly large variations from a direct path, with the path for $$T=20$$ reaching a height near 300.

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

The problem formulation in the setupproblem1(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.9398

Fmin2 = 28.7967

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

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

compartable=2×2 table
                       Time      Cost 
                      ______    ______

    Original          1.9517    24.609
    Air Resistance    1.9398    28.797

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

[minprob,p] = setupproblem1(Tmin2,true);
sol = solve(minprob,Options=opts);

Solving problem using coneprog.
Optimal solution found.

plottrajandaccel(sol,p,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."

Extend Model to Include Variable Mass

A jet operates by expelling mass. If you include the effect of diminishing mass as the jet operates, the equations of motion no longer fit into the framework of a second-order cone problem. The problem becomes harder to solve numerically, leading to longer solution times and less accurate answers.

Change the model to include an applied force $$F(t)$$ and a mass $$m(t)$$. The net acceleration is

$$a(t)=F(t)/m(t)+g[0,0,-1]$$.

The rate of change of mass is

where $$r$$ is a constant.

Assume that $$m(0)=2$$, $$r=1/1000$$, and the maximum force is $$F_{max}=50$$. These assumptions imply that, at time 0, the maximum applied acceleration is $$F_{max}/m(0)=25$$, the same maximum acceleration as in the previous model.

The setupproblemfuel1 function creates a problem based on these equations and parameters. Do not include air resistance for this instance of the variable mass model.

[trajprob,p] = setupproblemfuel1(20);

What is the default solver for the resulting problem?

defaultsolver = solvers(trajprob)

defaultsolver = 
"fmincon"

The fmincon solver requires an initial point. Try a random initial point for T = 20.

rng default
y0 = randn(49,3);
x0.F = y0;

Specify options for the solver to produce plots as it progresses and to allow for more function evaluations and iterations than the default. Try to obtain more accuracy by reducing the StepTolerance from the default 1e-10 to 1e-11.

opts = optimoptions(defaultsolver,Display="none",PlotFcn="optimplotfvalconstr",...
    MaxFunctionEvaluations=1e5,MaxIterations=1e4,StepTolerance=1e-11);

Call the solver.

[sol,fval,eflag,output] = solve(trajprob,x0,Options=opts)

sol = struct with fields:
    F: [49×3 double]

fval = 348.1493

eflag = 
    StepSizeBelowTolerance

output = struct with fields:
              iterations: 2393
               funcCount: 8148
         constrviolation: 2.7960e-12
                stepsize: 1.8893e-11
               algorithm: 'interior-point'
           firstorderopt: 0.0719
            cgiterations: 76125
                 message: 'Local minimum possible. Constraints satisfied.↵↵fmincon stopped because the size of the current step is less than↵the value of the step size tolerance and constraints are ↵satisfied to within the value of the constraint tolerance.↵↵<stopping criteria details>↵↵Optimization stopped because the relative changes in all elements of x are↵less than options.StepTolerance = 1.000000e-11, and the relative maximum constraint↵violation, 1.474630e-15, is less than options.ConstraintTolerance = 1.000000e-06.'
            bestfeasible: [1×1 struct]
     objectivederivative: "reverse-AD"
    constraintderivative: "reverse-AD"
                  solver: 'fmincon'

Plot the trajectory and acceleration. The plottrajandaccel function uses the values in the a field of the solution structure, so first copy the solution from the F field to the a field.

sol.a = sol.F;
plottrajandaccel(sol,p,p0,pF)

Smooth Solution by Adding Cost for Variable Acceleration

The applied force is essentially "bang-bang," yet the force has oscillatory behavior. Vanderbei [1] calls this type of behavior "ringing." To remove the ringing, add some cost for oscillatory acceleration, and then solve again. The setupproblemfuel2 helper function includes an additional term t*1e-4*sum(diff(fnorm).^2 in the objective function, which penalizes changes in the norm of the acceleration:

 % Add cost for acceleration changes
 trajectoryprob.Objective = sum(fnorm)*t + t*1e-4*sum(diff(fnorm).^2);

Solve the problem again.

[trajprob,p] = setupproblemfuel2(20);
[sol,fval,eflag,output] = solve(trajprob,x0,Options=opts)

sol = struct with fields:
    F: [49×3 double]

fval = 348.5616

eflag = 
    StepSizeBelowTolerance

output = struct with fields:
              iterations: 3755
               funcCount: 7927
         constrviolation: 3.2683e-11
                stepsize: 1.6872e-10
               algorithm: 'interior-point'
           firstorderopt: 0.1728
            cgiterations: 46335
                 message: 'Local minimum possible. Constraints satisfied.↵↵fmincon stopped because the size of the current step is less than↵the value of the step size tolerance and constraints are ↵satisfied to within the value of the constraint tolerance.↵↵<stopping criteria details>↵↵Optimization stopped because the relative changes in all elements of x are↵less than options.StepTolerance = 1.000000e-11, and the relative maximum constraint↵violation, 1.723720e-14, is less than options.ConstraintTolerance = 1.000000e-06.'
            bestfeasible: [1×1 struct]
     objectivederivative: "reverse-AD"
    constraintderivative: "reverse-AD"
                  solver: 'fmincon'

Plot the trajectory and acceleration using the sol.F data.

sol.a = sol.F;
plottrajandaccel(sol,p,p0,pF)

Supply Better Initial Point

Most of the ringing is gone, but the solution still has more than one interval of zero force.

Specifying a different initial point might result in a more satisfactory solution. To cause the solver to have small acceleration during intermediate times, specify the initial point equal to 0 for indices 10 through 40, and equal to 20 for indices 1 through 9 and 41 through 49. Add some random noise to the initial point.

rng default
y0 = 20*ones(49,3);
y0(10:40,:) = 0;
y0 = y0 + randn(size(y0));
x0.F = y0;

Solve the problem starting from this new point.

[sol,fval,eflag,output] = solve(trajprob,x0,Options=opts)

sol = struct with fields:
    F: [49×3 double]

fval = 348.2770

eflag = 
    StepSizeBelowTolerance

output = struct with fields:
              iterations: 1653
               funcCount: 5422
         constrviolation: 7.6383e-14
                stepsize: 2.2832e-11
               algorithm: 'interior-point'
           firstorderopt: 0.0318
            cgiterations: 26724
                 message: 'Local minimum possible. Constraints satisfied.↵↵fmincon stopped because the size of the current step is less than↵the value of the step size tolerance and constraints are ↵satisfied to within the value of the constraint tolerance.↵↵<stopping criteria details>↵↵Optimization stopped because the relative changes in all elements of x are↵less than options.StepTolerance = 1.000000e-11, and the relative maximum constraint↵violation, 6.578331e-17, is less than options.ConstraintTolerance = 1.000000e-06.'
            bestfeasible: [1×1 struct]
     objectivederivative: "reverse-AD"
    constraintderivative: "reverse-AD"
                  solver: 'fmincon'

Plot the trajectory and acceleration.

sol.a = sol.F;
plottrajandaccel(sol,p,p0,pF)

The solution seems satisfactory: it is essentially "bang-bang" acceleration with just one interval of zero applied acceleration.

How much mass remains from the initial 2 at the end of the object's journey?

T = 20;
N = 50;
t = T/N;
fnorm = zeros(N-1,1);
r = 1/1000;
for i = 1:length(fnorm)
    fnorm(i) = norm(sol.F(i,:));
end
m = 2 - r*t*sum(fnorm)

m = 1.6518

What is the maximum applied acceleration with this reduced mass?

Fmax = 50;
Fmax/m

ans = 30.2700

When the model includes the effect of variable mass, the maximum applied acceleration during the trajectory changes from 25 to 30.

Include Air Resistance and Variable Mass

Include air resistance in the model again. In this case, the setupproblemfuel2 helper function calls the airResistance helper function using fcn2optimexpr to evaluate the velocity with air resistance. Coding a for loop using a separate function, and calling that function using fcn2optimexpr, creates a more efficient problem. For more information, see Create for Loop for Static Analysis and Static Analysis of Optimization Expressions.

[trajprob2,p] = setupproblemfuel2(20,true);

Based on the result of the earlier model that included air resistance, set the initial interval of zero force to be later: 15 through 45 instead of 10 through 40.

rng default
y0 = 20*ones(49,3);
y0(15:45,:) = 0;
y0 = y0 + randn(size(y0));
x0.F = y0;

Solve the problem.

[sol2,fval2,eflag2,output2] = solve(trajprob2,x0,Options=opts)

sol2 = struct with fields:
    F: [49×3 double]

fval2 = 352.7138

eflag2 = 
    StepSizeBelowTolerance

output2 = struct with fields:
              iterations: 6686
               funcCount: 12620
         constrviolation: 2.1938e-13
                stepsize: 1.5226e-11
               algorithm: 'interior-point'
           firstorderopt: 0.2178
            cgiterations: 93737
                 message: 'Local minimum possible. Constraints satisfied.↵↵fmincon stopped because the size of the current step is less than↵the value of the step size tolerance and constraints are ↵satisfied to within the value of the constraint tolerance.↵↵<stopping criteria details>↵↵Optimization stopped because the relative changes in all elements of x are↵less than options.StepTolerance = 1.000000e-11, and the relative maximum constraint↵violation, 1.532271e-15, is less than options.ConstraintTolerance = 1.000000e-06.'
            bestfeasible: [1×1 struct]
     objectivederivative: "reverse-AD"
    constraintderivative: "reverse-AD"
                  solver: 'fmincon'

sol2.a = sol2.F;
plottrajandaccel(sol2,p,p0,pF)

What is the mass of the remaining fuel?

for i = 1:length(fnorm)
    fnorm(i) = norm(sol2.F(i,:));
end
m2 = 2 - r*t*sum(fnorm)

m2 = 1.6474

What is the maximum applied acceleration with this reduced mass?

Fmax/m2

ans = 30.3517

The inclusion of air resistance does not change the fuel usage significantly. The trajectory seems to use air resistance to help slow the object, so the object does not use much fuel for deceleration during the final portion of the journey. Notice that the maximum height of the trajectory with air resistance is under 130, but the maximum height without air resistance is about 300. With the current parameters, air resistance makes a significant difference in the trajectory, but not in fuel usage.

References

[1] Vanderbei, R. J. "Case Studies in Trajectory Optimization: Trains, Planes, and Other Pastimes." Optimization and Engineering 2, 215–243 (2001). Available at https://vanderbei.princeton.edu/tex/trajopt/trajopt.pdf.

Helper Functions

This code creates the setupproblem1 helper function.

function [trajectoryprob,p] = setupproblem1(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;
    a = optimvar("a",N-1,3,"LowerBound",-Amax,"UpperBound",Amax);
    v = optimexpr(N,3);
    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
    np = 2:N;
    n0 = 1:(N-1);
    v0 = [0 0 0];
    if air
        v(1, :) = v0;
        for i = 2:N
            v(i,:) = v(i-1,:)*exp(-t) + t*(a(i-1,:) + g);
        end
    else
        gbig = repmat(g,size(a,1),1);
        v = cumsum([v0; t*(a + gbig)]);
    end
    p = cumsum([p0;t*(v(np,:) + v(n0,:))/2]); % Independent of "air"
    endcons = v(N,:) == [0 0 0];
    pcons2 = p(N,:) == pF;
    trajectoryprob.Constraints.acons = acons;
    trajectoryprob.Constraints.scons = scons;
    trajectoryprob.Constraints.endcons = endcons;
    trajectoryprob.Constraints.pcons2 = pcons2;
end

This code creates the plottrajandaccel helper function.

function plottrajandaccel(sol,p,p0,pF)
figure
psol = evaluate(p, sol);
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 = setupproblem1(T,air);
    opts = optimoptions("coneprog",Display="none",OptimalityTolerance=1e-9);
    [~,F] = solve(problem,"Options",opts);
end

This code creates the setupproblemfuel1 helper function.

function [trajectoryprob,p] = setupproblemfuel1(T,air)
    r = 1/1000;
    if nargin == 1
        air = false;
    end
    N = 50;
    g = [0 0 -9.81];
    p0 = [0 0 0];
    pF = [5 10 3];
    Fmax = 50;
    t = T/N;
    F = optimvar("F",N-1,3,"LowerBound",-Fmax,"UpperBound",Fmax);
    v = optimexpr(N,3);
    fnorm = sqrt(sum(F(1:N-1,:).^2,2));
    m = 2 - r*t*cumsum(fnorm);
    trajectoryprob = optimproblem;
    trajectoryprob.Objective = sum(fnorm)*t;
    Fcons = fnorm <= Fmax;
    v0 = [0 0 0];
    if air
        v = fcn2optimexpr(@airResistance, v, v0, N, t, F, m, g);
    else
        gbig = repmat(g,size(F,1),1);
        mbig = repmat(m,1,3);
        v = cumsum([v0; t*(F./mbig + gbig)]);
    end
    np = 2:N;
    n0 = 1:(N-1);
    p = cumsum([p0;t*(v(np,:) + v(n0,:))/2]); % Independent of "air"
    endcons = v(N,:) == [0 0 0];
    pcons2 = p(N,:) == pF;
    trajectoryprob.Constraints.Fcons = Fcons;
    trajectoryprob.Constraints.endcons = endcons;
    trajectoryprob.Constraints.pcons2 = pcons2;
end

This code creates the setupproblemfuel2 helper function.

function [trajectoryprob,p] = setupproblemfuel2(T,air)
    r = 1/1000;
    if nargin == 1
        air = false;
    end
    N = 50;
    g = [0 0 -9.81];
    p0 = [0 0 0];
    pF = [5 10 3];
    Fmax = 50;
    t = T/N;
    F = optimvar("F",N-1,3,"LowerBound",-Fmax,"UpperBound",Fmax);
    v = optimexpr(N,3);
    fnorm = sqrt(sum(F(1:N-1,:).^2,2));
    m = 2 - r*t*cumsum(fnorm);
    trajectoryprob = optimproblem;
    % Add cost for acceleration changes
    trajectoryprob.Objective = sum(fnorm)*t + t*1e-4*sum(diff(fnorm).^2);
    Fcons = fnorm <= Fmax;
    v0 = [0 0 0];
    if air
        v = fcn2optimexpr(@airResistance, v, v0, N, t, F, m, g);
    else
        gbig = repmat(g,size(F,1),1);
        mbig = repmat(m,1,3);
        v = cumsum([v0; t*(F./mbig + gbig)]);
    end
    np = 2:N;
    n0 = 1:(N-1);
    p = cumsum([p0;t*(v(np,:) + v(n0,:))/2]); % Independent of "air"
    endcons = v(N,:) == [0 0 0];
    pcons2 = p(N,:) == pF;
    trajectoryprob.Constraints.Fcons = Fcons;
    trajectoryprob.Constraints.endcons = endcons;
    trajectoryprob.Constraints.pcons2 = pcons2;
end

This code creates the airResistance helper function, which is used in setupproblemfuel1 and setupproblemfuel2.

function v = airResistance(v, v0, N, t, F, m, g)
v(1, :) = v0;
for i = 2:N
    v(i,:) = v(i-1,:)*exp(-t) + t*(F(i-1,:)/m(i-1) + g);
end
end