CN111578942B - Navigation method and system of land leveler - Google Patents

Navigation method and system of land leveler Download PDF

Info

Publication number
CN111578942B
CN111578942B CN202010359054.2A CN202010359054A CN111578942B CN 111578942 B CN111578942 B CN 111578942B CN 202010359054 A CN202010359054 A CN 202010359054A CN 111578942 B CN111578942 B CN 111578942B
Authority
CN
China
Prior art keywords
land
navigation
path
tractor
real
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
Application number
CN202010359054.2A
Other languages
Chinese (zh)
Other versions
CN111578942A (en
Inventor
刘刚
景云鹏
金志坤
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
China Agricultural University
Original Assignee
China Agricultural University
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by China Agricultural University filed Critical China Agricultural University
Priority to CN202010359054.2A priority Critical patent/CN111578942B/en
Publication of CN111578942A publication Critical patent/CN111578942A/en
Application granted granted Critical
Publication of CN111578942B publication Critical patent/CN111578942B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

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
    • A—HUMAN NECESSITIES
    • A01—AGRICULTURE; FORESTRY; ANIMAL HUSBANDRY; HUNTING; TRAPPING; FISHING
    • A01B—SOIL WORKING IN AGRICULTURE OR FORESTRY; PARTS, DETAILS, OR ACCESSORIES OF AGRICULTURAL MACHINES OR IMPLEMENTS, IN GENERAL
    • A01B35/00—Other machines for working soil not specially adapted for working soil on which crops are growing
    • A01B35/02—Other machines for working soil not specially adapted for working soil on which crops are growing with non-rotating tools
    • A01B35/04—Other machines for working soil not specially adapted for working soil on which crops are growing with non-rotating tools drawn by animal or tractor or man-power
    • A01B35/08—Other machines for working soil not specially adapted for working soil on which crops are growing with non-rotating tools drawn by animal or tractor or man-power with rigid tools
    • 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/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C9/00—Measuring inclination, e.g. by clinometers, by levels
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Automation & Control Theory (AREA)
  • Zoology (AREA)
  • Mechanical Engineering (AREA)
  • Soil Sciences (AREA)
  • Environmental Sciences (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Operation Control Of Excavators (AREA)

Abstract

The embodiment of the invention provides a navigation method and a system of a land scraper, wherein the method comprises the following steps: determining a navigation planning path of the land leveler according to the terrain information of the farmland to be leveled and based on the working distance required by the land leveler; acquiring real-time state data of the running of the land scraper, and acquiring a real-time error between the running path of the land scraper and the navigation planning path according to the real-time state data; and generating a control parameter according to the real-time error, and realizing navigation of the land leveler based on the control parameter. The real-time error between the real-time state data of the running of the grader and the navigation planning path data is calculated, and the control parameters of the grader are further adjusted according to the real-time error, so that the running state of the grader is controlled and navigated.

Description

Navigation method and system of land leveler
Technical Field
The invention relates to the field of automatic navigation, in particular to a land leveler navigation method and a land leveler navigation system.
Background
The automatic navigation of agricultural machinery is an important component in the field of modern intelligent agricultural machinery, and has wide application in the aspects of automatic pesticide spraying, intertillage weeding, rice transplanting and planting, crop harvesting, automatic vehicle walking and the like. The key technologies of the automatic navigation system mainly include environment perception (positioning), path planning and automatic control.
RTK-GNSS, machine vision, lidar and multi-sensor fusion technologies are currently common automatic navigation systems. The main information acquisition mode of adoption includes GNSS farmland leveling system, and along with the continuous improvement of GNSS precision and the decline of application cost, the application of GNSS navigation technique in the aspect of agricultural intelligent robot and agricultural vehicle automatic positioning navigation will be more extensive.
The farmland leveling technology based on the GNSS (global navigation satellite system) has the advantages of high operation speed and high positioning precision, is not influenced by external factors such as strong light, strong wind and the like, and is suitable for land leveling operation of large-area farmland.
On the basis, the automatic navigation technology is applied to a GNSS farmland leveling system, so that the land leveling operation has scientific path guidance. And the controller automatically controls the vehicle and the agricultural implement, so that the operation time of workers can be greatly reduced, the working efficiency is improved, and the controller has high research and development values.
However, in the current land leveling machine, since a traction type connection method is adopted between the land leveling blade and the tractor in the process of leveling a farmland by using the unmanned tractor, a phenomenon that the towed land leveling blade does not travel to a planned path when the tractor in the land leveling machine travels to the planned path occurs, and the problems that the land leveling blade and the tractor cannot be completely synchronized in the land leveling process and the land leveling precision is low are caused.
Disclosure of Invention
The embodiment of the invention provides a land leveling machine navigation method and system, which are used for solving the problems that a land leveling shovel and a tractor in a land leveling machine cannot be completely synchronized in a land leveling process and land leveling precision is low in the prior art.
In a first aspect, an embodiment of the present invention provides a motorgrader navigation method, including:
determining a navigation planning path of the land leveler according to the terrain information of the farmland to be leveled and based on the working distance required by the land leveler;
acquiring real-time state data of the running of the land scraper, and acquiring a real-time error between the running path of the land scraper and the navigation planning path according to the real-time state data;
and generating a control parameter according to the real-time error, and realizing navigation of the land leveler based on the control parameter.
Optionally, the acquiring, according to the real-time status data, a real-time error between the motorgrader travel path and the navigation planning path specifically includes:
establishing a kinematic model equation set according to the real-time state data of the land scraper in running;
and acquiring a real-time error between the running path of the land scraper and the navigation planning path according to the kinematic model equation set and the data of the target planning path.
Optionally, the kinematic model equation set is:
Figure BDA0002474435740000021
wherein x isFAnd yFIs the space coordinate position of the center point of the rear axle of the land leveling shovel in the land leveling machine, vFIs the speed of travel, θ, of the bladeFIs the heading angle, omega, of the virtual front wheel of the land leveling bladeFIs the angular velocity, delta, of the virtual front wheel heading angle of the land leveling bladeFIs the control steering angle of the virtual front wheel of the land leveling shovel,
Figure BDA0002474435740000022
is the included angle between the tractor and the axle of the land leveling shovel in the land leveling machine, vTIs the running speed of the tractor in the grader, deltaTIs the front wheel corner, L, of the tractor1Is the wheel base L of the front and rear wheel axles of the tractor2Is the distance L between the connection part of the land leveling shovel and the tractor3Is the distance between the connecting hole and the axle of the rear wheel of the land leveling shovel.
Optionally, the obtaining a real-time error between the motorgrader travel path and the navigation planned path according to the kinematic model equation set and the data of the target planned path specifically includes:
calculating a difference value between the space coordinate position of the center point of the axle behind the land leveling shovel and the coordinate position of the land leveling shovel on the navigation planning path as a position difference value;
calculating the difference value between the course angle of the virtual front wheel of the land leveling shovel and the course angle of the land leveling shovel on the navigation planning path as a course angle difference value;
calculating the difference value between the included angle between the tractor and the axle of the land leveling shovel and the corresponding included angle on the navigation planning path as the included angle difference value;
and taking the position difference, the course angle difference and the included angle difference as real-time errors between the running path of the land scraper and the navigation planning path.
Optionally, the generating a control parameter according to the real-time error, and based on the control parameter, implementing navigation of the grader specifically includes:
establishing a navigation error equation set according to the real-time error and the kinematics model equation set;
verifying the stability condition of the navigation error equation set through a Lyapunov function to obtain a control parameter;
and adjusting the numerical value of the control parameter, and controlling the real-time error within a preset range to realize navigation of the land scraper.
Optionally, the navigation error equation set is:
Figure BDA0002474435740000031
wherein x isF eIs the longitudinal error, y, between the grader blade travel path and the navigation plan pathF eIs the lateral error between the grader blade travel path and the navigation plan path, θF eIs the heading error between the grader blade travel path and the navigation plan path,
Figure BDA0002474435740000032
is the included angle error v of the tractor and the axle of the land scraper in the land levelerF dAnd vT dThe speed of travel, x, of the flat shovel and tractor, respectively, to the planned path positioneIs the transverse error y between the driving wheel of the flat forklift and the planned patheIs the longitudinal error, C(s), between the running wheels of the flatbed forklift and the planned pathF) And C (C)ST) The slope of the land scraper and tractor, respectively, to the planned path position.
Optionally, verifying the stability condition of the navigation error equation set through the Lyapunov function to obtain a control parameter, specifically including:
the Lyapunov function is derived based on the same running speeds of a land leveling shovel and a tractor in the land leveling machine;
calculating a travel speed of the tractor and an angular speed of a front wheel steering angle when a derivative of the Lyapunov function is less than zero;
and taking the running speed of the tractor and the angular speed of the front wheel corner as control parameters.
Optionally, the control parameter is:
Figure BDA0002474435740000041
wherein v isTIs the running speed, omega, of the tractor in the graderTIs the angular velocity of the tractor front wheel corner.
In a second aspect, an embodiment of the present invention provides a navigation system of a grader, including:
the system comprises an antenna module, a steering wheel rotating platform, a core processing platform, an angle sensor and a navigation controller;
the antenna module is used for acquiring the direction of the land leveling shovel, measuring spatial position data and course speed data in the running process of the land leveling shovel in real time and measuring spatial position data and course speed data in the running process of the tractor in real time;
the core processing platform is used for receiving the spatial position data and the course speed data of the flat shovel and the tractor and the corner information of the angle sensor, which are sent by the antenna module, processing the spatial position data and the course speed data and sending an instruction to the navigation controller;
the steering wheel rotating platform is arranged in the tractor body and used for realizing automatic adjustment of the heading of the tractor;
the navigation controller is arranged in a tractor cab and used for controlling the rotation of the steering wheel rotating platform according to the instruction of the core processing platform;
the angle sensor is arranged on a fender of a front wheel of the tractor and used for collecting the information of the front wheel turning angle of the tractor and feeding back the information to the core processing platform in real time.
Optionally, the core processing platform includes:
the planning module is used for determining a navigation planning path of the land leveler according to the terrain information of the farmland to be leveled and based on the working distance required by the land leveler;
the calculation module is used for acquiring real-time state data of the running of the land scraper and acquiring a real-time error between the running path of the land scraper and the navigation planning path according to the real-time state data;
and the navigation module is used for generating control parameters according to the real-time errors and realizing navigation of the land leveler based on the control parameters.
According to the land leveler navigation method and the system, the running real-time state data of the land leveler are obtained, the real-time error between the running real-time state data of the land leveler and the navigation planning path data is calculated, and the control parameters of the land leveler are further adjusted according to the real-time error, so that the running state of the land leveler is controlled and navigated, the land leveler and the tractor in the land leveler are synchronized in the land leveling operation and the navigation process, and the land leveling operation precision is improved.
Drawings
In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings used in the description of the embodiments or the prior art will be briefly described below, and it is obvious that the drawings in the following description are some embodiments of the present invention, and those skilled in the art can also obtain other drawings according to the drawings without creative efforts.
Fig. 1 is a schematic flow chart of a navigation method of a grader according to an embodiment of the present invention;
FIG. 2 is a schematic illustration of a grader path provided in accordance with an embodiment of the present invention;
FIG. 3 is a schematic structural view of a grader during driving according to an embodiment of the present disclosure;
FIG. 4 is a schematic structural diagram of a core processing platform of a grader, according to an embodiment of the present invention;
FIG. 5 is a schematic structural diagram of a navigation system of a grader provided in an embodiment of the present invention;
FIG. 6 is a schematic structural diagram of another core processing platform of a grader, according to an embodiment of the present invention;
501, a tractor; 502. a directional antenna; 503. a steering wheel rotation platform; 504. a core processing platform; 505. an angle sensor; 506. a radio station antenna; 507. positioning an antenna; 508. a navigation controller; 509. a power supply; 510. a hydraulic system; 511. an inertial sensor; 512. leveling the land; 513. a base station; 514. a tension sensor.
Detailed Description
In order to make the objects, technical solutions and advantages of the embodiments of the present invention clearer, the technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the drawings in the embodiments of the present invention, and it is obvious that the described embodiments are some, but not all, embodiments of the present 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, a motorgrader navigation method provided in an embodiment of the present invention includes:
determining a navigation planning path of the land leveler according to the terrain information of the farmland to be leveled and based on the working distance required by the land leveler;
acquiring real-time state data of the running of the land scraper, and acquiring a real-time error between the running path of the land scraper and the navigation planning path according to the real-time state data;
and generating a control parameter according to the real-time error, and realizing navigation of the land leveler based on the control parameter.
Specifically, before the navigation planning path of the land scraper is determined, the terrain information inside the farmland is obtained. Position and elevation information inside a farmland are obtained through a GNSS positioning and directional antenna on the land leveling shovel, and the obtained data are transmitted into a navigation device through a communication port. Dividing the interior of the farmland into a plurality of plots with the same area according to boundary information of the farmland, wherein the farmland areas occupied by different plots are the same, and obtaining the earth excavation and filling amount in the plots by an earth excavation and filling amount volume calculation method. And then, planning a flat ground navigation path by combining terrain information in the farmland, dividing the planned path of a farmland working plot into two modes of boundary-to-boundary and line-changing driving, and dividing the path planning of a land head steering area into three modes of RLR, arc and RSR.
As shown in fig. 2, the paths of the farmland working land are divided into two types: the path 201 is a straight-line planned path from one side boundary of the working area to the other side boundary, and the land leveler can dig and fill earthwork in a line of blocks in the working area in the farmland according to the path; the path 204 is a planned route for changing rows, i.e., a planned route for the grader to move from one block to another while traveling within the work area, which is used for the rational removal of earth volume within a field with unevenly distributed terrain.
The planned routes of the land head turning area are divided into three types, the line spacing of the land head turning area is different according to the difference of the land head line changing driving of the land leveler, the working area S of the land is (iW) x (jL), the minimum turning radius of the tractor is r, wherein i is the line number of the land blocks divided in the land working area, j is the column number of the land blocks divided in the land working area, W is the width of one land block, and L is the length of one land block. When the land-head line-changing interval (for convenient calculation, the land-head line-changing point is set as the middle point of the land block) iw < R, the land-head steering path 202 is L (left arc) R (right arc) L (left arc) or RLR type in the Dubins route; when the land leveling machine land line changing distance iw is equal to r, the land turning path is an arc path with the radius of r (at this time, the limit condition that only one large arc in the middle of the path 202 is left can be considered); when the grader ground headway interval iw > r, the ground turn path 204 is of the LS (straight line) L or RSR type in the Dubins route.
For path 201: and calculating a path from one side boundary of the working area to the other side boundary of the working area in the working area. Establishing a coordinate system between spaces, and calculating a linear equation ax + by + c of the farmland boundary BC to be 0 according to the data of the terrain information module, wherein the actual navigation path is as follows:
Figure BDA0002474435740000071
calculating the line-changing path in the working land parcel of the path 204, and respectively obtaining the central point (x) of the initial land parcel of the line-changing path after the global planning path1，y1) And a center point (x) of the ending parcel2，y2) According to the two-point available navigation path:
(y2-y1)x-(x2-x1)y+x2y1-x1y2＝0
calculating the paths 202 and 203 of the turning area of the ground head to mainly obtain the position of the circle center, and when iw is less than r, setting the coordinate of the turning starting point as
Figure BDA0002474435740000072
The coordinates of the end point are
Figure BDA0002474435740000073
The positions of the three circle centers are respectively
Figure BDA0002474435740000074
And
Figure BDA0002474435740000075
when iw is equal to r, the coordinate of the center of the circle is
Figure BDA0002474435740000076
When iw is greater than r, the coordinate of the circle center is
Figure BDA0002474435740000077
And
Figure BDA0002474435740000081
if the boundary of the grader start of operation is AB: bx-ay + m is 0, the equation of the straight line tangent to the two arcs is:
Figure BDA0002474435740000082
the real-time state data of the running of the land leveler can be obtained through various receiving and positioning antennas arranged on the land leveler, and the real-time state data comprises the space coordinate position of the central point of a rear axle of the land leveler, the course angle size and the angular speed of a virtual front wheel of the land leveler, the control steering angle of the virtual front wheel of the land leveler and the like; and calculating the real-time error between the running path of the land scraper and the navigation planning path according to the difference between the real-time state data and the data value of the planning path.
Verifying the stability condition of the navigation error equation set through a Lyapunov function according to the real-time error between the running path of the land leveler and the navigation planning path to obtain a control parameter; by adjusting the values of the control parameters, control and navigation of the grader can be achieved.
As an embodiment of the present invention, the acquiring a real-time error between the motor grader travel path and the navigation planning path according to the real-time status data specifically includes:
establishing a kinematic model equation set according to the real-time state data of the land scraper in running;
and acquiring a real-time error between the running path of the land scraper and the navigation planning path according to the kinematic model equation set and the data of the target planning path.
Specifically, in order to calculate the real-time error between the running path of the grader and the navigation planned path, a kinematic equation set needs to be established according to the real-time state data in the running process of the grader, and the error between the value of the real-time state data and the data value of the navigation state of the grader in the navigation planned path is obtained according to the state data value of the navigation of the grader in the navigation planned path, and is used as the real-time error between the running path of the grader and the navigation planned path.
As an embodiment of the present invention, the kinematic model equation set is:
Figure BDA0002474435740000091
wherein x isFAnd yFIs the space coordinate position of the center point of the rear axle of the land leveling shovel in the land leveling machine, vFIs the speed of travel, θ, of the bladeFIs the heading angle, omega, of the virtual front wheel of the land leveling bladeFIs the angular velocity, delta, of the virtual front wheel heading angle of the land leveling bladeFIs the control steering angle of the virtual front wheel of the land leveling shovel,
Figure BDA0002474435740000092
is the included angle between the tractor and the axle of the land leveling shovel in the land leveling machine, vTIs the running speed of the tractor in the grader, deltaTIs the front wheel corner of the tractor, as shown in fig. 3, L1Is the wheel base L of the front and rear wheel axles of the tractor2Is the distance L between the connection part of the land leveling shovel and the tractor3Is the distance between the connecting hole and the axle of the rear wheel of the land leveling shovel.
Specifically, the kinematic model equation set may be established differently according to different correspondences of the acquired data values, and the kinematic model equation set established according to the spatial coordinate position of the central point of the rear axle of the land leveling blade in the grader, the heading angle of the virtual front wheel of the land leveling blade, and the included angle between the tractor and the shaft of the land leveling blade is provided in this embodiment.
As an embodiment of the present invention, the acquiring a real-time error between the running path of the grader and the navigation planned path according to the kinematic model equation set and the data of the target planned path specifically includes:
calculating a difference value between the space coordinate position of the center point of the axle behind the land leveling shovel and the coordinate position of the land leveling shovel on the navigation planning path as a position difference value;
calculating the difference value between the course angle of the virtual front wheel of the land leveling shovel and the course angle of the land leveling shovel on the navigation planning path as a course angle difference value;
calculating the difference value between the included angle between the tractor and the axle of the land leveling shovel and the corresponding included angle on the navigation planning path as the included angle difference value;
and taking the position difference, the course angle difference and the included angle difference as real-time errors between the running path of the land scraper and the navigation planning path.
The evaluation standard of the real-time error between the running path of the land scraper and the navigation planning path comprises the steps of calculating the difference value of the space coordinate position of the central point of the axle of the land scraper and the coordinate position of the land scraper on the navigation planning path; calculating the difference value of the course angle of the virtual front wheel of the land leveling shovel and the course angle of the land leveling shovel on the navigation planning path; and calculating the difference value of the included angle between the tractor and the flat shovel axle and the corresponding included angle on the navigation planning path. And taking the difference value as a real-time error between the running path of the land scraper and the navigation planning path.
As an embodiment of the present invention, the generating a control parameter according to the real-time error, and implementing navigation of the grader based on the control parameter specifically includes:
establishing a navigation error equation set according to the real-time error and the kinematics model equation set;
verifying the stability condition of the navigation error equation set through a Lyapunov function to obtain a control parameter;
and adjusting the numerical value of the control parameter, and controlling the real-time error within a preset range to realize navigation of the land scraper.
Specifically, the stability condition of the navigation error equation set is verified through a Lyapunov function by combining the navigation error equation set, so that control parameters are obtained, and a nonlinear navigation controller for controlling the running speed and the steering angle of the land leveller is designed. The Lyapunov function V is chosen as:
Figure BDA0002474435740000101
the function is derived:
Figure BDA0002474435740000102
during the land leveling operation of the tractor by the flat shovel, the navigation control input quantity mainly changes the motion state of the tractor, and the running speeds of the flat shovel and the tractor can be regarded as equal in numerical value in the running process of the combined system, namely vF＝vT，
Figure BDA0002474435740000103
The derivative of the Lyapunov function V can finally be converted into:
Figure BDA0002474435740000104
further, when the control input amount v is inputTAnd ωTThe values of (A) are as follows:
Figure BDA0002474435740000105
Figure BDA0002474435740000111
wherein k is a constant (k)>0) At this time, the derivative function
Figure BDA0002474435740000112
The system is stabilized by adjusting the input vTAnd ωTThe value of (2) enables the system to keep stable, and realizes the control and navigation of the land scraper.
As an embodiment of the present invention, the navigation error equation set is:
Figure BDA0002474435740000113
wherein x isF eIs the longitudinal error, y, between the grader blade travel path and the navigation plan pathF eIs the lateral error between the grader blade travel path and the navigation plan path, θF eIs the heading error between the grader blade travel path and the navigation plan path,
Figure BDA0002474435740000114
is the included angle error v of the tractor and the axle of the land scraper in the land levelerF dAnd vT dThe speed of travel, x, of the flat shovel and tractor, respectively, to the planned path positioneIs the transverse error y between the driving wheel of the flat forklift and the planned patheIs the longitudinal error, C(s), between the running wheels of the flatbed forklift and the planned pathF) And C (C)ST) The slope of the land scraper and tractor, respectively, to the planned path position.
Specifically, the front wheel steering angle control amount of the tractor is:
Figure BDA0002474435740000115
the path planning method and the navigation control method are combined in the navigation process, and the navigation error can be obtained. When the navigation tracking path is a linear path in a working land block or a land head turning area, the course error theta of the tractor and the flat shovel in the driving processeComprises the following steps:
Figure BDA0002474435740000116
the GNSS positioning and orienting antenna can measure the course angle theta of the flat ground shovel and the tractor, and can measure the space position coordinates (x, y) of the flat ground shovel and the tractor in real time, so that the transverse error d of the tractor and the flat ground shovel in the driving process can be calculated:
Figure BDA0002474435740000117
included angle error between tractor and flat shovel
Figure BDA0002474435740000118
Is the target included angle when the two reach the planning path
Figure BDA0002474435740000121
Measuring the included angle with GNSS in real time
Figure BDA0002474435740000122
According to the spatial angle relationship, the calculation formula of the included angle error is as follows:
Figure BDA0002474435740000123
when the path tracked by navigation is a curve path of a ground head steering area, the course error theta of the tractor and the flat shovel in the driving processeComprises the following steps:
Figure BDA0002474435740000124
the transverse error d of the tractor and the flat shovel in the driving process is as follows:
Figure BDA0002474435740000125
wherein (P)n,Qn) The coordinates of the dots calculated for the path plan, R is the radius of the corresponding circle. Similarly, the included angle error between the tractor and the land leveling shovel
Figure BDA0002474435740000126
The calculation formula of (2) is as follows:
Figure BDA0002474435740000127
as an embodiment of the present invention, the verifying the stability condition of the navigation error equation set by the Lyapunov function to obtain the control parameter specifically includes:
the driving speeds of a land leveling shovel and a tractor in the land leveling machine are the same, and a Lyapunov function is derived;
calculating a travel speed of the tractor and an angular speed of a front wheel steering angle when a derivative of the Lyapunov function is less than zero;
and taking the running speed of the tractor and the angular speed of the front wheel corner as control parameters.
Specifically, the Lyapunov function V is chosen as:
Figure BDA0002474435740000128
the function is derived:
Figure BDA0002474435740000129
during the land leveling operation of the tractor by the flat shovel, the navigation control input quantity mainly changes the motion state of the tractor, and the running speeds of the flat shovel and the tractor can be regarded as equal in numerical value in the running process of the combined system, namely vF＝vT，
Figure BDA00024744357400001210
The derivative of the Lyapunov function V can finally be converted into:
Figure BDA00024744357400001211
the travel speed of the tractor and the angular speed of the front wheel steering angle when the derivative of the Lyapunov function is less than zero are calculated as control parameters.
As an embodiment of the present invention, the control parameters are:
Figure BDA0002474435740000131
wherein v isTIs the running speed, omega, of the tractor in the graderTIs the angular velocity of the tractor front wheel corner. Specifically, when the input amount v is controlledTAnd ωTThe values of (A) are as follows:
Figure BDA0002474435740000132
Figure BDA0002474435740000133
wherein k is a constant (k)>0) At this time, the derivative function
Figure BDA0002474435740000134
The system is stabilized.
As shown in fig. 5, a schematic structural diagram of a motorgrader navigation system provided for the embodiment includes: an antenna module, a steering wheel rotating platform 503, a core processing platform 504, an angle sensor 505, and a navigation controller 508;
the antenna module includes: the directional antenna 502 is fixedly arranged at the joint of the tractor 501 and the flat ground shovel 512 and is used for measuring the direction of the flat ground shovel 512; the positioning antenna 507 is fixedly arranged above a scraper knife of the land leveling shovel 512 and is used for measuring spatial position data and course speed data of the land leveling shovel 512 in the driving process in real time; the radio antenna 506 is arranged in the body of the tractor 501 and is used for measuring spatial position data and course speed data in the running process of the tractor 501 in real time;
the core processing platform 504 is configured to receive information sent by the antenna module, process the information, and send an instruction to the navigation controller 508;
the steering wheel rotating platform 503 is arranged in the body of the tractor 501 and is used for realizing automatic adjustment of the heading of the tractor 501;
the navigation controller 508 is disposed in the cab of the tractor 501, and is configured to control the rotation of the steering wheel rotation platform 503 according to the instruction of the core processing platform 504;
the angle sensor 505 is arranged on a fender of a front wheel of the tractor 501 and used for collecting the information of the front wheel rotation angle of the tractor 501 and feeding back the information to the core processing platform 504 in real time.
Specifically, the antenna module and the angle sensor 505 are both connected to the core processing platform 504, the internal structure of the core processing platform 504 is as shown in fig. 4, and the core processing platform 504 can acquire real-time data sent by the antenna module and the angle sensor 505; the core processing platform 504 is connected with the navigation controller 508 and can send instructions to control the navigation controller 508; the navigation controller 508 is connected to the steering wheel selection platform 503, and the navigation controller 508 controls the direction and speed of the grader by controlling the steering wheel selection platform 503 according to instructions from the core processing platform 504.
As an embodiment of the present invention, the core processing platform 504 includes:
the planning module 601 is used for determining a navigation planning path of the land leveler according to the terrain information of the farmland to be leveled and based on the working distance required by the land leveler;
a calculating module 602, configured to obtain real-time state data of the grader, and obtain a real-time error between the grader running path and the navigation planning path according to the real-time state data;
and a navigation module 603, configured to generate a control parameter according to the real-time error, and implement navigation on the grader based on the control parameter.
Specifically, as shown in fig. 6, another structural schematic diagram of the core processing platform is shown, wherein the planning module 601 is configured to determine a navigation planning path of the grader based on that the minimum working distance required by the grader is the shortest according to the terrain information of the farmland to be leveled received by the antenna module. When the grader runs along the planned path, the technical module 602 acquires real-time state data of the running of the grader in real time through the antenna module, and calculates a real-time error between the running path of the grader and the navigation planned path according to the real-time state data in order to ensure that the error is kept within a controllable range in the running process of the grader. Finally, the navigation module 603 generates control parameters for adjusting the running and working states of the grader, mainly the running speed and direction, according to the real-time errors, thereby ensuring that the grader can run and work within an error range according to the planned path.
In addition, the system further comprises: a hydraulic system 510, an inertial sensor 511, a tension sensor 514, and a power supply 509. The hydraulic system 510 is arranged at the front end of a shovel blade of the flat shovel 512 and is used for controlling the lifting of the flat shovel 512 according to an instruction sent by the core processing platform 504; the inertial sensor 511 is arranged on the surface of the flat ground shovel 512, the tension sensor 514 is arranged in a connecting hole between the flat ground shovel 512 and the tractor 501, and the inertial sensor 511 and the tension sensor 514 are used for receiving corresponding information and transmitting the information to the core processing platform 504; the power supply system 509 is used to supply power to the navigation system of the grader in this embodiment.
The above-described embodiments of the apparatus are merely illustrative, and the units described as separate parts may or may not be physically separate, and parts displayed as units may or may not be physical units, may be located in one place, or may be distributed on a plurality of network units. Some or all of the modules may be selected according to actual needs to achieve the purpose of the solution of the present embodiment. One of ordinary skill in the art can understand and implement it without inventive effort.
Through the above description of the embodiments, those skilled in the art will clearly understand that each embodiment can be implemented by software plus a necessary general hardware platform, and certainly can also be implemented by hardware. With this understanding in mind, the above-described technical solutions may be embodied in the form of a software product, which can be stored in a computer-readable storage medium such as ROM/RAM, magnetic disk, optical disk, etc., and includes instructions for causing a computer device (which may be a personal computer, a server, or a network device, etc.) to execute the methods described in the embodiments or some parts of the embodiments.
Finally, it should be noted that: the above examples are only intended to illustrate the technical solution of the present invention, but not to limit it; although the present invention has been described in detail with reference to the foregoing embodiments, it will be understood by those of ordinary skill in the art that: the technical solutions described in the foregoing embodiments may still be modified, or some technical features may be equivalently replaced; and such modifications or substitutions do not depart from the spirit and scope of the corresponding technical solutions of the embodiments of the present invention.

Claims (6)

1. A method of motorgrader navigation, comprising:
determining a navigation planning path of the land leveler according to the terrain information of the farmland to be leveled and based on the working distance required by the land leveler;
acquiring real-time state data of the running of the land scraper, establishing a kinematics model equation set according to the real-time state data, and acquiring a real-time error between a running path of the land scraper and a navigation planning path according to the kinematics model equation set and data of a target planning path;
generating a control parameter according to the real-time error, and realizing navigation of the land leveler based on the control parameter;
wherein the kinematic model equation set is:
Figure FDA0003399514190000011
wherein x isFAnd yFIs the space coordinate position of the center point of the rear axle of the land leveling shovel in the land leveling machine, vFIs the speed of travel, θ, of the bladeFIs the heading angle, omega, of the virtual front wheel of the land leveling bladeFIs the angular velocity, delta, of the virtual front wheel heading angle of the land leveling bladeFIs the control steering angle of the virtual front wheel of the land leveling shovel,
Figure FDA0003399514190000012
is the included angle between the tractor and the axle of the land leveling shovel in the land leveling machine, vTIs the running speed of the tractor in the grader, deltaTIs the front wheel corner, L, of the tractor1Is the wheel base L of the front and rear wheel axles of the tractor2Is the distance L between the connection part of the land leveling shovel and the tractor3The distance between the connecting hole and the axle of the rear wheel of the land leveling shovel;
and acquiring a real-time error between the running path of the land scraper and the navigation planning path according to the kinematic model equation set and the data of the target planning path, wherein the real-time error specifically comprises the following steps:
calculating a difference value between the space coordinate position of the center point of the axle behind the land leveling shovel and the coordinate position of the land leveling shovel on the navigation planning path as a position difference value;
calculating the difference value between the course angle of the virtual front wheel of the land leveling shovel and the course angle of the land leveling shovel on the navigation planning path as a course angle difference value;
calculating the difference value between the included angle between the tractor and the axle of the land leveling shovel and the corresponding included angle on the navigation planning path as the included angle difference value;
and taking the position difference, the course angle difference and the included angle difference as real-time errors between the running path of the land scraper and the navigation planning path.
2. The method according to claim 1, wherein the generating of the control parameter according to the real-time error and the navigation of the grader based on the control parameter includes:
establishing a navigation error equation set according to the real-time error and the kinematics model equation set;
verifying the stability condition of the navigation error equation set through a Lyapunov function to obtain a control parameter;
and adjusting the numerical value of the control parameter, and controlling the real-time error within a preset range to realize navigation of the land scraper.
3. The method of claim 2, wherein the system of navigation error equations is:
Figure FDA0003399514190000021
wherein x isF eIs the longitudinal error, y, between the grader blade travel path and the navigation plan pathF eIs the lateral error between the grader blade travel path and the navigation plan path, θF eIs the heading error between the grader blade travel path and the navigation plan path,
Figure FDA0003399514190000022
is the included angle error v of the tractor and the axle of the land scraper in the land levelerF dAnd vT dThe speed of travel, x, of the flat shovel and tractor, respectively, to the planned path positioneIs the transverse error y between the driving wheel of the flat forklift and the planned patheIs the longitudinal error, C(s), between the running wheels of the flatbed forklift and the planned pathF) And C (C)ST) The slope of the land scraper and tractor, respectively, to the planned path position.
4. The method according to claim 2, wherein the verifying the stability condition of the navigation error equation set by the Lyapunov function to obtain the control parameter specifically comprises:
the Lyapunov function is derived based on the same running speeds of a land leveling shovel and a tractor in the land leveling machine;
calculating a travel speed of the tractor and an angular speed of a front wheel steering angle when a derivative of the Lyapunov function is less than zero;
and taking the running speed of the tractor and the angular speed of the front wheel corner as control parameters.
5. The method of claim 2, wherein the control parameters are:
Figure FDA0003399514190000031
wherein v isTIs the running speed, omega, of the tractor in the graderTIs the angular velocity, x, of the tractor front wheel cornerF eIs the longitudinal error, y, between the grader blade travel path and the navigation plan pathF eIs the land leveling shovel in the land leveling machineA lateral error between a driving path and the navigation plan path,
Figure FDA0003399514190000032
is the included angle error v of the tractor and the axle of the land scraper in the land levelerF dAnd vT dThe travel speeds, C(s), of the flatshovel and the tractor, respectively, to the planned path positionF) And C (C)ST) The slopes, θ, of the flat blade and tractor, respectively, to the planned path positionF eIs the course error between the land leveling shovel driving path and the navigation planning path in the land leveling machine, k is a constant, and k is>0。
6. A grader navigation system comprising:
the system comprises an antenna module, a steering wheel rotating platform, a core processing platform, an angle sensor and a navigation controller;
the antenna module is used for acquiring the direction of the land leveling shovel, measuring spatial position data and course speed data in the running process of the land leveling shovel in real time and measuring spatial position data and course speed data in the running process of the tractor in real time;
the core processing platform is used for receiving the spatial position data and the course speed data of the flat shovel and the tractor and the corner information of the angle sensor, which are sent by the antenna module, processing the spatial position data and the course speed data and sending an instruction to the navigation controller;
the steering wheel rotating platform is arranged in the tractor body and used for realizing automatic adjustment of the heading of the tractor;
the navigation controller is arranged in a tractor cab and used for controlling the rotation of the steering wheel rotating platform according to the instruction of the core processing platform;
the angle sensor is arranged on a fender of a front wheel of the tractor and used for acquiring the information of the front wheel turning angle of the tractor and feeding the information back to the core processing platform in real time;
wherein the core processing platform comprises:
the planning module is used for determining a navigation planning path of the land leveler according to the terrain information of the farmland to be leveled and based on the working distance required by the land leveler;
the computing module is used for acquiring real-time state data of the running of the land leveler and establishing a kinematic model equation set according to the real-time state data; acquiring a real-time error between the running path of the land scraper and the navigation planning path according to the kinematic model equation set and the data of the target planning path;
the navigation module is used for generating control parameters according to the real-time errors and realizing navigation of the land leveler based on the control parameters;
wherein the kinematic model equation set is:
Figure FDA0003399514190000041
wherein x isFAnd yFIs the space coordinate position of the center point of the rear axle of the land leveling shovel in the land leveling machine, vFIs the speed of travel, θ, of the bladeFIs the heading angle, omega, of the virtual front wheel of the land leveling bladeFIs the angular velocity, delta, of the virtual front wheel heading angle of the land leveling bladeFIs the control steering angle of the virtual front wheel of the land leveling shovel,
Figure FDA0003399514190000042
is the included angle between the tractor and the axle of the land leveling shovel in the land leveling machine, vTIs the running speed of the tractor in the grader, deltaTIs the front wheel corner, L, of the tractor1Is the wheel base L of the front and rear wheel axles of the tractor2Is the distance L between the connection part of the land leveling shovel and the tractor3The distance between the connecting hole and the axle of the rear wheel of the land leveling shovel;
and acquiring a real-time error between the running path of the land scraper and the navigation planning path according to the kinematic model equation set and the data of the target planning path, wherein the real-time error specifically comprises the following steps:
calculating a difference value between the space coordinate position of the center point of the axle behind the land leveling shovel and the coordinate position of the land leveling shovel on the navigation planning path as a position difference value;
calculating the difference value between the course angle of the virtual front wheel of the land leveling shovel and the course angle of the land leveling shovel on the navigation planning path as a course angle difference value;
calculating the difference value between the included angle between the tractor and the axle of the land leveling shovel and the corresponding included angle on the navigation planning path as the included angle difference value;
and taking the position difference, the course angle difference and the included angle difference as real-time errors between the running path of the land scraper and the navigation planning path.
CN202010359054.2A 2020-04-29 2020-04-29 Navigation method and system of land leveler Active CN111578942B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010359054.2A CN111578942B (en) 2020-04-29 2020-04-29 Navigation method and system of land leveler

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010359054.2A CN111578942B (en) 2020-04-29 2020-04-29 Navigation method and system of land leveler

Publications (2)

Publication Number Publication Date
CN111578942A CN111578942A (en) 2020-08-25
CN111578942B true CN111578942B (en) 2022-04-05

Family

ID=72110186

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010359054.2A Active CN111578942B (en) 2020-04-29 2020-04-29 Navigation method and system of land leveler

Country Status (1)

Country Link
CN (1) CN111578942B (en)

Families Citing this family (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112627261B (en) * 2020-11-19 2022-06-14 徐州徐工筑路机械有限公司 Shovel blade attitude control system and method based on machine vision and land leveler
CN112650221B (en) * 2020-12-03 2021-12-03 广州极飞科技股份有限公司 Flat ground path generation method, flat ground path generation device, processing equipment and storage medium
CN113393656A (en) * 2021-06-10 2021-09-14 徐州徐工筑路机械有限公司 Unmanned system of land leveler
CN113341985B (en) * 2021-06-16 2023-09-22 北京博创联动科技有限公司 High-quality operation rice transplanter and navigation control method thereof
CN115060265B (en) * 2022-05-27 2024-05-28 华南农业大学 Positioning device for farmland navigation and positioning method thereof

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US5806016A (en) * 1996-03-28 1998-09-08 Caterpillar Inc. Method for determining the course of a machine
CN106502252A (en) * 2016-12-05 2017-03-15 聊城大学 The tractor navigation control system of Multi-sensor Fusion and its positioning, control method
CN107045347A (en) * 2017-01-22 2017-08-15 无锡卡尔曼导航技术有限公司 For agricultural machinery unpiloted automatic turn around path planning and its control method
CN107544520A (en) * 2017-10-25 2018-01-05 上海联适导航技术有限公司 A kind of control method for four-wheel carrier automatic Pilot
CN108919792A (en) * 2018-05-30 2018-11-30 华南农业大学 A kind of automated navigation system path planning control method
CN110203275A (en) * 2019-04-09 2019-09-06 丰疆智能科技股份有限公司 Driving path control method for tractor

Family Cites Families (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104019815B (en) * 2014-06-04 2017-01-25 中国农业大学 GNSS (Global Navigation Satellite System) ground operation path dynamic planning and navigation method based on forklift load monitoring
CN108168560B (en) * 2017-12-27 2021-06-08 沈阳智远弘业机器人有限公司 Composite navigation control method for omnidirectional AGV

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US5806016A (en) * 1996-03-28 1998-09-08 Caterpillar Inc. Method for determining the course of a machine
CN106502252A (en) * 2016-12-05 2017-03-15 聊城大学 The tractor navigation control system of Multi-sensor Fusion and its positioning, control method
CN107045347A (en) * 2017-01-22 2017-08-15 无锡卡尔曼导航技术有限公司 For agricultural machinery unpiloted automatic turn around path planning and its control method
CN107544520A (en) * 2017-10-25 2018-01-05 上海联适导航技术有限公司 A kind of control method for four-wheel carrier automatic Pilot
CN108919792A (en) * 2018-05-30 2018-11-30 华南农业大学 A kind of automated navigation system path planning control method
CN110203275A (en) * 2019-04-09 2019-09-06 丰疆智能科技股份有限公司 Driving path control method for tractor

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
Path Tracking of Orchard Tractor Based on Linear Time-varying Model Predictive Control;Xue, Peilin，Wu Yuan，Yin Guodong etc.;《2019 Chinese Control And Decision Conference (CCDC)》;20190912;全文 *
基于预瞄追踪模型的农机导航路径跟踪控制方法;王辉，王桂民，罗锡文等;《农业工程学报》;20190228;第35卷(第4期);全文 *

Also Published As

Publication number Publication date
CN111578942A (en) 2020-08-25

Similar Documents

Publication Publication Date Title
CN111578942B (en) Navigation method and system of land leveler
US11052943B2 (en) System and method for controlling a vehicle
US11006564B2 (en) Path planning system for autonomous off-road vehicles
US10168714B2 (en) GNSS and optical guidance and machine control
US20180364739A1 (en) Choreographer system for autonomous off-road vehicles
EP3878258B1 (en) Method and system for estimating surface roughness of ground for an off-road vehicle to control ground speed
EP3878257B1 (en) Method and system for estimating surface roughness of ground for an off-road vehicle to control ground speed
US7593798B2 (en) Vehicular guidance system having compensation for variations in ground elevation
BR102021003756A2 (en) Method and system for estimating the surface roughness of a soil
WO2000010063A1 (en) Method and apparatus for determining a path to be traversed by a mobile machine
EP3254547A1 (en) System and method for vehicle steering calibration
US9651381B2 (en) Terrain mapping system using virtual tracking features
Wang et al. Autonomous maneuvers of a robotic tractor for farming
CN110430746A (en) The autonomous driving system of agricultural operation vehicle
JP2020028243A (en) Automatic running system for work vehicle
CN106647770A (en) Field turning path planning and control method used for farm machinery driverless driving
CN113721619B (en) Bulldozer path planning system and operation method based on RTK technology
CN110850407A (en) Terrain measurement method based on radar technology
Bi et al. A positioning system on robot for agricultural environment
RU2794670C1 (en) Road construction machine autonomous control system
CN112698564A (en) Intelligent control method of self-propelled land leveler based on dynamic double-triangle principle
Li et al. Research on human-guided active following mode with 3D spatial relative positioning for vehicles in hilly and mountainous orchards
CN118466484A (en) Unmanned carrying equipment, control method, device and medium thereof
CN117873054A (en) Unmanned method and system for intelligent harvester
JP2023181797A (en) work vehicle

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