How to integrate the Automated Driving Toolbox with the motorcycle simulation?

Question

Vinicius Dreher le 21 Oct 2021

0
Lien

Utiliser le lien direct vers cette question

https://fr.mathworks.com/matlabcentral/answers/1569108-how-to-integrate-the-automated-driving-toolbox-with-the-motorcycle-simulation

Hello guys,

I'm writing this question because after a lot of research, I couldn't find a good approach to how start study to to my project.

I would like to learn how can I integrate my code to the Automated Driving Toolbox.

% Initial Commands
clc;
clear;
close all force
diary('off')
fclose('all') ;
% Simulation physics
x0= 0; %Initial position [s] 
v0= 0.1; %Initial velocity [m/s]
t0= 0; %Final time [s]
tf=100; % [s]
dt= 0.1; % Time increment [s]
% Physical parameters 
global par; 
par.p_max= 22080; % maximum power [W]
par.r= 70; % Rider Mass [Kg]
par.bike= 145.4; % Motorcycle Mass [Kg]
par.m_c= par.r + par.bike; % Total Mass [Kg]
par.k_a= 0.65; % Aerodynamic drag factor [kg/m]
par.c_r= 0.02; % Roll resistance factor
par.mu = 1.1; % Friction Coefficient
par.w= -0; % Wing speed [m/s]
pi= 3.141592; %pi
par.teta= 0/180*pi; % Lane slope angle [rad]
par.g=9.81; % Gravity Acc. [m/s^2]
par.p_d= 1.345; % Wheelbase [m]
par.p_cg= 0.63; % Cog from rear wheel With rider h [m]
par.h_cg= 0.58; % CoG height h with rider h [m]
par.h_cp=0.78; % Pressure CoG height
% Vectors
q0= [x0;v0];
% t1 =t0:dt:tf;
% Solution
[t,q] = ode45(@s,[t0,tf],q0) ; % Correct usage of ode function ,
x=q(:,1); % Change in dimensions
v=q(:,2);
%Graphs
plot(t,v*3.6,'k-')
% xlabel('$t\quad[s]$','fontsize',4);
% ylabel('$v\quad[km/h]$','fontsize',4);
function f=s(t,q)
    x=q(1,1);
    v=q(2,1);
    global par; % Global keyword needed
    %Rider
    alpha=1; % BRAAAAAAAAAAP Acc
    beta_t= 0; % Rear Brake
    beta_d=0; % Front Brake
    % Engine
    p_max= par.p_max; % Max. power cte
    %Track
    teta=par.teta; %Angle
    mu=par.mu; %Friction Coefficient
    w=par.w; %Wing speed [m/s] % 'w' insteal of 'W'
    %Forces
    f_grx=-par.m_c*par.g*sin(teta); % x gravity component
    f_gry= -par.m_c*par.g*cos(teta); % y gravity component
    f_pro= alpha*p_max/v; %
    f_aer=-par.k_a*(v-w)^2; % Drag
    f_rol= -par.c_r*abs(f_gry); % Roll resistance
    % d function
    d=(f_pro+f_aer+f_rol+f_grx+beta_t*mu*f_gry+(beta_d-beta_t)*mu/par.p_d*(-f_aer*par.h_cp + -f_grx*par.h_cg+f_gry*par.p_cg))/(par.m_c*(1-(beta_d-beta_t)*mu*par.p_cg/par.p_d));
    % Check function d for any mistakes in variable names
    % f(1)=v;
    % f(2)=d;
    f = [v;d]; % f needs to be a column matrix 
end

For example, I manage to get a track with way points and the velocity for each way point manually, however I wish to learn how to automate my velocity using only the Geographic Reference points. So the question is, is there any course that matlab offers that cover these subject?

function [scenario, egoVehicle] = Donington();
% createDrivingScenario Returns the drivingScenario defined in the Designer
s = drivingScenario('SampleTime', 0.05);
% Generated by MATLAB(R) 9.9 (R2020b) and Automated Driving Toolbox 3.2 (R2020b).
% Generated on: 20-Apr-2021 20:42:46
% Construct a drivingScenario object.
scenario = drivingScenario('GeographicReference', [52.83055 -1.3749 0], ...
    'VerticalAxis', 'Y');
% Add all road segments
roadCenters = [
    -135.18 -112.094 -0.0024148;
    -602.759 -25.3128 -0.0284712;
    -602.759 -25.3128 -0.0284712;
    -624.879 -19.0225 -0.0305736;
    -630.359 -17.3525 -0.0311069;
    -634.544 -16.0277 -0.0315175;
    -639.855 -14.2242 -0.0320427;
    -649.755 -9.10391 -0.0330321;
    -654.911 -5.47539 -0.0335541;
    -660.033 -1.33496 -0.0340788;
    -665.579 5.24262 -0.0346559;
    -668.982 12.1315 -0.0350205;%Redgate
    
    -668.982 12.1315 -0.0350205;
    -672.236 19.1873 -0.0353793;
    -673.96 26.688 -0.0355879;
    -674.613 33.0758 -0.0356867;
    -673.952 37.6605 -0.0356424;
    -672.886 43.002 -0.0355639;
    -670.776 48.755 -0.0353835;
    -668.47 53.5955 -0.0351808;
    -665.808 57.9129 -0.0349406;
    -623.703 112.314 -0.0314196;
    -598.539 144.816 -0.029669;
    -593.74 150.202 -0.029346;
    -588.604 156.01 -0.0290104;
    -584.182 160.784 -0.0287233;
    -578.722 166.748 -0.0283798;
    -575.567 169.919 -0.0281787;
    -570.66 174.448 -0.027861;
    -565.174 178.364 -0.0274818;
    -561.21 180.845 -0.0272025;
    -543.578 191.17 -0.0259799;
    -532.261 197.368 -0.0252163;
    -522.091 202.307 -0.0245323;
    -513.113 205.968 -0.0239224;
    -504.385 209.339 -0.0233375;
    -495.69 211.741 -0.0227367;
    -483.147 214.545 -0.02187;
    -469.311 217.704 -0.0209461;
    -457.637 219.594 -0.0201645;
    -448.855 220.428 -0.0195705;
    -428.03 221.027 -0.0181627;
    -412.077 221.282 -0.0171232; 
    -400.438 221.092 -0.0163768;
    -390.079 220.267 -0.0157077;
    -380.67 218.798 -0.0150898;
    -369.907 216.86 -0.0143917;
    -348.745 211.428 -0.0130195;
    -318.552 202.468 -0.0111526;
    -293.636 195.422 -0.00973957;
    -277.824 192.606 -0.00894706;%Redgate-Hollywood-Craner Curves
    
    -277.824 192.606 -0.00894706;
    -262.316 193.94 -0.00833225;
    -250.394 195.998 -0.007917;
    -243.755 197.055 -0.00769296;
    -235.418 199.002 -0.00744092;
    -221.945 203.23 -0.00709226;
    -201.382 211.487 -0.00667981;
    -176.013 223.593 -0.00634394;
    -163.908 230.114 -0.00625407;
    -149.384 237.102 -0.00615415;
    -138.142 241.898 -0.00608144;
    -103.238 252.246 -0.00582337;
    -78.2267 258.867 -0.0057337;
    -63.9117 262.773 -0.00573431;
    -57.4282 263.074 -0.00568515;
    -51.922 262.751 -0.00562474;
    -47.5682 262.217 -0.00556887;
    -40.5927 260.013 -0.00543052;
    -35.6593 258.077 -0.00532242;
    -31.2382 255.228 -0.00518461;
    -24.9434 249.764 -0.00494057;
    -5.05474 230.211 -0.00415796;
    66.4203 159.937 -0.00235103;
    104.299 120.498 -0.00198959;
    110.088 115.513 -0.00199441;
    113.815 112.598 -0.00200754;
    118.574 110.483 -0.00205706;
    126.25 107.512 -0.00215329;
    133.846 104.764 -0.00226209;
    143.882 101.359 -0.00242508;
    153.237 98.5102 -0.00259786;
    160.664 96.5185 -0.00274978;
    170.552 93.6255 -0.00296283;
    179.442 91.3779 -0.00317361;
    188.406 89.4196 -0.00340379; %'Old Hairpin-Craner Curves-841515306-Starkey''s Bridge
    
    188.406 89.4196 -0.00340379;
    198.347 87.684 -0.00368046;
    207.547 86.3045 -0.00395375;
    220.232 84.6692 -0.00435629;
    231.898 83.6014 -0.00475483;
    243.026 82.9342 -0.00515953;
    255.272 82.167 -0.00562695;
    312.386 78.1753 -0.00811294; %841515343'
    
    312.386 78.1753 -0.00811294;
    324.538 78.0314 -0.00871664;
    331.696 77.9985 -0.00908368;
    339.952 78.5667 -0.00952444;
    348.141 79.1793 -0.0099728;
    357.199 80.5376 -0.0104896;
    364.977 82.2631 -0.010951;
    372.303 84.5894 -0.011404;
    380.445 87.6837 -0.0119252;
    387.38 91.2008 -0.0123911;
    393.513 94.7735 -0.0128178;
    410.031 104.623 -0.0140102; %841515343''
    
    410.031 104.623 -0.0140102;
    437.481 120.996 -0.0161197;
    445.205 124.913 -0.0167285;
    451.277 127.741 -0.0172104;
    462.754 131.97 -0.0181172;
    473.645 135.521 -0.0189895;
    483.687 138.293 -0.019801;
    493.103 140.086 -0.0205596;
    502.073 140.899 -0.0212758;
    508.112 140.544 -0.0217452;
    514.01 139.61 -0.0221962;
    520.278 138.453 -0.0226781;
    526.384 136.395 -0.0231337;
    531.54 133.969 -0.023509;
    535.409 130.988 -0.02377;
    539.184 127.649 -0.0240196;
    542.918 123.388 -0.0242518;
    546.154 118.725 -0.0244389;
    554.547 105.662 -0.0249317;
    608.248 8.33989 -0.0289464;
    629.221 -27.2903 -0.0310296; %McLeans
    
    629.221 -27.2903 -0.0310296;
    668.747 -94.4221 -0.0356836;
    680.397 -114.207 -0.0372367;
    685.102 -123.02 -0.0379034;
    687.085 -129.463 -0.0382437;
    688.272 -134.225 -0.0384699;
    688.886 -139.133 -0.0386413;
    688.941 -144.697 -0.038771;
    687.58 -151.897 -0.038792;
    684.595 -160.032 -0.0386706;
    674.144 -179.174 -0.0380689;
    667.466 -190.115 -0.0376849;
    661.273 -197.783 -0.0372744;
    654.978 -203.904 -0.0368191;
    645.873 -210.293 -0.0361;
    634.281 -218.051 -0.0351998;
    619.514 -226.588 -0.0340491;
    606.661 -231.32 -0.0329862;
    592.041 -234.281 -0.0317234;
    579.316 -236.019 -0.0306215;
    562.721 -236.689 -0.0291638;
    550.818 -236.056 -0.0281035;
    279.216 -211.497 -0.00960633;
    84.4823 -193.554 -0.00349612;
    13.3861 -187.69 -0.00277651; %Coppice
    
    13.3861 -187.69 -0.00277651;
    8.21635 -188.658 -0.00279634;
    4.98778 -189.593 -0.00282074;
    2.06925 -190.928 -0.00285897;
    -0.626843 -192.62 -0.00290954;
    -3.32294 -194.912 -0.00298004;
    -5.51353 -198.184 -0.00308241;
    -7.50865 -202.368 -0.00321587;
    -9.0724 -207.331 -0.00337736;
    -10.4339 -211.371 -0.00351207;
    -12.2336 -215.188 -0.00364294;
    -14.559 -218.415 -0.00375755;
    -17.0394 -221.042 -0.00385419;
    -20.3287 -224.269 -0.0039765; %28381201
    
    -20.3287 -224.269 -0.0039765;
    -24.1168 -226.784 -0.00407863;
    -28.3025 -228.898 -0.00417134;
    -33.1488 -230.478 -0.00425156;
    -38.7028 -231.457 -0.00431825;
    -44.9376 -232.003 -0.00437886;
    -73.9344 -232.125 -0.00465294;
    -177.93 -232.567 -0.00671804;
    -237.393 -233.143 -0.00867098;
    -253.213 -233.554 -0.00929314;
    -268.021 -234.243 -0.00992222;
    -298.953 -237.435 -0.0114122;
    -330.012 -240.928 -0.0130713;
    -435.998 -254.54 -0.0199511;
    -445.28 -255.73 -0.0206386;
    -450.618 -256.442 -0.0210413;
    -454.959 -256.475 -0.0213502;
    -459.3 -255.929 -0.0216387;
    -462.825 -254.493 -0.0218355;
    -465.669 -252.668 -0.0219695;
    -468.318 -250.186 -0.0220651;
    -471.074 -246.18 -0.0221117;
    -472.631 -242.196 -0.0220741;
    -473.318 -237.333 -0.021942;
    -473.459 -232.492 -0.0217741;
    -472.98 -227.384 -0.0215544;
    -472.299 -223.444 -0.0213648;
    -471.173 -219.906 -0.0211587;
    -468.342 -216.445 -0.0208322; %Melbourne Hairpin
    
    -468.342 -216.445 -0.0208322;
    -464.971 -214.442 -0.0205184;
    -459.869 -212.44 -0.0200823;
    -450.789 -210.515 -0.0193716;
    -440.665 -209.036 -0.018617;
    -317.418 -190.183 -0.010718; %Melbourne Hairpin
    
    -317.418 -190.183 -0.010718;
    -272.137 -184.477 -0.00846203;
    -227.759 -178.448 -0.00655505;
    -208.853 -176.579 -0.00585728;
    -186.151 -175.634 -0.00512973;
    -168.721 -175.39 -0.00463914;
    -153.246 -175.803 -0.00426072;
    -138.013 -176.326 -0.00392811;
    -124.054 -176.727 -0.00365305;
    -115.77 -176.293 -0.00348563;
    -110.263 -175.258 -0.00335974;
    -106.819 -174.112 -0.00326985;
    -103.267 -172.688 -0.00317273;
    -100.025 -170.485 -0.00306188;
    -97.6589 -168.181 -0.00296412;
    -95.5762 -165.455 -0.00286131;
    -93.8506 -162.65 -0.00276358;
    -92.7182 -159.646 -0.00267112;
    -91.8554 -155.84 -0.0025645;
    -91.6464 -152.613 -0.00248344;
    -91.6733 -149.063 -0.00239985;
    -92.172 -146.047 -0.00233723;
    -93.0818 -142.108 -0.0022614;
    -94.45 -138.279 -0.00219729;
    -95.8249 -135.108 -0.00214976;
    -97.7121 -131.569 -0.00210433;
    -99.9767 -128.319 -0.00207313;
    -102.7 -124.925 -0.00204889;
    -105.268 -122.544 -0.00204445;
    -108.537 -119.628 -0.00204375;
    -110.525 -118.326 -0.00205353;
    -112.372 -117.169 -0.00206436;
    -118.748 -115.143 -0.00214274; %Goddards-841515325
    
    -118.748 -115.143 -0.00214274;
    -135.18 -112.094 -0.0024148]; %Wheatcroft Straight
               
roadWidth = 15; % (m)
road(s,roadCenters,roadWidth);
% Add the ego vehicle
egoCar = vehicle(s);
waypoints = [-177.3 -104.7 0;
    -191.2 -100.4 0;
    -226.2 -94.1 0;
    -278.7 -84.3 0;
    -315.7 -77.4 0;
    -404.8 -60.5 0;
    -569.4 -30.8 0;
    -626.9 -17.6 0;
    -652.3 -7.5 0;
    -671.5 11 0;
    -676.3 30 0;
    -674.6 45.4 0;
    -669.9 57.9 0;
    -659.3 73.8 0;
    -646.6 91.4 0;
    -614.3 132.6 0;
    -569.1 182.5 0;
    -544.1 197.4 0;
    -509.7 212.9 0;
    -476 221 0;
    -435.5 227 0;
    -390.3 227.7 0;
    -338.2 211.7 0;
    -319.5 205 0;
    -294.2 198.3 0;
    -262.2 195.4 0;
    -238.4 199.9 0;
    -214.9 209.5 0;
    -177.8 227 0;
    -108.5 257.5 0;
    -60.7 265.7 0;
    -45.5 264 0;
    -34 260.2 0;
    -15.7 246.7 0;
    0.5 229.3 0;
    18.8 209.1 0;
    43.4 186 0;
    66 166.2 0;
    87.2 140.2 0;
    114.2 116.6 0;
    168.6 99.8 0;
    203.8 90.6 0;
    259.7 83.4 0;
    323.4 79.9 0;
    341.4 80.3 0;
    361.1 82.5 0;
    394.5 97.5 0;
    424 115 0;
    456.6 132.2 0;
    486.5 140.7 0;
    510.1 142.8 0;
    532 136.6 0;
    549.1 118.2 0;
    557.5 101.9 0;
    630.9 -29.5 0;
    654.2 -70.4 0;
    682 -111.6 0;
    683.9 -149.3 0;
    670.1 -178.5 0;
    659.6 -194.3 0;
    641.6 -208.9 0;
    618.8 -223.1 0;
    583.6 -234.3 0;
    542 -235.1 0;
    467.9 -226.8 0;
    182.1 -199.2 0;
    25.1 -185.1 0;
    5.5 -185.6 0;
    -10.6 -205.6 0;
    -23.2 -222.1 0;
    -46.7 -229.5 0;
    -228.4 -233.6 0;
    -344.5 -243 0;
    -435.9 -256.7 0;
    -473.2 -237.1 0;
    -464.8 -215 0;
    -434 -206.6 0;
    -235.6 -176.2 0;
    -122 -175 0;
    -93.2 -158.4 0;
    -97.2 -134.8 0;
    -121.6 -113.7 0;
    -177.1 -103.6 0]; % (m)
speed = [100;100;100;100;100;100;57;45;35;25;19;50;100;100;100;100;100;100;100;100;100;100;100;30;100;100;100;100;100;100;100;100;32;100;100;100;100;100;100;100;100;100;100;100;100;100;100;100;100;100;59;59;100;100;100;100;100;35;50;100;100;100;100;100;100;100;100;50;40;30;100;100;100;50;30;20;100;100;100;50;30;50;100]; 
trajectory(egoCar, waypoints, speed);
plot(s);
f = figure(1);
f.Position = [500 100 1060 800];
% Play scenario
while advance(s);
pause(s.SampleTime);
end