CN-121994236-A - Non-rigid connection agricultural machinery and agricultural tool navigation pose synchronous estimation method

CN121994236ACN 121994236 ACN121994236 ACN 121994236ACN-121994236-A

Abstract

The invention discloses a non-rigid connection agricultural machinery and agricultural machinery navigation pose synchronous estimation method, which comprises the steps of S1, installing a GNSS master antenna and a GNSS master antenna on the agricultural machinery and the agricultural machinery respectively to obtain three-dimensional position coordinates of the agricultural machinery and the agricultural machinery, S2, determining the distance from a connecting point on a horizontal projection plane to the master antenna and the slave antenna by taking the connecting point as a starting point, S3, constructing a heading vector triangle on the horizontal projection plane, S4, obtaining heading angles of the agricultural machinery and the agricultural machinery at the current moment according to the GNSS master antenna and the GNSS master antenna respectively through the position coordinates of the current moment and the last moment, judging the offset state of the agricultural machinery relative to the agricultural machinery through the magnitude relation of the heading angles of the two heading angles, S5, calculating the heading angles of the agricultural machinery and the agricultural machinery through the geometric relation in the main antenna-connecting point-slave antenna triangle according to the offset state of the agricultural machinery, and S6, and outputting the filtered heading angles of the agricultural machinery and the agricultural machinery. The invention has low cost and easy arrangement, and belongs to the technical field of agricultural machinery navigation control.

Inventors

HU LIAN
WANG JINGTING
HE JIE
HUANG PEIKUI
GAO ZIFENG
CHEN YUQI
WANG PEI
ZHAO RUNMAO

Assignees

华南农业大学

Dates

Publication Date: 20260508
Application Date: 20260128

Claims (9)

1. A synchronous estimation method for navigation pose of a non-rigid connection agricultural machine and an agricultural tool is characterized by comprising the following steps: s1, GNSS master-slave antennas are respectively arranged on an agricultural implement and an agricultural machine to respectively acquire three-dimensional position coordinates of the agricultural implement and the agricultural machine; S2, taking a connection point of the agricultural machinery and the agricultural tool as a starting point, respectively connecting the agricultural machinery and the agricultural tool to a main antenna point and a slave antenna point, and determining the distance from the connection point on the horizontal projection plane to the main antenna and the slave antenna according to the geometric relationship; S3, constructing a heading vector triangle on the horizontal projection plane through the three-dimensional position coordinates of the master antenna and the slave antenna and the distance between the connection point on the horizontal projection plane and the master antenna; S4, acquiring heading angles of the agricultural machinery and the agricultural machinery at the current moment according to the GNSS master-slave antenna through the position coordinates of the current moment and the previous moment respectively, and judging the deviation state of the agricultural machinery relative to the agricultural machinery through the magnitude relation of the heading angles of the agricultural machinery and the agricultural machinery; S5, calculating the course angle of the agricultural machinery and the agricultural machinery in the main antenna-connecting point-auxiliary antenna triangle through the geometric relationship according to the deviation state of the agricultural machinery; S6, outputting the filtered agricultural machinery course angle and the agricultural machinery course angle.
2. The method for synchronously estimating navigation postures of a non-rigidly connected agricultural machine and an agricultural machine according to claim 1, wherein in step S1, the main antenna is mounted on the agricultural machine, the sub-antenna is mounted on the agricultural machine, and the position coordinates of the sub-antenna are calculated as The position coordinates of the main antenna are calculated as 。
3. The method for synchronously estimating navigation poses of a non-rigidly connected agricultural machine and farm implement according to claim 2, wherein in step S2, the connection point is expressed as # ) And the calculation distance formula is as follows: = , = , = , Wherein, the Projection distance from the antenna to the connection point; Projecting a distance from the main antenna to the connection point; is the projected distance between the master and slave antennas.
4. A synchronous estimating method for navigation pose of non-rigid connection agricultural machinery and farm machinery according to claim 1 is characterized in that in step S3, on a horizontal projection plane, whether a main antenna point, a connection point and a slave antenna point are collinear or not is judged according to the magnitude relation of the distance from the main antenna point to the main antenna point, the distance from the connection point to the slave antenna point, so as to judge whether a heading vector triangle needs to be constructed or not, wherein the judging principle is that if the main antenna point, the slave antenna point and the connection point are collinear, the agricultural machinery and the farm machinery are equal to the heading measured by the main antenna and the slave antenna, and if the main antenna point, the slave antenna point and the connection point are not collinear, the main antenna-connection point-slave antenna triangle is constructed.
5. The method for synchronously estimating navigation poses of a non-rigid coupling agricultural machine and agricultural implement according to claim 4, wherein in step S3, it is determined whether three points are collinear by the following formula: 。
6. The method for synchronously estimating navigation poses of a non-rigidly connected agricultural machine and agricultural implement according to claim 5, wherein in step S4, the calculation formula of the heading angle of the agricultural machine and the agricultural implement in the initial stage of movement is: , , Wherein, the At the sampling time for the agricultural machinery Is a course angle of (2); At the sampling time for farm tools Is a course angle of (2); at the sampling time for the slave antenna Plane coordinate values of (a); at the sampling time for the slave antenna Plane coordinate values of (a); At the sampling time for the main antenna Plane coordinate values of (a); At the sampling time for the main antenna Plane coordinate values of (a).
7. The method for synchronously estimating navigation pose of non-rigidly connected agricultural machinery and agricultural machinery according to claim 6, wherein in step S4, the deviation state of agricultural machinery relative to agricultural machinery is determined according to the magnitude relation between the heading angle of agricultural machinery and the heading angle of agricultural machinery, and the determination principle is as follows: , Wherein, the At sampling time for master-slave antenna The measured heading angle.
8. The method for synchronously estimating navigation pose of non-rigid connection agricultural machinery and agricultural machinery according to claim 7, wherein in step S5, the equation set for accurately calculating the heading angle of agricultural machinery and the heading angle of agricultural machinery by the geometric relationship of two-dimensional heading vector triangle is: left bias state: Right bias state: Wherein, the By cosine law And (5) calculating.
9. The method for synchronously estimating the navigation pose of the non-rigidly connected agricultural machinery and the agricultural machinery according to claim 8, wherein the step S6 is to output the filtered heading angle of the agricultural machinery and the heading angle of the agricultural machinery, which are obtained by the following equation: , , Wherein, the The course angle of the agricultural machinery after the moving average filtering treatment is set; The agricultural implement course angle after the moving average filtering treatment is adopted; new input data for filtering calculation, namely the latest agricultural machine course angle obtained from the antenna history data; the new input data used for filtering calculation, namely the latest farm tool course angle obtained from the historical data of the main antenna; Is the agricultural heading angle from the last filtering from the antenna; The agricultural implement course angle at the last filtering from the main antenna; Is a smoothing factor.

Description

Non-rigid connection agricultural machinery and agricultural tool navigation pose synchronous estimation method Technical Field The invention relates to the technical field of agricultural machinery navigation control, in particular to a synchronous estimation method for navigation pose of a non-rigid connection agricultural machinery and an agricultural tool. Background The agricultural machine navigation technology can improve the agricultural production efficiency, reduce the labor cost and promote the development of agriculture to intellectualization and modernization. Currently, global Navigation Satellite System (GNSS) based agricultural machinery navigation technology is becoming mature, and most navigation control strategies use agricultural machinery as a control point, namely, a default agricultural machinery is rigidly connected with the agricultural machinery, and the agricultural machinery are regarded as a whole rigid body to control, so that the agricultural machinery and the agricultural machinery can travel according to a preset path. However, under complex terrains such as paddy fields, sloping fields and the like, the posture of the agricultural machinery changes frequently, and the large deformation of the hinged rotation, the loose rigid connection or the elastic connection between the agricultural machinery and the agricultural machinery is inevitably caused, so that the actual operation track of the agricultural machinery deviates from a preset (agricultural machinery) path seriously, and the accuracy of the operation path is reduced and even fails. Therefore, the realization of precise control on the track of the farm tool becomes a key for improving the operation precision under the complex working condition. How to obtain real-time position and course angle information of the farm tool center with low cost, universality and accuracy is a core problem for realizing accurate control of tool track. Currently, the main flow technical scheme for acquiring the position and heading angle of the farm tool mainly comprises the following three types: 1. According to the double-system independent resolving scheme, independent GNSS double-antenna systems are deployed on the agricultural machinery and the agricultural machinery respectively, and the position and heading information of the agricultural machinery and the agricultural machinery are resolved independently through two sets of GNSS cards. For example, in the patent CN111578942a, two GNSS systems are required to be configured in such a manner, which results in high hardware cost and complex system deployment. 2. According to the hinge angle sensing calculation scheme, an angle sensor (such as a rotary encoder) is additionally arranged at the hinge position of the agricultural machine and the agricultural tool, the relative rotation angle of the agricultural tool relative to the agricultural machine is measured, and the attitude of the agricultural tool is indirectly calculated by combining a kinematic model. However, this approach is subject to mechanical vibration interference during field operations, causing sensor signal drift and error accumulation, and is not suitable enough especially under complex terrain conditions such as hillside fields, paddy fields, etc. 3. The GNSS/INS fusion+single antenna auxiliary scheme utilizes a GNSS/INS integrated navigation system at the agricultural machinery end to acquire the agricultural machinery pose, simultaneously installs a single GNSS antenna on the agricultural machinery to directly acquire the agricultural machinery position, and derives the agricultural machinery heading through a kinematic model. For example, the patent CN119512106a adopts this method, although the cost is relatively low, since the agricultural machine can not directly measure the heading by using only a single antenna, and the heading of the agricultural machine is completely calculated by relying on the model, the precision is limited in the scenes of hinge rotation, loose rigid connection, elastic connection deformation and the like between the agricultural machine and the agricultural machine. In conclusion, the existing farm tool positioning and orientation information acquisition mode mainly has the technical defects of high system cost, complex deployment, insufficient adaptability to complex working conditions such as paddy fields, sloping fields and the like, high dependence on a kinematic model and difficulty in adapting to an automatic navigation scene of non-rigid connection of farm tools and farm tools. Disclosure of Invention Aiming at the technical problems in the prior art, the invention aims to provide a non-rigid connection agricultural machinery and agricultural machinery navigation pose synchronous estimation method, which only adopts a set of GNSS system, introduces geometrical information in a horizontal projection plane to perform space geometrical relationship calculation, and can synchronousl