Point-to-Boundary Collision Avoidance

In trajectory planning, it is sometimes necessary to avoid boundaries with complex shapes, such as road edges, building outlines, etc. The point-to-boundary collision avoidance feature enables collision avoidance constraints between a point and a boundary composed of multiple line segments.

Modeling

Below is an example of a point-to-boundary collision avoidance inequality constraint:

left_bound = boundary('left_bound', max_dim=50, left_positive=False)
right_bound = boundary('right_bound', max_dim=50, left_positive=True)
ineq_left = point_to_boundary_inequality(left_bound, px, py, distance_to_avoid)
ineq_right = point_to_boundary_inequality(right_bound, px, py, distance_to_avoid)
prob.inequality(ineq_left)
prob.inequality(ineq_right)

First, define the boundary using the boundary interface. This interface defines a 2D boundary composed of several points connected sequentially by line segments to form a polyline. Its parameters are as follows:

• name: Boundary name
• max_dim: Maximum number of points composing the boundary, must be an integer greater than or equal to 2
• left_positive: Boolean type, specifies whether the left side of the boundary is the positive distance region (the left side is defined as the left side when traveling from the boundary's starting point along the point sequence direction)

info

• The boundary extends infinitely in the reverse direction of the first line segment
• The boundary extends infinitely in the direction of the last line segment
• When the above two rays intersect with each other or with other line segments of the boundary, the extension stops

warning

• When line segments intersect, it may cause errors in sign or distance calculations

Then, define the point-to-boundary collision avoidance inequality constraint using the point_to_boundary_inequality interface. Its parameters are as follows:

• boundary: A boundary defined using the boundary interface
• px, py: Coordinates of the point that needs collision avoidance, expressions in terms of optimization variable $v$
• distance_to_avoid: Avoidance distance, can be a constant, an expression in terms of parameter $p$, or an expression in terms of optimization variable $v$

Solving

Boundary data is stored in the prob struct. Before solving, you need to set the boundary data. Below is a C/C++ code example:

prob.boundary_left_bound.valid_dim = 6;
prob.boundary_left_bound.x[0] = -5.0; prob.boundary_left_bound.y[0] = -5.0;
prob.boundary_left_bound.x[1] = 5.0; prob.boundary_left_bound.y[1] = -5.0;
prob.boundary_left_bound.x[2] = 5.0; prob.boundary_left_bound.y[2] = 1.0;
prob.boundary_left_bound.x[3] = 15.0; prob.boundary_left_bound.y[3] = 1.0;
prob.boundary_left_bound.x[4] = 25.0; prob.boundary_left_bound.y[4] = -1.0;
prob.boundary_left_bound.x[5] = 35.0; prob.boundary_left_bound.y[5] = -3.0;

prob.boundary_right_bound.valid_dim = 5;
prob.boundary_right_bound.x[0] = -5.0; prob.boundary_right_bound.y[0] = 4.0;
prob.boundary_right_bound.x[1] = 5.0; prob.boundary_right_bound.y[1] = 4.0;
prob.boundary_right_bound.x[2] = 15.0; prob.boundary_right_bound.y[2] = 4.0;
prob.boundary_right_bound.x[3] = 30.0; prob.boundary_right_bound.y[3] = 1.0;
prob.boundary_right_bound.x[4] = 35.0; prob.boundary_right_bound.y[4] = 1.0;

The following notes apply:

• left_bound and right_bound are the boundary names, consistent with the names defined during modeling
• valid_dim is the number of valid points of the boundary, i.e., how many points define the boundary. It must be less than or equal to max_dim defined during modeling and greater than or equal to 2
• x and y are arrays of coordinates for each point on the boundary

info

• When valid_dim is less than 2, the solver returns a distance of 0
• When valid_dim is greater than max_dim, the solver uses max_dim as the number of valid points
• The computational cost is proportional to valid_dim, so it is recommended to merge adjacent collinear points to reduce valid_dim

Results

Below is the problem setup for vehicle collision avoidance trajectory planning using the point-to-boundary collision avoidance feature:

• The vehicle starts at position (-30, 2) m, with the goal of tracking the reference line along the x-axis while avoiding the left and right boundaries (the boundary values are the left_bound and right_bound defined above)
• The vehicle's optimized trajectory consists of 100 stages. At each stage, the vehicle avoids both boundaries (implemented through point-to-boundary collision avoidance using different corner points and interior points of the vehicle), with an avoidance distance of 0.3 m

Below is the trajectory planning result for boundary collision avoidance: