Unmanned tractor path planning and roadblock avoiding method based on ant colony algorithm and JPS + algorithm

Abstract

The invention provides an unmanned tractor path planning and roadblock avoiding method based on an ant colony algorithm and a JPS + algorithm, and the method comprises the steps: firstly constructing a field operation region environment grid map which comprises the shape and size of a farmland boundary, the starting position and ending position of the operation of a tractor, and the length, width, height and the like of an obstacle in the farmland; secondly, operating an ant colony algorithm, and solving the overall situation by taking the energy consumption of the tractor as a target function to obtain a global optimal path from a starting point to an ending point; and finally, running a JPS + algorithm, planning a local path, and planning an optimal path with global and local obstacle avoidance for the tractor. According to the method, the ant colony algorithm and the JPS + algorithm are combined, the planning efficiency and precision of the local path are improved, the preprocessing cost of the grid map is reduced, the path planning steps are simplified by screening obstacles, the path planning speed is increased, and the working efficiency of the tractor is improved.

Description

Technical Field

[0001] The present invention relates to the technical field of autonomous driving technology for intelligent agricultural machinery, and particularly to a path planning and obstacle avoidance method for driverless tractors based on the ant colony algorithm and the JPS+ algorithm. Background Art

[0002] Promoting electric vehicles can reduce waste gas emissions, which is beneficial to purifying the urban environment and creating a low-carbon society. Especially in the agricultural field, the promotion of electric equipment is even more important. In the field of electric driverless tractors, the path planning of tractors is a key technology, which can ensure that tractors drive efficiently in complex farmland environments, avoid obstacles, increase the coverage area of the tractor's working area, and also achieve the effect of energy conservation.

[0003] Currently, a variety of path planning methods have been proposed for tractor operations. For example, sensors are used to scan the working environment of the tractor, and a grid map of the tractor's working environment is established. The position coordinates of the starting point, the ending point, and each obstacle are marked on the grid map. An algorithm is used to process the grid map to find the optimal path between the starting point and the ending point. Representative algorithms include the ant colony algorithm, the A* algorithm, the whale algorithm, the sparrow algorithm, the JPS+ algorithm, etc. The accuracy of generating a path using the ant colony algorithm alone is limited, and the ant colony algorithm has insufficient local path planning ability. Using the JPS+ algorithm for global path planning will result in too high preprocessing costs and large memory occupancy. And the above algorithms do not judge whether the obstacles will affect the passage of the tractor according to the passability of the tractor and the size of the obstacles when performing path planning, which easily causes the tractor to perform unnecessary obstacle avoidance and affects the working efficiency of the tractor. Summary of the Invention

[0004] Aiming at the above problems, the present invention provides a path planning and obstacle avoidance method for driverless tractors based on the ant colony algorithm and the JPS+ algorithm, which can improve the accuracy of path planning while reducing the preprocessing cost of the grid map, and screen the obstacles to improve the path planning speed and the working efficiency of the tractor.

[0005] The present invention is specifically implemented through the following technical solutions. A path planning and obstacle avoidance method for driverless tractors based on the ant colony algorithm and the JPS+ algorithm proposed according to the present invention includes the following steps:

[0006] (1). Construct an environmental grid map of the field operation area. The grid map includes the shape and size of the farmland boundary, the starting position and the ending position of the tractor operation, and the length, width, and height of the obstacles in the farmland.

[0007] (2) Run the ant colony algorithm, taking the energy consumption of the tractor as the objective function, and solve globally to obtain a globally optimal path from the starting position to the ending position of the tractor's operation.

[0008] (3) Run the JPS+ algorithm to plan the local path, and combine step (1) to plan an optimal path for the tractor that combines global and obstacle avoidance.

[0009] Further, the specific steps of constructing the environmental grid map of the field operation area in step (1) include: Install sensors on the unmanned aerial vehicle (UAV) to scan the working plot (field operation area), construct the environmental grid map of the field operation area, use the sensors on the UAV to scan each obstacle in the field operation area one by one, and set the coordinates of each obstacle and the length, width, and height parameters of the obstacle in the grid map; Set the boundary of the field operation area in the grid map and set the coordinates of the starting point and the ending point of the tractor's operation; Determine the width of the headland according to the turning radius R of the tractor, the headland turning method, and the actual operation width W of the tractor in the grid map, and then evenly divide the field operation area (working area) into several operation rows according to the actual operation width of the tractor.

[0010] Further, the said sensor can adopt a 3D lidar.

[0011] Further, the specific steps of using the ant colony algorithm to obtain the globally optimal path of the tractor in step (2) include: Initialize the path, number each operation row, and set the parameters of the ant colony algorithm, the number of ant colonies, the importance factor α of pheromone, the importance factor β of the heuristic function, the pheromone constant coefficient Q, and the maximum number of iterations n.

[0012] Set the electric energy consumption function of the electric tractor. The electric energy consumption function of the tractor is:

[0013]

[0014] In formula (1), μ is the ground friction coefficient; G is the gravity of the electric tractor itself (unit: N); α is the slope angle of the road surface where the electric tractor travels; S is the distance between the starting point and the ending point of the tractor's operation (unit: m); n is the motor speed (unit: r / min); R is the radius of the tractor wheel (unit: m); U is the battery voltage (unit: V); i g is the transmission ratio; i0 is the main reducer ratio; η T is the transmission efficiency of the driveline.

[0015] Set the path cost function of the ant colony:

[0016]

[0017] In formula (2), D(I,j) is the path cost between the position i of the tractor and the position j of the task point. (x i ,y i ) are the position coordinates of the tractor, and (x j ,y j ) are the position coordinates of the task point.

[0018] Each ant selects the next task point according to the transition probability P. The transition probability function is:

[0019]

[0020] In formula (3), Tau is the pheromone concentration between node s i and node s j . Eta is the heuristic function, α is the importance factor of pheromone, β is the importance factor of the heuristic function, P is the transition probability, and D is the path cost.

[0021] Through the transition probability, the ant colony starts to iterate. Each ant leaves pheromone on the traveled operation rows. The longer the operation row distance, the less pheromone. Using the energy consumption of the tractor as the evaluation function, calculate the energy consumption of each ant passing through the path. Determine whether the number of iterations is greater than the set maximum number of iterations; if it is greater than the set maximum number of iterations, output the global optimal route; if it is less than the number of iterations, continue to find the optimal path.

[0022] Further, the specific steps of using the JPS+ algorithm for local path planning in step (3) include: judging whether the tractor can directly pass through the obstacles in the grid map according to the passability of the tractor and the information of the obstacles; if it is judged that the tractor can pass through the obstacle, delete the obstacle in the grid map. If it is judged that the tractor cannot pass through the obstacle, retain the obstacle. Perform local path planning, preprocess the grid map of the field operation area according to the JPS+ algorithm, and calculate all the jump points in the grid map; judge whether the current node is a jump point. If it is, mark the jump point in the grid map; if not, calculate the distances from the current node in each direction to the jump points; plan the local obstacle avoidance optimal path according to the jump points; smoothly connect the local obstacle avoidance optimal path with the global optimal path to construct a complete optimal path with both global and obstacle avoidance capabilities.

[0023] The process of using the aforementioned path planning and roadblock avoidance method for driverless tractors based on the ant colony algorithm and the JPS+ algorithm for obstacle avoidance is as follows:

[0024] The on-vehicle sensor (3D lidar) is used to scan the surrounding environment to determine whether there are obstacles around. If there are no obstacles, the tractor runs normally. If there are obstacles, the distance from the tractor to the obstacle is measured; the obstacles are classified according to the distance between the tractor and the obstacle. If the distance between the tractor and the obstacle is greater than R + 0.5W (R is the turning radius of the tractor, and W is the working width), this obstacle is determined to be of ordinary threat level. If the distance between the tractor and the obstacle is not greater than R + 0.5W (R is the turning radius of the tractor, and W is the working width), this obstacle is determined to be of severe threat level. When the on-vehicle sensor detects an obstacle and determines it to be of severe threat level, the tractor takes active avoidance measures and enters the emergency avoidance state. The on-vehicle sensor immediately sends a signal to the VCU (vehicle control unit), and the VUC sends a signal to the braking system, and the tractor brakes emergently; then the VCU sends a signal to the steering system to control the tractor to drive away from the obstacle. When the tractor has passed the obstacle and the distance between the tractor and the obstacle is greater than R + 0.5W (R is the turning radius of the tractor, and W is the working width), it is determined that the tractor has completed obstacle avoidance. After completing obstacle avoidance, the VCU sends a signal to the steering system to control the tractor to return to the path of the original driving direction. When the sensor detects an obstacle and determines it to be of ordinary threat level, the VCU sends a signal to the braking system to control the vehicle to stop; after stopping, the VCU controls the tractor to sound the horn for the first time. After 30 s of the first horn sounding, if the sensor detects a change in the distance between the vehicle and the obstacle, it is determined that this obstacle is a dynamic obstacle, and the tractor sounds the horn again. If the obstacle in front disappears, the tractor moves forward along the original route; if it does not disappear, it continues to stop and wait until the obstacle in front disappears and then moves forward along the original route; after 30 s of the first horn sounding, if the sensor detects that the distance between the tractor and the obstacle has not changed, it is determined that this obstacle is a static obstacle, and obstacle avoidance is carried out according to the locally optimal path planned by the JPS+ algorithm.

[0025] Compared with the prior art, the advantages of the present invention are as follows: The ant colony algorithm is combined with the JPS+ algorithm. The ant colony algorithm is used for the global path planning, and the JPS+ algorithm is used for the local path planning. The present invention also optimizes the JPS+ algorithm. After considering the passability of the tractor and the size of the obstacle, the obstacles are further screened, some unnecessary obstacle points are deleted, and then the threat levels of the obstacles are classified, and different processing methods are proposed for obstacles with different threat levels. By combining the ant colony algorithm and the JPS+ algorithm, the present invention not only improves the efficiency and accuracy of the global path planning and the local path planning when encountering obstacles, but also reduces the preprocessing cost of the grid map. By screening the obstacles, the path planning steps are simplified, the path planning speed is increased, and the working efficiency of the tractor is improved. Brief Description of the Drawings

[0026] Figure 1 It is a flow chart of the global path planning of the tractor based on the ant colony algorithm.

[0027] Figure 2 It is a flow chart of the local path planning of the tractor based on the JPS+ algorithm

[0028] Figure 3 It is a flow chart of the tractor obstacle avoidance. Specific implementation manners

[0029] To make the objectives, technical solutions and advantages of the embodiments of the present invention clearer, the technical solutions of the present invention will be clearly and completely described below in conjunction with specific embodiments. Obviously, the described embodiments are some, rather than all, of the embodiments of the present invention. All other embodiments obtained by those of ordinary skill in the art based on the embodiments of the present invention without creative efforts shall fall within the protection scope of the present invention.

[0030] A path planning and obstacle avoidance method for an unmanned tractor based on the ant colony algorithm and the JPS+ algorithm proposed by the present invention includes the following steps:

[0031] (1). Construct an environmental grid map of the field operation area, which includes the shape and size of the farmland boundary, the starting position and the ending position of the tractor operation, the length, width and height of the obstacles in the farmland;

[0032] (2). Run the ant colony algorithm, taking the tractor energy consumption as the objective function, solve globally, and obtain a global optimal path from the starting position to the ending position of the tractor operation;

[0033] (3). Run the JPS+ algorithm to plan the local path, and combine step (1) to plan an optimal path for the tractor that combines global and obstacle avoidance.

[0034] As Figure 1 shown, the specific steps of the global path planning include:

[0035] Install a sensor on the unmanned aerial vehicle to scan the working plot, construct an environmental grid map of the field operation area, use the sensor on the unmanned aerial vehicle to scan each obstacle in the field operation area one by one, set the coordinates of each obstacle and the length, width and height parameters of the obstacle in the grid map; set the boundary of the field operation area in the grid map and set the starting point coordinates and ending point coordinates of the tractor operation; determine the headland width in the grid map according to the turning radius R of the tractor, the headland turning method and the actual operation width W of the tractor, and then evenly divide the field operation area (working area) into several operation rows according to the actual operation width of the tractor.

[0036] The sensor described above can adopt a 3D lidar.

[0037] Initialize the path, number each job line, and set the parameters of the ant colony algorithm, the number of ant colonies, the importance factor α of pheromone, the importance factor β of the heuristic function, the pheromone constant coefficient Q, and the maximum number of iterations n.

[0038] Set the power consumption function of the electric tractor. The power consumption function of the tractor is:

[0039]

[0040] In formula (1), μ is the ground friction coefficient; G is the gravity of the electric tractor itself (unit: N); α is the slope angle of the road surface where the electric tractor travels; S is the distance between the starting point and the ending point of the tractor's work (unit: m); n is the motor speed (unit: r / min); R is the radius of the tractor wheel (unit: m); U is the battery voltage (unit: V); i g is the transmission ratio; i0 is the main reducer ratio; η T is the transmission efficiency of the driveline.

[0041] Set the path cost function of the ant colony:

[0042]

[0043] In formula (2), D(I,j) is the path cost between the position i of the tractor and the position j of the task point, (x i , y i ) are the position coordinates of the tractor, and (x j , y j ) are the position coordinates of the task point.

[0044] Each ant selects the next task point according to the transition probability P. The transition probability function is:

[0045]

[0046] In formula (3), Tau is the pheromone concentration between node s i and node s j , Eta is the heuristic function, α is the importance factor of pheromone, β is the importance factor of the heuristic function, P is the transition probability, and D is the path cost.

[0047] Through the transition probability, the ant colony starts to iterate. Each ant leaves pheromone on the job lines it has walked. The longer the job line distance, the less pheromone. Using the energy consumption of the tractor as the evaluation function, calculate the energy consumption of each ant passing through the path. Determine whether the number of iterations is greater than the set maximum number of iterations; if it is greater than the set maximum number of iterations, output the global optimal route; if it is less than the number of iterations, continue to find the optimal path.

[0048] As Figure 2 shown, the local path planning steps are as follows:

[0049] Judge whether the tractor can directly pass through the obstacles in the grid map based on the passability of the tractor and the information of the obstacles; if it is judged that the tractor can pass through the obstacle, delete the obstacle in the grid map. If it is judged that the tractor cannot pass through the obstacle, keep the obstacle. Perform local path planning, preprocess the grid map of the field operation area according to the JPS+ algorithm, and calculate all the jump points in the grid map; judge whether the current node is a jump point, if so, mark the jump point in the grid map; if not, calculate the distances from the current node in each direction to the jump points; plan the optimal local obstacle avoidance path according to the jump points; smoothly connect the optimal local obstacle avoidance path with the global optimal path to construct a complete optimal path with both global and obstacle avoidance capabilities.

[0050] As Figure 3 shown, the process of obstacle avoidance using the aforementioned path planning and roadblock avoidance method for driverless tractors based on the ant colony algorithm and JPS+ algorithm is as follows:

[0051] Use an on-vehicle sensor (3D lidar) to scan the surrounding environment, and determine whether there are obstacles around. If there are no obstacles, the tractor travels normally. If there are obstacles, measure the distance between the tractor and the obstacles; classify the obstacles according to the distance between the tractor and the obstacles. If the distance between the tractor and the obstacle is greater than R + 0.5W (R is the turning radius of the tractor, and W is the working width), determine that this obstacle is of ordinary threat level. If the distance between the tractor and the obstacle is not greater than R + 0.5W (R is the turning radius of the tractor, and W is the working width), determine that this obstacle is of serious threat level. When the on-vehicle sensor detects an obstacle and determines it to be of serious threat level, the tractor takes active avoidance measures and enters the emergency avoidance state. The on-vehicle sensor immediately sends a signal to the VCU (vehicle control unit), and the VUC sends a signal to the braking system, and the tractor brakes emergently; then the VCU sends a signal to the steering system to control the tractor to drive away from the obstacle. When the tractor has passed the obstacle and the distance between the tractor and the obstacle is greater than R + 0.5W (R is the turning radius of the tractor, and W is the working width), it is determined that the tractor has completed obstacle avoidance. After completing obstacle avoidance, the VCU sends a signal to the steering system to control the tractor to return to the path of the original driving direction. When the sensor detects an obstacle and determines it to be of ordinary threat level, the VCU sends a signal to the braking system to control the vehicle to stop; after stopping, the VCU controls the tractor to sound the horn for the first time. After 30 seconds of the first horn sounding, if the sensor detects a change in the distance between the vehicle and the obstacle, it is determined that this obstacle is a dynamic obstacle, and the tractor sounds the horn again. If the obstacle in front disappears, the tractor proceeds along the original route; if it does not disappear, it continues to stop and wait until the obstacle in front disappears and then proceeds along the original route; after 30 seconds of the first horn sounding, if the sensor detects that the distance between the tractor and the obstacle has not changed, it is determined that this obstacle is a static obstacle, and obstacle avoidance is performed according to the locally optimal path planned by the JPS+ algorithm.

[0052] The present invention combines the ant colony algorithm with the JPS+ algorithm. The ant colony algorithm is used for the global path planning of the path, and the JPS+ algorithm is used for the local path planning of the path. At the same time, the JPS+ algorithm is optimized. By combining the ant colony algorithm and the JPS+ algorithm, the efficiency and accuracy of the global path planning and the local path planning when encountering obstacles are improved, and the preprocessing cost of the grid map is reduced. By screening the obstacles, the path planning steps are simplified, the path planning speed is increased, and the working efficiency of the tractor is improved.

[0053] The above are only embodiments of the present invention, and do not impose any form of limitation on the present invention. The present invention may also have other forms of embodiments based on the above structure and function, which will not be listed one by one. Therefore, any person skilled in the relevant art, without departing from the scope of the technical solution of the present invention, any simple modification, equivalent change and modification made to the above embodiments according to the technical essence of the present invention shall still fall within the scope of the technical solution of the present invention.

Claims

1. A path planning and obstacle avoidance method for driverless tractors based on the ant colony algorithm and the JPS+ algorithm, characterized in that It includes the following steps: (1) Construct an environmental grid map of the field operation area, where the grid map includes the shape and size of the farmland boundary, the starting and ending positions of the tractor's operation, and the length, width, and height of the obstacles in the farmland; (2) Run the ant colony algorithm, taking the tractor energy consumption as the objective function, and solve globally to obtain a globally optimal path from the starting position to the ending position of the tractor's operation; (3) Run the JPS+ algorithm to plan the local path, and combine step (1) to plan an optimal path for the tractor that combines global and obstacle avoidance.

2. The path planning and obstacle avoidance method for an autonomous tractor based on the ant colony algorithm and the JPS+ algorithm according to claim 1, wherein The specific steps for constructing the environmental grid map of the field operation area in step (1) include: installing a sensor on the unmanned aerial vehicle (UAV) to scan the working plot to construct the environmental grid map of the field operation area, using the sensor on the UAV to scan each obstacle in the field operation area one by one, setting the coordinates of each obstacle and the length, width, and height parameters of the obstacle in the grid map; setting the boundary of the field operation area in the grid map and setting the starting point coordinates and ending point coordinates of the tractor's operation; determining the headland width in the grid map according to the turning radius of the tractor, the headland turning method, and the actual operation width of the tractor, and then evenly dividing the field operation area into several operation rows according to the actual operation width of the tractor.

3. The path planning and obstacle avoidance method for driverless tractors based on the ant colony algorithm and the JPS+ algorithm according to claim 2, characterized in that The said sensor adopts a 3D lidar.

4. The path planning and roadblock avoidance method of an autonomous tractor based on the ant colony algorithm and the JPS+ algorithm according to claim 1, characterized in that The specific steps for obtaining the globally optimal path of the tractor using the ant colony algorithm in step (2) include: initializing the path, numbering each operation row, setting the parameters of the ant colony algorithm and the number of ant colonies, the importance factor α of pheromone, the importance factor β of the heuristic function, the pheromone constant coefficient Q, and the maximum number of iterations n; setting the electric energy consumption function of the electric tractor, the path cost function of the ant colony, and the transfer probability function, and each ant selects the next task point according to the transfer probability P.

5. The path planning and roadblock avoidance method of an autonomous tractor based on the ant colony algorithm and the JPS+ algorithm according to claim 4, characterized in that The electric energy consumption function of the tractor is: In formula (1), μ is the ground friction coefficient; G is the gravity of the electric tractor itself, unit: N; α is the slope angle of the road surface where the electric tractor travels; S is the distance between the starting point and the ending point of the tractor's operation, unit: m; n represents the motor speed, unit: r / min; R represents the radius of the tractor wheel, unit: m; U represents the battery voltage, unit: V; i g represents the transmission ratio; i0 represents the final drive ratio; η T represents the transmission efficiency of the driveline; Path cost function: In formula (2), D(I, j) is the path cost between the position i of the tractor and the position j of the task point, (x i , y i ) are the position coordinates of the tractor, and (x j , y j ) are the position coordinates of the task point; The transfer probability function is: In formula (3), Tau is the pheromone concentration between node s i and node s i , Eta is the heuristic function, α is the importance factor of pheromone, β is the importance factor of the heuristic function, P is the transition probability, and D is the path cost.

6. The path planning and obstacle avoidance method for an autonomous tractor based on the ant colony algorithm and the JPS+ algorithm as claimed in claim 4 or 5, characterized in that Through the transfer probability, the ant colony starts to iterate. Each ant leaves pheromone on the operation rows it has traveled. The longer the operation row distance, the less pheromone. Taking the energy consumption of the tractor as the evaluation function, calculate the energy consumption of each ant passing through the path, and judge whether the number of iterations is greater than the set maximum number of iterations. If it is greater than the set maximum number of iterations, then output the globally optimal route; If it is less than the number of iterations, then continue to search for the optimal path.

7. The path planning and roadblock avoidance method of an autonomous tractor based on the ant colony algorithm and the JPS+ algorithm according to claim 1, characterized in that The specific steps of using the JPS+ algorithm for local path planning in step (3) include: judging whether the tractor can directly pass through the obstacles in the grid map according to the passability of the tractor and the information of the obstacles; if it is judged that the tractor can pass through the obstacle, deleting the obstacle in the grid map; if it is judged that the tractor cannot pass through the obstacle, retaining the obstacle; performing local path planning, preprocessing the grid map of the field operation area according to the JPS+ algorithm, and calculating all the jump points in the grid map; judging whether the current node is a jump point, if so, marking the jump point in the grid map; if not, calculating the distances from the current node in each direction to the jump points; planning the optimal local obstacle avoidance path according to the jump points; smoothly connecting the optimal local obstacle avoidance path with the global optimal path to construct a complete optimal path with both global and obstacle avoidance capabilities.

8. The path planning and roadblock avoidance method for driverless tractors based on the ant colony algorithm and the JPS+ algorithm according to claim 1, characterized in that The process of obstacle avoidance includes: Scanning the surrounding environment using on-vehicle sensors to judge whether there are obstacles around. If there are no obstacles, the tractor travels normally. If there are obstacles, measuring the distance between the tractor and the obstacles; classifying the obstacles according to the distance between the tractor and the obstacles; if the distance between the tractor and the obstacle is greater than R + 0.5W, determining that this obstacle is of ordinary threat level; if the distance between the tractor and the obstacle is not greater than R + 0.5W, determining that this obstacle is of serious threat level; where R is the turning radius of the tractor and W is the operation width. When the on-vehicle sensor detects an obstacle and determines it to be of serious threat level, the tractor takes active avoidance measures and enters the emergency avoidance state; the on-vehicle sensor immediately sends a signal to the VCU, and the VUC sends a signal to the braking system, and the tractor brakes emergently; then the VCU sends a signal to the steering system to control the tractor to drive away from the obstacle. When the tractor has passed the obstacle and the distance between the tractor and the obstacle is greater than R + 0.5W, it is judged that the tractor has completed obstacle avoidance; after completing obstacle avoidance, the VCU sends a signal to the steering system to control the tractor to return to the path in the original driving direction. When the sensor detects an obstacle and determines it to be of ordinary threat level, the VCU sends a signal to the braking system to control the vehicle to stop; after stopping, the VCU controls the tractor to sound the horn for the first time. After 30s of the first horn sounding, if the sensor detects that the distance between the vehicle and the obstacle has changed, it is determined that this obstacle is a dynamic obstacle, and the tractor sounds the horn again. If the obstacle in front has disappeared, the tractor proceeds along the original route; if it has not disappeared, it continues to stop and wait until the obstacle in front has disappeared and then proceeds along the original route; after 30s of the first horn sounding, if the sensor detects that the distance between the tractor and the obstacle has not changed, it is determined that this obstacle is a static obstacle, and obstacle avoidance is performed according to the locally optimal path planned by the JPS+ algorithm.

Images