Variable Occupation Maps for trajectory planning

1 次查看（过去 30 天）
Alexander Maier
Alexander Maier 2020-4-19
回答： Alexander Maier 2020-4-24
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

请先登录，再回答此问题。

采纳的回答

Alexander Maier
Alexander Maier 2020-4-24
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.

更多回答（0 个）

类别

在 Help Center 和 File Exchange 中查找有关 Robotics 的更多信息

标签

产品

Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!

Translated by