Global path planning method based on global road network and local grid map

Abstract

The invention discloses a global path planning method based on a global road network and a local grid map, and the method comprises the steps: controlling a vehicle to drive on an expected path in advance, recording path points, building a global grid map, and generating a global road network set according to the recorded path points; according to the position relation between the starting point and the path point closest to the starting point in the global road network set and the position relation between the target point and the path point closest to the target point in the global road network set, path segmentation planning is carried out, path splicing is carried out on planned paths, smoothing processing is carried out, and a global path is generated. According to the method, the advantages of the autonomous path planning algorithm and manual intervention are brought into full play, mutual complementation is achieved, the strong computing power and adaptability of the autonomous path planning algorithm are fully utilized, valuable early-stage knowledge and experience provided by manual intervention can be used for reference, and therefore the actual use requirement is better met.

Description

Background Art

[0002] With the development of automobile intelligence, autonomous driving technology has become an important research direction in the field of intelligent vehicles, among which path planning technology is one of the key technologies of autonomous driving technology. Commonly used path planning algorithms include graph search algorithms, sampling-based planning algorithms, interpolation-based planning algorithms, and machine learning-based planning algorithms. The graph search planning algorithm is a classic path planning algorithm that searches for the optimal path by searching for nodes and edges in the graph. The sampling-based planning algorithm generates candidate solutions through random sampling, and then selects the optimal solution from them. The interpolation-based planning algorithm uses interpolation polynomials or spline curves to calculate the motion trajectory of the unmanned vehicle. The machine learning-based planning algorithm uses machine learning technology to train the model and uses the model for path planning.

[0003] At present, the commonly used path planning algorithms mentioned above have the following shortcomings:

[0004] (1) When it comes to larger-scale maps, traditional path planning algorithms face a significant increase in the search space and need to check more candidate paths, resulting in reduced search efficiency and prolonged computation time.

[0005] (2) It is impossible to manually intervene in the path planning process based on actual conditions and personal preferences, which means that the generated paths are often based on preset rules and algorithms and cannot adapt to actual needs or deviate from user expectations. Summary of the invention

[0006] In view of the problems existing in the prior art, the purpose of the present disclosure is to provide a global path planning method, system, electronic device and computer-readable storage medium based on a global road network and a local grid map, thereby overcoming one or more problems caused by the limitations and defects of the relevant technology at least to a certain extent.

[0007] To achieve the above object, the present invention provides a global path planning method based on a global road network and a local grid map, comprising the following steps:

[0008] Control the vehicle to travel in advance on the desired path and record the path points, establish a global grid map and generate a global road network set A based on the recorded path points;

[0009] Calculate the distances of all path points in the global road network set from the starting point S and the distances from the target point G, and obtain the path point P closest to the starting point 1s and the path point P closest to the target point 1g ;

[0010] According to the positional relationship between the starting point and the path point closest to the starting point in the global road network set, and between the target point and the path point closest to the target point in the global road network set, a first path planning based on the local grid map and the improved hybrid A-star algorithm is performed on the path from the starting point into the global road network, a second path planning based on the standard A-star algorithm is performed on the path of the global road network, and a third path planning based on the local grid map and the hybrid A-star algorithm is performed on the path of the global road network into the target point;

[0011] The first path L1 of the first path planning, the second path L2 of the second path planning, and the third path L3 of the third path planning are concatenated and smoothed to generate a global path.

[0012] Furthermore, the first segment path planning of the path from the starting point into the global road network based on the local grid map and the improved hybrid A-star algorithm includes:

[0013] A local coordinate system X1O1Y1 is established with the starting point as the dot and the direction of the starting point as the X axis. The dot of the local coordinate system X1O1Y1 is taken as the center point, and a local grid map map1 of a predetermined size is intercepted on the global coordinate system.

[0014] The path points in the global road network are converted from the global coordinate system under the global grid map to the local coordinate system through coordinate conversion;

[0015] Define a list named openlist0 to store path points in both the global road network and the local grid map;

[0016] Convert the starting point and the target point from the global coordinate system to the local coordinate system;

[0017] Calculate the Euclidean distance between all points in the openlist0 list and the target point, and count the point closest to the target point as the local target point;

[0018] Path planning based on the improved hybrid A-star algorithm is performed between the starting point and the local target point in the local grid map map1 according to the predetermined planning termination judgment condition, and the planned path points are converted from the local coordinate system to the global coordinate system to obtain the first path L1.

[0019] Furthermore, the cost function of the improved hybrid A-star algorithm is

[0020] F h (k) = G h (k)+H h (k)+L h (k)

[0021]

[0022] Among them, G h (k) represents the cost from the starting point to the current search node, step represents the length of each step in the search process, θ h (k) represents the direction of the vehicle at the current moment, θ h (k-1) represents the direction of the vehicle at the last moment, δ is the angle penalty coefficient; H h (k) represents the cost from the current search node to the local target point, L RS Indicates the length of the RS curve between the current search node and the target point, L A Indicates the shortest path length from the current search node to the local target point; L h (k) represents the cost from the current search node to the road network point, d i (i=1,2,...n) is the Euclidean distance from the i-th road network point in the list openlist0 to the current search node.

[0023] Furthermore, the predetermined planning termination judgment condition is that the current search node coincides with the local target point or the current search node coincides with the road network point P in the openlist0 list. j (x j ,y j ,θ j ,v j )coincide.

[0024] Furthermore, the following formula is used to determine whether the current search node coincides with the local target point:

[0025]

[0026] Among them, x k ,y k is the coordinate of the current search node, is the coordinate of the local target point, d se is the preset distance threshold, θ k is the vehicle orientation angle of the current search node, is the vehicle orientation angle of the local target point, θ se is the preset angle threshold.

[0027] Further, the current search node and the road network point P in the openlist0 list are compared according to the following formula j (x j ,y j ,θ j ,v j ) Overlap judgment:

[0028]

[0029] |θk -θ j |<θ se

[0030] Among them, x k ,y k is the coordinate of the current search node, x j ,y j is the coordinates of the road network points in the openlist0 list, d se is the preset distance threshold, θ k is the vehicle orientation angle of the current search node, θ j is the vehicle orientation angle of the road network point in the openlist0 list, θ se is the preset angle threshold.

[0031] Furthermore, the second path planning of the path of the global road network based on the standard A-star algorithm includes:

[0032] Compare the distance error and angle error between the starting point and the nearest path point on the global road network;

[0033] Compare the distance error and angle error between the target point and the nearest path point on the global road network;

[0034] When the distance errors and angle errors between the starting point and the target point and the nearest path points on their respective global road networks meet predetermined conditions, in the global coordinate system, the last path point of the first path planning is used as the starting point, and the first path point of the third path planning is used as the target point to perform path planning based on the standard A-star algorithm and generate the second path L2; or

[0035] When the distance error and angle error between the starting point and the nearest path point on the global road network meet the predetermined conditions, and the distance error and angle error between the target point and the nearest path point on the global road network do not meet the predetermined conditions, in the global coordinate system, the last path point of the first path planning is used as the starting point, and the preset vehicle travel end point in the global coordinate system is used as the target point to perform path planning based on the standard A-star algorithm and generate the second path L2; or

[0036] When the distance error and angle error between the starting point and the nearest path point on the global road network do not satisfy the predetermined conditions, and the distance error and angle error between the target point and the nearest path point on the global road network satisfy the predetermined conditions, in the global coordinate system, the vehicle travel starting point is taken as the starting point, and the first path point of the third path planning is taken as the target point to perform path planning based on the standard A-star algorithm and generate the second path L2; or

[0037] When the distance error and angle error between the starting point and the target point and the nearest path point on their respective global road networks do not meet the predetermined conditions, the path planning is performed based on the standard A-star algorithm in the global coordinate system with the vehicle's starting point as the starting point and the vehicle's preset destination point in the global coordinate system as the target point to generate the second path L2.

[0038] Furthermore, the third path planning of the path of the global road network to the target point based on the local grid map and the hybrid A-star algorithm includes:

[0039] The path point P closest to the target point in the global road network 1g As a dot, take the direction vector from the dot to the next road network point as the X axis, establish a local coordinate system X2O2Y2, take the dot of the local coordinate system X2O2Y2 as the center point, and intercept the local grid map map2 of a preset size in the global coordinate system;

[0040] From the path point P closest to the target point 1g Start searching for multiple points backward along the opposite direction of the global road network, which is the entry point from the global road network to the target point. Convert the entry point from the global coordinate system XOY to the local coordinate system X2O2Y2, perform path planning between the entry point and the target point in the local grid map map2 using the hybrid A-star algorithm, and convert the planned path points from the local coordinate system to the global coordinate system to obtain the third path L3.

[0041] A second aspect of the present invention provides an electronic device, comprising:

[0042] Processor; and

[0043] A memory having computer-readable instructions stored therein, wherein the computer-readable instructions implement the above method when executed by the processor.

[0044] A third aspect of the present invention provides a computer-readable storage medium having a computer program stored thereon, wherein the computer program implements the above method when executed by a processor.

[0045] The present invention determines whether segmented path planning is needed according to the relationship between the starting point, the target point and the global road network, and improves the hybrid A-star algorithm so that a tangent path to a fixed road network can be quickly searched at the starting point, and a reasonable path to the target point can be planned at the target point. This method gives full play to the advantages of the autonomous path planning algorithm and manual intervention, complements each other, makes full use of the powerful computing power and adaptability of the autonomous path planning algorithm, and can also draw on and utilize the valuable preliminary knowledge and experience provided by manual intervention, so as to better meet the actual use needs.

[0046] It is to be understood that the foregoing general description and the following detailed description are exemplary and explanatory only and are not restrictive of the present disclosure. BRIEF DESCRIPTION OF THE DRAWINGS

[0047] The above and other features and advantages of the present disclosure will become more apparent by describing in detail example embodiments thereof with reference to the attached drawings.

[0048] Figure 1 A flowchart of a global path planning method based on a global road network and a local grid map according to an embodiment of the present invention;

[0049] Figure 2 A schematic diagram of a cutout of a local grid map at a starting point according to an embodiment of the present invention;

[0050] Figure 3 A schematic diagram of local target point calculation according to an embodiment of the present invention;

[0051] Figure 4 This is a schematic diagram of capturing a local grid map at a target point according to an embodiment of the present invention. DETAILED DESCRIPTION

[0052] Example embodiments will now be described more fully with reference to the accompanying drawings. However, example embodiments can be implemented in many forms and should not be construed as limited to the embodiments set forth herein; rather, these embodiments are provided so that this disclosure will be comprehensive and complete and will fully convey the concepts of the example embodiments to those skilled in the art. The same reference numerals in the figures represent the same or similar parts, and thus their repeated description will be omitted.

[0053] In addition, the described features, structures or characteristics may be combined in one or more embodiments in any suitable manner. In the following description, many specific details are provided to provide a full understanding of the embodiments of the present disclosure. However, those skilled in the art will appreciate that the technical solutions of the present disclosure may be practiced without one or more of the specific details, or other methods, components, materials, devices, steps, etc. may be adopted. In other cases, known structures, methods, devices, implementations, materials or operations are not shown or described in detail to avoid blurring the various aspects of the present disclosure.

[0054] The block diagrams shown in the accompanying drawings are merely functional entities and do not necessarily correspond to physically independent entities. That is, these functional entities may be implemented in software form, or these functional entities or parts of functional entities may be implemented in one or more software hardened modules, or these functional entities may be implemented in different networks and / or processor devices and / or microcontroller devices.

[0055] like Figure 1As shown, the global path planning method based on the global road network and the local grid map of the present invention comprises the following steps:

[0056] Step S110: Control the vehicle to travel in advance on the desired path and record the path points, establish a global grid map and generate a global road network set A according to the recorded path points;

[0057] Step S120: Calculate the distances of all path points in the global road network set from the starting point S and the distances from the target point G, and obtain the path point P closest to the starting point 1s and the path point P closest to the target point 1g ;

[0058] Step S130: According to the positional relationship between the starting point and the path point closest to the starting point in the global road network set and the path point closest to the target point in the global road network set, a first path planning based on the local grid map and the improved hybrid A-star algorithm is performed on the path from the starting point to the global road network, a second path planning based on the standard A-star algorithm is performed on the path of the global road network, and a third path planning based on the local grid map and the hybrid A-star algorithm is performed on the path of the global road network to the target point;

[0059] Step S140: concatenate and smooth the first path L1 of the first path planning, the second path L2 of the second path planning, and the third path L3 of the third path planning to generate a global path.

[0060] In one embodiment of the present invention, the path points recorded in step S110 include the coordinates of the path points and the corresponding speeds traveled by the vehicle. A global road network is constructed by collecting the coordinates and speeds of these path points, which is recorded as A = {P1(x1, y1, θ1, v1), P2(x2, y2, θ2, v2) ... P i (x i ,y i ,θ i ,v i ).....},i=1,2...n, where x i ,y i represents the coordinate value of the i-th point in the global coordinate system, θ i Representation vector Angle with x-axis, v i Represents the vehicle speed corresponding to the i-th point, and the distance between two adjacent path points in set A is a fixed value d0.

[0061] In one embodiment of the present invention, the first segment path planning based on the local grid map and the improved hybrid A-star algorithm for the path from the starting point into the global road network in step S130 includes:

[0062] The local coordinate system X1O1Y1 is established with the starting point as the dot and the direction of the starting point as the X axis. The dot of the local coordinate system X1O1Y1 is taken as the center point, and the local grid map map1 of a predetermined size is intercepted on the global coordinate system (see Figure 2 );

[0063] The path points in the global road network are converted from the global coordinate system under the global grid map to the local coordinate system through coordinate conversion;

[0064] Define a list named openlist0 to store path points in both the global road network and the local grid map;

[0065] Convert the starting point and the target point from the global coordinate system to the local coordinate system;

[0066] Calculate the Euclidean distance between all points in the openlist0 list and the target point, and count the point closest to the target point as the local target point; (see Figure 3 )

[0067] Path planning based on the improved hybrid A-star algorithm is performed between the starting point and the local target point in the local grid map map1 according to the predetermined planning termination judgment condition, and the planned path points are converted from the local coordinate system to the global coordinate system to obtain the first path L1.

[0068] In one embodiment of the present invention, the cost function of the improved hybrid A-star algorithm is:

[0069] F h (k) = G h (k)+H h (k)+L h (k)

[0070]

[0071] Among them, G h (k) represents the cost from the starting point to the current search node, step represents the length of each step in the search process, θ h (k) represents the direction of the vehicle at the current moment, θ h (k-1) represents the direction of the vehicle at the last moment, δ is the angle penalty coefficient; H h (k) represents the cost from the current search node to the local target point, L RS Indicates the length of the RS curve between the current search node and the target point, L A Indicates the shortest path length from the current search node to the local target point; L h (k) represents the cost from the current search node to the road network point, d i(i=1,2,...n) is the Euclidean distance from the ith road network point in the list openlist0 to the current search node. The present invention adds the road network cost L to the cost function h (k), that is, the closer the current searched node is to the global road network, the smaller its cost is, so that the planned path points are closer to the path points on the global road network and more in line with the actual kinematic characteristics of the vehicle.

[0072] In one embodiment of the present invention, the predetermined planning termination judgment condition is that the current search node coincides with the local target point or the current search node coincides with the road network point P in the openlist0 list. j (x j ,y j ,θ j ,v j )coincide.

[0073] In one embodiment of the present invention, the following formula is used to determine whether the current search node coincides with the local target point:

[0074]

[0075] Among them, x k ,y k is the coordinate of the current search node, is the coordinate of the local target point, d se is the preset distance threshold, θ k is the vehicle orientation angle of the current search node, is the vehicle orientation angle of the local target point, θ se is the preset angle threshold.

[0076] In one embodiment of the present invention, the current search node and the road network point P in the openlist0 list are compared according to the following formula: j (x j ,y j ,θ j ,v j ) Overlap judgment:

[0077]

[0078] |θ k -θ j |<θ se

[0079] Among them, x k ,y k is the coordinate of the current search node, x j ,y j is the coordinates of the road network points in the openlist0 list, d se is the preset distance threshold, θk is the vehicle orientation angle of the current search node, θ j is the vehicle orientation angle of the road network point in the openlist0 list, θ se is the preset angle threshold.

[0080] In one embodiment of the present invention, the second path planning based on the standard A-star algorithm for the path of the global road network in step S130 includes:

[0081] Compare the distance error and angle error between the starting point and the nearest path point on the global road network;

[0082] Compare the distance error and angle error between the target point and the nearest path point on the global road network;

[0083] When the distance errors and angle errors between the starting point and the target point and the nearest path points on their respective global road networks meet predetermined conditions, in the global coordinate system, the last path point of the first path planning is used as the starting point, and the first path point of the third path planning is used as the target point to perform path planning based on the standard A-star algorithm and generate the second path L2; or

[0084] When the distance error and angle error between the starting point and the nearest path point on the global road network meet the predetermined conditions, and the distance error and angle error between the target point and the nearest path point on the global road network do not meet the predetermined conditions, in the global coordinate system, the last path point of the first path planning is used as the starting point, and the preset vehicle travel end point in the global coordinate system is used as the target point to perform path planning based on the standard A-star algorithm and generate the second path L2; or

[0085] When the distance error and angle error between the starting point and the nearest path point on the global road network do not satisfy the predetermined conditions, and the distance error and angle error between the target point and the nearest path point on the global road network satisfy the predetermined conditions, in the global coordinate system, the vehicle travel starting point is taken as the starting point, and the first path point of the third path planning is taken as the target point to perform path planning based on the standard A-star algorithm and generate the second path L2; or

[0086] When the distance error and angle error between the starting point and the target point and the nearest path point on their respective global road networks do not meet the predetermined conditions, the path planning is performed based on the standard A-star algorithm in the global coordinate system with the vehicle's starting point as the starting point and the vehicle's preset destination point in the global coordinate system as the target point to generate the second path L2.

[0087] In one embodiment of the present invention, the third path planning based on the local grid map and the hybrid A-star algorithm for the path of the global road network entering the target point in step S130 includes:

[0088] The path point P closest to the target point in the global road network 1g The local coordinate system X2O2Y2 is established with the dot of the local coordinate system X2O2Y2 as the center point, and the local grid map map2 of the preset size is intercepted in the global coordinate system (see Figure 4 );

[0089] From the path point P closest to the target point 1g Start looking for several points backwards along the reverse direction of the global network (see Figure 4 , the curve in the figure is the global road network, and the direction of the arrow on the curve is the direction of the global road network). This point is the entry point from the global road network to the target point. The entry point is converted from the global coordinate system XOY to the local coordinate system X2O2Y2. A hybrid A-star algorithm is used for path planning between the entry point and the target point in the local grid map map2, and the planned path points are converted from the local coordinate system to the global coordinate system to obtain the third path L3.

[0090] In order to enable those skilled in the art to more clearly understand the working principle of the above path planning method, a specific embodiment is listed below to explain the above method in detail:

[0091] Example 1

[0092] Step 1): Establish a global grid map: Place the unmanned vehicle in the environment where it will be driving, manually control the vehicle to move in the environment where it will be driving, establish a global grid map, and determine the global coordinate system XOY.

[0093] Step 2): Establish a global road network: Make the unmanned vehicle move on the desired path at a certain speed, record the coordinates of the path points and the corresponding speeds that the vehicle has traveled, and build a global road network by collecting the coordinates and speeds of these path points, denoted as A = {P1(x1,y1,θ1,v1),P2(x2,y2,θ2,v2)......P i (x i ,y i ,θ i ,v i ).....},i=1,2...n, where x i ,y i represents the coordinate value of the i-th point in the global coordinate system, θ i Representation vector Angle with x-axis, v i Represents the vehicle speed corresponding to the i-th point, and the distance between two adjacent path points in set A is a fixed value d0.

[0094] Step 3): Set and obtain relevant parameters

[0095] Get the minimum turning radius r of the unmanned vehicle.

[0096] Set the allowable error value when the unmanned vehicle reaches the target point, the distance error d to the target point ge , the angle error is θ ge The predetermined threshold for whether the unmanned vehicle needs to perform tangent planning at the starting point is d se and θ se .

[0097] Determine the starting point of the unmanned vehicle in the global coordinate system, denoted as S(x s ,y s ,θ s ), and the target point that the unmanned vehicle is going to reach is recorded as G(x g ,y g ,θ g ). The starting point can be a manually set starting position of the vehicle, or a starting position of the vehicle located by a position positioning device. The target point can also be a manually set end point of the vehicle.

[0098] Set the judgment mark of whether the unmanned vehicle needs to perform path planning at the starting point to startplan, and set its initial value to 0. A startplan value of 1 indicates that tangent planning is required at the starting point, and a startplan value of 0 indicates that tangent planning is not required at the starting point. Set the judgment mark of whether the unmanned vehicle needs to perform path planning at the target point to goalplan, and set its initial value to 0. A goalplan value of 1 indicates that path planning is required at the target point, and a goalplan value of 0 indicates that path planning is not required at the target point.

[0099] Step 4): Traverse all points in the global road network set A and calculate the distance of each point from the starting point S(x s ,y s ,θ s ) and the distance to the target point G(x g ,y g ,θ g ), and the point closest to the starting point in set A is recorded as P 1s (x 1s ,y 1s ,θ 1s ,v 1s ), the distance from this point to the starting point is recorded as d s , the point closest to the target point in set A is denoted as P 1g (x 1g ,y 1g ,θ 1g ,v 1g), the distance from this point to the target point is recorded as d g .

[0100] Step 5): Determine whether segmented path planning is required.

[0101] If P 1s (x 1s ,y 1s ,θ 1s ,v 1s ) and the starting point are very small, then it is considered that there is no need to perform tangent path planning at the starting point, that is, if there is d s <d se And |θ 1s -θ s |<θ se , it is considered that there is no need to perform tangent planning at the starting point, and startplan is set to 0; otherwise, tangent planning at the starting point is required, and startplan is set to 1.

[0102] If P 1g (x 1g ,y 1g ,θ 1g ,v 1g ) and the target point are very small, it is considered that path planning at the target point is not necessary, that is, if there is d g <d ge And |θ 1g -θ g |<θ ge , it is considered that path planning at the target point is not necessary, and goalplan is set to 0; otherwise, path planning at the target point is required, and goalplan is set to 1.

[0103] Step 6): Segmented path planning

[0104] If startplan is 1 and goalplan is 1, execute step 6-1), step 6-2), and step 6-3) in sequence.

[0105] If startplan is 1 and goalplan is 0, execute step 6-1) and step 6-3) in sequence.

[0106] If startplan is 0 and goalplan is 1, execute step 6-2) and step 6-3) in sequence.

[0107] If startplan is 0 and goalplan is 0, execute step 6-3).

[0108] Step 6-1): Tangent planning at the starting point.

[0109] Step 6-1-1): Take the starting point S(x s ,y s ,θ s ) as the dot, and the direction of the starting point is the x-axis to establish a local coordinate system X1O1Y1. With the dot of the local coordinate system X1O1Y1 as the center point, intercept the local grid map map1 with a size of 2m×2m in the global coordinate system, such as Figure 2 shown.

[0110] Step 6-1-2): Use the coordinate conversion formula to transform the path point A in the global road network into {P1(x1,y1,θ1,v1),P2(x2,y2,θ2,v2)...P i (x i ,y i ,θ i ,v i ).....}, i=1,2...n is transformed from the global coordinate system XOY to the local coordinate system X1O1Y1, and the road network points in the local coordinate system X1O1Y1 are recorded as

[0111] Step 6-1-3): Define a list named openlist0 to store points that are both in the global network and in map1. and Then Add to the openlist0 list.

[0112] Step 6-1-4): Set the starting point S(x s ,y s ,θ s ) and the target point G(x g ,y g ,θ g ) Use the coordinate conversion formula to convert from the global coordinate system XOY to the local coordinate system X1O1Y1. The starting point after conversion is recorded as The target point is recorded as

[0113] Step 6-1-5) Calculate the local target point: Calculate the distance from all points in the openlist0 list to the target point The Euclidean distance of the target point in the list openlist0 is the local target point, which is recorded as like Figure 3 shown.

[0114] Step 6-1-6) As the starting point, For the local target point, the improved hybrid A-star algorithm is used to plan the path on the local grid map map1. If the current searched node meets the planning termination condition, the planning is considered successful. The current node is recorded as the tangent point from the starting point to the global road network. The tangent point is converted from the local coordinate system X1O1Y1 to the global coordinate system XOY and recorded as P q (x q ,y q ,θ q ). The path points generated by the planning are converted from the local coordinate system X1O1Y1 to the global coordinate system XOY and recorded as L1.

[0115] The improved hybrid A-star algorithm mainly includes the following two improvements:

[0116] (1) Improvement of cost function

[0117] The cost function of the standard hybrid A-star algorithm consists of two parts: the actual cost G h (k) and the estimated cost H h (k), the optimal point needs to be found through the cost function. The improved cost function in this method is shown in formula (1), which adds the road network cost L to the original cost function. h (k), that is, the closer the currently searched node is to the global road network, the smaller its cost.

[0118] F h (k) = G h (k)+H h (k)+L h (k)(1)

[0119] Among them G h (k), H h (k), and L h (k) is calculated as follows:

[0120]

[0121] Among them, G h (k) is the actual cost, which means the cost of the vehicle moving from the starting point to the current point. Step represents the length of each step in the search process. θ h (k) represents the direction of the vehicle at the current moment, θ h (k-1) represents the direction of the vehicle at the previous moment, and δ is the angle penalty coefficient. That is, the smaller the difference between the current and previous vehicle directions, the smaller the G h (k) is smaller.

[0122] H h (k) is the estimated cost, which represents the estimated cost from the current search node k to the end point, L RSIndicates the length of the RS curve from the current position to the target point. L A Indicates the shortest path length from the current position to the target point found by the A* algorithm, H h (k) is the comparison L RS and L A size, choose the larger one.

[0123] L h (k) is the road network cost, which represents the cost from the current node k to the road network point. i (i=1,2,...n) is the Euclidean distance from the ith road network point in the list openlist0 to the current node, that is, the closer the point is to the global road network, the smaller its cost is. If the node being searched is a point on the road network, the greater the probability of being selected.

[0124] (2) Judgment conditions for planning termination

[0125] In the standard hybrid A-star algorithm, if the algorithm searches for the target node, the planning is considered successful. In this method, if the algorithm finds the target node or finds a point on the road network, the tangent planning from the starting point to the global road network is considered successful. That is, if the currently searched node P k (x k ,y k ,θ k ,v k ) The planning is considered successful if any of the following two conditions are met:

[0126] 1) The node currently being searched coincides with the local target point

[0127]

[0128] 2) The node currently being searched is at a point P in openlist0 j (x j ,y j ,θ j ,v j )coincide

[0129]

[0130] |θ k -θ j |<θ se (5)

[0131] Step 6-2) Path planning to the target point.

[0132] Step 6-2-1) Take the point P calculated in step 4) 1g (x 1g ,y 1g ,θ 1g ,vg ) is the coordinate point, from this point to the next road network point P 1g+1 (x 1g+1 ,y 1g+1 ,θ 1g+1 , v 1g+1 ) direction vector As the x-axis, establish the local coordinate system X2O2Y2. Take the dot of the local coordinate system X2O2Y2 as the center point, and intercept the local grid map map2 with a size of 2m×2m in the global coordinate system, such as Figure 4 shown.

[0133] Step 6-2-2) From point P 1g (x 1g ,y 1g ,θ 1g ,v 1g ) starts to search for multiple points in the reverse direction of the global road network. This point is the entry point from the global road network to the target point. The entry point is converted from the global coordinate system XOY to the local coordinate system X2O2Y2. The converted coordinate point is recorded as like Figure 4 shown.

[0134] Step 6-2-3) Set the target point G(x g ,y g ,θ g ) is transformed from the global coordinate system XOY to the local coordinate system X2O2Y2, and the transformed point is recorded as

[0135] Step 6-2-4) In the local coordinate system X2O2Y2, As the starting point, The standard hybrid A-star algorithm is used for path planning, and the path points generated by the planning are converted from the local coordinate system X2O2Y2 to the global coordinate system XOY. The converted path points are recorded as L3.

[0136] Step 6-3) Road network planning

[0137] The path points in the road network are recorded when the vehicle is actually driving. These path points conform to the kinematic characteristics of the vehicle. Therefore, the standard A-star algorithm is used for planning in the global road network planning. There are four specific cases:

[0138] If startplan is 1 and goalplan is 1, the tangent point pose P is in the global coordinate system. q As the starting point, the entry point Path planning is performed for the target point, and the generated path is recorded as L2.

[0139] If startplan is 1 and goalplan is 0, the tangent point pose P is in the global coordinate system. q As the starting point, G(x g ,y g ,θ g ) is used to plan the path to the target point, and the generated path is recorded as L2.

[0140] If startplan is 0 and goalplan is 1, the global coordinate system is S(x s ,y s ,θ s ) as the starting point and the entry point Path planning is performed for the target point, and the generated path is recorded as L2.

[0141] If startplan is 0 and goalplan is 0, the global coordinate system is S(x s ,y s ,θ s ) as the starting point, with G(x g ,y g ,θ g ) is used to plan the path to the target point, and the generated path is recorded as L2.

[0142] Step 7) Path splicing.

[0143] If startplan is 1 and goalplan is 1, L1, L2, and L3 are concatenated into a path in order and smoothed to generate a new path L, which is the generated global path.

[0144] If startplan is 1 and goalplan is 0, L1 and L2 are concatenated into a path in sequence and smoothed to generate a new path L, which is the generated global path.

[0145] If startplan is 0 and goalplan is 1, L2 and L3 are concatenated into a path in sequence and smoothed to generate a new path L, which is the generated global path.

[0146] If startplan is 0 and goalplan is 0, L2 is the generated global path.

[0147] In summary, the present invention determines whether segmented path planning is needed according to the relationship between the starting point, the target point and the global road network, and improves the hybrid A-star algorithm so that a tangent path to the fixed road network can be quickly searched at the starting point, and a reasonable path to the target point can be planned at the target point. This method gives full play to the advantages of autonomous path planning algorithm and manual intervention, complements each other, makes full use of the powerful computing power and adaptability of the autonomous path planning algorithm, and can also draw on and utilize the valuable preliminary knowledge and experience provided by manual intervention, so as to better meet the actual use needs.

[0148] In addition, in an exemplary embodiment of the present disclosure, an electronic device capable of implementing the above method is also provided.

[0149] It will be appreciated by those skilled in the art that various aspects of the present invention may be implemented as systems, methods or program products. Therefore, various aspects of the present invention may be specifically implemented in the following forms, namely: complete hardware embodiments, complete software embodiments (including firmware, microcode, etc.), or embodiments combining hardware and software aspects, which may be collectively referred to herein as "circuits", "modules" or "systems".

[0150] The electronic device is presented in the form of a general-purpose computing device. The components of the electronic device may include, but are not limited to: at least one processing unit, at least one storage unit, a bus connecting different system components (including the storage unit and the processing unit), and a display unit.

[0151] The storage unit stores program codes, which can be executed by the processing unit, so that the processing unit performs the steps according to various exemplary embodiments of the present invention described in the above “Exemplary Method” section of this specification. For example, the processing unit can perform the following steps: Figure 2 Steps S200 to S230 shown in FIG.

[0152] The storage unit may include a readable medium in the form of a volatile memory unit, such as a random access memory unit (RAM) and / or a cache memory unit, and may further include a read-only memory unit (ROM).

[0153] The storage unit may also include a program / utility having a set (at least one) of program modules, such program modules including but not limited to: an operating system, one or more application programs, other program modules, and program data, each of which or some combination may include an implementation of a network environment.

[0154] The bus may represent one or more of several types of bus structures, including a memory unit bus or memory unit controller, a peripheral bus, an accelerated graphics port, a processing unit, or a local bus using any of a variety of bus architectures.

[0155] The electronic device can also communicate with one or more external devices (such as keyboards, pointing devices, Bluetooth devices, etc.), and can also communicate with one or more devices that enable users to interact with the electronic device, and / or communicate with any device (such as routers, modems, etc.) that enables the electronic device to communicate with one or more other computing devices. This communication can be carried out through an input / output (I / O) interface. In addition, the electronic device can also communicate with one or more networks (such as local area networks (LANs), wide area networks (WANs) and / or public networks, such as the Internet) through a network adapter. As shown in the figure, the network adapter communicates with other modules of the electronic device through a bus. It should be understood that, although not shown in the figure, other hardware and / or software modules can be used in conjunction with the electronic device, including but not limited to: microcode, device drivers, redundant processing units, external disk drive arrays, RAID systems, tape drives, and data backup storage systems, etc.

[0156] Through the description of the above embodiments, it is easy for those skilled in the art to understand that the example embodiments described here can be implemented by software, or by software combined with necessary hardware. Therefore, the technical solution according to the embodiment of the present disclosure can be embodied in the form of a software product, which can be stored in a non-volatile storage medium (which can be a CD-ROM, a USB flash drive, a mobile hard disk, etc.) or on a network, and includes several instructions to enable a computing device (which can be a personal computer, a server, a terminal device, or a network device, etc.) to execute the method according to the embodiment of the present disclosure.

[0157] In an exemplary embodiment of the present disclosure, a computer-readable storage medium is also provided, on which a program product capable of implementing the above method of the present specification is stored. In some possible embodiments, various aspects of the present invention can also be implemented in the form of a program product, which includes a program code, and when the program product is run on a terminal device, the program code is used to enable the terminal device to perform the steps according to various exemplary embodiments of the present invention described in the above "Exemplary Method" section of the present specification.

[0158] The program product may be a portable compact disk read-only memory (CD-ROM) and include program code, and can be run on a terminal device, such as a personal computer. However, the program product of the present invention is not limited thereto, and in this document, a readable storage medium may be any tangible medium containing or storing a program, which may be used by or in combination with an instruction execution system, apparatus, or device.

[0159] The program product may use any combination of one or more readable media. The readable medium may be a readable signal medium or a readable storage medium. The readable storage medium may be, for example, but not limited to, an electrical, magnetic, optical, electromagnetic, infrared, or semiconductor system, device or device, or any combination of the above. More specific examples (non-exhaustive list) of readable storage media include: an electrical connection with one or more wires, a portable disk, a hard disk, a random access memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or flash memory), an optical fiber, a portable compact disk read-only memory (CD-ROM), an optical storage device, a magnetic storage device, or any suitable combination of the above.

[0160] Computer readable signal media may include data signals propagated in baseband or as part of a carrier wave, in which readable program code is carried. Such propagated data signals may take a variety of forms, including but not limited to electromagnetic signals, optical signals, or any suitable combination of the above. Readable signal media may also be any readable medium other than a readable storage medium, which may send, propagate, or transmit a program for use by or in conjunction with an instruction execution system, apparatus, or device.

[0161] The program code embodied on the readable medium may be transmitted using any appropriate medium, including but not limited to wireless, wired, optical cable, RF, etc., or any suitable combination of the foregoing.

[0162] Program code for performing the operations of the present invention may be written in any combination of one or more programming languages, including object-oriented programming languages ​​such as Java, C++, etc., and conventional procedural programming languages ​​such as "C" or similar programming languages. The program code may be executed entirely on the user computing device, partially on the user device, as a separate software package, partially on the user computing device and partially on a remote computing device, or entirely on a remote computing device or server. In cases involving a remote computing device, the remote computing device may be connected to the user computing device through any type of network, including a local area network (LAN) or a wide area network (WAN), or may be connected to an external computing device (e.g., through the Internet using an Internet service provider).

[0163] In addition, the above-mentioned figures are only schematic illustrations of the processes included in the method according to an exemplary embodiment of the present invention, and are not intended to be limiting. It is easy to understand that the processes shown in the above-mentioned figures do not indicate or limit the time sequence of these processes. In addition, it is also easy to understand that these processes can be performed synchronously or asynchronously, for example, in multiple modules.

[0164] Those skilled in the art will readily appreciate other embodiments of the present disclosure after considering the specification and practicing the invention disclosed herein. This application is intended to cover any variations, uses, or adaptations of the present disclosure that follow the general principles of the present disclosure and include common knowledge or customary technical means in the art that are not disclosed in the present disclosure. The specification and examples are to be considered as exemplary only, and the true scope and spirit of the present disclosure are indicated by the claims.

[0165] It should be understood that the present disclosure is not limited to the exact structures that have been described above and shown in the drawings, and that various modifications and changes may be made without departing from the scope thereof. The scope of the present disclosure is limited only by the appended claims.

Claims

1. A global path planning method based on a global road network and a local grid map, characterized in that: The steps include: Control the vehicle to travel in advance on the desired path and record the path points, establish a global grid map and generate a global road network set A based on the recorded path points; Calculate the distances of all path points in the global road network set from the starting point S and the distances from the target point G, and obtain the path point P closest to the starting point 1s and the path point P closest to the target point 1g ; According to the positional relationship between the starting point and the path point closest to the starting point in the global road network set, and between the target point and the path point closest to the target point in the global road network set, a first path planning based on the local grid map and the improved hybrid A-star algorithm is performed on the path from the starting point into the global road network, a second path planning based on the standard A-star algorithm is performed on the path of the global road network, and a third path planning based on the local grid map and the hybrid A-star algorithm is performed on the path of the global road network into the target point; The first path L1 of the first path planning, the second path L2 of the second path planning, and the third path L3 of the third path planning are concatenated and smoothed to generate a global path.

2. The global path planning method according to claim 1, characterized in that: The first segment of path planning for the path from the starting point into the global road network based on the local grid map and the improved hybrid A-star algorithm includes: A local coordinate system X1O1Y1 is established with the starting point as the dot and the direction of the starting point as the X axis. The dot of the local coordinate system X1O1Y1 is taken as the center point, and a local grid map map1 of a predetermined size is intercepted on the global coordinate system. The path points in the global road network are converted from the global coordinate system under the global grid map to the local coordinate system through coordinate conversion; Define a list named openlist0 to store path points in both the global road network and the local grid map; Convert the starting point and the target point from the global coordinate system to the local coordinate system; Calculate the Euclidean distance between all points in the openlist0 list and the target point, and count the point closest to the target point as the local target point; Path planning based on the improved hybrid A-star algorithm is performed between the starting point and the local target point in the local grid map map1 according to the predetermined planning termination judgment condition, and the planned path points are converted from the local coordinate system to the global coordinate system to obtain the first path L1.

3. The global path planning method according to claim 2, characterized in that: The cost function of the improved hybrid A-star algorithm is F h (k) = G h (k)+H h (k)+L h (k) Among them, G h (k) represents the cost from the starting point to the current search node, step represents the length of each step in the search process, θ h (k) represents the direction of the vehicle at the current moment, θ h (k-1) represents the direction of the vehicle at the last moment, δ is the angle penalty coefficient; H h (k) represents the cost from the current search node to the local target point, L RS Indicates the length of the RS curve between the current search node and the target point, L A Indicates the shortest path length from the current search node to the local target point; L h (k) represents the cost from the current search node to the road network point, d i (i=1,2,...n) is the Euclidean distance from the i-th road network point in the list openlist0 to the current search node.

4. The global path planning method according to claim 2, characterized in that: The predetermined planning termination judgment condition is that the current search node coincides with the local target point or the current search node coincides with the road network point P in the openlist0 list. j (x j ,y j ,θ j ,v j )coincide.

5. The global path planning method according to claim 4, characterized in that: The following formula is used to determine whether the current search node coincides with the local target point: Among them, x k ,y k is the coordinate of the current search node, is the coordinate of the local target point, d se is the preset distance threshold, θ k is the vehicle orientation angle of the current search node, is the vehicle orientation angle of the local target point, θ se is the preset angle threshold.

6. The global path planning method according to claim 4, characterized in that: According to the following formula, the current search node and the road network point P in the openlist0 list are compared. j (x j ,y j ,θ j ,v j ) Overlap judgment: |θ k -θ j |<θ se Among them, x k ,y k is the coordinate of the current search node, x j ,y j is the coordinates of the road network points in the openlist0 list, d se is the preset distance threshold, θ k is the vehicle orientation angle of the current search node, θ j is the vehicle orientation angle of the road network point in the openlist0 list, θ se is the preset angle threshold.

7. The global path planning method according to claim 1, characterized in that: The second stage of path planning for the global road network based on the standard A-star algorithm includes: Compare the distance error and angle error between the starting point and the nearest path point on the global road network; Compare the distance error and angle error between the target point and the nearest path point on the global road network; When the distance errors and angle errors between the starting point and the target point and the nearest path points on their respective global road networks meet predetermined conditions, in the global coordinate system, the last path point of the first path planning is used as the starting point, and the first path point of the third path planning is used as the target point to perform path planning based on the standard A-star algorithm and generate the second path L2; or When the distance error and angle error between the starting point and the nearest path point on the global road network meet the predetermined conditions, and the distance error and angle error between the target point and the nearest path point on the global road network do not meet the predetermined conditions, in the global coordinate system, the last path point of the first path planning is used as the starting point, and the preset vehicle travel end point in the global coordinate system is used as the target point to perform path planning based on the standard A-star algorithm and generate the second path L2; or When the distance error and angle error between the starting point and the nearest path point on the global road network do not satisfy the predetermined conditions, and the distance error and angle error between the target point and the nearest path point on the global road network satisfy the predetermined conditions, in the global coordinate system, the vehicle travel starting point is taken as the starting point, and the first path point of the third path planning is taken as the target point to perform path planning based on the standard A-star algorithm and generate the second path L2; or When the distance error and angle error between the starting point and the target point and the nearest path point on their respective global road networks do not meet the predetermined conditions, the path planning is performed based on the standard A-star algorithm in the global coordinate system with the vehicle's starting point as the starting point and the vehicle's preset destination point in the global coordinate system as the target point to generate the second path L2.

8. The global path planning method according to claim 1, characterized in that: The third path planning of the path to the target point in the global road network based on the local grid map and the hybrid A-star algorithm includes: The path point P closest to the target point in the global road network 1g As a dot, take the direction vector from the dot to the next road network point as the X axis, establish a local coordinate system X2O2Y2, take the dot of the local coordinate system X2O2Y2 as the center point, and intercept the local grid map map2 of a preset size in the global coordinate system; From the path point P closest to the target point 1g Start searching for multiple points backward along the opposite direction of the global road network, which is the entry point from the global road network to the target point. Convert the entry point from the global coordinate system XOY to the local coordinate system X2O2Y2, perform path planning between the entry point and the target point in the local grid map map2 using the hybrid A-star algorithm, and convert the planned path points from the local coordinate system to the global coordinate system to obtain the third path L3.

9. An electronic device, characterized in that: include: processor; as well as A memory having computer-readable instructions stored thereon, wherein the computer-readable instructions, when executed by the processor, implement the method according to any one of claims 1 to 8.

10. A computer-readable storage medium having a computer program stored thereon, wherein the computer program implements the method according to any one of claims 1 to 8 when executed by a processor.

Images