CN-121007565-B - Unmanned tractor path planning method based on improved PSO fusion DWA optimization algorithm

Abstract

The invention provides an unmanned tractor path planning method based on an improved PSO fusion DWA optimization algorithm, which comprises the steps of initializing a grid map, forming a preliminary global path by utilizing the improved PSO algorithm, carrying out local refined optimization on the preliminary global path through single-dimensional neighborhood disturbance and greedy selection, carrying out discrete path serialization, redundant node elimination and smoothing treatment on the preliminary global path by utilizing the greedy optimization method, completely reserving a path point sequence of the final optimized global path as a local target point of the DWA algorithm, guiding the local path planning and real-time obstacle avoidance, carrying out speed sampling and track prediction based on a kinematic model, speed constraint and the local target point, and selecting an optimal predicted track and a speed space by utilizing an improved evaluation function in the DWA optimization algorithm. The method effectively solves the problems of poor smoothness, long planning time, multiple inflection points, discontinuous path curvature and incapability of realizing global optimum existing in the conventional planning method.

Inventors

• XIAO MAOHUA
• LIANG JINXIONG
• YU LILI
• XU WENXIANG
• ZHANG FEICHEN
• DU WENJUN
• MENG WEIGUO
• GUO PEIQI
• TIAN FENGYU

Assignees

• 南京农业大学

Dates

Publication Date

20260508

Application Date

20251028

Claims (5)

1. 1. The unmanned tractor path planning method based on the improved PSO fusion DWA optimization algorithm is characterized by comprising the following steps of: s1, initializing a grid map; S2, setting a starting point and an end point of path planning, and expanding a particle swarm by using an improved PSO algorithm to form a preliminary global path; S3, carrying out local refinement optimization on the preliminary global path through single-dimensional neighborhood disturbance and greedy selection; s4, performing discrete path continuity, redundant node elimination and smoothing treatment on the preliminary global path subjected to local refinement optimization by a greedy optimization method to obtain a further optimized global path; s5, completely reserving a path point sequence of the global path after the optimization of S4 as a local target point of a DWA algorithm, guiding path planning and real-time obstacle avoidance, carrying out speed sampling and track prediction based on a kinematic model, speed constraint and the local target point, selecting an optimal predicted track and a speed space through an improved evaluation function in the DWA optimization algorithm, realizing fusion of an improved PSO algorithm and the DWA optimization algorithm, and then issuing a speed instruction to a chassis controller through a vehicle-mounted industrial personal computer so that a tractor moves along the optimal track; And S6, after the upper control unit judges that the tractor reaches the end point, backtracking all nodes through the human-computer interaction interface module, and extracting and outputting an optimal path.
2. 2. The unmanned tractor path planning method based on the improved PSO fusion DWA optimization algorithm according to claim 1, wherein the specific process of S2 is as follows: S2.1, setting particle swarm related parameters including particle number and particle dimension Initializing a particle speed maximum value related parameter, and determining an inertia weight range and a learning factor range; s2.2, determining a starting point and an ending point of path planning, constructing a particle position matrix and initializing a particle swarm; s2.3 traversing each dimension of each particle, extracting the first Intermediate points Raster data of the column, i.e. the first Raster data of a column, a passable raster row index in the column is screened, one is randomly selected from the passable raster row index as the middle point row coordinate, wherein, ; S2.4, starting iterative search, dynamically adjusting inertia weight and learning factors in each iteration, updating an individual optimal position and a global optimal position by using an adaptability function of an improved PSO algorithm, and updating particle speed and position according to a speed updating formula and a position updating formula; wherein, the dynamic adjustment formula of inertia weight and learning factor is: ; ; ; in the formula, 、 Respectively inertial weights Maximum and minimum of (2); representing the current iteration number; Representing a maximum number of iterations; Is a constant greater than-1; 、 Learning factors of individuals Maximum and minimum of (2); 、 Respectively social learning factors Upper and lower bounds of (2); Estimating the path quality based on two dimensions of the path length and the collision risk, wherein the path length is the sum of Euclidean distances between adjacent points on the path, and for the paths with collision, the collision risk is quantified by adopting the number of collisions and the map area, and the collision penalty value of the tractor and the obstacle is set as the map area multiplied by the number of collisions; the path planning is converted into an optimization problem by weighting the path length and the collision penalty value, so the fitness function is: ; Wherein, the 、 To adjust the relative importance weighting factor between the path length and the collision penalty value, Is a fitness function; Is the path length; a collision penalty value; S2.5, evaluating the path quality represented by the particles according to the path length and a collision penalty mechanism, so as to calculate a fitness value and serve as a particle updating effect rating index; comparing the calculated fitness value with the particle historical individual optimal fitness value, and if the current fitness value is better, updating a local optimal solution; comparing the calculated moderate value with the global optimal fitness value to update a global optimal solution, and determining the position and the speed of particle iteration according to a particle speed and position updating formula; s2.6, continuously executing S2.3 to S2.5 before the maximum iteration times are reached, and continuously updating the particle state and the global optimal solution; S2.7, when the maximum iteration number is reached, the final global optimal position at the moment is obtained And combining the corresponding path intermediate points, and combining the starting point and the end point to form a preliminary global path.
3. 3. The unmanned tractor path planning method based on the improved PSO fusion DWA optimization algorithm according to claim 2, wherein the specific process of S3 is as follows: s3.1 extracting the globally optimal solution obtained in S2 Decision variable dimension of (i.e. number of path intermediate points) and calculating the current global optimal solution Is adapted to the degree of adaptation value of (a) ; S3.2 for globally optimal solution Each intermediate node of (a) The neighborhood solution is constructed in sequence, : Keep dividing by The positions of all nodes except the intermediate nodes are unchanged, and the method is in a feasible domain The inner uniform random selecting an integer as new intermediate node And replace the original intermediate node Generating a neighborhood solution , The number of rows corresponding to the grid map; S3.3 solution for each neighborhood Calculate its fitness value And is matched with the original global optimal solution Is adapted to the degree of adaptation value of (a) Comparing if Then the globally optimal solution is updated to Otherwise, the original solution is reserved and the updating is not carried out.
4. 4. The unmanned tractor path planning method based on the improved PSO fusion DWA optimization algorithm according to claim 3, wherein the specific process of S4 is as follows: s4.1, aiming at the preliminary global path after local refinement in S3 Wherein each waypoint , , For the index of the line of path points, For the index of the path point column, For the number of path points, As a starting point, the position of the object is, Traversing each pair of adjacent path points in increasing order from the start point as the end point And Converting discrete path points into continuous paths by calculating the line difference of adjacent path points, and gradually constructing the continuous paths ; S4.2, aiming at each pair of adjacent path points, according to the line difference Refinement of the path fill logic ensures that the continuous path does not collide with obstacles: when the temperature is less than or equal to 1, the mixture is directly added Added to ; Checking connectivity of intermediate grids between columns where two path points are located when h >1, setting the intermediate grids as Starting from along the row Longitudinally extend to Formed grid set ; If it meets When then At the time, will in turn These waypoint additions When (when) When in use, then in turn These waypoint additions Finally, add Thereby forming a vertical continuous path through which the flow of the fluid passes, wherein, Representing grid map No Line 1 The state of the column is such that, Representing a free-form grid that can be passed through, Representing an obstacle grid; If it is In columns Find the closest in the middle grid of (2) Obstacle grid of (a) The coordinates satisfy Is least and At the same time with Adjacent columns In (1) to Is taken as a starting point and is directed along Line coordinates Direction search passable alternate points of (c) And the substitution point satisfies ; Specifically, a search step is set first Initial value of (1) If no substitute point is found in one search, the step size is adjusted according to the following formula: ; in the formula, Is the first The number of search steps is a number, Is the first The number of search steps is a number, The search step size, i.e. the number of grids searched longitudinally each time, Is a positive integer and satisfies , The number of rows of the grid map; If find the passable substitute point, the original path point Temporarily updating to substitute points Recalculating the line differences Returning to the filling logic processing until collision-free continuous segments are generated, if the search step size If the current segment is reduced to 1, no alternative point is found yet, and the number of attempts reaches the upper limit, skipping the current segment, and continuing to process the subsequent adjacent node pair based on the existing node; s4.3 for the continuous path generated above Smoothing and redundant node elimination operations are performed, where , , For the index of the line of path points, For the index of the path point column, Number of consecutive waypoints: Sequentially selecting three continuous path points Construction vector Sum vector Based on the vector cross multiplication principle, the method uses a calculation formula To determine whether the three points are approximately collinear, wherein, 、 、 Respectively are path points 、 、 Is used for the row index of (1), 、 、 Respectively are path points 、 、 Is a column index of (2); in particular, if It may be determined to be approximately collinear, wherein, A collinearly decision threshold; If the three points are approximately collinear, further checking And (3) with Whether the grid area covered by the connecting wire is free of barriers or not, and the coverage area is as follows: ; Wherein, the The corresponding row index is used to determine the row, A corresponding column index; if it meets Description of intermediate points As redundant nodes, the path after removal is still continuous and collision-free, and From a continuous path Middle eliminating, otherwise, reserving intermediate point Wherein, the method comprises the steps of, Representing grid map No Line 1 The state of the column, Representing a passable free grid; S4.4 repeating S4.3 until the traversing is completed All the combinable three points in the path are finally obtained after the optimization 。
5. 5. The unmanned tractor path planning method based on the improved PSO fusion DWA optimization algorithm according to claim 4, wherein the specific process of S5 is as follows: s5.1, reserving a path point sequence of the global path after the optimization of S4.4, carrying out speed sampling on the tractor in a speed space set based on a tractor kinematic model and speed constraint, and simulating infinite feasible motion tracks in a certain time interval; S5.2, extracting a path point with the minimum distance from the current position of the tractor from a path point sequence of the reserved global path, and taking the path point as a reference anchor point of local guidance to ensure that the guidance direction is matched with the current motion stage of the tractor; specifically, tractor is obtained through GPS Real-time pose at moment , wherein, Is at the tractor The plane coordinates of the moment of time, Corresponding tractor is at Course angle of moment, in global path By calculating Euclidean distance between the path point and the current position of the tractor, determining and The path point with the smallest distance is defined as At the same time, by Starting at the global path Selecting 2 subsequent continuous path points to form a local guiding target sequence ; S5.3, evaluating the track by using an evaluation function of the DWA algorithm, and selecting an optimal track from the infinite number of possible motion tracks generated in the S5.1, wherein the evaluation function of the DWA algorithm is shown in the following formula: ; in the formula, Representing the angular difference of the end direction of the sample trajectory from the local target direction, Representing the distance between the sampling track tail end point and the local target point; Representing the current speed; 、 、 Respectively obtaining weight coefficients corresponding to the sub-functions of the evaluation function; representing a normalization process; s5.4, reserving a speed space corresponding to the optimal track, and performing speed smoothing treatment; S5.5, issuing a speed instruction to the chassis controller through the vehicle-mounted industrial personal computer, so that the tractor moves along the optimal track obtained in the S5.3; And S5.6, the upper control unit judges whether the tractor moves to the end point or not, if so, the tractor ends, otherwise, the upper control unit repeatedly executes S5.1 to S5.5.

Description

Unmanned tractor path planning method based on improved PSO fusion DWA optimization algorithm Technical Field The invention belongs to the technical field of tractor path planning and design, and particularly relates to an unmanned tractor path planning method based on an improved PSO fusion DWA optimization algorithm. Background With the rapid development of intelligent agriculture, unmanned tractors are increasingly widely applied to farmland operation, and the requirements for efficient and intelligent path planning are increasingly urgent. Under the complex circumstances in farmland, there are the crop to distribute various, topography is fluctuated, agricultural machinery equipment and other conditions such as dynamic disturbance for traditional manual driving tractor operation not only inefficiency, intensity of labour are big, still are difficult to accurately agree with the high requirement of fine agricultural production to the route, like row spacing accurate control, obstacle avoidance timeliness etc. therefore unmanned tractor's path planning technique becomes the key support of wisdom agricultural development. The PSO algorithm is used as a group intelligent optimization algorithm, global optimization can be realized through particle search in path planning, but the traditional PSO algorithm has the problem of easy local optimization, and the planned path may have redundancy, poor smoothness and lack of accurate matching of the kinematic characteristics of the tractor. The DWA algorithm is a widely used local obstacle avoidance algorithm, but the original DWA algorithm can accurately 'know' unknown obstacles to realize local real-time obstacle avoidance, but is easy to suffer from local optimization and cannot realize global optimization. Currently, in existing path planning optimization schemes, there have been studies attempting to combine the PSO and DWA algorithms, but there are still corresponding limitations. The prior art (CN 118067126A) discloses a robot path planning method disclosed as Shi Xinxin, which utilizes a DWA algorithm to sample in a speed space, generates a plurality of groups of linear speed and angular speed combinations, deduces a track through a kinematic model, screens out a current optimal track according to an evaluation function, then takes the optimal speed combination output by the DWA as an initial particle position of a PSO algorithm, takes the evaluation function of the DWA as an fitness function, and further optimizes a speed pair through iterative updating of particle swarms, thereby obtaining a shorter, safer and more accurate path. Although the simulation result shows that the method can effectively shorten the path length and improve the obstacle avoidance success rate, the optimization core of the method is to perform secondary optimization on the speed pair of the single-step output of the DWA, no matter how PSO is improved, the search range is always limited in the speed space near the optimal solution of the DWA at the current moment, the global visual field is lacking, the adaptability function of the PSO directly uses the evaluation function of the DWA, and the function only comprehensively evaluates the instantaneous and local cost of azimuth angle, obstacle clearance distance, speed and the like, and lacks global consideration of the whole path, such as the total path length, the whole smoothness and the like. In order to solve the problems, the invention designs a brand-new unmanned tractor path planning method based on an improved PSO fusion DWA optimization algorithm. Disclosure of Invention Aiming at the problems existing in the prior art, the invention provides the unmanned tractor path planning method based on the improved PSO fusion DWA optimization algorithm, which firstly generates a global path based on the improved PSO algorithm, changes the optimized gravity center from a local speed space to a global path node sequence, realizes the normal form transition from local optimization to global planning, and then adopts the improved PSO algorithm, the fitness function of which introduces a path length and an explicit collision penalty mechanism, so that the algorithm can simultaneously optimize two global key indexes of path shortest and collision safety when searching the global path, and in addition, creatively adopts a double-layer framework of global planning, local refinement and local execution, perfectly combines the guidance of the global path with the real-time obstacle avoidance capability of the local algorithm DWA, and effectively solves the problems of local optimization scope, lack of global vision, single optimization target, insufficient consideration of the overall path characteristics and the like existing in the traditional path planning method. The present invention achieves the above technical object by the following technical means. An unmanned tractor path planning method based on an improved PSO fusion DWA optimization