Vehicle motion path planning method
By optimizing the A* search algorithm, combining multi-layer map information, considering sliding parameters and obstacles, a safe path in off-road scenarios is generated, solving the problem of insufficient safety in path planning in the existing technology, and improving the driverless vehicle Safety and track quality.
Patent Information
- Application Number
- CN202411234644.7
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-09-04
- Publication Date
- 2024-11-29
AI Technical Summary
The existing path planning methods fail to fully consider sliding parameters in off-road scenarios, resulting in insufficient safety of planned paths and low trajectory quality, which makes it impossible to effectively improve the safety of unmanned vehicles.
The optimized A* search algorithm is adopted, combining obstacle raster map, height information raster map and sliding parameter prediction raster map, and by modifying the cost sum and expansion area of the node, considering vehicle sliding parameters, obstacle distance and stability, Generate a safe path in a three-dimensional scenario.
It improves the safety of path planning in off-road scenarios, reduces the difficulty of trajectory tracking, realizes avoidance and deceleration in high sliding parameter risk areas, and outputs a safe and collision-free path.
Smart Images

Figure CN119043364A_ABST
Abstract
Description
Technical Field
[0001] The present invention relates to the technical field of intelligent vehicles, and in particular to a method for planning a vehicle motion path. 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 a simple greedy search or the Dijkstra algorithm, and is widely used in fields such as path planning. Although the A* search algorithm can improve the shortcomings of brute force search to a certain extent, it has the disadvantage of incompleteness.
[0003] 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
[0004] In view of the above analysis, an embodiment of the present invention aims to provide a vehicle motion path planning method to solve the problem of insufficient safety of the planned path due to the lack of consideration of the influence of sliding parameters on path planning in the existing solutions.
[0005] In a first aspect, an embodiment of the present invention provides a method for planning a vehicle motion path, comprising the following steps:
[0006] Obtain the obstacle grid map and sliding parameter prediction grid map of the target scene;
[0007] Determine the motion path from the starting position to the target position in the obstacle grid map based on the optimized A* search algorithm; wherein,
[0008] 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. 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; wherein the estimated value of the vehicle sliding parameter at the child node position is obtained by indexing the sliding parameter prediction grid map.
[0009] Based on the further improvement of the above planning method, the method further includes the following steps:
[0010] Get the height information grid map of the target scene; where:
[0011] In the optimized A* search algorithm, the expansion area of the child node is determined by the expansion area of the child node in the obstacle grid map and the height information of the child node and the current parent node position; wherein the height information of the child node and the current parent node position is obtained by indexing the height information grid map.
[0012] Based on further improvement of the above planning method, the movement cost of the child node also includes the turning cost and weight of the child node and the current parent node determined by the direction of the current parent node and the extension direction of the child node.
[0013] Based on a further improvement of the above planning method, the movement cost of the subnode also includes an obstacle distance cost determined by the shortest distance between the subnode and the obstacle.
[0014] Based on further improvement of the above planning method, the movement cost of the subnode also includes the subnode stability cost and its weight determined by the pitch angle and roll angle of the vehicle at the subnode and the maximum pitch angle and maximum roll angle allowed by the vehicle.
[0015] Based on a further improvement of the above planning method, the heuristic function value of the child node is determined by the distance from the child node to the target position.
[0016] Based on the further improvement of the above planning method, the extension area of the child node in the obstacle grid map is determined by:
[0017] When the distance between the subnode and the obstacle is greater than the first threshold, the expansion radius of the subnode is the maximum preset value;
[0018] When the distance between the subnode and the obstacle is less than the first threshold and greater than the minimum preset value, the expansion radius of the subnode is the distance between the subnode and the obstacle;
[0019] When the distance between a child node and an obstacle is less than a minimum preset value, the child node is not expanded.
[0020] Based on the further improvement of the above planning method, the obstacle grid map is determined by the following method:
[0021] Based on the obstacle detection algorithm, determine a basic obstacle map of the target scene, wherein the format of the basic obstacle map is a grid;
[0022] Obstacles in the basic obstacle map are expanded according to the geometric size of the vehicle and discrete areas in the basic obstacle map are filled to obtain an obstacle grid map.
[0023] Based on the further improvement of the above planning method, the height information grid map is determined by the following method:
[0024] Determining a traversable area from the obstacle grid map based on a region growing segmentation algorithm;
[0025] Acquire point cloud data of the traversable area;
[0026] In the traversable area, the average value of the point cloud clusters in the same grid is used as the height information of the grid to obtain a height information grid map.
[0027] Based on the further improvement of the above planning method, the sliding parameter prediction grid map is determined by the following method:
[0028] Obtain point cloud data, image data and vehicle trajectory data of the target scene;
[0029] Projecting the point cloud data of the target scene into a two-channel image containing height information and intensity information;
[0030] 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;
[0031] Encoding the two-channel image of the target scene in a raster format and the raster representing the same position in the image data;
[0032] 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;
[0033] 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,
[0034] 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;
[0035] 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;
[0036] 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;
[0037] 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.
[0038] Compared with the prior art, the present invention can achieve at least one of the following beneficial effects:
[0039] 1. The solution of the present invention combines multi-layer maps, expands the A* search algorithm based on binary plane grid maps to multi-layer maps, and uses the information in the obstacle layer, height layer, and vehicle sliding parameter estimation layer to expand the nodes in the A* search algorithm and modify the cost function, and finally outputs a safe and collision-free path in a three-dimensional scene.
[0040] 2. At the same time, in order to address 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 A* search algorithm to achieve avoidance and deceleration of high sliding parameter risk areas in off-road scenes, thereby improving the safety of the path planning method and reducing the difficulty of trajectory tracking.
[0041] 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
[0042] 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;
[0043] Figure 1 A schematic flow chart of a vehicle motion path planning method according to an embodiment of the present invention is shown.
[0044] Figure 2 A schematic flow chart of a method for estimating vehicle sliding parameters according to an embodiment of the present invention is shown.
[0045] Figure 3 A schematic diagram of acquiring point cloud data of a target scene according to an embodiment of the present invention is shown.
[0046] Figure 4 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.
[0047] Figure 5 A schematic diagram of a network framework of a sliding parameter estimation model according to an embodiment of the present invention is shown.
[0048] Figure 6 A schematic diagram of the network structure of a sliding parameter estimation model according to another embodiment of the present invention is shown.
[0049] Figure 7 A schematic diagram of a point cloud projection image according to an embodiment of the present invention is shown.
[0050] Figure 8 A schematic diagram of a generation process of a sliding parameter map according to an embodiment of the present invention is shown.
[0051] Fig. 9 An example of performing node expansion in an obstacle grid map according to an embodiment of the present invention is shown.
[0052] Fig.10 A schematic diagram of a process for generating an obstacle grid map according to an embodiment of the present invention is shown. DETAILED DESCRIPTION
[0053] 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.
[0054] Figure 1 FIG. 2 is a flow chart showing a method for planning a vehicle motion path according to an embodiment of the present invention. Figure 1 As shown, the vehicle motion path planning method includes the following steps:
[0055] Step S10: Obtain an obstacle grid map and a sliding parameter prediction grid map of the target scene.
[0056] 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.
[0057] 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.
[0058] 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.
[0059] The following is a description of the process of generating the sliding parameter prediction grid map of the target scene in this embodiment.
[0060] Figure 2 FIG. 2 shows a flow chart of a method for estimating vehicle sliding parameters according to an embodiment of the present invention. Figure 2 As shown, the method for estimating the vehicle sliding parameters includes the following steps:
[0061] Step S201: Acquire point cloud data, image data of the target scene and trajectory data of the vehicle.
[0062] 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.
[0063] 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.
[0064] In some embodiments, the point cloud data of the target scene can be obtained by the following method:
[0065] 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.
[0066] Figure 3 FIG. 2 shows a schematic diagram of obtaining point cloud data of a target scene according to an embodiment of the present invention. Figure 3As 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 3 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:
[0067]
[0068] 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:
[0069]
[0070] 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.
[0071] 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.
[0072] 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.
[0073] 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.
[0074] Figure 4 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 4 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.
[0075] 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.
[0076] 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.
[0077] 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 3 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.
[0078] 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:
[0079] (1) Mean:
[0080]
[0081] (2) Standard deviation:
[0082]
[0083] (3) Square root amplitude:
[0084]
[0085] (4) Root mean square value:
[0086]
[0087] (5) Maximum absolute value:
[0088] f5=max|x(i)|
[0089] 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.
[0090] 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.
[0091] Figure 5 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 5 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 5 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 5 Point cloud data ConvLSTM) and camera data convolutional long short-term memory network (i.e. Figure 5 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.
[0092] Figure 6 FIG. 4 shows a schematic diagram of a network structure of a sliding parameter estimation model according to another embodiment of the present invention. Figure 6 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.
[0093] 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. Figure 7 FIG. 4 is a schematic diagram showing a point cloud projection image according to an embodiment of the present invention. Figure 7 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.
[0094] 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:
[0095] (1) mean, (2) standard deviation, (3) root amplitude, (4) root mean square value, (5) maximum absolute value.
[0096] For the longitudinal velocity, lateral velocity, and vertical velocity in the velocity information, the following five features are calculated respectively:
[0097] (1) mean, (2) standard deviation, (3) root amplitude, (4) root mean square value, (5) maximum absolute value.
[0098] For the steering degree information, the following five features are calculated respectively:
[0099] (1) mean, (2) standard deviation, (3) root amplitude, (4) root mean square value, (5) maximum absolute value.
[0100] For the above six variables, each variable can generate 5 statistical features, and a total of 30 statistical features can be generated.
[0101] Back to Figure 6 , 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.
[0102] The three-channel image information is input into a residual network module (i.e. Figure 6 The two-channel point cloud information is input into another residual network module (i.e. Figure 6 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. Figure 6 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.
[0103] 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.
[0104] 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:
[0105] 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.
[0106] 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.
[0107] and, Figure 2 The estimation methods in also include:
[0108] 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.
[0109] Figure 8 A schematic diagram of a generation process of a sliding parameter map according to an embodiment of the present invention is shown.
[0110] like Figure 8 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,y , α x,y,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).
[0111] The following is an explanation of the training process of the sliding parameter estimation model in the embodiment of the present invention.
[0112] The training method of the sliding parameter estimation model in the embodiment of the present invention includes:
[0113] Step S100: Obtain point cloud data, image data, vehicle trajectory data and corresponding sliding parameter labels of the target scene.
[0114] 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:
[0115] The generation of sliding parameters is expressed in terms of the absolute value of the slip, as follows:
[0116]
[0117] 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.
[0118] 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.
[0119] 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.
[0120] 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:
[0121]
[0122] Here, n is the number of samples.
[0123] Step S20: Determine a motion path from a starting position to a target position in the obstacle grid map based on an optimized A* search algorithm.
[0124] When the traditional 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.
[0125] 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.
[0126] The optimized A* search algorithm in this embodiment is described below at a certain moment in the middle:
[0127] First, expand the current parent node in the obstacle grid map. Fig. 9FIG. 2 shows an example of performing node expansion in an obstacle grid map according to an embodiment of the present invention. Fig. 9 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. 9 In the k As it continued to expand, it collided with an obstacle ( Fig. 9 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.
[0128] 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:
[0129] g(n i )=g(n parent )+w dn d(n parent , n i ))+w slip slip(n i )
[0130] Among them, wdn 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.
[0131] 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:
[0132] g(n i )=g(n parent )+w dn d(n parent , n i )+w s s(n i )+w t t(n parent , n i )+w obs d obs (n i )+w slip slip(n i )
[0133] 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.
[0134] 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.
[0135] 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:
[0136]
[0137] 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.
[0138] 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.
[0139] 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:
[0140]
[0141] 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:
[0142]
[0143] 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:
[0144] f(n i )=w g g(n i )+w h ζh(n i )
[0145] 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:
[0146]
[0147] 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.
[0148] In some other embodiments, the obstacle grid map in step S10 is determined by the following method:
[0149] 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.
[0150] 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.
[0151] Fig.10 FIG. 2 shows a schematic diagram of a process for generating an obstacle grid map according to an embodiment of the present invention. Fig.10 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.
[0152] In other embodiments, after the obstacle grid map is obtained, a height information grid map of the target scene may be constructed based on the obstacle grid map.
[0153] The following is an explanation of the construction method of the height information grid map. The height information grid map can be determined by the following method:
[0154] Step S1101: determining a traversable area from the obstacle grid map based on a region growing segmentation algorithm.
[0155] In this embodiment, the vehicle position can be selected as the only seed point and the growth area, and then the adjacent grids around the growth area are traversed in the obstacle grid map based on the breadth-first search method, and the passable grids are marked as visited and added to the current growth area. The growth process is repeated until the growth area remains unchanged, and the stop criterion is reached. At this time, the queue stored in the growth area represents the current passable area.
[0156] Step S1102: Acquire point cloud data of the traversable area.
[0157] Step S1103: In the traversable area, the average value of the point cloud clusters in the same grid is used as the height information of the grid to obtain a height information grid map.
[0158] In this embodiment, after the current passable area is obtained, a grid map containing scene height information can be obtained by assigning a value to each grid through point cloud data.
[0159] In this embodiment, for the optimized A* search algorithm, the expansion area of the child node can be determined by the expansion area of the child node in the obstacle grid map and the height information of the child node and the current parent node. In this embodiment, the obstacle grid map and the height information grid map can be used to modify the node expansion method in the traditional A* search algorithm. Fig. 9 The example below illustrates the process.
[0160] In this embodiment, the plane subnode n obtained by expanding the obstacle grid map can be i (x i ,y i ) and the current parent node n p (x p ,y p ) is projected onto the height information grid map to obtain the child node position n with height information i (x i ,y i ,z i ) and the current parent node position n p (x p ,y p ,z p ). Here, you can use the grid position coordinates to index between different maps. After that, you can use the height change rate △h between the child node and the current parent node i Add a height layer reduction factor η to the expansion radius of the child node i , which is calculated as follows:
[0161]
[0162] η i =1-η
[0163] Among them, △h max is the maximum height change rate of all child nodes in the current parent node. Finally, the reduction coefficient η of each child node i Its extended radius R in the obstacle grid map o Multiply to get the final expansion radius R i For: R i =η i ×R o In this embodiment, by reducing the expansion radius of the node in places where the height change is large, the safety of the planned path can be improved.
[0164] Compared with the prior art, the embodiments of the present invention can achieve at least one of the following beneficial effects:
[0165] 1. The solution of the present invention combines multi-layer maps, expands the A* search algorithm based on binary plane grid maps to multi-layer maps, and uses the information in the obstacle layer, height layer, and vehicle sliding parameter estimation layer to expand the nodes in the A* search algorithm and modify the cost function, and finally outputs a safe and collision-free path in a three-dimensional scene.
[0166] 2. At the same time, in order to address 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 A* search algorithm to achieve avoidance and deceleration of high sliding parameter risk areas in off-road scenes, thereby improving the safety of the path planning method and reducing the difficulty of trajectory tracking.
[0167] 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 method for planning a vehicle motion path, characterized in that: The following steps are involved: Obtain the obstacle grid map and sliding parameter prediction 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 A* search algorithm; wherein, 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. 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; wherein the estimated value of the vehicle sliding parameter at the child node position is obtained by indexing the sliding parameter prediction grid map.
2. The planning method according to claim 1, characterized in that: The following steps are also included: Get the height information grid map of the target scene; where: In the optimized A* search algorithm, the expansion area of the child node is determined by the expansion area of the child node in the obstacle grid map and the height information of the child node and the current parent node position; wherein the height information of the child node and the current parent node position is obtained by indexing the height information grid map.
3. The planning method according to claim 2, characterized in that: The movement cost of the child node also includes the turning cost and the weight thereof between the child node and the current parent node determined by the direction of the current parent node and the extension direction of the child node.
4. The planning method according to claim 2 or 3, characterized in that: The movement cost of the child node also includes an obstacle distance cost determined by the shortest distance between the child node and the obstacle.
5. The planning method according to claim 4, characterized in that: The movement cost of the subnode also includes the subnode stability cost and its weight determined by the pitch angle and roll angle of the vehicle at the subnode and the maximum pitch angle and maximum roll angle allowed by the vehicle.
6. The planning method according to claim 1, characterized in that: The heuristic function value of the child node is determined by the distance from the child node to the target position.
7. The planning method according to claim 2, characterized in that: The extended area of the child node in the obstacle grid map is determined by: When the distance between the subnode and the obstacle is greater than the first threshold, the expansion radius of the subnode is the maximum preset value; When the distance between the subnode and the obstacle is less than the first threshold and greater than the minimum preset value, the expansion radius of the subnode is the distance between the subnode and the obstacle; When the distance between a child node and an obstacle is less than a minimum preset value, the child node is not expanded.
8. The planning method according to claim 2, characterized in that: The obstacle grid map is determined by the following method: Based on the obstacle detection algorithm, determine a basic obstacle map of the target scene, wherein the format of the basic obstacle map is a grid; Obstacles in the basic obstacle map are expanded according to the geometric size of the vehicle and discrete areas in the basic obstacle map are filled to obtain an obstacle grid map.
9. The planning method according to claim 8, characterized in that: The height information grid map is determined by the following method: Determining a traversable area from the obstacle grid map based on a region growing segmentation algorithm; Acquire point cloud data of the traversable area; In the traversable area, the average value of the point cloud clusters in the same grid is used as the height information of the grid to obtain a height information grid map.
10. The planning method according to claim 1, 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.
Citation Information
Patent Citations
Unmanned local path planning method based on equal-step sampling A* algorithm
CN108444488A
Motion planning method for ensuring safe driving of unmanned vehicle on three-dimensional terrain
CN113044029A
Mobile robot complex path planning method based on A-Star algorithm
CN116414139A
Cited By
Intelligent monitoring system for moving trajectory data of sand blasting equipment based on artificial intelligence
CN119251226A
Multi-map path planning method and device
CN119803502A
