Rectangle-to-Rectangle Collision Avoidance

In trajectory planning, rectangles are commonly used to represent bounding boxes of moving entities such as vehicles and robots. The rectangle-to-rectangle collision avoidance feature enables collision avoidance constraints between two rectangles, ensuring that the two rectangles do not collide.

Modeling

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

rect2rect_ineq = rectangle_to_rectangle_inequality(
    x_a, y_a, phi_a, width_a, length_a,
    x_b, y_b, phi_b, width_b, length_b,
    distance_to_avoid,
    center_point_avoidance
)
prob.inequality(rect2rect_ineq)

The parameters of the rectangle_to_rectangle_inequality interface are as follows:

• x_a, y_a, phi_a: Center coordinates and orientation of rectangle a (counterclockwise is positive)
• width_a, length_a: Width and length of rectangle a
• x_b, y_b, phi_b: Center coordinates and orientation of rectangle b (counterclockwise is positive)
• width_b, length_b: Width and length of rectangle b
• distance_to_avoid: Avoidance distance
• center_point_avoidance: Whether to perform collision avoidance on the center points of the rectangles (default is True, i.e., center point avoidance is enabled). When True, it adds a collision avoidance constraint for rectangle a's center point against rectangle b and rectangle b's center point against rectangle a, with avoidance distances of distance_to_avoid + min(width_a, length_a) / 2 (i.e., the distance between rectangle a's center point and rectangle b must be greater than the avoidance distance plus half the width or half the length of rectangle a) and distance_to_avoid + min(width_b, length_b) / 2 respectively

info

All parameters of the rectangle_to_rectangle_inequality interface (x_a, y_a, phi_a, width_a, length_a, x_b, y_b, phi_b, width_b, length_b, distance_to_avoid) can be constants, expressions in terms of parameter $p$, or expressions in terms of optimization variable $v$.

warning

• The principle of rectangle_to_rectangle_inequality is to construct 8 point_to_rectangle_inequality constraints: 4 constraints for the four corner points of rectangle a against rectangle b, and 4 constraints for the four corner points of rectangle b against rectangle a
• This constraint cannot handle cases where rectangle a and rectangle b overlap but each rectangle's four corner points are outside the other rectangle (e.g., cross-shaped overlap). You can set center_point_avoidance to True to add center point point_to_rectangle constraints to reduce the occurrence of such cases, or you can manually add point_to_rectangle constraints at other points to handle this situation

Results

Below is the problem setup for vehicle collision avoidance trajectory planning using the rectangle-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 other vehicles
• The vehicle's optimized trajectory consists of 100 stages. At each stage, i.e., at each time point, the vehicle avoids both other vehicles with an avoidance distance of 0.2 m

Note that the positions of other vehicles can differ at different time points, meaning dynamic vehicle-to-vehicle 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):