Diagnose Infeasibility of Simple 3-DoF Trajectory Optimization

Question

Ayden Clay 2022-6-13

0
链接

此问题的直接链接

https://ww2.mathworks.cn/matlabcentral/answers/1739385-diagnose-infeasibility-of-simple-3-dof-trajectory-optimization

评论： Ayden Clay 2022-6-14

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 个评论
显示 1更早的评论隐藏 1更早的评论

Benjamin Thompson 2022-6-13

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 2022-6-14

Thank you both for the help. I'll follow through the more relevant documentation.

请先登录，再进行评论。

请先登录，再回答此问题。

Answer 1

Bjorn Gustavsson 2022-6-13

0
链接

此回答的直接链接

https://ww2.mathworks.cn/matlabcentral/answers/1739385-diagnose-infeasibility-of-simple-3-dof-trajectory-optimization#answer_984775

在 MATLAB Online 中打开

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 个评论
显示 5更早的评论隐藏 5更早的评论

Ayden Clay 2022-6-14

在 MATLAB Online 中打开

I've tried to recreate my problem with the new discrete version linked above but am struggling to get the same output as before. I'm now getting an error about operands needing to be convertible to logical forms.

function [sol,fval,eflag,output] = DiscreteOptimization(T)
% [sol,fval,eflag,output]
N = 50;
g = 9.81;
degs = pi/180;
x0 = [50 15*degs 0 0]';
xN = [0 0 50 0]';
t = ceil(T/N);
x = optimvar('x',4,N); % v, g, x, h.
u = optimvar('u',2,N-1); % 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.
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;
[sol,fval,eflag,output] = solve(prob);
end
function dydt = dfu(y,u)
g = 9.81;
dydt = zeros(1,4);
dydt(1) = -g*sin(y(2)) + u(1);
dydt(2) = -(g*cos(y(2)))/y(1) + u(2);
dydt(3) = y(1)*cos(y(2));
dydt(4) = y(1)*sin(y(2));
end

Alan Weiss 2022-6-14

在 MATLAB Online 中打开

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 2022-6-14

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!

请先登录，再进行评论。

Diagnose Infeasibility of Simple 3-DoF Trajectory Optimization

3 个评论
显示 1更早的评论隐藏 1更早的评论

回答（1 个）

7 个评论
显示 5更早的评论隐藏 5更早的评论

另请参阅

类别

标签

产品

版本

Community Treasure Hunt

Diagnose Infeasibility of Simple 3-DoF Trajectory Optimization

3 个评论 显示 1更早的评论隐藏 1更早的评论

回答（1 个）

7 个评论 显示 5更早的评论隐藏 5更早的评论

另请参阅

类别

标签

产品

版本

Community Treasure Hunt

3 个评论
显示 1更早的评论隐藏 1更早的评论

7 个评论
显示 5更早的评论隐藏 5更早的评论