Speed adaptation behavior

The purpose of this behavior is to prevent the vehicle from driving with with excessive lateral acceleration.

MSP side

The MSP creates lane curvature segments, which are sections of a lane defined by uniform curvature.

Driver side and the ercs

The Driver has a RouteCurvatureSegment data structure that looks like this:

This is the core data structure used for processing curvature segments. Each time curvature segments are scanned, the logic within road_element_wrapper.cpp constructs a vector of these segments along the route.

This basic structure is then extended with additional internal data required for further computations. The result is the ExtendedRouteCurvatureSegment, commonly referred to in the code as ercs. This data structure plays a central role and is used extensively throughout the system.

Traversing curve segment use cases

While traversing a curvature segment, four possible cases can occur. In the following graphs, the base of each arrow represents the actor's current state in terms of longitudinal position and speed (lon_pos, lon_speed), while the arrowhead indicates the target objective's (lon_pos, lon_speed).

The above use cases can be grouped into three segment types:

BRAKE Segment (Cases #1 and #3): The system should actively decelerate the vehicle.
SPEED_SATURATION (or SPEED_SAT) Segment (Case #2): The system should prevent the vehicle from accelerating.
SAFE Segment (Case #4): No intervention is required.

Creating the BRAKE and SPEED_SAT segments vectors

When iterating over the RouteCurvatureSegment ahead vector, the module will create, initialise and classify its corresponding extended version. Each erc end speed is calculated. This end speed will then be taken as the start speed for the next segment.

By comparing the entrance and exit speeds to the segment’s max_allowed_speed, the segment type can be determined:

If entrance_speed > max_allowed_speed then this is a BRAKE segment.
If exit_speed > max_allowed_speed, it is a SPEED_SAT segment.
Otherwise it is a SAFE segment.

Adjustments to the Output Objective

Some longitudinal position adjustments are applied to the output objective as curvature ahead is processed:

BRAKE Segment

The initial target longitudinal position (lon_pos) for the output objective in a BRAKE segment is set to the start of the segment. From there, one or more of the following adjustments may be applied:

Shift the lon_pos backward by a configurable distance (brake_segment_distance_tolerance).
This adjustment is always applied.
If reaching the adjusted target lon_pos is not feasible — neither with comfortable nor aggressive deceleration — move the target forward until it becomes feasible with aggressive deceleration.
If the input objective’s lon_pos is before the adjusted target, set the adjusted target to match the input objective’s lon_pos.
If reaching the adjusted target would require extending the objective’s end time beyond that of the input objective, move the target backward as needed to align the end times.

SPEED_SAT Segment

The initial target lon_pos for the SPEED_SAT segment is set to the end of the segment. The following adjustments may be applied:

Shift the lon_pos forward by a configurable distance (speed_sat_segment_distance_tolerance).
If the input objective’s lon_pos is before the adjusted target, update the target to match the input objective’s lon_pos.

Parameters

Parameter	Type	Description	Default
enable	bool	True: active, False: inactive	True
max_lateral_acceleration	acceleration	The maximum lateral acceleration that will be allowed when speed adaptation is performed.	[2..3]mpsps
brake_segment_distance_tolerance	length	The backward offset from the start of a BRAKE segment.	[1.8..2.2]m
speed_sat_segment_distance_tolerance	length	The forward offset from the end of a SPEED_SAT segment.	[0.8..1.2]m
max_allowed_speed_tolerance	speed	The offset (subtracted) from a segment’s max allowed speed. Used to determine the actual target speed for the behavior.	[0.8..1.2]mps