Greenhouse agricultural machinery vehicle positioning method and system, computer device and storage medium

Abstract

This invention belongs to the field of positioning technology and discloses a method, system, computer equipment, and storage medium for locating agricultural machinery vehicles in greenhouses. The method includes: acquiring several consecutive photos of working scenes and IMU data of each photo; obtaining camera relative pose data and IMU relative pose data for each photo; constructing a 3D camera position map and extracting the camera relative coordinates of each photo from the 3D camera position map; obtaining the gravity, camera velocity, and camera scale of each photo based on the camera relative pose data and IMU relative pose data; transforming the camera relative coordinates of each photo in the 3D camera position map to a horizontal coordinate system; and finally fusing GNSS satellite data and the camera relative coordinates of each photo in the horizontal coordinate system to obtain the positioning coordinates of the agricultural machinery vehicles. This method can accurately determine the positioning coordinates of agricultural machinery vehicles, ensuring accurate positioning, and is cost-effective.

Description

TECHNICAL FIELD

[0001] The application belongs to the technical field of positioning, and relates to a greenhouse agricultural vehicle positioning method and system, computer equipment and a storage medium. BACKGROUND

[0002] The agricultural vehicle automatic driving is a cross-disciplinary technology involving computer science, pattern recognition, electronics, communication and control. The technology is to perceive the environmental information around the vehicle, combine the positioning of the vehicle itself, plan the most suitable path for walking and working, control the direction and speed of the agricultural vehicle, and finally realize the autonomous walking and working of the agricultural vehicle. The agricultural vehicle automatic driving technology has important significance for reducing the labor intensity of agriculture, improving work efficiency and improving agricultural productivity. In order to realize the autonomous walking of the agricultural vehicle, accurate positioning needs to be realized during the driving process of the agricultural vehicle.

[0003] Computer vision positioning, satellite positioning and inertial navigation are key technologies for calculating the pose information and navigation positioning of the vehicle during driving in the field of automatic driving, and play a crucial role in automatic driving, which is one of the prerequisites for successful automatic driving. At present, the navigation and positioning function of automatic driving is mostly realized by GNSS (Global Navigation Satellite System) plus radar system. Common radar systems such as laser radar and millimeter wave radar.

[0004] However, GNSS has the phenomenon of signal loss in indoor or weak signal working scenarios. At the same time, although the positioning accuracy of the radar system is relatively accurate in high-speed motion mode, for the low-speed or static working scenarios commonly seen in agricultural operations, the perception effect of the radar system is poor, and it cannot achieve the positioning accuracy required by the agricultural vehicle automatic driving and its cost is very high. This leads to the fact that the GNSS plus radar system cannot provide accurate positioning information for the agricultural vehicle automatic driving operation in agricultural scenarios such as greenhouses. SUMMARY

[0005] The purpose of the present application is to overcome the shortcomings of inaccurate positioning information of agricultural vehicles in the prior art, and to provide a greenhouse agricultural vehicle positioning method, system, computer equipment and storage medium.

[0006] In order to achieve the above purpose, the following technical solutions are adopted:

[0007] The first aspect of the present application is a greenhouse agricultural vehicle positioning method, comprising:

[0008] Obtaining a plurality of continuous working scene photos and IMU data of each working scene photo;

[0009] According to the several continuous working scene photos and the IMU data of each working scene photo, camera relative pose data and IMU relative pose data of each working scene photo are obtained;

[0010] According to the camera relative pose data of each working scene photo, a camera position three-dimensional graph is constructed, and camera relative coordinates of each working scene photo in the camera position three-dimensional graph are extracted;

[0011] According to the camera relative pose data and the IMU relative pose data of each working scene photo, gravity, camera speed and camera scale of each working scene photo are obtained;

[0012] According to the gravity, camera speed and camera scale of each working scene photo, the camera relative coordinates of each working scene photo in the camera position three-dimensional graph are converted to a horizontal coordinate system, and camera relative coordinates of each working scene photo in the horizontal coordinate system are obtained;

[0013] GNSS satellite data of any working scene photo is obtained, and according to the camera relative coordinates of each working scene photo in the horizontal coordinate system and the GNSS satellite data of any working scene photo, through a ceres optimizer, a farm vehicle positioning coordinate is obtained.

[0014] Optionally, the camera relative pose data and the IMU relative pose data of each working scene photo are obtained according to the several continuous working scene photos and the IMU data of each working scene photo, including:

[0015] The feature points in the preset position of each working scene photo are tracked by using an optical flow method to obtain feature point tracking results, and the camera relative pose data of each working scene photo is calculated according to the feature point tracking results;

[0016] The IMU data of each working scene photo is pre-integrated by using a midpoint interpolation method to obtain the IMU relative pose data of each working scene photo.

[0017] Optionally, the camera relative pose data of each working scene photo is obtained according to the camera relative rotation data of each working scene photo, including:

[0018] According to the camera relative rotation data of each working scene photo, a first three-dimensional graph is constructed from the first working scene photo to the last working scene photo by using a PNP algorithm, and a second three-dimensional graph is constructed from the last working scene photo to the first working scene photo, and the first three-dimensional graph and the second three-dimensional graph are fused to obtain a camera position three-dimensional graph; the camera position three-dimensional graph is optimized by using a ceres optimizer, and the camera relative coordinates of each working scene photo in the optimized camera position three-dimensional graph are extracted.

[0019] Optionally, the obtaining of the gravity, the camera speed and the camera scale of each working scene photo according to the camera relative pose data and the IMU relative pose data of each working scene photo comprises:

[0020] calculating the IMU offset matrix of each working scene photo according to the camera relative pose data and the IMU relative pose data of each working scene photo;

[0021] optimizing the IMU relative pose data of each working scene photo according to the camera relative pose data, the IMU data and the IMU offset matrix of each working scene photo, obtaining the optimized IMU relative pose data of each working scene photo, and calculating the Jacobian matrix and the covariance matrix of each working scene photo;

[0022] obtaining the gravity, the camera speed and the camera scale of each working scene photo by using the ceres optimizer according to the optimized IMU relative pose data, the Jacobian matrix and the covariance matrix of each working scene photo.

[0023] Optionally, the calculating of the Jacobian matrix and the covariance matrix of each working scene photo comprises:

[0024] setting the Jacobian matrix of the first working scene photo as a unit matrix and the covariance matrix of the first working scene photo as a zero matrix;

[0025] the Jacobian matrix of the second working scene photo to the last working scene photo is respectively the Jacobian matrix of the last working scene photo multiplied by the IMU offset matrix;

[0026] the covariance matrix of the second working scene photo to the last working scene photo is respectively the covariance matrix of the last working scene photo added by a noise matrix; the noise matrix is composed of the square of the acceleration and the IMU noise in the IMU data of the current working scene photo.

[0027] Optionally, the converting of the camera relative coordinates of each working scene photo in the camera position three-dimensional graph to the horizontal coordinate system according to the gravity, the camera speed and the camera scale of each working scene photo comprises:

[0028] optimizing the camera relative coordinates of each working scene photo in the camera position three-dimensional graph by using the minimum reprojection error method according to the camera scale of each working scene photo, obtaining the optimized camera relative coordinates of each working scene photo;

[0029] fixing the first working scene photo as the world coordinate system, and calculating the rotation data of the current world coordinate system relative to the horizontal coordinate system according to the gravity of each working scene photo;

[0030] According to rotation data of the current world coordinate system relative to the horizontal coordinate system, and camera speed and camera scale of each working scene photo, the optimized camera relative coordinates of each working scene photo are converted to the horizontal coordinate system, to obtain the camera relative coordinates of each working scene photo under the horizontal coordinate system.

[0031] Optionally, the agricultural machinery vehicle positioning coordinates are obtained by the ceres optimizer according to the camera relative coordinates of each working scene photo under the horizontal coordinate system and GNSS satellite data of any working scene photo.

[0032] GNSS satellite data of any working scene photo is obtained, and the global coordinates of the working scene photo are obtained by solving the global pose graph model by the ceres optimizer, as the agricultural machinery vehicle positioning coordinates.

[0033] In the second aspect of the present application, a greenhouse agricultural machinery vehicle positioning system comprises:

[0034] The data acquisition module is configured to acquire a plurality of continuous working scene photos and IMU data of each working scene photo.

[0035] The pose processing module is configured to obtain camera relative pose data and IMU relative pose data of each working scene photo according to the plurality of continuous working scene photos and the IMU data of each working scene photo.

[0036] The relative coordinate determination module is configured to construct a camera position three-dimensional graph according to the camera relative pose data of each working scene photo, and extract camera relative coordinates of each working scene photo in the camera position three-dimensional graph.

[0037] The optimization module is configured to obtain gravity, camera speed and camera scale of each working scene photo according to the camera relative pose data and the IMU relative pose data of each working scene photo.

[0038] The coordinate conversion module is configured to convert the camera relative coordinates of each working scene photo in the camera position three-dimensional graph to the horizontal coordinate system according to the gravity, camera speed and camera scale of each working scene photo, to obtain the camera relative coordinates of each working scene photo under the horizontal coordinate system.

[0039] The positioning module is configured to obtain GNSS satellite data of any working scene photo, and obtain the agricultural machinery vehicle positioning coordinates by the ceres optimizer according to the camera relative coordinates of each working scene photo under the horizontal coordinate system and the GNSS satellite data of any working scene photo.

[0040] In a third aspect, the application provides a computer device, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor implements the steps of the greenhouse agricultural vehicle positioning method when executing the computer program.

[0041] In a fourth aspect, the application provides a computer readable storage medium, which stores a computer program, wherein the computer program implements the steps of the greenhouse agricultural vehicle positioning method when executed by a processor.

[0042] Compared with the prior art, the application has the following beneficial effects:

[0043] The greenhouse agricultural vehicle positioning method of the application obtains camera relative pose data and IMU relative pose data of each working scene photo according to a plurality of continuous working scene photos and IMU data of each working scene photo, then constructs a camera position three-dimensional graph according to the camera relative pose data of each working scene photo, realizes acquisition of the camera relative coordinates of each working scene photo, then obtains gravity, camera speed and camera scale of each working scene photo through the camera relative pose data and the IMU relative pose data of each working scene photo, and converts the camera relative coordinates of each working scene photo in the camera position three-dimensional graph to a horizontal coordinate system based on the data, to obtain the camera relative coordinates of each working scene photo in the horizontal coordinate system; then realizes fusion of the camera relative coordinates of each working scene photo in the horizontal coordinate system and GNSS satellite data based on the acquired GNSS satellite data of any working scene photo, and further determines the agricultural vehicle positioning coordinates, so that even if part of the GNSS satellite data is caused by weak GNSS signal in the room, the agricultural vehicle positioning coordinates can still be accurately determined, the agricultural vehicle positioning accuracy is ensured, and the cost is low. BRIEF DESCRIPTION OF DRAWINGS

[0044] Figure 1 The greenhouse agricultural vehicle positioning method flowchart of the embodiment of the application.

[0045] Figure 2 The greenhouse agricultural vehicle positioning system structure block diagram of the embodiment of the application. DETAILED DESCRIPTION

[0046] In order to enable personnel in the art to better understand the present application, the technical solutions in the embodiments of the application will be described clearly and completely below with reference to the accompanying drawings in the embodiments of the application. Obviously, the described embodiments are only a part of the embodiments of the application, rather than all the embodiments of the application. Based on the embodiments in the application, all other embodiments obtained by those of ordinary skill in the art without creative work should fall within the protection scope of the application.

[0047] It is to be understood that the terminology used herein is for the purpose of describing particular embodiments only and is not intended to be limiting, unless the context clearly indicates otherwise. It will be further understood that the use of relational terms such as first and second, and the like are used solely to distinguish one from another entity without necessarily implying a relationship or order between these entities. Unless otherwise defined, all terms (including technical and scientific terms) used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this application belongs. It will be further understood that terms, such as those defined in commonly used dictionaries, should be interpreted as having a meaning that is consistent with their meaning in the context of the relevant art and will not be interpreted in an overly literal or overly formal sense unless expressly so defined herein.

[0048] The application will be described in further detail in conjunction with the drawings in which:

[0049] Referring to Figure 1 In an embodiment of the present application, a greenhouse agricultural vehicle positioning method is provided, specifically a greenhouse agricultural vehicle positioning method fusing satellite vision and inertia, which effectively solves the problems of poor positioning function effect and high cost in current greenhouse agricultural vehicle automatic driving.

[0050] Specifically, the greenhouse agricultural vehicle positioning method comprises the following steps:

[0051] S1: Obtain a plurality of continuous working scene photos and IMU data of each working scene photo.

[0052] S2: Obtain camera relative pose data and IMU relative pose data of each working scene photo according to the plurality of continuous working scene photos and the IMU data of each working scene photo.

[0053] S3: Construct a camera position three-dimensional graph according to the camera relative pose data of each working scene photo, and extract camera relative coordinates of each working scene photo in the camera position three-dimensional graph.

[0054] S4: Obtain gravity, camera speed and camera scale of each working scene photo according to the camera relative pose data and the IMU relative pose data of each working scene photo.

[0055] S5: Convert the camera relative coordinates of each working scene photo in the camera position three-dimensional graph to a horizontal coordinate system according to the gravity, camera speed and camera scale of each working scene photo, to obtain camera relative coordinates of each working scene photo in the horizontal coordinate system.

[0056] S6: obtaining GNSS satellite data of any working scene photo, and obtaining the agricultural machinery vehicle positioning coordinate through the ceres optimizer according to the camera relative coordinates of each working scene photo in the horizontal coordinate system and the GNSS satellite data of any working scene photo.

[0057] The working scene photo is a photo taken by a camera mounted on the agricultural machinery vehicle when the agricultural machinery vehicle is working. The IMU data of each working scene photo is data collected by an IMU (Inertial Measurement Unit) mounted on the agricultural machinery vehicle at the time of taking each working scene photo. The camera relative pose data and the IMU relative pose data of each working scene photo are camera relative pose data and IMU relative pose data at the time of taking each working scene photo.

[0058] In summary, the greenhouse agricultural machinery vehicle positioning method according to the present application obtains camera relative pose data and IMU relative pose data of each working scene photo according to a plurality of continuous working scene photos and IMU data of each working scene photo, then constructs a camera position three-dimensional graph according to the camera relative pose data of each working scene photo to realize the acquisition of the camera relative coordinates of each working scene photo, then obtains the gravity, camera speed and camera scale of each working scene photo through the camera relative pose data and the IMU relative pose data of each working scene photo, and converts the camera relative coordinates of each working scene photo in the camera position three-dimensional graph to the horizontal coordinate system based on these data to obtain the camera relative coordinates of each working scene photo in the horizontal coordinate system. Then, based on the GNSS satellite data of any working scene photo, the fusion of the camera relative coordinates of each working scene photo in the horizontal coordinate system and the GNSS satellite data is realized, and then the agricultural machinery vehicle positioning coordinate is determined, so that even if part of the GNSS satellite data is caused by the weak indoor signal of GNSS, the agricultural machinery vehicle positioning coordinate can still be accurately determined, the positioning accuracy of the agricultural machinery vehicle is ensured, and the cost is low.

[0059] In a possible implementation, the obtaining of the camera relative pose data and the IMU relative pose data of each working scene photo according to the plurality of continuous working scene photos and the IMU data of each working scene photo includes: tracking feature points at a preset position in each working scene photo by using an optical flow method to obtain a feature point tracking result, and calculating the camera relative pose data of each working scene photo according to the feature point tracking result; and pre-integrating the IMU data of each working scene photo by using a midpoint interpolation method to obtain the IMU relative pose data of each working scene photo.

[0060] Specifically, in the embodiment, when tracking the feature points at the preset positions in each work scene photo by using the optical flow method, the feature points at the 8 preset positions in each work scene photo are tracked, and the 8-point method is used to calculate the camera relative pose data of each work scene photo.

[0061] In a possible implementation, the constructing a camera position three-dimensional graph according to the camera relative rotation data of each work scene photo, and extracting the camera relative coordinates of each work scene photo in the camera position three-dimensional graph comprises: constructing a first three-dimensional graph from the first work scene photo to the last work scene photo, and constructing a second three-dimensional graph from the last work scene photo to the first work scene photo according to the camera relative rotation data of each work scene photo by using a pnp algorithm, and fusing the first three-dimensional graph and the second three-dimensional graph to obtain the camera position three-dimensional graph; and optimizing the camera position three-dimensional graph by using a ceres optimizer, and extracting the camera relative coordinates of each work scene photo in the optimized camera position three-dimensional graph.

[0062] The PnP (Perspective-n-Point) algorithm is a method for solving the correspondence of 3D to 2D points. It describes how to estimate the pose of a camera when n 3D space points and their positions are known. If the 3D positions of feature points in one of the two images are known, at least 3 point pairs (and at least one additional verification point to verify the result) can be used to calculate the motion of the camera. The ceres optimizer is used in the front end or the back end of slam, and the ceres algorithm is set to perform nonlinear optimization on the camera pose and the landmark points.

[0063] In a possible implementation, the obtaining the gravity, the camera speed, and the camera scale of each work scene photo according to the camera relative pose data and the imu relative pose data of each work scene photo comprises: calculating an imu offset matrix of each work scene photo according to the camera relative pose data and the imu relative pose data of each work scene photo; optimizing the imu relative pose data of each work scene photo according to the camera relative pose data, the imu data, and the imu offset matrix of each work scene photo to obtain optimized imu relative pose data of each work scene photo, and calculating a Jacobian matrix and a covariance matrix of each work scene photo; and obtaining the gravity, the camera speed, and the camera scale of each work scene photo by using a ceres optimizer according to the optimized imu relative pose data, the Jacobian matrix, and the covariance matrix of each work scene photo.

[0064] In the process of optimizing the gravity, camera speed, and camera scale of photos in various work scenes using the Ceres optimizer, the direction of gravity is used as the initial value, with a modulus of 9.8. Gravity is further optimized by adjusting the tangential direction of the gravity modulus. Since it is linear, the direction of gravity is manually adjusted four times. The adjustment is handled using the Gauss-Newton method. Then, based on the gravity optimization results, the camera speed and camera scale are further optimized using the Gauss-Newton method. If the camera scale is greater than zero, the optimization is considered successful.

[0065] The calculation of the Jacobian matrix and covariance matrix of each work scene photo includes: setting the Jacobian matrix of the first work scene photo as the identity matrix and the covariance matrix of the first work scene photo as the zero matrix; the Jacobian matrices of the second to the last work scene photos are respectively the Jacobian matrix of the adjacent previous work scene photo multiplied by the IMU offset matrix; the covariance matrices of the second to the last work scene photos are respectively the covariance matrix of the adjacent previous work scene photo plus a noise matrix; the noise matrix is ​​composed of the square of the acceleration and the IMU noise in the IMU data of the current work scene photo.

[0066] In one possible implementation, the step of transforming the camera relative coordinates of each work scene photo in the 3D camera position map to the horizontal coordinate system based on the gravity, camera speed, and camera scale of each work scene photo to obtain the camera relative coordinates of each work scene photo in the horizontal coordinate system includes: optimizing the camera relative coordinates of each work scene photo in the 3D camera position map using the method of minimizing reprojection error based on the camera scale of each work scene photo to obtain the optimized camera relative coordinates of each work scene photo; fixing the first work scene photo as the world coordinate system, calculating the rotation data of the current world coordinate system relative to the horizontal coordinate system based on the gravity of each work scene photo; and transforming the optimized camera relative coordinates of each work scene photo to the horizontal coordinate system based on the rotation data of the current world coordinate system relative to the horizontal coordinate system, as well as the camera speed and camera scale of each work scene photo to obtain the camera relative coordinates of each work scene photo in the horizontal coordinate system.

[0067] Specifically, when processing several consecutive work scene photos, a sliding window approach can be used. The above processing can be considered as processing consecutive work scene photos within a single window.

[0068] In one possible implementation, obtaining the agricultural machinery vehicle positioning coordinates based on the camera relative coordinates of each work scene photo in the horizontal coordinate system and the GNSS satellite data of any work scene photo through a Ceres optimizer includes: acquiring the GNSS satellite data of any work scene photo, constructing a global pose graph model through a Ceres optimizer based on the camera relative coordinates of each work scene photo in the horizontal coordinate system and the GNSS satellite data of any work scene photo, and solving the global pose graph model through a Ceres optimizer to obtain the global coordinates of the work scene photo, which are used as the agricultural machinery vehicle positioning coordinates.

[0069] Specifically, GNSS satellite data can be obtained through GNSS global sensors mounted on agricultural vehicles. The coordinates in the horizontal coordinate system of the obtained work scene photos are fused with the GNSS satellite data, and a global pose graph model is constructed by the Ceres optimizer. After solving the global graph optimization model, a globally unified coordinate system with local accuracy and no global drift is obtained, enabling precise positioning of the agricultural machinery during autonomous driving.

[0070] The following are embodiments of the apparatus of the present invention, which can be used to execute embodiments of the method of the present invention. For details not disclosed in the apparatus embodiments, please refer to the embodiments of the method of the present invention.

[0071] See Figure 2 In another embodiment of the present invention, a positioning system for agricultural machinery vehicles in a greenhouse is provided, which can be used to implement the above-mentioned positioning method for agricultural machinery vehicles in a greenhouse. Specifically, the positioning system for agricultural machinery vehicles in a greenhouse includes a data acquisition module, a pose processing module, a relative coordinate determination module, an optimization module, a coordinate transformation module, and a positioning module.

[0072] The system comprises the following modules: a data acquisition module for acquiring several consecutive work scene photos and their IMU data; a pose processing module for obtaining camera relative pose data and IMU relative pose data for each work scene photo based on the consecutive work scene photos and their IMU data; a relative coordinate determination module for constructing a 3D camera position map based on the camera relative pose data of each work scene photo and extracting the camera relative coordinates of each work scene photo from the 3D camera position map; an optimization module for obtaining the gravity, camera velocity, and camera scale of each work scene photo based on the camera relative pose data and IMU relative pose data; a coordinate transformation module for transforming the camera relative coordinates of each work scene photo in the 3D camera position map to a horizontal coordinate system based on the gravity, camera velocity, and camera scale of each work scene photo, thus obtaining the camera relative coordinates of each work scene photo in the horizontal coordinate system; and a positioning module for acquiring GNSS satellite data of any work scene photo and, based on the camera relative coordinates of each work scene photo in the horizontal coordinate system and the GNSS satellite data of any work scene photo, obtaining the positioning coordinates of the agricultural machinery vehicle through a Ceres optimizer.

[0073] All relevant content of each step involved in the aforementioned embodiments of the greenhouse agricultural machinery vehicle positioning method can be referenced from the functional description of the corresponding functional modules of the greenhouse agricultural machinery vehicle positioning system in this invention, and will not be repeated here. The module division in the embodiments of this invention is illustrative and is only a logical functional division. In actual implementation, there may be other division methods. In addition, the functional modules in the various embodiments of this invention can be integrated into a processor, exist separately, or two or more modules can be integrated into one module. The integrated module can be implemented in hardware or as a software functional module.

[0074] In another embodiment of the present invention, a computer device is provided, comprising a processor and a memory. The memory stores a computer program, which includes program instructions. The processor executes the program instructions stored in the computer storage medium. The processor may be a Central Processing Unit (CPU), or other general-purpose processors, digital signal processors (DSPs), application-specific integrated circuits (ASICs), field-programmable gate arrays (FPGAs), or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. It is the computing and control core of the terminal, suitable for implementing one or more instructions, specifically suitable for loading and executing one or more instructions from the computer storage medium to achieve a corresponding method flow or corresponding function. The processor described in this embodiment of the present invention can be used in the operation of the above-mentioned greenhouse agricultural machinery vehicle positioning method.

[0075] In another embodiment of the present invention, a storage medium is provided, specifically a computer-readable storage medium (Memory), which is a memory device in a computer device used to store programs and data. It is understood that the computer-readable storage medium here can include both the built-in storage medium in the computer device and extended storage media supported by the computer device. The computer-readable storage medium provides storage space that stores the terminal's operating system. Furthermore, the storage space also stores one or more instructions suitable for loading and execution by a processor. These instructions can be one or more computer programs (including program code). It should be noted that the computer-readable storage medium here can be high-speed RAM or non-volatile memory, such as at least one disk storage device. The processor can load and execute one or more instructions stored in the computer-readable storage medium to implement the corresponding steps of the method for positioning agricultural machinery vehicles in a greenhouse as described in the above embodiments.

[0076] Those skilled in the art will understand that embodiments of the present invention can be provided as methods, systems, or computer program products. Therefore, the present invention can take the form of a completely hardware embodiment, a completely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, the present invention can take the form of a computer program product embodied on one or more computer-usable storage media (including, but not limited to, disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0077] This invention is described with reference to flowchart illustrations and / or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of the invention. It will be understood that each block of the flowchart illustrations and / or block diagrams, and combinations of blocks in the flowchart illustrations and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, special-purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, generate instructions for implementing the flowchart illustrations and / or block diagrams. Figure 1 One or more processes and / or boxes Figure 1 A device that provides the functions specified in one or more boxes.

[0078] These computer program instructions may also be stored in a computer-readable storage medium that can direct a computer or other programmable data processing device to function in a particular manner, such that the instructions stored in the computer-readable storage medium produce an article of manufacture including instruction means, which are implemented in a process Figure 1 One or more processes and / or boxes Figure 1 The function specified in one or more boxes.

[0079] These computer program instructions may also be loaded onto a computer or other programmable data processing equipment to cause a series of operational steps to be performed on the computer or other programmable equipment to produce a computer-implemented process, thereby providing instructions that execute on the computer or other programmable equipment for implementing the process. Figure 1 One or more processes and / or boxes Figure 1 The steps of the function specified in one or more boxes.

[0080] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention and not to limit it. Although the present invention has been described in detail with reference to the above embodiments, those skilled in the art should understand that modifications or equivalent substitutions can still be made to the specific implementation of the present invention. Any modifications or equivalent substitutions that do not depart from the spirit and scope of the present invention should be covered within the scope of protection of the claims of the present invention.

Claims

1. A method for positioning agricultural machinery vehicles inside a greenhouse, characterized in that, include: Acquire several consecutive work scene photos and the IMU data of each work scene photo; Based on several consecutive work scene photos and the IMU data of each work scene photo, the camera relative pose data and IMU relative pose data of each work scene photo are obtained. Based on the relative camera rotation data of each work scene photo, the PNP algorithm is used to construct a first 3D map from the first work scene photo to the last work scene photo, and a second 3D map from the last work scene photo to the first work scene photo. The first and second 3D maps are then fused to obtain a 3D camera position map. The 3D camera position map is then optimized using the Ceres optimizer, and the relative camera coordinates of each work scene photo in the optimized 3D camera position map are extracted. Based on the camera relative pose data and IMU relative pose data of each work scene photo, the gravity, camera speed and camera scale of each work scene photo are obtained. Based on the gravity, camera speed, and camera scale of each work scene photo, the relative camera coordinates of each work scene photo in the 3D camera position map are transformed to the horizontal coordinate system to obtain the relative camera coordinates of each work scene photo in the horizontal coordinate system. Acquire GNSS satellite data for any work scene photo, and based on the camera relative coordinates of each work scene photo in the horizontal coordinate system and the GNSS satellite data of any work scene photo, construct a global pose graph model through the Ceres optimizer, and solve the global pose graph model through the Ceres optimizer to obtain the global coordinates of the work scene photo, which are used as the positioning coordinates of agricultural machinery vehicles.

2. The method for positioning agricultural machinery vehicles inside a greenhouse according to claim 1, characterized in that, The step of obtaining camera relative pose data and IMU relative pose data for each work scene photo based on several consecutive work scene photos and IMU data of each work scene photo includes: The optical flow method is used to track feature points at preset positions in each work scene photo to obtain feature point tracking results, and the camera relative pose data of each work scene photo is calculated based on the feature point tracking results. The IMU data of each work scene photo is pre-integrated using the midpoint interpolation method to obtain the relative pose data of the IMU of each work scene photo.

3. The method for positioning agricultural machinery vehicles inside a greenhouse according to claim 1, characterized in that, The process of obtaining the gravity, camera speed, and camera scale of each work scene photo based on the camera relative pose data and IMU relative pose data includes: Based on the camera relative pose data and IMU relative pose data of each work scene photo, calculate the IMU offset matrix of each work scene photo; Based on the camera relative pose data, IMU data and IMU offset matrix of each work scene photo, optimize the IMU relative pose data of each work scene photo to obtain the optimized IMU relative pose data of each work scene photo, and calculate the Jacobian matrix and covariance matrix of each work scene photo. Based on the optimized IMU relative pose data, Jacobian matrix, and covariance matrix of each work scene photo, the Ceres optimizer is used to obtain the gravity, camera speed, and camera scale of each work scene photo.

4. The method for positioning agricultural machinery vehicles inside a greenhouse according to claim 3, characterized in that, The calculation of the Jacobian matrix and covariance matrix of each work scene photo includes: Set the Jacobian matrix of the first work scene photo to be the identity matrix, and the covariance matrix of the first work scene photo to be the zero matrix; The Jacobian matrices of the second to the last work scene photos are the Jacobian matrix of the adjacent previous work scene photo multiplied by the IMU offset matrix, respectively. The covariance matrices of the second to the last work scene photos are the covariance matrices of the adjacent previous work scene photos plus a noise matrix; the noise matrix is ​​composed of the squares of the acceleration and IMU noise in the IMU data of the current work scene photo.

5. The method for positioning agricultural machinery vehicles inside a greenhouse according to claim 1, characterized in that, The process of transforming the relative camera coordinates of each work scene photo in the 3D camera position map to the horizontal coordinate system based on the gravity, camera speed, and camera scale of each work scene photo, to obtain the relative camera coordinates of each work scene photo in the horizontal coordinate system, includes: Based on the camera scale of each work scene photo, the camera relative coordinates of each work scene photo in the 3D map of the camera position are optimized by minimizing the reprojection error method, and the optimized camera relative coordinates of each work scene photo are obtained. The first work scene photo is fixed as the world coordinate system. Based on the gravity of each work scene photo, the rotation data of the current world coordinate system relative to the horizontal coordinate system is calculated. Based on the rotation data of the current world coordinate system relative to the horizontal coordinate system, as well as the camera speed and camera scale of each work scene photo, the optimized camera relative coordinates of each work scene photo are transformed to the horizontal coordinate system to obtain the camera relative coordinates of each work scene photo in the horizontal coordinate system.

6. A greenhouse agricultural machinery vehicle positioning system based on the greenhouse agricultural machinery vehicle positioning method of claim 1, characterized in that, include: The data acquisition module is used to acquire several consecutive work scene photos and the IMU data of each work scene photo; The pose processing module is used to obtain the camera relative pose data and IMU relative pose data of each work scene photo based on several consecutive work scene photos and the IMU data of each work scene photo. The relative coordinate determination module is used to construct a 3D map of camera position based on the camera relative pose data of each work scene photo, and extract the camera relative coordinates of each work scene photo in the 3D map of camera position. The optimization module is used to obtain the gravity, camera speed, and camera scale of each work scene photo based on the camera relative pose data and IMU relative pose data of each work scene photo. The coordinate transformation module is used to transform the relative camera coordinates of each work scene photo in the 3D map of the camera position to the horizontal coordinate system based on the gravity, camera speed and camera scale of each work scene photo, so as to obtain the relative camera coordinates of each work scene photo in the horizontal coordinate system. The positioning module is used to acquire GNSS satellite data of any work scene photo, and obtain the positioning coordinates of agricultural machinery vehicles through the CERES optimizer based on the camera relative coordinates of each work scene photo in the horizontal coordinate system and the GNSS satellite data of any work scene photo.

7. A computer device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that, When the processor executes the computer program, it implements the steps of the method for positioning agricultural machinery vehicles in a greenhouse as described in any one of claims 1 to 5.

8. A computer-readable storage medium storing a computer program, characterized in that, When the computer program is executed by the processor, it implements the steps of the method for positioning agricultural machinery vehicles in a greenhouse as described in any one of claims 1 to 5.

Images