Path planning method based on improved bidirectional A star search algorithm

Abstract

The invention relates to a path planning method based on an improved bidirectional A star search algorithm, relates to the technical field of intelligent vehicles, and solves the problem of low search efficiency caused by bloated search nodes or special topographic reasons in an existing path search scheme. The path planning method based on the improved bidirectional A star search algorithm comprises the following steps: acquiring an obstacle grid map of a target scene; determining a motion path from a starting point position to a target position in the barrier grid map based on an optimized bidirectional A * search algorithm; wherein in the optimized bidirectional A * search algorithm, the forward heuristic function value of the child node is a heuristic function value from the child node to the child node with the minimum reverse total cost at present, and the reverse heuristic function value of the child node is a heuristic function value from the child node to the child node with the minimum forward total cost at present.

Description

Technical Field

[0001] The present invention relates to the technical field of intelligent vehicles, and in particular to a path planning method based on an improved bidirectional A-star search algorithm. Background Art

[0002] Path planning refers to obtaining a path point connecting the starting position to the target position through searching and other methods based on known environmental information. The A* search algorithm is a heuristic search algorithm used to find the shortest path between two points in a graph or grid. The A* search algorithm uses the heuristic function f(n) = g(n) + h(n) to determine the priority of each child node and the next expansion direction, where g(n) represents the cumulative moving cost from the starting point to the current position, and h(n) represents the heuristic function value (referred to as "heuristic value") from the current position to the target position. The A* search algorithm combines the ideas of the shortest path search and the best-first search of the Dijkstra algorithm. In many cases, it is more efficient than the simple greedy search or Dijkstra algorithm, and is widely used in path planning and other fields. Although the A* search algorithm can improve the shortcomings of brute force search to a certain extent, a large number of invalid search nodes are often generated during the search process due to obstacles and other reasons, thereby affecting the space and time efficiency of the algorithm.

[0003] The bidirectional A* search algorithm is a heuristic search algorithm used to solve search problems. It is an improvement of the A* search algorithm, which aims to reduce the number of nodes to be searched, thereby improving the efficiency of the search. The bidirectional A* search algorithm starts searching from the starting point and the end point at the same time, and uses heuristic functions to evaluate the priority of the nodes respectively. In each step, the node with the lowest priority is selected from both directions for expansion. When the search paths of the two search directions intersect, a path from the starting point to the end point is found. However, due to the bloated search nodes in some locations during the search process of the bidirectional A* search algorithm, the search efficiency is reduced, and the intermediate nodes cannot be searched due to the terrain, and finally it becomes a one-way search to generate a path to the corresponding location. Since the bidirectional A* search algorithm searches alternately in the forward and reverse directions during the search, when the intermediate nodes cannot be searched due to the terrain, the solution time of the bidirectional A* search algorithm is longer than that of the one-way A* search algorithm.

[0004] Slip and sliding are the phenomena in which the grounded part of a vehicle moves relative to the ground during driving. Slip and sliding (collectively referred to as "slip") are parameters that are widely present in the driving process of vehicles and are inevitable, and have an important impact on the driving of vehicles. After a long period of research and development, the field of path planning has become relatively mature, and a variety of frameworks and methods have been formed. However, among these methods, there are few planning methods for tracked platforms suitable for off-road scenarios, and even fewer studies involve the impact of sliding parameters on path planning. However, in off-road scenarios, tracked vehicles are more obviously affected by sliding parameters. The existing path planning schemes do not fully utilize the guiding role of sliding parameters in path selection and generation, which leads to the unsatisfactory performance of the planned path results in terms of sliding parameters, reduced trajectory quality, and insufficient safety. How to reasonably plan the driving path based on sliding parameters and improve the safety of unmanned driving is a problem that needs to be solved urgently. Summary of the invention

[0005] In view of the above analysis, an embodiment of the present invention aims to provide a path planning method based on an improved bidirectional A-star search algorithm to solve the problem of reduced search efficiency in existing path search schemes due to bloated search nodes or special terrain reasons.

[0006] In a first aspect, an embodiment of the present invention provides a path planning method based on an improved bidirectional A-star search algorithm, comprising the following steps:

[0007] Obtain the obstacle grid map of the target scene;

[0008] Determine the motion path from the starting position to the target position in the obstacle grid map based on the optimized bidirectional A* search algorithm; wherein,

[0009] In the optimized bidirectional A* search algorithm, the total forward cost of a child node includes the forward movement cost of the child node and its weight and the forward heuristic function value of the child node and its weight, and the total reverse cost of a child node includes the reverse movement cost of the child node and its weight and the reverse heuristic function value of the child node and its weight; wherein,

[0010] The forward heuristic function value of the child node is the heuristic function value from the child node to the child node currently having the minimum reverse total cost, and the reverse heuristic function value of the child node is the heuristic function value from the child node to the child node currently having the minimum forward total cost.

[0011] Based on the further improvement of the above path planning method, the planning method further includes the following steps:

[0012] Get the sliding parameter prediction grid map of the target scene; where:

[0013] The forward movement cost of the child node includes the forward movement cost of the current parent node, the distance cost between the current parent node and the child node and its weight, and the estimated value of the vehicle sliding parameter at the child node position and its weight; the reverse movement cost of the child node includes the reverse movement cost of the current parent node, the distance cost between the current parent node and the child node and its weight, and the estimated value of the vehicle sliding parameter at the child node position and its weight; wherein,

[0014] The estimated value of the vehicle sliding parameter at the subnode position is obtained by indexing the sliding parameter prediction grid map.

[0015] Based on the further improvement of the above path planning method, the planning method further includes the following steps:

[0016] The motion path is subjected to a first round of smoothing by a gradient descent method, wherein in the gradient descent method, an energy function for determining an optimization direction includes a path curvature.

[0017] Based on the further improvement of the above path planning method, the planning method further includes the following steps:

[0018] A spline interpolation method is used to perform a second round of smoothing on the motion path after the first round of smoothing.

[0019] Based on the further improvement of the above path planning method, the first round of smoothing the motion path by gradient descent method includes the following steps:

[0020] Step 110: In each iteration update of the motion path, the following steps are performed respectively:

[0021] Step 10: determining a candidate node set of the node to be adjusted according to adjacent nodes of the node to be adjusted in the motion path, wherein the node to be adjusted does not include a starting position and a target position;

[0022] Step 20: Select a node with the minimum energy function value from the candidate node set of the node to be adjusted to replace the node to be adjusted, and calculate the sum of the energy function values ​​of all replaced nodes;

[0023] Step 120: In response to the total number of iterative updates exceeding a first threshold or the improvement difference of the sum of the energy function values ​​calculated after a single iterative update being less than a second threshold, stopping the iterative update.

[0024] Based on the further improvement of the above path planning method, the spline interpolation method is a cubic spline interpolation method.

[0025] Based on a further improvement of the above path planning method, the path curvature is determined by the accumulated distances from the nodes in the candidate node set of the node to be adjusted to the node to be adjusted.

[0026] Based on further improvement of the above path planning method, the energy function also includes obstacle distance cost and its weight, wherein the obstacle distance cost is determined by the distance from the node in the candidate node set of the node to be adjusted to the obstacle.

[0027] Based on the further improvement of the above path planning method, the energy function also includes a sliding parameter cost, wherein the sliding parameter cost is determined by the estimated value of the vehicle sliding parameter at the node position in the candidate node set of the node to be adjusted.

[0028] Based on the further improvement of the above path planning method, the sliding parameter prediction grid map is determined by the following method:

[0029] Obtain point cloud data, image data and vehicle trajectory data of the target scene;

[0030] Projecting the point cloud data of the target scene into a two-channel image containing height information and intensity information;

[0031] Segmenting the two-channel image and image data of the target scene according to the geometric size of the vehicle and the grid resolution to obtain the two-channel image and image data of the target scene in a grid format;

[0032] Encoding the two-channel image of the target scene in a raster format and the raster representing the same position in the image data;

[0033] Inputting the two-channel image of the target scene in raster format, the image data and the trajectory data of the vehicle into a pre-trained sliding parameter estimation model to obtain an estimation result of the sliding parameter;

[0034] The estimated result of the sliding parameter is restored to the grid at the corresponding position based on the grid encoding to generate a sliding parameter prediction grid map of the target scene; wherein,

[0035] The sliding parameter estimation model includes a feature extraction network, a feature fusion network, a sliding parameter identification network, and a linear layer connected in sequence;

[0036] The feature extraction network includes a first residual network for extracting point cloud features, a second residual network for extracting image features, and a trajectory feature extraction network;

[0037] The feature fusion network includes a first fusion network for fusing point cloud features and trajectory features and a second fusion network for fusing image features and trajectory features;

[0038] The sliding parameter recognition network includes a first convolutional long short-term memory network for learning sliding parameters for fused point cloud features and trajectory features and a second convolutional long short-term memory network for learning sliding parameters for fused image features and trajectory features.

[0039] Compared with the prior art, the present invention can achieve at least one of the following beneficial effects:

[0040] 1. The scheme of the present invention updates the heuristic item calculation of a single forward search in the bidirectional A* search algorithm from the end point to the node with the lowest total cost of the current reverse search, and updates the heuristic item calculation of the reverse search from the starting point to the node with the lowest total cost of the forward search, thereby preventing the phenomenon that the distance is very close but it takes a long time to update during the search process, thereby improving the search efficiency.

[0041] 2. Aiming at the problem that the sliding parameters are ignored in the motion planning process of off-road scenes, resulting in insufficient vehicle driving safety, the solution of the present invention proposes an optimized two-way A* search algorithm using a multi-layer map. The optimized two-way A* search and reference line smoothing are performed on a multi-layer map including an obstacle layer, a height layer, and a sliding parameter layer to achieve avoidance of high sliding parameter risk areas in off-road scenes, improve the timeliness and safety of existing planning methods, and reduce the difficulty of trajectory tracking.

[0042] In the present invention, the above-mentioned technical solutions can also be combined with each other to achieve more preferred combination solutions. Other features and advantages of the present invention will be described in the subsequent description, and some advantages can become obvious from the description, or can be understood by practicing the present invention. The purpose and other advantages of the present invention can be realized and obtained through the contents particularly pointed out in the description and the drawings. BRIEF DESCRIPTION OF THE DRAWINGS

[0043] The accompanying drawings are only used for the purpose of illustrating specific embodiments and are not to be considered as limiting the present invention. In the entire drawings, the same reference symbols represent the same components;

[0044] Figure 1 A schematic flow chart of a path planning method based on an improved bidirectional A-star search algorithm according to an embodiment of the present invention is shown.

[0045] Figure 2 A schematic diagram of a process for generating an obstacle grid map according to an embodiment of the present invention is shown.

[0046] Figure 3 A conceptual diagram showing an intermediate moment of an optimized bidirectional A* search algorithm according to an embodiment of the present invention is shown.

[0047] Figure 4A schematic diagram comparing search results of a traditional bidirectional A* search algorithm and an optimized bidirectional A* search algorithm in this embodiment is shown.

[0048] Figure 5 A schematic flow chart of a method for estimating vehicle sliding parameters according to an embodiment of the present invention is shown.

[0049] Figure 6 A schematic diagram of acquiring point cloud data of a target scene according to an embodiment of the present invention is shown.

[0050] Figure 7 A schematic diagram showing the temporal synchronization of point cloud data, image data and vehicle trajectory data in an embodiment of the present invention is shown.

[0051] Figure 8 A schematic diagram of a network framework of a sliding parameter estimation model according to an embodiment of the present invention is shown.

[0052] Fig. 9 A schematic diagram of the network structure of a sliding parameter estimation model according to another embodiment of the present invention is shown.

[0053] Fig.10 A schematic diagram of a point cloud projection image according to an embodiment of the present invention is shown.

[0054] Fig.11 A schematic diagram of a generation process of a sliding parameter map according to an embodiment of the present invention is shown.

[0055] Fig.12 An example of performing node expansion in an obstacle grid map according to an embodiment of the present invention is shown.

[0056] Fig.13 An example of using the gradient descent method to smooth a motion path according to an embodiment of the present invention is shown. DETAILED DESCRIPTION

[0057] The preferred embodiments of the present invention are described in detail below in conjunction with the accompanying drawings, wherein the accompanying drawings constitute a part of this application and are used together with the embodiments of the present invention to illustrate the principles of the present invention, but are not used to limit the scope of the present invention.

[0058] Figure 1 FIG. 2 is a flow chart of a path planning method based on an improved bidirectional A-star search algorithm according to an embodiment of the present invention. Figure 1 As shown, the path planning method based on the improved bidirectional A-star search algorithm includes the following steps:

[0059] Step 100: Obtain an obstacle grid map of the target scene.

[0060] In this embodiment, the vehicle's own sensors can be used to collect environmental information around the vehicle. Specifically, a laser radar can be used to collect a point cloud map of the vehicle's surroundings.

[0061] In this embodiment, obstacle detection can be performed on the point cloud data of the target scene to obtain an obstacle map. Afterwards, the obstacle map is segmented according to the grid resolution to obtain an obstacle grid map. In this embodiment, the point cloud data of the target scene refers to the point cloud data located in the target area in the point cloud map collected by the vehicle's own laser radar. For example, the target area can be a block area located at a certain distance in front of the vehicle.

[0062] In some embodiments, the obstacle grid map in step 100 is determined by the following method:

[0063] Step S1001: determining a basic obstacle map of a target scene based on an obstacle detection algorithm, wherein the format of the basic obstacle map is a grid.

[0064] Step S1002: expanding the obstacles in the basic obstacle map according to the geometric size of the vehicle and filling the discrete areas in the basic obstacle map to obtain an obstacle grid map.

[0065] Figure 2 FIG. 2 shows a schematic diagram of a process for generating an obstacle grid map according to an embodiment of the present invention. Figure 2 As shown, first, point cloud data of the scene can be generated; then, a basic obstacle map is obtained by performing an obstacle detection algorithm on the point cloud data; then, obstacle expansion processing is performed on the basic obstacle map; finally, the blank areas between discrete obstacles are filled to expand the area of ​​the obstacles and reduce the risk of collision. In this embodiment, the obstacle expansion processing can be to enlarge the obstacles according to a certain ratio. In this embodiment, the selection of discrete areas can be determined by the geometric size of the vehicle itself.

[0066] Step 200: Determine a motion path from a starting position to a target position in the obstacle grid map based on an optimized bidirectional A* search algorithm.

[0067] When the traditional one-way A* search algorithm performs path search on a flat grid map, it first puts the starting point into the open set, then traverses and calculates the sum of the movement cost and the heuristic value of the adjacent nodes; in each iteration, it selects the node with the smallest total cost in the open set, generates child nodes for it and adds its parent node to the closed set. If the generated child node collides with an obstacle or is already in the closed set, the child node is skipped; if the generated child node is already in the open set, it chooses whether to update the sum of its movement cost and heuristic value based on whether the current generation method has a better path; if the generated child node is the target position, it is added to the closed set, the algorithm ends, and a target path can be generated by tracing back to the starting point along the parent node pointer.

[0068] The traditional bidirectional A* search algorithm refers to the search phase in which both ends (i.e., the starting position and the target position) start to search alternately in the middle, and the two ends maintain their own open sets until they meet at a node in the middle. The specific algorithm flow is as follows: forward A* search and reverse A* search are performed at the starting position and the target position respectively. The end point of the forward A* search is the target position, and the end point of the reverse A* search is the starting position. Each end initializes its own open set and closed set; then alternately generate and select child nodes for the forward search and the reverse search, update their respective open sets and closed sets, record the nodes that have been searched, update the sum of the node costs, and set the parent-child relationship between the corresponding nodes; then, through continuous iteration until there are common elements in the two open sets, they are used as the meeting nodes and added to the closed set of the forward search, the parent-child relationship of the nodes in the reverse search is reversed, and the two closed sets are spliced ​​to obtain the planned path. If one of the open sets is empty, it means that there is no feasible path.

[0069] In this embodiment, an optimized bidirectional A* search algorithm is used, which is specifically reflected in the change of the end point in the forward A* search and the change of the end point in the reverse A* search. In the optimized bidirectional A* search algorithm, the forward total cost of the child node includes the forward movement cost of the child node and its weight and the forward heuristic function value of the child node and its weight, and the reverse total cost of the child node includes the reverse movement cost of the child node and its weight and the reverse heuristic function value of the child node and its weight; the forward heuristic function value of the child node is the heuristic function value from the child node to the child node with the minimum reverse total cost at present, and the reverse heuristic function value of the child node is the heuristic function value from the child node to the child node with the minimum forward total cost at present.

[0070] The optimized bidirectional A* search algorithm in this embodiment is described below at a certain moment in the middle:

[0071] Figure 3FIG. 4 is a conceptual diagram showing an intermediate moment of an optimized bidirectional A* search algorithm according to an embodiment of the present invention. Figure 3 As shown, the optimized bidirectional A* search algorithm in this embodiment needs to find a navigable path from the starting position to the target position on the map. The starting point of the forward A* search is the "starting position" in the figure, and the starting point of the reverse A* search is the "target position" in the figure. Figure 3 In the figure, the gray circle and the yellow circle represent all the nodes searched during the forward A* search from the start time to the current time, and the blue circle and the brown circle represent all the nodes searched during the reverse A* search from the start time to the current time. The yellow line segment represents the traversable path formed during the forward A* search at the current time, and the brown line segment represents the traversable path formed during the reverse A* search at the current time. The yellow circle also represents the latest node on the path formed during the forward A* search at the current time, and the brown circle represents the latest node on the path formed during the reverse A* search at the current time. At the current time, the end point of the forward A* search is the node represented by the brown circle, and the end point of the reverse A* search is the node represented by the yellow circle. Figure 3 It can be seen that in this embodiment, the end point of the forward A* search is changed from the traditional target position to the node with the lowest total cost in the current reverse A* search (i.e., the node represented by the brown circle), and the end point of the reverse A* search is changed from the traditional starting point position to the node with the lowest total cost in the current forward A* search (i.e., the node represented by the yellow circle).

[0072] Figure 4 FIG. 4 shows a schematic diagram comparing the search results of a conventional bidirectional A* search algorithm and an optimized bidirectional A* search algorithm in this embodiment. Figure 4 As shown, the traditional bidirectional A* search algorithm needs to search and update nodes 171 times before they can meet (each small circle represents an updated node), while the optimized bidirectional A* search algorithm in this embodiment only needs 40 node updates to find the meeting position. The optimized bidirectional A* search algorithm in this embodiment reduces the search time from the original 4.98ms to 1.02ms, thereby significantly improving the search efficiency.

[0073] In some embodiments, the optimized bidirectional A* search algorithm also includes optimization of the A* search algorithm, which is specifically embodied in the modification of the cost function in the A* search algorithm. That is, the optimized bidirectional A* search algorithm in this embodiment also includes the modification of the cost function in the forward A* search algorithm and the modification of the cost function in the reverse A* search algorithm. Since the modification method of the cost function in the forward A* search algorithm and the modification method of the cost function in the reverse A* search algorithm are the same in this embodiment, they are uniformly represented here as the optimized A* search algorithm.

[0074] In this embodiment, an optimized A* search algorithm is used, which is specifically reflected in the modification of the cost function g(n). In the optimized A* search algorithm, the sum of the costs of the child nodes includes the movement cost of the child nodes and its weight and the heuristic function value of the child nodes and its weight, wherein the movement cost of the child nodes includes the movement cost of the current parent node, the distance cost between the current parent node and the child node and its weight, and the estimated value of the vehicle sliding parameter at the child node position and its weight. In this embodiment, the estimated value of the vehicle sliding parameter at the child node position is obtained by indexing the sliding parameter prediction grid map.

[0075] In this embodiment, each grid in the sliding parameter prediction grid map records the estimated value of the vehicle sliding parameter at the grid position. The estimated value of the vehicle sliding parameter can be between 0 and 1. When the estimated value of the vehicle sliding parameter is 0, the vehicle does not slip or spin; when the estimated value of the vehicle sliding parameter is 1, the vehicle is in a pure sliding state or a pure sliding state.

[0076] The following is a description of the process of generating the sliding parameter prediction grid map of the target scene in this embodiment.

[0077] Figure 5 FIG. 2 shows a flow chart of a method for estimating vehicle sliding parameters according to an embodiment of the present invention. Figure 5 As shown, the method for estimating the vehicle sliding parameters includes the following steps:

[0078] Step S201: Acquire point cloud data, image data of the target scene and trajectory data of the vehicle.

[0079] In this embodiment, the vehicle's own sensors can be used to collect environmental information around the vehicle and the vehicle's trajectory data. Specifically, sensors such as laser radar, monocular RGB camera, inertial navigation system, GPS receiver, and vehicle bottom controller information can be used to collect point cloud maps and image data of the vehicle's surrounding environment, three-axis acceleration information of the inertial navigation system, positioning information of the GPS system, and speed and steering information of the vehicle bottom layer.

[0080] In this embodiment, the point cloud data of the target scene refers to the point cloud data located in the target area in the point cloud map collected by the laser radar of the vehicle itself. For example, the target area can be a block area located at a certain distance in front of the vehicle.

[0081] In some embodiments, the point cloud data of the target scene can be obtained by the following method:

[0082] The point cloud block data of the target area is intercepted from the point cloud map according to the geometric size of the vehicle and the driving trajectory of the vehicle.

[0083] Figure 6 FIG. 2 shows a schematic diagram of obtaining point cloud data of a target scene according to an embodiment of the present invention. Figure 6 As shown, the geometric center of the vehicle is the origin o, the forward direction of the vehicle is the x-axis, the speed of the vehicle is v (on the x-axis), and the vehicle coordinate system xoy is established with the geometric center of the vehicle as the origin. In this embodiment, the corresponding point cloud block can be extracted according to the geometric size of the vehicle. The size of the point cloud block is obtained according to the rectangular frame surrounding the outer contour of the left and right tracks of the vehicle. In order to ensure that the vehicle can completely fall within the point cloud block and reserve a certain error range, the length and width of the vehicle can be rounded up. Figure 6 In the figure, the length of the rectangular box is [length] and the width is [width]. According to the vehicle's driving trajectory, the point cloud block at a distance d in front of the vehicle can be intercepted. It is known that the vehicle's geometric center point o is the current discrete trajectory point, and the heading angle at this point is θ. The vertex coordinates of the target area can be obtained as A(x a ,y a )、B(x b ,y b )、C(x c ,y c ) and D(x d ,y d ), as shown in the following table:

[0084]

[0085] It is known that the coordinates of the origin o in the xoy coordinate system are (x o ,y o ), the heading angle is θ, and the point A(x A ,y A ),B(x B ,y B ),C(x C ,y C ),D(x D ,y D ) are shown in the following table:

[0086]

[0087] Finally, according to the geodetic coordinate system, point A(x A ,y A ),B(x B ,y B ),C(x C ,y C ),D(x D ,y D )’s real coordinates are used to intercept the point cloud and obtain the point cloud data of the target scene.

[0088] In this embodiment, by extracting corresponding point cloud blocks according to the geometric dimensions of the vehicle, the quality and relevance of the features expressed by the point cloud data can be improved.

[0089] In this embodiment, the image data of the target scene and the track data of the vehicle after time and space alignment can be obtained by performing time alignment and space alignment on the point cloud data, the image data and the track data of the vehicle. The time and space alignment method of the point cloud data, the image data and the track data of the vehicle is described below.

[0090] When collecting data, the camera acquires image frames through instantaneous exposure, so the image data frequency is relatively high, while the lidar acquires point cloud data through laser beam rotation, so the point cloud data frequency is relatively low. In addition, the GPS and inertial navigation systems also collect data at a high frequency, so the vehicle trajectory data frequency is also relatively high. Since there are certain differences in the sampling frequencies of these three types of data, it is first necessary to synchronize the point cloud data, image data, and vehicle trajectory data in time.

[0091] Figure 7 FIG. 1 is a schematic diagram showing the time synchronization of point cloud data, image data and vehicle trajectory data in an embodiment of the present invention. Figure 7 As shown, a multi-threading and queue caching method can be used to simultaneously open three threads to receive point cloud, image and trajectory data. The timestamp of the lower-frequency point cloud data can be used as the message synchronization benchmark, and the image and trajectory data can be compared with the benchmark timestamp until the time difference is within the set threshold, thus completing the time synchronization of a frame of data.

[0092] After completing the time synchronization of the point cloud data, image data, and vehicle trajectory data, it is necessary to obtain the pose transformation relationship of the laser radar and camera coordinate systems, and then realize the alignment of the spatial positions of the image data and the point cloud data. It should be noted that the pose transformation relationship of the laser radar and camera coordinate systems is a hyperparameter, which can be learned in the training phase of the sliding parameter estimation model to be described below. In the application phase of the sliding parameter estimation model, as long as the positions of the laser radar and camera on the vehicle do not change, the pose transformation relationship of the laser radar and camera coordinate systems learned in the training phase can be used.

[0093] The following is an explanation of the process of learning the pose transformation relationship of the laser radar and camera coordinate systems. Existing calibration methods can be used here to quickly calibrate the camera and laser radar together. For example, you can first place feature objects at the key corner points on the ground and obtain real-time image data online; then use the corner detection algorithm based on Harris corner points and boundary tracking in the OpenCV library to obtain the camera's calibration internal parameters; then select the four corner points of the calibration object as seed points for the point cloud data, and obtain the point cloud data of the key point area scanned by the regional growing algorithm; then, through plane fitting based on the random consistency algorithm, you can get multiple sets of pose data representations for the same position area of ​​the image data and point cloud data, and then solve the spatial transformation relationship between the image data and the point cloud data.

[0094] After learning the spatial transformation relationship between image data and point cloud data, we can use the position information of key points in the point cloud data of the target scene (e.g. Figure 6 The matching positions of the key points in the image data can be obtained through the spatial transformation relationship between the two, and then the image data representing the same position area can be obtained.

[0095] For the sliding parameters, in addition to being able to be reflected to a certain extent through image data and point cloud data, the driving state of the vehicle will also have a certain impact on the sliding parameters. Therefore, in order to achieve accurate estimation of the sliding parameters, this embodiment also takes into account the trajectory data of the vehicle, such as the speed and acceleration information of the vehicle. Since the image data and point cloud data of the target scene obtained according to this embodiment are low-frequency signals, and the inertial navigation signal and GPS signal are high-frequency signals, when matching the trajectory data, the speed information, acceleration information, and vehicle steering degree information in a fixed time window near the most recent time corresponding to a frame of point cloud data of the target scene can be used. The speed information, acceleration information, and vehicle steering degree information in the fixed time window are the trajectory data of the vehicle after spatiotemporal alignment. For the speed, acceleration, and steering degree information x(i) of N sampling signals in the time window, the following 5 commonly used statistics can be used to characterize its characteristics:

[0096] (1) Mean:

[0097]

[0098] (2) Standard deviation:

[0099]

[0100] (3) Square root amplitude:

[0101]

[0102] (4) Root mean square value:

[0103]

[0104] (5) Maximum absolute value:

[0105] f5=max|x(i)|

[0106] In some embodiments, the acceleration information may include longitudinal acceleration and vertical acceleration for analysis. Longitudinal acceleration reflects the acceleration or deceleration of the vehicle, during which the sliding parameter changes greatly. Vertical acceleration is a measure of the acceleration of the vehicle in the vertical direction, and the gravity acceleration a needs to be eliminated in advance. g =9.8m / s 2 The influence of represents the degree of vibration during vehicle driving and can reflect the current ground properties to a certain extent. The speed information can be analyzed by selecting the three-axis speeds, where the longitudinal speed represents the vehicle's driving speed, the lateral speed represents the vehicle's steering and other operations, and the vertical speed represents the vehicle's vertical climb. The steering degree information is the real-time steering curvature of the vehicle calculated by the left and right speeds of the underlying motor combined with the inertial navigation system. For the above six variables, each variable can generate 5 statistical features, and a total of 30 statistical features can be generated.

[0107] Step S202: inputting the point cloud data, image data and vehicle trajectory data of the target scene into a pre-trained sliding parameter estimation model to obtain an estimation result of the sliding parameter.

[0108] Figure 8 FIG. 4 shows a schematic diagram of a network framework of a sliding parameter estimation model according to an embodiment of the present invention. Figure 8 As shown, the sliding parameter estimation model includes a sensor data feature extraction network, a feature fusion network, a parameter (i.e., sliding parameter) identification network, and a hierarchical linearization layer connected in sequence. The sensor data feature extraction network includes a point cloud data residual network, a camera data residual network, and a trajectory feature extraction network. The point cloud data residual network is used to extract point cloud features from the point cloud data of the target scene. The camera data residual network is used to extract image features from the image data of the target scene. The trajectory feature extraction network is used to extract trajectory features from the trajectory data of the vehicle. The feature fusion network includes a first fusion network and a second fusion network ( Figure 8 The first fusion network is used to fuse the point cloud features and the trajectory features. The second fusion network is used to fuse the image features and the trajectory features. The parameter (i.e., sliding parameter) recognition network includes a point cloud data convolutional long short-term memory network (i.e., Figure 8 Point cloud data ConvLSTM) and camera data convolutional long short-term memory network (i.e. Figure 8 The output of the first fusion network is input to the point cloud data convolutional long short-term memory network, and the output of the second fusion network is input to the camera data convolutional long short-term memory network. The point cloud data convolutional long short-term memory network is used to learn the sliding parameters of the fused point cloud features and trajectory features, and the camera data convolutional long short-term memory network is used to learn the sliding parameters of the fused image features and trajectory features. The output results of the point cloud data ConvLSTM and the camera data ConvLSTM are input to the hierarchical linearization layer, and the estimation results of the sliding parameters are finally output through the fusion and extraction of multiple linearization layers.

[0109] Fig. 9 FIG. 4 shows a schematic diagram of a network structure of a sliding parameter estimation model according to another embodiment of the present invention. Fig. 9 As shown, the input data of the sliding parameter estimation model consists of three parts, namely: three-channel image information (RGB) representing the image data of the target scene, two-channel image information (height and reflection intensity) representing the point cloud data of the target scene, and speed, acceleration, and steering information representing the trajectory data of the vehicle.

[0110] In this embodiment, before inputting the point cloud data of the target scene into the sliding parameter estimation model, the point cloud data of the target scene may be preprocessed to obtain two-dimensional image information of the point cloud data. Specifically, each point cloud block may be projected into a two-channel image containing height and intensity information. Fig.10 FIG. 4 is a schematic diagram showing a point cloud projection image according to an embodiment of the present invention. Fig.10 As shown in Figure 2, each point in the 3D point cloud block is assigned to the nearest pixel in the image, and each pixel unit corresponds to many point cloud data points p. i , called a point cloud cluster. Calculate the height mean h of the point cloud cluster in each pixel mean and the mean reflection intensity i mean , and assign it to the red and blue channels of the corresponding pixels of the image. For the missing areas of the point cloud, linear interpolation is performed through the surrounding pixel blocks to fill them.

[0111] In this embodiment, before the trajectory data of the vehicle is input into the sliding parameter estimation model, feature extraction may be performed on the trajectory data of the vehicle. For example, for the longitudinal acceleration and vertical acceleration in the acceleration information, the following five features are calculated respectively:

[0112] (1) mean, (2) standard deviation, (3) root amplitude, (4) root mean square value, (5) maximum absolute value.

[0113] For the longitudinal velocity, lateral velocity, and vertical velocity in the velocity information, the following five features are calculated respectively:

[0114] (1) mean, (2) standard deviation, (3) root amplitude, (4) root mean square value, (5) maximum absolute value.

[0115] For the steering degree information, the following five features are calculated respectively:

[0116] (1) mean, (2) standard deviation, (3) root amplitude, (4) root mean square value, (5) maximum absolute value.

[0117] For the above six variables, each variable can generate 5 statistical features, and a total of 30 statistical features can be generated.

[0118] Back to Fig. 9 , the input parameters of the three-channel image information are [b,3,a,b'], the first dimension parameter b represents the size of a batch, the second dimension parameter represents that the image has three channels, and the third and fourth dimension parameters represent the size information of the image. The input parameters of the vehicle trajectory data are [b,6*5], the first dimension parameter b represents the size of a batch, and 6*5 represents "six variables, each variable can generate 5 statistical features". The input parameters of the point cloud data are [b,3,a,b'], the first dimension parameter b represents the size of a batch, the second dimension parameter represents that the image has two channels, and the third and fourth dimension parameters represent the size information of the image.

[0119] The three-channel image information is input into a residual network module (i.e. Fig. 9 The two-channel point cloud information is input into another residual network module (i.e. Fig. 9 The trajectory features are expanded to [b,w*h] through continuous linearization layers and reshaped to [b,w,h] through reshape operations. The upper residual network module can extract image features from three-channel image information, and the lower residual network module can extract point cloud features from two-channel point cloud information. After that, the image features and point cloud features are fused with the trajectory features through element-wise multiplication, and the fusion results are convolved, batch normalized, activated, and pooled. Fig. 9 The "CBRP" modules in the figure represent the convolution layer (C), batch normalization layer (B), activation layer (R), and pooling layer (P). The fused features output by the "CBRP" module are input into the ConvLSTM module for learning the sliding parameters. Finally, the outputs of the two ConvLSTM modules are flattened and concatenated, and then the data is linearized layer by layer and then channel by channel, and finally regressed to the estimated sliding parameter value.

[0120] In some embodiments, the image feature extraction uses the ResNet-34 network, and the point cloud feature extraction uses the ResNet-18 network. Image data contains rich environmental features, and the features contained in the image can be fully utilized by using a more complex ResNet-34 network.

[0121] In some embodiments, before inputting the two-channel image and image data of the target scene into the pre-trained sliding parameter estimation model, the following operations may be performed on the two-channel image and image data of the target scene:

[0122] Step S111: segmenting the two-channel image and image data of the target scene according to the geometric size of the vehicle and the grid resolution, and obtaining the two-channel image and image data of the target scene in a grid format.

[0123] Step S222: Encode the two-channel image of the target scene in the raster format and the raster representing the same position in the image data.

[0124] and, Figure 5 The estimation methods in also include:

[0125] Step S203: restoring the estimation result of the sliding parameter to the grid at the corresponding position based on the grid encoding, and generating a sliding parameter prediction grid map of the target scene.

[0126] Fig.11 A schematic diagram of a generation process of a sliding parameter map according to an embodiment of the present invention is shown.

[0127] like Fig.11 As shown, the color image on the left represents the two-channel image information of the point cloud data representing the target scene, the gray image on the left represents the three-channel image information of the image data representing the target scene, and the function "f 1.2.3.4.5 (v x,v ,α x,v.z ,δ steer)" represents the "six variables, each of which can generate five statistical features" in the trajectory features described above. The middle module "module" represents the sliding parameter estimation model in the embodiment of the present invention. First, the two images on the left can be segmented in position in combination with the vehicle size and grid resolution to obtain a series of point cloud data and image data representing the same position. In the process of generating the grid map, each position can be encoded to ensure that the sliding parameter prediction result output by the sliding parameter estimation model can match the corresponding position. Finally, the point cloud data and image data are combined with the trajectory features of the vehicle as input, and the trained sliding parameter estimation model is used for estimation, and the estimation result is restored to the corresponding grid position based on the position encoding to generate the sliding parameter prediction grid map of the current scene (i.e., the target scene in this embodiment).

[0128] The following describes the training process of the sliding parameter estimation model in the embodiment of the present invention.

[0129] The training method of the sliding parameter estimation model in the embodiment of the present invention includes:

[0130] Step S100: Obtain point cloud data, image data, vehicle trajectory data and corresponding sliding parameter labels of the target scene.

[0131] The method for obtaining the point cloud data, image data, and vehicle trajectory data of the target scene has been described in step S201 and will not be repeated here. The processing process of the sliding parameter label in this embodiment is as follows:

[0132] The generation of sliding parameters is expressed in terms of the absolute value of the slip, as follows:

[0133]

[0134] Among them, s is the sliding parameter label; v q is the vehicle drag speed, i.e. the actual driving speed of the vehicle; v s is the winding speed, is the driving speed of the vehicle motor. When the vehicle does not have any slip, v q =v s , at this time the sliding parameter is 0; when the driving wheel is locked and fully braked, v s =0, it is a pure slip state and the sliding parameter is 1.

[0135] Step S200: inputting the point cloud data, image data and vehicle trajectory data of the target scene into the initial model of the sliding parameter estimation model to obtain a preliminary estimation result of the sliding parameter.

[0136] Step S300: Based on the comparison between the preliminary estimation result of the sliding parameter and the sliding parameter label, the initial model of the sliding parameter estimation model is updated.

[0137] In this embodiment, the training process of the initial model of the sliding parameter estimation model can use the mean square error (MSE). The mean square error is a loss function in a regression task. It is the mean of the sum of squares of the difference between the predicted value f(x) and the target value y. It is used to measure the difference between the predicted value and the true value of the initial model of the sliding parameter estimation model. The calculation formula is as follows:

[0138]

[0139] Here, n is the number of samples.

[0140] The optimized A* search algorithm in this embodiment is described below at a certain moment in the middle:

[0141] First, expand the current parent node in the obstacle grid map. Fig.12 FIG. 2 shows an example of performing node expansion in an obstacle grid map according to an embodiment of the present invention. Fig.12 As shown, with the current parent node n parent On a circle with △θ as the center, N child nodes n0, n0, ..., n can be uniformly generated with △θ as the angle difference. N , each child node needs to determine whether it collides with an obstacle when it continues to expand. Fig.12 In the k As it continued to expand, it collided with an obstacle ( Fig.12 Indicated by a star in the middle); child node n k+1 When the expansion continues, it does not collide with obstacles. In some embodiments, when the current parent node is expanded, the expansion radius can be set with upper and lower limits to prevent infinite expansion, that is, the expansion radius R o Need to meet R o ∈[R min ,R max ], where R min is the lower bound of the expansion radius, R max In some embodiments, when the current parent node is expanded, if the distance between the current parent node and the obstacle is far, the upper limit of the expansion radius R is selected. max As the expansion radius between the child node and the current parent node; if the distance between the current parent node and the obstacle is close, the distance between the current parent node and the obstacle is selected as the expansion radius between the child node and the current parent node; if the expansion radius between the child node and the current parent node is selected as the lower bound of the expansion radius R min, if the child node still collides with the obstacle, the current parent node is ignored.

[0142] For the heuristic function f(n) = g(n) + h(n), the moving cost of the child node g(n i ) contains the movement cost g(n) of the current parent node parent ), the Euclidean distance d(n parent ,n i ), and the estimated value of the vehicle sliding parameter slip(n i ). Specifically, g(n i ) is calculated as follows:

[0143] g(n i )=g(n paent ))+w dn d(n parent , n i )+w slip slip(n i )

[0144] Among them, w dn is the weight of the distance cost between the parent and child nodes, w slip is the weight of the sliding parameter cost, the estimated value of the vehicle sliding parameter slip(n i ) is obtained by predicting the data in the grid map through the index sliding parameter. It should be noted that the weight w of the parent-child node distance cost dn and the weight w of the sliding parameter cost slip Both can be adjusted according to the quality of the final planned path.

[0145] In some other embodiments, the movement cost g(n i ) contains the movement cost g(n) of the current parent node parent ), the Euclidean distance d(n parent , n i ), the stability cost s(n i ), the turning cost of the child node t(n parent , n i ), the obstacle distance cost of the child node dis obs (n i ). Specifically, g(n i ) is calculated as follows:

[0146] g(n i )=g(n parent )+w dn d(n parent , n i )+w ss(n i )+wt t (n parent , n i )+w obs d obs (n i )+w slip slip(n i )

[0147] Among them, w dn is the weight of the distance cost between the parent and child nodes, w s is the weight of the stability cost at the child node position, w t is the weight of the child node’s turn cost, w obs is the weight of the obstacle distance cost of the child node, w slip is the weight of the sliding parameter cost. It should be noted that the weights in the above calculation formula can be adjusted according to the quality of the final planned path.

[0148] The stability cost s(n i ), the turning cost of the child node t(n parent , n i ) and the obstacle distance cost dis of the child node obs (n i ) is explained below.

[0149] For the stability cost s(n i ), which can be determined by the pitch angle and roll angle of the vehicle at the child node and the maximum pitch angle and maximum roll angle allowed by the vehicle. i ) can be calculated along the direction from the current parent node to the child node, and the point cloud data of the vehicle geometry at the child node is obtained. Then, the pitch angle and roll angle of the vehicle at the child node are obtained by plane fitting, and finally, the pitch angle and roll angle are obtained by proportionally comparing the pitch angle threshold and roll threshold. The specific calculation formula is as follows:

[0150]

[0151] Among them, pitch i is the pitch angle of the vehicle at the i-th child node, pitch max is the maximum pitch angle allowed for the vehicle, roll i is the roll angle of the vehicle at the i-th child node, roll max The maximum roll angle allowed for the vehicle.

[0152] For the child node’s turn cost t(n parent ,n i), which can be determined by the direction of the current parent node and the expansion direction of the child node. parent ,n i ) is obtained by the direction of the current parent node and the direction of the child node expansion. The direction of the current parent node is the direction of the line between the current parent node and the parent node of the current parent node. The child node expansion direction is the direction of the line between the child node and the current parent node. The turning cost t(n parent ,n i ) is the angle between the direction of the current parent node and the expansion direction of the child node.

[0153] For the child node, the obstacle distance cost dis obs (n i ), which can be determined by the shortest distance between the child node and the obstacle. obs (n i ) can be the shortest distance d(x) between the child node and the obstacle. i ,y i ). The specific calculation formula is as follows:

[0154]

[0155] In some other embodiments, the heuristic function value h(n i ) can be represented by child node n i To the target position n goal In the A* search algorithm, the heuristic value h(n) is mainly used to guide the search direction toward the target node so as to find a suitable path faster. In the process of selecting the heuristic value, when the heuristic value is overestimated, it may cause the search to go in the wrong direction; when the heuristic value is too small, although the search speed is faster, the search results are often incomplete. Since the vehicle can move in any direction, the heuristic function value h(n) of the child node i ) can be represented by child node n i To the target position n goal The distance here can be the Euclidean distance, and the specific calculation formula is as follows:

[0156]

[0157] In the optimized A* search algorithm of this embodiment, the child node n i The sum of the costs f(n i ) can be expressed as:

[0158] f(n i )=w g g(n i )+w h ζh(ni )

[0159] Among them, w g represents the movement cost weight, w h represents the weight of the heuristic value, ζ represents the proportional factor, and the proportional factor ζ is calculated as follows:

[0160]

[0161] Among them, h min represents the minimum cost of moving between adjacent nodes, T except Indicates the maximum number of nodes expected in a path.

[0162] In summary, the heuristic functions at both ends of the optimized bidirectional A* search algorithm in this embodiment are:

[0163] f f (n) = w gf g f (n)+w hf zeh f (n)

[0164] f b (n) = w gb g b (n)+w hb zeh b (n)

[0165] Among them, f f (n) represents the distance from the starting point to the reverse node n k The total cost of the forward search, g f (n) represents the cost of moving from the starting point to the current forward node, h f (n) represents the direction from the current forward node to the reverse node n k The inspiration cost of b (n) represents the direction from the target position to the forward node n f The total cost of reverse search, g b (n) represents the moving cost from the target position to the current reverse node, h b (n) represents the heuristic cost from the current reverse node to the forward node nf; w gf 、w hf 、w gb 、w hb Represents the weight of the moving cost and heuristic cost of forward search and reverse search; reverse node n k Represents the child node with the minimum total cost of reverse search, forward node n f Represents the child node that currently has the minimum total cost of the forward search.

[0166] In some embodiments, Figure 1 The path planning method based on the improved bidirectional A-star search algorithm includes the following steps:

[0167] Step 300: Perform a first round of smoothing on the motion path by using a gradient descent method, wherein in the gradient descent method, an energy function used to determine the optimization direction includes path curvature.

[0168] The motion path searched by the optimized double A* search algorithm in this embodiment is a discrete broken line segment. In order to enable the vehicle to run smoothly in the environment, it is necessary to reduce the angle and curvature between the broken line segments to generate a continuous smooth curve. The gradient descent method is to iteratively adjust the position of each point on the path to reduce the path curvature as the goal to achieve the goal of optimizing the path. When using the gradient descent method for iterative update, it is necessary to define an energy function J E As the optimization target, it can determine the optimization direction and progress. In this embodiment, the energy function J E The curvature of the new path formed after the iterative update may be included. In this embodiment, the direction of the iterative update process can be indicated by minimizing the path curvature, thereby reducing the degree of turning angles and curvature between broken line segments and generating a continuous smooth curve.

[0169] In some embodiments, the energy function J E Can include path length cost and its weight, obstacle distance cost d obs (n i , n′ i ) and its weight, sliding parameter cost s slip (n i , n′ i ) and its weights. Specifically, the energy function J E It is expressed as:

[0170]

[0171] The path length cost is Obstacle distance cost d obs (n i , n′ i ), sliding parameter cost s slip (n i , n′ i ) for explanation.

[0172] Fig.13 An example of using the gradient descent method to smooth a motion path according to an embodiment of the present invention is shown. First, in order to ensure the accuracy of the starting point position and the target position, the starting point position and the target position on the path are not adjusted in this embodiment. i,like Fig.13 As shown, the parameters of the obstacle grid map can be combined with the parameters of the two nodes n i-1 、n i+1 After that, the point to be adjusted n i To n i-1 、n i+1 The line connecting the two is the projection point n′ i . Then, according to n i , n′ i The grid resolution of the connection line and obstacle grid map is generated to adjust the point n i The candidate node set S i ={n i ,n i0 ,n i1 ,…,n im ,n i ′}. Then, from n i0 Start evaluating candidate nodes one by one: if a candidate node collides with an obstacle, skip the candidate node; otherwise, calculate the energy function value J of the candidate node. Ei ,J Ei0 ,J Ei1 ,…,J Eim ,J Ei′ For the path length cost It is the candidate node to the point to be adjusted n i For example, for candidate node n i0 , whose path length cost is candidate node n i0 To the point to be adjusted n i The distance d(n i0 ,n i ); for candidate node n i1 , whose path length cost is candidate node n i0 To the point to be adjusted n i The distance d(n i0 ,n i ) plus candidate node n i1 To candidate node n i0 The distance d(n i1 ,n i0 ), and so on. For the obstacle distance cost d obs (n i , n′ i ), which is the shortest distance from the candidate node to the obstacle. For the sliding parameter cost s slip (n i , n′ i), which is the estimated value of the vehicle sliding parameter at the candidate node position. The estimated value of the vehicle sliding parameter at the candidate node position can be obtained by indexing the sliding parameter prediction grid map. Finally, the candidate node with the minimum energy function value is selected to replace node n i , so far the point to be adjusted n is completed i Smoothing process.

[0173] The above method can be used to adjust all nodes on the motion path except the starting point and the target position, thereby completing a single iterative update of the motion path. Afterwards, the sum of the energy function values ​​of all nodes after this iterative update of the path can be calculated. Finally, determine whether the termination condition is met. If so, stop the iterative update, otherwise continue with the next iterative update. In this embodiment, the termination condition may be that the sum of the number of iterative updates exceeds a first threshold (for example, 10,000 times) or the improved difference of the sum of the energy function values ​​calculated after a single iterative update is less than a second threshold. Fig.13 In the figure, the black line segment represents the original motion path, the orange line segment represents the new motion path formed after the first iterative update, the blue circle represents the projection point, and the yellow circle represents the candidate node in the first iterative update process.

[0174] It should be noted that in the energy function J E The weight w of the path length cost d , the weight of obstacle distance cost w o , the weight w of the sliding parameter cost s Both can be adjusted according to the quality of the final result of the smoothing process.

[0175] In some embodiments, Figure 1 The path planning method based on the improved bidirectional A-star search algorithm includes the following steps:

[0176] Step 400: using a spline interpolation method to perform a second round of smoothing on the motion path after the first round of smoothing.

[0177] Spline interpolation is a mathematical method used to construct a continuous and smooth curve through a set of discrete data points. The basic idea of ​​spline interpolation is to construct a high-order polynomial between each data point. These polynomials are continuous on the data points, and their first-order and second-order derivatives are also continuous on the data points, thus ensuring the smoothness of the interpolation curve.

[0178] In order to obtain a uniform continuous path for corridor construction and local planning, this embodiment may use a piecewise cubic spline interpolation method for further smoothing to generate a curvature continuous path.

[0179] Cubic spline interpolation is to divide the interval containing N+1 discrete points into N fitting small intervals, where each small interval contains two points {[n0,n1],[n1,n2],…,[n N-1 ,n N ]}, and then use different cubic functions y=a at two points in each interval i +b i x+c i x 2 +d i x 3 Fitting is performed to achieve that the curve passes through the node and is first-order continuous and second-order equal. The interpolation function in each small interval can be expressed as S i (x i ), there are four parameters that need to be determined, so there are a total of 4N undetermined parameters, and it is necessary to construct equivalent equations to meet the solution requirements.

[0180] Since both ends of the interval need to pass through the nodes, 2N equations about the node positions can be listed. In order to meet the high-order continuity requirements of the path curve, first-order continuity and second-order equality need to be met at adjacent interval nodes, and 2N-2 equations can be listed. Combined with the natural boundary conditions, that is, the second-order derivative at the starting point and the final point is 0, 2 equations can be constructed. According to the above conditions, a total of 4N equations are constructed, which is the same as the number of parameters to be determined. The matrix solution can be used to obtain the fitting function of all intervals, and they are connected to generate a reference path that passes through all nodes and is continuous and smooth.

[0181] Compared with the prior art, the embodiments of the present invention can achieve at least one of the following beneficial effects:

[0182] 1. The scheme of the present invention updates the heuristic item calculation of a single forward search in the bidirectional A* search algorithm from the end point to the node with the lowest total cost of the current reverse search, and updates the heuristic item calculation of the reverse search from the starting point to the node with the lowest total cost of the forward search, thereby preventing the phenomenon that the distance is very close but it takes a long time to update during the search process, thereby improving the search efficiency.

[0183] 2. Aiming at the problem that the sliding parameters are ignored in the motion planning process of off-road scenes, resulting in insufficient vehicle driving safety, the solution of the present invention proposes an optimized two-way A* search algorithm using a multi-layer map. The optimized two-way A* search and reference line smoothing are performed on a multi-layer map including an obstacle layer, a height layer, and a sliding parameter layer to achieve avoidance of high sliding parameter risk areas in off-road scenes, improve the timeliness and safety of existing planning methods, and reduce the difficulty of trajectory tracking.

[0184] The above description is only a preferred specific implementation manner of the present invention, but the protection scope of the present invention is not limited thereto. Any changes or substitutions that can be easily conceived by any technician familiar with the technical field within the technical scope disclosed by the present invention should be covered within the protection scope of the present invention.

Claims

1. A path planning method based on an improved bidirectional A-star search algorithm, characterized in that: The following steps are involved: Obtain the obstacle grid map of the target scene; Determine the motion path from the starting position to the target position in the obstacle grid map based on the optimized bidirectional A* search algorithm; wherein, In the optimized bidirectional A* search algorithm, the total forward cost of a child node includes the forward movement cost of the child node and its weight and the forward heuristic function value of the child node and its weight, and the total reverse cost of a child node includes the reverse movement cost of the child node and its weight and the reverse heuristic function value of the child node and its weight; wherein, The forward heuristic function value of the child node is the heuristic function value from the child node to the child node currently having the minimum reverse total cost, and the reverse heuristic function value of the child node is the heuristic function value from the child node to the child node currently having the minimum forward total cost.

2. The path planning method according to claim 1, characterized in that: The following steps are also included: Get the sliding parameter prediction grid map of the target scene; where: The forward movement cost of the child node includes the forward movement cost of the current parent node, the distance cost between the current parent node and the child node and its weight, and the estimated value of the vehicle sliding parameter at the child node position and its weight; the reverse movement cost of the child node includes the reverse movement cost of the current parent node, the distance cost between the current parent node and the child node and its weight, and the estimated value of the vehicle sliding parameter at the child node position and its weight; wherein, The estimated value of the vehicle sliding parameter at the subnode position is obtained by indexing the sliding parameter prediction grid map.

3. The path planning method according to claim 2, characterized in that: The following steps are also included: The motion path is subjected to a first round of smoothing by a gradient descent method, wherein in the gradient descent method, an energy function for determining an optimization direction includes a path curvature.

4. The path planning method according to claim 3, characterized in that: The following steps are also included: A spline interpolation method is used to perform a second round of smoothing on the motion path after the first round of smoothing.

5. The path planning method according to claim 3, characterized in that: The first round of smoothing of the motion path by the gradient descent method comprises the following steps: Step 110: In each iteration update of the motion path, the following steps are performed respectively: Step 10: determining a candidate node set of the node to be adjusted according to adjacent nodes of the node to be adjusted in the motion path, wherein the node to be adjusted does not include a starting position and a target position; Step 20: Select a node with the minimum energy function value from the candidate node set of the node to be adjusted to replace the node to be adjusted, and calculate the sum of the energy function values ​​of all replaced nodes; Step 120: In response to the total number of iterative updates exceeding a first threshold or the improvement difference of the sum of the energy function values ​​calculated after a single iterative update being less than a second threshold, stopping the iterative update.

6. The path planning method according to claim 4, characterized in that: The spline interpolation method is a cubic spline interpolation method.

7. The path planning method according to claim 5, characterized in that: The path curvature is determined by the accumulated distances from nodes in the candidate node set of the node to be adjusted to the node to be adjusted.

8. The path planning method according to claim 7, characterized in that: The energy function also includes an obstacle distance cost and a weight thereof, wherein the obstacle distance cost is determined by the distance from a node in the candidate node set of the node to be adjusted to an obstacle.

9. The path planning method according to claim 7, characterized in that: The energy function further includes a sliding parameter cost, wherein the sliding parameter cost is determined by a vehicle sliding parameter estimation value at a node position in a candidate node set of the node to be adjusted.

10. The path planning method according to claim 2, characterized in that: The sliding parameter prediction grid map is determined by the following method: Obtain point cloud data, image data and vehicle trajectory data of the target scene; Projecting the point cloud data of the target scene into a two-channel image containing height information and intensity information; Segmenting the two-channel image and image data of the target scene according to the geometric size of the vehicle and the grid resolution to obtain the two-channel image and image data of the target scene in a grid format; Encoding the two-channel image of the target scene in a raster format and the raster representing the same position in the image data; Inputting the two-channel image of the target scene in raster format, the image data and the trajectory data of the vehicle into a pre-trained sliding parameter estimation model to obtain an estimation result of the sliding parameter; The estimated result of the sliding parameter is restored to the grid at the corresponding position based on the grid encoding to generate a sliding parameter prediction grid map of the target scene; wherein, The sliding parameter estimation model includes a feature extraction network, a feature fusion network, a sliding parameter identification network, and a linear layer connected in sequence; The feature extraction network includes a first residual network for extracting point cloud features, a second residual network for extracting image features, and a trajectory feature extraction network; The feature fusion network includes a first fusion network for fusing point cloud features and trajectory features and a second fusion network for fusing image features and trajectory features; The sliding parameter recognition network includes a first convolutional long short-term memory network for learning sliding parameters for fused point cloud features and trajectory features and a second convolutional long short-term memory network for learning sliding parameters for fused image features and trajectory features.

Images