Path planning method for VFH-MLP multi-level histogram fusion facing unstructured environment

Abstract

The invention relates to a VFH-MLP multi-level histogram fusion path planning method for an unstructured environment, and belongs to the technical field of local path planning. The method comprises the following steps: acquiring radar data and dividing the radar data into a plurality of sectors Ki, establishing an obstacle vector field histogram # imgabs0 # based on the obstacle intensity of each sector, establishing an auxiliary histogram # imgabs1 # based on various environmental influence factors, smoothing all histograms to obtain smoothed histogram intensity # imgabs2 #, and determining an initial threshold value Tk corresponding to each environmental factor by adopting a pre-trained artificial neural network based on a multi-layer perceptron; determining a final feasible region based on the determined initial threshold Tk; and selecting the candidate direction with the minimum cost as the next advancing direction based on the improved cost function, and repeating the process until the target position is reached. According to the method, a plurality of histograms and a plurality of thresholds are used, and influence factors of various environments are further considered, so that the robot can plan the optimal path in the unstructured road.

Description

Technical Field

[0001] The present invention belongs to the technical field of local path planning and relates to a VFH-MLP multi-level histogram fusion path planning method for unstructured environments. Background Art

[0002] Autonomous navigation for mobile robots primarily involves tasks such as positioning, path planning, and obstacle avoidance. Path planning can be categorized into two types based on the level of environmental information available: global path planning, when all environmental information is known, and local path planning, when only some of the environmental information is known. Local path planning relies on sensors to acquire obstacle information in real time, enabling the robot to dynamically avoid obstacles. However, due to the lack of global environmental information, this approach is prone to falling into local optimal solutions, causing the robot to deviate from its target direction.

[0003] To improve the autonomous obstacle avoidance capability of robots, researchers at home and abroad have conducted extensive research and proposed some classic robot obstacle avoidance methods, such as the grid method, visibility graph method, virtual potential field method (VFF), etc. Among them, the vector field histogram method (VFH) is a widely used local obstacle avoidance planning algorithm.

[0004] In order to solve the problem that early mobile robot obstacle avoidance algorithms (such as potential field method) have local optimal solution problems and oscillation phenomena, and are difficult to operate stably in dynamic environments. J.Borenstein and Y.Koren proposed the VFH algorithm. The VFH algorithm has strong real-time performance, is suitable for dynamic environments, and can effectively avoid obstacles. However, the algorithm did not initially consider the size and kinematic characteristics of the mobile robot itself, and the robot may be unable to execute the planned path. To address this problem, J.Borenstein and L.Ulrich proposed the VFH+ algorithm, which takes the size and kinematic characteristics of the robot into consideration through an implicit construction space, and Figure 2 To further overcome the limitations of the VFH algorithm, J. Borenstein and L. Ulricn combined the heuristic search A* algorithm to propose the VFH* algorithm. This algorithm can predict the possible consequences of a robot choosing a direction, allowing the robot to move more closely towards the target direction.

[0005] In recent years, research on autonomous driving technology has been divided into two main areas: autonomous driving for structured road scenarios (such as urban roads and highways) and path planning for unstructured road scenarios (such as hilly terrain). Traditional algorithms fail to consider the various factors that influence local path planning; the robot only considers obstacle avoidance during driving.

[0006] The University of Electronic Science and Technology of China proposed a method to modify the VFH threshold based on physical parameters, motion characteristics, and the target environment. This method can improve the threshold sensitivity of traditional algorithms and ensure that robots can reach their target locations in narrow passages using a shorter, collision-free path.

[0007] However, this method is sensitive to changes in parameter thresholds. A higher threshold increases the number of feasible sectors, making the robot less sensitive to changes in obstacles and unable to adjust its direction, potentially leading to collisions. A lower threshold reduces the number of feasible sectors, making the robot more sensitive to changes in obstacles, causing it to change direction multiple times and resulting in an uneven path. Furthermore, this method is prone to getting stuck in local optima. The VFH algorithm is a local path planning method that lacks consideration of the global environment. In complex environments, this method can become stuck in local optima, preventing it from reaching the target point and leading to dead ends with no way out. Furthermore, this method is sensitive to environmental changes, requiring the parameters used for robot movement to be readjusted. Manual parameter adjustment is also ineffective and time-consuming. Furthermore, this method cannot plan an optimal path on unstructured roads. Traditional VFH algorithms fail to consider the various factors that influence robot path planning, resulting in the robot only being able to avoid obstacles rather than achieving an optimal path. Summary of the Invention

[0008] In view of this, the purpose of the present invention is to provide a path planning method based on VFH-MLP multi-level histogram fusion for unstructured environments. Based on the VFH (Vector Field Histogram) algorithm, a multi-layer perceptron (MLP) is used to train different environmental data to construct an adaptive path planning model. Taking into account various influencing factors (such as slope, ground friction coefficient, obstacle distribution, etc.), the feasible area of each factor is calculated separately through multiple histogram modeling, and the different histograms are fused and optimized based on a variable cost function evaluation mechanism. Finally, by calculating the intersection of feasible areas under multiple constraints, the optimal feasible path that meets all environmental constraints is screened out, thereby achieving a relatively optimal local path planning.

[0009] In order to achieve the above object, the present invention provides the following technical solutions:

[0010] A VFH-MLP multi-level histogram fusion path planning method for unstructured environments, the method comprising:

[0011] S1. Obtain radar data and divide the radar scanning range into several sectors K at even intervals. i ;

[0012] S2. Calculate the obstacle strength of each sector in the polar coordinate system to establish the obstacle vector field histogram At the same time, unstructured road environment map information is obtained, and auxiliary histograms are constructed based on multiple environmental influencing factors.

[0013] S3. Obstacle vector field histogram and auxiliary histogram Perform histogram smoothing to obtain the smoothed histogram intensity

[0014] S4, using a pre-trained artificial neural network based on a multi-layer perceptron to determine the initial threshold T corresponding to each environmental factor according to each histogram information k ;

[0015] S5. Based on the determined initial threshold T k Conduct feasibility assessment on each sector, divide it into feasible areas corresponding to each environmental factor, and merge multiple feasible areas corresponding to multiple environmental factors to determine the final feasible area;

[0016] S6. Based on the improved cost function, the candidate direction with the minimum cost is selected as the next forward direction, and steps S1-S6 are repeated after forwarding until the target position is reached.

[0017] Furthermore, in step S1, the azimuth angle corresponding to the current radar scanning point of the robot is first calculated, and the calculation method is:

[0018] current_angle=angle_min+n*angle_increment

[0019] Where angle_min represents the initial point of the lidar scan, n is the number of laser points, and angle_increment represents the resolution of the lidar;

[0020] Then, the radar scanning range is divided into N sectors evenly according to the preset size intervals, and each sector is numbered in the order of radar scanning. The i-th sector is recorded as K i (i∈{1,2,...,N}).

[0021] Furthermore, in step S2, the radar data is first processed and the obstacle strength of each sector is directly calculated in the polar coordinate system. For each sector K i , obstacle density ρ calculated based on radar ranging data i , defined as all valid ranging points r in the sector k The sum of the reciprocals of :

[0022]

[0023] Based on this, the obstacle vector field histogram in polar coordinates is constructed

[0024] Then, a variety of environmental influencing factors are introduced, including at least slope, geographical location, rolling resistance and wind resistance, and an auxiliary histogram for energy consumption is constructed based on all environmental influencing parameters. Where E represents energy consumption.

[0025] Furthermore, in constructing an auxiliary histogram for energy consumption In the process, there are:

[0026]

[0027] Where F is the total force, m is the mass of the robot, a is the acceleration, and f is the rr (s) represents the rolling resistance coefficient at position s, g is the acceleration due to gravity, θ s represents the slope at position s, ρ s is the air density at position s, A is the frontal area, C D is the drag coefficient, Z(s) represents the altitude at s, Indicates average speed;

[0028] The energy consumption E is calculated from this:

[0029]

[0030] Finally, an auxiliary histogram is constructed based on the calculated energy consumption E

[0031] Furthermore, in step S3, smoothing processing is performed on all constructed histograms, wherein:

[0032]

[0033] Where 2l+1 represents the size of the smoothing window, and the center point is The left point is The l point on the right is The processing weights of the corresponding points on both sides that are closer to the center point are 1, 2, …, l-1, and the weight of the center point is l, where the subscripts α∈(ρ, E) represent the processed obstacle vector field histogram and energy consumption auxiliary histogram, respectively.

[0034] Further, in step S4, the intensity of each histogram is The distance D from the target point is used as the data input of the artificial neural network based on the multi-layer perceptron to obtain the corresponding threshold. The artificial neural network based on the multi-layer perceptron includes an input layer, a hidden layer, and an output layer. The input layer is used to receive external input signals, the hidden layer is used to extract the features of the input layer, and the output layer is used to output the prediction results of the model.

[0035] The input layer receives a vector, each input feature x i There exists a corresponding weight w i , the input features are linearly combined with the weights, and a bias term b is added on this basis to obtain the preliminary processed features x i ',Right now:

[0036]

[0037] Feature x i ′Preliminary feature extraction is performed through two fully connected hidden layers, and the activation functions of the two hidden layers are both ReLU functions:

[0038]

[0039] Output the final threshold X in the output layer:

[0040] X=[T ρ ,T E ,…T j ,μ1,μ2,μ3,…μ j+3 ]

[0041] Where D is the intensity of each histogram The distance from the target point, D is the distance from the current position end point, T ρ Represents the threshold of the obstacle histogram, T E Represents the threshold of the energy consumption histogram, T j represents the threshold of any auxiliary histogram, μ1, μ2, μ3 represent the weight of the calculation cost, j represents the number of auxiliary histograms, μ j+3 Parameter representing the cost of computing the auxiliary histogram.

[0042] Furthermore, the artificial neural network based on the multi-layer vector perceptron is trained through the back-propagation algorithm. When the output data does not match the actual data, the error signal is fed back to the MLP, and the weights are updated according to the partial derivatives of the error signal and the weights.

[0043] During training, the intensity of each histogram is considered for different factors The distance D from the target point is used as the data input of the MLP, and the corresponding threshold data set T is selected. α As the data output of MLP;

[0044] The order of input data is disrupted, and the data set is divided into training set, validation set, and test set according to the preset ratio;

[0045] The Adam optimization algorithm is used for training. The parameter update calculation formula of the Adam optimization algorithm is as follows:

[0046] v t =β1v t-1 +(1-β1)g t

[0047] s t =β2s t-1 +(1-β2)g t 2

[0048]

[0049] Where: v, s are the first-order moment and second-order moment of the gradient respectively; are the first-order moment and second-order moment of the corrected gradient respectively; θ is the parameter of the neural network; g t is the gradient, the subscript or superscript t, t-1 represents the current time and the previous time respectively; β1, β2 are hyperparameters, 0≤β1<1, 0≤β2<1; λ represents a hyperparameter; ε is a constant to prevent the denominator from being zero;

[0050] Finally, the threshold is determined in real time based on the trained artificial neural network based on multi-layer perceptron.

[0051] Further, in step S5, for all sectors K in the histogram i , if its cost C i ≤T k , then it is recorded as a feasible sector;

[0052] Merge adjacent feasible sectors into continuous candidate feasible regions R j , and record the starting sector number K i,s and end sector number K i,e ;

[0053] If there is no feasible area at present, gradually increase the threshold T with a preset step size k , until at least one feasible region is found; at the same time, an upper threshold T is set. k,max , satisfying T k ≤T k,max ;

[0054] The final feasible area must simultaneously meet the threshold conditions of all histograms, namely:

[0055]

[0056] In the formula, for the same histogram, all fan-shaped areas that are smaller than the corresponding threshold are screened out to form a subset, and the union of all subsets is taken to form a feasible area. Then, the intersection of the feasible areas of different histograms is taken to generate the final feasible area.

[0057] Furthermore, in step S6, the candidate direction with the minimum cost is selected as the next forward direction through the improved cost function:

[0058]

[0059] Where, L i Represents a certain way of calculating cost, L i Represents a certain cost calculation method; μ1, μ2, μ3 represent weight constants, which are set manually and will be modified by the neural network. i ,i∈[4,n] represents the weight constant of the environmental factors introduced, which will be modified by the neural network, Δ(c,k t ) is expressed as the angle between the candidate direction and the target direction, μ2Δ(c,k c ) is the angle between the candidate direction and the current motion, μ3Δ(c,k c-1 ) is the angle between the candidate direction and the previous movement direction, k t ,k c ,k c-1 They represent the target direction, current movement direction, and previous movement direction respectively.

[0060] Change different environments, constantly modify the threshold, and continuously calculate the direction of travel until reaching the target location.

[0061] The beneficial effects of the present invention are:

[0062] First, during the construction of the obstacle vector field histogram in polar coordinates, the present invention directly calculates the obstacle strength of each sector in polar coordinates, eliminating the need for conversion to a rectangular coordinate system. This improvement simplifies the calculation process and improves algorithm efficiency. Furthermore, by incorporating multiple environmental factors (such as slope gradient and ground friction coefficient) and constructing auxiliary histograms based on these parameters, the algorithm more comprehensively considers various factors in the actual environment, providing more accurate information for subsequent path planning.

[0063] Secondly, during the process of smoothing the polar coordinate histogram and determining feasible sectors, the present invention avoids sharp jumps through smoothing, making the histogram more continuous and smooth, which is beneficial for subsequent path planning. Furthermore, by setting an initial threshold and gradually adjusting it until a feasible region that meets all histogram threshold conditions is found, this process ensures global consistency and optimality in path planning.

[0064] Through comparative verification, the present invention shows good versatility in different starting points, end points and map environments, and exhibits excellent effects in improving algorithm efficiency, enhancing the global consistency and optimality of path planning, and reducing manual parameter adjustment time.

[0065] Other advantages, objects, and features of the present invention will be described in part in the following description and, in part, will be apparent to those skilled in the art upon examination of the following description or may be learned from practice of the present invention. The objects and other advantages of the present invention may be realized and obtained through the following description. BRIEF DESCRIPTION OF THE DRAWINGS

[0066] In order to make the purpose, technical solutions and advantages of the present invention more clear, the present invention will be described in detail below with reference to the accompanying drawings, in which:

[0067] Figure 1 Schematic diagram of the overall process of the VFH-MLP multi-level histogram fusion path planning method for unstructured environments under an embodiment of the present invention;

[0068] Figure 2 Schematic diagram of the network structure of the MLP network according to an embodiment of the present invention;

[0069] Figure 3 Schematic diagram of a merging process of multiple feasible regions according to an embodiment of the present invention;

[0070] Figure 4 Schematic diagram of a path planning example of the VFH-MLP algorithm according to an embodiment of the present invention;

[0071] Figure 5 This is a schematic diagram of a path planning example using the traditional VFH algorithm under a larger threshold;

[0072] Figure 6 This is a schematic diagram of a path planning example using the traditional VFH algorithm under a smaller threshold;

[0073] Figure 7 Schematic diagram of path planning example of traditional VFH algorithm under appropriate fixed threshold;

[0074] Figure 8 Schematic diagram of a path planning example of the VFH-MLP algorithm in another starting point, end point and map according to an embodiment of the present invention;

[0075] Figure 9 This is a schematic diagram of traveling in the direction with the least energy consumption to reach the destination according to an embodiment of the present invention. DETAILED DESCRIPTION

[0076] The following describes the embodiments of the present invention by means of specific examples, and those skilled in the art can easily understand other advantages and effects of the present invention from the contents disclosed in this specification. The present invention can also be implemented or applied through other different specific embodiments, and the details in this specification can also be modified or changed in various ways based on different viewpoints and applications without departing from the spirit of the present invention. It should be noted that the illustrations provided in the following embodiments are only schematic illustrations of the basic concept of the present invention, and the following embodiments and features in the embodiments can be combined with each other without conflict.

[0077] Among them, the accompanying drawings are only for illustrative purposes and represent only schematic diagrams rather than actual pictures, and should not be understood as limiting the present invention. In order to better illustrate the embodiments of the present invention, some parts of the accompanying drawings may be omitted, enlarged or reduced, and do not represent the dimensions of actual products. For those skilled in the art, it is understandable that some well-known structures and their descriptions may be omitted in the accompanying drawings.

[0078] The same or similar numbers in the drawings of the embodiments of the present invention correspond to the same or similar parts; in the description of the present invention, it should be understood that if there are terms such as "upper", "lower", "left", "right", "front", "back", etc. indicating directions or positional relationships, they are based on the directions or positional relationships shown in the drawings. They are only for the convenience of describing the present invention and simplifying the description, and do not indicate or imply that the device or element referred to must have a specific direction, be constructed and operate in a specific direction. Therefore, the terms describing the positional relationship in the drawings are only used for illustrative purposes and cannot be understood as limiting the present invention. For ordinary technicians in this field, the specific meanings of the above terms can be understood according to specific circumstances.

[0079] See also Figures 1 to 9 , which is a VFH-MLP multi-level histogram fusion path planning method for unstructured environments.

[0080] Traditional VFH algorithms are sensitive to threshold parameters and cannot adapt to the needs of different environments. In practical applications, thresholds are usually set manually based on experience. A larger threshold can be set for relatively open environments with few obstacles, while a smaller threshold is required in environments with dense obstacles. The present invention uses a multilayer perceptron (MLP) to train a model based on data from different environments and directly determine the best candidate area. Traditional VFH algorithms are prone to dead ends and local minima. MLP can reduce the number of dead ends and local minima encountered by the robot by learning from the path planning history, thereby improving the global consistency of path planning. Selecting better candidate areas can, to a certain extent, avoid getting stuck in local optimal solutions and smooth the robot's travel path. The traditional VFH algorithm is improved by using multiple histograms and multiple thresholds, taking into account the influencing factors of various environments, allowing the robot to plan the optimal path, rather than the shortest path, on unstructured roads.

[0081] Example

[0082] This embodiment first describes in detail the specific steps of the VFH-MLP multi-level histogram fusion path planning method for unstructured environments.

[0083] like Figure 1 The flowchart of the VFH-MLP multi-level histogram fusion path planning method for unstructured environments is shown, and the method includes the following steps:

[0084] S1. Obtain radar data and divide the radar scanning range into several sectors K at even intervals. i ;

[0085] S2. Calculate the obstacle strength of each sector in the polar coordinate system to establish the obstacle vector field histogram At the same time, unstructured road environment map information is obtained, and auxiliary histograms are constructed based on multiple environmental influencing factors.

[0086] S3. Obstacle vector field histogram and auxiliary histogram Perform histogram smoothing to obtain the smoothed histogram intensity

[0087] S4, using a pre-trained multi-layer perceptron-based artificial neural network to determine the initial threshold corresponding to each environmental factor according to each histogram information;

[0088] S5. Perform feasibility assessment on each sector based on the determined initial threshold, divide the sector into feasible areas corresponding to each environmental factor, and merge multiple feasible areas corresponding to multiple environmental factors to determine a final feasible area.

[0089] S6. Based on the improved cost function, the candidate direction with the minimum cost is selected as the next forward direction, and steps S1-S6 are repeated after forwarding until the target position is reached.

[0090] In step S1 of this embodiment, it is first necessary to calculate the azimuth angle corresponding to the current radar scanning point of the robot, and the calculation method is:

[0091] current_angle=angle_min+n*angle_increment

[0092] In the formula, angle_min represents the initial point of the lidar scan, n is the number of laser points, and angle_increment represents the resolution of the lidar. According to the above formula, the azimuth angle corresponding to the current lidar scan point can be calculated. Then, the lidar scan range is divided into N sectors evenly at intervals of a preset size, and each sector is numbered in the order of the lidar scan. The i-th sector is recorded as K i (i∈{1,2,...,N}).

[0093] In this embodiment, the radar scanning range (360°) is evenly divided into 144 sectors at intervals of 2.5°, and each sector is numbered in the order of radar scanning. The i-th sector is denoted as K i (i∈{1, 2, ..., 144}), which is used for subsequent obstacle distribution statistics and path feasibility analysis.

[0094] In step S2 of this embodiment, radar data is first processed. Unlike the traditional VFH algorithm, this embodiment directly calculates the obstacle strength of each sector in a polar coordinate system without converting to a rectangular coordinate system.

[0095] Specifically, for each sector K i , obstacle density ρ calculated based on radar ranging data i , defined as all valid ranging points r in the sector k The sum of the reciprocals of :

[0096]

[0097] Based on this, the obstacle vector field histogram in polar coordinates is constructed

[0098] Then, in order to adapt to complex unstructured road environments (such as rugged terrain, loose sand, etc.), a variety of environmental influencing factors are introduced, including slope, geographical location, rolling resistance and wind resistance, and auxiliary histograms are constructed based on these parameters.

[0099] In building auxiliary histograms for energy consumption In the process, there are:

[0100]

[0101] Where F is the total force, m is the mass of the robot, a is the acceleration, and f is the rr (s) represents the rolling resistance coefficient at position s, g is the acceleration due to gravity, θ s represents the slope at position s, ρ s is the air density at position s, A is the frontal area, C D is the drag coefficient, Z(s) represents the altitude at s, Indicates average speed;

[0102] The energy consumption E is calculated from this:

[0103]

[0104] Finally, an auxiliary histogram is constructed based on the calculated energy consumption E

[0105] In step S3 of this embodiment, smoothing is performed on all constructed histograms to prevent the occurrence of sharp jump points. Smoothing is performed on each sector area in each histogram. The specific method is as follows:

[0106]

[0107] Where 2l+1 represents the size of the smoothing window, and the center point is The left point is The l point on the right is The processing weights of the corresponding points on both sides that are closer to the center point are 1, 2, …, l-1, and the weight of the center point is l, where the subscripts α∈(ρ, E) represent the processed obstacle vector field histogram and energy consumption auxiliary histogram, respectively.

[0108] In step S4 of this embodiment, the intensity of each histogram is The distance D from the target point is used as the data input of the MLP to obtain the corresponding threshold, where the subscript α∈(ρ,E) represents the processed obstacle vector field histogram and energy consumption auxiliary histogram, respectively.

[0109] The artificial neural network (MLP) based on multi-layer perceptron is a feedforward neural network that contains multiple neural network layers. MLP-ANN usually consists of three layers: input layer, hidden layer, and output layer. The input layer is used to receive external input signals, the hidden layer is used to extract the features of the input layer, and the output layer is used to output the prediction results of the model. The structure of the multi-layer perceptron is as follows: Figure 2 shown.

[0110] The input layer of the MLP receives a vector, where each input feature x i There exists a corresponding weight w i , the input features are linearly combined with the weights, and a bias term b is added on this basis to obtain the preliminary processed features x i ',Right now:

[0111]

[0112] Feature x i ′Preliminary feature extraction is performed through two fully connected hidden layers, and the activation functions of the two hidden layers are both ReLU functions.

[0113]

[0114] Output the final threshold X in the output layer:

[0115] X=[T ρ ,T E ,…T j ,μ1,μ2,μ3,…μ j+3 ]

[0116] Where D is the intensity of each histogram The distance from the target point, D is the distance from the current position end point, T ρ Represents the threshold of the obstacle histogram, T E Represents the threshold of the energy consumption histogram, T j represents the threshold of any auxiliary histogram, μ1, μ2, μ3 represent the weight of the calculation cost, j represents the number of auxiliary histograms, μ j+3 Parameter representing the cost of computing the auxiliary histogram.

[0117] The training of MLP is achieved through the back propagation algorithm. When the output data does not match the actual data, the error signal is fed back to the MLP, and the weights are updated according to the partial derivative of the error signal and the weight. During the training process, the intensity of each histogram of different factors will be considered. The distance D from the target point is used as the data input of MLP, and the better threshold data set T α (α∈ρ,E) is used as the output of the MLP data to train the model.

[0118] Specifically, the order of the input data was shuffled. The dataset was divided into training, validation, and test sets in a ratio of 14:3:3. The Adam optimization algorithm was used during model training.

[0119] Adam adaptively adjusts the learning rates of different parameters based on the first and second moments of the gradient, ensuring that the parameter update amplitude is not affected by gradient rescaling. Adam is suitable for sparse gradients and can naturally perform long annealing processes. The parameter update calculation formula is as follows:

[0120] v t =β1v t-1 +(1-β1)g t

[0121] s t =β2s t-1 +(1-β2)g t 2

[0122]

[0123] Where: v, s are the first-order moment and second-order moment of the gradient respectively; are the first-order moment and second-order moment of the corrected gradient respectively; θ is the parameter of the neural network; g t is the gradient, the subscript or superscript t, t-1 represents the current time and the previous time respectively; β1, β2 are hyperparameters, 0≤β1<1, 0≤β2<1; λ represents a hyperparameter; ε is a constant to prevent the denominator from being zero;

[0124] Finally, the threshold value is determined in real time based on the trained artificial neural network based on the multi-layer perceptron. In this embodiment, for each histogram (such as obstacle ρ, energy consumption E, etc.), the corresponding initial threshold value T is determined. α (α∈ρ,E).

[0125] In step S5 of this embodiment, each sector is divided according to the determined initial threshold, which specifically includes:

[0126] S51. For all sectors K in the histogram i , if its cost C i ≤T α , then it is recorded as a feasible sector. For example, in the obstacle histogram, i ≤T ρ The sector of is recorded as the barrier-free sector. For example, in the energy consumption histogram, if E i ≤T E , recorded as the energy consumption feasible sector.

[0127] S52: Merge adjacent feasible sectors into continuous candidate feasible regions R j , and record the starting sector number K i,s and end sector number K i,e ;

[0128] S53. If there is no feasible area at present, gradually increase the threshold T with a step size of 0.05 α , until at least one feasible region is found. In order to avoid the threshold being too large, it is required to set an upper threshold T α,max , needs to satisfy T α ≤T α,max ;

[0129] S54. The final feasible region must simultaneously meet the threshold conditions of all histograms, namely:

[0130]

[0131] Where α∈(ρ,E), for the same histogram, all fan-shaped areas smaller than the corresponding threshold are screened out to form a subset, and the union of all subsets is taken to form a feasible region. Then, the intersection of the feasible regions of different histograms is taken to generate the final feasible region.

[0132] In step S6 of this embodiment, the candidate direction with the minimum cost is selected as the next forward direction through the improved cost function:

[0133]

[0134] Where, L i Represents a certain way of calculating cost, L i Represents a certain cost calculation method; μ1, μ2, μ3 represent weight constants, which are set manually and will be modified by the neural network. i ,i∈[4,n] represents the weight constant of the environmental factors introduced, which will be modified by the neural network, Δ(c,k t ) is expressed as the angle between the candidate direction and the target direction, μ2Δ(c,k c ) is the angle between the candidate direction and the current motion, μ3Δ(c,k c-1 ) is the angle between the candidate direction and the previous movement direction, k t ,k c ,k c-1 They represent the target direction, current movement direction, and previous movement direction respectively.

[0135] By changing different environments and constantly modifying the threshold, the robot can smoothly pass through different environments. The data of each histogram and the optimal threshold at each moment are saved as the data set of the multi-layer perceptron.

[0136] In this embodiment, the solution of the present application is simulated and compared with the existing VFH algorithm in combination with the solution described above. Specifically, Figure 4 Schematic diagram of a path planning example of the VFH-MLP algorithm of the present invention, Figure 5 The figure shows an example of path planning using the traditional VFH algorithm under a larger threshold. Compared with the VFH-MLP algorithm, the traditional VFH algorithm is more likely to fall into a local optimum when using a larger threshold, and it still cannot reach the target point when it stops after exceeding the number of iterations. Figure 6 This figure shows an example of path planning using a traditional VFH algorithm at a small threshold. Compared to the VFH-MLP algorithm, the sensor-based VFH algorithm runs out of feasible regions when using a small threshold, and the robot stops moving. In complex environments, the traditional VFH algorithm lacks optimal parameters to reach the destination. Figure 7 The figure shows an example of path planning using the traditional VFH algorithm under a good fixed threshold. As can be seen from the figure, compared with the VFH-MLP algorithm, the traditional VFH algorithm can reach the destination in a relatively simple environment even if a more appropriate fixed threshold is used. However, compared with the VFH-MLP algorithm, its path smoothness is lower. Figure 8 This is another example diagram of path planning based on the VFH-MLP algorithm of the present invention under different starting points, end points and maps. Figure 8 As can be seen from the figure, when the starting point, end point and the entire map are changed, the robot's path planning is not affected, which shows that the model has a certain versatility.

[0137] Figure 9 This figure illustrates the results of the VFH algorithm using a slope histogram. An energy histogram is introduced as an auxiliary histogram. The car follows the tangents of the contour lines, allowing it to plan the direction that consumes the least energy, rather than the shortest path. This allows for driving on unstructured roads.

[0138] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention and are not limiting. Although the present invention has been described in detail with reference to the preferred embodiments, those skilled in the art should understand that the technical solutions of the present invention can be modified or replaced by equivalents without departing from the purpose and scope of the technical solutions, which should all be included in the scope of the claims of the present invention.

Claims

1. A path planning method based on VFH-MLP multi-level histogram fusion for unstructured environments, characterized by: The method comprises: S1. Obtain radar data and divide the radar scanning range into several sectors K at even intervals. i ; S2. Calculate the obstacle strength of each sector in the polar coordinate system to establish the obstacle vector field histogram At the same time, unstructured road environment map information is obtained, and auxiliary histograms are constructed based on multiple environmental influencing factors. S3. Obstacle vector field histogram and auxiliary histogram Perform histogram smoothing to obtain the smoothed histogram intensity S4, using a pre-trained artificial neural network based on a multi-layer perceptron to determine the initial threshold T corresponding to each environmental factor according to each histogram information k ; S5. Based on the determined initial threshold T k Conduct feasibility assessment on each sector, divide it into feasible areas corresponding to each environmental factor, and merge multiple feasible areas corresponding to multiple environmental factors to determine the final feasible area; S6. Based on the improved cost function, the candidate direction with the minimum cost is selected as the next forward direction, and steps S1-S6 are repeated after forwarding until the target position is reached.

2. The path planning method for unstructured environments based on VFH-MLP multi-level histogram fusion according to claim 1, characterized in that: In step S1, the azimuth angle corresponding to the robot's current radar scanning point is first calculated as follows: current_angle=angle_min+n*angle_increment Where angle_min represents the initial point of the lidar scan, n is the number of laser points, and angle_increment represents the resolution of the lidar; Then, the radar scanning range is divided into N sectors evenly according to the preset size intervals, and each sector is numbered in the order of radar scanning. The i-th sector is recorded as K i (i∈{1,2,…,N}).

3. The VFH-MLP multi-level histogram fusion path planning method for unstructured environments according to claim 1, characterized in that: In step S2, the radar data is first processed and the obstacle strength of each sector is calculated directly in the polar coordinate system. For each sector K i , obstacle density ρ calculated based on radar ranging data i , defined as all valid ranging points r in the sector k The sum of the reciprocals of : Based on this, the obstacle vector field histogram in polar coordinates is constructed Then, a variety of environmental influencing factors are introduced, including at least slope, geographical location, rolling resistance and wind resistance, and an auxiliary histogram for energy consumption is constructed based on all environmental influencing parameters. Where E represents energy consumption.

4. The VFH-MLP multi-level histogram fusion path planning method for unstructured environments according to claim 3, characterized in that: In building auxiliary histograms for energy consumption In the process, there are: Where F is the total force, m is the mass of the robot, a is the acceleration, and f is the rr (s) represents the rolling resistance coefficient at position s, g is the acceleration due to gravity, θ s represents the slope at position s, ρ s is the air density at position s, A is the frontal area, C D is the drag coefficient, Z(s) represents the altitude at s, Indicates average speed; The energy consumption E is calculated from this: Finally, an auxiliary histogram is constructed based on the calculated energy consumption E 5. The VFH-MLP multi-level histogram fusion path planning method for unstructured environments according to claim 1, characterized in that: In step S3, smoothing is performed on all constructed histograms, where: Where 2l+1 represents the size of the smoothing window, and the center point is The left point is The l point on the right is The processing weights of the corresponding points on both sides that are closer to the center point are 1, 2, …, l-1, and the weight of the center point is l, where the subscripts α∈(ρ, E) represent the processed obstacle vector field histogram and energy consumption auxiliary histogram, respectively.

6. The VFH-MLP multi-level histogram fusion path planning method for unstructured environments according to claim 1, characterized in that: In step S4, the intensity of each histogram is The distance D from the target point is used as the data input of the artificial neural network based on the multi-layer perceptron to obtain the corresponding threshold. The artificial neural network based on the multi-layer perceptron includes an input layer, a hidden layer, and an output layer. The input layer is used to receive external input signals, the hidden layer is used to extract the features of the input layer, and the output layer is used to output the prediction results of the model. The input layer receives a vector, each input feature x i There exists a corresponding weight w i , the input features are linearly combined with the weights, and a bias term b is added on this basis to obtain the preliminary processed features x i ',Right now: Feature x i ′Preliminary feature extraction is performed through two fully connected hidden layers, and the activation functions of the two hidden layers are both ReLU functions: Output the final threshold X in the output layer: X=[T ρ ,T E ,…T j ,μ1,μ2,μ3,…μ j+3 ] Where D is the intensity of each histogram The distance from the target point, D is the distance from the current position end point, T ρ Represents the threshold of the obstacle histogram, T E Represents the threshold of the energy consumption histogram, T j represents the threshold of any auxiliary histogram, μ1, μ2, μ3 represent the weight of the calculation cost, j represents the number of auxiliary histograms, μ j+3 Parameter representing the cost of computing the auxiliary histogram.

7. The VFH-MLP multi-level histogram fusion path planning method for unstructured environments according to claim 6, characterized in that: The artificial neural network based on the multi-layer vector perceptron is trained using the back-propagation algorithm. When the output data does not match the actual data, the error signal is fed back to the MLP, and the weights are updated based on the partial derivatives of the error signal and the weights. During training, the intensity of each histogram is considered for different factors The distance D from the target point is used as the data input of the MLP, and the corresponding threshold data set T is selected. α As the data output of MLP; The order of input data is disrupted, and the data set is divided into training set, validation set, and test set according to the preset ratio; The Adam optimization algorithm is used for training. The parameter update calculation formula of the Adam optimization algorithm is as follows: v t =β1v t-1 +(1-β1)g t s t =β2s t-1 +(1-β2)g t 2 Where: v, s are the first-order moment and second-order moment of the gradient respectively; are the first-order moment and second-order moment of the corrected gradient respectively; θ is the parameter of the neural network; g t is the gradient, the subscript or superscript t, t-1 represents the current time and the previous time respectively; β1, β2 are hyperparameters, 0≤β1<1, 0≤β2<1; λ represents the hyperparameter; ε is a constant to prevent the denominator from being 0; Finally, the threshold is determined in real time based on the trained artificial neural network based on multi-layer perceptron.

8. The VFH-MLP multi-level histogram fusion path planning method for unstructured environments according to claim 1, characterized in that: In step S5, for all sectors K in the histogram i , if its cost C i ≤T k , then it is recorded as a feasible sector; Merge adjacent feasible sectors into continuous candidate feasible regions R j , and record the starting sector number K i,s and end sector number K i,e ; If there is no feasible area at present, gradually increase the threshold T with a preset step size k , until at least one feasible region is found; at the same time, an upper threshold T is set. k,max , satisfying T k ≤T k,max ; The final feasible area must simultaneously meet the threshold conditions of all histograms, namely: In the formula, for the same histogram, all fan-shaped areas that are smaller than the corresponding threshold are screened out to form a subset, and the union of all subsets is taken to form a feasible area. Then, the intersection of the feasible areas of different histograms is taken to generate the final feasible area.

9. The VFH-MLP multi-level histogram fusion path planning method for unstructured environments according to claim 1, characterized in that: In step S6, the candidate direction with the minimum cost is selected as the next forward direction through the improved cost function: Where, L i Represents a certain way of calculating cost, L i Represents a certain cost calculation method; μ1, μ2, μ3 represent weight constants, which are set manually and will be modified by the neural network. i ,i∈[4,n] represents the weight constant of the environmental factors introduced, which will be modified by the neural network, Δ(c,k t ) is expressed as the angle between the candidate direction and the target direction, μ2Δ(c,k c ) is the angle between the candidate direction and the current motion, μ3Δ(c,k c-1 ) is the angle between the candidate direction and the previous movement direction, k t ,k c ,k c-1 They represent the target direction, current movement direction, and previous movement direction respectively. Change different environments, constantly modify the threshold, and continuously calculate the direction of travel until reaching the target location.

Images