Robot orchard inter-row positioning method based on multi-modal semantic graph optimization

Abstract

The invention discloses a robot orchard inter-row positioning method based on multi-modal semantic graph optimization, and aims to solve the problems of inter-row leakage and inter-row jump in positioning in orchard moving operation. Time synchronization and external parameter calibration are carried out on multi-sensor data, semantic analysis and geometric preprocessing are carried out, and the positioning accuracy of the orchard inter-row positioning method is improved. The method comprises the following steps: generating tree trunk, pile body, tree wall and ground boundary observation, constructing a semantic factor graph containing row identifiers and continuous variables, adopting a graph converter to perform adaptive weighting and covariance calibration, combining soft and hard two-stage increment optimization of anti-fact mutual exclusion gating and discrete continuous combination, outputting the row identifiers, robot trajectories, transverse deviation and course angles, and constructing a semantic factor graph containing the row identifiers and the continuous variables. The technical effects of stably inhibiting the inter-row jump and improving the row identification judgment accuracy and the transverse deviation and course angle estimation precision are achieved.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of agricultural robot positioning and navigation, and particularly relates to a robot orchard inter-row positioning method based on multi-modal semantic graph optimization. BACKGROUND

[0002] With the increasing application of agricultural robots in orchards, inter-row positioning has become a key link to realize autonomous navigation and precision operation. The existing technology mostly adopts a multi-sensor fusion scheme to obtain environment and motion information by using cameras, laser radars, inertial measurement units and global satellite navigation systems; at the algorithm level, common practices include visual odometry, laser radar odometry and factor graph optimization, and some schemes introduce semantic segmentation and target detection to extract semantic elements such as tree trunks, tree walls and ground boundaries, and then fit the row center line and perform incremental optimization in a sliding window. To suppress outliers and noise, robust kernel functions, Mahalanobis distance gating and threshold-based observation screening are commonly used. Overall, the existing methods can provide usable inter-row positioning capability in conventional environments.

[0003] However, the existing technology still has deficiencies in the scene of the orchard with highly repetitive structure, occlusion change and limited satellite signal, mainly manifested as:

[0004] 1. Cross-row leakage is prone to occur and difficult to suppress in time. Due to the high similarity in geometry and texture between rows, the observation data association is easily confused, and local gating based on residual size or distance is difficult to capture the competitive interpretation relationship across rows and the constraints at the sequence level, resulting in row identification jumping between adjacent rows;

[0005] 2. The factor weights and covariances are mostly dependent on fixed experience or simple heuristics, and it is difficult to adaptively adjust them according to the scene, view angle and sensor quality, and when the scales of heterogeneous factors are inconsistent, it is easy to cause the unexpected dominance of some factors on the optimization results, thereby amplifying the influence of erroneous observations;

[0006] 3. Discrete variables such as row identification and continuous variables such as pose and center line are often processed separately, and the row identification is often given by the nearest center line or threshold rule, lacking joint inference with continuous states, and the use of left and right boundary pair consistency and topological order constraints along the row direction is insufficient, which is prone to label flipping and time sequence inconsistency problems.

[0007] Therefore, a robot orchard inter-row positioning method that can solve the above deficiencies of the existing technology is needed to solve the problem for those skilled in the art. SUMMARY

[0008] One objective of this invention is to propose a robotic orchard row localization method based on multimodal semantic graph optimization. Addressing the problems of existing technologies, such as cross-row leakage in repeating row structures, difficulty in adaptive weighting, and separation of discrete and continuous variables, this invention proposes a semantic factor graph containing continuous variables such as row identifiers, poses, and row centerlines within a sliding window. A graph transformer is used for adaptive weighting and covariance calibration of the factors. Combined with pairwise consistency constraints on left and right boundaries and sequence topological consistency constraints, a counterfactual evaluation-based mutual exclusion gating is generated and implemented for rejection or weight reduction. A two-stage incremental optimization scheme combining discrete and continuous variables is employed. This invention effectively suppresses cross-row jumps, improves the reliability of row identifier determination, reduces lateral deviation and heading angle estimation accuracy, and adapts to different sensing conditions.

[0009] A robotic orchard row localization method based on multimodal semantic graph optimization according to an embodiment of the present invention includes:

[0010] S1. Acquire camera image frames, lidar point cloud frames, inertial measurement sequences, and global satellite navigation system observation sequences, and perform time synchronization and external parameter calibration;

[0011] S2. Based on the above data, perform semantic parsing and geometric preprocessing to generate semantic observations of tree trunks, stakes, tree walls and ground boundaries, and calculate their confidence and geometric attributes. Establish an initial semantic factor graph based on a sliding window.

[0012] S3. Using the initial semantic factor graph as input, run the graph transformer adaptive weighting model to output the weights and covariance parameters of each factor.

[0013] S4. Apply factor weights and covariance parameters to the initial semantic factor graph to form evaluation conditions, perform counterfactual evaluation for each candidate observation factor, calculate risk scores, and generate gated decisions.

[0014] S5. Apply the gating decision to the initial semantic factor graph and combine factor weights and covariance parameters. Use discrete and continuous joint incremental optimization. In the soft stage, obtain the probability distribution of row identifiers and the initial estimate of continuous variables. In the hard stage, solidify the row identifiers and re-optimize the continuous variables. Output the probability distribution of row identifiers, the initial estimate of continuous variables, the robot pose trajectory, the row centerline parameters and the final estimate of the row identifier sequence.

[0015] S6. Based on the robot's pose trajectory, the final estimate of the row centerline parameters and the row identifier sequence, calculate the directional lateral distance and heading angle from the current pose to the row centerline, label the row identifier at the current moment, and output the lateral deviation, heading angle and the row identifier at the current moment.

[0016] Optionally, step S1 specifically includes:

[0017] Acquire camera image frames, lidar point cloud frames, inertial measurement sequences, and global navigation satellite system observation sequences;

[0018] The collected data is synchronized in time. The time synchronization includes at least a unified clock reference, timestamp alignment and delay compensation, so that the camera image frames and the lidar point cloud frames are aligned in the same time reference, and the inertial measurement sequence and the global satellite navigation system observation sequence are aligned in the same time reference.

[0019] Perform extrinsic parameter calibration on each sensor. The extrinsic parameter parameters obtained by the calibration include the rotation and translation of each sensor relative to the robot base coordinate system, as well as the rotation and translation between each sensor.

[0020] Output time-aligned image frames, time-aligned point cloud frames, inertial measurement sequences, global navigation satellite system observation sequences, and sensor extrinsic parameters.

[0021] Terminology definition:

[0022] The camera image frame is a two-dimensional image data unit that is captured by the camera at a single moment and labeled with a timestamp;

[0023] The lidar point cloud frame is a three-dimensional point set data unit that is collected by lidar at a single moment and labeled with a timestamp;

[0024] The inertial measurement sequence is a sequence of data consisting of triaxial acceleration and angular velocity observations output by the inertial measurement unit, recorded in chronological order.

[0025] The global navigation satellite system observation sequence is a sequence of observation data from a global navigation satellite system receiver recorded in chronological order, including pseudorange, carrier phase, Doppler, and satellite information, etc.

[0026] The term "global satellite navigation system" is a collective term for satellite navigation systems that provide positioning and timing services, including GPS, BeiDou, GLONASS, Galileo, etc.

[0027] The robot base coordinate system is a reference coordinate system fixed on the robot body base, used to uniformly express the spatial relationship between each sensor and the robot's pose.

[0028] Optionally, step S2 specifically includes:

[0029] The input includes image frames, point cloud frames, inertial measurement sequences, global navigation satellite system observation sequences, and sensor extrinsic parameters, among which at least image frames, point cloud frames, and sensor extrinsic parameters are used.

[0030] Semantic parsing and geometric preprocessing are performed on image frames and point cloud frames to generate semantic observations of tree trunks, piles, tree walls, and ground boundaries. Confidence and geometric attributes are calculated for each semantic observation. The geometric attributes include at least three-dimensional position and local orientation.

[0031] An initial semantic factor graph is established based on a sliding window, wherein the sliding window includes multiple consecutive time points, and the initial semantic factor graph includes discrete variable row identifiers, continuous variable robot pose, continuous variable row centerline parameters, and continuous variable left and right boundary landmarks.

[0032] For the aforementioned semantic observations, measurement factors, row identifier consistency factors, row centerline directional lateral distance factors, left and right boundary pair consistency factors, and sequence topology consistency factors are established in the initial semantic factor graph, respectively. The sequence topology consistency factor is modeled based on the landmark order relationship along the row direction.

[0033] Terminology definition:

[0034] The semantic parsing involves performing semantic segmentation, target detection, and category recognition on image frames and point cloud frames to extract semantic elements such as tree trunks, stakes, tree walls, and ground boundaries.

[0035] The geometric preprocessing involves denoising, segmenting, and geometrically fitting the observed data, as well as unifying the coordinate reference, to obtain geometric quantities that can be used for mapping and optimization.

[0036] The semantic observations are observation instances obtained by semantic parsing and accompanied by confidence and geometric attributes;

[0037] The tree trunk semantic observation is an observation instance corresponding to the fruit tree trunk target, reflecting the position and orientation of the tree trunk in space;

[0038] The semantic observation of the pile body refers to the observation examples of column targets such as orchard support piles;

[0039] The tree wall semantic observation refers to the observation examples of the band-shaped or approximately planar structure formed by the canopy / branches and leaves that are continuously distributed along the row direction;

[0040] The ground boundary semantic observation refers to the observation examples of the boundary line or boundary zone that corresponds to the boundary between the inter-row passage and the vegetation area.

[0041] The confidence level is a probabilistic score that reflects the correctness and reliability of semantic observations;

[0042] The geometric attributes are a set of parameters characterizing the semantic observation space characteristics, including at least three-dimensional position and local orientation;

[0043] The three-dimensional position refers to the spatial coordinates of the semantic observation representative point or geometric center given under a unified coordinate reference.

[0044] The local orientation is a vector that characterizes the semantic observation principal axis or normal direction under a unified coordinate reference.

[0045] The sliding window is a subset of data that covers multiple consecutive moments and is updated over time, used to limit the time range for the current factor graph construction and optimization.

[0046] The initial semantic factor graph is a graph structure in which the constraint relationship is represented by variable nodes and factor edges within a sliding window, integrating semantic observations and geometric constraints for subsequent weighting and optimization;

[0047] The row identifier is a discrete variable indicating the row number or category of the fruit tree where the robot is located;

[0048] The robot pose is a continuous variable representing the robot's position and orientation under a unified coordinate reference.

[0049] The row centerline parameter is a parameterized description of the current fruit tree row centerline, used to constrain and calculate geometric quantities related to the row.

[0050] The left and right boundary landmarks are continuous landmark variables that represent the geometric position and direction of the left and right boundaries of the current row;

[0051] The measurement factor is a factor that correlates semantic observations with model predictions and forms residual constraints;

[0052] The row identifier consistency factor is a factor that constrains the row identifiers within the sliding window to maintain continuous consistency in the time dimension.

[0053] The directional lateral distance factor of the row centerline is a factor that constrains the signed lateral distance from the robot pose to the row centerline.

[0054] The left and right boundary pair consistency factor is a factor that constrains the pair relationship between the left and right boundaries in terms of normal symmetry, spacing stability, etc.

[0055] The sequence topological consistency factor is a factor that constrains landmark observations along the row direction to satisfy monotonic order and temporal consistency.

[0056] The order of landmarks along the row direction is a monotonically consistent relationship in which the order or index of similar landmarks is maintained with reference to the row direction.

[0057] Optionally, step S3 specifically includes:

[0058] Using the initial semantic factor graph as input, features for weighting are constructed for each node and factor in the initial semantic factor graph. The features include at least node type, edge type, observation confidence, local observability index, initial value of factor residual and adjacency context.

[0059] The features are encoded into vectors and input into the graph transformer adaptive weighting model, so that the graph transformer performs attention-based message passing on the topology of the initial semantic factor graph, generates factor weights and covariance parameters corresponding to each factor, and performs numerical calibration on the factor weights and covariance parameters to ensure that the scales of different factors are comparable.

[0060] Output calibrated factor weights and covariance parameters.

[0061] Terminology definition:

[0062] The features used for weighting are feature vectors that are constructed based on information such as node type, edge type, observation confidence, local observability index, initial value of factor residual and adjacency context in the factor graph and are used to calculate factor weights and covariance parameters.

[0063] The node type is the category identifier of the variable node in the factor graph, which is used to distinguish between discrete row identifiers and continuous variable types such as robot pose, row centerline parameters, and left and right boundary landmarks.

[0064] The edge type is the constraint category identifier to which the factor edge belongs in the factor graph, used to distinguish between measurement factors, row identifier consistency factors, row centerline directed lateral distance factors, left and right boundary pair consistency factors, and sequence topology consistency factors.

[0065] The local observability index is a quantitative indicator that measures the strength of the constraint of a certain observation or factor on relevant variables under the current sliding window and sensor apparent geometry conditions.

[0066] The initial residual value of the factor is the initial residual of the factor calculated by factor measurement and model prediction under the current linearization state, which is used to reflect the current degree of inconsistency of the factor;

[0067] The adjacency context is a set of local connections and statistical information formed by the target node or factor's adjacency nodes and factors and their attributes in the factor graph.

[0068] The graph transformer adaptive weighting model is a model based on graph neural networks and attention mechanisms that performs feature aggregation and updating on the factor graph topology and outputs the weights and covariance parameters of each factor.

[0069] The attention-based message passing is a graph-based information propagation process that assigns learnable attention weights to adjacent elements and performs feature weighted aggregation and updating accordingly.

[0070] The factor weight is a scalar parameter used to adjust the contribution of the factor residual to the optimization objective;

[0071] The covariance parameter is a parameter describing the measurement uncertainty of the factor, and is used to construct or adjust the weighting terms or information matrix of the factor;

[0072] The numerical calibration is a process of scaling and standardizing the weights and covariances of different factor categories to unify their dimensions and magnitudes.

[0073] The scale comparability refers to the property that the trade-off scales of different factors in the optimization objective are comparable after calibration, and that avoids the undue dominance of a single type of factor in the solution.

[0074] The factor graph topology is a graph structure consisting of variable nodes, factor edges and their connections, used to limit the adjacency relationships and paths of message passing and weighting operations.

[0075] Optionally, step S4 specifically includes:

[0076] The factor weights and covariance parameters are used as inputs and applied to the initial semantic factor graph to form evaluation conditions;

[0077] For each candidate observation factor in the initial semantic factor graph, the objective function is linearized based on the current solution, and the cost increments of the row identifier consistency term, the row center line directed lateral distance term, the left and right boundary pair consistency term, and the sequence topology consistency term are calculated respectively when the candidate observation factor is accepted.

[0078] The counterfactual risk score is obtained by summing the cost increments according to the set weights.

[0079] Gating decisions are generated based on counterfactual risk scores, wherein a rejection decision is generated when the counterfactual risk score is greater than the gate threshold, and a deweighting decision is generated when the counterfactual risk score is not greater than the gate threshold. The deweighting decision determines the weight reduction ratio or covariance expansion ratio of the candidate observation factor based on the counterfactual risk score.

[0080] Output gating decision set.

[0081] Terminology definition:

[0082] The evaluation criteria are an optimized configuration for cost calculation and decision-making, formed by applying calibrated factor weights and covariance parameters to the initial semantic factor graph.

[0083] The candidate observation factor is a single factor in the set of observation-related factors that need to be determined in the initial semantic factor graph whether to participate in the current optimization.

[0084] The current solution is the set of variable estimates obtained within the sliding window, including the current values ​​of row identifier, robot pose, row centerline parameters, and left and right boundary landmarks;

[0085] The linearization approximation of the objective function is a process of making a first-order approximation of the weighted objective at the current solution to quickly assess the impact of accepting candidate factors;

[0086] The cost increment is the weighted cost of each constraint term on the objective function after accepting a candidate observation factor;

[0087] The row identifier consistency term is a cost term that measures the continuous consistency of row identifiers over time after the factor is adopted;

[0088] The directional lateral distance term of the row centerline is a cost term that measures the consistency of the signed lateral distance from the pose to the row centerline after accepting the factor.

[0089] The left and right boundary pair consistency term is a cost term that measures the consistency of the left and right boundaries in the pair relationship after accepting the factor.

[0090] The sequence topology consistency term is a cost term that measures the consistency between the landmark order and the temporal sequence along the row direction after the factor is adopted;

[0091] The set weighting coefficient is a preset or adjustable weighting parameter used to summarize each cost increment and form a comprehensive evaluation;

[0092] The counterfactual risk score is a score used to evaluate the degree of risk of accepting a factor, obtained by summing each cost increment and a set weight coefficient without actually changing the factor graph structure.

[0093] The gating decision is a discrete decision based on the counterfactual risk score and the gating threshold to reject or reduce the weight of candidate factors;

[0094] The gating threshold is the dividing line between rejection and deweighting decisions;

[0095] The rejection decision is to prohibit the candidate observation factor from participating in the current optimization and subsequent incremental update processes;

[0096] The weight reduction decision is to adjust the weight and covariance of the candidate observation factor according to its risk score while retaining the candidate observation factor.

[0097] The weight reduction ratio is a parameter representing the magnitude of weight reduction for the candidate observation factor under the weight reduction decision.

[0098] The covariance amplification ratio is a measure of the amplification of the covariance of the candidate observation factor under weighted decision-making.

[0099] The gating decision set is a collection of gating decisions formed for all candidate observation factors, which is used for subsequent joint optimization.

[0100] Optionally, step S5 specifically includes:

[0101] Using the gating decision set as input, the gating decision is applied to the initial semantic factor graph and combined with factor weights and covariance parameters. The rejected observation factors are closed, and the weights and covariance parameters of the downweighted observation factors are adjusted.

[0102] Discrete and continuous joint incremental optimization is performed within a sliding window. Specifically, the soft-stage joint optimization constructs an objective function composed of the weighted sum of squares of the residuals of each factor, using the gating and weighted factor graph as conditions. The probability distribution of the row identifier is obtained by probabilistic inference, and the objective function is expected based on the probability distribution of the row identifier to obtain the initial estimate of the continuous variable. The probability distribution of the row identifier and the initial estimate of the continuous variable are output.

[0103] The hard-stage joint optimization takes the output of the soft stage as input, solidifies the row identifiers into definite values ​​according to the maximum a posteriori criterion, re-linearizes and minimizes the weighted sum of squares objective function under the same conditions, and obtains the final estimates of the robot pose trajectory, row centerline parameters and row identifier sequence.

[0104] Output the final estimate of the robot's pose trajectory, row centerline parameters, and row identifier sequence.

[0105] Terminology definition:

[0106] The discrete and continuous joint incremental optimization is an incremental and coupled solution process for discrete variables (row identifiers) and continuous variables (robot pose, row centerline parameters, etc.) within the same sliding window to achieve synchronous updates and constraint consistency.

[0107] The soft stage is the first stage of joint incremental optimization. It performs probabilistic inference and constructs a weighted sum of squares objective while preserving the uncertainty of row identifiers, thereby obtaining an initial estimate of the continuous variable.

[0108] The weighted sum of squares objective function is the sum of squares of the residuals of the factors involved in the optimization, weighted according to their respective weights and covariances. It is used to evaluate the current solution and serve as the optimization objective.

[0109] The factor residual is the difference between the observation and the model prediction corresponding to the factor, which is used to form constraints and participate in the calculation of the objective function.

[0110] The probability inference is an inference process that calculates the probability distribution of each candidate value for the row identifier based on the current factor graph and evaluation conditions.

[0111] The probability distribution of the row identifier is a discrete probability mass function given for each candidate row number, which is used to characterize the uncertainty of the row identifier and participate in subsequent expectation calculation.

[0112] The expectation process involves calculating the expectation of the weighted sum of squares objective using the probability distribution of row identifiers, thus enabling continuous variable estimation to take into account the impact of discrete uncertainty.

[0113] The initial estimates of the continuous variables are preliminary values ​​of the continuous states such as robot pose and line centerline parameters output in the soft stage, which are used as initial values ​​for solving the hard stage.

[0114] The hard stage is the second stage of joint incremental optimization. Based on the maximum a posteriori criterion, the row identifier is solidified into a definite value, and under the same evaluation conditions, it is re-linearized and the weighted sum of squares is minimized to obtain the final estimate.

[0115] The maximum a posteriori criterion is a criterion for selecting the row identifier value that maximizes the posterior probability.

[0116] The relinearization involves taking the soft-stage results as a starting point in the hard stage and re-approximating the factor residuals and the objective function linearly.

[0117] The minimization of the weighted sum of squares is an optimization process that minimizes the weighted sum of squares objective in the hard stage.

[0118] The term "close observation factor" refers to disabling the rejected factor in the current factor graph, so that it no longer participates in the objective function and subsequent incremental optimization.

[0119] The robot pose trajectory is a final estimated sequence of robot poses arranged in chronological order and covering the time sequence of the sliding window;

[0120] The final estimate of the row identifier sequence is the row identifier determination value sequence obtained in the hard phase and covering the sliding window timing.

[0121] Optionally, step S6 specifically includes:

[0122] The final estimates of the robot's pose trajectory, row centerline parameters, and row identifier sequence are taken as input;

[0123] Select the robot pose and row identifier at the current moment from the robot pose trajectory and row identifier sequence, and use them together with the row centerline parameters to calculate the localization result;

[0124] Based on the robot's current pose position, find the nearest point on the centerline represented by the line centerline parameters, and take the normal of the centerline at the nearest point as the direction reference. Combine the left and right relationship between the robot's current pose and the centerline, calculate the directional lateral distance from the pose position to the centerline to obtain the lateral deviation.

[0125] Calculate the heading angle based on the angle between the robot's current pose orientation and the tangential direction of the centerline at the nearest point;

[0126] Output the lateral deviation, heading angle, and current pose marker.

[0127] Terminology definition:

[0128] The centerline is a curve determined by the row centerline parameters, representing the geometric center path of the fruit tree row;

[0129] The nearest point is the point on the center line corresponding to the point where the distance from the current robot position to the center line is the smallest;

[0130] The normal direction is the unit direction that is orthogonal to the tangent of the centerline at the nearest point and is used to determine the lateral distance sign;

[0131] The tangential direction is the unit direction along the centerline at the nearest point;

[0132] The left-right relationship is determined based on the geometric relationship between the centerline normal and the robot's position relative to the centerline, which determines whether it is on the left or right.

[0133] The directional lateral distance is the lateral distance from the robot's position to the center line, and is marked with a sign. The sign is determined by the left-right relationship.

[0134] The lateral deviation is the numerical value of the directional lateral distance, used to quantify the degree and direction of the robot's lateral deviation relative to the centerline;

[0135] The heading angle is the signed angle between the robot's heading and the tangential direction of the centerline, used to quantify the heading error relative to the row direction;

[0136] The row identifier at the current moment is the row number or category corresponding to the current pose, which is used for the row label in the output result;

[0137] The positioning result is an output consisting of lateral deviation, heading angle, and current time row identifier, used for navigation and control decisions.

[0138] The beneficial effects of this invention are:

[0139] 1. Significantly suppresses cross-row leakage and inter-row jumps: By rejecting or downweighting candidate observations through counterfactual mutual exclusion gating, and combining row identifier consistency, sequence topological consistency and left and right boundary pair consistency factors, row identifiers are stably inferred in the joint optimization of soft and hard stages, reducing the risk of misassignment between adjacent rows;

[0140] 2. Improve robustness and positioning accuracy: The graph transformer adaptively weights and calibrates the covariance based on node and edge types, observation confidence, local observability and residual context, making heterogeneous factor scaling comparable, suppressing noise and outlier effects, improving the reliability of row marker determination and the estimation accuracy of lateral deviation and heading angle.

[0141] 3. Enhanced real-time performance and scene adaptability: Discrete and continuous incremental optimization within the sliding window; the row identifier probability distribution provided by the soft stage provides a good initial value for the hard stage optimization; it can maintain continuous and stable positioning even under conditions of occlusion changes, unstable lighting, and limited satellite signals, and adapts to different sensor configurations and orchard structures. Attached Figure Description

[0142] The accompanying drawings are provided to further illustrate the invention and form part of the specification. They are used in conjunction with embodiments of the invention to explain the invention and do not constitute a limitation thereof. In the drawings:

[0143] Figure 1 This is a flowchart of a robot orchard row localization method based on multimodal semantic graph optimization proposed in this invention. Detailed Implementation

[0144] The present invention will now be described in further detail with reference to the accompanying drawings. These drawings are simplified schematic diagrams, illustrating only the basic structure of the invention, and therefore only show the components relevant to the invention.

[0145] refer to Figure 1 A robot-based orchard row localization method based on multimodal semantic graph optimization includes:

[0146] S1. Acquire camera image frames, lidar point cloud frames, inertial measurement sequences, and global satellite navigation system observation sequences, and perform time synchronization and external parameter calibration;

[0147] S2. Based on the above data, perform semantic parsing and geometric preprocessing to generate semantic observations of tree trunks, stakes, tree walls and ground boundaries, and calculate their confidence and geometric attributes. Establish an initial semantic factor graph based on a sliding window.

[0148] S3. Using the initial semantic factor graph as input, run the graph transformer adaptive weighting model to output the weights and covariance parameters of each factor.

[0149] S4. Apply factor weights and covariance parameters to the initial semantic factor graph to form evaluation conditions, perform counterfactual evaluation for each candidate observation factor, calculate risk scores, and generate gated decisions.

[0150] S5. Apply the gating decision to the initial semantic factor graph and combine factor weights and covariance parameters. Use discrete and continuous joint incremental optimization. In the soft stage, obtain the probability distribution of row identifiers and the initial estimate of continuous variables. In the hard stage, solidify the row identifiers and re-optimize the continuous variables. Output the probability distribution of row identifiers, the initial estimate of continuous variables, the robot pose trajectory, the row centerline parameters and the final estimate of the row identifier sequence.

[0151] S6. Based on the robot's pose trajectory, the final estimate of the row centerline parameters and the row identifier sequence, calculate the directional lateral distance and heading angle from the current pose to the row centerline, label the row identifier at the current moment, and output the lateral deviation, heading angle and the row identifier at the current moment.

[0152] In this specific embodiment, S1 specifically refers to:

[0153] The system comprises three parts: data acquisition, time synchronization and delay compensation, and multi-sensor extrinsic parameter calibration. The robot platform, equipped with a camera, LiDAR, inertial measurement unit, and global navigation satellite system, operates in parallel to generate raw image frames, point cloud frames, inertial measurement sequences, and satellite observation sequences. To ensure comparability of cross-modal data under the same time reference, a linear clock model is established for each sensor.

[0154] ;

[0155] in Represents the raw timestamp scalar recorded under the local clock of the sensor. This represents the corrected timestamp scalar mapped to a unified clock reference. The frequency scaling factor scalar representing the sensor clock relative to a uniform clock. A scalar or subscript representing the fixed offset of the sensor's clock relative to a uniform clock. Used to identify specific sensor instances and for camera access. LiDAR Inertial Measurement Unit With Global Navigation Satellite System Four categories;

[0156] After considering the pipeline delay caused by end-to-end processing and transmission of sensors, further delay compensation is performed to:

[0157] ;

[0158] in Indicates the final timestamp scalar after compensation. This represents the total delay scalar of the sensor;

[0159] By solving and and application This achieves frame-level alignment between camera image frames and LiDAR point cloud frames under a unified clock reference. Simultaneously, it interpolates or resamples inertial measurement sequences and satellite observation sequences at a unified timescale to obtain a synchronized sequence with consistent timing. For external parameter calibration, the robot's base coordinate system is used. Using the reference coordinate system, for each sensor (in Can be or Establish rigid external parameters:

[0160] ;

[0161] In the formula Indicates from the sensor coordinate system to base coordinate system homogeneous transformation matrix, Represented by the sensor coordinate system to base coordinate system Rotation matrix Represented by the sensor coordinate system to base coordinate system Translation vector, 1 represents the zero row vector, and 1 represents the unit scalar in homogeneous coordinates;

[0162] Various methods can be used to obtain results through hand-eye calibration in static or dynamic scenes, point cloud and checkerboard / AprilTag registration, or multi-view least squares solutions. and Consistency and closed-loop verification are performed. When it is necessary to obtain the relative pose between different sensors, coordinate transformation composite is used. Complete the calculation, in the formula Indicates from the lidar coordinate system To the camera coordinate system homogeneous transformation matrix, and These represent the coordinates from the camera coordinate system. With lidar coordinate system to base coordinate system homogeneous transformation matrix, superscript and Identify the target coordinate system of the transformation matrix respectively. and Subscript and Identify the source coordinate system of the transformation matrix respectively. and This represents the matrix inversion operator;

[0163] Through the above time synchronization, delay compensation and extrinsic parameter calibration processes, the final output includes time-aligned image frames, time-aligned point cloud frames, inertial measurement sequences, global navigation satellite system observation sequences, and extrinsic parameters of all sensors relative to the base and to each other for use in subsequent steps.

[0164] In this specific embodiment, S2 specifically refers to:

[0165] Using image frames, point cloud frames, inertial measurement sequences, global navigation satellite system observation sequences, and sensor extrinsic parameters as inputs, with at least image frames, point cloud frames, and extrinsic parameters, semantic parsing and geometric preprocessing are performed on the images and point clouds to generate semantic observations such as "tree trunk," "stake," "tree wall," and "ground boundary," and for each time step... With each type of observation Calculate the 3D position, local orientation, and confidence level. The 3D position is in the lidar coordinate system. The following is obtained through geometric fitting: Mapped to the robot base coordinate system via extrinsic parameters for:

[0166] ;

[0167] In the formula Indicates time Semantic observation exist Three-dimensional position vector in the system, Indicates the corresponding position is Three-dimensional position vector in the system, Indicates from Tie The homogeneous transformation matrix of the system, where 1 represents the unit scalar in homogeneous coordinates. Represents a time-indexed scalar, Represents a semantic category index scalar;

[0168] The local orientation was obtained through principal component analysis as follows:

[0169] ;

[0170] In the formula Indicates observation At any moment unit orientation vector, Represents the candidate unit direction vector, This represents the covariance matrix of the observation point cluster;

[0171] The observation confidence level is given by a logistic function as follows:

[0172] ;

[0173] in Indicates time Observation confidence scalar Represents the natural constant, This represents the log-odds scalar obtained by fusing the image semantic score and geometric quality.

[0174] Subsequently, an initial semantic factor graph is constructed within a sliding window encompassing multiple consecutive time points, with its variable set including discrete row identifiers. Continuous robot pose , row centerline parameters and the left and right boundary landmarks and For the aforementioned semantic observations and variables, measurement factors, row identifier consistency factors, centerline directed lateral distance factors, left and right boundary pair consistency factors, and sequence topological consistency factors are set, where the measurement factors are based on residuals:

[0175] Characterization;

[0176] in Represents semantic observation At any moment Measurement residual vector, Indicates the pose With centerline parameters Predict the mapping vector for the expected location of this semantic observation;

[0177] Centerline directional lateral distance factor Modeling, in the formula Indicates time directional lateral distance scalar to the centerline Scalar representing the arc length parameter along the centerline The arc length parameter is Three-dimensional point vectors on the center line, This represents the unit normal vector of the centerline at that point. Express posture The translation part of the vector;

[0178] Pairwise consistency of left and right boundaries and sequence topological consistency are achieved using literal rule constraints to reduce unnecessary formula expansions, including pairwise consistency at the same time. and The normal symmetry constraint and the monotonicity constraint of the landmark order along the row direction are used to obtain the initial semantic factor graph for subsequent weighting and optimization.

[0179] In this specific embodiment, S3 specifically refers to:

[0180] The initial semantic factor graph is used as input, and weighting and covariance calibration are performed on the topology of the graph. The factor graph is denoted as... In the formula Represents semantic factor graph, Represents a set of variable nodes, Represents the set of factor edges;

[0181] For each factor edge, denoted as the weighting object, Feature vectors are constructed and encoded based on the nodes associated with the factor and the observed attributes. ;

[0182] in Representation factor eigenvectors, Indicated by factors A one-time encoded vector composed of connected variable node types (including discrete row identifiers, robot pose, row centerline parameters, and left and right boundary landmarks). Representation factor One-time encoding vector for edge types (including measurement factor, row identifier consistency factor, centerline directed lateral distance factor, left and right boundary pair consistency factor, and sequence topological consistency factor). Representation and Factor Related observation confidence scalar, Representation factor Local observability scalar, Representation factor Linearize the initial residual vector at the current solution. scalar with norm 2 and Representation factor The initial residual vector;

[0183] The graph transformer performs attention-based message passing and adjacency context aggregation on the factor graph topology to obtain factor-level context representation vectors. The factor weights and covariance are then scaled using a lightweight mapping head:

[0184] ;

[0185] in Representation factor Weight scalar, Represents the Sigmoid logical function. This indicates that the context will be represented. Parameter vector mapped to weights Representation factor Covariance scaling factor scalar Represents the natural exponential function, This indicates that the context will be represented. The parameter vector mapped to the covariance scaling factor Representation factor Context-enhanced representation vector;

[0186] After obtaining the scaling factor, the uncalibrated covariance is constructed using the baseline covariance of the factor type as follows:

[0187] ;

[0188] in Representation factor The uncalibrated covariance matrix Indicates factor type The benchmark covariance matrix, Indicates the factor Type identifier mapped to a specific factor type;

[0189] To ensure scale comparability between different factors, the weights and covariance are numerically calibrated. The weight calibration is as follows: Covariance calibration is In the formula Represents the calibrated factor weight scalar, Representation type The mean scalar of all factor weights within the range Represents a small constant scalar that avoids division by zero. Represents the calibrated covariance matrix, Represents the matrix trace operator, Representation type The reference covariance matrix;

[0190] The final output of calibrated factor weights and covariance parameters is used for subsequent counterfactual evaluation and joint optimization.

[0191] In this specific embodiment, S4 specifically refers to:

[0192] The calibrated factor weights and covariance are used as evaluation criteria in the initial semantic factor graph for the candidate observation factor set. Each solution is evaluated counterfactually under the linearized approximation of the current solution, and the increments of four types of cost terms are calculated, where the candidate factors are denoted as... The cost increment is denoted as and respectively. (Indicates acceptance factor) (Cost increment scalar of time-of-flight identifier consistency item) (Indicates acceptance factor) (cost increment scalar of the directional lateral distance term of the centerline) (Indicates acceptance factor) The cost increment scalar of the pairwise consistency terms at the left and right boundaries) and (Indicates acceptance factor) The cost increment scalar of the time series topological consistency term is used to summarize the above increments according to the weighting coefficients to obtain the risk score:

[0193] ;

[0194] in Representation factor Counterfactual risk score scalar These represent the set weight coefficient scalars for the four types of cost items;

[0195] Then based on the gate threshold Generate gated decisions:

[0196] ;

[0197] in Representation factor Gated decision discrete variables, The threshold scalar represents the boundary between rejection and deweighting; rejection and deweighting represent two decision categories, respectively.

[0198] Factors under reduced weighting The weights and covariance are linearly adjusted according to risk proportions as follows:

[0199] ;

[0200] in Representation factor Calibrated original weight scalar, Representation factor The calibrated original covariance matrix and Represent the updated weight scalar and the updated covariance matrix after applying the weight reduction, respectively. Represents the scalar of the weight reduction factor. The coefficient of covariance expansion is a scalar. This represents a function that takes the minimum value.

[0201] In the event of rejection, directly shutting down the factor can make It will no longer participate in subsequent optimizations, and the final decision and update results of all candidate factors will be aggregated into a gated decision set. Joint incremental optimization for S5.

[0202] In this specific embodiment, S5 specifically includes:

[0203] Using the gating decision set as input, rejected observations are turned off, and the downweighted observations are given their updated weights and covariances. Joint incremental optimization is performed on discrete and continuous variables within a sliding window. The set of factors that still participate in the optimization after gating is denoted as . The time-series index set of the sliding window is denoted as The set of continuous variables is denoted as ,in Indicates time robot pose vectors, Represents the row centerline parameter vector, and Representing time respectively The left and right boundary landmark vectors;

[0204] Discrete row identifier sequence denoted as The updated factor weights and covariances are denoted as follows: and ,in Indicates factor index, For weighted scalars, (where covariance matrix is ​​used), the soft stage uses the weighted sum of squares of the expected value of discrete variables as the objective function:

[0205] ;

[0206] In the formula The scalar representing the soft-stage objective function Represents the joint distribution Mathematical expectation operator Representation factor In continuous variables with discrete variables The residual vector under Represents the weighted L2 norm, Representation factor Information matrix;

[0207] The soft phase obtains the discrete probability mass function of the row identifier at each time step through probabilistic inference. And provide initial estimates for continuous variables. As the initial values ​​for the hard stage, the discrete variables are solidified according to the maximum a posteriori criterion in the hard stage. And minimize the weighted sum of squares objective under the same evaluation conditions:

[0208] ;

[0209] In the formula Indicates time Maximum posterior row identifier value, Indicates the total number of candidate row numbers, Indicates the solidified row identifier sequence, Represents the scalar of the hard-stage objective function;

[0210] Obtained through incremental solution And output the robot's pose trajectory , row centerline parameters , row identifier sequence and the aforementioned and .

[0211] In this specific embodiment, S6 specifically refers to:

[0212] The final estimate is taken as input, which includes the robot's pose trajectory. , row centerline parameters with row identifier sequence Index at the current time The robot's current pose is selected from the pose trajectory, and its position vector in the world coordinate system is obtained. With the forward unit vector of orientation At the same time, based on the row identifier With parameters Select the centerline of the corresponding row and define its arc length parameterized representation. Tangential unit vector With the normal unit vector (in the horizontal plane and) (and satisfy the right-hand rule)

[0213] First, find the arc length parameter closest to the current position on the center line. for ,in The scalar parameter representing the arc length from the nearest point to the centerline at the current moment. Indicates by and The arc length on the determined center line is 3D point vectors Represents the norm 2 operator;

[0214] Then, the directional lateral distance is calculated using the normal at the nearest point as the directional reference. ,in A scalar representing the directed lateral distance from the current position to the center line. Indicates the arc length The normal unit vector of the centerline, This represents the transpose operator;

[0215] Next, the heading angle is defined as the angle between the robot's forward direction and the tangential direction of the centerline. In the formula The heading angle scalar representing the current moment, This function represents the signed arctangent function and returns the angle range. The vector cross product operator, Indicates the arc length Tangential unit vector of the centerline Represents the vertically upward unit vector in the world coordinate system. Indicates by The rotating part acts on the forward unit vector of the vehicle body to obtain the forward unit vector of the robot;

[0216] Final output lateral deviation Heading angle The row identifier at the current time Used for positioning and control decisions.

[0217] The above description is only a preferred embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any equivalent substitutions or modifications made by those skilled in the art within the scope of the technology disclosed in the present invention, based on the technical solution and inventive concept of the present invention, should be covered within the scope of protection of the present invention.

[0218] This invention achieves synergistic effects on the technical problems to be solved across the entire chain: S1's time synchronization and extrinsic parameter calibration unify the observations of each modality to the same clock and coordinate reference, reducing the source of cross-modal correlation errors; S2, through semantic parsing and geometric preprocessing, makes high-value structured observations such as tree trunks, stakes, tree walls, and ground boundaries explicit, and establishes a semantic factor graph containing discrete variables such as "row identifiers" and continuous variables such as pose, centerline, and left and right boundaries within a sliding window, coupling row-level constraints and geometric constraints within the same optimization framework; S3 uses a graph transformer to perform adaptive weighting and covariance calibration on the factor graph topology, solving the problem of single-class factor "domination" caused by the incomparability of heterogeneous factor scaling and fixed weights; S4 uses inverse events... The evaluation calculates the cost increment for candidate observation factors and generates mutually exclusive gating. It rejects or downweights competitive interpretations across rows, significantly suppressing cross-row leakage and inter-row jumps. S5 uses a two-stage discrete-continuous joint incremental optimization. In the soft stage, it first gives the expected initial values ​​of row identifier probability and continuous state. In the hard stage, it solidifies the row identifier and then relinearizes to minimize the weighted sum of squares, thereby stabilizing the row identifier inference and improving the estimation accuracy of lateral deviation and heading angle. S6 uses the finally estimated pose, centerline and row identifier to calculate the current lateral deviation and heading angle, forming a positioning result that can be directly used for control. Overall, it can maintain continuous and stable inter-row positioning effect under occlusion changes, unstable lighting and satellite-limited scenarios.

[0219] In terms of algorithm structure, this invention proposes several targeted improvements to address technical problems and forms a closed loop: By explicitly introducing discrete "row identifiers" into the sliding window factor graph and superimposing pairwise consistency factors for left and right boundaries and sequence topological consistency factors, the symmetry and temporal monotonicity of the row structure are transformed into intrinsic constraints for optimization, avoiding label flipping and temporal inconsistencies caused by relying solely on nearest distance or local rules; through graph transformer weighting and covariance calibration, utilizing features such as node / edge type, observation confidence, observability, and residual context, the weights and uncertainties are adaptively changed with the scene and sensing quality, suppressing the influence of noise and outliers on the solution and improving the performance of different... The scaling comparability between factors; through counterfactual risk-driven mutual exclusion gating, the incremental cost of "accepting an observation" to row identifier consistency, lateral distance, pairwise consistency, and topological consistency is explicitly quantified and rejected or downweighted accordingly, providing stronger global consistency screening for cross-row competitive interpretations; through soft and hard two-stage joint optimization, the bidirectional coupling between discrete and continuous states is broken down, the probability output of the soft stage provides robust initial values ​​for the hard stage, and the deterministic optimization of the hard stage improves the controllability and real-time performance of the solution; combined with the incremental update and numerical calibration strategy of sliding window, the overall structure enhances the technical effects of cross-row suppression and accuracy improvement while ensuring real-time performance.

Claims

1. A robot orchard row localization method based on multimodal semantic graph optimization, characterized in that, include: S1. Acquire camera image frames, lidar point cloud frames, inertial measurement sequences, and global satellite navigation system observation sequences, and perform time synchronization and external parameter calibration; S2. Based on the above data, perform semantic parsing and geometric preprocessing to generate semantic observations of tree trunks, stakes, tree walls and ground boundaries, and calculate their confidence and geometric attributes. Establish an initial semantic factor graph based on a sliding window. S3. Using the initial semantic factor graph as input, run the graph transformer adaptive weighting model to output the weights and covariance parameters of each factor. S4. Apply factor weights and covariance parameters to the initial semantic factor graph to form evaluation conditions, perform counterfactual evaluation for each candidate observation factor, calculate risk scores, and generate gated decisions. S5. Apply the gating decision to the initial semantic factor graph and combine factor weights and covariance parameters. Use discrete and continuous joint incremental optimization. In the soft stage, obtain the probability distribution of row identifiers and the initial estimate of continuous variables. In the hard stage, solidify the row identifiers and re-optimize the continuous variables. Output the probability distribution of row identifiers, the initial estimate of continuous variables, the robot pose trajectory, the row centerline parameters and the final estimate of the row identifier sequence. S6. Based on the robot's pose trajectory, the final estimate of the row centerline parameters and the row identifier sequence, calculate the directional lateral distance and heading angle from the current pose to the row centerline, label the row identifier at the current moment, and output the lateral deviation, heading angle and the row identifier at the current moment.

2. The robot orchard row localization method based on multimodal semantic graph optimization according to claim 1, characterized in that, S1 specifically refers to: Acquire camera image frames, lidar point cloud frames, inertial measurement sequences, and global navigation satellite system observation sequences; The collected data is synchronized in time. The time synchronization includes at least a unified clock reference, timestamp alignment and delay compensation, so that the camera image frames and the lidar point cloud frames are aligned in the same time reference, and the inertial measurement sequence and the global satellite navigation system observation sequence are aligned in the same time reference. Perform extrinsic parameter calibration on each sensor. The extrinsic parameter parameters obtained by the calibration include the rotation and translation of each sensor relative to the robot base coordinate system, as well as the rotation and translation between each sensor. Output time-aligned image frames, time-aligned point cloud frames, inertial measurement sequences, global navigation satellite system observation sequences, and sensor extrinsic parameters.

3. The robot orchard row localization method based on multimodal semantic graph optimization according to claim 1, characterized in that, S2 specifically refers to: The input includes image frames, point cloud frames, inertial measurement sequences, global navigation satellite system observation sequences, and sensor extrinsic parameters, among which at least image frames, point cloud frames, and sensor extrinsic parameters are used. Semantic parsing and geometric preprocessing are performed on image frames and point cloud frames to generate semantic observations of tree trunks, piles, tree walls, and ground boundaries. Confidence and geometric attributes are calculated for each semantic observation. The geometric attributes include at least three-dimensional position and local orientation. An initial semantic factor graph is established based on a sliding window, wherein the sliding window includes multiple consecutive time points, and the initial semantic factor graph includes discrete variable row identifiers, continuous variable robot pose, continuous variable row centerline parameters, and continuous variable left and right boundary landmarks. For the aforementioned semantic observations, measurement factors, row identifier consistency factors, row centerline directional lateral distance factors, left and right boundary pair consistency factors, and sequence topology consistency factors are established in the initial semantic factor graph, respectively. The sequence topology consistency factor is modeled based on the landmark order relationship along the row direction.

4. The robot orchard row localization method based on multimodal semantic graph optimization according to claim 1, characterized in that, S3 specifically refers to: Using the initial semantic factor graph as input, features for weighting are constructed for each node and factor in the initial semantic factor graph. The features include at least node type, edge type, observation confidence, local observability index, initial value of factor residual and adjacency context. The features are encoded into vectors and input into the graph transformer adaptive weighting model, so that the graph transformer performs attention-based message passing on the topology of the initial semantic factor graph, generates factor weights and covariance parameters corresponding to each factor, and performs numerical calibration on the factor weights and covariance parameters to ensure that the scales of different factors are comparable. Output calibrated factor weights and covariance parameters.

5. The robot orchard row localization method based on multimodal semantic graph optimization according to claim 1, characterized in that, S4 specifically refers to: The factor weights and covariance parameters are used as inputs and applied to the initial semantic factor graph to form evaluation conditions; For each candidate observation factor in the initial semantic factor graph, the objective function is linearized based on the current solution, and the cost increments of the row identifier consistency term, the row center line directed lateral distance term, the left and right boundary pair consistency term, and the sequence topology consistency term are calculated respectively when the candidate observation factor is accepted. The counterfactual risk score is obtained by summing the cost increments according to the set weights. Gating decisions are generated based on counterfactual risk scores, wherein a rejection decision is generated when the counterfactual risk score is greater than the gate threshold, and a deweighting decision is generated when the counterfactual risk score is not greater than the gate threshold. The deweighting decision determines the weight reduction ratio or covariance expansion ratio of the candidate observation factor based on the counterfactual risk score. Output gating decision set.

6. The robot orchard row localization method based on multimodal semantic graph optimization according to claim 1, characterized in that, S5 specifically refers to: Using the gating decision set as input, the gating decision is applied to the initial semantic factor graph and combined with factor weights and covariance parameters. The rejected observation factors are closed, and the weights and covariance parameters of the downweighted observation factors are adjusted. Discrete and continuous joint incremental optimization is performed within a sliding window. Specifically, the soft-stage joint optimization constructs an objective function composed of the weighted sum of squares of the residuals of each factor, using the gating and weighted factor graph as conditions. The probability distribution of the row identifier is obtained by probabilistic inference, and the objective function is expected based on the probability distribution of the row identifier to obtain the initial estimate of the continuous variable. The probability distribution of the row identifier and the initial estimate of the continuous variable are output. The hard-stage joint optimization takes the output of the soft stage as input, solidifies the row identifiers into definite values ​​according to the maximum a posteriori criterion, re-linearizes and minimizes the weighted sum of squares objective function under the same conditions, and obtains the final estimates of the robot pose trajectory, row centerline parameters and row identifier sequence. Output the final estimate of the robot's pose trajectory, row centerline parameters, and row identifier sequence.

7. The robot orchard row localization method based on multimodal semantic graph optimization according to claim 1, characterized in that, S6 specifically refers to: The final estimates of the robot's pose trajectory, row centerline parameters, and row identifier sequence are taken as input; Select the robot pose and row identifier at the current moment from the robot pose trajectory and row identifier sequence, and use them together with the row centerline parameters to calculate the localization result; Based on the robot's current pose position, find the nearest point on the centerline represented by the line centerline parameters, and take the normal of the centerline at the nearest point as the direction reference. Combine the left and right relationship between the robot's current pose and the centerline, calculate the directional lateral distance from the pose position to the centerline to obtain the lateral deviation. Calculate the heading angle based on the angle between the robot's current pose orientation and the tangential direction of the centerline at the nearest point; Output the lateral deviation, heading angle, and current pose marker.

Images