Greenhouse mobile platform multi-source fusion pose estimation and navigation method based on candidate observation gating

Abstract

The application discloses a greenhouse mobile platform multi-source fusion pose estimation and navigation method based on candidate observation gating, and relates to the technical field of robot pose estimation and navigation. The method comprises the following steps: performing preset detection on four candidate observations to identify whether the dual-antenna heading is degraded, so that reliable target GNSS observations can be screened out when the greenhouse GNSS signal is abnormal; when the heading is degraded, the IMU and encoder data are fused to generate a degraded heading observation, so that the continuity of the heading estimation is ensured; the screened GNSS position, heading and encoder wheel speed are packaged into a joint observation vector and a joint noise covariance matrix is constructed, the fusion of multi-source heterogeneous observations is realized through untraceable Kalman filtering update; if there is visual measurement data in the current frame, the data is subjected to gating screening and then used as an independent pose observation to perform asynchronous update and additional correction on the pose. The application can continuously output continuous, stable and highly available fusion pose results in a complex greenhouse scene.

Description

Technical Field

[0001] This application relates to the field of robot pose estimation and navigation technology, and in particular to a multi-source fusion pose estimation and navigation method for a greenhouse mobile platform based on candidate observation gating. Background Technology

[0002] Greenhouses typically contain metal frames, irrigation systems, crop canopies, and narrow passageways. These factors can lead to problems such as GNSS-RTK signal obstruction, multipath reflection, reduced satellite count, and dual-antenna baseline anomalies. For mobile platforms that need to travel in straight lines along rows, perform low-speed precision operations, or turn around on the spot, these problems can cause: dual-antenna RTK may output incorrect headings when baseline calculation is abnormal, easily leading to control command jitter and path deviation; single-antenna positions may jump when locally degraded, causing abrupt attitude changes. Moreover, relying solely on IMUs for short-term propagation makes them prone to integral drift when dual-antenna headings are unavailable. Summary of the Invention

[0003] The purpose of this application is to provide a multi-source fusion pose estimation and navigation method for greenhouse mobile platforms based on candidate observation gating, which can provide continuous, stable and highly available fusion pose results when the complex environment of the greenhouse causes GNSS signal degradation and instability.

[0004] To achieve the above objectives, this application provides the following solution: Firstly, this application provides a multi-source fusion pose estimation method for a greenhouse mobile platform based on candidate observation gating, including: Acquire the current frame sensing data of the mobile platform; the sensing data includes measurement data from the front and rear dual-antenna RTK, IMU, and encoder; Based on the RTK measurement data from the front and rear dual antennas, a preset detection of multiple candidate observations is performed to obtain the detection results. Among them, the multiple candidate observations include the front antenna's individual position observation, the rear antenna's individual position observation, the dual antennas' joint position observation, and the dual antennas' joint position and heading observation. The detection results include the target GNSS observation and whether the dual antenna heading has degraded. The target GNSS observation is the selected candidate observation. If the dual-antenna heading degrades, the IMU measurement data and encoder measurement data are fused to generate a degraded heading observation; The position observations from the target GNSS observations, the heading observations or degraded heading observations from the target GNSS observations, and the left and right wheel speed observations from the encoder are packaged into a joint observation vector, and a corresponding joint observation noise covariance matrix is ​​constructed; the left and right wheel speed observations from the encoder are determined based on the encoder's measurement data; Based on the joint observation vector and the joint observation noise covariance matrix, an unscented Kalman filter update is performed to obtain the first pose estimate; If the current frame perception data also includes visual measurement data, then the visual measurement data detected by gating will be used as an independent pose observation, and the first pose estimation will be updated by an unscented Kalman filter once to obtain the second pose estimation; the visual measurement data includes the lateral deviation and heading deviation of the mobile platform relative to the centerline of the channel.

[0005] Secondly, this application provides a multi-source fusion navigation method for a greenhouse mobile platform based on candidate observation gating, including: The real-time pose of the mobile platform itself is determined by the multi-source fusion pose estimation method for greenhouse mobile platforms based on candidate observation gating described in the first aspect. Path tracking is performed based on the deviation between the mobile platform's current pose and the planned path.

[0006] According to the specific embodiments provided in this application, this application has the following technical effects: This application provides a multi-source fusion pose estimation and navigation method for a greenhouse mobile platform based on candidate observation gating. It constructs four candidate observations: single position of the front antenna, single position of the rear antenna, joint position of both antennas, joint position of both antennas, and heading. A preset detection is performed to identify whether the heading of the two antennas has degraded, enabling the selection of reliable target GNSS observations when the greenhouse GNSS signal is abnormal. When the heading degrades, IMU and encoder data are fused to generate a degraded heading observation, ensuring the continuity of heading estimation. Finally, the filtered GNSS position, heading (normal or degraded), and encoder wheel speed are packaged into a joint observation vector, and a joint noise covariance matrix is ​​constructed. This is then updated using unscented Kalman filtering to achieve the fusion of multi-source heterogeneous observations. If visual measurement data exists in the current frame, it is gated and filtered before being used as an independent pose observation for asynchronous updates, providing additional pose corrections. Through this scheme, this application can continuously output continuous, stable, and highly available fused pose results in complex greenhouse scenarios, addressing GNSS signal attenuation and positioning instability caused by multipath interference and signal partial obstruction. Attached Figure Description

[0007] To more clearly illustrate the technical solutions in the embodiments of this application or the prior art, the drawings used in the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments of this application. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0008] Figure 1A flowchart illustrating a multi-source fusion pose estimation method for a greenhouse mobile platform based on candidate observation gating, provided in an embodiment of this application; Figure 2 A flowchart illustrating a multi-source fusion pose estimation and navigation method for a greenhouse mobile platform based on candidate observation gating, provided in an embodiment of this application; Figure 3 A schematic diagram of the technical framework of a multi-source fusion pose estimation and navigation method for a greenhouse mobile platform based on candidate observation gating provided in an embodiment of this application; Figure 4 This is a schematic diagram of a greenhouse mobile platform provided in one embodiment of this application. Detailed Implementation

[0009] The technical solutions of the embodiments of this application will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of this application, and not all embodiments. Based on the embodiments of this application, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of this application.

[0010] To make the above-mentioned objectives, features and advantages of this application more apparent and understandable, the application will be further described in detail below with reference to the accompanying drawings and specific embodiments.

[0011] In one exemplary embodiment, such as Figure 1 As shown, a multi-source fusion pose estimation method for a greenhouse mobile platform based on candidate observation gating is provided, including the following steps: Step 101: Obtain the current frame perception data of the mobile platform; the perception data includes measurement data from the front and rear dual-antenna RTK, IMU, and encoder.

[0012] Step 102: Perform a preset detection of multiple candidate observations based on the front and rear dual-antenna RTK measurement data to obtain the detection results; wherein, the multiple candidate observations include front antenna single position observation, rear antenna single position observation, dual-antenna joint position observation, and dual-antenna joint position and heading observation; the detection results include target GNSS observation and whether the dual-antenna heading has degraded; the target GNSS observation is the selected candidate observation.

[0013] Step 103: If the dual-antenna heading degrades, then the IMU measurement data and encoder measurement data are fused to generate a degraded heading observation.

[0014] Step 104: Package the position observation, heading observation or degraded heading observation in the target GNSS observation, and the left and right wheel speed observations of the encoder into a joint observation vector, and construct the corresponding joint observation noise covariance matrix; the left and right wheel speed observations of the encoder are determined based on the measurement data of the encoder.

[0015] Step 105: Based on the joint observation vector and the joint observation noise covariance matrix, perform unscented Kalman filtering update to obtain the first pose estimate.

[0016] Step 106: If the current frame perception data also includes visual measurement data, then the visual measurement data detected by gating is used as an independent pose observation, and the first pose estimation is updated by an unscented Kalman filter once to obtain the second pose estimation; the visual measurement data includes the lateral deviation and heading deviation of the mobile platform relative to the centerline of the channel.

[0017] As an optional implementation, the preset detection includes preliminary geometric consistency detection and subsequent comprehensive selection; the preliminary geometric consistency detection includes: dual-antenna baseline length detection, dual-antenna heading consistency with the current filtered heading consistency detection, dual-antenna heading consistency with the GNSS track direction consistency detection, and single-antenna jump detection; wherein: The dual-antenna baseline length detection includes: comparing the current baseline length with the nominal baseline length to obtain the degree of deviation; when the degree of deviation exceeds the hard tolerance threshold, the dual-antenna joint position observation and the dual-antenna joint position and heading observation are determined to be unusable and discarded; when the deviation is between the soft tolerance threshold and the hard tolerance threshold, the observation noise corresponding to the dual-antenna joint position observation and the dual-antenna joint position and heading observation is amplified.

[0018] The consistency detection of the dual-antenna heading and the current filtered heading includes: comparing the current dual-antenna heading with the current filtered heading to obtain a first heading consistency deviation; when the first heading consistency deviation exceeds a first consistency threshold, amplifying the observation noise corresponding to the dual-antenna heading.

[0019] The dual-antenna heading and GNSS track direction consistency detection includes: comparing the current dual-antenna heading with the track direction corresponding to the first preset duration GNSS displacement to obtain a second heading consistency deviation; when the second heading consistency deviation exceeds the second consistency threshold, it is determined that the dual-antenna heading has degraded.

[0020] The single-antenna jump detection includes: for observation of the front antenna's single position or observation of the rear antenna's single position, when the position change of two adjacent frames exceeds the jump threshold within the allowed jump detection period, a first score penalty of a second preset duration is continuously applied to the comprehensive cost calculation of the corresponding candidate observation.

[0021] The subsequent comprehensive selection includes: based on the previous geometric consistency detection, calculating the normalized innovation square for the remaining candidate observation set, and further calculating the comprehensive cost to obtain the corresponding comprehensive cost; then, based on the preset selection strategy, selecting the target GNSS observation; the remaining candidate observation set is all candidate observations that have not been discarded or degraded.

[0022] As an optional implementation, the comprehensive cost includes normalized innovation squared divided by the candidate observation dimension, mode switching penalty, mode bias penalty, jump penalty, and online degradation penalty; the mode switching penalty is the cost imposed when the current candidate observation is not the same as the previously selected candidate observation; the mode bias penalty is the prior preference cost for different candidate observations; the jump penalty is the first score penalty; the online degradation penalty is the penalty cost added to candidate observations that depend on the antenna with the relatively poor score, based on the online degradation score of the front and rear antennas within a third preset time period.

[0023] As an optional implementation, the preset selection strategy includes: If there is a candidate observation in the remaining candidate observation set that is of the same type as the previously selected candidate observation and is gated by NIS, it is identified as the target GNSS observation; otherwise, the candidate observation that is gated by NIS and has the lowest overall cost is selected first; if all candidate observations are not gated by NIS, the candidate observation that contains only location and has the lowest overall cost is selected first.

[0024] As an optional implementation, the fusion of IMU measurement data and encoder measurement data to generate degraded heading observations specifically includes: 1-1) Calculate the IMU heading angular velocity and the encoder heading angular velocity based on the IMU measurement data and the encoder measurement data, respectively.

[0025] 1-2) Based on the noise variance of the IMU and the encoder, the fusion weights of the IMU azimuth angular velocity and the encoder azimuth angular velocity are determined respectively, and the IMU azimuth angular velocity and the encoder azimuth angular velocity are further fused to obtain the degraded azimuth angular velocity.

[0026] 1-3) Integrate the degraded heading angular velocity integral to obtain the degraded heading observation.

[0027] As an optional implementation, when the dual-antenna heading is within the degradation window, the process noise covariance and observation noise covariance of the degradation heading are adaptively updated according to the vehicle's motion state, specifically including: 2-1) When an encoder consistency alarm, drift alarm, or RTK residual mean exceeds the threshold is detected, increase the observation noise covariance of the degraded heading to reduce the fusion weight of the degraded heading observation.

[0028] 2-2) When the speed is higher than the first preset threshold, reduce the observation noise covariance of the degraded heading to improve the fusion weight of the degraded heading observation.

[0029] 2-3) When a straight-ahead condition is detected, reduce the process noise covariance of the degraded heading to reduce the rate of change of the degraded heading observation and suppress integral drift.

[0030] As an optional implementation, the method further includes: introducing a nonholonomic constraint with zero lateral velocity as a virtual observation, and adding it to the joint observation vector; When wheel slip is detected, the observation noise covariance corresponding to the encoder measurement data is increased, and the observation noise covariance corresponding to the nonholonomic constraint is also increased.

[0031] As an optional implementation, the gated detection of the visual measurement data includes one or more of the following: time validity gate, confidence gate, amplitude gate, turning state gate, and time continuity gate. In the asynchronous unscented Kalman filter update, the observation noise covariances corresponding to the lateral deviation and heading deviation of the visual measurement data are different; wherein, the observation noise covariance of the lateral deviation is smaller than the observation noise covariance of the heading deviation; and, when dual-antenna heading degradation is determined, the observation noise covariance of the visual measurement data is reduced.

[0032] As an optional implementation, the method further includes: When the mobile platform is detected to be stationary, the observation noise covariance corresponding to the GNSS position observation in the joint observation vector is amplified; when the mobile platform is detected to start moving from stationary, the amplification of the GNSS position observation noise is gradually reduced to a normal value within a preset time window; when the stationary turning condition is detected, the observation noise covariance corresponding to the GNSS position observation is amplified, and the output position remains unchanged.

[0033] In another exemplary embodiment, a multi-source fusion navigation method for a greenhouse mobile platform based on candidate observation gating is also provided, including: Step 201: Determine the real-time pose of the mobile platform itself; the real-time pose of the mobile platform itself is determined through an embodiment of the greenhouse mobile platform multi-source fusion pose estimation method based on candidate observation gating.

[0034] Step 202: Perform path tracking based on the deviation between the mobile platform's current pose and the planned path.

[0035] To aid understanding by those skilled in the art, the following embodiments will be used for further explanation.

[0036] (1) Overview of the method provided in this embodiment: This embodiment targets a mobile platform equipped with a front RTK, a rear RTK, an IMU (Inertial Measurement Unit), and a wheel encoder, employing an unscented Kalman filter as the main fusion framework. The system uses IMU high-frequency prediction as the main thread, with near-synchronous updates of the front and rear dual-antenna RTK and encoder as the main update link. After the main update, the channel centerline deviation output from an external vision module can be optionally received as an asynchronous additional constraint. The entire method includes sensor access and preprocessing, initialization and warm-up, high-frequency prediction, construction of multiple candidate RTK observations, candidate screening and selection, degenerate heading generation, encoder and nonholonomic constraint construction, adaptive fusion update, health monitoring and soft reset, output stabilization, and path tracking to achieve closed-loop navigation. See also... Figures 2-4 .

[0037] Among them, such as Figure 2 As shown, after receiving sensor data, the front and rear RTK and encoder data are approximately synchronized. Simultaneously, the IMU independently triggers high-frequency prediction, and visual data is asynchronously cached. After completing dual RTK initialization and filtering warm-up, high-frequency UKF prediction is continuously performed based on the IMU. Subsequently, multi-dimensional RTK observations are constructed, and reliable observations are screened through baseline consistency, jump detection, and NIS scoring. If the dual-antenna heading is reliable, it is directly adopted; otherwise, a degraded heading observation is generated, while encoder wheel speed observations and nonholonomic constraints are fused. Then, the validity of visual measurements is judged, and asynchronous constraints are constructed or adaptive UKF updates are directly executed. At the same time, the residual health status is continuously monitored. If consecutive bad frames exceed the threshold, a soft reset is triggered. Finally, a stable pose result is output and the next loop begins. The output pose data is synchronously supplied to the host computer for map editing, path planning, and vehicle motion controller. After the controller calculates the target speed through kinematics, it sends it to the servo driver and motor to realize closed-loop navigation control.

[0038] like Figure 3 As shown, the technical framework of the multi-source fusion pose estimation and navigation method for greenhouse mobile platforms based on candidate observation gating is divided into three layers: the sensor data perception layer on the left, the core pose estimation fusion algorithm layer in the middle, and the output data layer on the right.

[0039] The sensor data perception layer connects to devices such as a front-end RTK receiver, a rear-end RTK receiver, a wheeled odometer, an inertial measurement unit (IMU), and an RGB camera, providing multi-source raw data. The core algorithm layer is based on unscented Kalman filtering (UKF). It first aligns the encoder with the dual RTK time and performs timing matching, then completes state propagation and angle normalization based on the IMU. Next, it constructs a variable-dimensional measurement vector, and then optimizes the measurement information through mechanisms such as adaptive noise covariance, gated detection, and robust processing. At the same time, it is equipped with health maintenance modules such as residual monitoring and drift soft reset to ensure system stability. Finally, the pose calculation is completed through UKF measurement update, drift suppression, and smoothing. The output data layer provides RTK+IMU+encoder fusion state estimation results, visual constraint results, odometer information, and pose data for high-level navigation planning, providing reliable positioning support for subsequent control and planning.

[0040] like Figure 4 As shown, a greenhouse mobile platform is provided, which is used to implement the multi-source fusion pose estimation and navigation method for greenhouse mobile platforms based on candidate observation gating described above. The greenhouse mobile platform includes: an RGB-D camera, a lifting mast, a rotary servo motor, an obstacle avoidance radar, a navigation camera, an RTK rover, and drive wheels.

[0041] The lifting rod, the RGB-D camera, and the rotary servo are used for phenotypic acquisition; The obstacle avoidance radar detects obstacles around the platform in real time, assisting in safe obstacle avoidance and speed control.

[0042] The navigation camera is independently mounted on the top of the aluminum bracket at the front of the platform. It provides environmental texture and visual navigation information and can be fused with RTK and IMU data to participate in pose estimation and path planning.

[0043] The dual-antenna RTK rover provides centimeter-level high-precision positioning and heading information, serving as a benchmark for GNSS observation, heading determination, and platform navigation.

[0044] The drive wheel receives motion control commands and drives the platform to complete forward movement, turning, and other motion actions.

[0045] Here, RTK refers to an RTK rover, which is also referred to as an RTK antenna or antenna in this article.

[0046] (2) Method and scheme (obtaining pose): (2.1) Sensor access and preprocessing: a. The front RTK and back RTK are connected via serial port. After parsing, fields such as latitude and longitude, positioning mode, number of satellites and valid bits are extracted.

[0047] b. The encoder is accessed via WebSocket or a communication link.

[0048] c. The IMU accesses the ROS topic at high frequency to read attitude quaternions, angular velocity, and linear acceleration.

[0049] d. The vision module outputs lateral deviation, angular deviation, and confidence information.

[0050] (2.2) Initialization: After startup, the first frame of RTK observation is not used directly for initialization. Several frames of front and rear dual-antenna RTK data are cached. When the distance between the front and rear antennas is close to the nominal baseline length, the frame is included in the initialization buffer. The front and rear RTK data are averaged separately. After WGS84 coordinates are converted to local ENU coordinates, the initial position is determined by the midpoint of the dual antennas, and the initial heading is determined by the line connecting the front and rear antennas.

[0051] WGS84 refers to World Geodetic System 1984; ENU refers to East-North-Up coordinate system.

[0052] Here, RTK data refers to the data that is directly received by the GNSS receiver from the RTK antenna and then parsed by the ROS system. It mainly includes latitude and longitude, timestamp, positioning accuracy, number of satellites, etc.

[0053] The distance between the front and rear antennas refers to the relative distance between the positions of the front and rear RTK antennas on the local plane, which is calculated. The midpoint of the dual antennas represents the geometric center of the average position of the front RTK antenna and the average position of the rear RTK antenna.

[0054] (2.3) State-defined IMU high-frequency prediction: The filter uses an eight-dimensional state vector, and the IMU is only used for high-frequency prediction. It is not repeatedly injected as a measurement during the main update phase to avoid the same information being reused.

[0055] The eight-dimensional state vector is defined as follows: ; in, These are local ENU plane coordinates; For the platform's heading angle; For planar velocity; Angular velocity (or simply angular velocity) is the heading angular velocity. This is planar acceleration.

[0056] (2.4) Encoder differential velocity construction: Wheel speeds are calculated based on the difference between the cumulative encoded values ​​of the left and right wheels. To ensure encoder speed reliability, the system gates the following quantities; only after passing through this gate are the left and right wheel speeds included in the fusion update chain: a. Minimum and maximum thresholds for Δt; Δt represents the sampling time interval between adjacent frames. That is, a minimum and maximum threshold are set for the time interval between encoder data of adjacent frames. If the time interval Δt between the current frame encoder data and the previous frame encoder data is not between the minimum and maximum thresholds, the encoder data of that frame is determined to be invalid and will not be used for fusion update.

[0057] b. Upper limit of absolute speed of left and right wheels. That is, a maximum allowable value is set for the measured speed of the left and right wheels as an upper limit. When the absolute speed of either wheel exceeds the upper limit, the corresponding encoder data is determined to be invalid and will not be used for fusion update.

[0058] c. Upper limit of left and right wheel speed difference. That is, a maximum allowable value is set for the difference between the measured left and right wheel speeds as an upper limit. When the difference between the left and right wheel speeds exceeds this upper limit, the corresponding encoder data is determined to be invalid and will not be used for fusion update.

[0059] (2.5) Constructing GNSS candidate observations: To avoid directly writing degraded dual-antenna results into the filter, the following GNSS candidate observation set is constructed: a. Two-dimensional position observation of the front antenna alone; two-dimensional refers to the two positions of ENU plane coordinates x and y, and three-dimensional refers to the observation including angle (i.e., heading angle).

[0060] b. Two-dimensional position observation of the rear antenna alone.

[0061] c. Two-dimensional joint observation using dual antennas.

[0062] d. Three-dimensional joint observation with dual antennas.

[0063] The observation equations for the four GNSS candidate observations are constructed below. These equations are used to calculate the observation priors, and then to calculate the residuals by comparing them with the actual measurements.

[0064] In the observation modeling, instead of first hard-solving the antenna position into the vehicle reference point position, the antenna position or the average position of the two antennas is directly used as the observation. The observation model is constructed through the external parameter relationship between the antenna and the vehicle reference point, so that the attitude retains the constraint effect on the antenna position prediction.

[0065] Let the plane position of the vehicle body reference point be... The external parameters of the front and rear antennas in the vehicle system are as follows: and Then the prediction model for each candidate observation can be expressed as: ; ; ; in, This indicates the coordinate position of the vehicle body in a two-dimensional plane; , These represent the offsets of the front and rear antennas relative to the center of the vehicle body, i.e., external parameters; , , These represent the predicted positions of the front antenna, rear antenna, and midpoint of the dual antennas in the world coordinate system under the current state, respectively, which are the observation priors and the output of the prediction model. Indicates the vehicle's heading angle; The state vector predicted by the current filter (containing eight dimensions, but only a small portion is actually used here). , , R( ), which is the state prior, is the input to the prediction model; R() is a two-dimensional rotation matrix used to rotate the vehicle coordinate system to the world coordinate system.

[0066] After obtaining the corresponding observational priors, these priors are used to calculate the residuals with the relevant sensor measurement data. The residuals (multiplied by the filter gain) are then used to correct the corresponding state priors, resulting in the corresponding state posteriors. This process is called fusion. The filter gain is related to the observation noise; for example, the greater the observation noise of the corresponding sensor, the larger the observation noise covariance, and the smaller the corresponding filter gain.

[0067] In this embodiment, which involves multiple observations, instead of assigning an independent filter gain to each observation, a single gain matrix, also known as joint gain, is used for all observations. For example, the following quantities are combined into a joint update vector: target GNSS observations, encoder left and right wheel speed observations, degraded heading observations, and vehicle lateral speed nonholonomic constraints. This forms a large observation vector, and the joint residual and joint Kalman gain are calculated all at once. Then, the state prior is corrected all at once to obtain the state posterior. In other words, all observations are updated in a single joint update step using a unified gain matrix to correct the state. When the "confidence" of an observation automatically decreases (corresponding to increased measurement noise), the state posterior is corrected. As the measurement noise covariance increases, the corresponding value in the gain matrix will decrease.

[0068] In addition to the average position, dual-antenna three-dimensional observations also include the baseline heading from the rear antenna to the front antenna. Correspondingly: ; ; ; ; in, This indicates that the angle is normalized to ; , Representing the predicted positions of the front and rear antennas respectively, which are observation priors, they are respectively equal to , ; The output baseline prediction heading is a state prior (derived from other state priors). Indicates three-dimensional candidate observations; This represents the arctangent function, which calculates the heading from the position coordinates.

[0069] In the calculation formula, the subscripts x and y in parentheses represent the x and y components of the predicted position difference between the front and rear antennas, respectively.

[0070] (2.6) GNSS degradation discrimination and candidate selection: The following degradation criteria are applied to candidate observations: a) Dual-antenna baseline length detection: The measured baseline length is compared with the nominal baseline length. When the deviation exceeds the hard tolerance threshold, the dual-antenna joint observation is deemed unusable (excluding dual-antenna 3D joint observations, which are not included in subsequent NIS calculations and overall cost calculations; dual-antenna 2D joint observations can be retained as degradation candidates but with increased observation noise). When the deviation is between the soft tolerance threshold and the hard tolerance threshold, the candidate is retained, and its corresponding observation noise is increased, for example, by multiplying the original observation noise (i.e., the nominal observation noise covariance) by an amplification factor. The original observation noise refers to the observation noise used when the deviation is below the soft tolerance threshold. In this paper, scaled observation noise refers to scaled observation noise covariance.

[0071] The hard tolerance threshold is used to determine whether dual-antenna baseline observations have entered a severely degraded state. If the baseline deviation exceeds the hard tolerance threshold, dual-antenna heading observations are no longer used. Dual-antenna two-dimensional position observations can be retained as degradation candidates, but their observation noise will be significantly increased, and they will continue to be screened through the NIS test and the comprehensive cost function. The soft tolerance threshold is used to identify observation quality degradation in advance. When the deviation is between the soft and hard tolerance thresholds, the candidate is still retained, but its weight is reduced by amplifying the corresponding observation noise covariance.

[0072] b. Consistency detection between dual-antenna heading (also known as dual-antenna baseline heading) and current filtered heading: If the deviation exceeds the consistency threshold (first consistency threshold), the reliability of the dual-antenna heading observation is reduced, i.e., the corresponding observation noise is increased. The current filtered heading is the prior of the filtered heading before the current observation update, that is, the state prior after the prediction step, not the posterior after fusing the GNSS of this frame. .

[0073] c. Consistency detection of dual-antenna heading and GNSS track direction: When the dual-antenna heading conflicts with the displacement direction of the second preset time (exceeding the second consistency threshold), the dual-antenna heading is determined to be degraded (excluding dual-antenna three-dimensional joint observation, and not participating in subsequent NIS calculation and comprehensive cost calculation).

[0074] Determining the displacement direction for the second preset duration: Using the earliest and latest GNSS observations within the corresponding time window of the cache, the direction angle of the corresponding GNSS displacement is calculated through the position vector.

[0075] d. Single-antenna jump detection: If the position of the front or rear antenna changes beyond the threshold (exceeding the jump threshold) between two adjacent frames within the maximum time interval threshold allowed for jump detection (i.e., the allowed jump detection period), a duration penalty is applied to the corresponding candidate. Applying duration penalty: After a certain path is triggered, in the next T seconds (the second preset duration), each time the comprehensive cost of a candidate is calculated, a fractional penalty (i.e., jump penalty) is continuously added to the candidate observations that depend on this antenna.

[0076] 1) The baseline length and deviation of the dual antennas are calculated using the following formula: ; ; ; in, The nominal mechanical baseline length (i.e., the physical baseline length). This represents the average measured position of the front and rear antennas. Indicates the actual baseline length currently measured; || ||2 represents the Euclidean distance; This indicates the deviation between the currently measured actual baseline length and the physical baseline length (i.e., the nominal baseline length); It represents the ratio of the currently measured actual baseline length to the physical baseline length.

[0077] when <0.5 or A baseline value >1.6 is considered a severe baseline abnormality; when Exceeding the soft tolerance threshold However, it did not exceed the hard tolerance threshold. Instead of immediately discarding location observations, the weights are reduced by amplifying noise.

[0078] In some embodiments, the corresponding position noise and heading noise amplification factors are: ; ; ; ; ; in, This indicates how much the current baseline length deviation is relative to the soft tolerance threshold, that is, the degree of deviation; This indicates the configured soft tolerance threshold; It is mainly used to prevent the denominator from being 0, which is a very small value protection quantity; This refers to the noise amplification factor for dual-antenna position observation; This is the noise amplification factor for dual-antenna heading observation; This is the dual-antenna heading weight scaling factor, used to reduce the correction amount (to prevent the heading from changing too much). This is the noise scaling factor for the subsequent course degradation process; It is a limiting function that restricts the result to the range given in parentheses, avoiding extreme maxima and minima.

[0079] 2) The consistency between the dual-antenna baseline heading and the current filtered heading is calculated as follows: ; ; in, For the current filtered heading, This refers to the installation offset angle between the dual antenna baseline and the vehicle's heading. When When the consistency threshold is exceeded, the dual-antenna heading is considered unreliable. The filtered heading is the heading component in the filter state, which is predicted at high frequency using an IMU in the filter and updated using RTK, odometer, etc., to advance the filter state.

[0080] 3) The consistency between the baseline heading and the track direction of the dual antennas is calculated as follows: Record the earliest and latest RTK location samples within the time window as follows: and ,but: ; ; ; in, The direction angle of the displacement vector of the GNSS position within the current time window; This represents the original angular difference between the baseline heading and the track direction of the dual antennas; The final angle difference after processing for consistency of the direction axis.

[0081] when When the trajectory axis consistency threshold is exceeded, the dual-antenna heading direction is considered to be contaminated, and even if the baseline length may still be approximately reasonable, the three-dimensional dual-antenna heading candidate will no longer be used.

[0082] 4) The transition detection of a single antenna is calculated as follows: ; ; The superscript (k) indicates the current time, or the kth frame. This represents the positional step distance of the front antenna between two adjacent frames; This indicates the RTK position of the front antenna in the k-th frame; , Similarly.

[0083] When 0 < < and At that time, the front antenna jump flag is valid until The same applies to the rear antenna. It is the time interval between the current frame and the previous frame; It is the maximum time interval threshold allowed for transition detection. If the interval exceeds this, large displacements will not be directly considered as transitions. This represents the step distance threshold for determining when a single antenna transitions. It is the timestamp corresponding to the k-th frame; It is the duration of the penalty after the transition is triggered.

[0084] The jump flag is a marker used to indicate that a jump has occurred in the RTK of a particular path. Set the jump flag to active. The significance is that by adding this time delay, the filter's distrust of the data from the transition path continues for a short period of time, rather than just penalizing the current frame. This prevents several candidate modes from frequently switching back and forth between the front and rear antennas, and allows the selector to prioritize using another, more stable antenna path after a short-term anomaly.

[0085] 5) After passing the geometric consistency judgment above, normalized innovation squared (NIS) is calculated for each candidate.

[0086] Let the first One candidate observation is Its predicted value is ,but ; ; ; If the observation contains an angular component, then the corresponding innovation vector is uniformly calculated according to... deal with.

[0087] in, Indicates the first The innovation vector of each candidate observation, which is the difference between the measured value and the predicted value; For the first The prediction covariance matrix of candidate observations represents the uncertainty of the prediction of the observations themselves under the current UKF prior state; Indicates the first The measurement noise covariance matrix of a candidate observation, which represents the noise level of the candidate observation from the sensor measurement end; It is the first The innovative covariance matrix of each candidate observation; Indicates the first The normalized squared innovation of a candidate observation is the degree of matching between the candidate observation and the current filtered state after considering the total uncertainty.

[0088] In addition to position, a three-dimensional candidate observation also includes a heading angle component, which is the heading angle in the observation state.

[0089] Based on the candidate observation dimension, different thresholds are used to perform NIS consistency gating, and a comprehensive cost is constructed by combining candidate handover penalty, minimum hold frame count, single antenna hopping penalty, and online degradation score of front and rear antennas.

[0090] Let the four candidate categories be denoted as follows: , , and Then the first The combined cost of each candidate is: ; ; ; ; ; ; in, The dimension of the candidate observations; This refers to the mode switching penalty, not a physical quantity; that is, if the current candidate... If the candidate observation type is different from that selected in the previous frame, there will be an additional cost for this switch; These are configured parameters, representing the candidate mode switching penalty coefficient, and are pre-configured dimensionless scoring parameters used to suppress frequent switching of candidate observation types between adjacent frames. It is a mode bias penalty, which represents the prior preference for different candidate observations (types), and can be preset for each candidate by human (according to preference or needs); Front antenna candidates, Rear antenna candidates and dual-antenna two-dimensional candidates and three-dimensional candidates ; This indicates a jump penalty if the candidate If a candidate antenna that it relies on has recently experienced a jump anomaly, a penalty will be imposed on that candidate. These are pre-configured parameter coefficients for the jump penalty; The online degradation penalty is determined by increasing the penalty for the worse antenna based on the online degradation scores of the front and rear antennas over a recent period (a third preset duration). For example, for a dual-antenna two-dimensional candidate antenna, since it depends on both the front and rear antennas, its online degradation penalty can be determined by the weighted average of the degradation scores of the two antennas. The scaling factor is preferentially constructed for the penalty term in the online degradation scoring of the front and rear antennas; Indicates the scaling factor; These are pre-configured weighting coefficients for online degradation penalties; and These represent the online defect scores for the front and rear antennas, respectively. It is the upper limit value, used for amplitude limiting.

[0091] It should be noted that each penalty item is determined by preset parameters, candidate type, previous selection result, single antenna jump detection result, and online antenna degradation score.

[0092] The candidate observation with the lowest overall cost and that passes NIS gating is selected as the current GNSS update mode. When all candidates fail to pass NIS gating, the GNSS update is not immediately abandoned; instead, the candidate with the lowest overall cost (2D location) is selected first. , , ).

[0093] In some embodiments, to avoid mode jitter, if the previous candidate is still gated by NIS and is not prohibited from being held due to the current transition event, the candidate from the previous moment will continue to be used for a preset number of holding frames.

[0094] For each candidate observation, the determination of whether it passes the gating is: after a candidate has entered the scoring process, the corresponding NIS <= the threshold of the corresponding dimension.

[0095] The reason why two-dimensional position candidates do not exist is that neither the front nor rear antennas are currently available, or the coordinate transformation is invalid, and the two-dimensional candidate is not constructed at all. However, in reality, as long as either the front or rear antenna is normal, there will usually be at least one two-dimensional position candidate. The above are extreme cases and not the norm.

[0096] The specific meaning of a jump event: A single-antenna RTK path experiences an unreasonable displacement within a very short period of time. In the code, this is indicated if the time difference Δt between two adjacent frames is less than or equal to... And the position increment of the antenna >= Then, enable the jump flag along this path and maintain it until... .

[0097] (2.7) Degenerate heading generation: When the dual-antenna heading is reliable, the system directly uses the dual-antenna heading as the heading observation and simultaneously resets the internal state of the degraded heading. When the dual-antenna heading degrades, the following degraded heading generation steps are executed: a. Obtain the IMU heading angular velocity and perform zero bias compensation; b. Calculate the encoder's yaw angular velocity based on the speeds of the left and right encoder wheels; c. Fuse the IMU heading angular velocity and the encoder heading angular velocity according to the current reliability; d. Integrate the fused heading angular velocity to form a degraded heading estimate; e. Under straight-line conditions, reduce the degraded course variance and maintain a stable course; f. When the track direction is continuous and reliable, apply a gradual pullback to the degraded course using the short-term track direction.

[0098] This mechanism ensures that the data acquisition platform still has continuous attitude reference when the dual-antenna heading is temporarily unavailable.

[0099] Resetting the internal state of the degraded heading means realigning the degraded heading to the current reliable dual-antenna heading. Since the reliable dual-antenna heading is used directly at this time, the degraded heading does not need to participate in the update. It also clears the short-term trajectory history previously saved for track pullback and restores the relevant internal parameters to the normal state.

[0100] How to determine the reliability of a short-term trajectory direction: The judgment condition is not based on just one frame, but on whether the motion is stable within a third preset time period. Only when there is indeed motion, a sufficiently long displacement accumulated in a short period of time, and this displacement (data) itself is continuously valid, will the trajectory direction be regarded as a reliable reference.

[0101] Gradual pullback: When the dual-antenna heading is temporarily unreliable, but the short-term track direction is reliable, the backup heading (degraded heading estimate) will not be directly changed to the track direction. Instead, it will only move closer to the track direction a little at a time. This can gradually correct the deviation accumulated during the degradation period, and will not cause the heading to suddenly change due to fluctuations in positioning in a certain frame.

[0102] 1) When the dual-antenna heading is unreliable, the degraded heading estimate is recursively derived using the following formula. First, construct the heading angular velocities of the IMU and encoder: ; ; ; in, This is the original zero-bias estimate of the IMU; This represents the IMU's heading angular velocity after zero bias compensation; This represents the raw heading angular velocity directly measured by the IMU; generally, Even when stationary, it carries a fixed deviation, meaning that even when the device is completely stationary (without angular velocity or acceleration), there is a non-zero constant deviation value. , , These represent the linear velocities of the left and right wheels, and the wheel track, respectively. This indicates the steering speed estimated by the encoder; This represents the estimated forward speed of the encoder.

[0103] IMU raw bias estimate It is a fixed zero bias that is pre-calibrated and written into the configuration file.

[0104] To suppress long-term integral drift, a degenerate heading-specific gyroscope zero-bias estimate is maintained. .

[0105] When the encoder angular velocity is valid, the linear velocity is higher than the corresponding threshold, and no wheel slip is detected, update in the following manner: .

[0106] Determining whether the encoder angular velocity is valid: mainly based on whether the encoder data itself is valid, and whether the angular velocity calculated from the left and right wheel speeds is a normal value (not empty, not abnormal).

[0107] The significance of exceeding the corresponding threshold: If the vehicle speed is too low, the speed of the left and right wheels is already very small. At this time, the encoder quantization error, mechanical backlash and vibration noise will be amplified, and the calculated angular velocity is easy to be unstable.

[0108] Subsequently, an exponential moving average is used to update the degraded heading-specific zero bias: ; in, Update the gain to zero bias; within the straight-line locking heading window, take the larger value. This allows for faster absorption of the small gyroscope bias during the low-speed straight-line phase. Initially, the degenerate heading-specific zero bias is set to 0.

[0109] It should be noted that online zero-bias compensation is not enabled initially. When the update conditions are met for the first time, the angular velocity difference between the IMU and the encoder is used for direct initialization, and then the exponential sliding method is used for recursive updates.

[0110] After the above compensation is completed, the IMU heading angular velocity is written as: .

[0111] Then, a weighted fused angular velocity is constructed based on the noise variance of the IMU and the encoder.

[0112] Let the variance of the IMU angular velocity be... The fundamental variance of the encoder's angular velocity is derived as follows: ; in, This represents the speed variance of a single-wheel encoder. These represent the wheel track.

[0113] If a system consistency alarm is triggered, the variance is further amplified; if slippage (wheel slip) is detected, it is multiplied by a wheel slip amplification factor. ; in, This represents the roller skating intensity score, which is the current assessment of whether the wheels are slipping.

[0114] This consistency alarm is triggered primarily because it indicates a discrepancy between the vehicle motion reflected by the encoder and the motion state estimated by the current filter. Specifically, after each update, the system first deduces the theoretical left and right wheel speeds based on the current (fused) state, then compares these speeds with the actual left and right wheel speeds measured by the encoder. The average absolute value of the left and right wheel errors (i.e., averaging the absolute values ​​of the left and right wheel errors) is taken as the encoder inconsistency level. If this inconsistency level exceeds a threshold, a consistency alarm is triggered.

[0115] This yields the fusion weight: ; Note the preceding... This represents the angular velocity produced by the encoder. The two notations represent the information weights corresponding to the encoder; they are similar in appearance but have different actual meanings.

[0116] The degraded heading angular velocity fusion value is: ; Then, by integrating it, a degraded heading estimate is obtained.

[0117] When the system determines that it is in the straight-ahead heading window, it directly adopts... .

[0118] Determining whether it is in the straight-line locking heading window: mainly by judging whether the absolute value of the linear velocity derived by the encoder is higher than the minimum velocity threshold and the absolute value of the angular velocity derived by the encoder is less than the straight-line angular velocity threshold. At this time, it is judged to be in the straight-line locking heading window.

[0119] 2) To ensure that the degraded course remains both continuous and not overly rigid within the degradation window, its process noise and observation variance are also adaptively updated.

[0120] Let the basic process noise be The scaling factor given by the current RTK degradation level is: Then the target process noise is: .

[0121] Wherein, scaling factor For the determination of , please refer to the corresponding formula above.

[0122] If the system has consistency alarms, drift alarms, or the mean RTK residual exceeds the threshold, the noise in the target process will continue to be amplified.

[0123] Drift alarm: Detects whether the current RTK position residual exceeds the RTK residual threshold. If the current RTK residual exceeds the threshold, the drift alarm flag will be set.

[0124] RTK residual mean exceeds threshold: Each time the historical average is updated with the current RTK position residual, if this average exceeds the RTK residual threshold, the RTK residual mean is considered to exceed the threshold.

[0125] The target process noise is preferably approximated to the actual process noise using a first-order smoothing method: ; Among them, when Greater than When, take the larger one Conversely, take the smaller one. This is to achieve "rapid magnification and slow recovery". The parameter used to measure the inherent instability of the degraded course, i.e., the intensity of process noise; It is a smoothing gain that brings the current process noise closer to the target value; essentially, it is an update rate coefficient.

[0126] Degraded heading variance The recursion is as follows: ; In the formula, This represents the time interval between the current frame and the previous frame (i.e., the prediction step size of the filter). Indicates the index of the discrete time step (i.e., which frame). , respectively represent The lower and upper limits.

[0127] When vehicle speed is high, the variance of degraded heading observations is tightened using speed: ; ; ; in, This represents the speed tightening factor, meaning that the higher the vehicle speed, the more reliable the directional constraint imposed by the direction of motion can be. This represents the speed threshold (first preset threshold), a reference item for determining whether the speed is high enough. This represents the speed-related basic heading noise, i.e., how much uncertainty is allowed in the degraded heading when considering only the speed angle; This represents the candidate variance of the degraded heading, calculated from the current vehicle speed. The higher the speed, the smaller this value becomes. This represents the measurement variance ultimately used for degraded heading observations.

[0128] When the system is in the straight-ahead heading window, it is preferable to further tighten the restrictions as follows: ; This indicates a small range of uncertainty that is allowed to remain in the degraded heading under the straight-line locking heading condition. It is the upper limit of the allowable jitter set for the degraded heading when the straight-line stable heading is maintained. The smaller the value, the more it indicates that the vehicle is indeed traveling straight and the degraded heading should be more stable.

[0129] Finally, the degradation heading measurement and the measurement variance are as follows: .

[0130] This is a backup heading estimate maintained by the degraded heading module. If the normal dual-antenna heading is unreliable, it is used to continue providing a usable heading reference for the filter. For this value, if the reliable heading of the dual antennas is recovered, it is directly re-aligned using this reliable heading; if the dual-antenna heading is unreliable, it is updated by integrating the angular velocities derived from the IMU angular velocity and encoder based on the previous time step, according to the current reliability.

[0131] (2.8) Encoder observation, nonholonomic constraints, and wheel slip suppression: The system uses the speeds of the left and right wheels as encoder observations for updates. At the same time, under differential chassis conditions, a non-holonomic constraint on the lateral speed of the vehicle body is introduced, meaning that the lateral speed of the platform is approximately zero under normal, non-slip conditions.

[0132] a. To prevent the system from being misled by roller skating, the system further constructs roller skating detection parameters.

[0133] b. Compare the heading angular velocity measured by the IMU with the heading angular velocity calculated by the encoder.

[0134] c. Smooth the accumulation of differences.

[0135] d. When the cumulative difference exceeds the threshold, roller skating is determined to have occurred.

[0136] e. After roller skating occurs, the system performs the following actions: increases encoder observation noise and decreases encoder observation weight; relaxes nonholonomic constraint noise to prevent erroneous lateral constraints from forcibly suppressing real motion.

[0137] Non-holonomic constraints: These mainly apply to vehicles with differential chassis. Under normal conditions without sideslip, the lateral velocity of the vehicle should be close to 0, meaning the vehicle should move in the direction of its front and should not slide sideways significantly. Relaxing these constraints means increasing the corresponding observation noise to reduce its constraint force in the filter update and avoid masking actual tire sideslip.

[0138] During the main update phase, all observations (GNSS position, heading, encoder, nonholonomic constraints) share a large joint covariance matrix. Noise amplification during the candidate phase also affects the corresponding terms of the joint covariance matrix in the main update phase.

[0139] (2.9) Protection for stationary and in-place turning conditions: The following conditions are preferred for determining whether the vehicle is stationary or turning in place: The static condition determination is mainly based on the fact that the speeds of both left and right wheel encoders are lower than the static determination threshold, and the output position is held after the set holding time is met.

[0140] The determination of the stationary turning condition is mainly based on the plane translation speed being close to zero (the plane translation speed is lower than the stationary determination threshold), and the left and right wheels moving in opposite directions or the encoder calculating the angular velocity being higher than or equal to the turning threshold. At this time, the heading change is allowed, but the output position is maintained to suppress the position drift caused by RTK two-dimensional position jitter.

[0141] Under stationary conditions, GNSS observations are prone to multipath drift. This can be mitigated by amplifying GNSS position observation noise to prevent the position from being "dragged away." During the start-up and recovery phase, the amplified GNSS position observation noise is gradually released according to a set time window. In stationary turning conditions, the platform's actual position remains almost unchanged while its heading changes significantly. Further amplification of GNSS position observation noise reduces false displacements during stationary turns. Zero-velocity updates can also be performed when the encoder has been judged to be stationary for an extended period, converging the planar velocity to zero and tightening the velocity-related covariance.

[0142] The amplification factor refers to the scaling factor of the noise observed by GNSS position. Once the stationary state is exited, the stationary amplification factor will linearly decrease from its current value to 1 within a set time window.

[0143] (2.10) UKF Fusion Update: At the current moment, the following quantities are combined into a joint update quantity: a. Target GNSS observation; b. Observation of the speed of the left and right wheels of the encoder; c. Degraded heading observation; d. Non-holonomic constraint on lateral velocity of the vehicle body.

[0144] The observation noise is dynamically adjusted based on the current quality and robustness assessment results of each observation, and UKF measurement updates are performed to obtain the posterior state and posterior covariance. Simultaneously, the residuals and consistency health status are maintained; when the RTK residuals remain above a threshold for a period of time, a soft reset is performed, i.e., the current more reliable GNSS position and optional heading are used to reset the filter position and partial state to suppress long-term drift accumulation.

[0145] It should be noted that the "period of time" mentioned above corresponds to multiple consecutive frames of RTK residual exceeding the limit in the implementation. If the residual recovers, the count is decremented. The specific number of consecutive frames is set by parameters, and the current implementation defaults to triggering the limit after 5 consecutive frames.

[0146] The RTK residual refers to the GNSS observation residual, which is the residual between the currently selected RTK candidate observation and the filter prediction. A residual higher than a threshold indicates an RTK anomaly, filter state drift, or both. A soft reset uses absolute RTK observations to forcibly pull the filter back on track.

[0147] Reset filter position: This indicates a position reset. The position parameters x and y will always be reset, and the heading parameters will only be reset under certain conditions. The GNSS candidate observations will only be reset if the currently selected GNSS candidate observations themselves have a reliable heading.

[0148] Some states include These will all be reset to zero. It represents angular velocity.

[0149] Before each observation enters the joint update, adaptive weights for the RTK, IMU, and encoder are constructed. In Kalman filtering, the weights essentially function as scaling factors affecting the observation noise covariance. When the observation confidence decreases, the corresponding noise covariance increases, and its impact in the joint update decreases. In fact, the noise scaling mentioned earlier is essentially adjusting the scaling factor. The main update (joint update) phase combines the noise covariances corresponding to each observation to form a joint noise covariance matrix.

[0150] In some embodiments, the corresponding weights can also be adjusted based on the quality of the RTK. The following is how RTK quality is determined: For any antenna RTK, if its fix_mode and the number of satellites are respectively and The minimum number of satellites threshold is Then the RTK quality is: ; ; Among them, fix_mode represents the positioning accuracy, which is divided into centimeter-level accuracy and decimeter-level accuracy in RTK, and is obtained directly from RTK data.

[0151] The single-antenna RTK quality is: .

[0152] If both the front and rear antennas are effective, then the smaller of the two values ​​should be taken to determine the overall RTK quality: .

[0153] Determining if the antenna is valid: If the RTK message itself can be parsed normally, such as the latitude and longitude not being empty values ​​and the number of satellites meeting the minimum number of satellites, then it is considered valid.

[0154] (2.11) Visual constraint construction and updating: In some embodiments, current channel (e.g., ridge) centerline deviation measurement information output by an external vision module is also received. Before the vision measurements are fused, at least the following gating is performed: a) Time validity gating, used to remove expired visual data.

[0155] b. Confidence gating is used to eliminate visual results with low confidence.

[0156] c. Amplitude gating is used to eliminate results where the lateral or directional deviations significantly exceed the greenhouse passage range.

[0157] Heading deviation can be understood as angular deviation, which is the angular error between the current orientation of the vehicle body and the direction of the centerline of the passage.

[0158] d. Steering state gating, used to suppress visual misdirection during low-speed sharp turns or stationary turns.

[0159] The steering state corresponding to the visual measurement: the current fusion result is used to determine the steering state. First, the current linear velocity and angular velocity of the car are detected. If it is determined that the car is in a low-speed sharp turn or a stationary turn (very low speed and relatively large angular velocity), the visual measurement of this frame is directly blocked.

[0160] e. Temporal continuity gating is used to suppress visual false detections caused by single-frame abrupt changes.

[0161] For gated visual measurements, lateral and heading deviations are converted into visual attitude constraint observations based on the current fused pose reference point. Different measurement noises are set along the channel direction and the lateral direction to make the visual constraints focus more on lateral correction. Then, normalized innovative squared gating is applied again, and the result is injected into the UKF as an independent pose observation update. When GNSS degradation worsens, the visual constraint weights can be appropriately increased (by reducing the scaling factor), but at the same time, the maximum correction amount per frame is tightened to prevent visual false detections from excessively pulling the system.

[0162] Let the lateral deviation and heading deviation after visual symbol correction, camera extrinsic parameter correction, and time continuity gating be respectively... and Then the vision is at the current reference pose. , The correction amount is: ; ; If the current conditions for course correction are not met, such as insufficient forward sight distance or invalid visual heading, then it is preferable to let =0.

[0163] Reference pose , It refers to the fusion result at the current moment, that is, x, y, after the UKF master update. . It represents the lateral error gain, which determines how much the lateral deviation given by vision needs to be corrected; This represents the heading error gain, which determines how much the heading deviation given by vision needs to be corrected. This indicates the maximum horizontal correction amount per frame; This indicates the maximum heading correction in a single frame.

[0164] ; Where the left side of the formula represents the visually corrected poses obtained from the reference fused pose and the correction amount, respectively; This represents the X-coordinate of the vehicle body in a two-dimensional plane after visual constraint correction. This represents the Y-coordinate of the vehicle body in a two-dimensional plane after visual constraint correction. This indicates the vehicle heading angle after visual constraint correction.

[0165] After obtaining the above visual pose observations, the system does not directly use them as the final output pose, nor does it directly rewrite the filter state with the proportional correction. Instead, it uses them as an independent visual pose measurement input to the unscented Kalman filter.

[0166] (2.12) Output stability and network transmission: To ensure that the pose results are applicable to the controller and host computer, the system sets up an independent output stabilization layer after the UKF output, including: a. Static Hold: When the vehicle is detected to be stationary, the output position from the previous moment remains unchanged.

[0167] b. Freeze-in-place turning: When a turn in place is detected, only the heading is allowed to change, and the plane position is frozen.

[0168] c. Position jump limit: Limits the maximum position change in the output pose between two adjacent frames.

[0169] That is, the displacement of this step is obtained by subtracting the current original output position from the smoothed output position of the previous frame. If this step is too large and exceeds the threshold, the entire step is reduced to within the threshold proportionally to limit the total displacement between the two points.

[0170] d. Stabilization of Straight-Line Operation: Establish a reference straight-line coordinate system during the straight-line segment, perform scaling, limiting, or smoothing on lateral deviations, smooth the heading angle, and pull back the output heading using the track direction when necessary. For example, when RTK only provides two-dimensional position, dual-antenna heading degradation, or heading reliability is insufficient. If the platform is in a stationary hold or stationary turn hold state, this pullback is not performed to avoid misjudging turning or stopping phases as straight-line heading.

[0171] When sending data to the external network, the system uses an independent thread to send data, retaining only the latest frame to avoid accumulating old frames; and it can perform light prediction and light smoothing of pose results based on velocity and angular velocity to reduce the lag caused by communication and scheduling.

[0172] (3) Path planning and navigation (using pose): (3.1) Host computer (PC): The onboard industrial control computer responsible for fusion computing provides the real-time pose, and the PC host computer software acquires the pose: a. Real-time map display: The host computer map editing software receives the real-time fused pose and combines it with the coordinates of the base station to display the robot's position in the grid map in real time. Users can intuitively see the vehicle's current position, orientation, and path execution status in real time.

[0173] b. Path editing: In the map interface, a series of target points containing pose information can be set. These target points are not only two-dimensional coordinate points, but can also contain the target orientation, so they are essentially road sign pose nodes.

[0174] c. Path Relationship Organization: After setting the target points, specific work routes are formed by defining the path relationships between points. For example, from the starting point to the entrance of a certain row of ridges, then proceeding along the row, and turning into the next route after reaching the destination.

[0175] d. Task queue generation: These paths will be organized into a task sequence, which will then be sent by the host computer to the vehicle motion controller.

[0176] (3.2) Motion controller: Autonomous control is achieved based on the path and real-time pose. The motion controller of the mobile platform is an intermediate layer connecting the navigation and positioning results with the underlying motor execution.

[0177] a. Input information: The motion controller receives two types of core inputs: real-time pose (x, y, θ) from the industrial computer and task queues from the host computer.

[0178] b. Path tracking: The motion controller performs path tracking based on the deviation between the current position and the planned path. Its main objective is to ensure that the vehicle travels along the predetermined route and meets attitude requirements at key points.

[0179] c. Online Speed ​​Planning: Building upon path tracking, the motion controller further performs online speed planning. It generates reasonable linear and angular velocities for the current moment based on factors such as path curvature, turning requirements, current deviation, and distance to the target point.

[0180] d. Generation of target speeds for left and right wheels: Since the chassis has a two-wheel differential structure, the motion controller must ultimately convert the path tracking and speed planning results into target speeds for the left and right wheels.

[0181] (3.3) Low-level execution: servo drive, speed closed loop and feedback.

[0182] After the motion controller generates the target speeds for the left and right wheels, it enters the execution layer.

[0183] a. Control command issuance: The target speeds of the left and right wheels are written to the servo driver via the CAN bus interface.

[0184] b. Servo Drive and Servo Motor Execution: The servo driver drives the servo motor to make the left and right wheels reach the target speed. The underlying system uses typical speed closed-loop control, so the motor does not simply "turn open-loop according to the command", but continuously compares the target speed with the actual feedback speed.

[0185] c. Speed ​​Feedback: The servo system feeds back the actual wheel speed. This feedback is used for bottom-level closed-loop correction and can also flow back to the upper levels to form encoder and odometer information, thus forming the vehicle's execution feedback chain.

[0186] (4) Closed-loop operation logic of the entire system: Connecting the above parts together, we obtain the main closed loop of the entire navigation system during operation: Sensor data acquisition → Data time synchronization → IMU / kinematic prediction → Construction of RTK / vision / encoder observation → Quality gating and health monitoring → UKF update → Output fused pose → Host computer display and task issuance / controller path tracking → Generation of left and right wheel target speeds → Servo execution → Encoder feedback → Then enter the next round of fusion and control.

[0187] In summary, this embodiment does not use single GNSS results indefinitely. Instead, it constructs multiple candidate observations for dual-antenna RTK, and performs degradation discrimination and observation selection based on baseline length, heading consistency, track consistency, single-antenna jump, and normalized innovation square. When the heading degrades with dual antennas, the heading is not abandoned directly. Instead, the IMU and encoder are fused to generate degraded heading observations to maintain attitude continuity. Under conditions such as slippage, stationary conditions, start-up, and in-place turning, encoder constraints, nonholonomic constraints, and GNSS position observation noise are adaptively adjusted to suppress false displacements and erroneous constraints. The channel centerline deviation measurement output by the external vision module is converted into asynchronous pose constraints and fused after being gated by time, confidence level, amplitude, temporal continuity, and NIS.

[0188] As shown in Table 1, based on greenhouse mobile platforms (such as...) Figure 4 The proposed fusion pose estimation and navigation method was applied to enable the platform to conduct autonomous navigation tests between rows in a real greenhouse. Test error recording points were marked along the center of the rows, and the lateral and heading deviations of the platform at each point were recorded. The test data shows that the platform achieved stable closed-loop tracking, with moderate fluctuations in lateral and heading errors at each measurement point. The average lateral error of the system along the mission path was approximately 0.06m (RMSE: 0.074m), and the average heading error was 3.35° (RMSE: 3.79°). The maximum lateral error occurred at point P7, which may be due to vehicle vibration caused by local uneven ground, leading to a slight control deviation.

[0189] Table 1. Statistics on the overall error level of each measurement point along the mission path.

[0190] The technical features of the above embodiments can be combined in any way. For the sake of brevity, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.

[0191] This document uses specific examples to illustrate the principles and implementation methods of this application. The descriptions of the above embodiments are only for the purpose of helping to understand the methods and core ideas of this application. Furthermore, those skilled in the art will recognize that, based on the ideas of this application, there will be changes in the specific implementation methods and application scope. Therefore, the content of this specification should not be construed as a limitation of this application.

Claims

1. A multi-source fusion pose estimation method for a greenhouse mobile platform based on candidate observation gating, characterized in that, include: Acquire the current frame sensing data of the mobile platform; the sensing data includes measurement data from the front and rear dual-antenna RTK, IMU, and encoder; Based on the RTK measurement data from the front and rear dual antennas, a preset detection of multiple candidate observations is performed to obtain the detection results. Among them, the multiple candidate observations include the front antenna's individual position observation, the rear antenna's individual position observation, the dual antennas' joint position observation, and the dual antennas' joint position and heading observation. The detection results include the target GNSS observation and whether the dual antenna heading has degraded. The target GNSS observation is the selected candidate observation. If the dual-antenna heading degrades, the IMU measurement data and encoder measurement data are fused to generate a degraded heading observation; The position observations from the target GNSS observations, the heading observations or degraded heading observations from the target GNSS observations, and the left and right wheel speed observations from the encoder are packaged into a joint observation vector, and a corresponding joint observation noise covariance matrix is ​​constructed; the left and right wheel speed observations from the encoder are determined based on the encoder's measurement data; Based on the joint observation vector and the joint observation noise covariance matrix, an unscented Kalman filter update is performed to obtain the first pose estimate; If the current frame perception data also includes visual measurement data, then the visual measurement data detected by gating will be used as an independent pose observation, and the first pose estimation will be updated by an unscented Kalman filter once to obtain the second pose estimation; the visual measurement data includes the lateral deviation and heading deviation of the mobile platform relative to the centerline of the channel.

2. The multi-source fusion pose estimation method for a greenhouse mobile platform based on candidate observation gating as described in claim 1, characterized in that, The preset detection includes preliminary geometric consistency detection and subsequent comprehensive selection; The preliminary geometric consistency detection includes: dual-antenna baseline length detection, dual-antenna heading consistency with the current filtered heading consistency detection, dual-antenna heading consistency with the GNSS track direction consistency detection, and single-antenna jump detection; among which... The dual-antenna baseline length detection includes: comparing the current baseline length with the nominal baseline length to obtain the degree of deviation; when the degree of deviation exceeds the hard tolerance threshold, the dual-antenna joint position observation and the dual-antenna joint position and heading observation are determined to be unusable and discarded; when the deviation is between the soft tolerance threshold and the hard tolerance threshold, the observation noise corresponding to the dual-antenna joint position observation and the dual-antenna joint position and heading observation is amplified. The consistency detection between the dual-antenna heading and the current filtered heading includes: comparing the current dual-antenna heading with the current filtered heading to obtain a first heading consistency deviation; when the first heading consistency deviation exceeds a first consistency threshold, amplifying the observation noise corresponding to the dual-antenna heading. The dual-antenna heading and GNSS track direction consistency detection includes: comparing the current dual-antenna heading with the track direction corresponding to the first preset duration GNSS displacement to obtain a second heading consistency deviation; when the second heading consistency deviation exceeds a second consistency threshold, it is determined that the dual-antenna heading has degraded. The single-antenna jump detection includes: for observation of the front antenna's single position or observation of the rear antenna's single position, when the position change of two adjacent frames exceeds the jump threshold within the allowed jump detection period, a first score penalty of a second preset duration is continuously applied to the comprehensive cost calculation of the corresponding candidate observation. The subsequent comprehensive selection includes: based on the previous geometric consistency detection, calculating the normalized innovation square for the remaining candidate observation set, and further calculating the comprehensive cost to obtain the corresponding comprehensive cost; then, based on the preset selection strategy, selecting the target GNSS observation; the remaining candidate observation set is all candidate observations that have not been discarded or degraded.

3. The multi-source fusion pose estimation method for a greenhouse mobile platform based on candidate observation gating according to claim 2, characterized in that, The comprehensive cost includes normalized innovation squared divided by the candidate observation dimension, mode switching penalty, mode bias penalty, jump penalty, and online degradation penalty; the mode switching penalty is the cost imposed when the current candidate observation is not the same as the previously selected candidate observation; the mode bias penalty is the prior preference cost for different candidate observations; the jump penalty is the first score penalty; the online degradation penalty is the penalty cost added to candidate observations that depend on the antenna with the relatively poor score, based on the online degradation score of the front and rear antennas within a third preset time period.

4. The multi-source fusion pose estimation method for a greenhouse mobile platform based on candidate observation gating according to claim 2, characterized in that, The preset selection strategy includes: Check if there are any candidate observations in the remaining candidate observation set that are of the same type as the previously selected candidate observation and are gated by NIS. If so, identify them as the target GNSS observation. Otherwise, priority is given to candidate observations that are gated by NIS and have the lowest overall cost; If none of the candidate observations pass the NIS gating, the candidate observation containing only the location with the lowest overall cost will be selected first.

5. The multi-source fusion pose estimation method for a greenhouse mobile platform based on candidate observation gating according to claim 1, characterized in that, The fusion of IMU measurement data and encoder measurement data to generate degraded heading observations specifically includes: Calculate the IMU angular velocity and the encoder angular velocity based on the IMU measurement data and the encoder measurement data, respectively. Based on the noise variance of the IMU and the encoder, the fusion weights of the IMU azimuth angular velocity and the encoder azimuth angular velocity are determined respectively, and the IMU azimuth angular velocity and the encoder azimuth angular velocity are further fused to obtain the degraded azimuth angular velocity. The degraded heading angular velocity is integrated to obtain the degraded heading observation.

6. The multi-source fusion pose estimation method for a greenhouse mobile platform based on candidate observation gating according to claim 5, characterized in that, When the dual-antenna heading is within the degradation window, the process noise covariance and observation noise covariance of the degradation heading are adaptively updated based on the vehicle's motion state, specifically including: When an encoder consistency alarm, drift alarm, or RTK residual mean exceeds the threshold is detected, the observation noise covariance of the degraded heading is increased to reduce the fusion weight of the degraded heading observations. When the speed exceeds the first preset threshold, the observation noise covariance of the degraded course is reduced to improve the fusion weight of the degraded course observations; When a straight-ahead condition is detected, the process noise covariance of the degraded heading is reduced to decrease the rate of change of the degraded heading observation and suppress integral drift.

7. The multi-source fusion pose estimation method for a greenhouse mobile platform based on candidate observation gating according to claim 1, characterized in that, Also includes: A nonholonomic constraint with zero lateral velocity is introduced as a virtual observation, and added to the joint observation vector; When wheel slip is detected, the observation noise covariance corresponding to the encoder measurement data is increased, and the observation noise covariance corresponding to the nonholonomic constraint is also increased.

8. The multi-source fusion pose estimation method for a greenhouse mobile platform based on candidate observation gating according to claim 1, characterized in that, The gating detection of the visual measurement data includes one or more of the following: time validity gating, confidence gating, amplitude gating, turning state gating, and time continuity gating. In the asynchronous unscented Kalman filter update, the observation noise covariances corresponding to the lateral deviation and heading deviation of the visual measurement data are different; wherein, the observation noise covariance of the lateral deviation is smaller than the observation noise covariance of the heading deviation; and, when dual-antenna heading degradation is determined, the observation noise covariance of the visual measurement data is reduced.

9. The multi-source fusion pose estimation method for a greenhouse mobile platform based on candidate observation gating according to claim 1, characterized in that, Also includes: When the mobile platform is detected to be stationary, the observation noise covariance corresponding to the GNSS position observation in the joint observation vector is amplified. When the mobile platform is detected to start moving from rest, the amplification of GNSS position observation noise is gradually reduced to a normal value within a preset time window; When a stationary turning condition is detected, the observation noise covariance corresponding to the GNSS position observation is amplified, while the output position remains unchanged.

10. A multi-source fusion navigation method for a greenhouse mobile platform based on candidate observation gating, characterized in that, include: The real-time pose of the mobile platform itself is determined by the multi-source fusion pose estimation method for greenhouse mobile platforms based on candidate observation gating as described in any one of claims 1 to 9. Path tracking is performed based on the deviation between the mobile platform's current pose and the planned path.

Images