discretized optimal trajectory, problem-凯发k8网页登录
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 solves for the optimal trajectory over a fixed time and uses that solution to find the optimal , meaning the time that minimizes the cost. the example then shows how to include air resistance, and finally how to include the effect of variable mass as the object consumes fuel.
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 . 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 .
problem description
the problem is to move an object from position at time to position at time using a controlled jet. represent position as a vector , velocity as a vector , and applied acceleration as a vector . in continuous time, the equations of motion, including the force of gravity, are
.
the jet applies an acceleration . the resulting acceleration has an added term for gravity.
the maximum acceleration the jet can apply is .
solve the problem by discretizing time. divide time into equal intervals of size . the position at time step is a vector , the velocity is a vector , and the applied acceleration is a vector . 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 as applying to the center of time interval , the integration scheme is a midpoint rule for acceleration. the overall integration scheme gives simple equations: the position and velocity at step depend only on the position, velocity, and acceleration at step . the equations are also easy to modify for air resistance.
the boundary conditions are , , and . 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 , and have the indices of position and velocity go from through instead of through . with this indexing, the acceleration has indices through .
the cost of using the jet to create force for time is . the total cost is the sum of the norms of the accelerations times .
to convert this cost to a linear cost in optimization variables, create variables 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 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 .
the applied acceleration is the main optimization variable in the problem. as vanderbei 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 . for n
steps, 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 . 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 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 causes the cost to be minimal? for too small a time, such as , 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 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 .
[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 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 after time . 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 and a mass . the net acceleration is
.
the rate of change of mass is
where is a constant.
assume that , , and the maximum force is . these assumptions imply that, at time 0, the maximum applied acceleration is , the same maximum acceleration as in the previous model.
the 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.↵↵↵↵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 calls this type of behavior "ringing." to remove the ringing, add some cost for oscillatory acceleration, and then solve again. the 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.↵↵↵↵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.↵↵↵↵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 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 and .
[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.↵↵↵↵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 .
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 = optimexpr(n-1); for i = 1:length(fnorm) fnorm(i) = norm(f(i,:)); end 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 = optimexpr(n-1); for i = 1:length(fnorm) fnorm(i) = norm(f(i,:)); end 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
see also
|