Rapid path deviation correction algorithm based on intelligent robot

Abstract

The invention provides a fast path correction algorithm based on an intelligent robot, which is characterized in that a genetic algorithm (GA) and a particle swarm optimization (PSO) algorithm are fused to improve a Steiner (Stanley) method, so that a GAPSO-Stanley algorithm is formed. When it is detected that the robot deviates, points are randomly inserted within the turning radius range of the robot to serve as an initial point of a GAPSO algorithm, the Euclidean distance between the initial point of the algorithm and an original path point serves as an evaluation standard, and the robot returns to an original path in time through rapid convergence iteration. Finally, according to the actual measurement result, the algorithm provided by the invention greatly improves the tracking precision of the robot and reduces the energy consumption of the robot.

Description

Technical Field

[0001] The present invention relates to the technical field of intelligent agricultural machinery, and in particular to a fast path deviation correction algorithm based on an intelligent robot. Background Art

[0002] In today's era of rapid technological development, agricultural robots such as harvesting robots, pesticide spraying robots and irrigation robots have changed the traditional agricultural production model, alleviated the current situation of labor shortage, and promoted the development of modern agriculture and forestry. At this stage, agricultural robots are also transforming towards small, low-consumption, intelligent and automated, constantly changing and influencing the development of modern agriculture. The research and development of agricultural robots is of great significance to the long-term development of China's future agriculture. The intelligence of agricultural robots has made automatic navigation technology one of the main hotspots in the field of agricultural research. The key and difficulty of navigation control technology is path tracking. Therefore, the research on agricultural robot path tracking algorithm is urgent.

[0003] Path tracking means that after the path planning algorithm has planned an optimal path from the starting point to the target point, the robot follows the optimal path. Common path tracking algorithms include Stanley method, Pure pursuit algorithm, and fuzzy predictive control (MPC) algorithm. Among them, Pure Pursuit has good robustness in large tracking errors and discontinuous path scenarios. The key to its control lies in the determination of the optimal forward preview distance. Increasing the forward preview distance will improve the stability of vehicle control, but it will also lead to reduced path tracking performance and increased steady-state error. In the process of vehicle modeling, model mismatch is inevitable due to vehicle parameter errors or time-varying, unmodeled dynamics and external interference. The MPC control law itself does not have good robustness to model mismatch, and there are problems of infeasible solutions and high computational complexity. The Stanley method is the path tracking method used by Stanford University in the DARPA challenge. Compared with the Pure Pursuit algorithm, the Stanley front wheel feedback algorithm also takes into account the yaw angle deviation, so in most scenarios, the tracking performance is better. Therefore, the present invention adopts Stanley method to perform path tracking, thereby completing the robot's autonomous operation task. However, in actual tests, it is found that Stanley algorithm is prone to large path deviation and low efficiency in complex environments such as farmland, lawn, orchard, etc. Summary of the invention

[0004] 1. Technical issues to be solved

[0005] In view of the shortcomings of the prior art, the present invention adopts the genetic particle swarm optimization (GAPSO) fusion algorithm to optimize the Stanley method, so that when the path deviation occurs in the intelligent robot, it can not only quickly converge to the optimal position but also avoid the local optimum, which greatly improves the problem of low tracking efficiency caused by oversteering due to the lack of forward preview setting in the traditional Stanley method.

[0006] (II) Technical solution

[0007] To achieve the above object, the present invention provides the following technical solution: a fast path correction algorithm based on an intelligent robot, which is composed of a GAPSO module, an adaptive weight module and a Stanley algorithm, and includes the following steps:

[0008] Step 1: Calculate the error Δ between the robot's offset path and the original path in the system. If the error is greater than the specified value, proceed to step 2), otherwise continue the operation;

[0009] Step 2: Randomly insert points into the robot's turning radius, and each point corresponds to a particle;

[0010] Step 3: Use the formula as a criterion to evaluate the GAPSO algorithm and find the optimal solution for particles within the robot’s turning radius;

[0011] Step 4: According to the given hybridization probability, determine the size of the hybridization pool, randomly select two particles from the hybridization pool for hybridization, iteratively update the formula to update the speed and position of the particles, and calculate and update the fitness value, global optimality, and individual optimality of the new particles after hybridization;

[0012] Step 5: According to the given mutation probability, determine the size of the mutation pool, randomly select particles from the mutation pool and perform mutation operations according to the formula, and calculate and update the fitness value, global optimality, and individual optimality of the new particles generated after mutation;

[0013] Step 6: Determine whether the number of iterations is met. If not, return to step 4 to continue iterating. Otherwise, the iteration ends.

[0014] Step 7: Use the optimal particle selected by the algorithm as the target point of the Stanley method, and rotate the robot toward the target point;

[0015] Step 8: The robot drives to the target point and then operates according to the global planning path. The derivation uses the following algorithms: GAPSO algorithm and Stanley method.

[0016] Wherein, the robot is: an intelligent robot.

[0017] Wherein, the experimental scenario: any experimental scenario.

[0018] (III) Beneficial effects

[0019] Compared with the prior art, the present invention provides a fast path correction algorithm based on an intelligent robot, which has the following beneficial effects: using the Stanley method based on GAPSO optimization proposed in the present invention can shorten the robot's moving distance, reduce the time for the robot to approach the original path, and realize fast correction of path deviation that occurs in actual operation of the intelligent robot, accurately follow the planned path, and greatly improve the real-time and robustness of the system. BRIEF DESCRIPTION OF THE DRAWINGS

[0020] Figure 1 It is the Stanley method model.

[0021] Figure 2 It is the flow chart of the algorithm of the present invention.

[0022] Figure 3 It is an overall comparison diagram of the algorithm path of the present invention and the real path.

[0023] Figure 4 This is a comparison chart of trajectory errors between the algorithm of the present invention and the PSO-Pure pursuit algorithm. DETAILED DESCRIPTION

[0024] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0025] See also Figure 1-4 , a fast path correction algorithm based on intelligent robots, is composed of a GAPSO module, an adaptive weight module and a Stanley algorithm, and includes the following steps:

[0026] Step 1: The error Δ between the robot and the actual prescribed route is defined as (p ix , p iy ) represents the offset position of the robot; (x ireal ,y ireal ) represents the position of the robot on the original path; Δ represents the Euclidean distance between the robot's offset position and the original path position;

[0027] Step 2: Initialize the particle swarm (speed and position), inertia factor, acceleration constant, maximum iteration number and population size, mutation and hybridization probability, etc.;

[0028] Step 3: When the robot is detected to be offset, multiple sets of new tracking points are inserted within the turning radius according to the robot's head direction as the initial points of the GAPSO-Stanley algorithm;

[0029] Step 4: The Euclidean distance between the initial point and the original path point is used as the evaluation criterion of the GAPSO algorithm;

[0030] Step 5: The screened particles are formed into a hybridization pool, and two independent particles are randomly selected from the pool for hybridization until all particles in the hybridization pool have completed hybridization, thereby generating the position and speed update formula of the new individual; the size of the mutation pool is determined according to the mutation probability, and particles are randomly selected from the mutation pool for Gaussian mutation until all particles in the mutation pool have completed mutation; the weight is adaptively adjusted according to the number of iterations and the weight value range, and the particle position and speed are updated on this basis;

[0031] Step 6: When the robot's route deviates due to external factors and is greater than the specified error Δ during its movement, the system will automatically use the GAPSO algorithm to correct the path and quickly achieve path correction.

[0032] In summary, when the agricultural machinery is detected to be offset, the fast path correction algorithm based on the intelligent robot inserts multiple sets of new tracking points within the turning radius of the robot according to the direction of the robot's head as the initial points of the GAPSO-Stanley algorithm. The Euclidean distance between the new tracking points and the original path points is used as the evaluation criterion of the GAPSO algorithm, so that the robot gradually approaches the original path, which can greatly improve the real-time and robustness of the system and reduce the energy consumption of the robot.

[0033] It should be noted that, in this article, relational terms such as first and second, etc. are only used to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Moreover, the terms "include", "comprise" or any other variants thereof are intended to cover non-exclusive inclusion, so that a process, method, article or device including a series of elements includes not only those elements, but also other elements not explicitly listed, or also includes elements inherent to such process, method, article or device.

[0034] Although embodiments of the present invention have been shown and described, it will be appreciated by those skilled in the art that various changes, modifications, substitutions and variations may be made to the embodiments without departing from the principles and spirit of the present invention, and that the scope of the present invention is defined by the appended claims and their equivalents.

Claims

1. A fast path correction algorithm based on an intelligent robot is composed of a GAPSO module, an adaptive weight module and a Stanley algorithm, and includes the following steps: Step 1: Calculate the error Δ between the robot's offset path and the original path in the system. If the error is greater than the specified value, proceed to step 2), otherwise continue the operation; Step 2: Randomly insert points into the robot's turning radius, and each point corresponds to a particle; Step 3: Use the formula as a criterion to evaluate the GAPSO algorithm and find the optimal solution for particles within the robot’s turning radius; Step 4: According to the given hybridization probability, determine the size of the hybridization pool, randomly select two particles from the hybridization pool for hybridization, iteratively update the formula to update the speed and position of the particles, and calculate and update the fitness value, global optimality, and individual optimality of the new particles after hybridization; Step 5: According to the given mutation probability, determine the size of the mutation pool, randomly select particles from the mutation pool and perform mutation operations according to the formula, and calculate and update the fitness value, global optimality, and individual optimality of the new particles generated after mutation; Step 6: Determine whether the number of iterations is met. If not, return to step 4 to continue iterating. Otherwise, the iteration ends. Step 7: Use the optimal particle selected by the algorithm as the target point of the Stanley method, and rotate the robot toward the target point; Step 8: The robot drives to the target point and then operates according to the global planning path.

2. According to the fast path correction algorithm based on intelligent robots according to claim 1, it is characterized in that: The applied algorithm is an algorithm for achieving optimal control in robot path tracking.

3. The fast path correction algorithm based on intelligent robots according to claim 1 is characterized in that: The experimental scenarios are divided into: arbitrary experimental scenarios.

Images