Autonomous parking path collision detection method based on quadtree map

Abstract

The invention discloses an autonomous parking path collision detection method based on a quad-tree map, which comprises the following steps: acquiring obstacle information in real time through an ultrasonic radar, dispersing an obstacle into points, determining a quad-tree map node where the obstacle is located, and storing the discrete points in a minimum unit node of the quad-tree map, using an AABB bounding box as a discrete point bounding body to construct a quadtree map; a planned parking path is obtained, and a path point closest to the current vehicle serves as a starting point to preview a section of path forwards. The vehicle AABB bounding box and OBB bounding box set is traversed in sequence, screening is carried out according to whether the vehicle AABB bounding box intersects with the quadtree node AABB bounding box with the obstacle discrete points, and collision detection is carried out through iteration of the vehicle OBB bounding box and the screened quadtree node AABB bounding box. The problem that a traditional path collision detection method consumes long time in a complex scene is solved. The time consumption of the collision detection method in a complex scene is reduced, and the detection efficiency is improved while the path collision detection precision is improved.

Description

Technical Field

[0001] The present invention relates to autonomous driving vehicle technology, and in particular to collision detection technology in path planning in an autonomous parking system. Background Art

[0002] In the development of autonomous driving technology, automatic parking system, as one of the important applications, has received extensive attention and research. Traditional automatic parking path planning and obstacle collision detection methods usually rely on global maps and path planning based on grid maps. These methods can achieve good results in simple environments, but in complex parking scenarios, especially due to the limited detection range of sensors, global obstacle information cannot be obtained. For obstacles that appear during parking, their calculation time and detection accuracy are often difficult to meet actual needs.

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

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

[0005] Polygon-based collision detection method: This method simplifies obstacles and vehicles into polygons and determines collisions through geometric calculations. This method can theoretically achieve high detection accuracy, but the calculation complexity is high, especially when dealing with a large number of obstacles, the calculation time is often not negligible.

[0006] In summary, the existing technologies generally have problems such as long calculation time, low collision detection accuracy, and poor real-time performance when dealing with obstacle collision detection in complex environments. Therefore, it is of great practical significance and application value to develop a method that can efficiently and accurately perform obstacle collision detection in complex scenes. The present invention proposes an autonomous parking path collision detection method based on a quadtree map, which aims to solve the deficiencies in the existing technologies and improve detection efficiency and accuracy. Summary of the invention

[0007] The present invention solves the problem that the traditional path collision detection method takes a long time in complex scenes. The present invention establishes a quadtree map and iterates the collision detection between the vehicle OBB bounding box and the screened quadtree node AABB bounding box with discrete obstacle points. The time consumption of the algorithm in complex scenes is reduced, and the detection efficiency is improved while the accuracy of path collision detection is improved.

[0008] To achieve the above object, the present invention adopts the following technical solution: an autonomous parking path collision detection method based on a quadtree map, the method comprising the following steps:

[0009] Step S1: Obtain obstacle information in real time through ultrasonic radar and perform coordinate conversion;

[0010] Step S2: Discretize the obstacle into points, determine the quadtree nodes where the obstacle is located, and iteratively subdivide these nodes until they are subdivided into minimum unit nodes, thereby constructing a quadtree map; the quadtree map includes:

[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. It is necessary to recursively establish the AABB bounding box and corresponding nodes of each discrete point of the obstacle;

[0013] c. The node AABB bounding box of each discrete obstacle point must be the minimum unit node AABB bounding box;

[0014] Step S3: Obtain the planned parking path, take the path point closest to the current vehicle as the starting point, preview a section of the path forward; and calculate the vehicle AABB bounding box and OBB bounding box set at the previewed path point;

[0015] Step S4: traverse the vehicle AABB bounding box and the OBB bounding box set in sequence, and filter whether the vehicle AABB bounding box intersects with the quadtree node AABB bounding box with the obstacle discrete point, and iterate the collision detection between the vehicle OBB bounding box and the filtered quadtree node AABB bounding box.

[0016] Furthermore, the specific steps of step S1 are as follows:

[0017] Step S1-1: Obtain obstacle information in the environment in real time through ultrasonic radar, including the height attributes and type of the obstacle, as well as the starting and ending 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, the coordinates of the obstacle are converted to the parking coordinate system; the formula for the coordinate conversion includes:

[0019]

[0020] Among them, 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.

[0021] Furthermore, the specific steps of step S2 are as follows:

[0022] Step S2-1: Initialize the quadtree map, define the center coordinates, length and width of the map, and the size of the smallest unit node AABB bounding box, and construct the root node AABB bounding box of the quadtree map;

[0023] Step S2-2: Discretize the obstacle information obtained in step S1, and discretize the line segment representing the obstacle into multiple discrete points using the following steps;

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

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

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

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

[0028] S224, starting from k=0, at each x along the line k Make the following judgments:

[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, a total of Δ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 AABB bounding boxes as the discrete point bounding volumes to construct the quadtree map.

[0033] Furthermore, the specific steps of step S3 are as follows:

[0034] Step S3-1: obtaining a pre-planned parking path, which consists of a series of path points, each of which includes position information and direction information;

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

[0036] Step S3-3: Taking the matching point as the starting point, preview a certain number of path points from this point forward, and generate a set of vehicle AABB and OBB bounding boxes based on the previewed path points. The vehicle AABB and OBB bounding boxes can be calculated based on the geometric size of the vehicle and the direction information of the path points.

[0037] Furthermore, the specific steps of step S4 are as follows:

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

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

[0040] Step S4-3: Obtain the vehicle OBB bounding box at the current path point, and use the following steps to determine whether the vehicle collides with an obstacle;

[0041] S431, calculate the projection axis of the vehicle OBB bounding box and the quadtree node AABB bounding box where the obstacle is located:

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

[0043] S433, determining whether there is overlap in the projections on the projection axis, if there is overlap, it means that the vehicle OBB bounding box and the quadtree node AABB bounding box intersect;

[0044] S434, for the intersecting quadtree node, obtain all child nodes, and repeat S431 for each child node until the intersecting quadtree node is a minimum unit node; if the minimum unit node is an obstacle node, it means that the vehicle has collided with the obstacle;

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

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

[0047] First, the quadtree structure is used to effectively manage and retrieve obstacle information, and the vehicle AABB bounding box is screened based on whether it intersects with the quadtree node AABB bounding box with discrete obstacle points. This reduces the calculation time of the collision detection method in complex scenarios and can significantly improve the efficiency of obstacle collision detection.

[0048] Secondly, the obstacles are discretized and stored in the smallest unit node of the quadtree map, and the AABB bounding box is used as the bounding volume of the discrete points of the obstacle, and the OBB bounding box is used as the bounding volume of the vehicle, which significantly improves the obstacle collision detection accuracy. BRIEF DESCRIPTION OF THE DRAWINGS

[0049] Figure 1 It is the algorithm flow chart of the present invention;

[0050] Figure 2 It is the environmental information map after coordinate conversion of the present invention;

[0051] Figure 3 A quadtree map constructed for the present invention;

[0052] Figure 4 The vehicle OBB bounding box set of the present invention that will participate in the collision detection of this cycle;

[0053] Figure 5 This is a rendering of the autonomous parking path collision detection method of the present invention;

[0054] Figure 6 This is the effect diagram of the obstacle discrete algorithm of the present invention;

[0055] Figure 7 It is a schematic diagram of collision detection between the vehicle OBB bounding box and the quadtree node AABB bounding box of the present invention;

[0056] Figure 8 Schematic diagram of obstacle path distribution in three scenarios: vertical parking space, horizontal parking space, and end-of-parking space; DETAILED DESCRIPTION

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

[0058] Figure 1The algorithm flow chart of the present invention is as follows: 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 a section of the path is previewed forward to generate a set of vehicle AABB and OBB bounding boxes. Finally, the vehicle AABB bounding box is screened to see whether it intersects with the quadtree node AABB bounding box with the discrete obstacle point, and the vehicle OBB bounding box is iterated with the screened quadtree node AABB bounding box for collision detection. Finally, the result of the collision detection is output.

[0059] Figure 2 It is the environment information map after coordinate conversion of the present invention, including obstacles detected by the ultrasonic radar, the target parking space, and the current vehicle position.

[0060] Figure 3 The quadtree map constructed for the present invention has a map length of 16m, a width of 12m, and a minimum node resolution of 0.05m. When there is an obstacle at a certain location, the quadtree map will be subdivided until the minimum unit node is reached.

[0061] Figure 4 The vehicle OBB bounding box set of the present invention that will participate in the collision detection of this cycle is combined with the detection range of the ultrasonic radar and 20 path points are pre-aimed to participate in the collision detection of this cycle.

[0062] Figure 5 This is a rendering of the autonomous parking path collision detection method of the present invention. The vehicle OBB bounding box set generated according to the previewed path points is traversed, and the collision detection algorithm proposed by the present invention is used to detect whether the vehicle OBB bounding box and the quadtree node AABB bounding box have collided. In the figure, the 13th vehicle OBB bounding box has collided with the quadtree node AABB bounding box in front.

[0063] Figure 6 Schematic diagram of the obstacle discretization algorithm of the present invention. 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 It is a schematic diagram of collision detection between the vehicle OBB bounding box and the quadtree node AABB bounding box of the present invention.

[0065] Figure 8 Schematic diagram of obstacle path distribution in three scenarios: vertical parking space, horizontal parking space, and end-of-parking space;

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

[0067] Step S1: Obtain obstacle information in real time through ultrasonic radar and perform coordinate conversion.

[0068] Step S1-1: Obtain obstacle information in the environment in real time through ultrasonic radar, including the height attributes and type of the obstacle, as well as the starting and ending 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 obstacle are converted to the parking coordinate system. The formula for the coordinate conversion includes:

[0070]

[0071] Among them, dx, dy, θ are the coordinates and angles of the origin of the parking coordinate system in the vehicle coordinate system; (x, y) are the coordinates of the obstacle in the vehicle coordinate system, and (x′, y′) are the coordinates of the obstacle in the parking coordinate system. Figure 2 shown.

[0072] Step S2: determine the quadtree map node where it is located, and iteratively subdivide these nodes until they are subdivided into minimum unit nodes, save the discrete points in the minimum unit node of the quadtree map, and use the AABB bounding box as the discrete point bounding volume to construct the quadtree map.

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

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

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

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

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

[0078] p0=2Δy-Δx

[0079] 4. Starting from k=0, at each x along the line k 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 discrete points of the obstacle, determine the quadtree map nodes where they are located, and iteratively subdivide these nodes until they are subdivided into the smallest unit nodes, save the discrete points in the smallest unit node of the quadtree map, and use the AABB bounding box as the bounding volume of the discrete points to construct the quadtree map. The constructed quadtree map is as follows Figure 3 shown.

[0084] Step S3: Get the planned parking path, start from the path point closest to the current vehicle, and preview a section of the path forward. Calculate the vehicle AABB bounding box and OBB bounding box set at the previewed path point;

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

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

[0087] Step S3-3: Taking the matching point as the starting point, preview a certain number of path points from this point forward, and generate a set of vehicle AABB and OBB bounding boxes based on the previewed path points. The vehicle AABB and OBB bounding boxes can be calculated based on the geometric size of the vehicle and the position and direction information of the path points.

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

[0089] Step S4: traverse the vehicle AABB bounding box and the OBB bounding box set in sequence, and filter whether the vehicle AABB bounding box intersects with the quadtree node AABB bounding box with the obstacle discrete point, and iterate the collision detection between the vehicle OBB bounding box and the filtered quadtree node AABB bounding box.

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

[0091] Step S4-2: Determine whether the vehicle AABB bounding box intersects with the quadtree node AABB bounding box with the obstacle discrete point. If so, proceed to step S4-3; if not, proceed to step S4-1.

[0092] Step S4-3: Traverse the vehicle OBB bounding box set and use the following steps to determine whether the vehicle has collided with an obstacle. Figure 7 Shown

[0093] The vertices of the vehicle OBB bounding box are A, B, C, and D, and the four vertices of the quadtree 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 quadtree node AABB bounding box, select the normal vector of each edge as the projection axis. Since the vehicle OBB bounding box and the quadtree node AABB bounding box are rectangular and the edges are perpendicular to each other, select the unit vector of each edge as the projection axis. Therefore, a total of 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 quadtree node AABB bounding box to On axis.

[0098] 3. Calculate the projection interval:

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

[0100] Calculate the AABB bounding box of the quadtree node The projection interval on the axis is [a0,c0].

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

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

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

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

[0105] 5. Repeat the above steps:

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

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

[0108] 6. Get child nodes and iterate to detect collisions:

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

[0110] The collision detection results are as follows: Figure 5 As shown in the figure, the 13th vehicle OBB bounding box intersects with the AABB bounding box of the quadtree obstacle node in front, which means that the vehicle collides with the obstacle in front.

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

[0112] At this time, the autonomous parking path obstacle collision detection method of the present invention is adopted. At a higher resolution, the detection efficiency of its algorithm is significantly higher than that of the collision detection algorithm based on the grid map, which plays a vital role in saving computing resources, improving the accuracy of collision detection, and realizing real-time dynamic planning. As shown in Table 1, at different resolutions (the resolution of the method of the present invention is 0.05m, and the grid-based one is 0.1m), the detection efficiency of the method of the present invention and the collision detection method based on the grid map are compared (compared in three scenarios, the hardware platform CPU is Intel Core i7-6820HK@2.7GHz, the memory is 16GB, and it is developed under Linux using C++). It can be seen that in different scenarios, the computing efficiency and detection accuracy of the method of the present invention are significantly higher than those of the grid-based collision detection method. As shown in Table 1, the detection efficiency of the method of the present invention and the collision detection method based on the grid map are compared in three scenarios, the hardware platform CPU is Intel Core i7-6820HK@2.7GHz, the memory is 16GB, and it is developed under Linux using C++. Figure 8 As shown, it is a schematic diagram of obstacle path distribution in three scenarios: vertical parking space, horizontal parking space, and end-of-parking space.

[0113] Table 1

[0114]

[0115] As can be seen from the above, the present invention has at least the following advantages:

[0116] 1. Effectively manage and retrieve obstacle information through the quadtree structure, and use the intersection of the vehicle AABB bounding box and the quadtree node AABB bounding box with discrete obstacle points for screening, which reduces the calculation time of the collision detection method in complex scenes and can significantly improve the efficiency of obstacle collision detection.

[0117] 2. The obstacles are discretized and stored in the smallest unit node of the quadtree map. The AABB bounding box is used as the bounding volume of the discrete points of the obstacle, and the OBB bounding box is used as the bounding volume of the vehicle, which significantly improves the accuracy of obstacle collision detection.

Claims

1. A method for autonomous parking path collision detection based on quadtree map, characterized in that: The method comprises the following steps: Step S1: Obtain obstacle information in real time through ultrasonic radar and perform coordinate conversion; Step S2: discretize the obstacles into points, determine the quadtree nodes where they are located, and iteratively subdivide these nodes until they are subdivided into the smallest unit nodes, thereby constructing a quadtree map; The quadtree map includes: 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. b. It is necessary to recursively establish the AABB bounding box and corresponding nodes of each discrete point of the obstacle; c. The node AABB bounding box of each discrete obstacle point must be the minimum unit node AABB bounding box; Step S3: Obtain the planned parking path, take the path point closest to the current vehicle as the starting point, preview a section of the path forward; and calculate the vehicle AABB bounding box and OBB bounding box set at the previewed path point; Step S4: traverse the vehicle AABB bounding box and the OBB bounding box set in sequence, and filter whether the vehicle AABB bounding box intersects with the quadtree node AABB bounding box with the obstacle discrete point, and iterate the collision detection between the vehicle OBB bounding box and the filtered quadtree node AABB bounding box.

2. The autonomous parking path collision detection method based on quadtree map as claimed 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 the obstacle, as well as the starting and ending point information in the vehicle coordinate system; 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 obstacle are converted to the parking coordinate system; The coordinate transformation formula includes: Among them, 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 quadtree map as claimed 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 and width of the map, and the size of the smallest unit node AABB bounding box, and construct the root node AABB bounding box of the quadtree map; Step S2-2: Discretize the obstacle information obtained in step S1, and discretize the line segment representing the obstacle into multiple discrete points using the following steps; S221. Input the coordinates of the two endpoints of the obstacle, where the coordinates of the starting point of the obstacle are (x0, y0) and the coordinates of the end point of the obstacle are (x l ,y l ); S222, taking the starting point (x0, y0) of the obstacle as the first discrete point; S223, calculate constant Δx=x l -x0,Δy=y l -y0, and get the first value of the decision parameter: p0=2Δy-Δx; S224, starting from k=0, at each x along the line k Make the following judgments: If p k <0, then the next obstacle discrete point is (x k +1,y k ), and p k+1 =p k +2Δy 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 S225, repeat step S224, a total of Δx times; 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 AABB bounding boxes as the discrete point bounding volumes to construct the quadtree map.

4. The autonomous parking path collision detection method based on quadtree map as claimed in claim 1, characterized in that: The specific steps of step S3 are as follows: Step S3-1: obtaining a pre-planned parking path, which consists of a series of path points, each of which includes position information and direction information; Step S3-2: Get the vehicle positioning information and find the path point closest to the vehicle, called the matching point Step S3-3: Taking the matching point as the starting point, preview a certain number of path points from this point forward, and generate a set of vehicle AABB and OBB bounding boxes based on the previewed path points. The vehicle AABB and OBB bounding boxes are calculated based on the geometric size of the vehicle and the direction information of the path points.

5. The autonomous parking path collision detection method based on quadtree map as claimed in claim 1, characterized in that: The specific steps of step S4 are as follows: Step S4-1: Traverse the vehicle AABB bounding box set and OBB bounding box set generated in step S3 Step S4-2: Determine whether the vehicle AABB bounding box intersects with the quadtree node AABB bounding box with the obstacle discrete point; if so, proceed to step S4-3; if not, proceed to step S4-1; Step S4-3: Obtain the vehicle OBB bounding box at the current path point, and use the following steps to determine whether the vehicle collides with an obstacle; S431, calculate the projection axis of the vehicle OBB bounding box and the quadtree node AABB bounding box where the obstacle is located: S432, traverse all projection axes, and obtain the projection of the vehicle OBB bounding box and the quadtree node AABB bounding box on each projection axis: S433, determining whether there is overlap in the projections on the projection axis, if there is overlap, it means that the vehicle OBB bounding box and the quadtree node AABB bounding box intersect; S434, for the intersecting quadtree node, obtain all child nodes, and repeat S431 for each child node until the intersecting quadtree node is a minimum unit node; if the minimum 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 corresponding obstacle avoidance measures to ensure that the vehicle can park safely.

Images