An autonomous parking path collision detection method based on quadtree map

Abstract

The application discloses an autonomous parking path collision detection method based on a quadtree map, and comprises the following steps: obtaining obstacle information in real time through an ultrasonic radar, discretizing the obstacle into points, determining a quadtree map node where the obstacle is located, saving the discrete points in a minimum unit node of the quadtree map, and using an AABB bounding box as a discrete point bounding volume to construct the quadtree map; obtaining a planned parking path, taking the path point closest to the current vehicle as a starting point, and pre-aiming a section of the path; sequentially traversing a vehicle AABB bounding box and an OBB bounding box set, screening whether the vehicle AABB bounding box intersects with an AABB bounding box of a quadtree node with an obstacle discrete point, and iteratively performing collision detection on the vehicle OBB bounding box and the screened AABB bounding box of the quadtree node. The application solves the problem that a traditional path collision detection method is time-consuming in a complex scene. The collision detection method is time-consuming in the complex scene, and the path collision detection precision and the detection efficiency are improved.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of autonomous driving vehicles, in particular to collision detection technology in path planning in autonomous parking systems. BACKGROUND

[0002] In the development process of autonomous driving technology, autonomous parking system as one of the important applications, has been widely concerned and researched. Traditional autonomous parking path planning and obstacle collision detection methods usually rely on global map and grid map based path planning. These methods can achieve good results in simple environment, but in complex parking scenarios, especially due to the limited detection range of sensors, it is impossible to obtain global obstacle information, and when obstacles appear in the parking process, the calculation time and detection accuracy are often difficult to meet the actual demand.

[0003] In the prior art, the common path collision detection methods mainly include the following:

[0004] Grid map based method: this method discretizes the environment into fixed size grids, and detects whether collision occurs by detecting the position of the vehicle on the grid map. The advantage of this method is simple to implement, but there is a contradiction between resolution and computational efficiency. The calculation amount is large under high resolution, and the detection accuracy is low under low resolution.

[0005] Polygon collision detection based method: this method simplifies the obstacle and vehicle into polygon, and judges the collision through geometric calculation. This method can achieve high detection accuracy in theory, but the calculation complexity is high, especially when dealing with a large number of obstacles, the calculation time is often negligible.

[0006] In summary, the prior art has the problems of long calculation time, low collision detection accuracy and poor real-time performance in dealing with obstacle collision detection in complex environment. Therefore, it is of great practical significance and application value to develop a method that can efficiently and accurately detect obstacle collision in complex scenes. The present application proposes a four tree map based autonomous parking path collision detection method, which aims to solve the problems in the prior art and improve the detection efficiency and accuracy. SUMMARY

[0007] The present application solves the problem of long time consumption of traditional path collision detection method in complex scene. The present application establishes a quad tree map, and iteratively detects collision between the OBB bounding box of the vehicle and the AABB bounding box of the screened obstacle discrete points of the quad tree node. The time consumption of the algorithm in complex scene is reduced, and the detection efficiency is improved while the path collision detection accuracy is improved.

[0008] In order to achieve the above object, the application adopts the following technical scheme: An autonomous parking path collision detection method based on a quadtree map, which comprises the following steps:

[0009] Step S1: Real-time acquisition of obstacle information by ultrasonic radar and coordinate conversion;

[0010] Step S2: Discretization of obstacles, determination of the quadtree nodes where the obstacles are located, and iterative subdivision of the nodes until the smallest unit nodes are obtained, so as to construct a quadtree map; the quadtree map comprises:

[0011] a. Each node in the quadtree map is an AABB bounding box, each parent node has four child nodes, and the length and width of the child node bounding box are half of the parent node;

[0012] b. Recursive establishment of the AABB bounding box of each obstacle discretization point and the corresponding node;

[0013] c. The node AABB bounding box of each obstacle discretization point is necessarily the smallest unit node AABB bounding box;

[0014] Step S3: Acquisition of the planned parking path, taking the path point closest to the current vehicle as the starting point, and pre-aiming a section of the path; and calculation of the vehicle AABB bounding box and the OBB bounding box set at the pre-aimed path point;

[0015] Step S4: Iterative traversal of the vehicle AABB bounding box and the OBB bounding box set, with the vehicle AABB bounding box and the quadtree node AABB bounding box with obstacle discretization points as the intersection for screening, and the vehicle OBB bounding box and the screened quadtree node AABB bounding box for iterative collision detection.

[0016] Further, the step S1 comprises the following specific steps:

[0017] Step S1-1: Real-time acquisition of obstacle information in the environment by ultrasonic radar, including the height attribute, type of the obstacle, and the start and end point information in the vehicle coordinate system;

[0018] Step S1-2: Taking the point at the upper right corner of the target parking space as the origin of the parking coordinate system, and converting the coordinates of the obstacle to the parking coordinate system; the formula for coordinate conversion comprises:

[0019]

[0020] Wherein, dx, dy, and θ are the coordinates and angle of the origin of the parking coordinate system in the vehicle coordinate system; (x, y) is the coordinates of the obstacle in the vehicle coordinate system, and (x', y') is the coordinates of the obstacle in the parking coordinate system.

[0021] Further, the step S2 is specifically as follows:

[0022] Step S2-1: initializing the quadtree map, defining the center coordinates, length and width of the map, and the minimum unit node AABB bounding box size, and constructing the root node AABB bounding box of the quadtree map;

[0023] Step S2-2: discretizing the obstacle information obtained from step S1, and discretizing the obstacle line segment into multiple discrete points by the following steps;

[0024] S221, input the coordinates of the two endpoints of the obstacle, wherein the obstacle starting point coordinates are (x0, y0), and the obstacle end point coordinates are (x l , y l );

[0025] S222, taking the obstacle starting point (x0, y0) as the first discrete point;

[0026] S223, calculating the constant Δx = x l -x0, Δy = y l -y0, and obtaining the first value of the decision parameter:

[0027] p0 = 2Δy - Δx;

[0028] S224, starting from k = 0, at each x k along the line, the following judgment is made:

[0029] If p k < 0, then the next obstacle discrete point is (x k + 1, y k ), and p k+1 = p k + 2Δy

[0030] If p k ≥ 0, then the next obstacle discrete point is (x k + 1, y k + 1), and p k+1 = p k + 2Δy - 2Δx

[0031] S225, repeat step S224 for Δx times;

[0032] Step S2-3: traverse the obstacle discrete points, determine the quadtree map nodes where they are located, and iteratively subdivide these nodes until they are subdivided into minimum unit nodes. Save the obstacle discrete points in the minimum unit nodes of the quadtree map, and use the AABB bounding box as the discrete point bounding volume to construct the quadtree map.

[0033] Further, the step S3 is specifically as follows:

[0034] Step S3-1: Obtain a pre-planned parking path composed of a series of path points, the path points including position information and direction information;

[0035] Step S3-2: Obtain vehicle positioning information, find the path point closest to the vehicle, called the matching point

[0036] Step S3-3: Take the matching point as the starting point, and pre-look a certain number of path points from the point, generate a vehicle AABB and OBB bounding box set according to the pre-looked path points. The vehicle AABB and OBB bounding box can be calculated by the geometric size of the vehicle and the direction information of the path point.

[0037] Further, the step S4 is specifically as follows:

[0038] Step S4-1: Traverse the vehicle AABB bounding box set and the OBB bounding box set generated in step S3

[0039] Step S4-2: Determine whether the vehicle AABB bounding box intersects with the four-tree node AABB bounding box of the discrete point of the obstacle; if intersecting, proceed to step S4-3, and if not intersecting, proceed to step S4-1;

[0040] Step S4-3: Obtain the vehicle OBB bounding box of the current path point, and determine whether the vehicle collides with the obstacle by using the following steps;

[0041] S431, calculate the projection axis of the vehicle OBB bounding box and the four-tree node AABB bounding box of the obstacle:

[0042] S432, traverse all the projection axes to obtain the projection of the vehicle OBB bounding box and the four-tree node AABB bounding box on each projection axis:

[0043] S433, determine whether the projections on the projection axes overlap, and if there is overlap, it means that the vehicle OBB bounding box and the four-tree node AABB bounding box intersect;

[0044] S434, for the intersecting four-tree node, obtain all the child nodes, and repeat S431 for the child nodes respectively until the intersecting four-tree node is the minimum unit node; if the minimum unit node is the obstacle node, it means that the vehicle collides with the obstacle;

[0045] Step S4-4: If a collision is detected, take appropriate obstacle avoidance measures to ensure that the vehicle can safely park.

[0046] Compared with the prior art, the present application has at least the following advantages:

[0047] Firstly, the obstacle information is effectively managed and retrieved through the quadtree structure, and the intersection of the vehicle AABB bounding box and the quadtree node AABB bounding box of the obstacle discrete point is used for screening, so that the calculation time of the collision detection method in a complex scene is reduced, and the obstacle collision detection efficiency can be obviously improved.

[0048] Secondly, the obstacle is discretized and stored in the minimum unit node of the quadtree map, the AABB bounding box is used as the enclosure of the obstacle discrete point, and the OBB bounding box is used as the enclosure of the vehicle, so that the obstacle collision detection accuracy is obviously improved. BRIEF DESCRIPTION OF DRAWINGS

[0049] Figure 1 The algorithm flowchart of the present application is shown in the figure;

[0050] Figure 2 The environmental information diagram after coordinate conversion of the present application is shown in the figure;

[0051] Figure 3 The quadtree map constructed by the present application is shown in the figure;

[0052] Figure 4 The vehicle OBB bounding box set to participate in the present cycle collision detection of the present application is shown in the figure;

[0053] Figure 5 The autonomous parking path collision detection method effect diagram of the present application is shown in the figure;

[0054] Figure 6 The obstacle discretization algorithm effect diagram of the present application is shown in the figure;

[0055] Figure 7 The vehicle OBB bounding box and the quadtree node AABB bounding box collision detection schematic diagram of the present application is shown in the figure;

[0056] Figure 8 The obstacle path distribution schematic diagram of the vertical parking space, the horizontal parking space and the broken head parking space is shown in the figure; DETAILED DESCRIPTION

[0057] The technical solutions in the embodiments of the present application will be described clearly and completely in combination with the accompanying drawings in the embodiments of the present application.

[0058] Figure 1For the algorithm flowchart of the application, first, the environmental information is collected by ultrasonic radar, and the obstacle information is converted to the parking coordinate system through coordinate conversion. Then the obstacles are discretized to construct a quadtree map. Then the planned parking path is obtained, the matching points are calculated, and the vehicle AABB and OBB bounding box set is generated by looking ahead a certain path. Finally, whether the vehicle AABB bounding box intersects with the quadtree node AABB bounding box of the discretized obstacle points is screened, and the vehicle OBB bounding box and the screened quadtree node AABB bounding box are iterated for collision detection. Finally, the result of collision detection is output.

[0059] Figure 2 For the environmental information graph converted by the application, it includes the obstacles detected by the ultrasonic radar, the target parking space, and the current vehicle position.

[0060] Figure 3 For the quadtree map constructed by the application, the map length is 16m, the width is 12m, and the minimum resolution of the node is 0.05m. When there is an obstacle at a certain position, the quadtree map will be continuously subdivided until the minimum unit node is reached.

[0061] Figure 4 For the vehicle OBB bounding box set of the application to participate in the current cycle collision detection, in combination with the detection range of the ultrasonic radar, 20 path points are looked ahead to participate in the current cycle collision detection.

[0062] Figure 5 For the effect diagram of the autonomous parking path collision detection method of the application, the vehicle OBB bounding box set generated according to the looked-ahead path points is traversed, and whether the vehicle OBB bounding box and the quadtree node AABB bounding box collide is detected by using the collision detection algorithm proposed in the application. The 13th vehicle OBB bounding box in the figure collides with the quadtree node AABB bounding box in front.

[0063] Figure 6 For the obstacle discretization algorithm schematic diagram of the application, it can be seen that each point discretized according to the obstacle line segment is located in the minimum unit node of the quadtree map.

[0064] Figure 7 For the vehicle OBB bounding box and quadtree node AABB bounding box collision detection schematic diagram of the application.

[0065] Figure 8 For the obstacle path distribution schematic diagram of the vertical parking space, the horizontal parking space, and the broken-head parking space;

[0066] The application proposes an autonomous parking path collision detection method based on a quadtree map, and the specific steps are as follows:

[0067] Step S1: Real-time acquisition of obstacle information by ultrasonic radar, and coordinate conversion.

[0068] Step S1-1: Real-time acquisition of obstacle information in the environment by ultrasonic radar, including the height attribute, type of the obstacle, and the start and end point information in the vehicle coordinate system.

[0069] Step S1-2: Taking the point at the upper right corner of the target parking space as the origin of the parking coordinate system, the coordinates of the obstacles are converted to the parking coordinate system. The formula of the coordinate conversion includes:

[0070]

[0071] Where dx, dy, θ are the coordinates and angle of the origin of the parking coordinate system in the vehicle coordinate system; (x, y) is the coordinates of the obstacle in the vehicle coordinate system, and (x', y') is the coordinates of the obstacle in the parking coordinate system. As shown in Figure 2

[0072] Step S2: Determine the four-tree map node it is in, and iteratively subdivide these nodes until the subdivision is the smallest unit node. Save the discrete points in the smallest unit node of the four-tree map, and use the AABB bounding box as the discrete point bounding volume to construct the four-tree map.

[0073] Step S2-1: Initialize the four-tree map, define the center coordinates of the map as the origin of the parking coordinate, the length as 16m, the width as 12m, and the minimum unit node AABB bounding box size as 0.05m, and construct the root node AABB bounding box of the four-tree map.

[0074] Step S2-2: Discretize the obstacle information obtained from step S1, and discretize the line segment representing the obstacle into multiple discrete points using the following steps. The discretization effect is shown in Figure 6 As can be seen, each point discretized from the obstacle line segment is located in the smallest unit node of the four-tree map.

[0075] 1. Input the coordinates of the two endpoints of the obstacle, where the obstacle start point coordinates are (x0, y0), and the obstacle end point coordinates are (x l , y l )

[0076] 2. Take the obstacle start point (x0, y0) as the first discrete point;

[0077] 3. Calculate the constants Δx = x l -x0, Δy = y l -y0, and get the first value of the decision parameter:

[0078] p0 = 2Δy - Δx​

[0079] 4. From k = 0, at each x k along the line, make the following judgments:

[0080] If p k < 0, then the next obstacle discrete point is (x k + 1, y k ), and p k+1 = p k + 2Δy

[0081] If p k ≥ 0, then the next obstacle discrete point is (x k + 1, y k + 1), and p k+1 = p k + 2Δy - 2Δx

[0082] 5. Repeat step 4 for a total of Δx times.

[0083] Step S2-3: Traverse the obstacle discrete points, determine the quadtree map nodes they are in, and iteratively subdivide these nodes until they are subdivided into the smallest unit nodes. Save the discrete points in the smallest unit nodes of the quadtree map, and use the AABB bounding box as the discrete point bounding volume to construct the quadtree map. The constructed quadtree map is shown in Figure 3 .

[0084] Step S3: Obtain the planned parking path, take the path point closest to the current vehicle as the starting point, and pre-look ahead a certain distance of the path. And calculate the vehicle AABB bounding box and the OBB bounding box set at the pre-looked path point;

[0085] Step S3-1: Obtain the pre-planned parking path, which consists of a series of path points, including position information and direction information.

[0086] Step S3-2: Obtain the vehicle positioning information, find the path point closest to the vehicle, called the matching point

[0087] Step S3-3: Take the matching point as the starting point, pre-look ahead a certain number of path points from the point, and generate the vehicle AABB and OBB bounding box set according to the pre-looked path points. The vehicle AABB and OBB bounding box can be calculated from the geometric dimensions of the vehicle and the position and direction information of the path point.

[0088] The generated vehicle OBB bounding box set is shown in Figure 4 .

[0089] Step S4: Traverse the vehicle AABB bounding box and the OBB bounding box set in sequence, and perform collision detection by checking whether the vehicle AABB bounding box intersects with the four-tree node AABB bounding box of the discrete point of the obstacle.

[0090] Step S4-1: Traverse the vehicle AABB bounding box set and the OBB bounding box set generated in step S3

[0091] Step S4-2: Determine whether the vehicle AABB bounding box intersects with the four-tree node AABB bounding box of the discrete point of the obstacle. If yes, proceed to step S4-3, and if no, proceed to step S4-1.

[0092] Step S4-3: Traverse the vehicle OBB bounding box set, and determine whether the vehicle collides with the obstacle by using the following steps. The schematic diagram is shown in Figure 7

[0093] The vertices of the vehicle OBB bounding box are A, B, C, and D, and the four vertices of the four-tree node AABB bounding box are a, b, c, and d.

[0094] 1. Calculate the projection axis:

[0095] For the vehicle OBB bounding box and the four-tree node AABB bounding box, the normal vector of each edge is selected as the projection axis. Since the vehicle OBB bounding box and the four-tree node AABB bounding box are rectangles, the edges are perpendicular to each other, and therefore the unit vector of each edge is selected as the projection axis. Therefore, four projection axes are obtained

[0096] 2. Traverse all projection axes:

[0097] For each projection axis, for example Project the edges of the vehicle OBB bounding box and the four-tree node AABB bounding box onto the axis.

[0098] 3. Calculate the projection interval:

[0099] Calculate the projection interval [A0, D0] of the vehicle OBB bounding box on the axis.

[0100] Calculate the projection interval [a0, c0] of the four-tree node AABB bounding box on the axis.

[0101] 4. Check the overlap of the projection intervals:

[0102] Check whether [A0, D0] and [a0, c0] overlap.

[0103] ​If there is overlap, continue checking the next projection axis.

[0104] If there is no overlap, then the vehicle's OBB bounding box and the quadtree node's AABB bounding box do not intersect.

[0105] 5. Repeat the above steps:

[0106] Repeat the above steps to check for overlap of projection intervals on all projection axes.

[0107] If the projection ranges on all projection axes overlap, then the vehicle's OBB bounding box and the quadtree node's AABB bounding box intersect.

[0108] 6. Obtain child nodes and iteratively detect collisions:

[0109] For intersecting quadtree nodes, obtain all child nodes and repeat step 1 for each child node until the intersecting quadtree node is the smallest unit node. If this smallest unit node is an obstacle node, it means that the vehicle has collided with an obstacle.

[0110] Collision detection results as follows Figure 5 As shown in the diagram, the 13th vehicle's OBB bounding box intersects with the bounding box of the quadtree obstacle node AABB in front. This indicates that the vehicle has collided with the obstacle in front.

[0111] Step S4-4: If a collision is detected, take appropriate obstacle avoidance measures to ensure that the vehicle can be parked safely.

[0112] At this point, the autonomous parking path obstacle collision detection method of the present invention exhibits significantly higher detection efficiency than the grid-based collision detection algorithm at higher resolutions. This plays a crucial role in saving computational resources, improving collision detection accuracy, and achieving real-time dynamic planning. Table 1 shows a comparison of the detection efficiency of the present invention and the grid-based collision detection method at different resolutions (0.05m for the present invention's method and 0.1m for the grid-based method). (Comparisons were made in three scenarios; the hardware platform consisted of an Intel Core i7-6820HK@2.7GHz CPU, 16GB of memory, and was developed using C++ under Linux). It is evident that the computational efficiency and detection accuracy of the present invention are significantly higher than those of the grid-based collision detection method in different scenarios. Figure 8 The diagram shows the obstacle path distribution for three scenarios: vertical parking space, horizontal parking space, and dead-end parking space.

[0113] Table 1

[0114]

[0115] From the above, the present application has at least the following advantages:

[0116] 1. Efficiently manage and retrieve obstacle information through quadtree structure, and use vehicle AABB bounding box and whether the quadtree node AABB bounding box of obstacle discrete point intersects to do screening, which reduces the calculation time of collision detection method in complex scene, and can obviously improve the efficiency of obstacle collision detection.

[0117] 2. Discretize and store obstacles in the smallest unit node of quadtree map, use AABB bounding box as the bounding volume of obstacle discrete point, and use OBB bounding box as the bounding volume of vehicle, which obviously improves the accuracy of obstacle collision detection.

Claims

1. An autonomous parking path collision detection method based on quadtree maps, characterized in that, The method includes the following steps: Step S1: Obstacle information is acquired in real time using ultrasonic radar, and coordinate transformation is performed; Step S2: Discretize the obstacles into points, determine the quadtree nodes they belong to, and iteratively subdivide these nodes until they are subdivided into the smallest unit nodes, thereby constructing a quadtree map; The quadtree map includes: a. In a quadtree map, each node is an AABB bounding box, and each parent node has four child nodes, with the length and width of the child node's bounding box being half that of the parent node; b. It is necessary to recursively build the AABB bounding box and corresponding nodes for each discrete point of the obstacle; c. The AABB bounding box of each discrete point of an obstacle must be the smallest unit node AABB bounding box; Step S3: Obtain the planned parking path, starting from the path point closest to the current vehicle, and preview a path forward; calculate the vehicle's AABB bounding box and OBB bounding box set at the previewed path point; Step S4: Iterate through the vehicle's AABB bounding box and OBB bounding box sets sequentially. Filter by whether the vehicle's AABB bounding box intersects with the quadtree node's AABB bounding box containing the discrete points of obstacles. Iterate through the vehicle's OBB bounding box with the filtered quadtree node's AABB bounding box to perform collision detection. The specific steps of Step S4 are as follows: Step S4-1: Traverse the AABB bounding box set and OBB bounding box set of the vehicles generated in step S3. Step S4-2: Determine whether the vehicle's AABB bounding box intersects with the quadtree node's AABB bounding box containing the discrete points of obstacles; if they intersect, proceed to step S4-3; if they do not intersect, proceed to step S4-1. Step S4-3: Obtain the vehicle's OBB bounding box at the current path point, and use the following steps to determine whether the vehicle has collided with an obstacle; S431. Calculate the projection axes of the vehicle's OBB bounding box and the bounding box of the quadtree node AABB containing the obstacle: S432. Traverse all projection axes and obtain the projections of the vehicle's OBB bounding box and the quadtree node's AABB bounding box onto each projection axis: S433. Determine whether there is an overlap in the projection on the projection axis. If there is an overlap, it means that the vehicle's OBB bounding box and the quadtree node's AABB bounding box intersect. S434. For intersecting quadtree nodes, obtain all child nodes, and repeat S431 for each child node until the intersecting quadtree node is the smallest unit node; if this smallest unit node is an obstacle node, it means that the vehicle has collided with the obstacle. Step S4-4: If a collision is detected, take appropriate obstacle avoidance measures to ensure that the vehicle can be parked safely.

2. The autonomous parking path collision detection method based on a quadtree map as described in claim 1, characterized in that, The specific steps of step S1 are as follows: Step S1-1: Obtain obstacle information in the environment in real time through ultrasonic radar, including the height attributes and type of obstacles, as well as the start and end point information in the vehicle coordinate system; Step S1-2: Using the upper right corner of the target parking space as the origin of the parking coordinate system, transform the coordinates of the obstacle to the parking coordinate system; The formula for coordinate transformation includes: Where dx, dy, and θ are the coordinates and angles of the origin of the parking coordinate system in the vehicle coordinate system, respectively; (x, y) is the coordinate of the obstacle in the vehicle coordinate system, and (x′, y′) is the coordinate of the obstacle in the parking coordinate system.

3. The autonomous parking path collision detection method based on a quadtree map as described in claim 1, characterized in that, The specific steps of step S2 are as follows: Step S2-1: Initialize the quadtree map, define the center coordinates, length, width and the size of the smallest unit node's AABB bounding box, and construct the root node's AABB bounding box of the quadtree map; Step S2-2: Discretize the obstacle information obtained from step S1 by using the following steps to discretize the line segments representing obstacles into multiple discrete points; S221. Input the coordinates of the two endpoints of the obstacle, where the starting point coordinates are (x0, y0) and the ending point coordinates are (x0, y0). l ,y l ); S222. Take the starting point (x0, y0) of the obstacle as the first discrete point; S223. Calculate the constant Δx = x l -x0,Δy=y l -y0, and obtain the first value of the decision parameter: p0 = 2Δy - Δx; S224. Starting from k=0, at each x along the line k At this point, make the following judgments: If p k If < 0, then the next discrete point of the obstacle is (x k +1,y k ), and p k+1 =p k +2Δy If p k If ≥0, then the next discrete point of the obstacle is (x k +1,y k +1), and p k+1 =p k +2Δy-2Δx S225. Repeat step S224 for a total of Δx times; Step S2-3: Traverse the discrete points of obstacles, determine the quadtree map node where they are located, and iteratively subdivide these nodes until they are subdivided into the smallest unit node. Store the discrete points of obstacles in the smallest unit node of the quadtree map, and use AABB bounding boxes as the bounding volume of discrete points to construct the quadtree map.

4. The autonomous parking path collision detection method based on a quadtree map as described in claim 1, characterized in that, The specific steps of step S3 are as follows: Step S3-1: Obtain the pre-planned parking path, which consists of a series of waypoints, including location and direction information; Step S3-2: Obtain vehicle location information and find the nearest path point to the vehicle, called the matching point. Step S3-3: Starting from the matching point, aim forward a certain number of path points. Generate a set of vehicle AABB and OBB bounding boxes based on the aiming path points. The vehicle AABB and OBB bounding boxes are calculated using the vehicle's geometric dimensions and the direction information of the path points.

Images