Path planning method based on mixed A-Star algorithm and parking control module

Abstract

The invention relates to a path planning method based on a hybrid A-Star algorithm and a parking control module. The method comprises the following steps: generating a plurality of Reeds-Shepp curves with different curvature values based on an input attitude and a target attitude; carrying out collision detection on the plurality of Reeds-Shepp curves; a Reeds-Shepp curve which does not meet the path planning requirement is filtered through a hard constraint rule; calculating the comprehensive cost of the residual Reeds-Shepp curve by using a cost function; and selecting the Reeds-Shepp curve with the lowest comprehensive cost as an optimal path. According to the invention, the accuracy and reliability of self-path planning can be improved.

Description

Technical Field

[0001] The present invention relates to autonomous driving technology, and particularly to a path planning method, a parking control module, an autonomous mobile robot, a drone, and an automatic guided vehicle based on a hybrid A-Star algorithm. Background Art

[0002] In the fields of robotics and autonomous navigation, using advanced algorithms for path planning is a core issue. Path planning involves finding a collision-free trajectory from a starting point to a target point while considering motion constraints and environmental obstacles. Among them, the hybrid A-Star algorithm, as an algorithm that combines discrete and continuous state space searches, is particularly suitable for path planning of nonholonomic constraint vehicles (such as cars).

[0003] The hybrid A-Star algorithm mainly consists of two stages: a forward search stage and an analytical extension stage. In the forward search stage, the algorithm searches for possible path nodes in the discrete state space, and these nodes represent the positions and postures that the vehicle may reach. In the analytical extension stage, the algorithm uses a specific curve model to connect these nodes, thereby generating a continuous and feasible path. Among them, in the analytical extension stage of the hybrid A-Star algorithm, the Reeds-Shepp curve is widely used due to its accuracy and efficiency.

[0004] However, there are still some problems and deficiencies in the application of the existing hybrid A-Star algorithm and Reeds-Shepp curve to the automatic parking scenario. Therefore, it is necessary to improve and optimize the existing technology to improve the accuracy and reliability of automatic parking path planning. Summary of the Invention

[0005] Based on the above problems in the existing technology, the present invention aims to provide a path planning method and a path planning device based on a hybrid A-Star algorithm that can improve the accuracy and reliability of self-path planning.

[0006] The path planning method based on the hybrid A-Star algorithm of the present disclosure includes the following steps:

[0007] Generating a plurality of Reeds-Shepp curves based on multiple curvature values from an input posture to a target posture;

[0008] Performing collision detection on the plurality of Reeds-Shepp curves;

[0009] Filtering the Reeds-Shepp curves that do not meet the path planning requirements through hard constraint rules;

[0010] Calculating the comprehensive cost of the remaining Reeds-Shepp curves using a cost function; and

[0011] Select the Reeds-Shepp curve with the lowest comprehensive cost as the optimal path. Description of the Drawings

[0012] Figure 1 Shows Reeds-Shepp paths with different curvature values.

[0013] Figure 2 It is a schematic flowchart of a path planning method based on the Reeds-Shepp curve according to an embodiment of the present invention.

[0014] Figure 3 It is a schematic diagram of a parking control module applying the path planning method based on the hybrid A-Star algorithm of the present invention. Detailed Embodiments

[0015] The following are some of the multiple embodiments of the present invention, aiming to provide a basic understanding of the present invention, and not aiming to identify the key or decisive elements of the present invention or limit the scope to be protected.

[0016] First, the inventors of the present invention studied the hybrid A-Star algorithm and the Reeds-Shepp curve in the prior art.

[0017] The Reeds-Shepp curve is a curve model used to calculate the shortest path between two points of a vehicle. It can fully consider the steering limitations and kinematic constraints of the vehicle and generate a path that conforms to the actual driving characteristics of the vehicle.

[0018] Figure 1 Shows Reeds-Shepp paths with different curvature values.

[0019] Reeds-Shepp paths are paths for vehicle movement, especially suitable for omnidirectional vehicles (such as cars). These paths are characterized by their curvature (degree of turning) and length (total distance of the path). As Figure 1 shown, the three Reeds-Shepp paths with different curvature values represented in the figure are respectively:

[0020] LSR1: Curvature = 0.25, path length is about 19.034 units. This path starts from the origin, moves forward a certain distance first, and then gradually turns until it reaches near the target point;

[0021] LSR2: Curvature = 0.5, path length is about 16.762 units. This path also starts from the origin, but the degree of turning is greater than that of the path of LSR1 (curvature is 0.25), and finally it also reaches near the target point;

[0022] LSR3: Curvature = 1.0, path length is approximately 15.892 units. This path has a greater degree of turning and is more curved, and finally reaches near the target point.

[0023] It can be observed from Figure 1 that as the curvature increases, the degree of curvature of the path also increases, while the path length decreases. That is, the greater the curvature, the shorter the path, but the sharper the turn. The selection of these paths depends on specific application scenarios, such as the optimization objectives in path planning, such as finding the shortest path or the smoothest path.

[0024] However, in the existing technology, when using a Reeds-Shepp curve with a fixed curvature to connect the input pose and the target pose, there are the following deficiencies:

[0025] First, the methods for filtering Reeds-Shepp curves provided in the existing technology are not specific to the parking scenario. These filtering methods may not fully consider the special requirements and constraints during the parking process. Therefore, the filtered Reeds-Shepp curves cannot fully meet the requirements of the parking path.

[0026] Second, in actual vehicle tests, it is found that there will be very short moving segments in the Reeds-Shepp curve. These short moving segments may cause the vehicle to frequently adjust its pose during driving, increasing the driving difficulty and uncertainty. At the same time, some filtered Reeds-Shepp curves will directly connect to the target point with an arc, lacking sufficient straight segments for the vehicle to adjust its final parking pose. If there are some deviations or errors when the vehicle executes the path, then these curves lacking straight segments will make it difficult for the vehicle to adjust its pose at the final parking point, resulting in the vehicle being unable to park correctly.

[0027] Based on the above research, the path planning method based on the Reeds-Shepp curve of the present invention is proposed.

[0028] Figure 2 is a schematic flowchart of the path planning method based on the Reeds-Shepp curve according to an embodiment of the present invention.

[0029] As Figure 2 shown, the path planning method based on the Reeds-Shepp curve according to an embodiment of the present invention includes the following steps:

[0030] Step S100: Generate multiple Reeds-Shepp curves with different curvature values based on the input pose and the target pose;

[0031] Step S200: Perform collision checks along multiple Reeds-Shepp curves respectively;

[0032] Step S300: Filter the Reeds-Shepp curves according to the hard constraint rules;

[0033] Step S400: Calculate the cost of each Reeds-Shepp curve; and

[0034] Step S500: Select the Reeds-Shepp curve with the lowest cost as the optimal curve, and perform path planning based on the optimal curve.

[0035] Hereinafter, these steps S100 to S500 will be specifically described.

[0036] In step S100, multiple Reeds-Shepp curves are generated according to different curvature values from the input pose to the target pose. This is an improvement over the prior art where Reeds-Shepp curves with a fixed curvature are usually used. That is, in the present invention, instead of using only Reeds-Shepp curves with a single curvature, multiple Reeds-Shepp curves are generated according to different curvature values from the input pose to the target pose.

[0037] As an example of generating multiple Reeds-Shepp curves, it can be achieved by iterating different minimum turning radii R. For example, taking M*R as the step size, iteratively traversing the range from R to N*R to generate multiple Reeds-Shepp curves, where 0 < M < 1 and N is a natural number greater than 1. Here, for example, it is illustrated that starting from R to 4R, iterating with a step size of 0.5*R to generate Reeds-Shepp curves with different curvature values. By generating multiple Reeds-Shepp curves, the diversity of the Reeds-Shepp curves can be increased, providing more choices for path planning, and thus it is possible to find a better path.

[0038] In step S200, performing collision checking along the Reeds-Shepp curve is to ensure that the path is collision-free, which is usually achieved by checking whether the Reeds-Shepp curve intersects with the obstacles in the environment, that is, filtering out the Reeds-Shepp curves that intersect with the obstacles in the environment.

[0039] In step S300, the Reeds-Shepp curves are filtered according to the hard constraint rules. As examples, one or more of the following hard constraint rules can be listed:

[0040] a. Since the vehicle has a minimum path driving limit, it is necessary to check whether a certain section of the Reeds-Shepp curve is very short. If so, it is directly filtered out;

[0041] b. In the parking scenario, it is desired to set the paths for the last few parking maneuvers to be as straight as possible. This is because such a more perpendicular entry path is easier for the vehicle controller to execute, thereby reducing the deviation of the execution path. With this consideration in mind, starting from the end of the Reeds-Shepp curve, check the angular deviation between the obtained pose and the target pose. If the angular deviation is too large, then discard the Reeds-Shepp curve.

[0042] c. Control the length of the generated Reeds-Shepp curve. This is because a Reeds-Shepp curve with too long a length may contain paths that are not suitable for certain vehicles to execute. Therefore, the overly long Reeds-Shepp curves can be filtered out by a fixed value.

[0043] By introducing these hard constraint rules, it is possible to directly exclude those Reeds-Shepp curves that do not meet the parking requirements, thereby reducing the computational burden of the subsequent steps and improving the efficiency of finding a suitable path.

[0044] In step S400, a novel cost function is adopted to calculate the cost of each curve. This cost function takes into account the Voronoi field cost and the movement cost along the Reeds-Shepp curve path. Among them, the Voronoi field cost represents the cost of approaching obstacles, and the movement cost includes the length of the path, the steering angle, and the number of steering switches.

[0045] Among them, the Voronoi field used in the cost function is based on the Voronoi diagram, which divides the space into regions according to the nearest distance to each obstacle. The Voronoi field cost increases when approaching obstacles, which encourages staying away from them.

[0046] Specifically, in this embodiment, the cost function combines the Voronoi field cost and the movement cost along the Reeds-Shepp curve path. As an example, the cost function is calculated according to the following formula:

[0047] G = sigma_1 * v + sigma_2 * m

[0048] Where sigma_1 and sigma_2 are normalization coefficients, v is the Voronoi field cost, and m is the movement cost. Among them, m can be calculated according to the following formula:

[0049] m = lp + sp + cp

[0050] Among them, lp = w_1 * [sum l_i], sp = w_2 * [sum s_i], cp = w_3 * [sum c_i]. Here, l_i is the length of each straight line or curve segment, s_i is the turning angle, c_i is the turning switch, and w_1, w_2, and w_3 are constant weights.

[0051] Thus, compared with the prior art where the cost function may only consider the length of the path or the collision risk, the cost function of the present invention comprehensively considers safety and efficiency, and finds the optimal path by balancing the Voronoi field cost and the movement cost.

[0052] In step S500, based on the cost curve calculated in step S400, the curve with the lowest cost is selected as the optimal curve, and path planning is performed based on the optimal curve.

[0053] As described above, according to the path planning method based on the hybrid A-Star algorithm of the present invention, a new cost function is introduced to evaluate the safety and efficiency of the Reeds-Shepp curve. This cost function comprehensively considers the Voronoi field cost and the movement cost along the Reeds-Shepp curve, including factors such as path length, turning angle, and the number of turning switches. The cost function can balance the safety and efficiency of the path, ensuring that the planned path is both safe and efficient.

[0054] Furthermore, according to the path planning method based on the hybrid A-Star algorithm of the present invention, hard constraint rules are added to directly filter out Reeds-Shepp curves that do not meet the path planning requirements (such as parking requirements). These hard constraint rules include checking the short initial segment of the curve, whether the angle deviation is too large, and whether the total length of the curve exceeds a set value. Thus, it is possible to ensure that the planned path meets the actual requirements through the hard constraint rules, and avoid the inability to adjust the attitude when executing the path.

[0055] Still further, according to the path planning method based on the hybrid A-Star algorithm of the present invention, multiple Reeds-Shepp curves can be generated. For example, by iterating different minimum turning radii, a rich set of curves can be generated, and the most suitable path can be selected from them. In this way, by providing multiple path options, the flexibility of path selection is increased, which helps to find a more suitable path.

[0056] In summary, according to the path planning method based on the hybrid A-Star algorithm of the present invention, by introducing a new cost function and adding hard constraint rules, it can ensure that the planned path is far from obstacles and avoid potential collision risks. At the same time, by selecting a Reeds-Shepp curve with an appropriate curvature value, it can ensure smooth attitude adjustment during path execution, reduce the path failure probability. Further, by using the cost function, it can comprehensively consider the safety and efficiency of the path and balance the relationship between the two, and find a path that is both safe and efficient.

[0057] Figure 3 It is a schematic diagram showing a parking control module applying the path planning method based on the hybrid A-Star algorithm of the present invention.

[0058] As Figure 3 shown, the parking control module 100 of the present invention includes: a path planning module 110 based on the hybrid A-Star algorithm, wherein the path planning module 110 further includes: a search module 111 for searching for possible path nodes in a discrete state space, and these nodes represent the positions and attitudes that the vehicle may reach; and an analysis module 112 for generating a continuous and feasible Reeds-Shepp curve, wherein the analysis module 112 is used to execute the path planning method based on the hybrid algorithm of the present invention above to generate multiple Reeds-Shepp curves and select a Reeds-Shepp curve with the lowest cost function as the optimal path.

[0059] On this basis, the present invention can provide a domain controller, and the domain controller includes the above-mentioned parking control module.

[0060] In addition, the path planning method based on the hybrid A-Star algorithm of the present invention is not only applicable to the automatic parking scenario, but also can be applied to other scenarios involving non-holonomic mobile vehicle path planning, such as automatic guided vehicle (AGV), autonomous mobile robot (AMR) and unmanned aerial vehicle, etc.

[0061] Specifically, in warehouses and manufacturing facilities, automatic guided vehicles (AGVs) usually need to navigate in narrow spaces with various obstacles, so the path planning method based on the hybrid A-Star algorithm of the present invention can be adopted to achieve this; autonomous mobile robots (AMRs) are service robots operating in indoor environments (such as hospitals, hotels or office buildings), so the path planning method based on the hybrid A-Star algorithm of the present invention can be adopted to navigate efficiently and safely; for unmanned aerial vehicles flying in indoor environments or limited outdoor spaces, the path planning method based on the hybrid A-Star algorithm of the present invention can be adopted to avoid collisions and plan efficient flight paths.

[0062] Therefore, the present invention can provide an autonomous mobile robot, which includes a storage module, a processor, and a computer program stored on the storage module and executable on the processor. When the processor executes the computer program, the above-mentioned path planning method based on the hybrid A-Star algorithm is implemented.

[0063] The present invention can also provide a drone, which includes a storage module, a processor, and a computer program stored on the storage module and executable on the processor. When the processor executes the computer program, the above-mentioned path planning method based on the hybrid A-Star algorithm is implemented.

[0064] The present invention can also provide an automated guided vehicle, which includes a storage module, a processor, and a computer program stored on the storage module and executable on the processor. When the processor executes the computer program, the above-mentioned path planning method based on the hybrid A-Star algorithm is implemented.

[0065] The present invention can also provide a computer program product, including a computer program, which implements the above-mentioned path planning method based on the hybrid A-Star algorithm when executed by a processor.

[0066] The present invention can also provide a computer-readable medium, on which a computer program is stored. Among them, when the computer program is executed by a processor, the above-mentioned path planning method based on the hybrid A-Star algorithm is implemented.

[0067] The above is only the specific implementation manner of the present application, but the protection scope of the present application is not limited thereto. Those skilled in the art can think of other feasible changes or substitutions according to the technical scope disclosed in the present application, and such changes or substitutions are all covered by the protection scope of the present application. Without conflict, the embodiments of the present application and the features in the embodiments can also be combined with each other. The protection scope of the present application is subject to the records of the claims.

Claims

1. A path planning method based on a hybrid A-Star algorithm, characterized in that: The following steps are involved: Generate multiple Reeds-Shepp curves with different curvature values ​​based on the input posture and the target posture; Performing collision detection on the plurality of Reeds-Shepp curves to filter out Reeds-Shepp curves intersecting with obstacles; Filter the Reeds-Shepp curves that do not meet the path planning requirements through hard constraint rules; Calculating the combined cost of the remaining Reeds-Shepp curves using the cost function; and The Reeds-Shepp curve with the lowest comprehensive cost is selected as the optimal path.

2. The path planning method based on the hybrid A-Star algorithm as claimed in claim 1, characterized in that: The generation of multiple Reeds-Shepp curves with different curvature values ​​based on the input posture and the target posture is achieved by the following method: Set the minimum turning radius to R; and With a step size of M*R, the range from R to N*R is iterated to generate multiple Reeds-Shepp curves, where 0<M<1 and N is a natural number greater than 1.

3. The path planning method based on the hybrid A-Star algorithm as claimed in claim 1, characterized in that: The generation of multiple Reeds-Shepp curves based on multiple curvature values ​​is achieved in the following manner: Set the minimum turning radius to R; and With a step size of 0.5R, the range from R to 4R is iterated to generate multiple Reeds-Shepp curves.

4. The path planning method based on the hybrid A-Star algorithm as claimed in claim 1, characterized in that: The hard constraint rules include at least one of the following: a. Filter the Reeds-Shepp curves that are shorter than the preset minimum path length; b. filtering the Reeds-Shepp curves whose angle deviation between the end posture and the target posture exceeds the threshold; and c. Filter out Reeds-Shepp curves that exceed the preset maximum path length.

5. The path planning method based on the hybrid A-Star algorithm as claimed in claim 1, characterized in that: The cost function is generated based on the Voronoi field cost and the cost of movement along the Reeds-Shepp curve path.

6. The path planning method based on the hybrid A-Star algorithm as claimed in claim 5, characterized in that: The Voronoi field cost represents the cost of approaching an obstacle, and the movement cost includes the length of the path, the turning angle, and the number of turning switches.

7. The path planning method based on the hybrid A-Star algorithm as claimed in claim 6, characterized in that: The cost function is calculated as follows: G=sigma_1*v+sigma_2*m Among them, sigma_1 and sigma_2 are normalization coefficients, v is the Voronoi field cost, and m is the movement cost, where m can be calculated as follows: m=lp+sp+cp Where, lp = w_1*[suml_i], sp = w_2*[sums_i], cp = w_3*[sumc_i] Here, l_i is the length of each straight line or curve segment, s_i is the steering angle, c_i is the steering switch, and w_1, w_2, and w_3 are constant weights.

8. A parking control module, comprising a storage module, a processor, and a computer program stored in the storage module and executable on the processor, characterized in that: When the processor executes the computer program, the path planning method based on the hybrid A-Star algorithm described in any one of claims 1 to 7 is implemented.

9. A domain controller, comprising a memory, characterized in that: The memory executes the path planning method based on the hybrid A-Star algorithm as described in any one of claims 1 to 7.

10. An autonomous mobile robot, characterized in that: The invention comprises a storage module, a processor, and a computer program stored in the storage module and executable on the processor, characterized in that: When the processor executes the computer program, the path planning method based on the hybrid A-Star algorithm described in any one of claims 1 to 7 is implemented.

11. An automatic guided vehicle, characterized in that: The invention comprises a storage module, a processor, and a computer program stored in the storage module and executable on the processor, characterized in that: When the processor executes the computer program, the path planning method based on the hybrid A-Star algorithm described in any one of claims 1 to 7 is implemented.

12. A computer program product, comprising a computer program, characterized in that When the computer program is executed by a processor, the path planning method based on the hybrid A-Star algorithm described in any one of claims 1 to 7 is implemented.

Images