├── .gitattributes ├── .travis.yml ├── Figures ├── Algorithm wheel_move.jpg ├── Algorithm.jpg ├── Avon_slipAngle_dataGraph.JPG ├── Car_model_angles.JPG ├── DLC_test_output.png ├── FIgure_38.JPG ├── FSRA16_front.jpeg ├── Figure_34.png ├── Figure_34_noControl.png ├── Figure_38.PNG ├── Figure_39.PNG ├── Figure_40.PNG ├── Figure_41_right.PNG ├── Figure_48.PNG ├── Figure_49.PNG ├── Figure_50.PNG ├── Figure_54.PNG ├── Figure_56.png ├── Figure_57_down.PNG ├── Figure_57_up.PNG ├── Figure_58_right.PNG ├── Load_Trasnfer_FSRA16_DLC.JPG ├── Read_Me.md ├── Vehicle_angles_newFont.png ├── Vehicle_angles_preFinal.jpg └── slip angle.png ├── LICENSE ├── Matlab ├── DoubleLaneChangeTest.m ├── READ_ME.md ├── doubleLaneChange.m ├── generateOrientation.m ├── plotData.m ├── tire_Avon.mat ├── userDefinedTest.m ├── v07.m ├── wheel_move.m └── wheel_speeds.mat ├── README.md └── Модел кретања спортског возила са електро погоном на свим точковима Torque Vectoring.pdf /.gitattributes: -------------------------------------------------------------------------------- 1 | # Auto detect text files and perform LF normalization 2 | * text=auto 3 | -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | # We pretend to be Java because we need GNU Octave which is not 2 | # available (as of January 2016) 3 | language: java 4 | 5 | ############################################################################### 6 | cache: 7 | # Downloading octave takes a while, so let's cache apt 8 | apt: true 9 | directories: 10 | # Cache octave packages 11 | - $HOME/octave 12 | # Cache other packages 13 | - $HOME/external_cache 14 | 15 | ############################################################################### 16 | env: 17 | matrix: 18 | - COVERAGE="false" 19 | - COVERAGE="true" 20 | 21 | matrix: 22 | allow_failures: 23 | - env: COVERAGE="true" 24 | fast_finish: true 25 | 26 | ############################################################################### 27 | # Command to install dependencies 28 | before_install: 29 | # Remember the directory where our repository to test is located 30 | - REPOPATH="$(pwd)" && pwd 31 | # --------------------------------------------------------------------------- 32 | # Check whether we need to upgrade the gcc and g++ versions. We do need to if 33 | # the version is less the 4.8 and we need to install the image package from 34 | # Octave Forge (possibly for other packages as well). 35 | # For now, lets just always say we do need to do this. 36 | - UPGRADE_GCC="true" 37 | # --------------------------------------------------------------------------- 38 | # Add repository for octave 39 | - travis_retry sudo add-apt-repository -y ppa:octave/stable 40 | # Add repository for installing g++-4.8 on Ubuntu 12.04 41 | - if [ "$UPGRADE_GCC" = "true" ]; then 42 | sudo add-apt-repository -y ppa:ubuntu-toolchain-r/test; 43 | fi 44 | # Update apt-get to include the contents from the new locations 45 | - travis_retry sudo apt-get update -qq 46 | # Install octave with apt-get 47 | - travis_retry sudo apt-get install -y octave liboctave-dev 48 | # Add a C++11 compiler so we can install image package 49 | - if [ "$UPGRADE_GCC" = "true" ]; then 50 | travis_retry sudo apt-get install -y gcc-4.8 g++-4.8; 51 | sudo update-alternatives --install /usr/bin/gcc gcc /usr/bin/gcc-4.8 50; 52 | sudo update-alternatives --install /usr/bin/g++ g++ /usr/bin/g++-4.8 50; 53 | fi; 54 | # Check which versions of g++ and gcc we are using 55 | - which g++; 56 | g++ --version; 57 | which gcc; 58 | gcc --version; 59 | # --------------------------------------------------------------------------- 60 | # Go up one level and retrieve MOxUnit from its repository 61 | - cd ..; 62 | ls -alh; 63 | # Remove directory if it is already there from previous build 64 | - rm -rf MOxUnit; 65 | - git clone https://github.com/MOxUnit/MOxUnit.git 66 | # Install MOxUnit, which adds itself to the startup path 67 | - make -C MOxUnit install 68 | # Install MOcov, which adds itself to the startup path 69 | - if [ "$COVERAGE" = "true" ]; then 70 | rm -rf MOcov; 71 | git clone https://github.com/MOcov/MOcov.git; 72 | make -C MOcov install; 73 | fi; 74 | # Go back to the repository directory 75 | - cd ${REPOPATH} 76 | 77 | ############################################################################### 78 | install: 79 | # Ensure external packages folder exists and is empty 80 | - rm -rf external; 81 | mkdir -p external; 82 | # Install required packages for Octave 83 | - if [ -f requirements-octave.txt ]; then 84 | mopi/mopi.sh requirements-octave.txt external "$HOME/external_cache"; 85 | fi; 86 | # Install other required packages 87 | - if [ -f requirements.txt ]; then 88 | mopi/mopi.sh requirements.txt external "$HOME/external_cache"; 89 | fi; 90 | # Install developmental requirements 91 | - if [ -f requirements-dev.txt ]; then 92 | mopi/mopi.sh requirements-dev.txt external "$HOME/external_cache"; 93 | fi; 94 | # Before running the tests, we will have to add our package and our 95 | # external dependencies to the octave path 96 | - PACKAGE_FOLDER="package_name"; 97 | ADDPATH_COMMAND="addpath(genpath(fullfile(pwd, '$PACKAGE_FOLDER')));"; 98 | ADDPATH_COMMAND+=" addpath(genpath(fullfile(pwd, 'external')));"; 99 | echo "ADDPATH_COMMAND| $ADDPATH_COMMAND"; 100 | 101 | ############################################################################### 102 | before_script: 103 | - TEST_ARGS="'-recursive', '-verbose', '-junit_xml_file', 'testresults.xml'"; 104 | if [ "$COVERAGE" = "true" ]; then 105 | TEST_ARGS+=", '-with_coverage', '-cover', '$PACKAGE_FOLDER'"; 106 | TEST_ARGS+=", '-cover_exclude', 'tests'"; 107 | TEST_ARGS+=", '-cover_xml_file', 'coverage.xml'"; 108 | TEST_ARGS+=", '-cover_json_file', 'coveralls.json'"; 109 | fi; 110 | TEST_COMMAND="exit(~moxunit_runtests('tests', $TEST_ARGS));"; 111 | echo "TEST_COMMAND| $TEST_COMMAND"; 112 | # Double-check we are still in the right directory 113 | - pwd 114 | # Check what octave packages we have installed 115 | -octave -q --eval "ver" 116 | # --------------------------------------------------------------------------- 117 | # Remove any cached results files from previous build, if present 118 | - rm -f testresults.xml; 119 | rm -f coverage.xml; 120 | rm -f .coverage; 121 | rm -f coveralls.json; 122 | # --------------------------------------------------------------------------- 123 | # Set up folders for test results on Shippable 124 | - if [ "$SHIPPABLE" = "true" ]; then 125 | rm -fr shippable; 126 | mkdir -p shippable/testresults; 127 | mkdir -p shippable/codecoverage; 128 | fi; 129 | 130 | ############################################################################### 131 | script: 132 | - octave -q --eval "$ADDPATH_COMMAND $TEST_COMMAND"; 133 | 134 | ############################################################################### 135 | after_script: 136 | # Check where we ended up and what's going on where we are 137 | - pwd 138 | - ls -alh 139 | # --------------------------------------------------------------------------- 140 | # Move results and coverage files into appropriate places 141 | - if [ "$SHIPPABLE" = "true" ] && [ -f testresults.xml ]; then 142 | mv testresults.xml shippable/testresults/; 143 | fi; 144 | if [ "$SHIPPABLE" = "true" ] && [ -f coverage.xml ]; then 145 | cp coverage.xml shippable/codecoverage/; 146 | fi; 147 | 148 | ############################################################################### 149 | after_success: 150 | # Only run coveralls on Travis. When running on a public Travis-CI, the 151 | # repo token is automatically inferred, but to run coveralls on Shippable 152 | # the repo token needs to be specified in a .coveralls.yml or as an 153 | # environment variable COVERALLS_REPO_TOKEN. This should be kept hidden 154 | # from public viewing, either by encrypting the token or running on a 155 | # private build. 156 | # We ignore coverage push failures because the servers are not 100% 157 | # reliable and we don't want the CI to report a failure just because the 158 | # coverage report wasn't published. 159 | # For Codecov, we use this https://github.com/codecov/codecov-bash 160 | - if [ "$COVERAGE" = "true" ] && [ "$TRAVIS" = "true" ] && [ "$SHIPPABLE" != "true" ]; then 161 | curl --verbose -F json_file=@`pwd`/coveralls.json https://coveralls.io/api/v1/jobs || echo "Coveralls push failed"; 162 | bash <(curl -s https://codecov.io/bash) || echo "Codecov push failed"; 163 | fi; 164 | 165 | ############################################################################### 166 | # Enable archiving of artifacts on Shippable (does nothing on Travis) 167 | archive: true 168 | -------------------------------------------------------------------------------- /Figures/Algorithm wheel_move.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Tepic/Vehicle_Model/8dd5c9dee51ae082fd61abec54cda2c15d91eb9c/Figures/Algorithm wheel_move.jpg -------------------------------------------------------------------------------- /Figures/Algorithm.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Tepic/Vehicle_Model/8dd5c9dee51ae082fd61abec54cda2c15d91eb9c/Figures/Algorithm.jpg -------------------------------------------------------------------------------- /Figures/Avon_slipAngle_dataGraph.JPG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Tepic/Vehicle_Model/8dd5c9dee51ae082fd61abec54cda2c15d91eb9c/Figures/Avon_slipAngle_dataGraph.JPG -------------------------------------------------------------------------------- /Figures/Car_model_angles.JPG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Tepic/Vehicle_Model/8dd5c9dee51ae082fd61abec54cda2c15d91eb9c/Figures/Car_model_angles.JPG -------------------------------------------------------------------------------- /Figures/DLC_test_output.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Tepic/Vehicle_Model/8dd5c9dee51ae082fd61abec54cda2c15d91eb9c/Figures/DLC_test_output.png -------------------------------------------------------------------------------- /Figures/FIgure_38.JPG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Tepic/Vehicle_Model/8dd5c9dee51ae082fd61abec54cda2c15d91eb9c/Figures/FIgure_38.JPG -------------------------------------------------------------------------------- /Figures/FSRA16_front.jpeg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Tepic/Vehicle_Model/8dd5c9dee51ae082fd61abec54cda2c15d91eb9c/Figures/FSRA16_front.jpeg -------------------------------------------------------------------------------- /Figures/Figure_34.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Tepic/Vehicle_Model/8dd5c9dee51ae082fd61abec54cda2c15d91eb9c/Figures/Figure_34.png -------------------------------------------------------------------------------- /Figures/Figure_34_noControl.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Tepic/Vehicle_Model/8dd5c9dee51ae082fd61abec54cda2c15d91eb9c/Figures/Figure_34_noControl.png -------------------------------------------------------------------------------- /Figures/Figure_38.PNG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Tepic/Vehicle_Model/8dd5c9dee51ae082fd61abec54cda2c15d91eb9c/Figures/Figure_38.PNG -------------------------------------------------------------------------------- /Figures/Figure_39.PNG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Tepic/Vehicle_Model/8dd5c9dee51ae082fd61abec54cda2c15d91eb9c/Figures/Figure_39.PNG -------------------------------------------------------------------------------- /Figures/Figure_40.PNG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Tepic/Vehicle_Model/8dd5c9dee51ae082fd61abec54cda2c15d91eb9c/Figures/Figure_40.PNG -------------------------------------------------------------------------------- /Figures/Figure_41_right.PNG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Tepic/Vehicle_Model/8dd5c9dee51ae082fd61abec54cda2c15d91eb9c/Figures/Figure_41_right.PNG -------------------------------------------------------------------------------- /Figures/Figure_48.PNG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Tepic/Vehicle_Model/8dd5c9dee51ae082fd61abec54cda2c15d91eb9c/Figures/Figure_48.PNG -------------------------------------------------------------------------------- /Figures/Figure_49.PNG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Tepic/Vehicle_Model/8dd5c9dee51ae082fd61abec54cda2c15d91eb9c/Figures/Figure_49.PNG -------------------------------------------------------------------------------- /Figures/Figure_50.PNG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Tepic/Vehicle_Model/8dd5c9dee51ae082fd61abec54cda2c15d91eb9c/Figures/Figure_50.PNG -------------------------------------------------------------------------------- /Figures/Figure_54.PNG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Tepic/Vehicle_Model/8dd5c9dee51ae082fd61abec54cda2c15d91eb9c/Figures/Figure_54.PNG -------------------------------------------------------------------------------- /Figures/Figure_56.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Tepic/Vehicle_Model/8dd5c9dee51ae082fd61abec54cda2c15d91eb9c/Figures/Figure_56.png -------------------------------------------------------------------------------- /Figures/Figure_57_down.PNG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Tepic/Vehicle_Model/8dd5c9dee51ae082fd61abec54cda2c15d91eb9c/Figures/Figure_57_down.PNG -------------------------------------------------------------------------------- /Figures/Figure_57_up.PNG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Tepic/Vehicle_Model/8dd5c9dee51ae082fd61abec54cda2c15d91eb9c/Figures/Figure_57_up.PNG -------------------------------------------------------------------------------- /Figures/Figure_58_right.PNG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Tepic/Vehicle_Model/8dd5c9dee51ae082fd61abec54cda2c15d91eb9c/Figures/Figure_58_right.PNG -------------------------------------------------------------------------------- /Figures/Load_Trasnfer_FSRA16_DLC.JPG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Tepic/Vehicle_Model/8dd5c9dee51ae082fd61abec54cda2c15d91eb9c/Figures/Load_Trasnfer_FSRA16_DLC.JPG -------------------------------------------------------------------------------- /Figures/Read_Me.md: -------------------------------------------------------------------------------- 1 | #Figures 2 | 3 | In this folder are placed figures as snapshots of simulation outpus 4 | -------------------------------------------------------------------------------- /Figures/Vehicle_angles_newFont.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Tepic/Vehicle_Model/8dd5c9dee51ae082fd61abec54cda2c15d91eb9c/Figures/Vehicle_angles_newFont.png -------------------------------------------------------------------------------- /Figures/Vehicle_angles_preFinal.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Tepic/Vehicle_Model/8dd5c9dee51ae082fd61abec54cda2c15d91eb9c/Figures/Vehicle_angles_preFinal.jpg -------------------------------------------------------------------------------- /Figures/slip angle.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Tepic/Vehicle_Model/8dd5c9dee51ae082fd61abec54cda2c15d91eb9c/Figures/slip angle.png -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2018 Tepic 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. -------------------------------------------------------------------------------- /Matlab/DoubleLaneChangeTest.m: -------------------------------------------------------------------------------- 1 | % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % 2 | % 3 | % Copyright (c) * 4 | % All rights reserved by Milan Tepic 5 | % 6 | % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % 7 | % 8 | % Filename: DoubleLaneChangeTest.m 9 | % 10 | % 11 | % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % 12 | % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % 13 | % 14 | % 15 | % DESCRIPTION 16 | % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % 17 | % 18 | % A test programe before final doubleLaneChange function is developed according to this *main* program of single predefined double lane change 19 | % 20 | % 21 | % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % 22 | % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % 23 | % 24 | % 25 | % HISTORY 26 | % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % 27 | % Version: 1.7 28 | % Author/Date: Milan Tepic / 2017-02-07 29 | % Change: Initial version 30 | % 31 | % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % 32 | % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % 33 | 34 | clear all 35 | close all 36 | clc 37 | 38 | % load('wheel_speeds.mat'); 39 | 40 | % SIMULATION PARAMETERS 41 | g = 9.80037; % [m/s^2] 42 | SIM_TIME = 3.5; % [s] 43 | dt = 1e-3; % [s] 44 | 45 | % PI CONTROLER PARAMETERS 46 | Kp = 5; 47 | Ti = 0.08; 48 | interrupt = 1e-3; % [s] 49 | 50 | % VEHICLE PARAMETERS 51 | L = 1600*1e-3; % [mm] 52 | T_front = 1250*1e-3; % [mm] 53 | T_rear = 1200*1e-3; % [mm] 54 | CG_height = 340*1e-3; % [mm] 55 | a = 830*1e-3; % [mm] 56 | b = 770*1e-3; % [mm] 57 | 58 | unsprung_mass = 70; 59 | sprung_mass = 174; 60 | driver_mass = 75; 61 | mass = sprung_mass+... 62 | unsprung_mass+... 63 | driver_mass; % [kg] 64 | m1_s = mass*(b/L)/2; % [kg] 65 | m2_s = mass*(b/L)/2; % [kg] 66 | m3_s = mass*(a/L)/2; % [kg] 67 | m4_s = mass*(a/L)/2; % [kg] 68 | max_steer = 135; % [deg] 69 | max_steerSpeed = 540; % [deg/s] 70 | max_Dsteer = max_steerSpeed*dt; 71 | 72 | % ENGINE PARAMETERS 73 | max_speed = 135; 74 | accel_max = 10*dt; % [m/s^2 * dt] 75 | speed_max = 137.8; % [kmh] 76 | brake_max = 20*dt; % [m/s^2 * dt] 77 | 78 | % TYRE PARAMETERS 79 | load('tire_Avon.mat'); 80 | 81 | sliped = -tire_Avon(:,2); 82 | lateralLoaded = tire_Avon(:,3).*1e3; 83 | 84 | slipAngle = -12:1e-3:12; 85 | lateralLoad = interp1(sliped,lateralLoaded,slipAngle,'pchip'); 86 | C_alpha = (2.7778/2)*1e-03; % [N/deg] 87 | WHEEL_DIAMETER = 0.33/2; 88 | 89 | beta_FL = 0; 90 | beta_FR = 0; 91 | beta_RL = 0; 92 | beta_RR = 0; 93 | 94 | %% DOUBLE LANE CHANGE TEST 95 | speed = 80; 96 | steer = 0; 97 | psi = 0; 98 | 99 | [orientationVector timeVector] = generateOrientation(0,0,0,0.32,dt,'step'); 100 | [orientationVector timeVector] = ... 101 | generateOrientation(orientationVector,timeVector,17,0.55,dt,'ramp'); 102 | [orientationVector timeVector] = ... 103 | generateOrientation(orientationVector,timeVector,17,0.08,dt,'step'); 104 | [orientationVector timeVector] = ... 105 | generateOrientation(orientationVector,timeVector,0,0.55,dt,'ramp'); 106 | [orientationVector timeVector] = ... 107 | generateOrientation(orientationVector,timeVector,0,0.05,dt,'step'); 108 | [orientationVector timeVector] = ... 109 | generateOrientation(orientationVector,timeVector,-14,0.55,dt,'ramp'); 110 | [orientationVector timeVector] = ... 111 | generateOrientation(orientationVector,timeVector,-14,0.05,dt,'step'); 112 | [orientationVector timeVector] = ... 113 | generateOrientation(orientationVector,timeVector,0,0.55,dt,'ramp'); 114 | [orientationVector timeVector] = ... 115 | generateOrientation(orientationVector,timeVector,0,.45,dt,'step'); 116 | 117 | m1 = m1_s; m2 = m2_s; m3 = m3_s; m4 = m4_s; 118 | 119 | vehicle_Params = [L T_front T_rear CG_height a b mass m1_s m2_s m3_s m4_s psi]; 120 | sym_Params = [steer(1) speed max_steer max_speed C_alpha g dt]; 121 | tire_Params = [beta_FL beta_FR beta_RL beta_RR]; 122 | wheel_location = [-a,0]; 123 | 124 | % INIT START LOCATION 125 | x_CG(1) = 0; 126 | y_CG(1) = 0; 127 | 128 | x_CC(1) = -b*cos(psi(1)); 129 | y_CC(1) = -b*sin(psi(1)); 130 | 131 | x_FR(1) = x_CG(1)+sqrt(a^2+(T_front/2)^2)*cos(psi(1)-atan((T_front/2)/a)); 132 | y_FR(1) = y_CG(1)+sqrt(a^2+(T_front/2)^2)*sin(psi(1)-atan((T_front/2)/a)); 133 | 134 | x_RR(1) = x_CG(1)-b*cos(psi(1))+(T_rear/2)*sin(psi(1)); 135 | y_RR(1) = y_CG(1)-b*sin(psi(1))-(T_rear/2)*cos(psi(1)); 136 | 137 | x_FL(1) = x_FR(1)-T_front*sin(psi(1)); 138 | y_FL(1) = y_FR(1)+T_front*cos(psi(1)); 139 | 140 | x_RL(1) = x_RR(1)-T_rear*sin(psi(1)); 141 | y_RL(1) = y_RR(1)+T_rear*cos(psi(1)); 142 | 143 | d_psi = 0; sigma_CG = 0; 144 | 145 | m1 = m1_s; m2 = m2_s; 146 | m3 = m3_s; m4 = m4_s; 147 | 148 | dm_longitudinal = [0; 0]; 149 | dm_lateral = [0; 0]; 150 | 151 | FL_speed = 0; 152 | FR_speed = 0; 153 | RL_speed = 0; 154 | RR_speed = 0; 155 | wheel_speeds = [FL_speed' FR_speed' RL_speed' RR_speed']; 156 | 157 | acp_CG = 0; acp_psi = 0; 158 | 159 | time = 0; 160 | e = []; 161 | u = []; 162 | lastState = 0; 163 | for position = 1:length(timeVector) 164 | if((mod(timeVector(position),interrupt))==0) 165 | e(position) = round((orientationVector(position)-psi(position)*180/pi)*1e4)/1e4; 166 | u(position) = Kp*e(position)+Kp*dt*e(position)/Ti; 167 | 168 | if(position>1) 169 | if(u(position)>0 && u(position-1)>0 &&... 170 | u(position)-u(position-1)>max_Dsteer) 171 | 172 | u(position) = u(position-1)+max_Dsteer; 173 | else if(u(position)>0 && u(position-1)>0 &&... 174 | u(position)-u(position-1)<-max_Dsteer) 175 | u(position) = u(position-1)-max_Dsteer; 176 | end 177 | end 178 | if(u(position)<0 && u(position-1)<0 &&... 179 | u(position)-u(position-1)<-max_Dsteer) 180 | 181 | u(position) = u(position-1)-max_Dsteer; 182 | else if(u(position)<0 && u(position-1)<0 &&... 183 | u(position)-u(position-1)>max_Dsteer) 184 | u(position) = u(position-1)+max_Dsteer; 185 | end 186 | end 187 | 188 | if(u(position)>0 && u(position-1)<0 &&... 189 | u(position)-u(position-1)>max_Dsteer) 190 | 191 | u(position) = u(position-1)+max_Dsteer; 192 | else if(u(position)<0 && u(position-1)>0 &&... 193 | u(position)-u(position-1)<-max_Dsteer) 194 | u(position) = u(position-1)-max_Dsteer; 195 | end 196 | end 197 | else 198 | if(u(position)>max_Dsteer) 199 | u(position) = max_Dsteer; 200 | else if(u(position)<-max_Dsteer) 201 | u(position) = -max_Dsteer; 202 | end 203 | end 204 | end 205 | 206 | if(u(position)>135) 207 | u(position) = 135; 208 | else if(u(position)<-135) 209 | u(position) = -135; 210 | end 211 | end 212 | else 213 | e(position) = e(position-1); 214 | u(position) = Kp*e(position)+Kp*dt*e(position)/Ti; 215 | end 216 | steer = [steer; u(position)]; 217 | 218 | vehicle_Params = [L T_front T_rear CG_height a b mass m1(position) m2(position) m3(position) m4(position) psi(position)]; 219 | sym_Params = [steer(position) speed(1) max_steer max_speed C_alpha g dt]; 220 | tire_Params = [beta_FL(position) beta_FR(position) beta_RL(position) beta_RR(position)]; 221 | wheel_location = [x_CG(position) y_CG(position)... 222 | x_FR(position) y_FR(position)... 223 | x_RR(position) y_RR(position)... 224 | x_FL(position) y_FL(position)... 225 | x_RL(position) y_RL(position)]; 226 | [output lastState] = wheel_move(wheel_speeds,vehicle_Params,sym_Params,tire_Params,wheel_location,slipAngle,lateralLoad,lastState); 227 | 228 | x_FL = [x_FL; output(2,1)]; y_FL = [y_FL; output(2,7)]; 229 | x_FR = [x_FR; output(2,2)]; y_FR = [y_FR; output(2,8)]; 230 | x_RL = [x_RL; output(2,3)]; y_RL = [y_RL; output(2,9)]; 231 | x_RR = [x_RR; output(2,4)]; y_RR = [y_RR; output(2,10)]; 232 | x_CG = [x_CG; output(2,5)]; y_CG = [y_CG; output(2,11)]; 233 | x_CC = [x_CC; output(2,6)]; y_CC = [y_CC; output(2,12)]; 234 | 235 | psi = [psi; output(2,13)]; d_psi = [d_psi; output(2,14)]; 236 | 237 | beta_FL = [beta_FL; output(2,15)]; 238 | beta_FR = [beta_FR; output(2,16)]; 239 | beta_RL = [beta_RL; output(2,17)]; 240 | beta_RR = [beta_RR; output(2,18)]; 241 | 242 | sigma_CG = [sigma_CG; output(2,19)]; %time = [time; output(2,20)]; 243 | 244 | m1 = [m1; output(2,21)]; m2 = [m2; output(2,22)]; 245 | m3 = [m3; output(2,23)]; m4 = [m4; output(2,24)]; 246 | dm_longitudinal = [dm_longitudinal output(:,27)]; 247 | dm_lateral = [dm_lateral -output(:,28)]; 248 | 249 | FL_speed = [FL_speed output(2,29)/WHEEL_DIAMETER]; 250 | FR_speed = [FR_speed output(2,30)/WHEEL_DIAMETER]; 251 | RL_speed = [RL_speed output(2,31)/WHEEL_DIAMETER]; 252 | RR_speed = [RR_speed output(2,32)/WHEEL_DIAMETER]; 253 | wheel_speeds = [FL_speed(end) FR_speed(end) RL_speed(end) RR_speed(end)]; 254 | 255 | acp_CG = [acp_CG; output(2,25)]; acp_psi = [acp_psi; output(2,26)]; 256 | time = [time time(end)+dt]; 257 | end 258 | 259 | wheel_speeds = [FL_speed' FR_speed' RL_speed' RR_speed']; 260 | 261 | x = [x_FL x_FR x_RL x_RR x_CG x_CC]; 262 | y = [y_FL y_FR y_RL y_RR y_CG y_CC]; 263 | 264 | m = [m1 m2 m3 m4 mass.*ones(length(time),1) dm_lateral' dm_longitudinal']; % size = length(time) x (4+1+2+2) 265 | beta = [beta_FL beta_FR beta_RL beta_RR]; 266 | 267 | vehicle_Params = [L T_front T_rear CG_height a b g speed dt]; 268 | tire_Params = [C_alpha WHEEL_DIAMETER]; 269 | 270 | orientationVector = [orientationVector; orientationVector(end)]; 271 | timeVector = [timeVector; timeVector(end)]; 272 | 273 | data = [x y m beta wheel_speeds psi d_psi acp_psi acp_CG sigma_CG steer time' orientationVector timeVector]; 274 | disp(['Total time for ISO-standard test: ', num2str(max(time)),'s.']); 275 | disp(['Distance: ', num2str(max(x_CG)),'m.']); 276 | disp(['Max lateral G: ', num2str(max(acp_CG)/g),'G.']); 277 | 278 | plotData(data,vehicle_Params,tire_Params,'DLC'); -------------------------------------------------------------------------------- /Matlab/READ_ME.md: -------------------------------------------------------------------------------- 1 | # TESTS 2 | 3 | **v07** - its is the **main** where everything originates (estimated time execution ~1min @ [i3-2xxx, 8GB RAM]) 4 | 5 | **DoubleLaneChange** - it is designed to simulate control for the standard ISO3888 Double Lane Change ( **DLC** ) test. Downside of this simulation test is that the speed is maintained during the simulated test. It is needed to implement friction model in order to give additional action-reaction to have an influence on the vehicle speed and handling. 6 | 7 | **userDefinedTest** - it present a code for different vehicle handlings - *FREE TEST* 8 | 9 | # FUNCTIONS 10 | Additional functions used in two previous tests: 11 | 12 | *doubleLaneChange* - it generates cones positions for DLC and gates in positions which are input parameters 13 | 14 | *plotData* - it plots all simulation parameters. The last input argument must be: {'DLC', 'etc'}. *DLC* is for Double-Lane-Change test, whilst *etc* is for everything else for now (free test) 15 | 16 | *generateOrientation* - it is used for generating control reference for car handling. Reference is car orientation over time. It returns a vector of vehicle's orientation over time and it also returns time vector. The last input argument is one of {'ramp', 'step'}. It defines the time of generated control reference - does the control reference changes with unit step type or it is more "smooth" using ramp type. 17 | 18 | # Additional files 19 | 20 | *tire_Avon* - it holds the data about tires used on FSRA16 (Road Arrow vehicle from 2016) 21 | 22 | *wheel_speeds* - angular speed of each wheel generated from previous simulations (currently unused so far, since the control of vehicle is not implemented to control wheels, only steering wheel). For such improvements, it is needed to add momentum equations about load and speed of each wheel over the whole system (vehicle). System is already too non-linear, therefor this was not implemented due to deadline to finish final Bachelor thesis. This is one of the following steps of improvements. 23 | -------------------------------------------------------------------------------- /Matlab/doubleLaneChange.m: -------------------------------------------------------------------------------- 1 | % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % 2 | % 3 | % Copyright (c) * 4 | % All rights reserved by Milan Tepic 5 | % 6 | % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % 7 | % 8 | % Function: doubleLaneChange 9 | % Input parameters: none 10 | % Return parameters: 11 | % - output << generated vehicle trajectory for control 12 | % - cones << position of cones for double lane change test 13 | % 14 | % 15 | % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % 16 | % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % 17 | % 18 | % 19 | % DESCRIPTION 20 | % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % 21 | % 22 | % The main program, where everything originates 23 | % 24 | % 25 | % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % 26 | % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % 27 | % 28 | % 29 | % HISTORY 30 | % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % 31 | % Version: 1.7 32 | % Author/Date: Milan Tepic / 2017-02-07 33 | % Change: Initial version 34 | % 35 | % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % 36 | % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % 37 | 38 | function [output,cones] = doubleLaneChange() 39 | 40 | % generate double-lane change cones 41 | %cones_X = [10 15 20 31 37.5 44 57 63 69]-10; 42 | 43 | cones_X = [0 6 12 25.5 31 36.5 49 55 61]+830*1e-3; 44 | cones_Y_left = [0.8125 0.8125 0.8125... 45 | 3.9375 3.9375 3.9375... 46 | 1.5 1.5 1.5]; 47 | cones_Y_right = [-0.8125 -0.8125 -0.8125... 48 | 1.8125 1.8125 1.8125... 49 | -1.5 -1.5 -1.5]; 50 | cones = [cones_X; cones_Y_left; cones_Y_right]'; 51 | %' 52 | % generate vehicle trajectory 53 | s_1 = atan(-5:0.05:4.5); %-5.7 54 | s_1 = s_1+1.45; 55 | s_1 = s_1-s_1(1); 56 | % s_2 = -atan(-5.7:0.05:4.5); %neprekidnost funkcije s_1 i s_2 57 | s_2 = -atan(-4:0.05:4.5); 58 | s_2 = s_2+1.45; 59 | s_2 = s_2-s_2(end); 60 | x_1 = 0:0.05:15; 61 | x_2 = linspace(15,37.5,length(s_1)); 62 | x_3 = linspace(37.5,63,length(s_2)); 63 | x_4 = 63:0.05:80; 64 | p_1 = zeros(1,length(x_1)); 65 | p_4 = zeros(1,length(x_4)); 66 | x = [x_1 x_2 x_3 x_4]; 67 | y = [p_1 s_1 s_2 p_4]; 68 | 69 | output = [x' y']; 70 | end -------------------------------------------------------------------------------- /Matlab/generateOrientation.m: -------------------------------------------------------------------------------- 1 | % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % 2 | % 3 | % Copyright (c) * 4 | % All rights reserved by Milan Tepic 5 | % 6 | % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % 7 | % 8 | % Function: generateOrientation 9 | % Input parameters: 10 | % Return parameters: 11 | % 12 | % 13 | % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % 14 | % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % 15 | % 16 | % 17 | % DESCRIPTION 18 | % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % 19 | % 20 | % It is used for generating control reference for car 21 | % handling. Reference is car orientation over time. It 22 | % returns a vector of vehicle's orientation over time and 23 | % it also returns time vector. The last input argument is 24 | % one of {'ramp', 'step'}. It defines the time of generated 25 | % control reference - does the control reference changes 26 | % with unit step type or it is more "smooth" using ramp type. 27 | % 28 | % 29 | % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % 30 | % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % 31 | % 32 | % 33 | % HISTORY 34 | % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % 35 | % Version: 1.7 36 | % Author/Date: Milan Tepic / 2017-02-07 37 | % Change: Initial version 38 | % 39 | % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % 40 | % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % 41 | 42 | function [orientationVector timeVector] = generateOrientation(orientationVector,timeVector,finalOrientation,time_transition,dt,style) 43 | if(length(style)==4) 44 | if(mean(style=='step') || mean(style=='ramp')) 45 | multiplier = round(time_transition/dt); 46 | addTime = linspace(timeVector(end,1),timeVector(end,1)+multiplier*dt,multiplier+1); 47 | 48 | timeVector = [timeVector; addTime']; 49 | if(mean(style=='ramp')==1) 50 | addOrientation = linspace(orientationVector(end,1),finalOrientation,multiplier+1); 51 | else if(mean(style=='step')==1) 52 | addOrientation = linspace(finalOrientation,finalOrientation,multiplier+1); 53 | end 54 | end 55 | orientationVector = [orientationVector; addOrientation']; 56 | else 57 | 58 | disp('Wrong input for signal configuration. RAMP or STEP are supported ONLY currently'); 59 | end 60 | else 61 | disp('Wrong input for signal configuration. RAMP or STEP are supported ONLY currently'); 62 | end 63 | end -------------------------------------------------------------------------------- /Matlab/plotData.m: -------------------------------------------------------------------------------- 1 | % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % 2 | % 3 | % Copyright (c) * 4 | % All rights reserved by Milan Tepic 5 | % 6 | % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % 7 | % 8 | % Function: plotData 9 | % Input parameters: 10 | % Return parameters: 11 | % 12 | % 13 | % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % 14 | % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % 15 | % 16 | % 17 | % DESCRIPTION 18 | % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % 19 | % 20 | % it plots all simulation parameters. The last input argument 21 | % must be: {'DLC', 'etc'}. *DLC* is for Double-Lane-Change 22 | % test, whilst *etc* is for everything else for now (free test) 23 | % 24 | % 25 | % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % 26 | % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % 27 | % 28 | % 29 | % HISTORY 30 | % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % 31 | % Version: 1.7 32 | % Author/Date: Milan Tepic / 2017-02-07 33 | % Change: Initial version 34 | % 35 | % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % 36 | % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % 37 | 38 | function done = plotData(data,vehicle_Params,tire_Params,test) 39 | if(length(test)==3) 40 | if(test=='DLC') 41 | x = data(:,1:6); 42 | y = data(:,7:12); 43 | m = data(:,13:21); 44 | beta = data(:,22:25); 45 | wheel_speeds = data(:,26:29); 46 | 47 | psi = data(:,30); 48 | d_psi = data(:,31); 49 | acp_psi = data(:,32); 50 | acp_CG = data(:,33); 51 | sigma_CG = data(:,34); 52 | steer = data(:,35); 53 | time = data(:,36); 54 | 55 | orientationVector = data(:,37); 56 | timeVector = data(:,38); 57 | 58 | x_FL = x(:,1); y_FL = y(:,1); 59 | x_FR = x(:,2); y_FR = y(:,2); 60 | x_RL = x(:,3); y_RL = y(:,3); 61 | x_RR = x(:,4); y_RR = y(:,4); 62 | x_CG = x(:,5); y_CG = y(:,5); 63 | x_CC = x(:,6); y_CC = y(:,6); 64 | 65 | beta_FL = beta(:,1); 66 | beta_FR = beta(:,2); 67 | beta_RL = beta(:,3); 68 | beta_RR = beta(:,4); 69 | 70 | m1 = m(:,1); m2 = m(:,2); 71 | m3 = m(:,3); m4 = m(:,4); 72 | mass = m(:,5); 73 | dm_lateral = m(:,6:7); 74 | dm_longitudinal = m(:,8:9); 75 | 76 | FL_speed = wheel_speeds(:,1); 77 | FR_speed = wheel_speeds(:,2); 78 | RL_speed = wheel_speeds(:,3); 79 | RR_speed = wheel_speeds(:,4); 80 | 81 | L = vehicle_Params(1); 82 | T_front = vehicle_Params(2); 83 | T_rear = vehicle_Params(3); 84 | CG_height = vehicle_Params(4); 85 | a = vehicle_Params(5); 86 | b = vehicle_Params(6); 87 | g = vehicle_Params(7); 88 | speed = vehicle_Params(8); 89 | dt = vehicle_Params(9); 90 | 91 | C_alpha = tire_Params(1); 92 | WHEEL_DIAMETER = tire_Params(2); 93 | 94 | load('tire_Avon.mat'); 95 | 96 | sliped = -tire_Avon(:,2); 97 | lateralLoaded = tire_Avon(:,3).*1e3; 98 | 99 | slipAngle = -12:1e-3:12; 100 | lateralLoad = interp1(sliped,lateralLoaded,slipAngle,'pchip'); 101 | 102 | 103 | [trajectory, cones] = doubleLaneChange; 104 | 105 | cones_X = cones(:,1); 106 | cones_Y_left = cones(:,2); 107 | cones_Y_right = cones(:,3); 108 | trajectory_x = trajectory(:,1); 109 | trajectory_y = trajectory(:,2); 110 | 111 | tg = uitabgroup; 112 | fig1 = figure(1); 113 | fig1.Name = 'Vehicle model'; 114 | fig1.Position = [10 55 1342 623]; 115 | fig1.NumberTitle = 'off'; 116 | 117 | % fig1 = figure; 118 | % % fig1.Position = [56 54 1221 617]; 119 | % fig1.Position = [5 63 1221 617]; 120 | tab1 = uitab(tg); 121 | axes('Parent',tab1); 122 | tab1.Title = 'Main'; 123 | 124 | subplot(4,4,[1 2 5 6 9 10 13 14]); 125 | plot3(x_CG,y_CG,time,'k',x_FL,y_FL,time,'c',x_FR,y_FR,time,'r',x_RL,y_RL,time,'g',x_RR,y_RR,time,'b'); 126 | hold all 127 | % cones_time_vector = linspace(0,max(time),length(cones_X)); 128 | plot(cones_X(1:3),cones_Y_left(1:3),'k',... 129 | cones_X(4:6),cones_Y_left(4:6),'k',... 130 | cones_X(7:9),cones_Y_left(7:9),'k',... 131 | cones_X(1:3),cones_Y_right(1:3),'k',... 132 | cones_X(4:6),cones_Y_right(4:6),'k',... 133 | cones_X(7:9),cones_Y_right(7:9),'k'); 134 | hold all 135 | plot(cones_X,cones_Y_left,'gO',cones_X,cones_Y_right,'gO',... 136 | 'Color',[1 .5 0],'LineWidth',4); 137 | grid 138 | title('Trajectory'); 139 | xlabel('x [m]'); 140 | ylabel('y [m]'); 141 | view([0 0 1]) 142 | axis equal 143 | set(gcf,'Color',[0.85,0.85,0.85]); 144 | set(gca,'Color',[0.7 0.7 0.7]); 145 | 146 | subplot(4,4,3) 147 | plot(time,sigma_CG.*(180/pi)); 148 | grid 149 | xlim([0 max(time)]); 150 | title('CG speed vector angle'); 151 | xlabel('time [s]'); 152 | ylabel(['\sigma_{CG} [ ',char(176),']']); 153 | 154 | subplot(4,4,7) 155 | plot(time,d_psi./dt) 156 | grid 157 | xlim([0 max(time)]); 158 | title('Angular speed'); 159 | xlabel('time [s]'); 160 | ylabel('\omega [rad/s]'); 161 | 162 | subplot(4,4,4) 163 | plot(time,d_psi.*180/pi) 164 | grid 165 | title('\Delta\Psi'); 166 | xlim([0 max(time)]); 167 | 168 | subplot(4,4,8) 169 | plot(time,steer.*(135/30)); 170 | grid 171 | xlabel('time[s]'); 172 | ylabel(['\delta [ ',char(176),']']); 173 | title('Steering angle'); 174 | xlim([0 max(time)]); 175 | 176 | subplot(4,4,[11 12 15 16]); 177 | plot(time,m1,'c',time,m2,'r',time,m3,'g',time,m4,'b'); 178 | grid 179 | xlabel('time [s]'); 180 | ylabel('mass[kg]'); 181 | l = legend('Front_{left}','Front_{right}','Rear_{left}','Rear_{right}','Location','SouthWest'); 182 | l.Position = [0.9141 0.1096 0.0781 0.1618]; 183 | title('Load transfer'); 184 | xlim([0 max(time)]); 185 | ylim([0 0.6*max(mass)]); 186 | 187 | % fig2 = figure; 188 | % fig2.Position = [137 46 1221 617]; 189 | tab2 = uitab(tg); 190 | axes('Parent',tab2); 191 | tab2.Title = 'Telemetry'; 192 | subplot(231) 193 | plot(x_CG,y_CG,'k',x_FL,y_FL,'c',x_FR,y_FR,'r',x_RL,y_RL,'g',x_RR,y_RR,'b'); 194 | grid 195 | axis equal 196 | title('Trajectory'); 197 | xlabel('X_{axis} [m]'); 198 | ylabel('Y_{axis} [m]'); 199 | 200 | subplot(232) 201 | psi_calc = mod(psi,2*pi).*180/pi; 202 | psi_calc(psi_calc>180) = psi_calc(psi_calc>180)-360; 203 | plot(time,psi_calc) 204 | hold all 205 | plot(time,orientationVector,'k'); 206 | grid 207 | title('Orientation'); 208 | xlabel('time [s]'); 209 | ylabel(['\Psi [ ',char(176),']']); 210 | xlim([0 max(time)]); 211 | 212 | subplot(234) 213 | plot(time,d_psi.*180/pi) 214 | grid 215 | title('\Delta\Psi'); 216 | xlabel('time [s]'); 217 | ylabel(['\Delta\Psi [ ', char(176),']']); 218 | xlim([0 max(time)]); 219 | 220 | subplot(235) 221 | plot(time,beta_FL.*180/pi,'c',time,beta_FR.*180/pi,'r'); 222 | hold all 223 | plot(time,beta_RL.*180/pi,'g',time,beta_RR.*180/pi,'b'); 224 | grid 225 | title('Slip angle'); 226 | xlabel('time [s]'); 227 | ylabel(['\beta [ ', char(176),']']); 228 | l = legend('Front_{left}','Front_{right}','Rear_{left}','Rear_{right}','Location','NorthWest'); 229 | % l.Position = [0.5620 0.4581 0.0636 0.0641]; 230 | l.Position = [0.5998 0.3878 0.0778 0.1540]; 231 | xlim([0 max(time)]); 232 | 233 | subplot(233) 234 | plot(time,x_CC,[time(1) time(end)], [b b],'r'); 235 | grid 236 | title('Curvature center X_{car}') 237 | xlabel('time [s]'); 238 | ylabel('Rx_C [m]'); 239 | xlim([0 max(time)]); 240 | 241 | subplot(236) 242 | plot(time,y_CC); 243 | grid 244 | title('Curvature center Y_{car}') 245 | xlabel('time [s]'); 246 | ylabel('Ry_C [m]'); 247 | % if(median(y_CC)~=0) 248 | % ylim([-10*abs(median(y_CC)) 10*abs(median(y_CC))]); 249 | % else 250 | % ylim([-5 5]); 251 | % end 252 | xlim([0 max(time)]); 253 | 254 | % fig3 = figure(3); 255 | % fig3.Position = [1396 81 906 618]; 256 | % % fig3.Position = [75 56 906 618]; 257 | tab3 = uitab(tg); 258 | axes('Parent',tab3); 259 | tab3.Title = 'Load transfer'; 260 | subplot(2,4,1) 261 | plot(time,acp_CG./g); 262 | grid 263 | xlabel('time [s]'); 264 | ylabel('Lateral [G]'); 265 | title('A_{cp} CG'); 266 | xlim([0 max(time)]); 267 | 268 | subplot(2,4,2) 269 | plot(-acp_psi.*180/pi,time); 270 | grid 271 | xlabel(['Longitudinal[ ', char(176), ']']); 272 | ylabel('time [s]'); 273 | title('A_{cp} \Psi'); 274 | ylim([0 max(time)]); 275 | 276 | subplot(2,4,[5 6]) 277 | plot(time,dm_longitudinal); 278 | grid 279 | xlabel('time [s]'); 280 | ylabel('load [%]'); 281 | title('\Deltam_{longitudinal}'); 282 | xlim([0 max(time)]); 283 | legend('Front axle','Rear axle'); 284 | 285 | subplot(2,4,[3 4 7 8]) 286 | plot(dm_lateral.*100,time); 287 | grid 288 | xlabel('load [%]'); 289 | ylabel('time [s]'); 290 | title('\Deltam_{lateral}'); 291 | ylim([0 max(time)]); 292 | legend('Front axle','Rear axle'); 293 | 294 | % fig4 = figure(4); 295 | % fig4.Position = [48 87 1269 579]; 296 | % % subplot(211) 297 | tab4 = uitab(tg); 298 | axes('Parent',tab4); 299 | subplot(3,4,[1:8]) 300 | tab4.Title = 'ISO DoubleLaneChange'; 301 | plot(x_FL,y_FL,'c',x_FR,y_FR,'r',x_RL,y_RL,'g',x_RR,y_RR,'b','Linewidth',2.5); 302 | hold all 303 | plot(x_CG,y_CG,'k','Linewidth',2.5) 304 | hold all 305 | plot(cones_X,cones_Y_left,'gO',cones_X,cones_Y_right,'gO',... 306 | 'Color',[1 .5 0],'LineWidth',4); 307 | hold all 308 | plot(cones_X(1:3),cones_Y_left(1:3),'k',... 309 | cones_X(4:6),cones_Y_left(4:6),'k',... 310 | cones_X(7:9),cones_Y_left(7:9),'k',... 311 | cones_X(1:3),cones_Y_right(1:3),'k',... 312 | cones_X(4:6),cones_Y_right(4:6),'k',... 313 | cones_X(7:9),cones_Y_right(7:9),'k'); 314 | hold all 315 | % plot([0 11 27 39 51 65],[0 0 3 3 0 0],'c','Linewidth',2); 316 | grid 317 | axis([-5 65 -5 6]); 318 | if(max(max(x(:,1:4)))>65) 319 | xlim([min(min(x(:,1:4)))-5 max(max(x(:,1:4)))+5]); 320 | end 321 | if(max(max(y(:,1:4)))>65) 322 | ylim([min(min(y(:,1:4)))-5 max(max(y(:,1:4)))+5]); 323 | end 324 | xlabel('X_{axis} [m]'); 325 | ylabel('Y_{axis} [m]'); 326 | title(['ISO - Double Lane Change ', num2str(speed),'km/h']); 327 | legend('Front Left','Front Right','Rear Left', 'Rear Right','C/G',... 328 | 'Cones','Location','South'); 329 | set(gcf,'Color',[0.85,0.85,0.85]); 330 | set(gca,'Color',[0.7 0.7 0.7]); 331 | lateral_G = acp_CG./g; 332 | max_G_txt = ['\leftarrow G_{lateral} = ',num2str(max(lateral_G)),'G']; 333 | maxG_xLoc = find(lateral_G==max(lateral_G)); 334 | maxG_yLoc = find(lateral_G==max(lateral_G)); 335 | text(x_RR(maxG_xLoc([1 end])),... 336 | y_RR(maxG_yLoc([1 end])),max_G_txt) 337 | annotation('textbox',... 338 | [0.8 0.73 0.10 0.10],... 339 | 'String',{' 70 km/h'},... 340 | 'FontSize',14,... 341 | 'FontName','Arial',... 342 | 'LineWidth',2,... 343 | 'BackgroundColor',[0.9 0.9 0.9],... 344 | 'Color',[0 0 0]); 345 | 346 | % fig5 = figure(5); 347 | % fig5.Position = [1858 528 787 398]; 348 | % % fig5.Position = [560 272 787 398]; 349 | subplot(3,4,9) 350 | plot(time,FL_speed); 351 | grid 352 | xlim([0 max(time)]); 353 | title('Front left'); 354 | xlabel('time [s]'); 355 | ylabel('\omega [rad/s]') 356 | subplot(3,4,10) 357 | plot(time,RL_speed); 358 | grid 359 | xlim([0 max(time)]); 360 | title('Rear left'); 361 | xlabel('time [s]'); 362 | ylabel('\omega [rad/s]') 363 | subplot(3,4,11) 364 | plot(time,RR_speed); 365 | grid 366 | xlim([0 max(time)]); 367 | title('Rear rigth'); 368 | xlabel('time [s]'); 369 | ylabel('\omega [rad/s]') 370 | subplot(3,4,12) 371 | plot(time,FR_speed); 372 | grid 373 | xlim([0 max(time)]); 374 | title('Front right'); 375 | xlabel('time [s]'); 376 | ylabel('\omega [rad/s]') 377 | 378 | tab5 = uitab(tg); 379 | axes('Parent',tab5); 380 | tab5.Title = 'Car & tires'; 381 | subplot(2,1,1) 382 | plot(slipAngle,lateralLoad,sliped,lateralLoaded,'r','Linewidth',1.4); 383 | grid 384 | ylabel('Lateral force [kN]'); 385 | xlabel(['Slip angle [ ',char(176), ']']); 386 | title('AVON - Slip angle diagram'); 387 | legend('Approx','Look-up table','Location','SouthEast'); 388 | 389 | subplot(2,1,2) 390 | plot(time,sqrt((x_FL-x_FR).^2+(y_FL-y_FR).^2),'Linewidth',1.6) 391 | hold all 392 | plot(time,sqrt((x_RL-x_RR).^2+(y_RL-y_RR).^2),'Linewidth',1.6) 393 | hold all 394 | plot(time,sqrt((x_FR-x_RR).^2+(y_FR-y_RR).^2),'Linewidth',1.6) 395 | hold all 396 | plot(time,sqrt((x_FL-x_RL).^2+(y_FL-y_RL).^2),'Linewidth',1.6) 397 | grid 398 | xlim([0 max(time)]); 399 | title('Car measures'); 400 | xlabel('time[s]'); 401 | ylabel('Distance [m]'); 402 | legend('Track_{front}','Track_{rear}',... 403 | 'Wheelbase_{left}','Wheelbase_{right}',... 404 | 'Location','East'); 405 | 406 | 407 | tg.SelectedTab = tab4; 408 | done = 1; 409 | 410 | else if(test=='etc') 411 | x = data(:,1:6); 412 | y = data(:,7:12); 413 | m = data(:,13:21); 414 | beta = data(:,22:25); 415 | wheel_speeds = data(:,26:29); 416 | 417 | psi = data(:,30); 418 | d_psi = data(:,31); 419 | acp_psi = data(:,32); 420 | acp_CG = data(:,33); 421 | sigma_CG = data(:,34); 422 | steer = data(:,35); 423 | time = data(:,36); 424 | 425 | orientationVector = data(:,37); 426 | timeVector = data(:,38); 427 | 428 | x_FL = x(:,1); y_FL = y(:,1); 429 | x_FR = x(:,2); y_FR = y(:,2); 430 | x_RL = x(:,3); y_RL = y(:,3); 431 | x_RR = x(:,4); y_RR = y(:,4); 432 | x_CG = x(:,5); y_CG = y(:,5); 433 | x_CC = x(:,6); y_CC = y(:,6); 434 | 435 | beta_FL = beta(:,1); 436 | beta_FR = beta(:,2); 437 | beta_RL = beta(:,3); 438 | beta_RR = beta(:,4); 439 | 440 | m1 = m(:,1); m2 = m(:,2); 441 | m3 = m(:,3); m4 = m(:,4); 442 | mass = m(:,5); 443 | dm_lateral = m(:,6:7); 444 | dm_longitudinal = m(:,8:9); 445 | 446 | FL_speed = wheel_speeds(:,1); 447 | FR_speed = wheel_speeds(:,2); 448 | RL_speed = wheel_speeds(:,3); 449 | RR_speed = wheel_speeds(:,4); 450 | 451 | L = vehicle_Params(1); 452 | T_front = vehicle_Params(2); 453 | T_rear = vehicle_Params(3); 454 | CG_height = vehicle_Params(4); 455 | a = vehicle_Params(5); 456 | b = vehicle_Params(6); 457 | g = vehicle_Params(7); 458 | speed = vehicle_Params(8); 459 | dt = vehicle_Params(9); 460 | 461 | C_alpha = tire_Params(1); 462 | WHEEL_DIAMETER = tire_Params(2); 463 | 464 | load('tire_Avon.mat'); 465 | 466 | sliped = -tire_Avon(:,2); 467 | lateralLoaded = tire_Avon(:,3).*1e3; 468 | 469 | slipAngle = -12:1e-3:12; 470 | lateralLoad = interp1(sliped,lateralLoaded,slipAngle,'pchip'); 471 | 472 | tg = uitabgroup; 473 | fig1 = figure(1); 474 | fig1.Name = 'Vehicle model'; 475 | fig1.Position = [10 55 1342 623]; 476 | fig1.NumberTitle = 'off'; 477 | 478 | % fig1 = figure; 479 | % % fig1.Position = [56 54 1221 617]; 480 | % fig1.Position = [5 63 1221 617]; 481 | tab1 = uitab(tg); 482 | axes('Parent',tab1); 483 | tab1.Title = 'Main'; 484 | 485 | subplot(4,4,[1 2 5 6 9 10 13 14]); 486 | plot3(x_CG,y_CG,time,'k',x_FL,y_FL,time,'c',x_FR,y_FR,time,'r',x_RL,y_RL,time,'g',x_RR,y_RR,time,'b'); 487 | hold all 488 | grid 489 | 490 | title('Trajectory'); 491 | xlabel('x [m]'); 492 | ylabel('y [m]'); 493 | view([0 0 1]) 494 | axis equal 495 | set(gcf,'Color',[0.85,0.85,0.85]); 496 | set(gca,'Color',[0.7 0.7 0.7]); 497 | 498 | subplot(4,4,3) 499 | plot(time,sigma_CG.*(180/pi)); 500 | grid 501 | xlim([0 max(time)]); 502 | title('CG speed vector angle'); 503 | xlabel('time [s]'); 504 | ylabel(['\sigma_{CG} [ ',char(176),']']); 505 | 506 | subplot(4,4,7) 507 | plot(time,d_psi./dt) 508 | grid 509 | xlim([0 max(time)]); 510 | title('Angular speed'); 511 | xlabel('time [s]'); 512 | ylabel('\omega [rad/s]'); 513 | 514 | subplot(4,4,4) 515 | plot(time,d_psi.*180/pi) 516 | grid 517 | title('\Delta\Psi'); 518 | xlim([0 max(time)]); 519 | 520 | subplot(4,4,8) 521 | plot(time,steer.*(135/30)); 522 | grid 523 | xlabel('time[s]'); 524 | ylabel(['\delta [ ',char(176),']']); 525 | title('Steering angle'); 526 | xlim([0 max(time)]); 527 | 528 | subplot(4,4,[11 12 15 16]); 529 | plot(time,m1,'c',time,m2,'r',time,m3,'g',time,m4,'b'); 530 | grid 531 | xlabel('time [s]'); 532 | ylabel('mass[kg]'); 533 | l = legend('Front_{left}','Front_{right}','Rear_{left}','Rear_{right}','Location','SouthWest'); 534 | l.Position = [0.9141 0.1096 0.0781 0.1618]; 535 | title('Load transfer'); 536 | xlim([0 max(time)]); 537 | ylim([0 0.6*max(mass)]); 538 | 539 | % fig2 = figure; 540 | % fig2.Position = [137 46 1221 617]; 541 | tab2 = uitab(tg); 542 | axes('Parent',tab2); 543 | tab2.Title = 'Telemetry'; 544 | subplot(231) 545 | plot(x_CG,y_CG,'k',x_FL,y_FL,'c',x_FR,y_FR,'r',x_RL,y_RL,'g',x_RR,y_RR,'b'); 546 | grid 547 | axis equal 548 | title('Trajectory'); 549 | xlabel('X_{axis} [m]'); 550 | ylabel('Y_{axis} [m]'); 551 | 552 | subplot(232) 553 | psi_calc = mod(psi,2*pi).*180/pi; 554 | psi_calc(psi_calc>180) = psi_calc(psi_calc>180)-360; 555 | plot(time,psi_calc) 556 | hold all 557 | plot(time,orientationVector,'k'); 558 | grid 559 | title('Orientation'); 560 | xlabel('time [s]'); 561 | ylabel(['\Psi [ ',char(176),']']); 562 | xlim([0 max(time)]); 563 | 564 | subplot(234) 565 | plot(time,d_psi.*180/pi) 566 | grid 567 | title('\Delta\Psi'); 568 | xlabel('time [s]'); 569 | ylabel(['\Delta\Psi [ ', char(176),']']); 570 | xlim([0 max(time)]); 571 | 572 | subplot(235) 573 | plot(time,beta_FL.*180/pi,'c',time,beta_FR.*180/pi,'r'); 574 | hold all 575 | plot(time,beta_RL.*180/pi,'g',time,beta_RR.*180/pi,'b'); 576 | grid 577 | title('Slip angle'); 578 | xlabel('time [s]'); 579 | ylabel(['\beta [ ', char(176),']']); 580 | l = legend('Front_{left}','Front_{right}','Rear_{left}','Rear_{right}','Location','NorthWest'); 581 | % l.Position = [0.5620 0.4581 0.0636 0.0641]; 582 | l.Position = [0.5998 0.3878 0.0778 0.1540]; 583 | xlim([0 max(time)]); 584 | 585 | subplot(233) 586 | plot(time,x_CC,[time(1) time(end)], [b b],'r'); 587 | grid 588 | title('Curvatore center X_{car}') 589 | xlabel('time [s]'); 590 | ylabel('Rx_C [m]'); 591 | xlim([0 max(time)]); 592 | 593 | subplot(236) 594 | plot(time,y_CC); 595 | grid 596 | title('Curvatore center Y_{car}') 597 | xlabel('time [s]'); 598 | ylabel('Ry_C [m]'); 599 | % if(median(y_CC)~=0) 600 | % ylim([-10*abs(median(y_CC)) 10*abs(median(y_CC))]); 601 | % else 602 | % ylim([-5 5]); 603 | % end 604 | xlim([0 max(time)]); 605 | 606 | % fig3 = figure(3); 607 | % fig3.Position = [1396 81 906 618]; 608 | % % fig3.Position = [75 56 906 618]; 609 | tab3 = uitab(tg); 610 | axes('Parent',tab3); 611 | tab3.Title = 'Load transfer'; 612 | subplot(2,4,1) 613 | plot(time,acp_CG./g); 614 | grid 615 | xlabel('time [s]'); 616 | ylabel('Lateral [G]'); 617 | title('A_{cp} CG'); 618 | xlim([0 max(time)]); 619 | 620 | subplot(2,4,2) 621 | plot(-acp_psi.*180/pi,time); 622 | grid 623 | xlabel(['Longitudinal[ ', char(176), ']']); 624 | ylabel('time [s]'); 625 | title('A_{cp} \Psi'); 626 | ylim([0 max(time)]); 627 | 628 | subplot(2,4,[5 6]) 629 | plot(time,dm_longitudinal); 630 | grid 631 | xlabel('time [s]'); 632 | ylabel('load [%]'); 633 | title('\Deltam_{longitudinal}'); 634 | xlim([0 max(time)]); 635 | legend('Front axle','Rear axle'); 636 | 637 | subplot(2,4,[3 4 7 8]) 638 | plot(dm_lateral.*100,time); 639 | grid 640 | xlabel('load [%]'); 641 | ylabel('time [s]'); 642 | title('\Deltam_{lateral}'); 643 | ylim([0 max(time)]); 644 | legend('Front axle','Rear axle'); 645 | 646 | % fig4 = figure(4); 647 | % fig4.Position = [48 87 1269 579]; 648 | % % subplot(211) 649 | tab4 = uitab(tg); 650 | axes('Parent',tab4); 651 | subplot(3,4,[1:8]) 652 | tab4.Title = 'User defined free test'; 653 | plot(x_FL,y_FL,'c',x_FR,y_FR,'r',x_RL,y_RL,'g',x_RR,y_RR,'b','Linewidth',2.5); 654 | hold all 655 | plot(x_CG,y_CG,'k','Linewidth',2.5) 656 | hold all 657 | grid 658 | if(max(max(x(:,1:4)))>65) 659 | xlim([min(min(x(:,1:4)))-5 max(max(x(:,1:4)))+5]); 660 | end 661 | if(max(max(y(:,1:4)))>65) 662 | ylim([min(min(y(:,1:4)))-5 max(max(y(:,1:4)))+5]); 663 | end 664 | xlabel('X_{axis} [m]'); 665 | ylabel('Y_{axis} [m]'); 666 | title(['User defined free test at ', num2str(speed),'km/h']); 667 | legend('Front Left','Front Right','Rear Left', 'Rear Right','C/G',... 668 | 'Location','South'); 669 | set(gcf,'Color',[0.85,0.85,0.85]); 670 | set(gca,'Color',[0.7 0.7 0.7]); 671 | lateral_G = acp_CG./g; 672 | max_G_txt = ['\leftarrow G_{lateral} = ',num2str(max(lateral_G)),'G']; 673 | maxG_xLoc = find(lateral_G==max(lateral_G)); 674 | maxG_yLoc = find(lateral_G==max(lateral_G)); 675 | text(x_RR(maxG_xLoc([1 end])),... 676 | y_RR(maxG_yLoc([1 end])),max_G_txt) 677 | annotation('textbox',... 678 | [0.8 0.73 0.10 0.10],... 679 | 'String',{' 70 km/h'},... 680 | 'FontSize',14,... 681 | 'FontName','Arial',... 682 | 'LineWidth',2,... 683 | 'BackgroundColor',[0.9 0.9 0.9],... 684 | 'Color',[0 0 0]); 685 | 686 | % fig5 = figure(5); 687 | % fig5.Position = [1858 528 787 398]; 688 | % % fig5.Position = [560 272 787 398]; 689 | subplot(3,4,9) 690 | plot(time,FL_speed); 691 | grid 692 | xlim([0 max(time)]); 693 | title('Front left'); 694 | xlabel('time [s]'); 695 | ylabel('\omega [rad/s]') 696 | subplot(3,4,10) 697 | plot(time,RL_speed); 698 | grid 699 | xlim([0 max(time)]); 700 | title('Rear left'); 701 | xlabel('time [s]'); 702 | ylabel('\omega [rad/s]') 703 | subplot(3,4,11) 704 | plot(time,RR_speed); 705 | grid 706 | xlim([0 max(time)]); 707 | title('Rear rigth'); 708 | xlabel('time [s]'); 709 | ylabel('\omega [rad/s]') 710 | subplot(3,4,12) 711 | plot(time,FR_speed); 712 | grid 713 | xlim([0 max(time)]); 714 | title('Front right'); 715 | xlabel('time [s]'); 716 | ylabel('\omega [rad/s]') 717 | 718 | tab5 = uitab(tg); 719 | axes('Parent',tab5); 720 | tab5.Title = 'Car & tires'; 721 | subplot(2,1,1) 722 | plot(slipAngle,lateralLoad,sliped,lateralLoaded,'r','Linewidth',1.4); 723 | grid 724 | ylabel('Lateral force [kN]'); 725 | xlabel(['Slip angle [ ',char(176), ']']); 726 | title('AVON - Slip angle diagram'); 727 | legend('Approx','Look-up table','Location','SouthEast'); 728 | 729 | subplot(2,1,2) 730 | plot(time,sqrt((x_FL-x_FR).^2+(y_FL-y_FR).^2),'Linewidth',1.6) 731 | hold all 732 | plot(time,sqrt((x_RL-x_RR).^2+(y_RL-y_RR).^2),'Linewidth',1.6) 733 | hold all 734 | plot(time,sqrt((x_FR-x_RR).^2+(y_FR-y_RR).^2),'Linewidth',1.6) 735 | hold all 736 | plot(time,sqrt((x_FL-x_RL).^2+(y_FL-y_RL).^2),'Linewidth',1.6) 737 | grid 738 | xlim([0 max(time)]); 739 | title('Car measures'); 740 | xlabel('time[s]'); 741 | ylabel('Distance [m]'); 742 | legend('Track_{front}','Track_{rear}',... 743 | 'Wheelbase_{left}','Wheelbase_{right}',... 744 | 'Location','East'); 745 | 746 | 747 | tg.SelectedTab = tab4; 748 | done = 1; 749 | else 750 | disp('Wrong test input. Set DLC(Double Lane Change) or etc(user defined free test'); 751 | end 752 | end 753 | else 754 | 755 | disp('Wrong test input. Set DLC(Double Lane Change) or etc(user defined free test'); 756 | end 757 | end -------------------------------------------------------------------------------- /Matlab/tire_Avon.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Tepic/Vehicle_Model/8dd5c9dee51ae082fd61abec54cda2c15d91eb9c/Matlab/tire_Avon.mat -------------------------------------------------------------------------------- /Matlab/userDefinedTest.m: -------------------------------------------------------------------------------- 1 | % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % 2 | % 3 | % Copyright (c) * 4 | % All rights reserved by Milan Tepic 5 | % 6 | % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % 7 | % 8 | % Filename: userDefinedTest.m 9 | % 10 | % 11 | % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % 12 | % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % 13 | % 14 | % 15 | % DESCRIPTION 16 | % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % 17 | % 18 | % it present a code for different vehicle handlings - *FREE TEST* 19 | % 20 | % 21 | % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % 22 | % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % 23 | % 24 | % 25 | % HISTORY 26 | % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % 27 | % Version: 1.7 28 | % Author/Date: Milan Tepic / 2017-02-07 29 | % Change: Initial version 30 | % 31 | % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % 32 | % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % 33 | 34 | clear all 35 | close all 36 | clc 37 | 38 | % load('wheel_speeds.mat'); 39 | 40 | % SIMULATION PARAMETERS 41 | g = 9.80037; % [m/s^2] 42 | SIM_TIME = 3.5; % [s] 43 | dt = 1e-4; % [s] 44 | 45 | % PI CONTROLER PARAMETERS 46 | Kp = 5; 47 | Ti = 0.08; 48 | interrupt = 1e-3; % [s] 49 | 50 | % VEHICLE PARAMETERS 51 | L = 1600*1e-3; % [mm] 52 | T_front = 1250*1e-3; % [mm] 53 | T_rear = 1200*1e-3; % [mm] 54 | CG_height = 340*1e-3; % [mm] 55 | a = 830*1e-3; % [mm] 56 | b = 770*1e-3; % [mm] 57 | 58 | unsprung_mass = 70; 59 | sprung_mass = 174; 60 | driver_mass = 75; 61 | mass = sprung_mass+... 62 | unsprung_mass+... 63 | driver_mass; % [kg] 64 | m1_s = mass*(b/L)/2; % [kg] 65 | m2_s = mass*(b/L)/2; % [kg] 66 | m3_s = mass*(a/L)/2; % [kg] 67 | m4_s = mass*(a/L)/2; % [kg] 68 | max_steer = 135; % [deg] 69 | max_steerSpeed = 540; % [deg/s] 70 | max_Dsteer = max_steerSpeed*dt; 71 | 72 | % ENGINE PARAMETERS 73 | max_speed = 135; 74 | accel_max = 10*dt; % [m/s^2 * dt] 75 | speed_max = 137.8; % [kmh] 76 | brake_max = 20*dt; % [m/s^2 * dt] 77 | 78 | % TYRE PARAMETERS 79 | load('tire_Avon.mat'); 80 | 81 | sliped = -tire_Avon(:,2); 82 | lateralLoaded = tire_Avon(:,3).*1e3; 83 | 84 | slipAngle = -12:1e-3:12; 85 | lateralLoad = interp1(sliped,lateralLoaded,slipAngle,'pchip'); 86 | C_alpha = (2.7778/2)*1e-03; % [N/deg] 87 | WHEEL_DIAMETER = 0.33/2; 88 | 89 | beta_FL = 0; 90 | beta_FR = 0; 91 | beta_RL = 0; 92 | beta_RR = 0; 93 | 94 | %% USER DEFINED TEST 95 | speed = 57; 96 | steer = 0; 97 | psi = 0; 98 | 99 | [orientationVector timeVector] = generateOrientation(0,0,180,3.5,dt,'ramp'); 100 | [orientationVector timeVector] = ... 101 | generateOrientation(orientationVector,timeVector,180,0.5,dt,'step'); 102 | 103 | m1 = m1_s; m2 = m2_s; m3 = m3_s; m4 = m4_s; 104 | 105 | vehicle_Params = [L T_front T_rear CG_height a b mass m1_s m2_s m3_s m4_s psi]; 106 | sym_Params = [steer(1) speed max_steer max_speed C_alpha g dt]; 107 | tire_Params = [beta_FL beta_FR beta_RL beta_RR]; 108 | wheel_location = [-a,0]; 109 | 110 | % INIT START LOCATION 111 | 112 | x_CG(1) = 0; 113 | y_CG(1) = 0; 114 | 115 | x_CC(1) = -b*cos(psi(1)); 116 | y_CC(1) = -b*sin(psi(1)); 117 | 118 | x_FR(1) = x_CG(1)+sqrt(a^2+(T_front/2)^2)*cos(psi(1)-atan((T_front/2)/a)); 119 | y_FR(1) = y_CG(1)+sqrt(a^2+(T_front/2)^2)*sin(psi(1)-atan((T_front/2)/a)); 120 | 121 | x_RR(1) = x_CG(1)-b*cos(psi(1))+(T_rear/2)*sin(psi(1)); 122 | y_RR(1) = y_CG(1)-b*sin(psi(1))-(T_rear/2)*cos(psi(1)); 123 | 124 | x_FL(1) = x_FR(1)-T_front*sin(psi(1)); 125 | y_FL(1) = y_FR(1)+T_front*cos(psi(1)); 126 | 127 | x_RL(1) = x_RR(1)-T_rear*sin(psi(1)); 128 | y_RL(1) = y_RR(1)+T_rear*cos(psi(1)); 129 | 130 | d_psi = 0; sigma_CG = 0; 131 | 132 | m1 = m1_s; m2 = m2_s; 133 | m3 = m3_s; m4 = m4_s; 134 | 135 | dm_longitudinal = [0; 0]; 136 | dm_lateral = [0; 0]; 137 | 138 | FL_speed = 0; 139 | FR_speed = 0; 140 | RL_speed = 0; 141 | RR_speed = 0; 142 | wheel_speeds = [FL_speed' FR_speed' RL_speed' RR_speed']; 143 | 144 | acp_CG = 0; acp_psi = 0; 145 | 146 | time = 0; 147 | e = []; 148 | u = []; 149 | lastState = 0; 150 | for position = 1:length(timeVector) 151 | if((mod(timeVector(position),interrupt))==0) 152 | e(position) = round((orientationVector(position)-psi(position)*180/pi)*1e4)/1e4; 153 | u(position) = Kp*e(position)+Kp*dt*e(position)/Ti; 154 | 155 | if(position>1) 156 | if(u(position)>0 && u(position-1)>0 &&... 157 | u(position)-u(position-1)>max_Dsteer) 158 | 159 | u(position) = u(position-1)+max_Dsteer; 160 | else if(u(position)>0 && u(position-1)>0 &&... 161 | u(position)-u(position-1)<-max_Dsteer) 162 | u(position) = u(position-1)-max_Dsteer; 163 | end 164 | end 165 | if(u(position)<0 && u(position-1)<0 &&... 166 | u(position)-u(position-1)<-max_Dsteer) 167 | 168 | u(position) = u(position-1)-max_Dsteer; 169 | else if(u(position)<0 && u(position-1)<0 &&... 170 | u(position)-u(position-1)>max_Dsteer) 171 | u(position) = u(position-1)+max_Dsteer; 172 | end 173 | end 174 | 175 | if(u(position)>0 && u(position-1)<0 &&... 176 | u(position)-u(position-1)>max_Dsteer) 177 | 178 | u(position) = u(position-1)+max_Dsteer; 179 | else if(u(position)<0 && u(position-1)>0 &&... 180 | u(position)-u(position-1)<-max_Dsteer) 181 | u(position) = u(position-1)-max_Dsteer; 182 | end 183 | end 184 | else 185 | if(u(position)>max_Dsteer) 186 | u(position) = max_Dsteer; 187 | else if(u(position)<-max_Dsteer) 188 | u(position) = -max_Dsteer; 189 | end 190 | end 191 | end 192 | 193 | if(u(position)>135) 194 | u(position) = 135; 195 | else if(u(position)<-135) 196 | u(position) = -135; 197 | end 198 | end 199 | else 200 | e(position) = e(position-1); 201 | u(position) = Kp*e(position)+Kp*dt*e(position)/Ti; 202 | end 203 | steer = [steer; u(position)]; 204 | 205 | vehicle_Params = [L T_front T_rear CG_height a b mass m1(position) m2(position) m3(position) m4(position) psi(position)]; 206 | sym_Params = [steer(position) speed(1) max_steer max_speed C_alpha g dt]; 207 | tire_Params = [beta_FL(position) beta_FR(position) beta_RL(position) beta_RR(position)]; 208 | wheel_location = [x_CG(position) y_CG(position)... 209 | x_FR(position) y_FR(position)... 210 | x_RR(position) y_RR(position)... 211 | x_FL(position) y_FL(position)... 212 | x_RL(position) y_RL(position)]; 213 | [output lastState] = wheel_move(wheel_speeds,vehicle_Params,sym_Params,tire_Params,wheel_location,slipAngle,lateralLoad,lastState); 214 | 215 | x_FL = [x_FL; output(2,1)]; y_FL = [y_FL; output(2,7)]; 216 | x_FR = [x_FR; output(2,2)]; y_FR = [y_FR; output(2,8)]; 217 | x_RL = [x_RL; output(2,3)]; y_RL = [y_RL; output(2,9)]; 218 | x_RR = [x_RR; output(2,4)]; y_RR = [y_RR; output(2,10)]; 219 | x_CG = [x_CG; output(2,5)]; y_CG = [y_CG; output(2,11)]; 220 | x_CC = [x_CC; output(2,6)]; y_CC = [y_CC; output(2,12)]; 221 | 222 | psi = [psi; output(2,13)]; d_psi = [d_psi; output(2,14)]; 223 | 224 | beta_FL = [beta_FL; output(2,15)]; 225 | beta_FR = [beta_FR; output(2,16)]; 226 | beta_RL = [beta_RL; output(2,17)]; 227 | beta_RR = [beta_RR; output(2,18)]; 228 | 229 | sigma_CG = [sigma_CG; output(2,19)]; %time = [time; output(2,20)]; 230 | 231 | m1 = [m1; output(2,21)]; m2 = [m2; output(2,22)]; 232 | m3 = [m3; output(2,23)]; m4 = [m4; output(2,24)]; 233 | dm_longitudinal = [dm_longitudinal output(:,27)]; 234 | dm_lateral = [dm_lateral -output(:,28)]; 235 | 236 | FL_speed = [FL_speed output(2,29)/WHEEL_DIAMETER]; 237 | FR_speed = [FR_speed output(2,30)/WHEEL_DIAMETER]; 238 | RL_speed = [RL_speed output(2,31)/WHEEL_DIAMETER]; 239 | RR_speed = [RR_speed output(2,32)/WHEEL_DIAMETER]; 240 | wheel_speeds = [FL_speed(end) FR_speed(end) RL_speed(end) RR_speed(end)]; 241 | 242 | acp_CG = [acp_CG; output(2,25)]; acp_psi = [acp_psi; output(2,26)]; 243 | time = [time time(end)+dt]; 244 | end 245 | 246 | wheel_speeds = [FL_speed' FR_speed' RL_speed' RR_speed']; 247 | 248 | x = [x_FL x_FR x_RL x_RR x_CG x_CC]; 249 | y = [y_FL y_FR y_RL y_RR y_CG y_CC]; 250 | 251 | m = [m1 m2 m3 m4 mass.*ones(length(time),1) dm_lateral' dm_longitudinal']; % size = length(time) x (4+1+2+2) 252 | beta = [beta_FL beta_FR beta_RL beta_RR]; 253 | 254 | vehicle_Params = [L T_front T_rear CG_height a b g speed dt]; 255 | tire_Params = [C_alpha WHEEL_DIAMETER]; 256 | 257 | orientationVector = [orientationVector; orientationVector(end)]; 258 | timeVector = [timeVector; timeVector(end)]; 259 | 260 | data = [x y m beta wheel_speeds psi d_psi acp_psi acp_CG sigma_CG steer time' orientationVector timeVector]; 261 | disp(['Total time for ISO-standard test: ', num2str(max(time)),'s.']); 262 | disp(['Distance: ', num2str(max(x_CG)),'m.']); 263 | disp(['Max lateral G: ', num2str(max(acp_CG)/g),'G.']); 264 | 265 | plotData(data,vehicle_Params,tire_Params,'etc'); 266 | -------------------------------------------------------------------------------- /Matlab/v07.m: -------------------------------------------------------------------------------- 1 | clear all 2 | close all 3 | clc 4 | 5 | % load('wheel_speeds.mat'); 6 | % used in initial testings with predefined 7 | % generated speed of each wheel 8 | 9 | % ========================================================================= 10 | % SIMULATION PARAMETERS 11 | % ========================================================================= 12 | g = 9.80037; % [m/s^2] 13 | SIM_TIME = 3.5; % [s] 14 | dt = 1e-4; % [s] 15 | 16 | % ========================================================================= 17 | % PI CONTROLER PARAMETERS 18 | % ========================================================================= 19 | % PI controller used to control the steering 20 | % wheel steered angle 21 | Kp = 5; 22 | Ti = 0.08; 23 | interrupt = 1e-3; % [s] 24 | % interrupt is used to simulate time when 25 | % the controller samples and 26 | % generates new output 27 | 28 | % ========================================================================= 29 | % VEHICLE PARAMETERS of FSRA16 30 | % Description: In simulation are used parameters for the vehicle 31 | % FSRA16 - Formula Student Road Arrow 2016 vehicle 32 | % ========================================================================= 33 | L = 1600*1e-3; % [mm] - Length 34 | T_front = 1250*1e-3; % [mm] - Width of FRONT track (FRONT axel width) 35 | T_rear = 1200*1e-3; % [mm] - Width of REAR track (REAR axel width) 36 | CG_height = 340*1e-3; % [mm] - Height of CG (center of gravity) 37 | a = 830*1e-3; % [mm] - Distance from CG to FRONT axel 38 | b = 770*1e-3; % [mm] - Distance from CG to REAR axel 39 | 40 | unsprung_mass = 70; 41 | sprung_mass = 174; 42 | driver_mass = 75; % Estimated Driver mass for average driver 43 | mass = sprung_mass+... 44 | unsprung_mass+... 45 | driver_mass; % [kg] 46 | m1_s = mass*(b/L)/2; % [kg] 47 | m2_s = mass*(b/L)/2; % [kg] 48 | m3_s = mass*(a/L)/2; % [kg] 49 | m4_s = mass*(a/L)/2; % [kg] 50 | max_steer = 135; % [deg] 51 | max_steerSpeed = 540; % [deg/s] - estimated driver's speed of steering 52 | max_Dsteer = max_steerSpeed*dt; 53 | 54 | % ========================================================================= 55 | % ENGINE PARAMETERS 56 | % ========================================================================= 57 | max_speed = 135; % [km/h] - Projected vehicle's max speed of FSRA16 58 | accel_max = 10*dt; % [m/s^2 * dt] 59 | speed_max = 137.8; % [kmh] 60 | brake_max = 20*dt; % [m/s^2 * dt] 61 | 62 | % ========================================================================= 63 | % TYRE PARAMETERS 64 | % Description: FSRA16 uses Avon tires and parameters of these tires are 65 | % stored in the additional file tire_Avon.mat 66 | % This file is generated according to the datasheet from tire 67 | % manufacturer. In following lines, the data had to be 68 | % interpolated since the data from the datasheet has big 69 | % steps in the table review of tire's performances. 70 | % ========================================================================= 71 | load('tire_Avon.mat'); 72 | 73 | sliped = -tire_Avon(:,2); 74 | lateralLoaded = tire_Avon(:,3).*1e3; 75 | 76 | slipAngle = -12:1e-3:12; 77 | lateralLoad = interp1(sliped,lateralLoaded,slipAngle,'pchip'); 78 | C_alpha = (2.7778/2)*1e-03; % [N/deg] 79 | WHEEL_DIAMETER = 0.33/2; 80 | 81 | beta_FL = 0; 82 | beta_FR = 0; 83 | beta_RL = 0; 84 | beta_RR = 0; 85 | 86 | % ========================================================================= 87 | % DOUBLE LANE CHANGE TEST % 88 | % Description: Test is performed at the speed of 80 km/h and maintains that 89 | % speed along the test 90 | % ========================================================================= 91 | speed = 80; % [km/h] 92 | steer = 0; 93 | psi = 0; 94 | 95 | % Generating control reference parameters for different test parts 96 | % PART 1 - first straight 97 | [orientationVector timeVector] = generateOrientation(0,0,0,0.34,dt,'step'); 98 | % PART 2 - steering to the left 99 | [orientationVector timeVector] = ... 100 | generateOrientation(orientationVector,timeVector,18,0.55,dt,'ramp'); 101 | % PART 3 - going straight before turning right to enter into second lane 102 | [orientationVector timeVector] = ... 103 | generateOrientation(orientationVector,timeVector,18,0.065,dt,'step'); 104 | % PART 4 - steering to the right to enter second lane 105 | [orientationVector timeVector] = ... 106 | generateOrientation(orientationVector,timeVector,0,0.55,dt,'ramp'); 107 | % PART 5 - second straight 108 | [orientationVector timeVector] = ... 109 | generateOrientation(orientationVector,timeVector,0,0.06,dt,'step'); 110 | % PART 6 - steering to the right, exiting second straight 111 | [orientationVector timeVector] = ... 112 | generateOrientation(orientationVector,timeVector,-14,0.55,dt,'ramp'); 113 | % PART 7 - going straight before turning left to enter into first lane 114 | [orientationVector timeVector] = ... 115 | generateOrientation(orientationVector,timeVector,-14,0.05,dt,'step'); 116 | % PART 8 - steering to the left to enter first lane again 117 | [orientationVector timeVector] = ... 118 | generateOrientation(orientationVector,timeVector,0,0.55,dt,'ramp'); 119 | % PART 9 - vehicle returned to the first lane, going straight again 120 | [orientationVector timeVector] = ... 121 | generateOrientation(orientationVector,timeVector,0,.45,dt,'step'); 122 | 123 | % Mass distribution on each wheel 124 | m1 = m1_s; % Front left 125 | m2 = m2_s; % Front rigth 126 | m3 = m3_s; % Rear left 127 | m4 = m4_s; % Rear right 128 | 129 | % ========================================================================= 130 | % Packing all parameters into one vector variable 131 | % Description: 132 | % >> vehicle parameters - length, width, CG, masses, orientation 133 | % >> simulation parameters - initial steering position, max speed, max 134 | % speed of steering... 135 | % >> tire parameters - sleep angle of each wheel 136 | % (FL - front left; FR - front right) 137 | % (RL - rear left; RR - rear right) 138 | % >> wheel location - initial location of CG and vehicle 139 | % orientation 140 | % ========================================================================= 141 | vehicle_Params = [L T_front T_rear CG_height a b mass m1_s m2_s m3_s m4_s psi]; 142 | sym_Params = [steer(1) speed max_steer max_speed C_alpha g dt]; 143 | tire_Params = [beta_FL beta_FR beta_RL beta_RR]; 144 | wheel_location = [-a,0]; 145 | 146 | % ========================================================================= 147 | % INIT START LOCATION 148 | % Description: Calculate initial position of center of gravity (CG), car 149 | % center (CC), position of each wheel 150 | % 151 | % NOTICE: Movement is placed only in xy-plane ! ! ! 152 | % ========================================================================= 153 | x_CG(1) = 0; 154 | y_CG(1) = 0; 155 | 156 | x_CC(1) = -b*cos(psi(1)); 157 | y_CC(1) = -b*sin(psi(1)); 158 | 159 | x_FR(1) = x_CG(1)+sqrt(a^2+(T_front/2)^2)*cos(psi(1)-atan((T_front/2)/a)); 160 | y_FR(1) = y_CG(1)+sqrt(a^2+(T_front/2)^2)*sin(psi(1)-atan((T_front/2)/a)); 161 | 162 | x_RR(1) = x_CG(1)-b*cos(psi(1))+(T_rear/2)*sin(psi(1)); 163 | y_RR(1) = y_CG(1)-b*sin(psi(1))-(T_rear/2)*cos(psi(1)); 164 | 165 | x_FL(1) = x_FR(1)-T_front*sin(psi(1)); 166 | y_FL(1) = y_FR(1)+T_front*cos(psi(1)); 167 | 168 | x_RL(1) = x_RR(1)-T_rear*sin(psi(1)); 169 | y_RL(1) = y_RR(1)+T_rear*cos(psi(1)); 170 | 171 | d_psi = 0; sigma_CG = 0; 172 | 173 | m1 = m1_s; m2 = m2_s; 174 | m3 = m3_s; m4 = m4_s; 175 | 176 | dm_longitudinal = [0; 0]; 177 | dm_lateral = [0; 0]; 178 | 179 | FL_speed = 0; 180 | FR_speed = 0; 181 | RL_speed = 0; 182 | RR_speed = 0; 183 | wheel_speeds = [FL_speed' FR_speed' RL_speed' RR_speed']; 184 | 185 | acp_CG = 0; acp_psi = 0; 186 | 187 | % ========================================================================= 188 | % START SIMULATION 189 | % Description: For specific angles and orientations refer to the figure of 190 | % car model -> (../Figures/Vehicle_angles_newFont.png) 191 | % ========================================================================= 192 | time = 0; 193 | e = []; % car orientation error in comparison to the reference 194 | u = []; % steering control 195 | lastState = 0; 196 | for position = 1:length(timeVector) 197 | if((mod(timeVector(position),interrupt))==0) 198 | e(position) = round((orientationVector(position)-psi(position)*180/pi)*1e4)/1e4; 199 | u(position) = Kp*e(position)+Kp*dt*e(position)/Ti; 200 | % u(position) = e(position); 201 | 202 | % ! ! ! Notice ! ! ! 203 | % Always saturate max steering angle and steering angle speed 204 | if(position>1) 205 | if(u(position)>0 && u(position-1)>0 &&... 206 | u(position)-u(position-1)>max_Dsteer) 207 | 208 | u(position) = u(position-1)+max_Dsteer; 209 | else if(u(position)>0 && u(position-1)>0 &&... 210 | u(position)-u(position-1)<-max_Dsteer) 211 | u(position) = u(position-1)-max_Dsteer; 212 | end 213 | end 214 | if(u(position)<0 && u(position-1)<0 &&... 215 | u(position)-u(position-1)<-max_Dsteer) 216 | 217 | u(position) = u(position-1)-max_Dsteer; 218 | else if(u(position)<0 && u(position-1)<0 &&... 219 | u(position)-u(position-1)>max_Dsteer) 220 | u(position) = u(position-1)+max_Dsteer; 221 | end 222 | end 223 | 224 | if(u(position)>0 && u(position-1)<0 &&... 225 | u(position)-u(position-1)>max_Dsteer) 226 | 227 | u(position) = u(position-1)+max_Dsteer; 228 | else if(u(position)<0 && u(position-1)>0 &&... 229 | u(position)-u(position-1)<-max_Dsteer) 230 | u(position) = u(position-1)-max_Dsteer; 231 | end 232 | end 233 | else 234 | if(u(position)>max_Dsteer) 235 | u(position) = max_Dsteer; 236 | else if(u(position)<-max_Dsteer) 237 | u(position) = -max_Dsteer; 238 | end 239 | end 240 | end 241 | 242 | if(u(position)>135) 243 | u(position) = 135; 244 | else if(u(position)<-135) 245 | u(position) = -135; 246 | end 247 | end 248 | else 249 | e(position) = e(position-1); 250 | u(position) = Kp*e(position)+Kp*dt*e(position)/Ti; 251 | end 252 | steer = [steer; u(position)]; 253 | 254 | % Store new recalculated parameters of the car 255 | vehicle_Params = [L T_front T_rear CG_height a b mass m1(position) m2(position) m3(position) m4(position) psi(position)]; 256 | sym_Params = [steer(position) speed(1) max_steer max_speed C_alpha g dt]; 257 | tire_Params = [beta_FL(position) beta_FR(position) beta_RL(position) beta_RR(position)]; 258 | wheel_location = [x_CG(position) y_CG(position)... 259 | x_FR(position) y_FR(position)... 260 | x_RR(position) y_RR(position)... 261 | x_FL(position) y_FL(position)... 262 | x_RL(position) y_RL(position)]; 263 | 264 | % Move the car to the new position 265 | [output lastState] = wheel_move(wheel_speeds,vehicle_Params,sym_Params,tire_Params,wheel_location,slipAngle,lateralLoad,lastState); 266 | 267 | x_FL = [x_FL; output(2,1)]; y_FL = [y_FL; output(2,7)]; 268 | x_FR = [x_FR; output(2,2)]; y_FR = [y_FR; output(2,8)]; 269 | x_RL = [x_RL; output(2,3)]; y_RL = [y_RL; output(2,9)]; 270 | x_RR = [x_RR; output(2,4)]; y_RR = [y_RR; output(2,10)]; 271 | x_CG = [x_CG; output(2,5)]; y_CG = [y_CG; output(2,11)]; 272 | x_CC = [x_CC; output(2,6)]; y_CC = [y_CC; output(2,12)]; 273 | 274 | psi = [psi; output(2,13)]; d_psi = [d_psi; output(2,14)]; 275 | 276 | beta_FL = [beta_FL; output(2,15)]; 277 | beta_FR = [beta_FR; output(2,16)]; 278 | beta_RL = [beta_RL; output(2,17)]; 279 | beta_RR = [beta_RR; output(2,18)]; 280 | 281 | sigma_CG = [sigma_CG; output(2,19)]; %time = [time; output(2,20)]; 282 | 283 | m1 = [m1; output(2,21)]; m2 = [m2; output(2,22)]; 284 | m3 = [m3; output(2,23)]; m4 = [m4; output(2,24)]; 285 | dm_longitudinal = [dm_longitudinal output(:,27)]; 286 | dm_lateral = [dm_lateral -output(:,28)]; 287 | 288 | FL_speed = [FL_speed output(2,29)/WHEEL_DIAMETER]; 289 | FR_speed = [FR_speed output(2,30)/WHEEL_DIAMETER]; 290 | RL_speed = [RL_speed output(2,31)/WHEEL_DIAMETER]; 291 | RR_speed = [RR_speed output(2,32)/WHEEL_DIAMETER]; 292 | wheel_speeds = [FL_speed(end) FR_speed(end) RL_speed(end) RR_speed(end)]; 293 | 294 | acp_CG = [acp_CG; output(2,25)]; acp_psi = [acp_psi; output(2,26)]; 295 | time = [time time(end)+dt]; 296 | end 297 | 298 | wheel_speeds = [FL_speed' FR_speed' RL_speed' RR_speed']; 299 | 300 | x = [x_FL x_FR x_RL x_RR x_CG x_CC]; 301 | y = [y_FL y_FR y_RL y_RR y_CG y_CC]; 302 | 303 | m = [m1 m2 m3 m4 mass.*ones(length(time),1) dm_lateral' dm_longitudinal']; % size = length(time) x (4+1+2+2) 304 | beta = [beta_FL beta_FR beta_RL beta_RR]; 305 | 306 | vehicle_Params = [L T_front T_rear CG_height a b g speed dt]; 307 | tire_Params = [C_alpha WHEEL_DIAMETER]; 308 | 309 | orientationVector = [orientationVector; orientationVector(end)]; 310 | timeVector = [timeVector; timeVector(end)]; 311 | 312 | % Pack all the data and prepare to plot 313 | data = [x y m beta wheel_speeds psi d_psi acp_psi acp_CG sigma_CG steer time' orientationVector timeVector]; 314 | disp(['Total time for ISO-standard test: ', num2str(max(time)),'s.']); 315 | disp(['Distance: ', num2str(max(x_CG)),'m.']); 316 | disp(['Max lateral G: ', num2str(max(acp_CG)/g),'G.']); 317 | 318 | % Plot the Double Lane Change Test executed simulation 319 | plotData(data,vehicle_Params,tire_Params,'DLC'); -------------------------------------------------------------------------------- /Matlab/wheel_move.m: -------------------------------------------------------------------------------- 1 | % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % 2 | % 3 | % Copyright (c) * 4 | % All rights reserved by Milan Tepic 5 | % 6 | % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % 7 | % 8 | % Function: wheel_move 9 | % Input parameteres: 10 | % - wheel_speeds >> Angular speed of each wheel generated from previous simulations (currently unused so far, since the control of vehicle is not implemented to control wheels, only steering wheel). For such improvements, it is needed to add momentum equations about load and speed of each wheel over the whole system (vehicle). System is already too non-linear, therefor this was not implemented due to deadline to finish final Bachelor thesis. This is one of the following steps of improvements. 11 | % - vehicle_Params >> 12 | % - sym_Params >> 13 | % - tire_Params >> 14 | % - wheel_location >> 15 | % - slipAngle >> 16 | % - lateralLoad >> 17 | % - laststate >> 18 | % Return parameteres: 19 | % - output << new generated states and dynamical parameters used in the next iteration 20 | % - state_Num << {0,1,2} <=> {'straight', 'left', 'right'} 21 | % 22 | % 23 | % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % 24 | % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % 25 | % 26 | % 27 | % DESCRIPTION 28 | % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % 29 | % 30 | % Calculates movement of each wheel separatelly according 31 | % to the vehicle geometry and different parameters. 32 | % 33 | % 34 | % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % 35 | % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % 36 | % 37 | % 38 | % HISTORY 39 | % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % 40 | % Version: 1.7 41 | % Author/Date: Milan Tepic / 2017-02-07 42 | % Change: Initial version 43 | % 44 | % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % 45 | % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % 46 | 47 | function [output,state_Num] = wheel_move(wheel_speeds,vehicle_Params,sym_Params,tire_Params,wheel_location,slipAngle,lateralLoad,laststate) 48 | 49 | % INIT 50 | x_CC = []; y_CC = []; x_FC = []; y_FC = []; R_FC = []; 51 | sigma_CG = []; v_tan = []; omega = []; d_psi = []; 52 | acp_CG = []; acp_psi = []; 53 | dm_lateral_front = []; dm_lateral_rear = []; 54 | dm_longitudinal_front = []; dm_longitudinal_rear = []; 55 | 56 | x_start = wheel_location(1); 57 | y_start = wheel_location(2); 58 | 59 | L = vehicle_Params(1); 60 | T_front = vehicle_Params(2); 61 | T_rear = vehicle_Params(3); 62 | CG_height = vehicle_Params(4); 63 | a = vehicle_Params(5); 64 | b = vehicle_Params(6); 65 | mass = vehicle_Params(7); 66 | m1 = vehicle_Params(8); 67 | m2 = vehicle_Params(9); 68 | m3 = vehicle_Params(10); 69 | m4 = vehicle_Params(11); 70 | psi = vehicle_Params(12); 71 | 72 | steer = sym_Params(1); 73 | speed = sym_Params(2); 74 | max_steer = sym_Params(3); 75 | max_speed = sym_Params(4); 76 | C_alpha = sym_Params(5); 77 | g = sym_Params(6); 78 | dt = sym_Params(7); 79 | 80 | beta_FL = tire_Params(1); 81 | beta_FR = tire_Params(2); 82 | beta_RL = tire_Params(3); 83 | beta_RR = tire_Params(4); 84 | 85 | m1_s = mass*(b/L)/2; % [kg] 86 | m2_s = mass*(b/L)/2; % [kg] 87 | m3_s = mass*(a/L)/2; % [kg] 88 | m4_s = mass*(a/L)/2; % [kg] 89 | 90 | % TELEMETRY 91 | v = speed*1000/3600; 92 | delta = steer*pi/180; 93 | 94 | sigma_FL = 0; 95 | sigma_FR = 0; 96 | sigma_RR = 0; 97 | sigma_RL = 0; 98 | 99 | k_F = 0; n_F = 0; 100 | k_R = 0; n_R = 0; 101 | 102 | x_CG = wheel_location(1); 103 | y_CG = wheel_location(2); 104 | 105 | x_FR = wheel_location(3); 106 | y_FR = wheel_location(4); 107 | 108 | x_RR = wheel_location(5); 109 | y_RR = wheel_location(6); 110 | 111 | x_FL = wheel_location(7); 112 | y_FL = wheel_location(8); 113 | 114 | x_RL = wheel_location(9); 115 | y_RL = wheel_location(10); 116 | 117 | v_frontLeft = wheel_speeds(1); 118 | v_frontRight = wheel_speeds(2); 119 | v_rearLeft = wheel_speeds(3); 120 | v_rearRight = wheel_speeds(4); 121 | 122 | dw = 20; 123 | 124 | wheel_speeds = [wheel_speeds; wheel_speeds]; 125 | 126 | counter = 1; 127 | 128 | if(delta(counter)>0 || laststate==1) 129 | if(T_front>=T_rear) 130 | x_FR = L; y_FR = 0; 131 | x_RR = 0; y_RR = (T_front-T_rear)/2; 132 | 133 | x_FL = L; y_FL = T_front; 134 | x_RL = 0; y_RL = T_rear+(T_front-T_rear)/2; 135 | 136 | x_G = b; y_G = T_front/2; 137 | else 138 | x_FR = L; y_FR = (T_rear-T_front)/2; 139 | x_RR = 0; y_RR = 0; 140 | 141 | x_FL = L; y_FL = T_front+(T_front-T_rear)/2; 142 | x_RL = 0; y_RL = T_rear; 143 | 144 | x_G = b; y_G = T_rear/2; 145 | end 146 | 147 | state = 'left'; 148 | if(laststate~=0) 149 | state_Num = 1; 150 | else 151 | state_Num = 0; 152 | end 153 | m_FL = m1(counter); 154 | m_FR = m2(counter); 155 | m_RL = m3(counter); 156 | m_RR = m4(counter); 157 | 158 | sigma_FL(counter) = delta(counter)-beta_FL(counter); 159 | sigma_FR(counter) = delta(counter)-beta_FR(counter); 160 | sigma_RL(counter) = -beta_RL(counter); 161 | sigma_RR(counter) = -beta_RR(counter); 162 | 163 | k_R(counter) = tan(sigma_RR(counter)-pi/2); 164 | n_R(counter) = y_RR-x_RR*k_R(counter); 165 | 166 | k_F(counter) = tan(sigma_FR(counter)-pi/2); 167 | n_F(counter) = y_FR-x_FR*k_F(counter); 168 | 169 | x_CC(counter) = (n_F(counter)-n_R(counter))/(k_R(counter)-k_F(counter)); 170 | y_CC(counter) = x_CC(counter)*k_F(counter)+n_F(counter); 171 | 172 | x_FC(counter) = x_CC(counter); 173 | y_FC(counter) = y_CG(counter); 174 | 175 | R_FC(counter) = y_FC(counter)-y_CC(counter); 176 | sigma_CG(counter) = atan((x_G-x_CC)/(y_CC-y_G)); 177 | 178 | 179 | R_FR = sqrt((y_FR-y_CC(counter))^2+(x_FR-x_CC(counter))^2); 180 | R_RR = sqrt((y_RR-y_CC(counter))^2+(x_RR-x_CC(counter))^2); 181 | 182 | R_FL = sqrt((y_FL-y_CC(counter))^2+(x_FL-x_CC(counter))^2); 183 | R_RL = sqrt((y_RL-y_CC(counter))^2+(x_RL-x_CC(counter))^2); 184 | 185 | R_CG = sqrt((y_G-y_CC(counter))^2+(x_G-x_CC(counter))^2); 186 | 187 | %v_tan(counter) = v_frontRight*R_CG/R_FR; 188 | 189 | v_tan(counter) = v/cos(sigma_CG(counter)); 190 | omega(counter) = v_tan(counter)/R_CG; 191 | d_psi(counter) = omega(counter)*dt; 192 | d_S = R_CG*d_psi(counter); 193 | dx_CG = d_S*cos(sigma_CG(counter)+d_psi(counter)); 194 | dy_CG = d_S*sin(sigma_CG(counter)+d_psi(counter)); 195 | 196 | v_frontLeft = v_tan(counter)*R_FL/R_CG; 197 | v_rearLeft = v_tan(counter)*R_RL/R_CG; 198 | 199 | v_frontRight = v_tan(counter)*R_FR/R_CG; 200 | v_rearRight = v_tan(counter)*R_RR/R_CG; 201 | 202 | F_fl = (m_FL*v_frontLeft^2/R_FL); 203 | if(F_fl>=min(lateralLoad) && F_fl<=max(lateralLoad)) 204 | position = find(lateralLoad<=F_fl); 205 | slipFrontLeft = slipAngle(position(end)); 206 | else if(F_fl>max(lateralLoad)) 207 | slipFrontLeft = max(slipAngle); 208 | else 209 | slipFrontLeft = min(slipAngle); 210 | end 211 | end 212 | beta_FL(counter+1) = slipFrontLeft*pi/180; 213 | 214 | F_fr = (m_FR*v_frontRight^2/R_FR); 215 | if(F_fr>=min(lateralLoad) && F_fr<=max(lateralLoad)) 216 | position = find(lateralLoad<=F_fr); 217 | slipFrontRight = slipAngle(position(end)); 218 | else if(F_fr>max(lateralLoad)) 219 | slipFrontRight = max(slipAngle); 220 | else 221 | slipFrontRight = min(slipAngle); 222 | end 223 | end 224 | beta_FR(counter+1) = slipFrontRight*pi/180; 225 | 226 | F_rl = (m_RL*v_rearLeft^2/R_RL); 227 | if(F_rl>=min(lateralLoad) && F_rl<=max(lateralLoad)) 228 | position = find(lateralLoad<=F_rl); 229 | slipRearLeft = slipAngle(position(end)); 230 | else if(F_rl>max(lateralLoad)) 231 | slipRearLeft = max(slipAngle); 232 | else 233 | slipRearLeft = min(slipAngle); 234 | end 235 | end 236 | beta_RL(counter+1) = slipRearLeft*pi/180; 237 | 238 | F_rr = (m_RR*v_rearRight^2/R_RR); 239 | if(F_rr>=min(lateralLoad) && F_rr<=max(lateralLoad)) 240 | position = find(lateralLoad<=F_rr); 241 | slipRearRight = slipAngle(position(end)); 242 | else if(F_rr>max(lateralLoad)) 243 | slipRearRight = max(slipAngle); 244 | else 245 | slipRearRight = min(slipAngle); 246 | end 247 | end 248 | beta_RR(counter+1) = slipRearRight*pi/180; 249 | 250 | psi(counter+1) = psi(counter)+d_psi(counter); 251 | x_CG(counter+1) = x_CG(counter)+dx_CG*cos(psi(counter+1))-dy_CG*sin(psi(counter+1)); 252 | y_CG(counter+1) = y_CG(counter)+dx_CG*sin(psi(counter+1))+dy_CG*cos(psi(counter+1)); 253 | 254 | x_FR(counter+1) = x_CG(counter+1)+sqrt(a^2+(T_front/2)^2)*cos(psi(counter+1)-atan((T_front/2)/a)); 255 | y_FR(counter+1) = y_CG(counter+1)+sqrt(a^2+(T_front/2)^2)*sin(psi(counter+1)-atan((T_front/2)/a)); 256 | 257 | % x_RR(counter+1) = x_FR(counter+1)-L*cos(psi(counter+1)); 258 | % y_RR(counter+1) = y_FR(counter+1)-L*sin(psi(counter+1)); 259 | 260 | x_RR(counter+1) = x_CG(counter+1)-b*cos(psi(counter+1))+(T_rear/2)*sin(psi(counter+1)); 261 | y_RR(counter+1) = y_CG(counter+1)-b*sin(psi(counter+1))-(T_rear/2)*cos(psi(counter+1)); 262 | 263 | x_FL(counter+1) = x_FR(counter+1)-T_front*sin(psi(counter+1)); 264 | y_FL(counter+1) = y_FR(counter+1)+T_front*cos(psi(counter+1)); 265 | 266 | x_RL(counter+1) = x_RR(counter+1)-T_rear*sin(psi(counter+1)); 267 | y_RL(counter+1) = y_RR(counter+1)+T_rear*cos(psi(counter+1)); 268 | 269 | else if(delta(counter)<0 || laststate==2) 270 | 271 | if(T_front>=T_rear) 272 | x_FR = L; y_FR = 0; 273 | x_RR = 0; y_RR = (T_front-T_rear)/2; 274 | 275 | x_FL = L; y_FL = T_front; 276 | x_RL = 0; y_RL = T_rear+(T_front-T_rear)/2; 277 | 278 | x_G = b; y_G = T_front/2; 279 | else 280 | x_FR = L; y_FR = (T_rear-T_front)/2; 281 | x_RR = 0; y_RR = 0; 282 | 283 | x_FL = L; y_FL = T_front+(T_front-T_rear)/2; 284 | x_RL = 0; y_RL = T_rear; 285 | 286 | x_G = b; y_G = T_rear/2; 287 | end 288 | 289 | state = 'right'; 290 | if(laststate~=0) 291 | state_Num = 2; 292 | else 293 | state_Num = 0; 294 | end 295 | m_FL = m1(counter); 296 | m_FR = m2(counter); 297 | m_RL = m3(counter); 298 | m_RR = m4(counter); 299 | 300 | sigma_FL(counter) = delta(counter)+beta_FL(counter); 301 | sigma_RL(counter) = beta_RL(counter); 302 | 303 | k_R(counter) = tan(sigma_RL(counter)-pi/2); 304 | n_R(counter) = y_RL-x_RL*k_R(counter); 305 | 306 | k_F(counter) = tan(sigma_FL(counter)-pi/2); 307 | n_F(counter) = y_FL-x_FL*k_F(counter); 308 | 309 | x_CC(counter) = (n_F(counter)-n_R(counter))/(k_R(counter)-k_F(counter)); 310 | y_CC(counter) = x_CC(counter)*k_F(counter)+n_F(counter); 311 | 312 | x_FC(counter) = x_CC(counter); 313 | y_FC(counter) = y_CG(counter); 314 | 315 | R_FC(counter) = y_FC(counter)-y_CC(counter); 316 | sigma_CG(counter) = atan((x_G(counter)-x_CC(counter))/(y_CC(counter)-y_G(counter))); 317 | 318 | R_FL = sqrt((y_FL-y_CC(counter))^2+(x_FL-x_CC(counter))^2); 319 | R_RL = sqrt((y_RL-y_CC(counter))^2+(x_RL-x_CC(counter))^2); 320 | 321 | R_FR = sqrt((y_FR-y_CC(counter))^2+(x_FR-x_CC(counter))^2); 322 | R_RR = sqrt((y_RR-y_CC(counter))^2+(x_RR-x_CC(counter))^2); 323 | 324 | R_CG = sqrt((y_G-y_CC(counter))^2+(x_G-x_CC(counter))^2); 325 | 326 | v_tan(counter) = v/cos(sigma_CG(counter)); 327 | omega(counter) = v_tan(counter)/R_CG; 328 | d_psi(counter) = -omega(counter)*dt; 329 | d_S = R_CG*d_psi(counter); %*sign(delta(counter)); 330 | dx_CG = -d_S*cos(sigma_CG(counter)+d_psi(counter)); 331 | dy_CG = d_S*sin(sigma_CG(counter)+d_psi(counter)); 332 | 333 | v_frontLeft = v_tan(counter)*R_FL/R_CG; 334 | v_rearLeft = v_tan(counter)*R_RL/R_CG; 335 | 336 | v_frontRight = v_tan(counter)*R_FR/R_CG; 337 | v_rearRight = v_tan(counter)*R_RR/R_CG; 338 | 339 | F_fl = (m_FL*v_frontLeft^2/R_FL); 340 | if(F_fl>=min(lateralLoad) && F_fl<=max(lateralLoad)) 341 | position = find(lateralLoad<=F_fl); 342 | slipFrontLeft = slipAngle(position(end)); 343 | else if(F_fl>max(lateralLoad)) 344 | slipFrontLeft = max(slipAngle); 345 | else 346 | slipFrontLeft = min(slipAngle); 347 | end 348 | end 349 | beta_FL(counter+1) = slipFrontLeft*pi/180; 350 | 351 | F_fr = (m_FR*v_frontRight^2/R_FR); 352 | if(F_fr>=min(lateralLoad) && F_fr<=max(lateralLoad)) 353 | position = find(lateralLoad<=F_fr); 354 | slipFrontRight = slipAngle(position(end)); 355 | else if(F_fr>max(lateralLoad)) 356 | slipFrontRight = max(slipAngle); 357 | else 358 | slipFrontRight = min(slipAngle); 359 | end 360 | end 361 | beta_FR(counter+1) = slipFrontRight*pi/180; 362 | 363 | F_rl = (m_RL*v_rearLeft^2/R_RL); 364 | if(F_rl>=min(lateralLoad) && F_rl<=max(lateralLoad)) 365 | position = find(lateralLoad<=F_rl); 366 | slipRearLeft = slipAngle(position(end)); 367 | else if(F_rl>max(lateralLoad)) 368 | slipRearLeft = max(slipAngle); 369 | else 370 | slipRearLeft = min(slipAngle); 371 | end 372 | end 373 | beta_RL(counter+1) = slipRearLeft*pi/180; 374 | 375 | F_rr = (m_RR*v_rearRight^2/R_RR); 376 | if(F_rr>=min(lateralLoad) && F_rr<=max(lateralLoad)) 377 | position = find(lateralLoad<=F_rr); 378 | slipRearRight = slipAngle(position(end)); 379 | else if(F_rr>max(lateralLoad)) 380 | slipRearRight = max(slipAngle); 381 | else 382 | slipRearRight = min(slipAngle); 383 | end 384 | end 385 | beta_RR(counter+1) = slipRearRight*pi/180; 386 | 387 | psi(counter+1) = psi(counter)+d_psi(counter); 388 | x_CG(counter+1) = x_CG(counter)+dx_CG*cos(psi(counter+1))-dy_CG*sin(psi(counter+1)); 389 | y_CG(counter+1) = y_CG(counter)+dx_CG*sin(psi(counter+1))+dy_CG*cos(psi(counter+1)); 390 | 391 | x_FR(counter+1) = x_CG(counter+1)+sqrt(a^2+(T_front/2)^2)*cos(psi(counter+1)-atan((T_front/2)/a)); 392 | y_FR(counter+1) = y_CG(counter+1)+sqrt(a^2+(T_front/2)^2)*sin(psi(counter+1)-atan((T_front/2)/a)); 393 | 394 | % x_RR(counter+1) = x_FR(counter+1)-L*cos(psi(counter+1)); 395 | % y_RR(counter+1) = y_FR(counter+1)-L*sin(psi(counter+1)); 396 | 397 | x_RR(counter+1) = x_CG(counter+1)-b*cos(psi(counter+1))+(T_rear/2)*sin(psi(counter+1)); 398 | y_RR(counter+1) = y_CG(counter+1)-b*sin(psi(counter+1))-(T_rear/2)*cos(psi(counter+1)); 399 | 400 | x_FL(counter+1) = x_FR(counter+1)-T_front*sin(psi(counter+1)); 401 | y_FL(counter+1) = y_FR(counter+1)+T_front*cos(psi(counter+1)); 402 | 403 | x_RL(counter+1) = x_RR(counter+1)-T_rear*sin(psi(counter+1)); 404 | y_RL(counter+1) = y_RR(counter+1)+T_rear*cos(psi(counter+1)); 405 | 406 | else 407 | state = 'straight'; 408 | state_Num = 0; 409 | 410 | sigma_FL(counter)=0; 411 | sigma_FR(counter)=0; 412 | sigma_RL(counter)=0; 413 | sigma_RR(counter)=0; 414 | 415 | x_CC(counter) = 0; 416 | y_CC(counter) = 0; 417 | 418 | x_FC(counter) = x_CG(counter); 419 | y_FC(counter) = y_CG(counter); 420 | 421 | sigma_CG(counter) = 0; 422 | 423 | R_CG = 0; 424 | v_tan(counter) = v; 425 | omega(counter) = 0; 426 | d_psi(counter) = 0; 427 | d_S = v_tan(counter)*dt; 428 | dx_CG = d_S*cos(psi(counter)); 429 | dy_CG = d_S*sin(psi(counter)); 430 | 431 | R_F = 0; 432 | R_R = 0; 433 | v_frontLeft = v_tan(counter); 434 | v_frontRight = v_tan(counter); 435 | v_rearLeft = v_tan(counter); 436 | v_rearRight = v_tan(counter); 437 | 438 | beta_FL(counter+1) = 0; 439 | beta_FR(counter+1) = 0; 440 | beta_RL(counter+1) = 0; 441 | beta_RR(counter+1) = 0; 442 | 443 | psi(counter+1) = psi(counter); 444 | x_CG(counter+1) = x_CG(counter)+dx_CG; 445 | y_CG(counter+1) = y_CG(counter)+dy_CG; 446 | 447 | x_FR(counter+1) = x_CG(counter+1)+sqrt(a^2+(T_front/2)^2)*cos(psi(counter+1)-atan((T_front/2)/a)); 448 | y_FR(counter+1) = y_CG(counter+1)+sqrt(a^2+(T_front/2)^2)*sin(psi(counter+1)-atan((T_front/2)/a)); 449 | 450 | x_RR(counter+1) = x_CG(counter+1)-b*cos(psi(counter+1))+(T_rear/2)*sin(psi(counter+1)); 451 | y_RR(counter+1) = y_CG(counter+1)-b*sin(psi(counter+1))-(T_rear/2)*cos(psi(counter+1)); 452 | 453 | x_FL(counter+1) = x_FR(counter+1)-T_front*sin(psi(counter+1)); 454 | y_FL(counter+1) = y_FR(counter+1)+T_front*cos(psi(counter+1)); 455 | 456 | x_RL(counter+1) = x_RR(counter+1)-T_rear*sin(psi(counter+1)); 457 | y_RL(counter+1) = y_RR(counter+1)+T_rear*cos(psi(counter+1)); 458 | end 459 | end 460 | 461 | if(R_CG~=0) 462 | acp_CG(counter) = v_tan(counter)^2/R_CG; 463 | else 464 | acp_CG(counter) = 0; 465 | end 466 | acp_psi(counter) = sigma_CG(counter)+pi/2*(-sign(delta(counter))); 467 | 468 | %dm_lateral(counter) = (mass*(acp_CG(counter)/g)*sin(acp_psi(counter)))/2; 469 | %dm_longitudinal(counter) = (mass*(acp_CG(counter)/g)*abs(cos(acp_psi(counter))))/2; 470 | 471 | %dm_lateral_front(counter) = (-sign(delta(counter))*mass*acp_CG(counter)*abs(sin(acp_psi(counter)))*CG_height/T_front)/g; 472 | %dm_lateral_rear(counter) = (-sign(delta(counter))*mass*acp_CG(counter)*abs(sin(acp_psi(counter)))*CG_height/T_rear)/g; 473 | %dm_longitudinal_front(counter) = (mass*acp_CG(counter)*cos(acp_psi(counter))*CG_height/a)/g; 474 | %dm_longitudinal_rear(counter) = (-mass*acp_CG(counter)*cos(acp_psi(counter))*CG_height/b)/g; 475 | 476 | load_direction = -sign(delta(counter)); 477 | arb_front = 1; 478 | arb_rear = 1; 479 | 480 | dm_lateral_front(counter) = (acp_CG(counter)*abs(sin(acp_psi(counter)))*CG_height/T_front)/g; 481 | dm_lateral_front(counter) = dm_lateral_front(counter)*load_direction; 482 | if(dm_lateral_front(counter)>0.5) 483 | dm_lateral_front(counter)=0.5; 484 | else if(dm_lateral_front(counter)<-0.5) 485 | dm_lateral_front(counter)=-0.5; 486 | end 487 | end 488 | 489 | dm_lateral_rear(counter) = (acp_CG(counter)*abs(sin(acp_psi(counter)))*CG_height/T_rear)/g; 490 | dm_lateral_rear(counter) = dm_lateral_rear(counter)*load_direction; 491 | if(dm_lateral_rear(counter)>0.5) 492 | dm_lateral_rear(counter)=0.5; 493 | else if(dm_lateral_rear(counter)<-0.5) 494 | dm_lateral_rear(counter)=-0.5; 495 | end 496 | end 497 | 498 | dm_longitudinal_front(counter) = b/L*(-acp_CG(counter)*cos(acp_psi(counter))*CG_height/L)/g; 499 | dm_longitudinal_rear(counter) = a/L*(acp_CG(counter)*cos(acp_psi(counter))*CG_height/L)/g; 500 | 501 | % dm_longitudinal_front(counter) = (acp_CG(counter)*abs(cos(acp_psi(counter)+pi/2))*CG_height/L)/g; 502 | % if(dm_longitudinal_front(counter)>0.5) 503 | % dm_longitudinal_front(counter)=0.5; 504 | % else if(dm_longitudinal_front(counter)<-0.5) 505 | % dm_longitudinal_front(counter)=-0.5; 506 | % end 507 | % end 508 | % dm_longitudinal_rear(counter) = -dm_longitudinal_front(counter); 509 | 510 | m1(counter+1) = (0.5+dm_lateral_front(counter))*arb_front*(m1_s+m2_s)+mass*dm_longitudinal_front(counter); 511 | m2(counter+1) = (0.5-dm_lateral_front(counter))*arb_front*(m1_s+m2_s)+mass*dm_longitudinal_front(counter); 512 | m3(counter+1) = (0.5+dm_lateral_rear(counter))*arb_rear*(m3_s+m4_s)+mass*dm_longitudinal_rear(counter); 513 | m4(counter+1) = (0.5-dm_lateral_rear(counter))*arb_rear*(m3_s+m4_s)+mass*dm_longitudinal_rear(counter); 514 | 515 | % da li i lateral treba ovde deliti sa dva o.O 516 | % m1(counter+1) = (0.5+dm_lateral_front(counter))*arb_front*(m1_s+m2_s)+dm_longitudinal_front(counter)*m1_s; 517 | % m2(counter+1) = (0.5-dm_lateral_front(counter))*arb_front*(m1_s+m2_s)+dm_longitudinal_front(counter)*m2_s; 518 | % m3(counter+1) = (0.5+dm_lateral_rear(counter))*arb_rear*(m3_s+m4_s)+dm_longitudinal_rear(counter)*m3_s; 519 | % m4(counter+1) = (0.5-dm_lateral_rear(counter))*arb_rear*(m3_s+m4_s)+dm_longitudinal_rear(counter)*m4_s; 520 | 521 | if(m1(counter+1)<1) 522 | m1(counter+1) = 1; 523 | end 524 | if(m2(counter+1)<1) 525 | m2(counter+1) = 1; 526 | end 527 | if(m3(counter+1)<1) 528 | m3(counter+1) = 1; 529 | end 530 | if(m4(counter+1)<1) 531 | m4(counter+1) = 1; 532 | end 533 | 534 | if(m1(counter+1)>mass/2-1) 535 | m1(counter+1) = mass/2-1; 536 | end 537 | if(m2(counter+1)>mass/2-1) 538 | m2(counter+1) = mass/2-1; 539 | end 540 | if(m3(counter+1)>mass/2-1) 541 | m3(counter+1) = mass/2-1; 542 | end 543 | if(m4(counter+1)>mass/2-1) 544 | m4(counter+1) = mass/2-1; 545 | end 546 | 547 | sigma_CG = [sigma_CG sigma_CG(end)]; 548 | k_F = [k_F k_F(end)]; 549 | k_R = [k_R k_R(end)]; 550 | 551 | x_CC = [0 x_CC]; 552 | y_CC = [0 y_CC]; 553 | d_psi = [0 d_psi]; 554 | 555 | x = [x_FL' x_FR' x_RL' x_RR' x_CG' x_CC']; 556 | y = [y_FL' y_FR' y_RL' y_RR' y_CG' y_CC']; 557 | beta = [beta_FL' beta_FR' beta_RL' beta_RR']; 558 | 559 | acp_CG = [0 acp_CG]; acp_psi = [0 acp_psi]; 560 | dm_longitudinal = [dm_longitudinal_front dm_longitudinal_rear]; 561 | dm_lateral = [dm_lateral_front dm_lateral_rear]; 562 | 563 | masses = [m1' m2' m3' m4']; 564 | 565 | if(k_R(end)>0) 566 | dt = 0; 567 | end 568 | 569 | wheel_speeds(2,:) = [v_frontLeft v_frontRight v_rearLeft v_rearRight]; 570 | 571 | output = [x y psi' d_psi' beta sigma_CG' [0 dt]' masses acp_CG' acp_psi' dm_longitudinal' dm_lateral' wheel_speeds]; 572 | end -------------------------------------------------------------------------------- /Matlab/wheel_speeds.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Tepic/Vehicle_Model/8dd5c9dee51ae082fd61abec54cda2c15d91eb9c/Matlab/wheel_speeds.mat -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # A model of sport vehicle handling with 4 wheel electric drivetrain 2 | ## Final Bachelor Thesis 3 | 4 | Vehicle Model is based on the vehicle of Road Arrow Formula Student Team from University of Belgrade, Serbia 5 | 6 | Upcoming years will bring radical changes to how we view commuting and traveling. Electric cars and self-driving vehicles are stepping in. In order to keep up with the future in the fast lane, we have to put ourselves in a position of the engineer of the future and be part of that inevitable change. This does not mean to only rehash existing ideas, but also build on them and improve them ourselves. This article is about my work and approach to the solution of one of the major parts of any type of a vehicle so far – TORQUE VECTORING development. My solution was designed for a 4 wheel electric (sport) car. 7 | 8 | 9 | 10 | During my time as a Formula Student member of [Formula Student Road Arrow Team University of Belgrade](http://stt.org.rs/), I had a great time with developing electronics for a combustion car. During that time, we came up with the project of an electric vehicle for the same competition. My goal was to develop a base for a Torque Vectoring algorithm. Most algorithms that I have read are based on real time data sensoring without pre-model. Since our team hadn’t had a sufficient amount of funding to start building a car right away, I started making a physical-mathematical model of the car's behavior. This was my final thesis for my BSc. A model of sport vehicle handling with 4 wheel electric drivetrain. All simulations and implementations have been conducted using Matlab. 11 | 12 | All vehicles modeling starts with a bicycle model. This is very good if we consider a car traveling at a low speed, or to be more precise, taking corners at a low speed. But, in the motosport world, we drive totally opposite – finding a perfect racing line, late braking, starting acceleration from apex etc. Therefore, the car reaches its boundaries. So, I started my model with 2 parallel bicycles which are hard connected. One is for the left side of the car and the second is for the right. I have introduced this approach since it will include weight (mass) transfer and a slip angle. So, the center of the corner will not be aligned with the rear axle, but somewhere towards the middle. Since, outer wheels have more grip during cornering due to higher weight caused by weight transfer and lateral forces, they dictate car handling during cornering. That is the reason for having two bicycle models. 13 | 14 | ![Vehicle Mathematical Model](https://raw.githubusercontent.com/Tepic/Vehicle_Model/master/Figures/Vehicle_angles_newFont.png) 15 | 16 | As seen on the previous picture, the car is driving to the right so the front left wheel, or more precisely the front left tire’s grip and slip, will dictate cornering and the car’s handling. Due to this fact, all calculations will be calculated according to this side of the car, and the right side wheels’ behavior is derived from these calculations. Same applies vice verse for the left hand corner. Parameters are explained as follows: 17 | 18 | Parameters: R - diameter of the curve L - base length δ - steering angle Ψ - yaw β - slip angle α - difference in angles δ(t)- β(t) (xcm, ycm) - coordinates of the vehicles center of mass 19 | 20 | As it is known, tires' contact patch is the only part that interacts with the ground and makes a car go forward (or sideways). Well, this happens when the tire exceeds its lateral force grip and starts to slide. Best racing drivers are able to feel when the car is at its (tire’s) limit and they drive at the maximum slip angle during cornering, moving the pivot point (center of the curve) towards the middle. Unfortunately, tires’ slip angle can be only approximately calculated during racing, but tires factories provide test results of their tires’ slip angle. 21 | 22 | Implementing slip angle and weight transfer influence inside of my algorithm, the results are shown in the following picture: 23 | 24 | ![Double Lane Change Simulation Output](https://raw.githubusercontent.com/Tepic/Vehicle_Model/master/Figures/DLC_test_output.png) 25 | 26 | 27 | This is the ISO3888 double lane change test. On the lower 4 diagrams, the difference of the angular speed of each wheel can be noticed. So, these speeds are references for the electric motors inside each wheel. As I have previously noted, this is only a base model. These values would be corrected during actual driving with a conventional implementation of Torque vectoring using only steering wheel and angular (yaw) sensor. But, using this model, the handling of the car could be foreseen and therefore we would be able to act before sliding happens. Also, thanks to this model (and its weight transfer) we can predict the threshold of wheel speed in traction control and not wait for a wheel to actually slip. 28 | 29 | 30 | 31 | This model misses a suspension model which would improve weight transfer, roll-bar as well, which is part of the real world suspension. My moto is always to come as prepared as possible for upcoming situations – as a driver looks towards the apex of the approaching corner. 32 | 33 | 34 | Wish you a nice racing weekend, good luck! 35 | -------------------------------------------------------------------------------- /Модел кретања спортског возила са електро погоном на свим точковима Torque Vectoring.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Tepic/Vehicle_Model/8dd5c9dee51ae082fd61abec54cda2c15d91eb9c/Модел кретања спортског возила са електро погоном на свим точковима Torque Vectoring.pdf --------------------------------------------------------------------------------