CN112197775B - Agricultural machinery multi-machine collaborative operation path planning method - Google Patents
Agricultural machinery multi-machine collaborative operation path planning method Download PDFInfo
- Publication number
- CN112197775B CN112197775B CN202011261969.6A CN202011261969A CN112197775B CN 112197775 B CN112197775 B CN 112197775B CN 202011261969 A CN202011261969 A CN 202011261969A CN 112197775 B CN112197775 B CN 112197775B
- Authority
- CN
- China
- Prior art keywords
- agricultural machine
- path
- agricultural
- node
- agricultural machinery
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 28
- 239000011159 matrix material Substances 0.000 claims abstract description 26
- 238000004458 analytical method Methods 0.000 claims description 3
- 238000005457 optimization Methods 0.000 description 9
- 238000010586 diagram Methods 0.000 description 6
- 238000006243 chemical reaction Methods 0.000 description 2
- 230000006872 improvement Effects 0.000 description 2
- 238000004519 manufacturing process Methods 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000006467 substitution reaction Methods 0.000 description 2
- 241000251468 Actinopterygii Species 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 230000010354 integration Effects 0.000 description 1
- 230000008569 process Effects 0.000 description 1
- 230000009467 reduction Effects 0.000 description 1
- 238000005096 rolling process Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0287—Control of position or course in two dimensions specially adapted to land vehicles involving a plurality of land vehicles, e.g. fleet or convoy travelling
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Aviation & Aerospace Engineering (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Guiding Agricultural Machines (AREA)
Abstract
The invention discloses a planning method of a multi-machine collaborative operation path of an agricultural machine in the technical field of agricultural machine operation control, which comprises the following steps of (1) inputting the operation breadth and the minimum turning radius of the agricultural machine; (2) determining a turning mode; (3) Dividing a target farmland into a plurality of operation rows according to the operation breadth; (4) Simplifying the middle points of the two ends of each operation row of the target farmland into a node; (5) Obtaining the geographic information of a target farmland to obtain the coordinates of each node, calculating the distance between the mutually reachable nodes according to the coordinates of the nodes, and forming a distance matrix; (6) Inputting the number of agricultural machines, the number of nodes and a distance matrix to obtain an optimal operation path track and an optimal operation path total distance of each agricultural machine; (7) Establishing a kinematic model of the agricultural machine, and obtaining a control variable for controlling the steering wheel angle of the agricultural machine according to the relative geometric relation between the pose of the agricultural machine and a given path to realize automatic running of the agricultural machine; the invention improves the operation efficiency of the agricultural machinery.
Description
Technical Field
The invention belongs to the technical field of agricultural machinery operation control, and particularly relates to a planning method for a multi-machine collaborative operation path of an agricultural machinery.
Background
The agricultural machinery needs to conduct path planning in advance during navigation operation, the path planning of the agricultural machinery means that the agricultural machinery conducts orderly traversal on operation lines under certain constraint conditions, the constraint conditions mainly comprise operation breadth and turning radius of the agricultural machinery and geographical information of farmlands, the path planning method can help the agricultural machinery to search an optimal operation path, the optimal operation path means an operation path meeting an optimization target set in advance, and the optimization target generally mainly comprises the shortest operation total distance, the shortest operation total duration, the minimum turning times and the shortest turning total duration.
In the prior art, a single machine path planning method is used for realizing agricultural machinery operation, and the method mainly comprises a land block whole area coverage path optimization algorithm, a whole arrangement algorithm and path planning according to different optimization targets; the path planning method is divided into a straight path planning method and a turning path planning method according to the paths, wherein the turning mode mainly comprises an arc shape, a U shape, an omega shape, an R shape, a fish tail shape and a slope shape; the method is mainly divided into a rectangular farmland path planning method and a trapezoid farmland path planning method according to different geometrical shapes of farmland plots; the method is mainly characterized in that farmland operation is divided into different areas based on prior information such as farmland land parcel geometry, operation tool parameters, land turning modes and the like, an optimal operation direction is calculated according to different path optimization targets, an optimal operation path is generated, and the operation efficiency is low.
Disclosure of Invention
The invention aims to overcome the defects in the prior art, and provides a planning method for a multi-machine collaborative operation path of an agricultural machine, which solves the technical problem of low operation efficiency in the prior art.
The purpose of the invention is realized in the following way: a planning method for multi-machine collaborative operation path of agricultural machinery comprises the following steps,
(1) Inputting the operation width and the minimum turning radius of the agricultural machinery;
(2) According to the operation breadth and the minimum turning radius input by the user, automatically determining a turning mode;
(3) Dividing a target farmland into a plurality of operation rows according to the operation breadth;
(4) Simplifying the midpoints of two ends of each operation row of the target farmland into a node and numbering the nodes in sequence;
(5) Obtaining the geographic information of a target farmland to obtain the coordinates of each node, automatically calculating the distance between the mutually reachable nodes according to the coordinates of the nodes, and forming a distance matrix;
(6) Inputting the number of agricultural machines, the number of nodes and a distance matrix to obtain an optimal operation path track and an optimal operation path total distance of each agricultural machine;
(7) And establishing a kinematic model of the agricultural machine, establishing a nonlinear feedback function based on the transverse position deviation de and the course angle deviation thetae, and realizing that the transverse tracking error index is converged to 0, and acquiring a control variable for controlling the steering wheel angle of the agricultural machine according to the relative geometrical relationship between the pose of the agricultural machine and a given path so as to realize the automatic running of the agricultural machine.
In order to further improve the working efficiency, in the step (2), the turning mode is determined according to the working width,
(201)W＝2R min : the agricultural machinery enters adjacent rows for operation in a U-shaped turning mode;
(202)W＞2R min : the agricultural machinery enters adjacent rows for operation in an arched turning mode;
(203)W＜2R min in the time-course of which the first and second contact surfaces,the agricultural machinery enters the operation rows spaced by Z-1 rows in a U-shaped turning mode to operate, and Z is the operation row number.
As a further improvement of the present invention, in the step (203), the agricultural machine enters the interval Z rows and works with the work rows of the upper rows in an arcuate turning manner.
As a further improvement of the invention, the sequence numbers of the rows and the columns in the distance matrix are composed of node sequence numbers and virtual node sequence numbers of the farmland model, the elements in the distance matrix represent the path length of the row sequence numbers of the elements reaching the column sequence numbers of the elements, only element 0 represents the fact that the elements cannot reach between the row sequence numbers and the column sequence numbers, the distances between the virtual nodes and other nodes are set, the virtual nodes are uniformly distributed among the real node sequence numbers, the respective operation paths of a plurality of agricultural machines are finally obtained, and when the number of the agricultural machines is m, the added virtual nodes are m-1.
In order to further obtain the optimal operation path track, in the step (6), the method for obtaining the optimal operation path track is specifically that each node only appears once in a single traversal, a group of node serial numbers are obtained after each traversal is finished, the total length of the multi-machine operation path composed of the group of node serial numbers is calculated according to an input distance matrix, the algorithm temporarily stores the result, the result is convenient to compare with the result of the next traversal, if the total length of the operation path obtained by the next algorithm traversal is smaller than the last one, the latest result is reserved, otherwise, the result is abandoned, multiple rounds of traversal are performed, and finally the total length of the multi-machine operation path composed of the group of node serial numbers, namely the optimal operation path track and the optimal operation path total length, and virtual nodes are also contained in the group of node serial numbers.
In order to further realize automatic running of the agricultural machine, in the step (7), the agricultural machine is simplified into a two-wheel vehicle model for kinematic analysis, and an agricultural machine kinematic model is established, wherein the kinematic model is shown in the following formula:
wherein de (t) is the lateral position deviation of the agricultural machine relative to the planned path at time t; v (t) is the movement speed of the agricultural machinery at the moment t; θ is the heading angle of the agricultural machine; θe (t) is the heading deviation angle of the agricultural machinery at the moment t, namely the included angle between the tangent line of a point closest to the center of the front axle on the planned path and the movement direction of the agricultural machinery vehicle body; l is the agricultural machinery wheelbase; delta (t) is the front wheel steering angle of the agricultural machinery at the moment t; u is the steering angular speed of the front wheels of the agricultural machinery;
the relation between the steering angle delta of the front wheel of the control quantity agricultural machine and the two deviation amounts is as follows:
δ(t)＝δ de (t)+δ θe (t) (2)
wherein delta de (t) is the front wheel steering angle control amount calculated by the transverse position error at the time t; delta theta e (t) is the control quantity of the steering angle of the front wheel calculated by the course angle error at the moment t;
when the agricultural machinery transverse position error de is not considered, the direction of the front wheel of the agricultural machinery is the same as the tangential direction of a point closest to the center of the front wheel axle on the planned path, and at the moment, the heading deviation angle θe of the agricultural machinery is the same as the steering angle of the front wheel, as shown in the formula:
δ θe (t)＝θ e (t) (3)
when the agricultural machine course angle deviation theta e is not considered, assuming that the expected track of the agricultural machine intersects with a tangent line of a point closest to the agricultural machine front axle center on the planned path at the position of the agricultural machine front axle center l (t), the following nonlinear proportional function relation can be obtained according to the geometric relation:
wherein l (t) is related to the agricultural machine running speed v (t), and the formula is as follows, wherein v (t) and the gain parameter k are used for representing l (t):
bringing equation (5) into equation (4) yields the following equation:
when the transverse position error is given, the steering angle of the front wheel pointing to the expected path can be obtained through an arctan function; after two factors are comprehensively considered, the control law of the final front wheel steering angle is obtained as shown in a formula (7):
the gain parameter k determines the speed at which the lateral position error de (t) converges to 0.
Drawings
FIG. 1 is a U-turn path diagram of the present invention.
FIG. 2 is a diagram of an arcuate turn path in accordance with the present invention.
FIG. 3 is a diagram of a model of a farmland with 8 rows in the present invention.
FIG. 4 is a diagram of a farmland model with a number of rows of 9 in the present invention.
FIG. 5 is a general view of the path trajectories in the farmland with the number of lines of 8 in the present invention.
FIG. 6 is a general view of the path trajectories in the farmland with the line number of 9 in the present invention.
FIG. 7 is a graph of the path 1 trace in farmland with 8 rows in the present invention.
FIG. 8 is a graph of the path 1 trace in a farmland with a number of rows of 9 according to the present invention.
FIG. 9 is a graph of the path 2 trace in farmland with 8 rows in the present invention.
FIG. 10 is a graph of the path 2 trace in a farmland with a line number of 9 according to the present invention.
FIG. 11 is a graph of the path 3 trace in farmland with 8 rows in the present invention.
FIG. 12 is a graph of the path 3 trace in the farmland with the line number of 9 in the present invention.
FIG. 13 is a graph showing the comparison of the total path length after optimization and the total path length before non-optimization in a farmland with the number of lines of 8.
FIG. 14 is a graph showing the comparison of the total path length after optimization and the total path length before non-optimization in a farmland with the number of lines of 9.
FIG. 15 is a diagram of a kinematic model of an agricultural machine in accordance with the present invention.
FIG. 16 is a tracking control diagram of an actual work path and a planned work path of an agricultural machine according to the present invention.
FIG. 17 is a graph showing the course angle deviation with working distance according to the present invention.
FIG. 18 is a graph showing the variation of the lateral position error with working distance according to the present invention.
Detailed Description
The invention is further described below with reference to the accompanying drawings.
The method for planning the multi-machine collaborative work path of the agricultural machinery as shown in figures 1-18 comprises the following steps,
(1) Inputting the operation width and the minimum turning radius of the agricultural machinery;
(2) According to the operation breadth and the minimum turning radius input by the user, automatically determining a turning mode;
(3) Dividing a target farmland into a plurality of operation rows according to the operation breadth;
(4) Simplifying the midpoints of two ends of each operation row of the target farmland into a node and numbering the nodes in sequence;
(5) Obtaining the geographic information of a target farmland to obtain the coordinates of each node, automatically calculating the distance between the mutually reachable nodes according to the coordinates of the nodes, and forming a distance matrix;
(6) Inputting the number of agricultural machines, the number of nodes and a distance matrix to obtain an optimal operation path track and an optimal operation path total distance of each agricultural machine;
(7) And establishing a kinematic model of the agricultural machine, establishing a nonlinear feedback function based on the transverse position deviation de and the course angle deviation thetae, and realizing that the transverse tracking error index is converged to 0, and acquiring a control variable for controlling the steering wheel angle of the agricultural machine according to the relative geometrical relationship between the pose of the agricultural machine and a given path so as to realize the automatic running of the agricultural machine.
In order to further improve the working efficiency, in the step (2), the turning mode is determined according to the working width,
(201)W＝2R min : the agricultural machinery enters adjacent rows for operation in a U-shaped turning mode;
(202)W＞2R min : the agricultural machinery enters adjacent rows for operation in an arched turning mode;
(203)W＜2R min in the time-course of which the first and second contact surfaces,the agricultural machinery enters an operation line of Z-1 line at intervals in a U-shaped turning mode to operate, or enters the interval Z line and an operation line of the upper line in an arched turning mode to operate, wherein Z is the operation line; the U-shaped and arched turning modes are in the prior art, and turning models are not repeated in the invention.
In the step (5), the sequence numbers of the rows and the columns in the distance matrix are composed of node sequence numbers and virtual node sequence numbers of the farmland model, the elements in the distance matrix represent the path length of the row sequence numbers of the elements reaching the column sequence numbers of the elements, only the element 0 represents that the elements cannot reach between the row sequence numbers and the column sequence numbers, the distances between the virtual node sequence numbers and other nodes enable the virtual nodes to be uniformly distributed among the real node sequence numbers, and when the number of agricultural machinery is m, the added virtual nodes are m-1; the field coordinates are processed as follows:
converting longitude and latitude under the WGS-84 system into coordinates under an ECEF space coordinate system by adopting a Gaussian forward calculation formula, and then converting the ECEF coordinates into a local space rectangular coordinate system (ENU) by adopting a formula (5-1);
wherein x0, y0 and z0 are coordinates of the origin of coordinates of the ENU coordinate system in the ECEF coordinate system; the elements of matrix M are shown in equation (5-2),
wherein λ0 andcoordinates of the origin of coordinates of the ENU coordinate system in the WGS-84 coordinate system, respectively, in the inverted form as shown in formula (5-3),
the rotation angle is obtained according to the plot coordinates, then the plot can be parallel to the coordinate axis by carrying out rotation processing on each point, as shown in the formula (5-4),
wherein t ' is a rotation angle, (x, y) is a coordinate before conversion, (x ', y ') is a coordinate after conversion, e, n, u are coordinates of a certain point under a rectangular coordinate system of a place, (x 1, y 1) and (x 4, y 4) are coordinates of any two vertexes of four vertexes constituting a rectangular land block;
and (3) carrying out inverse rotation by adopting a formula (5-5), realizing coordinate reduction, and further carrying out final coordinate output:
in the step (6), the method for obtaining the optimal operation path track is specifically that each node only appears once in a single traversal, a group of node serial numbers are obtained after each traversal is finished, the total length of the multi-machine operation path formed by the group of node serial numbers is calculated according to an input distance matrix, the algorithm temporarily stores the result, the result is convenient to compare with the result of the next traversal, if the total length of the operation path obtained by the next algorithm traversal is smaller than the last traversal, the latest result is reserved, otherwise, the operation path is abandoned, the multi-cycle traversal is carried out, and finally, the total length of the multi-machine operation path formed by the group of node serial numbers, namely the optimal operation path track and the optimal operation path total length, and virtual nodes are also contained in the group of node serial numbers.
In the invention, farmland models with the row numbers of 8 and 9 are selected, the row width is 20m, the operation width W=2.65m, the minimum turning radius Rmin= 3.975m, the midpoints of the two ends of each operation row are simplified into a node, the nodes are numbered, 3 operation machines are selected, and 2 virtual nodes are added in the path planning of respective farmland; in an agricultural field with the number of lines of 8, the number of original nodes is 16, after 2 virtual nodes 17 and 18 are added, the number of the existing nodes is 18, and finally an 18×18 distance matrix (shown as a matrix 1) is formed; in a farmland with the line number of 9, the number of the original nodes is 18, after 2 virtual nodes 19 and 20 are added, the number of the existing nodes is 20, and finally a distance matrix (shown as a matrix 2) of 20×20 is formed;
the matrix 1 is:
the matrix 2 is:
the row number and the column number in the matrix 1 and the matrix 2 are representative nodes, the meaning of the element 0 is that the nodes represented by the row number and the column number of the element cannot reach each other, and the agricultural machinery has constraint conditions on the traversing sequence of the nodes due to the characteristic of operation of the agricultural machinery in a farmland; in the farmland model with the line number of 8, the distance between the nodes positioned at the two ends of the straight line path is set to be 5, and the node pairs meeting the condition are as follows:the distances from the virtual nodes 17 and 18 to the rest points are set to 6; in the farmland model with the line number of 9, the distance between the nodes at the two ends of the straight line path is set to be 5, and the node pair meeting the condition comprises: /> The distances from the virtual nodes 19 and 20 to the remaining points are set to 6.
FIGS. 5 and 6 are the paths of the optimal agricultural machinery multi-machine collaborative operation in the farmland model with the number of rows of 8 and 9, respectively; in the farmland model with the row number of 8, the traversing sequence of the agricultural machinery to the nodes is as follows: 1- > 16- > 13- > 4- > 7- > 10- > 17- > 8- > 9- > 12- > 5- > 2- > 15- > 18- > 6- > 11- > 14- > 3 (17 and 18 are virtual nodes); in the farmland model with the row number of 9, the traversing sequence of the agricultural machinery to the nodes is as follows: 1- > 18- > 15- > 4- > 7- > 12- > 20- > 2- > 17- > 14- > 5- > 8- > 11- > 19- > 3- > 16- > 13- > 6- > 9- > 10 (19 and 20 are virtual nodes);
fig. 7, 9 and 11 show the path trajectories of 3 working tools in the farmland model with the number of lines of 8, respectively, and the path trajectories are: 1- > 16- > 13- > 4- > 7- > 10 (fig. 7), 6- > 11- > 14- > 3 (fig. 9), 8- > 9- > 12- > 5- > 2- > 15 (fig. 11); fig. 8, 10, and 12 show the path trajectories of 3 working tools in the farmland model with the number of lines of 9, respectively, and the path trajectories are: 1- > 18- > 15- > 4- > 7- > 12 (fig. 8), 2- > 17- > 14- > 5- > 8- > 11 (fig. 10), 3- > 16- > 13- > 6- > 9- > 10 (fig. 12); from the above results, the combination of path planning and virtual nodes jointly realizes uniform path division, and the operation path operation gap of each finally obtained agricultural machine is small, and each agricultural machine works more reasonably and uniformly.
Fig. 14 is a graph comparing the effect between the total distance of the optimal multi-machine cooperative operation path of the agricultural machine obtained after the processing of the invention and the total distance of the randomly selected multi-machine cooperative operation path of the agricultural machine, which clearly shows the good optimizing effect of the invention on the multi-machine cooperative operation path of the agricultural machine, can effectively reduce the total distance of the multi-machine cooperative operation path of the agricultural machine, greatly saves the production cost and improves the production efficiency.
In step 7, in order to realize the tracking control of the agricultural machine on the planned path, based on the nonlinear feedback function of the transverse position deviation de and the course angle deviation θe, a front wheel steering angle control amount is calculated by the two deviations through an algorithm, and then the two control amounts are overlapped to realize the tracking of the agricultural machine on the planned path. The following assumptions are made: (1) the agricultural machinery and the suspended working machine are a rigid body; (2) ignoring deformation problems when the tire is in contact with the ground; (3) In the movement process of the agricultural machine, the front wheel and the rear wheel are purely rolling instead of sliding, so that the speed direction of the agricultural machine is along the central line direction of the vehicle body; (4) the agricultural machinery does not move vertically; (5) the steering angles of the two front wheels are the same. Simplifying the agricultural machinery into a two-wheel vehicle model for kinematic analysis (shown in fig. 15), and establishing an agricultural machinery kinematic model, wherein the kinematic model is shown in the following formula:
wherein de (t) is the lateral position deviation of the agricultural machine relative to the planned path at time t; v (t) is the movement speed of the agricultural machinery at the moment t; θ is the heading angle of the agricultural machine; θe (t) is the agricultural machine course deviation angle (the angle between the tangent line of the closest point on the planned path to the front axle center and the agricultural machine body movement direction) at the moment t; l is the agricultural machinery wheelbase; delta (t) is the front wheel steering angle of the agricultural machinery at the moment t; u is the steering angular speed of the front wheels of the agricultural machinery;
the relation between the steering angle delta of the front wheel of the control quantity agricultural machine and the two deviation amounts is as follows:
δ(t)＝δ de (t)+δ θe (t) (2)；
wherein δde (t) is a front wheel steering angle control amount calculated by a lateral position error at time t; delta theta e (t) is the control quantity of the steering angle of the front wheel obtained by calculating the course angle error at the moment t;
when the agricultural machinery transverse position error de is not considered, the direction of the front wheel of the agricultural machinery is the same as the tangential direction of a point closest to the center of the front wheel axle on the planned path, and at the moment, the heading deviation angle θe of the agricultural machinery is the same as the steering angle of the front wheel, as shown in the formula:
δ θe (t)＝θ e (t) (3)；
when the agricultural machine course angle deviation theta e is not considered, assuming that the expected track of the agricultural machine intersects with a tangent line of a point closest to the agricultural machine front axle center on the planned path at the position of the agricultural machine front axle center l (t), the following nonlinear proportional function relation can be obtained according to the geometric relation:
wherein l (t) is related to the agricultural machine running speed v (t), and the formula is as follows, wherein v (t) and the gain parameter k are used for representing l (t):
bringing equation (5) into equation (4) yields the following equation:
when the transverse position error is given, the steering angle of the front wheel pointing to the expected path can be obtained through an arctan function, and after two factors are comprehensively considered, the control law of the final steering angle of the front wheel is shown as a formula (7):
wherein the gain parameter k determines the speed at which the lateral position error de (t) converges to 0;
the formula of the change rate of the transverse position error obtained according to the established two-wheel agricultural machinery kinematics model is as follows:
in fig. 15, the following formula is available according to the geometrical relationship in the agricultural machine kinematics model:
as can be seen from equation (5), when the lateral position deviation is small, the equation is as follows:
the formula obtained by integration is as follows:
the final lateral position error will converge to 0 and the gain parameter k directly affects the convergence speed.
Compared with the prior art, the invention improves the working efficiency by the cooperative operation of a plurality of agricultural machines, and when the agricultural machines work, the optimal working path with the shortest total path of the working path is obtained through the combination of the turning mode and the path planning, thereby further improving the working efficiency; can be applied to the unmanned operation of the field.
The invention is not limited to the above embodiments, and based on the technical solution disclosed in the invention, a person skilled in the art may make some substitutions and modifications to some technical features thereof without creative effort according to the technical content disclosed, and all the substitutions and modifications are within the protection scope of the invention.
Claims (6)
1. A planning method for a multi-machine collaborative operation path of an agricultural machine is characterized by comprising the following steps,
(1) Inputting the operation width and the minimum turning radius of the agricultural machinery;
(2) According to the operation breadth and the minimum turning radius input by the user, automatically determining a turning mode;
(3) Dividing a target farmland into a plurality of operation rows according to the operation breadth;
(4) Simplifying the midpoints of two ends of each operation row of the target farmland into a node and numbering the nodes in sequence;
(5) Obtaining the geographic information of a target farmland to obtain the coordinates of each node, automatically calculating the distance between the mutually reachable nodes according to the coordinates of the nodes, and forming a distance matrix;
(6) Inputting the number of agricultural machines, the number of nodes and a distance matrix to obtain an optimal operation path track and an optimal operation path total distance of each agricultural machine;
(7) And establishing a kinematic model of the agricultural machine, establishing a nonlinear feedback function based on the transverse position deviation de and the course angle deviation thetae, and realizing that the transverse tracking error index is converged to 0, and acquiring a control variable for controlling the steering wheel angle of the agricultural machine according to the relative geometrical relationship between the pose of the agricultural machine and a given path so as to realize the automatic running of the agricultural machine.
2. The method for planning a coordinated operation route of multiple agricultural machines according to claim 1, wherein in the step (2), the turning mode is determined according to the operation width,
(201)W＝2R min : the agricultural machinery enters adjacent rows for operation in a U-shaped turning mode;
(202)W＞2R min : the agricultural machinery enters adjacent rows for operation in an arched turning mode;
(203)W＜2R min in the time-course of which the first and second contact surfaces,z is more than 1, and the agricultural machinery enters the operation row of Z-1 row at intervals to operate in a U-shaped turning mode.
3. The method for planning a collaborative work path for an agricultural machine according to claim 2, wherein in step (203), the agricultural machine performs work in a work row that is spaced apart by Z rows and is up to the Z rows in an arcuate turn.
4. The method for planning a multi-machine collaborative operation path of an agricultural machine according to any one of claims 1-3, wherein the sequence numbers of the rows and the columns in the distance matrix are composed of node sequence numbers and virtual node sequence numbers of a farmland model, the elements in the distance matrix represent the path length of the row sequence number of the element reaching the column sequence number of the element, only element 0 represents the path length between the row sequence number and the column sequence number of the element, the distance between the virtual node sequence number and each other node is such that the virtual node is uniformly distributed in the middle of the real node sequence numbers, and when the number of the agricultural machine is m, the added virtual node is m-1.
5. The method for planning a coordinated operation path of multiple agricultural machines according to claim 4, wherein in the step (6), the method for obtaining the optimal operation path trajectory is specifically that each node only appears once in a single traversal, a set of node serial numbers are obtained after each traversal is finished, the total length of the multiple operation paths composed of the set of node serial numbers is calculated according to the input distance matrix, the algorithm temporarily stores the result, the result is convenient to compare with the result of the next traversal, if the total length of the operation path obtained by the next algorithm traversal is smaller than the last one, the latest result is reserved, otherwise, the method is abandoned, multiple rounds of traversal are performed, finally a set of node serial numbers and the total length of the multiple operation paths composed of the set of node serial numbers, namely the optimal operation path trajectory and the optimal operation path total length, and virtual nodes are contained in the set of node serial numbers.
6. The method for planning a multi-machine collaborative work path of an agricultural machine according to any one of claims 1-3, wherein in the step (7), the agricultural machine is simplified into a two-wheel vehicle model for kinematic analysis, and an agricultural machine kinematic model is established, as shown in the following formula:
wherein de (t) is the lateral position deviation of the agricultural machine relative to the planned path at time t; v (t) is the movement speed of the agricultural machinery at the moment t; θ is the heading angle of the agricultural machine; θe (t) is the heading deviation angle of the agricultural machinery at the moment t, namely the included angle between the tangent line of a point closest to the center of the front axle on the planned path and the movement direction of the agricultural machinery vehicle body; l is the agricultural machinery wheelbase; delta (t) is the front wheel steering angle of the agricultural machinery at the moment t; u is the steering angular speed of the front wheels of the agricultural machinery;
the relation between the steering angle delta of the front wheel of the control quantity agricultural machine and the two deviation amounts is as follows:
δ(t)＝δ de (t)+δ θe (t) (2)
wherein delta de (t) is the front wheel steering angle control amount calculated by the transverse position error at the time t; delta theta e (t) is the control quantity of the steering angle of the front wheel calculated by the course angle error at the moment t;
when the agricultural machinery transverse position error de is not considered, the direction of the front wheel of the agricultural machinery is the same as the tangential direction of a point closest to the center of the front wheel axle on the planned path, and at the moment, the heading deviation angle θe of the agricultural machinery is the same as the steering angle of the front wheel, as shown in the formula:
δ θe (t)＝θ e (t) (3)
when the agricultural machine course angle deviation theta e is not considered, assuming that the expected track of the agricultural machine intersects with a tangent line of a point closest to the agricultural machine front axle center on the planned path at the position of the agricultural machine front axle center l (t), the following nonlinear proportional function relation can be obtained according to the geometric relation:
wherein l (t) is related to the agricultural machine running speed v (t), and the formula is as follows, wherein v (t) and the gain parameter k are used for representing l (t):
bringing equation (5) into equation (4) yields the following equation:
when the transverse position error is given, the steering angle of the front wheel pointing to the expected path can be obtained through an arctan function;
after two factors are comprehensively considered, the control law of the final front wheel steering angle is obtained as shown in a formula (7):
the gain parameter k determines the speed at which the lateral position error de (t) converges to 0.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011261969.6A CN112197775B (en) | 2020-11-12 | 2020-11-12 | Agricultural machinery multi-machine collaborative operation path planning method |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011261969.6A CN112197775B (en) | 2020-11-12 | 2020-11-12 | Agricultural machinery multi-machine collaborative operation path planning method |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN112197775A CN112197775A (en) | 2021-01-08 |
| CN112197775B true CN112197775B (en) | 2024-02-09 |
Family
ID=74034270
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202011261969.6A Active CN112197775B (en) | 2020-11-12 | 2020-11-12 | Agricultural machinery multi-machine collaborative operation path planning method |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN112197775B (en) |
Families Citing this family (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112904867B (en) * | 2021-01-29 | 2023-01-06 | 山东理工大学 | Agricultural machine unmanned ground turning control method and system for wide-width operation |
| CN113848880B (en) * | 2021-08-30 | 2023-12-22 | 中车大连电力牵引研发中心有限公司 | Agricultural machinery path optimization method based on improved Q-learning |
| CN114115224B (en) * | 2021-10-13 | 2024-04-05 | 北京市农林科学院智能装备技术研究中心 | Method, device and system for controlling farm machinery section |
| CN114554308B (en) * | 2021-11-30 | 2023-12-22 | 潍坊中科晶上智能装备研究院有限公司 | Intelligent agricultural machinery remote control system |
| CN114019982B (en) * | 2021-12-07 | 2024-05-03 | 洛阳智能农业装备研究院有限公司 | Unmanned intelligent agricultural machinery cluster operation method |
| CN115309159B (en) * | 2022-08-12 | 2024-07-12 | 博创联动科技股份有限公司 | Agricultural machine interplanting operation planning method and device, computer terminal and storage medium |
Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111256700A (en) * | 2020-03-26 | 2020-06-09 | 洛阳智能农业装备研究院有限公司 | Edge narrowing planning method for planning operation path of automatic driving agricultural machine |
| CN111596652A (en) * | 2019-12-30 | 2020-08-28 | 山东省科学院自动化研究所 | Pesticide spraying intelligent agricultural machinery path finding navigation control method and system |
-
2020
- 2020-11-12 CN CN202011261969.6A patent/CN112197775B/en active Active
Patent Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111596652A (en) * | 2019-12-30 | 2020-08-28 | 山东省科学院自动化研究所 | Pesticide spraying intelligent agricultural machinery path finding navigation control method and system |
| CN111256700A (en) * | 2020-03-26 | 2020-06-09 | 洛阳智能农业装备研究院有限公司 | Edge narrowing planning method for planning operation path of automatic driving agricultural machine |
Non-Patent Citations (2)
| Title |
|---|
| 基于Bezier曲线优化的农机自动驾基于Bezier曲线优化的农机自动驾驶避障控制方法驶避障控制方法;奚小波 等;农业工程学报;第35卷(第19期);全文 * |
| 拖拉机自动驾驶路径规划算法研究与系统仿真;沈飞;陈伏州;戴伟;;拖拉机与农用运输车(02);全文 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN112197775A (en) | 2021-01-08 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN112197775B (en) | Agricultural machinery multi-machine collaborative operation path planning method | |
| CN109634276B (en) | Agricultural vehicle unmanned control method and system and agricultural vehicle | |
| US11052943B2 (en) | System and method for controlling a vehicle | |
| CN112015176B (en) | Unmanned tractor field operation path planning method and device | |
| US20180364726A1 (en) | System and method for generating partitioned swaths | |
| US20180359905A1 (en) | System and method for determining swath connections | |
| CN108955695B (en) | Global path planning method for farmland robot | |
| CN107390691B (en) | AGV path tracking method | |
| CA2593486C (en) | Path planner and method for planning a contour path of a vehicle | |
| CN109388140B (en) | Improved pure tracking control method for ground vehicle path tracking | |
| WO2015146777A1 (en) | Work vehicle cooperation system | |
| Eaton et al. | Autonomous farming: Modelling and control of agricultural machinery in a unified framework | |
| CN109238298A (en) | A kind of unmanned paths planning method with avoidance | |
| CN113359710B (en) | LOS theory-based agricultural machinery path tracking method | |
| CN111578942B (en) | Navigation method and system of land leveler | |
| CN106909150A (en) | For the unpiloted avoidance of agricultural machinery, turn around path planning and its control method | |
| CN106462164A (en) | Work vehicle cooperation system | |
| CN109471432A (en) | A kind of independent navigation agricultural vehicle Short Path Avoiding Obstructions planing method | |
| CN109656250A (en) | A kind of path following method of laser fork truck | |
| CN111595355B (en) | Unmanned rolling machine group path planning method | |
| CN110954122A (en) | Automatic driving track generation method under high-speed scene | |
| CN108205322A (en) | A kind of agricultural machinery working method and system that working path is not automatically generated by map | |
| Rastgoftar et al. | A data-driven approach for autonomous motion planning and control in off-road driving scenarios | |
| CN112985401A (en) | Headline full path planning and tracking method | |
| He et al. | Dynamic path planning method for headland turning of unmanned agricultural vehicles |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |