A field navigation and positioning system and method for agricultural machinery in GPS-denied environments

Abstract

The application is suitable for the field navigation positioning technical field of agricultural machinery, and provides a GPS denial environment field navigation positioning system and method for an agricultural vehicle body, which comprises: an agricultural vehicle body; a first fixing device is arranged on the top of the agricultural vehicle body, and a second fixing device is arranged on the agricultural vehicle body; the first fixing device and the second fixing device each comprise three layers of positions; an industrial computer is installed on the first fixing device, and a UWB positioning terminal module is also installed on the first fixing device; an IMU measurement module is arranged on the middle layer of the first fixing device; from the bottom to the top of the second fixing device, a binocular camera, a laser radar and an ultra-wideband radar signal generation and receiving module are arranged, respectively; the application has the beneficial effects that the agricultural vehicle body can obtain higher positioning accuracy, can effectively avoid obstacles and is helpful for the realization of intelligence.

Description

Technical Field

[0001] This invention belongs to the field of agricultural machinery navigation and positioning technology, and particularly relates to an agricultural machinery vehicle field navigation and positioning system and method in a GPS-denied environment. Background Technology

[0002] Automatic navigation technology for agricultural machinery is the core of realizing smart agriculture. It can effectively reduce the labor intensity of agricultural machinery operators and improve operational accuracy and efficiency. In recent years, various scholars have proposed many methods for navigation and positioning of agricultural machinery. Commonly used positioning methods in automatic driving technology for agricultural machinery include GPS positioning, machine vision positioning, and inertial navigation positioning. GPS positioning systems can achieve centimeter-level accuracy in open outdoor environments. However, due to environmental influences and obstacles, GPS signals are weakened or absent, making it difficult for GPS positioning systems to provide continuous and reliable positioning accuracy. Visual sensors are easily affected by lighting conditions, and positioning is prone to loss under conditions of strong exposure and low brightness. Inertial navigation positioning technology is not affected by signal obstruction, but inertial measurement elements drift with increasing operating time and distance. After several rounds of operation, the positioning error of inertial navigation accumulates and increases, and the positioning accuracy gradually decreases.

[0003] Single sensors have limitations, and multi-sensor fusion positioning is often used to improve navigation and positioning accuracy and reliability. Application No. 202110124820.1 discloses a UWB-based agricultural machinery field positioning system and method. UWB positioning tags and IMU (Inertial Measurement Unit) are installed on the agricultural machinery body, and a base station is installed at the edge of the positioning field. The UWB positioning results are used to correct and compensate the IMU, improving the positioning accuracy of the agricultural machinery body. However, fusion positioning cannot be performed when the communication range of the UWB positioning tag is outside the range of the base station. Application No. 202011424886.4 discloses a method for obstacle detection in navigation vehicles based on lidar. This method uses lidar and a satellite antenna installed on the agricultural machinery body to determine potential obstacles in front of the vehicle. However, without satellite signals, accurate positioning is difficult to obtain, leading to inaccurate obstacle detection. Application No. 201710646549.1 discloses an autonomous navigation tractor nighttime panoramic visual relative positioning system and method. This method arranges three sets of binocular vision systems in an equilateral triangle and mounts them on the top of the tractor. The three sets of binocular vision systems simultaneously detect the same target in the environment in their respective directions, and infer the movement of the agricultural machinery body from this, forming displacement vectors in different coordinate systems. These vectors are then converted to the same coordinate system to achieve relative positioning. However, this method struggles to construct a 3D navigation map and cannot initially determine the location of obstacles. Especially in GPS-denied environments, providing high-precision positioning and obstacle avoidance for the agricultural machinery body is crucial for achieving intelligent operation of agricultural machinery.

[0004] In GPS-denied environments, traditional single positioning technologies are insufficient to meet the needs of long-term autonomous cyclic operation of agricultural machinery, and their positioning accuracy is low, failing to meet the requirements of automated operation of agricultural machinery. There is still a significant gap between the current situation and the goal of autonomous and precise operation of agricultural machinery. Therefore, new technologies and methods are needed to solve the problem of long-term high-precision autonomous positioning of agricultural machinery. Summary of the Invention

[0005] The purpose of this invention is to provide a field navigation and positioning system and method for agricultural machinery in GPS-denied environments, aiming to solve the problems mentioned in the background art.

[0006] The present invention is implemented as follows: a field navigation and positioning system for agricultural machinery in a GPS-denied environment, comprising:

[0007] Agricultural machinery body, lidar, ultra-wideband radar signal generator and receiver module, binocular camera, IMU measurement module, UWB positioning base station, UWB positioning terminal module, mobile guide rail, data processing unit, industrial control computer, control module, fixed rod, mobile power supply, high-precision attitude sensor, data display system, connecting fasteners, bracket, first fixing device, second fixing device, fixing clamp, slider, tripod, motor;

[0008] The first fixing device is installed on the top of the agricultural machinery body, and the second fixing device is installed at the front of the agricultural machinery body. Both the first and second fixing devices consist of three layers. An industrial control computer is installed on the first fixing device, and a UWB positioning terminal module is installed on the top layer of the first fixing device. An IMU measurement module is arranged in the middle layer of the first fixing device. On the second fixing device, a binocular camera, a lidar, and an ultra-wideband radar signal generating and receiving module are installed from bottom to top, respectively. The UWB positioning terminal module is fixed to the first fixing device by a fixing clip, the ultra-wideband radar signal generating and receiving module is fixed to the second fixing device by the fixing clip, and several other sensors are fixed to their respective fixing devices by bolts.

[0009] The movable guide rail is bolted and fixed above the tripod. The tripod is set in the edge area of ​​the positioning farmland boundary. The fixing rod is bolted and fixed to the slider of the movable guide rail. The high-precision attitude sensor is fixed to the slider. The connecting fastener is installed on the fixing rod. The bracket is installed on the connecting fastener. The UWB positioning base station is installed on the bracket through the fixing clamp. The connecting fastener is bolted and fixed to the fixing rod.

[0010] The IMU measurement module transmits the collected data to the data processing unit in the industrial control computer. The data processing unit converts the received measurement data from the IMU measurement module into the position and attitude of the agricultural machinery body.

[0011] The UWB positioning terminal module obtains the distance between the agricultural machinery vehicle body and the four UWB positioning base stations in real time during the operation of the agricultural machinery vehicle body and transmits it to the data processing unit in the industrial control computer through wireless communication. The data processing unit calculates the three-dimensional position coordinates of the agricultural machinery vehicle body according to the positioning algorithm. The positioning algorithm is specifically: robust weighted least multiplication, overall least squares, and multidimensional calibration algorithm.

[0012] The lidar collects point cloud data of the agricultural machinery in real time and transmits it to the data processing unit in the industrial control computer. The data processing unit performs point cloud denoising, correction and segmentation on the point cloud data to obtain an irregular triangular mesh of three-dimensional point cloud data. The triangular mesh is interpolated using cubic spline curves. Then, the point cloud data is accurately registered in the irregular triangular mesh to obtain a three-dimensional scene model of the agricultural machinery operation.

[0013] The binocular camera acquires images of the working environment in front of the agricultural machinery and transmits the images to the data processing unit in the industrial control computer. The data processing unit performs feature point detection and matching on the images and calculates the spatial coordinates of the feature points in the images using the parallax principle.

[0014] The ultra-wideband radar signal generating and receiving module transmits the received signal to the data processing unit in the industrial control computer. The data processing unit uses an imaging algorithm to obtain an image of the scanning area in front of the agricultural machinery body, thereby obtaining the distance between the agricultural machinery body and the obstacle in front. The imaging algorithm is specifically: compressed sensing imaging algorithm and spotlight imaging algorithm.

[0015] The high-precision attitude sensor sends attitude data to the Bluetooth data receiving module via the Bluetooth data sending module, and the data processing unit calculates the three-dimensional coordinates of the UWB positioning base station after it moves.

[0016] The industrial control computer sends signals to the lidar, ultra-wideband radar signal generating and receiving module, binocular camera, IMU measurement module, and UWB positioning terminal module. The lidar, ultra-wideband radar signal generating and receiving module, binocular camera, IMU measurement module, and UWB positioning terminal module receive the signals and latch the signals they have collected the moment they are received. Then, they send the signals to the industrial control computer through the bus, thereby keeping all sensors synchronized in time.

[0017] The lidar, the ultra-wideband radar signal generator and receiver module, the binocular camera, the UWB positioning terminal module, the IMU measurement module, and the control module are all connected to the industrial control computer, and the output of the industrial control computer is connected to the input of the data display system.

[0018] The stepper motor drives the slider on the moving guide rail to move via a synchronous belt. The motor is connected to the computer via a serial port, and the portable power supply provides power to the motor.

[0019] Preferably, in the point cloud data denoising, correction and segmentation processing of the point cloud data by the data processing unit, the point cloud denoising method adopts the chord height difference method to remove noise points and the median filtering algorithm; the correction method adopts the iterative nearest point algorithm, and at the same time uses KI-DTree to speed up the search for nearest points and improve the registration speed; the segmentation processing adopts the surface growth segmentation method.

[0020] Preferably, a method for agricultural machinery field navigation and positioning in a GPS-denied environment is characterized by comprising:

[0021] S01: Based on the boundary environment of the farmland area, two movable guide rails are placed on the outer side of the boundary edge. The fixing rod is installed on the slider of the guide rail by bolts. Four UWB positioning base stations are installed on the bracket, and a high-precision attitude sensor is installed on the slider.

[0022] S02: Install a lidar, an ultra-wideband radar signal generator and receiver module, a binocular camera, a UWB positioning terminal module, and an IMU measurement module on the first and second fixed devices on the agricultural machinery body, respectively.

[0023] S03: Select a preset location to establish a positioning coordinate system. Based on the working area of ​​the agricultural machinery vehicle and the communication range of the UWB positioning base station, randomly deploy four UWB positioning base stations and use a total station to measure and calibrate the three-dimensional position coordinates of the four UWB positioning base stations. Input the measurement results of the base stations into the industrial control computer and network the four UWB positioning base stations with the UWB positioning terminal module.

[0024] S04: The location PDOP value is calculated in the industrial control computer. When the PDOP value is greater than 2, the layout of the positioning base stations is optimized in the industrial control computer using an optimization algorithm. The four UWB positioning base stations are readjusted according to the optimization results. When the PDOP value is less than 2, the current layout is taken as the final layout scheme of the UWB positioning base stations.

[0025] S05: Initialize the binocular camera and the lidar, calibrate the intrinsic parameters of the binocular camera, and calibrate the extrinsic parameters between the binocular camera and the lidar to establish the coordinate system relationship between the binocular camera and the lidar;

[0026] S06: Determine the current weather conditions. If the current weather is cloudy, rainy, or foggy, activate the ultra-wideband radar signal receiving module. In the data processing unit, obtain the distance between the agricultural machinery vehicle and the obstacle based on the signal from the ultra-wideband radar signal receiving module. If the weather is sunny, activate the binocular camera and the lidar.

[0027] If the ultra-wideband radar signal receiving module is activated, the distance between the agricultural machinery vehicle and the obstacle is obtained in the data processing unit based on the signal from the ultra-wideband radar signal receiving module; if the binocular camera and the lidar are activated, a three-dimensional map model is constructed in the industrial control computer, the current operating status of the agricultural machinery vehicle is determined in real time, the three-dimensional position coordinates of the obstacle in front of the agricultural machinery vehicle are obtained, and the distance between the agricultural machinery vehicle and the obstacle is obtained in the data processing unit.

[0028] S07: Start the UWB positioning terminal module and the IMU measurement module, obtain the agricultural machinery body position and attitude fused by UWB and IMU in the data processing unit, and transmit the calculation results to the data display system to display the positioning results in real time;

[0029] S08: The agricultural machinery vehicle body operates according to the preset operation path. The existence of obstacles in front of the agricultural machinery vehicle body is detected in real time through step S6, the distance between the agricultural machinery vehicle body and the obstacles is obtained, the operation path that the agricultural machinery vehicle body can pass is obtained by artificial potential field method, and the planning information is transmitted to the control module. The control module sends a control signal to drive the agricultural machinery vehicle body to run according to the planned path.

[0030] S09: The counter module determines whether the agricultural machinery has completed 10 cycles of operation. If the agricultural machinery has completed 10 cycles, the control motor drives the slider to move along the guide rail. Based on the measurement data of the high-precision attitude sensor and the moving distance of the slider, the trajectory tracking algorithm is used to calculate the three-dimensional position coordinates of the four UWB positioning base stations after the movement in the data processing unit, and the three-dimensional position coordinates of the base stations after the movement are transmitted to the industrial control computer to perform continuous cyclic positioning of the agricultural machinery.

[0031] Preferably, the PDOP value is obtained from the observation matrix H, the weighting matrix W, and the matrix M, wherein the observation matrix H and the weighting matrix W are respectively:

[0032]

[0033]

[0034] In the formula This represents the coordinates of the agricultural machinery's body position calculated by the UWB system. This represents the i-th UWB positioning base station. Let represent the variance of the ranging error of the i-th UWB positioning base station, where i = 1, 2, 3, 4.

[0035] The calculation process for matrix M is as follows:

[0036]

[0037] Furthermore, the formula for calculating PDOP is expressed as follows:

[0038]

[0039] In the formula M jj Let j represent the diagonal elements of matrix M, where j = 1, 2, 3.

[0040] Preferably, the layout of the positioning base station is optimized in the industrial control computer using an optimization algorithm, specifically a virus intrusion optimization algorithm or a water circulation optimization algorithm.

[0041] Preferably, the calibration of the binocular camera specifically includes: calibrating the intrinsic parameters of the left and right cameras respectively using the Zhang Zhengyou calibration method; and calibrating the radial distortion coefficients of the left and right cameras. and tangential distortion coefficient Calibration is performed; the relative pose relationship between the left and right cameras is calibrated to determine the length of the binocular baseline.

[0042] Preferably, the extrinsic parameter calibration of the lidar and the binocular camera involves the following steps:

[0043] S11: A rectangular plate is selected as the calibration plate. Four circular holes of the same size are set on the rectangular plate, and a three-sided metal reflector is fixed in the center of the calibration plate to enhance the reflectivity of the laser radar. The center line of the four circular holes forms a rectangle. The radius of the circular holes and the side length of the rectangle are accurately measured with a tape measure.

[0044] S12: The binocular camera acquires images of the circular holes on the calibration plate, uses the Sobel detection operator to create edge images, and extracts information about the circular holes and trihedral reflectors from the images based on the random Hough transform method; the lidar scans the circular hole calibration plate, extracts the point cloud information of the circular holes and trihedral reflectors, performs filtering and noise reduction processing, and uses the random sampling consensus algorithm to refit the calibration plate to obtain a more accurate calibration plate plane;

[0045] S13: Calculate the translation matrix and rotation matrix The details are as follows:

[0046] (a) First assume it is a rotation matrix Given the identity matrix, the translation matrix is ​​roughly calculated through edge feature matching. ;

[0047] (b) Construct a set of feature points detected by the binocular camera and the lidar, and optimize the translation matrix using edge detection error and reprojection error. And calculate the rotation matrix. .

[0048]

[0049] In the formula This represents the set of feature points established by the feature points detected by the lidar. This represents the set of feature points established by the feature points detected by the binocular camera.

[0050] S14: Calculate the three-dimensional coordinates from the lidar point cloud data. Converted to pixel coordinates of the camera object As shown in the following formula:

[0051]

[0052] in, Indicates the focal length of the left and right cameras. The z-axis represents the origin of a pixel in a visual sensor. c This represents the z-axis coordinate in the visual coordinate system.

[0053] Preferably, the data obtained by the lidar, the binocular camera, the IMU measurement module, and the ultra-wideband radar signal generator / receiver module are converted to the world coordinate system, wherein the three-dimensional coordinates in the binocular camera coordinate system are converted to the world coordinate system. Three-dimensional coordinates integrated into the world coordinate system The format is as follows:

[0054]

[0055] In the formula This represents the translation parameter from the origin of the camera coordinate system C to the origin of the world coordinate system W; This represents the rotation matrix between the camera coordinate system and the world coordinate system, where The calculation is as follows:

[0056]

[0057] In the formula ;

[0058] ;

[0059] in , , This represents the rotation angle between the camera coordinate system and the three coordinate axes X, Y, and Z of the world coordinate system.

[0060] Preferably, the positioning results of the IMU measurement module and the UWB positioning terminal module are fused into the world coordinate system using the Bursa seven-parameter model, and the fusion error e is written in the following form:

[0061]

[0062] In the formula , , This is the positioning result after fusion of UWB and IMU, where e represents the error matrix of the fusion of IMU and UWB positioning results. , , Mean after fusion.

[0063] Furthermore, the cost function for the fusion error is constructed as follows: According to the least squares principle, the error parameter is obtained when the cost function reaches its minimum value. , , , , , The positioning results of the IMU are corrected by using error parameters, thereby enabling the agricultural machinery body to obtain higher accuracy positioning results.

[0064] In the data processing unit, the difference between the positioning result of the UWB positioning system and the position estimate of the IMU measurement module is calculated. If the error is less than a set threshold, the error is fed back and fused into the error state-based extended Kalman filter model to obtain the final positioning result of the agricultural machinery body. If the difference is greater than the set threshold, the positioning result of the UWB positioning system is used as the final positioning result of the agricultural machinery body. At the same time, the positioning result of the IMU measurement module is corrected for error at the next moment, and the corrected result is used as the positioning result of the IMU measurement module.

[0065] Preferably, variational volume Kalman filtering is used in the data processing unit to smooth the positioning data of the agricultural machinery body, so as to obtain a more stable attitude and position coordinates.

[0066] Preferably, the data processing unit performs image matching on radar scan images at two adjacent time points, and the data calculation unit obtains the distance between the obstacle and the agricultural machinery body. When the distance between the obstacle and the agricultural machinery body is less than a set threshold, the A* algorithm is used to replan the working path of the agricultural machinery body; when the distance between the obstacle and the agricultural machinery body is greater than the set threshold, the agricultural machinery body continues to operate according to the originally planned path.

[0067] Preferably, the method for calculating the distance between the agricultural machinery body and the obstacle is as follows: Based on the fusion positioning results of IMU and UWB, the position coordinates of the agricultural machinery body in world coordinates are obtained. The lidar and the binocular camera obtain the position coordinates of the obstacle in the world coordinate system. The distance d between the obstacle and the agricultural machinery body can be expressed as:

[0068]

[0069] Preferably, the process of calculating the obstacle's position coordinates in the world coordinate system is as follows: based on the pixels of the obstacle's feature points calculated by the binocular camera, the three-dimensional coordinates of the obstacle in the binocular camera coordinate system are calculated in the data calculation unit; then, the three-dimensional coordinates of the binocular camera coordinate system are transformed to the world coordinate system, thereby obtaining the obstacle's three-dimensional coordinates in the world coordinate system, wherein the pixels in the camera feature points are... Depth is The corresponding three-dimensional coordinates in the binocular camera coordinate system It can be represented as:

[0070]

[0071] The present invention provides a field navigation and positioning system and method for agricultural machinery in a GPS-denied environment, which has the following beneficial effects:

[0072] (1) The UWB positioning system, consisting of a positioning base station and a positioning terminal, can achieve positioning in GPS denied environments. The fusion positioning of UWB and IMU can improve the positioning accuracy of agricultural machinery vehicles. In complex environments, agricultural machinery vehicles can obtain higher positioning accuracy.

[0073] (2) By combining lidar and binocular camera, a map model of the agricultural machinery body can be constructed, and the position coordinates of obstacles in front of the agricultural machinery body can be obtained at the same time; the ultra-wideband radar signal generating and receiving module has a strong penetration capability, which enables the agricultural machinery body to accurately obtain the distance between the agricultural machinery body and obstacles in rainy, cloudy or foggy weather, so that the agricultural machinery body can effectively avoid obstacles.

[0074] (3) When the UWB positioning terminal module on the agricultural machinery body exceeds the communication range of the positioning base station, the UWB positioning base station can move along the guide rail, avoiding manual relocation of the positioning base station. The coordinates of the positioning base station after the movement can be quickly calculated using the trajectory shifting algorithm, realizing long-term continuous operation of the agricultural machinery body, improving the working efficiency of the agricultural machinery body, and contributing to the realization of intelligentization of the agricultural machinery body. Attached Figure Description

[0075] Figure 1 This is a schematic diagram of the layout and installation of the UWB positioning base station in a GPS denied environment according to the present invention;

[0076] Figure 2 This is a schematic diagram of the sensor layout and installation on the agricultural machinery body under GPS denial conditions according to the present invention;

[0077] Figure 3This is a schematic diagram showing the installation positions of the IMU measurement module and the UWB positioning terminal of the present invention;

[0078] Figure 4 This is a schematic diagram of the agricultural machinery vehicle field navigation and positioning system under GPS rejection conditions according to the present invention;

[0079] Figure 5 This is a flowchart of the agricultural machinery vehicle field navigation and positioning method under GPS rejection conditions according to the present invention;

[0080] Figure 6 This is a schematic diagram of the calibration board for the joint calibration of the binocular camera and lidar of the present invention;

[0081] Figure 7 This is a flowchart of the IMU measurement module and UWB fusion positioning process of the present invention.

[0082] In the diagram: 1. Agricultural machinery body; 2. UWB positioning base station; 3. Connecting fastener; 4. Moving guide rail; 5. Bracket; 6. High-precision attitude sensor; 7. Slider; 8. Tripod; 9. Power supply; 10. Motor; 11. Fixing rod; 12. UWB positioning terminal module; 13. Ultra-wideband radar signal generator and receiver module; 14. LiDAR; 15. Binocular camera; 16. IMU measurement module; 17. Industrial control computer; 18. First fixing device; 19. Data processing unit; 20. Data display system; 21. Control module; 22. Second fixing device; 23. Fixing clamp; 24. Calibration plate; 25. Circular hole; 26. Three-sided metal reflector. Detailed Implementation

[0083] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative and not intended to limit the invention.

[0084] The specific implementation of the present invention will be described in detail below with reference to specific embodiments.

[0085] The present invention provides a field navigation and positioning system and method for agricultural machinery in GPS-denied environments, which solves the technical problems in the background art.

[0086] like Figure 1 The diagram shown is a main flowchart of a field navigation and positioning system and method for agricultural machinery in a GPS-denied environment, according to an embodiment of the present invention. The present invention will be further described below with reference to the accompanying drawings and embodiments.

[0087] exist Figures 1-3A field navigation and positioning system for agricultural machinery in a GPS-denied environment is characterized by comprising: an agricultural machinery body 1, a UWB positioning base station 2, a connecting fastener 3, a moving guide rail 4, a bracket 5, a high-precision attitude sensor 6, a slider 7, a triangular fixing frame 8, a mobile power supply 9, a motor 10, a fixing rod 11, a UWB positioning terminal module 12, an ultra-wideband radar signal generating and receiving module 13, a lidar 14, a binocular camera 15, an IMU measurement module 16, an industrial control computer 17, a first fixing device 18; a data processing unit 19, a data display system 20, a control module 21, a second fixing device 22, a fixing clamp 23, a calibration plate 24, a circular hole 25, and a three-sided metal reflector 26.

[0088] The first fixing device 18 is installed on the top of the agricultural machinery body 1, and the second fixing device 22 is installed at the front of the agricultural machinery body 1. Both the first fixing device 18 and the second fixing device 22 have three layers. The industrial control computer 17 is installed on the bottom layer of the first fixing device 18, and the UWB positioning terminal module 12 is installed on the top layer of the first fixing device 18. The IMU measurement module 16 is installed on the middle layer of the first fixing device. The binocular camera 15 is installed on the bottom layer of the second fixing device 22, and the ultra-wideband radar signal generating and receiving module 13 is installed on the top layer of the second fixing device 22. The lidar 14 is installed on the middle layer of the second fixing device 22. The UWB positioning terminal module 12 is fixed to the first fixing device 18 by the fixing clip 23, and the ultra-wideband radar signal generating and receiving module 13 is fixed to the second fixing device 22 by the fixing clip 23. The remaining sensors are fixed to their respective fixing devices by bolts.

[0089] The movable guide rail 4 is bolted and fixed above the tripod 8. The tripod 8 is set in the edge area of ​​the positioning farmland boundary. The fixed rod 11 is bolted and fixed to the slider 7 of the movable guide rail 4. The high-precision attitude sensor 6 is fixed to the slider 7. The connecting fastener 3 is installed on the fixed rod 11. The bracket 5 is installed on the connecting fastener 3. The UWB positioning base station 12 is installed on the bracket 5 through the fixing clamp 23. The connecting fastener 3 is bolted and fixed to the fixed rod 11. The mobile power supply 9 supplies power to the motor 10.

[0090] The IMU measurement module 16 transmits the collected data to the data processing unit 19 in the industrial control computer 17. The data processing unit 19 converts the received measurement data from the IMU measurement module 16 into the position and attitude of the agricultural machinery body 1.

[0091] The UWB positioning terminal module 12 obtains the distance between the agricultural machinery vehicle body 1 and the four UWB positioning base stations 2 in real time during the operation and transmits it to the data processing unit 19 in the industrial control computer 17 via wireless communication. The data processing unit 19 calculates the three-dimensional position coordinates of the agricultural machinery vehicle body 1 according to the positioning algorithm. The positioning algorithm is specifically: robust weighted least multiplication, overall least squares, and multidimensional calibration algorithm.

[0092] The lidar 14 collects point cloud data of the agricultural machinery vehicle 1 in real time during operation and transmits it to the data processing unit 19 in the industrial control computer 17. The data processing unit 19 performs point cloud denoising, correction and segmentation processing on the point cloud data to obtain an irregular triangular mesh of three-dimensional point cloud data. The triangular mesh is interpolated using cubic spline curves, and then the point cloud data is accurately registered in the irregular triangular mesh to obtain a three-dimensional scene model of the agricultural machinery vehicle 1 during operation.

[0093] The binocular camera 15 acquires images of the working environment in front of the agricultural machinery vehicle 1 and transmits the images to the data processing unit 19 in the industrial control computer 17. The data processing unit 19 performs feature point detection and matching on the images and calculates the spatial coordinates of the feature points in the images using the parallax principle.

[0094] The ultra-wideband radar signal generating and receiving module 13 transmits the received signal to the data processing unit 19 in the industrial control computer 17. The data processing unit 19 uses an imaging algorithm to obtain an image of the scanning area in front of the agricultural machinery vehicle body 1, thereby obtaining the distance between the agricultural machinery vehicle body 1 and the obstacle in front. The imaging algorithm specifically includes: compressed sensing imaging algorithm and spotlight imaging algorithm.

[0095] The high-precision attitude sensor 5 sends attitude data to the Bluetooth data receiving module via the Bluetooth data sending module, and the data processing unit 19 calculates the three-dimensional coordinates of the UWB positioning base station 2 after it moves.

[0096] The motor 10 drives the slider 7 on the moving guide rail 4 to move via a synchronous belt. The motor 10 is connected to the computer via a serial port, and the mobile power supply 9 supplies power to the motor 10.

[0097] In the point cloud data denoising, correction and segmentation processing of the data processing unit 19, the point cloud denoising method adopts the chord height difference method to remove noise points and the median filtering algorithm; the correction method adopts the iterative nearest point algorithm, and at the same time uses KI-DTree to speed up the search for nearest points and improve the registration speed; the segmentation processing adopts the surface growth segmentation method.

[0098] exist Figure 4In this system, the lidar 14, the ultra-wideband radar signal generator and receiver module 13, the binocular camera 15, the UWB positioning terminal module 12, the IMU measurement module 16, and the control module 21 are all connected to the industrial control computer 17, and the output of the industrial control computer 17 is connected to the input of the data display system 20.

[0099] The industrial control computer 17 sends signals to the lidar 14, the ultra-wideband radar signal generating and receiving module 13, the binocular camera 15, the IMU measurement module 16, and the UWB positioning terminal module 12. The lidar 14, the ultra-wideband radar signal generating and receiving module 13, the binocular camera 15, the IMU measurement module 16, and the UWB positioning terminal module 12 receive the signals and latch the signals they have collected at the moment of receipt. Then, they send the signals to the industrial control computer 1 through the bus, thereby keeping all sensors synchronized in time.

[0100] Example 2

[0101] exist Figure 5 A method for field navigation and positioning of agricultural machinery in a GPS-denied environment includes:

[0102] S01: Based on the environment of the farmland area boundary, two of the moving guide rails 4 are placed on the outer side of the boundary edge, and the fixing rod is installed on the slider 7 of the guide rail by bolts. Four of the UWB positioning base stations 2 are installed on the bracket 5, and the high-precision attitude sensor 6 is installed on the slider 7.

[0103] S02: The lidar 14, the ultra-wideband radar signal generating and receiving module 13, the binocular camera 15, the UWB positioning terminal module 12, and the IMU measurement module 16 are respectively installed on the first fixing device 18 and the second fixing device 22 on the agricultural machinery body 1.

[0104] S03: Select a suitable location to establish a positioning coordinate system. Based on the working area of ​​the agricultural machinery vehicle 1 and the communication range of the UWB positioning base station 2, randomly deploy 4 UWB positioning base stations 2 and use a total station to measure and calibrate the three-dimensional position coordinates of the 4 UWB positioning base stations 2. Input the measurement results of the base stations into the industrial control computer 17 and network the 4 UWB positioning base stations 2 with the UWB positioning terminal module 12.

[0105] S04: The position geometric precision factor (PDOP) is calculated in the industrial control computer 17. When the PDOP value is greater than 2, the layout of the positioning base stations is optimized in the industrial control computer 17 using an optimization algorithm. The four UWB positioning base stations 2 are readjusted according to the optimization results. When the PDOP value is less than 2, the current layout is taken as the final layout scheme of the UWB positioning base stations 2.

[0106] S05: Initialize the binocular camera 15 and the lidar 14, calibrate the intrinsic parameters of the binocular camera 15, and calibrate the extrinsic parameters between the binocular camera 15 and the lidar 14 to establish the coordinate system relationship between the binocular camera 15 and the lidar 14.

[0107] S06: Determine the current weather conditions. If it is cloudy, rainy, or foggy, activate the ultra-wideband radar signal generating and receiving module 13. The data processing unit 19 obtains the distance between the agricultural machinery vehicle body 1 and the obstacle based on the signal from the ultra-wideband radar signal generating and receiving module 13. If it is sunny, activate the binocular camera 15 and the lidar 14.

[0108] If the ultra-wideband radar signal generating and receiving module 13 is activated, the distance between the agricultural machinery vehicle body 1 and the obstacle is obtained in the data processing unit 19 based on the signal from the ultra-wideband radar signal generating and receiving module 13; if the binocular camera 15 and the lidar 14 are activated, a three-dimensional map model is constructed in the industrial control computer 17, the current operating status of the agricultural machinery vehicle body 1 is determined in real time, and the three-dimensional position coordinates of the obstacle in front of the agricultural machinery vehicle body 1 are obtained, and the distance between the agricultural machinery vehicle body 1 and the obstacle is obtained in the data processing unit 19.

[0109] S07: Start the UWB positioning terminal module 12 and the IMU measurement module 16, obtain the agricultural machinery body position and attitude fused by UWB and IMU in the data processing unit 19, and transmit the calculation results to the data display system 20 to display the positioning results in real time.

[0110] S08: The agricultural machinery vehicle body 1 operates according to the preset operation path. In step S6, the existence of obstacles in front of the agricultural machinery vehicle body 1 is detected in real time, the distance between the agricultural machinery vehicle body 1 and the obstacles is obtained, the operation path that the agricultural machinery vehicle body 1 can pass is obtained by using the artificial potential field method, and the planning information is transmitted to the control module 21. The control module 21 sends a control signal to drive the agricultural machinery vehicle body 1 to run according to the planned path.

[0111] S09: The counter module determines whether the agricultural machinery vehicle body 1 has completed 10 cycles of operation. If the agricultural machinery vehicle body 1 has completed 10 cycles of operation, the motor 10 drives the slider to move along the guide rail. Based on the measurement data of the high-precision attitude sensor 6 and the moving distance of the slider, the trajectory pushing algorithm is used to calculate the three-dimensional position coordinates of the four UWB positioning base stations 2 after the movement in the data processing unit 19, and the three-dimensional position coordinates of the base stations after the movement are transmitted to the industrial control computer 17 to perform continuous cyclic positioning of the agricultural machinery vehicle body 1.

[0112] In step S04, the virus intrusion optimization algorithm or the water cycle optimization algorithm optimizes the layout of the positioning base stations; simultaneously, the PDOP value is obtained from the observation matrix H, the weighting matrix W, and the matrix M, where the observation matrix H and the weighting matrix W are respectively:

[0113]

[0114]

[0115] In the formula This represents the coordinates of the agricultural machinery's body position calculated by the UWB system. This represents the i-th UWB positioning base station. Let represent the variance of the ranging error of the i-th UWB positioning base station, where i = 1, 2, 3, 4.

[0116] The calculation process for matrix M is as follows:

[0117]

[0118] Furthermore, the position precision factor PDOP can be obtained as follows:

[0119]

[0120] In the formula M jj Let j represent the diagonal elements of matrix M, where j = 1, 2, 3.

[0121] In Figure 6, the calibration of the binocular camera 15 specifically includes: calibrating the intrinsic parameters of the left and right cameras respectively using the Zhang Zhengyou calibration method; and calibrating the radial distortion coefficients of the left and right cameras. and tangential distortion coefficient Calibration is performed; the relative pose relationship between the left and right cameras is calibrated to determine the length of the binocular baseline;

[0122] Further extrinsic parameter calibration of the lidar 14 and the binocular camera 15 is performed using the following steps:

[0123] S11: A rectangular plate is selected as the calibration plate. Four circular holes of the same size are set on the rectangular plate, and a three-sided metal reflector is fixed in the center of the calibration plate to enhance the reflectivity of the lidar 14. The center line of the four circular holes forms a rectangle. The radius of the circular holes and the side length of the rectangle are accurately measured with a tape measure.

[0124] S11: The binocular camera 15 acquires images of the circular holes on the calibration plate, uses the Sobel detection operator to create edge images, and extracts information about the circular holes and trihedral reflectors from the images based on the random Hough transform method; the lidar 14 scans the circular hole calibration plate, extracts the point cloud information of the circular holes and trihedral reflectors, performs filtering and noise reduction processing, and uses the random sampling consensus algorithm to refit the calibration plate to obtain a more accurate calibration plate plane;

[0125] S13: Calculate the translation matrix and rotation matrix The details are as follows:

[0126] (a) First assume it is a rotation matrix Given the identity matrix, the translation matrix is ​​roughly calculated through edge feature matching. ;

[0127] (b) Construct a set of feature points detected by the binocular camera 15 and the laser radar 14, and optimize the translation matrix using edge detection error and reprojection error. And calculate the rotation matrix. .

[0128]

[0129] In the formula This represents the set of feature points established by the feature points detected by the lidar 14. This represents the set of feature points established by the feature points detected by the binocular camera 15.

[0130] S14: Calculate the three-dimensional coordinates from the point cloud data of the lidar 14. Converted to pixel coordinates of the camera object 15 As shown in the following formula:

[0131]

[0132] in, Indicates the focal length of the left and right cameras. The z-axis represents the origin of a pixel in a visual sensor. c This represents the z-axis coordinate in the visual coordinate system.

[0133] exist Figure 7 In this process, the data obtained by the lidar 14, the binocular camera 15, the IMU measurement module 16, and the ultra-wideband radar signal generating and receiving module 13 are converted to the world coordinate system, wherein the three-dimensional coordinates in the coordinate system of the binocular camera 15 are converted to the world coordinate system. Three-dimensional coordinates integrated into the world coordinate system The format is as follows:

[0134]

[0135] In the formula This represents the translation parameter from the origin of the camera coordinate system C to the origin of the world coordinate system W; This represents the rotation matrix between the camera coordinate system and the world coordinate system, where The calculation is as follows:

[0136]

[0137] In the formula ;

[0138] ;

[0139] in , , This represents the rotation angle between the camera coordinate system and the three coordinate axes X, Y, and Z of the world coordinate system;

[0140] The positioning results of the IMU measurement module 16 and the UWB positioning terminal module 12 are fused into the world coordinate system using the Bursa seven-parameter model. The fusion error e is written in the following form:

[0141]

[0142] In the formula , , This is the positioning result after fusion of UWB and IMU, where e represents the error matrix of the fusion of IMU and UWB positioning results. , , Mean after fusion.

[0143] Furthermore, the cost function for the fusion error is constructed as follows: According to the least squares principle, the error parameter is obtained when the cost function reaches its minimum value. , , , , , The positioning results of the IMU are corrected by using error parameters, thereby enabling the agricultural machinery body to obtain higher accuracy positioning results.

[0144] The data processing unit 19 calculates the difference between the positioning result of the UWB positioning system and the position estimate of the IMU measurement module 16. If the error is less than a set threshold, the error is fed back and fused into the error state extended Kalman filter model to obtain the final positioning result of the agricultural machinery body. If the difference is greater than the set threshold, the positioning result of the UWB positioning system is used as the final positioning result of the agricultural machinery body. At the same time, the positioning result of the IMU measurement module 16 is corrected for error at the next moment, and the corrected result is used as the positioning result of the IMU measurement module 16.

[0145] In the data processing unit 19, variational capillary Kalman filtering is used to smooth the positioning data of the agricultural machinery body, obtaining more stable attitude and position coordinates. In the data processing unit 19, image matching is performed on radar scan images at two adjacent time points, and the distance between obstacles and the agricultural machinery body is obtained in the data calculation unit 19. When the distance between the obstacle and the agricultural machinery body is less than a set threshold, the A* algorithm is used to replan the working path of the agricultural machinery body; when the distance between the obstacle and the agricultural machinery body is greater than the set threshold, the agricultural machinery body continues to operate according to the originally planned path.

[0146] The method for calculating the distance between the agricultural machinery vehicle and obstacles is as follows: Based on the fusion positioning results of IMU and UWB, the vehicle's own position coordinates in world coordinates are obtained. The lidar 14 and the binocular camera 15 obtain the position coordinates of the obstacle in the world coordinate system. The distance d between the obstacle and the agricultural machinery body can be expressed as:

[0147]

[0148] The process of calculating the obstacle's position coordinates in the world coordinate system is as follows: Based on the pixel values ​​of the obstacle's feature points calculated by the binocular camera 15, the three-dimensional coordinates of the obstacle in the coordinate system of the binocular camera 15 are calculated in the data calculation unit 19. Then, the three-dimensional coordinates of the binocular camera 15 coordinate system are transformed to the world coordinate system to obtain the obstacle's three-dimensional coordinates in the world coordinate system, where the pixel values ​​of the camera feature points are... Depth is The corresponding three-dimensional coordinates in the binocular camera 15 coordinate system It can be represented as:

[0149]

[0150] The above embodiments of the present invention provide a field navigation and positioning system and method for agricultural machinery in GPS-denied environments. The UWB positioning system, composed of a positioning base station and a positioning terminal, enables positioning even in GPS-denied environments. The fusion positioning of UWB and IMU improves the positioning accuracy of the agricultural machinery. In complex environments, the agricultural machinery can achieve high positioning accuracy. A map model of the agricultural machinery can be constructed using a combination of lidar and a binocular camera, simultaneously obtaining the coordinates of obstacles in front of the machinery. The ultra-wideband radar signal receiving module has strong penetration capabilities, enabling the agricultural machinery to accurately determine the distance between itself and obstacles in rainy, cloudy, or foggy weather, allowing for effective obstacle avoidance. When the UWB positioning terminal module on the agricultural machinery exceeds the communication range of the positioning base station, the UWB positioning base station can move along a guide rail, avoiding manual relocation. The trajectory shifting algorithm quickly calculates the coordinates of the relocated positioning base station, enabling long-term continuous operation of the agricultural machinery, improving its working efficiency, and contributing to the intelligentization of the agricultural machinery.

[0151] In order for the above methods and systems to operate smoothly, the system may include more or fewer components than those described above, or combine certain components, or different components, in addition to the various modules mentioned above. For example, it may include input / output devices, network access devices, buses, processors, and memory.

[0152] It should be understood that although the steps in the flowcharts of the various embodiments of the present invention are shown sequentially according to the arrows, these steps are not necessarily executed in the order indicated by the arrows. Unless explicitly stated herein, there is no strict order restriction on the execution of these steps, and they can be executed in other orders. Moreover, at least some steps in the various embodiments may include multiple sub-steps or multiple stages. These sub-steps or stages are not necessarily completed at the same time, but can be executed at different times. The execution order of these sub-steps or stages is not necessarily sequential, but can be performed alternately or in turn with other steps or at least a portion of the sub-steps or stages of other steps.

[0153] The technical features of the above embodiments can be combined in any way. For the sake of brevity, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.

[0154] The embodiments described above are merely illustrative of several implementations of the present invention, and while the descriptions are specific and detailed, they should not be construed as limiting the scope of the present invention. It should be noted that those skilled in the art can make various modifications and improvements without departing from the concept of the present invention, and these modifications and improvements all fall within the scope of protection of the present invention. Therefore, the scope of protection of this patent should be determined by the appended claims.

[0155] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, and improvements made within the spirit and principles of the present invention should be included within the protection scope of the present invention.

Claims

1. A field navigation and positioning system for agricultural machinery in a GPS-denied environment, characterized in that, The system includes: Agricultural machinery body, lidar, ultra-wideband radar signal generator and receiver module, binocular camera, IMU measurement module, UWB positioning base station, UWB positioning terminal module, mobile guide rail, data processing unit, industrial control computer, control module, fixed rod, mobile power supply, high-precision attitude sensor, data display system, connecting fasteners, bracket, first fixing device, second fixing device, fixing clamp, slider, tripod, motor; The top of the agricultural machinery body is equipped with a first fixing device, and the front of the agricultural machinery body is equipped with a second fixing device. Both the first fixing device and the second fixing device include three layers. An industrial control computer is installed on the first fixed device, a UWB positioning terminal module is installed on the top layer of the first fixed device, and an IMU measurement module is arranged in the middle layer of the first fixed device. A binocular camera, a lidar, and an ultra-wideband radar signal generating and receiving module are respectively installed on the second fixed device from bottom to top; The UWB positioning terminal module is fixed to the first fixing device by a fixing clip, the ultra-wideband radar signal generating and receiving module is fixed to the second fixing device by the fixing clip, and the remaining sensors are fixed to their respective fixing devices by bolts. A tripod is installed at the edge of the farmland boundary, with a movable guide rail mounted on the tripod. A fixed rod is mounted on the slider of the movable guide rail, a connecting fastener is mounted on the fixed rod, and a bracket is mounted on the connecting fastener. The UWB positioning base station is mounted on the bracket via the fixed clamp. A high-precision attitude sensor is mounted on the slider. The IMU measurement module transmits the collected data to the data processing unit in the industrial control computer. The data processing unit converts the received measurement data from the IMU measurement module into the position and attitude of the agricultural machinery body. The UWB positioning terminal module obtains the distance between the agricultural machinery vehicle body and the four UWB positioning base stations in real time during the operation of the agricultural machinery vehicle body and transmits it to the data processing unit in the industrial control computer through wireless communication. The data processing unit calculates the three-dimensional position coordinates of the agricultural machinery vehicle body according to the positioning algorithm. The positioning algorithm is specifically: robust weighted least multiplication, overall least squares, and multidimensional calibration algorithm. The lidar collects point cloud data of the agricultural machinery in real time and transmits it to the data processing unit in the industrial control computer. The data processing unit performs point cloud denoising, correction and segmentation on the point cloud data to obtain an irregular triangular mesh of three-dimensional point cloud data. The triangular mesh is interpolated using cubic spline curves. Then, the point cloud data is accurately registered in the irregular triangular mesh to obtain a three-dimensional scene model of the agricultural machinery operation. The binocular camera acquires images of the working environment in front of the agricultural machinery and transmits the images to the data processing unit in the industrial control computer. The data processing unit performs feature point detection and matching on the images and calculates the spatial coordinates of the feature points in the images using the parallax principle. The ultra-wideband radar signal generating and receiving module transmits the received signal to the data processing unit in the industrial control computer. The data processing unit uses an imaging algorithm to obtain an image of the scanning area in front of the agricultural machinery body, thereby obtaining the distance between the agricultural machinery body and the obstacle in front. The imaging algorithm is specifically: compressed sensing imaging algorithm and spotlight imaging algorithm. The high-precision attitude sensor sends attitude data to the Bluetooth data receiving module via the Bluetooth data sending module, and the data processing unit calculates the three-dimensional coordinates of the UWB positioning base station after it moves. The industrial control computer sends signals to the lidar, ultra-wideband radar signal generation and receiving module, binocular camera, IMU measurement module, and UWB positioning terminal module. The lidar, ultra-wideband radar signal generation and receiving module, binocular camera, IMU measurement module, and UWB positioning terminal module receive the signals and latch the signals they have collected the moment they are received. Then, they send the signals to the industrial control computer through the bus, thereby keeping all sensors synchronized in time. The lidar, the ultra-wideband radar signal generator and receiver module, the binocular camera, the UWB positioning terminal module, the IMU measurement module, and the control module are all connected to the industrial control computer, and the output of the industrial control computer is connected to the input of the data display system. The motor drives the slider on the moving guide rail to move via a synchronous belt. The motor is connected to the computer via a serial port, and the portable power supply provides power to the motor.

2. The field navigation and positioning system for agricultural machinery in a GPS-denied environment according to claim 1, characterized in that, In the point cloud data denoising, correction and segmentation processing of the data processing unit, the point cloud denoising adopts the chord height difference method to remove noise points and the median filtering algorithm; the correction adopts the iterative nearest point algorithm, and at the same time uses KI-DTree to speed up the search for nearest points to improve the registration speed; the segmentation processing adopts the surface growth segmentation method.

3. A method for field navigation and positioning of agricultural machinery in a GPS-denied environment, characterized in that, The method includes the following steps: S01: Based on the boundary environment of the farmland area, two movable guide rails are placed on the outside of the boundary edge. The fixing rod is installed on the slider of the guide rail by bolts. Four UWB positioning base stations are installed on the bracket, and high-precision attitude sensors are installed on the slider. S02: Install a lidar, an ultra-wideband radar signal generator and receiver module, a binocular camera, a UWB positioning terminal module, and an IMU measurement module on the first and second fixed devices on the agricultural machinery body, respectively. S03: Select a preset location to establish a positioning coordinate system. Based on the working area of ​​the agricultural machinery vehicle and the communication range of the UWB positioning base station, randomly deploy four UWB positioning base stations and use a total station to measure and calibrate the three-dimensional position coordinates of the four UWB positioning base stations. Input the measurement results of the base stations into the industrial control computer and network the four UWB positioning base stations with the UWB positioning terminal module. S04: The position geometric precision factor (PDOP) is calculated in the industrial control computer. When the PDOP value is greater than 2, the layout of the positioning base stations is optimized in the industrial control computer using an optimization algorithm. The four UWB positioning base stations are readjusted according to the optimization results. When the PDOP value is less than 2, the current layout is taken as the final layout scheme of the UWB positioning base stations. S05: Initialize the binocular camera and the lidar, calibrate the intrinsic parameters of the binocular camera, and calibrate the extrinsic parameters between the binocular camera and the lidar to establish the coordinate system relationship between the binocular camera and the lidar; S06: Determine the current weather conditions. If the current weather is cloudy, rainy, or foggy, activate the ultra-wideband radar signal receiving module. In the data processing unit, obtain the distance between the agricultural machinery vehicle and the obstacle based on the signal from the ultra-wideband radar signal receiving module. If the weather is sunny, activate the binocular camera and the lidar. If the ultra-wideband radar signal receiving module is activated, the distance between the agricultural machinery vehicle and the obstacle is obtained in the data processing unit based on the signal from the ultra-wideband radar signal receiving module; if the binocular camera and the lidar are activated, a three-dimensional map model is constructed in the industrial control computer to determine the current operating status of the agricultural machinery vehicle in real time, and at the same time, the three-dimensional position coordinates of the obstacle in front of the agricultural machinery vehicle are obtained, and the distance between the agricultural machinery vehicle and the obstacle is obtained in the data processing unit. S07: Start the UWB positioning terminal module and the IMU measurement module, obtain the agricultural machinery body position and attitude fused by UWB and IMU in the data processing unit, and transmit the calculation results to the data display system to display the positioning results in real time; S08: The agricultural machinery vehicle body operates according to the preset operation path. The existence of obstacles in front of the agricultural machinery vehicle body is detected in real time through step S6, the distance between the agricultural machinery vehicle body and the obstacles is obtained, the operation path that the agricultural machinery vehicle body can pass is obtained by artificial potential field method, and the planning information is transmitted to the control module. The control module sends a control signal to drive the agricultural machinery vehicle body to run according to the planned path. S09: The counter module determines whether the agricultural machinery has completed 10 cycles of operation. If the agricultural machinery has completed 10 cycles, the control motor drives the slider to move along the guide rail. Based on the measurement data of the high-precision attitude sensor and the moving distance of the slider, the trajectory tracking algorithm is used to calculate the three-dimensional position coordinates of the four UWB positioning base stations after the movement in the data processing unit, and the three-dimensional position coordinates of the base stations after the movement are transmitted to the industrial control computer to perform continuous cyclic positioning of the agricultural machinery.

4. The method for field navigation and positioning of agricultural machinery in a GPS-denied environment according to claim 3, characterized in that, The PDOP value is obtained from the observation matrix H, the weighting matrix W, and the matrix M, where the observation matrix H and the weighting matrix W are respectively... ； ； In the formula This represents the coordinates of the agricultural machinery's body position calculated by the UWB system. This represents the i-th UWB positioning base station. Let represent the variance of the ranging error of the i-th UWB positioning base station, where i = 1, 2, 3, 4; The calculation process for matrix M is as follows: ； Position accuracy factor PDOP can be expressed as: ； In the formula M jj Let j represent the diagonal elements of matrix M, where j = 1, 2, 3; The layout of the positioning base stations is optimized using optimization algorithms in the industrial control computer, specifically: a virus intrusion optimization algorithm or a water circulation optimization algorithm.

5. The method for field navigation and positioning of agricultural machinery in a GPS-denied environment according to claim 4, characterized in that, The specific steps for calibrating a stereo camera include: calibrating the intrinsic parameters of the left and right cameras separately using the Zhang Zhengyou calibration method; and calibrating the radial distortion coefficients of the left and right cameras. and tangential distortion coefficient Calibration is performed; the relative pose relationship between the left and right cameras is calibrated to determine the length of the binocular baseline.

6. The method for field navigation and positioning of agricultural machinery in a GPS-denied environment according to claim 3, characterized in that, The specific steps for calibrating the extrinsic parameters of the lidar and the binocular camera include: S11: A rectangular plate is selected as the calibration plate. Four circular holes of the same size are set on the rectangular plate, and a three-sided metal reflector is fixed in the center of the calibration plate to enhance the reflectivity of the laser radar. The center line of the four circular holes forms a rectangle. The radius of the circular holes and the side length of the rectangle are accurately measured with a tape measure. S12: The binocular camera acquires images of the circular holes on the calibration plate, uses the Sobel detection operator to create edge images, and extracts information about the circular holes and trihedral reflectors from the images based on the random Hough transform method; the lidar scans the circular hole calibration plate, extracts the point cloud information of the circular holes and trihedral reflectors, performs filtering and noise reduction processing, and uses the random sampling consensus algorithm to refit the calibration plate to obtain a more accurate calibration plate plane; S13: Calculate the translation matrix and rotation matrix The details are as follows: (a) First assume it is a rotation matrix Given the identity matrix, the translation matrix is ​​roughly calculated through edge feature matching. ; (b) Construct a set of feature points detected by the binocular camera and the lidar, and optimize the translation matrix using edge detection error and reprojection error. And calculate the rotation matrix. ; ； In the formula This represents the set of feature points established by the feature points detected by the lidar. This represents the set of feature points established by the feature points detected by the binocular camera; S14: 3D coordinates from lidar point cloud data Convert to pixel coordinates of camera object As shown in the following formula: ； in, Indicates the focal length of the left and right cameras. The z-axis represents the origin of a pixel in a visual sensor. c This represents the z-axis coordinate in the visual coordinate system.

7. The method for field navigation and positioning of agricultural machinery in a GPS-denied environment according to claim 4, characterized in that, The data obtained by the lidar, the binocular camera, the IMU measurement module, and the ultra-wideband radar signal generator / receiver module are converted to the world coordinate system, wherein the three-dimensional coordinates in the binocular camera coordinate system are converted to the world coordinate system. Three-dimensional coordinates integrated into the world coordinate system The format is as follows: ； In the formula This represents the translation parameter from the origin of the camera coordinate system C to the origin of the world coordinate system W; This represents the rotation matrix between the camera coordinate system and the world coordinate system, where The calculation is as follows: ； In the formula ; ； in , , This represents the rotation angle between the camera coordinate system and the three coordinate axes X, Y, and Z of the world coordinate system.

8. The method for field navigation and positioning of agricultural machinery in a GPS-denied environment according to claim 4, characterized in that, The positioning results of the IMU measurement module and the UWB positioning terminal module are fused into the world coordinate system using the Bursa seven-parameter model. The fusion error e is written in the following form: ； In the formula , , This is the positioning result after fusion of UWB and IMU, where e represents the error matrix of the fusion of IMU and UWB positioning results. , , Mean after fusion; The cost function for constructing the fusion error is: According to the least squares principle, the error parameter is obtained when the cost function reaches its minimum value. , , , , , The positioning results of the IMU are corrected by using error parameters, thereby improving the positioning results of the agricultural machinery. The data processing unit calculates the difference between the positioning result of the UWB positioning system and the position estimate of the IMU measurement module. If the error is less than a set threshold, the error is fed back and fused into the error state-based extended Kalman filter model to obtain the final positioning result of the agricultural machinery vehicle. If the error is not less than the set threshold, the positioning result of the UWB positioning system is used as the final positioning result of the agricultural machinery vehicle. At the same time, the positioning result of the IMU measurement module is corrected for error at the next moment, and the corrected result is used as the positioning result of the IMU measurement module. The data processing unit employs variational volumetric Kalman filtering to smooth the positioning data of the agricultural machinery, thereby obtaining more stable attitude and position coordinates.

9. The method for field navigation and positioning of agricultural machinery in a GPS-denied environment according to claim 4, characterized in that, In the data processing unit, image matching is performed on radar scan images at two adjacent time points. In the data calculation unit, the distance between obstacles and the agricultural machinery is obtained. When the distance between the obstacle and the agricultural machinery is less than a set threshold, the A* algorithm is used to replan the agricultural machinery's operating path. When the distance is not less than the set threshold, the agricultural machinery continues to operate according to the originally planned path. The method for calculating the distance between the agricultural machinery and obstacles is as follows: the position coordinates of the agricultural machinery in world coordinates are obtained based on the fusion positioning results of IMU and UWB. The lidar and binocular camera obtain the position coordinates of the obstacle in the world coordinate system. The distance d between the obstacle and the agricultural machinery body is represented as: 。 10. The method for field navigation and positioning of agricultural machinery in a GPS-denied environment according to claim 9, characterized in that, The process of calculating the obstacle's position coordinates in the world coordinate system is as follows: Based on the pixel values ​​of the obstacle's feature points calculated by the binocular camera, the three-dimensional coordinates of the obstacle in the binocular camera coordinate system are calculated in the data processing unit. Then, the three-dimensional coordinates of the binocular camera coordinate system are transformed to the world coordinate system to obtain the obstacle's three-dimensional coordinates in the world coordinate system, where the pixel values ​​of the camera feature points are... Depth is 3D coordinates in the corresponding binocular camera coordinate system It can be represented as: 。

Images