Disclosure of Invention
In view of the above, the invention provides an intelligent light truck path planning method to solve the problems that in the prior art, the track smoothness is insufficient in an obstacle avoidance scene, local optimum is easily trapped in an obstacle-dense area, and the driving characteristics of the light truck are not designed.
An intelligent light truck path planning method, comprising:
step S1, constructing various road obstacle avoidance scenes of the light truck, and acquiring the coordinates of a starting point, a terminal point and the position point of an obstacle in the road obstacle avoidance scenes;
Step S2, introducing a target distance evaluation function, a headstock distance evaluation function and a curvature evaluation function into a dynamic window algorithm to obtain a one-time improved dynamic window algorithm;
Step S3, dynamically adjusting the weight value of the primary improved dynamic window algorithm by adopting a weight value global self-adaptive adjustment strategy to obtain a secondary improved dynamic window algorithm;
S4, adopting an improved artificial lemming optimization algorithm to optimize path variances in a secondary improved dynamic window algorithm to obtain a tertiary improved dynamic window algorithm, wherein the improved artificial lemming optimization algorithm is based on an artificial lemming optimization algorithm, introducing an encircling prey mechanism in a whale algorithm to improve a foraging model of the artificial lemming algorithm, and introducing an exploring prey mechanism in the whale algorithm to improve a migration model of the artificial lemming algorithm;
And S5, carrying out path planning of the light truck by adopting a three-time improved dynamic window algorithm based on the starting point, the finishing point and the position point coordinates of the obstacle in the road obstacle avoidance scene, and obtaining an obstacle avoidance path.
The intelligent light truck path planning method provided by the invention has the following beneficial effects:
(1) According to the invention, the evaluation function is improved on the basis of the traditional dynamic window algorithm, the target distance evaluation function, the head distance evaluation function and the curvature evaluation function are added, and the weight value of the dynamic window algorithm which is improved at one time is dynamically adjusted by adopting the weight value global self-adaptive adjustment strategy, so that the problems that the traditional dynamic window algorithm is insufficient in track smoothness in an obstacle scene and easily falls into local optimum in an obstacle dense area are effectively solved, and the obstacle avoidance efficiency can be improved. In addition, when the target distance evaluation function, the head distance evaluation function and the curvature evaluation function are designed, the adjustment can be carried out aiming at the light truck, the longer obstacle avoidance distance is set, and the smoothness and the continuity of a planned path are realized aiming at the long body of the light truck.
(2) In order to adapt to multiple complex obstacle scenes and improve the global exploration and local optimizing capacity of a dynamic window algorithm, an improved artificial lemming optimization algorithm is adopted to optimize the path variance in a secondary improved dynamic window algorithm, so that the safety and reliability of path planning can be improved.
Detailed Description
Embodiments of the present invention are described in detail below, examples of which are illustrated in the accompanying drawings, wherein like or similar reference numerals refer to like or similar elements or elements having like or similar functions throughout. The embodiments described below by referring to the drawings are exemplary and intended to illustrate embodiments of the invention and should not be construed as limiting the invention.
Referring to fig. 1, an embodiment of the present invention provides an intelligent light truck path planning method, which includes steps S1 to S5:
step S1, constructing various road obstacle avoidance scenes of the light truck, and acquiring the coordinates of a starting point, an ending point and the position point of an obstacle in the road obstacle avoidance scenes.
And S2, introducing a target distance evaluation function, a headstock distance evaluation function and a curvature evaluation function into the dynamic window algorithm to obtain a one-time improved dynamic window algorithm.
The expression of the target distance evaluation function is as follows:
;
Wherein, theFor the euclidean distance between the current point and the target point,AndThe x-axis coordinate and the y-axis coordinate of the last point in the travel track,AndThe x-axis coordinates and the y-axis coordinates of the target point, respectively.
The expression of the head distance evaluation function is:
;
Wherein, theIndicating the minimum distance of the vehicle head,The representation takes the minimum value of the value,Is the parallelogram distance between the left end of the vehicle head and the obstacle,Is the parallelogram distance between the right end of the vehicle head and the obstacle.
The curvature evaluation function is expressed as:
;
;
;
Wherein, theThe curvature evaluation value is represented by a graph,Is the first in the driving trackThe curvature of the individual points is such that,Is the average value of the curvature and,For the total number of points in the driving trajectory,Is the direction angle variation of adjacent line segments in the running track,AndA first direction angle and a second direction angle of adjacent line segments in the running track respectively,In order to provide for the time interval of time,Representing the remainder operation.
In the head distance evaluation function, the following formula is satisfied:
;
;
;
;
;
;
Wherein, theAndThe x-axis coordinates and the y-axis coordinates of the obstacle,AndRespectively an x-axis coordinate and a y-axis coordinate of the left end of the headstock,AndRespectively an x-axis coordinate and a y-axis coordinate of the right end of the headstock,Vehicle width for light trucks, e.g.Is the number of the particles to be used for the liquid crystal display,For wheelbases of light trucks, e.g.Is the number of the particles to be used for the liquid crystal display,AndRespectively the x-axis and y-axis coordinates of the light truck,Indicating the heading angle of the light truck.
And S3, dynamically adjusting the weight value of the primary improved dynamic window algorithm by adopting a weight value global self-adaptive adjustment strategy to obtain a secondary improved dynamic window algorithm.
The expression of the weight value global self-adaptive regulation strategy is as follows:
;
;
;
;
;
;
Wherein, theFor the weight value of the target distance evaluation function after the self-adaptation adjustment,For the weight value of the head distance evaluation function after self-adaptive adjustment,For the weight value of the curvature evaluation function after the self-adaptation adjustment,For the weight value of the target distance evaluation function before the adaptive adjustment,For the weight value of the head distance evaluation function before the self-adaptive adjustment,For the weight value of the curvature evaluation function before the adaptive adjustment,、、、、、In order to control the parameters of the device,As a function of the first adjustment function,As a function of the second adjustment function,As a function of the third adjustment function,Is the real-time speed of the light truck,Is the highest speed of the light truck,For real-time loading of light trucks,Is the maximum load of the light truck.
Wherein, the total evaluation function of the dynamic window algorithm with the second improvementThe expression of (2) is:
;
Wherein, theIs the orientation evaluation function of the orientation of the object,Is an obstacle distance evaluation function,Is a function of the speed evaluation and,In order to orient the weight value of the evaluation function,For the weight value of the obstacle distance evaluation function,The weight value of the speed evaluation function.
And S4, optimizing path variances in the secondary improved dynamic window algorithm by adopting an improved artificial lemming optimization algorithm to obtain a tertiary improved dynamic window algorithm, wherein the improved artificial lemming optimization algorithm is that a surrounding prey mechanism in a whale algorithm is introduced to improve a foraging model of the artificial lemming algorithm on the basis of an artificial lemming optimization algorithm, and a searching prey mechanism in the whale algorithm is introduced to improve a migration model of the artificial lemming algorithm.
Wherein, the foraging model of the improved artificial lemming algorithm satisfies the following formula:
;
;
;
Wherein, theIs the first in foraging modelIndividual lemming at the firstThe position at the time of the iteration,In foraging modelThe position of the optimal solution in the next iteration; Coefficient vectors, which are foraging models, for controlling behavior surrounding a prey; Distance between whale and prey in foraging model; for a linearly decreasing coefficient, linearly decreasing from 1 to 0 as the number of iterations increases; is a random number, and ranges between [0,1 ]; Is the first in foraging modelIndividual lemming at the firstThe position at the time of the iteration.
Aiming at a migration model, a hunting mechanism in a whale algorithm is introduced, so that lemming individuals not only consider self migration behaviors during migration, but also randomly select one individual as a target by referring to a whale hunting mode, and move towards the target. The migration model of the improved artificial lemming algorithm satisfies the following equation:
;
;
Wherein, theIs the first migration modelIndividual lemming at the firstThe position at the time of the iteration,Is the location of the individual selected at random,Is a coefficient vector of the migration model,For the distance between whale and prey in the migration model,Is the first migration modelIndividual lemming at the firstThe position at the time of the iteration.
Meanwhile, in order to enhance the global exploration and local development capability of the improved artificial lemming algorithm, the population scale is dynamically adjusted according to the diversity and fitness distribution of the population, and the improved artificial lemming optimization algorithm also meets the following formula:
;
Wherein, theIs the firstThe population size at the time of the iteration,Is the firstThe population size at the time of the iteration,For the adjustment of the step size of the population size,As an indicator of the diversity of a population (e.g. the average distance between individuals in the population),The diversity threshold is used for judging whether the population size needs to be adjusted.
Fitness function of improved artificial lemming optimization algorithmThe expression of (2) is:
;
;
;
;
;
;
Wherein, theIs a path length value; The path penalty value is used for measuring the feasibility of the path; Is thatWeight coefficient of (2); The method is a path length variance value and is mainly used for measuring the fluctuation of the path point distance; Is thatFor controlling the smoothness optimization strength,;AndIs adjacent to the firstThe path point and the firstThe coordinates of the points of the path,As the total number of path points,Is the minimum distance of the light truck from the obstacle,For the distance adjustment threshold value,For example, it is 1m,In order for the safety distance to be a minimum,Is the firstThe path point and the firstThe spacing between the points of the path,Is the average road segment length.
And S5, carrying out path planning of the light truck by adopting a three-time improved dynamic window algorithm based on the starting point, the finishing point and the position point coordinates of the obstacle in the road obstacle avoidance scene, and obtaining an obstacle avoidance path.
The coordinates of the starting point, the end point and the position point of the obstacle in the road obstacle avoidance scene are input into a three-time improved dynamic window algorithm, so that the path planning of the light truck can be performed, and the obstacle avoidance path is obtained.
A simulation test is carried out below, two complex grid obstacle environments are set in the simulation, namely a scene 1 and a scene 2, the simulation test compares and analyzes the difference of the method and the traditional DWA algorithm in path planning performance, and evaluation indexes comprise path length, curvature change and obstacle safety distance meeting conditions. The results are shown in fig. 2,3 and table 1.
TABLE 1
As can be seen from fig. 2, fig. 3 and table 1, for the path length, since the present invention introduces the objective distance evaluation function, the headstock distance evaluation function and the curvature evaluation function, the path search strategy is optimized, and the detour path is reduced to a certain extent, and in both scene 1 and scene 2, the path length of the present invention is shorter than that of the conventional DWA algorithm, which indicates that the present invention is better in the path optimizing capability, and can effectively shorten the moving distance and improve the path efficiency. Compared with the traditional DWA algorithm, the curvature change is smaller, and the planned path is smoother while ensuring that the safety distance is met, so that the method is beneficial to reducing the mechanical abrasion and the motion impact of the vehicle in the actual running process.
In summary, the intelligent light truck path planning method according to the above embodiment has the following beneficial effects:
(1) According to the invention, the evaluation function is improved on the basis of the traditional dynamic window algorithm, the target distance evaluation function, the head distance evaluation function and the curvature evaluation function are added, and the weight value of the dynamic window algorithm which is improved at one time is dynamically adjusted by adopting the weight value global self-adaptive adjustment strategy, so that the problems that the traditional dynamic window algorithm is insufficient in track smoothness in an obstacle scene and easily falls into local optimum in an obstacle dense area are effectively solved, and the obstacle avoidance efficiency can be improved. In addition, when the target distance evaluation function, the head distance evaluation function and the curvature evaluation function are designed, the adjustment can be carried out aiming at the light truck, the longer obstacle avoidance distance is set, and the smoothness and the continuity of a planned path are realized aiming at the long body of the light truck.
(2) In order to adapt to multiple complex obstacle scenes and improve the global exploration and local optimizing capacity of a dynamic window algorithm, an improved artificial lemming optimization algorithm is adopted to optimize the path variance in a secondary improved dynamic window algorithm, so that the safety and reliability of path planning can be improved.
The above examples illustrate only a few embodiments of the invention, which are described in detail and are not to be construed as limiting the scope of the invention. It should be noted that it will be apparent to those skilled in the art that several variations and modifications can be made without departing from the spirit of the invention, which are all within the scope of the invention. Accordingly, the scope of protection of the present invention is to be determined by the appended claims.