├── .gitattributes ├── Models ├── ImagesforGit │ ├── nst.PNG │ ├── model.PNG │ ├── unrealImage.PNG │ ├── errorResolved.PNG │ ├── thetaComparison.png │ ├── effectofThetaabrupt.PNG │ ├── errorHighVelocity.PNG │ └── smoothThetaComparison.png ├── 2_Stanley_USHighway │ ├── USHighway.mat │ ├── stanleyHighway.slx │ ├── Images │ │ ├── DriverPic.jpg │ │ ├── LookupTable.PNG │ │ ├── visualization.JPG │ │ └── imageforlivescript.jpg │ ├── velocityProfile.mlx │ ├── radius.m │ └── setUpModel.m └── 1_Stanley_SimpleTrack │ ├── setUpModel.mlx │ ├── simpleTrack.mat │ ├── stanleySimple.slx │ └── Images │ ├── DriverPic.jpg │ ├── LookupTable.PNG │ ├── visualization.JPG │ └── imageforlivescript.jpg ├── SECURITY.md ├── license.txt └── README.md /.gitattributes: -------------------------------------------------------------------------------- 1 | # Auto detect text files and perform LF normalization 2 | * text=auto 3 | -------------------------------------------------------------------------------- /Models/ImagesforGit/nst.PNG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mathworks/vehicle-stanley-controller/HEAD/Models/ImagesforGit/nst.PNG -------------------------------------------------------------------------------- /Models/ImagesforGit/model.PNG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mathworks/vehicle-stanley-controller/HEAD/Models/ImagesforGit/model.PNG -------------------------------------------------------------------------------- /Models/ImagesforGit/unrealImage.PNG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mathworks/vehicle-stanley-controller/HEAD/Models/ImagesforGit/unrealImage.PNG -------------------------------------------------------------------------------- /Models/ImagesforGit/errorResolved.PNG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mathworks/vehicle-stanley-controller/HEAD/Models/ImagesforGit/errorResolved.PNG -------------------------------------------------------------------------------- /Models/2_Stanley_USHighway/USHighway.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mathworks/vehicle-stanley-controller/HEAD/Models/2_Stanley_USHighway/USHighway.mat -------------------------------------------------------------------------------- /Models/ImagesforGit/thetaComparison.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mathworks/vehicle-stanley-controller/HEAD/Models/ImagesforGit/thetaComparison.png -------------------------------------------------------------------------------- /Models/1_Stanley_SimpleTrack/setUpModel.mlx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mathworks/vehicle-stanley-controller/HEAD/Models/1_Stanley_SimpleTrack/setUpModel.mlx -------------------------------------------------------------------------------- /Models/ImagesforGit/effectofThetaabrupt.PNG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mathworks/vehicle-stanley-controller/HEAD/Models/ImagesforGit/effectofThetaabrupt.PNG -------------------------------------------------------------------------------- /Models/ImagesforGit/errorHighVelocity.PNG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mathworks/vehicle-stanley-controller/HEAD/Models/ImagesforGit/errorHighVelocity.PNG -------------------------------------------------------------------------------- /Models/1_Stanley_SimpleTrack/simpleTrack.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mathworks/vehicle-stanley-controller/HEAD/Models/1_Stanley_SimpleTrack/simpleTrack.mat -------------------------------------------------------------------------------- /Models/2_Stanley_USHighway/stanleyHighway.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mathworks/vehicle-stanley-controller/HEAD/Models/2_Stanley_USHighway/stanleyHighway.slx -------------------------------------------------------------------------------- /Models/ImagesforGit/smoothThetaComparison.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mathworks/vehicle-stanley-controller/HEAD/Models/ImagesforGit/smoothThetaComparison.png -------------------------------------------------------------------------------- /Models/1_Stanley_SimpleTrack/stanleySimple.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mathworks/vehicle-stanley-controller/HEAD/Models/1_Stanley_SimpleTrack/stanleySimple.slx -------------------------------------------------------------------------------- /Models/2_Stanley_USHighway/Images/DriverPic.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mathworks/vehicle-stanley-controller/HEAD/Models/2_Stanley_USHighway/Images/DriverPic.jpg -------------------------------------------------------------------------------- /Models/2_Stanley_USHighway/velocityProfile.mlx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mathworks/vehicle-stanley-controller/HEAD/Models/2_Stanley_USHighway/velocityProfile.mlx -------------------------------------------------------------------------------- /Models/1_Stanley_SimpleTrack/Images/DriverPic.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mathworks/vehicle-stanley-controller/HEAD/Models/1_Stanley_SimpleTrack/Images/DriverPic.jpg -------------------------------------------------------------------------------- /Models/2_Stanley_USHighway/Images/LookupTable.PNG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mathworks/vehicle-stanley-controller/HEAD/Models/2_Stanley_USHighway/Images/LookupTable.PNG -------------------------------------------------------------------------------- /Models/1_Stanley_SimpleTrack/Images/LookupTable.PNG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mathworks/vehicle-stanley-controller/HEAD/Models/1_Stanley_SimpleTrack/Images/LookupTable.PNG -------------------------------------------------------------------------------- /Models/1_Stanley_SimpleTrack/Images/visualization.JPG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mathworks/vehicle-stanley-controller/HEAD/Models/1_Stanley_SimpleTrack/Images/visualization.JPG -------------------------------------------------------------------------------- /Models/2_Stanley_USHighway/Images/visualization.JPG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mathworks/vehicle-stanley-controller/HEAD/Models/2_Stanley_USHighway/Images/visualization.JPG -------------------------------------------------------------------------------- /Models/1_Stanley_SimpleTrack/Images/imageforlivescript.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mathworks/vehicle-stanley-controller/HEAD/Models/1_Stanley_SimpleTrack/Images/imageforlivescript.jpg -------------------------------------------------------------------------------- /Models/2_Stanley_USHighway/Images/imageforlivescript.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mathworks/vehicle-stanley-controller/HEAD/Models/2_Stanley_USHighway/Images/imageforlivescript.jpg -------------------------------------------------------------------------------- /SECURITY.md: -------------------------------------------------------------------------------- 1 | # Reporting Security Vulnerabilities 2 | 3 | If you believe you have discovered a security vulnerability, please report it to 4 | [security@mathworks.com](mailto:security@mathworks.com). Please see 5 | [MathWorks Vulnerability Disclosure Policy for Security Researchers](https://www.mathworks.com/company/aboutus/policies_statements/vulnerability-disclosure-policy.html) 6 | for additional information. -------------------------------------------------------------------------------- /Models/2_Stanley_USHighway/radius.m: -------------------------------------------------------------------------------- 1 | function r = radius (x1,y1,x2,y2,x3,y3) 2 | x12 = x1 - x2; 3 | x13 = x1 - x3; 4 | 5 | y12 = y1 - y2; 6 | y13 = y1 - y3; 7 | 8 | y31 = y3 - y1; 9 | y21 = y2 - y1; 10 | 11 | x31 = x3 - x1; 12 | x21 = x2 - x1; 13 | 14 | % x1^2 - x3^2 15 | sx13 = x1^2 - x3^2; 16 | 17 | % y1^2 - y3^2 18 | sy13 = y1^2 - y3^2; 19 | 20 | sx21 = x2^2 - x1^2; 21 | sy21 = y2^2 - y1^2; 22 | 23 | f = ((sx13)*(x12) + (sy13)*(x12) + (sx21)*(x13) + (sy21)*(x13)) / (2 * ((y31) * (x12) - (y21) * (x13))); 24 | g = ((sx13) * (y12) + (sy13) * (y12) + (sx21) * (y13) + (sy21) * (y13)) / (2 * ((x31) * (y12) - (x21) * (y13))); 25 | 26 | c = -x1^2 - y1^2 - 2 * g * x1 - 2 * f * y1; 27 | 28 | df=(3.14*1000000); 29 | %eqn of circle be x^2 + y^2 + 2*g*x + 2*f*y + c = 0 30 | % where centre is (h = -g, k = -f) and radius r 31 | % as r^2 = h^2 + k^2 - c 32 | h = -g; 33 | k = -f; 34 | sqr_of_r = h * h + k * k - c; 35 | 36 | % r is the radius 37 | r = sqrt(sqr_of_r)/df; 38 | end 39 | 40 | -------------------------------------------------------------------------------- /license.txt: -------------------------------------------------------------------------------- 1 | Copyright (c) 2020, The MathWorks, Inc. 2 | All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 5 | 6 | 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 7 | 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. 8 | 3. In all cases, the software is, and all modifications and derivatives of the software shall be, licensed to you solely for use in conjunction with MathWorks products and service offerings. 9 | 10 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -------------------------------------------------------------------------------- /Models/2_Stanley_USHighway/setUpModel.m: -------------------------------------------------------------------------------- 1 | % Model Initialization 2 | % ADT Stanley Controller for US Highway 3 | % Copyright 2021 The MathWorks, Inc. 4 | 5 | %% add Images to the path 6 | addpath(genpath('Images')); 7 | 8 | %% load the scene data files 9 | % load data from Driving Scenario Designer 10 | load('USHighway.mat'); 11 | refPose = data.ActorSpecifications(1,46).Waypoints; 12 | 13 | %% define reference points 14 | xRef = refPose(:,1); 15 | yRef = -refPose(:,2); 16 | 17 | %% define vehicle parameters used in the models 18 | X_o = xRef(1); % initial vehicle position in x direction 19 | Y_o = yRef(1); % initial vehicle position in y direction 20 | 21 | %% calculating reference pose vectors 22 | % Based on how far the vehicle travels, the pose is generated using 1-D 23 | % lookup tables. 24 | 25 | % calculate distance vector 26 | distancematrix = squareform(pdist(refPose)); 27 | distancesteps = zeros(length(refPose)-1,1); 28 | for i = 2:length(refPose) 29 | distancesteps(i-1,1) = distancematrix(i,i-1); 30 | end 31 | totalDistance = sum(distancesteps); % Total traveled distance 32 | distbp = cumsum([0; distancesteps]); % Distance for each waypoint 33 | gradbp = linspace(0,totalDistance,50); % Linearize distance 34 | 35 | % linearize X and Y vectors based on distance 36 | xRef2 = interp1(distbp,xRef,gradbp); 37 | yRef2 = interp1(distbp,yRef,gradbp); 38 | yRef2s = smooth(gradbp,yRef2); 39 | xRef2s = smooth(gradbp,xRef2); 40 | 41 | %% calculate curvature vector 42 | curvature = getCurvature(xRef2,yRef2); 43 | 44 | %% calculate theta vector 45 | % theta = orientation angle of the path at reference points 46 | thetaRef = zeros(length(gradbp),1); 47 | for i = 2:length(gradbp) 48 | thetaRef(i,1) = atan2d((yRef2(i)-yRef2(i-1)),(xRef2(i)-xRef2(i-1))); 49 | end 50 | thetaRefs = smooth(gradbp,thetaRef); % smoothing of theta 51 | 52 | 53 | %% Removing abrupt changes in theta 54 | 55 | % Please refer to the "README.html" file understand this section with images. 56 | 57 | % The built-in function, "findchangepts" finds abrupt changes in the signal. 58 | % When using atan2, if theta is tending towards |180| degrees, the interpolation might 59 | % generate additional points leading to a fluctuating signal between -180 60 | % degrees and 180 degrees. Hence, we follow these steps to smoothen the 61 | % signal: 62 | % -> Find abrupt changes in the signal using findchangepts 63 | % -> Visualize the plot and find the change points where the change is 64 | % causing significant deviation 65 | % -> Define the regions where the signal has to be replaced by a 66 | % neighboring smooth value 67 | % Note: It is a manual process. So it's recommended to visualize the data and tune the 68 | % changepoints and regions to remove the abrupt changes from the signal 69 | % 70 | % If there is no fluctuation in theta i.e. if -180= gradbp(gradbp1)) 90 | thetaRefab(i) = thetaRefs(gradbp1); 91 | else 92 | thetaRefab(i) = thetaRefs(i); 93 | end 94 | end 95 | psi_o = thetaRefab(1)*(pi/180); % initial yaw angle 96 | 97 | %% plot to visualize the difference between thetaRefs and thetaRefab 98 | % figure 99 | % plot(gradbp,thetaRefab,'Linewidth',5,'Color','r') 100 | % hold on 101 | % plot(gradbp,thetaRefs,'b') 102 | % xlabel('distance (m)') 103 | % ylabel('theta (deg)') 104 | 105 | %% create direction vector 106 | direction = ones(length(gradbp),1); 107 | 108 | %% Curvature Function 109 | 110 | function curvature = getCurvature(xRef,yRef) 111 | % Calculate gradient by the gradient of the X and Y vectors 112 | DX = gradient(xRef); 113 | D2X = gradient(DX); 114 | DY = gradient(yRef); 115 | D2Y = gradient(DY); 116 | curvature = (DX.*D2Y - DY.*D2X) ./(DX.^2+DY.^2).^(3/2); 117 | end 118 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Vehicle Path Tracking Using Stanley Controller 2 | 3 | ![alt text]() 4 | 5 | The submission contains a model to show the implementation of Stanley controller on a vehicle moving in a US Highway scene. Steps below describe the workflow: 6 | 1. Generating waypoints 7 | 2. Smoothing vehicle reference position and orientation 8 | 3. Generating velocity profile 9 | 4. Implementing Stanley controller 10 | 5. Visualizing vehicle final path in 2D, Bird's-Eye Scope, and a 3D simulation environment. 11 | 12 | The users can refer to this model to perform path tracking applications for given waypoints. The results can be visualized in a 2D plot that compares the obtained and the reference trajectory. 13 | 14 | ## Model 15 | 16 | ![alt text]() 17 | 18 | ### stanleyHighway.slx 19 | The model implements a Stanley controller to drive the vehicle through the US Highway scene. 20 | ## Supporting files and folders (Before running the model, please make sure all these files are in the current folder) 21 | ### Images 22 | The folder contains images for masking certain blocks in the model 23 | ### setUpModel.m 24 | The file initializes the parameters required to run the model 25 | ### USHighway.mat 26 | The file contains data for the US Highway scene 27 | ### velocityProfile.mlx 28 | The live script generates velocity profile based on trapezoidal profile 29 | ## Product Requirements 30 | 31 | The models are developed in MATLAB R2020b version and use the following MathWorks products: 32 | 1. Automated Driving Toolbox 33 | 2. Curve Fitting Toolbox 34 | 3. MATLAB 35 | 4. Simulink 36 | 5. Simulink Coder 37 | 6. Statistics and Machine Learning Toolbox 38 | 7. Vehicle Dynamics Blockset 39 | 8. DSP System Toolbox 40 | 9. Signal Processing Toolbox 41 | 10. ROS Toolbox 42 | 43 | ## About the model 44 | The model shows the implementation of Stanley controller on a vehicle moving in a US Highway scene: 45 | - It comprises of a vehicle dynamics model based on a 3 DOF rigid two-axle vehicle body and a simplified powertrain and driveline. The vehicle dynamics model has been taken from one of the reference applications titled, [Scene Interrogation with Camera and Ray Tracing Reference Application](https://www.mathworks.com/help/vdynblks/ug/scene-interrogation-reference-application.html). For more detailed vehicle dynamics models, please check out the [Vehicle Reference Applications](https://www.mathworks.com/help/vdynblks/examples.html?category=index&s_tid=CRUX_topnav). 46 | - The model uses a trapezoidal velocity profile to generate the reference velocity 47 | - The mat file contains the waypoints for the US Highway scene exported from the Driving Scenario Designer 48 | - The Stanley controller outputs steering, acceleration, and deceleration commands to track the reference trajectory 49 | - The model displays the vehicle motion in the 2D plot, 3D Unreal Engine US highway scene, and in Bird's-Eye Scope 50 | 51 | ## How to run the model? 52 | 53 | Open and run the stanleyHighway.slx model. The model automatically loads the setUpModel.m and velocityProfile.mlx files that initializes the vehicle parameters and reference velocity profile required to run the model. 54 | 55 | ## How to use the model? 56 | 57 | Please note that the model has been tuned for a given set of waypoints and a velocity map. In case, you wish to customize it for a different test case, here are the steps to be followed: 58 | 1. **Define a waypoint vector:** The waypoint is generated using the Driving Scenario Designer. For the current US Highway scene, the project has been saved as USHighway.mat file. If you want the vehicle to follow a different trajectory, use the Driving Scenario Designer to define the waypoints and save it as a mt file. In case, you already have the waypoints, make sure to call the x and y coordinates using the “setUpModel” script. 59 | 2. **Interpolate and smooth the reference position and orientation:** Note that one of the important steps while implementing the Stanley controller is to prepare the reference position and orientation. The waypoint vector sample points may not be appropriate to generate a smooth path. Hence, it is recommended to interpolate and remove the noise from the reference position and orientation. Please refer to the “Troubleshooting” section to understand the significance of interpolating and smoothing the reference signal. 60 | 3. **Define a reference velocity profile:** The live script “velocityProfile.mlx” generates the reference velocity based on the following methods 61 | - Basic trapezoidal profile 62 | - Radius of curvature and friction based trapezoidal profile 63 | - Maximum velocity and acceleration based trapezoidal profile 64 | Please check out the live script for more details. 65 | 66 | 4. **Tune the controller:** Please note that the performance of the Stanley controller depends upon various tuning parameters. Please go through the documentation of the [Longitudinal Controller Stanley](https://www.mathworks.com/help/driving/ref/longitudinalcontrollerstanley.html) and [Lateral Controller Stanley](https://www.mathworks.com/help/driving/ref/lateralcontrollerstanley.html) blocks to learn more about the selection of these parameters. 67 | 68 | ## Troubleshooting the model 69 | This section covers a few troubleshooting tips which you may encounter while modifying the model for a different set of reference waypoints and vehicle parameters: 70 | 1. **Signal smoothing:** The figure shows the reference theta (vehicle orientation angle) obtained from the vehicle reference waypoints 71 | 72 | 73 | 74 | As can be seen, theta in certain instances is noisy. This can lead to the deviation of the vehicle path from the reference path. Hence, it is recommended to remove the noise by smoothing the signal. We have used the built-in “smooth” function to remove the noise. 75 | 76 | 2. **Removal of abrupt changes in vehicle orientation angle:** Suppose you have interpolated and removed the noise from the reference theta and the vehicle still deviates from the path. 77 | 78 | 79 | 80 | The reason is when we use atan2 to calculate theta and when theta is approximately >= |180| deg, there will be continuous fluctuations. Hence, it is important to remove these abrupt changes. We have used the built-in “findchangepts” to find the abrupt changes and have implemented a simple logic to replace this signal from the previous smooth signal. 81 | 82 | 83 | 84 | Please note it's a manual process. So, it's recommended to visualize the data and tune the changepoints and regions to remove the abrupt changes from the signal. 85 | 86 | Important note: If there is no fluctuation in theta for -180 91 | 92 | One possible reason is at the turn the vehicle is at very high velocity. Hence, if we lower the velocity at the turn by increasing the “number of sharp turns” input in the “velocityProfile” script to 2, the model will run successfully. 93 | 94 | ![alt text]() 95 | 96 | ![alt text]() 97 | 98 | So, the above tips and tricks would help you tune your model. In case of any queries, please reach out to us at racinglounge@mathworks.com. 99 | 100 | Copyright 2021 The MathWorks, Inc. 101 | 102 | 103 | --------------------------------------------------------------------------------