Lane-Level Path Planning with RoadRunner Scenario

This example shows how to design and implement a lane-level path planner in MATLAB® and cosimulate with RoadRunner Scenario.

Introduction

RoadRunner Scenario is an interactive editor that enables you to design scenarios for simulating and testing automated driving systems. You can place vehicles, define their paths and interactions in the scenario, and then simulate the scenario in the editor. RoadRunner Scenario supports in-editor playback for scenario visualization and connecting to other simulators, such as MATLAB and Simulink®, for cosimulation.

This example shows how to construct a graphical representation of the road network in MATLAB, plan the vehicle path from the specified start and goal points, and follow the path in RoadRunner using a MATLAB System object™.

This figure shows an overview of the information exchanged between RoadRunner Scenario and the lane-level path planner. The path planner reads the map and vehicle runtime information from RoadRunner Scenario to construct the graph using a navGraph (Navigation Toolbox) object. The planner also plans the vehicle trajectory using a plannerAStar (Navigation Toolbox) object.

In this example, you:

• Set Up Environment — Configure MATLAB to interact with RoadRunner Scenario.

• Explore RoadRunner Scenario — Explore the RoadRunner scenario used to simulate the path planner.

• Design Path Planner — Construct the graph representation of the road and plan the vehicle trajectory.

• Simulate Scenario — Cosimulate the scenario using MATLAB and RoadRunner Scenario.

• Simulate Path Replanning — Replan the path during simulation when another vehicle obstructs the planned path.

Set Up Environment

This section shows how to set up the environment to cosimulate MATLAB with RoadRunner Scenario.

Start RoadRunner application interactively by using the roadrunnerSetup function. When the function opens a dialog box, specify the RoadRunner Project Folder and RoadRunner Installation Folder locations.

rrApp = roadrunnerSetup;

This example uses two files that you must add to the RoadRunner project.

• scenario_LLPP_01_CustomEndPoint.rrscenario — Scenario file based on the ScenarioBasic.rrscene scene that ships with RoadRunner.

• LaneLevelPathPlanner.rrbehavior.rrmeta — Behavior file that associates the path-planning behavior, implemented using the MATLAB System object, to the ego vehicle in RoadRunner Scenario.

Copy these files to the RoadRunner project. To learn more about the RoadRunner environment, see RoadRunner Project and Scene System (RoadRunner).

copyfile("scenario_LLPP_01_CustomEndPoint.rrscenario",fullfile(rrApp.status.Project.Filename,"Scenarios"))
copyfile("LaneLevelPathPlanner.rrbehavior.rrmeta",fullfile(rrApp.status.Project.Filename,"Assets/Behaviors/"))

Explore RoadRunner Scenario

Open scenario_LLPP_01_CustomEndPoint scenario.

openScenario(rrApp,"scenario_LLPP_01_CustomEndPoint.rrscenario")

The scenario contains three vehicles. The blue lead car follows the lane-following built-in behavior, and the white ego car follows the custom path-planner behavior. To specify the goal position of the path, this example uses a stationary vehicle named Goal.

To simulate path-planning behavior for the ego vehicle, specify custom behavior for it using the LaneLevelPathPlanner.rrbehavior.rrmeta file, which points to the LaneLevelPathPlanner MATLAB System object. You can configure the path-planning as well as path-following behavior using these properties of the System object:

• GoalPoint — Destination of the ego vehicle path. Use this parameter if the scenario does not specify a stationary vehicle named Goal, as shown in this example.

• LaneChangeDistance — Distance, in meters, over which the ego vehicle completes the lane change maneuver during path planning. Deafult is 20.

• EnableGroundFollowing — Flag to consider banking and grade of the road when following the planned path. Default is false.

• Replan — Flag to replan the path for the same goal point. Default is false.

• ObstacleDistance — Distance, in meters, between the ego vehicle and an obstacle. Default is 30. Use this distance to replan the path of the ego vehicle and avoid collision with an obstacle.

You can tune the path-planning behavior by changing values of custom behavior parameters using the Change Behavior Parameter action. Execution of this action during simulation updates properties of the MATLAB System object. For more information on tuning the behavior using parameters, see Tune Actor Parameters.

This scenario contains variables that enable you to update behavior parameters, the start position, and the goal position.

• EgoGoalPosition — Goal position of the ego vehicle. This variable controls the GoalPoint behavior parameter.

• EnableGroundFollowing — Flag to consider banking and grade of the road. This variable controls the EnableGroundFollowing behavior parameter.

• LaneChangeDistance — Distance, in meters, over which ego the vehicle completes the lane change maneuver. This variable controls the LaneChangeDistance behavior parameter.

• EgoInitialPosition — Initial position of the ego vehicle.

• GoalVehiclePosition — Position of the Goal vehicle.

• LeadCarSpeed — Speed of the lead vehicle.

• ReplanTriggerDistance — Distance, in meters, from obstacle at which replan is triggered.

• ObstacleDistance — Distance, in meters, between the ego vehicle and an obstacle. This variable controls the ObstacleDistance behavior parameter.

Connect to the RoadRunner Scenario server for cosimulation using the createSimulation function.

rrSim = createSimulation(rrApp);

Design Path Planner

This example uses LaneLevelPathPlanner MATLAB System object to plan the path between the start and goal points of the ego vehicle.

The LaneLevelPathPlanner MATLAB System object calls the setupImpl function during initialization. The function constructs a graphical representation of road network to plan the path. In this graphical representation, nodes or states represent the midpoint of lane centers, and edges or links represent connections between lanes.

To construct a graphical representation of the road network, get the nodes and edges of the scene in tabular format using the helperGetNodesEdges helper function.

[nodeTable,edgeTable] = helperGetNodesEdges;

To design a path planner, first create a graph data structure using a navGraph (Navigation Toolbox) object.

graph = navGraph(nodeTable,edgeTable)

graph = 
  navGraph with properties:

           States: [423×2 table]
            Links: [696×2 table]
    LinkWeightFcn: @nav.algs.distanceEuclidean

Get the starting position of the ego vehicle by reading the EgoInitialPosition variable from RoadRunner Scenario using the getScenarioVariable function.

egoStartPosition = getScenarioVariable(rrApp,"EgoInitialPosition");
egoStartPosition = str2double(split(egoStartPosition,","))';

Get the position of the goal vehicle by reading the GoalVehiclePosition variable from RoadRunner Scenario. The position of the stationary vehicle Goal is the goal position of the ego vehicle.

egoGoalPosition = getScenarioVariable(rrApp,"GoalVehiclePosition");
egoGoalPosition = str2double(split(egoGoalPosition,","))';

Create an A* planner from the navGraph object using a plannerAStar (Navigation Toolbox) object.

planner = plannerAStar(graph);

Use the planPath method of the HelperPlanPath helper class to plan the path between the start position and the goal position of the ego vehicle using the plan function. planPath takes the plannerAStar object, navGraph object, ego vehicle start position, and goal position as inputs, as well as these optional name-value arguments:

• UseCustomWeightFunction — Set this flag to true to specify a custom weight function using the LinkWeightFunction property of the navGraph object. Default is false.

• LaneChangeDistance — Distance to complete the lane change maneuver in the planned path, in meters. Default is 20.

refPath = HelperPlanPath.planPath(planner,graph,egoStartPosition,egoGoalPosition);

Plot the planned path.

pRefPath = plot(refPath(:,1),refPath(:,2),Color="g",LineWidth=2);
% Plot markers for start and goal positions
pStart = plot(egoStartPosition(1),egoStartPosition(2),"o",MarkerFaceColor="b",MarkerSize=4);
pEnd = plot(egoGoalPosition(1),egoGoalPosition(2),"o",MarkerFaceColor="r",MarkerSize=4);
% Add legends
legend([pRefPath pStart pEnd],{"Reference path","Start position","Goal position"},Location="northwest")

To enable the ego vehicle to follow the planned path, the LaneLevelPathPlanner MATLAB System object calls the stepImpl function at each simulation step. The function calls the HelperPolylineEvaluator object, which computes the next ego position using the current ego speed and current ego position.

If you enable the Replan flag, the stepImpl function replans the ego trajectory using the replanPath function of the LaneLevelPathPlanner System object. The replanPath function has this primary interface:

replanPath(obj,pose,obstacleDistance)

• pose — Pose information of the ego vehicle at the time step when replan is triggered.

• obstacleDistance — Distance between the ego vehicle and the obstacle at the time step when replan is triggered.

replanPath method updates cost of the obstructing lane to infinite and replans the path with updated weight function using planPath method of HelperPlanPath helper class.

Simulate Scenario

Assign the EgoGoalPosition variable using the setScenarioVariable function.

setScenarioVariable(rrApp,EgoGoalPosition=['[',num2str(egoGoalPosition),']'])

Simulate the scenario.

set(rrSim,SimulationCommand="Start")
while strcmp(get(rrSim,"SimulationStatus"),"Running")
    pause(0.01)
end

Notice that the ego vehicle travels on the planned path.

Simulate Path Replanning

This example provides a scenario that is configured to perform path replanning for the ego vehicle when it comes within 30 meters of a stationary lead vehicle along the planned path.

Update the lead vehicle speed to 0 m/s using the LeadCarSpeed variable.

setScenarioVariable(rrApp,LeadCarSpeed="0")

Set the ReplanTriggerDistance and ObstacleDistance variables to 30.

obstacleDistance = 30;
setScenarioVariable(rrApp,ReplanTriggerDistance=num2str(obstacleDistance));
setScenarioVariable(rrApp,ObstacleDistance=num2str(obstacleDistance));

Run the simulation and notice that the ego vehicle changes lane once it is within 30 meters of the lead vehicle, and starts following the replanned path.

set(rrSim,SimulationCommand="Start")
while strcmp(get(rrSim,"SimulationStatus"),"Running")
    pause(0.01)
end