Curvature continuous autonomous parking path planning method and system based on reverse search

Abstract

The present invention discloses a curvature-continuous autonomous parking path planning method and system based on reverse search. The method comprises the following steps: initializing a parking grid map, a starting point and an end point; performing node replacement on the original starting point and end point; performing CC-HA* node expansion based on the new starting point and end point, and obtaining an initial path sequence by backtracking when the latest expanded node meets the end point cutoff condition; performing path reversal according to the initial path sequence to obtain a reverse path sequence; performing loose connection processing on the target point, the path end point coincides with the target parking space, setting a search cutoff condition, dividing the path planning into a reverse search stage and a loose connection stage, obtaining a final parking path, combining Hybrid A* with the reverse search, ensuring the continuity of the first-order curvature of the path, fully considering the unbalanced distribution of obstacles around the driving lane and the parking space, and performing a reverse search from the target parking space to the current position of the vehicle, thereby reducing useless expansion nodes in the search process and improving efficiency.

Description

Technical Field

[0001] The present invention belongs to the field of unmanned driving path planning, and in particular relates to a curvature continuous autonomous parking path planning method and system based on reverse search. Background Art

[0002] As an important part of autonomous driving technology, autonomous parking has gone through four stages: APA (Autoparking Assist), RPA (Remote Parking Assist), HPA (Home-zone Parking Assist) and AVP (Automated Valet Parking), corresponding to L1-L4 levels of autonomous driving. A typical autonomous parking system includes parking space recognition, path planning, path tracking, vehicle positioning and chassis control. The present invention mainly focuses on autonomous parking planning tasks.

[0003] Parking planning can be defined as a process in which the unmanned vehicle generates a smooth, collision-free path that meets the position and orientation requirements of the target parking space in a short period of time. Due to the non-integrity constraints of unmanned vehicles (i.e., the lateral movement of the vehicle is limited due to the limited turning radius and it cannot change direction without longitudinal movement), previous methods have focused on improving the smoothness of the path, such as path continuity, curvature continuity, and higher-order curvature continuity. These methods can be divided into four categories: grid search-based algorithms, sampling-based algorithms, optimization-based algorithms, and learning-based algorithms.

[0004] For grid search algorithms, the classic A* algorithm and its variants D*, ARA*, and Theta* only use straight line segments as motion primitives in the search process. The planned path is a broken line and cannot be directly used as the driving path of the unmanned vehicle. The Hybrid A* algorithm adds arc segments to the motion primitives to improve the smoothness of the path, thereby satisfying the non-complete kinematic constraints of the unmanned vehicle. As a result, many parking planning algorithms based on Hybrid A* have emerged. The recently proposed CC-HA* algorithm adds spiral segments to the motion primitives, further improving the smoothness of the path. The smoothness of the path can be improved by introducing multiple types of motion primitives, but the dimension of the search space increases, which brings more time overhead.

[0005] For sampling algorithms, the classic RRT and its variants RRT-connect, Anytime RRTs, and RRT* cannot be directly used for parking planning due to non-complete constraints. The Hybrid Curvature Steer algorithm improves the smoothness of the path, but also brings greater time overhead. Optimization algorithms treat the parking planning problem as an optimization problem. The constraints include vehicle kinematic constraints, physical constraints, and collision detection constraints. However, optimization algorithms often encounter local minima problems, and solving optimization problems often requires a long time. Learning algorithms can use a model-based reinforcement learning algorithm to iteratively perform data generation, data evaluation, and model training. However, learning algorithms are mostly targeted at simulation environments, and it is difficult to conduct experimental verification under real parking scenarios.

[0006] In summary, many current parking planning algorithms are difficult to take into account the comprehensive performance indicators of path smoothness, terminal position accuracy, and calculation real-time performance, and their practicality still needs to be improved. Summary of the invention

[0007] In order to solve the problems existing in the prior art, the present invention provides a curvature continuous autonomous parking path planning method based on reverse search (RS-HA*), which combines CC-HA* with the reverse search strategy, proposes a path reversal processing algorithm and a shift point processing algorithm, and proposes a loose connection strategy to solve the problem of initial posture misalignment.

[0008] In order to achieve the above object, the technical solution adopted by the present invention is: a curvature continuous autonomous parking path planning method based on reverse search, which specifically includes the following steps:

[0009] Initialize the parking grid map, starting point and end point, and discretize the search node through the grid map;

[0010] Replace the original starting point and end point with new ones.

[0011] Based on the new starting point and end point, the CC-HA* node is expanded. When the latest expanded node meets the end point cutoff condition, the initial path sequence is obtained by backtracking.

[0012] According to the initial path sequence Reverse the path to get the reverse path sequence

[0013] For the reverse path sequence For each shift point in the , the Boolean variable id_back is inverted, and finally the shift point path is obtained.

[0014] Shift point path based The target point is loosely connected, the end point of the path coincides with the target parking space, the search cutoff condition is set, and the path planning is divided into a reverse search phase and a loose connection phase to obtain the final parking path.

[0015] Initialize the grid map, the starting point and the end point, and the search node is discretized through the grid map and recorded as:

[0016] S=[id_x, id_y, id_theta, id_kappa, id_moton, id_back] T ,

[0017] Where id_kappa is a finite discrete numerical variable, expressed as:

[0018]

[0019] Where id_moton is an enumeration variable, indicating the increase or decrease of the curvature of adjacent nodes, expressed as:

[0020]

[0021] Where id_back is a Boolean variable, indicating the forward or backward direction of the current node, expressed as:

[0022]

[0023] The path finally obtained by searching is recorded as The current position of the vehicle is taken as the starting point S0, and the target parking space is taken as the end point S g .

[0024] The original starting point and end point are replaced by nodes as follows: g Perform node replacement, that is, take the position and orientation of the target parking space as the starting point of the search, and the current position and orientation of the vehicle as the end point of the search, and get the new starting point S n0 =S g and the end point S ng =S0.

[0025] Based on the new starting point and end point, the CC-HA* node is expanded. When the latest expanded node meets the end point cutoff condition, the initial path sequence is obtained by backtracking. The details are as follows:

[0026] Select the node with the smallest f value from the linked list OPEN_LIST as the current node. The calculation of the f value includes two parts: the cost function and the heuristic function.

[0027] f(n)=g(n)+h(n)

[0028] Where g(n) represents the cost function between the current node and the search starting point, measured by the cost of the distance from the starting point, and h(n) represents the heuristic function with the target node, calculated using the following formula:

[0029] h CC-HA* (n) = max{h R-S (n), h 2D (n)}

[0030] The heuristic function consists of two parts. The first part h R-S is the length of the Reeds-Shepp curve from the current position of the vehicle to the end point, indicating that only the kinematic constraints of the vehicle are considered, without considering the surrounding obstacles; the second part h 2D is the path length of 2D A*, which means that only surrounding obstacles are considered without considering whether the generated path is drivable;

[0031] Determine whether the newly expanded node meets the end-point cutoff condition. If not, delete it from OPEN_LIST and add it to CLOSED_LIST. If it meets the end-point cutoff condition, the search process ends and the initial path sequence is obtained by tracing back to the parent node from this node.

[0032] According to the initial path sequence Reverse the path to get the reverse path sequence Specifically: For the path Each node in the reverse operation is performed, the Boolean variable id_back describing the direction of vehicle movement is inverted, the absolute value of the discrete numerical variable id_kappa remains unchanged, the sign is changed, the enumeration variable id_moton describing the curvature change is also inverted, id_x, id_y, id_theta remain unchanged, and the reverse path sequence is obtained.

[0033] The reverse search phase is as follows: run the reverse search algorithm from the starting point to the end point; when reaching any node that has been searched in the appropriate area in the middle of the path, end the reverse search process of this phase in advance, and use the midpoint of the path as the last node of the reverse search phase;

[0034] The loose connection stage is as follows: re-plan a path from the current node to the target location, and connect it with the path obtained in the reverse search stage, and finally form a complete parking planning path.

[0035] When the target point is loosely connected, the end point of the path coincides with the target parking space through reverse search. The cutoff condition of the search process is that the position error does not exceed 25cm and the direction error does not exceed 5 degrees. The end point cutoff condition of the search process is relaxed.

[0036] On the other hand, the present invention also provides a curvature continuous autonomous parking path planning system based on reverse search, including an initialization module, a node replacement module, an initial path sequence acquisition module, a reverse path sequence acquisition module, a shift point processing module and a final path acquisition module;

[0037] The initialization module is used to initialize the parking grid map, the starting point and the end point, and discretize the search node through the grid map;

[0038] The node replacement module is used to replace the original starting point and end point to obtain a new starting point and end point;

[0039] The initial path sequence acquisition module is used to expand the CC-HA* node based on the new starting point and end point. When the latest expanded node meets the end point cutoff condition, the initial path sequence is obtained by backtracking.

[0040] The reverse path sequence acquisition module is used to obtain the reverse path sequence according to the initial path sequence. Reverse the path to get the reverse path sequence

[0041] Shift point processing module for reverse path sequence For each shift point in the , the Boolean variable id_back is inverted, and finally the shift point path is obtained.

[0042] The final path acquisition module is used to obtain the path based on the shift point The target point is loosely connected, the end point of the path coincides with the target parking space, the search cutoff condition is set, and the path planning is divided into a reverse search phase and a loose connection phase to obtain the final parking path.

[0043] The present invention also provides a computer device, including a processor and a memory, the memory is used to store a computer executable program, the processor reads the computer executable program from the memory and executes it, and when the processor executes the computer executable program, the curvature continuous autonomous parking path planning method based on reverse search according to the present invention can be implemented.

[0044] The present invention may also provide a computer-readable storage medium, in which a computer program is stored. When the computer program is executed by a processor, the curvature continuous autonomous parking path planning method based on reverse search described in the present invention can be implemented.

[0045] Compared with the prior art, the present invention has at least the following beneficial effects:

[0046] By using reverse search, collision detection of dense obstacles around parking spaces is prioritized, which greatly reduces useless expansion nodes in the search process of CCHA* algorithm, realizes the pruning effect of search tree, significantly improves the real-time performance of the algorithm, and greatly improves the efficiency of parking planning; by using reverse search, the parking end point is reversed to the starting point of the search path, so the end point and the parking target position completely overlap, theoretically eliminating the error between the two, thereby achieving higher parking planning end point accuracy; a loose connection strategy for the starting point position is proposed, and the single-stage path search process is divided into two stages. In the loose connection stage, due to fewer obstacles and longer paths, the position error when the path reaches the starting point can be significantly reduced, which is more conducive to the execution of the control system; the CCHA* algorithm is organically combined with reverse search. Compared with many existing parking planning algorithms, the searched path achieves first-order curvature continuity, which improves ride comfort. BRIEF DESCRIPTION OF THE DRAWINGS

[0047] Figure 1 The present invention is a flowchart of an implementable method.

[0048] Figure 2 This is a schematic diagram of OCP during a reverse search process.

[0049] Figure 3 This is the schematic diagram of the loose connection of the terminal.

[0050] Figure 4 This is a schematic diagram of the parking space coordinate system.

[0051] Figure 5 This is a comparison diagram of the forward and reverse search experiment when reversing into a parking space.

[0052] Figure 6 This is a comparison diagram of the forward and reverse search experiment for parallel parking. DETAILED DESCRIPTION

[0053] The present invention is further described in detail below with reference to the accompanying drawings and specific embodiments: The present application provides a curvature continuous autonomous parking path planning method based on reverse search, the overall process is as follows: Figure 1 As shown in the figure, CC-HA* is combined with reverse search, and the continuity of the first-order curvature of the path is guaranteed, which is called RS-HA*. RS-HA* fully considers the particularity of the parking scene, that is, the unbalanced distribution of obstacles around the driving lane and the parking space. By performing a reverse search from the target parking space to the current position of the vehicle, the collision detection of dense obstacles around the parking space is prioritized, which can reduce useless expansion nodes in the search process and complete the pruning of the search process, thereby improving the real-time performance of the algorithm. The present invention calls this pruning process OCP (Obstacle-Collision-Pruning), as shown in the figure. Figure 2 The core idea of ​​RS-HA* is: assuming there is a car in the target parking space, the invention plans a path from the parking space to the current position, and then reverses the path as the final output. The specific steps are as follows:

[0054] A curvature continuous autonomous parking path planning method based on reverse search specifically comprises the following steps:

[0055] Step 1: Initialize the grid map, the starting point and the end point, and discretize the search node through the grid map, which is recorded as:

[0056] S=[id_x, id_y, id_theta, id_kappa, id_moton, id_back] T ,

[0057] Where id_kappa is a finite discrete numerical variable, expressed as:

[0058]

[0059] Where id_moton is an enumeration variable, indicating the increase or decrease of the curvature of adjacent nodes, expressed as:

[0060]

[0061] Where id_back is a Boolean variable, indicating the forward or backward direction of the current node, expressed as:

[0062]

[0063] The path finally obtained by searching is recorded as The current position of the vehicle is taken as the starting point S0, and the target parking space is taken as the end point S g .

[0064] Step 2: Change the original starting point S0 and the end point S g Perform node replacement, that is, take the position and orientation of the target parking space as the starting point of the search, and the current position and orientation of the vehicle as the end point of the search, so as to obtain a new starting point S n0 =S g and the end point S ng =S0.

[0065] Step 3: Based on the new starting point S n0 and the end point S ng Perform CC-HA* node expansion. Maintain two linked lists OPEN_LIST and CLOSED_LIST, and select the node with the smallest f value from the linked list OPEN_LIST as the current node. The calculation of the f value includes two parts: the cost function and the heuristic function.

[0066] f(n)=g(n)+h(n)

[0067] Where g(n) represents the cost function between the current node and the search starting point, measured by the cost of the distance from the starting point. h(n) represents the heuristic function with the target node, calculated using the following formula:

[0068] h CC-HA* (n) = max{h R-S (n), h 2D (n)}

[0069] The heuristic function consists of two parts. The first part h R-S The length of the Reeds-Shepp curve from the vehicle's current position to the end point is called "non-holonomic without obstacles", which means that only the kinematic constraints of the vehicle are considered without considering the surrounding obstacles. 2D is the path length of 2D A*, which is called "non-holonomic withoutobstacles", meaning that it only considers the surrounding obstacles without considering whether the generated path is drivable.

[0070] Determine whether the newly expanded node meets the end-point cutoff condition. If not, delete it from OPEN_LIST and add it to CLOSED_LIST. If it meets the end-point cutoff condition, the search process ends and the initial path sequence is obtained by tracing back to the parent node from this node.

[0071] Step 4: Reverse the path. Reverse. For the path To perform a reversal operation on each node in the path, the Boolean variable id_back describing the direction of vehicle movement needs to be inverted, the absolute value of the discrete numerical variable id_kappa remains unchanged, and the sign is changed. The enumeration variable id_moton describing the curvature change is also inverted, and id_x, id_y, and id_theta remain unchanged. The complete path reversal implementation process is shown in Algorithm 1. The algorithm outputs the reverse path sequence P re .

[0072] Table 1, Path reversal algorithm

[0073]

[0074] Step 5: Process the shift points in the path. In parking planning, the vehicle often needs to move forward and backward frequently. The simplest case is that the planned path has only one shift point. Before the shift point, the vehicle is in a forward state, and after the shift point, the vehicle is in a backward state. For each shift point in , the Boolean variable id_back needs to be inverted, as shown in Algorithm 2, and finally the shift point path is obtained.

[0075] Table 2 Shift point processing algorithm

[0076]

[0077] Step six, achieve loose connection of the target point. The specific implementation steps are as follows: Through reverse search, the end point of the path coincides with the target parking space, which means that the end point error is eliminated, but the starting point of the path and the current position of the vehicle do not coincide. The initial error of the path caused in this way will bring difficulties to the subsequent control module. Therefore, it is necessary to accurately connect the starting point of the path and the current position of the vehicle. The cutoff conditions of the search process are that the position error is 25cm and the direction error is 5 degrees. Although the distance between the starting point of the path and the current position is very close, it is very difficult for a vehicle to complete such a small range of precise movement. The present invention calls such a small range connection a tight connection. In order to solve this problem, the present invention relaxes the end point cutoff conditions of the search process and divides the entire path planning process into two stages, namely the reverse search stage and the loose connection stage. As Figure 3 As shown, the present invention can use any node that has been searched in the rectangular frame as the last node of the first stage of the reverse search. Then, in the second stage, the present invention plans another path from the current node to the target position. Because the distance between the two points is relatively far, and there are basically no obstacles in the rectangular frame, it is easy to eliminate the initial error. Such a large-scale connection is called a loose connection. Compared with a tight connection, a loose connection can avoid initial errors and end the reverse search process of the first stage in advance, thereby improving the efficiency of the search. The specific implementation steps are as follows:

[0078] The entire path planning process is divided into two stages, namely the reverse search stage and the loose connection stage.

[0079] Reverse search phase: Run the reverse search algorithm from the starting point to the end point; when reaching any node that has been searched in the appropriate area in the middle of the path (generally the midpoint of the path), end the reverse search process of this phase in advance, and use the midpoint of the path as the last node of the reverse search phase.

[0080] Loose connection stage: re-plan a path from the current node to the target location, and connect it with the path obtained in the reverse search stage to form a complete parking planning path.

[0081] Based on ROS and Gazebo, on a computing platform with an Intel Core i7-10700 CPU and 16GB RAM, the parking accuracy and speed performance of CC-HA* and RS-HA* were compared in two parking scenarios: parallel parking and reversing into a garage.

[0082] For the parking planning task, the lower left corner of the entire parking area is taken as the coordinate origin, and the straight line at the bottom of the parking space is taken as the X-axis. Figure 4 The parking space coordinate system shown. The present invention sets the vehicle's driving direction to the side where the X axis is greater than 0 (otherwise the entire system is mirrored). Lane lines and parking space boundaries can be considered as fixed obstacles. The parking area is a part of the driving lane near the parking space, and the parking point is the location of the rear axle center of the vehicle, that is, the location where the vehicle switches from normal driving to parking planning.

[0083] The vehicle plans parking at any parking spot in the parking area. In the following experimental part, we can see that different parking spots have a great impact on the search time.

[0084] Therefore, some important parameters in parking planning can be obtained, which have a great impact on the search time. The parameters are: the width SL of the parking space, the depth SW of the parking space, the width CL of the parking area, and the lateral offset BL relative to the parking space, which can be derived from the Y coordinate of the vehicle:

[0085] BL=Y-SW

[0086] For reversing into a garage, the present invention sets SL=3m, SW=5.5m, and CL=8m; for parallel parking, the present invention sets SL=7m, SW=3m, and CL=6.5m; the length of the unmanned vehicle is L=5m, and its width is W=2m.

[0087] refer to Figure 5 and Figure 6 The present invention sets 12 different parking points for comparative experiments. For reverse parking, the starting point coordinates include (3,7)(3,8)(3,9)(11,7)(11,8)(11,9); for parallel parking, the starting point coordinates include (2,4)(2,5)(2,6)(16,4)(16,5)(16,6).

[0088] It can be seen from Table 1 that compared with the forward search strategy used by CC-HA*, the reverse search strategy adopted by the present invention has greatly improved accuracy and efficiency; the average length of the reverse search path is 98.84% (reversing into a garage) and 104.05% (side parking) of the forward search, and the path length remains basically unchanged; the average number of reverse search nodes is 37.35% (reversing into a garage) and 5.25% (side parking), and the time overhead is 25.43% (reversing into a garage) and 3.34% (side parking), and the search efficiency is greatly improved; the end point direction error is 39.27% ​​(reversing into a garage) and 60.00% (side parking), and the end point position error is 20.74% (reversing into a garage) and 53.33% (side parking), and the parking accuracy is greatly improved; the number of discontinuous points in the generated path is basically the same as that of the forward search, which means that the two algorithms have similar smoothness.

[0089] Table 1 Comparison of efficiency, accuracy and path smoothness of forward and reverse search for parallel parking and reversing.

[0090]

[0091] In terms of the autonomous parking path planning algorithm, the present invention combines Hybrid A* with reverse search by utilizing the unbalanced distribution of obstacles around the starting point and the target point. Compared with the SOTA algorithm, the method of the present invention can greatly reduce the planning time and improve the planning accuracy of the target point.

[0092] On the other hand, the present invention also provides a curvature continuous autonomous parking path planning system based on reverse search, including an initialization module, a node replacement module, an initial path sequence acquisition module, a reverse path sequence acquisition module, a shift point processing module and a final path acquisition module;

[0093] The initialization module is used to initialize the parking grid map, the starting point and the end point, and discretize the search node through the grid map;

[0094] The node replacement module is used to replace the original starting point and end point to obtain a new starting point and end point;

[0095] The initial path sequence acquisition module is used to expand the CC-HA* node based on the new starting point and end point. When the latest expanded node meets the end point cutoff condition, the initial path sequence is obtained by backtracking.

[0096] The reverse path sequence acquisition module is used to obtain the reverse path sequence according to the initial path sequence. Reverse the path to get the reverse path sequence

[0097] Shift point processing module for reverse path sequence For each shift point in the , the Boolean variable id_back is inverted, and finally the shift point path is obtained.

[0098] The final path acquisition module is used to obtain the path based on the shift point The target point is loosely connected, the end point of the path coincides with the target parking space, the search cutoff condition is set, and the path planning is divided into a reverse search phase and a loose connection phase to obtain the final parking path.

[0099] In addition, the present invention can also provide a computer device, including a processor and a memory, the memory is used to store a computer executable program, the processor reads part or all of the computer executable program from the memory and executes it, and when the processor executes part or all of the computer executable program, the curvature continuous autonomous parking path planning method based on reverse search according to the present invention can be implemented.

[0100] As an example, the computer equipment used in the driverless car includes two industrial computers: one of which is the Nuvo-5095GC series, detailed information is as follows: https: / / www.neousys-tech.com.cn / cn / product / application / rugged-embedded / nuvo-5095gc-gpu-computer, Nuvo-5095GC is a highly integrated, compact, stable and reliable high-performance GPU computing platform for emerging fields such as autonomous driving; it supports 75W The GPU and subsequent PASCAL architecture GPU have 768 CUDA cores, providing powerful computing power for mathematical operations / graphics display. The card slot technology and innovative thermal design designed by Neousys Technology effectively remove the heat generated by the GPU, allowing this compact system to run reliably at 100% full load at an ambient temperature of 60℃.

[0101] Nuvo-5095GC is based on the sixth-generation Intel Skylake platform, supports 35W / 65W sixth-generation Core processors, and can support up to 32GB DDR4 memory; provides a wealth of I / O interfaces, such as Gigabit Ethernet, USB 3.1Gen1 and serial ports, which can easily connect to external devices, integrated in a very compact size of 240mm×225mm×110mm. For the rapidly growing GPU computing applications, Nuvo-5095GC is an industrial-grade compact, highly reliable platform that combines the high-speed computing capabilities of CPU and GPU to provide performance far exceeding that of traditional industrial computers; the other is the Nuvo-6108GC series, detailed information is as follows: https: / / www.neousys-tech.com.cn / cn / product / application / rugged-embedded / nuvo-6108gc-gpu-computing, Nuvo-6108GC is an industrial-grade wide-temperature vehicle-mounted embedded industrial computer that supports GPU high-end graphics cards. By supporting 250W GPU provides an ideal solution for the new generation of GPU accelerated applications such as artificial intelligence, virtual reality, autonomous driving and CUDA computing.

[0102] use C236 chipset, Nuvo-6108GC supports Xeon with up to 32GB ECC / non-ECC DDR4 memory E3V5 or sixth generation Intel SkyLake Core i7 / i5 CPU. It integrates general computer I / O such as Gigabit Ethernet, USB 3.0 and serial ports. In addition to the x16 PCIe port for GPU installation, Nuvo-6108GC also provides two x8 PCIe slots so that you can use other devices for information collection and communication.

[0103] Nuvo-6108GC is equipped with a sophisticated power design that can handle the heavy power consumption and power transients of a 250W GPU. In addition, to ensure reliable GPU performance in industrial environments, Nuvo-6108GC adopts Neousys' unique heat dissipation design, which can control the airflow of the cold air intake to effectively remove the heat generated by the GPU. Its heat dissipation design ensures that even in an environment of 60℃, the GPU of Nuvo-6108GC still maintains 100% loading capacity, making Nuvo-6108GC reliable for use in harsh environments.

[0104] On the other hand, the present invention provides a computer-readable storage medium, in which a computer program is stored. When the computer program is executed by a processor, the curvature continuous autonomous parking path planning method based on reverse search described in the present invention can be implemented.

[0105] The computer device may be a laptop computer, a desktop computer or a workstation.

[0106] The processor may be a central processing unit (CPU), a graphics processing unit (GPU), a digital signal processor (DSP), an application specific integrated circuit (ASIC), or an off-the-shelf field programmable gate array (FPGA).

[0107] The memory described in the present invention may be an internal storage unit of a notebook computer, a desktop computer or a workstation, such as a memory or a hard disk; or an external storage unit, such as a mobile hard disk or a flash memory card.

[0108] Computer-readable storage media may include computer storage media and communication media. Computer storage media include volatile and non-volatile, removable and non-removable media implemented by any method or technology for storing information such as computer-readable instructions, data structures, program modules or other data. Computer-readable storage media may include: read-only memory (ROM), random access memory (RAM), solid-state drive (SSD) or optical disk, etc. Among them, random access memory may include resistance random access memory (ReRAM) and dynamic random access memory (DRAM).

[0109] The above description is only a preferred specific implementation manner of the present invention, but the protection scope of the present invention is not limited thereto. Any technician familiar with the technical field can make equivalent replacements or changes according to the technical solutions and inventive concepts of the present invention within the technical scope disclosed by the present invention, which should be covered by the protection scope of the present invention.

Claims

1. A curvature continuous autonomous parking path planning method based on reverse search, characterized in that: The specific steps include: Initialize the parking grid map, starting point and end point, and discretize the search node through the grid map; Replace the original starting point and end point with new ones; Based on the new starting point and end point, the CC-HA* node is expanded. When the latest expanded node meets the end point cutoff condition, the initial path sequence is obtained by backtracking. Based on the new starting point and end point, the CC-HA* node is expanded. When the latest expanded node meets the end point cutoff condition, the initial path sequence is obtained by backtracking. The details are as follows: Select the node with the smallest f value from the linked list OPEN_LIST as the current node. The calculation of the f value includes two parts: the cost function and the heuristic function. f(n)=g(n)+h(n) Where g(n) represents the cost function between the current node and the search starting point, measured by the cost of the distance from the starting point, and h(n) represents the heuristic function with the target node, calculated using the following formula: The heuristic function consists of two parts. The first part h R-S is the length of the Reeds-Shepp curve from the current position of the vehicle to the end point, indicating that only the kinematic constraints of the vehicle are considered, without considering the surrounding obstacles; the second part h 2D is the path length of 2D A*, which means that only surrounding obstacles are considered without considering whether the generated path is drivable; Determine whether the newly expanded node meets the end-point cutoff condition. If not, delete it from OPEN_LIST and add it to CLOSED_LIST. If it meets the end-point cutoff condition, the search process ends and the initial path sequence is obtained by tracing back to the parent node from this node. According to the initial path sequence Reverse the path to get the reverse path sequence For the reverse path sequence For each shift point in the , the Boolean variable id_back is inverted, and finally the shift point path is obtained. Shift point path based The target point is loosely connected, the end point of the path coincides with the target parking space, and the search cutoff condition is set. The path planning is divided into a reverse search phase and a loose connection phase to obtain the final parking path; the reverse search phase is specifically: run the reverse search algorithm from the starting point to the end point; when reaching any node that has been searched in the appropriate area in the middle of the path, end the reverse search process of this phase in advance, and use the midpoint of the path as the last node of the reverse search phase; The loose connection stage is specifically as follows: re-plan a path from the current node to the target location, and connect it end to end with the path obtained in the reverse search stage, and finally form a complete parking planning path.

2. The curvature continuous autonomous parking path planning method based on reverse search according to claim 1, characterized in that: Initialize the grid map, the starting point and the end point, and the search node is discretized through the grid map and recorded as: S=[id_x,id_y,id_theta,id_kappa,id_moton,id_back] T , Where id_kappa is a finite discrete numerical variable, expressed as: Where id_moton is an enumeration variable, indicating the increase or decrease of the curvature of adjacent nodes, expressed as: Where id_back is a Boolean variable, indicating the forward or backward direction of the current node, expressed as: The path finally obtained by searching is recorded as The current position of the vehicle is taken as the starting point S0, and the target parking space is taken as the end point S g .

3. The curvature continuous autonomous parking path planning method based on reverse search according to claim 2 is characterized in that: The original starting point and end point are replaced by nodes as follows: g Perform node replacement, that is, take the position and orientation of the target parking space as the starting point of the search, and the current position and orientation of the vehicle as the end point of the search, and get the new starting point S n0 =S g and the end point S ng =S0.

4. The curvature continuous autonomous parking path planning method based on reverse search according to claim 1, characterized in that: According to the initial path sequence Reverse the path to get the reverse path sequence Specifically: For the path Each node in the reverse operation is performed, the Boolean variable id_back describing the direction of vehicle movement is inverted, the absolute value of the discrete numerical variable id_kappa remains unchanged, the sign is changed, the enumeration variable id_moton describing the curvature change is also inverted, id_x, id_y, id_theta remain unchanged, and the reverse path sequence is obtained.

5. The curvature continuous autonomous parking path planning method based on reverse search according to claim 1, characterized in that: When the target point is loosely connected, the end point of the path coincides with the target parking space through reverse search. The cutoff condition of the search process is that the position error does not exceed 25cm and the direction error does not exceed 5 degrees. The end point cutoff condition of the search process is relaxed.

6. A curvature continuous autonomous parking path planning system based on reverse search, characterized in that: It includes an initialization module, a node replacement module, an initial path sequence acquisition module, a reverse path sequence acquisition module, a shift point processing module and a final path acquisition module; The initialization module is used to initialize the parking grid map, the starting point and the end point, and discretize the search node through the grid map; The node replacement module is used to replace the original starting point and end point to obtain a new starting point and end point; The initial path sequence acquisition module is used to expand the CC-HA* node based on the new starting point and end point. When the latest expanded node meets the end point cutoff condition, the initial path sequence is obtained by backtracking. Based on the new starting point and end point, the CC-HA* node is expanded. When the latest expanded node meets the end point cutoff condition, the initial path sequence is obtained by backtracking. The details are as follows: Select the node with the smallest f value from the linked list OPEN_LIST as the current node. The calculation of the f value includes two parts: the cost function and the heuristic function. f(n)=g(n)+h(n) Where g(n) represents the cost function between the current node and the search starting point, measured by the cost of the distance from the starting point, and h(n) represents the heuristic function with the target node, calculated using the following formula: The heuristic function consists of two parts. The first part h R-S is the length of the Reeds-Shepp curve from the current position of the vehicle to the end point, indicating that only the kinematic constraints of the vehicle are considered, without considering the surrounding obstacles; the second part h 2D is the path length of 2D A*, which means that only surrounding obstacles are considered without considering whether the generated path is drivable; Determine whether the newly expanded node meets the end-point cutoff condition. If not, delete it from OPEN_LIST and add it to CLOSED_LIST. If it meets the end-point cutoff condition, the search process ends and the initial path sequence is obtained by tracing back to the parent node from this node. The reverse path sequence acquisition module is used to obtain the reverse path sequence according to the initial path sequence. Reverse the path to get the reverse path sequence Shift point processing module for reverse path sequence For each shift point in the , the Boolean variable id_back is inverted, and finally the shift point path is obtained. The final path acquisition module is used to obtain the path based on the shift point The target point is loosely connected, the end point of the path coincides with the target parking space, and the search cutoff condition is set. The path planning is divided into a reverse search phase and a loose connection phase to obtain the final parking path. The reverse search phase is specifically as follows: run the reverse search algorithm from the starting point to the end point; when reaching any node that has been searched in the appropriate area in the middle of the path, end the reverse search process of this phase in advance, and use the midpoint of the path as the last node of the reverse search phase; The loose connection stage is specifically as follows: re-plan a path from the current node to the target location, and connect it end to end with the path obtained in the reverse search stage, and finally form a complete parking planning path.

7. A computer device, characterized in that: The invention comprises a processor and a memory, wherein the memory is used to store a computer executable program, the processor reads the computer executable program from the memory and executes it, and when the processor executes the computer executable program, the curvature continuous autonomous parking path planning method based on reverse search according to any one of claims 1 to 5 can be implemented.

8. A computer-readable storage medium, characterized in that: A computer program is stored in the computer-readable storage medium. When the computer program is executed by a processor, the curvature continuous autonomous parking path planning method based on reverse search as claimed in any one of claims 1 to 5 can be implemented.

Images