Pose graph optimization method and device, orchard robot and orchard operation system

Abstract

The embodiment of the specification discloses a pose graph optimization method and device, an orchard robot and an orchard operation system, belongs to the technical field of agricultural robots, and can realize efficient and accurate loop reset of the cumulative error of a SLAM system. The method comprises the following steps: predicting a global pose graph according to a plurality of sensor data carrying orchard environment information; determining a double-loop verification result of current frame visual data in the plurality of sensor data through a geometric relationship matching algorithm and a pose relationship matching algorithm; when the loop verification results are all passed, if two radar frame data corresponding to the visual data are determined to be passed through a judgment operation, then the imported data corresponding to the current frame visual data is input into a continuous attractor network carrying a three-dimensional pose cell associated with a local view cell to determine a pose correction amount; and obtaining an optimized global pose graph according to the pose correction amount and the global pose graph.

Description

Technical Field

[0001] This document relates to the field of agricultural robot technology, and in particular to a pose graph optimization method, device, orchard robot and orchard operation system. Background Art

[0002] Developing intelligent agricultural machinery technology is an effective way to increase the mechanization and intelligence of orchards. By replacing manual operations with robots, orchard processes can be optimized, achieving precise and efficient operations. The development of autonomous navigation technology is crucial for improving robots' ability to perceive the orchard environment. Accurate positioning and mapping are prerequisites for robots to independently complete orchard operations and are crucial to the modernization of orchards.

[0003] Simultaneous Localization and Mapping (SLAM) technology is one of the key technologies in the field of agricultural robot navigation. It updates the robot's position and posture by continuously observing environmental features to build an incremental map, achieving real-time positioning and mapping in unknown environments. However, SLAM systems require sensors to provide accurate perception data. The results of simple orchard operation tests are acceptable. However, in complex orchard environments, factors such as vegetation density, terrain, various crops, and layout can cause interference to sensors and increase the cumulative error of the system, making widespread application more difficult. At the same time, in order to have appropriate application and energy costs, orchard operation robots are often equipped with fewer sensors. The SLAM system used in orchard operations cannot improve the anti-interference, stability, and consistency of the SLAM system application by setting up sensors on a large scale. Therefore, there are still major challenges in applying SLAM technology in complex orchard environments. Summary of the Invention

[0004] The purpose of the embodiments of this specification is to provide a pose graph optimization method, device, orchard robot and orchard operation system to avoid loop detection misjudgment caused by the sensors of the agricultural robot equipped with the SLAM system being affected by the complex orchard environment, and then use an appropriate amount of sensors to achieve efficient and accurate loop reset of the accumulated error of the SLAM system without increasing the loop detection matching data, and at the same time optimize the pose graph to improve the orchard operation efficiency, map accuracy and real-time positioning, and have the characteristics of anti-interference, stability and consistency.

[0005] In order to achieve the above objectives, the embodiments of this specification adopt the following scheme:

[0006] In a first aspect, a pose graph optimization method based on multi-source SLAM loop closure is provided, comprising:

[0007] Predicting a global pose graph based on multiple sensor data carrying orchard environment information, wherein if the similarity of the descriptors of the current frame visual data in the multiple sensor data does not meet the double loop verification threshold condition, the current frame visual data is used as a reference frame visual data and recorded in a designated data storage unit;

[0008] If the similarity of the descriptor of the current frame visual data meets the double loop verification threshold condition, a first loop verification result is determined by a geometric relationship matching algorithm based on a group of sample points randomly sampled from a matching point set between the current frame visual data and a frame of reference frame visual data in a designated data storage unit, and a second loop verification result is determined by a pose relationship matching algorithm based on a matching point selected as an inlier of the geometric relationship matching algorithm from the matching point set;

[0009] When both the first loop verification result and the second loop verification result are passed, if the two radar frame data judgment operations corresponding to the current frame visual data and a frame of reference frame visual data in the data storage unit are respectively passed, then the imported data corresponding to the current frame visual data is input into a continuous attractor network carrying three-dimensional pose cells associated with local view cells to determine a pose correction amount; wherein the imported data includes an image stream after the current frame visual data, which is used for mapping with the activity change amount of each local view cell;

[0010] An optimized global pose graph is obtained according to the pose correction amount and the global pose graph.

[0011] In a second aspect, a pose graph optimization device based on multi-source SLAM loop is provided, comprising:

[0012] A graph prediction module is configured to predict a global pose graph based on multiple sensor data carrying orchard environment information. If the similarity of the descriptors of the current frame visual data in the multiple sensor data does not meet the double-loop verification threshold condition, the current frame visual data is used as a reference frame visual data and recorded in a designated data storage unit;

[0013] a double loop verification module, configured to determine a first loop verification result by using a geometric relationship matching algorithm based on a group of sample points randomly sampled from a matching point set between the current frame visual data and a frame of reference frame visual data in a designated data storage unit, if the similarity of the descriptors of the current frame visual data meets the double loop verification threshold condition; and determine a second loop verification result by using a pose relationship matching algorithm based on matching points selected from the matching point set as inliers of the geometric relationship matching algorithm;

[0014] a radar frame judgment module, configured to input the imported data corresponding to the current frame visual data into a continuous attractor network carrying three-dimensional pose cells associated with local view cells to determine a pose correction amount when both the first loop verification result and the second loop verification result are passed and the two radar frame data corresponding to the current frame visual data and a frame of reference frame visual data in the data storage unit are judged to be passed; wherein the imported data includes an image stream following the current frame visual data, which is used for mapping with the activity change amount of each local view cell;

[0015] The graph correction module is used to obtain an optimized global pose graph based on the pose correction amount and the global pose graph.

[0016] In a third aspect, an orchard robot is provided, wherein the orchard robot is provided with a plurality of sensors, and the orchard robot comprises:

[0017] at least one processor;

[0018] a memory connected to the at least one processor;

[0019] The memory stores instructions that can be executed by the at least one processor, and the at least one processor implements the aforementioned method by executing the instructions stored in the memory.

[0020] In a fourth aspect, an orchard operation system is provided, comprising a machine storing machine instructions, wherein when the machine instructions are run on the machine, the machine executes the aforementioned method.

[0021] In the scheme of the embodiment of this specification, low-loop similar visual data is stored for the loop detection process of subsequent high-loop similar visual data, and the high-loop similar visual data is passed through a loop detection process including two loop verifications and one radar frame judgment. It is not necessary to use continuous frames for repeated loop detection to determine whether there is a loop. Both loop verifications use the same matching point data from the visual data source, with low computational overhead, high loop efficiency and high accuracy. The SLAM system correction process is triggered when the loop detection is passed, and in the correction process, the imported data after the current frame loop detection passes is input into the three-dimensional continuous attractor network to obtain the posture correction amount, so as to optimize the global posture graph, so that the cumulative error reset timing matches the actual operation of the robot in the orchard environment, avoiding loop detection misjudgment in complex orchard environments, and can use an appropriate amount of sensors to achieve efficient and accurate loop reset of the cumulative error of the SLAM system without increasing the loop detection matching data (no need to use continuous frames for loop detection), and at the same time optimize the posture graph to improve orchard operation efficiency, map accuracy and positioning real-time performance, and has the characteristics of anti-interference, stability and consistency.

[0022] Other features and advantages of the embodiments of this specification will be described in detail in the subsequent detailed description. BRIEF DESCRIPTION OF THE DRAWINGS

[0023] The accompanying drawings described herein are used to provide a further understanding of this specification and constitute a part of this specification. The present disclosure is described in an illustrative rather than a limiting manner in the accompanying drawings, and in particular should not constitute the sole limitation or inappropriate limitation of this specification. In the accompanying drawings:

[0024] Figure 1 Schematic diagram of the main steps of an exemplary pose graph optimization method based on multi-source SLAM loop closure in an embodiment of this specification;

[0025] Figure 2 Schematic diagram of an exemplary optimized pose graph architecture of an embodiment of this specification;

[0026] Figure 3 This is a schematic diagram of a continuous attractor network under an exemplary local excitation according to an embodiment of this specification;

[0027] Figure 4 This is a schematic diagram of a continuous attractor network of an exemplary input image stream under local excitation according to an embodiment of this specification;

[0028] Figure 5 This is a schematic diagram of an exemplary pose graph optimization device module based on multi-source SLAM loopback according to an embodiment of this specification. DETAILED DESCRIPTION

[0029] To make the purpose, technical solutions and advantages of this specification more clear, the technical solutions of this specification will be clearly and completely described below in conjunction with the specific embodiments of this specification and the corresponding drawings. It is understandable that the described embodiments are only part of the embodiments of this specification, rather than all the embodiments. Based on the embodiments in this specification, any modifications, equivalent substitutions, improvements, etc. made by ordinary technicians in this field without departing from the present disclosure shall fall within the scope of protection of this document.

[0030] As mentioned above, SLAM technology currently uses visual odometry including feature matching, optical flow method and direct method to construct local maps, and uses lidar frame loop detection to reduce cumulative errors and improve map accuracy.

[0031] However, in precision orchard management, we need to deal with the complex and ever-changing large-scale orchards. When agricultural robots perform operations, the SLAM system they carry has to deal with the dense layout of fruit trees, the changing vegetation cover, and potential terrain changes in the orchard. This makes the sensors installed on the robot susceptible to interference when collecting data, and loop detection is very prone to misjudgment. Multiple loop failures will further expand the system's cumulative error and make positioning unstable. Multiple loop errors will distort the constructed map and make it difficult to continue real-time positioning, which will seriously affect the SLAM system's ability to build maps and positioning capabilities. Building accurate maps and real-time positioning are technically difficult. In addition, the tortuous nature of complex orchard paths is also very likely to lead to loop detection misjudgment, which brings efficiency challenges to the SLAM system's accurate identification and execution of loop detection in complex environments, affecting the reliability of the orchard robot's autonomous navigation.

[0032] In view of this, the solution of this specification provides a pose graph optimization method and application solution based on multi-source SLAM loop, which stores low-loop similar visual data for subsequent loop detection process of high-loop similar visual data, and passes the high-loop similar visual data through a loop detection process including two loop verifications and one radar frame judgment. Both loop verifications use the same matching point data from the visual data source, with low computational overhead, high loop efficiency and high accuracy, so as to trigger the SLAM system correction process when passing the loop detection, and in the correction process, the imported data after the current frame loop detection passes is input into the continuous attractor network to obtain the pose correction value, so as to perform global pose graph optimization, avoid loop detection misjudgment in complex orchard environments, and can use an appropriate amount of sensors to achieve efficient and accurate loop reset of the accumulated error of the SLAM system without increasing the loop detection matching data (no need to use continuous frames for loop detection), and at the same time perform pose graph optimization, improve orchard operation efficiency, map accuracy and positioning real-time, and have the characteristics of anti-interference, stability and consistency.

[0033] The technical solutions provided by the various embodiments of this specification are described in detail below with reference to the accompanying drawings.

[0034] One embodiment of this specification provides a pose graph optimization method based on multi-source SLAM loop closure, please refer to Figure 1 , which may include:

[0035] S1: Predicting a global pose graph based on multiple sensor data containing orchard environment information. If the similarity of descriptors of the current frame's visual data among the multiple sensor data does not meet a double-loop verification threshold, the current frame's visual data is recorded as a reference frame of visual data in a designated data storage unit. The visual data may be an image or data mapped to a designated format.

[0036] In the embodiments of this specification, multi-source SLAM loopback can refer to loopback detection in a SLAM system using multiple sensor data sources. The pose graph can be a global pose graph; the sensor data collected by the orchard robot can be used as observations, i.e., edges in the global pose graph; the orchard robot's pose in the orchard and orchard feature points (extracted through algorithmic processing) can be used as state variables, i.e., nodes in the global pose graph.

[0037] In some possible implementations, at least three different sensor data may be used, and each sensor data may be provided by a corresponding sensor on the orchard robot.

[0038] In some possible examples, the orchard robot may be provided with a depth camera, an inertial measurement unit (IMU, also known as an inertial sensor, denoted as an IMU sensor), and a (lidar) radar sensor. Figure 2 , the aforementioned step S1 may include:

[0039] S101, obtaining a perception factor of a corresponding sensor through a driver of each sensor and corresponding captured sensor data, wherein the obtained perception factor includes a visual factor, an IMU factor, and a radar factor.

[0040] To obtain visual factors, the first step is to ensure that the camera hardware is properly connected to the system. Subsequently, the depth camera driver is activated through the software interface. Once activated, the driver begins receiving continuous image frames from the depth camera hardware. Each frame contains detailed information about the orchard environment, such as tree morphology, lighting variations, ground conditions, and dynamic elements. These image frames are then fed into the image processing module to parse and extract valid visual factors (which represent visual features). The image processing module can be configured with a program that performs feature extraction or a visual algorithm for feature extraction. The feature extraction algorithm can be selected based on needs and test results.

[0041] To obtain IMU factors, the first step is to obtain raw IMU data. The IMU sensor can determine the robot's position and attitude by measuring angular velocity, acceleration, and magnetometers. Any rigid body in a spatial coordinate system can be uniquely represented by its position and attitude information. Position information is obtained through the X, Y, and Z coordinates, while attitude information is obtained through the angle between the rigid body and the X, Y, and Z axes. The IMU sensor directly obtains the robot's position and attitude (i.e., IMU factors) through its own calculations (pre-integration method). This short-term high accuracy compensates for the limitations of visual and radar data during rapid movement in the orchard or when light levels fluctuate.

[0042] To obtain radar factors, the radar's raw point cloud data is first acquired. Radar measures the distance and direction of target objects by emitting electromagnetic waves and receiving echoes. The radar sensor acquires three-dimensional information about the orchard environment, including the distance and direction of trees, fruit, and obstacles, to generate a point cloud map of the orchard and form the radar factors. Each frame of radar data contains the distance and direction of all scanned points. The distance obtained by the lidar is merely a scalar, while a 360-degree scan yields the angle of each corresponding point. Based on the distance and angle, the corresponding vector (including direction) is then derived, forming a complete and accurate point cloud map.

[0043] When acquiring visual factors, IMU factors, and radar factors, it is also necessary to ensure the spatiotemporal synchronization of each data frame. Time synchronization means that the data captured by different sensors need to be aligned in timestamps so that the orchard environment status at the same moment can be accurately reflected in subsequent data fusion and processing. Spatial synchronization (sensor external parameter calibration) refers to eliminating measurement errors caused by differences in installation positions by calibrating the relative positions and postures between different sensors. Through precise time synchronization and space synchronization, the sensor data from depth cameras, IMU sensors, and radars can be effectively fused together to form a comprehensive and accurate perception of the orchard environment, that is, there can be a synchronous correspondence between the sensor data frames of different sensors, and the sensor data frames of the other two ( / two) sensors that correspond synchronously can be determined based on one frame of sensor data from one ( / one) sensor. The aforementioned step S1 may also include:

[0044] S102: Construct the visual factor, the IMU factor, and the radar factor into a factor graph, and optimize the factor graph using an incremental smoothing and mapping (ISAM) optimizer to predict a global pose graph, wherein the global pose graph carries pose information of the robot for arranging each of the sensors.

[0045] The factors obtained in step S101 above can be used to perform global pose prediction using the ISAM optimizer. A factor graph is created in advance for the three sensor factors obtained above to represent the relationship between the robot state variables (such as position and orientation) and the observed quantities (such as sensor readings). In the factor graph, the state variables are nodes and the observed quantities are edges. All the aforementioned sensor data can be added to the factor graph. The visual factor connects the depth camera pose and the map point, the IMU factor connects the poses of adjacent time steps, and the radar factor connects the poses of the radar scan.

[0046] The ISAM optimizer can be used to optimize the aforementioned factor graph, and the predicted global pose graph is obtained by solving the optimization problem. ISAM can effectively smooth incremental data and optimize global pose estimation. The core advantage of ISAM lies in the use of Bayesian tree data structure to efficiently factorize and incrementally update sparse matrices, thereby avoiding global re-optimization and improving computational efficiency. Subsequently, the nonlinear least squares problem corresponding to the factor graph is solved iteratively. In each iteration, the residual is calculated based on the current state variable estimate, and the state variables are updated to reduce the residual. When the iteration meets certain convergence conditions, the optimization ends, and the optimal or near-optimal robot pose is obtained. These obtained robot poses (position information and posture information) can be used as the global pose graph. This global pose graph provides a basis for improving system accuracy, enhancing robustness, and real-time performance.

[0047] In some possible implementations, an image stream may be captured from a depth camera and various descriptors of visual data may be extracted therefrom.

[0048] In some possible examples, the aforementioned step S1 may further process the visual data of the depth camera. The descriptor may be obtained by:

[0049] S103, image stream obtained in real time from the depth camera;

[0050] S104 uses the Oriented Fast and Rotated BRIEF (ORB) algorithm to detect feature points in each frame of the visual image in the image stream and extract descriptors for the corresponding frame of the visual image. BRIEF stands for Binary Robust Independent Elementary Features, and FAST stands for Fast Corner Detection.

[0051] First, necessary preprocessing, such as grayscale conversion and noise reduction, is performed on each visual image frame in the image stream. Feature point detection is then performed, starting with the original visual image frame and gradually reducing the image resolution to form an image pyramid. On each layer of the pyramid image, the Fast Corner Detection (Features from Accelerated Segment Test, FAST) algorithm is used to detect corners. To avoid clustering of corners, a non-maximum suppression algorithm can be used to retain the corners with the largest response values ​​within a certain area as feature points. For each feature point, the grayscale centroid method is used to calculate its main direction, making the descriptor rotationally invariant.

[0052] After detecting a feature point, descriptor extraction is performed. First, a fixed-size region is selected centered on the feature point. Within this region, N point pairs are selected using a fixed binary pattern and the differences between the pixel values ​​of these pairs are compared. Based on the comparison results (greater than or less than), an N-dimensional binary descriptor is generated. For example, a comparison result of greater than is 1, and a comparison result of less than is 0. Before generating the descriptor, the region around the feature point is rotated so that its main direction coincides with the X-axis to ensure rotational invariance of the descriptor.

[0053] The extracted descriptors are then post-processed, such as removing duplicate or redundant descriptors and optimizing the quality and distribution of descriptors. In this way, the descriptors of each frame of visual image (the visual data at this time) can help improve the accuracy and efficiency of feature matching in subsequent steps. The (ORB feature) descriptors of each frame of visual data combine FAST key point detection and BRIEF descriptors. The FAST key point detection algorithm quickly determines the position of corner points by comparing the brightness difference between pixels and their surrounding pixels, while the BRIEF descriptor generates a binary string as the feature descriptor of the key point (i.e., the pixel at the corner point position) by comparing the brightness difference between the pixel pairs surrounding the key point. In other words, the (ORB feature) descriptors of each frame of visual image are obtained. The binary string of the feature descriptor of the key point can be implemented by referring to the aforementioned comparison results to obtain a binary value, or it can be designed according to actual needs.

[0054] In the embodiments of this specification, in order to further enhance the system's anti-interference capability and scene adaptability (reduce sensitivity to rotation and translation transformations), semantic similarity may be calculated instead of image similarity.

[0055] In some possible implementations, a visual data storage unit serving as a reference frame can be constructed through a designated data storage unit, which can be selected as a database. In the SLAM system carried by the orchard robot, whenever a new (machine) visual image is captured, the system extracts the ORB feature descriptors of the visual image of that frame. These descriptors are then converted into bag-of-words vectors, that is, the frequency (or weight) of each visual word appearing in the new image is counted, and the database can be configured with a data structure that stores historical visual images of various frames and their corresponding bag-of-words vectors. A historical visual image in the database can be used as a reference visual image to provide a basis for similarity comparison for the current visual image captured subsequently.

[0056] As the system runs, new visual images are constantly captured and added to the database. The generated bag-of-words vectors are used for similarity matching with the bag-of-words vectors in the established database, thereby achieving fast retrieval and re-localization.

[0057] The aforementioned step S1 may further include:

[0058] S105, determine the bag-of-words vector of the descriptor of the current frame visual image, calculate the similarity between the descriptor of the current frame visual image and the descriptor of the reference frame visual image, and use the bag-of-words model to compare the similarity between the bag-of-words vector of the descriptor of the current frame visual image and the bag-of-words vector of the descriptor of the reference frame visual image in the database to determine whether the double-loop verification threshold condition is met.

[0059] In some possible examples, the method for obtaining the similarity of the descriptor includes: converting the descriptor of the current frame visual image extracted by the ORB algorithm into a current bag-of-words vector; performing similarity calculation on the current bag-of-words vector and the bag-of-words vector of the historical reference frame visual image in the database to determine the similarity of the current bag-of-words vector.

[0060] In this bag-of-words model, closed-loop detection can be achieved. The aforementioned (ORB feature) descriptors can be selected as features for each visual image frame, or the scale-invariant feature transform (SIFT) algorithm can be used separately to extract features for each visual image frame. Then, a comprehensive visual vocabulary set is constructed from all extracted feature descriptors using the K-means clustering algorithm to cluster the descriptors and obtain the constructed ORB dictionary. Next, the descriptors of the current visual image frame and the reference visual image frame in the database described in step 4 are matched with the vocabulary in this ORB dictionary. Each descriptor is assigned to the nearest word center, and a histogram (i.e., a bag-of-words vector) based on the visual word frequency is generated for each visual image frame. Subsequently, the Euclidean distance (or other distances such as cosine distance, Manhattan distance, etc., depending on the numerical mapping requirements and test results) similarity metric is used to compare the histograms of the current visual image frame with the reference visual image frames in the database, and the descriptor similarity between them is calculated. The similarity between the descriptor of the current visual image frame and the descriptor of each reference visual image frame can be calculated. Finally, by comparing the similarities (scores) of the feature descriptors, the sorting order of each frame of visual image is configured in the database so that the image most similar to the current frame of visual image is placed at the front of the query result list.

[0061] The decision on whether to perform the double loop verification process can be made based on the similarity between the current frame visual image and the first reference frame visual image in the query result list. In step S1:

[0062] EX, if the similarity of the descriptors of the current frame visual data among the multiple sensor data does not meet the double-loop verification threshold condition, the current frame visual data is recorded as a frame of reference frame visual data in the designated data storage unit. For example, if the similarity of the descriptors of the current frame visual image does not meet the double-loop verification threshold condition, the current frame visual image is recorded as a frame of reference frame visual image in association with the corresponding similarity in the database.

[0063] In some possible examples, each frame of visual data is a frame of visual image in an image stream, and the similarity of the descriptor of the current frame of visual data in the multiple sensor data is greater than the configured double loop verification threshold, that is, it can be regarded as meeting the double loop verification threshold condition, and is less than or equal to the configured double loop verification threshold, that is, it can be regarded as not meeting the double loop verification threshold condition. Each frame of visual image corresponding to the above step EX can be stored in the aforementioned database as a reference frame visual image for subsequent visual images. In addition, the difference between the similarity of the descriptor of the current frame of visual data and the double loop verification threshold can be compared with a fixed value to determine whether the double loop verification threshold condition is met, and a similar comparison result can also be obtained. The designated data storage unit can be the aforementioned database. In addition to the above database, a structured data storage file (such as a Json file) can also be sampled, and the above process of retrieving the most similar image can be realized by reading and writing the file.

[0064] In the embodiment of this specification, when the similarity of the descriptors of the current frame visual data meets the double loop verification threshold condition, it indicates that the current posture of the robot may have a loop, and a double loop verification process can be performed. Furthermore, the aforementioned pose graph optimization method based on multi-source SLAM loop can also include:

[0065] S2, if the similarity of the descriptor of the current frame visual data meets the double loop verification threshold condition, then the first loop verification result is determined by a geometric relationship matching algorithm based on a group of sample points randomly sampled from a matching point set between the current frame visual data and a frame of reference frame visual data in a specified data storage unit, and the second loop verification result is determined by a posture relationship matching algorithm based on the matching points selected as the inner points of the geometric relationship matching algorithm from the matching point set.

[0066] In some possible implementations, the two loop verifications can use the same set of matching points. The matching points can be feature point pairs, where one feature point can be a feature point in the current frame visual image, and the other feature point can be a feature point in the aforementioned reference frame visual image. The two feature points have similar local features in the image and are considered to be corresponding points in the same object or scene. Feature point matching can be determined by a geometric relationship matching algorithm.

[0067] In some possible examples, the geometric relationship matching algorithm is selected as a Random Sample Consensus (RANSAC) algorithm, wherein determining the first loop verification result may include:

[0068] S201 , constructing a geometric model from a set of randomly sampled sample points of matching points between the current frame visual data and a frame of reference frame visual data in a designated data storage unit.

[0069] The geometric model can be a homography matrix or a fundamental matrix.

[0070] S202, matching each matching point in the matching point set with the geometric model to determine whether each matching point is an interior point;

[0071] For example, if the error between each matching point in the matching point set and the geometric model is within a preset error range, then the matching point conforms to the model and is an interior point; if the error is not within the preset error range, then the matching point does not conform to the model and is an exterior point.

[0072] S203: When it is determined that the number of matching points of the inliers is greater than the preset number, a first loop verification result of passing is obtained.

[0073] If the number of matching points that are determined to be inliers is less than or equal to the preset number, a failed first loop verification result is obtained. The greater the number of matching points that are determined to be inliers, the higher the possibility of a potentially correct loop.

[0074] It should be noted that, after traversing all matching points in the matching point set, the aforementioned steps S201 and S202 can resample the sample point group and construct a geometric model, and again determine the attribution of the inliers and outliers in the matching point set. Finally, the number of inliers and outliers in the geometric model between each iteration can be compared to determine the optimal geometric model with the largest number of inliers. The number of matching points determined as inliers according to the optimal geometric model can be used for the judgment of step S203. Thus, the first loop verification is completed. If a passing first loop verification result is obtained (if it fails, it can be directly jumped out, which has an efficiency advantage, or the subsequent second loop verification can be directly executed without distinction), a second loop verification can be performed. The matching points obtained in the first loop verification (and used for the judgment of step S203) as inliers can be used as input for the second loop verification to avoid loop misjudgment. The posture relationship matching algorithm is selected as a 3D to 2D point pair motion (Perspective-n-Point, PnP) algorithm, wherein determining the second loop verification result may include:

[0075] S204 , calculating the pose of the depth camera based on the matching points selected as the inner points of the geometric relationship matching algorithm in the matching point set and the corresponding projection points, as well as the internal parameters of the depth camera.

[0076] The matching point can be a 3D point, and the corresponding 2D projection point can be determined according to the world coordinates of the matching point. The intrinsic parameters of the depth camera can be known parameters, and the pose quantity can be a rotation matrix and a translation vector.

[0077] S205: Determine a posture error between the posture of the depth camera and the current posture of the system.

[0078] The current position and posture of the system can be provided by the robot navigation system or the SLAM system, which includes the current rotation matrix and the current translation vector of the system. From this, the rotation error and translation error can be determined respectively.

[0079] S206: If the posture error is less than the preset difference, a passed second loop verification result is obtained.

[0080] When both the rotation error and the translation error are less than the corresponding preset difference, a passed second loop verification result can be obtained. If any one of the rotation error and the translation error is greater than or equal to the corresponding preset difference, a failed second loop verification result is obtained. The smaller the rotation error and the translation error, the higher the possibility of a potential correct loop. For example, an agricultural picking robot needs to spend a certain amount of time performing tasks in front of the same fruit tree. At this time, the time span is long and the posture change is not obvious, which can easily cause loop misjudgment. By performing strict image pre-judgment on the visual data through the above two loop verifications (instead of direct radar frame matching and judgment), it is possible to avoid loop misjudgments caused by using a posture matching algorithm alone due to complex orchard environments such as diverse orchard vegetation coverage, sensor interference, and adaptation to different orchard operation conditions. This reduces the misjudgment rate in such scenarios and improves loop efficiency.

[0081] In the above example, the inliers can be matching points with high confidence, and the outliers can be matching points with too low confidence, so as to identify whether the current frame visual image is a scene with high confidence loop. In addition to the aforementioned preferred RANSAC algorithm and PnP algorithm, an optimization / improvement algorithm of the RANSAC algorithm can also be used, such as the improved RANSAC algorithm (Advanced RANSAC, DEGENSAC), which can also be used as the aforementioned geometric relationship matching algorithm to obtain the inliers, and an optimization / improvement algorithm of the PnP algorithm, such as the Efficient Perspective-n-Point algorithm (EPnP) and the Unified Perspective-n-Point algorithm (UPnP), which can also be used as the aforementioned pose relationship matching algorithm to obtain the pose quantity of the depth camera.

[0082] In the embodiment of the present specification, the aforementioned dual loop verification process and radar frame judgment operation can together constitute a loop detection process. When the first loop verification result and the second loop verification result are both passed, that is, the environmental features collected by the depth camera are very similar to a previously explored location, the radar frame judgment operation can be performed, and when one of the first loop verification result and the second loop verification result is not passed, the loop detection process can be jumped out, and the current frame visual data can be added to the database and the next frame visual data can be processed in the aforementioned manner. In this way, the misjudgment caused by the interference of the radar frame and the influence of the orchard environment can be avoided on the basis of dual loop verification. There is no need to use continuous frames to perform loop detection repeatedly (low efficiency) to determine whether there is an actual loop, reduce the storage requirements of the visual template, and reduce system energy consumption. The aforementioned pose graph optimization method based on multi-source SLAM loop can also include:

[0083] S3. When the first loop verification result and the second loop verification result are both passed, if the two radar frame data judgment operations corresponding to the current frame visual data and a frame of reference frame visual data in the data storage unit are respectively passed, then the imported data corresponding to the current frame visual data is input into a continuous attractor neural network (CANN) carrying three-dimensional pose cells associated with local view cells to determine the pose correction amount; wherein the imported data includes an image stream after the current frame visual data.

[0084] In some possible implementations, since the sensor data from the depth camera and radar are fused and synchronized in time and space in the SLAM system onboard the robot, then:

[0085] S301 can determine the corresponding two (groups) of radar frame data based on the frame information of the current frame visual data and a frame of reference frame visual data in the data storage unit, which can be the current frame radar frame data and the historical frame radar frame data respectively (depending on the different synchronization configurations, one frame of visual image can also correspond to one group of radar frame data), and compare (align) the point clouds of the two frames (groups) of radar frame data.

[0086] Feature extraction and matching algorithms (processed as images) or iterative nearest point (ICP, also known as point cloud registration) algorithms can be used to determine whether the two frames of radar frame data match. If they do not match, the radar frame judgment operation will be considered a failure, and the loop detection can be skipped. The current frame of visual data can be added to the database and the next frame of visual data can be processed in the same way as described above.

[0087] S302: If the two frames of radar frame data match, it can be determined whether the inter-frame interval between the two frames (groups) of radar frame data is greater than a preset time length.

[0088] If the inter-frame interval is greater than the preset time, this means that during this inter-frame interval, the robot's movement distance and posture changes may be large, and the system cumulative error needs to be reset. In this case, the two radar frame judgment operations are passed. If it is less than or equal to the preset time, the radar frame judgment operation is failed, and the loop detection can be skipped. The current frame visual data can be added to the database and the next frame visual data can be processed in the above manner. For the two groups of radar frame data mentioned above, the interval between the earliest frame in the current group of radar frame data and the last frame in the historical group of radar frame data can be used as the inter-frame interval for judgment. In some possible examples, the preset time can be 30 seconds, 45 seconds, or 60 seconds, etc., which are values ​​suitable for use in local orchard environments.

[0089] In some possible implementations, the continuous attractor network can be a neural network computational model with weighted excitation and inhibition connections, wherein the weighted excitation can be realized by triggering the local view cells to generate local excitation after the local view code and image stream are imported, and the inhibition connection can be realized by expressing the activity change between global cells as an attenuation form, please refer to Figure 3 .

[0090] The continuous attractor network can learn and record the activity association between each local view cell and the 3D pose cell based on each frame of visual data after the radar frame data judgment operation in the training data. In the absence of external stimulation, the change in activity between the local view cell and the 3D pose cell will gradually decrease over time to a completely steady state, which can be considered to be an activity association. The distance between the local view cell and the 3D pose cell is small, and it can be a direct connection relationship rather than a connection through other cells. Figure 3 The darker gray circles in the middle are activated local view cells. Active local view cells will generate local excitation to the surrounding cells (lighter gray) (with active correlation relationships).

[0091] The three-dimensional pose cells of the continuous attractor network carry three-dimensional coordinate information and pose information; the imported data may include the local view code and pose estimate of the current frame visual data and the image stream after the current frame visual data.

[0092] In the aforementioned step S3, inputting the imported data corresponding to the current frame visual data into a continuous attractor network having three-dimensional pose cells associated with local view cells to determine the pose correction amount may include:

[0093] S303: Adjust the three-dimensional coordinate information and posture information of the three-dimensional posture cells of the continuous attractor network according to the posture estimation value.

[0094] This pose estimate can be obtained from a depth camera or calculated by the robot's SLAM system based on IMU sensor data. It can be used as an initialization parameter for the continuous attractor network at the moment a loop closure is detected. Furthermore, in some possible examples, the pose estimate can be omitted, and the initialization parameters for the test can be configured based on the local orchard scene and the characteristics of the robot.

[0095] S304: activating the local view cells of the continuous attractor network according to the local view code to obtain active local view cells, where the active local view cells are used to activate the three-dimensional pose cells having the active association relationship.

[0096] Specifically, when the local view of the current environment is sufficiently similar to the local view previously recorded by a pose cell, the corresponding local view cell will be activated. The local view code can correspond one-to-one with the current frame visual image or can be a vector composed of eigenvalues. The local view code can perform weighted excitation on local view cells in one or more layers. The selected local view cell will be associated with the subsequent input image stream to produce an activity change, thereby being activated, resulting in an active local view cell. The local view cell can generate local excitation on (nearby) 3D pose cells with the said activity association, thereby further activating 3D pose cells, causing the active 3D pose cells to produce an activity change. In some possible networks, the local view code corresponding to the current frame visual image can be input instead of the local view code corresponding to the visual image one or several frames after the current frame visual image. This can be used to provide the initial activity change, and the image stream one or several frames later can also be selected for input to the network. Alternatively, the local view code can be set to a constant value as needed.

[0097] S305, using the vector of the image stream as the activity change of the corresponding local view cell in the continuous attractor network, and inputting it into the continuous attractor network to obtain a posture correction value;

[0098] The activity change of each local view cell in the continuous attractor network is used to stimulate three-dimensional pose cells with activity correlation relationships, and there is also an inhibitory correlation relationship between the three-dimensional pose cells in the continuous attractor network after stimulation.

[0099] Please refer to Figure 4 The image stream after the current frame visual data is the image stream after the aforementioned loop detection process, and the image stream vector V i Input to the network causes the corresponding bound local view cells to produce local excitement, LV1~LVn are n vectors V i Elements, each element (eigenvalue) represents the activity change of a local view cell (directly equal or linearly mapped), n is the number of local view cells in the continuous attractor network, and local view perception is formed in the continuous attractor network. Based on the import of the above data, the locally excited continuous attractor network will be sensitive to the subsequent input image stream, combined with the experience gained during learning and training (stored in the matrix), to generate the activity change corresponding to the image stream. Moreover, there is an inhibitory correlation between the three-dimensional pose cells after excitation, and the activity change corresponding to the image stream is suppressed and reduced, so that the cell activity eventually converges to a stable activity change range to finally obtain the pose correction. Therefore, image stream processing only needs to be performed after the current frame visual data passes the loop closure detection. Before the loop closure detection passes, the features of the continuous frame images do not need to be used. Before the loop closure detection passes, the features of the continuous frame images do not need to be used as the input of the neural network. It has the characteristics of high efficiency and reduces the system computational overhead.

[0100] The image stream after the current frame visual data can be represented as a vector V i , where the vector V iEach element in represents (can be mapped to via a continuous attractor network) the activity change of a local view cell, thereby generating an association with the three-dimensional pose cells to reset the accumulated errors generated during the SLAM operation. This process not only corrects errors, but also reviews and strengthens previous pose associations. Activating and inhibiting changes in cell activity is the process of obtaining the robot's pose correction. This process is performed by the robot automatically obtaining data from the image stream following the current frame visual data, and does not require manual intervention to obtain its own pose correction. This makes it possible to calculate the correction for pose deviations due to long-term accumulated errors. These corrections take into account not only position deviations (x, y, z coordinates), but also attitude / orientation deviations (i.e., Euler angles).

[0101] In the embodiment of the present specification, the image stream after the aforementioned loop closure detection is a continuous frame image. The image stream can be subjected to an efficient tracking algorithm such as a trained 3D convolutional neural network (3D-CNN) or an optical flow algorithm to obtain extracted features. The extracted features can be input to each local view cell through a dimensionality reduction mapping method to input the corresponding element values ​​in the vector of the image stream. For example, the extracted high-dimensional features can be processed by a PCA dimensionality reduction algorithm so that the number of elements in the vector matches the number of local view cells. The reduced dimensionality features can then be normalized, and the values ​​of the normalized elements are used as the activity change corresponding to a local view cell. The normalized values ​​(feature values) constitute the vector of the image stream, which is also the local view vector, so as to input the image stream vector as the change in the local view cell into the continuous attractor network.

[0102] After loop closure detection, the image stream may contain local view information containing features of the target object (such as the local orchard environment and fruit tree information) between consecutive frames. This information is converted into a local view vector (a vector in the image stream) through the activity patterns of local view cells and their interaction with the pose cell network. In a continuous attractor network, the local view vector refers to the activity pattern in the network (a vector consisting of the change in activity and pose of each cell in the network). These patterns can encode specific information, such as the robot's current actual position, posture, or other continuous variables.

[0103] After the image stream after the aforementioned loop detection is imported into the continuous attractor network, the corresponding pose cells will be stimulated by the image flow vector as the local view cell change amount. After the stimulation, the global inhibition of the continuous attractor network makes all cells tend to be stable. The position information and posture information carried by the three-dimensional pose cell with the largest activity change amount after stabilization is the pose information of the current actual pose of the robot (corrected pose amount), which can be expressed as a pose correction amount. The aforementioned pose graph optimization method based on multi-source SLAM loop can also include:

[0104] S4: Obtain an optimized global pose graph based on the pose correction amount and the global pose graph.

[0105] The optimized global pose graph can be used to directly provide the robot with the (corrected) pose quantities of the current (actual) pose, which may include corrected position information and pose information.

[0106] In addition, the robot can also obtain the pose prediction quantity based on the predicted global pose graph, and obtain the (corrected) pose quantity of the current (actual) pose based on the pose correction quantity and the pose prediction quantity. After obtaining the corrected quantity, the SLAM system can use the pose correction quantity to update the predicted global pose graph and adjust the affected nodes and edges in the global pose graph to ensure the accuracy and consistency of the entire map.

[0107] It should be noted that the global pose graph and the continuous attractor network can operate in parallel. As soon as the robot is powered on, it uses the global pose graph to solve for its current position and posture information (pose). At this point, the continuous attractor network is in a self-stable state. Only when the robot detects that its current pose has passed the aforementioned loop detection process and a loop is possible, the image stream following the current frame's visual data is input into the continuous attractor network, and the continuous attractor network begins to operate. The system's neural network computational overhead accounts for a relatively small portion of the total computational overhead. The system then obtains a pose correction for the current (actual) pose. The system then superimposes or replaces the pose correction with the predicted pose from the global pose graph (depending on its numerical representation) to adjust the final robot pose, thereby maintaining real-time positioning and composition accuracy. The pose correction can be represented as the current actual position and posture information, which can be directly replaced with the pose from the predicted global pose graph. The optimized global pose graph can then include the current actual position and posture information. The pose correction amount can also be expressed as the translation and rotation matrix / vector required from a certain pose amount in the predicted global pose graph to the position information and attitude information corresponding to the pose correction amount, so that the pose amount obtained in the predicted global pose graph can be substituted into the certain pose amount in the pose correction amount (in a superposition manner).

[0108] The continuous attractor network described above is highly adaptable to the environmental characteristics of an orchard. The complex and ever-changing nature of an orchard, such as leaf movement, long-term weather changes, and changes in fruit tree growth, leads to dynamic changes in environmental information. By learning from the stable characteristics of the orchard environment, the continuous attractor network improves loop detection accuracy in this dynamic environment. The continuous attractor network also exhibits excellent noise immunity, enabling it to cope with various noise sources present in an orchard, such as sensor noise and environmental noise. Through its powerful learning and generalization capabilities, the continuous attractor network effectively suppresses the impact of noise on loop detection, improving the robustness of the system.

[0109] In contrast, most pose cells in ordinary neural networks only include plane position and head orientation angle, that is, (X, Y, YAW) two-dimensional pose. However, in reality, the orchard environment is complex, with large height differences and rugged roads. It is difficult to accurately describe the robot's movement using two-dimensional poses. Therefore, the above embodiment introduces 6-degree-of-freedom (DOF) three-dimensional pose cells. The continuous attractor network of the above embodiment is applied to the system loop correction part of SLAM, which can be implemented only using visual data without using IMU and radar sensor data. It can correct the global pose graph and improve loop efficiency.

[0110] In the embodiments of this specification, the continuous attractor network can have a variety of configuration methods that are adapted to actual product needs, and the specific formula design representation in the continuous attractor network can be adjusted in combination with the test results, such as linear adjustment, setting initial parameters, setting coefficients, weights, etc. The following provides a preferred construction method, but not a limited only implementation method.

[0111] In one possible implementation, a neural network computational model of joint pose cells consisting of 3D grid cells and multi-layer head heading cells with weighted excitatory and inhibitory connections is established. The pose cells are updated through attractor dynamics and local view calibration.

[0112] The dynamics of the continuous attractor network model, connected by excitatory and inhibitory connections, resemble the navigation neurons found in rodents, known as grid cells. This dynamical behavior is achieved through local excitatory and global inhibitory connectivity.

[0113] For example, the aforementioned continuous attractor network may be constructed in the following manner:

[0114] T1, construct six degrees of freedom parameters for representing three-dimensional coordinate information and posture information, and arrange and represent three-dimensional pose cells carrying posture information through a six-dimensional matrix based on the six degrees of freedom parameters.

[0115] Expand the six dimensions of the attractor neural network into a one-dimensional representation, which is represented by w below (x', y', z', roll', pitch', yaw'); where roll' is roll / tumble, and the robot rotates around the Z axis; pitch' is pitch, and the robot rotates around the X axis; yaw' is heading, and the robot rotates around the Y axis. It can be defined as:

[0116] n=n x' ×n y' ×n z' ×n roll' ×n pitch' ×n yaw' (1)

[0117] Where n = nx' ×n y' ×n z' ×n roll' ×n pitch' ×n yaw' represents the range of the coordinate axis in the w dimension, and n is the product of its six values. (Three-dimensional) pose cells are arranged in a six-dimensional matrix, where (x', y', z') represents the absolute position, (x', y') represents the horizontal coordinate information, z' represents the height information, and (roll', pitch', yaw') represents the orientation at that position. This dimensional expansion of pose cells includes more dimensional information, such as pitch angle, roll angle, and height information. Compared to the commonly used pose cells (which only contain the horizontal coordinates x, y, and yaw angle), the improved pose cells can more comprehensively describe the robot's three-dimensional pose.

[0118] T2, based on the distance coefficient and width constant between the three-dimensional posture cells, a local excitation weight matrix is ​​created, and the total activity change under the inhibition form is determined based on the activity change of each three-dimensional posture cell under local excitation and the local excitation weight matrix.

[0119] A local excitation weight matrix ε can be created using a six-dimensional Gaussian distribution a,b,c,d,e,f The distribution calculation formula is:

[0120]

[0121] Where k p and k d are the width constants of position and direction respectively, and a, b, c, d, e, and f are the distance coefficients between cells expressed by (x', y', z', roll', pitch', yaw').

[0122] The activity change ΔP of posture cells under local stimulation w , which can be given by the following formula:

[0123]

[0124] Where, P i represents the i-th (numbered) pose cell activity matrix, and m represents the number of pose cell activity matrices.

[0125] The local excitation weight matrix index will also affect the relative surface, so it is necessary to calculate it modulo. The aforementioned distance coefficients can be expressed as:

[0126]

[0127] In the formula (x i ,y i ,zi ,roll i ,pitch i ,yaw i ) can be the pose quantity of the affected i-th pose cell.

[0128] Each cell also inhibits nearby cells using the inhibitory form of the local excitation weight matrix, that is, the pose cells inhibit nearby cells, which can have the same parameter values, but the weights are negative. Inhibition is performed after local excitation (rather than at the same time). In response to the input of the image stream, the activity of the stimulated pose cells can fluctuate and change, while performing a slight global inhibition. The total inhibition form can be expressed as:

[0129]

[0130] Where, a,b,c,d,e,f is the inhibitory form of the local excitation weight matrix, with negative weights. is the parameter that controls the global suppression strength, and P i The values ​​in are constrained to be non-negative and normalized to represent the inhibitory correlations between pose cells. In the absence of external input, the activity in the pose cell matrix converges to the activity range of a single pose cell after a few iterations, while all other cells are inactive.

[0131] T3, configure the learning relationship between the vector of the image stream and the activity change of each three-dimensional pose cell, and establish the corresponding relationship between the vector of the image stream and the activation of each local view cell. After activation, the active local view cell is used to generate local excitation for the three-dimensional pose cells with the activity association relationship to form the activity change of the corresponding three-dimensional pose cell.

[0132] When training is required, the vector of the configured image stream can be the vector V of the image stream during testing / after actual loop closure i , where the vector V i Each element in represents (may be mapped to via a continuous attractor network) the activity change of a local view cell, and the input layer of the local view cell can be set with a vector V related to the image flow i The corresponding parameters form a corresponding relationship, thereby inputting the image stream into the continuous attractor network and training the aforementioned learning relationship so that the continuous attractor network can obtain the experience of the activity change amount corresponding to the image stream during the loop.

[0133] In the system, the local view (image stream of the cell input) vector V i The learned relationship between the pose cells is stored in the matrix β. This learning process is based on an improved version of Hebb's law. The update formula of the matrix β can be expressed as:

[0134]

[0135] In the formula, λ represents the update constant of the relationship matrix, max() is the maximum value function, The learning matrices (w dimension) for the u-th pose cell at the t-th and t+1-th training iterations, respectively. The training data includes not only the prepared image streams but also the calibrated activity changes. This allows the continuous attractor network to learn the empirical relationship between pose and image streams. This can be applied to all active local view cells and pose cells.

[0136] T4, based on the strength of local visual correction, the number of active local view cells, the vector of image flow and the activity change of each 3D posture cell, constructs the total activity change under local excitation to output the posture correction amount.

[0137] The correction amount of pose cell activity in the continuous attractor network constructed above can be expressed as:

[0138]

[0139] Where the constant σ determines the strength of local vision correction, which controls the sensitivity of pose cell activity changes to local view cell activity; n act Represents the number of active local view cells. The vector of the image flow as the activity change of the local view cell can be stimulated by the activity association relationship of formula (7), so that the posture cell has the activity change of formula (3) and the weighted excitation affects the nearby posture cells through formula (4), and then affects the nearby posture cells through the inhibitory association relationship of formula (5) and the inhibitory connection through formula (4). Finally, the position information and posture information carried by the three-dimensional posture cell with the largest activity change after stabilization is the posture correction amount of the current actual posture of the robot. Therefore, the system can calculate the correction amount of posture deviation caused by long-term accumulated error.

[0140] In the embodiments of this specification, a preferred optimization method is also provided, which may illustratively include:

[0141] H1, obtain visual factors, IMU factors and radar factors from sensors such as depth cameras;

[0142] H2, use the ISAM optimizer to perform global pose prediction on the above-obtained factors;

[0143] H3, captures the image stream from the depth camera and extracts descriptors from it;

[0144] H4, import the descriptors into the ORB dictionary, create an image database, and add the extracted descriptors to the database;

[0145] H5, uses the bag-of-words model to compare the similarity between the descriptors extracted from the current image and the descriptors in the database;

[0146] H6, when the similarity is higher than the preset threshold, the RANSAC (Random Sample Consensus) and PNP (Perspective-n-Point) algorithms are used to verify the detected loop;

[0147] H7, construct and train a continuous attractor network;

[0148] H8 performs radar frame matching on the data that has passed the loop verification. When the inter-frame interval is greater than the preset duration, the relevant data is imported into the established attractor network, the correction value is calculated, and the global pose is corrected accordingly.

[0149] The steps of obtaining sensor data in step H1 may be:

[0150] Start the camera driver and obtain the visual factor;

[0151] Start the IMU driver and obtain the IMU factor;

[0152] Start the radar driver and obtain the radar factor.

[0153] Wherein, in step H2:

[0154] The method for obtaining global pose prediction may be:

[0155] Integrate the three sensor factors obtained in step H1 into a factor graph;

[0156] The factor graph is optimized using the ISAM optimizer to obtain the optimal or approximately optimal robot pose.

[0157] Wherein, in step H3:

[0158] The method for extracting the descriptor is:

[0159] Get real-time image stream from the depth camera;

[0160] The ORB algorithm is used to detect feature points and extract descriptors from image streams.

[0161] Wherein, in step H4:

[0162] Introduce the pre-set ORB dictionary into the system and establish a database;

[0163] The descriptors extracted from step H3 are added to this database.

[0164] Wherein, in step H5:

[0165] The steps for comparing the similarity between the visual dictionary imported in step H4 and the descriptors extracted in step H3 are as follows:

[0166] Quantize the descriptors into visual words and generate statistics into histograms;

[0167] Compare the histogram of the current image with that of the images in the database using a similarity metric;

[0168] According to the similarity measurement results, the images in the database are sorted.

[0169] Wherein, in step H6:

[0170] Compare the similarity measurement result with a preset threshold;

[0171] If the threshold is exceeded, the RANSAC algorithm is used to randomly sample matching points to build a geometric model (such as rotation and translation), and the matching verification loop is completed;

[0172] Next, the PNP algorithm is used to calculate the camera pose based on high-confidence matching points and compare it with the system pose to verify the loop.

[0173] Wherein, in step H7:

[0174] A neural network computational model of joint position cells composed of three-dimensional place cells and multi-layer head heading cells with weighted excitatory and inhibitory connections was established to represent the six-degree-of-freedom position (x', y', z', roll', pitch', yaw').

[0175] Among them, the attractor network connects each cell through excitability and inhibition, converging the connection of each unit to a stable state;

[0176] Pose cells can simultaneously express the current position information and posture information of the mobile robot;

[0177] Pose cells are updated through two parts: attractor dynamics and local view calibration.

[0178] Wherein, in step H8:

[0179] Perform radar frame matching on the data that has passed the loopback verification in step H6, and match the data frames (usually containing point cloud information) obtained by continuous radar scanning to determine the relative position relationship between adjacent frames;

[0180] If the inter-frame interval is greater than the preset time length, the relevant data is imported into the multi-dimensional continuous attractor network established in step H7, and the posture correction amount is obtained through the excitation and inhibition relationship;

[0181] The obtained correction amount is applied to the global pose obtained in step H2 to correct the global pose accordingly.

[0182] In the embodiments of this specification, a (multi-dimensional) continuous attractor network is a neural network model that simulates the neural mechanism of the rodent brain, and is particularly used to represent and process position and direction information in three-dimensional space. The interaction between its neurons is translation invariant, and its dynamic stable state constitutes a continuous subspace. This network model depicts the brain's encoding process of continuous variables such as orientation, movement direction, and spatial position. In computational neuroscience, continuous attractor networks are used to elucidate the mechanisms of many brain functions, such as the expression of spatial position and direction, the representation of head orientation, etc. The continuous attractor network constructed above performs loop detection. The attractor network has dynamic stability and can correct errors caused by sensor noise or environmental changes to a certain extent in a dynamic environment, enhance the system's adaptability to complex environments, and improve the robustness of loop detection; at the same time, the dynamic process in the attractor network usually has a faster convergence speed. This feature enables the system to quickly update posture estimation when processing self-motion information, thereby improving the real-time performance and efficiency of the system. The system uses multi-source data fusion, combining three different data sources: vision, IMU, and radar. This improves the accuracy and robustness of robot pose estimation in complex environments. The complementary nature of these multiple data sources also provides a more comprehensive reflection of environmental information, compensating for the errors and uncertainties associated with a single sensor.

[0183] In the embodiments of this specification, the problems faced by a single sensor in the field of agricultural production, especially in automated operations in orchards, such as accuracy bottlenecks and limited perception range, when constructing an operating environment map are overcome, the accuracy and robustness of the loop detection algorithm in complex environments are improved, and multi-source data fusion technology is used to effectively make up for the shortcomings of a single sensor in complex and changeable orchard environments, significantly improving the accuracy and robustness of the robot's pose estimation in such environments.

[0184] In the embodiments of this specification, in order to address the problem that general loop detection algorithms have low detection efficiency when facing complex orchard environments, dual loop verification and radar frame decision combination are used to jointly implement the loop detection process, and a multi-dimensional continuous attractor network is introduced into the multi-source sensor data loop processing to improve the loop detection accuracy and system robustness in complex orchard environments, providing a new solution for intelligent agricultural machinery to achieve higher-precision autonomous operations.

[0185] The embodiment of this specification also provides a pose graph optimization device based on multi-source SLAM loopback under the same inventive concept as the above embodiment, please refer to Figure 5 , the pose graph optimization device 1000 may include:

[0186] A graph prediction module 1001 is configured to predict a global pose graph based on multiple sensor data carrying orchard environment information. If the similarity of the descriptors of the current frame visual data in the multiple sensor data does not meet the double-loop verification threshold condition, the current frame visual data is used as a reference frame visual data and recorded in a designated data storage unit.

[0187] A double loop verification module 1002 is configured to determine a first loop verification result using a geometric relationship matching algorithm based on a group of sample points randomly sampled from a matching point set between the current frame visual data and a frame of reference frame visual data in a designated data storage unit if the similarity of the descriptors of the current frame visual data meets the double loop verification threshold condition, and determine a second loop verification result using a pose relationship matching algorithm based on matching points selected from the matching point set as inliers of the geometric relationship matching algorithm;

[0188] A radar frame judgment module 1003 is configured to input the imported data corresponding to the current frame visual data into a continuous attractor network carrying three-dimensional pose cells associated with local view cells to determine a pose correction amount when both the first loop verification result and the second loop verification result are passed and the two radar frame data corresponding to the current frame visual data and a frame of reference frame visual data in the data storage unit are judged to be passed. The imported data includes an image stream following the current frame visual data, which is used for mapping with the activity change amount of each local view cell.

[0189] The graph correction module 1004 is configured to obtain an optimized global pose graph based on the pose correction amount and the global pose graph.

[0190] Optionally, the predicting of a global pose graph based on multiple sensor data carrying orchard environment information includes:

[0191] Obtaining the perception factors of the corresponding sensors through the driver of each sensor and the corresponding captured sensor data, wherein the acquired perception factors include visual factors, IMU factors, and radar factors;

[0192] The vision factor, the IMU factor, and the radar factor are constructed into a factor graph, and the factor graph is optimized by an ISAM optimizer to predict a global pose graph, which carries the pose information of the robot for arranging each of the sensors.

[0193] Optionally, the method for obtaining the descriptor includes:

[0194] Real-time image stream from the depth camera;

[0195] The ORB algorithm is used to detect ORB feature points of each frame of visual image in the image stream and extract the descriptor of the corresponding frame of visual image.

[0196] Optionally, the designated data storage unit is a database, each frame of visual data is a visual image in an image stream, and the similarity of the descriptors is obtained by:

[0197] Convert the descriptor of the current frame visual image extracted by the ORB algorithm into the current bag-of-words vector;

[0198] The similarity between the current word bag vector and the word bag vector of the historical reference frame visual image in the database is calculated to determine the similarity of the current word bag vector; wherein,

[0199] If the similarity of the descriptors of the current frame visual image does not meet the double-loop verification threshold condition, the current frame visual image is used as a reference frame visual image and is associated with the corresponding similarity and recorded in the database.

[0200] Optionally, the geometric relationship matching algorithm is selected as a RANSAC algorithm, wherein determining the first loop verification result includes:

[0201] Constructing a geometric model from a set of randomly sampled sample points of matching points between the current frame visual data and a frame of reference frame visual data in a designated data storage unit;

[0202] Matching each matching point in the matching point set with the geometric model to determine whether each matching point is an interior point;

[0203] When it is determined that the number of matching points of the inliers is greater than the preset number, a passed first loop verification result is obtained.

[0204] Optionally, the posture relationship matching algorithm is selected as a PnP algorithm, wherein determining the second loop verification result includes:

[0205] Calculate the pose of the depth camera based on the matching points selected as the inner points of the geometric relationship matching algorithm and the corresponding projection points, as well as the internal parameters of the depth camera;

[0206] Determine a pose error between the depth camera's pose and the system's current pose;

[0207] If the posture error is less than the preset difference, a passed second loop verification result is obtained.

[0208] Optionally, the continuous attractor network is a neural network computational model having weighted excitatory and inhibitory connections; the continuous attractor network learns and records, based on image flows in training data, an association between image flow vectors serving as activity changes of corresponding local view cells and the activities of three-dimensional pose cells; the three-dimensional pose cells of the continuous attractor network carry three-dimensional coordinate information and pose information;

[0209] The imported data also includes a local view code and a pose estimate of the current frame visual data.

[0210] Optionally, the input data corresponding to the current frame visual data is input into a continuous attractor network having three-dimensional pose cells associated with local view cells to determine a pose correction amount, including:

[0211] Adjusting the three-dimensional coordinate information and posture information of the three-dimensional pose cells of the continuous attractor network according to the pose estimate;

[0212] activating local view cells of the continuous attractor network according to the local view code to obtain active local view cells, wherein the active local view cells are used to activate three-dimensional pose cells having the active association relationship;

[0213] The image stream is input into the continuous attractor network to obtain a posture correction value.

[0214] Optionally, the continuous attractor network is constructed by:

[0215] Constructing six degrees of freedom parameters for representing three-dimensional coordinate information and posture information, and arranging and representing three-dimensional pose cells carrying posture information through a six-dimensional matrix based on the six degrees of freedom parameters;

[0216] Based on the distance coefficient and width constant between the three-dimensional posture cells, a local excitation weight matrix is ​​created. Based on the activity change of each three-dimensional posture cell under local excitation and the local excitation weight matrix, the total activity change under the inhibition form is determined.

[0217] configuring a learning relationship between the vector of the image stream and the activity change of each 3D pose cell, and establishing a corresponding relationship between the vector of the image stream and the activation of each local view cell, wherein the activated local view cell is used to generate local excitation for the 3D pose cell with which the activity is associated, thereby forming the activity change of the corresponding 3D pose cell;

[0218] According to the strength of local visual correction, the number of active local view cells, the vector of image flow and the activity change of each 3D pose cell, the total activity change under local excitation is constructed to output the pose correction amount.

[0219] This embodiment of the specification also provides an orchard robot based on the same inventive concept as the aforementioned embodiment. The orchard robot is equipped with multiple sensors and includes: at least one processor; and a memory connected to the at least one processor. The memory stores instructions executable by the at least one processor, and the at least one processor implements the aforementioned method by executing the instructions stored in the memory. The multiple sensors include a depth camera, a radar sensor, and an IMU sensor.

[0220] The embodiments of this specification also provide an orchard operation system, including a machine storing machine instructions. When the machine instructions are executed on the machine, the machine executes the aforementioned method. The machine may be the orchard robot provided in the aforementioned embodiment. The orchard operation system may also include auxiliary equipment such as charging equipment and safety control equipment for the orchard robot. The method for correcting the posture of the orchard robot may include:

[0221] R1 can obtain the pose correction amount obtained in the pose graph optimization method based on multi-source SLAM loop closure in the aforementioned embodiment, and obtain the predicted global pose graph;

[0222] R2 calculates the predicted pose of the orchard robot's current (actual) position based on the predicted global pose graph. The pose correction is then added to or substituted with the predicted pose to obtain the corrected pose of the orchard robot's current position. This results in high efficiency, high map accuracy, and real-time positioning of the orchard robot, as well as anti-interference, stability, and consistency.

[0223] The aforementioned loop closure term refers to the process by which the robot recognizes that it has returned to a position and posture that it has previously visited while exploring the orchard environment.

[0224] It should also be noted that the aforementioned terms such as first and second are used only for distinguishing descriptions and do not indicate limitations such as order and importance. The terms "include," "comprises," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, commodity, or apparatus that includes elements of the present disclosure includes not only those elements but also includes the cooperation of other elements not explicitly listed, or includes elements inherent to such process, method, commodity, or apparatus.

[0225] During implementation, each step of the above method can be completed by hardware integrated logic circuits in a processor or by software instructions. The above processor can be an integrated circuit chip with signal processing capabilities. The processor can also be a general-purpose processor, including a central processing unit (CPU), a network processor (NP), etc.; it can also be a digital signal processor (DSP), an application-specific integrated circuit (ASIC), a field-programmable gate array (FPGA), or other programmable logic devices, discrete gate or transistor logic devices, or discrete hardware components. The various methods, steps, and logic block diagrams disclosed in the embodiments of this specification can be implemented or executed. The general-purpose processor can be a microprocessor or any conventional processor. The steps of the methods disclosed in conjunction with the embodiments of this specification can be directly implemented and executed by a hardware decoding processor, or by a combination of hardware and software modules in the decoding processor. The software module can be located in a storage medium mature in the art, such as random access memory, flash memory, read-only memory, programmable read-only memory, electrically erasable programmable memory, registers, etc. The storage medium is located in the memory, and the processor reads the information in the memory and completes the steps of the above method in combination with its hardware.

[0226] The foregoing description of this specification describes specific embodiments. Other embodiments are within the scope of the appended claims. In some cases, the actions or steps recited in the claims can be performed in an order different from that described in the embodiments and still achieve the desired results. Furthermore, the processes depicted in the accompanying drawings do not necessarily require the specific order shown or the sequential order to achieve the desired results. In certain embodiments, multitasking and parallel processing are also possible or may be advantageous.

[0227] In short, the above description is only a preferred embodiment of this specification and is not intended to limit the scope of protection of this specification. Any modifications, equivalent replacements, improvements, etc. made within the spirit and principles of this specification should be included in the scope of protection of this specification.

[0228] The devices, systems, or modules described in the above embodiments may be implemented by computer chips or entities, or by products with certain functions. A typical implementation device is a computer.

[0229] Memory can be a computer's storage medium, which can include permanent and non-permanent, removable and non-removable media, and can be implemented by any method or technology to store information. Information can be computer-readable instructions, data structures, program modules, or other data. Examples of computer storage media include, but are not limited to, phase change memory (PRAM), static random access memory (SRAM), dynamic random access memory (DRAM), other types of random access memory (RAM), read-only memory (ROM), electrically erasable programmable read-only memory (EEPROM), flash memory or other memory technology, compact disc read-only memory (CD-ROM), digital versatile disc (DVD) or other optical storage, magnetic cassettes, magnetic tape, disk storage or other magnetic storage devices, or any other non-transmission medium that can be used to store information that can be accessed by a computing device. As defined herein, computer-readable media does not include transitory media such as modulated data signals and carrier waves.

[0230] It should also be noted that the terms "comprises," "includes," or any other variations thereof are intended to encompass non-exclusive inclusion, such that a process, method, commodity, or apparatus that includes a series of elements includes not only those elements but also other elements not explicitly listed, or includes elements inherent to such process, method, commodity, or apparatus. In the absence of further limitations, an element defined by the phrase "comprises a ..." does not exclude the presence of other identical elements in the process, method, commodity, or apparatus that includes the element.

[0231] The various embodiments in this specification are described in a progressive manner. Similar parts between the various embodiments can be referred to in conjunction with each other. Each embodiment focuses on the differences between the other embodiments. In particular, the system embodiments are generally similar to the method embodiments, so the description is relatively simple. For relevant parts, refer to the description of the method embodiments.

Claims

1. A pose graph optimization method based on multi-source SLAM loop closure, characterized in that: include: Predicting a global pose graph based on multiple sensor data carrying orchard environment information, wherein if the similarity of the descriptors of the current frame visual data in the multiple sensor data does not meet the double loop verification threshold condition, the current frame visual data is used as a reference frame visual data and recorded in a designated data storage unit; If the similarity of the descriptor of the current frame visual data meets the double loop verification threshold condition, a first loop verification result is determined by a geometric relationship matching algorithm based on a group of sample points randomly sampled from a matching point set between the current frame visual data and a frame of reference frame visual data in a designated data storage unit, and a second loop verification result is determined by a pose relationship matching algorithm based on a matching point selected as an inlier of the geometric relationship matching algorithm from the matching point set; When both the first loop verification result and the second loop verification result are passed, if the two radar frame data judgment operations corresponding to the current frame visual data and a frame of reference frame visual data in the data storage unit are respectively passed, then the imported data corresponding to the current frame visual data is input into a continuous attractor network carrying three-dimensional pose cells associated with local view cells to determine a pose correction amount; wherein the imported data includes an image stream after the current frame visual data, which is used for mapping with the activity change amount of each local view cell; Obtaining an optimized global pose graph according to the pose correction amount and the global pose graph; The continuous attractor network is a neural network computational model with weighted excitation and inhibition connections; the continuous attractor network learns and records the activity association between the image flow vector, which is the change in activity of the corresponding local view cell, and the 3D pose cell based on the image flow in the training data; the 3D pose cells of the continuous attractor network carry 3D coordinate information and pose information; The imported data also includes a local view code and a pose estimate of the current frame visual data; The construction method of the continuous attractor network includes: Constructing six degrees of freedom parameters for representing three-dimensional coordinate information and posture information, and arranging and representing three-dimensional pose cells carrying posture information through a six-dimensional matrix based on the six degrees of freedom parameters; Based on the distance coefficient and width constant between the three-dimensional posture cells, a local excitation weight matrix is ​​created. Based on the activity change of each three-dimensional posture cell under local excitation and the local excitation weight matrix, the total activity change under the inhibition form is determined. configuring a learning relationship between the vector of the image stream and the activity change of each 3D pose cell, and establishing a corresponding relationship between the vector of the image stream and the activation of each local view cell, wherein the activated local view cell is used to generate local excitation for the 3D pose cell with which the activity is associated, thereby forming the activity change of the corresponding 3D pose cell; According to the strength of local visual correction, the number of active local view cells, the vector of image flow and the activity change of each 3D pose cell, the total activity change under local excitation is constructed to output the pose correction amount.

2. The pose graph optimization method based on multi-source SLAM loop according to claim 1, wherein The global pose graph is predicted based on the data of multiple sensors carrying orchard environment information, including: Obtaining the perception factors of the corresponding sensors through the driver of each sensor and the corresponding captured sensor data, wherein the acquired perception factors include visual factors, IMU factors, and radar factors; The vision factor, the IMU factor, and the radar factor are constructed into a factor graph, and the factor graph is optimized by an ISAM optimizer to predict a global pose graph, which carries the pose information of the robot for arranging each of the sensors.

3. The pose graph optimization method based on multi-source SLAM loop according to claim 1, wherein The method for obtaining the descriptor includes: Real-time image stream from the depth camera; The ORB algorithm is used to detect ORB feature points of each frame of visual image in the image stream and extract the descriptor of the corresponding frame of visual image.

4. The pose graph optimization method based on multi-source SLAM loop according to claim 3, wherein The designated data storage unit is selected as a database, each frame of visual data is a visual image of each frame in the image stream, and the similarity of the descriptor is obtained by: Convert the descriptor of the current frame visual image extracted by the ORB algorithm into the current bag-of-words vector; The similarity between the current word bag vector and the word bag vector of the historical reference frame visual image in the database is calculated to determine the similarity of the current word bag vector; wherein, If the similarity of the descriptors of the current frame visual image does not meet the double-loop verification threshold condition, the current frame visual image is used as a reference frame visual image and is associated with the corresponding similarity and recorded in the database.

5. The pose graph optimization method based on multi-source SLAM loop according to claim 1, wherein The geometric relationship matching algorithm is selected as the RANSAC algorithm, wherein determining the first loop verification result includes: Constructing a geometric model from a set of randomly sampled sample points of matching points between the current frame visual data and a frame of reference frame visual data in a designated data storage unit; Matching each matching point in the matching point set with the geometric model to determine whether each matching point is an interior point; When it is determined that the number of matching points of the inliers is greater than the preset number, a passed first loop verification result is obtained.

6. The pose graph optimization method based on multi-source SLAM loopback according to claim 5, wherein The pose relationship matching algorithm is selected as a PnP algorithm, wherein determining the second loop verification result includes: Calculate the pose of the depth camera based on the matching points selected as the inner points of the geometric relationship matching algorithm and the corresponding projection points, as well as the internal parameters of the depth camera; Determine a pose error between the depth camera's pose and the system's current pose; If the posture error is less than the preset difference, a passed second loop verification result is obtained.

7. The pose graph optimization method based on multi-source SLAM loopback according to claim 1, wherein The input data corresponding to the current frame visual data is input into a continuous attractor network having three-dimensional pose cells associated with local view cells to determine a pose correction amount, including: Adjusting the three-dimensional coordinate information and posture information of the three-dimensional pose cells of the continuous attractor network according to the pose estimate; activating local view cells of the continuous attractor network according to the local view code to obtain active local view cells, wherein the active local view cells are used to activate three-dimensional pose cells having the active association relationship; The vector of the image stream is used as the activity change of the corresponding local view cell in the continuous attractor network, and inputted into the continuous attractor network to obtain the posture correction amount; The activity change of each local view cell in the continuous attractor network is used to stimulate three-dimensional pose cells with activity correlation relationships, and there is also an inhibitory correlation relationship between the three-dimensional pose cells in the continuous attractor network after stimulation.

8. A pose graph optimization device based on multi-source SLAM loop closure, characterized in that: include: A graph prediction module is configured to predict a global pose graph based on multiple sensor data carrying orchard environment information. If the similarity of the descriptors of the current frame visual data in the multiple sensor data does not meet the double-loop verification threshold condition, the current frame visual data is used as a reference frame visual data and recorded in a designated data storage unit; a double loop verification module, configured to determine a first loop verification result by using a geometric relationship matching algorithm based on a group of sample points randomly sampled from a matching point set between the current frame visual data and a frame of reference frame visual data in a designated data storage unit, if the similarity of the descriptors of the current frame visual data meets the double loop verification threshold condition; and determine a second loop verification result by using a pose relationship matching algorithm based on matching points selected from the matching point set as inliers of the geometric relationship matching algorithm; a radar frame judgment module, configured to input the imported data corresponding to the current frame visual data into a continuous attractor network carrying three-dimensional pose cells associated with local view cells to determine a pose correction amount when both the first loop verification result and the second loop verification result are passed and the two radar frame data corresponding to the current frame visual data and a frame of reference frame visual data in the data storage unit are judged to be passed; wherein the imported data includes an image stream following the current frame visual data, which is used for mapping with the activity change amount of each local view cell; A graph correction module, configured to obtain an optimized global pose graph based on the pose correction amount and the global pose graph; The continuous attractor network is a neural network computational model with weighted excitation and inhibition connections; the continuous attractor network learns and records the activity association between the image flow vector, which is the change in activity of the corresponding local view cell, and the 3D pose cell based on the image flow in the training data; the 3D pose cells of the continuous attractor network carry 3D coordinate information and pose information; The imported data also includes a local view code and a pose estimate of the current frame visual data; The construction method of the continuous attractor network includes: Constructing six degrees of freedom parameters for representing three-dimensional coordinate information and posture information, and arranging and representing three-dimensional pose cells carrying posture information through a six-dimensional matrix based on the six degrees of freedom parameters; Based on the distance coefficient and width constant between the three-dimensional posture cells, a local excitation weight matrix is ​​created. Based on the activity change of each three-dimensional posture cell under local excitation and the local excitation weight matrix, the total activity change under the inhibition form is determined. configuring a learning relationship between the vector of the image stream and the activity change of each 3D pose cell, and establishing a corresponding relationship between the vector of the image stream and the activation of each local view cell, wherein the activated local view cell is used to generate local excitation for the 3D pose cell with which the activity is associated, thereby forming the activity change of the corresponding 3D pose cell; According to the strength of local visual correction, the number of active local view cells, the vector of image flow and the activity change of each 3D pose cell, the total activity change under local excitation is constructed to output the pose correction amount.

9. An orchard robot, characterized in that: The orchard robot is provided with a variety of sensors, including: at least one processor; a memory connected to the at least one processor; The memory stores instructions that can be executed by the at least one processor, and the at least one processor implements the method of any one of claims 1 to 7 by executing the instructions stored in the memory.

10. An orchard operation system, comprising a machine storing machine instructions, wherein when the machine instructions are executed on the machine, the machine executes the method according to any one of claims 1 to 7.

Images