├── .gitignore ├── docs └── images │ ├── 1.png │ ├── 2.gif │ └── 3.jpg ├── codes ├── find_distance.m ├── plant.m ├── solver.m ├── obj_fcn.m ├── initialize.m ├── sim_plots.m ├── simulate_plant.m └── main.m ├── README.md └── LICENSE /.gitignore: -------------------------------------------------------------------------------- 1 | *.asv 2 | 3 | media/ -------------------------------------------------------------------------------- /docs/images/1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MustafaLotfi/Vehicle-Lateral-Control/HEAD/docs/images/1.png -------------------------------------------------------------------------------- /docs/images/2.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MustafaLotfi/Vehicle-Lateral-Control/HEAD/docs/images/2.gif -------------------------------------------------------------------------------- /docs/images/3.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MustafaLotfi/Vehicle-Lateral-Control/HEAD/docs/images/3.jpg -------------------------------------------------------------------------------- /codes/find_distance.m: -------------------------------------------------------------------------------- 1 | function [dist, dist_arg]=find_distance(x, y, x_ref, y_ref) 2 | [dist, dist_arg] = min(sqrt(sum([x-x_ref;y-y_ref].^2))); -------------------------------------------------------------------------------- /codes/plant.m: -------------------------------------------------------------------------------- 1 | function qdot=plant(t, q, delta, params) 2 | L = params.L; 3 | v = params.v; 4 | qdot = v*[cos(delta+q(3)), sin(delta+q(3)), sin(delta)/L]'; 5 | -------------------------------------------------------------------------------- /codes/solver.m: -------------------------------------------------------------------------------- 1 | function q_new=solver(t, q, delta, params) 2 | dt = params.dt; 3 | method = 'r'; % r: runge-kutta, t: taylor expansion 4 | if method == 't' 5 | qdot = plant(t, q, delta, params); 6 | q_new = q+dt*qdot; 7 | elseif method == 'r' 8 | h = dt; 9 | y = q; 10 | k1 = plant(t, y, delta, params); 11 | k2 = plant(t+h/2, y+h*k1/2, delta, params); 12 | k3 = plant(t+h/2, y+h*k2/2, delta, params); 13 | k4 = plant(t+h, y+h*k3, delta, params); 14 | 15 | y_new = y + h/6*(k1+2*k2+2*k3+k4); 16 | 17 | q_new = y_new; 18 | end 19 | -------------------------------------------------------------------------------- /codes/obj_fcn.m: -------------------------------------------------------------------------------- 1 | function cost=obj_fcn(coefs, user_params) 2 | sim_vars = simulate_plant(coefs, user_params); 3 | q_mat = sim_vars.q_mat; 4 | delta_vec = sim_vars.delta_vec; 5 | err_vec = sim_vars.err_vec; 6 | x_ref = sim_vars.x_ref; 7 | y_ref = sim_vars.y_ref; 8 | 9 | delta_mean_abs = sum(abs(delta_vec))/numel(delta_vec); 10 | cost = sum(abs(err_vec))/numel(err_vec)+delta_mean_abs; 11 | 12 | dist_r2start = sqrt((q_mat(1,end)-x_ref(1))^2+(q_mat(2,end)-y_ref(1))^2); 13 | dist_r2end = sqrt((q_mat(1,end)-x_ref(end))^2+(q_mat(2,end)-y_ref(end))^2); 14 | 15 | if dist_r2start < dist_r2end 16 | cost = 10000 * cost; 17 | end -------------------------------------------------------------------------------- /codes/initialize.m: -------------------------------------------------------------------------------- 1 | function init_params=initialize(user_params) 2 | %% Problem parameters and variables 3 | % System 4 | L = user_params.L; 5 | v = user_params.v; 6 | 7 | % Initial conditions 8 | x0 = user_params.x0; 9 | y0 = user_params.y0; 10 | theta0 = user_params.theta0; 11 | 12 | % Constraints 13 | delta_min = user_params.delta_min; 14 | delta_max = user_params.delta_max; 15 | 16 | % Simulation 17 | dt = 0.05; 18 | t_vec = 0; 19 | 20 | % Initilization 21 | x_ref = linspace(0, 3.28, 328); 22 | y_ref = 0.6*sin(2*x_ref); 23 | q_mat = [x0, y0, theta0]'; 24 | delta_vec = 0; 25 | err_vec = 0; 26 | err_mat = []; 27 | 28 | init_params.L = L; 29 | init_params.v = v; 30 | init_params.delta_min = delta_min; 31 | init_params.delta_max = delta_max; 32 | init_params.dt = dt; 33 | init_params.t_vec = t_vec; 34 | init_params.x_ref = x_ref; 35 | init_params.y_ref = y_ref; 36 | init_params.q_mat = q_mat; 37 | init_params.delta_vec = delta_vec; 38 | init_params.err_vec = err_vec; 39 | init_params.err_mat = err_mat; -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Vehicle-Lateral-Control 2 | ## Problem 3 | In this project, lateral movement of a vehicle (a nonholonomic robot) has been controlled. A harmonic path has been considered as the trajectory of the robot in the 2D space. The robot model is a simple bicycle model which is suitable when the velocity is lower than 15 km/h. The controller is a modified PID. To find the best PID gains, a Gradient Descent Optimization Algorithm was utilized. 4 | 5 | 6 | ## Dynamical system, inputs and other variables 7 |
8 | 9 |
10 | 11 | ## Preview 12 |
13 | 14 |
15 | 16 | ## System input 17 |
18 | 19 |
20 | 21 | ## How to run 22 | 1.Clone the repository. 23 | 24 | 2.Change initial conditions in initialize.m file if you want. 25 | 26 | 3.Run main.m in Matlab. 27 | -------------------------------------------------------------------------------- /codes/sim_plots.m: -------------------------------------------------------------------------------- 1 | function sim_plots(vars, dynamic_plot) 2 | t_vec = vars.t_vec; 3 | q_mat = vars.q_mat; 4 | delta_vec = vars.delta_vec; 5 | err_vec = vars.err_vec; 6 | x_ref = vars.x_ref; 7 | y_ref = vars.y_ref; 8 | L = vars.L; 9 | 10 | figure(1) 11 | plot(x_ref, y_ref, 'g', "LineWidth", 8, "DisplayName", "Reference Path") 12 | hold on 13 | plot(q_mat(1,1), q_mat(2,1), 'pb', "DisplayName", "Start") 14 | plot(q_mat(1,end), q_mat(2,end), 'or', "DisplayName", "End") 15 | plot(q_mat(1,:), q_mat(2,:), "DisplayName", "Vehicle Path") 16 | legend show 17 | axis equal 18 | xlabel("x (m)") 19 | ylabel("y (m)") 20 | title("Static trajectory of vehicle, controlled by PID") 21 | 22 | figure(2) 23 | plot(t_vec, delta_vec*180/pi) 24 | xlabel("t (sec)") 25 | ylabel("\delta (degree)") 26 | title("Input: Steering wheel") 27 | 28 | figure(3) 29 | plot(t_vec, err_vec) 30 | xlabel("t (sec)") 31 | ylabel("Trajectory error (m)") 32 | 33 | % Dynamic plot 34 | if dynamic_plot 35 | for i=1:size(q_mat,2) 36 | figure(4) 37 | plot(x_ref, y_ref, 'g', "LineWidth", 20) 38 | axis equal 39 | hold on 40 | plot([q_mat(1,i),q_mat(1,i)+L*cos(q_mat(3,i))],[q_mat(2,i),... 41 | q_mat(2,i)+L*sin(q_mat(3,i))],... 42 | 'b', "LineWidth",8) 43 | hold off 44 | pause(0.00001) 45 | end 46 | end -------------------------------------------------------------------------------- /codes/simulate_plant.m: -------------------------------------------------------------------------------- 1 | function vars=simulate_plant(coefs, user_params) 2 | init_params=initialize(user_params); 3 | L = init_params.L; 4 | v = init_params.v; 5 | delta_min = init_params.delta_min; 6 | delta_max = init_params.delta_max; 7 | dt = init_params.dt; 8 | t_vec = init_params.t_vec; 9 | x_ref = init_params.x_ref; 10 | y_ref = init_params.y_ref; 11 | q_mat = init_params.q_mat; 12 | delta_vec = init_params.delta_vec; 13 | err_vec = init_params.err_vec; 14 | err_mat = init_params.err_mat; 15 | 16 | params.L = L; 17 | params.v = v; 18 | params.dt = dt; 19 | 20 | % Controller 21 | kp = coefs(1); 22 | kd = coefs(2); 23 | ki = coefs(3); 24 | %% Simulation Loop 25 | i = 1; 26 | while true 27 | [err, nearest_arg]=find_distance(q_mat(1,end), q_mat(2,end),... 28 | x_ref, y_ref); 29 | if nearest_arg == numel(x_ref) 30 | break 31 | end 32 | 33 | % Input (Steering wheel) 34 | coef1 = y_ref(nearest_arg) - q_mat(2,end); 35 | err_pid = [err, (err-err_vec(end))/dt, dt*trapz(err_vec)]'; 36 | delta = coef1*(kp*err_pid(1)+kd*err_pid(2)+ki*err_pid(3)); 37 | 38 | % Constraints 39 | if delta < delta_min 40 | delta = delta_min; 41 | elseif delta > delta_max 42 | delta = delta_max; 43 | end 44 | 45 | % solve state equation 46 | q_new = solver(t_vec(end), q_mat(:, end), delta, params); 47 | 48 | t_vec = [t_vec, t_vec(end)+dt]; 49 | q_mat = [q_mat, q_new]; 50 | delta_vec = [delta_vec, delta]; 51 | err_vec = [err_vec, err]; 52 | err_mat = [err_mat, err_pid]; 53 | 54 | % Uregent Stopping 55 | if i == 10000 56 | break 57 | end 58 | i = i + 1; 59 | end 60 | 61 | vars.t_vec = t_vec; 62 | vars.q_mat = q_mat; 63 | vars.delta_vec = delta_vec; 64 | vars.err_vec = err_vec; 65 | vars.x_ref = x_ref; 66 | vars.y_ref = y_ref; 67 | vars.L = L; -------------------------------------------------------------------------------- /codes/main.m: -------------------------------------------------------------------------------- 1 | %% Project Information 2 | %%% Vehicle Lateral Control 3 | %%% Programmer: Mostafa Lotfi 4 | %%% Date: 5/5/2022 5 | %%% Matlab version: 2021a 6 | %%% Brief description: In this project, lateral movement of a vehicle 7 | %%% (a nonholonomic robot) has been controlled. A harmonic path has been 8 | %%% considered as the trajectory of the robot in the 2D space. 9 | %%% The robot model is a simple bicycle model which is 10 | %%% suitable when the velocity is lower than 15 km/h. The controller is a 11 | %%% modified PID. To find the best PID gains, a Gradient Descent 12 | %%% Optimization Algorithm was utilized. 13 | clc; 14 | clear; 15 | close all; 16 | 17 | %% User parameters 18 | find_best_coefs = false; 19 | default_coefs = [65, 71, 2]; % [65, 71, 2] is a good set 20 | dynamic_plot = true; 21 | 22 | L = 0.2; % Rear and front axes distances 23 | v = 0.1; % Longitudinal velocities 24 | x0 = 0.5; % Initial position in horizontal direction 25 | y0 = 0; % Initial position in vertical direction 26 | theta0 = 0; % Initial robot head angle 27 | delta_min = -pi/4; % Lower bound for steering wheel 28 | delta_max = pi/4; % Upper bound for steering whell 29 | 30 | % Gathering data 31 | user_params.x0 = x0; 32 | user_params.y0 = y0; 33 | user_params.theta0 = theta0; 34 | user_params.L = L; 35 | user_params.v = v; 36 | user_params.delta_min = delta_min; 37 | user_params.delta_max = delta_max; 38 | 39 | if find_best_coefs 40 | %% Start of optimization 41 | % Objective function and constraints 42 | lb_val = -10; % Lower bound for coefficients 43 | ub_val = 100; % Upper bound for coefficients 44 | lb = ones(1, 3)*lb_val; 45 | ub=ones(1, 3)*ub_val; 46 | nvar=numel(lb); 47 | %% Optimization 48 | k0=lb+rand(1,nvar).*(ub-lb); 49 | final_obj_func = @(k) obj_fcn(k, user_params); 50 | options=optimoptions('fmincon','Display','iter','Algorithm',... 51 | 'sqp','MaxIterations',10); 52 | coefs=fmincon(final_obj_func,k0,[],[],[],[],lb,ub,[],options); 53 | else 54 | coefs = default_coefs; 55 | end 56 | 57 | %% Showing results 58 | disp('controller coefficients: ') 59 | disp(coefs) 60 | 61 | vars = simulate_plant(coefs, user_params); 62 | sim_plots(vars, dynamic_plot) -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Apache License 2 | Version 2.0, January 2004 3 | http://www.apache.org/licenses/ 4 | 5 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 6 | 7 | 1. Definitions. 8 | 9 | "License" shall mean the terms and conditions for use, reproduction, 10 | and distribution as defined by Sections 1 through 9 of this document. 11 | 12 | "Licensor" shall mean the copyright owner or entity authorized by 13 | the copyright owner that is granting the License. 14 | 15 | "Legal Entity" shall mean the union of the acting entity and all 16 | other entities that control, are controlled by, or are under common 17 | control with that entity. For the purposes of this definition, 18 | "control" means (i) the power, direct or indirect, to cause the 19 | direction or management of such entity, whether by contract or 20 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 21 | outstanding shares, or (iii) beneficial ownership of such entity. 22 | 23 | "You" (or "Your") shall mean an individual or Legal Entity 24 | exercising permissions granted by this License. 25 | 26 | "Source" form shall mean the preferred form for making modifications, 27 | including but not limited to software source code, documentation 28 | source, and configuration files. 29 | 30 | "Object" form shall mean any form resulting from mechanical 31 | transformation or translation of a Source form, including but 32 | not limited to compiled object code, generated documentation, 33 | and conversions to other media types. 34 | 35 | "Work" shall mean the work of authorship, whether in Source or 36 | Object form, made available under the License, as indicated by a 37 | copyright notice that is included in or attached to the work 38 | (an example is provided in the Appendix below). 39 | 40 | "Derivative Works" shall mean any work, whether in Source or Object 41 | form, that is based on (or derived from) the Work and for which the 42 | editorial revisions, annotations, elaborations, or other modifications 43 | represent, as a whole, an original work of authorship. For the purposes 44 | of this License, Derivative Works shall not include works that remain 45 | separable from, or merely link (or bind by name) to the interfaces of, 46 | the Work and Derivative Works thereof. 47 | 48 | "Contribution" shall mean any work of authorship, including 49 | the original version of the Work and any modifications or additions 50 | to that Work or Derivative Works thereof, that is intentionally 51 | submitted to Licensor for inclusion in the Work by the copyright owner 52 | or by an individual or Legal Entity authorized to submit on behalf of 53 | the copyright owner. For the purposes of this definition, "submitted" 54 | means any form of electronic, verbal, or written communication sent 55 | to the Licensor or its representatives, including but not limited to 56 | communication on electronic mailing lists, source code control systems, 57 | and issue tracking systems that are managed by, or on behalf of, the 58 | Licensor for the purpose of discussing and improving the Work, but 59 | excluding communication that is conspicuously marked or otherwise 60 | designated in writing by the copyright owner as "Not a Contribution." 61 | 62 | "Contributor" shall mean Licensor and any individual or Legal Entity 63 | on behalf of whom a Contribution has been received by Licensor and 64 | subsequently incorporated within the Work. 65 | 66 | 2. Grant of Copyright License. Subject to the terms and conditions of 67 | this License, each Contributor hereby grants to You a perpetual, 68 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 69 | copyright license to reproduce, prepare Derivative Works of, 70 | publicly display, publicly perform, sublicense, and distribute the 71 | Work and such Derivative Works in Source or Object form. 72 | 73 | 3. Grant of Patent License. Subject to the terms and conditions of 74 | this License, each Contributor hereby grants to You a perpetual, 75 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 76 | (except as stated in this section) patent license to make, have made, 77 | use, offer to sell, sell, import, and otherwise transfer the Work, 78 | where such license applies only to those patent claims licensable 79 | by such Contributor that are necessarily infringed by their 80 | Contribution(s) alone or by combination of their Contribution(s) 81 | with the Work to which such Contribution(s) was submitted. If You 82 | institute patent litigation against any entity (including a 83 | cross-claim or counterclaim in a lawsuit) alleging that the Work 84 | or a Contribution incorporated within the Work constitutes direct 85 | or contributory patent infringement, then any patent licenses 86 | granted to You under this License for that Work shall terminate 87 | as of the date such litigation is filed. 88 | 89 | 4. Redistribution. You may reproduce and distribute copies of the 90 | Work or Derivative Works thereof in any medium, with or without 91 | modifications, and in Source or Object form, provided that You 92 | meet the following conditions: 93 | 94 | (a) You must give any other recipients of the Work or 95 | Derivative Works a copy of this License; and 96 | 97 | (b) You must cause any modified files to carry prominent notices 98 | stating that You changed the files; and 99 | 100 | (c) You must retain, in the Source form of any Derivative Works 101 | that You distribute, all copyright, patent, trademark, and 102 | attribution notices from the Source form of the Work, 103 | excluding those notices that do not pertain to any part of 104 | the Derivative Works; and 105 | 106 | (d) If the Work includes a "NOTICE" text file as part of its 107 | distribution, then any Derivative Works that You distribute must 108 | include a readable copy of the attribution notices contained 109 | within such NOTICE file, excluding those notices that do not 110 | pertain to any part of the Derivative Works, in at least one 111 | of the following places: within a NOTICE text file distributed 112 | as part of the Derivative Works; within the Source form or 113 | documentation, if provided along with the Derivative Works; or, 114 | within a display generated by the Derivative Works, if and 115 | wherever such third-party notices normally appear. The contents 116 | of the NOTICE file are for informational purposes only and 117 | do not modify the License. You may add Your own attribution 118 | notices within Derivative Works that You distribute, alongside 119 | or as an addendum to the NOTICE text from the Work, provided 120 | that such additional attribution notices cannot be construed 121 | as modifying the License. 122 | 123 | You may add Your own copyright statement to Your modifications and 124 | may provide additional or different license terms and conditions 125 | for use, reproduction, or distribution of Your modifications, or 126 | for any such Derivative Works as a whole, provided Your use, 127 | reproduction, and distribution of the Work otherwise complies with 128 | the conditions stated in this License. 129 | 130 | 5. Submission of Contributions. Unless You explicitly state otherwise, 131 | any Contribution intentionally submitted for inclusion in the Work 132 | by You to the Licensor shall be under the terms and conditions of 133 | this License, without any additional terms or conditions. 134 | Notwithstanding the above, nothing herein shall supersede or modify 135 | the terms of any separate license agreement you may have executed 136 | with Licensor regarding such Contributions. 137 | 138 | 6. Trademarks. This License does not grant permission to use the trade 139 | names, trademarks, service marks, or product names of the Licensor, 140 | except as required for reasonable and customary use in describing the 141 | origin of the Work and reproducing the content of the NOTICE file. 142 | 143 | 7. Disclaimer of Warranty. Unless required by applicable law or 144 | agreed to in writing, Licensor provides the Work (and each 145 | Contributor provides its Contributions) on an "AS IS" BASIS, 146 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 147 | implied, including, without limitation, any warranties or conditions 148 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 149 | PARTICULAR PURPOSE. You are solely responsible for determining the 150 | appropriateness of using or redistributing the Work and assume any 151 | risks associated with Your exercise of permissions under this License. 152 | 153 | 8. Limitation of Liability. In no event and under no legal theory, 154 | whether in tort (including negligence), contract, or otherwise, 155 | unless required by applicable law (such as deliberate and grossly 156 | negligent acts) or agreed to in writing, shall any Contributor be 157 | liable to You for damages, including any direct, indirect, special, 158 | incidental, or consequential damages of any character arising as a 159 | result of this License or out of the use or inability to use the 160 | Work (including but not limited to damages for loss of goodwill, 161 | work stoppage, computer failure or malfunction, or any and all 162 | other commercial damages or losses), even if such Contributor 163 | has been advised of the possibility of such damages. 164 | 165 | 9. Accepting Warranty or Additional Liability. While redistributing 166 | the Work or Derivative Works thereof, You may choose to offer, 167 | and charge a fee for, acceptance of support, warranty, indemnity, 168 | or other liability obligations and/or rights consistent with this 169 | License. However, in accepting such obligations, You may act only 170 | on Your own behalf and on Your sole responsibility, not on behalf 171 | of any other Contributor, and only if You agree to indemnify, 172 | defend, and hold each Contributor harmless for any liability 173 | incurred by, or claims asserted against, such Contributor by reason 174 | of your accepting any such warranty or additional liability. 175 | 176 | END OF TERMS AND CONDITIONS 177 | 178 | APPENDIX: How to apply the Apache License to your work. 179 | 180 | To apply the Apache License to your work, attach the following 181 | boilerplate notice, with the fields enclosed by brackets "[]" 182 | replaced with your own identifying information. (Don't include 183 | the brackets!) The text should be enclosed in the appropriate 184 | comment syntax for the file format. We also recommend that a 185 | file or class name and description of purpose be included on the 186 | same "printed page" as the copyright notice for easier 187 | identification within third-party archives. 188 | 189 | Copyright [yyyy] [name of copyright owner] 190 | 191 | Licensed under the Apache License, Version 2.0 (the "License"); 192 | you may not use this file except in compliance with the License. 193 | You may obtain a copy of the License at 194 | 195 | http://www.apache.org/licenses/LICENSE-2.0 196 | 197 | Unless required by applicable law or agreed to in writing, software 198 | distributed under the License is distributed on an "AS IS" BASIS, 199 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 200 | See the License for the specific language governing permissions and 201 | limitations under the License. 202 | --------------------------------------------------------------------------------