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.
Variable Occupation Maps for trajectory planning
2 次查看(过去 30 天)
显示 更早的评论
Hello,
how could I create a custom scenario for this example https://www.mathworks.com/help/nav/ug/optimal-trajectory-generation-for-urban-driving.html#mw_rtc_OptimalTrajectoryGenerationForUrbanDrivingExample_B94A9390 ?
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 variables
clc
clear
[otherVehicles,globalPlanPoints,stateValidator] = exampleHelperUrbanSetup;
% Set positions A, B, C and D
positionA = [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 vehicle
egoInitPose = 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 GOAL
distanceToGoal = norm(currentEgoState(1:3) - goalPoint);
goalRadius = 10;
% Initialize Visualization
exampleHelperInitializeVisualization;
localPlanner = trajectoryOptimalFrenet(globalPlanPoints, stateValidator);
%Terminal States
localPlanner.TerminalStates.Longitudinal = [5 15 30 45]; %Longitudinal sampling distance in m
localPlanner.TerminalStates.Lateral = [-2 -1 0 1 2]; % Lateral deviation in meters from globalPlanPoints
localPlanner.TerminalStates.Time = 7; %Time in sec to reach end of trajectory
localPlanner.TerminalStates.Speed = 10; %Velocity in m/s
localPlanner.TerminalStates.Acceleration; % Acceleration at the end of trajectory in m/s^2
%Feasibility Parameters
localPlanner.FeasibilityParameters.MaxCurvature = 0.1; %Max feasible curvature value in m^-1
localPlanner.FeasibilityParameters.MaxAcceleration = 2.5; %Maximum feasible acceleration in m/s^2
%Weights
planner = trajectoryOptimalFrenet(globalPlanPoints, stateValidator, 'Time',1,'Deviation',1,'LateralSmoothness',3,'LongitudinalSmoothness',5);
%% Time
timeResolution=0.01;
localPlanner.TimeResolution = timeResolution;
replanInterval=25;
% Simulate till the ego vehicle reaches position B
simStep = 1;
% Check only for X as there is no change in Y.
% Simulate till the ego vehicle reaches position D
% Set Lane Width
laneWidth = [5 4 3 2.5 2 1.5 1 0 -1 -2];
% Check only for Y as there is no change in X at D
while (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
0 个评论
采纳的回答
更多回答(0 个)
另请参阅
类别
在 Help Center 和 File Exchange 中查找有关 Motion Planning 的更多信息
产品
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!