# MATLAB: Variable Occupation Maps for trajectory planning

autonomous drivingdriving scenarioMATLABoptimaltrajectoryfrenettrajectory planning

Hello,
I´ve simplified the example algorithm for collision-free motion and would like to test this in some other scenarios (different road layouts, different reference paths, no obstacles/static obstacles).
Thank you in Advance!
Here is the code, which works, in the example directory:
``% Load data from urbanScenarioData.mat file, initialize required variablesclc clear[otherVehicles,globalPlanPoints,stateValidator] = exampleHelperUrbanSetup;% Set positions A, B, C and DpositionA =  [5.1, -1.8, 0];positionB =  [60, -1.8, 0];positionC =  [74.45, -30.0, 0];positionD =  [74.45, -135, 0];goalPoint =  [145.70, -151.8, 0];% Set the initial state of the ego vehicleegoInitPose     = positionA;egoInitVelocity = [10, -0.3, 0];egoInitYaw      = -0.165;currentEgoState = [egoInitPose(1), egoInitPose(2), deg2rad(egoInitYaw),...    0, norm(egoInitVelocity), 0];vehicleLength = 4.7; % in meters% Replan interval in number of simulation steps% (default 50 simulation steps)%%Distance to GOALdistanceToGoal = norm(currentEgoState(1:3) - goalPoint);goalRadius = 10; % Initialize VisualizationexampleHelperInitializeVisualization;localPlanner = trajectoryOptimalFrenet(globalPlanPoints, stateValidator); %Terminal StateslocalPlanner.TerminalStates.Longitudinal = [5 15 30 45]; %Longitudinal sampling distance in mlocalPlanner.TerminalStates.Lateral = [-2 -1 0 1 2]; % Lateral deviation in meters from globalPlanPointslocalPlanner.TerminalStates.Time = 7; %Time in sec to reach end of trajectorylocalPlanner.TerminalStates.Speed = 10; %Velocity in m/slocalPlanner.TerminalStates.Acceleration; % Acceleration at the end of trajectory in m/s^2%Feasibility ParameterslocalPlanner.FeasibilityParameters.MaxCurvature = 0.1; %Max feasible curvature value in m^-1localPlanner.FeasibilityParameters.MaxAcceleration = 2.5; %Maximum feasible acceleration in m/s^2%Weights planner = trajectoryOptimalFrenet(globalPlanPoints, stateValidator, 'Time',1,'Deviation',1,'LateralSmoothness',3,'LongitudinalSmoothness',5);%% TimetimeResolution=0.01; localPlanner.TimeResolution = timeResolution; replanInterval=25; % Simulate till the ego vehicle reaches position BsimStep = 1;% Check only for X as there is no change in Y.% Simulate till the ego vehicle reaches position D% Set Lane WidthlaneWidth = [5 4 3 2.5 2 1.5 1 0 -1 -2];% Check only for Y as there is no change in X at Dwhile (distanceToGoal > goalRadius)        % Replan at every "replanInterval" simulation step    if rem(simStep, replanInterval) == 0 || simStep == 1        % Compute the replanning time        previousReplanTime = simStep*timeResolution;                % Updating occupancy map with vehicle information        exampleHelperUpdateOccupancyMap(otherVehicles,simStep,currentEgoState);                % TerminalState settings for negotiating Lane change        localPlanner.TerminalStates.Longitudinal = 20:5:40;        localPlanner.TerminalStates.Lateral = laneWidth;        localPlanner.TerminalStates.Speed = 10;        desiredTimeBound = localPlanner.TerminalStates.Longitudinal/...            ((currentEgoState(1,5) + localPlanner.TerminalStates.Speed)/2);        localPlanner.TerminalStates.Time = desiredTimeBound;        localPlanner.FeasibilityParameters.MaxCurvature = 0.5;        localPlanner.FeasibilityParameters.MaxAcceleration = 15;                % Generate optimal trajectory for current set of parameters        currentStateInFrenet = cart2frenet(localPlanner,[currentEgoState(1:5) 0]);        trajectory = plan(localPlanner,currentStateInFrenet);                % Visualize the ego-centric occupancy map        show(egoMap,"Parent",hAxes1)        title("Ego Centric Occupancy Map","Parent",hAxes1)                % Visualize ego vehicle on occupancy map        egoCenter = currentEgoState(1:2);        egoPolygon = exampleHelperTransformPointtoPolygon(rad2deg(currentEgoState(3)), egoCenter);        hold(hAxes1,"on")        fill(egoPolygon(1, :),egoPolygon(2, :),"g","Parent",hAxes1)                % Visualize the Trajectory reference path and trajectory        show(localPlanner,"Trajectory","optimal","Parent",hAxes1)    end        % Execute and Update Visualization    [isGoalReached, currentEgoState] = ...        exampleHelperExecuteAndVisualize(currentEgoState,simStep,...        trajectory,previousReplanTime);    if(isGoalReached)        break;    end        % Update the simulation step for the next iteration    simStep = simStep + 1;    pause(0.01);end``

#### Best Answer

• Ok, I´ve soved the problem. The Workspace Directory had to be updated. It is possible to create scenarios with the autonomous driving app and extract the relevant workspace data.