Main Content

# Optimal Trajectory Generation for Urban Driving

This example shows how to perform dynamic replanning in an urban scenario using `trajectoryOptimalFrenet`.

In this example, you will:

1. Explore an urban scenario with predefined vehicles.

2. Use `trajectoryOptimalFrenet` to do local planning for navigating the Urban scenario.

### Introduction

Automated driving in an urban scenario needs planning on two levels, global and local. The global planner finds the most feasible path between start and goal points. The local planner does dynamic replanning to generate an optimal trajectory based on the global plan and the surrounding information. In this example, an ego vehicle (green box) follows a global plan (dotted blue line). Local planning is done (solid orange line) while trying to avoid another vehicle (black rectangle).

Local planners use obstacle information to generate optimal collision-free trajectories. Obstacle information from various on-board sensors like camera, radar, and lidar are fused to keep the occupancy map updated. This occupancy map is egocentric, where the local frame is centered on the ego vehicle. The map is used for local planning when obstacles are detected from the sensors and placed on the map.

### Explore An Urban Scenario For Local Planning

This example scenario has four other vehicles (blue rectangles), which are moving in predefined paths at different velocities. The figure below illustrates this scenario and the global plan (solid red line) used in this example. Solid red dots in the below figure represent the waypoints of the global plan between the start and goal positions. The green rectangle represents the ego vehicle.

The ego vehicle uses `trajectoryOptimalFrenet` to navigate from position A to position D in three segments with three different configuration parameters.

• First (A to B), the vehicle demonstrates Adaptive Cruise Control (ACC) behavior.

• Second (B to C), the vehicle negotiates a turn to follow the global plan.

• Third (C to D), the vehicle performs a lane change maneuver.

Set up the required data and environment variables:

```% Load data from urbanScenarioData.mat file, initialize required variables [otherVehicles,globalPlanPoints,stateValidator] = exampleHelperUrbanSetup;```
1. otherVehicles: [1 x 4] Structure array containing fields: `Position`, `Yaw`, `Velocity`, and `SimulationTime`, of each vehicle in the scenario.

2. `globalPlanPoints``:` [18 x 2] Matrix contains precomputed global plan consisting of eighteen waypoints, each representing a position in the scenario.

3. `stateValidator`: `validatorOccupancyMap` object that acts as the state validator based on a given 2-D grip map. A fully occupied egocentric occupancy map is updated based on obstacle information and road boundaries. A custom state validator can also be used based on the application. For more information, see `nav.StateValidator`.

Plot the scenario.

`exampleHelperPlotUrbanScenario;`

#### Create local planner

Specify the state validator and global plan to create a local planner using `trajectoryOptimalFrenet`.

`localPlanner = trajectoryOptimalFrenet(globalPlanPoints,stateValidator);`

#### Explore properties of `localPlanner`

The `localPlanner` has a variety of properties that can be tuned to achieve the desired behavior. This section explores some of these properties and their default values.

`localPlanner.TerminalStates`

• `Longitudinal:` `[30 45 60 75 90]:` Defines longitudinal sampling distance in meters. This value can be a scalar or vector.

• `Lateral:` `[-2 -1 0 1 2]``:` Defines lateral deviation in meters from the reference path (Global plan in our case).

• `Time:` `7:` Time in seconds to reach the end of trajectory.

• `Speed:` `10:` Velocity in meters per second, to be achieved at the end of the trajectory

• `Acceleration:` `0: `Acceleration at the end of the trajectory in m/s^2.

`localPlanner.FeasibilityParameters`

• `MaxCurvature:` `0.1 :` Maximum feasible value for the curvature in m^-1

• `MaxAcceleration:` `2.5:` Maximum feasible acceleration in m/s^2.

`localPlanner.TimeResolution:` `0.1:` Trajectory discretization interval in seconds

### Use `trajectoryOptimalFrenet` to demonstrate Adaptive Cruise Control (ACC) behavior

In this section, assign the properties needed to configure `localPlanner` to demonstrate Adaptive Cruise Control (ACC) behavior.

To demonstrate ACC, the ego vehicle needs to follow a lead vehicle by maintaining a safe distance. The lead vehicle in this segment is fetched using` otherVehicles(1)`.

```% Get leadVehicle in segment from Postion A to Position B leadVehicle = otherVehicles(1); % Define ACC safe distance ACCSafeDistance = 35; % in meters % Adjusting the time resolution of planner object to make the ego vehicle % travel smoothly timeResolution = 0.01; localPlanner.TimeResolution = timeResolution;```

Set up the ego vehicle at position A and define its initial velocity and orientation (Yaw).

```% 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) replanInterval = 50;```

Visualize the simulation results.

```% Initialize Visualization exampleHelperInitializeVisualization;```

The ACC behavior is achieved by setting the `TerminalStates` of `localPlanner` as below`:`

To maintain the safe distance from lead vehicle, set `localPlanner.TerminalStates.Longitudinal` to Distance to Lead Vehicle - Vehicle Length;

To maintain relative velocity with respect to the lead vehicle, set `localPlanner.TerminalStates.Speed` to Lead Vehicle Velocity;

To continue navigating on the global plan, set` localPlanner.TerminalStates.Lateral` to 0;

In the following code snippet, `localPlanner` generates `trajectory` that is executed and visualized using `exampleHelperUpdateVisualization` for every simulation step. However, replanning is done at every replanInterval simulation step. The following is the sequence of events during replanning:

• Update the occupancy map using vehicle information using `exampleHelperUpdateOccupancyMap`.

• Update the `localPlanner.TerminalStates`.

• Trajectory generation using `plan(localPlanner,currentStateInFrenet)`.

```% Simulate till the ego vehicle reaches position B simStep = 1; % Check only for X as there is no change in Y. while currentEgoState(1) <= positionB(1) % Replan at every "replanInterval"th 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); % Compute distance to Lead Vehicle and leadVehicleVelocity distanceToLeadVehicle = pdist2(leadVehicle.Position(simStep,1:2), ... currentEgoState(1:2)); leadVehicleVelocity = leadVehicle.Velocity(simStep,:); % Set localPlanner.TerminalStates for ACC behavior if distanceToLeadVehicle <= ACCSafeDistance localPlanner.TerminalStates.Longitudinal = distanceToLeadVehicle - vehicleLength; localPlanner.TerminalStates.Speed = norm(leadVehicleVelocity); localPlanner.TerminalStates.Lateral = 0; desiredTimeBound = localPlanner.TerminalStates.Longitudinal/... localPlanner.TerminalStates.Speed; localPlanner.TerminalStates.Time = desiredTimeBound; localPlanner.FeasibilityParameters.MaxCurvature = 0.5; end % 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); hAxes1.Title.String = "Ego Centric Binary Occupancy Map"; % 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```

At the end of this execution, the ego vehicle is at position B.

Next, configure `trajectoryOptimalFrenet` for negotiating a turn in the second segment from position B to position C.

### Use `trajectoryOptimalFrenet` to Negotiate a Smooth Turn

The current set properties of the `localPlanner` are sufficient to negotiate a smooth turn. However, in the second segment, the lead vehicle is fetched from otherVehicles`(2)`.

```% Set Lead Vehicle to correspond to the vehicle in second segment % from position B to position C leadVehicle = otherVehicles(2); % Simulate till the ego vehicle reaches position C % Check only for Y as there is no change in X at C while currentEgoState(2) >= positionC(2) % Replan at every "replanInterval"th simulation step if rem(simStep, replanInterval) == 0 % Compute the replanning time previousReplanTime = simStep*timeResolution; % Updating occupancy map with vehicle information exampleHelperUpdateOccupancyMap(otherVehicles, simStep, currentEgoState); % Compute distance to Lead Vehicle and leadVehicleVelocity distanceToLeadVehicle = pdist2(leadVehicle.Position(simStep,1:2), ... currentEgoState(1:2)); leadVehicleVelocity = leadVehicle.Velocity(simStep,:); if(distanceToLeadVehicle <= ACCSafeDistance) localPlanner.TerminalStates.Longitudinal = distanceToLeadVehicle - vehicleLength; localPlanner.TerminalStates.Speed = norm(leadVehicleVelocity); localPlanner.TerminalStates.Lateral = 0; desiredTimeBound = localPlanner.TerminalStates.Longitudinal/... localPlanner.TerminalStates.Speed; localPlanner.TerminalStates.Time = desiredTimeBound; localPlanner.FeasibilityParameters.MaxCurvature = 0.5; localPlanner.FeasibilityParameters.MaxAcceleration = 5; end % 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); hAxes1.Title.String = "Ego Centric Binary Occupancy Map"; % 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```

At the end of this execution, the ego vehicle is at position C.

Next, configure `trajectoryOptimalFrenet` for performing a lane change maneuver from position C to position D.

### Use `trajectoryOptimalFrenet` to perform Lane Change maneuver

Lane change maneuver can be performed by appropriately configuring the Lateral terminal states of the planner. This can be achieved by setting the lateral terminal state to lane width (3.6m in this example) and assuming the reference path is aligned to the center of the current ego lane.

```% Simulate till the ego vehicle reaches position D % Set Lane Width laneWidth = 3.6; % Check only for Y as there is no change in X at D while currentEgoState(2) >= positionD(2) % Replan at every "replanInterval" simulation step if rem(simStep, replanInterval) == 0 % 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); hAxes1.Title.String = "Ego Centric Binary Occupancy Map"; % 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```

#### Simulate ego vehicle execution to reach the goal point

The `localPlanner` is now configured to navigate from position D to the goal position.

```% Simulate till the ego vehicle reaches Goal position % Check only for X as there is no change in Y at Goal. while currentEgoState(1) <= goalPoint(1) % Replan at every "replanInterval"th simulation step if rem(simStep, replanInterval) == 0 % Compute the replanning time previousReplanTime = simStep*timeResolution; % Updating occupancy map with vehicle information exampleHelperUpdateOccupancyMap(otherVehicles, simStep, currentEgoState); localPlanner.TerminalStates.Longitudinal = 20; localPlanner.TerminalStates.Lateral = [-1 0 1]; desiredTimeBound = localPlanner.TerminalStates.Longitudinal/... ((currentEgoState(1,5) + localPlanner.TerminalStates.Speed)/2); localPlanner.TerminalStates.Time = desiredTimeBound:0.2:desiredTimeBound+1; % 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); hAxes1.Title.String = "Ego Centric Binary Occupancy Map"; % 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); % Goal reached will be true only in this section. if(isGoalReached) break; end % Update the simulation step for the next iteration simStep = simStep + 1; pause(0.01); end```

Log the ego vehicle positions in `egoPoses` variable that is available in the base workspace. You can playback the vehicle path in DrivingScenario using `exampleHelperPlayBackInDS(egoPoses)`.

```% Clear workspace variables that were created during the example run. % This excludes egoPoses to allow the user to playback the simulation in DS exampleHelperUrbanCleanUp;```

### Conclusion

This example has explained how to perform dynamic re-planning in an urban scenario using `trajectoryOptimalFrenet.` In particular, we have learned how to use `trajectoryOptimalFrenet` to realize the following behavior.

• Adaptive Cruise Control

• Negotiating turns

• Lane Change Maneuver.