Autonomous obstacle avoidance method of pseudo-ginseng harvester

Abstract

The invention provides an automatic obstacle avoidance method for a pseudo-ginseng harvester, and belongs to the field of agricultural machinery intelligence. The method comprises the following steps: S1, an RTK-GPS and a laser radar work to receive position information of a pseudo-ginseng harvester and environmental obstacle information; s2, using an improved artificial potential field method to complete path planning; s3, calculating a transverse deviation; s4, calculating course deviation; s5, calculating a theoretical rotation angle; and S6, adjusting the error between the theoretical rotation angle tau and the actual rotation angle tau'by continuously controlling the rotation of the steering mechanism, and finally realizing the theoretical rotation angle so as to realize obstacle avoidance. In order to avoid falling into a local minimum in the process of completing path planning by using an improved artificial potential field method, a simulated annealing algorithm is introduced. The invention aims to overcome the defects of an existing pseudo-ginseng harvester autonomous obstacle avoidance technology, and provides a method capable of achieving pseudo-ginseng harvester autonomous obstacle avoidance.

Description

Technical Field

[0001] The present invention relates to an autonomous obstacle avoidance method for a pseudo-ginseng harvester, belonging to the field of agricultural machinery intelligence. Background Art

[0002] Pseudo-ginseng is a local Chinese medicinal material in Yunnan Province, with the effects of dissipating stasis and stopping bleeding, detumescence and alleviating pain. As an important part of the development of the pseudo-ginseng industry, the application of pseudo-ginseng harvesters is becoming increasingly widespread. However, due to the complex geographical environment where pseudo-ginseng grows, there are obstacles such as electric poles, stones, and trenches, which affect the continuity of the operation of the pseudo-ginseng harvester, thereby reducing the harvesting efficiency. To improve the operation quality of the pseudo-ginseng harvester, reduce equipment loss and failures, lower labor costs, and promote the development of agricultural intelligence, it is crucial to achieve autonomous obstacle avoidance for the pseudo-ginseng harvester.

[0003] Using an autonomous obstacle avoidance algorithm to complete path planning is the top priority for achieving autonomous obstacle avoidance. Currently, the algorithms for autonomous obstacle avoidance mainly include the artificial potential field method, genetic algorithm, neural network algorithm, etc. Compared with other algorithms, the artificial potential field method has good environmental adaptability, fast calculation speed, good real-time performance, strong continuity, and short time consumption. It is suitable for pseudo-ginseng harvesters and can make optimal obstacle avoidance processing for the obstacles appearing in the operation environment. However, there are problems that when the distance to the target point is relatively close, the gravitational force is 0 and the repulsive force is not 0, and the harvester cannot reach the target point, and when the harvester reaches a certain position, the resultant force of the repulsive force is equal in magnitude and opposite in direction to the gravitational force, and the resultant force is zero, resulting in getting stuck in a local minimum and being unable to move. Summary of the Invention

[0004] Based on the above background, to improve the working efficiency and obstacle avoidance performance of the pseudo-ginseng harvester, the present invention provides an autonomous obstacle avoidance method for a pseudo-ginseng harvester, including the following steps:

[0005] S1: RTK-GPS and lidar work to receive the position information of the pseudo-ginseng harvester and the information of environmental obstacles;

[0006] S2: Use an improved artificial potential field method to complete path planning;

[0007] S3: Calculate the lateral deviation:

[0008]

[0009] where (X C , Y C ) is the centroid coordinate of the vehicle, is the coordinate of the n:1th path planning point, is the coordinate of the nth path planning point;

[0010] S4: Calculate the heading deviation:

[0011]

[0012] Where atan2(X, Y) is the function that returns the azimuth angle in the C language, and (X T , Y T ) are the coordinates of the target point, and (X C , Y C ) are the coordinates of the vehicle's centroid. H is measured by RTK-GPS;

[0013] S5: Substitute the lateral deviation and the heading deviation into the harvester controller of Panax notoginseng to calculate the theoretical rotation angle τ;

[0014] S6: Apply the incremental PID algorithm, and the formula is as follows:

[0015] Δτ = k p (err k - err k-1 ) + k i err k + k d (err k - 2err k-1 + err k-2 ), where k p is the proportionality coefficient, k i is the integral coefficient, and k d is the differential coefficient. The parameters are adjusted by the Z:N empirical formula method; by continuously controlling the rotation of the steering mechanism to adjust the error Δτ between the theoretical rotation angle τ and the actual rotation angle τ', the theoretical rotation angle of the steering is finally output to achieve obstacle avoidance, where τ' is measured by the motor encoder of the harvester of Panax notoginseng;

[0016] As a further improvement of the present invention, the improved artificial potential field method is used to complete the path planning, and the specific steps are as follows:

[0017] S301: Environment modeling, defining environmental parameters, including the target point, the initial position, and the obstacle information;

[0018] S302: Construct the gravitational field function:

[0019]

[0020] Where β is the gravitational gain coefficient, q is the current position of the harvester of Panax notoginseng, q g is the target point position, and ρ(q, q g ) is the distance between the harvester and the target point. F att (q) The attractive force is the negative gradient of this potential field function:

[0021] F att (q) = -βρ(q, q g );

[0022] S303: Construct an improved repulsive force field function:

[0023]

[0024] where α is the repulsive force field gain coefficient, q0 is the position of the obstacle, ρ(q, q0) is the distance between the harvester and the obstacle, and ρ0 is the obstacle influence radius. F rep (q) The repulsive force is the negative gradient of this potential field function:

[0025]

[0026] where

[0027] After improving the repulsive force field function, compared with the traditional artificial potential field method, it can avoid the problem that when the distance to the target point is relatively close, the gravitational force is 0, the repulsive force is not 0, and the harvester cannot reach the target point;

[0028] S304: Calculate the resultant force and potential energy:

[0029] U total (q) = U rep(q) +U att (q); F total (q) = F att (q)+F rep (q).

[0030] As a further improvement of the present invention, to avoid getting stuck in a local minimum during the process of using the improved artificial potential field method to complete path planning in step S3, a simulated annealing algorithm is introduced. The specific steps are as follows:

[0031] S1: Set the initial state x current (i.e., the local minimum point), the initial temperature T o , and the termination temperature T f ;

[0032] S2: Generate a random point at the current position, x new = x current +random_step; Calculate the potential energy U(x new ) of the new position and the potential energy U(x current ) of the initial state;

[0033] S3: Calculate ΔU = U(x new ) - U(x current );

[0034] S4: If ΔU < 0, the new position is accepted, x current = x new , if ΔU > 0, then set x with probability pcurrent = x new , Let \(K\) be the Boltzmann constant and set the random probability \(a\). When \(p > a\), set \(x\) current = x new ; when \(p < a\), the random point setting fails and the random point is reset;

[0035] S5: Determine whether the pseudo-ginseng harvester has escaped from the local minimum point by calculating the distance from the current position to the local minimum. If the distance from the current position to the local minimum is greater than a preset threshold, it is determined that the pseudo-ginseng harvester has escaped from the local minimum. If not, reduce the temperature \(T\) and return to step S2 to repeat the above process until the local minimum is escaped.

[0036] As a further improvement of the present invention, after introducing the simulated annealing algorithm, due to the setting of the random point, the resultant force on the pseudo-ginseng harvester at this time is:

[0037] \(F\) total (q) = \(F\) att (q) + \(F\) rep (q) + \(F\) esc (q);

[0038] where \(F\) esc (q) is the random escape force: \(F\) esc (q) = \(\gamma\cdot rand(\theta,r)\) \(\theta\in[0,2\pi)\); \(r\in[0,r\) max ; \(\gamma\) is the escape force coefficient, \(rand(\theta,r)\) is a random vector, \(r\) max \(\approx0.1\times F\) max .

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

[0040] ① The present invention provides a method for the pseudo-ginseng harvester to avoid obstacles autonomously, which can realize the autonomous obstacle avoidance of the pseudo-ginseng harvester and improve the harvesting efficiency;

[0041] ② The present invention overcomes the problems of unreachable target points and getting stuck in local minima in the traditional artificial potential field method, and improves the obstacle avoidance performance. BRIEF DESCRIPTION OF THE DRAWINGS

[0042] Figure 1 is the overall flowchart of the present invention;

[0043] Figure 2 is the flowchart of introducing the simulated annealing method in the path planning using the improved artificial potential field method of the present invention;

[0044] Figure 3 is the flowchart of the simulated annealing method; DETAILED DESCRIPTION OF THE INVENTION

[0045] The present invention will be further described below in conjunction with the accompanying drawings and specific embodiments:

[0046] As Figure 1 shown, this embodiment provides an obstacle avoidance method for a Panax notoginseng harvesting autonomous machine based on an improved artificial potential field method, including the following steps:

[0047] S1: RTK-GPS ensures high-precision navigation and positioning, and the lidar sensor receives information on detected environmental obstacles;

[0048] S2: If the obstacle is within the influence range, use the improved artificial potential field method to complete path planning;

[0049] S3: Calculate the lateral deviation:

[0050]

[0051] where (X C , Y C ) is the centroid coordinate of the vehicle, is the coordinate of the n:1th path planning point, is the coordinate of the nth path planning point;

[0052] S4: Calculate the heading deviation:

[0053]

[0054] where atan2(X, Y) is the function in C language that returns the azimuth angle, (X T , Y T ) is the coordinate of the target point, (X C , Y C ) is the centroid coordinate of the vehicle, and H is measured by RTK-GPS;

[0055] S5: Substitute the lateral deviation and the heading deviation into the Panax notoginseng harvester controller. There is a path tracking algorithm in the Panax notoginseng harvester controller to calculate the theoretical steering angle τ;

[0056] S6: Apply the incremental PID algorithm. By using the STM32F407 single-chip microcomputer, continuously control the steering mechanism to rotate to adjust the error Δτ between the theoretical steering angle τ and the actual steering angle τ', and finally output the theoretical steering angle for obstacle avoidance. The actual steering angle τ' is measured by the encoder of the Panax notoginseng harvester. The encoder is installed parallel to the driving wheel of the Panax notoginseng harvester and is driven by a synchronous belt;

[0057] In step S3, the specific steps to complete path planning using the improved artificial potential field method are as follows:

[0058] S301: Environment modeling, define environmental parameters, including the target point, the initial position, and obstacle information;

[0059] S302: Construct the gravitational field function:

[0060]

[0061] where β is the gravitational gain coefficient, β = 10, q is the current position of the Sanqi harvester, q g is the target point position, ρ(q, q g ) is the distance between the harvester and the target point. F att (q) The attractive force is the negative gradient of this potential field function:

[0062] F att (q) = -βρ(q, q g );

[0063] S303: Construct the improved repulsive field function:

[0064]

[0065] where α is the repulsive field gain coefficient, α = 30, q0 is the obstacle position, ρ(q, q0) is the distance between the harvester and the obstacle, ρ0 is the obstacle influence radius, ρ0 = 2m, n = 2. F rep (q) The repulsive force is the negative gradient of this potential field function:

[0066]

[0067] where

[0068] After improving the repulsive field function, compared with the traditional artificial potential field method, it can avoid the problem that when the harvester is close to the target point, the gravitational force is 0 and the repulsive force is not 0, and the harvester cannot reach the target point;

[0069] S304: Calculate the resultant force and potential energy:

[0070] U total (q) = U rep(q) +U att (q); F total (q) = F att (q)+F rep (q);

[0071] If the situation of getting stuck in a local minimum occurs during the process of using the improved artificial potential field method for obstacle avoidance, then the simulated annealing algorithm is cited. As Figure 2 shown, the specific steps of the simulated annealing method are as follows:

[0072] S1: Set the initial state x current (i.e., the local minimum point), the initial temperature T o , To = 10, termination temperature T f , T f = 0.01;

[0073] S2: Generate a random point at the current position, x new = x current + random_step; Calculate the potential energy U(x new ) and the potential energy U(x current ) of the initial state;

[0074] S3: Calculate ΔU = U(x new ) - U(x current );

[0075] S4: If ΔU < 0, the new position is accepted, x current = x new , if ΔU > 0, then set x with probability p current = x new , Set the random probability a, a = 0.8. When p > a, set x current = x new ; When p < a, the random point setting fails and a new random point is set again;

[0076] S5: Determine whether the pseudo-ginseng harvester has escaped from the local minimum point by calculating the distance from the current position to the local minimum. If the distance from the current position to the local minimum is greater than the preset threshold, it is determined that the pseudo-ginseng harvester has escaped from the local minimum. If not, lower the temperature and return to step S2, repeating the above process until escaping from the local minimum, as Figure 3 shown.

Claims

1. An autonomous obstacle avoidance method for a pseudo-ginseng harvester, characterized in that Including the following steps: S1: RTK-GPS and lidar work to receive the position information of the pseudo-ginseng harvester and the information of environmental obstacles; S2: Use the improved artificial potential field method to complete path planning; S3: Calculate the lateral deviation: where (X C , Y C ) are the vehicle centroid coordinates, are the coordinates of the n:1th path planning point, are the coordinates of the nth path planning point; S4: Calculate the heading deviation: where atan2(X, Y) is the function in C language that returns the azimuth angle, (X T , Y T ) are the coordinates of the target point, (X C , Y C ) are the coordinates of the vehicle centroid, and H is measured by RTK-GPS; S5: Substitute the lateral deviation and the heading deviation into the controller of the pseudo-ginseng harvester to calculate the theoretical steering angle τ; S6: Apply the incremental PID algorithm, and the formula is as follows: Δτ = k p (err k - err k-1 ) + k i err k + k d (err k - 2err k-1 + err k-2 ), where k p is the proportionality coefficient, k i is the integral coefficient, k d is the differential coefficient. The parameters are adjusted by the Z:N empirical formula method; by continuously controlling the rotation of the steering mechanism to adjust the error Δτ between the theoretical angle τ and the actual angle τ', the theoretical angle of steering is finally output to achieve obstacle avoidance, where τ' is measured by the motor encoder of the three-seven harvester; In step S3, the specific steps to complete path planning using the improved artificial potential field method are as follows: S301: Environment modeling, define environmental parameters, including the target point, the initial position, and the obstacle information; S302: Construct the gravitational field function: where β is the gravitational gain coefficient, q is the current position of the pseudo-ginseng harvester, q g is the target point position, and ρ(q, q g ) is the distance between the harvester and the target point. The attractive force F att (q) is the negative gradient of this potential field function: F att (q) = -βρ(q, q g ); S303: Construct the improved repulsive field function: where α is the repulsive field gain coefficient, q0 is the position of the obstacle, ρ(q, q0) is the distance between the harvester and the obstacle, ρ0 is the influence radius of the obstacle, and F req (q) The repulsive force is the negative gradient of this potential field function: Among them S304: Calculate the resultant force and potential energy: U total U(q) = U rep(q) + U att (q); F total F(q) = F att (q) + F rep (q).

2. The autonomous obstacle avoidance method of a pseudo-ginseng harvester according to claim 1, characterized in that: In step S3, the improved artificial potential field method is used to complete path planning. To avoid falling into local minima during this process, the simulated annealing algorithm is introduced. The specific steps are as follows: S1: Set the initial state x current , the initial temperature T o , and the termination temperature T f ; S2: Generate a random point at the current position, x new = x current + random_step; Calculate the potential energy U(x new ) and the potential energy U(x current ) of the initial state; S3: Calculate ΔU = U(x new ) - U(x current ); S4: If ΔU < 0, the new position is accepted, x current = x new , if ΔU > 0, then set x with probability p current = x new , K is the Boltzmann constant, set a random probability a, when p > a, set x current = x new ; when p < a, the random point setting fails, reset the random point; S5: Determine whether the pseudo-ginseng harvester has escaped from the local minimum point by calculating the distance from the current position to the local minimum. If the distance from the current position to the local minimum is greater than the preset threshold, it is determined that the pseudo-ginseng harvester has escaped from the local minimum. If not, reduce the temperature T and return to step S2, repeating the above process until the local minimum is escaped.

3. The autonomous obstacle avoidance method of a pseudo-ginseng harvester according to claim 2, wherein: In step S2, due to the setting of random points, the resultant force on the pseudo-ginseng harvester at this time is: F total (q) = F att (q) + F rep (q) + F esc (q), where F esc (q) is the random escape force: F esc (q) = γ·rand(θ, r) θ ∈ [0, 2π); r ∈ [0, r max );; γ is the escape force coefficient, rand(θ, r) is a random vector, r max ≈ 0.1×F max .

Images