Unmanned vehicle dynamic detection and path optimization method based on multistage Haar wavelet enhancement

Abstract

The invention provides an unmanned vehicle dynamic detection and path optimization method based on multistage Haar wavelet enhancement, and aims to solve the problems of path planning delay and unmanned vehicle collision risk caused by insufficient real-time obstacle sensing capability of an unmanned vehicle in a dynamic environment. According to the method, firstly, point cloud intensity data are obtained through a 3D laser radar, a time sequence enhanced multistage Haar wavelet decomposition technology is introduced, edge features are extracted by means of three-stage wavelet transform layering, a high-frequency component captures a dynamic obstacle detail contour, and a low-frequency component retains a static background structure; on the basis, a principal component analysis method is adopted to reduce the data dimension, feature classification is realized through a lightweight convolutional neural network, and finally, a clustering algorithm is utilized to embed dynamic obstacle information into an evaluation function of a dynamic window method in real time, so that the local path generation efficiency is optimized. Through the method, the risk of collision of the unmanned vehicle in a dynamic environment is reduced, and the response speed of local path planning is improved.

Description

Technical Field

[0001] The present invention relates to the technical field of unmanned vehicle navigation, and specifically to a method for dynamic detection and path optimization of unmanned vehicles based on multi-level Haar wavelet enhancement. Background Technique

[0002] With the rapid development of robot technology, 3D lidar, as an important environmental perception sensor, plays a crucial role in fields such as robot navigation, obstacle avoidance, and map construction. At present, the navigation function of unmanned vehicles is of great importance. It ensures that the vehicle can drive autonomously in a complex environment safely and accurately, effectively avoid obstacles and adapt to dynamic changes, thereby improving driving safety and user experience. However, when unmanned vehicles perform navigation tasks, because the existing path planning algorithms are generally based on static environment models, they cannot effectively cope with the changes of dynamic obstacles, resulting in potential collision risks. In addition, the flexibility and adaptability are weak, and the path cannot be adjusted in real time to cope with emergencies, which reduces efficiency and user experience, thus performing poorly in complex and dynamic environments, and there are problems such as low planning efficiency leading to extended navigation time.

[0003] To solve the above problems, a method of integrating a dynamic obstacle detection algorithm into the path planning algorithm is proposed, and dynamic obstacles are identified in real time through laser intensity information. The specific process includes using a multi-level Haar wavelet fusion method with temporal enhancement to extract feature points, performing PCA dimensionality reduction processing, using a CNN convolutional neural network of the Voxel Net network for deep learning, and finally performing clustering through the K-Means algorithm. This method can effectively detect dynamic obstacles and integrate the detection results into the real-time update evaluation function of the DWA algorithm, thereby enhancing the safety of path planning and reducing the impact risk.

[0004] The literature "Wavelet-Based Dynamic Obstacle Detection for Mobile Robots in Unstructured Environments" proposed a real-time detection method for dynamic obstacles in complex unstructured environments. This method takes lidar point clouds as input, extracts multi-scale spatial features through optimized wavelet transform technology, and combines temporal analysis to distinguish dynamic and static obstacles. However, this literature does not involve the improvement of multi-level Haar wavelets and the fusion of dynamic weights.

[0005] The literature "Robot Path Planning Based on Improved A* Algorithm and Dynamic Window Method" improved the global path planning A* algorithm and the dynamic window method accordingly and integrated them. This improved method adopts an obstacle avoidance strategy, optimizes the way of selecting nodes, and increases the safety of the path; for the problems that the planning effect is not good when there are obstacles near the target point of the traditional dynamic window method and it is easy to fall into local optimality in concave groove-like obstacles, distance deviation and trajectory deviation are introduced into the original evaluation function. However, the shortcoming of this method is that it does not consider the impact of dynamic obstacles on the local path planning process.

[0006] The literature "Research on the Optimization and Fusion Strategy Based on A* and DWA Algorithms" proposed an optimized fusion strategy based on the A* algorithm and the DWA algorithm. By introducing a dynamic weight factor of environmental complexity, the evaluation function of the A* algorithm is optimized to improve the adaptability of the algorithm; the key nodes in the global path generated by the optimized A* algorithm are used as the temporary target points of the DWA algorithm for path planning to achieve the balance between global path optimization and real-time obstacle avoidance. However, this method has problems such as not considering the existence of local optimal solutions in the DWA algorithm and the response efficiency not being fast enough when facing dynamic obstacles. Summary of the Invention

[0007] In view of the deficiencies of the prior art, the present invention provides a method for dynamic detection and path optimization of an unmanned vehicle based on multi-level Haar wavelet enhancement, which solves the problems raised in the above background technology. The method is characterized by including the following steps:

[0008] S1: Path planning algorithm:

[0009] S1.1: The global path planning algorithm adopts an improved version of the A* search algorithm. This algorithm proposes a cost function f(n), which consists of two parts: g(n) and h(n). Among them, g(n) represents the actual path cost from the starting point to the current node n, while h(n) represents a heuristic estimate, that is, the estimated minimum cost from node n to the end point. This cost function f(n) = g(n) + h(n) is continuously optimized during the execution of the algorithm to ensure finding the global optimal path from the starting point to the end point; the algorithm first selects the node with the minimum cost in the open list for expansion and moves it to the closed list. In the closed list, this node will be marked as having been processed. Repeat this process until the target node is found or the open list is empty.

[0010] The cost function of the A* algorithm is expressed as:

[0011] f(n) = g(n) + h(n) (1)

[0012] Wherein, n represents the current node; f(n) represents the comprehensive cost index at node n, that is, the objective function of the A* algorithm. The smaller f(n) is, the higher the priority of node n; g(n) represents the distance value from the starting point to the current node n; h(n) represents the estimated distance from the current node n to the target point. The calculation models of the distance estimation function h(n) mainly include:

[0013] Euclidean distance model:

[0014]

[0015] Manhattan distance model:

[0016] h(n) = |x n - x g | + |y n - y g | (3)

[0017] Wherein, (x n , y n ) represents the current position, and (x g , y g ) represents the target point position.

[0018] S1.2: The unmanned vehicle plans a globally optimal path through the A* global path planning algorithm in the prior map. In order to enable the unmanned vehicle to avoid dynamic obstacles in a timely manner, the unmanned vehicle needs to sense the obstacles in the local map to prevent the unmanned vehicle from colliding with them. The traditional dynamic window method only works when it is very close to the dynamic obstacle, treating the obstacle statically. When encountering it, a collision occurs due to insufficient reaction space. Therefore, the local path planning algorithm of the present invention selects a fusion algorithm of a dynamic obstacle detection method based on 3D lidar point cloud intensity information and the dynamic window method (Dynamic Window Approach, DWA). This algorithm can dynamically generate a series of feasible speed instruction sets according to the dynamic characteristics of the unmanned vehicle and the real-time dynamic obstacle information, and then screen out the optimal speed instruction from them to guide the immediate movement of the unmanned vehicle and complete the real-time obstacle avoidance of the unmanned vehicle.

[0019] The motion model of the unmanned vehicle is:

[0020]

[0021] Wherein, Δt represents the sampling time, x(t) and x(t - 1) represent the abscissas of the unmanned vehicle at times t and t - 1; y(t) and y(t - 1) represent the ordinates of the unmanned vehicle at times t and t - 1; θ(t) and θ(t - 1) represent the angles between the unmanned vehicle and the X-axis at times t and t - 1; v(t) is the linear velocity of the unmanned vehicle at time t; ω(t) is the angular velocity of the unmanned vehicle at time t.

[0022] The mathematical model of the DWA algorithm can be expressed as:

[0023] V f easible = v|v ∈ [v min ,v max ,a ∈ [a min ,a max (5)

[0024] In the formula, V f easible represents the set of feasible velocity commands, v and a represent velocity and acceleration respectively, v min and v max represent the minimum and maximum velocities that the robot can reach respectively, a min and a max represent the minimum and maximum accelerations that the robot can generate respectively.

[0025] The dynamic obstacle detection method based on the 3D lidar point cloud intensity information is as follows.

[0026] S2: Initialize the robot and environment settings: Place the robot in an unknown scenario, and start the 3D lidar to collect depth information and point cloud intensity information in the environment;

[0027] S3: Data preprocessing: Preprocess and filter the collected raw data to remove noise and interference, improve the data quality, and at the same time normalize the point cloud intensity information to make it have better comparability and analyzability;

[0028] In the step of S3: data preprocessing, when removing noise from the collected raw data, first determine the grid spacing according to the average point spacing of the point cloud data points, then establish a three-dimensional grid of the point cloud, establish an outer space index grid based on the three-dimensional grid established by the point cloud, then count the number of points contained in each grid in the index grid, then set the index structure window, then traverse the index grid with the structure window as the basic unit to find the cubic grid containing only one data point, judge the discrete noise points according to the index result of the structure window, remove the data points of the determined discrete noise points, re-count the number of points contained in the grid in the index grid, randomly select an index grid as the seed grid, perform a diffusion operation, then remove the seed grid marked in the first diffusion operation, select a new seed grid again for diffusion operation until all the grids with points are marked, finally count the number of data points involved in each diffusion operation, retain the point cloud with the most points, determine the other data points as clustered noise points, and remove the clustered noise points.

[0029] The S3: method for filtering the collected raw data in the data preprocessing step: identifying gross error points in the laser radar point cloud and removing the gross error points, performing laser radar point cloud segmentation based on smooth surface growth to obtain objects, analyzing the multi-echo ratio characteristics of the object to identify potential ground objects, and removing the ground objects and the laser radar points contained in them, extracting the feature points of the object, and using the feature points to replace the original laser radar points contained in the object to participate in subsequent operations, performing object category discrimination based on the feature points, and updating the category of the original laser radar points contained in the object.

[0030] S4: Point cloud intensity information analysis:

[0031] S4.1: The laser intensity signal is decomposed into multiple scales by wavelet transform to reveal its characteristics at different frequencies. The formula of wavelet transform can be expressed as:

[0032]

[0033] Where f(t) is the input laser intensity signal, is the wavelet basis function, a is the scale parameter, b is the translation parameter, and W(a,b) is the result of wavelet transform.

[0034] Wavelet basis functions need to be used in the wavelet transform process, and the present invention uses an improved Haar wavelet. Haar wavelet uses a series of scaled and translated Haar waveforms to analyze signals, and can provide localized information of signals in both time and frequency dimensions. Haar wavelet has a short support length and a fast calculation speed, and can play a good role in dynamic obstacle edge detection with high real-time requirements. However, Haar wavelet single-frame decomposition only uses spatial information, and it is difficult to distinguish between dynamic obstacles (such as moving vehicles) and static artifacts (such as falling leaves), resulting in a high false detection rate. In view of the above problems, the present invention improves a time-series enhanced multi-level Haar wavelet fusion method to reduce the false detection rate of Haar wavelets.

[0035] The original point cloud output by the LiDAR is unstructured and needs to be converted into a regular three-dimensional voxel grid for wavelet transform. Voxelization divides the space into uniform cubic grids and counts the mean coordinates of the points in each voxel to obtain a structured 3D matrix V. t ∈R H×W×D , where H is the height, W is the width, and D is the depth. Then the voxel matrix V t It decomposes the image in three dimensions (x, y, z) step by step, generating low-frequency (L) and high-frequency (H) components at each level, where the high-frequency component captures edges and dynamic changes, and the low-frequency component retains the overall structure.

[0036] For a certain dimension of the voxel matrix V:

[0037] Low-frequency coefficients:

[0038]

[0039] High-frequency coefficients:

[0040]

[0041] Where i is the decomposition level.

[0042] Calculate the temporal variance of the high-frequency components for consecutive multiple frames to quantify the dynamics of the local region. Dynamic obstacles will cause significant fluctuations in the high-frequency components due to position changes, while the variance of the static background is low.

[0043] Temporal mean:

[0044]

[0045] Where μ i (x, y, z) is the temporal mean of the high-frequency component at the i-th level at position (x, y, z), T is the size of the temporal window, is the high-frequency component of a certain level in the t-th frame.

[0046] Unbiased variance time series:

[0047]

[0048] Where Var i (x, y, z) is the temporal variance of the high-frequency component at the i-th level at position (x, y, z), used to quantify the dynamics.

[0049] Set the threshold to 20% of the global maximum of the variance, mark the high-variance region as the dynamic candidate area, and the low-variance region as the static area.

[0050] Subsequently, perform the fusion of multi-level coefficients.

[0051] Dynamic region fusion weight:

[0052]

[0053] Where α i is the weight of the high-frequency component at the i-th level in the dynamic region.

[0054] Dynamic region fusion weight coefficient:

[0055]

[0056] Where λ is the variance influence coefficient, controlling the influence intensity of the variance on the weight.

[0057] Static region fusion weight:

[0058]

[0059] In the formula, β i is the weight of the i-th level high-frequency component in the static region.

[0060] Static region fusion weight coefficient:

[0061]

[0062] The dynamic region enhances the fine edges to improve the sensitivity, and the static region focuses on the stable structure to suppress the noise interference. Finally, an enhanced three-dimensional edge map is generated, and accurate obstacle contour information is output in real time to effectively distinguish moving targets from the environmental background.

[0063] S4.2: Use the principal component analysis method to reduce the dimension of the extracted features to reduce the complexity of subsequent processing. PCA maps the original data to a new low-dimensional space through a linear transformation while retaining the variance information of the original data as much as possible. The goal of PCA is to minimize the reconstruction error, that is:

[0064]

[0065] In the formula, x i is the original data vector, and U is the projection matrix, whose column vectors are the principal components.

[0066] S4.3: Use a deep learning algorithm to classify and identify the processed features. In the present invention, a convolutional neural network is adopted, and a specific Voxel Net model for 3D point cloud data is used. The Voxel Net network mainly consists of point cloud voxelization, voxel feature extraction, convolutional intermediate layer, and region proposal network. For point cloud voxelization, assuming that the depth, width, and height ranges of the point cloud along the x, y, and z directions are d, ω, and h respectively, define the sizes of each voxel as v d 、v ω and v h , then the size of the final three-dimensional voxel grid is and D, W, and H are the depth, width, and height of the three-dimensional voxel grid respectively.

[0067] Its convolutional layer operation can be expressed as:

[0068]

[0069] In the formula, is the element at the i-th row and j-th column of the output feature map of the l-th layer, f is the activation function, is the element at the m-th row and n-th column of the convolutional kernel of the l-th layer, is the corresponding position element of the output feature map of the (l - 1)-th layer, b l is the bias term.

[0070] S5: Obstacle Detection and Recognition:

[0071] S5.1: Set an intensity threshold to distinguish between obstacle and non-obstacle regions. For regions with intensity higher than the threshold, further use the K-means clustering algorithm to cluster adjacent obstacle points into independent obstacle objects. The iterative update formula of the K-means algorithm is:

[0072]

[0073] In the formula, is the center point of the k-th cluster at the t-th iteration, is the sample set belonging to the k-th cluster at the t-th iteration, x i is the sample point.

[0074] S5.2: Use a deep learning model to perform fine classification and recognition on the clustered obstacle objects, and learn the features of different types of obstacles through training the model to achieve high-precision obstacle recognition;

[0075] S6: Integrate the dynamic obstacle detection algorithm based on 3D lidar point cloud intensity data with the DWA algorithm: The dynamic obstacles in the environment can be monitored in real time and their motion predictions can be incorporated into the speed selection process of DWA. Specifically, first use the dynamic obstacle detection algorithm to obtain point cloud data from the 3D lidar, identify and track the trajectories of surrounding dynamic obstacles in real time, and generate prediction information about their future positions. Then, in DWA, use this prediction information to evaluate the safety of the generated speed combinations, specifically reflected in updating the evaluation function, and try to avoid collisions by increasing the distance cost to dynamic obstacles, and select the optimal action instruction.

[0076] The present invention provides a dynamic obstacle detection and path planning optimization method based on point cloud intensity information, having the following beneficial effects:

[0077] 1. The dynamic obstacle detection method based on point cloud intensity information proposed by the present invention analyzes the intensity signal of the lidar through the time-series enhanced multi-level Haar wavelet fusion method, can detect obstacles more accurately and extract key information of the obstacles, extracts multi-scale edge features of the point cloud through the improved time-series enhanced multi-level Haar wavelet fusion method, and combines time-series variance analysis to dynamically adjust the feature weights to distinguish dynamic obstacles from static backgrounds;

[0078] 2. In the stage of obstacle detection and recognition, this solution uses an improved algorithm to detect and classify obstacles, applies machine learning or deep learning algorithms to classify and recognize the processed features, distinguishes the obstacle and non-obstacle areas by setting an intensity threshold, and uses a clustering algorithm to cluster adjacent obstacle points into independent obstacle objects, which helps to separate the obstacles from the background and provides a clear boundary for further classification and recognition;

[0079] 3. By integrating the dynamic obstacle detection information into the DWA algorithm, the navigation effect of the overall path planning has been significantly improved. After introducing dynamic obstacle detection, the system can update the evaluation function based on the obstacle information obtained in real time, making the path planning more flexible and efficient. In a dynamic environment, an autonomous vehicle often faces unexpected events (such as a pedestrian suddenly appearing or another vehicle changing lanes, etc.), and the original static path planning method is difficult to quickly adapt to these changes. By integrating the dynamic obstacle detection algorithm, the system can monitor and analyze dynamic obstacles in real time and adjust the vehicle's motion strategy. This not only improves the safety of navigation, effectively avoids obstacles, but also optimizes the driving path, reduces unnecessary stops or detours, and enhances the coherence of driving. BRIEF DESCRIPTION OF THE DRAWINGS

[0080] In order to more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the following will briefly introduce the drawings required for the description of the embodiments or the prior art. Obviously, the following drawings are only some embodiments of the present invention. For those of ordinary skill in the art, other drawings can be obtained based on these drawings without creative efforts.

[0081] Figure 1 It is a flowchart of a dynamic detection and path optimization method for an autonomous vehicle enhanced by multi-level Haar wavelets;

[0082] Figure 2 It is a physical diagram of the autonomous vehicle and the 3D lidar used in this experiment;

[0083] Figure 3 It is a schematic diagram of detecting dynamic obstacles by improving the lidar intensity information according to the present invention;

[0084] Figure 4 It is a schematic diagram of detecting dynamic obstacles by traditional lidar intensity information;

[0085] Figure 5 It is a schematic diagram of the characteristic results of the present invention compared with the traditional method;

[0086] Figure 6 It is a schematic diagram of the simulation results of the classic path planning algorithm under a grid map;

[0087] Figure 7 It is a schematic diagram of the simulation result of the improved algorithm under the grid map. Specific implementation manners

[0088] To make the above objects, features and advantages of the present invention more obvious and understandable, a dynamic detection and path optimization method for an unmanned vehicle based on multi-level Haar wavelet enhancement, as Figure 1 shown, includes the following steps:

[0089] S1: Path planning algorithm:

[0090] S1.1: The global path planning algorithm adopts an improved version of the A* search algorithm. This algorithm proposes a cost function f(n), which consists of two parts: g(n) and h(n). Among them, g(n) represents the actual path cost from the starting point to the current node n, and h(n) represents a heuristic estimate, that is, the estimated minimum cost from node n to the end point. This cost function f(n)=g(n)+h(n) is continuously optimized during the execution of the algorithm to ensure finding the global optimal path from the starting point to the end point; the algorithm first selects the node with the minimum cost in the open list for expansion and moves it to the closed list. In the closed list, this node will be marked as having been processed. Repeat this process until the target node is found or the open list is empty.

[0091] The cost function of the A* algorithm is expressed as:

[0092] f(n)=g(n)+h(n) (1)

[0093] In the formula, n represents the current node; f(n) represents the comprehensive cost index at node n, that is, the objective function of the A* algorithm. The smaller f(n) is, the higher the priority of node n; g(n) represents the distance value from the starting point to the current node n; h(n) represents the distance estimate value of the current node n from the target point. The calculation models of the distance estimate function h(n) mainly include.

[0094] Euclidean distance model:

[0095]

[0096] Manhattan distance model:

[0097] h(n)=|x n -x g |+|y n -y g | (3)

[0098] In the formula, (x n ,y n ) represents the current position, (x g,y g ) represents the position of the target point.

[0099] S1.2: The unmanned vehicle plans a globally optimal path in the prior map through the A* global path planning algorithm. In order to enable the unmanned vehicle to avoid dynamic obstacles in a timely manner, the unmanned vehicle needs to sense the obstacles in the local map to prevent collisions with them. The traditional dynamic window method only works when it is very close to dynamic obstacles, treating the obstacles statically, and collisions occur due to insufficient reaction space when they meet. Therefore, the local path planning algorithm of the present invention selects a fusion algorithm of a dynamic obstacle detection method based on 3D lidar point cloud intensity information and the Dynamic Window Approach (DWA). This algorithm can dynamically generate a series of feasible speed instruction sets based on the dynamic characteristics of the unmanned vehicle and real-time dynamic obstacle information, and then screen out the optimal speed instruction from them to guide the immediate movement of the unmanned vehicle and complete the real-time obstacle avoidance of the unmanned vehicle.

[0100] The motion model of the unmanned vehicle is:

[0101]

[0102] In the formula, Δt represents the sampling time, x(t) and x(t - 1) represent the abscissas of the unmanned vehicle at times t and t - 1; y(t) and y(t - 1) represent the ordinates of the unmanned vehicle at times t and t - 1; θ(t) and θ(t - 1) represent the angles between the unmanned vehicle and the X-axis at times t and t - 1; v(t) is the linear velocity of the unmanned vehicle at time t; ω(t) is the angular velocity of the unmanned vehicle at time t.

[0103] The mathematical model of the DWA algorithm can be expressed as:

[0104] V f easible = v|v ∈ [v min ,v max , a ∈ [a min ,a max (5)

[0105] In the formula, V f easible represents the set of feasible speed instructions, v and a represent speed and acceleration respectively, v min and v max represent the minimum and maximum speeds that the robot can reach respectively, a min and a max represent the minimum and maximum accelerations that the robot can generate respectively.

[0106] S2: Initialize the robot and environment settings: Place the robot in an unknown scene and start the 3D lidar to collect depth information and point cloud intensity information in the environment.

[0107] S3: Data preprocessing: Preprocess and filter the collected raw data to remove noise and interference, improve data quality, and normalize the point cloud intensity information to make it more comparable and analytical.

[0108] In the step S3: when removing noise from the collected raw data in the data preprocessing step, first determine the grid spacing based on the average point spacing of the point cloud data points, then establish a three-dimensional grid of the point cloud, then establish a peripheral spatial index grid based on the three-dimensional grid established by the point cloud, then count the number of points contained in each grid in the index grid, then set the index structure window, then traverse the index grid with the structure window as the basic unit, find the cubic grid containing only one data point, judge the discrete noise point according to the index result of the structure window, remove the data points judged as discrete noise points, re-count the number of points contained in the grid in the index grid, randomly select the index grid as the seed grid, perform diffusion operation, then remove the seed grid marked in the first diffusion operation, select a new seed grid again to perform diffusion operation until all the grids with points are marked, finally count the number of data points involved in each diffusion operation, retain the point cloud containing the most points, judge the other data points as clustered noise points, and remove the clustered noise points.

[0109] The S3: method for filtering the collected raw data in the data preprocessing step: identifying gross error points in the laser radar point cloud and removing the gross error points, performing laser radar point cloud segmentation based on smooth surface growth to obtain objects, analyzing the multi-echo ratio characteristics of the object to identify potential ground objects, and removing the ground objects and the laser radar points contained in them, extracting the feature points of the object, and using the feature points to replace the original laser radar points contained in the object to participate in subsequent operations, performing object category discrimination based on the feature points, and updating the category of the original laser radar points contained in the object.

[0110] S4: Point cloud intensity information analysis:

[0111] S4.1: The laser intensity signal is decomposed into multiple scales by wavelet transform to reveal its characteristics at different frequencies. The formula of wavelet transform can be expressed as:

[0112]

[0113] Where f(t) is the input laser intensity signal, is the wavelet basis function, a is the scale parameter, b is the translation parameter, and W(a,b) is the result of wavelet transform.

[0114] Wavelet basis functions need to be used in the wavelet transform process, and the present invention uses an improved Haar wavelet. Haar wavelet uses a series of scaled and translated Haar waveforms to analyze signals, and can provide localized information of signals in both time and frequency dimensions. Haar wavelet has a short support length and a fast calculation speed, and can play a good role in dynamic obstacle edge detection with high real-time requirements. However, Haar wavelet single-frame decomposition only uses spatial information, and it is difficult to distinguish between dynamic obstacles (such as moving vehicles) and static artifacts (such as falling leaves), resulting in a high false detection rate. In view of the above problems, the present invention improves a time-series enhanced multi-level Haar wavelet fusion method to reduce the false detection rate of Haar wavelets.

[0115] The original point cloud output by the LiDAR is unstructured and needs to be converted into a regular three-dimensional voxel grid for wavelet transform. Voxelization divides the space into uniform cubic grids and counts the mean coordinates of the points in each voxel to obtain a structured 3D matrix V. t ∈R H×W×D , where H is the height, W is the width, and D is the depth. Then the voxel matrix V t It decomposes the image in three dimensions (x, y, z) step by step, generating low-frequency (L) and high-frequency (H) components at each level, where the high-frequency component captures edges and dynamic changes, and the low-frequency component retains the overall structure.

[0116] For a certain dimension of the voxel matrix V:

[0117] Low frequency coefficients:

[0118]

[0119] High frequency coefficients:

[0120]

[0121] In the formula, i is the number of decomposition levels.

[0122] The temporal variance of the high-frequency components of multiple consecutive frames is calculated to quantify the dynamics of the local area. Dynamic obstacles will cause significant fluctuations in the high-frequency components due to position changes, while the variance of the static background is low.

[0123] Time series mean:

[0124]

[0125] In the formula, μ i (x, y, z) is the time series mean of the i-th level high frequency component at position (x, y, z), T is the time series window size, is a certain level of high frequency component in the tth frame.

[0126] Unbiased variance time series:

[0127]

[0128] Wherein, Var i (x, y, z) is the temporal variance of the i-th level high-frequency component at the position (x, y, z), which is used to quantify the dynamics.

[0129] Set the threshold to 20% of the global maximum value of the variance. Mark the high-variance region as the dynamic candidate region and the low-variance region as the static region.

[0130] Subsequently, multi-level coefficient fusion is performed.

[0131] Dynamic region fusion weight:

[0132]

[0133] Wherein, α i is the weight of the i-th level high-frequency component in the dynamic region.

[0134] Dynamic region fusion weight coefficient:

[0135]

[0136] Wherein, λ is the variance influence coefficient, which controls the influence intensity of the variance on the weight.

[0137] Static region fusion weight:

[0138]

[0139] Wherein, β i is the weight of the i-th level high-frequency component in the static region.

[0140] Static region fusion weight coefficient:

[0141]

[0142] The dynamic region enhances the fine edges to improve the sensitivity, and the static region focuses on the stable structure to suppress the noise interference. Finally, an enhanced three-dimensional edge map is generated, and accurate obstacle contour information is output in real time to effectively distinguish the moving target from the environmental background.

[0143] S4.2: Use the principal component analysis method to reduce the dimension of the extracted features to reduce the complexity of subsequent processing. PCA maps the original data to a new low-dimensional space through linear transformation, while retaining the variance information of the original data as much as possible. The goal of PCA is to minimize the reconstruction error, that is:

[0144]

[0145] Wherein, xi is the original data vector, and U is the projection matrix whose column vectors are the principal components.

[0146] S4.3: Use deep learning algorithms to classify and identify the processed features. In this invention, a convolutional neural network is adopted, and a specific Voxel Net model for 3D point cloud data is used. The Voxel Net network mainly consists of point cloud voxelization, voxel feature extraction, convolutional intermediate layer, and region proposal network. For point cloud voxelization, assuming that the depth, width, and height ranges of the point cloud along the x, y, and z directions are d, ω, and h respectively, and defining the sizes of each voxel as v d , v ω and v h , then the size of the final three-dimensional voxel grid is and D, W, and H are the depth, width, and height of the three-dimensional voxel grid respectively.

[0147] Its convolutional layer operation can be expressed as:

[0148]

[0149] In the formula, is the element at the i-th row and j-th column of the output feature map of the l-th layer, f is the activation function, is the element at the m-th row and n-th column of the convolutional kernel of the l-th layer, is the corresponding position element of the output feature map of the (l - 1)-th layer, and b l is the bias term.

[0150] S5: Obstacle detection and recognition:

[0151] S5.1: Set an intensity threshold to distinguish between obstacle and non-obstacle regions. For regions with intensity higher than the threshold, further use the K-means clustering algorithm to cluster adjacent obstacle points into independent obstacle objects. The iterative update formula of the K-means algorithm is:

[0152]

[0153] In the formula, is the center point of the k-th cluster at the t-th iteration, is the sample set belonging to the k-th cluster at the t-th iteration, and x i is the sample point.

[0154] S5.2: Use the deep learning model to perform fine classification and recognition on the clustered obstacle objects, and learn the features of different types of obstacles through training the model to achieve high-precision obstacle recognition.

[0155] S6: Integrate the dynamic obstacle detection algorithm based on 3D lidar point cloud intensity data with the DWA algorithm: The movement prediction of dynamic obstacles in the environment can be monitored in real time and incorporated into the speed selection process of DWA. Specifically, first, use the dynamic obstacle detection algorithm to obtain point cloud data from the 3D lidar, identify and track the trajectories of surrounding dynamic obstacles in real time, and generate prediction information about their future positions. Then, in DWA, use this prediction information to evaluate the safety of the generated speed combinations, specifically by updating the evaluation function and minimizing collisions by increasing the distance cost to dynamic obstacles, and select the optimal action instruction.

[0156] Example: The experiment was conducted on a computer (RAM: 8.00GB; Processor: Intel(R) Core(TM) i5-9300H) with the Ubuntu 18.04-Melodic version system. The lidar is a 16-line lidar of the RoboSense model RS-LIDAR-16. Figure 2 Pictures of the unmanned vehicle and the 3D lidar used in this experiment.

[0157] The present invention is verified below in combination with the test results. Figure 3 Schematic diagram for detecting dynamic obstacles by the present invention. It can be seen that the intensity is higher at the edge of the dynamically moving vehicle. The present invention marks the edge of the dynamic obstacle in red and the static environment in green. Figure 4 Schematic diagram for detecting dynamic obstacles by traditional lidar intensity information. This figure is a direct mapping of the laser intensity to the single-frame point cloud intensity of the environment without algorithm intervention. Figure 5 Through comparison, it can be seen that the intensity characteristics of the vehicle hood part within the white solid line frame are more obvious, and the color characteristics of the right edge of the vehicle in this method are more obviously bright red, while the color of the vehicle edge in the classical method is uniform and the intensity is not prominent. Figure 6 Schematic diagram of the path planning algorithm integrating the classical A* algorithm and the DWA algorithm. Figure 7 Schematic diagram of the improved path planning algorithm of the present invention. When the initial position and the target location of the unmanned vehicle are the same, the present invention significantly improves the path planning performance in complex dynamic scenarios through real-time and accurate perception of dynamic obstacles and multi-scale feature fusion. While ensuring the optimality of the global path, the path length is shortened from 41.769m to 39.546m. Figure 7 The cube in is a simulated dynamic obstacle, moving in a uniform straight line on the path. Figure 7 It can be seen that the unmanned vehicle does not collide with the dynamic obstacle through the algorithm on the planned global path, successfully avoiding the dynamic obstacle, effectively solving the problems such as the overly long path caused by frequent replanning and missed detection of dynamic targets due to the classical algorithm's reliance on the static environment assumption, and having the advantages of both safety and efficiency.

[0158] As described above, it is only a preferred specific embodiment of the present invention, but the protection scope of the present invention is not limited thereto. Any person skilled in the art within the technical scope disclosed by the present invention should cover within the protection scope of the present invention by making equivalent substitutions or changes according to the technical solution and inventive concept of the present invention.

Claims

1. A dynamic detection and path optimization method for autonomous vehicles based on multi-level Haar wavelet enhancement, characterized in that, Including the following steps: S1: Initialize the unmanned vehicle and environmental settings: Place the unmanned vehicle in an unknown scenario and start the 3D lidar to collect depth information and point cloud intensity information in the environment; S2: Data preprocessing: Preprocess and filter the collected raw data to remove noise and interference, improve data quality, and at the same time normalize the point cloud intensity information to make it have better comparability and analyzability; S3: Analysis of point cloud intensity information S3.1: Perform multi-scale decomposition on the laser intensity signal through wavelet transform to reveal its characteristics at different frequencies. The formula for wavelet transform can be expressed as: where \(f(t)\) is the input laser intensity signal, is the wavelet basis function, \(a\) is the scale parameter, \(b\) is the translation parameter, and \(W(a, b)\) is the result of wavelet transform; In the wavelet transform process, wavelet basis functions need to be used. In this invention, an improved Haar wavelet is used. The Haar wavelet uses a series of scaled and translated Haar waveforms to analyze signals and can provide localized information of signals in both time and frequency dimensions. The Haar wavelet has a short support length and fast calculation speed, and can achieve good results in the edge detection of dynamic obstacles with high real-time requirements. However, the single-frame decomposition of the Haar wavelet only uses spatial information and is difficult to distinguish dynamic obstacles (such as moving vehicles) from static artifacts (such as floating leaves), resulting in a high false detection rate. To address the above problems, this invention improves a multi-level Haar wavelet fusion method with temporal enhancement to reduce the false detection rate of the Haar wavelet; The original point cloud output by the lidar is unstructured. First, it needs to be converted into a regular three-dimensional voxel grid for wavelet transform. Voxelization divides the space into uniform cubic grids, and the mean coordinates of the points in each voxel are statistically calculated to obtain the structured 3D matrix V t ∈R H×W×D , where H is the height, W is the width, and D is the depth. Subsequently, the voxel matrix V t is decomposed step by step in three dimensions (x, y, z), generating low-frequency (L) and high-frequency (H) components at each level. The high-frequency components capture edges and dynamic changes, while the low-frequency components retain the overall structure; For a certain dimension of the voxel matrix V: Low-frequency coefficient: High-frequency coefficient: In the formula, i is the decomposition level; Calculate the temporal variance of the high-frequency components of consecutive multiple frames to quantify the dynamics of the local area. The high-frequency components of dynamic obstacles will fluctuate significantly due to position changes, while the variance of the static background is low; Temporal mean: where μ i (x, y, z) is the temporal mean of the i-th level high-frequency component at the position (x, y, z), T is the temporal window size, is a certain level of high-frequency component of the t-th frame; Unbiased variance time series: where Var i (x, y, z) is the temporal variance of the i-th high-frequency component at the position (x, y, z), which is used to quantify the dynamics; Set the threshold to 20% of the global maximum of the variance, mark the high-variance area as the dynamic candidate area, and the low-variance area as the static area; Subsequently, perform the fusion of multi-level coefficients; Dynamic region fusion weight: where α i is the weight of the i-th high-frequency component in the dynamic region; Dynamic region fusion weight coefficient: In the formula, λ is the variance influence coefficient, which controls the influence intensity of the variance on the weight; Static region fusion weight: where β i is the weight of the i-th high-frequency component in the static region; Static region fusion weight coefficient: Strengthen the fine edges in the dynamic region to improve sensitivity, and focus on the stable structure in the static region to suppress noise interference. Finally, generate an enhanced three-dimensional edge map and output accurate obstacle contour information in real time to effectively distinguish moving targets from the environmental background; S3.2: Use the principal component analysis method to reduce the dimension of the extracted features to reduce the complexity of subsequent processing; S3.3: Use deep learning algorithms to classify and identify the processed features. In this invention, a convolutional neural network is adopted, and a specific Voxel Net model for 3D point cloud data is used; S4: Obstacle detection and recognition: S4.1: Set an intensity threshold to distinguish between obstacle and non-obstacle areas. For areas with intensity higher than the threshold, further use the K-means clustering algorithm to cluster adjacent obstacle points into independent obstacle objects; S4.2: Use a deep learning model to perform fine classification and recognition on the clustered obstacle objects, and learn the characteristics of different types of obstacles through training the model to achieve high-precision obstacle recognition; S5: Integrate the dynamic obstacle detection algorithm based on 3D lidar point cloud intensity data with the DWA algorithm: The dynamic obstacles in the environment can be monitored in real time and their motion predictions can be incorporated into the velocity selection process of DWA. Specifically, first use the dynamic obstacle detection algorithm to obtain the point cloud intensity data from the 3D lidar, identify and track the trajectories of surrounding dynamic obstacles in real time, and generate prediction information about their future positions. Then, in DWA, use this prediction information to evaluate the safety of the generated velocity combinations, which is specifically reflected in updating the evaluation function. By increasing the distance cost to dynamic obstacles, collisions can be avoided as much as possible, and the optimal action instruction can be selected.

Images