CN-117451058-B - Agricultural machinery vehicle body field navigation positioning system and method under GPS refusing environment

CN117451058BCN 117451058 BCN117451058 BCN 117451058BCN-117451058-B

Abstract

The invention is suitable for the technical field of agricultural machinery field navigation positioning, and provides an agricultural machinery vehicle body field navigation positioning system and method under a GPS refusing environment, wherein the system comprises an agricultural machinery vehicle body; the top of the agricultural machine body is provided with a first fixing device, the agricultural machine body is provided with a second fixing device, and the first fixing device and the second fixing device both comprise three layers of positions; the industrial personal computer is installed on the first fixing device, the UWB positioning terminal module is also installed on the first fixing device, and the IMU measuring module is distributed in the middle layer of the first fixing device for installation; the agricultural machinery vehicle body has the advantages that the agricultural machinery vehicle body can obtain higher positioning precision, can effectively avoid barriers and is beneficial to intelligent realization.

Inventors

CAO BO
GAO SHANG
ZHU PENGCHENG
WAN CHUANPING
WANG JIAN
JIANG CHUNXIA
DONG WENBIN
ZHU WANJIE
ZHANG YAJING
CHEN FENG
ZHANG HUA
FANG SHUPING

Assignees

安徽科技学院

Dates

Publication Date: 20260505
Application Date: 20221208

Claims (10)

1. An agricultural machinery vehicle body field navigation positioning system under a GPS refusing environment, which is characterized by comprising: The agricultural machinery vehicle comprises an agricultural machinery vehicle body, a laser radar, an ultra wideband radar signal generation and receiving module, a binocular camera, an IMU measuring module, a UWB positioning base station, a UWB positioning terminal module, a movable guide rail, a data processing unit, an industrial personal computer, a control module, a fixed rod, a movable power supply, a high-precision attitude sensor, a data display system, a connecting fixing piece, a bracket, a first fixing device, a second fixing device, a fixed clamp, a sliding block, a tripod and a motor; the top of the agricultural machine body is provided with a first fixing device, the right front of the agricultural machine body is provided with a second fixing device, and the first fixing device and the second fixing device both comprise three layers of positions; an industrial personal computer is installed on the first fixing device, a UWB positioning terminal module is installed on the uppermost layer of the first fixing device, and an IMU measuring module is distributed in the middle layer of the first fixing device; A binocular camera, a laser radar and an ultra-wideband radar signal generating and receiving module are respectively arranged on the second fixing device from bottom to top; the UWB positioning terminal module is fixed on the first fixing device through a fixing clamp, the ultra-wideband radar signal generating and receiving module is fixed on the second fixing device through the fixing clamp, and the rest sensors are fixed on the corresponding fixing devices through bolts; A tripod is arranged in the edge area of the boundary of the positioning farmland, a movable guide rail is arranged on the tripod, a fixed rod is arranged on a sliding block of the movable guide rail, a connecting fixing piece is arranged on the fixed rod, a bracket is arranged on the connecting fixing piece, and a UWB positioning base station is arranged on the bracket through the fixing clamp; the IMU measuring module transmits the acquired data to a data processing unit in the industrial personal computer, and the data processing unit converts the received measured data of the IMU measuring module into the position and the posture of the agricultural vehicle body; The UWB positioning terminal module obtains distances between the 4 UWB positioning base stations in the operation process of the agricultural machine body in real time and transmits the distances to the data processing unit in the industrial personal computer through wireless communication, and the data processing unit calculates three-dimensional position coordinates of the agricultural machine body according to a positioning algorithm, wherein the positioning algorithm comprises a robustness weighted least square method, an overall least square method and a multidimensional calibration algorithm; the laser radar acquires point cloud data in real time during operation of the agricultural machinery vehicle body and transmits the point cloud data to a data processing unit in the industrial personal computer, the data processing unit performs point cloud denoising, correcting and dividing processing on the point cloud data to obtain an irregular triangular network of three-dimensional point cloud data, the triangular network is subjected to interpolation processing by using a cubic spline curve, and then the point cloud data is accurately registered in the irregular triangular network to obtain a three-dimensional scene model of the agricultural machinery vehicle body operation; The binocular camera is used for collecting images of the working environment in front of the agricultural machine body, transmitting the images to a data processing unit in the industrial personal computer, detecting and matching characteristic points of the images in the data processing unit, and calculating space coordinates of the characteristic points in the images by utilizing a parallax principle; The ultra-wideband radar signal generation and reception module transmits the received signal to the data processing unit in the industrial personal computer, and an imaging algorithm is used in the data processing unit to acquire imaging of a front scanning area of the agricultural vehicle body so as to acquire the distance between the agricultural vehicle body and a front obstacle, wherein the imaging algorithm comprises a compressed sensing imaging algorithm and a beam-focusing imaging algorithm; the high-precision attitude sensor transmits attitude data to the Bluetooth data receiving module through the Bluetooth data transmitting module, and the data processing unit calculates three-dimensional coordinates of the UWB positioning base station after moving; The industrial personal computer sends signals to the laser radar, the ultra-wideband radar signal generation and receiving module, the binocular camera, the IMU measuring module and the UWB positioning terminal module, the laser radar, the ultra-wideband radar signal generation and receiving module, the binocular camera, the IMU measuring module and the UWB positioning terminal module receive the signals, latch the signals acquired by the industrial personal computer at the moment of receiving the signals, and then send the signals to the industrial personal computer through the bus, so that all sensors keep time synchronization; the laser radar, the ultra wideband radar signal generation and reception module, the binocular camera, the UWB positioning terminal module, the IMU measurement module and the control module are all connected with the industrial personal computer, and the output of the industrial personal computer is connected with the input end of the data display system; The motor drives the sliding blocks on the movable guide rail to move through the synchronous belt, the motor is connected with the computer through the serial port, and the movable power supply supplies power for the motor.
2. The agricultural machinery vehicle body field navigation positioning system under the GPS rejection environment according to claim 1 is characterized in that in the point cloud denoising, correction and segmentation processing of point cloud data by the data processing unit, the point cloud denoising adopts a chord height difference method to remove noise points and a median filtering algorithm, the correction adopts an iterative nearest point algorithm, meanwhile, near point searching is accelerated by using KI-DTree to improve registration speed, and the segmentation processing adopts a surface growth segmentation method.
3. The method for navigating and positioning the farm machinery vehicle body in the field under the GPS refusing environment is characterized by comprising the following steps: s01, according to the boundary environment of a farmland area, two movable guide rails are placed on the outer sides of the boundary edges, fixing rods are installed on sliding blocks of the guide rails through bolts, 4 UWB positioning base stations are installed on a bracket, and meanwhile high-precision attitude sensors are installed on the sliding blocks; S02, respectively installing a laser radar, an ultra-wideband radar signal generation and reception module, a binocular camera, a UWB positioning terminal module and an IMU measurement module on a first fixing device and a second fixing device on an agricultural machinery vehicle body; S03, selecting a preset position to establish a positioning coordinate system, randomly deploying 4 UWB positioning base stations according to the communication range of the operation area of the agricultural machinery vehicle body and the UWB positioning base stations, measuring and calibrating three-dimensional position coordinates of the 4 UWB positioning base stations by using a total station, inputting a measuring result of the base stations into an industrial personal computer, and networking the 4 UWB positioning base stations with the UWB positioning terminal module; S04, calculating a position geometric precision factor PDOP value in the industrial personal computer, optimizing the layout mode of the positioning base station by utilizing an optimization algorithm in the industrial personal computer when the PDOP value is larger than 2, and readjusting 4 UWB positioning base stations according to the optimized result; S05, initializing the binocular camera and the laser radar, calibrating internal parameters of the binocular camera, calibrating external parameters between the binocular camera and the laser radar, and constructing a coordinate system relation between the binocular camera and the laser radar; S06, judging the current weather condition, if the current weather is cloudy, rainy or foggy, starting the ultra-wideband radar signal generating and receiving module, and obtaining the distance between the agricultural machinery vehicle body and the obstacle according to the signal of the ultra-wideband radar signal generating and receiving module in the data processing unit; If the binocular camera and the laser radar are started, a three-dimensional map model is built in the industrial personal computer, the current running state of the agricultural vehicle body is judged in real time, meanwhile, the three-dimensional position coordinates of the obstacle in front of the agricultural vehicle body are obtained, and the distance between the agricultural vehicle body and the obstacle is obtained in the data processing unit; s07, starting the UWB positioning terminal module and the IMU measuring module, obtaining the position and the posture of the agricultural machinery vehicle body fused with UWB and IMU in the data processing unit, transmitting the resolving result to a data display system, and displaying the positioning result in real time; S08, the agricultural machinery vehicle body works according to a preset working path, the existence of an obstacle in front of the agricultural machinery vehicle body is detected in real time through the step S6, the distance between the agricultural machinery vehicle body and the obstacle is obtained, a working path which can be passed through by the agricultural machinery vehicle body is obtained by adopting an artificial potential field method, planning information is transmitted to a control module, and the control module sends a control signal to drive the agricultural machinery vehicle body to run according to the planning path; And S09, judging whether the agricultural machine body completes 10 times of circulating operation through a counter module, if the agricultural machine body completes 10 times of operation, controlling a motor to drive a sliding block to move along a guide rail, calculating three-dimensional position coordinates of the 4 UWB positioning base stations after movement in the data processing unit by adopting a track pushing algorithm according to the measurement data of the high-precision attitude sensor and the moving distance of the sliding block, and transmitting the three-dimensional position coordinates of the base stations after movement to the industrial personal computer to perform continuous circulating positioning on the agricultural machine body.
4. The method for positioning agricultural machinery vehicle body in field navigation under the GPS refusing environment according to claim 3, wherein the PDOP value is obtained by an observation matrix H, a weighting matrix W and a matrix M, wherein the observation matrix H and the weighting matrix W are respectively ; ; In the middle of Representing the agricultural machinery vehicle body position coordinates calculated by the UWB system, Indicating that the ith UWB positioning base station, Representing the range error variance of the ith UWB positioning base station, where i = 1,2,3,4; the calculation process of the matrix M is as follows: ; the position accuracy factor PDOP can be expressed as: ; Where M jj denotes the diagonal elements of matrix M, where j=1, 2,3; And optimizing the layout mode of the positioning base station by utilizing an optimization algorithm in the industrial personal computer, wherein the optimization algorithm is specifically a virus invasion optimization algorithm or a water circulation optimization algorithm.
5. The method for positioning agricultural machinery vehicle body in field navigation under the GPS rejection environment according to claim 4, wherein the specific calibration steps of the binocular camera comprise calibrating the respective internal references of the left and right cameras by using a Zhang Zhengyou-based calibration method, and calibrating the radial distortion coefficients of the left and right cameras And tangential distortion coefficient Calibrating the relative pose relationship of the left camera and the right camera, and determining the length of the binocular base line.
6. The method for positioning agricultural machinery vehicle body in field navigation under the GPS refusing environment according to claim 3, wherein the specific steps of calibrating the external parameters of the laser radar and the binocular camera comprise: S11, selecting a rectangular plate as a calibration plate, arranging four round holes with the same size on the rectangular plate, fixing a three-sided metal reflector at the center of the calibration plate, and enhancing the laser radar reflection capability, wherein the connecting line of the circle centers of the four round holes forms a rectangle, and accurately measuring the radius of the round hole and the side length of the rectangle by using a tape measure; S12, acquiring a round hole image of a calibration plate by the binocular camera, creating an edge image by using a sobel detection operator, and extracting information of round holes and a trihedron reflector in the image based on a random Hough transformation method; s13, calculating a translation matrix Rotation matrix The method is characterized by comprising the following steps: (a) First assume a rotation matrix Is a unit matrix, and a translation matrix is roughly calculated through edge feature matching ; (B) Constructing a feature point set detected by the binocular camera and the laser radar, and optimizing a translation matrix by using edge detection errors and reprojection errors And calculates a rotation matrix ; ; In the middle of Representing a set of feature points established by the feature points detected by the lidar, Representing a feature point set established by the feature points detected by the binocular camera; s14, three-dimensional coordinates under laser radar point cloud data Conversion to pixel coordinates of camera objects The following formula: ; Wherein, the Representing the focal length of the left and right cameras, Representing the origin of the visual sensor pixel point, and z c represents the z-axis coordinate in the visual coordinate system.
7. The method for field navigation and positioning of an agricultural vehicle body in a GPS reject environment according to claim 4, wherein data obtained by the laser radar, the binocular camera, the IMU measurement module, the ultra wideband radar signal generation and reception module is converted into a world coordinate system, wherein three-dimensional coordinates in the binocular camera coordinate system are converted into Three-dimensional coordinates fused to world coordinate system The form of (2) is as follows: ; In the middle of An origin translation parameter representing an origin of the camera coordinate system C to the world coordinate system W; representing a rotation matrix between a camera coordinate system and a world coordinate system, wherein Is calculated as follows: ; In the middle of ; ; Wherein the method comprises the steps of , , Representing the rotation angle between the camera coordinate system and the three coordinate axes X, Y, Z of the world coordinate system.
8. The method for positioning the agricultural machinery vehicle body in the field navigation under the GPS rejection environment according to claim 4, wherein the positioning results of the IMU measurement module and the UWB positioning terminal module are fused under a world coordinate system by using a Boolean seven parameter model, and the fused error e is written as follows: ; In the middle of , , Is the positioning result after the fusion of UWB and IMU, e represents the error matrix of the fusion of IMU and UWB positioning result, , , The cost function for constructing the fusion error is as follows: obtaining error parameters when the cost function reaches the minimum value according to the least square method principle , , , , , Correcting the positioning result of the IMU by utilizing the error parameter, so that the agricultural machinery vehicle body obtains the positioning result and improves; Calculating the difference between the positioning result of the UWB positioning system and the position estimation value of the IMU measuring module in the data processing unit, if the error is smaller than a set threshold value, fusing the error feedback to an expanded Kalman filtering model based on an error state to obtain the positioning result of the final agricultural vehicle body; and smoothing the positioning data of the agricultural machinery by adopting variable volume Kalman filtering in the data processing unit to obtain stable gesture and position coordinates.
9. The GPS refuse environment agricultural machinery vehicle body field navigation positioning method according to claim 4, characterized by that in the data processing unit the radar scan images of adjacent two moments are undergone the process of image matching, in the data resolving unit the distance between the obstacle and agricultural machinery vehicle body is obtained, when the distance between the obstacle and agricultural machinery vehicle body is less than the set threshold value, the operation path of agricultural machinery vehicle body is re-planned by using A-algorithm, when the distance between the obstacle and agricultural machinery vehicle body is not less than the set threshold value, the agricultural machinery vehicle body can continuously operate according to original planned path, and the calculation method of distance between agricultural machinery vehicle body and obstacle is characterized by that according to the fusion positioning result of IMU and UWB the position coordinate of agricultural machinery vehicle body itself under the world coordinate is obtained The laser radar and the binocular camera obtain the position coordinates of the obstacle in the world coordinate system as follows The distance d between the obstacle and the agricultural vehicle body is expressed as: 。
10. The method for field navigation and positioning of an agricultural vehicle body in a GPS rejection environment according to claim 9, wherein the process of calculating the position coordinates of the obstacle in the world coordinate system comprises calculating the three-dimensional coordinates of the obstacle in the binocular camera coordinate system in the data calculation unit based on the pixels of the obstacle feature points calculated by the binocular camera, and converting the three-dimensional coordinates of the binocular camera coordinate system into the world coordinate system to obtain the three-dimensional coordinates of the obstacle in the world coordinate system, wherein the pixels in the camera feature points are Depth of Three-dimensional coordinates in corresponding binocular camera coordinate system Can be expressed as: 。

Description

Agricultural machinery vehicle body field navigation positioning system and method under GPS refusing environment Technical Field The invention belongs to the technical field of agricultural machinery field navigation positioning, and particularly relates to an agricultural machinery vehicle body field navigation positioning system and method under a GPS refusing environment. Background The automatic navigation technology of the agricultural machinery is the core for realizing intelligent agriculture, can effectively lighten the labor intensity of operators of an agricultural machinery vehicle body and improves the operation precision and the operation efficiency. In recent years, various students have proposed a plurality of methods for positioning the agricultural vehicle body in a navigation way, and the positioning methods commonly used in the automatic driving technology of the agricultural vehicle body comprise GPS positioning, machine vision positioning, inertial navigation positioning and the like, and the GPS positioning system can achieve the positioning precision of centimeter level in an outdoor open environment, but due to the influence of the environment and the shielding of obstacles, the GPS signal is weakened or no GPS signal, so that the GPS positioning system is difficult to provide continuous and reliable positioning precision. The inertial navigation positioning technology is not shielded by signals, but the inertial measurement element has drift phenomenon along with the increase of the operation time and distance, and the positioning error of inertial navigation is continuously and cumulatively increased and the positioning precision is gradually reduced after the agricultural machine body is subjected to several rounds of operation. The single sensor has certain limitation, and in order to improve the navigation positioning precision and reliability, multi-sensor fusion positioning is often adopted. The application number is 202110124820.1, which discloses an agricultural machinery field positioning system and method based on UWB, wherein a UWB positioning tag and an IMU inertial measurement unit are installed on an agricultural machinery vehicle body, a base station is installed at the edge of a positioning field block, the IMU is corrected and compensated by using the positioning result of UWB, and the positioning precision of the agricultural machinery vehicle body is improved, but when the communication range of the UWB positioning tag is not in the range of the base station, fusion positioning cannot be carried out. The application number is 202011424886.4, which discloses a navigation vehicle obstacle detection method based on a laser radar, wherein the laser radar and a satellite antenna are arranged on an agricultural vehicle body to judge an obstacle possibly existing in front of the vehicle, but under the condition of no satellite signal, accurate positioning precision is difficult to obtain, and the obstacle judgment is inaccurate. The application number is 201710646549.1, which discloses a night panoramic vision relative positioning system and method of an autonomous navigation tractor, wherein three groups of binocular vision systems are arranged in an equilateral triangle manner and are arranged at the top of the tractor, the three groups of binocular vision systems detect the same target in the environment in the respective directions at the same time, the movement of an agricultural machinery vehicle body is reversely pushed according to the detection result, displacement vectors under different coordinate systems are formed, and the displacement vectors are converted into the same coordinate system to realize relative positioning, but the method is difficult to construct a three-dimensional navigation map, and cannot be used for initial judgment of the position of an obstacle. Particularly, under the GPS refusing environment, how to provide high-precision positioning and obstacle avoidance for the agricultural vehicle body is a key for realizing intelligent operation of the agricultural vehicle body. Aiming at the situation that GPS refuses to be used, the traditional single positioning technology is difficult to meet the requirement of long-term autonomous circulation operation of the agricultural machine body, the positioning precision is low, the requirement of automatic operation of the agricultural machine body cannot be met, a great gap is reserved between the GPS refusing single positioning technology and the agricultural machine body autonomous accurate operation, and therefore, a novel technology and a novel method are needed to solve the long-term high-precision autonomous positioning problem of the agricultural machine body. Disclosure of Invention The embodiment of the invention aims to provide a field navigation positioning system and method for an agricultural machinery vehicle body in a GPS refusing environment, and aims to solve the problems in the ba