CN113359710A - LOS theory-based agricultural machinery path tracking method - Google Patents
LOS theory-based agricultural machinery path tracking method Download PDFInfo
- Publication number
- CN113359710A CN113359710A CN202110561035.2A CN202110561035A CN113359710A CN 113359710 A CN113359710 A CN 113359710A CN 202110561035 A CN202110561035 A CN 202110561035A CN 113359710 A CN113359710 A CN 113359710A
- Authority
- CN
- China
- Prior art keywords
- path
- agricultural machinery
- tracking
- angle
- los
- 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.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 33
- 230000008569 process Effects 0.000 claims abstract description 10
- 230000008859 change Effects 0.000 claims abstract description 5
- 230000004927 fusion Effects 0.000 claims description 3
- 230000010354 integration Effects 0.000 claims description 3
- 238000012360 testing method Methods 0.000 claims description 3
- 230000000007 visual effect Effects 0.000 claims description 3
- 230000007246 mechanism Effects 0.000 claims description 2
- 229920000535 Tan II Polymers 0.000 claims 1
- 238000005516 engineering process Methods 0.000 description 7
- 238000004519 manufacturing process Methods 0.000 description 4
- 230000008901 benefit Effects 0.000 description 3
- 238000013461 design Methods 0.000 description 2
- 238000011161 development Methods 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 238000009313 farming Methods 0.000 description 2
- 230000006872 improvement Effects 0.000 description 2
- 238000004088 simulation Methods 0.000 description 2
- 206010063385 Intellectualisation Diseases 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000005540 biological transmission Effects 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 238000011160 research Methods 0.000 description 1
Images
Classifications
-
- 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/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
- G05D1/0246—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using a video camera in combination with image processing means
-
- 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/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0219—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory ensuring the processing of the whole working surface
-
- 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/0276—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Multimedia (AREA)
- Electromagnetism (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Guiding Agricultural Machines (AREA)
Abstract
The invention relates to an agricultural machinery path tracking method based on LOS theory, which belongs to the field of agricultural machinery intelligent control, wherein when an agricultural machinery runs on a set straight path, a navigation positioning module acquires real-time position information of the agricultural machinery; transmitting the position information to an LOS algorithm device through parameters such as coordinates and transverse tracking errors analyzed by an upper computer system to obtain a current driving course angle; and finally, the front wheel feedback controller outputs the wheel steering angle expected by the system according to the course angle and the transverse tracking error, so that the steering deviation angle of the wheels can be obtained, and the steps are repeated to realize path tracking control. The agricultural machinery system based on the LOS navigation method can be suitable for different agricultural machinery operation scenes, effectively solves the problem of overlarge path tracking error, and has the characteristics of robustness, accuracy and the like. The method has clear and simple thought, and can enable the change of the wheel turning angle of the agricultural machinery in the process of tracking the expected path to be more gradual and stable under the condition of external interference or uncertain system model parameters.
Description
Technical Field
The invention relates to an agricultural machinery path tracking method based on an LOS theory, and belongs to the technical field of intelligent control of agricultural machinery.
Background
China is a big agricultural country, agriculture is the foundation of social and economic development and the guarantee of people's material life, in order to promote the improvement of agricultural productivity and realize the development of agricultural modernization, people put forward the concept of precision agriculture, it mainly utilizes technologies such as navigation satellite positioning technology, sensor technology and remote sensing control to finish the agricultural machinery independently to operate, the automatic navigation purpose, has effectively reduced the farmland farming time, has improved the farming efficiency, and can replace the human labor to realize the autopilot, has alleviated the work fatigue of the driver, has reduced the production accident rate, has improved the production safety.
The agricultural machine path tracking method is a key technology of agricultural machine automatic navigation and is a research hotspot in the technical field of agricultural machine intelligent control. The existing agricultural machinery path tracking technology mainly depends on the navigation positioning technology of an agricultural machinery upper computer system and the accuracy and hydraulic steering of a lower computer system sensor to control, so that the complexity of system design and the dependency of a control model are greatly increased, the production cost is improved, the production benefit is reduced, and the adaptability of the agricultural machinery operation environment is not strong. Therefore, how to optimize a set of path tracking controller with strong robustness and high accuracy on the premise of simplifying the structure of the agricultural machinery automatic navigation system is a key point for realizing agricultural accuracy and intellectualization.
Disclosure of Invention
The invention provides an agricultural machinery path tracking control method based on LOS theory, which can stably and effectively track and control the path of an unmanned agricultural machinery under the condition that system model parameters are uncertain or the working environment has high disturbance, so that the agricultural machinery ensures the stability and the accuracy of a nonlinear feedback controller on the basis of simplifying the structure of an original hardware system.
In order to solve the technical problems, the invention adopts the following technical scheme:
an agricultural machinery path tracking method based on an LOS theory comprises the following steps:
s1, when the agricultural machinery runs on the set straight path, the navigation positioning module acquires the real-time position information of the agricultural machinery;
s2, transmitting the position information to an LOS algorithm device through parameters such as coordinates and transverse tracking errors analyzed by an upper computer system to obtain a current driving course angle;
and S3, outputting the wheel steering angle expected by the system by the front wheel feedback controller according to the course angle and the transverse tracking error, thereby obtaining the steering deviation angle of the wheels, and repeating the steps to realize the path tracking control.
Further, the step S1 specifically includes:
s11, setting the boundary of the agricultural machinery working field block through the Beidou RTK positioning module and the visual recognition CDD module, and then dotting a plurality of parallel linear tracks.
And S12, the Beidou RTK navigation module outputs the position and track point information of the agricultural machine in real time, simultaneously converts the position and track point information and three vertex coordinates of the rectangular field block into a Gaussian plane coordinate system, and stores the planned path into a navigation system in a two-dimensional array form to realize navigation path planning.
Further, the step S11 specifically includes:
determining a rectangle by using an RTK-CCD positioning vision sensor fusion system and measuring three coordinate vertexes of a test field, and assuming a positioning coordinate of a ground node A, B, C, D; taking the longest distance of each operation line as a principle, combining the actual geometric shape of the land, selecting a boundary AD as a reference line of operation line planning, taking the boundary AD as a reference line to define a plurality of parallel operation lines, wherein the distance of the parallel lines is determined according to the operation line distance, the shortest distance between the last operation line and the boundary BC is not less than 1/2 operation distances, taking the focuses of all Q parallel operation lines and the boundaries AB and CD as planning operation line nodes, and storing the corresponding operation line nodes into a two-dimensional array for being called by navigation control software based on a LOS algorithm.
Further, the step S2 specifically includes:
s21: determining a two-way path point
And
the defined straight line traces the path, and the origin of the path coordinate system is at
The coordinate of the agricultural machine in the coordinate system with the fixed path is pn(t)＝[x(t),y(t)]And satisfies the formula:
in the formula:
αkfor the north and the south of the geodetic coordinate system and the expected path PkAnd Pk+1A is a coefficient;
s22: let ε (t) ═ s (t), e (t)]T∈R2Wherein:
s(t)＝[x(t)-xk]cos(αk)+[y(t)-yk]sin(αk)
e(t)＝-[x(t)-xk]sin(αk)+[y(t)-yk]cos(αk)
s (t) is the tracking distance of the agricultural machinery path, e (t) is the transverse tracking error, and in the actual path tracking process, only the transverse deviation of the agricultural machinery running is needed to be concerned, because when the transverse deviation e (t) is 0, the agricultural machinery is converged on the expected tracking path, and the path tracking control target is:
s23: the LOS navigation algorithm is set as follows:
in the formula:
if agricultural machinery works asA running direction angle of
The course angle is then:
further, the step S3 specifically includes:
s31: the method comprises the following steps of acquiring information such as real-time positions, wheel speeds and corners of agricultural machinery through a navigation positioning system and a pose acquisition system respectively, and performing kinematic modeling by combining structural parameters of the agricultural machinery and a two-degree-of-freedom model theory, wherein the kinematic modeling specifically comprises the following steps:
considering the path tracking process of the agricultural machinery as low-speed motion in an X-Y plane coordinate system, the following results are obtained according to the upper triangular chord and the lower triangular chord theorem:
wherein deltaf、δrRespectively a front wheel turning angle and a rear wheel turning angle; lf、lrRespectively a front wheel base and a rear wheel base; beta is the direction angle of the running speed of the agricultural machine; r is a turning radius;
two-way degeneracy is multiplied on both sides
Obtaining:
under the condition of low-speed running working condition of the agricultural machinery, the change rate of the vehicle direction is as follows:
Since the agricultural machinery does not take into account the rear wheel steering situation, i.e. tan (delta)r) When the content is 0, finishing to obtain:
wherein
Is a course angle, v is a running speed, and delta is a front wheel rotation angle;
in a low-speed working environment, assuming that a wheel rotation direction is consistent with a speed direction, that is, δ is β, a kinematic model is:
s32: the front wheel steering angle can be obtained by the steering characteristic of the agricultural machinery:
wherein
The course angle can be obtained by an LOS algorithm; deltae(t) is the steering deviation angle;
the larger the lateral tracking error e (t) and the larger the front wheel steering angle without considering the driving tracking deviation, the following nonlinear proportional function is obtained according to the geometrical relationship, assuming that the expected trajectory of the vehicle intersects with the nearest tangent on a given path from the front wheel extension line d (t):
where d (t) is related to vehicle speed, represented by vehicle speed v (t) and gain parameter k.
Front wheel steering deviation angle is cut off from desired path without considering lateral tracking errorThe line direction is coincident, i.e. the front wheel direction is the same as the desired path direction in the absence of lateral error:
as the lateral tracking error increases, the non-linear proportional function produces a front wheel slip angle that points directly to the desired path, and:
in order to enable the two differential equations to have global gradual stable balance at the zero point error junction, the nonlinear front wheel feedback controller is designed by integrating two control factors as follows:
and from the geometrical relationships:
therefore, it is not only easy to use
Therefore, e (t) has a convergence rate between the linear convergence rate of v (t) and the exponential convergence rate of the gain parameter k, and when the lateral tracking error e (t) is small, (ke (t)/v (t))2Approaching 0, then
e(t)＝e(0)*exp-kt
for any transverse error, the differential equation is monotonically converged to 0, and the path tracking control target is realized.
And further, wheel steering deviation information obtained by the navigation controller is transmitted to an agricultural machinery lower computer system, the real deviation value is corrected by the values of the steering sensor and the photoelectric encoder, and the corrected deviation value is transmitted to a control mechanism hydraulic system to control the flow and the flow direction of the hydraulic system, so that the steering of the wheels is finally controlled, and the vehicle can run according to a set route.
Through the technical scheme, compared with the prior art, the invention has the following beneficial effects:
in order to solve the problems of poor adaptability, complex system design and unstable control of the agricultural machinery path tracking technology, the invention discloses an agricultural machinery path tracking method based on an LOS theory; the course angle tracked by the agricultural machinery path is obtained by using the LOS algorithm, and the tracking path target point can be automatically switched, so that the updating of state parameters such as the transverse tracking error and the like input by the current controller is ensured, the real-time control on the steering is realized, and the expected tracking path can be converged quickly and stably; and the LOS algorithm does not depend on a system parameter model, can output an accurate agricultural machine course angle when external disturbance is large, enhances the adaptability of the agricultural machine operation environment by combining the designed front wheel feedback nonlinear controller, simplifies the whole control system framework, has high robustness and is suitable for agricultural machinery.
Drawings
FIG. 1 is a diagram of a path tracking system of agricultural machinery based on LOS theory;
FIG. 2 is a LOS control algorithm schematic;
FIG. 3 is a view of a kinematic model of an agricultural machine;
FIG. 4 is a simulation comparison of two system control methods; (a) comparing the graph for the expected tracking path and the actual tracking path; (b) the path is traced for the desired trace and based on the LOS method.
Detailed Description
So that the manner in which the features and advantages of the invention, as well as the manner in which the above recited features and functions are attained and can be understood in detail, a more particular description of the invention, briefly summarized above, may be had by reference to the embodiments thereof which are illustrated in the appended drawings.
As shown in a system structure diagram of fig. 1, an agricultural machinery path tracking method based on LOS theory includes the following steps:
s1, when the agricultural machinery runs on the set straight path, the navigation positioning module acquires the real-time position information of the agricultural machinery;
s2, transmitting the position information to an LOS algorithm device through parameters such as coordinates and transverse tracking errors analyzed by an upper computer system to obtain a current driving course angle;
and S3, outputting the wheel steering angle expected by the system by the front wheel feedback controller according to the course angle and the transverse tracking error, thereby obtaining the steering deviation angle of the wheels, and repeating the steps to realize the path tracking control.
The step S1 specifically includes:
s11, setting the boundary of the agricultural machinery working field block through the Beidou RTK positioning module and the visual recognition CDD module, and then dotting a plurality of parallel linear tracks.
And S12, the Beidou RTK navigation module outputs the position and track point information of the agricultural machine in real time, simultaneously converts the position and track point information and three vertex coordinates of the rectangular field block into a Gaussian plane coordinate system, and stores the planned path into a navigation system in a two-dimensional array form to realize navigation path planning.
The step S11 specifically includes:
determining a rectangle by using an RTK-CCD positioning vision sensor fusion system and measuring three coordinate vertexes of a test field, and assuming a positioning coordinate of a ground node A, B, C, D; taking the longest distance of each operation line as a principle, combining the actual geometric shape of the land, selecting a boundary AD as a reference line of operation line planning, taking the boundary AD as a reference line to define a plurality of parallel operation lines, wherein the distance of the parallel lines is determined according to the operation line distance, the shortest distance between the last operation line and the boundary BC is not less than 1/2 operation distances, taking the focuses of all Q parallel operation lines and the boundaries AB and CD as planning operation line nodes, and storing the corresponding operation line nodes into a two-dimensional array for being called by navigation control software based on a LOS algorithm.
As shown in fig. 2, based on the LOS navigation algorithm processing procedure, step S2 specifically includes:
s21: determining a two-way path point
And
the defined straight line traces the path, and the origin of the path coordinate system is at
The coordinate of the agricultural machine in the coordinate system with the fixed path is pn(t)＝[x(t),y(t)]And satisfies the formula:
in the formula:
αkfor the north and the south of the geodetic coordinate system and the expected path PkAnd Pk+1The included angle of (a).
S22: let ε (t) ═ s (t), e (t)]T∈R2Wherein:
s(t)＝[x(t)-xk]cos(αk)+[y(t)-yk]sin(αk)
e(t)＝-[x(t)-xk]sin(αk)+[y(t)-yk]cos(αk)
s (t) is the tracking distance of the agricultural machinery path, e (t) is the transverse tracking error, in the actual path tracking process, we only need to pay attention to the transverse deviation of the agricultural machinery driving, because when the transverse deviation e (t) is 0, it means that the agricultural machinery has converged on the expected tracking path, the path tracking control target is:
s23: the LOS navigation algorithm is set as follows:
in the formula:
if the agricultural machinery is used as the driving direction angle
The course angle is then:
the step S3 specifically includes:
s31: as shown in fig. 3, information such as real-time position, wheel speed and rotation angle of the agricultural machine is acquired by a navigation positioning system and a pose acquisition system, and kinematic modeling is performed by combining structural parameters of the agricultural machine and a two-degree-of-freedom model theory, specifically:
considering the path tracking process of the agricultural machinery as low-speed motion in an X-Y plane coordinate system, the following results are obtained according to the upper triangular chord and the lower triangular chord theorem:
wherein deltaf、δrRespectively a front wheel turning angle and a rear wheel turning angle; lf、lrRespectively a front wheel base and a rear wheel base;
beta is the direction angle of the running speed of the agricultural machine; r is a turning radius;
two-way degeneracy is multiplied on both sides
Obtaining:
under the condition of low-speed running working condition of the agricultural machinery, the change rate of the vehicle direction is as follows:
Since the agricultural machinery does not take into account the rear wheel steering situation, i.e. tan (delta)r) When the content is 0, finishing to obtain:
wherein
Is the heading angle, v is the speed of travel, and δ is the front wheel steering angle.
In a low-speed working environment, assuming that a wheel rotation direction is consistent with a speed direction, that is, δ is β, a kinematic model is:
s32: the front wheel steering angle can be obtained by the steering characteristic of the agricultural machinery:
wherein
The course angle can be obtained by an LOS algorithm; deltae(t) is a steering deviation angle.
The larger the lateral tracking error e (t) and the larger the front wheel steering angle without considering the driving tracking deviation, the following nonlinear proportional function is obtained according to the geometrical relationship, assuming that the expected trajectory of the vehicle intersects with the nearest tangent on a given path from the front wheel extension line d (t):
where d (t) is related to vehicle speed, represented by vehicle speed v (t) and gain parameter k.
The front wheel steering deviation angle coincides with the desired path tangent direction without taking into account the lateral tracking error, i.e. in the absence of lateral error the front wheel direction is the same as the desired path direction:
as the lateral tracking error increases, the non-linear proportional function produces a front wheel slip angle that points directly to the desired path, and:
in order to enable the two differential equations to have global gradual stable balance at the zero point error junction, the nonlinear front wheel feedback controller is designed by integrating two control factors as follows:
and from the geometrical relationships:
therefore, it is not only easy to use
Therefore, e (t) has a convergence rate between the linear convergence rate of v (t) and the exponential convergence rate of the gain parameter k, and when the lateral tracking error e (t) is small, (ke (t)/v (t))2Approaching 0, then
e(t)＝e(0)*exp-kt
for any transverse error, the differential equation is monotonically converged to 0, and the path tracking control target is realized.
Through auxiliary equipment such as a GPS, a pose sensor, an angle sensor and the like of an upper computer, a lower computer and the like of the agricultural machine, the collected information of the position, the angle and the like is fed back between the controller and the hydraulic steering transmission system, and the process part of the information feedback is not repeated.
As shown in fig. 4, by comparing the simulations of the path tracking control, it can be concluded that: the agricultural machinery path tracking control method based on the LOS theory can enable the control target to be converged to the expected path more quickly and stably, and has the advantages of less rotation angle change times in the path tracking process, more gentle process and good control effect.
The above description is only for the preferred embodiment of the present invention and is not intended to limit the present invention, and any modification, replacement or improvement without departing from the principle and principle of the present invention is within the protection scope of the present invention.
Claims (6)
1. An agricultural machinery path tracking method based on an LOS theory is characterized by comprising the following steps:
s1, when the agricultural machinery runs on the set straight path, the navigation positioning module acquires the real-time position information of the agricultural machinery;
s2, transmitting the position information to an LOS algorithm device through parameters such as coordinates and transverse tracking errors analyzed by an upper computer system to obtain a current driving course angle;
and S3, outputting the wheel steering angle expected by the system by the front wheel feedback controller according to the course angle and the transverse tracking error, thereby obtaining the steering deviation angle of the wheels, and repeating the steps to realize the path tracking control.
2. The LOS theory-based agricultural machinery path tracking method according to claim 1, wherein the step S1 specifically comprises:
s11, setting the boundary of the agricultural machinery working field block through the Beidou RTK positioning module and the visual recognition CDD module, and then dotting a plurality of parallel linear tracks.
And S12, the Beidou RTK navigation module outputs the position and track point information of the agricultural machine in real time, simultaneously converts the position and track point information and three vertex coordinates of the rectangular field block into a Gaussian plane coordinate system, and stores the planned path into a navigation system in a two-dimensional array form to realize navigation path planning.
3. The method for tracking the agricultural machinery path based on the LOS theory as claimed in claim 2, wherein the step S11 specifically comprises:
determining a rectangle by using an RTK-CCD positioning vision sensor fusion system and measuring three coordinate vertexes of a test field, and assuming a positioning coordinate of a ground node A, B, C, D; taking the longest distance of each operation line as a principle, combining the actual geometric shape of the land, selecting a boundary AD as a reference line of operation line planning, taking the boundary AD as a reference line to define a plurality of parallel operation lines, wherein the distance of the parallel lines is determined according to the operation line distance, the shortest distance between the last operation line and the boundary BC is not less than 1/2 operation distances, taking the focuses of all Q parallel operation lines and the boundaries AB and CD as planning operation line nodes, and storing the corresponding operation line nodes into a two-dimensional array for being called by navigation control software based on a LOS algorithm.
4. The LOS theory-based agricultural machinery path tracking method according to claim 1, wherein the step S2 specifically comprises:
s21: determining a two-way path point
And
the defined straight line traces the path, and the origin of the path coordinate system is at
The coordinate of the agricultural machine in the coordinate system with the fixed path is pn(t)＝[x(t),y(t)]And satisfies the formula:
in the formula:
s22: let ε (t) ═ s (t), e (t)]T∈R2Wherein:
s(t)＝[x(t)-xk]cos(αk)+[y(t)-yk]sin(αk)
e(t)＝-[x(t)-xk]sin(αk)+[y(t)-yk]cos(αk)
s (t) is the tracking distance of the agricultural machinery path, e (t) is the transverse tracking error, and in the actual path tracking process, only the transverse deviation of the agricultural machinery running is needed to be concerned, because when the transverse deviation e (t) is 0, the agricultural machinery is converged on the expected tracking path, and the path tracking control target is:
s23: the LOS navigation algorithm is set as follows:
in the formula:
if the agricultural machinery is used as the driving direction angle
The course angle is then:
5. the method for tracking the agricultural machinery path based on the LOS theory as claimed in claim 4, wherein the step S3 specifically comprises:
s31: the method comprises the following steps of acquiring information such as real-time positions, wheel speeds and corners of agricultural machinery through a navigation positioning system and a pose acquisition system respectively, and performing kinematic modeling by combining structural parameters of the agricultural machinery and a two-degree-of-freedom model theory, wherein the kinematic modeling specifically comprises the following steps:
considering the path tracking process of the agricultural machinery as low-speed motion in an X-Y plane coordinate system, the following results are obtained according to the upper triangular chord and the lower triangular chord theorem:
wherein deltaf、δrRespectively a front wheel turning angle and a rear wheel turning angle; lf、lrRespectively a front wheel base and a rear wheel base; beta is the direction angle of the running speed of the agricultural machine; r is a turning radius;
two-way degeneracy is multiplied on both sides
Obtaining:
under the condition of low-speed running working condition of the agricultural machinery, the change rate of the vehicle direction is as follows:
Since the agricultural machinery does not take into account the rear wheel steering situation, i.e. tan (delta)r) When the content is 0, finishing to obtain:
wherein
Is a course angle, v is a running speed, and delta is a front wheel rotation angle;
in a low-speed working environment, assuming that a wheel rotation direction is consistent with a speed direction, that is, δ is β, a kinematic model is:
S32: the front wheel steering angle can be obtained by the steering characteristic of the agricultural machinery:
wherein
The course angle can be obtained by an LOS algorithm; deltae(t) is the steering deviation angle;
the larger the lateral tracking error e (t) and the larger the front wheel steering angle without considering the driving tracking deviation, the following nonlinear proportional function is obtained according to the geometrical relationship, assuming that the expected trajectory of the vehicle intersects with the nearest tangent on a given path from the front wheel extension line d (t):
wherein d (t) is related to vehicle speed, represented by vehicle speed v (t) and gain parameter k;
the front wheel steering deviation angle coincides with the desired path tangent direction without taking into account the lateral tracking error, i.e. in the absence of lateral error the front wheel direction is the same as the desired path direction:
as the lateral tracking error increases, the non-linear proportional function produces a front wheel slip angle that points directly to the desired path, and:
in order to enable the two differential equations to have global gradual stable balance at the zero point error junction, the nonlinear front wheel feedback controller is designed by integrating two control factors as follows:
and from the geometrical relationships:
therefore, it is not only easy to use
Therefore, e (t) has a convergence rate between the linear convergence rate of v (t) and the exponential convergence rate of the gain parameter k, and when the lateral tracking error e (t) is small, (ke (t)/v (t))2Approaching 0, then
e(t)＝e(0)*exp-kt
for any transverse error, the differential equation is monotonically converged to 0, and the path tracking control target is realized.
6. The LOS theory-based agricultural machinery path tracking method as claimed in claim 5, wherein the wheel steering deviation information obtained by the navigation controller is transmitted to an agricultural machinery lower computer system, the real deviation value is corrected by the values of the steering sensor and the photoelectric encoder, and the corrected deviation value is transmitted to a control mechanism hydraulic system to control the flow and the flow direction of the hydraulic system, and finally the steering of the wheels is controlled, so that the vehicle can run according to the set route.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110561035.2A CN113359710B (en) | 2021-05-21 | 2021-05-21 | LOS theory-based agricultural machinery path tracking method |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110561035.2A CN113359710B (en) | 2021-05-21 | 2021-05-21 | LOS theory-based agricultural machinery path tracking method |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN113359710A true CN113359710A (en) | 2021-09-07 |
| CN113359710B CN113359710B (en) | 2022-11-18 |
Family
ID=77526715
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202110561035.2A Active CN113359710B (en) | 2021-05-21 | 2021-05-21 | LOS theory-based agricultural machinery path tracking method |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113359710B (en) |
Cited By (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113635892A (en) * | 2021-10-18 | 2021-11-12 | 禾多科技（北京）有限公司 | Vehicle control method, device, electronic equipment and computer readable medium |
| CN114047748A (en) * | 2021-10-19 | 2022-02-15 | 江苏大学 | Adaptive feedforward model prediction control method and system for automatic driving of agricultural machinery |
| CN114115274A (en) * | 2021-11-25 | 2022-03-01 | 江苏大学 | Agricultural wheeled tractor path tracking output feedback control strategy |
| CN114128695A (en) * | 2021-11-11 | 2022-03-04 | 江苏大学 | Crawler-type autonomous accurate variable air-assisted spraying robot structure and path planning and variable spraying method thereof |
| CN114162127A (en) * | 2021-12-28 | 2022-03-11 | 华南农业大学 | Paddy field unmanned agricultural machine path tracking control method based on machine tool pose estimation |
| CN114312988A (en) * | 2022-01-20 | 2022-04-12 | 东南大学 | Trajectory control method based on yaw angle and steering system corner conversion model |
Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US5062583A (en) * | 1990-02-16 | 1991-11-05 | Martin Marietta Corporation | High accuracy bank-to-turn autopilot |
| CN105629973A (en) * | 2015-12-18 | 2016-06-01 | 广州中海达卫星导航技术股份有限公司 | RTK technology-based agricultural machinery straight driving method and RTK technology-based agricultural machinery straight driving device |
| CN106950955A (en) * | 2017-03-17 | 2017-07-14 | 武汉理工大学 | Based on the ship's track-keepping control method for improving LOS bootstrap algorithms |
| CN110673598A (en) * | 2019-09-29 | 2020-01-10 | 哈尔滨工程大学 | Intelligent path tracking control method for unmanned surface vehicle |
| CN111506086A (en) * | 2020-05-22 | 2020-08-07 | 中国石油大学(华东) | Improved L OS guide law and fuzzy PID combined unmanned ship path tracking control method |
| CN112414404A (en) * | 2019-08-20 | 2021-02-26 | 中国科学院沈阳自动化研究所 | Automatic navigation modeling and control method of agricultural machinery based on steer-by-wire |
| CN112612268A (en) * | 2020-12-10 | 2021-04-06 | 武汉轻工大学 | Path tracking control method, device, equipment and storage medium |
-
2021
- 2021-05-21 CN CN202110561035.2A patent/CN113359710B/en active Active
Patent Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US5062583A (en) * | 1990-02-16 | 1991-11-05 | Martin Marietta Corporation | High accuracy bank-to-turn autopilot |
| CN105629973A (en) * | 2015-12-18 | 2016-06-01 | 广州中海达卫星导航技术股份有限公司 | RTK technology-based agricultural machinery straight driving method and RTK technology-based agricultural machinery straight driving device |
| CN106950955A (en) * | 2017-03-17 | 2017-07-14 | 武汉理工大学 | Based on the ship's track-keepping control method for improving LOS bootstrap algorithms |
| CN112414404A (en) * | 2019-08-20 | 2021-02-26 | 中国科学院沈阳自动化研究所 | Automatic navigation modeling and control method of agricultural machinery based on steer-by-wire |
| CN110673598A (en) * | 2019-09-29 | 2020-01-10 | 哈尔滨工程大学 | Intelligent path tracking control method for unmanned surface vehicle |
| CN111506086A (en) * | 2020-05-22 | 2020-08-07 | 中国石油大学(华东) | Improved L OS guide law and fuzzy PID combined unmanned ship path tracking control method |
| CN112612268A (en) * | 2020-12-10 | 2021-04-06 | 武汉轻工大学 | Path tracking control method, device, equipment and storage medium |
Non-Patent Citations (1)
| Title |
|---|
| 田勇 等: "无人水面艇直线航迹跟踪控制器的设计与验证", 《大连海事大学学报》, vol. 41, no. 4, 30 November 2015 (2015-11-30), pages 14 * |
Cited By (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113635892A (en) * | 2021-10-18 | 2021-11-12 | 禾多科技（北京）有限公司 | Vehicle control method, device, electronic equipment and computer readable medium |
| CN113635892B (en) * | 2021-10-18 | 2022-02-18 | 禾多科技（北京）有限公司 | Vehicle control method, device, electronic equipment and computer readable medium |
| CN114047748A (en) * | 2021-10-19 | 2022-02-15 | 江苏大学 | Adaptive feedforward model prediction control method and system for automatic driving of agricultural machinery |
| CN114047748B (en) * | 2021-10-19 | 2024-05-14 | 江苏大学 | Adaptive feedforward model predictive control method and system for automatic driving of agricultural machinery |
| CN114128695A (en) * | 2021-11-11 | 2022-03-04 | 江苏大学 | Crawler-type autonomous accurate variable air-assisted spraying robot structure and path planning and variable spraying method thereof |
| CN114115274A (en) * | 2021-11-25 | 2022-03-01 | 江苏大学 | Agricultural wheeled tractor path tracking output feedback control strategy |
| CN114162127A (en) * | 2021-12-28 | 2022-03-11 | 华南农业大学 | Paddy field unmanned agricultural machine path tracking control method based on machine tool pose estimation |
| CN114162127B (en) * | 2021-12-28 | 2023-06-27 | 华南农业大学 | Paddy field unmanned agricultural machinery path tracking control method based on machine pose estimation |
| CN114312988A (en) * | 2022-01-20 | 2022-04-12 | 东南大学 | Trajectory control method based on yaw angle and steering system corner conversion model |
| CN114312988B (en) * | 2022-01-20 | 2023-10-20 | 东南大学 | Track control method based on yaw angle and steering system corner conversion model |
Also Published As
| Publication number | Publication date |
|---|---|
| CN113359710B (en) | 2022-11-18 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN113359710B (en) | LOS theory-based agricultural machinery path tracking method | |
| CN108955688B (en) | Method and system for positioning double-wheel differential mobile robot | |
| CN105867377A (en) | Automatic navigation control method of agricultural machine | |
| CN108007417B (en) | Automatic calibration method for angle sensor of automatic driving control system of agricultural machine | |
| Wooden et al. | Autonomous navigation for BigDog | |
| Yang et al. | An optimal goal point determination algorithm for automatic navigation of agricultural machinery: Improving the tracking accuracy of the Pure Pursuit algorithm | |
| CN106909151A (en) | For the unpiloted path planning of agricultural machinery and its control method | |
| CN113654555A (en) | Automatic driving vehicle high-precision positioning method based on multi-sensor data fusion | |
| CN106647770A (en) | Field turning path planning and control method used for farm machinery driverless driving | |
| CN111338342A (en) | Automatic tracking driving control system and method for wheel type engineering machinery | |
| Raikwar et al. | Navigation and control development for a four-wheel-steered mobile orchard robot using model-based design | |
| CN113310488A (en) | Orchard robot navigation method based on SLAM | |
| CN116839570B (en) | Crop interline operation navigation method based on sensor fusion target detection | |
| CN115993089B (en) | PL-ICP-based online four-steering-wheel AGV internal and external parameter calibration method | |
| Mosalanejad et al. | Evaluation of navigation system of a robot designed for greenhouse spraying | |
| Yang et al. | Control methods of mobile robot rough-terrain trajectory tracking | |
| Li et al. | Development of the automatic navigation system for combine harvester based on GNSS | |
| Jiao et al. | A sliding parameter estimation method based on UKF for agricultural tracked robot | |
| CN109542099B (en) | Agricultural machinery control method | |
| CN107861501A (en) | Underground sewage treatment works intelligent robot automatic positioning navigation system | |
| CN113960921A (en) | Visual navigation control method and system for orchard tracked vehicle | |
| Rapoport et al. | Navigation and control problems in precision farming | |
| Shi et al. | A-infinity control for path tracking with fuzzy hyperbolic tangent model | |
| Yu et al. | A Camera/Ultrasonic Sensors Based Trunk Localization System of Semi-Structured Orchards | |
| Bao et al. | Research on trajectory planning and control system of general mobile platform for Mountain Orchard |
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 |