1. A real-time path planning and control method for ground steering of an agricultural unmanned vehicle is characterized by comprising the following steps:

(1) starting turning of the vehicle and initialization of equipment;

(2) acquiring vehicle position and course information in real time as input information of real-time path planning;

(3) generating a shortest planned route taking the current position as a starting point through input information;

(4) judging whether a parking instruction is sent or not according to the distance between the position of the current stage of the vehicle and the end point updated in real time at the stage;

(5) if a parking instruction is sent, entering the next stage; and (5) if the parking instruction is not sent, returning to the step (2) and continuously updating the route.

2. The real-time path planning and control method for ground steering of an agricultural unmanned vehicle according to claim 1, wherein the step (3) is specifically:

and solving the arc values of the arcs in three stages of the optimal route through the input information to generate the shortest route, and iteratively updating the process during driving.

3. The real-time path planning and control method for ground steering of an agricultural unmanned vehicle according to claim 1, wherein the step (4) is specifically:

setting a tolerance, and sending a parking instruction when the distance between the position of the current stage of the vehicle and the end point of the stage is less than the tolerance.

4. The method for real-time path planning and control of headland steering of an agricultural unmanned vehicle of claim 3, wherein the tolerance is 0.1 meters.

5. The real-time path planning and control method for ground steering of an agricultural unmanned vehicle according to claim 1, wherein the step (5) is specifically:

and (3) when the vehicle is in the first stage when the parking instruction is sent, entering the second stage, returning to the step (2), continuously updating the route, and if the vehicle is not in the first stage, entering the third stage.

6. The real-time path planning and control method for headland steering of an agricultural unmanned vehicle of claim 5, further comprising the steps of:

(6) and the vehicle carries out pure tracking navigation in the third stage, and if the vehicle reaches the target point, the turning is finished.

7. The method for real-time path planning and control of headland steering of an agricultural unmanned vehicle as claimed in claim 1, wherein the vehicle turns with a minimum turning radius in the first two phases after the turning is started.

8. The method for real-time path planning and control of headland steering of an agricultural unmanned vehicle as claimed in claim 1, wherein the vehicle position and heading information is obtained by high precision GPS.

Background

The field heading steering refers to a process that an agricultural vehicle successfully drives in the next crop row after driving out from the current crop row. The automatic control of the ground steering is one of the important components for planning the autonomous path of the agricultural unmanned vehicle and realizing automatic navigation. The time consumed by turning the ground is reduced to the maximum extent, and the optimal ground turning motion track is selected in a self-adaptive mode, so that the important way for improving the efficiency of the agricultural automation operation is realized.

Currently, the related research on the path planning and control of turning over the ground of an agricultural unmanned vehicle focuses on how to more accurately track the path and how to control the vehicle to return to the planned path when the vehicle deviates from the path. However, due to the influence of factors such as soil environment, road surface fluctuation, depth of mud feet in field blocks, sudden vehicle running conditions and the like, deviation and yaw caused by sideslip in vehicle running cannot be avoided. At this time, if the original planned route is still tracked, the actual traveling track of the vehicle is inevitably different from the planned route, resulting in a certain final deviation and a longer track length. At the same time, longer flight paths lead to increased energy consumption.

Meanwhile, tracking control is conventionally performed according to a predetermined turning course, and there are frequent steering control and acceleration/deceleration control. Frequent mechanical control can cause mechanical fatigue and abrasion of vehicle parts, and long-time mechanical fatigue and abrasion can damage parts of agricultural machinery, shorten the service life of the agricultural machinery and cause economic loss.

The patent specification with the publication number CN110440823B discloses a path planning method and a path planning system, which are used for selecting a corresponding connection mode for re-planning by judging the distance between the current pose and the target pose of a vehicle, re-planning a parking path, eliminating errors and realizing the accuracy of parallel parking of the vehicle. The method provided by the embodiment of the invention comprises the following steps: acquiring a starting point pose and a target pose; determining the distance between the starting point pose and the target pose; if the distance is larger than a first preset threshold value, moving the target pose in the direction of the starting point pose by a first turning radius to obtain a first current pose, wherein the first turning radius is larger than or equal to the minimum turning radius of the vehicle; and connecting the first current pose and the starting point pose by using a target connection method, and if the first current pose and the starting point pose are successful, obtaining a first planning path, wherein the first planning path comprises a path between the starting point pose and the first current pose and a path between the first current pose and the target pose. And using the target connection method according to the starting point pose and the target pose, wherein if the target connection method fails, the method further comprises the following steps: moving the starting pose towards the target pose by a preset length in a straight line to obtain a second current pose; and connecting the second current pose and the target pose by using the target connection method, and if the target connection method is successful, determining a third planning path, wherein the third planning path comprises a path between the starting point pose and the second current pose and a path between the second current pose and the target pose.

The path planning method of the scheme aims at the complex farmland environment that the line spacing of field crops at the head of the farmland is narrow, is smaller than the minimum turning radius of an agricultural vehicle and can not complete turning at one time, and the method cannot be well applied.

Disclosure of Invention

The invention aims to provide a real-time path planning and control method for the ground steering of an agricultural unmanned vehicle, which can effectively solve the problems of increase of track length, reduction of accuracy and increase of energy consumption after yaw of the existing route planning and control and solve the problems of frequent steering control and acceleration and deceleration control.

A real-time path planning and control method for ground steering of an agricultural unmanned vehicle comprises the following steps:

(1) starting turning of the vehicle and initialization of equipment;

(2) acquiring vehicle position and course information in real time as input information of real-time path planning;

(3) generating a shortest planned route taking the current position as a starting point through input information;

(4) judging whether a parking instruction is sent or not according to the distance between the position of the current stage of the vehicle and the end point updated in real time at the stage;

(5) if a parking instruction is sent, entering the next stage; and (5) if the parking instruction is not sent, returning to the step (2) and continuously updating the route.

According to the scheme, the optimal turning route is continuously updated in the driving process, namely the turning route is updated and changed at any moment, tracking control is not required according to the set turning route, frequent steering control and acceleration and deceleration control are avoided, and meanwhile the increase of the track length is avoided.

Preferably, the step (3) is specifically:

and solving the arc values of the arcs in three stages of the optimal route through the input information to generate the shortest route, and iteratively updating the process during driving.

Preferably, the step (4) is specifically:

setting a tolerance, and sending a parking instruction when the distance between the position of the current stage of the vehicle and the end point of the stage is less than the tolerance.

Further preferably, the tolerance is 0.1 meter.

Preferably, the step (5) is specifically:

and (3) when the vehicle is in the first stage when the parking instruction is sent, entering the second stage, returning to the step (2), continuously updating the route, and if the vehicle is not in the first stage, entering the third stage.

Further preferably, the method further comprises the steps of:

(6) and the vehicle carries out pure tracking navigation in the third stage, and if the vehicle reaches the target point, the turning is finished.

Preferably, after the turning is started, the vehicle turns at the minimum turning radius in the first two stages.

Preferably, the vehicle location and heading information is acquired by high precision GPS.

The invention has the beneficial effects that:

(1) the increased track length of the vehicle after the vehicle is deviated is reduced.

(2) The accuracy of the vehicle entering the next crop row is improved.

(3) Less mechanical losses to the vehicle and reduced energy consumption.

Drawings

FIG. 1 is a track chart of a vehicle turning over at the ground;

FIG. 2 is a track planning diagram for shortest path;

FIG. 3 is a flow chart of the present invention.

Detailed Description

The technical solution in the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings in the embodiments of the present invention. It is to be understood that the described embodiments are merely exemplary of the invention, and not restrictive of the full scope of the invention. All other embodiments, which can be derived by a person skilled in the art from the embodiments given herein without making any creative effort, shall fall within the protection scope of the present invention.

As shown in fig. 1, the course of the vehicle turning over to the ground is: the vehicle starts from point a, turns forward with a forward minimum turning radius to point B (end point B), and then starts to reverse until point C (end point C), and the vehicle still turns with a reverse minimum turning radius while reversing. And finally, the vehicle tracks the route in a pure tracking control mode from the point C to reach the target point D. The process from point a to point B is referred to as the first stage, the process from point B to point C is referred to as the second stage, and the process from point C to point D is referred to as the third stage.

After the turning starts, the vehicle turns with the minimum turning radius for forward and reverse in the first two stages, respectively. According to the invention, the positions of the point B and the point C on the path in the turning process are updated in real time according to the real-time position and the course of the vehicle, so that when a parking instruction is sent out is controlled and judged, and the purpose of updating the route in real time is achieved. Whether a parking instruction is issued is judged by the distance r. When the vehicle is in the first stage (arc AB), the distance r is the linear distance of the vehicle from point B; when the vehicle is in the second phase (arc BC), the distance r is the straight-line distance between the vehicle and point C. A tolerance is set for vehicle safety control. When the distance r is smaller than the tolerance, a parking instruction is issued. In the third phase, the vehicle does not perform a route update, but tracks the intended route through a pure tracking algorithm. After the third-stage turning is finished, the ground turning task is finished.

The above section is an overview of the update route of the present invention, which is explained in detail below.

As shown in fig. 2 and 3, a real-time path planning and control method for ground steering of an agricultural unmanned vehicle comprises the following steps:

(1) starting turning of the vehicle and initialization of equipment;

(2) acquiring vehicle position and course information in real time through a high-precision GPS (global positioning system) as input information of real-time path planning;

(3) generating the shortest route with the current position as a starting point through the input information, and generating the shortest navigation route by solving three arc values of the optimal route;

the vehicle is driven out from the crop row 1, and a two-dimensional rectangular coordinate system is established by taking the point D of the vehicle at the moment when the vehicle drives into the crop row 2 as the origin of coordinates (0, 0). (a, b) is the coordinates of the current position A of the vehicle, and the included angle between the heading of the vehicle and the positive direction of the x axis is. Points B and C are shown. Theta₁Is the arc of a first arc, theta₂Is the arc of the second arc, theta₃Is the arc of the third arc, R_fMinimum forward turning radius, R, of the vehicle_bThe minimum reversing turning radius of the vehicle, and R is the radius of a third section of circular arc. At this time, in the turning process, the real-time updating algorithm of the coordinates of the point B and the point C is as follows:

the coordinates of the point B are as follows:

the coordinates of the point C are:

wherein, theta₁、θ₂、θ₃The geometrical relationship between the two is as follows:

third segment arc radius R:

at this time, the length of the path that the vehicle travels through, i.e. the sum s of the lengths of the three arcs, is:

when the vehicle is in the first stage, according to the formula (1) and theta₁、θ₂、θ₃The ternary binomial relation between the two can know that the minimum s (shortest path) has uniquely determined theta₁、θ₂、θ₃. When the vehicle is in the second stage, only the angle theta needs to be adjusted₁By substituting 0 into a ternary binomial equation, theta can be obtained₂、θ₃. According to the obtained theta₁、θ₂、θ₃A unique set of turning schemes, i.e. corresponding shortest path combinations in the current state, can be determined.

(4) Judging whether a parking instruction is sent or not according to the distance between the position of the current stage of the vehicle and the end point updated in real time at the stage;

in the embodiment, the tolerance is set to be 0.1 meter, and when the vehicle is in the first stage (arc AB) and the linear distance r between the vehicle and the point B is less than 0.1 meter, a parking instruction is sent out; when the vehicle is in the second stage (arc BC) and the straight-line distance r between the vehicle and the point C is less than 0.1 meter, a parking instruction is issued.

(5) If a parking instruction is sent, entering the next stage; if the reversing instruction is not sent, returning to the step (2) and continuously updating the route;

and (3) when the vehicle is in the first stage when the parking instruction is sent, entering the second stage, returning to the step (1), continuously updating the route, and entering the third stage if the vehicle is not in the first stage.

(6) The vehicle carries out pure tracking navigation in the third stage, and if the vehicle reaches a target point, turning is finished;

and when the vehicle is in the third stage, stopping updating the route, only performing pure tracking navigation in the third stage, and finishing turning if the vehicle reaches the target point D.

Although the present invention has been described in detail with reference to the foregoing embodiments, it will be apparent to those skilled in the art that various changes in the embodiments and/or modifications of the invention can be made, and equivalents and modifications of some features of the invention can be made without departing from the spirit and scope of the invention.