Real-time laser ultrasonic inertial navigation tight coupling high-precision positioning method for plant factory

Abstract

The invention discloses a real-time laser ultrasonic inertial navigation tight coupling high-precision positioning method for a plant factory, which is used for an agricultural robot to realize 10cm-magnitude global positioning in a single-layer plant factory, and comprises the following steps: calling an IMU odometer module, a laser odometer module, a loopback detection module and an acoustic beacon positioning module; in the SLAM process, mapping and positioning are completed in real time, meanwhile, information of all the modules is fused through a factor graph, and optimized high-precision positioning is obtained; wherein the IMU speedometer module is used for acquiring a motion state of an AGV trolley measured by an IMU, the laser speedometer module is used for acquiring a laser radar scanning point cloud of an environment, the loopback detection module is used for eliminating an accumulated drift error in a loopback process, and the acoustic beacon positioning module is used for measuring a distance between the AGV trolley and each beacon in real time through deployed beacons. And the position coordinates of the AGV in the plant factory are calculated, so that high-precision positioning is realized.

Description

Technical Field

[0001] The present invention relates to the field of indoor positioning technology, and in particular to a real-time laser ultrasonic inertial navigation tightly coupled high-precision positioning method for a plant factory, which is used for an agricultural robot to achieve global positioning of the order of 10 cm in a plant factory. Background Art

[0002] With the continuous development of global agricultural production, plant factories play an increasingly important role in modern agriculture. Plant factories provide a controlled environment that can optimize crop growth conditions, improve yield and quality, and reduce dependence on natural resources. At present, agricultural robots have been widely used in agricultural production, such as planting, watering, fertilizing, and picking. In order to achieve autonomous agricultural production, agricultural robots need to accurately perceive and understand the environment in the greenhouse and be able to perform high-precision positioning and navigation in complex scenarios.

[0003] In indoor plant factories, the traditional global positioning system (GPS) cannot provide sufficient positioning accuracy due to signal shielding. Therefore, accurate global positioning and positioning is a challenge for mobile robots and requires precise control operations to achieve. Traditional visual navigation methods are subject to lighting conditions and environmental characteristics, and may not have good positioning effects when encountering complex greenhouse structures.

[0004] After searching the prior art, it was found that Chinese patent document number CN113538410B, published on May 20, 2022, discloses an indoor SLAM mapping and positioning method based on 3D lidar and UWB, which is a method for realizing global positioning of mobile robots in the same field. This method deploys a UWB positioning system in an indoor scene, explores the indoor scene area through a robot carrying a 3D lidar sensor, and generates a map of the explored area using a SLAM algorithm that fuses lidar data and UWB data. However, this technology not only relies on the initial value information provided by the UWB system, but also has not tested the stability of the position information provided by the UWB system in the environment. Its positioning accuracy is relatively limited, and it cannot meet the needs of high-precision positioning operations of agricultural robots in plant factories;

[0005] Chinese patent document number CN118298122A, published on July 5, 2024, discloses a map construction method based on NDT-ICP tight coupling of laser radar and inertial navigation, including preprocessing the original point cloud data collected by the laser radar; performing IMU pre-integration based on the high-frequency signal collected by the inertial measurement unit to obtain the relative pose data of the laser radar frame, thereby eliminating the point cloud distortion of the preprocessed point cloud data to obtain the distortion-corrected point cloud data; extracting edge features and plane features from the distortion-corrected point cloud data; realizing accurate registration and pose estimation of the point cloud based on the NDT ICP registration algorithm; determining the closed-loop factor from the preprocessed point cloud data; constructing a local map, and adding the laser radar pose factor, ground constraint factor and closed-loop factor to the factor graph, updating the pose of all key frames in the local map, and obtaining a high-precision map; However, this method only relies on laser radar and inertial sensors, lacks global sensor information, and is prone to slippage in environments with high repetitiveness and environments with missing or occluded geometric features, resulting in obvious errors in map information. Summary of the invention

[0006] The present invention mainly provides a real-time laser ultrasonic inertial navigation tightly coupled high-precision positioning method for plant factories, which can assign corresponding weights and perform synthesis according to independently established evaluation criteria, ultimately completing the mapping function and achieving high-precision positioning.

[0007] The present invention provides a real-time laser ultrasonic inertial navigation tightly coupled high-precision positioning method for a plant factory, which is used for an agricultural robot to achieve 10 cm-level global positioning in a single-layer plant factory, comprising the following steps:

[0008] Call the IMU odometer module, laser odometer module, loop detection module and acoustic beacon positioning module to complete mapping and positioning in real time during the SLAM process. At the same time, use the factor graph to fuse the information of the above modules to obtain optimized high-precision positioning;

[0009] The IMU odometer module is used to obtain the motion state of the AGV car measured by the IMU at any key frame, the laser odometer module is used to obtain the laser radar scanning point cloud of the environment inside the plant factory in real time, the loop detection module is used to eliminate the accumulated drift error in the loop process, and the acoustic beacon positioning module is used to measure the distance from the AGV car to each beacon in real time through the beacons deployed at various positions in the plant factory, and calculate the position coordinates of the AGV car in the plant factory based on this, wherein the position coordinates belong to the acoustic beacon coordinate system.

[0010] According to an embodiment of the present invention, the ground of the plant factory is a horizontal plane, and an acoustic beacon positioning base station is deployed in each corner of the plant factory, or a plurality of acoustic beacon positioning base stations are evenly deployed on the outer edge of the plant factory.

[0011] According to an embodiment of the present invention, the physical structure of the IMU odometer module is an IMU sensor, which is bound to the AGV to obtain the three-dimensional velocity, angular velocity and acceleration data of the AGV. The IMU odometer module performs the following processing steps on the motion data of the sensor in sequence:

[0012] S11, obtain the measured angular velocity and acceleration data, and eliminate the error components:

[0013] ω mt =ω t +b ωt +n ωt

[0014] a mt =R BW (a t -g)+b at +n at

[0015] Among them, the variable assigned with subscript t is the relevant parameter at time t, the variable assigned with subscript m is the sensor data, parameter b is the sensor measurement deviation, parameter n is the measurement error caused by environmental white noise, g is the gravity constant in the environment, R BW It is the rotation matrix from the IMU coordinate system (i.e., the AGV car coordinate system) to the world coordinate system; after eliminating the corresponding error components, the usable IMU angular velocity and acceleration data are obtained;

[0016] S12, integration, obtain the motion parameters of the car at the next moment:

[0017] v t+Δt =v t +gΔt+R t (a mt -b at -n at )Δt

[0018]

[0019]

[0020] After the above calculations, the position, speed and rotation matrix of the AGV car at any time are obtained, and the change in the speed, position and rotation matrix of the AGV car from time ti to time tj is obtained:

[0021]

[0022] According to an embodiment of the present invention, the physical structure of the laser odometer module is a laser radar, which is used to collect laser point clouds of the environment inside the plant factory. The point cloud data of the sensor is processed in the following steps in sequence:

[0023] S21, marking the sampled data as key frames according to a predetermined rule;

[0024] S22, downsampling the point cloud data acquired by the sensor;

[0025] S23, extracting line and plane features, and grouping the point clouds that meet the requirements into several sets in a given Euclidean space;

[0026] S24, matching corresponding straight line and plane features between adjacent key frames, the specific implementation method is as follows:

[0027] Define the latest key frame in the coordinate system of the laser odometer and that has completed matching as t k , define the key frame that has not been matched in the laser radar coordinate system as t k+1 , find the pose transformation between two key frames, and the optimization function is:

[0028]

[0029] Get k+1 The coordinate transformation matrix R from the laser radar to the laser odometer measured by the IMU at the moment t+1 With position p t+1 , and t k+1 The radar point cloud at the time is transformed from the laser radar coordinate system to the odometer coordinate system;

[0030] To complete the transformation t k+1 The point cloud in is denoted as {p i}, for any p i , after completing the transformation t k Find the point cloud in the Euclidean space where p i The adjacent point p j , p l , p m ,but:

[0031]

[0032] Adjust R t+1 With p k+1 ,make The minimum value is obtained, and the result of iterative calculation is t after laser point cloud feature matching. k+1The coordinate transformation matrix R′ of the laser point cloud data at the moment to the laser odometer t+1 With position p′ t+1 ;

[0033] By fusing the transformed and matched key frame point cloud information, we can obtain a real-time updated point cloud map and a laser odometer starting from the initial pose.

[0034] According to an embodiment of the present invention, step S21 specifically includes marking the point cloud data of the current frame as a key frame at a predetermined time interval.

[0035] According to an embodiment of the present invention, step S23 specifically includes: marking and grouping point sets that satisfy the same edge feature and the same plane feature among neighboring points within a predetermined range into a plurality of sets.

[0036] According to an embodiment of the present invention, the loop detection module implements loop detection based on Euclidean straight line distance, specifically:

[0037] S31, calculate the position p obtained by IMU and LiDAR for each newly marked key frame i Compare with the historical key points one by one, and obtain the key frame K that is closest to it in the historical key frame j ;

[0038] S32, extract key frame K j , and K in the construction order j There are 2m+1 key frames centered at {K j-m , ..., K j , ..., K j+m}, where m is a parameter determined artificially;

[0039] S33, matching the newly marked key frame with the above series of key frames in sequence to obtain key frame K i Transformation matrix from the LiDAR coordinate system to the laser odometry coordinate system (map coordinate system).

[0040] According to an embodiment of the present invention, the physical structure of the acoustic beacon positioning module is an ultrasonic transceiver fixed on the AGV trolley and an acoustic beacon positioning base station fixed in the plant factory. The specific method includes:

[0041] S41, controlling the ultrasonic transceiver to transmit ultrasonic waves to the surroundings at a predetermined frequency, and at the same time, the acoustic beacon positioning base station in the plant factory generates ultrasonic waves of a specific frequency at the same time after receiving the ultrasonic waves, for the ultrasonic transceiver to receive;

[0042] The distance between the AGV and the acoustic beacon positioning base station is calculated using the following formula:

[0043]

[0044] Wherein, T is the time interval between the ultrasonic transceiver transmitting and receiving the ultrasonic wave, and v is the air sound speed, which is about 340m / s;

[0045] Obtain the distance from the AGV to each acoustic beacon positioning base station at any time, and calculate the position of the AGV in the environment through geometric calculation;

[0046] S42, establish relevant reliability criteria for acoustic positioning results:

[0047]

[0048] Among them, f u is the sound frequency of ultrasound, is the position of the AGV at time t, v max is the artificially agreed maximum speed of the AGV, β is the artificially agreed intermediate evaluation index, is the acoustic beacon signal quality function measured when the AGV is stationary at different positions, and is a function of the distribution function of the signal noise with position:

[0049]

[0050] Technical effect: Compared with the prior art, the real-time laser ultrasonic inertial navigation tightly coupled high-precision positioning method for plant factories provided by the present invention can obtain accurate global position and posture by integrating the processed information of the IMU odometer module, laser odometer module, loop detection module and acoustic beacon positioning module, and assigning corresponding weights using a factor graph structure. It can achieve an excellent positioning accuracy of 5.4 cm in a given indoor experimental environment with a certain degree of repeatability, which can fully meet the requirements of high-precision real-time global positioning, has excellent robustness and real-time performance, and is expected to further enhance the positioning and navigation capabilities of mobile robots in large-scale plant factories.

[0051] These and other objects, features and advantages of the present invention will be fully reflected in the following detailed description. BRIEF DESCRIPTION OF THE DRAWINGS

[0052] Figure 1 A schematic flow chart of the real-time laser ultrasonic inertial navigation tightly coupled high-precision positioning method used in a plant factory of the present application is shown.

[0053] Figure 2 A schematic diagram of the steps of the real-time laser ultrasonic inertial navigation tightly coupled high-precision positioning method used in a plant factory of the present application is shown.

[0054] Figure 3A schematic diagram of the operating environment of the AGV vehicle of the present application is shown.

[0055] Figure 4 An image of the error raw data in the prior art is shown.

[0056] Figure 5 The parameter image obtained after data processing by the present application and can be used for reference sensor effectiveness weights is shown, wherein at the sensor position, the larger the value in the image, the larger the error, the lower the reliability, and the smaller the impact of the sensor on the positioning result. DETAILED DESCRIPTION

[0057] The following description is used to disclose the present invention so that those skilled in the art can implement the present invention. The preferred embodiments described below are only examples, and those skilled in the art can think of other obvious variations. The basic principles of the present invention defined in the following description can be applied to other embodiments, variations, improvements, equivalents, and other technical solutions that do not deviate from the spirit and scope of the present invention.

[0058] Those skilled in the art should understand that, in the disclosure of the specification, the orientation or position relationship indicated by the terms "longitudinal", "lateral", "up", "down", "front", "back", "left", "right", "vertical", "horizontal", "top", "bottom", "inside", "outside", etc. are based on the orientation or position relationship shown in the drawings, which are only for the convenience of describing the present invention and simplifying the description, and do not indicate or imply that the device or element referred to must have a specific orientation, be constructed and operate in a specific orientation. Therefore, the above terms should not be understood as limiting the present invention.

[0059] It is to be understood that the term "one" should be understood as "at least one" or "one or more", that is, in one embodiment, the number of an element may be one, while in another embodiment, the number of the element may be multiple, and the term "one" should not be understood as a limitation on the quantity.

[0060] refer to Figures 1 to 5According to a preferred embodiment of the present invention, a real-time laser ultrasonic inertial navigation tightly coupled high-precision positioning method for plant factories is used for agricultural robots to achieve global positioning of 10 cm in a single-layer plant factory, that is, global positioning within 10 cm, and successfully achieves 5.4 cm global positioning. The test environment of the positioning method is a single-layer agricultural greenhouse, and the deployment environment of the algorithm is an AGV car equipped with an Ubuntu 18.04.6LTS operating system, using an i7-8750H processor, including: Livox horizon laser radar sensor, motion control system (can be remotely controlled by the controller to complete forward, backward, steering and other movements), LPMS-IG1 IMU sensor, Super-MP-3D ultrasonic sensor and Marvelmind acoustic beacon positioning system. At the same time, the AGV car is equipped with a ROS control system, and the above components can realize real-time data transmission in the ROS system. The size of the AGV car is approximately 1023mm×778mm×788mm.

[0061] The core task of this positioning method is divided into two parts.

[0062] The first part is parameter determination and mapping, which is to calibrate the world coordinate system, control the AGV to move in the scene, complete the map construction, and determine the transformation matrix from the acoustic beacon coordinate system to the world coordinate system, as well as the transformation matrix from the mapping coordinate system to the world coordinate system;

[0063] The second part is real-time high-precision positioning, which means that during the mapping process, the AGV uses the mapping information it holds, its own lidar information, and the parameter information and noise information fed back by the acoustic beacon to quickly obtain the transformation matrix from the AGV coordinate system to the world coordinate system at this moment.

[0064] Among them: the transformation matrix from the mapping coordinate system (the coordinate system where the laser odometer is located) to the world coordinate system is known;

[0065] The transformation matrix from the acoustic beacon coordinate system to the world coordinate system is known;

[0066] The sensors that can be used by AGV cars include IMU, lidar and acoustic beacon sensors.

[0067] Some possible aids to this positioning method include:

[0068] Configure the system hardware and software environment, build the ROS control platform, deploy and test the mapping and positioning algorithms, and test the acoustic beacon positioning system interface;

[0069] Install software drivers and install other software on your computer by installing the Jetpack development tool;

[0070] Install hardware drivers to achieve data acquisition and interaction by installing hardware drivers for lidar, IMU, and acoustic beacon positioning systems;

[0071] Install the ROS Melodic control system to call and manage the above software and hardware and their communications in the ROS platform;

[0072] Install the GTSAM factor graph optimization framework to call the factor graph optimization algorithm in SLAM and relocalization projects;

[0073] Deploy SLAM algorithm, relocalization algorithm, Marvelmind work package, initialize the working environment, and compile all projects;

[0074] Under the default configuration, run the SLAM algorithm test data set, complete the mapping test and verification;

[0075] In the configuration file, set the SLAM map generation path, the IMU sensor and lidar sensor models and topic names, which should be consistent with the local sensor device; set the relocation project map reading path, the IMU sensor and lidar sensor models and topic names, which should be consistent with the local sensor device.

[0076] In this positioning method, it is roughly divided into three steps: first, deploy the operating environment in the AGV car, then control the AGV car to start moving from any position and initial posture, and finally complete the map construction and return a high-precision 3-DOF position and posture, thereby completing the global positioning function.

[0077] The real-time laser ultrasonic inertial navigation tightly coupled high-precision positioning method for plant factories provided in the present application comprises the following steps: calling the IMU odometer module, the laser odometer module, the loop detection module and the acoustic beacon positioning module, completing the mapping and positioning in real time during the SLAM process, and using the factor graph to fuse the information of the above modules to obtain the optimized high-precision positioning, wherein the modules are tightly coupled and share basic information;

[0078] The IMU odometer module is used to obtain the motion state of the AGV car measured by the IMU at any key frame, the laser odometer module is used to obtain the laser radar scanning point cloud of the environment inside the plant factory in real time, the loop detection module is used to eliminate the accumulated drift error in the loop process, and the acoustic beacon positioning module is used to measure the distance from the AGV car to each beacon in real time through the beacons deployed at various positions in the plant factory, and calculate the position coordinates of the AGV car in the plant factory accordingly, wherein the position coordinates belong to the acoustic beacon coordinate system, thereby constructing a factor graph based on the confidence zone optimization algorithm information of the above four modules, wherein the laser odometer and the IMU odometer jointly complete the basic posture estimation and map construction of the AGV car, the acoustic beacon positioning provides the AGV car with global position information, the loop detection module improves the robustness of the mapping process, and corrects the accumulated drift error when the loop is detected, and the above four information are assigned corresponding weights and integrated according to the independently established evaluation criteria, which can finally complete the mapping function and achieve high-precision positioning.

[0079] In one embodiment, the ground of the plant factory is a horizontal plane. An acoustic beacon positioning base station is deployed in each corner of the plant factory. For example, in a square agricultural greenhouse, the size is 20m×12m, and a marker is placed every 2m along the length and width, with a high spatial repetition. At the same time, an acoustic beacon positioning base station is deployed in each of the four corners of the agricultural greenhouse, and its precise position is known; or multiple acoustic beacon positioning base stations are evenly deployed on the outer edge of the plant factory, such as 3-8 acoustic beacon positioning base stations are evenly deployed along the outer edge.

[0080] In one embodiment, the physical structure of the IMU odometer module is an IMU sensor, such as an LPMS-IG1 IMU sensor. The IMU sensor is bound to the AGV to obtain the three-dimensional velocity, angular velocity and acceleration data of the AGV. The IMU odometer module processes the motion data of the sensor in the following steps:

[0081] S11, obtain the measured angular velocity and acceleration data, and eliminate the error components:

[0082] ω mt =ω t +b ωt +n ωt

[0083] a mt =R BW (a t -g)+b at +n at

[0084] Among them, the variable assigned with subscript t is the relevant parameter at time t, the variable assigned with subscript m is the sensor data, parameter b is the sensor measurement deviation, parameter n is the measurement error caused by environmental white noise, g is the gravity constant in the environment, R BW It is the rotation matrix from the IMU coordinate system (i.e., the AGV car coordinate system) to the world coordinate system; after eliminating the corresponding error components, the usable IMU angular velocity and acceleration data are obtained;

[0085] S12, integration, obtain the motion parameters of the car at the next moment:

[0086] v t+Δt =v t +gΔt+R t (a mt -b at -n at )Δt

[0087]

[0088]

[0089] After the above calculations, the position, speed and rotation matrix of the AGV car at any time are obtained, and the i to j At this moment, the changes in the speed, position, and rotation matrix of the AGV are:

[0090]

[0091]

[0092]

[0093] In one embodiment, the physical structure of the laser odometer module is a laser radar, such as a Livox Horizon laser radar, which is used to collect laser point clouds of the environment inside the plant factory. The point cloud data of the sensor is processed in the following steps in sequence:

[0094] S21, marking the sampled data as key frames according to a predetermined rule, for example, marking the point cloud data of the current frame as a key frame at a certain time interval;

[0095] S22, downsampling the point cloud data acquired by the sensor, for example, omitting radar data points that are too far away from the sensor, filtering the point cloud area with too high density, omitting invalid information, reducing the point cloud scale, and improving the real-time performance of subsequent registration;

[0096] S23, extracting line and plane features, in a given Euclidean space, grouping the point clouds that meet the requirements into several sets, for example, marking and grouping the point sets that meet the same edge feature (meet the same spatial line equation, or the distance to the line is less than a certain threshold) and the same plane feature (meet the same spatial plane equation, or the vertical distance to the plane is less than a certain threshold) among the neighboring points within a certain range, and dividing them into several sets;

[0097] S24, matching corresponding straight line and plane features between adjacent key frames, the specific implementation method is as follows:

[0098] Define the latest key frame in the coordinate system of the laser odometer and that has completed matching as t k , define the key frame that has not been matched in the laser radar coordinate system as t k+1 , find the pose transformation between two key frames, and the optimization function is:

[0099]

[0100] Get k+1 The coordinate transformation matrix R from the laser radar to the laser odometer measured by the IMU at the moment t+1 With position p t+1 , and t k+1 The radar point cloud at the time is transformed from the laser radar coordinate system to the odometer coordinate system;

[0101] To complete the transformation t k+1 The point cloud in is denoted as {p i}, for any p i , after completing the transformation t k Find the point cloud in the Euclidean space where p i The adjacent point p j , p l , p m ,but:

[0102]

[0103] Adjust R t+1 With p t+1 ,make The minimum value is obtained, and the result of iterative calculation is t after laser point cloud feature matching. k+1 The coordinate transformation matrix R′ of the laser point cloud data at the moment to the laser odometer t+1 With position p′ t+1 ;

[0104] By fusing the transformed and matched key frame point cloud information, we can obtain a real-time updated point cloud map and a laser odometer starting from the initial pose.

[0105] Due to the possible problems of feature matching failure and iterative calculation failure, the laser odometer obtained by the IMU odometer module and the laser odometer module can only achieve a rough position estimate. Therefore, loop detection is necessary. The loop detection module implements loop detection based on the Euclidean straight line distance, specifically:

[0106] S31, calculate the position p obtained by IMU and LiDAR for each newly marked key frame i Compare with the historical key points one by one, and obtain the key frame K that is closest to it in the historical key frame j ;

[0107] S32, extract key frame K j , and K in the construction order j Take m key frames before and after the center, a total of 2m+1 key frames {K j-m , ..., K j , ..., K j+m}, where m is a parameter determined artificially;

[0108] S33, matching the newly marked key frame with the above series of key frames one by one, and obtaining the key frame K i The transformation matrix from the lidar coordinate system to the laser odometer coordinate system (map coordinate system) is not obtained through the key frame K i-1 Obtained, thereby being able to eliminate the accumulated drift error in the loop closure process through the loop closure detection module;

[0109] Specifically, in the process of feature matching, if the registration is successful, the loop detection module is called to generate a pose transformation matrix based on laser point cloud registration from the historical key frame to the current key frame based on historical similar key frames. At the same time, in the laser odometry, the pose transformation matrix based on laser point cloud registration from the first key frame to the historical key frame is obtained. The two are synthesized to obtain the pose transformation matrix from the first key frame to the current key frame based on laser point cloud registration, and the pose transformation matrix is ​​stored in the laser odometry module; if the registration fails, the loop detection module is not called, but the pose transformation matrix from the first key frame to the previous key frame is obtained from the laser odometry module, and the displacement and rotation of the AGV car from the previous key frame to the current key frame is obtained from the IMU odometry module.

[0110] The pose transformation matrix from the first key frame to the previous key frame is superimposed on the pose transformation matrix from the previous key frame to the current key frame measured based on the IMU odometer to obtain the initial value of the pose transformation matrix from the first key frame to the current key frame.

[0111] The current key frame is feature matched with the previous key frame, and the initial value of the iterative algorithm is the initial value of the pose transformation matrix from the first key frame to the current key frame obtained above. After the iteration is completed, the pose transformation matrix from the first key frame to the current key frame is obtained and stored in the laser odometer.

[0112] The above pose transformation matrix is ​​the global pose of the AGV (or mobile robot) obtained from the laser odometer module, IMU odometer module and loop detection module.

[0113] The current key frame that has completed the posture transformation is point cloud fused with the historical key frames to obtain a real-time point cloud map containing the current key frame.

[0114] In one embodiment, the acoustic beacon positioning module calls the acoustic beacon positioning system to obtain acoustic beacon positioning data, analyze data reliability, and dynamically adjust its weight. The physical structure of the acoustic beacon positioning module is an ultrasonic transceiver fixed on the AGV trolley and an acoustic beacon positioning base station fixed in the plant factory (the four corners of the agricultural greenhouse with a length of 20m and a width of 12m). The specific method includes:

[0115] S41, controlling the ultrasonic transceiver to transmit ultrasonic waves to the surroundings at a predetermined frequency (e.g., 10 Hz), and at the same time, after receiving the ultrasonic waves, the acoustic beacon positioning base station in the plant factory generates ultrasonic waves of a specific frequency at the same time for the ultrasonic transceiver to receive;

[0116] The distance between the AGV and the acoustic beacon positioning base station is calculated using the following formula:

[0117]

[0118] Wherein, T is the time interval between the ultrasonic transceiver transmitting and receiving the ultrasonic wave, and v is the air sound speed, which is about 340m / s;

[0119] The above formula can be used to calculate the distance from the AGV to each acoustic beacon positioning base station at any time, and the position of the AGV in the environment can be obtained through geometric calculation. The result is related to the established coordinate system.

[0120] S42, since the propagation of sound requires time, and the position of the AGV will change within the time interval T, the position obtained by the AGV through the above method may have a certain error. This error is related to the position, speed, and sound frequency of the AGV. Therefore, a relevant reliability criterion is established for the acoustic positioning result:

[0121]

[0122] Among them, f u is the sound frequency of ultrasound, is the position of the AGV at time t, v max is the artificially agreed maximum speed of the AGV, and β is an artificially agreed intermediate evaluation index, such as β = 0.3. is the acoustic beacon signal quality function measured when the AGV is stationary at different positions, and is a function of the distribution function of the signal noise with position:

[0123] Through experimental measurement, it is found that the signal noise is related to the relative position between the AGV and the acoustic beacon positioning base station. The closer the AGV is to the acoustic beacon positioning base station, the greater the measured signal noise. β is an artificially agreed intermediate evaluation index. According to this index, the speed data fed back by the acoustic beacon and the IMU odometer module and the fixed ultrasonic frequency are checked for reliability criteria, the reliability of the acoustic beacon data is verified, and different reliability criteria are applied. As the reliability of the acoustic positioning module is different, its weight in the factor graph optimization is also different.

[0124] The information provided by the above four modules is summarized in a designed factor graph according to the weight of each information. High-confidence information has a greater weight, and the final high-precision positioning information is synthesized.

[0125] Compared with the prior art, the positioning method provided by this application has the following advantages:

[0126] 1. Based on the original traditional laser SLAM framework, the acoustic beacon sensor data is integrated into the indoor scene, adding reliable observation constraints to the back-end pose graph of SLAM, making up for the lack of GPS observation constraints in indoor scenes, which is conducive to controlling the cumulative error of indoor 3D laser SLAM, and can improve the positioning accuracy and robustness of the algorithm, thereby further improving the accuracy of indoor maps constructed by laser SLAM;

[0127] 2. An error analysis system and credibility evaluation index were established for the data of IMU odometer and acoustic beacon positioning. The actual environmental noise of acoustic beacon positioning was measured in particular, and the acoustic positioning credibility area was delineated. When the factor graph is integrated and optimized, the system error of positioning can be further reduced in theory, and the positioning accuracy of the algorithm can be improved, thereby achieving a positioning effect of 10 cm under experimental conditions.

[0128] In summary, the present invention proposes a real-time laser ultrasonic inertial navigation tightly coupled high-precision positioning method for plant factories based on the laser inertial navigation tightly coupled mapping method and the acoustic beacon positioning method. The method obtains the real-time AGV car point cloud scanning results through the laser odometer module, performs downsampling and feature extraction processing, and forms map construction and laser odometer according to the feature matching between adjacent frames; the IMU odometer is realized through IMU integration; the accumulated drift during the loop is eliminated through the loop detection module, which can improve the robustness of the operation; the acoustic beacon positioning module acts as a pseudo "GPS" positioning solution in the indoor environment, and returns the global position estimate of the AGV car. The processed information of the above four sensor modules is integrated, and the corresponding weights are assigned using the factor graph structure, and finally the accurate global position posture is obtained. In a given indoor experimental environment with a certain degree of repeatability, the project achieved an excellent positioning accuracy of 5.4cm, which can fully meet the requirements of high-precision real-time global positioning. Compared with existing SLAM methods, this method demonstrates superior robustness and real-time performance, and is expected to further improve the positioning and navigation capabilities of mobile robots in large agricultural greenhouses.

[0129] It should be understood by those skilled in the art that the embodiments of the present invention described above and shown in the accompanying drawings are only examples and do not limit the present invention. The advantages of the present invention have been fully and effectively achieved. The functional and structural principles of the present invention have been demonstrated and explained in the embodiments, and the embodiments of the present invention may be deformed or modified in any way without departing from the principles.

Claims

1. A real-time laser ultrasonic inertial navigation tightly coupled high-precision positioning method for plant factories, which is used for agricultural robots to achieve 10cm-level global positioning in a single-layer plant factory, characterized in that: The following steps are involved: Call the IMU odometer module, laser odometer module, loop detection module and acoustic beacon positioning module to complete mapping and positioning in real time during the SLAM process. At the same time, use the factor graph to fuse the information of the above modules to obtain optimized high-precision positioning; The IMU odometer module is used to obtain the motion state of the AGV car measured by the IMU at any key frame, the laser odometer module is used to obtain the laser radar scanning point cloud of the environment inside the plant factory in real time, the loop detection module is used to eliminate the accumulated drift error in the loop process, and the acoustic beacon positioning module is used to measure the distance from the AGV car to each beacon in real time through the beacons deployed at various positions in the plant factory, and calculate the position coordinates of the AGV car in the plant factory based on this, wherein the position coordinates belong to the acoustic beacon coordinate system.

2. The real-time laser ultrasonic inertial navigation tightly coupled high-precision positioning method for plant factories according to claim 1, characterized in that: The ground of the plant factory is a horizontal plane, and an acoustic beacon positioning base station is deployed in each corner of the plant factory, or a plurality of acoustic beacon positioning base stations are evenly deployed on the outer edge of the plant factory.

3. The real-time laser ultrasonic inertial navigation tightly coupled high-precision positioning method for plant factories as claimed in claim 2 is characterized in that: The physical structure of the IMU odometer module is an IMU sensor. The IMU sensor is bound to the AGV to obtain the three-dimensional velocity, angular velocity and acceleration data of the AGV. The IMU odometer module processes the motion data of the sensor in the following steps in sequence: S11, obtain the measured angular velocity and acceleration data, and eliminate the error components: oh mt =ω t +b ωt +n ωt a mt =R BW (a t -g)+b at +n at Among them, the variable assigned with subscript t is the relevant parameter at time t, the variable assigned with subscript m is the sensor data, parameter b is the sensor measurement deviation, parameter n is the measurement error caused by environmental white noise, g is the gravity constant in the environment, R BW It is the rotation matrix from the IMU coordinate system (i.e., the AGV car coordinate system) to the world coordinate system; after eliminating the corresponding error components, the usable IMU angular velocity and acceleration data are obtained; S12, integration, obtain the motion parameters of the car at the next moment: v t+Δt =v t +gΔt+R t (a mt -b at -n at )Δt After the above calculations, the position, speed and rotation matrix of the AGV car at any time are obtained, and the i to j At this moment, the changes in the speed, position, and rotation matrix of the AGV are:

4. The real-time laser ultrasonic inertial navigation tightly coupled high-precision positioning method for plant factories as claimed in claim 2, characterized in that: The physical structure of the laser odometer module is a laser radar, which is used to collect laser point clouds of the environment inside the plant factory. It performs the following processing steps on the point cloud data of the sensor in sequence: S21, marking the sampled data as key frames according to a predetermined rule; S22, downsampling the point cloud data acquired by the sensor; S23, extracting line and plane features, and grouping the point clouds that meet the requirements into several sets in a given Euclidean space; S24, matching corresponding straight line and plane features between adjacent key frames, the specific implementation method is as follows: Define the latest key frame in the coordinate system of the laser odometer and that has completed matching as t k , define the key frame that has not been matched in the laser radar coordinate system as t k+1 , find the pose transformation between two key frames, and the optimization function is: Get k+1 The coordinate transformation matrix R from the laser radar to the laser odometer measured by the IMU at the moment t+1 With position p t+1 , and t k+1 The radar point cloud at the time is transformed from the laser radar coordinate system to the odometer coordinate system; To complete the transformation t k+1 The point cloud in is denoted as {p i }, for any p i , after completing the transformation t k Find the point cloud in the Euclidean space where p i The adjacent point p j , p l , p m ,but: Adjust R t+1 With p t+1 ,make The minimum value is obtained, and the result of iterative calculation is t after laser point cloud feature matching. k+1 The coordinate transformation matrix R′ of the laser point cloud data at the moment to the laser odometer t+1 With position p′ t+1 ; By fusing the transformed and matched key frame point cloud information, we can obtain a real-time updated point cloud map and a laser odometer starting from the initial pose.

5. The real-time laser ultrasonic inertial navigation tightly coupled high-precision positioning method for plant factories as claimed in claim 4 is characterized in that: Step S21 specifically includes marking the point cloud data of the current frame as a key frame at a predetermined time interval.

6. The real-time laser ultrasonic inertial navigation tightly coupled high-precision positioning method for plant factories according to claim 5, characterized in that: Step S23 specifically includes: marking and grouping the point sets satisfying the same edge feature and the same plane feature among the neighboring points within the predetermined range, and dividing them into several sets.

7. The real-time laser ultrasonic inertial navigation tightly coupled high-precision positioning method for plant factories as claimed in claim 4, characterized in that: The loop detection module implements loop detection based on Euclidean straight line distance, specifically: S31, calculate the position p obtained by IMU and LiDAR for each newly marked key frame i Compare with the historical key points one by one, and obtain the key frame K that is closest to it in the historical key frame j ; S32, extract key frame K j , and K in the construction order j There are 2m+1 key frames centered at {K j-m , ..., K j , ..., K j+m }, where m is a parameter determined artificially; S33, matching the newly marked key frame with the above series of key frames in sequence to obtain key frame K i Transformation matrix from the LiDAR coordinate system to the laser odometry coordinate system (map coordinate system).

8. The real-time laser ultrasonic inertial navigation tightly coupled high-precision positioning method for plant factories as claimed in claim 2, characterized in that: The physical structure of the acoustic beacon positioning module is an ultrasonic transceiver fixed on the AGV trolley and an acoustic beacon positioning base station fixed in the plant factory. The specific method includes: S41, controlling the ultrasonic transceiver to transmit ultrasonic waves to the surroundings at a predetermined frequency, and at the same time, the acoustic beacon positioning base station in the plant factory generates ultrasonic waves of a specific frequency at the same time after receiving the ultrasonic waves, for the ultrasonic transceiver to receive; The distance between the AGV and the acoustic beacon positioning base station is calculated using the following formula: Wherein, T is the time interval between the ultrasonic transceiver transmitting and receiving the ultrasonic wave, and v is the air sound speed, which is about 340m / s; Obtain the distance from the AGV to each acoustic beacon positioning base station at any time, and calculate the position of the AGV in the environment through geometric calculation; S42, establish relevant reliability criteria for acoustic positioning results: Among them, f u is the sound frequency of ultrasound, is the position of the AGV at time t, v max is the artificially agreed maximum speed of the AGV, β is the artificially agreed intermediate evaluation index, is the acoustic beacon signal quality function measured when the AGV is stationary at different positions, and is a function of the distribution function of the signal noise with position:

Images