Dynamic fruit detection tracking and real-time positioning method for citrus picking robot

Abstract

This invention discloses a dynamic fruit detection, tracking, and real-time positioning method for a citrus harvesting robot, belonging to the field of agricultural robot technology. The method collects motion data of citrus fruits under irregular disturbances in real time through a vision acquisition module and establishes an error dataset. It constructs an adaptive process noise matrix driven by a disturbance intensity index that integrates error amplitude and swing frequency, embeds it into a Kalman filter, and combines it with observation noise adaptive correction to achieve dual adjustment. It introduces a residual energy modulation factor to construct an outer closed-loop correction, reduces computational complexity through lightweight attenuation iterative optimization, and finally outputs spatial positioning results after normalization and coordinate transformation, achieving high-precision real-time positioning under continuous fruit swing.

Description

Technical Field

[0001] This invention relates to the field of agricultural robot technology, and in particular to a method for dynamic fruit detection, tracking and real-time positioning of citrus harvesting robots. Background Technology

[0002] With the continuous improvement of agricultural mechanization and intelligence, fruit recognition and positioning based on visual perception has become a core technology in automated orchard harvesting systems. In actual orchard environments, citrus fruits are affected by factors such as wind disturbance, branch elastic deformation, and external contact, often exhibiting a non-periodic, non-linear, and highly random swaying state, causing the target position to change rapidly over time. Research in the field of agricultural robotics shows that under natural wind conditions, the swaying amplitude of citrus fruits can reach several centimeters, and the frequency exhibits significant non-stationary characteristics at different times. Therefore, how to achieve continuous, stable, and high-precision trajectory prediction and spatial positioning under dynamic disturbance conditions has become a key technical problem restricting the operational efficiency and grasping success rate of citrus harvesting robots.

[0003] In existing technologies, most citrus harvesting systems locate fruits by fusing visual recognition and depth information, and combine this with trajectory prediction methods to assist the robotic arm in grasping. For example, Chinese invention patent CN118493395B discloses a fruit harvesting method based on a harvesting robot. This method uses a depth camera to acquire depth information to establish an orchard map and a robotic arm harvesting space model. After obtaining the fruit distribution coordinate set, it performs coverage path planning, establishes a kinematic model of the robotic arm and solves the inverse kinematics solution, and then optimizes the path planning into a navigation path. At the same time, it iteratively optimizes and evaluates the navigation path using evaluation indicators such as the number of collisions, and adjusts the inverse kinematics solution and updates the navigation path based on the evaluation results. The core of this technical solution lies in integrating the robotic arm harvesting posture as a constraint into the chassis path planning algorithm, and determining the optimal joint angle combination through a multi-objective loss function to achieve collision-free and highly efficient fruit harvesting. However, the positioning strategy of the above scheme relies on the assumption that the fruit's position is relatively stable. The process of obtaining the fruit's coordinate set uses single-shot or low-frequency positioning, and it does not construct a dynamic noise adaptation mechanism for the irregular swaying caused by natural wind fields. It also lacks quantitative modeling of the intensity of random disturbances and an adaptive covariance adjustment structure. When the fruit is in a continuous swaying state, the fixed-parameter filtering strategy in this scheme cannot match the changes in disturbances in real time, easily leading to trajectory prediction lag or positioning deviation, causing the robotic arm to fail to grasp the fruit due to positioning errors when approaching it.

[0004] Furthermore, while various adaptive filtering methods have been proposed in the field of Kalman filtering, such as adaptive estimation based on innovation sequences and adaptive estimation based on residual sequences, these methods are mostly designed for target tracking scenarios in aerospace, vehicle navigation, and other fields, and do not fully consider the special characteristics of wind disturbances in agricultural harvesting environments—namely, the strong coupling relationship between disturbance intensity and oscillation frequency. At the same time, existing adaptive filtering methods generally lack lightweight optimization designs for the residual correction process, making it difficult to meet real-time requirements on computationally limited embedded harvesting robot platforms.

[0005] A comprehensive analysis of the existing technologies reveals that traditional solutions primarily focus on optimizing the picking path and avoiding collisions. Their positioning models assume relatively stable or predictable target motion and fail to establish a dynamic mapping relationship between disturbance intensity and filtering parameters. Due to the lack of a residual feedback closed-loop correction mechanism and a lightweight real-time optimization structure, when the fruit is continuously swinging, the system often needs to wait for the swing amplitude to decrease before grasping it, severely reducing picking efficiency and operational continuity. Therefore, there is an urgent need for a fruit detection, tracking, and positioning method that can couple the filtering algorithm with the actual disturbance state in real time under dynamic disturbance conditions. This would fundamentally solve the technical shortcomings of traditional technologies in achieving both positioning accuracy and real-time performance in scenarios with irregular swinging motion. Summary of the Invention

[0006] The purpose of this invention is to provide a dynamic fruit detection, tracking, and real-time positioning method for citrus harvesting robots, in order to solve the technical problems in existing citrus harvesting systems where fixed filter parameters lead to increased trajectory prediction deviations under conditions of continuous and irregular fruit swaying, the lack of a dynamic mapping mechanism for disturbance intensity and noise parameters, and the lack of residual feedback closed-loop correction, which makes it difficult to balance positioning accuracy and real-time performance.

[0007] To achieve the above objectives, this invention provides a dynamic fruit detection, tracking, and real-time positioning method for a citrus harvesting robot, comprising: controlling a vision acquisition module mounted on the citrus harvesting robot to continuously acquire frames of citrus fruits in a state of irregular disturbance at a fixed sampling period; obtaining the three-dimensional spatial position vector of the fruit in the camera coordinate system through image segmentation and centroid extraction; calculating the instantaneous velocity vector and oscillation frequency based on time series data; introducing a weighted robust denoising index based on trajectory continuity to remove abnormal data points; constructing a short-time prediction error index to quantify trajectory fluctuation error and establishing an error dataset; extracting the coupling relationship between the error amplitude change rate and the oscillation frequency based on the time series of trajectory fluctuation error in the error dataset; and determining the contribution of three disturbance components—trajectory fluctuation error, the difference in trajectory fluctuation error between adjacent time moments, and the difference in oscillation frequency between adjacent time moments—based on the coupling relationship. The system contributes weights to construct a disturbance intensity index that integrates the trajectory fluctuation error, the difference in trajectory fluctuation error between adjacent time points, and the difference in oscillation frequency between adjacent time points. An adaptive process noise matrix driven by this disturbance intensity index is established, and time continuity is constrained through a recursive smoothing mechanism. The smoothed adaptive process noise matrix is ​​embedded into the state prediction and covariance propagation stages of the Kalman filter. Simultaneously, an adaptive correction matrix for observation noise is constructed based on the observation residual energy to complete dual adaptive state updates. A trajectory prediction residual vector is constructed from the updated state estimate, and an outer closed-loop re-correction is performed by introducing a modulation factor based on the residual energy. An exponential decay iterative sequence is constructed on the residual vector to perform lightweight correction with a fixed number of iterations. Finally, the position component is extracted from the corrected state estimate, and a robot-recognizable spatial positioning result is output after normalization mapping, deep fusion, and coordinate system transformation.

[0008] Compared with existing technologies, this invention has the following advantages: First, by constructing a dynamic mapping between the disturbance intensity index and the adaptive process noise matrix, this invention enables the Kalman filter covariance parameter to change in real time with the intensity of random disturbances, breaking through the limitations of traditional fixed noise parameters and significantly improving the adaptability of the filter prediction model to non-periodic and highly random disturbances. Second, this invention introduces a residual energy modulation factor to construct an outer closed-loop correction structure, which further compresses small prediction biases while maintaining the convergence stability of the Kalman principal structure, improving the fine compensation capability for high-frequency nonlinear disturbances and enhancing the continuity and fit of fruit trajectory estimation. Third, this invention compresses the computational complexity to a constant-order operation through a lightweight attenuation iterative correction mechanism, achieving a dynamic balance between accuracy and real-time performance, ensuring that the harvesting robot maintains a high-frequency positioning output capability in complex natural environments. Fourth, this invention establishes a complete data flow closed loop from the error dataset to the spatial positioning results. Through normalized mapping, deep fusion, and coordinate transformation, it achieves a stable transformation of visual observation data to the robot's base coordinate system. This enables the harvesting robot to complete high-precision positioning and path planning even when the fruit is continuously swinging, overcoming the limitation of existing technologies that require waiting for the fruit to come to rest before grasping it, significantly improving harvesting efficiency and operational continuity. Fifth, this invention uses structured modeling of irregular disturbance characteristics, uniformly quantifying and expressing trajectory fluctuation errors, swing frequency, and disturbance intensity indicators. This gives random disturbances a computable, adjustable, and feedback-able parameterized form, thereby achieving real-time matching between the filtering algorithm and real physical disturbances, significantly improving the system's stability and robustness under varying natural wind conditions. Attached Figure Description

[0009] Figure 1 This is an overall flowchart of the dynamic fruit detection, tracking, and real-time positioning method for citrus harvesting robots provided in this embodiment of the invention.

[0010] Figure 2 This is a detailed flowchart of the data acquisition and preprocessing steps provided in the embodiments of the present invention.

[0011] Figure 3 This is a detailed flowchart of the adaptive noise estimation model construction steps provided in the embodiments of the present invention.

[0012] Figure 4 This is a detailed flowchart of the Kalman filter adaptive fusion steps provided in the embodiments of the present invention.

[0013] Figure 5 This is a detailed flowchart of the trajectory residual correction and lightweight iterative optimization steps provided in the embodiments of the present invention.

[0014] Figure 6This is a detailed flowchart of the spatial coordinate transformation and positioning output steps provided in the embodiments of the present invention. Detailed Implementation

[0015] To make the objectives, technical solutions, and beneficial effects of this invention clearer, the invention will be further described in detail below with reference to the accompanying drawings and specific embodiments. It should be noted that, unless otherwise specified, the embodiments and technical features described in these embodiments can be combined with each other.

[0016] like Figure 1 As shown in the embodiment of the present invention, the dynamic fruit detection, tracking, and real-time positioning method for citrus harvesting robots addresses the irregular swaying of citrus fruits caused by wind disturbance, branch elastic deformation, and external contact in natural orchard environments. By refining the Kalman filter algorithm and combining it with disturbance feature adaptation optimization, high-precision real-time positioning of citrus fruits under irregular swaying conditions is achieved. The method comprises six deeply coupled core steps. The output of each step serves as the key input for the next, and the residual feedback results of subsequent steps influence the noise parameter adjustment of preceding steps, thus forming a complete closed-loop collaborative process. Specifically, step S1 establishes an error dataset through motion data acquisition and denoising preprocessing, providing a data foundation for disturbance intensity modeling in step S2; the adaptive process noise matrix constructed in step S2 is directly embedded in the Kalman filter prediction stage of step S3; the filter update result of step S3 is input into step S4 for residual evaluation and outer correction; the correction vector in step S4 undergoes lightweight iteration to compress computational complexity in step S5; and the correction state output in step S5 is finally transformed and positioned in step S6. The data flow between each step constitutes a unidirectional main link, while the residual statistics generated in step S4 can be used in reverse to fine-tune the online weighting coefficients of the disturbance intensity index in step S2, forming an indirect feedback channel across steps. The following section combines... Figures 1 to 6 Each step is explained in detail.

[0017] Step 1, Motion data acquisition and error dataset construction, such as Figure 2 As shown, in one embodiment of the present invention, the visual acquisition module uses a fixed sampling period. Continuous frame acquisition is performed on citrus fruits under irregular perturbation. Preferably, the sampling period is... The value range is 0.02s to 0.05s, corresponding to a sampling frequency of 20Hz to 50Hz. If the sampling frequency is too low, the transient characteristics of high-frequency oscillations cannot be captured; if the sampling frequency is too high, it increases the computational burden of subsequent data processing. In practical applications, the value can be selected based on the typical oscillation frequency range of citrus fruits under natural wind conditions. Generally, a selection of [missing value] is used. A sampling frequency of 30Hz can achieve a good balance between data accuracy and computational efficiency.

[0018] The two-dimensional pixel coordinates of the fruit in the camera coordinate system are obtained through image segmentation and centroid extraction, and the three-dimensional spatial position vector is calculated by combining the depth information provided by the depth sensor. ,in Indicates the sampling time sequence number. , , They represent the first The coordinates of the fruit in three spatial directions at any given time are expressed in meters (m). In a preferred embodiment of the invention, the image segmentation employs a joint segmentation strategy based on color features and depth information. First, the orange-yellow features of the citrus fruit are thresholded in the HSV color space. Then, the fruit outline boundary is further refined by combining connected component analysis of the depth map. Finally, the two-dimensional pixel coordinates are obtained by calculating the geometric centroid of the segmented region.

[0019] Calculate the instantaneous velocity vector based on time-series data acquired from continuous frames. Its expression is: ,in: Indicates the first The three-dimensional velocity components at time t, in m / s; Represents the position vector at the previous moment; This represents the time interval between two adjacent frames, measured in seconds. The formula converts a discrete position sequence into a velocity sequence using a forward differencing method, providing fundamental data support for subsequent extraction of oscillation frequencies and quantification of disturbance intensity. This is achieved by analyzing the velocity magnitude sequence... Perform a Fast Fourier Transform to extract the frequency component with the largest amplitude in the frequency domain as the oscillation frequency at the current moment. The unit is Hz. This frequency is directly related to the intensity of wind disturbance and the transmission characteristics of branch vibration, and is an important parameter for subsequent quantification of disturbance intensity. Preferably, the input window length of the Fast Fourier Transform is 64 to 128 sampling points, and the Hanning window function is used for spectral leakage suppression. The frequency resolution is the sampling frequency divided by the window length. In a natural orchard environment, the typical swing frequency range of citrus fruits is 0.5Hz to 5Hz. When the frequency exceeds this range, it can be considered as high-frequency noise interference rather than a real swing signal and should be filtered out.

[0020] To eliminate outlier data points introduced by ambient lighting fluctuations, background foliage occlusion, and image jitter, this invention introduces a weighted robust denoising index based on trajectory continuity. Specifically, a trajectory offset discriminant is constructed for the current sampling point. : ,in: Represents Euclidean L2 norm operations; The value of this coefficient is the weight of the influence of velocity change on anomaly detection. Its value ranges from 0.3 to 0.8, with a preferred value of 0.5. The larger the coefficient, the higher the sensitivity to anomaly detection of sudden velocity changes. This represents the velocity vector at the previous moment. This discriminant combines anomaly features from both position jumps and velocity abrupt changes, making it more effective at identifying false trajectory jumps caused by image processing errors or sensor noise compared to simple anomaly detection methods based solely on position differences. Through... Construct a sliding statistical window (window length preferably 15 to 30 frames), and calculate the results within the window. mean with standard deviation ,when The system determines that the current sampling point is abnormal data and removes it, thereby ensuring the continuity and physical consistency of the trajectory sequence. Preferably, the coefficient in the determination threshold is 2. If the coefficient is too small, normal large swing data may be mistakenly deleted; if the value is too large, the anomaly detection sensitivity is insufficient.

[0021] After denoising, a fluctuation error quantification model is established for the effective trajectory sequence. A short-time prediction error index is constructed to characterize the nonlinear fluctuation features of the trajectory; its expression is: ,in: Indicates the first The trajectory fluctuation error at any given time, expressed in meters (m). This represents the predicted position obtained through linear extrapolation based on the state at the previous moment. This error directly reflects the degree of deviation of the random disturbance from the linear motion model; its physical meaning lies in measuring the magnitude of the deviation of the fruit's actual trajectory from the assumption of uniform linear motion. When the fruit is at rest or in uniform motion... Approaching 0, when subjected to strong disturbances that intensify nonlinear motion. Significantly increased. For different disturbance intensities... Perform segmented statistics to form an error dataset. This dataset contains the correspondence between position fluctuation amplitude, velocity fluctuation amplitude, and frequency change trends, providing a quantitative basis for constructing the adaptive noise estimation model in subsequent steps. In a preferred embodiment of the invention, the segmented statistics are based on the disturbance intensity... Divided into weak disturbance intervals ( m), medium disturbance range ( m m) and strong disturbance range ( The error dataset is divided into three levels (m, m, m), and the mean, variance, and frequency distribution characteristics of the error are statistically analyzed within each interval. The purpose of this hierarchical strategy is to provide differentiated parameter initialization benchmarks for subsequent adaptive noise models, enabling the weight coefficients of the disturbance intensity index to obtain more reasonable initial configurations under different disturbance levels. It should be noted that the error dataset... It is not a static database, but a dynamic data structure that is continuously updated during the acquisition process. It is maintained using a first-in-first-out sliding window strategy, with the window length preferably between 200 and 500 frames, so as to ensure that the dataset always reflects the perturbation statistical characteristics within the current time period.

[0022] Step S2: Construction of the adaptive noise estimation model, such as Figure 3 As shown, in obtaining the error dataset Then, regarding trajectory fluctuation error The time series was structured and analyzed to extract the rate of change of error amplitude and oscillation frequency. The coupling relationship between them is used to characterize the non-periodic and highly random characteristics of irregular disturbances. In one embodiment of the present invention, the error change rate is constructed. and frequency change rate Two auxiliary indicators quantify the temporal evolution trend of disturbance characteristics.

[0023] To address the problem that traditional fixed noise covariance cannot reflect the rapid fluctuations in disturbance intensity over time, this invention constructs a disturbance intensity index. This is used to uniformly describe the comprehensive impact of current position error, error variation trend, and frequency variation on system uncertainty. The disturbance intensity index... The expression is: ,in: This represents the trajectory fluctuation error defined in step S1, in meters (m). This indicates the trajectory fluctuation error at the previous moment; This indicates the current oscillation frequency, in Hz. Indicates the oscillation frequency at the previous moment; , , These are the perturbation weighting coefficients, which respectively measure the contribution ratio of error amplitude, error rate of change, and frequency rate of change to the overall perturbation intensity. Preferably, The value range is from 0.3 to 0.5. The value range is from 0.2 to 0.4. The value range is from 0.1 to 0.3, and satisfies the following condition: In a preferred embodiment, take , , This set of parameters assigns the highest weight to the error amplitude itself, while also taking into account the dynamic trend of error changes and higher-order information about frequency changes. The construction of the algorithm maps nonlinear and nonstationary disturbance characteristics into a single scalar index, making subsequent noise estimation computable and real-time.

[0024] Based on disturbance intensity index An adaptive process noise estimation model is established. The adaptive process noise matrix is ​​defined. for: ,in: This represents the initial process noise covariance matrix, reflecting the fundamental uncertainty of the system under static or low-disturbance conditions. The typical range of its diagonal elements is... to The specific value is determined based on the sensor accuracy and system modeling error; The disturbance amplification factor controls the proportion of the influence of the disturbance intensity on the noise spread amplitude. Its value ranges from 0.5 to 2.0, with a preferred value of 1.0. It is an identity matrix, with dimensions identical to those of the state vector. This model allows when When it increases The synchronous increase enhances the filter's response to sudden state changes; when When decreasing convergence to This design maintains high-precision prediction even under weak disturbance scenarios. It embodies the core innovation of this invention—establishing a real-time dynamic mapping between disturbance intensity and process noise parameters, enabling the filter's uncertainty estimation to adjust synchronously with the actual physical disturbance state.

[0025] To further constrain the noise fluctuation range and avoid severe oscillations in the noise matrix caused by instantaneous error spikes, a smooth update mechanism is introduced. Perform recursive correction. Define the smoothed process noise matrix. for: ,in: This represents the smoothed noise matrix from the previous time step; This is a smoothing coefficient, with a value ranging from 0.6 to 0.9, and a preferred value of 0.8. When... When the value is large, the noise matrix has high sensitivity to tracking the current disturbance intensity but weak smoothing effect; when... When the value is small, the noise matrix exhibits good time continuity but its response speed to sudden disturbances decreases. Preferably, a larger value is used in orchard environments where wind speed changes are significant. The value is chosen to maintain a rapid response to disturbances, while taking a smaller value under relatively stable conditions. This value enhances the stability of the estimation. The recursive mechanism ensures that the noise estimation remains sensitive to disturbances while maintaining temporal continuity, avoiding filtering instability caused by frequent oscillations in the covariance parameter.

[0026] Through the disturbance intensity index With smoothing noise matrix The construction of the system achieved the goal of minimizing process noise and actual trajectory fluctuation errors. and frequency The dynamic mapping relationship between them breaks the limitations of traditional fixed noise parameters, providing a real-time parameter basis for deeply embedding the adaptive noise matrix into the Kalman filter in step S3. It is worth noting that the disturbance intensity index constructed in this invention... This invention differs fundamentally from the innovation sequence adaptive method commonly used in traditional adaptive Kalman filtering: the innovation sequence adaptive method uses the prediction residuals within the filter to indirectly infer system uncertainty, its adjustment has a certain lag, and it is prone to misjudgment in nonlinear, strongly disturbed scenarios; while the invention... Perturbation features are extracted directly from the raw motion data and constructed independently outside the filter, independent of the filter's internal state, thus exhibiting stronger independence and real-time performance. Furthermore, the frequency change rate term in the perturbation strength index... This invention introduces a field-specific parameter for the swaying characteristics of citrus fruits, which can capture the swaying mode switching caused by sudden changes in wind force. This is a capability that traditional general adaptive filtering methods do not possess.

[0027] Step S3: Kalman filter adaptive fusion, such as Figure 4 As shown, the smoothed process noise matrix is ​​obtained. Then, it is embedded into the state prediction and covariance propagation stage of the Kalman filter, so that the filter can predict the state based on the disturbance intensity index at each sampling time. Noise parameter reconstruction is completed. In one embodiment of the invention, a state vector containing position and velocity is constructed. ,in , , Consistent with the three-dimensional position components in step S1, , , For the corresponding three-dimensional velocity components, the dimension of the state vector is 6.

[0028] Based on sampling period Establish the state transition matrix This matrix is ​​a 6×6 block matrix, and its structure is as follows: ,in: It is a 3×3 identity matrix; It is a 3×3 zero matrix; The sampling period is in seconds. This state transition matrix establishes a linear coupling relationship between position and velocity based on the assumption of uniform linear motion; that is, the position at the next moment equals the current position plus the current velocity multiplied by the time interval. This approximation can well describe the local characteristics of fruit motion within a short sampling period, while the deviation caused by nonlinear perturbations is compensated for by an adaptive noise matrix and subsequent residual correction mechanism.

[0029] Perform state prediction calculations to obtain the predicted state. and predicted covariance Preferably, the propagation formula for predicting covariance is: , in: This represents the posterior covariance matrix of the previous time step, reflecting the uncertainty of the state estimate at the previous time step. This represents the transpose of the state transition matrix; This is the noise matrix of the smoothed adaptive process obtained in step S2. The core of this formula lies in making the increase in covariance directly subject to the disturbance intensity. Regulation: When the fruit is more severely affected by wind and branch swaying, Enlargement leads to Increase Synchronous expansion improves the filter's sensitivity and tolerance to state changes; as the disturbance weakens, As the covariance converges, the filter returns to its high-precision prediction mode. This mechanism allows the filter to automatically switch between sensitive response and high-precision convergence based on changes in the actual disturbance environment.

[0030] It should be further explained that the state prediction process involves predicting the state. The calculation follows the standard linear state transition model. ,in This is the posterior state estimate for the previous time step. The linear prediction model assumes that the fruit moves at approximately a constant speed over short time intervals; this assumption is relevant to the sampling period. This is reasonable under shorter conditions (e.g., 0.033s). For nonlinear motion components that exceed the range described by the linear model, this invention uses a dynamic amplification mechanism of the adaptive process noise matrix and a subsequent residual feedback correction mechanism for joint compensation, rather than using the more computationally expensive extended Kalman filter or unscented Kalman filter method. This design choice is based on the consideration of lightweight real-time requirements.

[0031] To further enhance the adaptability to observed anomalies, this invention constructs observation residuals. Adaptive correction matrix for observation noise The dynamic coupling relationship between them. The observation residual is defined as: ,in: This represents the three-dimensional observation vector output by the vision acquisition module. ; The observation matrix is ​​used to extract the position component from the state vector, and its form is: Constructing an observation noise correction matrix based on residual energy: ,in: Let represent the covariance matrix of the fundamental observation noise, with typical values ​​for its diagonal elements ranging from . to The specific value depends on the measurement accuracy of the depth sensor; This is the residual amplification factor, with a value ranging from 0.01 to 0.1, and preferably 0.05; This is the residual outer product matrix, used to quantify the current observation uncertainty. This construction makes it easier to adjust the observation error as it increases. Automatic expansion reduces the Kalman gain's confidence in anomalous observations, preventing state estimation oscillations caused by transient disturbances. In a preferred embodiment of the invention, when the residual energy... When the value exceeds a preset threshold (preferably 5 times the variance of the basic observation noise), increase the value appropriately. The value of is chosen to further suppress interference from anomalous observations.

[0032] Calculate the Kalman gain based on this. And complete the state update. The formula for calculating the Kalman gain is: The state update formula is: The covariance update formula is: Through the process noise matrix With observation noise matrix The dual adaptive adjustment mechanism enables the filtering algorithm to maintain stable convergence characteristics under non-periodic and highly random disturbances, providing a high-precision prediction benchmark for the trajectory residual correction term introduced in step S4. In one embodiment of the invention, the synergistic effect of the dual adaptive adjustment mechanism can be understood through the following physical explanation: process noise matrix The adaptive adjustment acts on the filter's prediction phase, and the control system's tolerance for model uncertainty, while the observation noise matrix... The adaptive adjustment operates during the filter update phase, controlling the system's level of trust in the observed data. This synergy allows the filter to achieve a dynamic balance between trusting the model predictions and trusting the observed data. When disturbances increase, constraints in both dimensions are simultaneously relaxed to prevent filter divergence; when disturbances decrease, constraints are simultaneously tightened to improve estimation accuracy. This dual-channel adaptive strategy exhibits greater robustness and adaptability compared to traditional schemes that only adjust a single noise matrix.

[0033] Step S4: Trajectory residual correction and outer loop closure correction as follows Figure 5 As shown, in the process of completing the adaptive noise matrix With observation noise matrix The dual adjustment is used to obtain the updated state estimate. Subsequently, due to the influence of high-frequency nonlinear disturbances, a small but persistent prediction error may still exist. Although the individual values ​​of this error are small, they will gradually accumulate during continuous sampling and affect the end-point positioning accuracy. To further reduce this residual error, this invention introduces a trajectory residual correction term to perform a secondary correction on the structural deviation between the predicted trajectory and the actual motion trajectory.

[0034] First, construct the trajectory prediction residual vector. This is used to characterize the true deviation between the filtered estimated position and the observed position. , in: The three-dimensional observation vector output by the vision acquisition module; To estimate from the state The three-dimensional position components extracted from it. It should be noted that this residual is different from the observation residual in step S3. --The observation residuals are established before the Kalman prediction update and are used to drive the standard Kalman gain calculation, while the trajectory prediction residuals are established after the Kalman update and are specifically used to characterize the systematic errors that remain after the filter's internal dynamic adjustments. This distinction allows the present invention to further improve estimation accuracy through additional outer corrections without compromising the optimality of the Kalman master structure.

[0035] To avoid the excessive amplification effect of transient disturbance spikes on the correction, a residual energy modulation factor is introduced. Its definition is: , in: The Euclidean second norm represents the trajectory prediction residual vector; This is a stability coefficient, ranging from 0.001 to 0.01, with a preferred value of 0.005, used to suppress numerical oscillations caused by the denominator approaching zero when the residual is extremely small. The design of this modulation factor embodies an adaptive correction strategy: when the residual is small ( ), As the residual approaches zero, the correction amount automatically decreases to near zero, avoiding unnecessary disturbance to already sufficiently accurate estimates; when the residual increases ( ), As the error gradually approaches 1, the correction strength increases, effectively compensating for estimates with large deviations. This adaptive adjustment mechanism based on residual energy ensures the robustness and gradualness of the correction process.

[0036] Based on this, a residual feedback correction vector is constructed. And then further revise the state estimate: , in: To correct the gain coefficient and control the overall correction strength, the value ranges from 0.1 to 0.5, with a preferred value of 0.3. If the coefficient is too large, it may cause overcorrection and induce estimation oscillations; if it is too small, the correction effect will be insignificant. This is a mapping matrix from position to state space, used to extend the three-dimensional position residual to a state space containing velocity components, and its form is: That is, the position residual is directly mapped to the position component of the state vector, and at the same time... The scaling factor is mapped to the velocity component, thereby achieving synchronous compensation of position and velocity. Superimposed on the current state estimate This reduces the hysteresis effect caused by irregular oscillations. The specific superposition method is as follows: ,in This represents the state estimate after residual feedback correction. This residual feedback mechanism forms an outer closed-loop correction without altering the Kalman filter master structure, ensuring the filtered estimation results maintain a higher degree of fit to nonlinear disturbances. It is particularly important to emphasize that the relationship between residual feedback correction and the Kalman filter master structure in this invention is a series addition rather than a substitution relationship: the Kalman filter master structure handles the main uncertainties in the system dynamics, providing the basic state estimate; the residual feedback correction focuses on compressing the small systematic deviations remaining after the Kalman update. This hierarchical correction architecture design allows the two mechanisms to perform their respective functions without interference, avoiding the parameter coupling and tuning difficulties that might result from concentrating all correction tasks on a single filter. Preferably, in the correction gain coefficient... In selecting the data, one can base it on the error dataset. The historical residual statistical characteristics are fine-tuned online, and the value is appropriately increased when the historical residual mean is consistently high. To strengthen the correction, the correction should be appropriately reduced when the historical residual mean is consistently low. To avoid over-correction.

[0037] Step S5: Lightweight Iterative Correction and Optimization The residual feedback correction vector is obtained in step S4. Subsequently, performing high-precision correction of the complete state dimension at each sampling moment would lead to frequent superposition of matrix multiplication and vector updates, increasing the computational burden and affecting the real-time response capability of the harvesting robot. In one embodiment of the present invention, for a system with a sampling frequency of 30Hz, the available computation time per frame is approximately 33ms, while complete matrix operations may occupy a large proportion of this time, leaving insufficient computation time for subsequent coordinate transformation and motion control. To reduce computational complexity, the present invention introduces a lightweight iterative correction mechanism, which compresses the scale of a single computation while maintaining correction accuracy by constructing a residual decay iterative sequence.

[0038] First, the trajectory prediction residual vector is... Construct a fast decay update model and convert it into iterative residuals. Its recursive formula is: , in: Indicates the number of iterations. ; This is the initial residual, i.e., the trajectory prediction residual vector calculated in step S4; This is the residual decay coefficient, with a value ranging from 0.3 to 0.6, preferably 0.5. This decay coefficient determines the proportion of residual reduction in each iteration. In the first iteration, the residual is 50% of the initial residual, while in the second iteration, the residual is only 25% of the initial residual. This recursive mechanism rapidly compresses higher-order residual components through exponential decay, concentrating the main corrections in the first iteration. Preferably, The value should not be too large (e.g., exceeding 0.7), otherwise higher-order iterations will still carry a large residual, requiring more iterations to converge, thus losing the meaning of lightweighting; The value should not be too small (e.g., below 0.2), otherwise the contribution of subsequent iterations to correction will be too weak, equivalent to performing only a single correction.

[0039] A lightweight state correction quantity is constructed based on the decayed iterative residual. Its expression is: ,in: , and All are consistent with the definitions in step S4. By updating the state only on the results of the first two iterations, the lightweight corrected state estimate is obtained. Its update format is as follows: This structure limits the number of iterations to a fixed value of two, thereby compressing the computational complexity from a dynamically increasing form related to the residual magnitude to a constant-order computational cost. Specifically, each iteration only requires one vector-scalar multiplication (attenuation coefficient multiplied by the residual vector) and one matrix-vector multiplication (mapping matrix multiplied by the attenuation residual), totaling four basic vector operations. Compared to the traditional iterative Kalman filtering, which requires repeated full gain calculations and covariance updates, the computational cost is significantly reduced. It decays rapidly with iteration, when At that point, the correction amount of the third iteration was only 12.5% ​​of the initial correction amount, and its contribution to the estimation accuracy was negligible. Therefore, two iterations were sufficient to cover the main error components. In actual testing, the results of one embodiment of the present invention show that the correction effect of two iterations reaches more than 95% of the convergence value of the complete iteration, while the computation time is only about 30% of that of the complete iteration scheme, fully verifying the effectiveness of the lightweight strategy.

[0040] This lightweight iterative algorithm does not change the perturbation strength index. Adaptive process noise matrix and residual modulation factor Under the premise of structural optimization, the computational burden of state correction is significantly reduced, laying an efficient and stable foundation for the normalization and spatial coordinate transformation of the corrected state data in step S6. Further analysis of the convergence of this lightweight strategy reveals that when the number of iterations approaches infinity, the sum of the geometric series of the iterative correction amounts is... Instead, only the sum of the corrections from the first two iterations is taken as... ,when In this case, the correction amount from two iterations covers 75% of the complete convergence value. Considering that the residuals corresponding to higher-order corrections are already very small, the actual accuracy loss is far lower than the theoretical truncation error. In one embodiment of the present invention, statistical analysis of 1000 frames of continuous tracking data shows that compared with the complete iterative convergence scheme, the root mean square position error of the two-iteration scheme increases by only about 0.2 mm, while the computation time is reduced from 8.5 ms per frame to 2.8 ms per frame, fully verifying the excellent balance between accuracy and efficiency of the lightweight strategy. In addition, the design of a fixed number of iterations also brings the deterministic advantage of execution time—the computation time for each frame remains constant, and there will be no jitter in computation time due to fluctuations in the residual size. This is of great significance for timing scheduling in real-time control systems.

[0041] Step S6: Spatial coordinate transformation and positioning output, such as... Figure 6 As shown, the state estimate after lightweight iterative correction is obtained. Then, the three-dimensional position components are extracted to form a corrected position vector. This vector inherits the disturbance intensity index. Noise matrix of adjustment and adaptive process Correction and residual modulation factor The compensated dynamic characteristics thus possess high adaptability and high precision to irregular oscillations. However, the corrected position vector is still in the camera coordinate system, and the data scale may differ under different disturbance intensities, making it unsuitable as direct control input for the robot actuator. Therefore, this step requires sequentially completing three sub-processes: normalization mapping, depth information fusion, and coordinate system transformation, to convert the corrected trajectory data in the camera coordinate system into accurate spatial positioning results in the robot's base coordinate system.

[0042] To eliminate the impact of data scale differences under different disturbance intensities on subsequent control calculations, this invention constructs a position normalization mapping model for... Amplitude compression and scaling are performed. A normalized position vector is defined. for: ,in: and These represent the error datasets respectively. The minimum and maximum three-dimensional position boundary vectors within the statistical period are consistent with the trajectory fluctuation error statistics established in step S1. Preferably, the length of the statistical period is the effective data of the most recent 100 to 300 frames. This window length can cover multiple complete swing cycles of citrus fruits under natural wind fields, thereby ensuring the statistical representativeness of the normalized benchmark. This normalization process maps the spatial trajectory under dynamic disturbance conditions to a uniform scale range of 0 to 1, enhancing the numerical stability of subsequent spatial transformations and avoiding the loss of computational accuracy caused by differences in the numerical magnitudes of different coordinate axes.

[0043] Then, the depth information acquired by the visual acquisition module is integrated. By fusing normalized 2D pixel coordinates with depth information, a real spatial point in the camera coordinate system is constructed. : ,in: , These are the normalized pixel coordinate components; The inverse of the camera intrinsic parameter matrix, the camera intrinsic parameter matrix Including focal length , and optical center shift , These parameters are obtained through calibration during the system initialization phase. This represents the current frame depth value, in meters (m), directly output by the depth sensor. This formula achieves a precise mapping from the image plane to the 3D camera space, giving the corrected trajectory data true spatial geometric meaning. Preferably, when the depth value... When an abnormal jump occurs (such as exceeding the typical working distance range of the fruit by 0.3m to 2.0m), the effective depth value of the previous moment is used as a substitute to ensure the continuity of spatial mapping.

[0044] Based on this, in order to enable the robot actuator to directly access the spatial positioning results, a coordinate system transformation matrix is ​​introduced. The camera coordinate system is transformed to the robot's base coordinate system to obtain the final spatial positioning vector. : ,in: This is a 4×4 homogeneous transformation matrix from the camera coordinate system to the robot base coordinate system, containing a 3×3 rotation matrix. With 3×1 translation vector These two parameters are precisely measured using a dedicated calibration board during the robot hand-eye calibration phase. The result is a 3D spatial positioning that can be directly recognized and used by the robot, expressed in meters (m). Through normalization, depth fusion, and continuous mapping of coordinate transformation, the corrected trajectory data achieves high-precision spatial representation while maintaining high real-time performance. Preferably, the homogeneous transformation matrix... It can be expressed in block form , where the rotation matrix For an orthogonal matrix to satisfy In the actual calibration process, it is preferable to use calibration samples of nine or more points and solve the extrinsic parameter matrix using the least squares method, and control the calibration residual within 0.5 mm to ensure coordinate transformation accuracy.

[0045] In one embodiment of the present invention, the final output spatial positioning vector It can be directly used as the target pose input for the robotic arm motion controller. Since the positioning result has undergone a complete processing chain of adaptive filtering, residual correction, lightweight iteration, and coordinate transformation, its positioning accuracy and time consistency can meet the requirements of the citrus harvesting robotic arm for end-effector positioning accuracy of ±5mm and an update frequency of over 30Hz. In another embodiment of the invention, the spatial positioning result can be further coupled with the robot's inverse kinematics module to convert the target fruit position into angle commands for each joint of the robotic arm, thereby achieving complete closed-loop control from visual perception to motion execution.

[0046] In a preferred embodiment of the present invention, the single-frame execution time of the complete processing link formed by steps S1 to S6 is controlled within 15ms (measured on an ARM Cortex-A72 processor platform), which is far lower than the 33ms limit of a 30Hz sampling period, indicating that the method has the ability to run in real time on an embedded platform. The entire data stream starts from the raw image data acquired visually, and sequentially goes through denoising preprocessing, error quantization, adaptive noise modeling, Kalman filter fusion, residual feedback correction, lightweight iterative optimization, normalization mapping, and coordinate transformation, finally outputting the three-dimensional positioning result in the robot's base coordinate system. This provides positioning data for the robotic arm motion controller, which can directly drive the actuator.

[0047] The method provided by this invention enables a citrus harvesting robot to stably plan its grasping path even when the fruit is continuously swinging, overcoming the limitation of existing technologies that require waiting for the fruit to come to rest before grasping, thus significantly improving harvesting efficiency and operational continuity. From a data flow perspective, this invention establishes a complete mapping link from the raw image data acquired by the vision acquisition module to the spatial positioning result in the robot's base coordinate system. Each link in this link undergoes targeted optimization design—denoising preprocessing ensures the quality of the input data, adaptive noise modeling achieves real-time matching of filtering parameters and disturbance states, dual adaptive Kalman filtering provides a high-precision state estimation benchmark, residual feedback correction further compresses residual bias, lightweight iterative optimization ensures real-time requirements, and normalization and coordinate transformation complete the final spatial representation. The various links are connected through clear data interfaces, forming a modular and scalable technical architecture. Preferably, in actual orchard testing, when the wind speed is 2 to 5 m / s and the fruit swing amplitude reaches 3 to 8 cm, the root mean square error of the positioning method of the present invention is controlled within 3 mm, which is more than 60% lower than the traditional fixed parameter Kalman filter method. At the same time, the positioning output frequency is maintained above 30 Hz, which meets the dual requirements of real-time performance and accuracy of the picking robot.

[0048] The above embodiments are only used to illustrate the technical solutions of the present invention and are not intended to limit the present invention in any way. Although the present invention has been described in detail with reference to preferred embodiments, those skilled in the art should understand that modifications or equivalent substitutions can be made to the technical solutions of the present invention without departing from the spirit and scope of the present invention.

Claims

1. A method for dynamic fruit detection, tracking, and real-time positioning of a citrus harvesting robot, characterized in that, include: The vision acquisition module on the citrus picking robot is controlled to continuously acquire frames of citrus fruits in an irregular perturbation state at a fixed sampling period. The three-dimensional spatial position vector of the fruit in the camera coordinate system is obtained through image segmentation and centroid extraction. The instantaneous velocity vector and sway frequency are calculated based on time series data. A weighted robust denoising index based on trajectory continuity is introduced to remove abnormal data points. A short-term prediction error index is constructed to quantify trajectory fluctuation error and establish an error dataset. Based on the time series of trajectory fluctuation errors in the error dataset, the coupling relationship between the rate of change of error amplitude and the oscillation frequency is extracted. Based on the coupling relationship, the contribution weights of the three disturbance components—the trajectory fluctuation error, the difference in trajectory fluctuation errors at adjacent times, and the difference in oscillation frequency at adjacent times—are determined. A disturbance intensity index that integrates the trajectory fluctuation error, the difference in trajectory fluctuation errors at adjacent times, and the difference in oscillation frequency at adjacent times is constructed. An adaptive process noise matrix driven by the disturbance intensity index is established, and the time continuity of the adaptive process noise matrix is ​​constrained by a recursive smoothing mechanism. The smoothed adaptive process noise matrix is ​​embedded into the state prediction and covariance propagation stage of the Kalman filter. At the same time, an adaptive correction matrix for observation noise is constructed based on the observation residual energy. The state update is completed through the dual adjustment of the adaptive process noise matrix and the adaptive correction matrix for observation noise. The updated state estimate is used to construct a trajectory prediction residual vector. A modulation factor based on residual energy is introduced to adaptively adjust the correction amplitude. The state estimate is then corrected by the residual feedback correction vector in the outer closed loop. An exponentially decaying iterative sequence is constructed for the trajectory prediction residual vector, and a lightweight state correction is superimposed and updated with a fixed number of iterations to obtain the lightweight corrected state estimate. The three-dimensional position components are extracted from the state estimate after the lightweight correction. The perturbation scale difference is eliminated by the position normalization mapping. The depth information is integrated and mapped to the three-dimensional camera space through the camera intrinsic parameter inverse matrix. Then, it is transformed to the robot base coordinate system through the coordinate system transformation matrix, and the spatial positioning result of the citrus fruit is output in real time.

2. The method according to claim 1, characterized in that, The weighted robust denoising index includes a trajectory offset discriminant constructed for the current sampling point. The trajectory offset discriminant is composed of the sum of the L2 norm of the position vector difference between adjacent time points and the weighted L2 norm of the velocity vector difference. The mean and standard deviation of the trajectory offset discriminant are calculated through a sliding statistical window. When the trajectory offset discriminant exceeds the threshold of a linear combination of the mean and standard deviation, it is judged as abnormal data and removed.

3. The method according to claim 1, characterized in that, The three weighting coefficients of the disturbance intensity index range from 0.3 to 0.5, 0.2 to 0.4, and 0.1 to 0.3, respectively. The smoothing coefficient in the recursive smoothing mechanism ranges from 0.6 to 0.

9. The disturbance amplification coefficient in the adaptive process noise matrix ranges from 0.5 to 2.

0.

4. The method according to claim 1, characterized in that, The residual amplification coefficient in the adaptive correction matrix for observation noise ranges from 0.01 to 0.1, the correction gain coefficient in the residual feedback correction vector ranges from 0.1 to 0.5, and the stability coefficient in the residual energy modulation factor ranges from 0.001 to 0.

01.

5. The method according to claim 1, characterized in that, The calculation of the trajectory fluctuation error includes: using the sum of the product of the previous moment's position vector and the previous moment's velocity vector multiplied by the sampling period as the linear extrapolation predicted position, and using the L2 norm of the difference between the current moment's actual position vector and the linear extrapolation predicted position as the trajectory fluctuation error. The trajectory fluctuation error reflects the degree of deviation of the random disturbance from the linear motion model.

6. The method according to claim 1, characterized in that, The calculation of the modulation factor based on residual energy includes: using the sum of the square of the second norm of the trajectory prediction residual vector and the stability coefficient as the denominator, and the square of the second norm as the numerator. The value of the modulation factor approaches 1 as the residual energy increases and approaches 0 as the residual energy decreases.

7. The method according to claim 1, characterized in that, The construction of the exponential decay iteration sequence includes: successively decaying the trajectory prediction residual vector with a residual decay coefficient, wherein the residual decay coefficient ranges from 0.3 to 0.6, the fixed number of iterations is 2, and the state update is achieved by accumulating the lightweight state correction amount of the first 2 iterations.

8. The method according to claim 1, characterized in that, The state vector is a six-dimensional column vector containing three-dimensional position components and three-dimensional velocity components, and the state transition matrix is ​​a block matrix constructed based on the sampling period, wherein the position components and velocity components are linearly coupled through the sampling period.

9. The method according to claim 1, characterized in that, The position normalization mapping model uses the minimum and maximum three-dimensional position boundary vectors within the statistical period of the error dataset as the normalization benchmark to map the spatial trajectory under dynamic disturbance conditions to a unified scale range of zero to one.

10. The method according to claim 1, characterized in that, The coordinate system transformation matrix is ​​a homogeneous transformation matrix from the camera coordinate system to the robot base coordinate system, which includes rotation matrix and translation vector information. The depth information and normalized pixel coordinates are fused through the inverse matrix of the camera intrinsic parameter matrix to construct a three-dimensional spatial point in the camera coordinate system.

Images