Obstacle Velocity Limiter

Purpose

This node limits the velocity when driving in the direction of an obstacle. For example, it allows to reduce the velocity when driving close to a guard rail in a curve.

Without this node	With this node

Inner-workings / Algorithms

Using a parameter min_ttc (minimum time to collision), the node set velocity limits such that no collision with an obstacle would occur, even without new control inputs for a duration of min_ttc.

To achieve this, the motion of the ego vehicle is simulated forward in time at each point of the trajectory to create a corresponding footprint. If the footprint collides with some obstacle, the velocity at the trajectory point is reduced such that the new simulated footprint do not have any collision.

Simulated Motion, Footprint, and Collision Distance

The motion of the ego vehicle is simulated at each trajectory point using the heading, velocity, and steering defined at the point. Footprints are then constructed from these simulations and checked for collision. If a collision is found, the distance from the trajectory point is used to calculate the adjusted velocity that would produce a collision-free footprint. Parameter simulation.distance_method allow to switch between an exact distance calculation and a less expensive approximation using a simple euclidean distance.

Two models can be selected with parameter simulation.model for simulating the motion of the vehicle: a simple particle model and a more complicated bicycle model.

Particle Model

The particle model uses the constant heading and velocity of the vehicle at a trajectory point to simulate the future motion. The simulated forward motion corresponds to a straight line and the footprint to a rectangle.

Footprint

The rectangle footprint is built from 2 lines parallel to the simulated forward motion and at a distance of half the vehicle width.

Distance

When a collision point is found within the footprint, the distance is calculated as described in the following figure.

Bicycle Model

The bicycle model uses the constant heading, velocity, and steering of the vehicle at a trajectory point to simulate the future motion. The simulated forward motion corresponds to an arc around the circle of curvature associated with the steering. Uncertainty in the steering can be introduced with the simulation.steering_offset parameter which will generate a range of motion from a left-most to a right-most steering. This results in 3 curved lines starting from the same trajectory point. A parameter simulation.nb_points is used to adjust the precision of these lines, with a minimum of 2 resulting in straight lines and higher values increasing the precision of the curves.

By default, the steering values contained in the trajectory message are used. Parameter trajectory_preprocessing.calculate_steering_angles allows to recalculate these values when set to true.

Footprint

The footprint of the bicycle model is created from lines parallel to the left and right simulated motion at a distance of half the vehicle width. In addition, the two points on the left and right of the end point of the central simulated motion are used to complete the polygon.

Distance

The distance to a collision point is calculated by finding the curvature circle passing through the trajectory point and the collision point.

Obstacle Detection

Obstacles are represented as points or linestrings (i.e., sequence of points) around the obstacles and are constructed from an occupancy grid, a pointcloud, or the lanelet map. The lanelet map is always checked for obstacles but the other source is switched using parameter obstacles.dynamic_source.

To efficiently find obstacles intersecting with a footprint, they are stored in a R-tree. Two trees are used, one for the obstacle points, and one for the obstacle linestrings (which are decomposed into segments to simplify the R-tree).

Obstacle masks

Dynamic obstacles

Moving obstacles such as other cars should not be considered by this module. These obstacles are detected by the perception modules and represented as polygons. Obstacles inside these polygons are ignored.

Only dynamic obstacles with a velocity above parameter obstacles.dynamic_obstacles_min_vel are removed.

To deal with delays and precision errors, the polygons can be enlarged with parameter obstacles.dynamic_obstacles_buffer.

Obstacles outside of the safety envelope

Obstacles that are not inside any forward simulated footprint are ignored if parameter obstacles.filter_envelope is set to true. The safety envelope polygon is built from all the footprints and used as a positive mask on the occupancy grid or pointcloud.

This option can reduce the total number of obstacles which reduces the cost of collision detection. However, the cost of masking the envelope is usually too high to be interesting.

Obstacles on the ego path

If parameter obstacles.ignore_obstacles_on_path is set to true, a polygon mask is built from the trajectory and the vehicle dimension. Any obstacle in this polygon is ignored.

File truncated at 100 lines see the full file