fmincon solver not working

Question

Ishu Jaiswal 2022-3-27

0
链接

此问题的直接链接

https://ww2.mathworks.cn/matlabcentral/answers/1681914-fmincon-solver-not-working

评论： Matt J 2022-4-1

close all;
clear all;
clc;
%u = sym('u', [4,2]);
A = [];
b = [];
Aeq = [];
beq = [];
lb = [-4;4];
ub = [ -4;4];
X0 = [1,1];
xd = [3,3].*ones(5,2);
[p,fval] = fmincon(@(u)running_cost(u(1),u(2),u(3),u(4),u(5),u(6),u(7),u(8)),X0,A,b,Aeq,beq,lb,ub);
function x_next = sysdynamics(current_state, current_input, sampling_time)
x(1) = current_state(1);
x(2) = current_state(2);
T = sampling_time;
v = current_input(1);
theta = current_input(2);
x_next = sym(zeros(1,2));
x_next(1) = x(1) + T*v*cos(theta);
x_next(2) = x(2) + T*v*sin(theta);
%next_state = [x_next(1), x_next(2)];
end
function [cost] = running_cost(u1,u2,u3,u4,u5,u6,u7,u8)
%M = 10; % number of subintervals between each sampling time
current_state=[1,1];
desired_state=[3,3].*ones(5,2);
prediction_horizon= 4;
Np = prediction_horizon;
T = 1; %(1/M);
Xd = desired_state; % size(41x2)
Xk = sym(zeros(Np+1, 2)); % size(41x2)
Xk(1,:) = current_state;
Uk(1,:) = [u1,u2];
Uk(2,:) =  [u3,u4];
Uk(3,:) =  [u5,u6];
Uk(4,:) =  [u7,u8];
cost1 = 0;
cost2 = 0;
cost3 = 0;
cost4 = 0;
for k = 1:Np
    
    Xk(k+1,:) = sysdynamics(Xk(k,:),Uk(k,:), T);           
end 
for i = 1:Np               
    cost1 = cost1 + (Xk(i +1, 1) - Xd(i + 1, 1))^2;
    cost2 = cost2 + (Xk(i +1, 2) - Xd(i +1, 2))^2;
    cost3  = cost3 + (Uk(i, 1))^2;
    cost4 = cost4 + (Uk(i, 2))^2;
    cost = cost1 + cost2 + cost3 + cost4;      
    
end
end