% point stabilization + Single shooting
clear all
close all
clc
import casadi.*
T = 0.15; % sampling time [s]
N = 25; % prediction horizon
rob_diam = 4.88; %visualization
Tp_max = 400; Tp_min = -400;
Ts_max = 400; Ts_min = -400;
u=SX.sym('u'); v = SX.sym('v'); r = SX.sym('r');
x = SX.sym('x'); y = SX.sym('y'); psi = SX.sym('psi');
obs = SX.sym('obs');
states = [u;v;r;x;y;psi;obs]; n_states = length(states);
Tp = SX.sym('Tp');Ts = SX.sym('Ts');
controls = [Tp;Ts]; n_controls = length(controls);
% mathetical model
rhs = [-1.1391*u+0.0028*(Tp+Ts)+0.6836;
0.0161*v-0.0052*r+0.002*(Tp-Ts)*2.44/2+0.0068;
8.2861*v-0.9860*r+0.0307*(Tp-Ts)*2.44/2+1.3276;
u*cos(psi)-v*sin(psi);
u*sin(psi)+v*cos(psi);
r;
-1.5]; % system r.h.s plus part
f = Function('f',{states,controls},{rhs}); % nonlinear mapping function f(x,u)
U = SX.sym('U',n_controls,N); % Decision variables (controls)
P = SX.sym('P',n_states + n_states); %stares and ref
% parameters (which include the initial and the reference state of the robot)
X = SX.sym('X',n_states,(N+1));
% A Matrix that represents the states over the optimization problem.
% compute solution symbolically
X(:,1) = P(1:7); % initial state
for k = 1:N
st = X(:,k); con = U(:,k);
f_value = f(st,con);
%4-RK
% k1 = f(st, con); % new
% k2 = f(st + T/2*k1, con); % new
% k3 = f(st + T/2*k2, con); % new
% k4 = f(st + T*k3, con); % new
% st_next=st +T/6*(k1 +2*k2 +2*k3 +k4); % new
st_next = st+ (T*f_value);
X(:,k+1) = st_next;
end
% this function to get the optimal trajectory knowing the optimal solution
ff=Function('ff',{U,P},{X});
obj = 0; % Objective function
g = []; % constraints vector
Q = zeros(7,7); Q(1,1)=1; Q(2,2)=0.1;Q(3,3)=0.1; Q(4,4) = 100;Q(5,5) = 100;Q(6,6) = 0.1; % weighing matrices (states)
R = zeros(2,2); R(1,1) = 0.03; R(2,2) = 0.001;% weighing matrices (controls)
% compute objective
for k=1:N
st = X(:,k); con = U(:,k);
obj = obj+(st-P(8:14))'*Q*(st-P(8:14)) + con'*R*con; % calculate obj
end
% Add constraints for collision avoidance
obs_x = 0; % meters
Beam = 2.44;
% compute constraints
for k = 1:N+1 % box constraints due to the map margins
g = [g ; -sqrt((X(4,k)-obs_x)^2+(X(5,k)-X(7,k))^2) + (4.88+0.5)];
end
% make the decision variables one column vector
OPT_variables = reshape(U,n_controls*N,1); %variables must be vector
nlp_prob = struct('f', obj, 'x', OPT_variables, 'g', g, 'p', P);
opts = struct;
opts.ipopt.max_iter = 100;
opts.ipopt.print_level =0;%0,3
opts.print_time = 0;
opts.ipopt.acceptable_tol =1e-8;
opts.ipopt.acceptable_obj_change_tol = 1e-6;
solver = nlpsol('solver', 'ipopt', nlp_prob,opts);
args = struct;
% inequality constraints (state constraints)
args.lbg = -inf; % inequality constraints
args.ubg = 0; % inequality constraints
% input constraints
args.lbx(1:2:2*N-1,1) = Ts_min;
args.ubx(1:2:2*N-1,1) = Tp_max;
args.lbx(2:2:2*N,1) = Ts_min;
args.ubx(2:2:2*N,1) = Ts_max;
%----------------------------------------------
% ALL OF THE ABOVE IS JUST A PROBLEM SETTING UP
% THE SIMULATION LOOP SHOULD START FROM HERE
%-------------------------------------------
t0 = 0;
x0 = [0;0;0; 0 ; 0 ; 90*pi/180; 55]; % initial condition.
load ref_usv_line2 ref_usv_line2
ref = ref_usv_line2(1:7,1:1:end); % Reference trajectory
xx(:,1) = x0; % xx contains the history of states
t(1) = t0;
u0 = zeros(N,2); % 2 control inputs
sim_tim = 35; % Maximum simulation time
% Start MPC
mpciter = 0;
xx1 = [];
u_cl=[];
% the main simulaton loop... it works as long as the error is greater
% than 10^-2 and the number of mpc steps is less than its maximum
% value.
main_loop = tic;
while(mpciter < sim_tim / T)
xs = ref(:,mpciter+1);
args.p = [x0;xs]; % set the values of the parameters vector
args.x0 = reshape(u0',n_controls*N,1); % initial value of the optimization variables
%tic
sol = solver('x0', args.x0, 'lbx', args.lbx, 'ubx', args.ubx,...
'lbg', args.lbg, 'ubg', args.ubg,'p',args.p);
%toc
uu = reshape(full(sol.x)',n_controls,N)';
ff_value = ff(uu',args.p); % compute OPTIMAL solution TRAJECTORY
xx1(:,1:7,mpciter+1)= full(ff_value)';
u_cl= [u_cl ; uu(1,:)];
t(mpciter+1) = t0;
[t0, x0, u0] = shift_control2ship(T, t0, x0, uu,f); % get the initialization of the next optimization step
xx(:,mpciter+2) = x0;
mpciter
mpciter = mpciter + 1;
end
main_loop_time = toc(main_loop);
ss_error = norm((x0-xs),2)
average_mpc_time = main_loop_time/(mpciter+1)
% U_pre = xx(1,2:end);V_pre = xx(2,2:end); R_pre = xx(3,2:end);
% X_pre = xx(4,:);Y_pre = xx(5,:); YY_pre=xx(7,:);
% plot(X_pre,Y_pre);hold on
% plot(zeros(1,301),YY_pre);axis([-5 5 0 60])
obs_diam = 4;
Draw_MPC_PSship_tracking_dynamicObstacles (t,xx,xx1,u_cl,xs,N,rob_diam,obs_diam,ref)在此代码上进行修改，加入卷积神经网络，提高模型的泛化能力和识别准确度。要完整代码
从12年初踏入IT行业，作者经历了半年的WebService开发和大部分时间的Android应用开发。通过自学，他亲手开发了五个APP和一个LBS应用，尽管过程充满挑战，但作者始终怀揣成为牛人的梦想，并相信每一次尝试都能带来成长。
）
扫一扫
4071
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
