Drivable Area design

Drivable Area represents the area where ego vehicle can pass.

Purpose / Role

In order to defined the area that ego vehicle can travel safely, we generate drivable area in behavior path planner module. Our drivable area is represented by two line strings, which are left_bound line and right_bound line respectively. Both left_bound and right_bound are created from left and right boundaries of lanelets. Note that left_bound and right bound are generated by generateDrivableArea function.

Assumption

Our drivable area has several assumptions.

• Drivable Area should have all of the necessary area but should not represent unnecessary area for current behaviors. For example, when ego vehicle is in follow lane mode, drivable area should not contain adjacent lanes.

• When generating a drivable area, lanes need to be arranged in the order in which cars pass by (More details can be found in following sections).

• Both left and right bounds should cover the front of the path and the end of the path.

Limitations

Currently, when clipping left bound or right bound, it can clip the bound more than necessary and the generated path might be conservative.

Parameters for drivable area generation

Static expansion

Name	Unit	Type	Description	Default value
drivable_area_right_bound_offset	[m]	double	right offset length to expand drivable area	5.0
drivable_area_left_bound_offset	[m]	double	left offset length to expand drivable area	5.0
drivable_area_types_to_skip	[-]	string	linestring types (as defined in the lanelet map) that will not be expanded	road_border

Dynamic expansion

Name	Unit	Type	Description	Default value
enabled	[-]	boolean	if true, dynamically expand the drivable area based on the path curvature	true
print_runtime	[-]	boolean	if true, runtime is logged by the node	true
max_expansion_distance	[m]	double	maximum distance by which the original drivable area can be expanded (no limit if set to 0)	0.0
smoothing.curvature_average_window	[-]	int	window size used for smoothing the curvatures using a moving window average	3
smoothing.max_bound_rate	[m/m]	double	maximum rate of change of the bound lateral distance over its arc length	1.0
smoothing.arc_length_range	[m]	double	arc length range where an expansion distance is initially applied	2.0
ego.extra_wheel_base	[m]	double	extra ego wheelbase	0.0
ego.extra_front_overhang	[m]	double	extra ego overhang	0.5
ego.extra_width	[m]	double	extra ego width	1.0
dynamic_objects.avoid	[-]	boolean	if true, the drivable area is not expanded in the predicted path of dynamic objects	true
dynamic_objects.extra_footprint_offset.front	[m]	double	extra length to add to the front of the ego footprint	0.5
dynamic_objects.extra_footprint_offset.rear	[m]	double	extra length to add to the rear of the ego footprint	0.5
dynamic_objects.extra_footprint_offset.left	[m]	double	extra length to add to the left of the ego footprint	0.5
dynamic_objects.extra_footprint_offset.right	[m]	double	extra length to add to the rear of the ego footprint	0.5
path_preprocessing.max_arc_length	[m]	double	maximum arc length along the path where the ego footprint is projected (0.0 means no limit)	100.0
path_preprocessing.resample_interval	[m]	double	fixed interval between resampled path points (0.0 means path points are directly used)	2.0
path_preprocessing.reuse_max_deviation	[m]	double	if the path changes by more than this value, the curvatures are recalculated. Otherwise they are reused	0.5
avoid_linestring.types	[-]	string array	linestring types in the lanelet maps that will not be crossed when expanding the drivable area	["road_border", "curbstone"]
avoid_linestring.distance	[m]	double	distance to keep between the drivable area and the linestrings to avoid	0.0

Inner-workings / Algorithms

This section gives details of the generation of the drivable area (left_bound and right_bound).

Drivable Lanes Generation

Before generating drivable areas, drivable lanes need to be sorted. Drivable Lanes are selected in each module (Lane Follow, Avoidance, Lane Change, Goal Planner, Pull Out and etc.), so more details about selection of drivable lanes can be found in each module's document. We use the following structure to define the drivable lanes.

struct DrivalbleLanes
{
    lanelet::ConstLanelet right_lanelet; // right most lane
    lanelet::ConstLanelet left_lanelet; // left most lane
    lanelet::ConstLanelets middle_lanelets; // middle lanes
};

The image of the sorted drivable lanes is depicted in the following picture.

Note that, the order of drivable lanes become

drivable_lanes = {DrivableLane1, DrivableLanes2, DrivableLanes3, DrivableLanes4, DrivableLanes5}

Drivable Area Generation

In this section, a drivable area is created using drivable lanes arranged in the order in which vehicles pass by. We created left_bound from left boundary of the leftmost lanelet and right_bound from right boundary of the rightmost lanelet. The image of the created drivable area will be the following blue lines. Note that the drivable area is defined in the Path and PathWithLaneId messages as

std::vector<geometry_msgs::msg::Point> left_bound;
std::vector<geometry_msgs::msg::Point> right_bound;

and each point of right bound and left bound has a position in the absolute coordinate system.

Drivable Area Expansion

Static Expansion

Each module can statically expand the left and right bounds of the target lanes by the parameter defined values. This enables large vehicles to pass narrow curve. The image of this process can be described as

Note that we only expand right bound of the rightmost lane and left bound of the leftmost lane.

Dynamic Expansion

The drivable area can also be expanded dynamically based on a minimum width calculated from the path curvature and the ego vehicle's properties. If static expansion is also enabled, the dynamic expansion will be done after the static expansion such that both expansions are applied.

Without dynamic expansion	With dynamic expansion

Next we detail the algorithm used to expand the drivable area bounds.

1 Calculate and smooth the path curvature

To avoid sudden changes of the dynamically expanded drivable area, we first try to reuse as much of the previous path and its calculated curvatures as possible. Previous path points and curvatures are reused up to the first previous path point that deviates from the new path by more than the reuse_max_deviation parameter. At this stage, the path is also resampled according to the resampled_interval and cropped according to the max_arc_length. With the resulting preprocessed path points and previous curvatures, curvatures of the new path points are calculated using the 3 points method and smoothed using a moving window average with window size curvature_average_window.

2 For each path point, calculate the closest bound segment and the minimum drivable area width

Each path point is projected on the original left and right drivable area bounds to calculate its corresponding bound index, original distance from the bounds, and the projected point. Additionally, for each path point, the minimum drivable area width is calculated using the following equation: $$W = \frac{a² + 2 al + 2kw + l² + w²}{2k + w}$$ Where \(W\) is the minimum drivable area width, \(a\), is the front overhang of ego, \(l\) is the wheelbase of ego, \(w\) is the width of ego, and \(k\) is the path curvature. This equation was derived from the work of Lim, H., Kim, C., and Jo, A., "Model Predictive Control-Based Lateral Control of Autonomous Large-Size Bus on Road with Large Curvature," SAE Technical Paper 2021-01-0099, 2021.

3 Calculate maximum expansion distances of each bound point based on dynamic objects and linestring of the vector map (optional)

For each drivable area bound point, we calculate its maximum expansion distance as its distance to the closest "obstacle" (either a map linestring with type avoid_linestrings.type, or a dynamic object footprint if dynamic_objects.avoid is set to true). If max_expansion_distance is not 0.0, it is use here if smaller than the distance to the closest obstacle.

4 Calculate by how much each bound point should be pushed away from the path

For each bound point, a shift distance is calculated. such that the resulting width between corresponding left and right bound points is as close as possible to the minimum width calculated in step 2 but the individual shift distance stays bellow the previously calculated maximum expansion distance.

5 Shift bound points by the values calculated in step 4 and remove all loops in the resulting bound

Finally, each bound point is shifted away from the path by the distance calculated in step 4. Once all points have been shifted, loops are removed from the bound and we obtain our final expanded drivable area.

Visualizing maximum drivable area (Debug)

Sometimes, the developers might get a different result between two maps that may look identical during visual inspection.

For example, in the same area, one can perform avoidance and another cannot. This might be related to the maximum drivable area issues due to the non-compliance vector map design from the user.

To debug the issue, the maximum drivable area boundary can be visualized.

The maximum drivable area can be visualize by adding the marker from /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/maximum_drivable_area

Expansion with hatched road markings area

If the hatched road markings area is defined in the lanelet map, the area can be used as a drivable area. Since the area is expressed as a polygon format of Lanelet2, several steps are required for correct expansion.