Diagnose Infeasibility of Simple 3-DoF Trajectory Optimization

1 Ansicht (letzte 30 Tage)
Ayden Clay
Ayden Clay am 13 Jun. 2022
Kommentiert: Ayden Clay am 14 Jun. 2022
I am trying to write a simple optimization program which finds the best initial launch angle and velocity to arrive at a target location. I have written - using this mathworks tutorial as a guide - a program which I believe ought to do this.
First, I define and simulate for a randomly chosen initial guess the trajectory of a 3-DoF aircraft model (and define some constants).
% Constants.
g = 9.81; % gravity
dt = 1e-1;
tspan = 0:dt:5;
e = [50,0]; % target loc.
degs = pi/180; % conversion.
a = 30*degs; % a is angle of launch in rads.
s = 50; % velocity of vehicle at launch.
x = 0;
h = 0;
y0 = [s,a,x,h]';
[~,Y] = ode45(@(t,y) dfu(t,y),tspan,y0);
figure
hold on
plot(Y(:,3),Y(:,4),'k')
plot(e(1),e(2),'bx')
Next, I set up the optimization program and determine the solution.
% Setup Optimization Problem
r = optimvar('r',1,'LowerBound',0,'UpperBound',90*degs);
v = optimvar('v',1,'LowerBound',0,'UpperBound',100);
f = fcn2optimexpr(@RtoODE,r,v,tspan,[v,r,x,h]);
obj = 0;
prob = optimproblem("Objective",obj);
prob.Constraints.terminalx = f(end,3) == e(1);
prob.Constraints.terminaly = f(end,4) == e(2);
r0.r = a;
r0.v = s;
options = optimoptions(@fmincon,'Display','iter');
[rsol,~] = solve(prob,r0,'Options',options);
Solving problem using fmincon. First-order Norm of Iter F-count f(x) Feasibility optimality step 0 3 0.000000e+00 4.520e+01 0.000e+00 1 6 0.000000e+00 4.176e+01 0.000e+00 4.799e-01 2 9 0.000000e+00 4.060e+01 0.000e+00 2.824e-01 3 12 0.000000e+00 4.031e+01 0.000e+00 1.569e-01 4 15 0.000000e+00 4.023e+01 0.000e+00 7.196e-02 5 18 0.000000e+00 4.021e+01 0.000e+00 3.717e-02 6 21 0.000000e+00 4.020e+01 0.000e+00 2.057e-02 7 24 0.000000e+00 4.020e+01 0.000e+00 1.345e-02 8 27 0.000000e+00 4.020e+01 0.000e+00 1.094e-02 9 30 0.000000e+00 4.019e+01 0.000e+00 1.021e-02 10 33 0.000000e+00 4.019e+01 0.000e+00 1.001e-02 11 36 0.000000e+00 4.019e+01 0.000e+00 9.964e-03 12 39 0.000000e+00 4.019e+01 0.000e+00 9.949e-03 13 42 0.000000e+00 4.019e+01 0.000e+00 9.946e-03 14 45 0.000000e+00 4.018e+01 0.000e+00 9.941e-03 15 48 0.000000e+00 4.018e+01 0.000e+00 9.933e-03 16 51 0.000000e+00 4.018e+01 0.000e+00 9.928e-03 17 54 0.000000e+00 4.018e+01 0.000e+00 9.938e-03 18 57 0.000000e+00 4.018e+01 0.000e+00 9.832e-03 19 60 0.000000e+00 4.017e+01 0.000e+00 9.925e-03 20 63 0.000000e+00 4.017e+01 0.000e+00 1.016e-02 21 66 0.000000e+00 4.017e+01 0.000e+00 9.898e-03 22 69 0.000000e+00 4.017e+01 0.000e+00 9.422e-03 23 72 0.000000e+00 4.017e+01 0.000e+00 1.026e-02 24 75 0.000000e+00 4.017e+01 0.000e+00 9.164e-03 25 78 0.000000e+00 4.016e+01 0.000e+00 2.072e-02 26 81 0.000000e+00 4.011e+01 0.000e+00 2.642e-01 27 84 0.000000e+00 4.011e+01 0.000e+00 1.351e-02 28 87 0.000000e+00 4.010e+01 0.000e+00 1.351e-02 29 90 0.000000e+00 4.010e+01 0.000e+00 1.287e-02 30 93 0.000000e+00 4.010e+01 0.000e+00 7.211e-03 First-order Norm of Iter F-count f(x) Feasibility optimality step 31 96 0.000000e+00 4.010e+01 0.000e+00 3.624e-03 32 99 0.000000e+00 4.006e+01 0.000e+00 1.711e-01 33 102 0.000000e+00 4.006e+01 0.000e+00 9.214e-03 34 105 0.000000e+00 3.973e+01 0.000e+00 1.683e+00 35 108 0.000000e+00 3.942e+01 0.000e+00 1.581e+00 36 111 0.000000e+00 3.878e+01 0.000e+00 6.448e+00 37 114 0.000000e+00 3.465e+01 0.000e+00 1.971e+01 38 118 0.000000e+00 3.458e+01 0.000e+00 7.287e-01 39 123 0.000000e+00 3.456e+01 0.000e+00 6.677e-01 40 126 0.000000e+00 3.455e+01 0.000e+00 2.639e-01 41 131 0.000000e+00 3.455e+01 0.000e+00 2.384e-01 42 135 0.000000e+00 3.455e+01 0.000e+00 1.527e-01 43 140 0.000000e+00 3.455e+01 0.000e+00 3.766e-02 44 145 0.000000e+00 3.455e+01 0.000e+00 3.395e-02 45 149 0.000000e+00 3.455e+01 0.000e+00 2.173e-02 46 154 0.000000e+00 3.455e+01 0.000e+00 3.291e-03 47 161 0.000000e+00 3.455e+01 0.000e+00 2.113e-04 48 175 0.000000e+00 3.455e+01 0.000e+00 8.856e-09 49 179 0.000000e+00 3.455e+01 0.000e+00 3.542e-09 Converged to an infeasible point. fmincon stopped because the size of the current step is less than the value of the step size tolerance but constraints are not satisfied to within the value of the constraint tolerance. Consider enabling the interior point method feasibility mode.
y0 = [rsol.v,rsol.r,x,h]';
% Model Production.
[~,Y] = ode45(@(t,y) dfu(t,y),tspan,y0);
plot(Y(:,3),Y(:,4),'r')
% Output check.
angle = rsol.r/degs
angle = 78.6686
velocity = rsol.v
velocity = 79.7992
downrange = Y(end,3)
downrange = 78.3958
altitude = Y(end,4)
altitude = 268.5932
targetx = e(1)
targetx = 50
targety = e(2)
targety = 0
Obviously, this is determining the problem to be infeasible. But I do not suspect this is the case. Just manually tuning some values returns successful solutions to within the required parameters.
Here are the additional functions required to run this program:
function dydt = dfu(~,y)
g = 9.81;
dydt = zeros(4,1);
dydt(1) = -g*sin(y(2));
dydt(2) = -(g*cos(y(2)))/y(1);
dydt(3) = y(1)*cos(y(2));
dydt(4) = y(1)*sin(y(2));
end
function solpts = RtoODE(r,v,tspan,y0)
sol = ode45(@(t,y)dfu(t,y),tspan,y0);
solpts = deval(sol,tspan);
solpts = solpts(3:4,:);
end
I was wondering if perhaps anyone can find my mistake, or offer general advice on simulating and solving optimization problems for this purpose.
Thank you.
  3 Kommentare
Benjamin Thompson
Benjamin Thompson am 13 Jun. 2022
Possibly due to the choice of states here. Try using x, y, x-dot, and y-dot as the state variables. Then calculate the initial launch angle and velocity from the results of the optimization.
Ayden Clay
Ayden Clay am 14 Jun. 2022
Thank you both for the help. I'll follow through the more relevant documentation.

Melden Sie sich an, um zu kommentieren.

Antworten (1)

Bjorn Gustavsson
Bjorn Gustavsson am 13 Jun. 2022
Why not write the equation of motion component-by-component instead:
function dydtd2ydt2 = dfu(~,y)
g = 9.81;
dydtd2ydt2 = zeros(4,1);
dydy = y(3:4);
d2ydt2 = [0;-g];
dydtd2ydt2 = [dydt(:);d2ydt2];
end
Then you look at the ballode-example for how to catch the ground-impact with event-detection. That will definitely give you the postition of the particle as y(2) (presumably vertical) is equal to zero. From that you can determine how far from your target you landed and that is your function to fit. From there fmincon should be able to do the job. Have look at the help, documentation and code for/of ballode.
HTH
  7 Kommentare
Alan Weiss
Alan Weiss am 14 Jun. 2022
I corrected a few minor errors in your syntax. I also changed sin to sind and cos to cosd in the dfu function; was that the right thing to do? I still get no feasible solution, and I'm not sure why.
function [sol,fval,eflag,output] = DiscreteOptimization(T)
N = 50;
g = 9.81;
degs = pi/180;
x0 = [50 15*degs 0 0]';
xN = [0 0 50 0]';
% t = ceil(T/N); % This looks like a mistake to me
t = T/N;
x = optimvar('x',4,N); % v, g, x, h.
ulb = repmat([-2;-5*degs],1,N-1); % Bound for u
uub = -ulb;
u = optimvar('u',2,N-1,'LowerBound',ulb,'UpperBound',uub); % a and r.
% Define problem.
prob = optimproblem;
% Convert nonlinear functions to optimizable.
% integ = fcn2optimexpr(@RK,x,u,t,'OutputSize',[4,1]); % Ignore for now.
dydt = fcn2optimexpr(@dfu,x,u,'OutputSize',[4,1]);
% % Control constraints. % I simply put bounds, which is more efficient
% ucons = optimconstr(2,N-1);
% for i = 1:N-1
% ucons(1,i) = norm(u(1,i)) <= 2;
% ucons(2,i) = norm(u(2,i)) <= 5*degs;
% end
% State constraints.
xcons = optimconstr(4,N+1);
xcons(:,1) = x(:,1) == x0;
xcons(:,N+1) = x(:,N) == xN;
% Dynamic constraints.
for i = 2:N
xcons(:,i) = x(:,i) == x(:,i-1) + dydt*t;
end
% Optimization:
% prob.Constraints.ucons = ucons;
prob.Constraints.xcons = xcons;
starti = zeros(4,N); % Linear interpolation for initial point
for i = 1:4
starti(i,:) = linspace(x0(i),xN(i),N); % linear interpolation
end
start.x = starti;
start.u = zeros(size(u));
opts = optimoptions("fmincon","MaxFunctionEvaluations",1000*N);
[sol,fval,eflag,output] = solve(prob,start,"Options",opts);
end
function dydt = dfu(y,u)
g = 9.81;
dydt = zeros(4,1); % Needed because the output size is 4-by-1
dydt(1) = -g*sind(y(2)) + u(1);
dydt(2) = -(g*cosd(y(2)))/y(1) + u(2);
dydt(3) = y(1)*cosd(y(2));
dydt(4) = y(1)*sind(y(2));
end
Alan Weiss
MATLAB mathematical toolbox documentation
Ayden Clay
Ayden Clay am 14 Jun. 2022
Thank you for correcting my syntax, the change to cosd and sind isn't necessary as I convert everything to radians by multiplying by "degs". Sorry thats my way of converting because then I can say "Oh it's 40 degrees" or "40*degs". I've converted it back and it's exceeding function evaluation limits, probably just a difficult problem for it to solve, but useful to see a working program. Thank you very much!

Melden Sie sich an, um zu kommentieren.

Kategorien

Mehr zu Programming finden Sie in Help Center und File Exchange

Produkte


Version

R2022a

Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!

Translated by