├── Control ├── Linear_Model_Predictive_Control(LMPC) │ ├── Collision_Avoidance_of_Car.m │ ├── Inverted_Pendulum.m │ └── Lateral_Control_of_Car.m ├── NonLinear_Model_Predictive_Control(NMPC) │ ├── CGMRES_method │ │ └── Semi_active_dumper.m │ └── Newton_method │ │ ├── Hovercraft.m │ │ └── Semi_active_dumper.m ├── README.md └── Trajectory_tracking_control(LMPC) │ ├── Lateral_Vehicle_Dynamics_by_YALMIP.m │ ├── Lateral_Vehicle_Dynamics_by_quadprog.m │ ├── mobile_robot_by_YALMIP.m │ └── mobile_robot_by_quadprog.m ├── GuiPlot ├── FigureManager.m ├── PyPlotJuggler.m ├── csvPlotter.m ├── csvPlotter2.m ├── csvPlotter3.m ├── customPlotter.m ├── customPlotter2.m ├── custom_plotter3.m ├── sample.csv ├── simple_data_viewer.m ├── testdata.csv ├── testdata.xlsx └── 新しいテキスト ドキュメント.txt ├── Ipopt ├── configure_flags.txt ├── examples │ ├── examplehs038.m │ ├── examplehs051.m │ ├── examplehs071.m │ ├── examplelasso.m │ └── lasso.m ├── ipopt.m ├── ipopt.mexa64 ├── ipopt.mexmaci64 ├── ipopt.mexw32 ├── ipopt.mexw64 └── ipopt_auxdata.m ├── MathematicalMethod ├── CircleFittingExample.m ├── Copy_of_test_co.slx ├── CurvatureCalcuration.m ├── DegitalFilter │ ├── AdvancedDigitalFilter.m │ └── BasicDigitalFilter.m ├── ErrorMetric.m ├── NonlinearOptimalMethod │ ├── Conjugate_gradient_method.m │ ├── Golden.m │ ├── Newton_method.m │ ├── Quasi_Newton_method.m │ └── SteepestDescentMethod.m ├── SimulinkBlock.m ├── SimulinkBlock_TestCode.m ├── Untitled2.m ├── commentblockdelete.m ├── delete_comment_out.m ├── delete_comment_out2.m ├── main.m └── test_co.slx ├── ModelPredictiveControl ├── Linear_Model_Predictive_Control(LMPC) │ ├── Collision_Avoidance_of_Car.m │ ├── Inverted_Pendulum.m │ └── Lateral_Control_of_Car.m ├── NonLinear_Model_Predictive_Control(NMPC) │ ├── CGMRES_method │ │ └── Semi_active_dumper.m │ └── Newton_method │ │ ├── Hovercraft.m │ │ └── Semi_active_dumper.m ├── README.md └── Trajectory_tracking_control(LMPC) │ ├── Lateral_Vehicle_Dynamics_by_YALMIP.m │ ├── Lateral_Vehicle_Dynamics_by_quadprog.m │ ├── mobile_robot_by_YALMIP.m │ └── mobile_robot_by_quadprog.m ├── PathPlanning ├── BSplinePath │ └── bspline_path.m ├── BezierPath │ └── bezier_path.m ├── ClothoidPath │ ├── clothoid_path_generation.m │ └── simple_clothoid_path_generation.m ├── CubicSpline │ ├── CubicSpline1D.m │ ├── CubicSpline1D_test.m │ ├── CubicSpline2D.m │ └── CubicSpline2D_test.m ├── FrenetOptimalTrajectory │ ├── FrenetPath.m │ └── main.m └── QuinticPolynomialsPlanner │ ├── QuarticPolynomial.m │ ├── QuinticPolynomial.m │ └── QuinticPolynomialExample.m ├── README.md └── VehicleDynamics ├── DynamicBicycleModel.m ├── HandleMotionAnimation.m ├── KinematicBicycleModel.m ├── UnicycleModel.m ├── VehicelMotionSimlation.m ├── VehicleMotionAnimation.m ├── going_down_slope.m └── going_up_slope.m /Control/Linear_Model_Predictive_Control(LMPC)/Collision_Avoidance_of_Car.m: -------------------------------------------------------------------------------- 1 | clear; 2 | close all; 3 | clc; 4 | 5 | %% Ipopt solver path 6 | dir = pwd; 7 | addpath(fullfile(dir,'Ipopt')) 8 | 9 | %% Setup and Parameters 10 | x0 = [0.0; 0.0; 0.0; 0.0; 0.0; 0.0]; % Initial state of the inverted pendulum 11 | 12 | dt = 0.1; % sample time 13 | nx = 6; % Number of states 14 | nu = 2; % Number of input 15 | 16 | P = 200 * eye(nx); % Termination cost weight matrix 17 | Q = diag([1.0, 1.0, 1.0, 1.0, 1.0, 1.0]); % Weight matrix of state quantities 18 | R = diag([0.1, 0.1]); % Weight matrix of input quantities 19 | N = 10; % Predictive Horizon 20 | p = 200; % salck weight value 21 | 22 | % Range of states and inputs 23 | xmin = [-5; -5; -5; -5; -5; -5]; 24 | xmax = [6; 6; 5; 5; 5; 5]; 25 | umin = [-1; -5]; 26 | umax = [1; 5]; 27 | 28 | % target state value 29 | x_target = [6.0; 1.0; 0.0; 0.0; 0.0; 0.0]; 30 | 31 | % Obstacle 32 | obs.pos = [3.0; 0.0]; 33 | obs.own_vehicle_diameter = 0.5; % [m] 34 | obs.obstacle_diameter = 0.5; % [m] 35 | obs.r = obs.own_vehicle_diameter + obs.obstacle_diameter; 36 | 37 | %% Linear Car models 38 | M = 1500; % [kg] 39 | I = 2500; % [kgm^2] 40 | lf = 1.1; % [m] 41 | lr = 1.6; % [m] 42 | l = lf + lr; % [m] 43 | Kf = 55000; % [N/rad] 44 | Kr = 60000; % [N/rad] 45 | V = 20; % [m/s] 46 | 47 | system.dt = dt; 48 | system.nx = nx; 49 | system.nu = nu; 50 | 51 | A53 = 2 * (Kf * lf + Kr * lr) / I; 52 | A55 = -2 * (Kf * lf + Kr * lr) / (I * V); 53 | A56 = -2 * (Kf * lf^2 + Kr * lr^2) / I; 54 | A63 = 2 * (Kf + Kr) / M; 55 | A65 = -2 * (Kf + Kr) / (M * V); 56 | A66 = -2 * (Kf * lf + Kr * lr) / (M * V); 57 | system.A = eye(nx) + [0.0, 0.0, 0.0, 1.0, 0.0, 0.0; 58 | 0.0, 0.0, 0.0, 0.0, 1.0, 0.0; 59 | 0.0, 0.0, 0.0, 0.0, 0.0, 1.0; 60 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; 61 | 0.0, 0.0, A53, 0.0, A55, A56; 62 | 0.0, 0.0, A63, 0.0, A65, A66] .* dt; 63 | 64 | B51 = 2 * Kf / M; 65 | B61 = 2 * Kf * lf / I; 66 | system.B = [0.0, 0.0; 67 | 0.0, 0.0; 68 | 0.0, 0.0; 69 | 0.0, 1.0; 70 | B51, 0.0; 71 | B61, 0.0] .* dt; 72 | system.xl = xmin; 73 | system.xu = xmax; 74 | system.ul = umin; 75 | system.uu = umax; 76 | 77 | system.target = x_target; 78 | 79 | %% MPC parameters 80 | params_mpc.Q = Q; 81 | params_mpc.R = R; 82 | params_mpc.P = P; 83 | params_mpc.N = N; 84 | params_mpc.p = p; 85 | 86 | %% main loop 87 | xTrue(:, 1) = x0; 88 | uk(:, 1) = [0; 0]; 89 | time = 3.0; 90 | time_curr = 0.0; 91 | current_step = 1; 92 | solvetime = zeros(1, time / dt); 93 | 94 | while time_curr <= time 95 | % update time 96 | time_curr = time_curr + system.dt; 97 | current_step = current_step + 1; 98 | 99 | % solve mac 100 | tic; 101 | uk(:, current_step) = mpc(xTrue(:, current_step - 1), system, obs, params_mpc); 102 | solvetime(1, current_step - 1) = toc; 103 | 104 | % update state 105 | xTrue(:, current_step) = system.A * xTrue(:, current_step - 1) + system.B * uk(:, current_step); 106 | end 107 | 108 | %% solve average time 109 | avg_time = sum(solvetime) / current_step; 110 | disp(avg_time); 111 | 112 | drow_figure(xTrue, uk, obs, x_target, current_step); 113 | 114 | %% model predictive control 115 | function uopt = mpc(xTrue, system, obs, params_mpc) 116 | % Solve MPC 117 | [feas, ~, u, ~] = solve_mpc(xTrue, system, obs, params_mpc); 118 | if ~feas 119 | uopt = []; 120 | return 121 | else 122 | uopt = u(:, 1); 123 | end 124 | end 125 | 126 | function [feas, xopt, uopt, Jopt] = solve_mpc(xTrue, system, obs, params) 127 | % extract variables 128 | N = params.N; 129 | % define variables and cost 130 | x = sdpvar(system.nx, N+1); 131 | u = sdpvar(system.nu, N); 132 | delta = sdpvar(1, N); 133 | constraints = []; 134 | cost = 0; 135 | 136 | % initial constraint 137 | constraints = [constraints; x(:,1) == xTrue]; 138 | % add constraints and costs 139 | for i = 1:N 140 | constraints = [constraints; 141 | system.xl <= x(:,i) <= system.xu; 142 | system.ul <= u(:,i) <= system.uu 143 | x(:,i+1) == system.A * x(:,i) + system.B * u(:,i)]; 144 | cost = cost + (x(:,i) - system.target)' * params.Q * (x(:,i) - system.target) + u(:,i)' * params.R * u(:,i); 145 | end 146 | % add collition avoidance constraints 147 | for i = 1:N 148 | pos = obs.pos; 149 | r = obs.r; 150 | distance = (x([1:2],i)-pos)'*((x([1:2],i)-pos)) - r^2; 151 | constraints = [constraints; distance >= delta(1, i)]; 152 | cost = cost + delta(1, i)' * params.p * delta(1, i); 153 | end 154 | % add terminal cost 155 | cost = cost + (x(:,N+1) - system.target)' * params.P * (x(:,N+1) - system.target); 156 | ops = sdpsettings('solver','ipopt','verbose',0); 157 | % solve optimization 158 | diagnostics = optimize(constraints, cost, ops); 159 | if diagnostics.problem == 0 160 | feas = true; 161 | xopt = value(x); 162 | uopt = value(u); 163 | Jopt = value(cost); 164 | else 165 | feas = false; 166 | xopt = []; 167 | uopt = []; 168 | Jopt = []; 169 | end 170 | end 171 | 172 | function drow_figure(xlog, ulog, obs, x_target, current_step) 173 | % Plot simulation 174 | figure(1) 175 | subplot(2, 1, 1) 176 | plot(0:current_step - 1, ulog(1,:), 'ko-',... 177 | 'LineWidth', 1.0, 'MarkerSize', 4); 178 | xlabel('Time Step','interpreter','latex','FontSize',10); 179 | ylabel('delta [rad]','interpreter','latex','FontSize',10); 180 | yline(1.0, '--b', 'LineWidth', 2.0); 181 | yline(-1.0, '--b', 'LineWidth', 2.0); 182 | 183 | subplot(2, 1, 2); 184 | plot(0:current_step - 1, ulog(2,:), 'ko-',... 185 | 'LineWidth', 1.0, 'MarkerSize', 4); 186 | xlabel('Time Step','interpreter','latex','FontSize',10); 187 | ylabel('Acceleration [$m/s^2$]','interpreter','latex','FontSize',10); 188 | yline(5.0, '--b', 'LineWidth', 2.0); 189 | yline(-5.0, '--b', 'LineWidth', 2.0); 190 | 191 | figure(2) 192 | % plot trajectory 193 | plot(xlog(1,:), xlog(2,:), 'ko-',... 194 | 'LineWidth', 1.0, 'MarkerSize', 4); 195 | hold on; 196 | grid on; 197 | axis equal; 198 | % plot initial position 199 | plot(xlog(1, 1), xlog(1, 2), 'dm', 'LineWidth', 2); 200 | % plot target position 201 | plot(x_target(1), x_target(2), 'dg', 'LineWidth', 2); 202 | xlabel('X[m]','interpreter','latex','FontSize',10); 203 | ylabel('Y[m]','interpreter','latex','FontSize',10); 204 | % plot obstacle 205 | pos = obs.pos; 206 | r = obs.obstacle_diameter; 207 | th = linspace(0,2*pi*100); 208 | x = cos(th); 209 | y = sin(th); 210 | plot(pos(1) + r*x, pos(2) + r*y, 'r', 'LineWidth', 2); 211 | plot(pos(1), pos(2), 'r', 'MarkerSize', 5, 'LineWidth', 2); 212 | % plot vegicle 213 | for i = 1:current_step 214 | r_vegicle = obs.own_vehicle_diameter; 215 | th = linspace(0,2*pi*100); 216 | x = cos(th); 217 | y = sin(th); 218 | X = xlog(1,i); 219 | Y = xlog(2,i); 220 | plot(X + r_vegicle * x, Y + r_vegicle * y, 'b','LineWidth', 2); 221 | hold on; 222 | end 223 | yline(0.5, '--k', 'LineWidth', 2.0); 224 | yline(2.0, 'k', 'LineWidth', 4.0); 225 | yline(-1.0, 'k', 'LineWidth', 4.0); 226 | 227 | legend('Motion trajectory','Initial position', 'Target position','Obstacle', 'Location','southeast',... 228 | 'interpreter','latex','FontSize',10.0); 229 | end 230 | -------------------------------------------------------------------------------- /Control/Linear_Model_Predictive_Control(LMPC)/Inverted_Pendulum.m: -------------------------------------------------------------------------------- 1 | clear; 2 | close all; 3 | clc; 4 | 5 | %% Ipopt solver path 6 | dir = pwd; 7 | addpath(fullfile(dir,'Ipopt')) 8 | 9 | %% Setup and Parameters 10 | x0 = [-0.02; 0.0; 0.1; 0.0]; % Initial state of the inverted pendulum 11 | 12 | dt = 0.1; % sample time 13 | nx = 4; % Number of states 14 | nu = 1; % Number of input 15 | 16 | P = 30 * eye(nx); % Termination cost weight matrix 17 | Q = diag([1.0, 1.0, 1.0, 1.0]); % Weight matrix of state quantities 18 | R = 0.01; % Weight matrix of input quantities 19 | N = 10; % Predictive Horizon 20 | 21 | % Range of states and inputs 22 | xmin = [-5; -5; -5; -5]; 23 | xmax = [5; 5; 5; 5]; 24 | umin = -5; 25 | umax = 5; 26 | 27 | %% Linear Inverted Pendulum 28 | M = 1.0; % [kg] 29 | m = 0.3; % [kg] 30 | g = 9.8; % [m/s^2] 31 | l = 2.0; % [m] 32 | system.dt = dt; 33 | system.nx = nx; 34 | system.nu = nu; 35 | system.A = eye(nx) + [0.0, 1.0, 0.0, 0.0; 36 | 0.0, 0.0, m * g / M, 0.0; 37 | 0.0, 0.0, 0.0, 1.0; 38 | 0.0, 0.0, g * (M + m) / (l * M), 0.0] .* dt; 39 | system.B = [0.0; 40 | 1.0 / M; 41 | 0.0; 42 | 1.0 / (l * M)] .* dt; 43 | system.xl = xmin; 44 | system.xu = xmax; 45 | system.ul = umin; 46 | system.uu = umax; 47 | 48 | %% MPC parameters 49 | params_mpc.Q = Q; 50 | params_mpc.R = R; 51 | params_mpc.P = P; 52 | params_mpc.N = N; 53 | 54 | %% main loop 55 | xTrue(:, 1) = x0; 56 | uk(:, 1) = 0; 57 | time = 20.0; 58 | time_curr = 0.0; 59 | current_step = 1; 60 | solvetime = zeros(1, time / dt); 61 | 62 | while time_curr <= time 63 | % update time 64 | time_curr = time_curr + system.dt; 65 | current_step = current_step + 1; 66 | 67 | % solve mac 68 | tic; 69 | uk(:, current_step)= mpc(xTrue(:, current_step - 1), system, params_mpc); 70 | solvetime(1, current_step - 1) = toc; 71 | 72 | % update state 73 | xTrue(:, current_step) = system.A * xTrue(:, current_step - 1) + system.B * uk(:, current_step); 74 | end 75 | 76 | %% solve average time 77 | avg_time = sum(solvetime) / current_step; 78 | disp(avg_time); 79 | 80 | drow_figure(xTrue, uk, current_step); 81 | 82 | %% model predictive control 83 | function uopt = mpc(xTrue, system, params_mpc) 84 | % Solve MPC 85 | [feas, ~, u, ~] = solve_mpc(xTrue, system, params_mpc); 86 | if ~feas 87 | uopt = []; 88 | return 89 | else 90 | uopt = u(:,1); 91 | end 92 | end 93 | 94 | function [feas, xopt, uopt, Jopt] = solve_mpc(xTrue, system, params) 95 | % extract variables 96 | N = params.N; 97 | % define variables and cost 98 | x = sdpvar(system.nx, N+1); 99 | u = sdpvar(system.nu, N); 100 | constraints = []; 101 | cost = 0; 102 | 103 | % initial constraint 104 | constraints = [constraints; x(:,1) == xTrue]; 105 | % add constraints and costs 106 | for i = 1:N 107 | constraints = [constraints; 108 | system.xl <= x(:,i) <= system.xu; 109 | system.ul <= u(:,i) <= system.uu 110 | x(:,i+1) == system.A * x(:,i) + system.B * u(:,i)]; 111 | cost = cost + x(:,i)' * params.Q * x(:,i) + u(:,i)' * params.R * u(:,i); 112 | end 113 | % add terminal cost 114 | cost = cost + x(:,N+1)' * params.P * x(:,N+1); 115 | ops = sdpsettings('solver','ipopt','verbose',0); 116 | % solve optimization 117 | diagnostics = optimize(constraints, cost, ops); 118 | if diagnostics.problem == 0 119 | feas = true; 120 | xopt = value(x); 121 | uopt = value(u); 122 | Jopt = value(cost); 123 | else 124 | feas = false; 125 | xopt = []; 126 | uopt = []; 127 | Jopt = []; 128 | end 129 | end 130 | 131 | function drow_figure(xlog, ulog, current_step) 132 | % Plot simulation 133 | figure(1) 134 | subplot(2,2,1) 135 | plot(0:current_step - 1, xlog(1,:), 'ko-',... 136 | 'LineWidth', 1.0, 'MarkerSize', 4); 137 | xlabel('Time Step','interpreter','latex','FontSize',10); 138 | ylabel('X[m]','interpreter','latex','FontSize',10); 139 | yline(0.0, '--r', 'LineWidth', 1.0); 140 | 141 | subplot(2,2,2) 142 | plot(0:current_step - 1, xlog(2,:), 'ko-',... 143 | 'LineWidth', 1.0, 'MarkerSize', 4); 144 | xlabel('Time Step','interpreter','latex','FontSize',10); 145 | ylabel('$\dot{X}$[m/s]','interpreter','latex','FontSize',10); 146 | yline(0.0, '--r', 'LineWidth', 1.0); 147 | 148 | subplot(2,2,3) 149 | plot(0:current_step - 1, xlog(3,:), 'ko-',... 150 | 'LineWidth', 1.0, 'MarkerSize', 4); 151 | xlabel('Time Step','interpreter','latex','FontSize',10); 152 | ylabel('$\theta$[rad]','interpreter','latex','FontSize',10); 153 | yline(0.0, '--r', 'LineWidth', 1.0); 154 | 155 | subplot(2,2,4) 156 | plot(0:current_step - 1, xlog(4,:), 'ko-',... 157 | 'LineWidth', 1.0, 'MarkerSize', 4); 158 | xlabel('Time Step','interpreter','latex','FontSize',10); 159 | ylabel('$\dot{\theta}$[rad/s]','interpreter','latex','FontSize',10); 160 | yline(0.0, '--r', 'LineWidth', 1.0); 161 | 162 | figure(2) 163 | plot(0:current_step - 1, ulog(1,:), 'ko-',... 164 | 'LineWidth', 1.0, 'MarkerSize', 4); 165 | xlabel('Time Step','interpreter','latex','FontSize',10); 166 | ylabel('Input F[N]','interpreter','latex','FontSize',10); 167 | yline(5.0, '--b', 'LineWidth', 2.0); 168 | yline(-5.0, '--b', 'LineWidth', 2.0); 169 | end 170 | -------------------------------------------------------------------------------- /Control/Linear_Model_Predictive_Control(LMPC)/Lateral_Control_of_Car.m: -------------------------------------------------------------------------------- 1 | clear; 2 | close all; 3 | clc; 4 | 5 | %% Ipopt solver path 6 | dir = pwd; 7 | addpath(fullfile(dir,'Ipopt')) 8 | 9 | %% Setup and Parameters 10 | x0 = [0.0; 2.0]; % Initial state of the inverted pendulum 11 | 12 | dt = 0.1; % sample time 13 | v = 30.0; % driving speed 14 | nx = 2; % Number of states 15 | nu = 1; % Number of input 16 | 17 | P = 30 * eye(nx); % Termination cost weight matrix 18 | Q = diag([1.0, 1.0]); % Weight matrix of state quantities 19 | R = 0.01; % Weight matrix of input quantities 20 | N = 20; % Predictive Horizon 21 | 22 | % Range of states and inputs 23 | xmin = [-5; -5]; 24 | xmax = [5; 5]; 25 | umin = -1; 26 | umax = 1; 27 | 28 | x_target = [0.0; 1.0]; % target state value 29 | 30 | %% Linear Inverted Pendulum 31 | system.dt = dt; 32 | system.nx = nx; 33 | system.nu = nu; 34 | system.A = [1.0, 0.0; 35 | v * dt, 1.0]; 36 | system.B = [dt; 37 | 0.5 * v * dt^2]; 38 | system.xl = xmin; 39 | system.xu = xmax; 40 | system.ul = umin; 41 | system.uu = umax; 42 | 43 | system.target = x_target; 44 | 45 | %% MPC parameters 46 | params_mpc.Q = Q; 47 | params_mpc.R = R; 48 | params_mpc.P = P; 49 | params_mpc.N = N; 50 | 51 | %% main loop 52 | xTrue(:, 1) = x0; 53 | uk(:, 1) = 0; 54 | time = 1.0; 55 | time_curr = 0.0; 56 | current_step = 1; 57 | solvetime = zeros(1, time / dt); 58 | 59 | while time_curr <= time 60 | % update time 61 | time_curr = time_curr + system.dt; 62 | current_step = current_step + 1; 63 | 64 | % solve mac 65 | tic; 66 | uk(:, current_step) = mpc(xTrue(:, current_step - 1), system, params_mpc); 67 | solvetime(1, current_step - 1) = toc; 68 | 69 | % update state 70 | xTrue(:, current_step) = system.A * xTrue(:, current_step - 1) + system.B * uk(:, current_step); 71 | end 72 | 73 | %% solve average time 74 | avg_time = sum(solvetime) / current_step; 75 | disp(avg_time); 76 | 77 | drow_figure(xTrue, uk, current_step); 78 | 79 | %% model predictive control 80 | function uopt = mpc(xTrue, system, params_mpc) 81 | % Solve MPC 82 | [feas, ~, u, ~] = solve_mpc(xTrue, system, params_mpc); 83 | if ~feas 84 | uopt = []; 85 | return 86 | else 87 | uopt = u(:,1); 88 | end 89 | end 90 | 91 | function [feas, xopt, uopt, Jopt] = solve_mpc(xTrue, system, params) 92 | % extract variables 93 | N = params.N; 94 | % define variables and cost 95 | x = sdpvar(system.nx, N+1); 96 | u = sdpvar(system.nu, N); 97 | constraints = []; 98 | cost = 0; 99 | 100 | % initial constraint 101 | constraints = [constraints; x(:,1) == xTrue]; 102 | % add constraints and costs 103 | for i = 1:N 104 | constraints = [constraints; 105 | system.xl <= x(:,i) <= system.xu; 106 | system.ul <= u(:,i) <= system.uu 107 | x(:,i+1) == system.A * x(:,i) + system.B * u(:,i)]; 108 | cost = cost + (x(:,i) - system.target)' * params.Q * (x(:,i) - system.target) + u(:,i)' * params.R * u(:,i); 109 | end 110 | % add terminal cost 111 | cost = cost + (x(:,N+1) - system.target)' * params.P * (x(:,N+1) - system.target); 112 | ops = sdpsettings('solver','ipopt','verbose',0); 113 | % solve optimization 114 | diagnostics = optimize(constraints, cost, ops); 115 | if diagnostics.problem == 0 116 | feas = true; 117 | xopt = value(x); 118 | uopt = value(u); 119 | Jopt = value(cost); 120 | else 121 | feas = false; 122 | xopt = []; 123 | uopt = []; 124 | Jopt = []; 125 | end 126 | end 127 | 128 | function drow_figure(xlog, ulog, current_step) 129 | % Plot simulation 130 | figure(1) 131 | subplot(2,1,1) 132 | plot(0:current_step - 1, xlog(1,:), 'ko-',... 133 | 'LineWidth', 1.0, 'MarkerSize', 4); 134 | xlim([0, 11]); 135 | xlabel('Time Step','interpreter','latex','FontSize',10); 136 | ylabel('Heading angle[deg]','interpreter','latex','FontSize',10); 137 | yline(0.0, '--r', 'LineWidth', 1.0); 138 | 139 | subplot(2,1,2); 140 | plot(0:current_step - 1, xlog(2,:), 'ko-',... 141 | 'LineWidth', 1.0, 'MarkerSize', 4); 142 | xlim([0, 11]); 143 | xlabel('Time Step','interpreter','latex','FontSize',10); 144 | ylabel('Lateral error[m]','interpreter','latex','FontSize',10); 145 | yline(1.0, '--r', 'LineWidth', 1.0); 146 | yline(1.5, '--k', 'LineWidth', 2.0); 147 | yline(2.5, 'k', 'LineWidth', 4.0); 148 | yline(0.5, 'k', 'LineWidth', 4.0); 149 | 150 | figure(2) 151 | plot(0:current_step - 1, ulog(1,:), 'ko-',... 152 | 'LineWidth', 1.0, 'MarkerSize', 4); 153 | xlim([0, 11]); 154 | xlabel('Time Step','interpreter','latex','FontSize',10); 155 | ylabel('Steering input[deg/s]','interpreter','latex','FontSize',10); 156 | yline(1.0, '--b', 'LineWidth', 2.0); 157 | yline(-1.0, '--b', 'LineWidth', 2.0); 158 | end 159 | -------------------------------------------------------------------------------- /Control/NonLinear_Model_Predictive_Control(NMPC)/CGMRES_method/Semi_active_dumper.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Ramune6110/MATLAB-Robotics-Engineering/cb44e950c5183f6000c183928f9c65cb3ced82da/Control/NonLinear_Model_Predictive_Control(NMPC)/CGMRES_method/Semi_active_dumper.m -------------------------------------------------------------------------------- /Control/NonLinear_Model_Predictive_Control(NMPC)/Newton_method/Hovercraft.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Ramune6110/MATLAB-Robotics-Engineering/cb44e950c5183f6000c183928f9c65cb3ced82da/Control/NonLinear_Model_Predictive_Control(NMPC)/Newton_method/Hovercraft.m -------------------------------------------------------------------------------- /Control/NonLinear_Model_Predictive_Control(NMPC)/Newton_method/Semi_active_dumper.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Ramune6110/MATLAB-Robotics-Engineering/cb44e950c5183f6000c183928f9c65cb3ced82da/Control/NonLinear_Model_Predictive_Control(NMPC)/Newton_method/Semi_active_dumper.m -------------------------------------------------------------------------------- /Control/README.md: -------------------------------------------------------------------------------- 1 | # Model Predictive Control(MPC) 2 | This directory provides Model Predictive Control sample code and documentation. 3 | 4 | ## Linear Model Predictive Control(LMPC) 5 | ![Fig10](https://user-images.githubusercontent.com/52307432/160145851-42205af4-de87-4490-a7b2-eff77d1de7b6.png) 6 | 7 | Sample code for automobile collision avoidance. 8 | 9 | Documents in Japanease: 10 | 線形モデル予測制御入門：MATLABサンプルプログラム - Ramune6110 11 | https://ramune6110.hatenablog.com/entry/2022/02/13/154405 12 | 13 | ## Trajectory tracking control(LMPC) 14 | ![fig3](https://user-images.githubusercontent.com/52307432/160147124-ef1dc4a0-5ee6-42bb-a416-6248afba4583.png) 15 | 16 | Sample code for automobile trajectory tracking control. 17 | 18 | Documents in Japanease: 19 | 二次計画法に基づく線形モデル予測制御による軌道追従制御：MATLABサンプルプログラム - Ramune6110 20 | https://ramune6110.hatenablog.com/entry/2022/02/24/000153 21 | 22 | ## Nonlinear Model Predictive Control(NMPC) 23 | ### Newton method 24 | ![fig11](https://user-images.githubusercontent.com/52307432/160147602-91d9b7cb-d87c-4deb-ada6-cb18a2f1df23.png) 25 | 26 | Sample code for automobile position control. 27 | 28 | Documents in Japanease: 29 | ニュートン法に基づく非線形モデル予測制御：MATLABサンプルプログラム - Ramune6110 30 | https://ramune6110.hatenablog.com/entry/2022/03/21/203716 31 | 32 | # Author 33 | Ramune6110([@ExcitingRamune](https://twitter.com/ExcitingRamune)) 34 | -------------------------------------------------------------------------------- /Control/Trajectory_tracking_control(LMPC)/Lateral_Vehicle_Dynamics_by_YALMIP.m: -------------------------------------------------------------------------------- 1 | clear; 2 | close all; 3 | clc; 4 | 5 | %% Ipopt solver path 6 | dir = pwd; 7 | addpath(fullfile(dir,'Ipopt')) 8 | 9 | %% Choice the type of trajectory 10 | while true 11 | model = input('Please choose the type of trajectory (1 = Lane change, 2 = Circle): '); 12 | switch model 13 | case 1 14 | trajectory_type = 'Lane change'; 15 | break; 16 | case 2 17 | trajectory_type = 'Circle'; 18 | break; 19 | otherwise 20 | disp('No System!'); 21 | end 22 | end 23 | 24 | %% Setup and Parameters 25 | dt = 0.1; % sample time 26 | nx = 4; % Number of states 27 | ny = 2; % Number of ovservation 28 | nu = 1; % Number of input 29 | 30 | S = diag([10.0, 1.0]); % Termination cost weight matrix 31 | Q = diag([10.0, 1.0]); % Weight matrix of state quantities 32 | R = 30.0; % Weight matrix of input quantities 33 | N = 15; % Predictive Horizon 34 | 35 | % Range of inputs 36 | umin = -pi / 60; 37 | umax = pi / 60; 38 | 39 | %% Linear Car models (Lateral Vehicle Dynamics) 40 | M = 1500; % [kg] 41 | I = 3000; % [kgm^2] 42 | lf = 1.2; % [m] 43 | lr = 1.6; % [m] 44 | l = lf + lr; % [m] 45 | Kf = 19000; % [N/rad] 46 | Kr = 33000; % [N/rad] 47 | Vx = 20; % [m/s] 48 | 49 | system.dt = dt; 50 | system.nx = nx; 51 | system.ny = ny; 52 | system.nu = nu; 53 | 54 | % states = [y_dot, psi, psi_dot, Y]; 55 | % Vehicle Dynamics and Control (2nd edition) by Rajesh Rajamani. They are in Chapter 2.3. 56 | A11 = -2 * (Kf + Kr)/(M * Vx); 57 | A13 = -Vx - 2 * (Kf*lf - Kr*lr)/(M * Vx); 58 | A31 = -2 * (lf*Kf - lr*Kr)/(I * Vx); 59 | A33 = -2 * (lf^2*Kf + lr^2*Kr)/(I * Vx); 60 | system.A = eye(nx) + [A11, 0.0, A13, 0.0; 61 | 0.0, 0.0, 1.0, 0.0; 62 | A31, 0.0, A33, 0.0; 63 | 1.0, Vx, 0.0, 0.0] * dt; 64 | 65 | B11 = 2*Kf/M; 66 | B13 = 2*lf*Kf/I; 67 | system.B = [B11; 68 | 0.0; 69 | B13; 70 | 0.0] * dt; 71 | 72 | system.C = [0 1 0 0; 73 | 0 0 0 1]; 74 | 75 | system.A_aug = [system.A, system.B; 76 | zeros(length(system.B(1,:)),length(system.A(1,:))),eye(length(system.B(1,:)))]; 77 | system.B_aug = [system.B; 78 | eye(length(system.B(1,:)))]; 79 | system.C_aug = [system.C,zeros(length(system.C(:,1)),length(system.B(1,:)))]; 80 | 81 | % Constants 82 | system.Vx = Vx; 83 | system.M = M; 84 | system.I = I; 85 | system.Kf = Kf; 86 | system.Kr = Kr; 87 | system.lf = lf; 88 | system.lr = lr; 89 | 90 | % input 91 | system.ul = umin; 92 | system.uu = umax; 93 | 94 | %% Load the trajectory 95 | if strcmp(trajectory_type, 'Lane change') 96 | t = 0:dt:10.0; 97 | elseif strcmp(trajectory_type, 'Circle') 98 | t = 0:dt:80.0; 99 | end 100 | [x_ref, y_ref, psi_ref] = trajectory_generator(t, Vx, trajectory_type); 101 | 102 | sim_length = length(t); % Number of control loop iterations 103 | 104 | refSignals = zeros(length(x_ref(:, 1)) * ny, 1); 105 | 106 | ref_index = 1; 107 | for i = 1:ny:length(refSignals) 108 | refSignals(i) = psi_ref(ref_index, 1); 109 | refSignals(i + 1) = y_ref(ref_index, 1); 110 | ref_index = ref_index + 1; 111 | end 112 | 113 | % initial state 114 | x0 = [0.0; psi_ref(1, 1); 0.0; y_ref(1, 1)]; % Initial state of Lateral Vehicle Dynamics 115 | 116 | %% MPC parameters 117 | params_mpc.Q = Q; 118 | params_mpc.R = R; 119 | params_mpc.S = S; 120 | params_mpc.N = N; 121 | 122 | %% main loop 123 | xTrue(:, 1) = x0; 124 | uk(:, 1) = 0.0; 125 | du(:, 1) = 0.0; 126 | current_step = 1; 127 | solvetime = zeros(1, sim_length); 128 | 129 | ref_sig_num = 1; % for reading reference signals 130 | for i = 1:sim_length-15 131 | % update time 132 | current_step = current_step + 1; 133 | 134 | % Generating the current state and the reference vector 135 | xTrue_aug = [xTrue(:, current_step - 1); uk(:, current_step - 1)]; 136 | ref_sig_num = ref_sig_num + ny; 137 | 138 | if ref_sig_num + ny * N - 1 <= length(refSignals) 139 | ref = refSignals(ref_sig_num:ref_sig_num+ny*N-1); 140 | else 141 | ref = refSignals(ref_sig_num:length(refSignals)); 142 | N = N - 1; 143 | end 144 | 145 | % solve mac 146 | tic; 147 | du(:, current_step) = mpc(xTrue_aug, system, params_mpc, ref); 148 | solvetime(1, current_step - 1) = toc; 149 | 150 | % add du input 151 | uk(:, current_step) = uk(:, current_step - 1) + du(:, current_step); 152 | 153 | % update state 154 | T = dt*i:dt:dt*i+dt; 155 | [T, x] = ode45(@(t,x) nonlinear_lateral_car_model(t, x, uk(:, current_step), system), T, xTrue(:, current_step - 1)); 156 | xTrue(:, current_step) = x(end,:); 157 | end 158 | 159 | %% solve average time 160 | avg_time = sum(solvetime) / current_step; 161 | disp(avg_time); 162 | 163 | drow_figure(xTrue, uk, du, x_ref, y_ref, current_step, trajectory_type); 164 | 165 | %% model predictive control 166 | function uopt = mpc(xTrue_aug, system, params, ref) 167 | % Solve MPC 168 | [feas, ~, u, ~] = solve_mpc(xTrue_aug, system, params, ref); 169 | if ~feas 170 | uopt = []; 171 | return 172 | else 173 | uopt = u(:, 1); 174 | end 175 | end 176 | 177 | function [feas, xopt, uopt, Jopt] = solve_mpc(xTrue_aug, system, params, ref) 178 | % extract variables 179 | N = params.N; 180 | % define variables and cost 181 | x = sdpvar(system.nx + system.nu, N+1); 182 | u = sdpvar(system.nu, N); 183 | constraints = []; 184 | cost = 0; 185 | ref_index = 1; 186 | 187 | % initial constraint 188 | constraints = [constraints; x(:,1) == xTrue_aug]; 189 | % add constraints and costs 190 | for i = 1:N-1 191 | constraints = [constraints; 192 | system.ul <= u(:,i) <= system.uu 193 | x(:,i+1) == system.A_aug * x(:,i) + system.B_aug * u(:,i)]; 194 | cost = cost + (ref(ref_index:ref_index + 1,1) - system.C_aug * x(:,i+1))' * params.Q * (ref(ref_index:ref_index + 1,1) - system.C_aug * x(:,i+1)) + u(:,i)' * params.R * u(:,i); 195 | ref_index = ref_index + system.ny; 196 | end 197 | % add terminal cost 198 | cost = cost + (ref(ref_index:ref_index+1,1) - system.C_aug * x(:,N+1))' * params.S * (ref(ref_index:ref_index+1,1) - system.C_aug * x(:,N+1)); 199 | ops = sdpsettings('solver','ipopt','verbose',0); 200 | % solve optimization 201 | diagnostics = optimize(constraints, cost, ops); 202 | if diagnostics.problem == 0 203 | feas = true; 204 | xopt = value(x); 205 | uopt = value(u); 206 | Jopt = value(cost); 207 | else 208 | feas = false; 209 | xopt = []; 210 | uopt = []; 211 | Jopt = []; 212 | end 213 | end 214 | 215 | %% trajectory generator 216 | function [x_ref, y_ref, psi_ref] = trajectory_generator(t, Vx, trajectory_type) 217 | if strcmp(trajectory_type, 'Lane change') 218 | x = linspace(0, Vx * t(end), length(t)); 219 | y = 3 * tanh((t - t(end) / 2)); 220 | elseif strcmp(trajectory_type, 'Circle') 221 | radius = 30; 222 | period = 1000; 223 | for i = 1:length(t) 224 | x(1, i) = radius * sin(2 * pi * i / period); 225 | y(1, i) = -radius * cos(2 * pi * i / period); 226 | end 227 | end 228 | 229 | dx = x(2:end) - x(1:end-1); 230 | dy = y(2:end) - y(1:end-1); 231 | 232 | psi = zeros(1,length(x)); 233 | psiInt = psi; 234 | 235 | psi(1) = atan2(dy(1),dx(1)); 236 | psi(2:end) = atan2(dy(:),dx(:)); 237 | dpsi = psi(2:end) - psi(1:end-1); 238 | 239 | psiInt(1) = psi(1); 240 | for i = 2:length(psiInt) 241 | if dpsi(i - 1) < -pi 242 | psiInt(i) = psiInt(i - 1) + (dpsi(i - 1) + 2 * pi); 243 | elseif dpsi(i - 1) > pi 244 | psiInt(i) = psiInt(i - 1) + (dpsi(i - 1) - 2 * pi); 245 | else 246 | psiInt(i) = psiInt(i - 1) + dpsi(i - 1); 247 | end 248 | end 249 | 250 | x_ref = x'; 251 | y_ref = y'; 252 | psi_ref = psiInt'; 253 | end 254 | 255 | function dx = nonlinear_lateral_car_model(t, xTrue, u, system) 256 | % In this simulation, the body frame and its transformation is used 257 | % instead of a hybrid frame. That is because for the solver ode45, it 258 | % is important to have the nonlinear system of equations in the first 259 | % order form. 260 | 261 | % Get the constants from the general pool of constants 262 | Vx = system.Vx; 263 | M = system.M; 264 | I = system.I; 265 | Kf = system.Kf; 266 | Kr = system.Kr; 267 | lf = system.lf; 268 | lr = system.lr; 269 | 270 | % % Assign the states 271 | y_dot = xTrue(1); 272 | psi = xTrue(2); 273 | psi_dot = xTrue(3); 274 | 275 | % The nonlinear equation describing the dynamics of the car 276 | dx(1,1) = -(2*Kf+2*Kr)/(M * Vx)*y_dot+(-Vx-(2*Kf*lf-2*Kr*lr)/(M * Vx))*psi_dot+2*Kf/M*u; 277 | dx(2,1) = psi_dot; 278 | dx(3,1) = -(2*lf*Kf-2*lr*Kr)/(I * Vx)*y_dot-(2*lf^2*Kf+2*lr^2*Kr)/(I * Vx)*psi_dot+2*lf*Kf/I*u; 279 | dx(4,1) = sin(psi) * Vx + cos(psi) * y_dot; 280 | end 281 | 282 | function drow_figure(xTrue, uk, du, x_ref, y_ref, current_step, trajectory_type) 283 | % Plot simulation 284 | figure(1) 285 | subplot(2, 1, 1) 286 | plot(0:current_step - 1, du(1,:), 'ko-',... 287 | 'LineWidth', 1.0, 'MarkerSize', 4); 288 | xlabel('Time Step','interpreter','latex','FontSize',10); 289 | ylabel('$\Delta \delta$ [rad]','interpreter','latex','FontSize',10); 290 | yline(pi / 60, '--b', 'LineWidth', 2.0); 291 | yline(-pi / 60, '--b', 'LineWidth', 2.0); 292 | 293 | subplot(2, 1, 2) 294 | plot(0:current_step - 1, uk(1,:), 'ko-',... 295 | 'LineWidth', 1.0, 'MarkerSize', 4); 296 | xlabel('Time Step','interpreter','latex','FontSize',10); 297 | ylabel('$\delta$ [rad]','interpreter','latex','FontSize',10); 298 | 299 | figure(2) 300 | if strcmp(trajectory_type, 'Lane change') 301 | % plot trajectory 302 | plot(x_ref(1: current_step, 1),y_ref(1: current_step, 1),'b','LineWidth',2) 303 | hold on 304 | plot(x_ref(1: current_step, 1),xTrue(4, 1:current_step),'r','LineWidth',2) 305 | hold on; 306 | grid on; 307 | % plot initial position 308 | plot(x_ref(1, 1), y_ref(1, 1), 'dg', 'LineWidth', 2); 309 | xlabel('X[m]','interpreter','latex','FontSize',10); 310 | ylabel('Y[m]','interpreter','latex','FontSize',10); 311 | yline(0.0, '--k', 'LineWidth', 2.0); 312 | yline(10.0, 'k', 'LineWidth', 4.0); 313 | yline(-10.0, 'k', 'LineWidth', 4.0); 314 | ylim([-12 12]) 315 | 316 | legend('Refernce trajectory','Motion trajectory','Initial position', 'Location','southeast',... 317 | 'interpreter','latex','FontSize',10.0); 318 | elseif strcmp(trajectory_type, 'Circle') 319 | % plot trajectory 320 | plot(x_ref(1: current_step, 1),y_ref(1: current_step, 1),'--b','LineWidth',2) 321 | hold on 322 | plot(x_ref(1: current_step, 1),xTrue(4, 1:current_step),'r','LineWidth',1) 323 | hold on; 324 | grid on; 325 | axis equal; 326 | % plot initial position 327 | plot(x_ref(1, 1), y_ref(1, 1), 'db', 'LineWidth', 2); 328 | xlabel('X[m]','interpreter','latex','FontSize',10); 329 | ylabel('Y[m]','interpreter','latex','FontSize',10); 330 | 331 | legend('Refernce trajectory','Motion trajectory','Initial position', 'Location','southeast',... 332 | 'interpreter','latex','FontSize',10.0); 333 | end 334 | 335 | % plot lateral error 336 | figure(3) 337 | Lateral_error = y_ref(1: current_step, 1) - xTrue(4, 1:current_step)'; 338 | plot(0:current_step - 1, Lateral_error, 'ko-',... 339 | 'LineWidth', 1.0, 'MarkerSize', 4); 340 | xlabel('Time Step','interpreter','latex','FontSize',10); 341 | ylabel('Lateral error [m]','interpreter','latex','FontSize',10); 342 | end 343 | -------------------------------------------------------------------------------- /Control/Trajectory_tracking_control(LMPC)/Lateral_Vehicle_Dynamics_by_quadprog.m: -------------------------------------------------------------------------------- 1 | clear; 2 | close all; 3 | clc; 4 | 5 | %% Choice the type of trajectory 6 | while true 7 | model = input('Please choose the type of trajectory (1 = Lane change, 2 = Circle): '); 8 | switch model 9 | case 1 10 | trajectory_type = 'Lane change'; 11 | break; 12 | case 2 13 | trajectory_type = 'Circle'; 14 | break; 15 | otherwise 16 | disp('No System!'); 17 | end 18 | end 19 | 20 | %% Setup and Parameters 21 | dt = 0.1; % sample time 22 | nx = 4; % Number of states 23 | ny = 2; % Number of ovservation 24 | nu = 1; % Number of input 25 | 26 | S = diag([10.0, 1.0]); % Termination cost weight matrix 27 | Q = diag([10.0, 1.0]); % Weight matrix of state quantities 28 | R = 30.0; % Weight matrix of input quantities 29 | N = 15; % Predictive Horizon 30 | 31 | % Range of inputs 32 | umin = -pi / 60; 33 | umax = pi / 60; 34 | 35 | %% Linear Car models (Lateral Vehicle Dynamics) 36 | M = 1500; % [kg] 37 | I = 3000; % [kgm^2] 38 | lf = 1.2; % [m] 39 | lr = 1.6; % [m] 40 | l = lf + lr; % [m] 41 | Kf = 19000; % [N/rad] 42 | Kr = 33000; % [N/rad] 43 | Vx = 20; % [m/s] 44 | 45 | system.dt = dt; 46 | system.nx = nx; 47 | system.ny = ny; 48 | system.nu = nu; 49 | 50 | % states = [y_dot, psi, psi_dot, Y]; 51 | % Vehicle Dynamics and Control (2nd edition) by Rajesh Rajamani. They are in Chapter 2.3. 52 | A11 = -2 * (Kf + Kr)/(M * Vx); 53 | A13 = -Vx - 2 * (Kf*lf - Kr*lr)/(M * Vx); 54 | A31 = -2 * (lf*Kf - lr*Kr)/(I * Vx); 55 | A33 = -2 * (lf^2*Kf + lr^2*Kr)/(I * Vx); 56 | system.A = eye(nx) + [A11, 0.0, A13, 0.0; 57 | 0.0, 0.0, 1.0, 0.0; 58 | A31, 0.0, A33, 0.0; 59 | 1.0, Vx, 0.0, 0.0] * dt; 60 | 61 | B11 = 2*Kf/M; 62 | B13 = 2*lf*Kf/I; 63 | system.B = [B11; 64 | 0.0; 65 | B13; 66 | 0.0] * dt; 67 | 68 | system.C = [0 1 0 0; 69 | 0 0 0 1]; 70 | 71 | system.A_aug = [system.A, system.B; 72 | zeros(length(system.B(1,:)),length(system.A(1,:))),eye(length(system.B(1,:)))]; 73 | system.B_aug = [system.B; 74 | eye(length(system.B(1,:)))]; 75 | system.C_aug = [system.C,zeros(length(system.C(:,1)),length(system.B(1,:)))]; 76 | 77 | % Constants 78 | system.Vx = Vx; 79 | system.M = M; 80 | system.I = I; 81 | system.Kf = Kf; 82 | system.Kr = Kr; 83 | system.lf = lf; 84 | system.lr = lr; 85 | 86 | % input 87 | system.ul = umin; 88 | system.uu = umax; 89 | 90 | %% Load the trajectory 91 | if strcmp(trajectory_type, 'Lane change') 92 | t = 0:dt:10.0; 93 | elseif strcmp(trajectory_type, 'Circle') 94 | t = 0:dt:80.0; 95 | end 96 | [x_ref, y_ref, psi_ref] = trajectory_generator(t, Vx, trajectory_type); 97 | 98 | sim_length = length(t); % Number of control loop iterations 99 | 100 | refSignals = zeros(length(x_ref(:, 1)) * ny, 1); 101 | 102 | ref_index = 1; 103 | for i = 1:ny:length(refSignals) 104 | refSignals(i) = psi_ref(ref_index, 1); 105 | refSignals(i + 1) = y_ref(ref_index, 1); 106 | ref_index = ref_index + 1; 107 | end 108 | 109 | % initial state 110 | x0 = [0.0; psi_ref(1, 1); 0.0; y_ref(1, 1)]; % Initial state of Lateral Vehicle Dynamics 111 | 112 | %% MPC parameters 113 | params_mpc.Q = Q; 114 | params_mpc.R = R; 115 | params_mpc.S = S; 116 | params_mpc.N = N; 117 | 118 | %% main loop 119 | xTrue(:, 1) = x0; 120 | uk(:, 1) = 0.0; 121 | du(:, 1) = 0.0; 122 | current_step = 1; 123 | solvetime = zeros(1, sim_length); 124 | 125 | ref_sig_num = 1; % for reading reference signals 126 | for i = 1:sim_length-15 127 | % update time 128 | current_step = current_step + 1; 129 | 130 | % Generating the current state and the reference vector 131 | xTrue_aug = [xTrue(:, current_step - 1); uk(:, current_step - 1)]; 132 | ref_sig_num = ref_sig_num + ny; 133 | 134 | if ref_sig_num + ny * N - 1 <= length(refSignals) 135 | ref = refSignals(ref_sig_num:ref_sig_num+ny*N-1); 136 | else 137 | ref = refSignals(ref_sig_num:length(refSignals)); 138 | N = N - 1; 139 | end 140 | 141 | % solve mac 142 | tic; 143 | du(:, current_step) = mpc(xTrue_aug, system, params_mpc, ref); 144 | solvetime(1, current_step - 1) = toc; 145 | 146 | % add du input 147 | uk(:, current_step) = uk(:, current_step - 1) + du(:, current_step); 148 | 149 | % update state 150 | T = dt*i:dt:dt*i+dt; 151 | [T, x] = ode45(@(t,x) nonlinear_lateral_car_model(t, x, uk(:, current_step), system), T, xTrue(:, current_step - 1)); 152 | xTrue(:, current_step) = x(end,:); 153 | end 154 | 155 | %% solve average time 156 | avg_time = sum(solvetime) / current_step; 157 | disp(avg_time); 158 | 159 | drow_figure(xTrue, uk, du, x_ref, y_ref, current_step, trajectory_type); 160 | 161 | %% model predictive control 162 | function uopt = mpc(xTrue_aug, system, params_mpc, ref) 163 | A_aug = system.A_aug; 164 | B_aug = system.B_aug; 165 | C_aug = system.C_aug; 166 | 167 | Q = params_mpc.Q; 168 | S = params_mpc.S; 169 | R = params_mpc.R; 170 | N = params_mpc.N; 171 | 172 | CQC = C_aug' * Q * C_aug; 173 | CSC = C_aug' * S * C_aug; 174 | QC = Q * C_aug; 175 | SC = S * C_aug; 176 | 177 | Qdb = zeros(length(CQC(:,1))*N,length(CQC(1,:))*N); 178 | Tdb = zeros(length(QC(:,1))*N,length(QC(1,:))*N); 179 | Rdb = zeros(length(R(:,1))*N,length(R(1,:))*N); 180 | Cdb = zeros(length(B_aug(:,1))*N,length(B_aug(1,:))*N); 181 | Adc = zeros(length(A_aug(:,1))*N,length(A_aug(1,:))); 182 | 183 | % Filling in the matrices 184 | for i = 1:N 185 | if i == N 186 | Qdb(1+length(CSC(:,1))*(i-1):length(CSC(:,1))*i,1+length(CSC(1,:))*(i-1):length(CSC(1,:))*i) = CSC; 187 | Tdb(1+length(SC(:,1))*(i-1):length(SC(:,1))*i,1+length(SC(1,:))*(i-1):length(SC(1,:))*i) = SC; 188 | else 189 | Qdb(1+length(CQC(:,1))*(i-1):length(CQC(:,1))*i,1+length(CQC(1,:))*(i-1):length(CQC(1,:))*i) = CQC; 190 | Tdb(1+length(QC(:,1))*(i-1):length(QC(:,1))*i,1+length(QC(1,:))*(i-1):length(QC(1,:))*i) = QC; 191 | end 192 | 193 | Rdb(1+length(R(:,1))*(i-1):length(R(:,1))*i,1+length(R(1,:))*(i-1):length(R(1,:))*i) = R; 194 | 195 | for j = 1:N 196 | if j<=i 197 | Cdb(1+length(B_aug(:,1))*(i-1):length(B_aug(:,1))*i,1+length(B_aug(1,:))*(j-1):length(B_aug(1,:))*j) = A_aug^(i-j)*B_aug; 198 | end 199 | end 200 | Adc(1+length(A_aug(:,1))*(i-1):length(A_aug(:,1))*i,1:length(A_aug(1,:))) = A_aug^(i); 201 | end 202 | Hdb = Cdb' * Qdb * Cdb + Rdb; 203 | Fdbt = [Adc' * Qdb * Cdb; -Tdb * Cdb]; 204 | 205 | % Calling the optimizer (quadprog) 206 | % Cost function in quadprog: min(du)*1/2*du'Hdb*du+f'du 207 | ft = [xTrue_aug', ref'] * Fdbt; 208 | 209 | % Call the solver (quadprog) 210 | A = []; 211 | b = []; 212 | Aeq = []; 213 | beq = []; 214 | lb = ones(1, N) * system.ul; 215 | ub = ones(1, N) * system.uu; 216 | x0 = []; 217 | options = optimset('Display', 'off'); 218 | [du, ~] = quadprog(Hdb,ft,A,b,Aeq,beq,lb,ub,x0,options); 219 | uopt = du(1); 220 | end 221 | 222 | %% trajectory generator 223 | function [x_ref, y_ref, psi_ref] = trajectory_generator(t, Vx, trajectory_type) 224 | if strcmp(trajectory_type, 'Lane change') 225 | x = linspace(0, Vx * t(end), length(t)); 226 | y = 3 * tanh((t - t(end) / 2)); 227 | elseif strcmp(trajectory_type, 'Circle') 228 | radius = 30; 229 | period = 1000; 230 | for i = 1:length(t) 231 | x(1, i) = radius * sin(2 * pi * i / period); 232 | y(1, i) = -radius * cos(2 * pi * i / period); 233 | end 234 | end 235 | 236 | dx = x(2:end) - x(1:end-1); 237 | dy = y(2:end) - y(1:end-1); 238 | 239 | psi = zeros(1,length(x)); 240 | psiInt = psi; 241 | 242 | psi(1) = atan2(dy(1),dx(1)); 243 | psi(2:end) = atan2(dy(:),dx(:)); 244 | dpsi = psi(2:end) - psi(1:end-1); 245 | 246 | psiInt(1) = psi(1); 247 | for i = 2:length(psiInt) 248 | if dpsi(i - 1) < -pi 249 | psiInt(i) = psiInt(i - 1) + (dpsi(i - 1) + 2 * pi); 250 | elseif dpsi(i - 1) > pi 251 | psiInt(i) = psiInt(i - 1) + (dpsi(i - 1) - 2 * pi); 252 | else 253 | psiInt(i) = psiInt(i - 1) + dpsi(i - 1); 254 | end 255 | end 256 | 257 | x_ref = x'; 258 | y_ref = y'; 259 | psi_ref = psiInt'; 260 | end 261 | 262 | function dx = nonlinear_lateral_car_model(t, xTrue, u, system) 263 | % In this simulation, the body frame and its transformation is used 264 | % instead of a hybrid frame. That is because for the solver ode45, it 265 | % is important to have the nonlinear system of equations in the first 266 | % order form. 267 | 268 | % Get the constants from the general pool of constants 269 | Vx = system.Vx; 270 | M = system.M; 271 | I = system.I; 272 | Kf = system.Kf; 273 | Kr = system.Kr; 274 | lf = system.lf; 275 | lr = system.lr; 276 | 277 | % % Assign the states 278 | y_dot = xTrue(1); 279 | psi = xTrue(2); 280 | psi_dot = xTrue(3); 281 | 282 | % The nonlinear equation describing the dynamics of the car 283 | dx(1,1) = -(2*Kf+2*Kr)/(M * Vx)*y_dot+(-Vx-(2*Kf*lf-2*Kr*lr)/(M * Vx))*psi_dot+2*Kf/M*u; 284 | dx(2,1) = psi_dot; 285 | dx(3,1) = -(2*lf*Kf-2*lr*Kr)/(I * Vx)*y_dot-(2*lf^2*Kf+2*lr^2*Kr)/(I * Vx)*psi_dot+2*lf*Kf/I*u; 286 | dx(4,1) = sin(psi) * Vx + cos(psi) * y_dot; 287 | end 288 | 289 | function drow_figure(xTrue, uk, du, x_ref, y_ref, current_step, trajectory_type) 290 | % plot input 291 | figure(1) 292 | subplot(2, 1, 1) 293 | plot(0:current_step - 1, du(1,:), 'ko-',... 294 | 'LineWidth', 1.0, 'MarkerSize', 4); 295 | xlabel('Time Step','interpreter','latex','FontSize',10); 296 | ylabel('$\Delta \delta$ [rad]','interpreter','latex','FontSize',10); 297 | yline(pi / 60, '--b', 'LineWidth', 2.0); 298 | yline(-pi / 60, '--b', 'LineWidth', 2.0); 299 | 300 | subplot(2, 1, 2) 301 | plot(0:current_step - 1, uk(1,:), 'ko-',... 302 | 'LineWidth', 1.0, 'MarkerSize', 4); 303 | xlabel('Time Step','interpreter','latex','FontSize',10); 304 | ylabel('$\delta$ [rad]','interpreter','latex','FontSize',10); 305 | 306 | figure(2) 307 | if strcmp(trajectory_type, 'Lane change') 308 | % plot trajectory 309 | plot(x_ref(1: current_step, 1),y_ref(1: current_step, 1),'b','LineWidth',2) 310 | hold on 311 | plot(x_ref(1: current_step, 1),xTrue(4, 1:current_step),'r','LineWidth',2) 312 | hold on; 313 | grid on; 314 | % plot initial position 315 | plot(x_ref(1, 1), y_ref(1, 1), 'dg', 'LineWidth', 2); 316 | xlabel('X[m]','interpreter','latex','FontSize',10); 317 | ylabel('Y[m]','interpreter','latex','FontSize',10); 318 | yline(0.0, '--k', 'LineWidth', 2.0); 319 | yline(10.0, 'k', 'LineWidth', 4.0); 320 | yline(-10.0, 'k', 'LineWidth', 4.0); 321 | ylim([-12 12]) 322 | 323 | legend('Refernce trajectory','Motion trajectory','Initial position', 'Location','southeast',... 324 | 'interpreter','latex','FontSize',10.0); 325 | elseif strcmp(trajectory_type, 'Circle') 326 | % plot trajectory 327 | plot(x_ref(1: current_step, 1),y_ref(1: current_step, 1),'--b','LineWidth',2) 328 | hold on 329 | plot(x_ref(1: current_step, 1),xTrue(4, 1:current_step),'r','LineWidth',1) 330 | hold on; 331 | grid on; 332 | axis equal; 333 | % plot initial position 334 | plot(x_ref(1, 1), y_ref(1, 1), 'db', 'LineWidth', 2); 335 | xlabel('X[m]','interpreter','latex','FontSize',10); 336 | ylabel('Y[m]','interpreter','latex','FontSize',10); 337 | 338 | legend('Refernce trajectory','Motion trajectory','Initial position', 'Location','southeast',... 339 | 'interpreter','latex','FontSize',10.0); 340 | end 341 | 342 | % plot lateral error 343 | figure(3) 344 | Lateral_error = y_ref(1: current_step, 1) - xTrue(4, 1:current_step)'; 345 | plot(0:current_step - 1, Lateral_error, 'ko-',... 346 | 'LineWidth', 1.0, 'MarkerSize', 4); 347 | xlabel('Time Step','interpreter','latex','FontSize',10); 348 | ylabel('Lateral error [m]','interpreter','latex','FontSize',10); 349 | end 350 | -------------------------------------------------------------------------------- /Control/Trajectory_tracking_control(LMPC)/mobile_robot_by_YALMIP.m: -------------------------------------------------------------------------------- 1 | clear; 2 | close all; 3 | clc; 4 | 5 | %% Ipopt solver path 6 | dir = pwd; 7 | addpath(fullfile(dir,'Ipopt')) 8 | 9 | %% Setup and Parameters 10 | dt = 0.1; % sample time 11 | nx = 3; % Number of states 12 | ny = 3; % Number of ovservation 13 | nu = 2; % Number of input 14 | 15 | S = diag([1.0, 1.0, 10.0]); % Termination cost weight matrix 16 | Q = diag([1.0, 1.0, 10.0]); % Weight matrix of state quantities 17 | R = diag([0.1, 0.001]); % Weight matrix of input quantities 18 | N = 5; % Predictive Horizon 19 | 20 | % Range of inputs 21 | umin = [-5; -4]; 22 | umax = [5; 4]; 23 | 24 | %% Mobile robot 25 | Vx = 0.0; % [m/s] 26 | thetak = 0.0; % [rad] 27 | 28 | system.dt = dt; 29 | system.nx = nx; 30 | system.ny = ny; 31 | system.nu = nu; 32 | 33 | % input 34 | system.ul = umin; 35 | system.uu = umax; 36 | 37 | %% Load the trajectory 38 | t = 0:dt:10.0; 39 | [x_ref, y_ref, psi_ref] = trajectory_generator(t); 40 | 41 | sim_length = length(t); % Number of control loop iterations 42 | 43 | refSignals = zeros(length(x_ref(:, 1)) * ny, 1); 44 | 45 | ref_index = 1; 46 | for i = 1:ny:length(refSignals) 47 | refSignals(i) = x_ref(ref_index, 1); 48 | refSignals(i + 1) = y_ref(ref_index, 1); 49 | refSignals(i + 2) = psi_ref(ref_index, 1); 50 | ref_index = ref_index + 1; 51 | end 52 | 53 | % initial state 54 | x0 = [x_ref(1, 1); y_ref(1, 1); psi_ref(1, 1)]; % Initial state of mobile robot 55 | 56 | %% Create A, B, C matrix 57 | % states = [x, y, psi]; 58 | [A, B, C] = model_system(Vx, thetak, dt); 59 | 60 | system.A = A; 61 | system.B = B; 62 | system.C = C; 63 | 64 | system.A_aug = [system.A, system.B; 65 | zeros(length(system.B(1,:)),length(system.A(1,:))),eye(length(system.B(1,:)))]; 66 | system.B_aug = [system.B; 67 | eye(length(system.B(1,:)))]; 68 | system.C_aug = [system.C,zeros(length(system.C(:,1)),length(system.B(1,:)))]; 69 | 70 | %% MPC parameters 71 | params_mpc.Q = Q; 72 | params_mpc.R = R; 73 | params_mpc.S = S; 74 | params_mpc.N = N; 75 | 76 | %% main loop 77 | xTrue(:, 1) = x0; 78 | uk(:, 1) = [0.0; 0.0]; 79 | du(:, 1) = [0.0; 0.0]; 80 | current_step = 1; 81 | solvetime = zeros(1, sim_length); 82 | 83 | ref_sig_num = 1; % for reading reference signals 84 | for i = 1:sim_length-15 85 | % update time 86 | current_step = current_step + 1; 87 | 88 | % Generating the current state and the reference vector 89 | [A, B, C] = model_system(Vx, thetak, dt); 90 | 91 | system.A = A; 92 | system.B = B; 93 | system.C = C; 94 | system.A_aug = [system.A, system.B; 95 | zeros(length(system.B(1,:)),length(system.A(1,:))),eye(length(system.B(1,:)))]; 96 | system.B_aug = [system.B; 97 | eye(length(system.B(1,:)))]; 98 | system.C_aug = [system.C,zeros(length(system.C(:,1)),length(system.B(1,:)))]; 99 | 100 | xTrue_aug = [xTrue(:, current_step - 1); uk(:, current_step - 1)]; 101 | 102 | ref_sig_num = ref_sig_num + ny; 103 | if ref_sig_num + ny * N - 1 <= length(refSignals) 104 | ref = refSignals(ref_sig_num:ref_sig_num+ny*N-1); 105 | else 106 | ref = refSignals(ref_sig_num:length(refSignals)); 107 | N = N - 1; 108 | end 109 | 110 | % solve mac 111 | tic; 112 | du(:, current_step) = mpc(xTrue_aug, system, params_mpc, ref); 113 | solvetime(1, current_step - 1) = toc; 114 | 115 | % add du input 116 | uk(:, current_step) = uk(:, current_step - 1) + du(:, current_step); 117 | 118 | % update state 119 | T = dt*i:dt:dt*i+dt; 120 | [T, x] = ode45(@(t,x) nonlinear_lateral_car_model(t, x, uk(:, current_step)), T, xTrue(:, current_step - 1)); 121 | xTrue(:, current_step) = x(end,:); 122 | X = xTrue(:, current_step); 123 | thetak = X(3); 124 | end 125 | 126 | %% solve average time 127 | avg_time = sum(solvetime) / current_step; 128 | disp(avg_time); 129 | 130 | drow_figure(xTrue, uk, du, x_ref, y_ref, current_step); 131 | 132 | %% model predictive control 133 | function uopt = mpc(xTrue_aug, system, params, ref) 134 | % Solve MPC 135 | [feas, ~, u, ~] = solve_mpc(xTrue_aug, system, params, ref); 136 | if ~feas 137 | uopt = []; 138 | return 139 | else 140 | uopt = u(:, 1); 141 | end 142 | end 143 | 144 | function [feas, xopt, uopt, Jopt] = solve_mpc(xTrue_aug, system, params, ref) 145 | % extract variables 146 | N = params.N; 147 | % define variables and cost 148 | x = sdpvar(system.nx + system.nu, N+1); 149 | u = sdpvar(system.nu, N); 150 | constraints = []; 151 | cost = 0; 152 | ref_index = 1; 153 | 154 | % initial constraint 155 | constraints = [constraints; x(:,1) == xTrue_aug]; 156 | % add constraints and costs 157 | for i = 1:N-1 158 | constraints = [constraints; 159 | system.ul <= u(:,i) <= system.uu 160 | x(:,i+1) == system.A_aug * x(:,i) + system.B_aug * u(:,i)]; 161 | cost = cost + (ref(ref_index:ref_index + 2,1) - system.C_aug * x(:,i+1))' * params.Q * (ref(ref_index:ref_index + 2,1) - system.C_aug * x(:,i+1)) + u(:,i)' * params.R * u(:,i); 162 | ref_index = ref_index + system.ny; 163 | end 164 | % add terminal cost 165 | cost = cost + (ref(ref_index:ref_index + 2,1) - system.C_aug * x(:,N+1))' * params.S * (ref(ref_index:ref_index + 2,1) - system.C_aug * x(:,N+1)); 166 | ops = sdpsettings('solver','ipopt','verbose',0); 167 | % solve optimization 168 | diagnostics = optimize(constraints, cost, ops); 169 | if diagnostics.problem == 0 170 | feas = true; 171 | xopt = value(x); 172 | uopt = value(u); 173 | Jopt = value(cost); 174 | else 175 | feas = false; 176 | xopt = []; 177 | uopt = []; 178 | Jopt = []; 179 | end 180 | end 181 | 182 | %% trajectory generator 183 | function [x_ref, y_ref, psi_ref] = trajectory_generator(t) 184 | % Circle 185 | radius = 30; 186 | period = 100; 187 | for i = 1:length(t) 188 | x(1, i) = radius * sin(2 * pi * i / period); 189 | y(1, i) = -radius * cos(2 * pi * i / period); 190 | end 191 | 192 | dx = x(2:end) - x(1:end-1); 193 | dy = y(2:end) - y(1:end-1); 194 | 195 | psi = zeros(1,length(x)); 196 | psiInt = psi; 197 | 198 | psi(1) = atan2(dy(1),dx(1)); 199 | psi(2:end) = atan2(dy(:),dx(:)); 200 | dpsi = psi(2:end) - psi(1:end-1); 201 | 202 | psiInt(1) = psi(1); 203 | for i = 2:length(psiInt) 204 | if dpsi(i - 1) < -pi 205 | psiInt(i) = psiInt(i - 1) + (dpsi(i - 1) + 2 * pi); 206 | elseif dpsi(i - 1) > pi 207 | psiInt(i) = psiInt(i - 1) + (dpsi(i - 1) - 2 * pi); 208 | else 209 | psiInt(i) = psiInt(i - 1) + dpsi(i - 1); 210 | end 211 | end 212 | 213 | x_ref = x'; 214 | y_ref = y'; 215 | psi_ref = psiInt'; 216 | end 217 | 218 | function [A, B, C] = model_system(Vk,thetak,dt) 219 | A = [1.0, 0.0, -Vk*dt*sin(thetak); 220 | 0.0 1.0 Vk*dt*cos(thetak); 221 | 0.0, 0.0, 1.0]; 222 | B = [dt*cos(thetak), -0.5*dt*dt*Vk*sin(thetak); 223 | dt*sin(thetak), 0.5*dt*dt*Vk*cos(thetak); 224 | 0.0 dt]; 225 | C = [1.0, 0.0, 0.0; 226 | 0.0, 1.0, 0.0; 227 | 0.0, 0.0, 1.0]; 228 | end 229 | 230 | function dx = nonlinear_lateral_car_model(t, xTrue, u) 231 | % In this simulation, the body frame and its transformation is used 232 | % instead of a hybrid frame. That is because for the solver ode45, it 233 | % is important to have the nonlinear system of equations in the first 234 | % order form. 235 | 236 | dx = [u(1)*cos(xTrue(3)); 237 | u(1)*sin(xTrue(3)); 238 | u(2)]; 239 | end 240 | 241 | function drow_figure(xTrue, uk, du, x_ref, y_ref, current_step) 242 | % Plot simulation 243 | figure(1) 244 | subplot(2,2,1) 245 | plot(0:current_step - 1, du(1,:), 'ko-',... 246 | 'LineWidth', 1.0, 'MarkerSize', 4); 247 | xlabel('Time Step','interpreter','latex','FontSize',10); 248 | ylabel('$\Delta v$ [m/s]','interpreter','latex','FontSize',10); 249 | yline(-5, '--b', 'LineWidth', 2.0); 250 | yline(5, '--b', 'LineWidth', 2.0); 251 | 252 | subplot(2,2,2) 253 | plot(0:current_step - 1, du(2,:), 'ko-',... 254 | 'LineWidth', 1.0, 'MarkerSize', 4); 255 | xlabel('Time Step','interpreter','latex','FontSize',10); 256 | ylabel('$\Delta w$ [rad/s]','interpreter','latex','FontSize',10); 257 | yline(-4, '--b', 'LineWidth', 2.0); 258 | yline(4, '--b', 'LineWidth', 2.0); 259 | 260 | subplot(2,2,3) 261 | plot(0:current_step - 1, uk(1,:), 'ko-',... 262 | 'LineWidth', 1.0, 'MarkerSize', 4); 263 | xlabel('Time Step','interpreter','latex','FontSize',10); 264 | ylabel('$v$ [m/s]','interpreter','latex','FontSize',10); 265 | 266 | subplot(2,2,4) 267 | plot(0:current_step - 1, uk(2,:), 'ko-',... 268 | 'LineWidth', 1.0, 'MarkerSize', 4); 269 | xlabel('Time Step','interpreter','latex','FontSize',10); 270 | ylabel('$w$ [rad/s]','interpreter','latex','FontSize',10); 271 | 272 | figure(2) 273 | % plot trajectory 274 | plot(x_ref(1: current_step, 1),y_ref(1: current_step, 1),'--b','LineWidth',2) 275 | hold on 276 | plot(xTrue(1, 1: current_step), xTrue(2, 1:current_step),'r','LineWidth',2) 277 | hold on; 278 | grid on; 279 | axis equal; 280 | % plot initial position 281 | plot(x_ref(1, 1), y_ref(1, 1), 'dg', 'LineWidth', 2); 282 | xlabel('X[m]','interpreter','latex','FontSize',10); 283 | ylabel('Y[m]','interpreter','latex','FontSize',10); 284 | 285 | legend('Refernce trajectory','Motion trajectory','Initial position', 'Location','southeast',... 286 | 'interpreter','latex','FontSize',10.0); 287 | 288 | % plot position error 289 | figure(3) 290 | Vertical_error = x_ref(1: current_step, 1) - xTrue(1, 1: current_step)'; 291 | Lateral_error = y_ref(1: current_step, 1) - xTrue(2, 1:current_step)'; 292 | 293 | subplot(2, 1, 1) 294 | plot(0:current_step - 1, Vertical_error, 'ko-',... 295 | 'LineWidth', 1.0, 'MarkerSize', 4); 296 | xlabel('Time Step','interpreter','latex','FontSize',10); 297 | ylabel('Vertical error [m]','interpreter','latex','FontSize',10); 298 | 299 | subplot(2, 1, 2) 300 | plot(0:current_step - 1, Lateral_error, 'ko-',... 301 | 'LineWidth', 1.0, 'MarkerSize', 4); 302 | xlabel('Time Step','interpreter','latex','FontSize',10); 303 | ylabel('Lateral error [m]','interpreter','latex','FontSize',10); 304 | end 305 | -------------------------------------------------------------------------------- /Control/Trajectory_tracking_control(LMPC)/mobile_robot_by_quadprog.m: -------------------------------------------------------------------------------- 1 | clear; 2 | close all; 3 | clc; 4 | 5 | %% Setup and Parameters 6 | dt = 0.1; % sample time 7 | nx = 3; % Number of states 8 | ny = 3; % Number of ovservation 9 | nu = 2; % Number of input 10 | 11 | S = diag([1.0, 1.0, 10.0]); % Termination cost weight matrix 12 | Q = diag([1.0, 1.0, 10.0]); % Weight matrix of state quantities 13 | R = diag([0.1, 0.001]); % Weight matrix of input quantities 14 | N = 5; % Predictive Horizon 15 | 16 | % Range of inputs 17 | umin = [-5; -4]; 18 | umax = [5; 4]; 19 | 20 | %% Mobile robot 21 | Vx = 0.0; % [m/s] 22 | thetak = 0.0; % [rad] 23 | 24 | system.dt = dt; 25 | system.nx = nx; 26 | system.ny = ny; 27 | system.nu = nu; 28 | 29 | % input 30 | system.ul = umin; 31 | system.uu = umax; 32 | 33 | %% Load the trajectory 34 | t = 0:dt:10.0; 35 | [x_ref, y_ref, psi_ref] = trajectory_generator(t); 36 | 37 | sim_length = length(t); % Number of control loop iterations 38 | 39 | refSignals = zeros(length(x_ref(:, 1)) * ny, 1); 40 | 41 | ref_index = 1; 42 | for i = 1:ny:length(refSignals) 43 | refSignals(i) = x_ref(ref_index, 1); 44 | refSignals(i + 1) = y_ref(ref_index, 1); 45 | refSignals(i + 2) = psi_ref(ref_index, 1); 46 | ref_index = ref_index + 1; 47 | end 48 | 49 | % initial state 50 | x0 = [x_ref(1, 1); y_ref(1, 1); psi_ref(1, 1)]; % Initial state of mobile robot 51 | 52 | %% Create A, B, C matrix 53 | % states = [x, y, psi]; 54 | [A, B, C] = model_system(Vx, thetak, dt); 55 | 56 | system.A = A; 57 | system.B = B; 58 | system.C = C; 59 | 60 | system.A_aug = [system.A, system.B; 61 | zeros(length(system.B(1,:)),length(system.A(1,:))),eye(length(system.B(1,:)))]; 62 | system.B_aug = [system.B; 63 | eye(length(system.B(1,:)))]; 64 | system.C_aug = [system.C,zeros(length(system.C(:,1)),length(system.B(1,:)))]; 65 | 66 | %% MPC parameters 67 | params_mpc.Q = Q; 68 | params_mpc.R = R; 69 | params_mpc.S = S; 70 | params_mpc.N = N; 71 | 72 | %% main loop 73 | xTrue(:, 1) = x0; 74 | uk(:, 1) = [0.0; 0.0]; 75 | du(:, 1) = [0.0; 0.0]; 76 | current_step = 1; 77 | solvetime = zeros(1, sim_length); 78 | 79 | ref_sig_num = 1; % for reading reference signals 80 | for i = 1:sim_length-15 81 | % update time 82 | current_step = current_step + 1; 83 | 84 | % Generating the current state and the reference vector 85 | [A, B, C] = model_system(Vx, thetak, dt); 86 | 87 | system.A = A; 88 | system.B = B; 89 | system.C = C; 90 | system.A_aug = [system.A, system.B; 91 | zeros(length(system.B(1,:)),length(system.A(1,:))),eye(length(system.B(1,:)))]; 92 | system.B_aug = [system.B; 93 | eye(length(system.B(1,:)))]; 94 | system.C_aug = [system.C,zeros(length(system.C(:,1)),length(system.B(1,:)))]; 95 | 96 | xTrue_aug = [xTrue(:, current_step - 1); uk(:, current_step - 1)]; 97 | 98 | ref_sig_num = ref_sig_num + ny; 99 | if ref_sig_num + ny * N - 1 <= length(refSignals) 100 | ref = refSignals(ref_sig_num:ref_sig_num+ny*N-1); 101 | else 102 | ref = refSignals(ref_sig_num:length(refSignals)); 103 | N = N - 1; 104 | end 105 | 106 | % solve mac 107 | tic; 108 | du(:, current_step) = mpc(xTrue_aug, system, params_mpc, ref); 109 | solvetime(1, current_step - 1) = toc; 110 | 111 | % add du input 112 | uk(:, current_step) = uk(:, current_step - 1) + du(:, current_step); 113 | 114 | % update state 115 | T = dt*i:dt:dt*i+dt; 116 | [T, x] = ode45(@(t,x) nonlinear_lateral_car_model(t, x, uk(:, current_step)), T, xTrue(:, current_step - 1)); 117 | xTrue(:, current_step) = x(end,:); 118 | X = xTrue(:, current_step); 119 | thetak = X(3); 120 | end 121 | 122 | %% solve average time 123 | avg_time = sum(solvetime) / current_step; 124 | disp(avg_time); 125 | 126 | drow_figure(xTrue, uk, du, x_ref, y_ref, current_step); 127 | 128 | %% model predictive control 129 | %% model predictive control 130 | function uopt = mpc(xTrue_aug, system, params_mpc, ref) 131 | A_aug = system.A_aug; 132 | B_aug = system.B_aug; 133 | C_aug = system.C_aug; 134 | 135 | Q = params_mpc.Q; 136 | S = params_mpc.S; 137 | R = params_mpc.R; 138 | N = params_mpc.N; 139 | 140 | CQC = C_aug' * Q * C_aug; 141 | CSC = C_aug' * S * C_aug; 142 | QC = Q * C_aug; 143 | SC = S * C_aug; 144 | 145 | Qdb = zeros(length(CQC(:,1))*N,length(CQC(1,:))*N); 146 | Tdb = zeros(length(QC(:,1))*N,length(QC(1,:))*N); 147 | Rdb = zeros(length(R(:,1))*N,length(R(1,:))*N); 148 | Cdb = zeros(length(B_aug(:,1))*N,length(B_aug(1,:))*N); 149 | Adc = zeros(length(A_aug(:,1))*N,length(A_aug(1,:))); 150 | 151 | % Filling in the matrices 152 | for i = 1:N 153 | if i == N 154 | Qdb(1+length(CSC(:,1))*(i-1):length(CSC(:,1))*i,1+length(CSC(1,:))*(i-1):length(CSC(1,:))*i) = CSC; 155 | Tdb(1+length(SC(:,1))*(i-1):length(SC(:,1))*i,1+length(SC(1,:))*(i-1):length(SC(1,:))*i) = SC; 156 | else 157 | Qdb(1+length(CQC(:,1))*(i-1):length(CQC(:,1))*i,1+length(CQC(1,:))*(i-1):length(CQC(1,:))*i) = CQC; 158 | Tdb(1+length(QC(:,1))*(i-1):length(QC(:,1))*i,1+length(QC(1,:))*(i-1):length(QC(1,:))*i) = QC; 159 | end 160 | 161 | Rdb(1+length(R(:,1))*(i-1):length(R(:,1))*i,1+length(R(1,:))*(i-1):length(R(1,:))*i) = R; 162 | 163 | for j = 1:N 164 | if j<=i 165 | Cdb(1+length(B_aug(:,1))*(i-1):length(B_aug(:,1))*i,1+length(B_aug(1,:))*(j-1):length(B_aug(1,:))*j) = A_aug^(i-j)*B_aug; 166 | end 167 | end 168 | Adc(1+length(A_aug(:,1))*(i-1):length(A_aug(:,1))*i,1:length(A_aug(1,:))) = A_aug^(i); 169 | end 170 | Hdb = Cdb' * Qdb * Cdb + Rdb; 171 | Fdbt = [Adc' * Qdb * Cdb; -Tdb * Cdb]; 172 | 173 | % Calling the optimizer (quadprog) 174 | % Cost function in quadprog: min(du)*1/2*du'Hdb*du+f'du 175 | ft = [xTrue_aug', ref'] * Fdbt; 176 | 177 | umin = ones(system.nu, N); 178 | umax = ones(system.nu, N); 179 | for i = 1:N 180 | umin(:, i) = system.ul; 181 | umax(:, i) = system.uu; 182 | end 183 | 184 | % Call the solver (quadprog) 185 | A = []; 186 | b = []; 187 | Aeq = []; 188 | beq = []; 189 | lb = umin; 190 | ub = umax; 191 | x0 = []; 192 | options = optimset('Display', 'off'); 193 | [du, ~] = quadprog(Hdb,ft,A,b,Aeq,beq,lb,ub,x0,options); 194 | 195 | uopt = du(1:system.nu, 1); 196 | end 197 | 198 | %% trajectory generator 199 | function [x_ref, y_ref, psi_ref] = trajectory_generator(t) 200 | % Circle 201 | radius = 30; 202 | period = 100; 203 | for i = 1:length(t) 204 | x(1, i) = radius * sin(2 * pi * i / period); 205 | y(1, i) = -radius * cos(2 * pi * i / period); 206 | end 207 | 208 | dx = x(2:end) - x(1:end-1); 209 | dy = y(2:end) - y(1:end-1); 210 | 211 | psi = zeros(1,length(x)); 212 | psiInt = psi; 213 | 214 | psi(1) = atan2(dy(1),dx(1)); 215 | psi(2:end) = atan2(dy(:),dx(:)); 216 | dpsi = psi(2:end) - psi(1:end-1); 217 | 218 | psiInt(1) = psi(1); 219 | for i = 2:length(psiInt) 220 | if dpsi(i - 1) < -pi 221 | psiInt(i) = psiInt(i - 1) + (dpsi(i - 1) + 2 * pi); 222 | elseif dpsi(i - 1) > pi 223 | psiInt(i) = psiInt(i - 1) + (dpsi(i - 1) - 2 * pi); 224 | else 225 | psiInt(i) = psiInt(i - 1) + dpsi(i - 1); 226 | end 227 | end 228 | 229 | x_ref = x'; 230 | y_ref = y'; 231 | psi_ref = psiInt'; 232 | end 233 | 234 | function [A, B, C] = model_system(Vk,thetak,dt) 235 | A = [1.0, 0.0, -Vk*dt*sin(thetak); 236 | 0.0 1.0 Vk*dt*cos(thetak); 237 | 0.0, 0.0, 1.0]; 238 | B = [dt*cos(thetak), -0.5*dt*dt*Vk*sin(thetak); 239 | dt*sin(thetak), 0.5*dt*dt*Vk*cos(thetak); 240 | 0.0 dt]; 241 | C = [1.0, 0.0, 0.0; 242 | 0.0, 1.0, 0.0; 243 | 0.0, 0.0, 1.0]; 244 | end 245 | 246 | function dx = nonlinear_lateral_car_model(t, xTrue, u) 247 | % In this simulation, the body frame and its transformation is used 248 | % instead of a hybrid frame. That is because for the solver ode45, it 249 | % is important to have the nonlinear system of equations in the first 250 | % order form. 251 | 252 | dx = [u(1)*cos(xTrue(3)); 253 | u(1)*sin(xTrue(3)); 254 | u(2)]; 255 | end 256 | 257 | function drow_figure(xTrue, uk, du, x_ref, y_ref, current_step) 258 | % Plot simulation 259 | figure(1) 260 | subplot(2,2,1) 261 | plot(0:current_step - 1, du(1,:), 'ko-',... 262 | 'LineWidth', 1.0, 'MarkerSize', 4); 263 | xlabel('Time Step','interpreter','latex','FontSize',10); 264 | ylabel('$\Delta v$ [m/s]','interpreter','latex','FontSize',10); 265 | yline(-5, '--b', 'LineWidth', 2.0); 266 | yline(5, '--b', 'LineWidth', 2.0); 267 | 268 | subplot(2,2,2) 269 | plot(0:current_step - 1, du(2,:), 'ko-',... 270 | 'LineWidth', 1.0, 'MarkerSize', 4); 271 | xlabel('Time Step','interpreter','latex','FontSize',10); 272 | ylabel('$\Delta w$ [rad/s]','interpreter','latex','FontSize',10); 273 | yline(-4, '--b', 'LineWidth', 2.0); 274 | yline(4, '--b', 'LineWidth', 2.0); 275 | 276 | subplot(2,2,3) 277 | plot(0:current_step - 1, uk(1,:), 'ko-',... 278 | 'LineWidth', 1.0, 'MarkerSize', 4); 279 | xlabel('Time Step','interpreter','latex','FontSize',10); 280 | ylabel('$v$ [m/s]','interpreter','latex','FontSize',10); 281 | 282 | subplot(2,2,4) 283 | plot(0:current_step - 1, uk(2,:), 'ko-',... 284 | 'LineWidth', 1.0, 'MarkerSize', 4); 285 | xlabel('Time Step','interpreter','latex','FontSize',10); 286 | ylabel('$w$ [rad/s]','interpreter','latex','FontSize',10); 287 | 288 | figure(2) 289 | % plot trajectory 290 | plot(x_ref(1: current_step, 1),y_ref(1: current_step, 1),'--b','LineWidth',2) 291 | hold on 292 | plot(xTrue(1, 1: current_step), xTrue(2, 1:current_step),'r','LineWidth',2) 293 | hold on; 294 | grid on; 295 | axis equal; 296 | % plot initial position 297 | plot(x_ref(1, 1), y_ref(1, 1), 'dg', 'LineWidth', 2); 298 | xlabel('X[m]','interpreter','latex','FontSize',10); 299 | ylabel('Y[m]','interpreter','latex','FontSize',10); 300 | 301 | legend('Refernce trajectory','Motion trajectory','Initial position', 'Location','southeast',... 302 | 'interpreter','latex','FontSize',10.0); 303 | 304 | % plot position error 305 | figure(3) 306 | Vertical_error = x_ref(1: current_step, 1) - xTrue(1, 1: current_step)'; 307 | Lateral_error = y_ref(1: current_step, 1) - xTrue(2, 1:current_step)'; 308 | 309 | subplot(2, 1, 1) 310 | plot(0:current_step - 1, Vertical_error, 'ko-',... 311 | 'LineWidth', 1.0, 'MarkerSize', 4); 312 | xlabel('Time Step','interpreter','latex','FontSize',10); 313 | ylabel('Vertical error [m]','interpreter','latex','FontSize',10); 314 | 315 | subplot(2, 1, 2) 316 | plot(0:current_step - 1, Lateral_error, 'ko-',... 317 | 'LineWidth', 1.0, 'MarkerSize', 4); 318 | xlabel('Time Step','interpreter','latex','FontSize',10); 319 | ylabel('Lateral error [m]','interpreter','latex','FontSize',10); 320 | end 321 | -------------------------------------------------------------------------------- /GuiPlot/FigureManager.m: -------------------------------------------------------------------------------- 1 | classdef FigureManager < handle 2 | properties 3 | fig 4 | ax 5 | parent 6 | x 7 | x_field_names 8 | y 9 | y_field_names 10 | fig_num 11 | end 12 | methods 13 | function obj = FigureManager(parent, fnum) 14 | [obj.fig, obj.ax] = deal(figure, axes); 15 | obj.parent = parent; 16 | obj.x = {}; 17 | obj.x_field_names = {}; 18 | obj.y = {}; 19 | obj.y_field_names = {}; 20 | obj.fig_num = fnum; 21 | 22 | set(obj.fig, 'WindowButtonDownFcn', @obj.on_click); 23 | grid(obj.ax, 'on'); 24 | pause(PLOT_DT); 25 | end 26 | 27 | function on_click(obj, ~, ~) 28 | if isempty(obj.parent.selected_fields) 29 | obj.parent.status_bar_str = 'Please click field'; 30 | else 31 | obj.set_data(obj.parent.data, obj.parent.selected_fields); 32 | pause(PLOT_DT); 33 | end 34 | obj.parent.clear_selection(); 35 | obj.parent.selected_fields = {}; 36 | end 37 | 38 | function set_data(obj, data, field_names) 39 | if isempty(obj.x) 40 | for i = 1:length(field_names) 41 | fn = field_names{i}; 42 | obj.x{end+1} = data.(fn); 43 | obj.x_field_names{end+1} = fn; 44 | end 45 | elseif isempty(obj.y) 46 | for i = 1:length(field_names) 47 | fn = field_names{i}; 48 | obj.y{end+1} = data.(fn); 49 | obj.y_field_names{end+1} = fn; 50 | end 51 | end 52 | obj.plot(); 53 | end 54 | 55 | function plot(obj) 56 | cla(obj.ax); 57 | 58 | if isempty(obj.y) 59 | for i = 1:length(obj.x) 60 | time = 1:length(obj.x{i}); 61 | plot(obj.ax, time, obj.x{i}, 'DisplayName', obj.x_field_names{i}); 62 | end 63 | xlabel(obj.ax, 'Time index'); 64 | legend(obj.ax); 65 | elseif length(obj.x) == 1 66 | plot(obj.ax, obj.x{1}, obj.y{1}, '-r'); 67 | xlabel(obj.ax, obj.x_field_names{1}); 68 | ylabel(obj.ax, obj.y_field_names{1}); 69 | end 70 | grid(obj.ax, 'on'); 71 | end 72 | 73 | function plot_time_line(obj, time) 74 | obj.plot(); 75 | if isempty(obj.y) 76 | axvline(obj.ax, time); 77 | 78 | for i = 1:length(obj.x) 79 | plot(obj.ax, time, obj.x{i}(time), 'xk'); 80 | text(obj.ax, time, obj.x{i}(time), ... 81 | [num2str(obj.x{i}(time), '%.3f'), ':', obj.x_field_names{i}]); 82 | end 83 | else 84 | plot(obj.ax, obj.x{1}(time), obj.y{1}(time), 'xk'); 85 | end 86 | end 87 | end 88 | end 89 | -------------------------------------------------------------------------------- /GuiPlot/PyPlotJuggler.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Ramune6110/MATLAB-Robotics-Engineering/cb44e950c5183f6000c183928f9c65cb3ced82da/GuiPlot/PyPlotJuggler.m -------------------------------------------------------------------------------- /GuiPlot/csvPlotter.m: -------------------------------------------------------------------------------- 1 | function csvPlotter 2 | % Create the main figure 3 | main_fig = figure('Name', 'CSV Plotter', 'Position', [100, 100, 800, 600]); 4 | 5 | % Load CSV button 6 | uicontrol('Parent', main_fig, 'Style', 'pushbutton', 'String', 'Load CSV', ... 7 | 'Position', [10, 550, 100, 30], 'Callback', @load_csv); 8 | 9 | % Variables table 10 | data_table = uitable(main_fig, 'ColumnName', {'Variable', 'Value'}, ... 11 | 'ColumnWidth', {100, 100}, 'Data', cell(0, 2), 'Position', [10, 50, 200, 450], ... 12 | 'CellSelectionCallback', @select_variable); 13 | 14 | % Plot button 15 | uicontrol('Parent', main_fig, 'Style', 'pushbutton', 'String', 'Plot 2D', ... 16 | 'Position', [220, 550, 100, 30], 'Callback', @plot_2D); 17 | uicontrol('Parent', main_fig, 'Style', 'pushbutton', 'String', 'Plot 3D', ... 18 | 'Position', [220, 510, 100, 30], 'Callback', @plot_3D); 19 | 20 | % Variables 21 | csv_data = []; 22 | selected_variables = {}; 23 | 24 | % Callbacks 25 | function load_csv(~, ~) 26 | [filename, pathname] = uigetfile('*.csv', 'Select CSV file'); 27 | if isequal(filename, 0) 28 | return; 29 | end 30 | csv_data = readtable(fullfile(pathname, filename)); 31 | data_table.Data = [csv_data.Properties.VariableNames', ... 32 | repmat({''}, length(csv_data.Properties.VariableNames), 1)]; 33 | end 34 | 35 | function select_variable(~, eventdata) 36 | if isempty(eventdata.Indices) 37 | return; 38 | end 39 | selected_variables = eventdata.Indices(:, 1)'; 40 | end 41 | 42 | function plot_2D(~, ~) 43 | if length(selected_variables) < 2 44 | warndlg('Select at least two variables to plot.', 'Warning'); 45 | return; 46 | end 47 | 48 | figure; 49 | hold on; 50 | plot(csv_data{:, selected_variables(1)}, csv_data{:, selected_variables(2)}); 51 | % for i = 1:length(selected_variables) 52 | % plot(csv_data{:, selected_variables(i)}); 53 | % end 54 | grid on; 55 | hold off; 56 | legend(csv_data.Properties.VariableNames(selected_variables)); 57 | end 58 | 59 | function plot_3D(~, ~) 60 | if length(selected_variables) < 3 61 | warndlg('Select at least three variables to plot.', 'Warning'); 62 | return; 63 | end 64 | 65 | figure; 66 | scatter3(csv_data{:, selected_variables(1)}, ... 67 | csv_data{:, selected_variables(2)}, ... 68 | csv_data{:, selected_variables(3)}); 69 | grid on; 70 | xlabel(csv_data.Properties.VariableNames{selected_variables(1)}); 71 | ylabel(csv_data.Properties.VariableNames{selected_variables(2)}); 72 | zlabel(csv_data.Properties.VariableNames{selected_variables(3)}); 73 | end 74 | end -------------------------------------------------------------------------------- /GuiPlot/csvPlotter2.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Ramune6110/MATLAB-Robotics-Engineering/cb44e950c5183f6000c183928f9c65cb3ced82da/GuiPlot/csvPlotter2.m -------------------------------------------------------------------------------- /GuiPlot/csvPlotter3.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Ramune6110/MATLAB-Robotics-Engineering/cb44e950c5183f6000c183928f9c65cb3ced82da/GuiPlot/csvPlotter3.m -------------------------------------------------------------------------------- /GuiPlot/customPlotter.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Ramune6110/MATLAB-Robotics-Engineering/cb44e950c5183f6000c183928f9c65cb3ced82da/GuiPlot/customPlotter.m -------------------------------------------------------------------------------- /GuiPlot/customPlotter2.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Ramune6110/MATLAB-Robotics-Engineering/cb44e950c5183f6000c183928f9c65cb3ced82da/GuiPlot/customPlotter2.m -------------------------------------------------------------------------------- /GuiPlot/custom_plotter3.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Ramune6110/MATLAB-Robotics-Engineering/cb44e950c5183f6000c183928f9c65cb3ced82da/GuiPlot/custom_plotter3.m -------------------------------------------------------------------------------- /GuiPlot/simple_data_viewer.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Ramune6110/MATLAB-Robotics-Engineering/cb44e950c5183f6000c183928f9c65cb3ced82da/GuiPlot/simple_data_viewer.m -------------------------------------------------------------------------------- /GuiPlot/testdata.csv: -------------------------------------------------------------------------------- 1 | 1,2 2 | 2,4 3 | 3,6 4 | 4,8 5 | 5,10 6 | -------------------------------------------------------------------------------- /GuiPlot/testdata.xlsx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Ramune6110/MATLAB-Robotics-Engineering/cb44e950c5183f6000c183928f9c65cb3ced82da/GuiPlot/testdata.xlsx -------------------------------------------------------------------------------- /GuiPlot/新しいテキスト ドキュメント.txt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Ramune6110/MATLAB-Robotics-Engineering/cb44e950c5183f6000c183928f9c65cb3ced82da/GuiPlot/新しいテキスト ドキュメント.txt -------------------------------------------------------------------------------- /Ipopt/configure_flags.txt: -------------------------------------------------------------------------------- 1 | All of the below used MatlabInterface.site file, and had Matlab on the path. 2 | 3 | Linux configured with (using GCC 4.8.0 built --with-pic): 4 | ../configure --enable-matlab-ma57 CC=/usr/local/gcc-4.8.0/bin/gcc \ 5 | CXX=/usr/local/gcc-4.8.0/bin/g++ F77=/usr/local/gcc-4.8.0/bin/gfortran 6 | 7 | Mac configured with (using GCC 4.8.3 from homebrew): 8 | ../configure --enable-matlab-ma57 CC=/usr/local/bin/gcc \ 9 | CXX=/usr/local/bin/g++ --enable-matlab-static 10 | 11 | Win32 and Win64 configured with: 12 | ../configure --enable-matlab-ma57 13 | -------------------------------------------------------------------------------- /Ipopt/examples/examplehs038.m: -------------------------------------------------------------------------------- 1 | % Test the "ipopt" Matlab interface on the Hock & Schittkowski test problem 2 | % #38. See: Willi Hock and Klaus Schittkowski. (1981) Test Examples for 3 | % Nonlinear Programming Codes. Lecture Notes in Economics and Mathematical 4 | % Systems Vol. 187, Springer-Verlag. 5 | % 6 | % Copyright (C) 2008 Peter Carbonetto. All Rights Reserved. 7 | % This code is published under the Eclipse Public License. 8 | % 9 | % Author: Peter Carbonetto 10 | % Dept. of Computer Science 11 | % University of British Columbia 12 | % September 18, 2008 13 | function [x, info] = examplehs038 14 | 15 | x0 = [-3 -1 -3 -1]; % The starting point. 16 | options.lb = [-10 -10 -10 -10]; % Lower bound on the variables. 17 | options.ub = [+10 +10 +10 +10]; % Upper bound on the variables. 18 | 19 | % The callback functions. 20 | funcs.objective = @objective; 21 | funcs.gradient = @gradient; 22 | funcs.hessian = @hessian; 23 | funcs.hessianstructure = @hessianstructure; 24 | funcs.iterfunc = @callback; 25 | 26 | % Set the IPOPT options. 27 | options.ipopt.mu_strategy = 'adaptive'; 28 | options.ipopt.print_level = 0; 29 | options.ipopt.tol = 1e-7; 30 | options.ipopt.max_iter = 100; 31 | 32 | % Run IPOPT. 33 | [x info] = ipopt(x0,funcs,options); 34 | 35 | % ---------------------------------------------------------------------- 36 | function f = objective (x) 37 | f = 100*(x(2)-x(1)^2)^2 + (1-x(1))^2 + 90*(x(4)-x(3)^2)^2 + (1-x(3))^2 + ... 38 | 10.1*(x(2)-1)^2 + 10.1*(x(4)-1)^2 + 19.8*(x(2)-1)*(x(4)-1); 39 | 40 | % ---------------------------------------------------------------------- 41 | function g = gradient (x) 42 | g(1) = -400*x(1)*(x(2)-x(1)^2) - 2*(1-x(1)); 43 | g(2) = 200*(x(2)-x(1)^2) + 20.2*(x(2)-1) + 19.8*(x(4)-1); 44 | g(3) = -360*x(3)*(x(4)-x(3)^2) -2*(1-x(3)); 45 | g(4) = 180*(x(4)-x(3)^2) + 20.2*(x(4)-1) + 19.8*(x(2)-1); 46 | 47 | % ---------------------------------------------------------------------- 48 | function H = hessianstructure() 49 | H = sparse([ 1 0 0 0 50 | 1 1 0 0 51 | 0 0 1 0 52 | 0 1 1 1 ]); 53 | 54 | % ---------------------------------------------------------------------- 55 | function H = hessian (x, sigma, lambda) 56 | H = [ 1200*x(1)^2-400*x(2)+2 0 0 0 57 | -400*x(1) 220.2 0 0 58 | 0 0 1080*x(3)^2- 360*x(4) + 2 0 59 | 0 19.8 -360*x(3) 200.2 ]; 60 | H = sparse(sigma*H); 61 | 62 | % ---------------------------------------------------------------------- 63 | function b = callback (t, f, x) 64 | fprintf('%3d %0.3g \n',t,f); 65 | b = true; -------------------------------------------------------------------------------- /Ipopt/examples/examplehs051.m: -------------------------------------------------------------------------------- 1 | % Test the "ipopt" Matlab interface on the Hock & Schittkowski test problem 2 | % #51. See: Willi Hock and Klaus Schittkowski. (1981) Test Examples for 3 | % Nonlinear Programming Codes. Lecture Notes in Economics and Mathematical 4 | % Systems Vol. 187, Springer-Verlag. 5 | % 6 | % Copyright (C) 2008 Peter Carbonetto. All Rights Reserved. 7 | % This code is published under the Eclipse Public License. 8 | % 9 | % Author: Peter Carbonetto 10 | % Dept. of Computer Science 11 | % University of British Columbia 12 | % September 18, 2008 13 | 14 | % fminsdp path 15 | addpath('C:\Users\ardui\Desktop\MATLAB_MK_2021\Ipopt\ipopt.mexw64'); 16 | 17 | function [x, info] = examplehs051 18 | 19 | x0 = [ 2.5 0.5 2 -1 0.5 ]; % The starting point. 20 | options.cl = [ 4 0 0 ]; % Lower bounds on constraints. 21 | options.cu = [ 4 0 0 ]; % Upper bounds on constraints. 22 | 23 | % Set the IPOPT options. 24 | options.ipopt.jac_c_constant = 'yes'; 25 | options.ipopt.hessian_approximation = 'limited-memory'; 26 | options.ipopt.mu_strategy = 'adaptive'; 27 | options.ipopt.tol = 1e-7; 28 | 29 | % The callback functions. 30 | funcs.objective = @objective; 31 | funcs.constraints = @constraints; 32 | funcs.gradient = @gradient; 33 | funcs.jacobian = @jacobian; 34 | funcs.jacobianstructure = @jacobian; 35 | 36 | % Run IPOPT. 37 | [x info] = ipopt(x0,funcs,options); 38 | end 39 | 40 | % ---------------------------------------------------------------------- 41 | function f = objective (x) 42 | f = (x(1) - x(2))^2 + ... 43 | (x(2) + x(3) - 2)^2 + ... 44 | (x(4) - 1)^2 + (x(5) - 1)^2; 45 | 46 | % ---------------------------------------------------------------------- 47 | function g = gradient (x) 48 | g = 2*[ x(1) - x(2); 49 | x(2) + x(3) - 2 - x(1) + x(2); 50 | x(2) + x(3) - 2; 51 | x(4) - 1; 52 | x(5) - 1 ]; 53 | end 54 | % ---------------------------------------------------------------------- 55 | function c = constraints (x) 56 | c = [ x(1) + 3*x(2); 57 | x(3) + x(4) - 2*x(5); 58 | x(2) - x(5) ]; 59 | end 60 | % ---------------------------------------------------------------------- 61 | function J = jacobian (x) 62 | J = sparse([ 1 3 0 0 0; 63 | 0 0 1 1 -2; 64 | 0 1 0 0 -1 ]); 65 | end -------------------------------------------------------------------------------- /Ipopt/examples/examplehs071.m: -------------------------------------------------------------------------------- 1 | % Test the "ipopt" Matlab interface on the Hock & Schittkowski test problem 2 | % #71. See: Willi Hock and Klaus Schittkowski. (1981) Test Examples for 3 | % Nonlinear Programming Codes. Lecture Notes in Economics and Mathematical 4 | % Systems Vol. 187, Springer-Verlag. 5 | % 6 | % Copyright (C) 2008 Peter Carbonetto. All Rights Reserved. 7 | % This code is published under the Eclipse Public License. 8 | % 9 | % Author: Peter Carbonetto 10 | % Dept. of Computer Science 11 | % University of British Columbia 12 | % September 18, 2008 13 | % fminsdp path 14 | 15 | % % Ipopt 16 | % addpath('C:\Users\ardui\Desktop\MATLAB_MK_2021\Ipopt'); 17 | 18 | function [x, info] = examplehs071 19 | 20 | x0 = [1 5 5 1]; % The starting point. 21 | options.lb = [1 1 1 1]; % Lower bound on the variables. 22 | options.ub = [5 5 5 5]; % Upper bound on the variables. 23 | options.cl = [25 40]; % Lower bounds on the constraint functions. 24 | options.cu = [inf 40]; % Upper bounds on the constraint functions. 25 | 26 | % Initialize the dual point. 27 | options.zl = [1 1 1 1]; 28 | options.zu = [1 1 1 1]; 29 | options.lambda = [1 1]; 30 | 31 | % Set the IPOPT options. 32 | options.ipopt.mu_strategy = 'adaptive'; 33 | options.ipopt.tol = 1e-7; 34 | 35 | % The callback functions. 36 | funcs.objective = @(x) x(1)*x(4)*sum(x(1:3)) + x(3); 37 | funcs.constraints = @(x) [ prod(x); sum(x.^2) ]; 38 | funcs.gradient = @gradient; 39 | funcs.jacobian = @(x) sparse([ prod(x)./x; 2*x ]); 40 | funcs.jacobianstructure = @() sparse(ones(2,4)); 41 | funcs.hessian = @hessian; 42 | funcs.hessianstructure = @() sparse(tril(ones(4))); 43 | 44 | % Run IPOPT. 45 | [x info] = ipopt(x0,funcs,options); 46 | 47 | % ---------------------------------------------------------------------- 48 | function g = gradient (x) 49 | g = [ x(1)*x(4) + x(4)*sum(x(1:3)) 50 | x(1)*x(4) 51 | x(1)*x(4) + 1 52 | x(1)*sum(x(1:3)) ]; 53 | 54 | % ---------------------------------------------------------------------- 55 | function H = hessian (x, sigma, lambda) 56 | 57 | H = sigma*[ 2*x(4) 0 0 0; 58 | x(4) 0 0 0; 59 | x(4) 0 0 0; 60 | 2*x(1)+x(2)+x(3) x(1) x(1) 0 ]; 61 | 62 | H = H + lambda(1)*[ 0 0 0 0; 63 | x(3)*x(4) 0 0 0; 64 | x(2)*x(4) x(1)*x(4) 0 0; 65 | x(2)*x(3) x(1)*x(3) x(1)*x(2) 0 ]; 66 | H = H + lambda(2)*diag([2 2 2 2]); 67 | H = sparse(H); 68 | -------------------------------------------------------------------------------- /Ipopt/examples/examplelasso.m: -------------------------------------------------------------------------------- 1 | % In this small MATLAB script, we compute the least squares solution to a 2 | % regression problem subject to L1 regularization, which rewards "sparse" 3 | % models that have regression coefficients of zero. See, for instance, 4 | % the work in the "Lasso" by the statistician Robert Tibshirani. 5 | % 6 | % Copyright (C) 2008 Peter Carbonetto. All Rights Reserved. 7 | % This code is published under the Eclipse Public License. 8 | % 9 | % Author: Peter Carbonetto 10 | % Dept. of Computer Science 11 | % University of British Columbia 12 | % September 18, 2008 13 | 14 | % Experiment parameters. 15 | lambda = 1; % Level of L1 regularization. 16 | n = 100; % Number of training examples. 17 | e = 1; % Std. dev. in noise of outputs. 18 | beta = [ 0 0 2 -4 0 0 -1 3 ]'; % "True" regression coefficients. 19 | 20 | % Set the random number generator seed. 21 | seed = 7; 22 | rand('state',seed); 23 | randn('state',seed); 24 | 25 | % CREATE DATA SET. 26 | % Generate the input vectors from the standard normal, and generate the 27 | % binary responses from the regression with some additional noise, and then 28 | % transform the results using the logistic function. The variable "beta" is 29 | % the set of true regression coefficients. 30 | m = length(beta); % Number of features. 31 | A = randn(n,m); % The n x m matrix of examples. 32 | noise = e * randn(n,1); % Noise in outputs. 33 | y = A * beta + noise; % The binary outputs. 34 | 35 | % COMPUTE SOLUTION WITH IPOPT. 36 | % Compute the L1-regularized maximum likelihood estimator. 37 | w = lasso(A,y,lambda); 38 | fprintf('Solution:\n'); 39 | disp(w); 40 | 41 | 42 | -------------------------------------------------------------------------------- /Ipopt/examples/lasso.m: -------------------------------------------------------------------------------- 1 | % This function executes IPOPT to find the maximum likelihood solution to 2 | % least squares regression with L1 regularization or the "Lasso". The inputs 3 | % are the data matrix A (in which each row is an example vector), the vector 4 | % of regression outputs y, and the penalty parameter lambda, a number 5 | % greater than zero. The output is the estimated vector of regression 6 | % coefficients. 7 | % 8 | % Copyright (C) 2008 Peter Carbonetto. All Rights Reserved. 9 | % This code is published under the Eclipse Public License. 10 | % 11 | % Author: Peter Carbonetto 12 | % Dept. of Computer Science 13 | % University of British Columbia 14 | % September 18, 2008 15 | function w = lasso (A, y, lambda) 16 | 17 | % Get the number of examples (n) and the number of regression 18 | % coefficients (m). 19 | [n m] = size(A); 20 | 21 | % The starting point. 22 | x0 = { zeros(m,1) 23 | ones(m,1) }; 24 | 25 | % The constraint functions are bounded from below by zero. 26 | options.cl = zeros(2*m,1); 27 | options.cu = repmat(Inf,2*m,1); 28 | 29 | % Set up the auxiliary data. 30 | options.auxdata = { n m A y lambda }; 31 | 32 | % Set the IPOPT options. 33 | options.ipopt.jac_d_constant = 'yes'; 34 | options.ipopt.hessian_constant = 'yes'; 35 | options.ipopt.mu_strategy = 'adaptive'; 36 | options.ipopt.max_iter = 100; 37 | options.ipopt.tol = 1e-8; 38 | 39 | % The callback functions. 40 | funcs.objective = @objective; 41 | funcs.constraints = @constraints; 42 | funcs.gradient = @gradient; 43 | funcs.jacobian = @jacobian; 44 | funcs.jacobianstructure = @jacobianstructure; 45 | funcs.hessian = @hessian; 46 | funcs.hessianstructure = @hessianstructure; 47 | 48 | % Run IPOPT. 49 | [x info] = ipopt_auxdata(x0,funcs,options); 50 | w = x{1}; 51 | 52 | % ------------------------------------------------------------------ 53 | function f = objective (x, auxdata) 54 | [n m A y lambda] = deal(auxdata{:}); 55 | [w u] = deal(x{:}); 56 | f = norm(y - A*w)^2/2 + lambda*sum(u); 57 | 58 | % ------------------------------------------------------------------ 59 | function c = constraints (x, auxdata) 60 | [w u] = deal(x{:}); 61 | c = [ w + u; u - w ]; 62 | 63 | % ------------------------------------------------------------------ 64 | function g = gradient (x, auxdata) 65 | [n m A y lambda] = deal(auxdata{:}); 66 | w = x{1}; 67 | g = { -A'*(y - A*w) 68 | repmat(lambda,m,1) }; 69 | 70 | % ------------------------------------------------------------------ 71 | function J = jacobianstructure (auxdata) 72 | m = auxdata{2}; 73 | I = speye(m); 74 | J = [ I I 75 | I I ]; 76 | 77 | % ------------------------------------------------------------------ 78 | function J = jacobian (x, auxdata) 79 | m = auxdata{2}; 80 | I = speye(m); 81 | J = [ I I 82 | -I I ]; 83 | 84 | % ------------------------------------------------------------------ 85 | function H = hessianstructure (auxdata) 86 | m = auxdata{2}; 87 | H = [ tril(ones(m)) zeros(m) 88 | zeros(m) zeros(m) ]; 89 | H = sparse(H); 90 | 91 | % ------------------------------------------------------------------ 92 | function H = hessian (x, sigma, lambda, auxdata) 93 | [n m A y lambda] = deal(auxdata{:}); 94 | H = [ tril(A'*A) zeros(m) 95 | zeros(m) zeros(m) ]; 96 | H = sparse(sigma * H); 97 | -------------------------------------------------------------------------------- /Ipopt/ipopt.m: -------------------------------------------------------------------------------- 1 | % IPOPT Call the IPOPT constrained, nonlinear solver. 2 | % The basic function call is 3 | % 4 | % [x, info] = IPOPT(x0,funcs,options) 5 | % 6 | % The first input is either a matrix or a cell array of matrices. It 7 | % declares the starting point for the solver. 8 | % 9 | % CALLBACK FUNCTIONS 10 | % 11 | % The second input must be struct containing function handles for various 12 | % MATLAB routines. For more information on using functions and function 13 | % handles in MATLAB, type HELP FUNCTION and HELP FUNCTION_HANDLE in the 14 | % MATLAB prompt. 15 | % 16 | % funcs.objective (required) 17 | % 18 | % Calculates the objective function at the current point. It takes one 19 | % point, the current iterate x. For example, the definition of the 20 | % objective function for the Hock & Schittkowski (H&S) test problem #71 21 | % (with 4 optimization variables) would be 22 | % 23 | % function f = objective (x) 24 | % f = x(1)*x(4)*sum(x(1:3)) + x(3); 25 | % 26 | % funcs.gradient (required) 27 | % 28 | % Computes the gradient of the objective at the current point. It takes 29 | % one input, the current iterate x. For H&S test problem #71, the 30 | % definition of the gradient callback would be 31 | % 32 | % function g = gradient (x) 33 | % g = [ x(1)*x(4) + x(4)*sum(x(1:3)) 34 | % x(1)*x(4) 35 | % x(1)*x(4) + 1 36 | % x(1)*sum(x(1:3)) ]; 37 | % 38 | % funcs.constraints (optional) 39 | % 40 | % This function is only required if there are constraints on your 41 | % variables. It evaluates the constraint functions at the current 42 | % point. It takes one input, x. The return value is a vector of length 43 | % equal to the number of constraints (it must be of the same length as 44 | % options.cl and options.cu). For H&S test problem #71, the 45 | % callback definition would be 46 | % 47 | % function c = constraints (x) 48 | % c = [ prod(x); sum(x.^2) ]; 49 | % 50 | % funcs.jacobian (optional) 51 | % 52 | % This function is only required if there are constraints on your 53 | % variables. Evaluates the Jacobian of the constraints at the current 54 | % point. It takes one input, x. The output must always be an M x N 55 | % sparse matrix, where M is the number of constraints and N is the 56 | % number of variables. Type HELP SPARSE for more information on 57 | % constructing sparse matrices in MATLAB. The definition of the 58 | % callback function for H&S test problem #71 would be 59 | % 60 | % function J = jacobian (x) 61 | % sparse([ prod(x)./x; 2*x ]); 62 | % 63 | % Notice that the return value is a sparse matrix. 64 | % 65 | % funcs.jacobianstructure (optional) 66 | % 67 | % This function is only required if there are constraints on your 68 | % variables. It takes no inputs. The return value is a sparse 69 | % matrix whereby an entry is nonzero if and only if the Jacobian of 70 | % the constraints is nonzero at ANY point. The callback function for 71 | % the H&S test problem #71 simply returns a 2 x 4 matrix of ones in 72 | % the sparse matrix format: 73 | % 74 | % function J = jacobianstructure() 75 | % J = sparse(ones(2,4)); 76 | % 77 | % funcs.hessian (optional) 78 | % 79 | % Evaluates the Hessian of the Lagrangian at the current point. It 80 | % must be specified unless you choose to use the limited-memory 81 | % quasi-Newton approximation to the Hessian (see below). 82 | % 83 | % The callback function has three inputs: the current point (x), a 84 | % scalar factor on the objective (sigma), and the Lagrange multipliers 85 | % (lambda), a vector of length equal to the number of constraints. The 86 | % function should compute 87 | % 88 | % sigma*H + lambda(1)*G1 + ... + lambda(M)*GM 89 | % 90 | % where M is the number of constraints, H is the Hessian of the 91 | % objective and the G's are the Hessians of the constraint 92 | % functions. The output must always be an N x N sparse, lower triangular 93 | % matrix, where N is the number of variables. In other words, if X is 94 | % the output value, then X must be the same as TRIL(X). 95 | % 96 | % Here is an implementation of the Hessian callback routine for the 97 | % H&S test problem #71: 98 | % 99 | % function H = hessian (x, sigma, lambda) 100 | % H = sigma*[ 2*x(4) 0 0 0; 101 | % x(4) 0 0 0; 102 | % x(4) 0 0 0; 103 | % 2*x(1)+x(2)+x(3) x(1) x(1) 0 ]; 104 | % H = H + lambda(1)*[ 0 0 0 0; 105 | % x(3)*x(4) 0 0 0; 106 | % x(2)*x(4) x(1)*x(4) 0 0; 107 | % x(2)*x(3) x(1)*x(3) x(1)*x(2) 0 ]; 108 | % H = sparse(H + lambda(2)*diag([2 2 2 2])); 109 | % 110 | % funcs.hessianstructure (optional) 111 | % 112 | % This function serves the same purpose as funcs.jacobianstructure, but 113 | % for the Hessian matrix. Again, it is not needed if you are using the 114 | % limited-memory quasi-Newton approximation to the Hessian. It takes no 115 | % inputs, and must return a sparse, lower triangular matrix. For H&S 116 | % test problem #71, the MATLAB callback routine is fairly 117 | % straightforward: 118 | % 119 | % function H = hessianstructure() 120 | % H = sparse(tril(ones(4))); 121 | % 122 | % funcs.iterfunc (optional) 123 | % 124 | % An additional callback routine that is called once per algorithm 125 | % iteration. It takes three inputs: the first is the current iteration 126 | % of the algorithm, the second is the current value of the objective, 127 | % and the third is a structure containing fields x, inf_pr, inf_du, mu, 128 | % d_norm, regularization_size, alpha_du, alpha_pr, and ls_trials. This 129 | % function should always return true unless you want IPOPT to terminate 130 | % prematurely for whatever reason. If you would like to use the third 131 | % input to iterfunc along with auxdata functionality, you will need to 132 | % modify the appropriate section of ipopt_auxdata.m. 133 | % 134 | % OPTIONS 135 | % 136 | % The options are passed through the third input. What follows is a 137 | % description of the fields you may optionally specify. 138 | % 139 | % options.lb 140 | % 141 | % Specify lower bounds on the variables. It must have the same number 142 | % of elements as x0. Set an entry to -Inf to specify no lower bound. 143 | % 144 | % options.ub 145 | % 146 | % Specify upper bounds on the variables. It must have the same number 147 | % of elements as x0. Set an entry to Inf to specify no upper bound. 148 | % 149 | % options.cl, options.cu 150 | % 151 | % Set lower and upper bounds on the constraints. Each should be a 152 | % vector of length equal to the number of constraints. As before, a 153 | % bound is removed by setting the entry to -Inf or +Inf. An equality 154 | % constraint is achieved by setting cl(i) = cu(i). 155 | % 156 | % options.auxdata 157 | % 158 | % Optionally, one may choose to pass additional auxiliary data to the 159 | % MATLAB callback routines listed above through the function call. For 160 | % instance, the objective callback function now takes two inputs, x and 161 | % auxdata. The auxiliary data may not change through the course of the 162 | % IPOPT optimization. The auxiliary data keep the same values as they 163 | % possessed in the initial call. If you need variables that change over 164 | % time, you may want to consider global variables (type HELP 165 | % GLOBAL). See the lasso.m file in the examples subdirectory for an 166 | % illustration of how the auxiliary data is passed to the various 167 | % callback functions. Starting with Ipopt version 3.11, you must call 168 | % ipopt_auxdata(x0,funcs,options) to use auxdata functionality. 169 | % 170 | % options.zl, options.zu, options.lambda 171 | % 172 | % These fields specify the initial value for the Lagrange multipliers, 173 | % which is especially useful for "warm starting" the interior-point 174 | % solver. They specify the Lagrange multipliers corresponding to the 175 | % lower bounds on the variables, upper bounds on the variables, and 176 | % constraints, respectively. 177 | % 178 | % options.ipopt 179 | % 180 | % Finally, you may also change the settings of IPOPT through this 181 | % field. For instance, to turn off the IPOPT output, use the 182 | % limited-memory BFGS approximation to the Hessian, and turn on the 183 | % derivative checker, do the following: 184 | % 185 | % options.ipopt.print_level = 0; 186 | % options.ipopt.hessian_approximation = 'limited-memory'; 187 | % options.ipopt.derivative_test = 'first-order'; 188 | % 189 | % For more details, see the documentation on the IPOPT website. 190 | % 191 | % OUTPUTS 192 | % 193 | % If the solver successfully converges to a stationary point or terminated 194 | % without an unrecoverable error, the function IPOPT outputs the candidate 195 | % solution x. In all other cases, an error is thrown. It also outputs some 196 | % additional information: 197 | % 198 | % info.zl, info.zu, info.lambda 199 | % 200 | % The value of the Lagrange multipliers at the solution. See the 201 | % "options" for more information on the Lagrange multipliers. 202 | % 203 | % info.status 204 | % 205 | % Upon termination, this field will take on one of these following 206 | % values (for a more up-to-date listing, see the IpReturnCodes.h header 207 | % file in the IPOPT C++ source directory): 208 | % 209 | % 0 solved 210 | % 1 solved to acceptable level 211 | % 2 infeasible problem detected 212 | % 3 search direction becomes too small 213 | % 4 diverging iterates 214 | % 5 user requested stop 215 | % 216 | % -1 maximum number of iterations exceeded 217 | % -2 restoration phase failed 218 | % -3 error in step computation 219 | % -10 not enough degrees of freedom 220 | % -11 invalid problem definition 221 | % -12 invalid option 222 | % -13 invalid number detected 223 | % 224 | % -100 unrecoverable exception 225 | % -101 non-IPOPT exception thrown 226 | % -102 insufficient memory 227 | % -199 internal error 228 | % 229 | % info.iter, info.cpu 230 | % 231 | % Number of iterations and CPU time (in seconds) taken by the Ipopt run 232 | % 233 | % Finally, for more information, please consult the following webpages: 234 | % 235 | % http://www.cs.ubc.ca/~pcarbo/ipopt-for-matlab 236 | % http://projects.coin-or.org/Ipopt 237 | % 238 | % Copyright (C) 2008 Peter Carbonetto. All Rights Reserved. 239 | % This code is published under the Eclipse Public License. 240 | % 241 | % Author: Peter Carbonetto 242 | % Dept. of Computer Science 243 | % University of British Columbia 244 | % September 19, 2008 245 | -------------------------------------------------------------------------------- /Ipopt/ipopt.mexa64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Ramune6110/MATLAB-Robotics-Engineering/cb44e950c5183f6000c183928f9c65cb3ced82da/Ipopt/ipopt.mexa64 -------------------------------------------------------------------------------- /Ipopt/ipopt.mexmaci64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Ramune6110/MATLAB-Robotics-Engineering/cb44e950c5183f6000c183928f9c65cb3ced82da/Ipopt/ipopt.mexmaci64 -------------------------------------------------------------------------------- /Ipopt/ipopt.mexw32: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Ramune6110/MATLAB-Robotics-Engineering/cb44e950c5183f6000c183928f9c65cb3ced82da/Ipopt/ipopt.mexw32 -------------------------------------------------------------------------------- /Ipopt/ipopt.mexw64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Ramune6110/MATLAB-Robotics-Engineering/cb44e950c5183f6000c183928f9c65cb3ced82da/Ipopt/ipopt.mexw64 -------------------------------------------------------------------------------- /Ipopt/ipopt_auxdata.m: -------------------------------------------------------------------------------- 1 | function [x, info] = ipopt_auxdata(x0, funcs, options) 2 | % Wrapper function to implement auxdata functionality using Matlab function handles 3 | 4 | if ~isfield(options, 'auxdata') 5 | % no auxdata given, call ipopt as normal 6 | if isfield(funcs, 'iterfunc') && nargin(funcs.iterfunc) == 2 7 | % check if iterfunc has only 2 inputs as before Ipopt version 3.11 8 | funcs_old = funcs; 9 | funcs.iterfunc = @(t, f, varstruct) funcs_old.iterfunc(t, f); 10 | end 11 | [x, info] = ipopt(x0, funcs, options); 12 | else 13 | % remove auxdata from options structure and modify function handles 14 | auxdata = options.auxdata; 15 | options_new = rmfield(options, 'auxdata'); 16 | funcs_new.objective = @(x) funcs.objective(x, auxdata); 17 | funcs_new.gradient = @(x) funcs.gradient(x, auxdata); 18 | if isfield(funcs, 'constraints') 19 | funcs_new.constraints = @(x) funcs.constraints(x, auxdata); 20 | end 21 | if isfield(funcs, 'jacobian') 22 | funcs_new.jacobian = @(x) funcs.jacobian(x, auxdata); 23 | end 24 | if isfield(funcs, 'jacobianstructure') 25 | funcs_new.jacobianstructure = @() funcs.jacobianstructure(auxdata); 26 | end 27 | if isfield(funcs, 'hessian') 28 | funcs_new.hessian = @(x, sigma, lambda) funcs.hessian(x, sigma, lambda, auxdata); 29 | end 30 | if isfield(funcs, 'hessianstructure') 31 | funcs_new.hessianstructure = @() funcs.hessianstructure(auxdata); 32 | end 33 | if isfield(funcs, 'iterfunc') 34 | % This assumes the pre-3.11 convention for iterfunc. If you would 35 | % like to use the additional information that is available via the 36 | % third input argument to iterfunc as of Ipopt version 3.11, you 37 | % will need to modify this section by uncommenting the line below. 38 | funcs_new.iterfunc = @(t, f, varstruct) funcs.iterfunc(t, f, auxdata); 39 | % funcs_new.iterfunc = @(t, f, varstruct) funcs.iterfunc(t, f, varstruct, auxdata); 40 | end 41 | [x, info] = ipopt(x0, funcs_new, options_new); 42 | end 43 | -------------------------------------------------------------------------------- /MathematicalMethod/CircleFittingExample.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Ramune6110/MATLAB-Robotics-Engineering/cb44e950c5183f6000c183928f9c65cb3ced82da/MathematicalMethod/CircleFittingExample.m -------------------------------------------------------------------------------- /MathematicalMethod/Copy_of_test_co.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Ramune6110/MATLAB-Robotics-Engineering/cb44e950c5183f6000c183928f9c65cb3ced82da/MathematicalMethod/Copy_of_test_co.slx -------------------------------------------------------------------------------- /MathematicalMethod/CurvatureCalcuration.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Ramune6110/MATLAB-Robotics-Engineering/cb44e950c5183f6000c183928f9c65cb3ced82da/MathematicalMethod/CurvatureCalcuration.m -------------------------------------------------------------------------------- /MathematicalMethod/DegitalFilter/AdvancedDigitalFilter.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Ramune6110/MATLAB-Robotics-Engineering/cb44e950c5183f6000c183928f9c65cb3ced82da/MathematicalMethod/DegitalFilter/AdvancedDigitalFilter.m -------------------------------------------------------------------------------- /MathematicalMethod/DegitalFilter/BasicDigitalFilter.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Ramune6110/MATLAB-Robotics-Engineering/cb44e950c5183f6000c183928f9c65cb3ced82da/MathematicalMethod/DegitalFilter/BasicDigitalFilter.m -------------------------------------------------------------------------------- /MathematicalMethod/ErrorMetric.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Ramune6110/MATLAB-Robotics-Engineering/cb44e950c5183f6000c183928f9c65cb3ced82da/MathematicalMethod/ErrorMetric.m -------------------------------------------------------------------------------- /MathematicalMethod/NonlinearOptimalMethod/Conjugate_gradient_method.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Ramune6110/MATLAB-Robotics-Engineering/cb44e950c5183f6000c183928f9c65cb3ced82da/MathematicalMethod/NonlinearOptimalMethod/Conjugate_gradient_method.m -------------------------------------------------------------------------------- /MathematicalMethod/NonlinearOptimalMethod/Golden.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Ramune6110/MATLAB-Robotics-Engineering/cb44e950c5183f6000c183928f9c65cb3ced82da/MathematicalMethod/NonlinearOptimalMethod/Golden.m -------------------------------------------------------------------------------- /MathematicalMethod/NonlinearOptimalMethod/Newton_method.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Ramune6110/MATLAB-Robotics-Engineering/cb44e950c5183f6000c183928f9c65cb3ced82da/MathematicalMethod/NonlinearOptimalMethod/Newton_method.m -------------------------------------------------------------------------------- /MathematicalMethod/NonlinearOptimalMethod/Quasi_Newton_method.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Ramune6110/MATLAB-Robotics-Engineering/cb44e950c5183f6000c183928f9c65cb3ced82da/MathematicalMethod/NonlinearOptimalMethod/Quasi_Newton_method.m -------------------------------------------------------------------------------- /MathematicalMethod/NonlinearOptimalMethod/SteepestDescentMethod.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Ramune6110/MATLAB-Robotics-Engineering/cb44e950c5183f6000c183928f9c65cb3ced82da/MathematicalMethod/NonlinearOptimalMethod/SteepestDescentMethod.m -------------------------------------------------------------------------------- /MathematicalMethod/SimulinkBlock.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Ramune6110/MATLAB-Robotics-Engineering/cb44e950c5183f6000c183928f9c65cb3ced82da/MathematicalMethod/SimulinkBlock.m -------------------------------------------------------------------------------- /MathematicalMethod/SimulinkBlock_TestCode.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Ramune6110/MATLAB-Robotics-Engineering/cb44e950c5183f6000c183928f9c65cb3ced82da/MathematicalMethod/SimulinkBlock_TestCode.m -------------------------------------------------------------------------------- /MathematicalMethod/Untitled2.m: -------------------------------------------------------------------------------- 1 | filename = 'test_co.slx'; 2 | L = strlength(filename); 3 | filename(1) 4 | 5 | extension_length = 4; 6 | temp_str = ''; 7 | for i = 1:L - extension_length 8 | temp_str = append(temp_str,filename(i)); 9 | end 10 | 11 | temp_str -------------------------------------------------------------------------------- /MathematicalMethod/commentblockdelete.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Ramune6110/MATLAB-Robotics-Engineering/cb44e950c5183f6000c183928f9c65cb3ced82da/MathematicalMethod/commentblockdelete.m -------------------------------------------------------------------------------- /MathematicalMethod/delete_comment_out.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Ramune6110/MATLAB-Robotics-Engineering/cb44e950c5183f6000c183928f9c65cb3ced82da/MathematicalMethod/delete_comment_out.m -------------------------------------------------------------------------------- /MathematicalMethod/delete_comment_out2.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Ramune6110/MATLAB-Robotics-Engineering/cb44e950c5183f6000c183928f9c65cb3ced82da/MathematicalMethod/delete_comment_out2.m -------------------------------------------------------------------------------- /MathematicalMethod/main.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Ramune6110/MATLAB-Robotics-Engineering/cb44e950c5183f6000c183928f9c65cb3ced82da/MathematicalMethod/main.m -------------------------------------------------------------------------------- /MathematicalMethod/test_co.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Ramune6110/MATLAB-Robotics-Engineering/cb44e950c5183f6000c183928f9c65cb3ced82da/MathematicalMethod/test_co.slx -------------------------------------------------------------------------------- /ModelPredictiveControl/Linear_Model_Predictive_Control(LMPC)/Collision_Avoidance_of_Car.m: -------------------------------------------------------------------------------- 1 | clear; 2 | close all; 3 | clc; 4 | 5 | %% Ipopt solver path 6 | dir = pwd; 7 | addpath(fullfile(dir,'Ipopt')) 8 | 9 | % Ipopt 10 | addpath('C:\Users\ardui\Desktop\MATLAB_MK_2021\Ipopt'); 11 | 12 | %% Setup and Parameters 13 | x0 = [0.0; 0.0; 0.0; 0.0; 0.0; 0.0]; % Initial state of the inverted pendulum 14 | 15 | dt = 0.1; % sample time 16 | nx = 6; % Number of states 17 | nu = 2; % Number of input 18 | 19 | P = 200 * eye(nx); % Termination cost weight matrix 20 | Q = diag([1.0, 1.0, 1.0, 1.0, 1.0, 1.0]); % Weight matrix of state quantities 21 | R = diag([0.1, 0.1]); % Weight matrix of input quantities 22 | N = 10; % Predictive Horizon 23 | p = 200; % salck weight value 24 | 25 | % Range of states and inputs 26 | xmin = [-5; -5; -5; -5; -5; -5]; 27 | xmax = [6; 6; 5; 5; 5; 5]; 28 | umin = [-1; -5]; 29 | umax = [1; 5]; 30 | 31 | % target state value 32 | x_target = [6.0; 1.0; 0.0; 0.0; 0.0; 0.0]; 33 | 34 | % Obstacle 35 | obs.pos = [3.0; 0.0]; 36 | obs.own_vehicle_diameter = 0.5; % [m] 37 | obs.obstacle_diameter = 0.5; % [m] 38 | obs.r = obs.own_vehicle_diameter + obs.obstacle_diameter; 39 | 40 | %% Linear Car models 41 | M = 1500; % [kg] 42 | I = 2500; % [kgm^2] 43 | lf = 1.1; % [m] 44 | lr = 1.6; % [m] 45 | l = lf + lr; % [m] 46 | Kf = 55000; % [N/rad] 47 | Kr = 60000; % [N/rad] 48 | V = 20; % [m/s] 49 | 50 | system.dt = dt; 51 | system.nx = nx; 52 | system.nu = nu; 53 | 54 | A53 = 2 * (Kf * lf + Kr * lr) / I; 55 | A55 = -2 * (Kf * lf + Kr * lr) / (I * V); 56 | A56 = -2 * (Kf * lf^2 + Kr * lr^2) / I; 57 | A63 = 2 * (Kf + Kr) / M; 58 | A65 = -2 * (Kf + Kr) / (M * V); 59 | A66 = -2 * (Kf * lf + Kr * lr) / (M * V); 60 | system.A = eye(nx) + [0.0, 0.0, 0.0, 1.0, 0.0, 0.0; 61 | 0.0, 0.0, 0.0, 0.0, 1.0, 0.0; 62 | 0.0, 0.0, 0.0, 0.0, 0.0, 1.0; 63 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; 64 | 0.0, 0.0, A53, 0.0, A55, A56; 65 | 0.0, 0.0, A63, 0.0, A65, A66] .* dt; 66 | 67 | B51 = 2 * Kf / M; 68 | B61 = 2 * Kf * lf / I; 69 | system.B = [0.0, 0.0; 70 | 0.0, 0.0; 71 | 0.0, 0.0; 72 | 0.0, 1.0; 73 | B51, 0.0; 74 | B61, 0.0] .* dt; 75 | system.xl = xmin; 76 | system.xu = xmax; 77 | system.ul = umin; 78 | system.uu = umax; 79 | 80 | system.target = x_target; 81 | 82 | %% MPC parameters 83 | params_mpc.Q = Q; 84 | params_mpc.R = R; 85 | params_mpc.P = P; 86 | params_mpc.N = N; 87 | params_mpc.p = p; 88 | 89 | %% main loop 90 | xTrue(:, 1) = x0; 91 | uk(:, 1) = [0; 0]; 92 | time = 3.0; 93 | time_curr = 0.0; 94 | current_step = 1; 95 | solvetime = zeros(1, time / dt); 96 | 97 | while time_curr <= time 98 | % update time 99 | time_curr = time_curr + system.dt; 100 | current_step = current_step + 1; 101 | 102 | % solve mac 103 | tic; 104 | uk(:, current_step) = mpc(xTrue(:, current_step - 1), system, obs, params_mpc); 105 | solvetime(1, current_step - 1) = toc; 106 | 107 | % update state 108 | xTrue(:, current_step) = system.A * xTrue(:, current_step - 1) + system.B * uk(:, current_step); 109 | end 110 | 111 | %% solve average time 112 | avg_time = sum(solvetime) / current_step; 113 | disp(avg_time); 114 | 115 | drow_figure(xTrue, uk, obs, x_target, current_step); 116 | 117 | %% model predictive control 118 | function uopt = mpc(xTrue, system, obs, params_mpc) 119 | % Solve MPC 120 | [feas, ~, u, ~] = solve_mpc(xTrue, system, obs, params_mpc); 121 | if ~feas 122 | uopt = []; 123 | return 124 | else 125 | uopt = u(:, 1); 126 | end 127 | end 128 | 129 | function [feas, xopt, uopt, Jopt] = solve_mpc(xTrue, system, obs, params) 130 | % extract variables 131 | N = params.N; 132 | % define variables and cost 133 | x = sdpvar(system.nx, N+1); 134 | u = sdpvar(system.nu, N); 135 | delta = sdpvar(1, N); 136 | constraints = []; 137 | cost = 0; 138 | 139 | % initial constraint 140 | constraints = [constraints; x(:,1) == xTrue]; 141 | % add constraints and costs 142 | for i = 1:N 143 | constraints = [constraints; 144 | system.xl <= x(:,i) <= system.xu; 145 | system.ul <= u(:,i) <= system.uu 146 | x(:,i+1) == system.A * x(:,i) + system.B * u(:,i)]; 147 | cost = cost + (x(:,i) - system.target)' * params.Q * (x(:,i) - system.target) + u(:,i)' * params.R * u(:,i); 148 | end 149 | % add collition avoidance constraints 150 | for i = 1:N 151 | pos = obs.pos; 152 | r = obs.r; 153 | distance = (x([1:2],i)-pos)'*((x([1:2],i)-pos)) - r^2; 154 | constraints = [constraints; distance >= delta(1, i)]; 155 | cost = cost + delta(1, i)' * params.p * delta(1, i); 156 | end 157 | % add terminal cost 158 | cost = cost + (x(:,N+1) - system.target)' * params.P * (x(:,N+1) - system.target); 159 | ops = sdpsettings('solver','ipopt','verbose',0); 160 | % solve optimization 161 | diagnostics = optimize(constraints, cost, ops); 162 | if diagnostics.problem == 0 163 | feas = true; 164 | xopt = value(x); 165 | uopt = value(u); 166 | Jopt = value(cost); 167 | else 168 | feas = false; 169 | xopt = []; 170 | uopt = []; 171 | Jopt = []; 172 | end 173 | end 174 | 175 | function drow_figure(xlog, ulog, obs, x_target, current_step) 176 | % Plot simulation 177 | figure(1) 178 | subplot(2, 1, 1) 179 | plot(0:current_step - 1, ulog(1,:), 'ko-',... 180 | 'LineWidth', 1.0, 'MarkerSize', 4); 181 | xlabel('Time Step','interpreter','latex','FontSize',10); 182 | ylabel('delta [rad]','interpreter','latex','FontSize',10); 183 | yline(1.0, '--b', 'LineWidth', 2.0); 184 | yline(-1.0, '--b', 'LineWidth', 2.0); 185 | 186 | subplot(2, 1, 2); 187 | plot(0:current_step - 1, ulog(2,:), 'ko-',... 188 | 'LineWidth', 1.0, 'MarkerSize', 4); 189 | xlabel('Time Step','interpreter','latex','FontSize',10); 190 | ylabel('Acceleration [$m/s^2$]','interpreter','latex','FontSize',10); 191 | yline(5.0, '--b', 'LineWidth', 2.0); 192 | yline(-5.0, '--b', 'LineWidth', 2.0); 193 | 194 | figure(2) 195 | % plot trajectory 196 | plot(xlog(1,:), xlog(2,:), 'ko-',... 197 | 'LineWidth', 1.0, 'MarkerSize', 4); 198 | hold on; 199 | grid on; 200 | axis equal; 201 | % plot initial position 202 | plot(xlog(1, 1), xlog(1, 2), 'dm', 'LineWidth', 2); 203 | % plot target position 204 | plot(x_target(1), x_target(2), 'dg', 'LineWidth', 2); 205 | xlabel('X[m]','interpreter','latex','FontSize',10); 206 | ylabel('Y[m]','interpreter','latex','FontSize',10); 207 | % plot obstacle 208 | pos = obs.pos; 209 | r = obs.obstacle_diameter; 210 | th = linspace(0,2*pi*100); 211 | x = cos(th); 212 | y = sin(th); 213 | plot(pos(1) + r*x, pos(2) + r*y, 'r', 'LineWidth', 2); 214 | plot(pos(1), pos(2), 'r', 'MarkerSize', 5, 'LineWidth', 2); 215 | % plot vegicle 216 | for i = 1:current_step 217 | r_vegicle = obs.own_vehicle_diameter; 218 | th = linspace(0,2*pi*100); 219 | x = cos(th); 220 | y = sin(th); 221 | X = xlog(1,i); 222 | Y = xlog(2,i); 223 | plot(X + r_vegicle * x, Y + r_vegicle * y, 'b','LineWidth', 2); 224 | hold on; 225 | end 226 | yline(0.5, '--k', 'LineWidth', 2.0); 227 | yline(2.0, 'k', 'LineWidth', 4.0); 228 | yline(-1.0, 'k', 'LineWidth', 4.0); 229 | 230 | legend('Motion trajectory','Initial position', 'Target position','Obstacle', 'Location','southeast',... 231 | 'interpreter','latex','FontSize',10.0); 232 | end 233 | -------------------------------------------------------------------------------- /ModelPredictiveControl/Linear_Model_Predictive_Control(LMPC)/Inverted_Pendulum.m: -------------------------------------------------------------------------------- 1 | clear; 2 | close all; 3 | clc; 4 | 5 | %% Ipopt solver path 6 | dir = pwd; 7 | addpath(fullfile(dir,'Ipopt')) 8 | 9 | %% Setup and Parameters 10 | x0 = [-0.02; 0.0; 0.1; 0.0]; % Initial state of the inverted pendulum 11 | 12 | dt = 0.1; % sample time 13 | nx = 4; % Number of states 14 | nu = 1; % Number of input 15 | 16 | P = 30 * eye(nx); % Termination cost weight matrix 17 | Q = diag([1.0, 1.0, 1.0, 1.0]); % Weight matrix of state quantities 18 | R = 0.01; % Weight matrix of input quantities 19 | N = 10; % Predictive Horizon 20 | 21 | % Range of states and inputs 22 | xmin = [-5; -5; -5; -5]; 23 | xmax = [5; 5; 5; 5]; 24 | umin = -5; 25 | umax = 5; 26 | 27 | %% Linear Inverted Pendulum 28 | M = 1.0; % [kg] 29 | m = 0.3; % [kg] 30 | g = 9.8; % [m/s^2] 31 | l = 2.0; % [m] 32 | system.dt = dt; 33 | system.nx = nx; 34 | system.nu = nu; 35 | system.A = eye(nx) + [0.0, 1.0, 0.0, 0.0; 36 | 0.0, 0.0, m * g / M, 0.0; 37 | 0.0, 0.0, 0.0, 1.0; 38 | 0.0, 0.0, g * (M + m) / (l * M), 0.0] .* dt; 39 | system.B = [0.0; 40 | 1.0 / M; 41 | 0.0; 42 | 1.0 / (l * M)] .* dt; 43 | system.xl = xmin; 44 | system.xu = xmax; 45 | system.ul = umin; 46 | system.uu = umax; 47 | 48 | %% MPC parameters 49 | params_mpc.Q = Q; 50 | params_mpc.R = R; 51 | params_mpc.P = P; 52 | params_mpc.N = N; 53 | 54 | %% main loop 55 | xTrue(:, 1) = x0; 56 | uk(:, 1) = 0; 57 | time = 20.0; 58 | time_curr = 0.0; 59 | current_step = 1; 60 | solvetime = zeros(1, time / dt); 61 | 62 | while time_curr <= time 63 | % update time 64 | time_curr = time_curr + system.dt; 65 | current_step = current_step + 1; 66 | 67 | % solve mac 68 | tic; 69 | uk(:, current_step)= mpc(xTrue(:, current_step - 1), system, params_mpc); 70 | solvetime(1, current_step - 1) = toc; 71 | 72 | % update state 73 | xTrue(:, current_step) = system.A * xTrue(:, current_step - 1) + system.B * uk(:, current_step); 74 | end 75 | 76 | %% solve average time 77 | avg_time = sum(solvetime) / current_step; 78 | disp(avg_time); 79 | 80 | drow_figure(xTrue, uk, current_step); 81 | 82 | %% model predictive control 83 | function uopt = mpc(xTrue, system, params_mpc) 84 | % Solve MPC 85 | [feas, ~, u, ~] = solve_mpc(xTrue, system, params_mpc); 86 | if ~feas 87 | uopt = []; 88 | return 89 | else 90 | uopt = u(:,1); 91 | end 92 | end 93 | 94 | function [feas, xopt, uopt, Jopt] = solve_mpc(xTrue, system, params) 95 | % extract variables 96 | N = params.N; 97 | % define variables and cost 98 | x = sdpvar(system.nx, N+1); 99 | u = sdpvar(system.nu, N); 100 | constraints = []; 101 | cost = 0; 102 | 103 | % initial constraint 104 | constraints = [constraints; x(:,1) == xTrue]; 105 | % add constraints and costs 106 | for i = 1:N 107 | constraints = [constraints; 108 | system.xl <= x(:,i) <= system.xu; 109 | system.ul <= u(:,i) <= system.uu 110 | x(:,i+1) == system.A * x(:,i) + system.B * u(:,i)]; 111 | cost = cost + x(:,i)' * params.Q * x(:,i) + u(:,i)' * params.R * u(:,i); 112 | end 113 | % add terminal cost 114 | cost = cost + x(:,N+1)' * params.P * x(:,N+1); 115 | ops = sdpsettings('solver','ipopt','verbose',0); 116 | % solve optimization 117 | diagnostics = optimize(constraints, cost, ops); 118 | if diagnostics.problem == 0 119 | feas = true; 120 | xopt = value(x); 121 | uopt = value(u); 122 | Jopt = value(cost); 123 | else 124 | feas = false; 125 | xopt = []; 126 | uopt = []; 127 | Jopt = []; 128 | end 129 | end 130 | 131 | function drow_figure(xlog, ulog, current_step) 132 | % Plot simulation 133 | figure(1) 134 | subplot(2,2,1) 135 | plot(0:current_step - 1, xlog(1,:), 'ko-',... 136 | 'LineWidth', 1.0, 'MarkerSize', 4); 137 | xlabel('Time Step','interpreter','latex','FontSize',10); 138 | ylabel('X[m]','interpreter','latex','FontSize',10); 139 | yline(0.0, '--r', 'LineWidth', 1.0); 140 | 141 | subplot(2,2,2) 142 | plot(0:current_step - 1, xlog(2,:), 'ko-',... 143 | 'LineWidth', 1.0, 'MarkerSize', 4); 144 | xlabel('Time Step','interpreter','latex','FontSize',10); 145 | ylabel('$\dot{X}$[m/s]','interpreter','latex','FontSize',10); 146 | yline(0.0, '--r', 'LineWidth', 1.0); 147 | 148 | subplot(2,2,3) 149 | plot(0:current_step - 1, xlog(3,:), 'ko-',... 150 | 'LineWidth', 1.0, 'MarkerSize', 4); 151 | xlabel('Time Step','interpreter','latex','FontSize',10); 152 | ylabel('$\theta$[rad]','interpreter','latex','FontSize',10); 153 | yline(0.0, '--r', 'LineWidth', 1.0); 154 | 155 | subplot(2,2,4) 156 | plot(0:current_step - 1, xlog(4,:), 'ko-',... 157 | 'LineWidth', 1.0, 'MarkerSize', 4); 158 | xlabel('Time Step','interpreter','latex','FontSize',10); 159 | ylabel('$\dot{\theta}$[rad/s]','interpreter','latex','FontSize',10); 160 | yline(0.0, '--r', 'LineWidth', 1.0); 161 | 162 | figure(2) 163 | plot(0:current_step - 1, ulog(1,:), 'ko-',... 164 | 'LineWidth', 1.0, 'MarkerSize', 4); 165 | xlabel('Time Step','interpreter','latex','FontSize',10); 166 | ylabel('Input F[N]','interpreter','latex','FontSize',10); 167 | yline(5.0, '--b', 'LineWidth', 2.0); 168 | yline(-5.0, '--b', 'LineWidth', 2.0); 169 | end 170 | -------------------------------------------------------------------------------- /ModelPredictiveControl/Linear_Model_Predictive_Control(LMPC)/Lateral_Control_of_Car.m: -------------------------------------------------------------------------------- 1 | clear; 2 | close all; 3 | clc; 4 | 5 | %% Ipopt solver path 6 | dir = pwd; 7 | addpath(fullfile(dir,'Ipopt')) 8 | 9 | %% Setup and Parameters 10 | x0 = [0.0; 2.0]; % Initial state of the inverted pendulum 11 | 12 | dt = 0.1; % sample time 13 | v = 30.0; % driving speed 14 | nx = 2; % Number of states 15 | nu = 1; % Number of input 16 | 17 | P = 30 * eye(nx); % Termination cost weight matrix 18 | Q = diag([1.0, 1.0]); % Weight matrix of state quantities 19 | R = 0.01; % Weight matrix of input quantities 20 | N = 20; % Predictive Horizon 21 | 22 | % Range of states and inputs 23 | xmin = [-5; -5]; 24 | xmax = [5; 5]; 25 | umin = -1; 26 | umax = 1; 27 | 28 | x_target = [0.0; 1.0]; % target state value 29 | 30 | %% Linear Inverted Pendulum 31 | system.dt = dt; 32 | system.nx = nx; 33 | system.nu = nu; 34 | system.A = [1.0, 0.0; 35 | v * dt, 1.0]; 36 | system.B = [dt; 37 | 0.5 * v * dt^2]; 38 | system.xl = xmin; 39 | system.xu = xmax; 40 | system.ul = umin; 41 | system.uu = umax; 42 | 43 | system.target = x_target; 44 | 45 | %% MPC parameters 46 | params_mpc.Q = Q; 47 | params_mpc.R = R; 48 | params_mpc.P = P; 49 | params_mpc.N = N; 50 | 51 | %% main loop 52 | xTrue(:, 1) = x0; 53 | uk(:, 1) = 0; 54 | time = 1.0; 55 | time_curr = 0.0; 56 | current_step = 1; 57 | solvetime = zeros(1, time / dt); 58 | 59 | while time_curr <= time 60 | % update time 61 | time_curr = time_curr + system.dt; 62 | current_step = current_step + 1; 63 | 64 | % solve mac 65 | tic; 66 | uk(:, current_step) = mpc(xTrue(:, current_step - 1), system, params_mpc); 67 | solvetime(1, current_step - 1) = toc; 68 | 69 | % update state 70 | xTrue(:, current_step) = system.A * xTrue(:, current_step - 1) + system.B * uk(:, current_step); 71 | end 72 | 73 | %% solve average time 74 | avg_time = sum(solvetime) / current_step; 75 | disp(avg_time); 76 | 77 | drow_figure(xTrue, uk, current_step); 78 | 79 | %% model predictive control 80 | function uopt = mpc(xTrue, system, params_mpc) 81 | % Solve MPC 82 | [feas, ~, u, ~] = solve_mpc(xTrue, system, params_mpc); 83 | if ~feas 84 | uopt = []; 85 | return 86 | else 87 | uopt = u(:,1); 88 | end 89 | end 90 | 91 | function [feas, xopt, uopt, Jopt] = solve_mpc(xTrue, system, params) 92 | % extract variables 93 | N = params.N; 94 | % define variables and cost 95 | x = sdpvar(system.nx, N+1); 96 | u = sdpvar(system.nu, N); 97 | constraints = []; 98 | cost = 0; 99 | 100 | % initial constraint 101 | constraints = [constraints; x(:,1) == xTrue]; 102 | % add constraints and costs 103 | for i = 1:N 104 | constraints = [constraints; 105 | system.xl <= x(:,i) <= system.xu; 106 | system.ul <= u(:,i) <= system.uu 107 | x(:,i+1) == system.A * x(:,i) + system.B * u(:,i)]; 108 | cost = cost + (x(:,i) - system.target)' * params.Q * (x(:,i) - system.target) + u(:,i)' * params.R * u(:,i); 109 | end 110 | % add terminal cost 111 | cost = cost + (x(:,N+1) - system.target)' * params.P * (x(:,N+1) - system.target); 112 | ops = sdpsettings('solver','ipopt','verbose',0); 113 | % solve optimization 114 | diagnostics = optimize(constraints, cost, ops); 115 | if diagnostics.problem == 0 116 | feas = true; 117 | xopt = value(x); 118 | uopt = value(u); 119 | Jopt = value(cost); 120 | else 121 | feas = false; 122 | xopt = []; 123 | uopt = []; 124 | Jopt = []; 125 | end 126 | end 127 | 128 | function drow_figure(xlog, ulog, current_step) 129 | % Plot simulation 130 | figure(1) 131 | subplot(2,1,1) 132 | plot(0:current_step - 1, xlog(1,:), 'ko-',... 133 | 'LineWidth', 1.0, 'MarkerSize', 4); 134 | xlim([0, 11]); 135 | xlabel('Time Step','interpreter','latex','FontSize',10); 136 | ylabel('Heading angle[deg]','interpreter','latex','FontSize',10); 137 | yline(0.0, '--r', 'LineWidth', 1.0); 138 | 139 | subplot(2,1,2); 140 | plot(0:current_step - 1, xlog(2,:), 'ko-',... 141 | 'LineWidth', 1.0, 'MarkerSize', 4); 142 | xlim([0, 11]); 143 | xlabel('Time Step','interpreter','latex','FontSize',10); 144 | ylabel('Lateral error[m]','interpreter','latex','FontSize',10); 145 | yline(1.0, '--r', 'LineWidth', 1.0); 146 | yline(1.5, '--k', 'LineWidth', 2.0); 147 | yline(2.5, 'k', 'LineWidth', 4.0); 148 | yline(0.5, 'k', 'LineWidth', 4.0); 149 | 150 | figure(2) 151 | plot(0:current_step - 1, ulog(1,:), 'ko-',... 152 | 'LineWidth', 1.0, 'MarkerSize', 4); 153 | xlim([0, 11]); 154 | xlabel('Time Step','interpreter','latex','FontSize',10); 155 | ylabel('Steering input[deg/s]','interpreter','latex','FontSize',10); 156 | yline(1.0, '--b', 'LineWidth', 2.0); 157 | yline(-1.0, '--b', 'LineWidth', 2.0); 158 | end 159 | -------------------------------------------------------------------------------- /ModelPredictiveControl/NonLinear_Model_Predictive_Control(NMPC)/CGMRES_method/Semi_active_dumper.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Ramune6110/MATLAB-Robotics-Engineering/cb44e950c5183f6000c183928f9c65cb3ced82da/ModelPredictiveControl/NonLinear_Model_Predictive_Control(NMPC)/CGMRES_method/Semi_active_dumper.m -------------------------------------------------------------------------------- /ModelPredictiveControl/NonLinear_Model_Predictive_Control(NMPC)/Newton_method/Hovercraft.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Ramune6110/MATLAB-Robotics-Engineering/cb44e950c5183f6000c183928f9c65cb3ced82da/ModelPredictiveControl/NonLinear_Model_Predictive_Control(NMPC)/Newton_method/Hovercraft.m -------------------------------------------------------------------------------- /ModelPredictiveControl/NonLinear_Model_Predictive_Control(NMPC)/Newton_method/Semi_active_dumper.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Ramune6110/MATLAB-Robotics-Engineering/cb44e950c5183f6000c183928f9c65cb3ced82da/ModelPredictiveControl/NonLinear_Model_Predictive_Control(NMPC)/Newton_method/Semi_active_dumper.m -------------------------------------------------------------------------------- /ModelPredictiveControl/README.md: -------------------------------------------------------------------------------- 1 | # Model Predictive Control(MPC) 2 | This directory provides Model Predictive Control sample code and documentation. 3 | 4 | ## Linear Model Predictive Control(LMPC) 5 | ![Fig10](https://user-images.githubusercontent.com/52307432/160145851-42205af4-de87-4490-a7b2-eff77d1de7b6.png) 6 | 7 | Sample code for automobile collision avoidance. 8 | 9 | Documents in Japanease: 10 | 線形モデル予測制御入門：MATLABサンプルプログラム - Ramune6110 11 | https://ramune6110.hatenablog.com/entry/2022/02/13/154405 12 | 13 | ## Trajectory tracking control(LMPC) 14 | ![fig3](https://user-images.githubusercontent.com/52307432/160147124-ef1dc4a0-5ee6-42bb-a416-6248afba4583.png) 15 | 16 | Sample code for automobile trajectory tracking control. 17 | 18 | Documents in Japanease: 19 | 二次計画法に基づく線形モデル予測制御による軌道追従制御：MATLABサンプルプログラム - Ramune6110 20 | https://ramune6110.hatenablog.com/entry/2022/02/24/000153 21 | 22 | ## Nonlinear Model Predictive Control(NMPC) 23 | ### Newton method 24 | ![fig11](https://user-images.githubusercontent.com/52307432/160147602-91d9b7cb-d87c-4deb-ada6-cb18a2f1df23.png) 25 | 26 | Sample code for automobile position control. 27 | 28 | Documents in Japanease: 29 | ニュートン法に基づく非線形モデル予測制御：MATLABサンプルプログラム - Ramune6110 30 | https://ramune6110.hatenablog.com/entry/2022/03/21/203716 31 | 32 | # Author 33 | Ramune6110([@ExcitingRamune](https://twitter.com/ExcitingRamune)) 34 | -------------------------------------------------------------------------------- /ModelPredictiveControl/Trajectory_tracking_control(LMPC)/Lateral_Vehicle_Dynamics_by_YALMIP.m: -------------------------------------------------------------------------------- 1 | clear; 2 | close all; 3 | clc; 4 | 5 | %% Ipopt solver path 6 | dir = pwd; 7 | addpath(fullfile(dir,'Ipopt')) 8 | 9 | %% Choice the type of trajectory 10 | while true 11 | model = input('Please choose the type of trajectory (1 = Lane change, 2 = Circle): '); 12 | switch model 13 | case 1 14 | trajectory_type = 'Lane change'; 15 | break; 16 | case 2 17 | trajectory_type = 'Circle'; 18 | break; 19 | otherwise 20 | disp('No System!'); 21 | end 22 | end 23 | 24 | %% Setup and Parameters 25 | dt = 0.1; % sample time 26 | nx = 4; % Number of states 27 | ny = 2; % Number of ovservation 28 | nu = 1; % Number of input 29 | 30 | S = diag([10.0, 1.0]); % Termination cost weight matrix 31 | Q = diag([10.0, 1.0]); % Weight matrix of state quantities 32 | R = 30.0; % Weight matrix of input quantities 33 | N = 15; % Predictive Horizon 34 | 35 | % Range of inputs 36 | umin = -pi / 60; 37 | umax = pi / 60; 38 | 39 | %% Linear Car models (Lateral Vehicle Dynamics) 40 | M = 1500; % [kg] 41 | I = 3000; % [kgm^2] 42 | lf = 1.2; % [m] 43 | lr = 1.6; % [m] 44 | l = lf + lr; % [m] 45 | Kf = 19000; % [N/rad] 46 | Kr = 33000; % [N/rad] 47 | Vx = 20; % [m/s] 48 | 49 | system.dt = dt; 50 | system.nx = nx; 51 | system.ny = ny; 52 | system.nu = nu; 53 | 54 | % states = [y_dot, psi, psi_dot, Y]; 55 | % Vehicle Dynamics and Control (2nd edition) by Rajesh Rajamani. They are in Chapter 2.3. 56 | A11 = -2 * (Kf + Kr)/(M * Vx); 57 | A13 = -Vx - 2 * (Kf*lf - Kr*lr)/(M * Vx); 58 | A31 = -2 * (lf*Kf - lr*Kr)/(I * Vx); 59 | A33 = -2 * (lf^2*Kf + lr^2*Kr)/(I * Vx); 60 | system.A = eye(nx) + [A11, 0.0, A13, 0.0; 61 | 0.0, 0.0, 1.0, 0.0; 62 | A31, 0.0, A33, 0.0; 63 | 1.0, Vx, 0.0, 0.0] * dt; 64 | 65 | B11 = 2*Kf/M; 66 | B13 = 2*lf*Kf/I; 67 | system.B = [B11; 68 | 0.0; 69 | B13; 70 | 0.0] * dt; 71 | 72 | system.C = [0 1 0 0; 73 | 0 0 0 1]; 74 | 75 | system.A_aug = [system.A, system.B; 76 | zeros(length(system.B(1,:)),length(system.A(1,:))),eye(length(system.B(1,:)))]; 77 | system.B_aug = [system.B; 78 | eye(length(system.B(1,:)))]; 79 | system.C_aug = [system.C,zeros(length(system.C(:,1)),length(system.B(1,:)))]; 80 | 81 | % Constants 82 | system.Vx = Vx; 83 | system.M = M; 84 | system.I = I; 85 | system.Kf = Kf; 86 | system.Kr = Kr; 87 | system.lf = lf; 88 | system.lr = lr; 89 | 90 | % input 91 | system.ul = umin; 92 | system.uu = umax; 93 | 94 | %% Load the trajectory 95 | if strcmp(trajectory_type, 'Lane change') 96 | t = 0:dt:10.0; 97 | elseif strcmp(trajectory_type, 'Circle') 98 | t = 0:dt:80.0; 99 | end 100 | [x_ref, y_ref, psi_ref] = trajectory_generator(t, Vx, trajectory_type); 101 | 102 | sim_length = length(t); % Number of control loop iterations 103 | 104 | refSignals = zeros(length(x_ref(:, 1)) * ny, 1); 105 | 106 | ref_index = 1; 107 | for i = 1:ny:length(refSignals) 108 | refSignals(i) = psi_ref(ref_index, 1); 109 | refSignals(i + 1) = y_ref(ref_index, 1); 110 | ref_index = ref_index + 1; 111 | end 112 | 113 | % initial state 114 | x0 = [0.0; psi_ref(1, 1); 0.0; y_ref(1, 1)]; % Initial state of Lateral Vehicle Dynamics 115 | 116 | %% MPC parameters 117 | params_mpc.Q = Q; 118 | params_mpc.R = R; 119 | params_mpc.S = S; 120 | params_mpc.N = N; 121 | 122 | %% main loop 123 | xTrue(:, 1) = x0; 124 | uk(:, 1) = 0.0; 125 | du(:, 1) = 0.0; 126 | current_step = 1; 127 | solvetime = zeros(1, sim_length); 128 | 129 | ref_sig_num = 1; % for reading reference signals 130 | for i = 1:sim_length-15 131 | % update time 132 | current_step = current_step + 1; 133 | 134 | % Generating the current state and the reference vector 135 | xTrue_aug = [xTrue(:, current_step - 1); uk(:, current_step - 1)]; 136 | ref_sig_num = ref_sig_num + ny; 137 | 138 | if ref_sig_num + ny * N - 1 <= length(refSignals) 139 | ref = refSignals(ref_sig_num:ref_sig_num+ny*N-1); 140 | else 141 | ref = refSignals(ref_sig_num:length(refSignals)); 142 | N = N - 1; 143 | end 144 | 145 | % solve mac 146 | tic; 147 | du(:, current_step) = mpc(xTrue_aug, system, params_mpc, ref); 148 | solvetime(1, current_step - 1) = toc; 149 | 150 | % add du input 151 | uk(:, current_step) = uk(:, current_step - 1) + du(:, current_step); 152 | 153 | % update state 154 | T = dt*i:dt:dt*i+dt; 155 | [T, x] = ode45(@(t,x) nonlinear_lateral_car_model(t, x, uk(:, current_step), system), T, xTrue(:, current_step - 1)); 156 | xTrue(:, current_step) = x(end,:); 157 | end 158 | 159 | %% solve average time 160 | avg_time = sum(solvetime) / current_step; 161 | disp(avg_time); 162 | 163 | drow_figure(xTrue, uk, du, x_ref, y_ref, current_step, trajectory_type); 164 | 165 | %% model predictive control 166 | function uopt = mpc(xTrue_aug, system, params, ref) 167 | % Solve MPC 168 | [feas, ~, u, ~] = solve_mpc(xTrue_aug, system, params, ref); 169 | if ~feas 170 | uopt = []; 171 | return 172 | else 173 | uopt = u(:, 1); 174 | end 175 | end 176 | 177 | function [feas, xopt, uopt, Jopt] = solve_mpc(xTrue_aug, system, params, ref) 178 | % extract variables 179 | N = params.N; 180 | % define variables and cost 181 | x = sdpvar(system.nx + system.nu, N+1); 182 | u = sdpvar(system.nu, N); 183 | constraints = []; 184 | cost = 0; 185 | ref_index = 1; 186 | 187 | % initial constraint 188 | constraints = [constraints; x(:,1) == xTrue_aug]; 189 | % add constraints and costs 190 | for i = 1:N-1 191 | constraints = [constraints; 192 | system.ul <= u(:,i) <= system.uu 193 | x(:,i+1) == system.A_aug * x(:,i) + system.B_aug * u(:,i)]; 194 | cost = cost + (ref(ref_index:ref_index + 1,1) - system.C_aug * x(:,i+1))' * params.Q * (ref(ref_index:ref_index + 1,1) - system.C_aug * x(:,i+1)) + u(:,i)' * params.R * u(:,i); 195 | ref_index = ref_index + system.ny; 196 | end 197 | % add terminal cost 198 | cost = cost + (ref(ref_index:ref_index+1,1) - system.C_aug * x(:,N+1))' * params.S * (ref(ref_index:ref_index+1,1) - system.C_aug * x(:,N+1)); 199 | ops = sdpsettings('solver','ipopt','verbose',0); 200 | % solve optimization 201 | diagnostics = optimize(constraints, cost, ops); 202 | if diagnostics.problem == 0 203 | feas = true; 204 | xopt = value(x); 205 | uopt = value(u); 206 | Jopt = value(cost); 207 | else 208 | feas = false; 209 | xopt = []; 210 | uopt = []; 211 | Jopt = []; 212 | end 213 | end 214 | 215 | %% trajectory generator 216 | function [x_ref, y_ref, psi_ref] = trajectory_generator(t, Vx, trajectory_type) 217 | if strcmp(trajectory_type, 'Lane change') 218 | x = linspace(0, Vx * t(end), length(t)); 219 | y = 3 * tanh((t - t(end) / 2)); 220 | elseif strcmp(trajectory_type, 'Circle') 221 | radius = 30; 222 | period = 1000; 223 | for i = 1:length(t) 224 | x(1, i) = radius * sin(2 * pi * i / period); 225 | y(1, i) = -radius * cos(2 * pi * i / period); 226 | end 227 | end 228 | 229 | dx = x(2:end) - x(1:end-1); 230 | dy = y(2:end) - y(1:end-1); 231 | 232 | psi = zeros(1,length(x)); 233 | psiInt = psi; 234 | 235 | psi(1) = atan2(dy(1),dx(1)); 236 | psi(2:end) = atan2(dy(:),dx(:)); 237 | dpsi = psi(2:end) - psi(1:end-1); 238 | 239 | psiInt(1) = psi(1); 240 | for i = 2:length(psiInt) 241 | if dpsi(i - 1) < -pi 242 | psiInt(i) = psiInt(i - 1) + (dpsi(i - 1) + 2 * pi); 243 | elseif dpsi(i - 1) > pi 244 | psiInt(i) = psiInt(i - 1) + (dpsi(i - 1) - 2 * pi); 245 | else 246 | psiInt(i) = psiInt(i - 1) + dpsi(i - 1); 247 | end 248 | end 249 | 250 | x_ref = x'; 251 | y_ref = y'; 252 | psi_ref = psiInt'; 253 | end 254 | 255 | function dx = nonlinear_lateral_car_model(t, xTrue, u, system) 256 | % In this simulation, the body frame and its transformation is used 257 | % instead of a hybrid frame. That is because for the solver ode45, it 258 | % is important to have the nonlinear system of equations in the first 259 | % order form. 260 | 261 | % Get the constants from the general pool of constants 262 | Vx = system.Vx; 263 | M = system.M; 264 | I = system.I; 265 | Kf = system.Kf; 266 | Kr = system.Kr; 267 | lf = system.lf; 268 | lr = system.lr; 269 | 270 | % % Assign the states 271 | y_dot = xTrue(1); 272 | psi = xTrue(2); 273 | psi_dot = xTrue(3); 274 | 275 | % The nonlinear equation describing the dynamics of the car 276 | dx(1,1) = -(2*Kf+2*Kr)/(M * Vx)*y_dot+(-Vx-(2*Kf*lf-2*Kr*lr)/(M * Vx))*psi_dot+2*Kf/M*u; 277 | dx(2,1) = psi_dot; 278 | dx(3,1) = -(2*lf*Kf-2*lr*Kr)/(I * Vx)*y_dot-(2*lf^2*Kf+2*lr^2*Kr)/(I * Vx)*psi_dot+2*lf*Kf/I*u; 279 | dx(4,1) = sin(psi) * Vx + cos(psi) * y_dot; 280 | end 281 | 282 | function drow_figure(xTrue, uk, du, x_ref, y_ref, current_step, trajectory_type) 283 | % Plot simulation 284 | figure(1) 285 | subplot(2, 1, 1) 286 | plot(0:current_step - 1, du(1,:), 'ko-',... 287 | 'LineWidth', 1.0, 'MarkerSize', 4); 288 | xlabel('Time Step','interpreter','latex','FontSize',10); 289 | ylabel('$\Delta \delta$ [rad]','interpreter','latex','FontSize',10); 290 | yline(pi / 60, '--b', 'LineWidth', 2.0); 291 | yline(-pi / 60, '--b', 'LineWidth', 2.0); 292 | 293 | subplot(2, 1, 2) 294 | plot(0:current_step - 1, uk(1,:), 'ko-',... 295 | 'LineWidth', 1.0, 'MarkerSize', 4); 296 | xlabel('Time Step','interpreter','latex','FontSize',10); 297 | ylabel('$\delta$ [rad]','interpreter','latex','FontSize',10); 298 | 299 | figure(2) 300 | if strcmp(trajectory_type, 'Lane change') 301 | % plot trajectory 302 | plot(x_ref(1: current_step, 1),y_ref(1: current_step, 1),'b','LineWidth',2) 303 | hold on 304 | plot(x_ref(1: current_step, 1),xTrue(4, 1:current_step),'r','LineWidth',2) 305 | hold on; 306 | grid on; 307 | % plot initial position 308 | plot(x_ref(1, 1), y_ref(1, 1), 'dg', 'LineWidth', 2); 309 | xlabel('X[m]','interpreter','latex','FontSize',10); 310 | ylabel('Y[m]','interpreter','latex','FontSize',10); 311 | yline(0.0, '--k', 'LineWidth', 2.0); 312 | yline(10.0, 'k', 'LineWidth', 4.0); 313 | yline(-10.0, 'k', 'LineWidth', 4.0); 314 | ylim([-12 12]) 315 | 316 | legend('Refernce trajectory','Motion trajectory','Initial position', 'Location','southeast',... 317 | 'interpreter','latex','FontSize',10.0); 318 | elseif strcmp(trajectory_type, 'Circle') 319 | % plot trajectory 320 | plot(x_ref(1: current_step, 1),y_ref(1: current_step, 1),'--b','LineWidth',2) 321 | hold on 322 | plot(x_ref(1: current_step, 1),xTrue(4, 1:current_step),'r','LineWidth',1) 323 | hold on; 324 | grid on; 325 | axis equal; 326 | % plot initial position 327 | plot(x_ref(1, 1), y_ref(1, 1), 'db', 'LineWidth', 2); 328 | xlabel('X[m]','interpreter','latex','FontSize',10); 329 | ylabel('Y[m]','interpreter','latex','FontSize',10); 330 | 331 | legend('Refernce trajectory','Motion trajectory','Initial position', 'Location','southeast',... 332 | 'interpreter','latex','FontSize',10.0); 333 | end 334 | 335 | % plot lateral error 336 | figure(3) 337 | Lateral_error = y_ref(1: current_step, 1) - xTrue(4, 1:current_step)'; 338 | plot(0:current_step - 1, Lateral_error, 'ko-',... 339 | 'LineWidth', 1.0, 'MarkerSize', 4); 340 | xlabel('Time Step','interpreter','latex','FontSize',10); 341 | ylabel('Lateral error [m]','interpreter','latex','FontSize',10); 342 | end 343 | -------------------------------------------------------------------------------- /ModelPredictiveControl/Trajectory_tracking_control(LMPC)/Lateral_Vehicle_Dynamics_by_quadprog.m: -------------------------------------------------------------------------------- 1 | clear; 2 | close all; 3 | clc; 4 | 5 | %% Choice the type of trajectory 6 | while true 7 | model = input('Please choose the type of trajectory (1 = Lane change, 2 = Circle): '); 8 | switch model 9 | case 1 10 | trajectory_type = 'Lane change'; 11 | break; 12 | case 2 13 | trajectory_type = 'Circle'; 14 | break; 15 | otherwise 16 | disp('No System!'); 17 | end 18 | end 19 | 20 | %% Setup and Parameters 21 | dt = 0.1; % sample time 22 | nx = 4; % Number of states 23 | ny = 2; % Number of ovservation 24 | nu = 1; % Number of input 25 | 26 | S = diag([10.0, 1.0]); % Termination cost weight matrix 27 | Q = diag([10.0, 1.0]); % Weight matrix of state quantities 28 | R = 30.0; % Weight matrix of input quantities 29 | N = 15; % Predictive Horizon 30 | 31 | % Range of inputs 32 | umin = -pi / 60; 33 | umax = pi / 60; 34 | 35 | %% Linear Car models (Lateral Vehicle Dynamics) 36 | M = 1500; % [kg] 37 | I = 3000; % [kgm^2] 38 | lf = 1.2; % [m] 39 | lr = 1.6; % [m] 40 | l = lf + lr; % [m] 41 | Kf = 19000; % [N/rad] 42 | Kr = 33000; % [N/rad] 43 | Vx = 20; % [m/s] 44 | 45 | system.dt = dt; 46 | system.nx = nx; 47 | system.ny = ny; 48 | system.nu = nu; 49 | 50 | % states = [y_dot, psi, psi_dot, Y]; 51 | % Vehicle Dynamics and Control (2nd edition) by Rajesh Rajamani. They are in Chapter 2.3. 52 | A11 = -2 * (Kf + Kr)/(M * Vx); 53 | A13 = -Vx - 2 * (Kf*lf - Kr*lr)/(M * Vx); 54 | A31 = -2 * (lf*Kf - lr*Kr)/(I * Vx); 55 | A33 = -2 * (lf^2*Kf + lr^2*Kr)/(I * Vx); 56 | system.A = eye(nx) + [A11, 0.0, A13, 0.0; 57 | 0.0, 0.0, 1.0, 0.0; 58 | A31, 0.0, A33, 0.0; 59 | 1.0, Vx, 0.0, 0.0] * dt; 60 | 61 | B11 = 2*Kf/M; 62 | B13 = 2*lf*Kf/I; 63 | system.B = [B11; 64 | 0.0; 65 | B13; 66 | 0.0] * dt; 67 | 68 | system.C = [0 1 0 0; 69 | 0 0 0 1]; 70 | 71 | system.A_aug = [system.A, system.B; 72 | zeros(length(system.B(1,:)),length(system.A(1,:))),eye(length(system.B(1,:)))]; 73 | system.B_aug = [system.B; 74 | eye(length(system.B(1,:)))]; 75 | system.C_aug = [system.C,zeros(length(system.C(:,1)),length(system.B(1,:)))]; 76 | 77 | % Constants 78 | system.Vx = Vx; 79 | system.M = M; 80 | system.I = I; 81 | system.Kf = Kf; 82 | system.Kr = Kr; 83 | system.lf = lf; 84 | system.lr = lr; 85 | 86 | % input 87 | system.ul = umin; 88 | system.uu = umax; 89 | 90 | %% Load the trajectory 91 | if strcmp(trajectory_type, 'Lane change') 92 | t = 0:dt:10.0; 93 | elseif strcmp(trajectory_type, 'Circle') 94 | t = 0:dt:80.0; 95 | end 96 | [x_ref, y_ref, psi_ref] = trajectory_generator(t, Vx, trajectory_type); 97 | 98 | sim_length = length(t); % Number of control loop iterations 99 | 100 | refSignals = zeros(length(x_ref(:, 1)) * ny, 1); 101 | 102 | ref_index = 1; 103 | for i = 1:ny:length(refSignals) 104 | refSignals(i) = psi_ref(ref_index, 1); 105 | refSignals(i + 1) = y_ref(ref_index, 1); 106 | ref_index = ref_index + 1; 107 | end 108 | 109 | % initial state 110 | x0 = [0.0; psi_ref(1, 1); 0.0; y_ref(1, 1)]; % Initial state of Lateral Vehicle Dynamics 111 | 112 | %% MPC parameters 113 | params_mpc.Q = Q; 114 | params_mpc.R = R; 115 | params_mpc.S = S; 116 | params_mpc.N = N; 117 | 118 | %% main loop 119 | xTrue(:, 1) = x0; 120 | uk(:, 1) = 0.0; 121 | du(:, 1) = 0.0; 122 | current_step = 1; 123 | solvetime = zeros(1, sim_length); 124 | 125 | ref_sig_num = 1; % for reading reference signals 126 | for i = 1:sim_length-15 127 | % update time 128 | current_step = current_step + 1; 129 | 130 | % Generating the current state and the reference vector 131 | xTrue_aug = [xTrue(:, current_step - 1); uk(:, current_step - 1)]; 132 | ref_sig_num = ref_sig_num + ny; 133 | 134 | if ref_sig_num + ny * N - 1 <= length(refSignals) 135 | ref = refSignals(ref_sig_num:ref_sig_num+ny*N-1); 136 | else 137 | ref = refSignals(ref_sig_num:length(refSignals)); 138 | N = N - 1; 139 | end 140 | 141 | % solve mac 142 | tic; 143 | du(:, current_step) = mpc(xTrue_aug, system, params_mpc, ref); 144 | solvetime(1, current_step - 1) = toc; 145 | 146 | % add du input 147 | uk(:, current_step) = uk(:, current_step - 1) + du(:, current_step); 148 | 149 | % update state 150 | T = dt*i:dt:dt*i+dt; 151 | [T, x] = ode45(@(t,x) nonlinear_lateral_car_model(t, x, uk(:, current_step), system), T, xTrue(:, current_step - 1)); 152 | xTrue(:, current_step) = x(end,:); 153 | end 154 | 155 | %% solve average time 156 | avg_time = sum(solvetime) / current_step; 157 | disp(avg_time); 158 | 159 | drow_figure(xTrue, uk, du, x_ref, y_ref, current_step, trajectory_type); 160 | 161 | %% model predictive control 162 | function uopt = mpc(xTrue_aug, system, params_mpc, ref) 163 | A_aug = system.A_aug; 164 | B_aug = system.B_aug; 165 | C_aug = system.C_aug; 166 | 167 | Q = params_mpc.Q; 168 | S = params_mpc.S; 169 | R = params_mpc.R; 170 | N = params_mpc.N; 171 | 172 | CQC = C_aug' * Q * C_aug; 173 | CSC = C_aug' * S * C_aug; 174 | QC = Q * C_aug; 175 | SC = S * C_aug; 176 | 177 | Qdb = zeros(length(CQC(:,1))*N,length(CQC(1,:))*N); 178 | Tdb = zeros(length(QC(:,1))*N,length(QC(1,:))*N); 179 | Rdb = zeros(length(R(:,1))*N,length(R(1,:))*N); 180 | Cdb = zeros(length(B_aug(:,1))*N,length(B_aug(1,:))*N); 181 | Adc = zeros(length(A_aug(:,1))*N,length(A_aug(1,:))); 182 | 183 | % Filling in the matrices 184 | for i = 1:N 185 | if i == N 186 | Qdb(1+length(CSC(:,1))*(i-1):length(CSC(:,1))*i,1+length(CSC(1,:))*(i-1):length(CSC(1,:))*i) = CSC; 187 | Tdb(1+length(SC(:,1))*(i-1):length(SC(:,1))*i,1+length(SC(1,:))*(i-1):length(SC(1,:))*i) = SC; 188 | else 189 | Qdb(1+length(CQC(:,1))*(i-1):length(CQC(:,1))*i,1+length(CQC(1,:))*(i-1):length(CQC(1,:))*i) = CQC; 190 | Tdb(1+length(QC(:,1))*(i-1):length(QC(:,1))*i,1+length(QC(1,:))*(i-1):length(QC(1,:))*i) = QC; 191 | end 192 | 193 | Rdb(1+length(R(:,1))*(i-1):length(R(:,1))*i,1+length(R(1,:))*(i-1):length(R(1,:))*i) = R; 194 | 195 | for j = 1:N 196 | if j<=i 197 | Cdb(1+length(B_aug(:,1))*(i-1):length(B_aug(:,1))*i,1+length(B_aug(1,:))*(j-1):length(B_aug(1,:))*j) = A_aug^(i-j)*B_aug; 198 | end 199 | end 200 | Adc(1+length(A_aug(:,1))*(i-1):length(A_aug(:,1))*i,1:length(A_aug(1,:))) = A_aug^(i); 201 | end 202 | Hdb = Cdb' * Qdb * Cdb + Rdb; 203 | Fdbt = [Adc' * Qdb * Cdb; -Tdb * Cdb]; 204 | 205 | % Calling the optimizer (quadprog) 206 | % Cost function in quadprog: min(du)*1/2*du'Hdb*du+f'du 207 | ft = [xTrue_aug', ref'] * Fdbt; 208 | 209 | % Call the solver (quadprog) 210 | A = []; 211 | b = []; 212 | Aeq = []; 213 | beq = []; 214 | lb = ones(1, N) * system.ul; 215 | ub = ones(1, N) * system.uu; 216 | x0 = []; 217 | options = optimset('Display', 'off'); 218 | [du, ~] = quadprog(Hdb,ft,A,b,Aeq,beq,lb,ub,x0,options); 219 | uopt = du(1); 220 | end 221 | 222 | %% trajectory generator 223 | function [x_ref, y_ref, psi_ref] = trajectory_generator(t, Vx, trajectory_type) 224 | if strcmp(trajectory_type, 'Lane change') 225 | x = linspace(0, Vx * t(end), length(t)); 226 | y = 3 * tanh((t - t(end) / 2)); 227 | elseif strcmp(trajectory_type, 'Circle') 228 | radius = 30; 229 | period = 1000; 230 | for i = 1:length(t) 231 | x(1, i) = radius * sin(2 * pi * i / period); 232 | y(1, i) = -radius * cos(2 * pi * i / period); 233 | end 234 | end 235 | 236 | dx = x(2:end) - x(1:end-1); 237 | dy = y(2:end) - y(1:end-1); 238 | 239 | psi = zeros(1,length(x)); 240 | psiInt = psi; 241 | 242 | psi(1) = atan2(dy(1),dx(1)); 243 | psi(2:end) = atan2(dy(:),dx(:)); 244 | dpsi = psi(2:end) - psi(1:end-1); 245 | 246 | psiInt(1) = psi(1); 247 | for i = 2:length(psiInt) 248 | if dpsi(i - 1) < -pi 249 | psiInt(i) = psiInt(i - 1) + (dpsi(i - 1) + 2 * pi); 250 | elseif dpsi(i - 1) > pi 251 | psiInt(i) = psiInt(i - 1) + (dpsi(i - 1) - 2 * pi); 252 | else 253 | psiInt(i) = psiInt(i - 1) + dpsi(i - 1); 254 | end 255 | end 256 | 257 | x_ref = x'; 258 | y_ref = y'; 259 | psi_ref = psiInt'; 260 | end 261 | 262 | function dx = nonlinear_lateral_car_model(t, xTrue, u, system) 263 | % In this simulation, the body frame and its transformation is used 264 | % instead of a hybrid frame. That is because for the solver ode45, it 265 | % is important to have the nonlinear system of equations in the first 266 | % order form. 267 | 268 | % Get the constants from the general pool of constants 269 | Vx = system.Vx; 270 | M = system.M; 271 | I = system.I; 272 | Kf = system.Kf; 273 | Kr = system.Kr; 274 | lf = system.lf; 275 | lr = system.lr; 276 | 277 | % % Assign the states 278 | y_dot = xTrue(1); 279 | psi = xTrue(2); 280 | psi_dot = xTrue(3); 281 | 282 | % The nonlinear equation describing the dynamics of the car 283 | dx(1,1) = -(2*Kf+2*Kr)/(M * Vx)*y_dot+(-Vx-(2*Kf*lf-2*Kr*lr)/(M * Vx))*psi_dot+2*Kf/M*u; 284 | dx(2,1) = psi_dot; 285 | dx(3,1) = -(2*lf*Kf-2*lr*Kr)/(I * Vx)*y_dot-(2*lf^2*Kf+2*lr^2*Kr)/(I * Vx)*psi_dot+2*lf*Kf/I*u; 286 | dx(4,1) = sin(psi) * Vx + cos(psi) * y_dot; 287 | end 288 | 289 | function drow_figure(xTrue, uk, du, x_ref, y_ref, current_step, trajectory_type) 290 | % plot input 291 | figure(1) 292 | subplot(2, 1, 1) 293 | plot(0:current_step - 1, du(1,:), 'ko-',... 294 | 'LineWidth', 1.0, 'MarkerSize', 4); 295 | xlabel('Time Step','interpreter','latex','FontSize',10); 296 | ylabel('$\Delta \delta$ [rad]','interpreter','latex','FontSize',10); 297 | yline(pi / 60, '--b', 'LineWidth', 2.0); 298 | yline(-pi / 60, '--b', 'LineWidth', 2.0); 299 | 300 | subplot(2, 1, 2) 301 | plot(0:current_step - 1, uk(1,:), 'ko-',... 302 | 'LineWidth', 1.0, 'MarkerSize', 4); 303 | xlabel('Time Step','interpreter','latex','FontSize',10); 304 | ylabel('$\delta$ [rad]','interpreter','latex','FontSize',10); 305 | 306 | figure(2) 307 | if strcmp(trajectory_type, 'Lane change') 308 | % plot trajectory 309 | plot(x_ref(1: current_step, 1),y_ref(1: current_step, 1),'b','LineWidth',2) 310 | hold on 311 | plot(x_ref(1: current_step, 1),xTrue(4, 1:current_step),'r','LineWidth',2) 312 | hold on; 313 | grid on; 314 | % plot initial position 315 | plot(x_ref(1, 1), y_ref(1, 1), 'dg', 'LineWidth', 2); 316 | xlabel('X[m]','interpreter','latex','FontSize',10); 317 | ylabel('Y[m]','interpreter','latex','FontSize',10); 318 | yline(0.0, '--k', 'LineWidth', 2.0); 319 | yline(10.0, 'k', 'LineWidth', 4.0); 320 | yline(-10.0, 'k', 'LineWidth', 4.0); 321 | ylim([-12 12]) 322 | 323 | legend('Refernce trajectory','Motion trajectory','Initial position', 'Location','southeast',... 324 | 'interpreter','latex','FontSize',10.0); 325 | elseif strcmp(trajectory_type, 'Circle') 326 | % plot trajectory 327 | plot(x_ref(1: current_step, 1),y_ref(1: current_step, 1),'--b','LineWidth',2) 328 | hold on 329 | plot(x_ref(1: current_step, 1),xTrue(4, 1:current_step),'r','LineWidth',1) 330 | hold on; 331 | grid on; 332 | axis equal; 333 | % plot initial position 334 | plot(x_ref(1, 1), y_ref(1, 1), 'db', 'LineWidth', 2); 335 | xlabel('X[m]','interpreter','latex','FontSize',10); 336 | ylabel('Y[m]','interpreter','latex','FontSize',10); 337 | 338 | legend('Refernce trajectory','Motion trajectory','Initial position', 'Location','southeast',... 339 | 'interpreter','latex','FontSize',10.0); 340 | end 341 | 342 | % plot lateral error 343 | figure(3) 344 | Lateral_error = y_ref(1: current_step, 1) - xTrue(4, 1:current_step)'; 345 | plot(0:current_step - 1, Lateral_error, 'ko-',... 346 | 'LineWidth', 1.0, 'MarkerSize', 4); 347 | xlabel('Time Step','interpreter','latex','FontSize',10); 348 | ylabel('Lateral error [m]','interpreter','latex','FontSize',10); 349 | end 350 | -------------------------------------------------------------------------------- /ModelPredictiveControl/Trajectory_tracking_control(LMPC)/mobile_robot_by_YALMIP.m: -------------------------------------------------------------------------------- 1 | clear; 2 | close all; 3 | clc; 4 | 5 | %% Ipopt solver path 6 | dir = pwd; 7 | addpath(fullfile(dir,'Ipopt')) 8 | 9 | %% Setup and Parameters 10 | dt = 0.1; % sample time 11 | nx = 3; % Number of states 12 | ny = 3; % Number of ovservation 13 | nu = 2; % Number of input 14 | 15 | S = diag([1.0, 1.0, 10.0]); % Termination cost weight matrix 16 | Q = diag([1.0, 1.0, 10.0]); % Weight matrix of state quantities 17 | R = diag([0.1, 0.001]); % Weight matrix of input quantities 18 | N = 5; % Predictive Horizon 19 | 20 | % Range of inputs 21 | umin = [-5; -4]; 22 | umax = [5; 4]; 23 | 24 | %% Mobile robot 25 | Vx = 0.0; % [m/s] 26 | thetak = 0.0; % [rad] 27 | 28 | system.dt = dt; 29 | system.nx = nx; 30 | system.ny = ny; 31 | system.nu = nu; 32 | 33 | % input 34 | system.ul = umin; 35 | system.uu = umax; 36 | 37 | %% Load the trajectory 38 | t = 0:dt:10.0; 39 | [x_ref, y_ref, psi_ref] = trajectory_generator(t); 40 | 41 | sim_length = length(t); % Number of control loop iterations 42 | 43 | refSignals = zeros(length(x_ref(:, 1)) * ny, 1); 44 | 45 | ref_index = 1; 46 | for i = 1:ny:length(refSignals) 47 | refSignals(i) = x_ref(ref_index, 1); 48 | refSignals(i + 1) = y_ref(ref_index, 1); 49 | refSignals(i + 2) = psi_ref(ref_index, 1); 50 | ref_index = ref_index + 1; 51 | end 52 | 53 | % initial state 54 | x0 = [x_ref(1, 1); y_ref(1, 1); psi_ref(1, 1)]; % Initial state of mobile robot 55 | 56 | %% Create A, B, C matrix 57 | % states = [x, y, psi]; 58 | [A, B, C] = model_system(Vx, thetak, dt); 59 | 60 | system.A = A; 61 | system.B = B; 62 | system.C = C; 63 | 64 | system.A_aug = [system.A, system.B; 65 | zeros(length(system.B(1,:)),length(system.A(1,:))),eye(length(system.B(1,:)))]; 66 | system.B_aug = [system.B; 67 | eye(length(system.B(1,:)))]; 68 | system.C_aug = [system.C,zeros(length(system.C(:,1)),length(system.B(1,:)))]; 69 | 70 | %% MPC parameters 71 | params_mpc.Q = Q; 72 | params_mpc.R = R; 73 | params_mpc.S = S; 74 | params_mpc.N = N; 75 | 76 | %% main loop 77 | xTrue(:, 1) = x0; 78 | uk(:, 1) = [0.0; 0.0]; 79 | du(:, 1) = [0.0; 0.0]; 80 | current_step = 1; 81 | solvetime = zeros(1, sim_length); 82 | 83 | ref_sig_num = 1; % for reading reference signals 84 | for i = 1:sim_length-15 85 | % update time 86 | current_step = current_step + 1; 87 | 88 | % Generating the current state and the reference vector 89 | [A, B, C] = model_system(Vx, thetak, dt); 90 | 91 | system.A = A; 92 | system.B = B; 93 | system.C = C; 94 | system.A_aug = [system.A, system.B; 95 | zeros(length(system.B(1,:)),length(system.A(1,:))),eye(length(system.B(1,:)))]; 96 | system.B_aug = [system.B; 97 | eye(length(system.B(1,:)))]; 98 | system.C_aug = [system.C,zeros(length(system.C(:,1)),length(system.B(1,:)))]; 99 | 100 | xTrue_aug = [xTrue(:, current_step - 1); uk(:, current_step - 1)]; 101 | 102 | ref_sig_num = ref_sig_num + ny; 103 | if ref_sig_num + ny * N - 1 <= length(refSignals) 104 | ref = refSignals(ref_sig_num:ref_sig_num+ny*N-1); 105 | else 106 | ref = refSignals(ref_sig_num:length(refSignals)); 107 | N = N - 1; 108 | end 109 | 110 | % solve mac 111 | tic; 112 | du(:, current_step) = mpc(xTrue_aug, system, params_mpc, ref); 113 | solvetime(1, current_step - 1) = toc; 114 | 115 | % add du input 116 | uk(:, current_step) = uk(:, current_step - 1) + du(:, current_step); 117 | 118 | % update state 119 | T = dt*i:dt:dt*i+dt; 120 | [T, x] = ode45(@(t,x) nonlinear_lateral_car_model(t, x, uk(:, current_step)), T, xTrue(:, current_step - 1)); 121 | xTrue(:, current_step) = x(end,:); 122 | X = xTrue(:, current_step); 123 | thetak = X(3); 124 | end 125 | 126 | %% solve average time 127 | avg_time = sum(solvetime) / current_step; 128 | disp(avg_time); 129 | 130 | drow_figure(xTrue, uk, du, x_ref, y_ref, current_step); 131 | 132 | %% model predictive control 133 | function uopt = mpc(xTrue_aug, system, params, ref) 134 | % Solve MPC 135 | [feas, ~, u, ~] = solve_mpc(xTrue_aug, system, params, ref); 136 | if ~feas 137 | uopt = []; 138 | return 139 | else 140 | uopt = u(:, 1); 141 | end 142 | end 143 | 144 | function [feas, xopt, uopt, Jopt] = solve_mpc(xTrue_aug, system, params, ref) 145 | % extract variables 146 | N = params.N; 147 | % define variables and cost 148 | x = sdpvar(system.nx + system.nu, N+1); 149 | u = sdpvar(system.nu, N); 150 | constraints = []; 151 | cost = 0; 152 | ref_index = 1; 153 | 154 | % initial constraint 155 | constraints = [constraints; x(:,1) == xTrue_aug]; 156 | % add constraints and costs 157 | for i = 1:N-1 158 | constraints = [constraints; 159 | system.ul <= u(:,i) <= system.uu 160 | x(:,i+1) == system.A_aug * x(:,i) + system.B_aug * u(:,i)]; 161 | cost = cost + (ref(ref_index:ref_index + 2,1) - system.C_aug * x(:,i+1))' * params.Q * (ref(ref_index:ref_index + 2,1) - system.C_aug * x(:,i+1)) + u(:,i)' * params.R * u(:,i); 162 | ref_index = ref_index + system.ny; 163 | end 164 | % add terminal cost 165 | cost = cost + (ref(ref_index:ref_index + 2,1) - system.C_aug * x(:,N+1))' * params.S * (ref(ref_index:ref_index + 2,1) - system.C_aug * x(:,N+1)); 166 | ops = sdpsettings('solver','ipopt','verbose',0); 167 | % solve optimization 168 | diagnostics = optimize(constraints, cost, ops); 169 | if diagnostics.problem == 0 170 | feas = true; 171 | xopt = value(x); 172 | uopt = value(u); 173 | Jopt = value(cost); 174 | else 175 | feas = false; 176 | xopt = []; 177 | uopt = []; 178 | Jopt = []; 179 | end 180 | end 181 | 182 | %% trajectory generator 183 | function [x_ref, y_ref, psi_ref] = trajectory_generator(t) 184 | % Circle 185 | radius = 30; 186 | period = 100; 187 | for i = 1:length(t) 188 | x(1, i) = radius * sin(2 * pi * i / period); 189 | y(1, i) = -radius * cos(2 * pi * i / period); 190 | end 191 | 192 | dx = x(2:end) - x(1:end-1); 193 | dy = y(2:end) - y(1:end-1); 194 | 195 | psi = zeros(1,length(x)); 196 | psiInt = psi; 197 | 198 | psi(1) = atan2(dy(1),dx(1)); 199 | psi(2:end) = atan2(dy(:),dx(:)); 200 | dpsi = psi(2:end) - psi(1:end-1); 201 | 202 | psiInt(1) = psi(1); 203 | for i = 2:length(psiInt) 204 | if dpsi(i - 1) < -pi 205 | psiInt(i) = psiInt(i - 1) + (dpsi(i - 1) + 2 * pi); 206 | elseif dpsi(i - 1) > pi 207 | psiInt(i) = psiInt(i - 1) + (dpsi(i - 1) - 2 * pi); 208 | else 209 | psiInt(i) = psiInt(i - 1) + dpsi(i - 1); 210 | end 211 | end 212 | 213 | x_ref = x'; 214 | y_ref = y'; 215 | psi_ref = psiInt'; 216 | end 217 | 218 | function [A, B, C] = model_system(Vk,thetak,dt) 219 | A = [1.0, 0.0, -Vk*dt*sin(thetak); 220 | 0.0 1.0 Vk*dt*cos(thetak); 221 | 0.0, 0.0, 1.0]; 222 | B = [dt*cos(thetak), -0.5*dt*dt*Vk*sin(thetak); 223 | dt*sin(thetak), 0.5*dt*dt*Vk*cos(thetak); 224 | 0.0 dt]; 225 | C = [1.0, 0.0, 0.0; 226 | 0.0, 1.0, 0.0; 227 | 0.0, 0.0, 1.0]; 228 | end 229 | 230 | function dx = nonlinear_lateral_car_model(t, xTrue, u) 231 | % In this simulation, the body frame and its transformation is used 232 | % instead of a hybrid frame. That is because for the solver ode45, it 233 | % is important to have the nonlinear system of equations in the first 234 | % order form. 235 | 236 | dx = [u(1)*cos(xTrue(3)); 237 | u(1)*sin(xTrue(3)); 238 | u(2)]; 239 | end 240 | 241 | function drow_figure(xTrue, uk, du, x_ref, y_ref, current_step) 242 | % Plot simulation 243 | figure(1) 244 | subplot(2,2,1) 245 | plot(0:current_step - 1, du(1,:), 'ko-',... 246 | 'LineWidth', 1.0, 'MarkerSize', 4); 247 | xlabel('Time Step','interpreter','latex','FontSize',10); 248 | ylabel('$\Delta v$ [m/s]','interpreter','latex','FontSize',10); 249 | yline(-5, '--b', 'LineWidth', 2.0); 250 | yline(5, '--b', 'LineWidth', 2.0); 251 | 252 | subplot(2,2,2) 253 | plot(0:current_step - 1, du(2,:), 'ko-',... 254 | 'LineWidth', 1.0, 'MarkerSize', 4); 255 | xlabel('Time Step','interpreter','latex','FontSize',10); 256 | ylabel('$\Delta w$ [rad/s]','interpreter','latex','FontSize',10); 257 | yline(-4, '--b', 'LineWidth', 2.0); 258 | yline(4, '--b', 'LineWidth', 2.0); 259 | 260 | subplot(2,2,3) 261 | plot(0:current_step - 1, uk(1,:), 'ko-',... 262 | 'LineWidth', 1.0, 'MarkerSize', 4); 263 | xlabel('Time Step','interpreter','latex','FontSize',10); 264 | ylabel('$v$ [m/s]','interpreter','latex','FontSize',10); 265 | 266 | subplot(2,2,4) 267 | plot(0:current_step - 1, uk(2,:), 'ko-',... 268 | 'LineWidth', 1.0, 'MarkerSize', 4); 269 | xlabel('Time Step','interpreter','latex','FontSize',10); 270 | ylabel('$w$ [rad/s]','interpreter','latex','FontSize',10); 271 | 272 | figure(2) 273 | % plot trajectory 274 | plot(x_ref(1: current_step, 1),y_ref(1: current_step, 1),'--b','LineWidth',2) 275 | hold on 276 | plot(xTrue(1, 1: current_step), xTrue(2, 1:current_step),'r','LineWidth',2) 277 | hold on; 278 | grid on; 279 | axis equal; 280 | % plot initial position 281 | plot(x_ref(1, 1), y_ref(1, 1), 'dg', 'LineWidth', 2); 282 | xlabel('X[m]','interpreter','latex','FontSize',10); 283 | ylabel('Y[m]','interpreter','latex','FontSize',10); 284 | 285 | legend('Refernce trajectory','Motion trajectory','Initial position', 'Location','southeast',... 286 | 'interpreter','latex','FontSize',10.0); 287 | 288 | % plot position error 289 | figure(3) 290 | Vertical_error = x_ref(1: current_step, 1) - xTrue(1, 1: current_step)'; 291 | Lateral_error = y_ref(1: current_step, 1) - xTrue(2, 1:current_step)'; 292 | 293 | subplot(2, 1, 1) 294 | plot(0:current_step - 1, Vertical_error, 'ko-',... 295 | 'LineWidth', 1.0, 'MarkerSize', 4); 296 | xlabel('Time Step','interpreter','latex','FontSize',10); 297 | ylabel('Vertical error [m]','interpreter','latex','FontSize',10); 298 | 299 | subplot(2, 1, 2) 300 | plot(0:current_step - 1, Lateral_error, 'ko-',... 301 | 'LineWidth', 1.0, 'MarkerSize', 4); 302 | xlabel('Time Step','interpreter','latex','FontSize',10); 303 | ylabel('Lateral error [m]','interpreter','latex','FontSize',10); 304 | end 305 | -------------------------------------------------------------------------------- /ModelPredictiveControl/Trajectory_tracking_control(LMPC)/mobile_robot_by_quadprog.m: -------------------------------------------------------------------------------- 1 | clear; 2 | close all; 3 | clc; 4 | 5 | %% Setup and Parameters 6 | dt = 0.1; % sample time 7 | nx = 3; % Number of states 8 | ny = 3; % Number of ovservation 9 | nu = 2; % Number of input 10 | 11 | S = diag([1.0, 1.0, 10.0]); % Termination cost weight matrix 12 | Q = diag([1.0, 1.0, 10.0]); % Weight matrix of state quantities 13 | R = diag([0.1, 0.001]); % Weight matrix of input quantities 14 | N = 5; % Predictive Horizon 15 | 16 | % Range of inputs 17 | umin = [-5; -4]; 18 | umax = [5; 4]; 19 | 20 | %% Mobile robot 21 | Vx = 0.0; % [m/s] 22 | thetak = 0.0; % [rad] 23 | 24 | system.dt = dt; 25 | system.nx = nx; 26 | system.ny = ny; 27 | system.nu = nu; 28 | 29 | % input 30 | system.ul = umin; 31 | system.uu = umax; 32 | 33 | %% Load the trajectory 34 | t = 0:dt:10.0; 35 | [x_ref, y_ref, psi_ref] = trajectory_generator(t); 36 | 37 | sim_length = length(t); % Number of control loop iterations 38 | 39 | refSignals = zeros(length(x_ref(:, 1)) * ny, 1); 40 | 41 | ref_index = 1; 42 | for i = 1:ny:length(refSignals) 43 | refSignals(i) = x_ref(ref_index, 1); 44 | refSignals(i + 1) = y_ref(ref_index, 1); 45 | refSignals(i + 2) = psi_ref(ref_index, 1); 46 | ref_index = ref_index + 1; 47 | end 48 | 49 | % initial state 50 | x0 = [x_ref(1, 1); y_ref(1, 1); psi_ref(1, 1)]; % Initial state of mobile robot 51 | 52 | %% Create A, B, C matrix 53 | % states = [x, y, psi]; 54 | [A, B, C] = model_system(Vx, thetak, dt); 55 | 56 | system.A = A; 57 | system.B = B; 58 | system.C = C; 59 | 60 | system.A_aug = [system.A, system.B; 61 | zeros(length(system.B(1,:)),length(system.A(1,:))),eye(length(system.B(1,:)))]; 62 | system.B_aug = [system.B; 63 | eye(length(system.B(1,:)))]; 64 | system.C_aug = [system.C,zeros(length(system.C(:,1)),length(system.B(1,:)))]; 65 | 66 | %% MPC parameters 67 | params_mpc.Q = Q; 68 | params_mpc.R = R; 69 | params_mpc.S = S; 70 | params_mpc.N = N; 71 | 72 | %% main loop 73 | xTrue(:, 1) = x0; 74 | uk(:, 1) = [0.0; 0.0]; 75 | du(:, 1) = [0.0; 0.0]; 76 | current_step = 1; 77 | solvetime = zeros(1, sim_length); 78 | 79 | ref_sig_num = 1; % for reading reference signals 80 | for i = 1:sim_length-15 81 | % update time 82 | current_step = current_step + 1; 83 | 84 | % Generating the current state and the reference vector 85 | [A, B, C] = model_system(Vx, thetak, dt); 86 | 87 | system.A = A; 88 | system.B = B; 89 | system.C = C; 90 | system.A_aug = [system.A, system.B; 91 | zeros(length(system.B(1,:)),length(system.A(1,:))),eye(length(system.B(1,:)))]; 92 | system.B_aug = [system.B; 93 | eye(length(system.B(1,:)))]; 94 | system.C_aug = [system.C,zeros(length(system.C(:,1)),length(system.B(1,:)))]; 95 | 96 | xTrue_aug = [xTrue(:, current_step - 1); uk(:, current_step - 1)]; 97 | 98 | ref_sig_num = ref_sig_num + ny; 99 | if ref_sig_num + ny * N - 1 <= length(refSignals) 100 | ref = refSignals(ref_sig_num:ref_sig_num+ny*N-1); 101 | else 102 | ref = refSignals(ref_sig_num:length(refSignals)); 103 | N = N - 1; 104 | end 105 | 106 | % solve mac 107 | tic; 108 | du(:, current_step) = mpc(xTrue_aug, system, params_mpc, ref); 109 | solvetime(1, current_step - 1) = toc; 110 | 111 | % add du input 112 | uk(:, current_step) = uk(:, current_step - 1) + du(:, current_step); 113 | 114 | % update state 115 | T = dt*i:dt:dt*i+dt; 116 | [T, x] = ode45(@(t,x) nonlinear_lateral_car_model(t, x, uk(:, current_step)), T, xTrue(:, current_step - 1)); 117 | xTrue(:, current_step) = x(end,:); 118 | X = xTrue(:, current_step); 119 | thetak = X(3); 120 | end 121 | 122 | %% solve average time 123 | avg_time = sum(solvetime) / current_step; 124 | disp(avg_time); 125 | 126 | drow_figure(xTrue, uk, du, x_ref, y_ref, current_step); 127 | 128 | %% model predictive control 129 | %% model predictive control 130 | function uopt = mpc(xTrue_aug, system, params_mpc, ref) 131 | A_aug = system.A_aug; 132 | B_aug = system.B_aug; 133 | C_aug = system.C_aug; 134 | 135 | Q = params_mpc.Q; 136 | S = params_mpc.S; 137 | R = params_mpc.R; 138 | N = params_mpc.N; 139 | 140 | CQC = C_aug' * Q * C_aug; 141 | CSC = C_aug' * S * C_aug; 142 | QC = Q * C_aug; 143 | SC = S * C_aug; 144 | 145 | Qdb = zeros(length(CQC(:,1))*N,length(CQC(1,:))*N); 146 | Tdb = zeros(length(QC(:,1))*N,length(QC(1,:))*N); 147 | Rdb = zeros(length(R(:,1))*N,length(R(1,:))*N); 148 | Cdb = zeros(length(B_aug(:,1))*N,length(B_aug(1,:))*N); 149 | Adc = zeros(length(A_aug(:,1))*N,length(A_aug(1,:))); 150 | 151 | % Filling in the matrices 152 | for i = 1:N 153 | if i == N 154 | Qdb(1+length(CSC(:,1))*(i-1):length(CSC(:,1))*i,1+length(CSC(1,:))*(i-1):length(CSC(1,:))*i) = CSC; 155 | Tdb(1+length(SC(:,1))*(i-1):length(SC(:,1))*i,1+length(SC(1,:))*(i-1):length(SC(1,:))*i) = SC; 156 | else 157 | Qdb(1+length(CQC(:,1))*(i-1):length(CQC(:,1))*i,1+length(CQC(1,:))*(i-1):length(CQC(1,:))*i) = CQC; 158 | Tdb(1+length(QC(:,1))*(i-1):length(QC(:,1))*i,1+length(QC(1,:))*(i-1):length(QC(1,:))*i) = QC; 159 | end 160 | 161 | Rdb(1+length(R(:,1))*(i-1):length(R(:,1))*i,1+length(R(1,:))*(i-1):length(R(1,:))*i) = R; 162 | 163 | for j = 1:N 164 | if j<=i 165 | Cdb(1+length(B_aug(:,1))*(i-1):length(B_aug(:,1))*i,1+length(B_aug(1,:))*(j-1):length(B_aug(1,:))*j) = A_aug^(i-j)*B_aug; 166 | end 167 | end 168 | Adc(1+length(A_aug(:,1))*(i-1):length(A_aug(:,1))*i,1:length(A_aug(1,:))) = A_aug^(i); 169 | end 170 | Hdb = Cdb' * Qdb * Cdb + Rdb; 171 | Fdbt = [Adc' * Qdb * Cdb; -Tdb * Cdb]; 172 | 173 | % Calling the optimizer (quadprog) 174 | % Cost function in quadprog: min(du)*1/2*du'Hdb*du+f'du 175 | ft = [xTrue_aug', ref'] * Fdbt; 176 | 177 | umin = ones(system.nu, N); 178 | umax = ones(system.nu, N); 179 | for i = 1:N 180 | umin(:, i) = system.ul; 181 | umax(:, i) = system.uu; 182 | end 183 | 184 | % Call the solver (quadprog) 185 | A = []; 186 | b = []; 187 | Aeq = []; 188 | beq = []; 189 | lb = umin; 190 | ub = umax; 191 | x0 = []; 192 | options = optimset('Display', 'off'); 193 | [du, ~] = quadprog(Hdb,ft,A,b,Aeq,beq,lb,ub,x0,options); 194 | 195 | uopt = du(1:system.nu, 1); 196 | end 197 | 198 | %% trajectory generator 199 | function [x_ref, y_ref, psi_ref] = trajectory_generator(t) 200 | % Circle 201 | radius = 30; 202 | period = 100; 203 | for i = 1:length(t) 204 | x(1, i) = radius * sin(2 * pi * i / period); 205 | y(1, i) = -radius * cos(2 * pi * i / period); 206 | end 207 | 208 | dx = x(2:end) - x(1:end-1); 209 | dy = y(2:end) - y(1:end-1); 210 | 211 | psi = zeros(1,length(x)); 212 | psiInt = psi; 213 | 214 | psi(1) = atan2(dy(1),dx(1)); 215 | psi(2:end) = atan2(dy(:),dx(:)); 216 | dpsi = psi(2:end) - psi(1:end-1); 217 | 218 | psiInt(1) = psi(1); 219 | for i = 2:length(psiInt) 220 | if dpsi(i - 1) < -pi 221 | psiInt(i) = psiInt(i - 1) + (dpsi(i - 1) + 2 * pi); 222 | elseif dpsi(i - 1) > pi 223 | psiInt(i) = psiInt(i - 1) + (dpsi(i - 1) - 2 * pi); 224 | else 225 | psiInt(i) = psiInt(i - 1) + dpsi(i - 1); 226 | end 227 | end 228 | 229 | x_ref = x'; 230 | y_ref = y'; 231 | psi_ref = psiInt'; 232 | end 233 | 234 | function [A, B, C] = model_system(Vk,thetak,dt) 235 | A = [1.0, 0.0, -Vk*dt*sin(thetak); 236 | 0.0 1.0 Vk*dt*cos(thetak); 237 | 0.0, 0.0, 1.0]; 238 | B = [dt*cos(thetak), -0.5*dt*dt*Vk*sin(thetak); 239 | dt*sin(thetak), 0.5*dt*dt*Vk*cos(thetak); 240 | 0.0 dt]; 241 | C = [1.0, 0.0, 0.0; 242 | 0.0, 1.0, 0.0; 243 | 0.0, 0.0, 1.0]; 244 | end 245 | 246 | function dx = nonlinear_lateral_car_model(t, xTrue, u) 247 | % In this simulation, the body frame and its transformation is used 248 | % instead of a hybrid frame. That is because for the solver ode45, it 249 | % is important to have the nonlinear system of equations in the first 250 | % order form. 251 | 252 | dx = [u(1)*cos(xTrue(3)); 253 | u(1)*sin(xTrue(3)); 254 | u(2)]; 255 | end 256 | 257 | function drow_figure(xTrue, uk, du, x_ref, y_ref, current_step) 258 | % Plot simulation 259 | figure(1) 260 | subplot(2,2,1) 261 | plot(0:current_step - 1, du(1,:), 'ko-',... 262 | 'LineWidth', 1.0, 'MarkerSize', 4); 263 | xlabel('Time Step','interpreter','latex','FontSize',10); 264 | ylabel('$\Delta v$ [m/s]','interpreter','latex','FontSize',10); 265 | yline(-5, '--b', 'LineWidth', 2.0); 266 | yline(5, '--b', 'LineWidth', 2.0); 267 | 268 | subplot(2,2,2) 269 | plot(0:current_step - 1, du(2,:), 'ko-',... 270 | 'LineWidth', 1.0, 'MarkerSize', 4); 271 | xlabel('Time Step','interpreter','latex','FontSize',10); 272 | ylabel('$\Delta w$ [rad/s]','interpreter','latex','FontSize',10); 273 | yline(-4, '--b', 'LineWidth', 2.0); 274 | yline(4, '--b', 'LineWidth', 2.0); 275 | 276 | subplot(2,2,3) 277 | plot(0:current_step - 1, uk(1,:), 'ko-',... 278 | 'LineWidth', 1.0, 'MarkerSize', 4); 279 | xlabel('Time Step','interpreter','latex','FontSize',10); 280 | ylabel('$v$ [m/s]','interpreter','latex','FontSize',10); 281 | 282 | subplot(2,2,4) 283 | plot(0:current_step - 1, uk(2,:), 'ko-',... 284 | 'LineWidth', 1.0, 'MarkerSize', 4); 285 | xlabel('Time Step','interpreter','latex','FontSize',10); 286 | ylabel('$w$ [rad/s]','interpreter','latex','FontSize',10); 287 | 288 | figure(2) 289 | % plot trajectory 290 | plot(x_ref(1: current_step, 1),y_ref(1: current_step, 1),'--b','LineWidth',2) 291 | hold on 292 | plot(xTrue(1, 1: current_step), xTrue(2, 1:current_step),'r','LineWidth',2) 293 | hold on; 294 | grid on; 295 | axis equal; 296 | % plot initial position 297 | plot(x_ref(1, 1), y_ref(1, 1), 'dg', 'LineWidth', 2); 298 | xlabel('X[m]','interpreter','latex','FontSize',10); 299 | ylabel('Y[m]','interpreter','latex','FontSize',10); 300 | 301 | legend('Refernce trajectory','Motion trajectory','Initial position', 'Location','southeast',... 302 | 'interpreter','latex','FontSize',10.0); 303 | 304 | % plot position error 305 | figure(3) 306 | Vertical_error = x_ref(1: current_step, 1) - xTrue(1, 1: current_step)'; 307 | Lateral_error = y_ref(1: current_step, 1) - xTrue(2, 1:current_step)'; 308 | 309 | subplot(2, 1, 1) 310 | plot(0:current_step - 1, Vertical_error, 'ko-',... 311 | 'LineWidth', 1.0, 'MarkerSize', 4); 312 | xlabel('Time Step','interpreter','latex','FontSize',10); 313 | ylabel('Vertical error [m]','interpreter','latex','FontSize',10); 314 | 315 | subplot(2, 1, 2) 316 | plot(0:current_step - 1, Lateral_error, 'ko-',... 317 | 'LineWidth', 1.0, 'MarkerSize', 4); 318 | xlabel('Time Step','interpreter','latex','FontSize',10); 319 | ylabel('Lateral error [m]','interpreter','latex','FontSize',10); 320 | end 321 | -------------------------------------------------------------------------------- /PathPlanning/BSplinePath/bspline_path.m: -------------------------------------------------------------------------------- 1 | % https://tajimarobotics.com/basis-spline-interpolation-program/ 2 | % https://1st-archiengineer.com/programing-bspline2/ 3 | function [] = bspline_path() 4 | clear; 5 | close all; 6 | clc; 7 | 8 | % Control Points (X-Y) 9 | % P = [ 10 | % 0.0, 0.0; 11 | % 1.0, 2.0; 12 | % 3.0, 2.0; 13 | % 3.0, 0.0; 14 | % 5.0, 2.0; 15 | % 6.0, 0.0; 16 | % ]; 17 | 18 | P = [ 3, 3; 19 | 6, 1; 20 | 9, 1; 21 | 9, 5; 22 | 12, 5; 23 | 16.5, 9.5; 24 | 9, 13; 25 | 6, 8]; 26 | 27 | p = length(P); % Number of Control Ponits 28 | n = 2; % Degree of B-Spline 29 | m = p + n + 1; % Number of Knots in Knot Vector 30 | 31 | % Define Knot Vector 32 | u = OpenUniformKnotVector(m,n); 33 | 34 | t = 0:0.01:u(end); 35 | 36 | % Calculate B-spline Curve 37 | S = zeros(length(t),2); 38 | S(1,:) = P(1,:); 39 | for i = 2:length(t) 40 | for j =1:p 41 | b = BasisFunction(u,j,n,t(i)); 42 | S(i,:) = S(i,:) + P(j,:)*b; 43 | end 44 | end 45 | 46 | figure; 47 | plot(P(:, 1), P(:, 2), '-o'); hold on; 48 | plot(S(:, 1), S(:, 2), 'LineWidth', 2); hold on; 49 | end 50 | 51 | % u : Knot Vector 52 | % m : Number of Knots in Knot Vector 53 | % n : Degree of B-Spline 54 | function u = OpenUniformKnotVector(m,n) 55 | u = zeros(1,m); 56 | for i = 1:1:m 57 | if i < n+1 58 | u(i) = 0; 59 | elseif i > m - (n + 1) 60 | u(i) = m - 1 - 2 * n; 61 | else 62 | u(i) = i - n - 1; 63 | end 64 | end 65 | u = u/u(end); 66 | end 67 | 68 | % u : Knot Vector 69 | % j : j-th Control Ponit 70 | % k : k-th Basis Function <-- n-th B-Spline 71 | % t : Time 72 | function var = BasisFunction(u,j,k,t) 73 | w1 = 0.0; 74 | w2 = 0.0; 75 | 76 | if k == 0 77 | if u(j) < t && t <= u(j+1) 78 | var = 1.0; 79 | else 80 | var = 0.0; 81 | end 82 | else 83 | if (u(j+k+1)-u(j+1)) ~= 0 84 | w1 = BasisFunction(u,j+1,k-1,t) * (u(j+k+1) - t)/(u(j+k+1)-u(j+1)); 85 | end 86 | if (u(j+k)-u(j)) ~= 0 87 | w2 = BasisFunction(u,j,k-1,t) * (t - u(j))/(u(j+k) - u(j)); 88 | end 89 | var = w1 + w2; 90 | end 91 | end 92 | -------------------------------------------------------------------------------- /PathPlanning/BezierPath/bezier_path.m: -------------------------------------------------------------------------------- 1 | % https://tajimarobotics.com/bezier-curve-interpolation/ 2 | % https://1st-archiengineer.com/programing-bspline2/ 3 | function [] = bezier_path() 4 | clear; 5 | close all; 6 | clc; 7 | 8 | % Control Points (X-Y) 9 | % P = [ 10 | % 0.0, 0.0; 11 | % 1.0, 2.0; 12 | % 3.0, 2.0; 13 | % 3.0, 0.0; 14 | % 5.0, 2.0; 15 | % 6.0, 0.0; 16 | % ]; 17 | 18 | P = [ 3, 3; 19 | 6, 1; 20 | 9, 1; 21 | 9, 5; 22 | 12, 5; 23 | 16.5, 9.5; 24 | 9, 13; 25 | 6, 8]; 26 | 27 | p = length(P); % Number of Control Ponits 28 | n = p - 1; % Degree of B-Spline 29 | m = p + n + 1; % Number of Knots in Knot Vector 30 | 31 | % Define Knot Vector 32 | u = OpenUniformKnotVector(m,n); 33 | 34 | t = 0:0.01:u(end); 35 | 36 | % Calculate B-spline Curve 37 | S = zeros(length(t),2); 38 | S(1,:) = P(1,:); 39 | for i = 2:length(t) 40 | for j =1:p 41 | b = BasisFunction(u,j,n,t(i)); 42 | S(i,:) = S(i,:) + P(j,:)*b; 43 | end 44 | end 45 | 46 | figure; 47 | plot(P(:, 1), P(:, 2), '-o'); hold on; 48 | plot(S(:, 1), S(:, 2), 'LineWidth', 2); hold on; 49 | end 50 | 51 | % u : Knot Vector 52 | % m : Number of Knots in Knot Vector 53 | % n : Degree of B-Spline 54 | function u = OpenUniformKnotVector(m,n) 55 | u = zeros(1,m); 56 | for i = 1:1:m 57 | if i < n+1 58 | u(i) = 0; 59 | elseif i > m - (n + 1) 60 | u(i) = m - 1 - 2 * n; 61 | else 62 | u(i) = i - n - 1; 63 | end 64 | end 65 | u = u/u(end); 66 | end 67 | 68 | % u : Knot Vector 69 | % j : j-th Control Ponit 70 | % k : k-th Basis Function <-- n-th B-Spline 71 | % t : Time 72 | function var = BasisFunction(u,j,k,t) 73 | w1 = 0.0; 74 | w2 = 0.0; 75 | 76 | if k == 0 77 | if u(j) < t && t <= u(j+1) 78 | var = 1.0; 79 | else 80 | var = 0.0; 81 | end 82 | else 83 | if (u(j+k+1)-u(j+1)) ~= 0 84 | w1 = BasisFunction(u,j+1,k-1,t) * (u(j+k+1) - t)/(u(j+k+1)-u(j+1)); 85 | end 86 | if (u(j+k)-u(j)) ~= 0 87 | w2 = BasisFunction(u,j,k-1,t) * (t - u(j))/(u(j+k) - u(j)); 88 | end 89 | var = w1 + w2; 90 | end 91 | end 92 | -------------------------------------------------------------------------------- /PathPlanning/ClothoidPath/clothoid_path_generation.m: -------------------------------------------------------------------------------- 1 | clear; 2 | close all; 3 | clc; 4 | 5 | %% main 6 | start_point = [0, 0]; 7 | start_orientation_list = 0.0; 8 | goal_point = [10.0, 0]; 9 | goal_orientation_list = linspace(-pi, pi, 75); 10 | num_path_points = 100; 11 | 12 | clothoid_paths = generate_clothoid_paths(... 13 | start_point, start_orientation_list, ... 14 | goal_point, goal_orientation_list, ... 15 | num_path_points); 16 | 17 | show_animation = true; 18 | 19 | if show_animation 20 | draw_clothoids(start_point, goal_point, ... 21 | num_path_points, clothoid_paths, ... 22 | false); 23 | end 24 | 25 | %% function 26 | function clothoids = generate_clothoid_paths(start_point, start_yaw_list, goal_point, goal_yaw_list, n_path_points) 27 | clothoids = {}; 28 | for start_yaw = start_yaw_list 29 | for goal_yaw = goal_yaw_list 30 | clothoid = generate_clothoid_path(start_point, start_yaw, goal_point, goal_yaw, n_path_points); 31 | clothoids{end+1} = clothoid; 32 | end 33 | end 34 | end 35 | 36 | function clothoid_path = generate_clothoid_path(start_point, start_yaw, goal_point, goal_yaw, n_path_points) 37 | dx = goal_point(1) - start_point(1); 38 | dy = goal_point(2) - start_point(2); 39 | r = hypot(dx, dy); 40 | 41 | phi = atan2(dy, dx); 42 | phi1 = normalize_angle(start_yaw - phi); 43 | phi2 = normalize_angle(goal_yaw - phi); 44 | delta = phi2 - phi1; 45 | 46 | try 47 | A = solve_g_for_root(phi1, phi2, delta); 48 | 49 | L = compute_path_length(r, phi1, delta, A); 50 | curvature = compute_curvature(delta, A, L); 51 | curvature_rate = compute_curvature_rate(A, L); 52 | catch exception 53 | fprintf('Failed to generate clothoid points: %s\n', exception.message); 54 | clothoid_path = []; 55 | return; 56 | end 57 | 58 | points = zeros(n_path_points, 2); 59 | s_values = linspace(0, L, n_path_points); 60 | for i = 1:n_path_points 61 | s = s_values(i); 62 | try 63 | x = start_point(1) + s * X(curvature_rate * s^2, curvature * s, start_yaw); 64 | y = start_point(2) + s * Y(curvature_rate * s^2, curvature * s, start_yaw); 65 | points(i, :) = [x, y]; 66 | catch exception 67 | fprintf('Skipping failed clothoid point: %s\n', exception.message); 68 | end 69 | end 70 | clothoid_path = points; 71 | end 72 | 73 | function result = X(a, b, c) 74 | result = integral(@(t) cos((a/2) .* t.^2 + b .* t + c), 0, 1); 75 | end 76 | 77 | function result = Y(a, b, c) 78 | result = integral(@(t) sin((a/2) .* t.^2 + b .* t + c), 0, 1); 79 | end 80 | 81 | function result = solve_g_for_root(theta1, theta2, delta) 82 | initial_guess = 3 * (theta1 + theta2); 83 | result = fsolve(@(A) Y(2*A, delta - A, theta1), initial_guess); 84 | end 85 | 86 | function result = compute_path_length(r, theta1, delta, A) 87 | result = r / X(2*A, delta - A, theta1); 88 | end 89 | 90 | function result = compute_curvature(delta, A, L) 91 | result = (delta - A) / L; 92 | end 93 | 94 | function result = compute_curvature_rate(A, L) 95 | result = 2 * A / (L^2); 96 | end 97 | 98 | function result = normalize_angle(angle_rad) 99 | result = mod(angle_rad + pi, 2 * pi) - pi; 100 | end 101 | 102 | %% draw function 103 | function [x_min, x_max, y_min, y_max] = get_axes_limits(clothoids) 104 | x_vals = []; 105 | y_vals = []; 106 | 107 | for i = 1:numel(clothoids) 108 | clothoid = clothoids{i}; 109 | x_vals = [x_vals, clothoid(:, 1)']; 110 | y_vals = [y_vals, clothoid(:, 2)']; 111 | end 112 | 113 | x_min = min(x_vals); 114 | x_max = max(x_vals); 115 | y_min = min(y_vals); 116 | y_max = max(y_vals); 117 | 118 | x_offset = 0.1 * (x_max - x_min); 119 | y_offset = 0.1 * (y_max - y_min); 120 | 121 | x_min = x_min - x_offset; 122 | x_max = x_max + x_offset; 123 | y_min = y_min - y_offset; 124 | y_max = y_max + y_offset; 125 | end 126 | 127 | function draw_clothoids(start, goal, num_steps, clothoidal_paths, save_animation) 128 | figure('Position', [0, 0, 600, 600]); 129 | [x_min, x_max, y_min, y_max] = get_axes_limits(clothoidal_paths); 130 | axes = gca; 131 | axes.XLim = [x_min, x_max]; 132 | axes.YLim = [y_min, y_max]; 133 | 134 | plot(start(1), start(2), 'ro'); 135 | plot(goal(1), goal(2), 'ro'); 136 | 137 | lines = gobjects(1, numel(clothoidal_paths)); 138 | for i = 1:numel(clothoidal_paths) 139 | lines(i) = animatedline('Color', 'b', 'LineWidth', 1); 140 | end 141 | 142 | for i = 1:num_steps 143 | for j = 1:numel(clothoidal_paths) 144 | clothoid_path = clothoidal_paths{j}; 145 | x = clothoid_path(1:i, 1); 146 | y = clothoid_path(1:i, 2); 147 | clearpoints(lines(j)); 148 | addpoints(lines(j), x, y); 149 | end 150 | pause(0.025); 151 | end 152 | 153 | if save_animation 154 | % Save the animation as a GIF file 155 | filename = 'clothoid.gif'; 156 | delay_time = 0.025; 157 | save_animated_gif(filename, delay_time); 158 | end 159 | end 160 | -------------------------------------------------------------------------------- /PathPlanning/ClothoidPath/simple_clothoid_path_generation.m: -------------------------------------------------------------------------------- 1 | clear; 2 | close all; 3 | clc; 4 | 5 | %% main 6 | start_point = [0, 0]; 7 | start_orientation_list = 0.0; 8 | goal_point = [15, 15]; 9 | goal_orientation_list = 0.0; 10 | num_path_points = 100; 11 | 12 | clothoid_paths = generate_clothoid_paths(... 13 | start_point, start_orientation_list, ... 14 | goal_point, goal_orientation_list, ... 15 | num_path_points); 16 | 17 | show_animation = true; 18 | 19 | if show_animation 20 | draw_clothoids(start_point, goal_point, ... 21 | num_path_points, clothoid_paths, ... 22 | false); 23 | end 24 | 25 | %% function 26 | function clothoids = generate_clothoid_paths(start_point, start_yaw, goal_point, goal_yaw, n_path_points) 27 | clothoids = generate_clothoid_path(start_point, start_yaw, goal_point, goal_yaw, n_path_points); 28 | end 29 | 30 | function clothoid_path = generate_clothoid_path(start_point, start_yaw, goal_point, goal_yaw, n_path_points) 31 | dx = goal_point(1) - start_point(1); 32 | dy = goal_point(2) - start_point(2); 33 | r = hypot(dx, dy); 34 | 35 | phi = atan2(dy, dx); 36 | phi1 = normalize_angle(start_yaw - phi); 37 | phi2 = normalize_angle(goal_yaw - phi); 38 | delta = phi2 - phi1; 39 | 40 | try 41 | A = solve_g_for_root(phi1, phi2, delta); 42 | 43 | L = compute_path_length(r, phi1, delta, A); 44 | curvature = compute_curvature(delta, A, L); 45 | curvature_rate = compute_curvature_rate(A, L); 46 | catch exception 47 | fprintf('Failed to generate clothoid points: %s\n', exception.message); 48 | clothoid_path = []; 49 | return; 50 | end 51 | 52 | points = zeros(n_path_points, 2); 53 | s_values = linspace(0, L, n_path_points); 54 | for i = 1:n_path_points 55 | s = s_values(i); 56 | try 57 | x = start_point(1) + s * X(curvature_rate * s^2, curvature * s, start_yaw); 58 | y = start_point(2) + s * Y(curvature_rate * s^2, curvature * s, start_yaw); 59 | points(i, :) = [x, y]; 60 | catch exception 61 | fprintf('Skipping failed clothoid point: %s\n', exception.message); 62 | end 63 | end 64 | clothoid_path = points; 65 | end 66 | 67 | function result = X(a, b, c) 68 | result = integral(@(t) cos((a/2) .* t.^2 + b .* t + c), 0, 1); 69 | end 70 | 71 | function result = Y(a, b, c) 72 | result = integral(@(t) sin((a/2) .* t.^2 + b .* t + c), 0, 1); 73 | end 74 | 75 | function result = solve_g_for_root(theta1, theta2, delta) 76 | initial_guess = 3 * (theta1 + theta2); 77 | result = fsolve(@(A) Y(2*A, delta - A, theta1), initial_guess); 78 | end 79 | 80 | function result = compute_path_length(r, theta1, delta, A) 81 | result = r / X(2*A, delta - A, theta1); 82 | end 83 | 84 | function result = compute_curvature(delta, A, L) 85 | result = (delta - A) / L; 86 | end 87 | 88 | function result = compute_curvature_rate(A, L) 89 | result = 2 * A / (L^2); 90 | end 91 | 92 | function result = normalize_angle(angle_rad) 93 | result = mod(angle_rad + pi, 2 * pi) - pi; 94 | end 95 | 96 | %% draw function 97 | function draw_clothoids(start, goal, num_steps, clothoidal_paths, save_animation) 98 | figure('Position', [0, 0, 400, 400]); 99 | 100 | plot(start(1), start(2), 'ro'); hold on; 101 | plot(goal(1), goal(2), 'ro'); hold on; 102 | 103 | for i = 1:num_steps 104 | clothoid_path = clothoidal_paths; 105 | x = clothoid_path(1:i, 1); 106 | y = clothoid_path(1:i, 2); 107 | plot(x, y, 'b'); hold on 108 | pause(0.025); 109 | end 110 | 111 | if save_animation 112 | % Save the animation as a GIF file 113 | filename = 'clothoid.gif'; 114 | delay_time = 0.025; 115 | save_animated_gif(filename, delay_time); 116 | end 117 | end 118 | -------------------------------------------------------------------------------- /PathPlanning/CubicSpline/CubicSpline1D.m: -------------------------------------------------------------------------------- 1 | classdef CubicSpline1D < handle 2 | properties 3 | a 4 | b 5 | c 6 | d 7 | x 8 | y 9 | nx 10 | end 11 | 12 | methods (Access = public) 13 | function obj = CubicSpline1D(x, y) 14 | h = diff(x); 15 | if any(h < 0) 16 | error('x coordinates must be sorted in ascending order'); 17 | end 18 | 19 | obj.a = y; 20 | obj.b = zeros(1, numel(y)); 21 | obj.c = zeros(1, numel(y)); 22 | obj.d = zeros(1, numel(y)); 23 | obj.x = x; 24 | obj.y = y; 25 | obj.nx = numel(x); 26 | 27 | A = obj.calc_A(h); 28 | B = obj.calc_B(h); 29 | obj.c = A \ B'; 30 | 31 | for i = 1:(obj.nx - 1) 32 | d = (obj.c(i + 1) - obj.c(i)) / (3 * h(i)); 33 | b = 1 / h(i) * (obj.a(i + 1) - obj.a(i)) ... 34 | - h(i) / 3 * (2 * obj.c(i) + obj.c(i + 1)); 35 | obj.d(i) = d; 36 | obj.b(i) = b; 37 | end 38 | end 39 | 40 | function position = calc_position(obj, x) 41 | if x < obj.x(1) || x > obj.x(end) 42 | position = nan; 43 | return; 44 | end 45 | 46 | i = obj.search_index(x); 47 | dx = x - obj.x(i); 48 | position = obj.a(i) + obj.b(i) * dx + ... 49 | obj.c(i) * dx^2 + obj.d(i) * dx^3; 50 | end 51 | 52 | function dy = calc_first_derivative(obj, x) 53 | if x < obj.x(1) || x > obj.x(end) 54 | dy = nan; 55 | return; 56 | end 57 | 58 | i = obj.search_index(x); 59 | dx = x - obj.x(i); 60 | dy = obj.b(i) + 2 * obj.c(i) * dx + 3 * obj.d(i) * dx^2; 61 | end 62 | 63 | function ddy = calc_second_derivative(obj, x) 64 | if x < obj.x(1) || x > obj.x(end) 65 | ddy = nan; 66 | return; 67 | end 68 | 69 | i = obj.search_index(x); 70 | dx = x - obj.x(i); 71 | ddy = 2 * obj.c(i) + 6 * obj.d(i) * dx; 72 | end 73 | 74 | function idx = search_index(obj, x) 75 | [~, idx] = histc(x, obj.x); 76 | if idx == obj.nx 77 | idx = idx - 1; 78 | end 79 | end 80 | 81 | function A = calc_A(obj, h) 82 | % calc matrix A for spline coefficient c 83 | A = zeros(obj.nx); 84 | A(1, 1) = 1.0; 85 | for i = 1:(obj.nx - 1) 86 | if i ~= (obj.nx - 1) 87 | A(i + 1, i + 1) = 2.0 * (h(i) + h(i + 1)); 88 | end 89 | A(i + 1, i) = h(i); 90 | A(i, i + 1) = h(i); 91 | end 92 | 93 | A(1, 2) = 0.0; 94 | A(obj.nx, obj.nx - 1) = 0.0; 95 | A(obj.nx, obj.nx) = 1.0; 96 | end 97 | 98 | function B = calc_B(obj, h) 99 | % calc matrix B for spline coefficient c 100 | B = zeros(1, obj.nx); 101 | for i = 1:(obj.nx - 2) 102 | B(i + 1) = 3.0 * (obj.a(i + 2) - obj.a(i + 1)) / h(i + 1)... 103 | - 3.0 * (obj.a(i + 1) - obj.a(i)) / h(i); 104 | end 105 | end 106 | end 107 | end -------------------------------------------------------------------------------- /PathPlanning/CubicSpline/CubicSpline1D_test.m: -------------------------------------------------------------------------------- 1 | % Create a script file and type the following code 2 | % Assume that CubicSpline1D class implementation has been saved in a file named 'CubicSpline1D.m' 3 | 4 | % https://github.com/AtsushiSakai/pycubicspline/tree/acafae87f16e78dda0464241c88585434f153da7 5 | 6 | clear; 7 | close all; 8 | clc; 9 | 10 | % x = 0:4; 11 | % y = [1.7, -6, 5, 6.5, 0.0]; 12 | % x = [-0.5, 0.0, 0.5, 1.0, 1.5]; 13 | % y = [3.2, 2.7, 6, 5, 6.5]; 14 | 15 | x = [-0.5, 0.0, 0.5, 1.0, 1.5]; 16 | y = [3.2, 2.7, 6, 5, 6.5]; 17 | 18 | % x = [1,2,3,4]; 19 | % y = [2.7,6,5,6.5]; 20 | 21 | sp = CubicSpline1D(x, y); 22 | 23 | xi = linspace(-2.0, 4.0, 100); 24 | yi = arrayfun(@(x) calc_position(sp, x), xi); 25 | 26 | pp = spline(x,y); 27 | yy = ppval(pp, xi); 28 | 29 | figure; 30 | plot(x, y, 'xb', 'DisplayName', 'Data points'); 31 | hold on; 32 | plot(xi, yi, 'r', 'DisplayName', 'Cubic spline interpolation'); 33 | hold on; 34 | %plot(xi, yy, 'g', 'DisplayName', 'Cubic spline interpolation'); 35 | hold on; 36 | grid on; 37 | legend; 38 | hold off; -------------------------------------------------------------------------------- /PathPlanning/CubicSpline/CubicSpline2D.m: -------------------------------------------------------------------------------- 1 | % CubicSpline2D.m 2 | 3 | classdef CubicSpline2D 4 | properties 5 | s 6 | ds 7 | sx 8 | sy 9 | end 10 | methods 11 | function obj = CubicSpline2D(x, y) 12 | % dx = diff(x); 13 | % dy = diff(y); 14 | % obj.ds = hypot(dx, dy); 15 | % obj.s = [0 cumsum(ds)]; 16 | 17 | [obj.s, obj.ds] = obj.calc_s(x, y); 18 | obj.sx = CubicSpline1D(obj.s, x); 19 | obj.sy = CubicSpline1D(obj.s, y); 20 | end 21 | 22 | function [s, ds] = calc_s(obj, x, y) 23 | dx = diff(x); 24 | dy = diff(y); 25 | ds = hypot(dx, dy); 26 | s = [0 cumsum(ds)]; 27 | end 28 | 29 | function [x, y] = calc_position(obj, s) 30 | x = calc_position(obj.sx, s); 31 | y = calc_position(obj.sy, s); 32 | end 33 | 34 | function k = calc_curvature(obj, s) 35 | dx = calc_first_derivative(obj.sx, s); 36 | ddx = calc_second_derivative(obj.sx, s); 37 | dy = calc_first_derivative(obj.sy, s); 38 | ddy = calc_second_derivative(obj.sy, s); 39 | k = (ddy * dx - ddx * dy) / ((dx^2 + dy^2)^(3/2)); 40 | end 41 | 42 | function yaw = calc_yaw(obj, s) 43 | dx = calc_first_derivative(obj.sx, s); 44 | dy = calc_first_derivative(obj.sy, s); 45 | yaw = atan2(dy, dx); 46 | end 47 | end 48 | end -------------------------------------------------------------------------------- /PathPlanning/CubicSpline/CubicSpline2D_test.m: -------------------------------------------------------------------------------- 1 | % CubicSpline2D_example.m 2 | clear; 3 | close all; 4 | clc; 5 | 6 | % Define x, y, and ds 7 | x = [-2.5, 0.0, 2.5, 5.0, 7.5, 3.0, -1.0]; 8 | y = [0.7, -6, 5, 6.5, 0.0, 5.0, -2.0]; 9 | ds = 0.1; % [m] distance of each interpolated points 10 | 11 | % Compute the cubic spline 12 | [rx, ry, ryaw, rk, s] = calc_spline_course(x, y, ds); 13 | 14 | % Plot the cubic spline path 15 | figure; 16 | plot(x, y, 'xb', 'DisplayName', 'Data points'); 17 | hold on; 18 | plot(rx, ry, '-r', 'DisplayName', 'Cubic spline path'); 19 | grid on; 20 | axis equal; 21 | xlabel('x[m]'); 22 | ylabel('y[m]'); 23 | legend; 24 | 25 | % Plot yaw angle 26 | figure; 27 | plot(s, rad2deg(ryaw), '-r', 'DisplayName', 'yaw'); 28 | grid on; 29 | legend; 30 | xlabel('line length[m]'); 31 | ylabel('yaw angle[deg]'); 32 | 33 | % Plot curvature 34 | figure; 35 | plot(s, rk, '-r', 'DisplayName', 'curvature'); 36 | grid on; 37 | legend; 38 | xlabel('line length[m]'); 39 | ylabel('curvature [1/m]'); 40 | 41 | function [rx, ry, ryaw, rk, s] = calc_spline_course(x, y, ds) 42 | % Compute the cubic spline 43 | sp = CubicSpline2D(x, y); 44 | s = 0:ds:sp.s(end); 45 | rx = []; 46 | ry = []; 47 | ryaw = []; 48 | rk = []; 49 | 50 | for i_s = s 51 | [ix, iy] = sp.calc_position(i_s); 52 | rx = [rx, ix]; 53 | ry = [ry, iy]; 54 | ryaw = [ryaw, sp.calc_yaw(i_s)]; 55 | rk = [rk, sp.calc_curvature(i_s)]; 56 | end 57 | end 58 | -------------------------------------------------------------------------------- /PathPlanning/FrenetOptimalTrajectory/FrenetPath.m: -------------------------------------------------------------------------------- 1 | % FrenetPath.m 2 | 3 | classdef FrenetPath 4 | properties 5 | t 6 | d 7 | d_d 8 | d_dd 9 | d_ddd 10 | s 11 | s_d 12 | s_dd 13 | s_ddd 14 | cd 15 | cv 16 | cf 17 | x 18 | y 19 | yaw 20 | ds 21 | c 22 | end 23 | 24 | methods 25 | function obj = FrenetPath() 26 | obj.t = []; 27 | obj.d = []; 28 | obj.d_d = []; 29 | obj.d_dd = []; 30 | obj.d_ddd = []; 31 | obj.s = []; 32 | obj.s_d = []; 33 | obj.s_dd = []; 34 | obj.s_ddd = []; 35 | obj.cd = 0.0; 36 | obj.cv = 0.0; 37 | obj.cf = 0.0; 38 | obj.x = []; 39 | obj.y = []; 40 | obj.yaw = []; 41 | obj.ds = []; 42 | obj.c = []; 43 | end 44 | end 45 | end -------------------------------------------------------------------------------- /PathPlanning/FrenetOptimalTrajectory/main.m: -------------------------------------------------------------------------------- 1 | % frenet_optimal_planning.m 2 | clear; 3 | close all; 4 | clc; 5 | 6 | %% Parameters 7 | param.MAX_SPEED = 50.0 / 3.6; % maximum speed [m/s] 8 | param.MAX_ACCEL = 2.0; % maximum acceleration [m/ss] 9 | param.MAX_CURVATURE = 1.0; % maximum curvature [1/m] 10 | param.MAX_ROAD_WIDTH = 7.0; % maximum road width [m] 11 | param.D_ROAD_W = 1.0; % road width sampling length [m] 12 | param.DT = 0.2; % time tick [s] 13 | param.MAX_T = 5.0; % max prediction time [m] 14 | param.MIN_T = 4.0; % min prediction time [m] 15 | param.TARGET_SPEED = 30.0 / 3.6; % target speed [m/s] 16 | param.D_T_S = 5.0 / 3.6; % target speed sampling length [m/s] 17 | param.N_S_SAMPLE = 1; % sampling number of target speed 18 | param.ROBOT_RADIUS = 1.0; 19 | param.OBSTACLE_RADIUS = 1.0; 20 | param.PLANNING_RADIUS = param.ROBOT_RADIUS + param.OBSTACLE_RADIUS; % robot radius [m] 21 | 22 | % cost weights 23 | param.K_J = 0.1; 24 | param.K_T = 0.1; 25 | param.K_D = 1.0; 26 | param.K_LAT = 1.0; 27 | param.K_LON = 1.0; 28 | 29 | %% main loop 30 | disp(strcat(mfilename, " start!!")); 31 | 32 | % way points 33 | wx = [0.0, 10.0, 20.5, 35.0, 70.5]; 34 | wy = [0.0, -6.0, 5.0, 6.5, 0.0]; 35 | 36 | % obstacle lists 37 | ob = [20.0, 10.0; 38 | 30.0, 6.0; 39 | 30.0, 8.0; 40 | 35.0, 8.0; 41 | 50.0, 3.0]; 42 | 43 | [tx, ty, tyaw, tc, csp] = generate_target_course(wx, wy); 44 | 45 | % initial state 46 | c_speed = 10.0 / 3.6; % current speed [m/s] 47 | c_accel = 0.0; % current acceleration [m/ss] 48 | c_d = 2.0; % current lateral position [m] 49 | c_d_d = 0.0; % current lateral speed [m/s] 50 | c_d_dd = 0.0; % current lateral acceleration [m/s] 51 | s0 = 0.0; % current course position 52 | 53 | area = 20.0; % animation area length [m] 54 | 55 | SIM_LOOP = 500; 56 | 57 | for i = 1:SIM_LOOP 58 | [path, fplist] = frenet_optimal_planning(param, csp, s0, c_speed, c_accel, ... 59 | c_d, c_d_d, c_d_dd, ob); 60 | 61 | s0 = path.s(2); 62 | c_d = path.d(2); 63 | c_d_d = path.d_d(2); 64 | c_d_dd = path.d_dd(2); 65 | c_speed = path.s_d(2); 66 | c_accel = path.s_dd(2); 67 | 68 | if hypot(path.x(2) - tx(end), path.y(2) - ty(end)) <= 1.0 69 | disp("Goal"); 70 | break; 71 | end 72 | 73 | % show_animation 74 | hold off; 75 | fig = figure(1); 76 | fig.Position = [0 0 1368 700]; 77 | plot(tx, ty, '-b'); hold on; 78 | 79 | th = linspace(0,2*pi*100); 80 | circle_x = cos(th) ; 81 | circle_y = sin(th) ; 82 | plot(path.x(2) + param.ROBOT_RADIUS * circle_x, ... 83 | path.y(2) + param.ROBOT_RADIUS * circle_y, 'm'); hold on; 84 | 85 | obs_radius = param.PLANNING_RADIUS / 2; 86 | for j = 1:length(ob(:, 1)) 87 | plot(ob(j, 1) + param.OBSTACLE_RADIUS * circle_x, ... 88 | ob(j, 2) + param.OBSTACLE_RADIUS * circle_y, 'k'); hold on; 89 | end 90 | 91 | for k = 1:length(fplist) 92 | fp = fplist(k); 93 | plot(fp.x(2:end), fp.y(2:end), 'g'); hold on; 94 | end 95 | 96 | plot(path.x(2:end), path.y(2:end), 'r', 'LineWidth', 5.0); hold on; 97 | xlim([-10, 80]); 98 | ylim([-20, 20]); 99 | title(strcat("v[km/h]:", num2str(c_speed * 3.6, '%.2f'))); 100 | axis equal; 101 | grid on; 102 | end 103 | 104 | disp("Finish"); 105 | 106 | % MATLAB version of the cubic_spline_planner should be implemented 107 | % and used as generate_target_course function. 108 | function [rx, ry, ryaw, rk, csp] = generate_target_course(x, y) 109 | csp = CubicSpline2D(x, y); 110 | s = 0 : 0.1 : csp.s(end); 111 | 112 | rx = []; 113 | ry = []; 114 | ryaw = []; 115 | rk = []; 116 | for i = 1:length(s) 117 | [ix, iy] = csp.calc_position(s(i)); 118 | rx = [rx, ix]; 119 | ry = [ry, iy]; 120 | ryaw = [ryaw, csp.calc_yaw(s(i))]; 121 | rk = [rk, csp.calc_curvature(s(i))]; 122 | end 123 | end 124 | 125 | function frenet_paths = calc_frenet_paths(param, c_speed, c_accel, c_d, c_d_d, c_d_dd, s0) 126 | frenet_paths = []; 127 | 128 | % generate path to each offset goal 129 | for di = -param.MAX_ROAD_WIDTH : param.D_ROAD_W : param.MAX_ROAD_WIDTH 130 | 131 | % Lateral motion planning 132 | for Ti = param.MIN_T : param.DT : param.MAX_T 133 | fp = FrenetPath(); 134 | 135 | % lat_qp = quintic_polynomial(c_d, c_d_d, c_d_dd, di, 0.0, 0.0, Ti) 136 | lat_qp = QuinticPolynomial(c_d, c_d_d, c_d_dd, di, 0.0, 0.0, Ti); 137 | 138 | fp.t = 0 : param.DT : Ti; 139 | fp.d = lat_qp.calc_point(fp.t); 140 | fp.d_d = lat_qp.calc_first_derivative(fp.t); 141 | fp.d_dd = lat_qp.calc_second_derivative(fp.t); 142 | fp.d_ddd = lat_qp.calc_third_derivative(fp.t); 143 | 144 | % Longitudinal motion planning (Velocity keeping) 145 | for tv = param.TARGET_SPEED - param.D_T_S * param.N_S_SAMPLE : param.D_T_S : param.TARGET_SPEED + param.D_T_S * param.N_S_SAMPLE 146 | %tfp = copy(fp); 147 | tfp = fp; 148 | lon_qp = QuarticPolynomial(s0, c_speed, c_accel, tv, 0.0, Ti); 149 | 150 | tfp.s = lon_qp.calc_point(fp.t); 151 | tfp.s_d = lon_qp.calc_first_derivative(fp.t); 152 | tfp.s_dd = lon_qp.calc_second_derivative(fp.t); 153 | tfp.s_ddd = lon_qp.calc_third_derivative(fp.t); 154 | 155 | Jp = sum(tfp.d_ddd.^2); % square of jerk 156 | Js = sum(tfp.s_ddd.^2); 157 | 158 | % Square of diff from target speed 159 | ds = (param.TARGET_SPEED - tfp.s_d(end))^2; 160 | 161 | tfp.cd = param.K_J * Jp + param.K_T * Ti + param.K_D * tfp.d(end)^2; 162 | tfp.cv = param.K_J * Js + param.K_T * Ti + param.K_D * ds; 163 | tfp.cf = param.K_LAT * tfp.cd + param.K_LON * tfp.cv; 164 | 165 | frenet_paths = [frenet_paths, tfp]; 166 | end 167 | end 168 | end 169 | end 170 | 171 | function fplist = calc_global_paths(fplist, csp) 172 | for fp_idx = 1:numel(fplist) 173 | fp = fplist(fp_idx); 174 | 175 | % calc global positions 176 | for i = 1:length(fp.s) 177 | [ix, iy] = calc_position(csp, fp.s(i)); 178 | if isempty(ix) 179 | break; 180 | end 181 | i_yaw = calc_yaw(csp, fp.s(i)); 182 | di = fp.d(i); 183 | fx = ix + di * cos(i_yaw + pi / 2.0); 184 | fy = iy + di * sin(i_yaw + pi / 2.0); 185 | fp.x = [fp.x, fx]; 186 | fp.y = [fp.y, fy]; 187 | end 188 | 189 | % calc yaw and ds 190 | for i = 1:length(fp.x) - 1 191 | dx = fp.x(i + 1) - fp.x(i); 192 | dy = fp.y(i + 1) - fp.y(i); 193 | fp.yaw = [fp.yaw, atan2(dy, dx)]; 194 | fp.ds = [fp.ds, hypot(dx, dy)]; 195 | end 196 | 197 | fp.yaw = [fp.yaw, fp.yaw(end)]; 198 | fp.ds = [fp.ds, fp.ds(end)]; 199 | 200 | % calc curvature 201 | for i = 1:length(fp.yaw) - 1 202 | fp.c = [fp.c, (fp.yaw(i + 1) - fp.yaw(i)) / fp.ds(i)]; 203 | end 204 | 205 | fplist(fp_idx) = fp; 206 | end 207 | end 208 | 209 | function valid_paths = check_paths(fplist, ob, param) 210 | ok_ind = []; 211 | MAX_SPEED = param.MAX_SPEED; 212 | MAX_ACCEL = param.MAX_ACCEL; 213 | MAX_CURVATURE = param.MAX_CURVATURE; 214 | 215 | for i = 1:numel(fplist) 216 | fp = fplist(i); 217 | % Max speed check 218 | if any(fp.s_d > MAX_SPEED) 219 | continue; 220 | end 221 | 222 | % Max accel check 223 | if any(abs(fp.s_dd) > MAX_ACCEL) 224 | continue; 225 | end 226 | 227 | % Max curvature check 228 | if any(abs(fp.c) > MAX_CURVATURE) 229 | continue; 230 | end 231 | 232 | % Collision check 233 | if ~check_collision(fp, ob, param) 234 | continue; 235 | end 236 | 237 | ok_ind = [ok_ind, i]; 238 | end 239 | 240 | valid_paths = fplist(ok_ind); 241 | end 242 | 243 | function is_collision_free = check_collision(fp, ob, param) 244 | is_collision_free = true; 245 | PLANNING_RADIUS = param.PLANNING_RADIUS; 246 | 247 | for i = 1:size(ob, 1) 248 | d = arrayfun(@(ix, iy) (ix - ob(i, 1))^2 + (iy - ob(i, 2))^2, fp.x, fp.y); 249 | 250 | collision = any(d <= PLANNING_RADIUS^2); 251 | 252 | if collision 253 | is_collision_free = false; 254 | return; 255 | end 256 | end 257 | end 258 | 259 | function [best_path, fplist] = frenet_optimal_planning(param, csp, s0, c_speed, c_accel, ... 260 | c_d, c_d_d, c_d_dd, ob) 261 | fplist = calc_frenet_paths(param, c_speed, c_accel, c_d, c_d_d, c_d_dd, s0); 262 | fplist = calc_global_paths(fplist, csp); 263 | fplist = check_paths(fplist, ob, param); 264 | 265 | % find minimum cost path 266 | min_cost = Inf; 267 | best_path = []; 268 | for i = 1:length(fplist) 269 | fp = fplist(i); 270 | if min_cost >= fp.cf 271 | min_cost = fp.cf; 272 | best_path = fp; 273 | end 274 | end 275 | end 276 | -------------------------------------------------------------------------------- /PathPlanning/QuinticPolynomialsPlanner/QuarticPolynomial.m: -------------------------------------------------------------------------------- 1 | % QuarticPolynomial.m 2 | 3 | classdef QuarticPolynomial 4 | properties 5 | a0 6 | a1 7 | a2 8 | a3 9 | a4 10 | end 11 | 12 | methods 13 | function obj = QuarticPolynomial(xs, vxs, axs, vxe, axe, time) 14 | obj.a0 = xs; 15 | obj.a1 = vxs; 16 | obj.a2 = axs / 2.0; 17 | 18 | A = [3 * time ^ 2, 4 * time ^ 3; 19 | 6 * time, 12 * time ^ 2]; 20 | b = [vxe - obj.a1 - 2 * obj.a2 * time; 21 | axe - 2 * obj.a2]; 22 | x = A \ b; 23 | 24 | obj.a3 = x(1); 25 | obj.a4 = x(2); 26 | end 27 | 28 | function xt = calc_point(obj, t) 29 | xt = obj.a0 + obj.a1 * t + obj.a2 * t .^ 2 + ... 30 | obj.a3 * t .^ 3 + obj.a4 * t .^ 4; 31 | end 32 | 33 | function xt = calc_first_derivative(obj, t) 34 | xt = obj.a1 + 2 * obj.a2 * t + ... 35 | 3 * obj.a3 * t .^ 2 + 4 * obj.a4 * t .^ 3; 36 | end 37 | 38 | function xt = calc_second_derivative(obj, t) 39 | xt = 2 * obj.a2 + 6 * obj.a3 * t + 12 * obj.a4 * t .^ 2; 40 | end 41 | 42 | function xt = calc_third_derivative(obj, t) 43 | xt = 6 * obj.a3 + 24 * obj.a4 * t; 44 | end 45 | end 46 | end -------------------------------------------------------------------------------- /PathPlanning/QuinticPolynomialsPlanner/QuinticPolynomial.m: -------------------------------------------------------------------------------- 1 | % QuinticPolynomial.m 2 | 3 | classdef QuinticPolynomial 4 | properties 5 | a0 6 | a1 7 | a2 8 | a3 9 | a4 10 | a5 11 | end 12 | 13 | methods 14 | function obj = QuinticPolynomial(xs, vxs, axs, xe, vxe, axe, time) 15 | obj.a0 = xs; 16 | obj.a1 = vxs; 17 | obj.a2 = axs / 2.0; 18 | 19 | A = [time ^ 3, time ^ 4, time ^ 5; 20 | 3 * time ^ 2, 4 * time ^ 3, 5 * time ^ 4; 21 | 6 * time, 12 * time ^ 2, 20 * time ^ 3]; 22 | b = [xe - obj.a0 - obj.a1 * time - obj.a2 * time ^ 2; 23 | vxe - obj.a1 - 2 * obj.a2 * time; 24 | axe - 2 * obj.a2]; 25 | x = A \ b; 26 | 27 | obj.a3 = x(1); 28 | obj.a4 = x(2); 29 | obj.a5 = x(3); 30 | end 31 | 32 | function xt = calc_point(obj, t) 33 | xt = obj.a0 + obj.a1 * t + obj.a2 * t .^ 2 + ... 34 | obj.a3 * t .^ 3 + obj.a4 * t .^ 4 + obj.a5 * t .^ 5; 35 | end 36 | 37 | function xt = calc_first_derivative(obj, t) 38 | xt = obj.a1 + 2 * obj.a2 * t + ... 39 | 3 * obj.a3 * t .^ 2 + 4 * obj.a4 * t .^ 3 + 5 * obj.a5 * t .^ 4; 40 | end 41 | 42 | function xt = calc_second_derivative(obj, t) 43 | xt = 2 * obj.a2 + 6 * obj.a3 * t + 12 * obj.a4 * t .^ 2 + 20 * obj.a5 * t .^ 3; 44 | end 45 | 46 | function xt = calc_third_derivative(obj, t) 47 | xt = 6 * obj.a3 + 24 * obj.a4 * t + 60 * obj.a5 * t .^ 2; 48 | end 49 | end 50 | end -------------------------------------------------------------------------------- /PathPlanning/QuinticPolynomialsPlanner/QuinticPolynomialExample.m: -------------------------------------------------------------------------------- 1 | function QuinticPolynomialExample() 2 | clear; 3 | close all; 4 | clc; 5 | 6 | disp("main start!"); 7 | 8 | sx = 10.0; % start x position [m] 9 | sy = 10.0; % start y position [m] 10 | syaw = deg2rad(10.0); % start yaw angle [rad] 11 | sv = 1.0; % start speed [m/s] 12 | sa = 0.1; % start accel [m/ss] 13 | gx = 30.0; % goal x position [m] 14 | gy = -10.0; % goal y position [m] 15 | gyaw = deg2rad(20.0); % goal yaw angle [rad] 16 | gv = 1.0; % goal speed [m/s] 17 | ga = 0.1; % goal accel [m/ss] 18 | max_accel = 1.0; % max accel [m/ss] 19 | max_jerk = 0.5; % max jerk [m/sss] 20 | dt = 0.1; % time tick [s] 21 | 22 | [time, x, y, yaw, v, a, j] = quintic_polynomials_planner(sx, sy, syaw, sv, sa, gx, gy, gyaw, gv, ga, max_accel, max_jerk, dt); 23 | 24 | % Plot x, y 25 | figure; 26 | plot(x, y, "-r"); 27 | grid on; 28 | 29 | % Plot yaw 30 | figure; 31 | plot(time, rad2deg(yaw), "-r"); 32 | xlabel("Time[s]"); 33 | ylabel("Yaw[deg]"); 34 | grid on; 35 | 36 | % Plot speed 37 | figure; 38 | plot(time, v, "-r"); 39 | xlabel("Time[s]"); 40 | ylabel("Speed[m/s]"); 41 | grid on; 42 | 43 | % Plot acceleration 44 | figure; 45 | plot(time, a, "-r"); 46 | xlabel("Time[s]"); 47 | ylabel("accel[m/ss]"); 48 | grid on; 49 | 50 | % Plot jerk 51 | figure; 52 | plot(time, j, "-r"); 53 | xlabel("Time[s]"); 54 | ylabel("jerk[m/sss]"); 55 | grid on; 56 | end 57 | 58 | function [time, rx, ry, ryaw, rv, ra, rj] = quintic_polynomials_planner(sx, sy, syaw, sv, sa, gx, gy, gyaw, gv, ga, max_accel, max_jerk, dt) 59 | MIN_T = 0.1; 60 | MAX_T = 100.0; 61 | 62 | vxs = sv * cos(syaw); 63 | vys = sv * sin(syaw); 64 | vxg = gv * cos(gyaw); 65 | vyg = gv * sin(gyaw); 66 | 67 | axs = sa * cos(syaw); 68 | ays = sa * sin(syaw); 69 | axg = ga * cos(gyaw); 70 | ayg = ga * sin(gyaw); 71 | 72 | time = []; 73 | rx = []; 74 | ry = []; 75 | ryaw = []; 76 | rv = []; 77 | ra = []; 78 | rj = []; 79 | 80 | for T = MIN_T:MIN_T:MAX_T 81 | xqp = QuinticPolynomial(sx, vxs, axs, gx, vxg, axg, T); 82 | yqp = QuinticPolynomial(sy, vys, ays, gy, vyg, ayg, T); 83 | 84 | time_temp = []; 85 | rx_temp = []; 86 | ry_temp = []; 87 | ryaw_temp = []; 88 | rv_temp = []; 89 | ra_temp = []; 90 | rj_temp = []; 91 | 92 | for t = 0.0:dt:T 93 | time_temp = [time_temp, t]; 94 | rx_temp = [rx_temp, xqp.calc_point(t)]; 95 | ry_temp = [ry_temp, yqp.calc_point(t)]; 96 | 97 | vx = xqp.calc_first_derivative(t); 98 | vy = yqp.calc_first_derivative(t); 99 | v = hypot(vx, vy); 100 | yaw = atan2(vy, vx); 101 | rv_temp = [rv_temp, v]; 102 | ryaw_temp = [ryaw_temp, yaw]; 103 | 104 | ax = xqp.calc_second_derivative(t); 105 | ay = yqp.calc_second_derivative(t); 106 | a = hypot(ax, ay); 107 | if length(rv_temp) >= 2 && rv_temp(end) - rv_temp(end-1) < 0.0 108 | a = -a; 109 | end 110 | ra_temp = [ra_temp, a]; 111 | 112 | jx = xqp.calc_third_derivative(t); 113 | jy = yqp.calc_third_derivative(t); 114 | j = hypot(jx, jy); 115 | if length(ra_temp) >= 2 && ra_temp(end) - ra_temp(end-1) < 0.0 116 | j = -j; 117 | end 118 | rj_temp = [rj_temp, j]; 119 | end 120 | 121 | if max(abs(ra_temp)) <= max_accel && max(abs(rj_temp)) <= max_jerk 122 | fprintf("find path!!\n"); 123 | time = time_temp; 124 | rx = rx_temp; 125 | ry = ry_temp; 126 | ryaw = ryaw_temp; 127 | rv = rv_temp; 128 | ra = ra_temp; 129 | rj = rj_temp; 130 | break; 131 | end 132 | end 133 | end 134 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # MATLAB-Robotics-Engineering 2 | MATLAB sample codes for Robotics engineering 3 | # Model Predictive control 4 | The following links provide sample code for Model Predictive control. 5 | https://github.com/Ramune6110/MATLAB-Robotics-Engineering/tree/main/Model_Predictive_Control 6 | -------------------------------------------------------------------------------- /VehicleDynamics/DynamicBicycleModel.m: -------------------------------------------------------------------------------- 1 | function [] = DynamicBicycleModel() 2 | clear; 3 | close all; 4 | clc; 5 | 6 | disp("start Dynamic Bicycle model simulation"); 7 | 8 | T = 100; 9 | a = ones(1, T); 10 | delta = deg2rad(ones(1, T)); 11 | 12 | state = struct('x', 0, 'y', 0, 'yaw', 0, 'vx', 0.01, 'vy', 0, 'omega', 0); 13 | 14 | x = []; 15 | y = []; 16 | yaw = []; 17 | vx = []; 18 | vy = []; 19 | time = []; 20 | t = 0.0; 21 | 22 | dt = 0.1; 23 | Cf = 1600 * 2.0; 24 | Cr = 1700 * 2.0; 25 | L = 5.0; 26 | Lr = L / 2.0; 27 | Lf = L - Lr; 28 | m = 1500; 29 | Iz = 2250; 30 | 31 | for i = 1:T 32 | t = t + dt; 33 | state = update_kinematic_bicycle(state, a(i), delta(i), dt, Cf, Cr, Lf, Lr, m, Iz); 34 | x = [x, state.x]; 35 | y = [y, state.y]; 36 | yaw = [yaw, state.yaw]; 37 | vx = [vx, state.vx]; 38 | vy = [vy, state.vy]; 39 | time = [time, t]; 40 | end 41 | 42 | figure; 43 | plot(x, y); 44 | xlabel("x[m]"); 45 | ylabel("y[m]"); 46 | axis("equal"); 47 | grid on; 48 | end 49 | 50 | function state = update_kinematic_bicycle(state, a, delta, dt, Cf, Cr, Lf, Lr, m, Iz) 51 | state.x = state.x + state.vx * cos(state.yaw) * dt - state.vy * sin(state.yaw) * dt; 52 | state.y = state.y + state.vx * sin(state.yaw) * dt + state.vy * cos(state.yaw) * dt; 53 | state.yaw = state.yaw + state.omega * dt; 54 | Ffy = -Cf * atan2(((state.vy + Lf * state.omega) / state.vx - delta), 1.0); 55 | Fry = -Cr * atan2((state.vy - Lr * state.omega) / state.vx, 1.0); 56 | state.vx = state.vx + (a - Ffy * sin(delta) / m + state.vy * state.omega) * dt; 57 | state.vy = state.vy + (Fry / m + Ffy * cos(delta) / m - state.vx * state.omega) * dt; 58 | state.omega = state.omega + (Ffy * Lf * cos(delta) - Fry * Lr) / Iz * dt; 59 | end 60 | -------------------------------------------------------------------------------- /VehicleDynamics/HandleMotionAnimation.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Ramune6110/MATLAB-Robotics-Engineering/cb44e950c5183f6000c183928f9c65cb3ced82da/VehicleDynamics/HandleMotionAnimation.m -------------------------------------------------------------------------------- /VehicleDynamics/KinematicBicycleModel.m: -------------------------------------------------------------------------------- 1 | function [] = KinematicBicycleModel() 2 | clear; 3 | close all; 4 | clc; 5 | 6 | disp("start Kinematic Bicycle model simulation"); 7 | 8 | T = 100; 9 | a = ones(1, T); 10 | delta = deg2rad(ones(1, T)); 11 | 12 | state = struct('x', 0, 'y', 0, 'yaw', 0, 'v', 0, 'beta', 0); 13 | 14 | x = []; 15 | y = []; 16 | yaw = []; 17 | v = []; 18 | beta = []; 19 | time = []; 20 | t = 0.0; 21 | dt = 0.1; 22 | L = 2.9; 23 | Lr = L / 2; 24 | 25 | for i = 1:T 26 | t = t + dt; 27 | state = update(state, a(i), delta(i), Lr, L, dt); 28 | x = [x, state.x]; 29 | y = [y, state.y]; 30 | yaw = [yaw, state.yaw]; 31 | v = [v, state.v]; 32 | beta = [beta, state.beta]; 33 | time = [time, t]; 34 | end 35 | 36 | figure; 37 | plot(x, y); 38 | xlabel("x[m]"); 39 | ylabel("y[m]"); 40 | axis("equal"); 41 | grid on; 42 | end 43 | 44 | function state = update(state, a, delta, Lr, L, dt) 45 | state.beta = atan2(Lr / L * tan(delta), 1.0); 46 | state.x = state.x + state.v * cos(state.yaw + state.beta) * dt; 47 | state.y = state.y + state.v * sin(state.yaw + state.beta) * dt; 48 | state.yaw = state.yaw + state.v / Lr * sin(state.beta) * dt; 49 | state.v = state.v + a * dt; 50 | end 51 | -------------------------------------------------------------------------------- /VehicleDynamics/UnicycleModel.m: -------------------------------------------------------------------------------- 1 | function [] = UnicycleModel() 2 | clear; 3 | close all; 4 | clc; 5 | 6 | disp("start unicycle simulation"); 7 | 8 | T = 100; 9 | a = ones(1, T); 10 | delta = deg2rad(ones(1, T)); 11 | 12 | state = struct('x', 0, 'y', 0, 'yaw', 0, 'v', 0); 13 | 14 | x = []; 15 | y = []; 16 | yaw = []; 17 | v = []; 18 | dt = 0.1; 19 | L = 2.9; 20 | 21 | for i = 1:T 22 | state = update_unicycle(state, a(i), delta(i), L, dt); 23 | 24 | x = [x, state.x]; 25 | y = [y, state.y]; 26 | yaw = [yaw, state.yaw]; 27 | v = [v, state.v]; 28 | end 29 | 30 | figure; 31 | plot(x, y); 32 | axis("equal"); 33 | grid on; 34 | end 35 | 36 | function state = update_unicycle(state, a, delta, L, dt) 37 | state.x = state.x + state.v * cos(state.yaw) * dt; 38 | state.y = state.y + state.v * sin(state.yaw) * dt; 39 | state.yaw = state.yaw + state.v / L * tan(delta) * dt; 40 | state.v = state.v + a * dt; 41 | end 42 | -------------------------------------------------------------------------------- /VehicleDynamics/VehicelMotionSimlation.m: -------------------------------------------------------------------------------- 1 | clear; 2 | close all; 3 | clc; 4 | 5 | % Parameters 6 | L = 2.5; % wheelbase [m] 7 | v = 10; % constant vehicle speed [m/s] 8 | dt = 0.01; % time step [s] 9 | T = 10; % total time [s] 10 | t = 0:dt:T; % time vector 11 | omega = 2 * pi / T; % angular frequency of the sine wave 12 | 13 | % Generate steering angle signal (sin wave) 14 | delta = sin(omega * t); 15 | 16 | % Initialize vehicle states 17 | x = zeros(1, length(t)); 18 | y = zeros(1, length(t)); 19 | theta = zeros(1, length(t)); 20 | 21 | % Simulate the bicycle model 22 | for i = 2:length(t) 23 | theta(i) = theta(i-1) + (v / L) * tan(delta(i-1)) * dt; 24 | x(i) = x(i-1) + v * cos(theta(i-1)) * dt; 25 | y(i) = y(i-1) + v * sin(theta(i-1)) * dt; 26 | end 27 | 28 | % Calculate traveled distance 29 | traveled_distance = sum(sqrt(diff(x).^2 + diff(y).^2)); 30 | traveled_distance_log = cumsum(sqrt(diff(x).^2 + diff(y).^2)); 31 | 32 | % Plot x, y, theta vs time 33 | figure(1); 34 | subplot(411); 35 | plot(t, delta); 36 | xlabel('Time [s]'); 37 | ylabel('delta [rad]'); 38 | 39 | subplot(412); 40 | plot(t, x); 41 | xlabel('Time [s]'); 42 | ylabel('x [m]'); 43 | 44 | subplot(413); 45 | plot(t, y); 46 | xlabel('Time [s]'); 47 | ylabel('y [m]'); 48 | 49 | subplot(414); 50 | plot(t, theta); 51 | xlabel('Time [s]'); 52 | ylabel('\theta [rad]'); 53 | 54 | % Plot x, y, theta vs traveled distance 55 | figure(2); 56 | subplot(411); 57 | plot(traveled_distance_log, delta(2:end)); 58 | xlabel('Traveled Distance [m]'); 59 | ylabel('delta [rad]'); 60 | 61 | subplot(412); 62 | plot(traveled_distance_log, x(2:end)); 63 | xlabel('Traveled Distance [m]'); 64 | ylabel('x [m]'); 65 | 66 | subplot(413); 67 | plot(traveled_distance_log, y(2:end)); 68 | xlabel('Traveled Distance [m]'); 69 | ylabel('y [m]'); 70 | 71 | subplot(414); 72 | plot(traveled_distance_log, theta(2:end)); 73 | xlabel('Traveled Distance [m]'); 74 | ylabel('\theta [rad]'); 75 | 76 | % trajectory 77 | figure(3); 78 | plot(x, y, 'b'); 79 | grid on; 80 | -------------------------------------------------------------------------------- /VehicleDynamics/VehicleMotionAnimation.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Ramune6110/MATLAB-Robotics-Engineering/cb44e950c5183f6000c183928f9c65cb3ced82da/VehicleDynamics/VehicleMotionAnimation.m -------------------------------------------------------------------------------- /VehicleDynamics/going_down_slope.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Ramune6110/MATLAB-Robotics-Engineering/cb44e950c5183f6000c183928f9c65cb3ced82da/VehicleDynamics/going_down_slope.m -------------------------------------------------------------------------------- /VehicleDynamics/going_up_slope.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Ramune6110/MATLAB-Robotics-Engineering/cb44e950c5183f6000c183928f9c65cb3ced82da/VehicleDynamics/going_up_slope.m --------------------------------------------------------------------------------