Optimal Trajectory Selection

Here we list all feasibility constraints and cost functions. The standard configuration and weights will be enough for a balanced behavior used in most scenarios. However, some cases require fine tuning the weight towards the desired behavior per scenario or adjusting to performance constraints.

Feasibility Constraints

Collision

Rejects trajectories colliding with other agents or static objects as predicted in the estimated traffic state. This function samples multiple points in time during the trajectory and predicts the dynamic traffic state at same time samples. For each sample, it measures the distance from SDV to other vehicles and objects. It rejects trajectories where the resulting distance is less than a threshold (vehicle radius * 2).

Off-lane

Off-Lane: rejects trajectories going beyond the lane boundary, Samples multiple points in time during the trajectory Rejects trajectories going beyond the lane boundary,

Maximum Jerk

Split into Longitudinal and Lateral Jerk. Maximum lateral jerk: Rejects trajectories with high longitudinal jerk above a threshold. Maximum lateral jerk: Rejects trajectories with high lateral jerk above a threshold.

This function samples multiple points in time during the trajectory and evaluate the instant jerk at time t (3rd derivative of displacement in [m/s/s/s]). The thresholds for acceptable Jerk are configurable to accomodate driving styles.

Acceleration

Split into Longitudinal and Lateral Acceleration. Maximum lateral acceleration: Rejects trajectories with high longitudinal acceleration above a threshold. Maximum lateral acceleration: Rejects trajectories with high lateral acceleration above a threshold.

This function samples multiple points in time during the trajectory and evaluate the instant acceleration at time t (2rd derivative of displacement in [m/s/s]). The thresholds for acceptable Acceleration are configurable to accomodate driving styles.

Direction Cost

Rejects trajectories that have a backwards component. Note that reverse maneuvers do not use this constraint.

Cost Functions

Efficiency Cost

Penalizes low average velocity based on the targt velocity

Lane off-set cost

Penalizes distance from lane centre during the entire trajectory. The cost is the average absolute distance per second and expected_offset_per_sec Logistic (0-1) * weight

Total Jerk Cost

Total Acceleration Cost

Penalizes high longitudinal acceleration and high lateral acceleration

Proximity cost

Penalizes proximity to other actors. Samples multiple points in time during the trajectory and predicts dynamic traffic state at same time samples. Computes the closest distance to any vehicle, pedestrian or object ahead. logistic (0-1) to min allowed distance Cost = sum 3 distances * weight

Implementation and Performance Considerations

Feasibility constraints are binary functions. By default, all functions are used. Assigning 0 (zero) means a function is not used. This calibration depends on the scenario goal. For example, a vehicle will reject trajectories with collision by default. However, if a scenario requires a vehicle to target a collision or near-collision, this constraint must be turned off by assigning ```'collision':0``` ManeuverConfig.py

feasibility_constraints: Dict = field(default_factory=lambda:{
    'max_lat_jerk':        1,
    'max_long_jerk':       1,
    'max_long_acc':        1,
    'max_lat_acc':         1,
    'collision':           1,
    'off_lane':            1,
    'direction':           1,
})

Thresholds are also adjusted in the ManeuverConfig file. ManeuverConfig.py

max_long_jerk:float = 10.0              # maximum longitudinal jerk [m/s/s/s]
max_lat_jerk:float = 10.0               # maximum lateral jerk [m/s/s/s]
max_long_acc:float = 12.0               # maximum longitudinal acceleration [m/s/s]
max_lat_acc:float = 4.9                 # maximum lateral acceleration [m/s/s]
expected_offset_per_sec:float = 0.5     # [m]
expected_long_acc_per_sec:float = 1     # [m/s/s]
expected_lat_acc_per_sec:float = 1      # [m/s/s]
expected_long_jerk_per_sec:float = 2    # [m/s/s/s]
expected_lat_jerk_per_sec:float = 2     # [m/s/s/s]

Cost function can have the weights adjusted. Assigning 0 (zero) means a function is not used. Weights must be balanced. For example, a higher proximity cost weight will ensure trajectories getting closer to other vehicles, pedestrians or objects are avoided. ManeuverConfig.py

cost_weight: Dict = field(default_factory=lambda:{
    'time_cost':                1,
    'effic_cost':               1,
    'lane_offset_cost':         1,
    'total_long_jerk_cost':     1,
    'total_lat_jerk_cost':      1,
    'total_long_acc_cost':      1,
    'total_lat_acc_cost':       1,
    'proximity_cost':           10,
})

Feasibility Constraints and cost functions are also calibrated per Maneuver Type. For example, during a Lane Swerve, the off_lane cost must not be used.

@dataclass
class MLaneSwerveConfig(MConfig):
    def __post_init__(self):
            self.feasibility_constraints['off_lane'] = 0

Performance considerations

Feasibility constraints and cost functions require considerable processing power to result in an optimal trajectory selection. In the implementation, we use a configuration option to adjust their precision to the processing budget and performance requirements of the scenario. For example, a scenario with multiple vehicles require more processing power to perform this optimization within the planning time and can have the precision reduced. The standard precision can be adjusted in the ManeuverConfig file, or per maneuver/vehicle from the Behavior Tree. Higher value means better precision, but impacts performance. Lower values have better performance, and affect how integrals are approximated. We recommend keeping at 10 for multi-vehicles and 100 for 1-2 vehicles in a scenario. Precision can also be adjusted to prioritize vehicles interacting with Ego, while the remainig traffic participants can use a lower value. ManeuverConfig.py

cost_precision:float = 100             #from 10 to 100.

This configuration is closely associated with the available Planning Time. Higher precision requires more time for the planning task (lower planner rate). If a scenario uses variable cost precision depending on the maneuver, we recommend setting USE_FIXED_PLANNING_TIME to False. SimConfig.py:

#Planning
PLANNER_RATE = 3                     #Planner tick rate
PLANNING_TIME = 0.150                #[s] Must be less than 1/PLANNTER_RATE (we recommend 0.100 for scenarios with <4 vehicles)
USE_FIXED_PLANNING_TIME = False      #True: the plan will target PLANNING_TIME. False, the planner will vary between PLANNING_TIME and max time (1/PLANNTER_RATE)

Finally, the configuration space defines the nuber of candiate trajectories to be evaluated. We recommend setting a small number of samples per parameter if the planner misses the expectd planning time. An Error message indicates when the planner misses the time. Each vehicle planner runs in a separate process and the precision can be configured per/vehicle.