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

Abstract

The embodiment of the invention discloses a pose map 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-back reset of accumulative errors of an SLAM (Simultaneous Localization and Mapping) system. The method comprises the following steps: predicting according to various sensor data carrying orchard environment information to obtain a global pose map; determining a double-loop verification result if the current frame of visual data in the various sensor data is subjected to a geometrical relationship matching algorithm and a pose relationship matching algorithm; when the loopback verification results are passed, if the judgment operation of the two radar frame data corresponding to the visual data is passed, inputting the import data corresponding to the current frame visual data into a continuous attractor network carrying three-dimensional pose cells associated with the local view cells, so as to determine the pose correction amount; and obtaining an optimized global pose map according to the pose correction amount and the global pose map.

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 improve the mechanization and intelligence of orchards. By replacing manual operations with robots, the operation process of orchards can be optimized and accurate and efficient operations can be achieved. The development of autonomous navigation technology is crucial to improving the robot's perception of the orchard environment. Accurate positioning and mapping are the prerequisites for robots to independently complete orchard operations and are also the key 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 incremental maps and achieve real-time positioning and mapping of unknown environments. However, the SLAM system requires sensors to provide accurate perception data. The results of the operation test in a simple orchard are acceptable. However, in a complex orchard environment, factors such as the density of vegetation, terrain, a variety of crops, and layout forms will cause the sensors to be interfered with and the system's cumulative error will increase, making it difficult to apply it universally. At the same time, in order to have a suitable application cost and energy cost, 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 great challenges in applying SLAM technology to 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 a 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, while optimizing the pose graph to improve orchard operation efficiency, map accuracy and positioning real-time, and have the characteristics of anti-interference, stability and consistency.

[0005] In order to achieve the above purpose, 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 is provided, comprising:

[0007] A global pose graph is predicted 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 frame of 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 sample point group 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 posture relationship matching algorithm based on a matching point selected as an inner point of the geometric relationship matching algorithm from the matching point set;

[0009] When the first loop verification result and the second loop verification result are both passed, if the judgment operation of the two radar frame data respectively corresponding to the current frame visual data and a frame of reference frame visual data in the data storage unit is passed, the imported data corresponding to the current frame visual data is input into the continuous attractor network carrying the three-dimensional posture cells associated with the local view cells to determine the posture correction amount; wherein the imported data includes the 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 used to predict a global pose graph based on a plurality of sensor data carrying orchard environment information, wherein if the similarity of the descriptors of the current frame visual data in the plurality of sensor data does not meet the double loop verification threshold condition, the current frame visual data is used as a frame of reference frame visual data and recorded in a designated data storage unit;

[0013] A double loop verification module, for determining a first loop verification result through a geometric relationship matching algorithm based on a sample point group 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 descriptor of the current frame visual data meets the double loop verification threshold condition, and determining a second loop verification result through a posture relationship matching algorithm based on a matching point selected as an inner point of the geometric relationship matching algorithm from the matching point set;

[0014] A radar frame judgment module, for inputting the imported data corresponding to the current frame visual data into a continuous attractor network carrying three-dimensional posture cells associated with local view cells to determine the posture correction amount 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 passed; 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;

[0015] The graph correction module is used to obtain an optimized global pose graph according to 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 executed on the machine, the machine executes the aforementioned method.

[0021] In the scheme of the embodiments 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 the orchard operation efficiency, map accuracy and positioning real-time, with 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 drawings described herein are used to provide a further understanding of the present specification and constitute a part of the present specification. The present disclosure is described in the attached drawings in an exemplary rather than limiting manner, and in particular should not constitute the sole and inappropriate limitation of the present specification. In the drawings:

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

[0025] Figure 2 A 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 input image stream under an exemplary local excitation according to an embodiment of this specification;

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

[0029] In order to make the purpose, technical solutions and advantages of this specification clearer, the technical solutions of this specification will be clearly and completely described below in combination with the specific embodiments of this specification and the corresponding drawings. It can be understood 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, all other embodiments obtained by any modification, equivalent replacement, improvement, 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 odometers including feature matching, optical flow and direct methods to construct local maps, and uses lidar frame loop detection to reduce cumulative errors and improve map accuracy.

[0031] However, in the precision orchard management, we need to face the complex and changeable large-scale orchards. When the agricultural robot performs operations, the SLAM system it carries has to deal with the dense layout of fruit trees, the changeable vegetation coverage and the potential terrain changes in the orchard, which makes the sensors set by the robot susceptible to interference when collecting data. The loop detection is very easy to misjudge. Multiple loop failures will further expand the system's cumulative error and make the 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. There are great technical difficulties in building accurate maps and real-time positioning. In addition, the tortuosity of complex orchard paths is also very easy to cause loop detection misjudgments, 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 scheme of the present specification provides a pose graph optimization method based on multi-source SLAM loopback and an application scheme, which stores low-loop similar visual data for subsequent loopback detection process of high-loop similar visual data, and passes the high-loop similar visual data through a loopback detection process including two loopback verifications and one radar frame judgment. Both loopback verifications use the same matching point data from the visual data source, with low computational overhead, high loopback efficiency and high accuracy, so as to trigger the SLAM system correction process when passing the loopback detection, and in the correction process, the imported data after the current frame loopback detection passes is input into the continuous attractor network to obtain the pose correction amount, so as to optimize the global pose graph, avoid loopback detection misjudgment in complex orchard environments, and can use an appropriate amount of sensors to achieve efficient and accurate loopback reset of the accumulated error of the SLAM system without increasing the loopback detection matching data (no need to use continuous frames for loopback detection), and at the same time optimize the pose graph to improve the orchard operation efficiency, map accuracy and positioning real-time, and has the characteristics of anti-interference, stability and consistency.

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

[0034] An 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 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 frame of reference frame visual data and recorded in a designated data storage unit. The visual data may be an image, or an image mapped to data in a designated format.

[0036] In the embodiments of this specification, multi-source SLAM loopback may refer to the use of multiple sensor data sources to perform SLAM system loopback detection. The pose graph may be a global pose graph; the sensor data collected by the orchard robot may be used as observations, i.e., edges in the global pose graph; the pose of the orchard robot in the orchard and the orchard feature points (extracted through algorithm processing) may 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, acquiring the perception factor of the corresponding sensor through the driver of each sensor and the corresponding captured sensor data, wherein the acquired perception factor includes a visual factor, an IMU factor and a radar factor.

[0040] In obtaining visual factors, first ensure that the connection between the camera hardware and the system is normal. Then, start the depth camera driver through the software interface. After the driver is started, it starts to receive continuous image frames from the depth camera hardware. Each frame is rich in detailed information about the orchard environment, such as tree morphology, lighting changes, ground conditions, dynamic elements, etc. These image frames are then sent to the image processing module to parse and extract effective visual factors (which can express visual features). The image processing module can be configured with a program with feature extraction or a visual algorithm function for extracting features. The algorithm for extracting features can be selected according to needs and test results.

[0041] In obtaining IMU factors, first obtain the IMU raw data. The IMU sensor can obtain the position and attitude of the robot by measuring angular velocity, acceleration and magnetometer. Any rigid body in the spatial coordinate system can be represented by its unique state through position information and attitude information. The position information is obtained through the XYZ three-axis coordinates, and the attitude information is obtained through the angle between the rigid body and the XYZ axis. The IMU sensor directly obtains the robot's posture state (i.e., IMU factor) through its own solution (pre-integration method), and uses short-term high precision to make up for the shortcomings of vision and radar data when they run quickly in the orchard or when the light changes.

[0042] In obtaining radar factors, the original point cloud data of the radar is first obtained. The radar measures the distance and direction of the target object by emitting electromagnetic waves and receiving echoes. The radar sensor obtains three-dimensional information of the orchard environment, including the distance and direction of trees, fruits, obstacles, etc., to generate an orchard point cloud map and form a radar factor. Each frame of radar data contains the distance and direction of all scanned points. The distance obtained by the laser radar is only a scalar, and after scanning 360 degrees, the angle of each corresponding point is obtained. Then, based on the distance and angle, the vector corresponding to each point (including direction) is obtained to form a complete and correct 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 time 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 sensor data frames of different sensors, and the sensor data frames of two (or two) sensors that correspond synchronously can be determined based on one frame of sensor data from one (or a) sensor. The aforementioned step S1 may also include:

[0044] S102, constructing the visual factor, the IMU factor and the radar factor into a factor graph, and optimizing the factor graph through 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 the above step S101 can be used for global pose prediction using the ISAM optimizer. A factor graph is established 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). Among them, the state variables in the factor graph 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 radar scans.

[0046] The ISAM optimizer can be used to optimize the aforementioned factor graph, and the predicted global pose graph can be obtained by solving the optimization problem. ISAM can effectively smooth the incremental data and optimize the global pose estimation. The core advantage of ISAM is that it uses the Bayesian tree data structure to efficiently factorize and incrementally update the sparse matrix, thereby avoiding global re-optimization and improving computational efficiency; then 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 variable is updated to reduce the residual. When the iteration meets certain convergence conditions, the optimization ends, and the optimal or approximately optimal robot pose is obtained. These obtained robot poses (position information and pose information) can be used as the global pose graph. This global pose graph can provide 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 visual data of the depth camera may also be processed in the aforementioned step S1. The method of obtaining the descriptor may include:

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

[0050] S104, using the Oriented FAST and Rotated BRIEF (ORB) algorithm, performs ORB feature point detection on each frame of the visual image in the image stream and extracts the descriptor of the corresponding frame of the visual image. Among them, BRIEF stands for Binary Robust Independent Elementary Features, robust binary independent basic features; FAST stands for fast corner detection, Features from Accelerated Segment Test, FAST.

[0051] First, necessary preprocessing is performed on each frame of the visual image in the image stream, such as grayscale, noise reduction, etc. Then, feature point detection is performed, starting from the original frame of the visual image, and the image resolution is reduced layer by layer 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 corner points. In order to avoid the clustering of corner points, the non-maximum suppression algorithm can be used to retain the corner points with the largest response value in a certain area as feature points. For each feature point, the grayscale centroid method is used to calculate its main direction so that the descriptor has rotation invariance.

[0052] After the feature point is detected, the descriptor is extracted. First, a fixed-size area is selected with the feature point as the center. In the selected area, N point pairs are selected using a fixed binary pattern, and the differences between the pixel values ​​of these point pairs are compared. An N-dimensional binary descriptor is generated based on the comparison result (greater than or less than), for example, the comparison result of greater than is 1, and the comparison result of less than is 0; before generating the descriptor, the area around the feature point is rotated to a position where the main direction coincides with the X-axis to ensure the rotation invariance of the descriptor.

[0053] The extracted descriptors are then post-processed, such as removing duplicate or redundant descriptors, optimizing the quality and distribution of descriptors, etc. In this way, the descriptors of each frame of visual image (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 the corner point by comparing the brightness difference between the pixel and the pixels around it, while the BRIEF descriptor generates a binary string as the feature descriptor of the key point by comparing the brightness difference of the pixel pairs around the key point (i.e., the pixel at the corner point position), that is, 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 realized by referring to the aforementioned comparison result to obtain the binary value, or it can be designed according to actual needs.

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

[0055] In some possible implementations, a visual data storage unit as a reference frame can be constructed through a designated data storage unit, and the data storage unit 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 frame visual image. Then, these descriptors will be converted into bag-of-words vectors, that is, the frequency (or weight) of each visual vocabulary appearing in the new image is counted, and the database can be configured with a data structure that stores historical frames of visual images and their corresponding bag-of-words vectors. A historical frame of visual images in the database can be used as a reference frame visual image to provide a basis for similarity comparison for the current frame 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 to perform similarity matching with bag-of-words vectors in the established database, thereby achieving fast retrieval and relocation.

[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; calculating the similarity between 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 the features of each frame of visual images, or the scale invariant feature transform (SIFT) algorithm can be used alone to extract the features of each frame of visual images. Then, a comprehensive visual vocabulary set is constructed from all the extracted feature descriptors by clustering the descriptors to obtain the constructed ORB dictionary. Next, the descriptors of the current frame of visual image and the reference frame of visual image in the database described in step 4 are matched with the vocabulary in the ORB dictionary vocabulary table, that is, each descriptor is assigned to the nearest vocabulary center, and then a histogram based on the frequency of visual vocabulary (that is, the bag of words vector at this time) is generated for each frame of visual image. Subsequently, the Euclidean distance (or other distances such as cosine, Manhattan distance, etc. are used according to the need for numerical mapping and the test effect) similarity measurement method is used to compare the histogram of the current frame of visual image with the reference frame of visual image in the database, and the descriptor similarity between them is calculated, wherein the descriptor similarity of the current frame of visual image and the descriptor of the reference frame of visual image one by one can be calculated. Finally, by comparing the similarities (scores) of the feature descriptors, the sorting order of each frame of visual images 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 in the multiple sensor data does not meet the double loop verification threshold condition, the current frame visual data is used as a frame of reference frame visual data and recorded 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 used as a frame of reference frame visual image and is associated with the corresponding similarity and recorded 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 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 it meets the double loop verification threshold condition, and a similar comparison result can also be obtained. The specified 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 to implement the above process of retrieving the most similar image by reading and writing the file.

[0064] In the embodiment of this specification, when the similarity of the descriptor 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 posture 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 determine the first loop verification result through 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 determine the second loop verification result through 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 previous 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 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.

[0069] The geometric model may 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 internal point; if the error is not within the preset error range, then the matching point does not conform to the model and is an external point.

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

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

[0074] It should be noted that the aforementioned steps S201 and S202 can traverse all the matching points in the matching point set, resample the sample point group and construct a geometric model, and again determine the attribution of the inner points and outer points in the matching point set. Finally, the number of inner and outer points in the geometric model between each iteration can be compared to determine the optimal geometric model with the largest number of inner points. The number of matching points determined as inner points 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 efficiency advantages, or the subsequent second loop verification can be directly performed without distinction), the second loop verification can be performed. The matching points obtained in the first loop verification (and used for the judgment of step S203) as inner points can be used as inputs 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 according to the matching points selected as the inner points of the geometric relationship matching algorithm in the matching point set and the corresponding projection points, and 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, determining a posture error between the posture amount of the depth camera and the current posture amount of the system.

[0078] The current position and posture of the system can be provided by the robot navigation system or the SLAM system, including the current rotation matrix and the current translation vector of the system, thereby respectively determining the rotation error and the translation error.

[0079] S206: If the posture error is less than a 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, and 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. Through the above two loop verifications, strict image pre-judgment of visual data (rather than direct radar frame matching and judgment) can be performed to avoid loop misjudgment caused by using a posture matching algorithm alone due to complex orchard environments such as diverse orchard vegetation coverage, sensor interference, and different adaptation to orchard operations. It reduces the misjudgment rate in such scenarios and provides loop efficiency.

[0081] In the above example, the inner point can be a matching point with high confidence, and the outer point can be a matching point with too low confidence, so as to identify whether the current frame visual image is a scene with a 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 inner point, and an optimization / improvement algorithm of the PnP algorithm can also be used, such as the efficient PnP algorithm (Efficient Perspective-n-Point, EPnP) and the unified PnP algorithm (Unified Perspective-n-Point, UPnP), which can also be used as the aforementioned posture relationship matching algorithm to obtain the posture quantity of the depth camera.

[0082] In the embodiments of the present specification, the aforementioned double loop verification process and radar frame judgment operation can jointly constitute a loop detection process. When both the first loop verification result and the second loop verification result are 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 interference with the radar frame and the influence of the orchard environment can be avoided on the basis of double loop verification. There is no need to use continuous frames to repeatedly perform loop detection (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 loops can also include:

[0083] S3. When the first loop verification result and the second loop verification result are both passed, if the judgment operations of 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 passed, 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 of the depth camera and radar are fused and synchronized in time and space in the SLAM system carried by the robot, then:

[0085] S301 can determine the corresponding two (groups of) radar frame data according to 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 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 (treated 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 exited. 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 manner 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 duration.

[0088] If the inter-frame interval is greater than the preset time, it means that during this inter-frame interval, the robot's moving distance and posture changes may be large, and the system cumulative error needs to be reset. 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 not passed. The loop detection 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 above manner. For the above two groups of radar frame data, 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 taken 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 the local orchard environment.

[0089] In some possible implementations, the continuous attractor network can be a neural network computing 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 changes between global cells in an attenuated form, please refer to Figure 3 .

[0090] The continuous attractor network can be based on the visual data of each frame after the radar frame data judgment operation in the training data passes, and learn and record the activity association relationship between each local view cell and the three-dimensional pose cell. In the absence of external stimulation, the activity change between the local view cell and the three-dimensional pose cell will gradually decrease over time to a completely steady state, which can be regarded as the existence of an activity association relationship. The distance between the local view cell and the three-dimensional pose cell is small, which 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 produce local excitations to the surrounding cells (lighter gray) (with active association 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 visual data of the current frame into a continuous attractor network carrying three-dimensional pose cells associated with local view cells to determine the pose correction amount may include:

[0093] S303, adjusting 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 the pose of a depth camera or calculated by the SLAM system on the robot based on IMU sensor data, and can be used as an initialization parameter of a continuous attractor network at the moment when the current loop detection passes. In addition, in some possible examples, the pose estimate can be omitted, and the initialization parameters of the test can be configured according to the characteristics of the local orchard scene and 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, wherein 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 similar enough to the local view previously recorded by a certain pose cell, the corresponding local view cell will be activated. The local view code can correspond to the current frame visual image one by one, or can be a vector composed of eigenvalues. The local view code can perform weighted excitation on the local view cells in one or more layers. The selected local view cells will be associated with the subsequent input image stream to produce activity changes, thereby being activated to obtain active local view cells. The local view cells can generate local excitations to the (nearby) three-dimensional pose cells with the activity association relationship, thereby further activating the three-dimensional pose cells, so that the active three-dimensional pose cells produce activity changes. In some possible networks, the local view code corresponding to the current frame visual image may not be input, but the local view code corresponding to the visual image of one frame / several frames after the current frame visual image may be input, which can be used to provide the initial activity change, and the image stream after one frame / several frames after the delay may also be selected for input to the network, or the local view code may be set to a constant value as needed.

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

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

[0099] Please refer to Figure 4 , the image stream after the current frame visual data, that is, the image stream after the aforementioned loop detection process, the image stream vector V i The 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, and combined with the experience obtained during learning and training (stored in the matrix), the activity change corresponding to the image stream is generated. Moreover, there is an inhibitory association relationship between the three-dimensional posture cells after the excitation, and the activity change corresponding to the image stream is suppressed and reduced, so that the cell activity finally converges to a stable range of activity change, so as to finally obtain the posture correction amount. Therefore, it is only necessary to process the image stream after the current frame visual data loop detection passes, and there is no need to use the features of the continuous frame image before the loop detection passes, and there is no need to use the features of the continuous frame image for the input of the neural network before the loop detection passes. It has high efficiency and reduces the system calculation 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 (may be mapped to by a continuous attractor network) the activity change of a local view cell, thereby generating an association with the three-dimensional pose cell to reset the accumulated error generated during the SLAM operation. This process can not only correct errors, but also review and strengthen 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 after the current frame of visual data, and does not require human intervention to obtain its own pose correction. In this way, the correction amount of the pose deviation caused by long-term accumulated errors can be calculated. These corrections not only take into account the position deviation (x, y, z coordinates), but also the attitude / orientation deviation (ie, Euler angle).

[0101] In the embodiments of the present specification, the image stream after the aforementioned loop detection is a continuous frame image, and the image stream can be extracted by an efficient tracking algorithm such as a trained 3D convolutional neural network (3D-CNN) or an optical flow algorithm, and the extracted features can be input to each local view cell through a dimensionality reduction mapping method. The corresponding element value 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, and then the reduced-dimensional features can be normalized, and the value of the normalized element is used as the activity change corresponding to a local view cell. The normalized value (feature value) constitutes the vector of the image stream, which is also the local view vector, so as to realize the input of the image stream vector as the change of the local view cell into the continuous attractor network.

[0102] The image stream after the loop detection may contain local view information containing the characteristics of the target object (local orchard environment information, fruit tree information, etc.) between consecutive frames, which is converted into a local view vector (a vector of the image stream) through the activity pattern of the local view cells and their interaction with the posture cell network. In a continuous attractor network, the local view vector refers to the activity pattern in the network (a vector composed of the activity change and posture of each cell in the network), which 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 posture cells will be stimulated by the image flow vector as the local view cell change. 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 posture cell with the largest activity change after stabilization is the posture information of the current actual posture of the robot (corrected posture quantity), which can be expressed as a posture correction quantity. The aforementioned posture graph optimization method based on multi-source SLAM loop can also include:

[0104] S4, obtaining an optimized global pose graph according to 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 quantity of the current (actual) pose, which may include the corrected position information and pose information.

[0106] In addition, the robot can also obtain the predicted pose 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 predicted pose 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 be operated in parallel. As long as the robot is started, the global pose graph can be used to solve the current position information and posture information (pose) of the robot. At this time, the continuous attractor network is in a self-stable state. Only when the robot detects that the current pose has passed the aforementioned loop detection process and there is a possibility of loop, the image stream after the current frame visual data is input into the continuous attractor network, and the continuous attractor network starts to work. The system neural network calculation overhead accounts for a small proportion of the total calculation overhead, and the pose correction amount of the current (actual) pose is obtained. The system superimposes or replaces the pose amount obtained by the predicted global pose graph with the pose correction amount (depending on the numerical expression of the pose correction amount), and adjusts the final robot pose, thereby maintaining the real-time positioning and composition accuracy. Among them, the pose correction amount can be expressed as the current actual position information and posture information, which can be directly replaced with the pose amount obtained by the predicted global pose graph, and the optimized global pose graph can contain the current actual position information and posture information. The posture correction amount can also be expressed as the translation and rotation matrix / vector required to obtain the position information and posture information corresponding to a certain posture amount in the predicted global posture graph, so that the posture amount obtained in the predicted global posture graph can be substituted into the certain posture amount in the posture correction amount (in a superimposed manner).

[0108] The above-mentioned continuous attractor network has strong adaptability to environmental characteristics. The complexity and variability in the orchard, such as leaf movement, long-term weather changes, changes in fruit tree growth, etc., lead to dynamic changes in environmental information. The continuous attractor network can improve the accuracy of loop detection in a dynamically changing environment by learning the stable characteristics of the orchard environment. The continuous attractor network has good noise resistance to cope with various noise sources that may exist in the orchard environment, such as sensor noise, environmental noise, etc. Through its powerful learning and generalization capabilities, the continuous attractor network can effectively suppress the impact of noise on loop detection and improve the robustness of the system.

[0109] In contrast, most of the pose cells in ordinary neural networks only include plane position and head orientation angle, that is, (X, Y, YAW) two-dimensional pose. But in fact, the orchard environment is complex, the height difference is large, and the road is rugged. It is difficult to accurately describe the movement of the robot using two-dimensional pose. Therefore, the above embodiment introduces 6 degrees 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, and 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 suitable for actual product needs, and the representation of the specific formula design 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 with weighted excitation and inhibition connections of joint pose cells composed of three-dimensional grid cells and multi-layer head orientation cells is established, wherein the pose cells are updated through attractor dynamics and local view calibration.

[0112] The dynamics of the continuous attractor network model are connected by excitatory and inhibitory connections, with characteristics similar to those of navigation neurons found in rodents, namely grid cells. This dynamic 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 according to the six degrees of freedom parameters, arrange and represent three-dimensional pose cells carrying posture information through a six-dimensional matrix.

[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, the robot rotates around the Z axis; pitch' is pitch, the robot rotates around the X axis; yaw' is heading, 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 its absolute position, (x', y') is the coordinate information in the horizontal plane, z' can represent the height information, and (roll', pitch', yaw') represents its orientation at that position. This pose cell dimensional expansion contains more dimensional information, such as pitch angle, roll angle, and height information. Compared with the commonly used pose cells (which only contain 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] In the formula, 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 its 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 i-th pose cell affected.

[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] In the formula, Ψ a,b,c,d,e,f is the suppression form of the local excitation weight matrix, with negative weights. is the parameter that controls the global inhibition strength, and P i The values ​​in are constrained to be non-negative and normalized to represent the inhibitory correlation 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 flow and the activity change of each three-dimensional pose cell, and establish the activation correspondence between the vector of the image flow and each local view cell. The activated active local view cells are used to generate local excitation to the three-dimensional pose cells with activity association relationship to form the activity change of the corresponding three-dimensional pose cells.

[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, so that the image stream is input into the continuous attractor network, and the aforementioned learning relationship is trained so that the continuous attractor network obtains the experience of the activity change amount corresponding to the image stream during the loop.

[0133] In the system, the local view (the image stream of the cell input) vector V i The learning 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, are the learning matrices (w dimension) of the tth and t+1th training iterations of the uth pose cell, respectively. In addition to the prepared image stream, the training data also contains the calibrated activity changes, so that the continuous attractor network can obtain the learning relationship of the association experience between the pose and the image stream. It 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 the 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 the pose cell activity in the above-mentioned continuous attractor network can be expressed as:

[0138]

[0139] Where the constant σ determines the strength of local visual correction, which controls the sensitivity of changes in pose cell activity 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 excited through 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 inhibition association relationship of formula (5) and the inhibition 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 of the current actual posture of the robot. Therefore, the system can calculate the correction amount of posture deviation caused by long-term accumulated errors.

[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, using 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, build and train a continuous attractor network;

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

[0149] The steps of acquiring 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 image stream from depth camera in real time;

[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 compile them into histograms;

[0167] Compare the histogram of the current image with 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 match the verification loop;

[0172] Next, the PNP algorithm is used to calculate the camera pose based on high-confidence matching points and compared 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 position 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 excitatory and inhibitory connections, converging the connections of each unit to a stable state;

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

[0177] Pose cells are updated via attractor dynamics and local view calibration.

[0178] Wherein, in step H8:

[0179] Perform radar frame matching on the data that has passed the loop 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 posture obtained in step H2, and the global posture is corrected 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 translationally invariant, and its dynamic stable state constitutes a continuous subspace. This network model characterizes the brain's encoding process of continuous variables such as orientation, direction of movement, and spatial position. In computational neuroscience, continuous attractor networks are used to illustrate the mechanisms of many brain functions, such as the expression of spatial position and direction, the representation of head orientation, etc. The aforementioned continuous attractor network is constructed for 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, improving the real-time performance and efficiency of the system. The multi-source data fusion is adopted, combining the fusion of three different data sources: visual factor, IMU factor, and radar factor, which improves the accuracy and robustness of robot posture estimation in complex environments. At the same time, the complementarity of multi-source data can more comprehensively reflect environmental information and make up for the errors and uncertainties brought by 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 robot's pose estimation accuracy and robustness in such environments.

[0184] In the embodiments of this specification, in order to address the problem that the general loop detection algorithm has low detection efficiency when facing a complex orchard environment, double 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 the complex environment of the orchard, providing a new solution for intelligent agricultural machinery to achieve higher-precision autonomous operation.

[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 used to predict a global pose graph based on a plurality of sensor data carrying orchard environment information, wherein if the similarity of the descriptors of the current frame visual data in the plurality of sensor data does not meet the double loop verification threshold condition, the current frame visual data is used as a frame of reference frame visual data and recorded in a designated data storage unit;

[0187] The double loop verification module 1002 is used to determine a first loop verification result through a geometric relationship matching algorithm based on a sample point group 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 descriptor of the current frame visual data meets the double loop verification threshold condition, and determine a second loop verification result through a posture relationship matching algorithm based on a matching point selected as an inner point of the geometric relationship matching algorithm from the matching point set;

[0188] The radar frame judgment module 1003 is used to input the imported data corresponding to the current frame visual data into the continuous attractor network carrying the three-dimensional posture cells associated with the local view cells to determine the posture correction amount 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; 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;

[0189] The graph correction module 1004 is used to obtain an optimized global pose graph according to the pose correction amount and the global pose graph.

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

[0191] Acquire the perception factor of the corresponding sensor through the driver of each sensor and the corresponding captured sensor data, wherein the acquired perception factor includes a visual factor, an IMU factor and a radar factor;

[0192] The visual 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, wherein the global pose graph carries 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 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 in the following manner:

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

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

[0199] If the similarity of the descriptor of the current frame visual image does not meet the double-loop verification threshold condition, the current frame visual image is taken 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] 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 is constructed as a geometric model;

[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 inner point is higher 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 according to 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;

[0206] Determine a posture error between a posture amount of the depth camera and a current posture amount of the system;

[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 computing model with weighted excitation and inhibition connections; the continuous attractor network learns and records the activity association relationship between the image flow vector as the corresponding local view cell activity change amount and the three-dimensional pose cell according to the image flow in the training data; the three-dimensional pose cell of the continuous attractor network carries three-dimensional coordinate information and posture information;

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

[0210] Optionally, 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, including:

[0211] According to the pose estimation, adjusting the three-dimensional coordinate information and the pose information of the three-dimensional pose cells of the continuous attractor network;

[0212] According to the local view code, activate the local view cells of the continuous attractor network to obtain active local view cells, wherein the active local view cells are used to activate the 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 in the following manner:

[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 according to the six degrees of freedom parameters;

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

[0217] A learning relationship between the vector of the image stream and the activity change amount of each three-dimensional pose cell is configured, and a corresponding relationship between the vector of the image stream and the activation of each local view cell is established, where the activated local view cell is used to generate local excitation to the three-dimensional pose cell with an activity association relationship, so as to form the activity change amount of the corresponding three-dimensional 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 posture cell, the total activity change under local excitation is constructed to output the posture correction.

[0219] The embodiment of this specification also provides an orchard robot under the same inventive concept as the above embodiment, wherein the orchard robot is provided with a plurality of sensors, and the orchard robot comprises: at least one processor; a memory connected to the at least one processor; wherein the memory stores instructions executable by the at least one processor, and the at least one processor implements the above method by executing the instructions stored in the memory. The plurality of 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, and 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, and the orchard operation system may also include auxiliary equipment such as charging equipment and safety control equipment of the orchard robot. The method for correcting the posture of the orchard robot may include:

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

[0222] R2, according to the predicted global posture graph, calculates the predicted posture of the orchard robot's current posture (actual), superimposes or replaces the posture correction with the predicted posture, and obtains the corrected posture of the orchard robot's current posture. This makes the orchard robot work efficiently in the orchard, with high map accuracy and good real-time positioning, and has the characteristics of 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 previously visited position and posture while exploring the orchard environment.

[0224] It should also be noted that the aforementioned terms such as first and second are only used 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 inclusions, so that a process, method, commodity or device including elements in the present disclosure includes not only those elements, but also includes the coordination of other elements not explicitly listed, or also includes elements inherent to such process, method, commodity or device.

[0225] In the implementation process, each step of the above method can be completed by an integrated logic circuit of hardware in the processor or an instruction in the form of software. 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 gates or transistor logic devices, discrete hardware components. The 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 the processor can also be any conventional processor, etc. The steps of the method disclosed in the embodiments of this specification can be directly embodied as a hardware decoding processor to execute, or a combination of hardware and software modules in the decoding processor to execute. The software module can be located in a mature storage medium in the field such as a random access memory, a flash memory, a read-only memory, a programmable read-only memory or an electrically erasable programmable memory, a register, 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 above is a description of a specific embodiment of the specification. Other embodiments are within the scope of the appended claims. In some cases, the actions or steps recorded in the claims can be performed in an order different from that in the embodiments and still achieve the desired results. In addition, the processes depicted in the drawings do not necessarily require the specific order or continuous order shown to achieve the desired results. In some 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 protection scope of this specification. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of this specification shall be included in the protection scope 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 storage medium, which can include permanent and non-permanent, removable and non-removable media. Information storage can be achieved by any method or technology. 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 disk read-only memory (CD-ROM), digital versatile disk (DVD) or other optical storage, magnetic cassettes, magnetic tape, disk storage or other magnetic storage devices or any other non-transmission media that can be used to store information that can be accessed by a computing device. As defined herein, computer-readable media does not include temporary computer-readable media (transitory media), such as modulated data signals and carrier waves.

[0230] It should also be noted that the terms "include", "comprises" or any other variations thereof are intended to cover non-exclusive inclusion, so that a process, method, commodity or device including a series of elements includes not only those elements, but also other elements not explicitly listed, or also includes elements inherent to such process, method, commodity or device. In the absence of more restrictions, the elements defined by the sentence "comprises a ..." do not exclude the existence of other identical elements in the process, method, commodity or device including the elements.

[0231] Each embodiment in this specification is described in a progressive manner, and the same or similar parts between the embodiments can be referred to each other, and each embodiment focuses on the differences from other embodiments. In particular, for the system embodiment, since it is basically similar to the method embodiment, the description is relatively simple, and the relevant parts can be referred to the partial description of the method embodiment.

Claims

1. A pose graph optimization method based on multi-source SLAM loop closure, characterized in that: include: A global pose graph is predicted 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 frame of 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 sample point group 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 posture relationship matching algorithm based on a matching point selected as an inner point of the geometric relationship matching algorithm from the matching point set; When the first loop verification result and the second loop verification result are both passed, if the judgment operation of the two radar frame data respectively corresponding to the current frame visual data and a frame of reference frame visual data in the data storage unit is passed, the imported data corresponding to the current frame visual data is input into the continuous attractor network carrying the three-dimensional posture cells associated with the local view cells to determine the posture correction amount; wherein the imported data includes the image stream after the current frame visual data, which is used for mapping with the activity change amount of each local view cell; An optimized global pose graph is obtained according to the pose correction amount and the global pose graph.

2. The pose graph optimization method based on multi-source SLAM loop according to claim 1, characterized in that: The global pose graph is predicted based on the multiple sensor data carrying the orchard environment information, including: Acquire the perception factor of the corresponding sensor through the driver of each sensor and the corresponding captured sensor data, wherein the acquired perception factor includes a visual factor, an IMU factor, and a radar factor; The visual 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, wherein the global pose graph carries 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, characterized in that: 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 loopback according to claim 3, characterized in that: 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 acquisition method of the descriptor includes: Convert the descriptor of the current frame visual image extracted by the ORB algorithm into a current bag-of-words vector; The current word bag vector is calculated similarly to the word bag vector of the historical reference frame visual image in the database to determine the similarity of the current word bag vector; wherein, If the similarity of the descriptor of the current frame visual image does not meet the double-loop verification threshold condition, the current frame visual image is taken 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, characterized in that: The geometric relationship matching algorithm is selected as the RANSAC algorithm, wherein determining the first loop verification result includes: 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 is constructed as a geometric model; 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 inner point is higher 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, characterized in that: The posture relationship matching algorithm is selected as a PnP algorithm, wherein determining the second loop verification result includes: Calculate the pose of the depth camera according to 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; Determine a posture error between a posture amount of the depth camera and a current posture amount of the system; 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, characterized in that: The continuous attractor network is a neural network computing model with weighted excitation and inhibition connections; the continuous attractor network learns and records the activity association relationship between the image flow vector as the corresponding local view cell activity change amount and the three-dimensional posture cell according to the image flow in the training data; the three-dimensional posture cell of the continuous attractor network carries three-dimensional coordinate information and posture information; The imported data also includes a local view code and a pose estimate of the current frame visual data.

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

9. The pose graph optimization method based on multi-source SLAM loopback according to claim 7, characterized in that: 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 according to the six degrees of freedom parameters; A local excitation weight matrix is ​​created according to the distance coefficient and width constant between the three-dimensional posture cells, and the total activity change under the inhibition form is determined according to the activity change of each three-dimensional posture cell under local excitation and the local excitation weight matrix; A learning relationship between the vector of the image stream and the activity change amount of each three-dimensional pose cell is configured, and a corresponding relationship between the vector of the image stream and the activation of each local view cell is established, where the activated local view cell is used to generate local excitation to the three-dimensional pose cell with an activity association relationship, so as to form the activity change amount of the corresponding three-dimensional 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 posture cell, the total activity change under local excitation is constructed to output the posture correction.

10. A pose graph optimization device based on multi-source SLAM loop closure, characterized in that: include: A graph prediction module is used to predict a global pose graph based on a plurality of sensor data carrying orchard environment information, wherein if the similarity of the descriptors of the current frame visual data in the plurality of sensor data does not meet the double loop verification threshold condition, the current frame visual data is used as a frame of reference frame visual data and recorded in a designated data storage unit; A double loop verification module, for determining a first loop verification result through a geometric relationship matching algorithm based on a sample point group 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 descriptor of the current frame visual data meets the double loop verification threshold condition, and determining a second loop verification result through a posture relationship matching algorithm based on a matching point selected as an inner point of the geometric relationship matching algorithm from the matching point set; A radar frame judgment module, for inputting the imported data corresponding to the current frame visual data into a continuous attractor network carrying three-dimensional posture cells associated with local view cells to determine the posture correction amount 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 passed; 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; The graph correction module is used to obtain an optimized global pose graph according to the pose correction amount and the global pose graph.

11. An orchard robot, characterized in that: The orchard robot is provided with a variety of sensors, and the orchard robot includes: 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 9 by executing the instructions stored in the memory.

12. 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 9.

Images