Path planning method for unmanned tractor based on improved PSO fusion DWA optimization algorithm

Abstract

The application provides an unmanned tractor path planning method based on an improved PSO fusion DWA optimization algorithm, comprising: grid map initialization; forming a preliminary global path by using an improved PSO algorithm; performing local fine optimization on the preliminary global path by one-dimensional neighborhood disturbance and greedy selection; performing discrete path continuous, redundant node elimination and smoothing processing on the preliminary global path by a greedy optimization method; completely retaining a path point sequence of the final optimized global path as a local target point of the DWA algorithm, guiding local path planning and real-time obstacle avoidance, performing velocity sampling and trajectory prediction based on a kinematics model, a velocity constraint and the local target point, and selecting an optimal predicted trajectory and a velocity space by an improved evaluation function in the DWA optimization algorithm. The application effectively solves the problems of poor smoothness, long planning time, many inflection points, discontinuous path curvature and inability to achieve global optimization of the existing planning method.

Description

Technical Field

[0001] This invention belongs to the field of tractor path planning and design technology, and in particular relates to an unmanned tractor path planning method based on an improved PSO fusion DWA optimization algorithm. Background Technology

[0002] With the rapid development of smart agriculture, the application of driverless tractors in farmland operations is becoming increasingly widespread, and the demand for efficient and intelligent path planning is becoming more and more urgent. In the complex environment of farmland, there are diverse crop distributions, undulating terrain, agricultural machinery and other dynamic interferences, which makes traditional manual tractor operation not only inefficient and labor-intensive, but also difficult to accurately meet the high requirements of precision agricultural production for paths, such as precise control of row spacing and timely obstacle avoidance. Therefore, path planning technology for driverless tractors has become a key support for the development of smart agriculture.

[0003] As a swarm intelligence optimization algorithm, Particle Search (PSO) can achieve global optimization in path planning through particle search. However, traditional PSO algorithms are prone to getting trapped in local optima, and the planned paths may be redundant, lack smoothness, and lack precise matching to the kinematic characteristics of tractors. DWA (Distance Avoidance Path) is a widely used local obstacle avoidance algorithm, but while the original DWA can accurately "recognize" unknown obstacles and achieve real-time local obstacle avoidance, it is easily trapped in local optima and cannot achieve global optimization.

[0004] Currently, some studies have attempted to combine PSO and DWA algorithms in existing path planning optimization schemes, but these still have limitations. Existing technology (CN118067126A) discloses a robot path planning method invented by Shi Xinxin et al. This method uses the DWA algorithm to sample in the velocity space, generating multiple combinations of linear and angular velocities. It then uses a kinematic model to deduce the trajectory and selects the current optimal trajectory based on an evaluation function. Subsequently, the optimal velocity combination output by DWA is used as the initial particle position for the PSO algorithm, and the DWA evaluation function is used as the fitness function. The velocity pairs are further optimized through iterative updates by the particle swarm optimization, resulting in a shorter, safer, and more accurate path. Although simulation results show that the method can effectively shorten the path length and improve the obstacle avoidance success rate, the core of the optimization of the method lies in the secondary optimization of the velocity pair output by DWA in a single step. No matter how PSO is improved, its search range is always limited to the velocity space near the optimal solution of DWA at the current moment, lacking a global perspective. Moreover, the fitness function of PSO directly adopts the evaluation function of DWA. This function only comprehensively evaluates the instantaneous and local costs such as azimuth angle, distance from obstacle and velocity, and lacks a global consideration of the entire path, such as the total path length and overall smoothness.

[0005] To address the aforementioned issues, this invention presents a novel path planning method for unmanned tractors based on an improved PSO-integrated DWA optimization algorithm. Summary of the Invention

[0006] To address the problems of existing technologies, this invention provides a path planning method for unmanned tractors based on an improved PSO fusion DWA optimization algorithm. First, a global path is generated based on the improved PSO algorithm, shifting the optimization focus from the local velocity space to the global path node sequence, thus realizing a paradigm shift from local optimization to global planning. Then, the improved PSO algorithm is employed, with its fitness function incorporating path length and an explicit collision penalty mechanism. This allows the algorithm to simultaneously optimize the two key global metrics of shortest path and collision safety when searching for a global path. Furthermore, an innovative two-layer architecture of "global planning + local refinement + local execution" is adopted, perfectly combining the guidance of the global path with the real-time obstacle avoidance capability of the local DWA algorithm. This effectively solves the problems of localized optimization scope, lack of global vision, singular optimization objective, and insufficient consideration of the overall characteristics of the path in traditional path planning methods.

[0007] The present invention achieves the above-mentioned technical objectives through the following technical means.

[0008] A path planning method for unmanned tractors based on an improved PSO-DWA optimization algorithm includes the following steps:

[0009] S1: Initialize the raster map;

[0010] S2: Set the starting and ending points of the path planning, and use the improved PSO algorithm to expand the particle swarm and form a preliminary global path;

[0011] S3: Perform local fine-grained optimization of the initial global path through one-dimensional neighborhood perturbation and greedy selection;

[0012] S4: The preliminary global path after local fine-tuning optimization is discretized into a continuous path, redundant nodes are removed and smoothed by a greedy optimization method to obtain a further optimized global path;

[0013] S5: The path point sequence of the global path optimized by S4 is completely preserved as the local target point of the DWA algorithm to guide path planning and real-time obstacle avoidance; based on the kinematic model, velocity constraints and local target points, velocity sampling and trajectory prediction are performed, and the optimal predicted trajectory and velocity space are selected through the improved evaluation function in the DWA optimization algorithm, realizing the fusion of the improved PSO algorithm and the DWA optimization algorithm. Then, the speed command is sent to the chassis controller through the on-board industrial control computer, so that the tractor moves along the optimal trajectory.

[0014] S6: After the upper control unit determines that the tractor has reached the destination, it backtracks through all nodes through the human-machine interface module to extract and output the optimal path.

[0015] Furthermore, the specific process of S2 is as follows:

[0016] S2.1: Set particle swarm parameters, including particle count and particle dimension. The maximum number of iterations, particle dimension (i.e., the number of intermediate points on the path), initialization of parameters related to the maximum particle velocity, determination of the inertia weight range and the learning factor range;

[0017] S2.2: Determine the starting and ending points of the path planning, construct the particle position matrix, and initialize the particle swarm;

[0018] S2.3: Traverse each particle and each dimension, extracting the... Midpoint The raster data in the column, i.e., the first For the raster data in a column, filter the accessible raster row indices in that column, and randomly select one from the accessible raster row indices as the row coordinates of the intermediate point. ;

[0019] S2.4: Start iterative search. In each iteration, dynamically adjust the inertia weight and learning factor, update the individual optimal position and global optimal position using the fitness function of the improved PSO algorithm, and update the particle velocity and position according to the velocity update formula and position update formula.

[0020] The dynamic adjustment formulas for inertia weight and learning factor are as follows:

[0021]

[0022]

[0023]

[0024] In the formula, , These are the inertia weights. The maximum and minimum values; Indicates the current iteration number; Indicates the maximum number of iterations; It is a constant greater than -1; , Individual learning factors The maximum and minimum values; , Social learning factors The upper and lower bounds;

[0025] The path quality is evaluated based on two dimensions: path length and collision risk. The path length is the sum of the Euclidean distances between adjacent points on the path. For paths with collisions, the collision risk is quantified by the number of collisions and the map area. The penalty value for a tractor colliding with an obstacle is set as: map area × number of collisions.

[0026] By using weighted path length and collision penalty values, path planning is transformed into an optimization problem; therefore, the fitness function is:

[0027]

[0028] in, , To adjust the relative importance weighting factor between path length and collision penalty value, The fitness function; This represents the path length. This is the collision penalty value;

[0029] S2.5: Evaluate the path quality represented by the particle based on path length and collision penalty mechanism, thereby calculating the fitness value and using it as a rating index for particle update effect; compare the calculated fitness value with the best fitness value of the particle in its history; if the current fitness value is better, update the local optimum; compare the calculated fitness value with the global optimum fitness value to update the global optimum, and determine the position and velocity of the particle iteration according to the particle velocity and position update formula;

[0030] S2.6: Before reaching the maximum number of iterations, continue executing S2.3 to S2.5 to continuously update the particle state and the global optimal solution;

[0031] S2.7: After reaching the maximum number of iterations, determine the final globally optimal position at this point. The corresponding intermediate points of the path, combined with the starting point and the ending point, form a preliminary global path.

[0032] Furthermore, the specific process of S3 is as follows:

[0033] S3.1: Extract the globally optimal solution obtained from S2 The decision variable dimension is the number of intermediate points on the path, and the current global optimal solution is calculated. fitness value ;

[0034] S3.2: For the global optimal solution Each intermediate node Construct neighborhood solutions sequentially. :

[0035] Keep except the first The positions of all nodes except the intermediate node remain unchanged within the feasible region. A uniformly random integer is selected as the new intermediate node. The row index is used to replace the original intermediate node. Generate neighborhood solutions , The corresponding row number of the raster map;

[0036] S3.3: For each neighborhood solution Calculate its fitness value and the original global optimal solution fitness value Compare; if Then the global optimal solution will be updated to Otherwise, retain the original solution and do not update it.

[0037] Furthermore, the specific process of S4 is as follows:

[0038] S4.1: Preliminary global path after local refinement in S3 , where each path point , , For path point row index, Index for path point columns, The number of path points. Starting from, Starting from the starting point and using the endpoint as the destination, traverse each pair of adjacent path points in ascending column order. and By calculating the row difference between adjacent path points, discrete path points are transformed into continuous paths, and continuous paths are gradually constructed. ;

[0039] S4.2: For each pair of adjacent path points, based on the row difference Refine the path filling logic to ensure that continuous paths do not collide with obstacles:

[0040] When ≤1, directly use Join ;

[0041] When h>1, check the connectivity of the intermediate grid between the columns containing the two path points; set the intermediate grid to be the one with the path points in the middle. Starting from the column, along the line Extending vertically to The resulting grid set ;

[0042] If satisfied Then when At that time, in sequence These waypoints are added ,when Then, in sequence, These waypoints are added Finally add This creates a vertically continuous path; among which, Represents the raster map. Line number The status of the column, Indicates a passable free grid. Represents an obstacle grid;

[0043] like In the list Find the closest one in the middle grid. Obstacle grid The coordinates satisfy The result is the smallest and In relation to Adjacent columns In China, with Starting from the direction, along row coordinates Directional search alternative points And the substitution point satisfies ;

[0044] Specifically, first set the search step size. initial value If no alternative point is found in a single search, adjust the step size as follows:

[0045]

[0046] In the formula, For the first Next search step size, For the first Next search step size, This indicates the search step size, which is the number of grid cells searched vertically in each step. are positive integers and satisfy , This represents the number of rows in the raster map.

[0047] If a passable alternative point is found, the original path point will be moved. Temporary update to alternative point Recalculate row differences Return to the filling logic processing until a collision-free continuous segment is generated; if the search step size is... If no alternative point is found after reducing the number of attempts to 1 and the maximum number of attempts has been reached, then skip the current segment and continue processing subsequent adjacent node pairs based on the existing nodes;

[0048] S4.3: For the continuous paths generated above Smoothing and redundant node removal operations are performed here. , , For path point row index, Index for path point columns, Number of consecutive path points:

[0049] Select three consecutive path points in sequence Construct vector sum vector Based on the principle of vector cross product, through calculation formula To determine whether three points are approximately collinear, we can use the following method: , , respectively path points , , row index, , , respectively path points , , Column index;

[0050] Specifically, if Then they can be determined to be approximately collinear, where, The threshold for determining collinearity;

[0051] If the three points are approximately collinear, further examination is needed. and Is the grid area covered by the connecting line free of obstacles? The covered area is:

[0052] in, Corresponding row index, Corresponding column index;

[0053] If satisfied This indicates the midpoint. If a node is redundant, removing it will ensure the path remains continuous and collision-free. From continuous path Remove the middle point; otherwise, keep the middle point. ;in, Represents the raster map. Line number Column status, Indicates a passable free grid;

[0054] S4.4: Repeat S4.3 until all iterations are complete. By considering all three combinable points, the optimized global path is finally obtained. .

[0055] Furthermore, the specific process of S5 is as follows:

[0056] S5.1: Retain the path point sequence of the global path optimized in S4.4; Based on the tractor kinematics model and velocity constraints, the tractor performs velocity sampling in the velocity space set to simulate an infinite number of feasible motion trajectories within a certain time interval;

[0057] S5.2: Extract the path point with the smallest distance to the tractor's current position from the sequence of path points in the preserved global path, and use it as the reference anchor point for local guidance to ensure that the guidance direction matches the current movement stage of the tractor;

[0058] Specifically, tractor acquisition via GPS Real-time pose at any moment ,in, For tractors in Planar coordinates at time, Corresponding tractor The heading angle at any given moment, in the global path The method determines the relationship between the tractor and the path point by calculating the Euclidean distance between the path point and the tractor's current position. The path point with the minimum distance is defined as... At the same time, with Starting from the global path Select the next two consecutive path points to form a local guided target sequence. ;

[0059] S5.3: Evaluate the trajectory using the evaluation function of the DWA algorithm, and select the optimal trajectory from the infinite number of feasible motion trajectories generated in S5.1; where the evaluation function of the DWA algorithm is shown in the following formula:

[0060]

[0061] In the formula, This represents the angular difference between the direction at the end of the sampling trajectory and the direction of the local target. This represents the distance between the end point of the sampling trajectory and the local target point; Indicates the current speed; , , These are the weight coefficients corresponding to each sub-function of the evaluation function; Represents the normalization process;

[0062] S5.4: Preserve the velocity space corresponding to the optimal trajectory and perform velocity smoothing processing;

[0063] S5.5: The on-board industrial control computer sends speed commands to the chassis controller, causing the tractor to move along the optimal trajectory obtained in S5.3;

[0064] S5.6: The upper control unit determines whether the tractor has moved to the destination. If it has moved to the destination, the process ends; otherwise, S5.1 to S5.5 are executed repeatedly.

[0065] The present invention has the following beneficial effects:

[0066] This invention solves the problems of poor smoothness, long planning time, many inflection points, discontinuous path curvature, and inability to achieve global optimum when existing PSO and DWA algorithms are used alone.

[0067] This invention improves the traditional PSO algorithm in multiple dimensions. First, it constructs a composite data structure coupling a multidimensional matrix and a hash table for particle information storage, significantly reducing path planning time. Second, it adopts an iterative process-driven dynamic adjustment strategy for inertia weights and learning factors to accurately balance the algorithm's global exploration capability and local development capability. Furthermore, it refines the global path converged by PSO iterations locally through single-dimensional neighborhood perturbation and greedy selection. Finally, it proposes a fitness function encompassing path continuity construction, smoothing, and collision penalty mechanisms, comprehensively considering path feasibility, smoothness, and obstacle avoidance requirements to achieve accurate selection of high-quality path solutions.

[0068] The trajectory sampling evaluation function of the original DWA algorithm is optimized. On the one hand, it is deeply integrated with the global path key nodes output by the improved PSO algorithm to enhance the fit of local path planning to the global optimal path and ensure that local obstacle avoidance behavior does not deviate from the global optimal trend. On the other hand, the obstacle distance sub-function is refined to accurately quantify obstacle avoidance requirements, improve local path search efficiency and obstacle avoidance sensitivity, and provide a guarantee for the safe operation of unmanned tractors in dynamic farmland environments.

[0069] Both the PSO and DWA algorithms have limitations when used individually. This invention integrates the global path planning capability of the improved PSO algorithm with the local real-time obstacle avoidance advantage of the DWA algorithm. By using the global path generated by PSO to extract local key guidance points, the DWA algorithm is guided to carry out local path planning and dynamic obstacle avoidance. This not only overcomes the problems of insufficient local fine optimization capability of the PSO algorithm and lack of global vision of the DWA algorithm, but also improves path planning efficiency, obstacle avoidance sensitivity and operational safety, and adapts to the high-efficiency and safe operation requirements of unmanned tractors in complex farmland environments.

[0070] Simulation comparison experiments were conducted using Matlab to verify the effectiveness and superiority of the improved PSO-DWA optimization algorithm proposed in this invention. Simultaneously, the algorithm was deployed on a self-built unmanned tractor test platform and tested in a simulated farmland operation path planning scenario. Experimental results show that the improved algorithm can efficiently plan paths that satisfy the tractor's kinematic constraints, and the fusion algorithm combines global optimal search capability with local real-time obstacle avoidance characteristics, adapting to the dynamic operation requirements of complex farmland environments and effectively supporting the precise and safe operation of unmanned tractors. Attached Figure Description

[0071] Figure 1 This is a flowchart of the unmanned tractor path planning method based on the improved PSO fusion DWA optimization algorithm described in this invention.

[0072] Figure 2 A schematic diagram illustrating the dynamic parameter changes in the improved PSO algorithm;

[0073] Figure 3 To improve the convergence curve of the PSO algorithm;

[0074] Figure 4 The simulation example of path planning for the improved PSO algorithm based on a greedy optimization strategy is shown in the figure.

[0075] Figure 5 The simulation results of global path planning using the traditional PSO algorithm in a 20*20 grid environment are shown in the figure.

[0076] Figure 6 This is a simulation result of the global path planning of the improved PSO algorithm in a 20*20 grid environment.

[0077] Figure 7 The simulation results of global path planning using the traditional PSO fusion DWA algorithm in a 20*20 grid environment are shown.

[0078] Figure 8 This is a simulation result of the global path planning of the improved PSO fusion DWA optimization algorithm in a 20*20 grid environment. Detailed Implementation

[0079] The present invention will be further described below with reference to the accompanying drawings and specific embodiments, but the scope of protection of the present invention is not limited thereto.

[0080] The path planning method for unmanned tractors based on the improved PSO fusion DWA optimization algorithm described in this invention is as follows: Figure 1 As shown, the specific process includes the following:

[0081] S1: Initialize the raster map;

[0082] S1.1: Define the raster map Basic parameters include raster resolution, map boundaries, and coordinate system. A unified raster value definition is used, defining a raster value of 0 as a passable area and a raster value of 1 as a static obstacle.

[0083] S1.2: Collect farmland environmental information, including the starting point and ending point of the tractor and the location of obstacles, convert them into map grid coordinates and mark them.

[0084] S2: Set the starting and ending points of the path planning, and use the improved Particle Swarm Optimization (PSO) algorithm to expand the particle swarm and form a preliminary global path;

[0085] S2.1: Set particle swarm parameters, including the number of particles N and the particle dimension (number of intermediate points on the path). 1. Maximum number of iterations Max_Iter; Initialize the parameters related to the maximum particle velocity, determine the range of inertia weight and learning factor, and set the data structure for storing particle information, such as particle position, velocity, individual optimal position and fitness value.

[0086] S2.2: First, determine the starting and ending points of the path planning, construct the particle position matrix and initialize the particle swarm, transforming the path planning problem into a particle search problem in multidimensional space.

[0087] S2.3: Unlike the traditional method of initializing particle positions by "randomly generating arbitrary values", this invention iterates through each particle and each dimension. That is, the first one on each path The intermediate node determines the initial position of the particle; specifically, the first intermediate node is extracted. intermediate nodes Column (number) Given raster data in a column, filter the row indices of accessible raster cells (raster values ​​of 0) in that column, and randomly select one of the accessible raster row indices as the row coordinate of the node.

[0088] S2.4: Then, the iterative search begins. In each iteration, the inertia weight and learning factor are dynamically adjusted, and the fitness function of the improved PSO algorithm is used to update the individual's optimal position. and global optimal position And update the particle velocity and position according to the velocity update formula and the position update formula;

[0089] The dynamic adjustment methods for inertia weight and learning factor are as follows:

[0090]

[0091]

[0092]

[0093] In the formula, , These are the inertia weights. The maximum and minimum values ​​are 0.9 and 0.4 respectively; Indicates the current iteration number; Indicates the maximum number of iterations; It is a constant greater than -1, as confirmed by experiments. The algorithm performs best when the value is 20. , Individual learning factors The maximum and minimum values; , Social learning factors The upper and lower boundaries.

[0094] In the algorithm iteration process, inertia weight From near Towards Dynamic adjustment allows the algorithm to rely on large inertia in the early stages to quickly expand the global search range, and then use small inertia and slower speed to focus on fine-tuning local optimization in the later stages; learning factor , Based on the iteration stage, in the early stage, by increasing... Strengthen the guidance of individual particle historical experience, maintain group diversity to ensure overall exploration efficiency, and simultaneously reduce To avoid premature convergence caused by excessive dominance of the group optimum, the weights of the two factors are adjusted in the later stages of iteration, by weakening... ,strengthen The driving force particles coordinate to gather towards the global optimum, accelerating the algorithm's convergence process; through staged parameter adaptation, the two achieve a dynamic balance between global exploration and local search capabilities, ensuring the algorithm's optimization efficiency throughout the entire iteration cycle.

[0095] To improve the overall efficiency of path planning and comprehensively optimize path quality, path quality is evaluated based on two dimensions: path length and collision risk. This ensures that the generated path is as short as possible while effectively avoiding all obstacles. The path length is defined as follows: It is the sum of the Euclidean distances between adjacent points on the path, i.e.:

[0096]

[0097] In the formula, , The first in the path Points Row and column coordinates on a two-dimensional plane; Indicates the number of path points;

[0098] For collision-prone paths, collision risk is quantified using the number of collisions and map area, and a penalty value for collisions between tractors and obstacles is defined. The value is "map area × number of collisions", that is:

[0099]

[0100] In the formula, The area of ​​the environmental map. Used to determine the first in the path Points Whether a collision occurs with an obstacle; specifically, if the point is located within an obstacle grid, i.e. ,but =1, otherwise 0;

[0101] By using weighted path length and collision penalty values, the tractor path planning problem is transformed into an optimization problem; therefore, the fitness function is:

[0102]

[0103] in, , To adjust the relative importance weighting factor between path length and collision penalty value, The fitness function is the criterion used in the PSO algorithm to evaluate path quality. The smaller the value, the higher the path quality.

[0104] S2.5: Evaluate the path quality represented by the particle based on path length and collision penalty mechanism to calculate the fitness value and use it as a rating index for particle update effect; compare the fitness value with the particle's historical best fitness value, and if the current fitness value is better, update the local optimum; compare the particle's fitness value with the global best fitness value to update the global optimum, and determine the particle's iteration position and velocity according to the velocity update and position update formulas:

[0105] The formulas for updating particle velocity and position are as follows:

[0106]

[0107] In the formula, and They are the first The particle in the first and The speed of time; Inertial weights are used to control the degree to which the particle's previous velocity affects its current velocity; A random number between (0, 1); and They are the first Individual particles and The position at that moment; This represents the optimal position found by the i-th particle in its own historical search. This represents the globally optimal position found by the entire particle swarm.

[0108] S2.6: Before reaching the maximum number of iterations, continue executing steps S2.3 to S2.5 to continuously update the particle state and the global optimal solution;

[0109] S2.7: After reaching the maximum number of iterations, determine the final globally optimal position at this point. The corresponding intermediate points of the path, combined with the starting point and the ending point, form a preliminary global path.

[0110] S3: Local refinement optimization of the initial global path for PSO iterative convergence is performed through one-dimensional neighborhood perturbation and greedy selection;

[0111] S3.1: Extract the global optimal solution The decision variable dimension, namely the number of intermediate points on the path. And calculate the fitness value of the current global optimal solution. This serves as a benchmark for evaluating the quality of subsequent optimizations.

[0112] S3.2: For the global optimal solution Each intermediate node Construct neighborhood solutions sequentially:

[0113] Specifically, keep except the first The positions of all nodes except the intermediate node remain unchanged within the feasible region. ( Corresponding grid map A new intermediate node is randomly selected uniformly from the rows (number of rows). The row index is used to replace the original intermediate node. Generate neighborhood solutions ;

[0114] S3.3: For each neighborhood solution The fitness value is calculated using the fitness function. and the original global optimal solution fitness value Compare; if Then the global optimal solution will be updated to Otherwise, retain the original solution and do not update it.

[0115] This invention improves the particle update function of the traditional PSO algorithm by employing a dynamic parameter adjustment strategy, including:

[0116] Reference Figure 2 , 3 As shown, unlike the traditional PSO algorithm where the inertia weight decreases linearly or is fixed, and the learning factor has a fixed value, this invention adjusts the inertia weight according to the number of iterations using a specific nonlinear formula. and learning factors , Dynamic adjustment. In the initial iteration phase, a large inertia weight and learning factor are used. Smaller This endows particles with strong global search capabilities, enabling them to rapidly explore a large area of ​​the farmland grid map and avoid getting trapped in local optima too early; in the later stages of iteration, the inertia weight is reduced. Lower and The increased particle focus allows for more refined local search, accelerating convergence to the global optimum. This dynamic collaborative mechanism precisely balances the global search and local exploitation capabilities at different stages of the algorithm, enhancing its adaptability to path planning in complex farmland environments.

[0117] This invention employs path length and collision risk as two dimensions to design the fitness function for path quality, fully considering the actual needs of path planning for unmanned tractors. It overcomes the limitations of traditional PSO (Path Planning Optimization) which only considers the objective function value, and includes:

[0118] For collision-free paths, a complete path topology is constructed by spatially stitching together particle positions and start and end coordinates. Relying on a continuous path generation algorithm, continuous interpolation of discrete path points is achieved. Then, a secondary path smoothing algorithm is used to process curvature constraints. The Euclidean length of the final smoothed path is used as a fitness metric to ensure path connectivity and kinematic smoothness, matching the trajectory feasibility requirements under nonholonomic constraints of tractors. For collision-involved paths, a penalty mechanism is introduced, using "map area × number of collisions" as the fitness penalty term, incorporating path safety into the multi-objective optimization dimension.

[0119] This invention employs single-dimensional neighborhood perturbation and greedy selection to perform "local fine-tuning" of the globally optimal path converged by PSO iteratively. This avoids the inefficiency of PSO's blind global search and compensates for its weak local optimization capability, forming a closed loop of "global-local" collaborative optimization, including:

[0120] Determine the global optimal solution The decision variable dimensions are determined, and their fitness values ​​are calculated. As an evaluation benchmark; within the feasible region Internal random perturbation Each intermediate node Generate one-dimensional neighborhood solutions Then update the strategy using a greedy selection algorithm. and ;

[0121] In summary, the fitness evaluation function proposed in this invention guides the algorithm to converge to a collision-free and kinematically feasible optimal path through a priority mechanism, effectively solving the coupling problem of obstacle avoidance constraints and trajectory quality optimization in path planning in farmland scenarios. In addition, based on the ideas of single-dimensional neighborhood perturbation and greedy selection, this invention performs local search on the initial global path generated by PSO, making up for the "local suboptimal" defect that PSO may have due to its reliance on "group extreme value - individual extreme value" guidance.

[0122] S4: A greedy optimization method is used to perform discrete path continuation, redundant node removal, and smoothing on the initial global path after local refinement optimization, resulting in a further optimized global path. (Refer to...) Figure 4 As shown; the specific process is as follows:

[0123] S4.1: Preliminary global path after local refinement in S3 Each node , For node row indexing, Indexing the node column, This represents the number of path nodes. Starting from, Starting from the starting point and using the endpoint as the destination, traverse each pair of adjacent path points in ascending column order. and By calculating the row difference between adjacent path points, discrete path points are transformed into continuous paths, and continuous paths are gradually constructed. (Initially empty, add the starting point first) );

[0124] S4.2: For each pair of adjacent path points (i.e., adjacent nodes), based on the row difference... Refine the path filling logic to ensure that continuous paths do not collide with obstacles:

[0125] When ≤1, directly use Join ;

[0126] When h>1, check the connectivity of the intermediate grid between the columns containing the two nodes; define the intermediate grid as... Starting from the column, along the line Extending vertically to The resulting grid set ;

[0127] If satisfied , then when At that time, in sequence These waypoints are added ,when Then, in sequence, These waypoints are added Finally add This creates a vertically continuous path; among which, Represents the raster map. Line number Column status ( Indicates a passable free grid. (Represents an obstacle grid);

[0128] like In the list Find the closest one in the middle grid. Obstacle grid The coordinates satisfy The result is the smallest and In relation to Adjacent columns In China, with Starting from the direction, along row coordinates Directional search alternative points And the substitution point satisfies ;

[0129] Specifically, first set the search step size. ( This indicates the number of grid cells searched vertically each time. are positive integers and satisfy , The initial value (for the number of rows in the raster map) If no alternative point is found in a single search, adjust the step size as follows:

[0130]

[0131] In the formula, For the first Next search step size, For the first Next search step size;

[0132] If a passable alternative point is found, the original node will be moved. Temporary update to alternative point Recalculate row differences Return to the filling logic processing until a collision-free continuous segment is generated; if the search step size is... If no alternative point is found after reducing the number of attempts to 1 and the maximum number of attempts has been reached, then skip the current segment and continue processing subsequent adjacent node pairs based on the existing nodes;

[0133] S4.3: For the continuous paths generated above (here) , , For node row indexing, Indexing the node column, Smoothing and redundant node removal operations are performed on the number of consecutive path points.

[0134] Select three consecutive path points in sequence Construct vector sum vector Based on the principle of vector cross product, through calculation formula To determine whether three points are approximately collinear, we can use the following method: , , respectively path points , , row index, , , respectively path points , , Column index;

[0135] Specifically, if Then they can be determined to be approximately collinear, where, The threshold for determining collinearity is set to 0.5.

[0136] If the three points are approximately collinear, further examination is needed. and Whether the grid area covered by the connecting line is free of obstacles; the coverage area is defined as:

[0137] in, Corresponding row index, Corresponding column index;

[0138] If satisfied This indicates the midpoint. If a node is redundant, removing it will ensure the path remains continuous and collision-free. From continuous path Remove the middle point; otherwise, keep the middle point. ;in, Represents the raster map. Line number Column status, Indicates a passable free grid;

[0139] S4.4: Repeat S4.3 until all iterations are complete. By considering all three combinable points, the optimized global path is ultimately obtained. .

[0140] S5: The path point sequence of the global path optimized in S4.4 is fully preserved as the local target point of the Dynamic Window Approach (DWA) algorithm to guide local path planning and real-time obstacle avoidance; velocity sampling and trajectory prediction are performed based on the kinematic model, velocity constraints, and local target points, and the optimal predicted trajectory and velocity space are selected through the improved evaluation function in the DWA optimization algorithm, realizing the fusion of the improved PSO algorithm and the DWA optimization algorithm, as detailed below:

[0141] S5.1: Based on the tractor kinematics model and velocity constraints, the tractor in the velocity space set Velocity sampling is performed to simulate an infinite number of feasible motion trajectories within a certain time interval;

[0142] The tractor kinematic model assumes that the tractor moves within a certain time period. The kinematic model for uniform linear motion inside the interior is as follows:

[0143]

[0144] In the formula, , These respectively represent the tractor in time, The row coordinate at time; , These respectively represent the tractor in time, The column coordinates of time; , These respectively represent the tractor in time, The heading angle at any given moment; , These respectively represent the tractor in Linear velocity and angular velocity at any given moment; Indicates the time resolution, taken as 0.1s;

[0145] Velocity space set This refers to the velocity space after velocity constraints. Since the velocity space obtained from the kinematic model during the forward simulation of the tractor is an ideal velocity space that does not take into account its own and environmental factors, in practice, it is necessary to consider the impact of constraints on velocity sampling and constrain the velocity space within a certain range. Velocity sampling will be subject to the following constraints:

[0146] 1. Due to the hardware limitations and environmental constraints of tractors, their speeds are subject to boundary restrictions, resulting in a sampleable speed space set. :

[0147]

[0148] In the formula, , These are the minimum and maximum values ​​of the linear velocity. , These represent the minimum and maximum angular velocities; , These represent the linear velocity and angular velocity of the tractor, respectively.

[0149] 2. Due to the limitations of the tractor's drive motor, the velocity space set is obtained. :

[0150]

[0151] In the formula, , The current linear velocity and angular velocity of the tractor. , The maximum linear and angular deceleration, , These are the maximum linear and angular accelerations;

[0152] 3. Constrained by a safe distance, the velocity space set is obtained. :

[0153]

[0154] In the formula, This represents the minimum distance between the end of the predicted trajectory and the obstacle;

[0155] Final feasible velocity space set for:

[0156]

[0157] S5.2: Extract the path point with the smallest distance to the tractor's current position from the sequence of path points in the preserved global path, and use it as the reference anchor point for local guidance to ensure that the guidance direction matches the current movement stage of the tractor;

[0158] Specifically, tractor acquisition via GPS Real-time pose at any moment ,in For tractors in Planar coordinates at time, Corresponding tractor The heading angle at any given moment, in the global path The method determines the relationship between the tractor and the path point by calculating the Euclidean distance between the path point and the tractor's current position. The path point with the minimum distance is defined as... At the same time, with Starting from the global path Select the next two consecutive path points to form a local guided target sequence. ;

[0159] S5.3: Use the evaluation function of the DWA algorithm to evaluate the trajectory and select the optimal trajectory from the infinite number of feasible motion trajectories generated in S5.1;

[0160] The evaluation function of the DWA algorithm is shown in the following formula:

[0161]

[0162] In the formula, This represents the angular difference between the direction at the end of the sampling trajectory and the direction of the local target. This represents the distance between the end point of the sampling trajectory and the local target point, which together make the predicted trajectory closer to the global path; Indicates the current speed; , , These are the weight coefficients corresponding to each sub-function of the evaluation function; This represents the normalization process; because a certain term in the evaluation function may be too prominent when scoring, it is necessary to divide each term of the evaluation function by the sum of the corresponding terms, which is the normalization process.

[0163] S5.4: Preserve the velocity space corresponding to the optimal trajectory and perform velocity smoothing processing;

[0164] S5.5: The on-board industrial control computer sends speed commands to the chassis controller, causing the tractor to move along the optimal trajectory obtained in S5.3;

[0165] S5.6: The upper control unit determines whether the tractor has moved to the destination. If it has moved to the destination, the process ends; otherwise, S5.1 to S5.5 are executed repeatedly.

[0166] This invention, based on the improved PSO algorithm for directional guidance by extracting local key points, integrates the DWA optimization algorithm for path planning and combines it with LiDAR to detect the surrounding environment in real time to achieve real-time dynamic obstacle avoidance. The DWA optimization algorithm performs local path planning between every two adjacent local key points. Simultaneously, the DWA evaluation function ensures that the local planning is as close as possible to the globally optimal path, and it has the ability to automatically switch to the next local target point for guidance when the predicted trajectory ends one conflict radius away from the nearest local target point.

[0167] S6: After reaching the destination, backtrack through all nodes via the human-computer interaction interface module, extract and output the optimal path, and complete the improved PSO-DWA optimization algorithm, thus completing the path planning and obstacle avoidance based on the algorithm.

[0168] Examples of simulation results for global path planning using the traditional PSO algorithm in a 20*20 grid environment are as follows: Figure 5 As shown, an example of the global path planning simulation results of the improved PSO algorithm in a 20*20 grid environment is as follows. Figure 6 As shown, an example of global path planning simulation results of the traditional PSO fusion DWA algorithm in a 20*20 grid environment is as follows. Figure 7 As shown, an example of the global path planning simulation results of the improved PSO-integrated DWA optimization algorithm in a 20*20 grid environment is as follows. Figure 8 As shown in the simulation results above, the path planning method of the present invention effectively solves the problems of poor smoothness, long planning time, many inflection points, discontinuous path curvature, and inability to achieve global optimum when the existing PSO algorithm and DWA algorithm are used alone.

[0169] The embodiments described above are preferred embodiments of the present invention, but the present invention is not limited to the above embodiments. Any obvious improvements, substitutions or modifications that can be made by those skilled in the art without departing from the essence of the present invention shall fall within the protection scope of the present invention.

Claims

1. A path planning method for unmanned tractors based on an improved PSO-DWA optimization algorithm, characterized in that, The process includes the following: S1: Initialize the raster map; S2: Set the starting and ending points of the path planning, and use the improved PSO algorithm to expand the particle swarm and form a preliminary global path; S3: Perform local fine-grained optimization of the initial global path through one-dimensional neighborhood perturbation and greedy selection; S4: The preliminary global path after local fine-tuning optimization is discretized into a continuous path, redundant nodes are removed and smoothed by a greedy optimization method to obtain a further optimized global path; S5: The path point sequence of the global path optimized by S4 is completely preserved as the local target point of the DWA algorithm to guide path planning and real-time obstacle avoidance; based on the kinematic model, velocity constraints and local target points, velocity sampling and trajectory prediction are performed, and the optimal predicted trajectory and velocity space are selected through the improved evaluation function in the DWA optimization algorithm, realizing the fusion of the improved PSO algorithm and the DWA optimization algorithm. Then, the speed command is sent to the chassis controller through the on-board industrial control computer, so that the tractor moves along the optimal trajectory. S6: After the upper control unit determines that the tractor has reached the destination, it backtracks through all nodes through the human-machine interface module to extract and output the optimal path.

2. The path planning method for unmanned tractors based on the improved PSO fusion DWA optimization algorithm according to claim 1, characterized in that, The specific process of S2 is as follows: S2.1: Set particle swarm parameters, including particle count and particle dimension. The maximum number of iterations, particle dimension (i.e., the number of intermediate points on the path), initialization of parameters related to the maximum particle velocity, determination of the inertia weight range and the learning factor range; S2.2: Determine the starting and ending points of the path planning, construct the particle position matrix, and initialize the particle swarm; S2.3: Traverse each particle and each dimension, extracting the first... Midpoint The raster data in the column, i.e., the first For the raster data in a column, filter the accessible raster row indices in that column, and randomly select one from the accessible raster row indices as the row coordinates of the intermediate point. ; S2.4: Start iterative search. In each iteration, dynamically adjust the inertia weight and learning factor, update the individual optimal position and global optimal position using the fitness function of the improved PSO algorithm, and update the particle velocity and position according to the velocity update formula and position update formula. The dynamic adjustment formulas for inertia weight and learning factor are as follows: ； ； ； In the formula, , These are the inertia weights. The maximum and minimum values; Indicates the current iteration number; Indicates the maximum number of iterations; It is a constant greater than -1; , Individual learning factors The maximum and minimum values; , Social learning factors The upper and lower bounds; The path quality is evaluated based on two dimensions: path length and collision risk. The path length is the sum of the Euclidean distances between adjacent points on the path. For paths with collisions, the collision risk is quantified by the number of collisions and the map area. The penalty value for a tractor colliding with an obstacle is set as: map area × number of collisions. By using weighted path length and collision penalty values, path planning is transformed into an optimization problem; therefore, the fitness function is: ； in, , To adjust the relative importance weighting factor between path length and collision penalty value, The fitness function; This is the path length; This is the collision penalty value; S2.5: Evaluate the path quality represented by the particle based on path length and collision penalty mechanism, thereby calculating the fitness value and using it as a rating index for particle update effect; compare the calculated fitness value with the best fitness value of the particle in its history; if the current fitness value is better, update the local optimum; compare the calculated fitness value with the global optimum fitness value to update the global optimum, and determine the position and velocity of the particle iteration according to the particle velocity and position update formula; S2.6: Before reaching the maximum number of iterations, continue executing S2.3 to S2.5 to continuously update the particle state and the global optimal solution; S2.7: After reaching the maximum number of iterations, determine the final globally optimal position at this point. The corresponding intermediate points of the path, combined with the starting point and the ending point, form a preliminary global path.

3. The path planning method for unmanned tractors based on the improved PSO fusion DWA optimization algorithm according to claim 2, characterized in that, The specific process of S3 is as follows: S3.1: Extract the globally optimal solution obtained from S2 The decision variable dimension is the number of intermediate points on the path, and the current global optimal solution is calculated. fitness value ; S3.2: For the global optimal solution Each intermediate node Construct neighborhood solutions sequentially. : Keep except the first The positions of all nodes except the intermediate node remain unchanged within the feasible region. A uniformly random integer is selected as the new intermediate node. The row index is used to replace the original intermediate node. Generate neighborhood solutions , The corresponding row number of the raster map; S3.3: For each neighborhood solution Calculate its fitness value and the original global optimal solution fitness value Compare; if Then the global optimal solution will be updated to ; Otherwise, retain the original solution and do not update it.

4. The path planning method for unmanned tractors based on the improved PSO fusion DWA optimization algorithm according to claim 3, characterized in that, The specific process of S4 is as follows: S4.1: Preliminary global path after local refinement in S3 , where each path point , , For path point row index, Index for path point columns, The number of path points. Starting from, Starting from the starting point and using the endpoint as the destination, traverse each pair of adjacent path points in ascending column order. and By calculating the row difference between adjacent path points, discrete path points are transformed into continuous paths, and continuous paths are gradually constructed. ; S4.2: For each pair of adjacent path points, based on the row difference Refine the path filling logic to ensure that continuous paths do not collide with obstacles: When ≤1, directly use Join ; When h>1, check the connectivity of the intermediate grid between the columns containing the two path points; set the intermediate grid to be connected by... Starting from the column Extending vertically to The resulting grid set ; If satisfied Then when At that time, in sequence These waypoints are added ,when Then, in sequence, These waypoints are added Finally add This creates a vertically continuous path; among which, Represents the raster map. Line number The status of the column, Indicates a passable free grid. Represents an obstacle grid; like In the list Find the closest one in the middle grid. Obstacle grid The coordinates satisfy The result is the smallest and In relation to Adjacent columns In China, with Starting from the direction, along row coordinates Directional search alternative points And the substitution point satisfies ; Specifically, first set the search step size. initial value If no alternative point is found in a single search, adjust the step size as follows: ； In the formula, For the first Next search step size, For the first Next search step size, This indicates the search step size, which is the number of grid cells searched vertically in each step. are positive integers and satisfy , This represents the number of rows in the raster map. If a passable alternative point is found, the original path point will be moved. Temporary update to alternative point Recalculate row differences Return to the filling logic processing until a collision-free continuous segment is generated; if the search step size is... If no alternative point is found after reducing the number of attempts to 1 and the maximum number of attempts has been reached, then skip the current segment and continue processing subsequent adjacent node pairs based on the existing nodes; S4.3: For the continuous paths generated above Smoothing and redundant node removal operations are performed here. , , For path point row index, Index for path point columns, Number of consecutive path points: Select three consecutive path points in sequence Construct vector sum vector Based on the principle of vector cross product, through calculation formula To determine whether three points are approximately collinear, where, , , respectively path points , , row index, , , respectively path points , , Column index; Specifically, if Then they can be determined to be approximately collinear, where, The threshold for determining collinearity; If the three points are approximately collinear, further examination is needed. and Is the grid area covered by the connecting line free of obstacles? The covered area is: ; in, Corresponding row index, Corresponding column index; If satisfied This indicates the midpoint. If a node is redundant, removing it will ensure the path remains continuous and collision-free. From continuous path Remove the middle point; otherwise, keep the middle point. ;in, Represents the raster map. Line number Column status, Indicates a passable free grid; S4.4: Repeat S4.3 until all iterations are complete. By considering all three combinable points, the optimized global path is ultimately obtained. .

5. The path planning method for unmanned tractors based on the improved PSO fusion DWA optimization algorithm according to claim 4, characterized in that, The specific process of S5 is as follows: S5.1: Retain the path point sequence of the global path optimized in S4.4; Based on the tractor kinematics model and velocity constraints, the tractor performs velocity sampling in the velocity space set to simulate an infinite number of feasible motion trajectories within a certain time interval; S5.2: Extract the path point with the smallest distance to the tractor's current position from the sequence of path points in the preserved global path, and use it as the reference anchor point for local guidance to ensure that the guidance direction matches the current movement stage of the tractor; Specifically, tractor acquisition via GPS Real-time pose at any moment ,in, For tractors in Planar coordinates at time, Corresponding tractor The heading angle at any given moment, in the global path The method determines the relationship between the tractor and the path point by calculating the Euclidean distance between the path point and the tractor's current position. The path point with the minimum distance is defined as... At the same time, with Starting from the global path Select the next two consecutive path points to form a local guided target sequence. ; S5.3: Evaluate the trajectory using the evaluation function of the DWA algorithm, and select the optimal trajectory from the infinite number of feasible motion trajectories generated in S5.1; wherein, the evaluation function of the DWA algorithm is shown in the following formula: ； In the formula, This represents the angular difference between the direction at the end of the sampling trajectory and the direction of the local target. This represents the distance between the end point of the sampling trajectory and the local target point; Indicates the current speed; , , These are the weight coefficients corresponding to each sub-function of the evaluation function; Represents the normalization process; S5.4: Preserve the velocity space corresponding to the optimal trajectory and perform velocity smoothing processing; S5.5: The on-board industrial control computer sends speed commands to the chassis controller, causing the tractor to move along the optimal trajectory obtained in S5.3; S5.6: The upper control unit determines whether the tractor has moved to the destination. If it has moved to the destination, the process ends; otherwise, S5.1 to S5.5 are executed repeatedly.

Images