A method and system for automatic selection of visual markers for mobile robots

Abstract

The present application belongs to the field of visual monitoring, and particularly relates to a visual marker automatic selection method and system for a mobile robot, comprising: acquiring monocular image frame sequences and odometer data at continuous time points; detecting and decoding visual markers for each image to obtain measurement poses of the visual markers, and adaptively assigning uncertainty covariance matrices according to pixel areas, detection confidence or distances of the visual markers; calculating traces of the covariance matrices corresponding to the measurement poses, screening out effective observations satisfying an accuracy threshold, and selecting the one with the minimum trace as a representative observation when there are multiple effective observations at the same time; calculating a pose change between adjacent representative observation time points according to the odometer and assigning a motion uncertainty covariance matrix; taking the pose at the representative observation time point as an optimization variable, constructing a weighted optimization objective function, and solving the objective function to obtain an optimized pose and output a current positioning result.

Description

Technical Field

[0001] This invention belongs to the field of visual monitoring, and particularly relates to a method and system for automatic selection of visual markers for mobile robots. Background Technology

[0002] In high-density planting environments such as smart agriculture greenhouses and plant factories, autonomous mobile robots (such as inspection robots and harvesting robots) need to perform all-weather operations in narrow, repetitive crop rows. They typically rely on visual markers (such as Apriltag codes and ArUco codes) pre-deployed in the environment for absolute positioning and navigation. The robot's monocular camera acquires images containing visual markers in real time, and obtains its own pose information in the marker coordinate system through decoding and pose calculation. This information is then fused with odometry data to achieve continuous positioning. Under complex conditions such as crop canopy shading, dynamic changes in light, and frequent robot starts and stops, the observation quality of visual markers exhibits strong time-varying characteristics: when the robot is far from the marker, the pose obtained from decoding a single frame image is limited by camera resolution and viewing angle, resulting in significant measurement noise and uncertainty; when the robot approaches the marker, the observation accuracy is higher, but the marker may move out of the field of view. Meanwhile, odometry inevitably introduces cumulative errors during long-distance operation, and factors such as wheel slippage and uneven ground further exacerbate positioning drift.

[0003] In existing technologies, a common approach is to use single-frame visual markers to calculate pose as the basis for absolute positioning, such as updating the position when the robot passes over a marker. This approach is feasible in scenarios where markers are densely deployed and the robot always passes close to the markers. However, in actual greenhouse environments, the spacing between markers is usually large. When the robot is running in an area far from the markers, the single-frame positioning error can reach the centimeter level or higher, which is difficult to meet the millimeter-level accuracy requirements of precision operations (such as fruit picking and targeted pesticide application).

[0004] Another approach is based on Extended Kalman Filtering (EKF) to fuse visual marker and odometry data, using marker observations as absolute corrections and suppressing errors through state prediction and updates. However, EKF is extremely sensitive to the tuning of the system noise model and the initial covariance matrix, and is prone to filter divergence or slow convergence when visual observations are intermittently lost or the noise is non-Gaussian distributed. Furthermore, the Markov property of EKF prevents it from fully utilizing the constraints between historical observations at multiple time points, and also introduces additional errors into the approximate linearization of nonlinear systems.

[0005] Therefore, the technical problem that the existing technology urgently needs to solve is how to automatically select visual marker observations at multiple effective moments during the continuous movement of the robot, and perform global joint optimization with odometry motion constraints, so as to achieve high-precision estimation of robot pose while ensuring real-time performance, and overcome the defects such as large uncertainty of single-frame observation, odometry cumulative error and complex filter design. Summary of the Invention

[0006] To address the shortcomings of existing technologies, this invention proposes an automatic visual marker selection method and system for mobile robots, comprising: acquiring a sequence of monocular image frames and odometry data at consecutive time points; performing visual marker detection and decoding on each frame to obtain the measured pose of each visual marker, and adaptively assigning an uncertainty covariance matrix based on the pixel area, detection confidence, or distance of the visual marker; calculating the trace of the covariance matrix corresponding to each measured pose, filtering out valid observations that meet the accuracy threshold, and selecting the observation with the smallest trace as the representative observation when multiple valid observations exist at the same time; calculating the pose change at adjacent representative observation times based on odometry and assigning the motion uncertainty covariance matrix; constructing a weighted optimization objective function using the pose at the representative observation time as the optimization variable, wherein the first constraint term constrains the deviation between the optimized pose and the measured pose, and the second constraint term constrains the deviation between the adjacent optimized pose changes and the odometry changes; solving the objective function to obtain the optimized pose and outputting the current localization result. This invention significantly improves the localization accuracy and robustness of long-distance visual markers by adaptively selecting high-precision visual marker observations through uncertainty evaluation and integrating odometry for nonlinear optimization.

[0007] To achieve the above objectives, the present invention provides the following technical solution:

[0008] An automatic visual tag selection method for mobile robots, comprising:

[0009] Acquire a sequence of monocular image frames and a sequence of odometry data frames at consecutive time points;

[0010] Visual marker detection and decoding are performed on the image frame at each time step to obtain the measured pose of each visual marker in the image at that time step. An uncertainty covariance matrix is ​​assigned to each measured pose based on at least one of the following factors: the pixel area of ​​the visual marker in the image, the detection confidence, or the distance between the visual marker and the camera.

[0011] For the image frames at the current time and the previous T consecutive time points, calculate the trace of the uncertainty covariance matrix corresponding to each measurement pose, compare the trace with a preset accuracy threshold, and filter out the measurement poses whose traces are less than or equal to the preset accuracy threshold to form an effective observation set.

[0012] When there are M visually marked measurement poses belonging to the effective observation set at the same time, the measurement pose with the smallest trace of the uncertainty covariance matrix is ​​selected as the representative observation at that time, and the representative observation and its corresponding uncertainty covariance matrix are recorded.

[0013] Based on the odometry data frame sequence, calculate the change in camera coordinate system pose between adjacent representative observation times, and assign a motion uncertainty covariance matrix to each pose change.

[0014] Using the camera pose to be optimized at each representative observation time as the optimization variable, a weighted optimization objective function is constructed;

[0015] The weighted optimization objective function is solved to obtain the optimized camera pose for each observation time. The optimized camera pose corresponding to the current time is extracted and output as the robot's current localization result.

[0016] Specifically, the objective function includes a first constraint term and a second constraint term. The first constraint term is the deviation between the camera pose to be optimized at each representative observation time and the measurement pose of the corresponding representative observation, and its weight is determined by the inverse matrix of the uncertainty covariance matrix corresponding to the representative observation. The second constraint term is the deviation between the change in the camera pose to be optimized at adjacent representative observation times and the change in pose, and its weight is determined by the inverse matrix of the motion uncertainty covariance matrix.

[0017] Specifically, the measured pose of each visual marker in the image at that moment is obtained, including:

[0018] Obtain the current monocular image frame;

[0019] Perform a visual marker detection algorithm on the image frame to generate a list of candidate marker regions;

[0020] Iterate through each candidate marker region in the candidate marker region list and perform the following operations:

[0021] Decode the candidate marked region and determine whether the decoding was successful.

[0022] If decoding is successful, the identification information of the visual mark and the two-dimensional pixel coordinates of the corresponding four corner points in the image coordinate system are obtained;

[0023] Based on the preset physical size of the visual marker, camera intrinsic parameters, and the two-dimensional pixel coordinates of the four corner points, the perspective n-point projection algorithm is used to calculate the rotation matrix and translation vector of the current camera coordinate system relative to the visual marker coordinate system, so as to obtain the measurement pose corresponding to the visual marker. The measurement pose includes position components and attitude angle components.

[0024] The pixel area occupied by the visual mark in the image is calculated based on the two-dimensional pixel coordinates of the four corner points;

[0025] The confidence score output by the decoding algorithm is used as the detection confidence score.

[0026] The distance between the visual marker and the camera is calculated based on the magnitude of the translation vector;

[0027] The measured pose is associated with the corresponding pixel area, detection confidence, and distance and stored as the observation data of the visual label;

[0028] If decoding fails, the candidate marker region is skipped and no observation data is generated;

[0029] After the traversal is complete, the set of observation data of all visual markers at the current time is output as the measurement pose and associated parameters of each visual marker in the image at that time.

[0030] Specifically, an uncertainty covariance matrix is ​​assigned to each measurement pose, including:

[0031] Obtain the pixel area of ​​the visual marker in the image;

[0032] If the pixel area is less than a preset area threshold, then the position variance and attitude angle variance corresponding to the measured pose are both set to the first preset variance value.

[0033] If the pixel area is greater than or equal to the preset area threshold, then the base values ​​of position variance and attitude angle variance are calculated according to the ratio of the pixel area to the preset reference area, in an inverse relationship, that is, the base values ​​are inversely proportional to the ratio.

[0034] Obtain the detection confidence score of the visual marker;

[0035] If the detection confidence is less than a preset confidence threshold, the base value is increased according to the confidence deficit ratio, where the confidence deficit ratio is the difference between the preset confidence threshold and the detection confidence divided by the preset confidence threshold.

[0036] Obtain the distance between the visual marker and the camera;

[0037] If the distance is greater than a preset distance threshold, the base value is further increased according to the distance excess ratio, where the distance excess ratio is the difference between the distance and the preset distance threshold divided by the preset distance threshold.

[0038] Using the final determined position variance and attitude angle variance as diagonal elements, a diagonal covariance matrix is ​​constructed, which serves as the uncertainty covariance matrix of the measured pose.

[0039] Specifically, the trace of the uncertainty covariance matrix corresponding to each measurement pose is calculated, including:

[0040] Obtain the measured pose of each visual marker and its corresponding uncertainty covariance matrix in the image frames of the current time and the previous N consecutive time steps;

[0041] For each of the measured poses, the trace of the uncertainty covariance matrix corresponding to the measured pose is calculated. The trace is the sum of the diagonal elements of the covariance matrix and is used to characterize the total uncertainty of the measured pose in the position component and the attitude angle component.

[0042] Each trace is compared with a preset accuracy threshold;

[0043] If the trace is less than or equal to the preset accuracy threshold, the measured pose is determined to meet the accuracy requirements and is included in the valid observation set.

[0044] If the trace is greater than the preset accuracy threshold, the measured pose is determined not to meet the accuracy requirements and is discarded.

[0045] If at the same time there are N visually marked measurement poses included in the effective observation set, then the size of the traces corresponding to each measurement pose is compared, the measurement pose with the smallest trace is selected as the representative observation at that time, and the representative observation and its corresponding uncertainty covariance matrix are recorded.

[0046] If only one visually marked measurement pose is included in the effective observation set at any given time, then the measurement pose is directly used as the representative observation at that time, and the representative observation and its corresponding uncertainty covariance matrix are recorded.

[0047] Output the representative observations and their uncertainty covariance matrices for each representative observation time.

[0048] Specifically, the calculation of the camera coordinate system pose change between adjacent representative observation times includes:

[0049] Acquire the odometry data frame sequence, which contains the robot's linear velocity and angular velocity measurements at each moment.

[0050] Determine the time interval between adjacent representative observation times, wherein the adjacent representative observation times include the first representative observation time t. i The second represents the observation time t i+1 , where t i <t i+1 ;

[0051] According to the odometer data frame sequence t i To t i+1For all odometer data frames within the time period, the linear velocity and angular velocity measurements are integrated to calculate the value from t. i Time to t i+1 The change in camera coordinate system pose at time t, wherein the change in pose includes the relative displacement vector and the relative rotation angle.

[0052] Specifically, calculating the change in camera coordinate system pose between adjacent representative observation times also includes:

[0053] Obtain preset odometer model noise parameters, including linear velocity measurement noise variance, angular velocity measurement noise variance, and zero bias stability parameters;

[0054] Based on the time interval between adjacent representative observation times, the number of odometer data frames used in the integration operation, and the noise parameters, the motion uncertainty covariance matrix of the pose change is calculated. The motion uncertainty covariance matrix is ​​a diagonal matrix, and its diagonal elements include a first diagonal element, a second diagonal element, and a third diagonal element. The first diagonal element corresponds to the variance of the relative displacement in the x-direction, the second diagonal element corresponds to the variance of the relative displacement in the y-direction, and the third diagonal element corresponds to the variance of the relative rotation angle.

[0055] The robot acquires wheel speed meter data and inertial measurement unit data within the time interval. Based on the consistency deviation between the wheel speed meter data and the inertial measurement unit data, the slip coefficient is calculated. The slip coefficient ranges from 0 to 1, where 0 represents no slip and 1 represents complete slip.

[0056] Specifically, calculating the change in camera coordinate system pose between adjacent representative observation times also includes:

[0057] If the sliding coefficient is greater than the preset sliding threshold, then according to the ratio of the difference between the sliding coefficient and the preset sliding threshold, the first and second diagonal elements of the motion uncertainty covariance matrix are linearly increased, and the increase is proportional to the ratio of the difference, to obtain the updated first and second diagonal elements.

[0058] Obtain the sequence of angular velocity measurements of the robot within the time interval, and calculate the variance of the angular velocity measurement sequence as an index of motion smoothness;

[0059] If the motion smoothness index is greater than the preset smoothness threshold, then the third diagonal element of the motion uncertainty covariance matrix is ​​linearly increased according to the ratio of the motion smoothness index to the preset smoothness threshold, and the increase is proportional to the ratio, to obtain the updated third diagonal element.

[0060] The updated first diagonal elements, second diagonal elements, and third diagonal elements constitute the final motion uncertainty covariance matrix of the pose change.

[0061] Specifically, solving the weighted optimization objective function includes:

[0062] The initial values ​​of the camera pose to be optimized at each representative observation time, the measurement pose corresponding to the representative observation and its uncertainty covariance matrix, and the pose change and motion uncertainty covariance matrix between adjacent representative observation times are obtained.

[0063] Based on the initial value, measured pose, pose change, and corresponding covariance matrix, a weighted optimization objective function for the current iteration is constructed. The weighted optimization objective function includes a first constraint term and a second constraint term. The first constraint term is the sum of squares of the deviations between the camera pose to be optimized and the corresponding measured pose at each representative observation time, weighted by the inverse of the corresponding uncertainty covariance matrix. The second constraint term is the sum of squares of the deviations between the changes in the camera pose to be optimized and the changes in pose at adjacent representative observation times, weighted by the inverse of the corresponding motion uncertainty covariance matrix.

[0064] The weighted optimization objective function is linearized at the current iteration value, the Jacobian matrix and Hessian matrix are calculated, and the incremental equation is solved to obtain the update increment of the camera pose to be optimized.

[0065] The camera pose to be optimized is updated according to the update increment to obtain the updated camera pose to be optimized;

[0066] Determine whether the modulus of the update increment is less than a preset convergence threshold;

[0067] If the magnitude of the update increment is less than the preset convergence threshold, the optimization convergence is determined, and the current updated camera pose to be optimized is output as the optimization result.

[0068] If the magnitude of the update increment is greater than or equal to the preset convergence threshold, then it is determined whether the current iteration number has reached the preset maximum iteration number.

[0069] If the current iteration count has not reached the preset maximum iteration count, the iteration count is increased, and the weighted optimization objective function value is recalculated.

[0070] If the current iteration count reaches the preset maximum iteration count, the optimization is determined to be non-convergent, and the camera pose to be optimized after the last update is output as the optimization result, or an optimization failure signal is output.

[0071] An automatic visual tag selection system for mobile robots includes:

[0072] The acquisition module is used to acquire continuous sequences of monocular image frames and odometry data frames.

[0073] The tag perception module performs visual tag detection and decoding on the image frame at each time step to obtain the measured pose of each visual tag in the image at that time step, and assigns an uncertainty covariance matrix to each measured pose based on at least one of the following factors: the pixel area of ​​the visual tag in the image, the detection confidence, or the distance between the visual tag and the camera.

[0074] The effective observation filtering module calculates the trace of the uncertainty covariance matrix corresponding to each measurement pose for the image frames of the current time and the previous T consecutive time points, compares the trace with a preset accuracy threshold, and filters out the measurement poses whose traces are less than or equal to the preset accuracy threshold to form an effective observation set.

[0075] The selection module selects the measurement pose with the smallest trace of the uncertainty covariance matrix as the representative observation at that moment when there are M visual markers whose measurement poses belong to the effective observation set at the same time, and records the representative observation and its corresponding uncertainty covariance matrix.

[0076] The motion estimation module calculates the change in camera coordinate system pose between adjacent representative observation times based on the odometry data frame sequence, and assigns a motion uncertainty covariance matrix to each pose change.

[0077] The optimization module uses the camera pose to be optimized at each representative observation time as the optimization variable to construct a weighted optimization objective function. The objective function includes a first constraint term and a second constraint term. The first constraint term is the deviation between the camera pose to be optimized at each representative observation time and the measurement pose of the corresponding representative observation, and its weight is determined by the inverse matrix of the uncertainty covariance matrix corresponding to the representative observation. The second constraint term is the deviation between the change in camera pose to be optimized between adjacent representative observation times and the change in pose, and its weight is determined by the inverse matrix of the motion uncertainty covariance matrix.

[0078] The solution module solves the weighted optimization objective function to obtain the optimized camera pose for each observation time, extracts the optimized camera pose corresponding to the current time, and outputs it as the robot's current localization result.

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

[0080] This invention effectively solves the technical problem of low positioning accuracy of long-distance visual markers by introducing an adaptive uncertainty evaluation mechanism based on the observation quality of visual markers. Specifically, based on multi-dimensional factors such as the pixel area of ​​the visual marker in the image, detection confidence, and distance from the camera, an uncertainty covariance matrix is ​​dynamically assigned to each measurement pose, which can objectively quantify the measurement reliability under different observation conditions. Furthermore, by calculating the trace of the covariance matrix, effective observations that meet the accuracy threshold are selected, and in multi-marker scenarios, the representative observation with the lowest uncertainty is automatically selected to participate in optimization, thus fundamentally ensuring the quality of the input data. Based on this, a weighted optimization objective function containing visual constraints and odometry constraints is constructed. The weight of the visual constraint is adaptively adjusted by the inverse matrix of the observation uncertainty covariance matrix, making high-confidence observations dominate the optimization, while the odometry constraint smooths the pose changes between adjacent time steps by using the inverse matrix of the motion uncertainty covariance matrix. By solving this objective function, global optimal fusion of observation information from multiple time steps is achieved, overcoming the defect of large noise in single-frame long-distance observations and effectively suppressing the cumulative error of odometry. The final optimized camera pose output, while maintaining real-time performance, significantly improves the positioning accuracy and robustness in complex scenarios such as long distances and weak textures, providing reliable support for high-precision autonomous navigation of mobile robots in environments such as smart agriculture. Attached Figure Description

[0081] Figure 1 This is a flowchart of an automatic visual marker selection method for a mobile robot according to Embodiment 1 of the present invention;

[0082] Figure 2 The trace logic diagram for calculating the uncertainty covariance matrix corresponding to each measurement pose in Embodiment 1 of the present invention;

[0083] Figure 3 This is a structural diagram of an automatic visual marker selection system for a mobile robot according to Embodiment 2 of the present invention. Detailed Implementation

[0084] Example 1

[0085] Please see Figure 1 The present invention provides an embodiment of an automatic visual marker selection method for mobile robots, comprising the following steps:

[0086] S1. Obtain the sequence of monocular image frames and the sequence of odometer data frames at consecutive time points;

[0087] S2. Perform visual marker detection and decoding on the image frame at each time step to obtain the measured pose of each visual marker in the image at that time step, and assign an uncertainty covariance matrix to each measured pose based on at least one of the following factors: the pixel area of ​​the visual marker in the image, the detection confidence, or the distance between the visual marker and the camera.

[0088] S3. For the image frames at the current time and the previous T consecutive time points, calculate the trace of the uncertainty covariance matrix corresponding to each measurement pose, compare the trace with a preset accuracy threshold, and filter out the measurement poses whose traces are less than or equal to the preset accuracy threshold to form an effective observation set.

[0089] S4. When there are M visual markers whose measured poses belong to the effective observation set at the same time, select the measured pose with the smallest trace of the uncertainty covariance matrix as the representative observation at that time, and record the representative observation and its corresponding uncertainty covariance matrix.

[0090] S5. Based on the odometry data frame sequence, calculate the change in camera coordinate system pose between adjacent representative observation times, and assign a motion uncertainty covariance matrix to each pose change.

[0091] S6. Using the camera pose to be optimized at each representative observation time as the optimization variable, construct a weighted optimization objective function. It should be further explained that the objective function in this embodiment includes a first constraint term and a second constraint term. The first constraint term is the deviation between the camera pose to be optimized at each representative observation time and the measurement pose of the corresponding representative observation, and its weight is determined by the inverse matrix of the uncertainty covariance matrix corresponding to the representative observation. The second constraint term is the deviation between the change in the camera pose to be optimized between adjacent representative observation times and the change in pose, and its weight is determined by the inverse matrix of the motion uncertainty covariance matrix.

[0092] S7. Solve the weighted optimization objective function to obtain the optimized camera pose for each observation time, extract the optimized camera pose corresponding to the current time, and output it as the robot's current localization result.

[0093] It should be further explained that this embodiment obtains the measured pose of each visual marker in the image at that moment, including:

[0094] S201. Obtain the monocular image frame at the current moment. Specifically, the image is acquired in real time by a monocular camera at a fixed frame rate (e.g., 30fps). The original RGB image frame at the current moment is obtained. The image resolution is preset to 1920×1080 pixels, and the image format is 8-bit grayscale or color image.

[0095] S202. Perform a visual marker detection algorithm on the image frame to generate a candidate marker region list; specifically:

[0096] 1.1 Obtain the current RGB image frame to be detected and convert it into an 8-bit grayscale image;

[0097] 1.2 The grayscale image is binarized using an adaptive threshold segmentation algorithm. The adaptive threshold window size is set to N×N (N is an odd number between 31 and 101). The mean value of pixels in the neighborhood of each pixel is calculated. The mean value minus a preset offset C (C is between 2 and 5) is used as the threshold value of that pixel.

[0098] 1.3 For each pixel, if its gray value is greater than the corresponding threshold, it is determined to be the foreground (white); otherwise, it is determined to be the background (black), and a binarized image is generated.

[0099] 2.1. Perform a contour detection algorithm on the binarized image to extract the contours of all connected components and filter out candidate contours that are closed contours and have ≥4 contour points.

[0100] 2.2 Perform polygon approximation processing on each candidate contour, setting the approximation accuracy to 0.02 times the contour perimeter, to obtain the approximation polygon of the contour;

[0101] 2.3. Select the contours with 4 sides that approximate the polygon, and denote them as the quadrilateral contour set;

[0102] 3.1 For each quadrilateral contour in the set of quadrilateral contours, perform a corner point sorting operation:

[0103] 3.1.1 Calculate the coordinates of the four corner points of the quadrilateral outline. Based on the coordinate positions of the corner points, sort them in a clockwise or counterclockwise direction to obtain an ordered corner point sequence P1(u1, v1), P2(u2, v2), P3(u3, v3), P4(u4, v4).

[0104] 3.2 Calculate the homography matrix H0 based on the sorted corner point sequence:

[0105] 3.2.1 Set the standard physical size of the Apriltag code to W×H (e.g., 16×16mm), and determine the coordinates of the four corner points of the standard Apriltag code in the world coordinate system: Q1(-W / 2, -H / 2, 0), Q2(W / 2, -H / 2, 0), Q3(W / 2, H / 2, 0), and Q4(-W / 2, H / 2, 0).

[0106] 3.2.2, Using the ordered corner point sequence {P} in the image coordinate system i} and the standard corner point sequence {Q} in the world coordinate system iGiven} as input, solve for the homography matrix H0 (3×3 matrix) using the DLT (Direct Linear Transformation) algorithm;

[0107] 3.3 Performing Apriltag code geometric constraint filtering based on homography matrix and corner sequence:

[0108] 3.3.1 Calculate the side length ratio of the quadrilateral outline: Calculate the Euclidean distances between adjacent corner points: L1=||P2-P1||, L2=||P3-P2||, L3=||P4-P3||, L4=||P1-P4||, and calculate the ratio of the longest side length to the shortest side length. ,like If the side length ratio exceeds the preset threshold (e.g., 1.0~1.5), the quadrilateral outline will be removed.

[0109] 3.3.2 Calculating the interior angles of a quadrilateral: Based on the sorted sequence of corner points, calculate the angle value θ of each interior angle using the vector dot product formula. i (i=1~4), if any internal angle θ i If the angle exceeds a preset threshold (e.g., 80°~100°), the quadrilateral outline will be discarded.

[0110] 3.3.3 Verify the rationality of the homography matrix: Calculate the singular values ​​of the homography matrix H0. If the maximum ratio of the singular values ​​exceeds the preset threshold (e.g., 10), it is determined that the quadrilateral contour does not meet the perspective transformation constraint and the contour is removed.

[0111] 4.1. Retain all quadrilateral contours that satisfy the above geometric constraints, and assign an ordered corner point sequence P to each contour. i (u i v i (i=1……4), homography matrix H0 and contour number are stored together;

[0112] 4.2. Integrate all eligible quadrilateral contour information to generate a candidate marker region list L_candidates. Each element in the list is a candidate marker region, containing only the two-dimensional pixel coordinates (u, u) of the four corner points of that region in the image coordinate system. i v i (i=1……4).

[0113] S203. Traverse each candidate marker region in the candidate marker region list and perform the following operations:

[0114] The existing steps only address scenarios where the Apriltag code is fully detected. They do not handle situations where the marker is partially obscured by crops (such as vines or leaves), leading to missing corner points. This can easily result in lost effective observations and affect the continuity of localization. Therefore, this embodiment adds corner point integrity verification and pose uncertainty adjustment steps before decoding the candidate marker region, including:

[0115] S203-1. Count the number of detectable corners n_corner in the candidate marker region, where the number of corners corresponding to the complete Apriltag code is 4;

[0116] S203-2. If n_corner < 2, then the candidate marked region is determined to be an invalid region, and the subsequent decoding and pose calculation steps of the candidate marked region are skipped.

[0117] S203-3. If 2 ≤ n_corner < 4, then activate the partial corner pose calculation mode, which includes:

[0118] S203-4. Obtain the effective observation poses of the Apriltag code within the last 10 frames and use them as the prior pose pose_prior.

[0119] S203-5. Based on the preset camera intrinsic parameter matrix, predict the pixel coordinates of the missing corner points in the image coordinate system according to the prior pose_prior.

[0120] S203-6. Combine the measured corner points and the predicted missing corner points in the candidate marked region into a corner point set containing 4 corner points, and perform Apriltag code decoding and pose calculation operations based on the corner point set;

[0121] S203-7. If n_corner=4, then directly perform the original Apriltag code decoding and pose calculation process on the candidate marker region.

[0122] S203-8. For the measured pose obtained through partial corner pose calculation mode, an a priori weight coefficient α is introduced to adjust its uncertainty covariance matrix, including the following steps:

[0123] S203-9. Calculate the prior weight coefficient α. The value of α is the proportion of the number of corner points, i.e., α = n_corner / 4. The value of α is in the range of 0 < α < 1. The smaller α is, the higher the dependence of the measured pose on the prior pose.

[0124] S203-10. Multiply the diagonal elements of the original uncertainty covariance matrix corresponding to the measured pose obtained by partial corner pose calculation by 1 / α to increase the uncertainty of pose in partial corner calculation scenarios.

[0125] S204. Decode the candidate marker region and determine whether the decoding was successful. Specifically:

[0126] Perform perspective correction on the image within the current candidate marker region to obtain a corrected image of fixed size, specifically:

[0127] 1.1 Extract the two-dimensional pixel coordinates of the four corner points of the candidate marker region in the image coordinate system;

[0128] 1.2. Set the target image after perspective correction as a fixed-size quadrilateral grid, and determine the coordinates of the four vertices of the target grid. The quadrilateral grid adopts a fixed specification such as 8×8.

[0129] 1.3. Using the corner coordinates of the candidate marked region as the original coordinates and the vertex coordinates of the target mesh as the target coordinates, the perspective transformation matrix is ​​obtained by solving.

[0130] 1.4 Apply the obtained perspective transformation matrix to the original image within the candidate marker region to complete the perspective correction of the image and obtain a corrected image with the same size as the target mesh.

[0131] The corrected image is binarized to generate a binary image with the same size as the corrected image, specifically as follows:

[0132] 2.1 Calculate the global grayscale mean of the corrected image, and use the global grayscale mean as the binarization threshold;

[0133] 2.2. Iterate through each pixel in the correction image one by one. If the gray value of a pixel is greater than the binarization threshold, then the pixel is assigned the value of white. If the gray value of a pixel is less than or equal to the binarization threshold, then the pixel is assigned the value of black. Generate a binary image with the same size as the correction image.

[0134] Extract the pixel values ​​of the binary image to generate the binary encoding of the Apriltag code, specifically as follows:

[0135] 3.1 Divide the binary image into cells of equal size, with each cell uniquely corresponding to a pixel in the binary image;

[0136] 3.2. Traverse all cells one by one in row-major order, extract the pixel color value corresponding to each cell, record white pixels as one value and black pixels as another value, and arrange all the extracted values ​​in the traversal order to form a one-dimensional binary code.

[0137] Based on a preset Apriltag series, the binary encoding is decoded and matched to calculate the Hamming distance, specifically as follows:

[0138] 4.1 Call the pre-set Apriltag tag series library. The tag series library can be a tag series such as tag36h11. The library stores the standard binary codes of all valid Apriltag codes under the corresponding tag series, as well as the unique identifier ID corresponding to each standard binary code.

[0139] 4.2. Iterate through each standard binary code in the tag series library one by one, and calculate the Hamming distance between the binary code to be decoded and each standard binary code. The Hamming distance is calculated based on the number of bits that are different at corresponding positions in two binary codes of equal length.

[0140] 4.3. Select the smallest distance from all the calculated Hamming distances and record it as the minimum Hamming distance. Also record the standard binary code corresponding to the minimum Hamming distance and the unique identifier ID corresponding to the code.

[0141] The decoding result is determined based on the minimum Hamming distance, specifically:

[0142] 5.1. Set a preset decoding threshold, such as a value of 2. Compare the minimum Hamming distance obtained from the screening with this preset decoding threshold. Further explanation is that the determination of the preset decoding threshold in this embodiment is based on a trade-off between the error correction coding principle of visual tags and engineering experience. During the decoding process, the system calculates the Hamming distance between the binary code to be decoded and each standard code in the tag series library, i.e., the number of different corresponding bits. By setting this threshold, for example, a value of 2, a small number of bit errors caused by image noise, partial occlusion, or perspective distortion are allowed, thereby improving the decoding success rate and system robustness while ensuring a low false detection rate. Typically, this value is calibrated based on the theoretical error correction capability of the selected tag series and the results of actual scenario testing. If the Hamming distance is less than this threshold, decoding is considered successful and the corresponding tag identifier is output; otherwise, it is deemed invalid, thus achieving highly reliable tag recognition.

[0143] 5.2 If the minimum Hamming distance is less than the preset decoding threshold, the Apriltag code of the candidate tag region is determined to be successfully decoded, and the unique identifier ID obtained by matching is output.

[0144] 5.3 If the minimum Hamming distance is greater than or equal to the preset decoding threshold, the Apriltag code decoding of the candidate marker region is determined to be unsuccessful, the candidate marker region is directly discarded, and no identification information is output.

[0145] S205. If decoding is successful, obtain the identification information of the visual marker and the two-dimensional pixel coordinates of its four corner points in the image coordinate system. Specifically, extract the unique identifier ID (e.g., ID=5) of the visual marker from the decoding result, while retaining the precise coordinates of the four corner points (u) of the candidate region obtained during the detection phase. i v i The coordinates are of floating-point type and the unit is pixels.

[0146] S206. Based on the preset physical size of the visual marker, camera intrinsic parameters, and the two-dimensional pixel coordinates of the four corner points, the perspective n-point projection algorithm is used to calculate the rotation matrix and translation vector of the current camera coordinate system relative to the visual marker coordinate system, thereby obtaining the measurement pose corresponding to the visual marker. The measurement pose includes position components and attitude angle components, specifically:

[0147] Obtain the preset physical side length of the visual marker L_marker (e.g., L_marker=0.12m), and the camera intrinsic parameter matrix K, where K includes the focal lengths f_x and f_y and the optical centers c_x and c_y (e.g., f_x=800, f_y=800, c_x=960, c_y=540); and convert the two-dimensional pixel coordinates of the four corner points (u i v i The camera coordinate system and its corresponding 3D physical coordinates in the marker coordinate system (e.g., the origin of the marker coordinate system is located at the center of the marker, and the coordinates of the four corner points are (-L_marker / 2, -L_marker / 2, 0), (L_marker / 2, -L_marker / 2, 0), (L_marker / 2, L_marker / 2, 0), (-L_marker / 2, L_marker / 2, 0)) are used as input, and the EPnP algorithm is used to solve for the camera coordinate system to the marker coordinate system. The rotation matrix R_cm (3×3 matrix) and translation vector t_cm (3D vector) of the coordinate system are marked; then the measured pose of the camera coordinate system in the marked coordinate system is obtained, where the position component is the three components of t_cm (e.g., x=0.15m, y=-0.02m, z=0.50m), and the attitude angle components are converted by the rotation matrix R_cm into Euler angles (e.g., pitch=-2.3°, yaw=10.5°, roll=1.2°) or quaternion representation.

[0148] S207. Calculate the pixel area occupied by the visual marker in the image based on the two-dimensional pixel coordinates of the four corner points, specifically: based on the two-dimensional pixel coordinates of the four corner points (u... i v i The area A_pixel of the quadrilateral is calculated using the shoelace formula:

[0149] A_pixel=0.5 |Σ(u i v i+1 -u i+1 v i )|, where i=1……4 and the 5th point is defined as the 1st point; obtain the pixel area occupied by the visual marker in the image (e.g., 15600 pixels). 2 If the quadrilateral is a convex quadrilateral and its area is greater than the preset minimum area threshold A_min (e.g., A_min = 100 pixels), then... 2 If the area value is less than or equal to the area of ​​the visual marker, then this area value is retained for subsequent uncertainty estimation. To further clarify, the preset minimum area threshold A_min in this embodiment is primarily based on the physical constraints of camera imaging resolution and reliable visual marker detection. In practical applications, the system comprehensively considers the effective pixel count of the monocular camera, the geometric relationship of the visual marker's projection in the image, and the accuracy requirements of subsequent perspective calculations for feature points, and determines this threshold through experimental calibration. When the pixel area occupied by the visual marker in the image is lower than this threshold, the marker corner point positioning error will significantly increase, leading to a sharp rise in the uncertainty of pose calculation, failing to meet the positioning accuracy requirements. Therefore, this threshold is usually set to the minimum effective area that ensures stable decoding and accurate pose estimation. For example, based on commonly used camera resolutions and marker physical sizes, after extensive testing and verification, a value of one hundred pixels squared is chosen to ensure the elimination of excessively small marker observations and guarantee the reliability of subsequent positioning processes.

[0150] S208. Obtain the confidence score output by the decoding algorithm as the detection confidence score. Specifically, obtain the confidence score c_conf of the visual tag during the decoding process. This score is calculated based on the decoding Hamming distance d_min, for example, c_conf=1-d_min / T_max, where T_max is the preset maximum Hamming distance (for example, T_max=10); the value range is between 0 and 1 (for example, c_conf=0.95), and the larger the value, the more reliable the decoding.

[0151] S209. Calculate the distance between the visual marker and the camera based on the magnitude of the translation vector;

[0152] S210. The measured pose is associated with and stored as the observation data of the visual marker, along with the corresponding pixel area, detection confidence, and distance. Specifically, this involves storing the current timestamp t_k, visual marker ID, measured pose pose_meas (including the position component t_cm and the attitude angle component R_cm), and pixel area A_pixel (e.g., 15600 pixels). 2The detection confidence level c_conf (e.g., 0.95) and distance d_dist (e.g., 0.52m) are combined into an observation data tuple and stored in the observation data cache at the current time, with the index being the tag ID.

[0153] S211. If decoding fails, the candidate marker region is skipped, and no observation data is generated. Specifically, for candidate regions where decoding fails (i.e., d_min ≥ T_decode), the region is discarded directly, no observation data is generated, and the process continues to traverse the next candidate region. Here, T_decode represents the Hamming distance threshold used to determine whether a visual marker is successfully recognized during the decoding process. Its preset value is mainly based on a comprehensive trade-off between the error correction coding capability of the selected visual marker label series and the image quality of the actual application scenario. The system first determines a basic threshold based on the theoretical error correction performance of the label series, for example, selecting a conservative value within the theoretical maximum correctable Hamming distance range to ensure the reliability of the decoding result. Subsequently, considering factors such as camera imaging quality, ambient light changes, and the distance between the marker and the camera, through extensive offline testing and field scenario verification, the decoding success rate and false detection rate under different thresholds are statistically analyzed. Finally, a value that can ensure a high decoding success rate while keeping the false detection rate within an acceptable range is selected as the preset threshold. This preset process aims to balance decoding robustness and positioning accuracy, ensuring that visual markers can still stably output effective observation data under complex working conditions.

[0154] S212. After the traversal is complete, output the set of observation data for all visual markers at the current time. This set serves as the measurement pose and associated parameters of each visual marker in the image at that time. Specifically, after traversing all candidate regions, integrate all observation data tuples in the current time buffer into a single observation data set O_t, indexed by the timestamp t_k, for subsequent steps to allocate the uncertainty covariance matrix and filter effective observations. If no observation data is generated at the current time, output an empty set.

[0155] It should be further explained that this embodiment assigns an uncertainty covariance matrix to each measurement pose, including:

[0156] S213. Obtain the pixel area of ​​the visual marker in the image;

[0157] S214. If the pixel area is less than a preset area threshold, the position variance and attitude angle variance corresponding to the measured pose are both set to the first preset variance value.

[0158] S215. If the pixel area is greater than or equal to the preset area threshold, then the basic values ​​of position variance and attitude angle variance are calculated according to the ratio of the pixel area to the preset reference area, in an inverse relationship, that is, the basic values ​​are inversely proportional to the ratio.

[0159] S216. Obtain the detection confidence level of the visual marker;

[0160] S217. If the detection confidence is less than a preset confidence threshold, the base value is increased according to the confidence deficit ratio, wherein the confidence deficit ratio is the difference between the preset confidence threshold and the detection confidence divided by the preset confidence threshold.

[0161] S218. Obtain the distance between the visual marker and the camera;

[0162] S219. If the distance is greater than a preset distance threshold, the base value is further increased according to the distance excess ratio, wherein the distance excess ratio is the difference between the distance and the preset distance threshold divided by the preset distance threshold.

[0163] S220. Using the final determined position variance and attitude angle variance as diagonal elements, construct a diagonal covariance matrix as the uncertainty covariance matrix of the measured pose.

[0164] For example, in this embodiment, the process of assigning an uncertainty covariance matrix to each measured pose is as follows: First, obtain the pixel area A_pixel of the visual marker in the image (e.g., 15600 pixels). 2 If A_pixel is less than the preset area threshold A_thresh=5000 pixels 2 Then, both the position variance σ_pos_low and the attitude angle variance σ_angle_low are set to the first preset variance value (σ_pos_low). 2 =0.01m 2 σ_angle_low 2 =0.0001rad 2 Otherwise, it depends on A_pixel and the preset reference area A_ref=10000 pixels. 2 The ratio r_area = 1.56, and the basic variance (σ_x_base) is calculated according to the inverse proportional relationship. 2 =σ_y_base 2 =0.005 / 1.56≈0.0032m 2 σ_θ_base 2 =0.00005 / 1.56≈3.21e-5rad 2Then, the detection confidence score is obtained as c_conf = 0.95. If c_conf is less than the preset confidence threshold c_thresh = 0.9, the base value is increased according to the confidence deficit ratio δ_c = (c_thresh - c_conf) / c_thresh; here, c_conf ≥ c_thresh, so it is not increased. Next, the distance d_dist = 0.52m is obtained. If d_dist is greater than the preset distance threshold d_thresh = 1.0m, the distance excess ratio δ_d = (d_dist - d_thresh) / d_thresh is further increased; here, d_dist ≤ d_thresh, so it is not increased. Finally, the location variance σ_x is used. 2 =σ_y 2 =0.0032m 2 and attitude angle variance σ_θ 2 =3.21e-5rad 2 Using diagonal elements, a diagonal covariance matrix R_k = diag(0.0032, 0.0032, 3.21e-5) is constructed as the uncertainty covariance matrix for this measurement pose. Further explanation is that the threshold settings in this example are based on an engineering trade-off between visual marker observation quality and positioning accuracy requirements, determined through a combination of theoretical analysis and experimental calibration. The preset area threshold A_thresh is set to 5,000 pixels squared, primarily based on camera resolution and the minimum detectable size of the marker. When the pixel area is lower than this value, the corner point extraction error of the marker increases significantly, leading to unreliable pose calculation. The preset reference area A_ref is set to 10,000 pixels squared, representing the typical area of ​​the marker under ideal close-range observation conditions, used to normalize the impact of area factors on uncertainty. The preset confidence threshold c_thresh is set to 0.9, based on the statistical distribution of the confidence level output by the decoding algorithm. Values ​​higher than this indicate higher reliability of the decoding result, while values ​​lower than this require increasing uncertainty to reflect potential errors in the decoding process. The preset distance threshold d_thresh is set to one meter, determined based on the camera's depth of field and the physical size of the marker. When the distance between the marker and the camera exceeds this threshold, the pixel projection error and depth estimation uncertainty will increase non-linearly, requiring compensation in the covariance matrix. Each threshold has been iteratively optimized through extensive offline testing and field scenario verification to ensure that the allocation of the uncertainty covariance matrix accurately reflects the measurement accuracy under different observation conditions, laying a reliable data foundation for subsequent effective observation screening and weighted optimization. The first preset variance value is set based on the maximum permissible positioning error of the visual marker under extreme observation conditions, determined through a combination of theoretical derivation and experimental calibration. The position variance σ_pos_low... 2The value is set to 0.01 square meters, representing the upper limit of the positional error that the perspective calculation algorithm may reach under long-distance or low-resolution conditions when the area of ​​the marked pixels is lower than the preset area threshold. This value is a conservative value adopted after calculating the theoretical error boundary based on the camera intrinsic parameter accuracy, the physical size of the marker, and the perspective projection model, and after extensive field testing. The attitude angle variance σ_angle_low 2 The variance is set to 0.0001 radians squared, corresponding to an upper limit of approximately 0.57 degrees for angular error. This reflects the pitch and yaw direction uncertainties that may be introduced into the rotation matrix solution when the marker imaging is blurred or the corner point positioning is inaccurate. The two variances are set to assign a uniform high uncertainty benchmark to low-quality observations, ensuring that such observations are given lower weights in subsequent optimizations and avoiding positioning biases introduced by over-reliance on unreliable measurements.

[0165] This process, through a series of optimization steps including adding robust processing for corner integrity verification and prior pose fusion, standardized Apriltag code detection and decoding processes, and dynamic allocation of the uncertainty covariance matrix for multi-dimensional parameter fusion, brings about multiple beneficial effects, with each effect forming a progressive derivation relationship: First, adding corner integrity verification before decoding the candidate marker region, and initiating a prior pose-assisted completion calculation mode for corner missing scenarios caused by partial occlusion, not only avoids the loss of effective observations and ensures the continuity of positioning observation in complex crop occlusion environments in smart agriculture, but also accurately quantifies the pose uncertainty of some corner calculations by adjusting the covariance matrix through prior weight coefficients, making the error representation of pose observation more consistent with the actual scenario; Second, through the refined processing of the entire process of adaptive threshold segmentation, geometric constraint screening, standardized perspective correction, and decoding matching, A Priltag code detection and decoding significantly improve the accuracy of visual marker detection and the success rate of decoding, laying a high-precision raw data foundation for subsequent pose calculation. Furthermore, based on three core observation parameters—pixel area, detection confidence, and camera-mark distance—the system dynamically calculates and adjusts the basic pose variance value in a hierarchical manner, ultimately constructing a diagonal covariance matrix that closely matches the actual observation error. This achieves personalized and precise allocation of the uncertainty covariance matrix, making the quantification of pose error more targeted. The high-precision pose calculation results, continuous observation data, and accurate uncertainty characterization provide a reliable visual observation basis for subsequent pose optimization by fusing odometry data. This effectively improves the accuracy and robustness of the overall positioning of agricultural robots and intelligent agricultural machinery in smart agriculture scenarios, while making the positioning error controllable and quantifiable, adapting to the high-precision requirements of autonomous navigation and operational positioning in complex agricultural environments such as greenhouses.

[0166] It needs to be further explained that, such as Figure 2As shown, this embodiment calculates the trace of the uncertainty covariance matrix corresponding to each measurement pose, including:

[0167] S301. Obtain the measured pose of each visual marker and its corresponding uncertainty covariance matrix in the image frames of the current time and the previous N consecutive time steps;

[0168] S302. For each of the measured poses, calculate the trace of the uncertainty covariance matrix corresponding to the measured pose. The trace is the sum of the diagonal elements of the covariance matrix and is used to characterize the total uncertainty of the measured pose in the position component and the attitude angle component.

[0169] S303. Compare each trace with a preset accuracy threshold;

[0170] S304. If the trace is less than or equal to the preset accuracy threshold, determine that the measured pose meets the accuracy requirements and include it in the effective observation set.

[0171] S305. If the trace is greater than the preset accuracy threshold, it is determined that the measured pose does not meet the accuracy requirements and is discarded.

[0172] S306. If at the same time there are N visually marked measurement poses included in the effective observation set, then compare the size of the traces corresponding to each measurement pose, select the measurement pose with the smallest trace as the representative observation at that time, and record the representative observation and its corresponding uncertainty covariance matrix.

[0173] S307. If only one visually marked measurement pose is included in the effective observation set at the same time, then the measurement pose is directly used as the representative observation at that time, and the representative observation and its corresponding uncertainty covariance matrix are recorded.

[0174] S308. Output the representative observations and their uncertainty covariance matrix corresponding to each representative observation time.

[0175] For example, in a smart greenhouse inspection scenario, the system presets a time window N=5, with the current time t_k=10.0s, and acquires visual marker observations from t=6.0s to 10.0s. For instance, at t=8.0s, two Apriltag codes, ID=3 and ID=7, are detected, with covariance matrices R3=diag(0.0032, 0.0032, 3.2e-5) and R7=diag(0.008, 0.008, 8e-5), respectively. The calculated traces are trace3=0.006432 and trace7=0.01608. With a preset accuracy threshold T_trace=0.015, ID=3 meets the requirement and is included in the valid observation set, while ID=7 is discarded. Since there is only one valid observation at t=8.0s, it is directly used as the representative observation for that time. Similar filtering is applied to other times, and the final output list of representative observations is used for optimization. If no valid observations are found for three consecutive time points, the system automatically expands the window to N=8 or increases the threshold to 0.025 for re-filtering to ensure positioning continuity. Further, the preset accuracy threshold T_trace in this embodiment is set to 0.015. This is based on the upper limit of the joint uncertainty of the position and attitude angle components, determined experimentally by analyzing the trace distribution of the covariance matrix under typical observation scenarios and combining it with the system's positioning accuracy requirements. This value reflects the system's tolerance boundary for the total uncertainty of the visual marker measurement pose; that is, when the sum of the position variance and the angle variance is below this threshold, the measurement results are considered valid for subsequent optimization. Specifically, the trace values ​​of ideal observations at different distances are first calculated based on the camera imaging model and the physical size of the markers. Then, the trace distribution of low-quality observations at long distances is statistically analyzed through extensive field tests. Finally, a critical value is selected that can both retain high-quality observations and effectively eliminate low-quality observations, ensuring that the representative observations participating in the optimization have sufficient accuracy.

[0176] This process calculates the trace of the uncertainty covariance matrix corresponding to the measured pose and uses this trace to screen effective observations and select representative observations, resulting in significant technical effects. The derivation logic for each effect is clear: First, the trace obtained by summing the diagonal elements of the covariance matrix is ​​used as a quantitative indicator of the total pose uncertainty, achieving a comprehensive error assessment of the position and attitude angle components. Compared to single-dimensional error judgment, this more comprehensively and accurately reflects the overall accuracy level of the measured pose, providing a scientific and unified basis for screening the effectiveness of observations. Second, by comparing the trace with a preset accuracy threshold, effective observations are screened, directly eliminating measured poses with high total uncertainty and substandard accuracy. This filters low-quality observation data at the source, preventing pose data with large errors from participating in subsequent optimization calculations and affecting overall positioning accuracy. Furthermore, for observations existing at the same time... In the case of multiple valid observations, the measurement pose with the smallest trace, i.e., the lowest total uncertainty, is selected as the representative observation. This ensures that the pose data participating in the optimization at each moment is the current best, further improving the overall quality of the observation data. At the same time, the observation data screening method based on time windows, as well as the window expansion and threshold adjustment strategies when there are no valid observations, not only ensures the spatiotemporal continuity of the valid observation data, but also avoids the positioning interruption problem caused by poor observation quality at local moments, ensuring the stability of the positioning process in the complex environment of smart agriculture. The high-quality, continuous and optimal representative observation data after screening provides a reliable visual observation foundation for the subsequent pose weighted optimization of odometer data fusion, effectively reducing the optimization error caused by low-precision observations, and ultimately improving the accuracy and reliability of the positioning results of agricultural robots and smart agricultural machinery in scenarios such as greenhouses.

[0177] It should be further explained that this embodiment calculates the change in camera coordinate system pose between adjacent representative observation times, including:

[0178] S501. Obtain the odometry data frame sequence, which contains the robot's linear velocity and angular velocity measurements at each moment. Existing steps directly integrate the odometry data, but water stains / mud on the greenhouse floor may cause odometry data jumps (outliers), introducing large pose errors after integration. Therefore, outlier detection and removal logic needs to be added. For this reason, this embodiment... i To t i+1 Outlier detection is performed on odometer data frames within a given time period, including:

[0179] S501-1. For the odometer data frames within the time period, calculate the sliding window statistics of linear velocity v and angular velocity ω respectively, wherein the sliding window size is 5 frames, and obtain the mean μ_v and standard deviation σ_v of linear velocity v, and the mean μ_ω and standard deviation σ_ω of angular velocity ω.

[0180] S501-2. Traverse each frame of odometer data within the time period, and determine the odometer data frames that satisfy |v_i-μ_v|>3σ_v or |ω_i-μ_ω|>3σ_ω as outlier frames; where v_i represents the linear velocity measurement value of the i-th frame of odometer data, and ω_i represents the angular velocity measurement value of the i-th frame of odometer data.

[0181] S501-3. The abnormal value frames are completed by using linear interpolation. Specifically, a linear trend is fitted with the effective odometer data of the three frames before and after the abnormal value frame, and the linear velocity and angular velocity completion values ​​of the abnormal value frame are calculated based on the linear trend.

[0182] S501-4. Count the number of consecutive outlier frames within the time period. If the number of consecutive outlier frames exceeds a preset threshold, pause the integration operation on the odometer data and switch to pure visual positioning mode. The specific implementation logic is as follows: When the system detects that the number of consecutive outlier frames exceeds the preset threshold, immediately pause the integration operation on the odometer data and stop constructing the second constraint term based on the odometer, instead relying solely on visual marker observations for pose estimation. In this mode, the system continuously performs visual marker detection and decoding on image frames from the current moment and the previous T consecutive moments. For each valid observation moment, the representative measurement pose selected at that moment is directly used as the optimized camera pose output for that moment. For example, in a smart greenhouse inspection scenario, when the robot runs on wet ground, causing three consecutive frames of wheel speed measurement data to deviate from the sliding window mean by more than three times the standard deviation, the system determines that the odometer has entered a persistent fault state and immediately switches to pure visual positioning mode, relying solely on Apriltag code observations for positioning. If there are no effective visual markers observed for several consecutive moments, the system retains the most recent effective positioning result and outputs a positioning hold signal. At the same time, it continuously monitors the quality of the odometry data. Once the odometry data returns to normal, it automatically switches back to the fusion positioning mode, thereby ensuring the continuity and reliability of the robot's positioning during odometry failure.

[0183] Furthermore, the preset threshold value in this embodiment is determined based on the continuous characteristics of the odometer data sampling frequency and the robot's motion state, by balancing anomaly detection sensitivity and system robustness. This threshold is typically set according to the sample size of the sliding window statistical analysis. For example, when the sliding window size is five frames, the threshold can be set to three frames, meaning that if three consecutive frames of data are judged as abnormal, the odometer is considered to have entered a persistent fault state. Setting it too small can lead to occasional noise false triggering of mode switching, affecting positioning continuity; setting it too large will prevent timely response to real odometer failure scenarios, potentially introducing significant errors into pose integration. Therefore, this value is determined through analysis of the statistical characteristics of odometer noise and the distribution of persistent anomaly duration in the actual operating environment, and after experimental calibration, a critical value that strikes a balance between reliably identifying real faults and avoiding false triggering.

[0184] S501-5. Calculate the data integrity coefficient β for the odometer data completed by the above steps, including the following steps:

[0185] S501-6. The formula for calculating the data integrity coefficient β is β = number of valid frames / total number of frames, where the number of valid frames is the number of odometer data frames that were not identified as outliers within the time period, and the total number of frames is the total number of odometer data frames within the time period. The lower the value of β, the higher the proportion of complete data in the odometer data.

[0186] S501-7. Multiply each diagonal element of the motion uncertainty covariance matrix corresponding to the pose change between adjacent representative observation times by 1 / β, thereby increasing the motion uncertainty corresponding to the odometry data containing the completed data.

[0187] S502. Determine the time interval between adjacent representative observation times, wherein the adjacent representative observation times include the first representative observation time t. i The second represents the observation time t i+1 , where t i <t i+1 ;

[0188] S503, based on t in the odometer data frame sequence i To t i+1 For all odometer data frames within the time period, the linear velocity and angular velocity measurements are integrated to calculate the value from t. i Time to t i+1 The change in camera coordinate system pose at time t, wherein the change in pose includes a relative displacement vector and a relative rotation angle;

[0189] Obtain preset odometer model noise parameters, including linear velocity measurement noise variance, angular velocity measurement noise variance, and zero bias stability parameters;

[0190] S504. Based on the time interval between adjacent representative observation times, the number of odometer data frames used in the integration operation, and the noise parameters, calculate the motion uncertainty covariance matrix of the pose change. The motion uncertainty covariance matrix is ​​a diagonal matrix, and its diagonal elements include a first diagonal element, a second diagonal element, and a third diagonal element. The first diagonal element corresponds to the variance of the relative displacement in the x-direction, the second diagonal element corresponds to the variance of the relative displacement in the y-direction, and the third diagonal element corresponds to the variance of the relative rotation angle.

[0191] S505. Obtain the wheel speed meter data and inertial measurement unit data of the robot within the time interval. Calculate the slip coefficient based on the consistency deviation between the wheel speed meter data and the inertial measurement unit data. The slip coefficient ranges from 0 to 1, where 0 represents no slip and 1 represents complete slip. The specific implementation steps for calculating the slip coefficient include:

[0192] At adjacent representative observation times t i To t i+1 Within a given time interval, the system acquires all wheel speed meter data frames and inertial measurement unit (IMU) data frames for that period. Wheel speed meter data includes the rotational speed measurements of the left and right wheels. The wheel speed meter displacement vector d_odo is obtained by integrating the wheel track and wheel radius parameters, including the displacement component in the direction of travel and the change in steering angle. IMU data includes measurements from the three-axis accelerometer and three-axis gyroscope. The system performs gravity compensation and double integration on the accelerometer data to obtain the inertial displacement vector d_imu, and simultaneously integrates the gyroscope data to obtain the inertial attitude change.

[0193] Secondly, the consistency deviation between the two is calculated. The system extracts the projection component d_odo_forward of the wheel velocimeter displacement vector in the robot's forward direction, and the projection component d_imu_forward of the inertial displacement vector in the same direction, and calculates the displacement ratio r = d_odo_forward / d_imu_forward. Simultaneously, the difference Δθ_diff between the heading angle change Δθ_odo calculated by the wheel velocimeter and the heading angle change Δθ_imu integrated by the inertial measurement unit is calculated as a directional consistency index.

[0194] Then, the slip coefficient s is calculated based on the displacement ratio r and the heading angle deviation. When the displacement ratio r is close to 1 and the heading angle deviation Δθ_diff is less than a preset threshold, the slip coefficient s approaches 0, indicating no slip. When the displacement ratio r is significantly greater than 1 and the heading angle deviation increases, the slip coefficient s approaches 1, indicating complete slip. The specific calculation formula is s = α × max(0, min(1, r-1)) + β × min(1, |Δθ_diff| / θ_thresh), where α and β are weighting coefficients, and θ_thresh is the heading deviation threshold, which is preset mainly based on the zero-bias stability of the inertial measurement unit gyroscope and the inherent error of the heading estimation by the wheel speedometer. Typically, this threshold is set as the upper limit of the statistical distribution of the difference in heading angle between the two components under no-slip conditions. For example, 0.05 radians corresponds to approximately 2.86 degrees. This value is obtained by fitting a normal distribution to the heading deviation data collected from the robot during uniform linear motion on a smooth road surface, and selecting the mean plus three times the standard deviation. This ensures that under normal driving conditions, the slip coefficient is mainly affected by the displacement ratio and not excessively dominated by the heading deviation term. The preset weighting coefficients α and β reflect the system's relative emphasis on displacement consistency and directional consistency. α is usually set to a larger value, such as 0.8, reflecting that displacement slippage is the main characteristic of slippage, and the false increase in displacement caused by wheel-ground slippage directly leads to positioning errors. β is set to a smaller value, such as 0.2, reflecting that the heading deviation serves as an auxiliary criterion, only correcting when the displacement ratio approaches a critical value. The sum of the two coefficients is the total system weight, and its ratio is obtained through fitting a large amount of slippage experimental data, ensuring that the slip coefficient output accurately reflects the actual degree of slippage. Furthermore, the preset zero and one thresholds for the displacement ratio in this embodiment are determined based on physical meaning: when the wheel speedometer displacement is less than or equal to the inertial displacement, the ratio is less than or equal to one. Taking max(0, r-1) ensures that the slip coefficient only responds to situations where the wheel speedometer displacement exceeds the inertial displacement; min(1, r-1) limits the ratio contribution to the range between zero and one, corresponding to a slip coefficient range of zero to α. The remaining β range is filled by the heading deviation term, ensuring that the overall slip coefficient is normalized between zero and one. All thresholds have been verified through real-world scenarios to ensure that the slip coefficient can accurately quantify the degree of robot wheel-to-ground slippage, providing a reliable basis for the dynamic adjustment of the motion uncertainty covariance matrix.

[0195] For example, in a smart greenhouse inspection scenario, the robot starts from time t i =8.0 seconds to run to t i+1=10.0 seconds, with a time interval of 2 seconds. The wheel speedometer integral yields a forward displacement of 0.52 meters, and the inertial measurement unit integral yields a forward displacement of 0.48 meters, with a displacement ratio r of 1.083. Simultaneously, the wheel speedometer calculates a heading angle change of 0.21 radians, the inertial measurement unit calculates a heading angle change of 0.19 radians, and the heading deviation is 0.02 radians, with a preset heading deviation threshold of 0.05 radians. Taking α=0.8 and β=0.2, the displacement contribution is 0.8×min(1, 0.083)=0.0664, the heading contribution is 0.2×min(1, 0.4)=0.08, and the slip coefficient s is calculated to be 0.1464. This value is between 0 and 1, indicating that the robot exhibits slight slippage. Based on this, the system accordingly increases the position variance of the motion uncertainty covariance matrix to reflect the positioning error introduced by the slippage.

[0196] S506. If the sliding coefficient is greater than a preset sliding threshold, then according to the ratio of the difference between the sliding coefficient and the preset sliding threshold, the first and second diagonal elements of the motion uncertainty covariance matrix are linearly increased, with the increase magnitude proportional to the difference ratio, to obtain updated first and second diagonal elements. Further, in this embodiment, the difference ratio is calculated as follows: the difference between the sliding coefficient and the preset sliding threshold is divided by the preset sliding threshold itself; that is, the difference ratio equals the sliding coefficient minus the preset sliding threshold divided by the preset sliding threshold. This ratio is used to quantify the relative degree to which the sliding coefficient exceeds the threshold. Subsequently, the system linearly increases the first and second diagonal elements of the motion uncertainty covariance matrix according to this difference ratio, with the increase magnitude proportional to the difference ratio; that is, the updated variance equals the original variance multiplied by one plus the difference ratio, thereby achieving an adaptive adjustment effect where the more severe the sliding, the more significant the increase in position uncertainty.

[0197] S507. Obtain the sequence of angular velocity measurements of the robot within the time interval, and calculate the variance of the angular velocity measurement sequence as a motion smoothness index.

[0198] S508. If the motion smoothness index is greater than the preset smoothness threshold, then according to the ratio of the motion smoothness index to the preset smoothness threshold, the third diagonal element of the motion uncertainty covariance matrix is ​​linearly increased, and the increase is proportional to the ratio, to obtain the updated third diagonal element.

[0199] S509. The updated first diagonal elements, second diagonal elements, and third diagonal elements constitute the final motion uncertainty covariance matrix of the pose change.

[0200] For example, in the smart greenhouse inspection scenario of this embodiment, the robot starts from t i =8.0s run to ti+1 =10.0s, time interval Δt=2.0s, the odometer outputs linear velocity and angular velocity data at a frequency of 100Hz, and obtains pose changes Δx=0.5m, Δy=0.1m, Δθ=0.2rad through pre-integration. Based on preset noise parameters (linear velocity measurement noise variance σ_v)... 2 =0.01m 2 / s 2 Angular velocity measurement noise variance σ_ω 2 =0.001rad 2 / s 2 The calculated covariance matrix of the basic motion uncertainty is diag(0.02, 0.02, 0.002). During this period, the slip coefficient s=0.3 (exceeding the slip threshold of 0.1) was detected by comparing the IMU and the wheel speed sensor, increasing the position variance to 0.0289; the angular velocity sequence variance ω_var=0.05rad. 2 / s 2 (Exceeding the smoothness threshold of 0.01), the angle variance is increased to 0.014. The final motion uncertainty covariance matrix Q=diag(0.0289, 0.0289, 0.014) is obtained and used for weighted constraints in subsequent optimization. Furthermore, the slip threshold setting in this embodiment is based on the analysis of the statistical distribution of the slip coefficient under normal robot driving conditions. By collecting slip coefficient data of the robot on typical ground surfaces such as hard surfaces without slippage, the mean and standard deviation are calculated. The mean plus three times the standard deviation is taken as the critical value to distinguish between normal driving conditions and abnormal slippage. This ensures that the slip coefficient is below this threshold in most normal driving conditions without triggering an increase in uncertainty, while also responding promptly to significant slippage. The smoothness threshold of 0.01 in this embodiment is set based on the measurement noise level of the inertial measurement unit gyroscope and the angular velocity fluctuation characteristics during smooth robot movement. By statistically analyzing the variance of the angular velocity measurement value sequence during uniform linear motion of the robot, the upper limit of the normal fluctuation range is selected as the threshold. This ensures that only severe angular velocity fluctuations caused by uneven ground, sharp turns, or vibrations trigger an increase in angular uncertainty, avoiding misinterpreting inherent sensor noise as motion unevenness. Both thresholds are calibrated through extensive field testing and statistical analysis to balance the sensitivity of abnormal state detection with the system stability under normal operating conditions.

[0201] This process improves odometer data quality and accurately quantifies motion uncertainty by adding outlier detection and completion, and multi-dimensional dynamic adjustment to the entire process of odometer data processing and pose change and motion uncertainty covariance matrix calculation. The beneficial effects are presented in a progressive logical progression: First, to address the odometer data jump problem caused by the complex greenhouse ground environment, a sliding window statistical method is used to detect outlier frames and complete them through linear interpolation. Simultaneously, when consecutive outliers exceed a threshold, a pure visual positioning mode is switched. This effectively eliminates jumpy outlier odometer data, avoiding significant pose errors introduced by outlier integration, and ensures the continuity of pose change calculation through data completion and positioning mode switching, preventing positioning interruptions due to odometer data failure. Second, the proportion of completed data is quantified by a data integrity coefficient, and the element values ​​of the motion uncertainty covariance matrix are increased accordingly, accurately representing the motion error brought by the completed data, making the quantification of motion uncertainty more consistent with actual data quality. Furthermore, based on… The slip coefficient is calculated based on the consistency deviation between wheel speed sensor and IMU data. For slip scenarios exceeding the threshold, the variance in the position direction is linearly increased. Combined with the motion smoothness index of the angular velocity sequence variance, the variance in the angular direction is linearly increased for non-smooth motion exceeding the threshold. This achieves dynamic adjustment of the motion uncertainty covariance matrix according to the robot's actual motion state (slippage, angular velocity fluctuation), making the quantification of motion error more targeted and accurate. The high-quality odometry data after anomaly processing, combined with the time interval, noise parameters, and motion state, dynamically calculates a precise and personalized motion uncertainty covariance matrix. This ensures the accuracy of the pose change calculation at adjacent representative observation times and provides a scientific basis for subsequent weighted optimization. It effectively balances the fusion weights of visual observation and odometry motion information, avoiding interference from low-quality odometry data or mismatched motion error weights on the optimization results. Ultimately, this improves the accuracy, robustness, and stability of fusion positioning of agricultural robots in complex operating environments such as greenhouses.

[0202] For example, the objective function in this embodiment specifically includes:

[0203] S601-1. Obtain the measurement pose and its uncertainty covariance matrix corresponding to each representative observation time. This uncertainty covariance matrix is ​​a diagonal matrix, and its diagonal elements represent the position variance in the x-direction, the position variance in the y-direction, and the attitude angle variance, respectively. Taking two representative observation times, t1 equals 8.0 seconds and t2 equals 10.0 seconds, as examples, the measurement poses are as follows: at the first time, the position is 0.5 meters in the x-direction, 0.1 meters in the y-direction, and the attitude angle is 0.2 radians; at the second time, the position is 1.0 meters in the x-direction, 0.2 meters in the y-direction, and the attitude angle is 0.4 radians. Its uncertainty covariance matrix is ​​determined by the aforementioned steps: the diagonal elements of the covariance matrix at the first time step are 0.0032 square meters in the x-direction, 0.0032 square meters in the y-direction, and 0.000032 square radians in the angle; the diagonal elements of the covariance matrix at the second time step are 0.0045 square meters in the x-direction, 0.0045 square meters in the y-direction, and 0.000045 square radians in the angle.

[0204] S601-2. Obtain the pose change and its motion uncertainty covariance matrix between adjacent representative observation times. This motion uncertainty covariance matrix is ​​a diagonal matrix, with its diagonal elements corresponding to the variances of displacement in the x-direction, displacement in the y-direction, and rotation angle between adjacent times, respectively. For the pose change between t1 and t2, the odometry integration yields: x-direction displacement of 0.5 meters, y-direction displacement of 0.1 meters, and rotation angle of 0.2 radians. The diagonal elements of its motion uncertainty covariance matrix are: x-direction displacement variance of 0.0289 square meters, y-direction displacement variance of 0.0289 square meters, and rotation angle variance of 0.014 square radians.

[0205] S601-3. Determine the weight matrix of the first constraint term based on the inverse matrix of the uncertainty covariance matrix of the measured pose. This weight matrix is ​​also a diagonal matrix, with its diagonal elements being the reciprocals of the corresponding variances. For the first time step, the diagonal elements of the weight matrix are: x-direction weight 312.5 per square meter (i.e., the reciprocal of 0.0032), y-direction weight 312.5 per square meter, and angle weight 31250 per square radian (i.e., the reciprocal of 0.000032). For the second time step, the diagonal elements of the weight matrix are: x-direction weight 222.22 per square meter, y-direction weight 222.22 per square meter, and angle weight 22222.22 per square radian.

[0206] S601-4. Determine the weight matrix of the second constraint term based on the inverse of the motion uncertainty covariance matrix. This weight matrix is ​​a diagonal matrix, and its diagonal elements are the reciprocals of the corresponding displacement variance and rotation angle variance, respectively. For the segment from t1 to t2, the diagonal elements of the weight matrix are: x-direction displacement weight 34.60 per square meter, y-direction displacement weight 34.60 per square meter, and rotation angle weight 71.43 per square radian.

[0207] S601-5. Constructing the First Constraint: Calculate the deviation vector between the camera pose to be optimized and the corresponding measurement pose at each representative observation time. This deviation vector includes the x-direction deviation, y-direction deviation, and angle deviation. Then, sum the squares of each deviation vector after weighting it with the corresponding weight matrix. That is, the first constraint is equal to the sum of the weighted squares of all times. Initially, the camera pose to be optimized is set to the measurement pose. Therefore, the deviation vector at the first time is 0 meters in the x-direction, 0 meters in the y-direction, and 0 radians in the angle. The deviation vector at the second time is also zero. The initial value of the first constraint is 0.

[0208] S601-6. Constructing the Second Constraint: Calculate the change in the pose of the camera to be optimized between adjacent observation times, i.e., the change obtained by subtracting the pose from the previous time step from the pose at the later time step, and then subtracting the pose change measured by the odometry, to obtain the deviation vector. This deviation vector includes the displacement deviation in the x-direction, the displacement deviation in the y-direction, and the angle deviation. Then, the deviation vector is weighted by the weight matrix of the corresponding second constraint and the sum of squares is calculated. That is, the second constraint is equal to the sum of the weighted sums of squares of all adjacent segments. Initially, according to the initial value of the pose of the camera to be optimized, the change from t1 to t2 is 0.5 meters in the x-direction, 0.1 meters in the y-direction, and 0.2 radians in the angle, which is completely consistent with the odometry change. Therefore, the deviation vector is zero, and the initial value of the second constraint is 0.

[0209] S601-7. Add the first constraint term to the second constraint term to obtain the weighted optimization objective function value for the current iteration. The initial objective function value is 0. In subsequent iterations, the pose of the camera to be optimized will be adjusted, causing the deviation vector to no longer be zero, and the objective function value will increase accordingly. For example, if the pose of the camera to be optimized at the first moment after a certain iteration becomes 0.48 meters in the x-direction, 0.11 meters in the y-direction, and 0.19 radians in the angle, then its deviation from the measured pose is -0.02 meters in the x-direction, +0.01 meters in the y-direction, and -0.01 radians in the angle. The sum of squares of the bias vector after being weighted by the weight matrix at the first time step is as follows: the contribution in the x-direction is -0.02 multiplied by 312.5 multiplied by -0.02, which equals 0.125; the contribution in the y-direction is 0.01 multiplied by 312.5 multiplied by 0.01, which equals 0.03125; and the contribution in the angle is -0.01 multiplied by 31250 multiplied by -0.01, which equals 3.125. The sum of these three terms is 3.28125. Simultaneously, the changes between adjacent time steps also introduce bias, correspondingly increasing the objective function value. The optimization algorithm minimizes this objective function to achieve an optimal balance between visual and odometry constraints on the camera pose, ultimately yielding a precise positioning result.

[0210] It should be further explained that the solution to the weighted optimization objective function in this embodiment includes:

[0211] S701. Obtain the initial values ​​of the camera pose to be optimized at each representative observation time, the measurement pose corresponding to the representative observation and its uncertainty covariance matrix, and the pose change and motion uncertainty covariance matrix between adjacent representative observation times.

[0212] S702. Based on the initial value, measured pose, pose change, and corresponding covariance matrix, construct the weighted optimization objective function for the current iteration. The weighted optimization objective function includes a first constraint term and a second constraint term. The first constraint term is the sum of squares of the deviations between the camera pose to be optimized and the corresponding measured pose at each representative observation time, weighted by the inverse matrix of the corresponding uncertainty covariance matrix. The second constraint term is the sum of squares of the deviations between the changes in the camera pose to be optimized and the changes in pose at adjacent representative observation times, weighted by the inverse matrix of the corresponding motion uncertainty covariance matrix.

[0213] S703. Linearize the weighted optimization objective function at the current iteration value, calculate the Jacobian matrix and Hessian matrix, and solve the incremental equation to obtain the update increment of the camera pose to be optimized.

[0214] S704. Update the camera pose to be optimized according to the update increment to obtain the updated camera pose to be optimized.

[0215] S705. Determine whether the modulus of the update increment is less than a preset convergence threshold;

[0216] S706. In response to the fact that the magnitude of the update increment is less than the preset convergence threshold, the optimization convergence is determined, and the current updated camera pose to be optimized is output as the optimization result.

[0217] S707. In response to the fact that the modulus of the update increment is greater than or equal to the preset convergence threshold, it is determined whether the current iteration number has reached the preset maximum iteration number.

[0218] S708. If the current iteration count has not reached the preset maximum iteration count, increase the iteration count and return to recalculate the weighted optimization objective function value.

[0219] S709. If the current iteration count reaches the preset maximum iteration count, the optimization is determined to be non-convergent. The camera pose to be optimized after the last update is output as the optimization result, or an optimization failure signal is output.

[0220] For example, in the smart greenhouse inspection scenario, this embodiment assumes that the current optimization window contains three representative observation times t1, t2, and t3, with measurement poses of pose_meas1=(0.1m, 0.2m, 0.5rad), pose_meas2=(0.5m, 0.3m, 0.7rad), and pose_meas3=(1.0m, 0.4m, 0.9rad), respectively, and the corresponding uncertainty covariance matrix R1=diag(0.003, 0.003, 3e-5). R2 = diag(0.004, 0.004, 4e-5), R3 = diag(0.005, 0.005, 5e-5); the odometry pose changes between adjacent time points are control1 = (0.4m, 0.1m, 0.2rad) and control2 = (0.5m, 0.1m, 0.2rad); the motion uncertainty covariance matrix is ​​Q1 = diag(0.02, 0.02, 0.014) and Q2 = diag(0.02, 0.02, 0.014). Using the camera poses pose1, pose2, and pose3 to be optimized as variables, and the initial values ​​set as the measured pose, a weighted least squares objective function is constructed. The Gauss-Newton method was used for iterative solution: after the first iteration, the incremental modulus was updated to 0.012, which is greater than the convergence threshold of 0.001; after the second iteration, the incremental modulus was 0.003; after the third iteration, the incremental modulus decreased to 0.0008, satisfying the convergence condition, and the optimized pose at time t3 was output as pose3=(0.98m, 0.41m, 0.89rad), which significantly improved the positioning accuracy compared to the original measured pose, effectively suppressing accumulated errors and long-distance observation noise. The convergence threshold was set based on a balance between the system's actual requirements for positioning accuracy and the convergence of the optimization iteration. By comprehensively considering the physical scale unit of the camera pose calculation, the sensor measurement noise level, and the positioning error tolerance required by the actual application scenario, a critical value was selected through experimental calibration that ensured sufficient accuracy of the optimization results while converging within a reasonable number of iterations.

[0221] This embodiment, after optimizing the camera pose at each representative observation time, constructs a continuous pose output module, including:

[0222] S711. Extract two adjacent representative observation times, and denot them as the previous representative observation time t_prev and the next representative observation time t_curr, respectively. At the same time, obtain the optimized camera pose pose_prev corresponding to t_prev and the optimized camera pose pose_curr corresponding to t_curr.

[0223] S712. Determine the moment \(t\) of the current pose to be output, where \(t\) satisfies \(t_{prev}<t<t_{curr}\), and calculate the time interpolation coefficient \(\gamma\). The calculation formula of \(\gamma\) is \(\gamma=(t - t_{prev}) / (t_{curr}-t_{prev})\).

[0224] S713. Perform linear interpolation operations on the position components \(x\) and \(y\) in the optimized camera pose to obtain the position components \(x(t)\) and \(y(t)\) corresponding to the moment \(t\), where \(x(t)=pose_{prev}.x+\gamma\) (pose_{curr}.x - pose_{prev}.x)\), and \(y(t)\) is calculated according to the interpolation rule of the said \(x(t)\); where, \(x(t)\) represents the position component in the \(x\) direction of the interpolation pose corresponding to the current moment \(t\) to be output; \(pose_{prev}.x\) represents the position component in the \(x\) direction of the optimized camera pose corresponding to the previous representative observation moment \(t_{prev}\); \(pose_{curr}.x\) represents the position component in the \(x\) direction of the optimized camera pose corresponding to the subsequent representative observation moment \(t_{curr}\); \(\gamma\) characterizes the relative time position of the current moment \(t\) between the two previous and subsequent representative observation moments. This formula realizes the smooth output of continuous poses at any moment between adjacent representative observation moments by linearly weighting the position components in the \(x\) direction of the two previous and subsequent optimized poses.

[0225] S714. Perform spherical linear interpolation (SLERP) operation on the attitude angle component \(\theta\) in the optimized camera pose to obtain the attitude angle component \(\theta(t)\) corresponding to the moment \(t\), \(\theta(t)=SLERP(pose_{prev}.\theta, pose_{curr}.\theta, \gamma)\), and avoid crossing the \(\pi\) boundary during the attitude angle interpolation process through spherical linear interpolation; \(SLERP()\) is the spherical linear interpolation function; where, in the spherical linear interpolation function \(SLERP(pose_{prev}.\theta, pose_{curr}.\theta, \gamma)\) of this embodiment, \(pose_{prev}.\theta\) represents the attitude angle component of the optimized camera pose corresponding to the previous representative observation moment \(t_{prev}\), and usually represents the orientation of the robot in the form of quaternion or Euler angle; \(pose_{curr}.\theta\) represents the attitude angle component of the optimized camera pose corresponding to the subsequent representative observation moment \(t_{curr}\); the \(SLERP\) function ensures that the attitude change smoothly transitions along the shortest arc path on the sphere during the interpolation process by performing spherical linear interpolation on the two previous and subsequent attitude angle components, thereby avoiding the mutation problem caused by the attitude angle crossing the \(\pi\) boundary that may occur when using linear interpolation, and ensuring the smoothness and physical rationality of the continuously output attitude.

[0226] S715. Calculate the total uncertainty σ(t) of the interpolated pose at time t, σ(t) = (1-γ)σ_prev + γσ_curr, where σ_prev is the trace of the uncertainty covariance matrix of the previous observation time t_prev corresponding to the optimized pose, and σ_curr is the trace of the uncertainty covariance matrix of the next observation time t_curr corresponding to the optimized pose.

[0227] S716. Continuously output the interpolated pose corresponding to each time t to be output and the total uncertainty σ(t) corresponding to the pose at a fixed frequency of 30Hz, until the optimized camera pose representing the next observation time is updated, and then repeat the above steps.

[0228] This embodiment achieves optimal fusion of visual and odometry data and continuous output of positioning results by constructing an optimization objective function based on the weighted inverse of the covariance matrix, adopting an iterative solution strategy, and adding a continuous pose output module. The beneficial effects are presented in a progressively deeper logical derivation: First, the inverse matrices of the uncertainty covariance matrix of the visual measurement pose and the uncertainty covariance matrix of the odometry motion are used as weights for the first and second constraint terms, respectively. The weights are inversely proportional to the data error, achieving differentiated weighting constraints by assigning high weights to high-precision observation data and low weights to low-precision data. This makes the optimization direction of the objective function more closely aligned with actual data quality, effectively balancing the absolute position constraints of visual observation and the motion continuity constraints of odometry, and avoiding excessive influence of single data errors on the positioning results. Second, a weighted least squares objective function is constructed using the measured pose as the initial value. Through iterative optimization by linearizing the objective function and calculating the Jacobian and Hessian matrices to solve the incremental equations, combined with a dual judgment rule of convergence threshold and maximum iteration count, the optimization results are guaranteed to converge to the global optimum, effectively suppressing... This approach mitigates the long-range noise of visual observation and the cumulative error of odometry, while also providing effective results even when optimization fails to converge, ensuring the reliability and stability of pose optimization. Furthermore, after obtaining the pose at discrete representative observation times, continuous output of positioning results is achieved through linear interpolation of the position component and spherical linear interpolation of the attitude angle component, combined with a 30Hz fixed-frequency output rule. Simultaneously, the total uncertainty of the interpolated pose is obtained by weighted fusion of the trace of the covariance matrix of adjacent representative times, ensuring that the continuously output pose possesses both accuracy and quantifiable error, avoiding the problem of angle interpolation crossing the π boundary, and improving the accuracy of continuous pose. The high-precision optimized pose and continuously quantifiable positioning results obtained through the above process not only meet the high requirements for absolute positioning accuracy for autonomous navigation of agricultural robots in complex scenarios such as greenhouses, but also achieve continuous output of positioning results, adapting to the needs of real-time robot operation. Ultimately, this significantly improves the accuracy, continuity, and reliability of positioning for agricultural robots and intelligent agricultural machinery in smart agriculture scenarios, ensuring that the positioning results are both accurate and meet the real-time motion control requirements of the equipment.

[0229] Example 2

[0230] Please see Figure 3 Another embodiment of the present invention provides: an automatic visual tag selection system for mobile robots, comprising:

[0231] The acquisition module is used to acquire continuous sequences of monocular image frames and odometry data frames.

[0232] The tag perception module performs visual tag detection and decoding on the image frame at each time step to obtain the measured pose of each visual tag in the image at that time step, and assigns an uncertainty covariance matrix to each measured pose based on at least one of the following factors: the pixel area of ​​the visual tag in the image, the detection confidence, or the distance between the visual tag and the camera.

[0233] The effective observation filtering module calculates the trace of the uncertainty covariance matrix corresponding to each measurement pose for the image frames of the current time and the previous T consecutive time points, compares the trace with a preset accuracy threshold, and filters out the measurement poses whose traces are less than or equal to the preset accuracy threshold to form an effective observation set.

[0234] The selection module selects the measurement pose with the smallest trace of the uncertainty covariance matrix as the representative observation at that moment when there are M visual markers whose measurement poses belong to the effective observation set at the same time, and records the representative observation and its corresponding uncertainty covariance matrix.

[0235] The motion estimation module calculates the change in camera coordinate system pose between adjacent representative observation times based on the odometry data frame sequence, and assigns a motion uncertainty covariance matrix to each pose change.

[0236] The optimization module uses the camera pose to be optimized at each representative observation time as the optimization variable to construct a weighted optimization objective function. The objective function includes a first constraint term and a second constraint term. The first constraint term is the deviation between the camera pose to be optimized at each representative observation time and the measurement pose of the corresponding representative observation, and its weight is determined by the inverse matrix of the uncertainty covariance matrix corresponding to the representative observation. The second constraint term is the deviation between the change in camera pose to be optimized between adjacent representative observation times and the change in pose, and its weight is determined by the inverse matrix of the motion uncertainty covariance matrix.

[0237] The solution module solves the weighted optimization objective function to obtain the optimized camera pose for each observation time, extracts the optimized camera pose corresponding to the current time, and outputs it as the robot's current localization result.

[0238] The embodiments of the present invention have been described above with reference to the accompanying drawings. However, the present invention is not limited to the specific embodiments described above. The specific embodiments described above are merely illustrative and not restrictive. Those skilled in the art can make changes, modifications, substitutions and variations to the above embodiments under the guidance of the present invention without departing from the spirit and scope of the present invention. All of these variations are within the protection scope of the present invention.

Claims

1. A method for automatic selection of visual markers for mobile robots, characterized in that, include: Acquire a sequence of monocular image frames and a sequence of odometry data frames at consecutive time points; Visual marker detection and decoding are performed on the image frame at each time step to obtain the measured pose of each visual marker in the image at that time step. An uncertainty covariance matrix is ​​assigned to each measured pose based on at least one of the following factors: the pixel area of ​​the visual marker in the image, the detection confidence, or the distance between the visual marker and the camera. For the image frames at the current time and the previous T consecutive time points, calculate the trace of the uncertainty covariance matrix corresponding to each measurement pose, compare the trace with a preset accuracy threshold, and filter out the measurement poses whose traces are less than or equal to the preset accuracy threshold to form an effective observation set. When there are M visually marked measurement poses belonging to the effective observation set at the same time, the measurement pose with the smallest trace of the uncertainty covariance matrix is ​​selected as the representative observation at that time, and the representative observation and its corresponding uncertainty covariance matrix are recorded. Based on the odometry data frame sequence, calculate the change in camera coordinate system pose between adjacent representative observation times, and assign a motion uncertainty covariance matrix to each pose change. Using the camera pose to be optimized at each representative observation time as the optimization variable, a weighted optimization objective function is constructed; The weighted optimization objective function is solved to obtain the optimized camera pose for each observation time. The optimized camera pose corresponding to the current time is extracted and output as the robot's current localization result.

2. The method for automatic selection of visual markers for mobile robots as described in claim 1, characterized in that, The objective function includes a first constraint term and a second constraint term. The first constraint term is the deviation between the camera pose to be optimized at each representative observation time and the measurement pose of the corresponding representative observation, and its weight is determined by the inverse matrix of the uncertainty covariance matrix corresponding to the representative observation. The second constraint term is the deviation between the change in the camera pose to be optimized at adjacent representative observation times and the change in pose, and its weight is determined by the inverse matrix of the motion uncertainty covariance matrix.

3. The method for automatic selection of visual markers for mobile robots as described in claim 2, characterized in that, The process of obtaining the measured pose of each visual marker in the image at that moment includes: Obtain the current monocular image frame; Perform a visual marker detection algorithm on the image frame to generate a list of candidate marker regions; Iterate through each candidate marker region in the candidate marker region list and perform the following operations: Decode the candidate marker region and determine whether the decoding was successful. If decoding is successful, the identification information of the visual mark and the two-dimensional pixel coordinates of the corresponding four corner points in the image coordinate system are obtained; Based on the preset physical size of the visual marker, camera intrinsic parameters, and the two-dimensional pixel coordinates of the four corner points, the perspective n-point projection algorithm is used to calculate the rotation matrix and translation vector of the current camera coordinate system relative to the visual marker coordinate system, so as to obtain the measurement pose corresponding to the visual marker. The measurement pose includes position components and attitude angle components. The pixel area occupied by the visual mark in the image is calculated based on the two-dimensional pixel coordinates of the four corner points; The confidence score output by the decoding algorithm is used as the detection confidence score. The distance between the visual marker and the camera is calculated based on the magnitude of the translation vector; The measured pose is associated with the corresponding pixel area, detection confidence, and distance and stored as the observation data of the visual label; If decoding fails, the candidate marker region is skipped and no observation data is generated; After the traversal is complete, the set of observation data of all visual markers at the current time is output as the measurement pose and associated parameters of each visual marker in the image at that time.

4. The method for automatic selection of visual markers for mobile robots as described in claim 3, characterized in that, Assigning an uncertainty covariance matrix to each measured pose includes: Obtain the pixel area of ​​the visual marker in the image; If the pixel area is less than a preset area threshold, then the position variance and attitude angle variance corresponding to the measured pose are both set to the first preset variance value. If the pixel area is greater than or equal to the preset area threshold, then the base values ​​of position variance and attitude angle variance are calculated according to the ratio of the pixel area to the preset reference area, in an inverse relationship, that is, the base values ​​are inversely proportional to the ratio. Obtain the detection confidence score of the visual marker; If the detection confidence is less than a preset confidence threshold, the base value is increased according to the confidence deficit ratio, where the confidence deficit ratio is the difference between the preset confidence threshold and the detection confidence divided by the preset confidence threshold. Obtain the distance between the visual marker and the camera; If the distance is greater than a preset distance threshold, the base value is further increased according to the distance excess ratio, where the distance excess ratio is the difference between the distance and the preset distance threshold divided by the preset distance threshold. Using the final determined position variance and attitude angle variance as diagonal elements, a diagonal covariance matrix is ​​constructed, which serves as the uncertainty covariance matrix of the measured pose.

5. The method for automatic selection of visual markers for mobile robots as described in claim 4, characterized in that, The calculation of the trace of the uncertainty covariance matrix corresponding to each measurement pose includes: Obtain the measured pose of each visual marker and its corresponding uncertainty covariance matrix in the image frames of the current time and the previous N consecutive time steps; For each of the measured poses, the trace of the uncertainty covariance matrix corresponding to the measured pose is calculated. The trace is the sum of the diagonal elements of the covariance matrix and is used to characterize the total uncertainty of the measured pose in the position component and the attitude angle component. Each trace is compared with a preset accuracy threshold; If the trace is less than or equal to the preset accuracy threshold, the measured pose is determined to meet the accuracy requirements and is included in the valid observation set. If the trace is greater than the preset accuracy threshold, the measured pose is determined not to meet the accuracy requirements and is discarded. If at the same time there are N visually marked measurement poses included in the effective observation set, then the size of the traces corresponding to each measurement pose is compared, the measurement pose with the smallest trace is selected as the representative observation at that time, and the representative observation and its corresponding uncertainty covariance matrix are recorded. If only one visually marked measurement pose is included in the effective observation set at any given time, then the measurement pose is directly used as the representative observation at that time, and the representative observation and its corresponding uncertainty covariance matrix are recorded. Output the representative observations and their uncertainty covariance matrices for each representative observation time.

6. The method for automatic selection of visual markers for mobile robots as described in claim 5, characterized in that, The calculation of the camera coordinate system pose change between adjacent representative observation times includes: Acquire the odometry data frame sequence, which contains the robot's linear velocity and angular velocity measurements at each moment. Determine the time interval between adjacent representative observation times, wherein the adjacent representative observation times include the first representative observation time t. i The second represents the observation time t i+1 , where t i <t i+1 ; According to the odometer data frame sequence t i To t i+1 For all odometer data frames within the time period, the linear velocity and angular velocity measurements are integrated to calculate the value from t. i Time to t i+1 The change in camera coordinate system pose at time t, wherein the change in pose includes the relative displacement vector and the relative rotation angle.

7. The method for automatic selection of visual markers for mobile robots as described in claim 6, characterized in that, The calculation of the camera coordinate system pose change between adjacent representative observation times also includes: Obtain preset odometer model noise parameters, including linear velocity measurement noise variance, angular velocity measurement noise variance, and zero bias stability parameters; Based on the time interval between adjacent representative observation times, the number of odometer data frames used in the integration operation, and the noise parameters, the motion uncertainty covariance matrix of the pose change is calculated. The motion uncertainty covariance matrix is ​​a diagonal matrix, and its diagonal elements include a first diagonal element, a second diagonal element, and a third diagonal element. The first diagonal element corresponds to the variance of the relative displacement in the x-direction, the second diagonal element corresponds to the variance of the relative displacement in the y-direction, and the third diagonal element corresponds to the variance of the relative rotation angle. The robot acquires wheel speed meter data and inertial measurement unit data within the time interval. Based on the consistency deviation between the wheel speed meter data and the inertial measurement unit data, the slip coefficient is calculated. The slip coefficient ranges from 0 to 1, where 0 represents no slip and 1 represents complete slip.

8. The method for automatic selection of visual markers for mobile robots as described in claim 7, characterized in that, The calculation of the camera coordinate system pose change between adjacent representative observation times also includes: If the sliding coefficient is greater than the preset sliding threshold, then according to the ratio of the difference between the sliding coefficient and the preset sliding threshold, the first and second diagonal elements of the motion uncertainty covariance matrix are linearly increased, and the increase is proportional to the ratio of the difference, to obtain the updated first and second diagonal elements. Obtain the sequence of angular velocity measurements of the robot within the time interval, and calculate the variance of the angular velocity measurement sequence as an index of motion smoothness; If the motion smoothness index is greater than the preset smoothness threshold, then the third diagonal element of the motion uncertainty covariance matrix is ​​linearly increased according to the ratio of the motion smoothness index to the preset smoothness threshold, and the increase is proportional to the ratio, to obtain the updated third diagonal element. The updated first diagonal elements, second diagonal elements, and third diagonal elements constitute the final motion uncertainty covariance matrix of the pose change.

9. The method for automatic selection of visual markers for mobile robots as described in claim 8, characterized in that, Solving the weighted optimization objective function includes: The initial values ​​of the camera pose to be optimized at each representative observation time, the measurement pose corresponding to the representative observation and its uncertainty covariance matrix, and the pose change and motion uncertainty covariance matrix between adjacent representative observation times are obtained. Based on the initial value, measured pose, pose change, and corresponding covariance matrix, a weighted optimization objective function for the current iteration is constructed. The weighted optimization objective function includes a first constraint term and a second constraint term. The first constraint term is the sum of squares of the deviations between the camera pose to be optimized and the corresponding measured pose at each representative observation time, weighted by the inverse of the corresponding uncertainty covariance matrix. The second constraint term is the sum of squares of the deviations between the changes in the camera pose to be optimized and the changes in pose at adjacent representative observation times, weighted by the inverse of the corresponding motion uncertainty covariance matrix. The weighted optimization objective function is linearized at the current iteration value, the Jacobian matrix and Hessian matrix are calculated, and the incremental equation is solved to obtain the update increment of the camera pose to be optimized. The camera pose to be optimized is updated according to the update increment to obtain the updated camera pose to be optimized; Determine whether the modulus of the update increment is less than a preset convergence threshold; If the magnitude of the update increment is less than the preset convergence threshold, the optimization convergence is determined, and the current updated camera pose to be optimized is output as the optimization result. If the magnitude of the update increment is greater than or equal to the preset convergence threshold, then it is determined whether the current iteration number has reached the preset maximum iteration number. If the current iteration count has not reached the preset maximum iteration count, the iteration count is increased, and the weighted optimization objective function value is recalculated. If the current iteration count reaches the preset maximum iteration count, the optimization is determined to be non-convergent, and the camera pose to be optimized after the last update is output as the optimization result, or an optimization failure signal is output.

10. An automatic visual marker selection system for a mobile robot, used to implement the automatic visual marker selection method for a mobile robot according to any one of claims 1-9, characterized in that, include: The acquisition module is used to acquire continuous sequences of monocular image frames and odometry data frames. The tag perception module performs visual tag detection and decoding on the image frame at each time step to obtain the measured pose of each visual tag in the image at that time step, and assigns an uncertainty covariance matrix to each measured pose based on at least one of the following factors: the pixel area of ​​the visual tag in the image, the detection confidence, or the distance between the visual tag and the camera. The effective observation filtering module calculates the trace of the uncertainty covariance matrix corresponding to each measurement pose for the image frames of the current time and the previous T consecutive time points, compares the trace with a preset accuracy threshold, and filters out the measurement poses whose traces are less than or equal to the preset accuracy threshold to form an effective observation set. The selection module selects the measurement pose with the smallest trace of the uncertainty covariance matrix as the representative observation at that moment when there are M visual markers whose measurement poses belong to the effective observation set at the same time, and records the representative observation and its corresponding uncertainty covariance matrix. The motion estimation module calculates the change in camera coordinate system pose between adjacent representative observation times based on the odometry data frame sequence, and assigns a motion uncertainty covariance matrix to each pose change. The optimization module uses the camera pose to be optimized at each representative observation time as the optimization variable to construct a weighted optimization objective function. The objective function includes a first constraint term and a second constraint term. The first constraint term is the deviation between the camera pose to be optimized at each representative observation time and the measurement pose of the corresponding representative observation, and its weight is determined by the inverse matrix of the uncertainty covariance matrix corresponding to the representative observation. The second constraint term is the deviation between the change in camera pose to be optimized between adjacent representative observation times and the change in pose, and its weight is determined by the inverse matrix of the motion uncertainty covariance matrix. The solution module solves the weighted optimization objective function to obtain the optimized camera pose for each observation time, extracts the optimized camera pose corresponding to the current time, and outputs it as the robot's current localization result.

Images