Motion Planner

Motion Planner Design

Note: prior to the adoption of a new design proposal in January 2022, this was referred to as the Path Planner.

Introduction

Generates a list of candidate paths as cubic spline curves. These curves begin at the car’s current state and end some distance away. Each path is assigned a cost based on safety, length, and comfortability. These candidate paths will be used to decide how the car should move through the environment. After these curves are generated, we will output the lowest-cost path, and assign it a velocity at every point in time.

Output use case - Behavior Planning

The selected path will be used as an input to the control nodes. The control nodes will determine how to best follow the path at the specified velocity.

Requirements

Since we consider all paths, regardless of whether they will cause a crash or not, requirements are necessary for the resulting costs of each path, along with requirements for the paths themselves.

• Paths should be continuous and smooth.

• The output list of paths should cover a large range of feasible actions.

• Unsafe paths should have a very high cost compared to safe paths.

  • Very unsafe paths should raise an alert with the Safety Manager.

Paths should be continuous and smooth

The usage of cubic spline curves to represent the paths ensures that the Motion Planner can only generate continuous and smooth paths. We use cubic splines to ensure the existence of the first three derivatives of the curve.

The cost function penalizes paths with very high curvature, which would make the car swerve a lot. Also, when deciding on a new path, we account for how similar the generated paths are to the old path, to penalize sudden swerving here as well.

This fulfills the requirement that the path will be able to be followed by the car. If the path had any discontinuities, the car would not physically be able to follow it.

The output list should cover a large range of feasible actions

In this context, a feasible action means that the car could, in theory, take it, even if it would cause the car to crash into a wall.

We use lateral offsets to generate a number of paths. These lateral offsets mean that the car could steer to follow the generated paths. Since we can vary the number of generated paths and the offsets that each path is generated with, we can cover a large range of feasible actions.

This fulfills the requirement that the behavior planner can use the set of generated paths to select a course of action. If there were many very similar paths or few paths to choose from, it would be difficult for the behavior planner to make a good decision.

Unsafe paths should have a very high cost compared to safe paths

An unsafe path is any path that poses significant immediate danger. Examples of unsafe paths are: paths that lead the car off the road, paths that crash the car into a stationary obstacle, paths that hit a moving obstacle, and paths that cause the car to steer so hard that it loses control or flips.

The cost function takes all these scenarios into account, so paths that have these problems will be assigned a high cost compared to paths that do not.

This fulfills the requirement that the behavior planner can use the set of generated paths and their costs to make an informed decision on what to do next. If the behavior planner did not have access to the path cost information, it would have no way of knowing the danger levels of each path.

If the minimum cost of all paths is above a certain threshold, the Motion Planner will raise an alert with the Safety Manager. This would mean that every path is unsafe in some way.

Mechanisms

We will use the algorithm from the paper Dynamic path planning for autonomous driving on various
roads with avoidance of static and moving obstacles by Hu et. al. This algorithm satisfies all the requirements detailed above. We will also modify the collision algorithm to use bounding-boxes instead of circles for collision detection.

This algorithm generates position paths, which we will then process afterwards to assign velocities to each position.

Design

Inputs

• Car state (position, direction, velocity) <<Additional information, such as the position of the steering wheel and state of turn signals, may also be useful here. After all, the wheel takes non-negligible time to move, and we shouldn't change lanes without signaling well in advance. -Joshua>>

• Previous path

• List of static obstacles modeled as bounding-boxes (birds-eye view)

• List of dynamic obstacles modeled as bounding-boxes (birds-eye view + velocity vector)

• HDMapRoute for preferred route

  • As well as a list of lanes adjacent to preferred route for lane changes.

Outputs

• Chosen path for the car to follow.

Constants/Parameters

• Lookahead distance (s_end)

• Number of path candidates generated (2N+1)

• Parameter for effective range of collision detection (σ)

• Car radius for collision detection

• Weight for smoothness (a)

• Weight for consistency (b)

• Maximum lateral acceleration (|a_l|_max = 5000 in paper)

• Safety gain for speed adjustment - influences driving style for overtaking/following a moving obstacle (k_safe = 0.8 in paper)

• Reference speed for path - speed limit (v_curve = 50 in paper)

• Weight for static safety cost function - increasing may make path safer but slower (w_s)

• Weight for comfortability cost function - increasing may make path shorter (w_c)

• weight for dynamic safety cost function - increasing may make overtaking moving obstacle more likely (w_d)

Behavior

The algorithm should transform the preferred route and adjacent lanes into cubic spline curves for use in the later computation steps. Arc length from last waypoint will be used as the parameter for the curve. The curve is represented as two parametric cubic equations, one for x and one for y, of arc length, s.

We then define a center line (another cubic spline curve) for the lane. We will use points along the center of the preferred route to calculate this.

We then have to find the car’s coordinate relative to the center line. This is done in s-p coordinate space, where s is the arc length along the curve, and p is the lateral (perpendicular) offset from the curve at that point. This coordinate transformation makes it possible to find the heading and curvature of the paths. We determine the car’s coordinate using numerical methods, quadrature is used in the paper.

We then generate candidate paths from the car by varying the end point’s lateral offset from the center line, while keeping the arc length constant.

Each candidate path is then evaluated as a combination of 3 cost functions: static safety, comfort, and dynamic safety. More details of the numeric computation can be found in the paper.

We will then pick the candidate path with the lowest cost (call this the position path). The position path will then have velocity information assigned to it. This will be done in two steps:

1. Determine the maximum safe velocity for the path (based on speed limit and lateral acceleration)
2. Lower the velocity as needed to smoothly consider the car's current velocity and target velocity at the end of the path.