Point-to-Rectangle Collision Avoidance

In trajectory planning, rectangles are commonly used to represent bounding boxes of moving entities such as vehicles and robots. The point-to-rectangle collision avoidance feature enables collision avoidance constraints between a point and a rectangle, ensuring that the point does not enter the area occupied by the rectangle.

Modeling

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

point2rect_ineq = point_to_rectangle_inequality(
    px, py,
    x, y, phi, width, length,
    distance_to_avoid
)
prob.inequality(point2rect_ineq)

The parameters of the point_to_rectangle_inequality interface are as follows:

• px, py: Coordinates of the point that needs collision avoidance
• x, y, phi: Center coordinates and orientation of the rectangle (counterclockwise is positive)
• width, length: Width and length of the rectangle
• distance_to_avoid: Avoidance distance

info

All parameters of the point_to_rectangle_inequality interface (px, py, x, y, phi, width, length, distance_to_avoid) can be constants, expressions in terms of parameter $p$, or expressions in terms of optimization variable $v$.

Results

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

• The vehicle starts at pose 0, with the goal of tracking the reference line along the x-axis (horizontal axis) while avoiding two pedestrians (described as points)
• The vehicle's optimized trajectory consists of 100 stages. At each stage, i.e., at each time point, the vehicle avoids both points with an avoidance distance of 1.0 m

Note that the positions of pedestrians can differ at different time points, meaning dynamic collision avoidance problems can be handled.

Below are trajectory planning results under different initial guesses:

Initial guess above

Initial guess: Optimal trajectory:

Initial guess in the middle

Initial guess: Optimal trajectory:

Initial guess below

Initial guess: Optimal trajectory (limited by maximum steering angle constraint, the solver returns solve_status = 3 infeasibility flag with the minimum constraint violation solution):