CN112954585B - UWB-based agricultural machine field positioning system and method - Google Patents

UWB-based agricultural machine field positioning system and method Download PDF

Info

Publication number
CN112954585B
CN112954585B CN202110124820.1A CN202110124820A CN112954585B CN 112954585 B CN112954585 B CN 112954585B CN 202110124820 A CN202110124820 A CN 202110124820A CN 112954585 B CN112954585 B CN 112954585B
Authority
CN
China
Prior art keywords
positioning
uwb
uwb positioning
field
tag
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
CN202110124820.1A
Other languages
Chinese (zh)
Other versions
CN112954585A (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.)
South China Agricultural University
Original Assignee
South 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 South China Agricultural University filed Critical South China Agricultural University
Priority to CN202110124820.1A priority Critical patent/CN112954585B/en
Publication of CN112954585A publication Critical patent/CN112954585A/en
Application granted granted Critical
Publication of CN112954585B publication Critical patent/CN112954585B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • H—ELECTRICITY
    • H04—ELECTRIC COMMUNICATION TECHNIQUE
    • H04W—WIRELESS COMMUNICATION NETWORKS
    • H04W4/00—Services specially adapted for wireless communication networks; Facilities therefor
    • H04W4/02—Services making use of location information
    • H04W4/025—Services making use of location information using location based information parameters
    • 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
    • H—ELECTRICITY
    • H04—ELECTRIC COMMUNICATION TECHNIQUE
    • H04W—WIRELESS COMMUNICATION NETWORKS
    • H04W64/00—Locating users or terminals or network equipment for network management purposes, e.g. mobility management
    • H04W64/003—Locating users or terminals or network equipment for network management purposes, e.g. mobility management locating network equipment
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02A—TECHNOLOGIES FOR ADAPTATION TO CLIMATE CHANGE
    • Y02A40/00—Adaptation technologies in agriculture, forestry, livestock or agroalimentary production
    • Y02A40/10—Adaptation technologies in agriculture, forestry, livestock or agroalimentary production in agriculture

Landscapes

  • Engineering & Computer Science (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Signal Processing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Position Fixing By Use Of Radio Waves (AREA)
  • Guiding Agricultural Machines (AREA)

Abstract

The invention discloses a field positioning system and method of agricultural machinery based on UWB, the system includes several UWB positioning base stations fixed on the edge of positioning field, UWB positioning label, IMU inertia measuring unit and terminal processor installed on the working machinery; the UWB positioning base stations are used as positioning reference points, and the number of the UWB positioning base stations is not less than 3; the UWB positioning tags are arranged on the top of the operation machine, are in wireless communication with the UWB positioning base stations, respectively measure the distances between the UWB positioning tags and the UWB positioning base stations, and send the measured distance information to the terminal processor; the IMU inertia measurement unit is used for acquiring real-time attitude information of the operation machine and sending the attitude information to the terminal processor; and the terminal processor is used for carrying out position calculation and correction according to the received distance information and the real-time attitude information to obtain stable position information of the working machine. The invention has low positioning cost, high precision and small external interference, is suitable for field positioning of various agricultural machines and has practical popularization value.

Description

UWB-based agricultural machine field positioning system and method
Technical Field
The invention relates to the technical field of agricultural machinery field positioning, in particular to a UWB-based agricultural machinery field positioning system and method.
Background
Automatic driving is the final direction of automatic and intelligent development of agricultural machinery, and the automatic driving technology of agricultural machinery is an important component of intelligent agriculture. Currently, common positioning methods in the agricultural machinery automatic driving technology include GNSS positioning, machine vision positioning, inertial navigation positioning, and the like. The GNSS positioning can realize outdoor high-precision positioning, the positioning precision can reach centimeter level, but the application cost is too high, and the GNSS positioning is easily influenced by external environments (weather, terrain and the like); the application scene of machine vision positioning is limited, and effective positioning characteristics need to be extracted; the inertial navigation positioning cost is low, but the positioning error is accumulated along with time, and the positioning effect is poor. How to reduce the cost and improve the positioning precision is a key problem which needs to be solved urgently when the agricultural machinery automatic driving technology is widely applied.
In recent years, with the rapid development of wireless positioning technology, UWB (Ultra-Wide-band) Ultra-wideband carrier-less communication technology is becoming mature, the communication distance and the positioning accuracy thereof are continuously improved, and the Ultra-wideband carrier-less communication technology has the characteristics of low cost, high positioning accuracy, small external interference and the like, and gradually shows the application potential of UWB in outdoor positioning. The UWB positioning technology has wide application prospect in field positioning of agricultural machinery, particularly in field positioning of small and medium-sized field agricultural machinery in the south.
Disclosure of Invention
Aiming at the problems and the defects in the prior art, the invention provides a UWB-based agricultural machine field positioning system and method, which can realize the accurate positioning of agricultural machines in a field operation area through lower cost investment.
In order to realize the task, the invention adopts the following technical scheme:
a UWB-based agricultural machinery field positioning system comprises a plurality of UWB positioning base stations fixed on the edge of a positioning field block, a UWB positioning tag arranged on a working machine, an IMU inertia measurement unit and a terminal processor;
the UWB positioning base stations are used as positioning reference points, and the number of the UWB positioning base stations is not less than 3 and is not distributed on the same straight line;
the UWB positioning tag is arranged at the top of the operation machine and used for carrying out wireless communication with each UWB positioning base station so as to respectively measure the distance between the UWB positioning tag and each UWB positioning base station and send the measured distance information to the terminal processor;
the IMU inertia measurement unit is used for acquiring real-time attitude information of the operation machine and sending the acquired real-time attitude information to the terminal processor;
and the terminal processor is used for carrying out position calculation and correction according to the received distance information and the real-time attitude information to obtain stable position information of the working machine.
Furthermore, the UWB positioning base station keeps the same horizontal height and is installed vertically to the ground, and the installation height is larger than the maximum height of the vehicle body when the operation machine operates in the field; two UWB positioning base stations are respectively arranged at two ends of the longest edge of the positioning field, and the rest UWB positioning base stations are uniformly distributed along the edge of the positioning field; and establishing a field positioning coordinate system according to the position distribution of the UWB positioning base station.
Further, the establishment process of the field positioning coordinate system is as follows:
facing to the center of the positioning field, taking the center of a UWB positioning base station at one end of the longest side of the positioning field as the origin of a positioning coordinate system, wherein the connecting line of the UWB positioning base stations at the two ends of the longest side of the positioning field is an X axis, and the side of the positioning field is the positive direction of the X axis; and (3) passing through an origin point in the horizontal plane and being vertical to the X axis as a Y axis, and passing through the origin point and being vertical to the horizontal plane and being vertical upwards as a Z axis, and establishing a space rectangular coordinate system as a field positioning coordinate system.
Furthermore, the UWB positioning tags are located on the bilateral symmetry axis at the top of the operation machine, and the distance between the UWB positioning tags and each UWB positioning base station is measured by utilizing a bilateral two-way ranging method through mutually transmitting data packets with each UWB positioning base station in wireless communication.
Further, the IMU inertia measurement unit comprises an accelerometer, a magnetometer and a gyroscope, is horizontally arranged on the operation machine, and obtains the Euler angle and the attitude of the operation machine through fusion calculation according to the measured three-dimensional acceleration and the angular velocity to serve as the real-time attitude information.
Further, the terminal processor utilizes a positioning algorithm to calculate the three-dimensional position coordinates of the UWB positioning tag according to the received distance information; and performing position correction on the calculated three-dimensional position coordinate by using the real-time attitude information, and performing data smoothing processing on the corrected position coordinate to obtain a stable position coordinate of the working machine.
Further, the positioning algorithm comprises:
defaulting that the plane where the height of each UWB positioning base station is located is a zero plane of a Z axis, and obtaining a multilateral positioning equation set according to a distance formula of a UWB positioning tag and the UWB positioning base station:
Figure BDA0002923593490000031
wherein x, y and z are three-dimensional position coordinates of the UWB positioning tag, and xi、yi(i ═ 1,2, …, n) is the two-dimensional position coordinates of UWB positioning base stations i, n is the number of UWB positioning base stations; diThe linear distance from the UWB positioning tag to a UWB positioning base station i;
and eliminating z to obtain a two-dimensional positioning matrix equation:
Figure BDA0002923593490000032
in the formula, a coefficient matrix is marked as A, and a constant matrix is marked as beta;
and (3) solving two-dimensional coordinate information (x, y) by using a least square method:
Figure BDA0002923593490000033
and substituting the two-dimensional coordinate information into the equation set (1), solving the default z coordinate as negative to obtain the z coordinate of each equation in the equation set (1), and taking the mean value to obtain the three-dimensional position coordinate of the UWB positioning tag.
Furthermore, the position correction is used for correcting position deviation caused by the fact that the installation position of the UWB positioning tag is not coincident with the gravity center position of the working machine, and roll angle and position deviation, pitch angle and position deviation are generated due to the fact that the terrain of a working field is uneven;
the position correction includes:
correcting the initial position: according to the installation height of the UWB positioning tag and the longitudinal position distance between the UWB positioning tag and the gravity center of the operation machine, correcting position coordinates:
Figure BDA0002923593490000034
in the formula (I), the compound is shown in the specification,
Figure BDA0002923593490000035
x, y, z are position coordinates of the work machine after correction, x0、y0、z0The position coordinates of the UWB positioning tag are measured, L is the distance between the gravity center of the operation machine and the longitudinal position of the UWB positioning tag, and H is the installation height of the UWB positioning tag;
and (3) correcting a transverse roll angle: correcting position coordinates according to the installation height of the UWB positioning tag, the longitudinal position distance from the center of gravity of the operation machine, the vehicle body yaw angle and the vehicle body roll angle:
Figure BDA0002923593490000041
in the formula, alpha is a vehicle body roll angle, and gamma is a vehicle body yaw angle;
and (3) correcting a pitch angle: correcting position coordinates according to the installation height of the UWB positioning tag, the longitudinal position distance between the UWB positioning tag and the gravity center of the operation machine, the vehicle body yaw angle and the vehicle body pitch angle:
when the pitch angle beta is greater than 0,
Figure BDA0002923593490000042
when the pitch angle beta is less than 0,
Figure BDA0002923593490000043
further, the data smoothing process is to smooth the position information output after the correction by using an arithmetic mean filter or a kalman filter.
In another aspect of the present invention, a UWB-based agricultural machine field positioning method is provided, which includes the following steps:
s1, arranging N UWB positioning base stations at the edge of the positioning field, wherein N is more than or equal to 3, and establishing a field positioning coordinate system;
s2, installing the UWB positioning tag on the top of the working machine, and installing an IMU inertia measurement unit and a terminal processor on the working machine;
respectively measuring the distance between a UWB positioning tag and each UWB positioning base station through wireless communication with each UWB positioning base station, and sending the measured distance information to a terminal processor;
s3, the terminal processor utilizes the positioning algorithm to calculate the three-dimensional position coordinates of the UWB positioning label according to the received distance information;
s4, the IMU inertia measurement unit measures the Euler angle and the attitude of the body of the working machine, and sends the measured real-time attitude information to the terminal processor;
s5, the terminal processor carries out position correction on the three-dimensional position coordinate calculated according to the Euler angle and the attitude of the operation body;
and S6, the terminal processor performs data smoothing processing on the corrected position coordinates to obtain stable position coordinate information of the working machine.
Compared with the prior art, the invention has the following technical characteristics:
1. the method realizes the field positioning of the agricultural machinery by the UWB wireless positioning technology, can overcome the defects of GNSS positioning and machine vision positioning, has the advantages of high positioning precision, small interference by external factors, low price and the like, and has universality for the positioning of the field operation of the agricultural machinery.
2. The method measures the real-time attitude information of the operation machine through the IMU sensor, and corrects the position information acquired by the UWB positioning system by utilizing the attitude information; when the complex paddy field environment works, high-precision position information and posture information of the working machine can be obtained.
Drawings
FIG. 1 is a schematic diagram of the system deployment of the present invention.
Fig. 2 is a system configuration diagram of the present invention.
Fig. 3 is a flow chart of the working principle of the present invention.
Detailed Description
The present invention will be described in detail below with reference to specific examples for easy understanding of the present invention, but the present invention is not limited to these examples.
Aiming at the influence of a complex field operation environment and the posture of a vehicle body on the positioning of the agricultural machinery, the invention measures the posture of the operation machinery by using an IMU (inertial Measurement unit) inertia measuring unit, and corrects the position measured by the UWB positioning system according to the posture information of the operation machinery to obtain the stable and accurate position information of the operation machinery. As shown in FIG. 1, one embodiment of the present invention is a field orientation of a rice transplanter used in a rectangular paddy field environment, with the direction of operation of the transplanter parallel to the Y-axis, and the path of operation as shown in FIG. 1.
The agricultural machine field positioning system based on UWB in the embodiment of the invention comprises 4 UWB positioning base stations, UWB positioning tags, IMU inertia measurement units and a terminal processor, wherein:
the 4 UWB positioning base stations keep the same horizontal height and are installed at four corners of a rectangular field, the installation height is slightly larger than the maximum height of a vehicle body when the transplanter operates in the field, the base stations are ordered and numbered clockwise, and a field positioning coordinate system is established.
The UWB positioning tag is installed on a bilateral symmetry axis at the top of the rice transplanter, data packets are mutually sent through wireless communication with each base station, the distance between the positioning tag and each base station is measured by using a bilateral two-way distance measurement method, and distance information is sent to the terminal processor through serial port communication.
The IMU inertia measurement unit is arranged on the transplanter, collects real-time attitude information of the transplanter and sends distance information to the terminal processor through serial port communication.
The terminal processor is arranged on the rice transplanter and used for solving the three-dimensional position coordinate of the UWB positioning tag by using a positioning algorithm according to the received distance information; and performing position correction on the three-dimensional position coordinate calculated by the attitude information of the rice transplanter, and performing data smoothing processing on the corrected position coordinate to obtain a stable position coordinate of the working machine.
The positioning algorithm defaults that the plane where the height of each base station is located is a zero plane of a Z axis, and obtains an equation set by using a four-point positioning algorithm according to the distance between the measured UWB positioning tag and each UWB positioning base station:
Figure BDA0002923593490000061
wherein x, y and z are three-dimensional position coordinates of the UWB positioning tag, and xi、yi(i is 1,2,3,4) is a two-dimensional position coordinate of the UWB positioning base station i, diThe linear distance of the tag to the UWB base station i is located for the UWB.
Eliminate z, resulting in an equation for the two-dimensional positioning matrix:
Figure BDA0002923593490000062
in the formula, the coefficient matrix is A, and the constant matrix is beta.
And solving two-dimensional coordinate information by using a least square method:
Figure BDA0002923593490000063
and then, the two-dimensional coordinate information is brought into the original equation set (1), the default z coordinate is negative, the z coordinates of all equations are obtained through solution, the average value is taken, and the three-dimensional position coordinate of the UWB positioning tag is obtained.
The position correction is carried out on the three-dimensional position coordinate calculated by the calculation according to the measured attitude information of the operating transplanter; the rice transplanter is insensitive to the position of the z axis when operating in the field, so that the position change of the z axis is not analyzed.
Correcting the initial position: the installation position of the UWB positioning tag is not coincident with the gravity center position of the rice transplanter, and the initial installation position deviation is introduced; correcting the position information according to the installation height of the UWB positioning tag and the longitudinal position distance between the UWB positioning tag and the center of gravity of the rice transplanter as follows:
Figure BDA0002923593490000071
in the formula (I), the compound is shown in the specification,
Figure BDA0002923593490000072
x and y are the position coordinates of the corrected transplanter, x0、y0And L is the distance between the center of gravity of the rice transplanter and the longitudinal position of the UWB positioning tag for measuring the position coordinate of the UWB positioning tag.
And (3) correcting a transverse roll angle: the terrain of the operation field is uneven, so that the body of the rice transplanter inclines to generate a roll angle and a course deviation, and the measured position coordinate is not coincident with the actual position coordinate; correcting position coordinates according to the installation height of the UWB positioning tag, the longitudinal position distance between the UWB positioning tag and the center of gravity of the rice transplanter, the vehicle body yaw angle and the vehicle body roll angle, and performing the following steps:
Figure BDA0002923593490000073
in the formula, H is the installation height of the UWB positioning tag, alpha is the roll angle of the vehicle body, and gamma is the yaw angle of the vehicle body.
And (3) correcting a pitch angle: the terrain of the operation field is uneven, so that the body of the rice transplanter tilts to generate pitch angle and course deviation, and the measured position coordinate is not coincident with the actual position coordinate; correcting position coordinates according to the installation height of the UWB positioning tag, the longitudinal position distance between the UWB positioning tag and the center of gravity of the rice transplanter, the vehicle body yaw angle and the vehicle body pitch angle:
when the pitch angle beta is greater than 0,
Figure BDA0002923593490000074
when the pitch angle beta is <0,
Figure BDA0002923593490000075
the processor performs smoothing processing on the corrected position coordinates by using an arithmetic mean filter, and finally outputs stable position coordinate information.
In the embodiment of the present invention, optionally, the model of the terminal processor is STM32F103ZET 6; the three-dimensional position coordinate of the rice transplanter obtained by the terminal processor can be directly displayed through a display (an LCD display screen or other visual GUI interfaces) for a driver to refer, so that the operation direction of the rice transplanter can be correspondingly adjusted, and the operation quality is improved; the three-dimensional position coordinates of the rice transplanter can also be directly used as the position information input quantity of an automatic driving system.
As shown in fig. 3, according to the above system, an embodiment of the present invention further provides a UWB-based agricultural machine field positioning method, including the following steps:
s1, arranging N UWB positioning base stations at the edge of the positioning field, wherein N is more than or equal to 3, and establishing a field positioning coordinate system;
s2, installing the UWB positioning tag on the top of the working machine, and installing an IMU inertia measurement unit and a terminal processor on the working machine;
respectively measuring the distance between a UWB positioning tag and each UWB positioning base station through wireless communication with each UWB positioning base station, and sending the measured distance information to a terminal processor;
s3, the terminal processor utilizes the positioning algorithm to calculate the three-dimensional position coordinates of the UWB positioning label according to the received distance information;
s4, the IMU inertia measurement unit measures the Euler angle and the attitude of the body of the working machine, and sends the measured real-time attitude information to the terminal processor;
s5, the terminal processor carries out position correction on the three-dimensional position coordinate calculated according to the Euler angle and the attitude of the operation body;
and S6, the terminal processor performs data smoothing processing on the corrected position coordinates to obtain stable position coordinate information of the working machine.
It should be noted that the layout manner, the related algorithm, the correction process, and the like of each device involved in the method embodiment correspond to the contents in the foregoing method embodiment, and are not described herein again.
The above embodiments are only used to illustrate the technical solutions of the present application, and not to limit the same; although the present application has been described in detail with reference to the foregoing embodiments, it should 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; such modifications and substitutions do not substantially depart from the spirit and scope of the embodiments of the present application and are intended to be included within the scope of the present application.

Claims (8)

1. A UWB-based agricultural machinery field positioning system is characterized by comprising a plurality of UWB positioning base stations fixed on the edge of a positioning field, a UWB positioning tag arranged on a working machine, an IMU inertia measuring unit and a terminal processor;
the UWB positioning base stations are used as positioning reference points, and the number of the UWB positioning base stations is not less than 3 and is not distributed on the same straight line;
the UWB positioning tag is arranged at the top of the operation machine and used for carrying out wireless communication with each UWB positioning base station so as to respectively measure the distance between the UWB positioning tag and each UWB positioning base station and send the measured distance information to the terminal processor;
the IMU inertia measurement unit is used for acquiring real-time attitude information of the operation machine and sending the acquired real-time attitude information to the terminal processor;
the terminal processor is used for carrying out position calculation and correction according to the received distance information and the real-time attitude information to obtain stable position information of the working machine;
the terminal processor utilizes a positioning algorithm to solve the three-dimensional position coordinate of the UWB positioning label according to the received distance information; performing position correction on the three-dimensional position coordinate calculated by the real-time attitude information, and performing data smoothing processing on the corrected position coordinate to obtain a stable position coordinate of the working machine;
the position correction is used for correcting position deviation caused by misalignment of the installation position of the UWB positioning tag and the gravity center position of the operation machine, and roll angle and position deviation, pitch angle and position deviation caused by inclination of a vehicle body of the operation machine due to uneven terrain of an operation field;
the position correction includes:
correcting the initial position: correcting the position coordinate according to the installation height of the UWB positioning tag and the longitudinal position distance between the UWB positioning tag and the gravity center of the operation machine:
Figure FDA0003567300550000011
in the formula (I), the compound is shown in the specification,
Figure FDA0003567300550000012
x, y, z are position coordinates of the work machine after correction, x0、y0、z0For the position coordinates of the measured UWB positioning tag, L is the distance between the gravity center of the operation machine and the longitudinal position of the UWB positioning tag, and H is the installation height of the UWB positioning tag;
and (3) correcting a transverse roll angle: correcting position coordinates according to the installation height of the UWB positioning tag, the longitudinal position distance from the center of gravity of the operation machine, the vehicle body yaw angle and the vehicle body roll angle:
Figure FDA0003567300550000021
in the formula, alpha is a vehicle body roll angle, and gamma is a vehicle body yaw angle;
and (3) correcting a pitch angle: correcting position coordinates according to the installation height of the UWB positioning tag, the longitudinal position distance between the UWB positioning tag and the gravity center of the operation machine, the vehicle body yaw angle and the vehicle body pitch angle:
when the pitch angle beta is greater than 0,
Figure FDA0003567300550000022
when the pitch angle beta is <0,
Figure FDA0003567300550000023
2. the UWB-based agricultural machine field positioning system of claim 1 wherein the UWB positioning base station is maintained at the same horizontal height, is installed vertically to the ground, and is installed at a height greater than the maximum height of the vehicle body when the working machine works in the field; two UWB positioning base stations are respectively arranged at two ends of the longest edge of the positioning field, and the rest UWB positioning base stations are uniformly distributed along the edge of the positioning field; and establishing a field positioning coordinate system according to the position distribution of the UWB positioning base station.
3. An agricultural machine field positioning system based on UWB according to claim 2 wherein the establishment process of the field positioning coordinate system is:
facing to the center of the positioning field, taking the center of a UWB positioning base station at one end of the longest side of the positioning field as the origin of a positioning coordinate system, wherein the connecting line of the UWB positioning base stations at the two ends of the longest side of the positioning field is an X axis, and the side of the positioning field is the positive direction of the X axis; and (3) passing through an origin point in the horizontal plane and being vertical to the X axis as a Y axis, and passing through the origin point and being vertical to the horizontal plane and being vertical upwards as a Z axis, and establishing a space rectangular coordinate system as a field positioning coordinate system.
4. The UWB-based agricultural machine field positioning system of claim 1 wherein the UWB positioning tags are located on the bilateral symmetry axis of the top of the work machine, and the distance between the UWB positioning tags and each UWB positioning base station is measured by a bilateral two-way ranging method through data packets mutually transmitted in wireless communication with each UWB positioning base station.
5. An agricultural machine field positioning system based on UWB according to claim 1 wherein the IMU inertial measurement unit includes an accelerometer, a magnetometer and a gyroscope, horizontally mounted on the work machine, and fusion-resolved the Euler angle and attitude of the work machine according to the measured three-dimensional acceleration and angular velocity as the real-time attitude information.
6. An agricultural machine field positioning system based on UWB according to claim 1 wherein the positioning algorithm comprises:
defaulting that the plane where the height of each UWB positioning base station is located is a zero plane of a Z axis, and obtaining a multilateral positioning equation set according to a distance formula of a UWB positioning tag and the UWB positioning base station:
Figure FDA0003567300550000031
wherein x, y and z are three-dimensional position coordinates of the UWB positioning tag, and xi、yi(i ═ 1,2, …, n) is the two-dimensional position coordinates of UWB positioning base stations i, n is the number of UWB positioning base stations; diThe linear distance from the UWB positioning tag to a UWB positioning base station i;
and eliminating z to obtain a two-dimensional positioning matrix equation:
Figure FDA0003567300550000032
in the formula, a coefficient matrix is marked as A, and a constant matrix is marked as beta;
and (3) solving two-dimensional coordinate information (x, y) by using a least square method:
Figure FDA0003567300550000033
and substituting the two-dimensional coordinate information into the equation set (1), solving the default z coordinate as negative to obtain the z coordinate of each equation in the equation set (1), and taking the mean value to obtain the three-dimensional position coordinate of the UWB positioning tag.
7. An agricultural machine field positioning system based on UWB according to claim 1 wherein the data smoothing process is a smoothing process of the position information output after correction using an arithmetic mean filter or Kalman filter.
8. A method for field positioning of agricultural machinery using the positioning system of any one of claims 1 to 7, comprising the steps of:
s1, arranging N UWB positioning base stations at the edge of the positioning field, wherein N is more than or equal to 3, and establishing a field positioning coordinate system;
s2, installing the UWB positioning tag on the top of the working machine, and installing an IMU inertia measurement unit and a terminal processor on the working machine;
respectively measuring the distance between a UWB positioning tag and each UWB positioning base station through wireless communication with each UWB positioning base station, and sending the measured distance information to a terminal processor;
s3, the terminal processor utilizes the positioning algorithm to calculate the three-dimensional position coordinates of the UWB positioning label according to the received distance information;
s4, the IMU inertia measurement unit measures the Euler angle and the attitude of the body of the working machine, and sends the measured real-time attitude information to the terminal processor;
s5, the terminal processor carries out position correction on the three-dimensional position coordinate calculated according to the Euler angle and the attitude of the operation body;
and S6, the terminal processor performs data smoothing processing on the corrected position coordinates to obtain stable position coordinate information of the working machine.
CN202110124820.1A 2021-01-29 2021-01-29 UWB-based agricultural machine field positioning system and method Active CN112954585B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110124820.1A CN112954585B (en) 2021-01-29 2021-01-29 UWB-based agricultural machine field positioning system and method

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110124820.1A CN112954585B (en) 2021-01-29 2021-01-29 UWB-based agricultural machine field positioning system and method

Publications (2)

Publication Number Publication Date
CN112954585A CN112954585A (en) 2021-06-11
CN112954585B true CN112954585B (en) 2022-06-07

Family

ID=76239449

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110124820.1A Active CN112954585B (en) 2021-01-29 2021-01-29 UWB-based agricultural machine field positioning system and method

Country Status (1)

Country Link
CN (1) CN112954585B (en)

Families Citing this family (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113352300B (en) * 2021-06-28 2024-06-18 广东工业大学 Spraying robot demonstrator and method
CN114234983B (en) * 2021-12-23 2023-09-12 仲继锋 Walking type farmland mobile platform robot navigation method
CN117109583B (en) * 2023-08-11 2024-06-11 淮阴工学院 Positioning navigation method for agricultural transportation equipment

Family Cites Families (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101806905A (en) * 2010-02-22 2010-08-18 中国农业大学 Navigation positioning method and device for agricultural machines
CN105547297B (en) * 2015-12-11 2018-08-21 武汉大学 A kind of indoor orientation method based on UWB positioning systems
WO2020124508A1 (en) * 2018-12-20 2020-06-25 Sz Dji Technology Co. , Ltd. Position determination method and device based on pose data
CN110133698B (en) * 2019-06-13 2023-06-27 湖南农业大学 UWB positioning method and system for automatic navigation and positioning of agricultural operation machinery
CN110927667A (en) * 2019-11-21 2020-03-27 华南农业大学 South small-area paddy field operation positioning system and method
CN111708038B (en) * 2020-06-23 2023-06-27 上海埃威航空电子有限公司 Unmanned ship laser radar point cloud data correction method based on attitude sensor and GNSS

Also Published As

Publication number Publication date
CN112954585A (en) 2021-06-11

Similar Documents

Publication Publication Date Title
CN112954585B (en) UWB-based agricultural machine field positioning system and method
CN111095355B (en) Real-time positioning and orientation tracker
CN110837080B (en) Rapid calibration method of laser radar mobile measurement system
CN108253963B (en) Robot active disturbance rejection positioning method and positioning system based on multi-sensor fusion
CN110501712B (en) Method, device and equipment for determining position attitude data in unmanned driving
CN106651990A (en) Indoor map construction method and indoor map-based indoor locating method
CN112629431B (en) Civil structure deformation monitoring method and related equipment
CN102661717A (en) Monocular vision measuring method for iron tower
CN109465830B (en) Robot monocular stereoscopic vision calibration system and method
CN105509716A (en) Geographic information collection method based on augmented reality technology and device
CN103646377B (en) Coordinate conversion method and device
CN109917432A (en) RTK measuring system and measurement method in a kind of restricted clearance
CN112197766A (en) Vision attitude measuring device for mooring rotor platform
CN113324544A (en) Indoor mobile robot co-location method based on UWB/IMU (ultra wide band/inertial measurement unit) of graph optimization
CN103292827A (en) Data correction method and electronic equipment
CN103134451A (en) Three-coordinate measuring machine three-axis perpendicularity error compensation system and method
CN109712198B (en) Calibration method of advanced driving assistance system
CN110940271A (en) Method for detecting, monitoring and intelligently carrying and installing large-scale industrial manufacturing of ships and the like based on space three-dimensional measurement and control network
WO2022126339A1 (en) Method for monitoring deformation of civil structure, and related device
CN110542418A (en) Indoor pipeline positioning method integrating two-dimensional code and inertial sensor
CN101674358A (en) Three-dimensional space locating mobile communication device
CN111121818B (en) Calibration method for camera and two-dimensional code in unmanned vehicle
CN113592951A (en) Method and device for calibrating external parameters of vehicle-road cooperative middle-road side camera and electronic equipment
CN111521996A (en) Laser radar installation calibration method
CN201491271U (en) Three-dimensional space positioning mobile communication terminal

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