Maneuver Models

The Maneuver layer is responsible for the actual vehicle motion on the road. It handles a decision from the Behavior Layer outputs a feasible trajectory that can be performed by a real vehicle in the same conditions.

To achieve this, ther model is bounded by a set of feasibility constraints that will not allow unexpected motion in traffic, respecting the vehicle dynamics and resulting in realistic movement.

Maneuvers are reacive, adjusting their acceleration in real time according to surrounding vehicles. A maneuver can be composed with one or multiple trajectories. Tajectories are kept short (2 to 5 seconds). The decision to start, finish, or abort a maneuver is handled by the Behavior layer.

Definitions:

MANEUVER: "a low-level driving task between a start state and a target state over a period of time in a run-time re-planning loop to account for the dynamics of the surrounding traffic."

TRAJECTORY: "Motion Profile (displacement, velocity, and acceleration) between start state and a target state over a fixed time."

All GeoScenario maneuvers follow a general model:

In the current implementation the model has 6 micro-models to cover typical driving tasks in traffic:

  • Velocity Keeping: reach and keep a target velocity while driving in the same lane.
  • Vehicle Following: the goal is to adjust the distance and velocity from the leading vehicle in order to keep a time gap.
  • Lane Swerve: reach any position in the lateral lane (if a valid lane exists in that location).
  • Cut-In
  • Stop
  • Reverse

Additional maneuvers will be added to extend the method to more complex scenarios and to cover a broader set of the driving task.

Next, we explore the 3 major steps fo the Model.

1. Target Finding

The main distinction between maneuvers is how they define a target state and how they select the optimal trajectory. They are controlled by a set of configurable maneuver parameters shaping the driving style, and allow erratic behavior to be injected in a scenario (e.g., unsafe time-gap between two vehicles leading to a near-crash during a maneuver).

Note: start state is non-deterministic and can never be assumed at design time.

Each maneuver model uses its own criteria to define a target and a trajectory time.
Defining this target requires evaluating the road configuration, static objects that can block the drivable surface, and how the surrounding traffic participant (vehicles and pedestrians) are moving.

Note: Parameters names used in the reference implementation.

Velocity Keeping

desired velocity time to reach velocity maximum velocity difference between current and target

Vehicle Following

Lane Swerve

Cut-In

Stop

Reverse

While defining the configuration (behavior calibration), some parameters can be defined as: - Hard constraints: specific configuration values that must be achieved (example, a vehicle driving at 10 m/s) - Soft constraints: range values around a target (exampple: vehicle driving within 50% around 10 m/s)

The constrains create the maneuver configuration space. However, instead of selecting a concrete value and creating a single target within this space, the model samples multiple values and create a target set.

  • Normal Sampling:
  • Uniform Random:
  • Linear Space Distribution:

Many values in this configuration space can lead to trajectories that are not feasible: - Do not respect the dynamics of the vehicle - Not reflect human-driving behavior - Lead to unexpected situations (for example, collisions) We leverage the configuration space to create multiple trajectory candidates, and later evaluate the best fitting.

2. Trajectory Fitting

For each target: fit a trajectory from Start State and Target State Humans drive balancing progress an perceived comfort (jerk) In a one-dimensional problem, quintic polynomials are the jerk-optimal connection between : P0 = [p0,p ̇0,p ̈0] P1 =[p1,p ̇1,p ̈1] within the time interval T := t1 − t0 Separate trajectory in S (longitudinal) and D (lateral) using the Frénet Frame.

Fit polynomial using boundaries: Position, Velocity, Acceleration at t0 and t1 Solve and find 6 coefficients (a0 to a5) satisfying boundary conditions s(t) = a0 + a1t + a2t2 + a3t3 + a4t4 + a5t5 s ̇(t) = a1t + 2a2t + 3a3t2 + 4a4t3 + 5a5t4 s ̇ ̇(t) = 2a2 + 6a6t + 12a4t2 + 20a5t3 Repeat the process for S and D for each target in Target State Set

3. Optimal Trajectory Selection

The best trajectory is the feasible option with lower cost.

Feasibility functions and cost can be adjusted by maneuver and per node in the Behavior Tree If a given scenario requires a vehicle to drive close to other vehicles, the proximity weight must be reduced.
Together, feasibility constraints and cost functions keep selected trajectories constrained to realistic vehicle motion,

Feasibility constraints: Collision: rejects trajectories colliding with other agents or static objects. Direction: rejects trajectories with any inversion. Off-Lane: rejects trajectories going beyond the lane boundary, Maximum lateral/longitudinal jerk: rejects high values of jerk above a threshold. Maximum lateral/longitudinal acceleration: rejects high values of acceleration above a threshold.

Goal: Evaluate all trajectories and eliminate bad candidates Binary Functions

Assign a lower cost to the best candidates Balancing conflicting qualities such as efficiency and comfort,
Keep the vehicle closer to the desired behavior.

Cost Functions: Time cost: Penalizes trajectories longer or shorter than target time. Efficiency cost: Penalizes low average velocity. Lane-offset cost: Penalizes distance from lane center during the entire trajectory. Total jerk cost: Penalizes high longitudinal and lateral jerk over the entire trajectory. Total acceleration cost: Penalizes high longitudinal and lateral acceleration over the entire trajectory

Execution