Static path planning method, device and system based on human-like trajectory

Abstract

The present invention discloses a static path planning method, device and system based on human-like trajectory, the method comprises obtaining artificially driven vehicle trajectory data; mapping the obtained starting position and terminal position to the artificially driven vehicle trajectory, obtaining corresponding starting point mapping points and terminal mapping points, and then forming a section trajectory; calculating the boundary information of the section trajectory and the value range of preset driving parameters according to the artificially driven vehicle trajectory data and the section trajectory; mapping the current node to the artificially driven vehicle trajectory, generating mapping point information, taking the boundary information of the section trajectory and the value range of preset driving parameters as constraints, performing node diffusion with variable step length based on the mapping point information to obtain diffusion nodes until connecting to the terminal position; the initial value of the current node is the starting position; connecting each diffusion node to obtain a smooth path. The present invention can effectively improve the environmental adaptability and rationality of path planning in complex environments.

Description

Technical Field

[0001] The present invention belongs to the field of intelligent driving technology, and specifically relates to a static path planning method, device and system based on human-like trajectory. Background Art

[0002] With the development of intelligent driving technology, it is of great significance to promote the unmanned mining trucks in open-pit mines due to the harsh working environment and complex road environment. The implementation of unmanned mining trucks can effectively improve the transportation efficiency and safety of open-pit mines. During the operation of unmanned mining trucks, the driving path has an important impact on the vehicle control performance, fuel consumption and wear. The existing path planning schemes mainly include: search algorithm, random sampling method, curve interpolation method, and artificial potential field method. It can be seen that the solutions for path planning mainly focus on the optimization of the algorithm, and do not combine the advantages of manual driving, and cannot fully utilize the experience of manual driving.

[0003] In the Chinese invention patent application with application number 201410809844.0, a path planning method for a coal mine rescue robot based on an artificial potential field is disclosed. Under the condition of a closed area with a known workspace, this method establishes a local workspace repulsive potential field, uses the Quasi-Geodesic method to establish a path partial differential equation, and gradually obtains a local path until the final path is obtained. The path is relatively accurate and short. However, the potential field method has strict requirements on the road environment. The roads in the open-pit mine area are irregular, and the road boundary shape is complex. The generated path is greatly affected by the boundary. Therefore, this method is not suitable for unmanned transportation environments in open-pit mines.

[0004] A Chinese invention patent application with application number 201910661538.X discloses a method for full coverage path planning of mobile robots in abandoned mining areas. This method uses the ox-ploughing decomposition method to decompose the original map, uses the depth-first search algorithm to complete the planning of the traversal order of each sub-area, and uses the bio-inspired neural network algorithm to complete the internal traversal of each sub-area and the path transfer between sub-areas, thereby obtaining full coverage of the entire environment. It can effectively reduce the difficulty of full coverage path planning and realize full coverage path planning of abandoned mining areas. However, this method is difficult to guarantee the optimal path. In addition, the use of neural network algorithms requires a large amount of data for training, the actual application conditions are too harsh, and the portability is poor.

[0005] In the Chinese invention patent application with application number 202011406640.4, a global path planning method and system for unstructured roads is disclosed. This method makes the searched path meet the non-complete constraint requirements of the vehicle in the A* algorithm, and obtains multiple layers of candidate sub-nodes by expanding forward multiple steps at the same discrete front wheel corner, thereby completing multiple steps in one node expansion and speeding up the efficiency of path search. However, when this method is applied to path planning in an open-pit mining environment, the planning results are too affected by the input parameters, and the rationality of the obtained path cannot be guaranteed. Summary of the invention

[0006] In view of the above problems, the present invention proposes a static path planning method, device and system based on human-like trajectory, which can effectively improve the environmental adaptability and rationality of path planning in complex environments.

[0007] In order to achieve the above technical objectives and the above technical effects, the present invention is implemented through the following technical solutions:

[0008] In a first aspect, the present invention provides a static path planning method based on human-like trajectory, comprising:

[0009] Acquire manually driven vehicle trajectory data, wherein the manually driven vehicle trajectory data includes the manually driven vehicle trajectory and driving parameters corresponding to each trajectory point on the manually driven vehicle trajectory;

[0010] Mapping the acquired starting point and end point positions to the manually driven vehicle trajectory to obtain corresponding starting point mapping points and end point mapping points, thereby forming a road segment trajectory;

[0011] Calculating the boundary information of the road section trajectory and the value range of the preset driving parameters according to the manually driven vehicle trajectory data and the road section trajectory;

[0012] Map the current node to the trajectory of the manually driven vehicle to generate mapping point information, and use the boundary information of the road segment trajectory and the value range of the preset driving parameters as constraints to perform node diffusion with a variable step length based on the mapping point information to obtain diffusion nodes until they are connected to the end position; the initial value of the current node is the starting position;

[0013] Connect the diffusion nodes that meet the requirements to obtain a smooth path.

[0014] Optionally, the driving parameters include vehicle position, speed, heading and curvature.

[0015] Optionally, the calculation method of the value range of each driving parameter includes:

[0016] Based on each trajectory point included in the road segment trajectory, obtaining corresponding driving parameters from the manually driven vehicle trajectory data;

[0017] Based on the acquired driving parameters, the average speed of the road section, the maximum speed of the road section, the minimum speed of the road section and the maximum curvature of the road section are calculated.

[0018] Optionally, performing variable-step node diffusion based on the mapping point information comprises the following steps:

[0019] Based on the preset initial node step size d0, calculate the node diffusion step size d i :

[0020]

[0021] Based on the preset maximum curvature k0 of the vehicle and the maximum curvature k of the road section max , mapping point curvature k i and node diffusion step d i , calculate the maximum offset angle α of the diffusion node i :

[0022]

[0023] Taking the road segment boundary information and the value range of each driving parameter as constraints, based on the node diffusion step length d i and the maximum deviation angle α i , perform collision detection on diffusion nodes and delete the diffusion nodes that fail;

[0024] Evaluate the diffusion nodes, obtain the cost of the diffusion nodes, and then obtain the diffusion node with the lowest cost as the current node for the next round of node diffusion. The evaluation formula is:

[0025] g(n)=∑(w a Δ a +w yaw Δ yaw +w dis Δ dis )

[0026] In the formula, w a 、w yaw 、w dis are the weights corresponding to each evaluation item, Δ a is the angle offset between the diffusion node and the mapping node, Δ yaw is the heading offset between the diffusion node and the mapping node, Δ dis The cost of the mapping point from the current diffusion node to the starting point.

[0027] Optionally, the curvature and the curvature change rate of the smooth path are both continuous.

[0028] In a second aspect, the present invention provides a static path planning device based on human-like trajectory, comprising:

[0029] An acquisition module, used to acquire manually driven vehicle trajectory data, wherein the manually driven vehicle trajectory data includes the manually driven vehicle trajectory and driving parameters corresponding to each trajectory point on the manually driven vehicle trajectory;

[0030] A mapping module is used to map the acquired starting position and end position to the manually driven vehicle trajectory, obtain the corresponding starting point mapping point and end point mapping point, and then form a road section trajectory;

[0031] A calculation module, used to calculate the boundary information of the road section trajectory and the value range of the preset driving parameters according to the manually driven vehicle trajectory data and the road section trajectory;

[0032] A node diffusion module is used to map the current node to the trajectory of the manually driven vehicle, generate mapping point information, and perform node diffusion with a variable step length based on the mapping point information with the boundary information of the road segment trajectory and the value range of the preset driving parameters as constraints to obtain diffusion nodes until the node is connected to the end position; the initial value of the current node is the starting position;

[0033] The connection module is used to connect the diffusion nodes to obtain a smooth path.

[0034] In a third aspect, the present invention provides a static path planning system based on human-like trajectory, characterized in that it includes a storage medium and a processor;

[0035] The storage medium is used to store instructions;

[0036] The processor is configured to operate according to the instructions to execute the steps of the method according to any one of the first aspects.

[0037] In a fourth aspect, the present invention provides a static path planning system based on human-like trajectory, comprising:

[0038] A data acquisition module, used to collect manually driven vehicle trajectory data, wherein the manually driven vehicle trajectory data includes the manually driven vehicle trajectory and driving parameters corresponding to each trajectory point on the manually driven vehicle trajectory;

[0039] A mapping information calculation module is used to map the acquired starting position and end position to the manually driven vehicle trajectory, obtain the corresponding starting point mapping point and end point mapping point, and then form a section trajectory, and calculate the value range of the preset driving parameter according to the manually driven vehicle trajectory data and the section trajectory;

[0040] The human-like parameter range definition module uses the preset driving parameter value range of the mapping information calculation module as a reference, receives the starting point position, takes the starting point as the initial current node, and calculates the current node diffusion range information;

[0041] The path planning module maps the acquired starting point position and end point position to the manually driven vehicle trajectory, obtains the corresponding starting point mapping point and end point mapping point, and then forms a section trajectory, and calculates the boundary information of the section trajectory according to the manually driven vehicle trajectory data and the section trajectory; maps the current node to the manually driven vehicle trajectory, generates mapping point information, takes the boundary information of the section trajectory and the value range of the preset driving parameters as constraints, performs node diffusion with variable step length based on the mapping point information to obtain diffusion nodes, until it is connected to the end point position; the initial value of the current node is the starting point position; and connects the diffusion nodes to obtain a smooth path.

[0042] Optionally, the driving parameters include vehicle position, speed, heading and curvature.

[0043] Optionally, the calculation method of the value range of each driving parameter includes:

[0044] Taking the road segment boundary information as a constraint, obtaining corresponding data from the manually driven vehicle trajectory data;

[0045] Based on the acquired data, the average speed of the road section, the maximum speed of the road section, the minimum speed of the road section and the maximum curvature of the road section are calculated.

[0046] Optionally, performing variable-step node diffusion based on the mapping point information comprises the following steps:

[0047] Based on the preset initial node step size d0, calculate the node diffusion step size d i :

[0048]

[0049] Based on the preset maximum curvature k0 of the vehicle and the maximum curvature k of the road section max , mapping point curvature k i and node diffusion step d i , calculate the maximum offset angle α of the diffusion node i :

[0050]

[0051] Taking the road segment boundary information and the value range of each driving parameter as constraints, based on the node diffusion step length d i and the maximum deviation angle α i , perform collision detection on diffusion nodes and delete the diffusion nodes that fail;

[0052] Evaluate the diffusion nodes, obtain the cost of the diffusion nodes, and then obtain the diffusion node with the lowest cost as the current node for the next round of node diffusion. The evaluation formula is:

[0053] g(n)=∑(w a Δ a +w yaw Δ yaw +w dis Δ dis )

[0054] In the formula, w a 、w yaw 、w dis are the weights corresponding to each evaluation item, Δ a is the angle offset between the diffusion node and the mapping node, Δ yaw is the heading offset between the diffusion node and the mapping node, Δ dis The cost of the mapping point from the current diffusion node to the starting point.

[0055] Optionally, the curvature and the curvature change rate of the smooth path are both continuous.

[0056] Compared with the prior art, the present invention has the following beneficial effects:

[0057] The present invention fully combines manual driving experience and dynamically adjusts planning algorithm parameters to solve the problems of inappropriate input parameters during path planning and poor vehicle control performance caused by unreasonable algorithm planning paths in complex mining road environments, thereby improving the environmental adaptability and rationality of path planning in complex environments. BRIEF DESCRIPTION OF THE DRAWINGS

[0058] In order to make the content of the present invention more clearly understood, the present invention is further described in detail below based on specific embodiments and in conjunction with the accompanying drawings, wherein:

[0059] Figure 1 A flowchart of a static path planning method based on human-like trajectory according to an embodiment of the present invention;

[0060] Figure 2 This is a diagram showing the node mapping effect of a path planning module according to an embodiment of the present invention;

[0061] Figure 3 This is a diagram of the variable step-size diffusion effect of the path planning module nodes in an embodiment of the present invention. DETAILED DESCRIPTION

[0062] In order to make the purpose, technical solution and advantages of the present invention more clearly understood, the present invention is further described in detail below in conjunction with the embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not intended to limit the scope of protection of the present invention.

[0063] The application principle of the present invention is described in detail below in conjunction with the accompanying drawings.

[0064] During the operation of unmanned mining trucks, the driving path has an important impact on the vehicle control performance, fuel consumption and wear. The driving habits of excellent drivers can effectively improve the stability of the vehicle and reduce vehicle fuel consumption and wear. To this end, the present invention proposes to combine the manual driving trajectory with the path planning algorithm, which can combine the advantages of high environmental adaptability and flexibility of manual driving with the portability and accuracy of the path planning algorithm, improve production efficiency and reduce production costs.

[0065] Example 1

[0066] An embodiment of the present invention provides a static path planning method based on human-like trajectory, comprising the following steps:

[0067] (1) obtaining manually driven vehicle trajectory data, wherein the manually driven vehicle trajectory data includes the manually driven vehicle trajectory and driving parameters corresponding to each trajectory point on the manually driven vehicle trajectory;

[0068] (2) Mapping the acquired starting point and end point to the manually driven vehicle trajectory to obtain corresponding starting point mapping points and end point mapping points, thereby forming a road segment trajectory; the path between the starting point and the end point is the path to be planned;

[0069] (3) calculating the boundary information of the road segment trajectory and the value range of the preset driving parameters according to the manually driven vehicle trajectory data and the road segment trajectory;

[0070] (4) using the boundary information of the road segment trajectory and the value range of the preset driving parameters as constraints, mapping the current node to the manually driven vehicle trajectory, generating mapping point information, and performing node diffusion with a variable step length based on the mapping point information to obtain diffusion nodes until the node is connected to the end position; the initial value of the current node is the starting position;

[0071] (5) Connect each diffusion node to obtain a smooth path.

[0072] In a specific implementation of the embodiment of the present invention, the manually driven vehicle trajectory data can be collected by using a collection module; the data collection module can be a combined navigation module, and the combined navigation module is used to obtain the vehicle position, speed v i , Yaw i and curvature k iAnd other data form driving parameters.

[0073] In a specific implementation of the embodiment of the present invention, mapping the acquired starting position and end position to the manually driven vehicle trajectory to obtain the corresponding starting point mapping point and end point mapping point, and then forming the road segment trajectory, specifically includes the following steps:

[0074] According to the acquired starting point position and end point position, finding a first track point and a second track point closest to the starting point and the end point on the manually driven vehicle track;

[0075] The first track point and the second track point obtained are used as the starting point mapping point and the ending point mapping point respectively;

[0076] A road segment trajectory is formed based on the first trajectory point, the second trajectory point, and all trajectory points therebetween.

[0077] In a specific implementation of the embodiment of the present invention, the method for calculating the value range of the preset driving parameter includes the following steps:

[0078] Based on each trajectory point included in the road segment trajectory, obtaining corresponding data from the manually driven vehicle trajectory data;

[0079] Based on the acquired data, calculate the average speed v of the road section mean , maximum speed of the road section v max , minimum speed of the road section v min and the maximum curvature k of the road segment max The average speed of the road section v mean The maximum speed v of the road segment is obtained by averaging the vehicle speeds of each track point contained in the road segment track. max , minimum speed of the road section v min and the maximum curvature k of the road segment max It is obtained by finding the maximum value of each trajectory point contained in the road segment trajectory.

[0080] In a specific implementation of the embodiment of the present invention, the variable-step node diffusion based on the mapping point information includes the following steps:

[0081] Map the current node to the trajectory of the artificially driven vehicle, obtain the mapping point, and the mapping point information, the mapping point information includes the speed v i , Yaw i and curvature k i ; The mapping point is the trajectory point on the trajectory of the manually driven vehicle that is closest to the current node;

[0082] Based on the preset initial node step size d0, calculate the node diffusion step size d i, the calculation formula is:

[0083]

[0084] Set the maximum curvature k0 of the vehicle and the maximum curvature k of the road segment max , and the mapping point curvature k i and node diffusion step d i , calculate the maximum offset angle α of the diffusion node i , the calculation formula is:

[0085]

[0086] The diffusion node is obtained by diffusion of the current node, refer to Figure 2 , the diffusion information of the current node includes the offset angle and step size (i.e. the distance between the current node and the diffusion node) from the current node to the next node. The offset angle and step size determine the path curvature between the current node and the diffusion node, and the path curvature ensures the drivability of the path. The specific diffusion process is: at the beginning of planning, the input starting point is used as the current node, and the current node diffuses outward from the current node position according to the step size and offset angle to obtain multiple diffusion nodes. When entering the next cycle, the node obtained by diffusion is used as the current starting point, and diffusion is performed again until the input end point is reached; the node diffusion step size d i and the maximum deviation angle α i Corresponding to a sector area A, refer to Figure 2 , including multiple diffusion nodes in the sector area A;

[0087] Taking the road segment boundary information and the value range of each driving parameter as constraints, based on the node diffusion step length d i and the maximum deviation angle α i , performing collision detection on the diffusion nodes and deleting the diffusion nodes that have not passed; the collision detection includes: determining the position of the diffusion node, and deleting the diffusion node when the diffusion node exceeds the boundary of the road section;

[0088] Evaluate the diffusion nodes, obtain the cost of the diffusion nodes, and then obtain the diffusion node with the lowest cost as the current node for the next round of node diffusion. For details, see Figure 3 , the evaluation formula is:

[0089] g(n)=∑(w a Δ a +w yaw Δ yaw +w dis Δ dis )

[0090] In the formula, w a 、wyaw 、w dis are the weights corresponding to each evaluation item, Δ a is the angle offset between the diffusion node and the mapping node, Δ yaw is the heading offset between the diffusion node and the mapping node, Δ dis is the cost of the mapping node corresponding to the distance from the current diffusion node to the starting point.

[0091] In a specific implementation of the embodiment of the present invention, the curvature and curvature change rate of the smooth path are both continuous. Specifically, spline curve interpolation is performed on the obtained diffusion nodes to obtain a smooth path with continuous curvature and curvature change rate.

[0092] Example 2

[0093] Based on the same inventive concept as that of Example 1, an embodiment of the present invention provides a static path planning device based on human-like trajectory, comprising:

[0094] An acquisition module, used to acquire manually driven vehicle trajectory data, wherein the manually driven vehicle trajectory data includes the manually driven vehicle trajectory and driving parameters corresponding to each trajectory point on the manually driven vehicle trajectory;

[0095] A mapping module is used to map the acquired starting point and end point to the manually driven vehicle trajectory, obtain the corresponding starting point mapping point and end point mapping point, and then form a road segment trajectory; the path between the starting point and the end point is the path to be planned;

[0096] A calculation module, used to calculate the boundary information of the road section trajectory and the value range of the preset driving parameters according to the manually driven vehicle trajectory data and the road section trajectory;

[0097] A node diffusion module is used to map the current node to the trajectory of the manually driven vehicle based on the boundary information of the road segment trajectory and the value range of the preset driving parameters as constraints, generate mapping point information, and perform node diffusion with a variable step length based on the mapping point information to obtain a diffusion node until it is connected to the end position; the initial value of the current node is the starting position;

[0098] The connection module is used to connect the diffusion nodes to obtain a smooth path.

[0099] In a specific implementation of the embodiment of the present invention, the manually driven vehicle trajectory data can be collected by using a collection module; the data collection module can be a combined navigation module, and the combined navigation module is used to obtain the vehicle position, speed v i , Yaw i and curvature k i And other data form driving parameters.

[0100] In a specific implementation of the embodiment of the present invention, mapping the acquired starting position and end position to the manually driven vehicle trajectory to obtain the corresponding starting point mapping point and end point mapping point, and then forming the road segment trajectory, specifically includes the following steps:

[0101] According to the acquired starting point position and end point position, finding a first track point and a second track point closest to the starting point and the end point on the manually driven vehicle track;

[0102] The first track point and the second track point obtained are used as the starting point mapping point and the ending point mapping point respectively;

[0103] A road segment trajectory is formed based on the first trajectory point, the second trajectory point, and all trajectory points therebetween.

[0104] In a specific implementation of the embodiment of the present invention, the method for calculating the value range of the preset driving parameter includes the following steps:

[0105] Based on each trajectory point included in the road segment trajectory, obtaining corresponding data from the manually driven vehicle trajectory data;

[0106] Based on the acquired data, calculate the average speed v of the road section mean , maximum speed of the road section v max , minimum speed of the road section v min and the maximum curvature k of the road segment max The average speed of the road section v mean The maximum speed v of the road segment is obtained by averaging the vehicle speeds of each track point contained in the road segment track. max , minimum speed of the road section v min and the maximum curvature k of the road segment max It is obtained by finding the maximum value of each trajectory point contained in the road segment trajectory.

[0107] In a specific implementation of the embodiment of the present invention, the variable-step node diffusion based on the mapping point information includes the following steps:

[0108] Map the current node to the trajectory of the artificially driven vehicle, obtain the mapping point, and the mapping point information, the mapping point information includes the speed v i , Yaw i and curvature k i ; The mapping point is the trajectory point on the trajectory of the manually driven vehicle that is closest to the current node;

[0109] Based on the preset initial node step size d0, calculate the node diffusion step size d i , the calculation formula is:

[0110]

[0111] Set the maximum curvature k0 of the vehicle and the maximum curvature k of the road segment max , and the mapping point curvature k i and node diffusion step d i , calculate the maximum offset angle α of the diffusion node i , the calculation formula is:

[0112]

[0113] The diffusion node is obtained by diffusion of the current node, refer to Figure 2 , the diffusion information of the current node includes the offset angle and step size (i.e. the distance between the current node and the diffusion node) from the current node to the next node. The offset angle and step size determine the path curvature between the current node and the diffusion node, and the path curvature ensures the drivability of the path. The specific diffusion process is: at the beginning of planning, the input starting point is used as the current node, and the current node diffuses outward from the current node position according to the step size and offset angle to obtain multiple diffusion nodes. When entering the next cycle, the node obtained by diffusion is used as the current starting point, and diffusion is performed again until the input end point is reached; the node diffusion step size d i and the maximum deviation angle α i Corresponding to a sector area A, refer to Figure 2 , including multiple diffusion nodes in the sector area A;

[0114] Taking the road segment boundary information and the value range of each driving parameter as constraints, based on the node diffusion step length d i and the maximum deviation angle α i , performing collision detection on the diffusion nodes and deleting the diffusion nodes that have not passed; the collision detection includes: determining the position of the diffusion node, and deleting the diffusion node when the diffusion node exceeds the boundary of the road section;

[0115] Evaluate the diffusion nodes, obtain the cost of the diffusion nodes, and then obtain the diffusion node with the lowest cost as the current node for the next round of node diffusion. For details, see Figure 3 , the evaluation formula is:

[0116] g(n)=∑(w a Δ a +w yaw Δ yaw +w dis Δ dis )

[0117] In the formula, w a 、w yaw 、w dis are the weights corresponding to each evaluation item, Δa is the angle offset between the diffusion node and the mapping node, Δ yaw is the heading offset between the diffusion node and the mapping node, Δ dis is the cost of the mapping node corresponding to the distance from the current diffusion node to the starting point.

[0118] In a specific implementation of the embodiment of the present invention, the curvature and curvature change rate of the smooth path are both continuous. Specifically, spline curve interpolation is performed on the obtained diffusion nodes to obtain a smooth path with continuous curvature and curvature change rate.

[0119] Example 3

[0120] Based on the same inventive concept as in Example 1, an embodiment of the present invention provides a static path planning system based on human-like trajectory, including a storage medium and a processor;

[0121] The storage medium is used to store instructions;

[0122] The processor is used to operate according to the instructions to execute the steps of the method according to any one of Embodiment 1.

[0123] Example 4

[0124] Based on the same inventive concept as in Example 1, a static path planning system based on human-like trajectory is provided in the embodiment of the present invention. Figure 1 As shown, including:

[0125] A data acquisition module, used to collect manually driven vehicle trajectory data, wherein the manually driven vehicle trajectory data includes the manually driven vehicle trajectory and driving parameters corresponding to each trajectory point on the manually driven vehicle trajectory;

[0126] A mapping information calculation module is used to map the acquired starting point position and end point position to the manually driven vehicle trajectory, obtain the corresponding starting point mapping point and end point mapping point, and then form a section trajectory, and calculate the value range of the preset driving parameter according to the manually driven vehicle trajectory data and the section trajectory; the path between the starting point and the end point is the path to be planned;

[0127] The human-like parameter range definition module uses the preset driving parameter value range of the mapping information calculation module as a reference, receives the starting point position, takes the starting point as the initial current node, and calculates the current node diffusion range information;

[0128] The path planning module maps the acquired starting point position and end point position to the manually driven vehicle trajectory, obtains the corresponding starting point mapping point and end point mapping point, and then forms a section trajectory, and calculates the boundary information of the section trajectory according to the manually driven vehicle trajectory data and the section trajectory; maps the current node to the manually driven vehicle trajectory with the boundary information of the section trajectory and the value range of the preset driving parameters as constraints, generates mapping point information, and performs node diffusion with variable step length based on the mapping point information to obtain diffusion nodes until it is connected to the end point position; the initial value of the current node is the starting point position; and each diffusion node is connected to obtain a smooth path.

[0129] In a specific implementation of the embodiment of the present invention, the manually driven vehicle trajectory data can be collected by using a collection module; the data collection module can be a combined navigation module, and the combined navigation module is used to obtain the vehicle position, speed v i , Yaw i and curvature k i And other data form driving parameters.

[0130] In a specific implementation of the embodiment of the present invention, mapping the acquired starting position and end position to the manually driven vehicle trajectory to obtain the corresponding starting point mapping point and end point mapping point, and then forming the road segment trajectory, specifically includes the following steps:

[0131] According to the acquired starting point position and end point position, finding a first track point and a second track point closest to the starting point and the end point on the manually driven vehicle track;

[0132] The first track point and the second track point obtained are used as the starting point mapping point and the ending point mapping point respectively;

[0133] A road segment trajectory is formed based on the first trajectory point, the second trajectory point, and all trajectory points therebetween.

[0134] In a specific implementation of the embodiment of the present invention, the method for calculating the value range of the preset driving parameter includes the following steps:

[0135] Based on each trajectory point included in the road segment trajectory, obtaining corresponding data from the manually driven vehicle trajectory data;

[0136] Based on the acquired data, calculate the average speed v of the road section mean , maximum speed of the road section v max , minimum speed of the road section v min and the maximum curvature k of the road segment max The average speed of the road section v mean The maximum speed v of the road segment is obtained by averaging the vehicle speeds of each track point contained in the road segment track.max , minimum speed of the road section v min and the maximum curvature k of the road segment max It is obtained by finding the maximum value of each trajectory point contained in the road segment trajectory.

[0137] In a specific implementation of the embodiment of the present invention, the variable-step node diffusion based on the mapping point information includes the following steps:

[0138] Map the current node to the trajectory of the artificially driven vehicle, obtain the mapping point, and the mapping point information, the mapping point information includes the speed v i , Yaw i and curvature k i ; The mapping point is the trajectory point on the trajectory of the manually driven vehicle that is closest to the current node;

[0139] Based on the preset initial node step size d0, calculate the node diffusion step size d i , the calculation formula is:

[0140]

[0141] Set the maximum curvature k0 of the vehicle based on the maximum curvature k of the road segment max , and the mapping point curvature k i and node diffusion step d i , calculate the maximum offset angle α of the diffusion node i , the calculation formula is:

[0142]

[0143] The diffusion node is obtained by diffusion of the current node, refer to Figure 2 , the diffusion information of the current node includes the offset angle and step size (i.e. the distance between the current node and the diffusion node) from the current node to the next node. The offset angle and step size determine the path curvature between the current node and the diffusion node, and the path curvature ensures the drivability of the path. The specific diffusion process is: at the beginning of planning, the input starting point is used as the current node, and the current node diffuses outward from the current node position according to the step size and offset angle to obtain multiple diffusion nodes. When entering the next cycle, the node obtained by diffusion is used as the current starting point, and diffusion is performed again until the input end point is reached; the node diffusion step size d i and the maximum deviation angle α i Corresponding to a sector area A, refer to Figure 2 , including multiple diffusion nodes in the sector area A;

[0144] Taking the road segment boundary information and the value range of each driving parameter as constraints, based on the node diffusion step length di and the maximum deviation angle α i , performing collision detection on the diffusion nodes and deleting the diffusion nodes that have not passed; the collision detection includes: determining the position of the diffusion node, and deleting the diffusion node when the diffusion node exceeds the boundary of the road section;

[0145] Evaluate the diffusion nodes, obtain the cost of the diffusion nodes, and then obtain the node with the lowest cost as the current node for the next round of node diffusion. For details, see Figure 3 , the evaluation formula is:

[0146] g(n)=∑(w a Δ a +w yaw Δ yaw +w dis Δ dis )

[0147] In the formula, w a 、w yaw 、w dis are the weights corresponding to each evaluation item, Δ a is the angle offset between the diffusion node and the mapping node, Δ yaw is the heading offset between the diffusion node and the mapping node, Δ dis is the cost of the mapping node corresponding to the distance from the current diffusion node to the starting point.

[0148] In a specific implementation of the embodiment of the present invention, the curvature and curvature change rate of the smooth path are both continuous. Specifically, spline curve interpolation is performed on the obtained diffusion nodes to obtain a smooth path with continuous curvature and curvature change rate.

[0149] The working process of the system in the embodiment of the present invention is described in detail below in conjunction with a specific implementation manner.

[0150] S1: Using a data acquisition module to collect trajectory data of a manually driven vehicle; the data acquisition module may be a combined navigation module to obtain the vehicle position, speed, heading and curvature, and after the acquisition is completed, the data is processed and uploaded;

[0151] S2: Mapping information calculation module, receiving the input starting point and end point positions, calculating and obtaining the first trajectory point and the second trajectory point corresponding to the starting point and the end point on the manual driving trajectory, as well as the vehicle position and speed v of each trajectory point in the trajectory segment where the first trajectory point and the second trajectory point are located i , Yaw i and curvature v i , and further calculate the average speed v of the road section mean , maximum speed v max , minimum speed v min , maximum curvature kmax etc. human-like mapping information, and output it to the human-like parameter range definition module;

[0152] S3: The humanoid parameter range definition module uses the humanoid mapping information output by the mapping information calculation module as a reference, receives the current node (i.e., the starting point of each round of node diffusion) sent by the path planning module, and calculates the diffusion information of the current node. The specific implementation process is as follows:

[0153] S31: Map the current node to the artificial trajectory and obtain the mapping point speed v i , Yaw i and curvature v i Information; the initial value of the current node is the input starting point;

[0154] S32: Set the reference node step d0 and the node diffusion step d i With the velocity v of the mapping point i The conversion relationship is:

[0155]

[0156] S33: Set the maximum curvature k0 of the vehicle and the maximum offset angle α of the diffusion node i and the curvature k of the mapping point i The relationship between them is:

[0157]

[0158] S34: Send the mapping point information and the diffusion information of the current node to the path planning module.

[0159] S4: The path planning module calculates the path based on the input parameters and returns the result. The specific implementation process is as follows:

[0160] S41: input the starting point and the end point to obtain the road section boundary information; the road section boundary information is the relevant information of the left and right boundaries, which is known information;

[0161] S42: Call the human-like parameter range definition module to obtain the current node diffusion step d i and the maximum deviation angle α i ; The current node is diffused according to the diffusion step and the offset angle to obtain multiple diffusion nodes, corresponding to the distance and heading angle difference between each diffusion node and the current node. The diffusion step and the maximum offset angle correspond to a sector, refer to Figure 2 , contains multiple diffusion nodes in the fan-shaped area;

[0162] S43: Based on the road segment boundary information, collision detection is performed on the diffusion nodes, and the diffusion nodes that have not passed are deleted; the current node diffuses multiple nodes within the diffusion angle interval, and the reference Figure 2 , each node that diffuses out is a diffusion node;

[0163] S44: Evaluate the diffusion node n. The cost f(n) of the diffusion node n is composed of the cost g(n) of the diffusion node n from the starting point and the estimated cost h(n) of the diffusion node n from the end point:

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

[0165] Among them, h(n) is evaluated by the Euclidean distance, and g(n) is calculated by the angular offset Δ between the diffusion node and the mapping node. α , the heading offset Δ between the diffusion node and the mapping node yaw , Δ dis is the cost of the mapping node corresponding to the current diffusion node from the starting point, that is,

[0166] g(n)=∑(w a Δ a +w yaw Δ yaw +w dis Δ dis )

[0167] where w α , w yaw , w dis is the weight corresponding to the evaluation item.

[0168] S45: Reference Figure 3 , loop through S42 to S44, and perform node diffusion with variable step length until it is connected to the end point.

[0169] S46: interpolating the node spline curve obtained by planning to obtain a smooth path with continuous curvature and curvature change rate.

[0170] The above shows and describes the basic principles and main features of the present invention and the advantages of the present invention. It should be understood by those skilled in the art that the present invention is not limited to the above embodiments. The above embodiments and descriptions are only for explaining the principles of the present invention. Without departing from the spirit and scope of the present invention, the present invention may have various changes and improvements, which fall within the scope of the present invention to be protected. The scope of protection of the present invention is defined by the attached claims and their equivalents.

Claims

1. A static path planning method based on human-like trajectory, characterized in that: include: Acquire manually driven vehicle trajectory data, wherein the manually driven vehicle trajectory data includes the manually driven vehicle trajectory and driving parameters corresponding to each trajectory point on the manually driven vehicle trajectory; Mapping the acquired starting point and end point positions to the manually driven vehicle trajectory to obtain corresponding starting point mapping points and end point mapping points, thereby forming a road segment trajectory; Calculating the boundary information of the road section trajectory and the value range of the preset driving parameters according to the manually driven vehicle trajectory data and the road section trajectory; Map the current node to the trajectory of the manually driven vehicle to generate mapping point information, and use the boundary information of the road segment trajectory and the value range of the preset driving parameters as constraints to perform node diffusion with a variable step length based on the mapping point information to obtain diffusion nodes until they are connected to the end position; the initial value of the current node is the starting position; Connect the diffusion nodes that meet the requirements to obtain a smooth path; The variable-step node diffusion based on the mapping point information comprises the following steps: Based on the preset initial node step size d0, calculate the node diffusion step size d i : Based on the preset maximum curvature k0 of the vehicle and the maximum curvature k of the road section max , mapping point curvature k i and node diffusion step d i , calculate the maximum offset angle α of the diffusion node i : Taking the road segment boundary information and the value range of each driving parameter as constraints, based on the node diffusion step length d i and the maximum deviation angle α i , perform collision detection on diffusion nodes and delete the diffusion nodes that fail; Evaluate the diffusion nodes, obtain the cost of the diffusion nodes, and then obtain the diffusion node with the lowest cost as the current node for the next round of node diffusion. The evaluation formula is: g(n)=Σ(w a D a +w yaw D yaw +w dis D dis ) In the formula, w a 、w yaw 、w dis are the weights corresponding to each evaluation item, Δ a is the angle offset between the diffusion node and the mapping node, Δ yaw is the heading offset between the diffusion node and the mapping node, Δ dis The cost of the mapping point from the current diffusion node to the starting point.

2. The static path planning method based on human-like trajectory according to claim 1, characterized in that: The driving parameters include vehicle position, speed, heading and curvature.

3. The static path planning method based on human-like trajectory according to claim 2, characterized in that: The calculation method of the value range of each driving parameter includes: Based on each trajectory point included in the road segment trajectory, obtaining corresponding driving parameters from the manually driven vehicle trajectory data; Based on the acquired driving parameters, the average speed of the road section, the maximum speed of the road section, the minimum speed of the road section and the maximum curvature of the road section are calculated.

4. The static path planning method based on human-like trajectory according to claim 1, characterized in that: The curvature and curvature change rate of the smooth path are both continuous.

5. A static path planning device based on human-like trajectory, characterized in that: include: An acquisition module, used to acquire manually driven vehicle trajectory data, wherein the manually driven vehicle trajectory data includes the manually driven vehicle trajectory and driving parameters corresponding to each trajectory point on the manually driven vehicle trajectory; A mapping module is used to map the acquired starting position and end position to the manually driven vehicle trajectory, obtain the corresponding starting point mapping point and end point mapping point, and then form a road section trajectory; A calculation module, used to calculate the boundary information of the road section trajectory and the value range of the preset driving parameters according to the manually driven vehicle trajectory data and the road section trajectory; A node diffusion module is used to map the current node to the trajectory of the manually driven vehicle, generate mapping point information, and perform node diffusion with a variable step length based on the mapping point information with the boundary information of the road segment trajectory and the value range of the preset driving parameters as constraints to obtain diffusion nodes until the node is connected to the end position; the initial value of the current node is the starting position; A connection module is used to connect each diffusion node to obtain a smooth path; The variable-step node diffusion based on the mapping point information comprises the following steps: Based on the preset initial node step size d0, calculate the node diffusion step size d i : Based on the preset maximum curvature k0 of the vehicle and the maximum curvature k of the road section max , mapping point curvature k i and node diffusion step d i , calculate the maximum offset angle α of the diffusion node i : Taking the road segment boundary information and the value range of each driving parameter as constraints, based on the node diffusion step length d i and the maximum deviation angle α i , perform collision detection on diffusion nodes and delete the diffusion nodes that fail; Evaluate the diffusion nodes, obtain the cost of the diffusion nodes, and then obtain the diffusion node with the lowest cost as the current node for the next round of node diffusion. The evaluation formula is: g(n)=Σ(w a D a +w yaw D yaw +w dis D dis ) In the formula, w a 、w yaw 、w dis are the weights corresponding to each evaluation item, Δ a is the angle offset between the diffusion node and the mapping node, Δ yaw is the heading offset between the diffusion node and the mapping node, Δ dis The cost of the mapping point from the current diffusion node to the starting point.

6. A static path planning system based on human-like trajectory, characterized in that: including storage media and processors; The storage medium is used to store instructions; The processor is configured to operate according to the instructions to execute the steps of the method according to any one of claims 1 to 4.

7. A static path planning system based on human-like trajectory, characterized in that: include: A data acquisition module, used to collect manually driven vehicle trajectory data, wherein the manually driven vehicle trajectory data includes the manually driven vehicle trajectory and driving parameters corresponding to each trajectory point on the manually driven vehicle trajectory; A mapping information calculation module is used to map the acquired starting position and end position to the manually driven vehicle trajectory, obtain the corresponding starting point mapping point and end point mapping point, and then form a section trajectory, and calculate the value range of the preset driving parameter according to the manually driven vehicle trajectory data and the section trajectory; The human-like parameter range definition module uses the preset driving parameter value range of the mapping information calculation module as a reference, receives the starting point position, takes the starting point as the initial current node, and calculates the current node diffusion range information; The path planning module maps the acquired starting position and end position to the manually driven vehicle trajectory, obtains the corresponding starting point mapping point and end point mapping point, and then forms a section trajectory, and calculates the boundary information of the section trajectory according to the manually driven vehicle trajectory data and the section trajectory; maps the current node to the manually driven vehicle trajectory, generates mapping point information, and performs node diffusion with variable step length based on the mapping point information with the boundary information of the section trajectory and the value range of the preset driving parameters as constraints to obtain diffusion nodes until it is connected to the end position; the initial value of the current node is the starting position; Connect each diffusion node to obtain a smooth path; The variable-step node diffusion based on the mapping point information comprises the following steps: Based on the preset initial node step size d0, calculate the node diffusion step size d i : Based on the preset maximum curvature k0 of the vehicle and the maximum curvature k of the road section max , mapping point curvature k i and node diffusion step d i , calculate the maximum offset angle α of the diffusion node i : Taking the road segment boundary information and the value range of each driving parameter as constraints, based on the node diffusion step length d i and the maximum deviation angle α i , perform collision detection on diffusion nodes and delete the diffusion nodes that fail; Evaluate the diffusion nodes, obtain the cost of the diffusion nodes, and then obtain the diffusion node with the lowest cost as the current node for the next round of node diffusion. The evaluation formula is: g(n)=∑(w a D a +w yaw D yaw +w dis D dis ) In the formula, w a 、w yaw 、w dis are the weights corresponding to each evaluation item, Δ a is the angle offset between the diffusion node and the mapping node, Δ yaw is the heading offset between the diffusion node and the mapping node, Δ dis The cost of the mapping point from the current diffusion node to the starting point.

8. The static path planning system based on human-like trajectory according to claim 7, characterized in that: The driving parameters include vehicle position, speed, heading and curvature.

9. The static path planning system based on human-like trajectory according to claim 7, characterized in that: The calculation method of the value range of each driving parameter includes: Taking the road segment boundary information as a constraint, obtaining corresponding data from the manually driven vehicle trajectory data; Based on the acquired data, the average speed of the road section, the maximum speed of the road section, the minimum speed of the road section and the maximum curvature of the road section are calculated.

10. The static path planning system based on human-like trajectory according to claim 7, characterized in that: The curvature and curvature change rate of the smooth path are both continuous.

Images