Hierarchical simultaneous localization and mapping method for dense orchard

Abstract

This invention relates to the field of harvesting robot technology and discloses a hierarchical autonomous synchronous localization and mapping method for densely planted orchards, comprising the following steps: localizing the robot through depth estimation and pose estimation; performing multi-level mapping through macroscopic and microscopic mapping; in a regular orchard environment, with prior environmental assumptions, this invention achieves real-time estimation of the robot's position and posture through geometric information; constructing a multi-level map from multiple macroscopic and microscopic perspectives and multi-task viewpoints, providing corresponding information for different tasks; and enabling the robot to autonomously construct a 3D point cloud model of the fruit trees based on the incomplete 3D model of the fruit trees already established, achieving the goal of rapid 3D reconstruction of the fruit trees.

Description

Technical Field

[0001] This invention relates to the field of harvesting robot technology, specifically to a hierarchical autonomous synchronous positioning and mapping method for densely planted orchards. Background Technology

[0002] High-density orchards refer to an orchard management model that utilizes a limited land area to plant fruit trees at a high density through meticulous planning and management, thereby achieving higher yields and greater efficiency. High-density orchards maximize the use of fruit tree growing space, increasing fruit yield and quality, shortening the fruit tree growth cycle, reducing competition among fruit trees, and helping to improve the trees' disease resistance and stress tolerance, while effectively conserving land resources. At the same time, high-density orchards also improve the spatial and labor efficiency of fruit tree planting, reducing labor costs and further enhancing the orchard's production efficiency and economic benefits.

[0003] However, in densely planted orchards, the lush foliage of the fruit trees can obscure each other, hindering the robot's ability to identify and locate the fruit, making it difficult to perform accurate 3D reconstruction of the trees. To improve planting efficiency, the trees in densely planted orchards are planted in neat and regular positions, and the trees are highly similar, making it difficult to match feature points during synchronous localization and mapping.

[0004] Traditional localization methods, in unknown environments, either use the ICP algorithm to match the pose with the minimum reprojection error in a large, dense point cloud to obtain the optimal robot pose estimate, or reduce computation by extracting feature points from image data, matching feature points, and minimizing reprojection error to estimate the robot pose. The former is computationally too demanding and cannot meet the real-time requirements of simultaneous localization and mapping; the latter, in a regular orchard environment, easily matches multiple feature point pairs, making it difficult to calculate the robot pose.

[0005] Traditionally constructed land Figure 1 Generally, maps can be divided into occupancy grid maps, 3D point cloud maps, and semantic maps. Figure 3Occupied grid maps and 3D point cloud maps each have their own characteristics. Occupied grid maps can intuitively represent the distribution of obstacles and free space in the environment, used for robot autonomous localization and path planning, supporting robot movement control and path planning. They can also be updated in real time, reacting promptly to environmental changes. 3D point cloud maps provide richer environmental details, representing the shape of objects, surface normals, and other 3D features; furthermore, they can be used for object recognition and classification, supporting robot perception and understanding. However, both face similar difficulties in representing real-world environmental information in the computer world. Map construction requires significant computational and storage resources, placing high demands on the robot's storage capacity and consuming substantial computer resources. When dealing with complex environments, the map may suffer from insufficient resolution, leading to distortion or errors in environmental information. For handling unknown environments or different types of environments, simultaneous localization and mapping (SLT / Map) systems may require parameter readjustment or algorithm optimization, resulting in limited versatility. Semantic simultaneous localization and mapping (SLAM) systems are developed based on traditional SLAM (Simultaneous Localization and Mapping). Their core idea is to semantically segment objects in the environment, labeling the category and location of each object to achieve a deeper understanding of the environment. Compared to traditional geometric SLAM, semantic SLAM provides richer environmental semantic information, including not only the spatial structure of the map but also the semantic information of each object. This expansion enables robots to have greater flexibility and intelligence in practical applications, bringing more possibilities for the development of robotics applications.

[0006] However, traditional mapping methods have certain problems, and the environmental maps they construct cannot meet the needs of various tasks. For example, common occupancy grid maps cannot visually display the surrounding environment, and because classic path planning algorithms are mostly based on 2D grid maps, these maps lose a lot of 3D spatial information. Similarly, 3D point cloud maps or octree maps cannot provide convenient assistance for basic path planning tasks of robots. In the verification experiments of traditional mapping methods, the robot is manually driven to continuously collect data back and forth in the environment to build a complete map. Such a robot is merely a tool for mapping and does not demonstrate the ability of intelligent robots to autonomously explore and map instead of humans. Summary of the Invention

[0007] To address the aforementioned technical problems, this invention provides a hierarchical autonomous synchronous positioning and mapping method for densely planted orchards.

[0008] To solve the above-mentioned technical problems, the present invention adopts the following technical solution:

[0009] A hierarchical autonomous synchronous positioning and mapping method for densely planted orchards includes the following steps:

[0010] Step 1: Localize the robot using depth estimation and pose estimation.

[0011] The robot is equipped with a binocular camera to acquire images of the fruit trees around it. The distance between the robot and the center point of the tree trunk is calculated from the fruit tree images, which gives the depth of the tree trunk center point. The robot's pose information is calculated from the depths of the tree trunk center points of multiple fruit trees. The robot's pose information includes the robot's position and posture.

[0012] Step 2: Conduct multi-level mapping through macro- and micro-mapping:

[0013] Macro-mapping: By projecting the center point of the tree trunk in the fruit tree image onto the world coordinate system using the robot's pose information, a macro-map is constructed;

[0014] Microscopic mapping: The robot moves to the fruit tree where the mapping is not yet complete, and starts the drone to scan and photograph the fruit tree from multiple angles. The point cloud model of the fruit tree is constructed using the onboard IMU data of the drone. The fruit tree images captured by the drone's camera are converted from RGB color space to HSV color space and input into a pre-trained neural network for semantic recognition and segmentation, thus constructing a point cloud model of the fruit tree with semantic information.

[0015] Furthermore, in step one, when calculating the distance between the robot and the center point of the trunk of the surrounding fruit trees using the fruit tree image, the trunk of the fruit tree is regarded as a cylinder.

[0016] The binocular camera consists of a left camera and a right camera. The left and right boundary pixels of the tree trunk are extracted horizontally from the fruit tree image captured by the left camera. The midpoint between these two boundary pixels is the center point of the tree trunk. Similarly, the left and right boundary pixels of the tree trunk are extracted horizontally from the fruit tree image captured by the right camera. The midpoint between these two boundary pixels is also the center point of the tree trunk. Thus, the centers of the left and right cameras, along with the center point of the tree trunk, form a triangle. The depth of the tree trunk center point is estimated using triangulation.

[0017] Let P be the center point of the tree trunk, and O be the center point of the left and right cameras, respectively. L O R The number of pixels that point P is imaged in the left and right cameras are respectively P_1 and P_2. L P R The pixel coordinates are u l u rThe distance between the centers of the left and right cameras is b, the focal length of the left and right cameras is f, and the coordinates of the principal points of the left and right cameras are respectively... w l h l These represent the horizontal and vertical distances of the left camera offset, respectively. r h r These represent the horizontal and vertical distances of the right camera offset, respectively.

[0018] The following relationship is obtained through similar triangles:

[0019]

[0020] The depth of the tree trunk center point P is Z:

[0021]

[0022] Furthermore, in step one, when calculating the robot's pose information using the depth of the center point of the trunks of multiple fruit trees, assuming a prior map is available, the depth value D1 of the center point of the trunk of the first fruit tree is:

[0023]

[0024] The center point of the trunk of the first fruit tree is (x1, y1), and the estimated robot position coordinates are set to (x0, y0). The depth errors of the center points of the trunks of all fruit trees are summed:

[0025]

[0026] The robot's position is estimated by minimizing the sum of depth errors at the center points of the trunks of all fruit trees using a nonlinear optimization method, where D... i Let x be the depth value of the center point of the trunk of the i-th fruit tree. i ,y i Let x and y be the x and y coordinates of the center point of the trunk of the i-th fruit tree, respectively, and n be the total number of fruit trees.

[0027] The robot's pose is estimated by minimizing the sum of reprojection errors of feature points in fruit tree images acquired by binocular cameras using a nonlinear optimization method.

[0028] Furthermore, in the micro-mapping task, when constructing a point cloud model of fruit trees using airborne IMU data from a drone:

[0029] The shape of a fruit tree is simulated using a point cloud model. The model consists of a cylinder and a sphere on top of the cylinder. The cylinder represents the trunk, and the sphere represents the crown. Let the trunk height be h and its radius be r; let the radius of the crown be R. Then the volume V of the fruit tree is:

[0030]

[0031] Voxel filtering is applied to the established fruit tree point cloud model to obtain a fruit tree block model. The radius of the fruit tree crown and the radius of the trunk can be obtained from the fruit tree image.

[0032] The completion degree η of a fruit tree is:

[0033]

[0034] Where v is the sum of the volumes of all voxels in the fruit tree block model; if η is greater than the set value, the fruit tree is considered complete; if η is less than or equal to the set value, the fruit tree is considered incomplete.

[0035] Furthermore, in step two, when constructing the macro map,

[0036]

[0037] Where K is the intrinsic parameter matrix of the binocular camera, R is the rotation matrix representing the robot's posture, P is the coordinate of the tree trunk center point in the world coordinate system, t is the three-dimensional displacement vector representing the robot's position, u and v are the x and y coordinates of the imaging point in the pixel coordinate system, respectively, and f is the focal length of the binocular camera; in the world coordinate system, a fruit tree is represented by a circle, with the center of the circle being the tree trunk center point, and the diameter of the circle being the diameter of the tree trunk.

[0038] Compared with the prior art, the beneficial technical effects of the present invention are:

[0039] (1) In a regular orchard environment, with prior environmental assumptions, the robot's position and posture can be estimated in real time using geometric information.

[0040] (2) From macro and micro perspectives and multi-task perspectives, a multi-level map that can provide corresponding information for different tasks is constructed.

[0041] (3) Based on the incomplete three-dimensional model of the fruit tree that the robot has already established, a method for evaluating the exploration degree of the fruit tree is proposed, so that the robot can autonomously construct a three-dimensional point cloud model of the fruit tree and achieve the purpose of rapid three-dimensional reconstruction of the fruit tree. Attached Figure Description

[0042] Figure 1 This is an overall framework diagram of the autonomous synchronous positioning and mapping system of the present invention;

[0043] Figure 2 This is a flowchart of the positioning module of the present invention;

[0044] Figure 3 This is a schematic diagram illustrating the extraction of the tree trunk center point according to the present invention;

[0045] Figure 4 This is a schematic diagram illustrating the principle of binocular camera triangulation in this invention.

[0046] Figure 5 This is a schematic diagram illustrating the estimation of the robot's position according to the present invention;

[0047] Figure 6 This is a schematic diagram of the fruit tree model of the present invention. Detailed Implementation

[0048] A preferred embodiment of the present invention will now be described in detail with reference to the accompanying drawings.

[0049] Terminology Explanation:

[0050] Autonomous system: Give full play to the robot's initiative. Without external intervention, the robot can autonomously decide whether to continue searching for fruit in the fruit tree or choose to perform 3D reconstruction of the next fruit tree based on the existing model. It can intelligently and quickly build a 3D model of the fruit tree with sufficient detail.

[0051] Hierarchical structure: This approach completes the overall task from different dimensions, levels of mapping detail, and layers. Different layers reflect the different task requirements, enabling problems to be solved from various perspectives and angles.

[0052] Simultaneous Localization and Mapping (SLOM): SLOM is a real-time autonomous localization and mapping technology based on mobile sensors, operating without prior knowledge. This technology primarily involves two aspects: robot localization and environmental mapping. In localization, the robot perceives information about its surrounding environment through sensors and compares this information with an existing map to estimate and predict its position and orientation. In mapping, the robot projects environmental information data from its own pose into a world coordinate system, based on the robot's coordinate system data collected by the sensors, to construct a global map.

[0053] This invention proposes a hierarchical autonomous synchronous positioning and mapping method for densely planted orchards. The overall system framework is as follows: Figure 1 As shown.

[0054] Based on the actual environment of densely planted orchards, the system framework is divided into two main parts: positioning and multi-level mapping; the details are as follows.

[0055] 1. Positioning module

[0056] like Figure 2As shown, the localization module is divided into two parts: depth estimation and pose estimation. After receiving images from the left and right cameras, the binocular cameras calculate the distance information between the robot and the surrounding tree trunks, i.e., the trunk depth, by processing the fruit tree images. This information is then passed to the pose estimation part, which quickly calculates the robot's pose matrix in a regular, densely planted orchard environment using the estimated trunk depth.

[0057] 1.1 Depth Estimation:

[0058] In a densely planted orchard environment, the robot is surrounded by fruit trees. In large-scale localization problems, the tree trunks can be considered as uniform cylinders, such as... Figure 3 As shown.

[0059] In the RGB image of the fruit tree captured by the left camera, the left and right boundary pixels of the trunk can be easily extracted horizontally. The midpoint between these two boundaries is the pixel location of the trunk's center point. Similarly, the center point of the trunk can be extracted from the image captured by the right camera. The centers of the left and right cameras, along with the trunk's center point, form a triangle. Triangulation can be used to estimate the depth of the trunk, such as... Figure 3 As shown.

[0060] Figure 4 The principle of triangulation is given, assuming the center point of the tree trunk is P, O L O R Let P be the center of the left and right cameras, respectively. The pixel value of point P as an image in the left and right cameras is P_i. L P R The pixel coordinates are u l u r The distance between the centers of the left and right cameras is the baseline b, and the focal lengths of the left and right cameras are f. The principal point coordinates of the left and right cameras are respectively... w l h l These represent the horizontal and vertical distances of the left camera offset, respectively. r h r Let be the horizontal and vertical distances of the right camera offset, respectively. Then, using similar triangles, we have:

[0061]

[0062] The depth Z of the tree trunk center point P can be calculated:

[0063]

[0064] 1.2 Pose Estimation:

[0065] The robot's pose information includes the robot's position and robot posture.

[0066] Depth estimation can be used to calculate the distance information (trunk center point depth) between the robot and the center point of the tree trunk in the fruit tree image. Therefore, given a prior map, we have:

[0067]

[0068] Where D1 is the estimated depth of the first tree trunk center point, with coordinates (x1, y1), and the estimated robot position coordinates are set to (x0, y0). Multiple tree trunk center points are extracted from the image, resulting in multiple depth estimates for each center point. The sum of the depth errors of all tree trunk center points is then given as:

[0069]

[0070] Among them, D i To estimate the depth of the i-th trunk center point, x i Let y be the x-coordinate of the center point of the first tree trunk. i Let be the ordinate of the center point of the first tree trunk. Minimizing the above equation using a nonlinear optimization method yields the optimal estimate of the robot's position. The robot position estimation is as follows: Figure 5 As shown.

[0071] Once the robot's position is determined, only the robot's pose remains unknown. We can list the sum of the reprojection errors of feature points in the fruit tree images acquired by the binocular cameras, and then use nonlinear optimization to estimate the robot's pose, thereby solving for all the robot's pose information.

[0072] 2. Multi-level mapping module

[0073] In densely planted orchard environments, robots need to perform path planning and autonomous navigation, and harvesting robots also need to identify and collect fruit. Therefore, considering these multiple tasks, this invention employs a multi-level map structure to construct a map from both macroscopic and microscopic perspectives.

[0074] 2.1 Macroscopic Mapping

[0075] The robot's pose information, calculated by the localization module, is transmitted to the macro-mapping component. This pose information allows for the construction of a two-dimensional map at a macro-level. In the left camera image, the pixel coordinates of the tree trunk's center point can be obtained through pose estimation. This center point can then be projected onto the world coordinate system using the robot's pose information to construct the macro-map.

[0076]

[0077] A macroscopic map is a point cloud map in a world coordinate system. The coordinates of each point cloud are calculated using the formula above, based on the pixel data u and v obtained from the image and the camera's focal length f. P is the coordinate of the point cloud on the macroscopic map in the world coordinate system. Different pixels can have different P values ​​calculated using the above formula, resulting in different coordinate values; that is, different pixels are projected to different locations.

[0078] Where K is the intrinsic parameter matrix of the binocular camera, R represents the rotation matrix of the robot's pose, P is the coordinate of the tree trunk center point in the world coordinate system, t is the three-dimensional displacement vector representing the robot's position, u and v are the x and y coordinates of the imaging point in the pixel coordinate system, and f is the focal length of the binocular camera. Therefore, in the world coordinate system, a fruit tree can be represented by a circle, with the center of the circle being the tree trunk center point and the diameter of the circle being the diameter of the tree trunk, as shown below. Figure 5 As shown in the figure. The macroscopic two-dimensional map constructed in this way is very convenient for robots to perform path planning and autonomous navigation.

[0079] 2.2 Microscopic Mapping

[0080] For fruit picking tasks, robots need to identify fruits. Compared to the RGB color space, the HSV color space provides a higher degree of visual accuracy for object perception. Therefore, in microscopic mapping, the fruit tree image is first processed by converting it from the RGB color space to the HSV color space to form an HSV image. The HSV image is then input into a neural network for deep learning.

[0081] The robot moves to a fruit tree that has not yet been scanned in detail and activates its onboard drone to scan the tree from multiple angles using its camera. The point cloud model of the fruit tree can be reconstructed using data from the drone's onboard IMU. The fruit tree images captured by the drone camera are converted to the HSV color space and input into a pre-trained neural network for semantic recognition and segmentation. The resulting point cloud model of the fruit tree contains semantic information, facilitating subsequent fruit-picking by the fruit-picking robot.

[0082] use Figure 6 The illustrated block model of a fruit tree simulates its shape, consisting of a cylinder topped with a sphere. Let the trunk height be h, its radius be r, and the radius of the crown be R. Then the overall volume of the fruit tree is:

[0083]

[0084] Voxel filtering is applied to the established fruit tree point cloud model, which involves grouping points within a certain range together and representing them as a cube. This cube is called a voxel. In this way, the entire fruit tree model is transformed from a collection of points into a collection of voxels, resulting in a fruit tree cube model. The diameters of the tree crown and trunk can be obtained from the fruit tree image, and then the radii of the crown and trunk can be measured. Therefore, the completion rate of constructing a fruit tree is:

[0085]

[0086] Where V is the sum of the volumes of all voxels in the fruit tree block model. If η > 0.8, it means that the construction of the fruit tree is detailed enough and no further scanning is needed; otherwise, the drone continues to hover around the fruit tree to scan the surface and build the fruit tree model.

[0087] It is worth noting that there are gaps in the fruit trees. After voxel filtering, a voxel will be marked as a blank area from its position to the boundary of the fruit tree block model, instead of an area that has not yet been built.

[0088] Microscopic mapping allows for a detailed exploration of the number of fruits in each tree. The number of fruits is then weighted and labeled on the trees in a macroscopic two-dimensional map, providing more detailed information for robot path planning tasks.

[0089] 3. Hardware Implementation

[0090] The robotic arm system of this invention, a deep learning-based visual grasping mobile robot, is an open-source six-axis collaborative robotic arm system driven by four wheels, with communication via the CAN bus protocol. The robot is equipped with an Intel RealSense D435i camera as a visual grasping sensor, capable of acquiring real-time image and depth information of the surrounding environment. This camera is powered by an onboard power supply and a USB port. Simultaneously, the robot's core embedded AI platform integrates traditional image processing with deep learning, including traditional OpenCV processing methods and classification and object detection based on the TensorFlow 2 deep learning framework. To verify the system's feasibility, a regular densely planted orchard environment was constructed in the laboratory using cardboard boxes, and the experiments were repeated after changing the environment to verify the robustness of the system. The feasibility of the invention was also verified in a real orchard setting. In a real densely planted orchard, the method described in this invention can quickly and accurately estimate the robot's position, and the trained neural network constructs multi-task maps of different levels of the surrounding orchard environment.

[0091] It will be apparent to those skilled in the art that the present invention is not limited to the details of the exemplary embodiments described above, and that the invention can be implemented in other specific forms without departing from its spirit or essential characteristics. Therefore, the embodiments should be considered in all respects as exemplary and non-limiting, and the scope of the invention is defined by the appended claims rather than the foregoing description. Thus, all variations falling within the meaning and scope of equivalents of the claims are intended to be included within the present invention, and no reference numerals in the claims should be construed as limiting the scope of the claims.

[0092] Furthermore, it should be understood that although this specification describes embodiments, not every embodiment contains only one independent technical solution. This narrative style is merely for clarity. Those skilled in the art should consider the specification as a whole, and the technical solutions in each embodiment can also be appropriately combined to form other embodiments that can be understood by those skilled in the art.

Claims

1. A hierarchical autonomous synchronous positioning and mapping method for densely planted orchards, comprising the following steps: Step 1: Localize the robot using depth estimation and pose estimation. The robot is equipped with a binocular camera to acquire images of the fruit trees around it. The distance between the robot and the center point of the tree trunk is calculated from the fruit tree images, which gives the depth of the tree trunk center point. The robot's pose information is calculated from the depths of the tree trunk center points of multiple fruit trees. The robot's pose information includes the robot's position and posture. Step 2: Conduct multi-level mapping through macro- and micro-mapping: Macro-mapping: By projecting the center point of the tree trunk in the fruit tree image onto the world coordinate system using the robot's pose information, a macro-map is constructed; Microscopic mapping: The robot moves to the fruit tree where the mapping is not yet complete, and starts the drone to scan and photograph the fruit tree from multiple angles. The point cloud model of the fruit tree is constructed using the onboard IMU data of the drone. The fruit tree images captured by the drone's camera are converted from RGB color space to HSV color space and input into a pre-trained neural network for semantic recognition and segmentation, thus constructing a point cloud model of the fruit tree with semantic information. In step one, when calculating the distance between the robot and the center point of the trunk of the surrounding fruit trees using fruit tree images, the trunk of the fruit tree is regarded as a cylinder. The binocular camera consists of a left camera and a right camera. The left and right boundary pixels of the tree trunk are extracted horizontally from the fruit tree image captured by the left camera. The midpoint between these two boundary pixels is the center point of the tree trunk. Similarly, the left and right boundary pixels of the tree trunk are extracted horizontally from the fruit tree image captured by the right camera. The midpoint between these two boundary pixels is also the center point of the tree trunk. Thus, the centers of the left and right cameras, along with the center point of the tree trunk, form a triangle. The depth of the tree trunk center point is estimated using triangulation. Let P be the center point of the tree trunk, and O be the center point of the left and right cameras, respectively. L O R The number of pixels that point P is imaged in the left and right cameras are respectively P_1 and P_2. L P R The pixel coordinates are u l u r The distance between the centers of the left and right cameras is b, the focal length of the left and right cameras is f, and the coordinates of the principal points of the left and right cameras are respectively... ; , These represent the horizontal and vertical distances of the left camera offset, respectively. , These represent the horizontal and vertical distances of the right camera offset, respectively. The following relationship is obtained through similar triangles: ； The depth of the tree trunk center point P is : ； In step one, when calculating the robot's pose information by determining the depth of the center point of the trunk of multiple fruit trees, given a prior map, the depth value of the center point of the trunk of the first fruit tree... for: ； The coordinates of the center point of the trunk of the first fruit tree are: The estimated robot position coordinates are set as Sum the depth errors of the center points of the trunks of all fruit trees: ； The robot's position is estimated by minimizing the sum of depth errors at the center points of the trunks of all fruit trees using a nonlinear optimization method. For the first The depth value of the center point of the trunk of each fruit tree. The first The x-coordinate and y-coordinate of the center point of the trunk of each fruit tree. The total number of fruit trees; The robot's pose is estimated by minimizing the sum of reprojection errors of feature points in fruit tree images acquired by binocular cameras using a nonlinear optimization method.

2. The hierarchical autonomous synchronous positioning and mapping method for densely planted orchards according to claim 1, characterized in that, In micro-mapping tasks, when constructing a point cloud model of fruit trees using airborne IMU data from a drone: The shape of a fruit tree is simulated using a point cloud model. The model consists of a cylinder and a sphere atop the cylinder. The cylinder represents the trunk, and the sphere represents the crown. Let the trunk height be h and its radius be r; let the radius of the crown be R. Then, the volume of the fruit tree... for: ； Voxel filtering is applied to the established fruit tree point cloud model to obtain a fruit tree block model. The radius of the fruit tree crown and the radius of the trunk can be obtained from the fruit tree image. The completion rate of a fruit tree construction for: ； in This is the sum of the volumes of all voxels in the fruit tree block model; if If the value is greater than the set value, then the fruit tree construction completion rate meets the standard; like If the value is less than or equal to the set value, the completion rate of the fruit tree construction is not up to standard.

3. The hierarchical autonomous synchronous positioning and mapping method for densely planted orchards according to claim 1, characterized in that, In step two, when constructing the macro map, ； Where K is the intrinsic parameter matrix of the stereo camera, R is the rotation matrix representing the robot's pose, P is the coordinate of the tree trunk center point in the world coordinate system, and t is the three-dimensional displacement vector representing the robot's position. , These are the x and y coordinates of the imaging point in the pixel coordinate system, respectively. The focal length of the binocular camera; in the world coordinate system, a fruit tree is represented by a circle, with the center of the circle being the center point of the trunk, and the diameter of the circle being the diameter of the trunk.

Images