AUV accurate positioning method based on intelligent noise estimation and multi-state decision
By employing an adaptive unscented Kalman filter algorithm and a multi-state decision-making mechanism, the problem of low positioning accuracy caused by GNSS signal obstruction in densely planted orchards was solved, achieving high-precision and stable positioning of the unmanned operation platform and improving operational efficiency and safety.
Patent Information
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- SOUTH CHINA AGRICULTURAL UNIVERSITY
- Filing Date
- 2026-01-21
- Publication Date
- 2026-04-17
AI Technical Summary
In densely planted orchard environments, GNSS signals are blocked, resulting in low positioning accuracy, which affects the path planning and motion control of unmanned operation platforms, and causes positioning information jumps, drifts, and security issues.
A method based on intelligent noise estimation and multi-state decision-making is adopted. The observation noise covariance is dynamically adjusted through an adaptive unscented Kalman filter algorithm to reduce the weight of outlier data. Combined with the multi-state decision-making mechanism, adaptive fusion of GNSS and IMU is achieved, thereby enhancing the robustness and accuracy of the positioning system.
It achieves continuous and stable high-precision positioning in complex environments, improving the operational efficiency and safety of unmanned operation platforms and avoiding positioning result drift and abnormal data contamination.
Smart Images
Figure CN121878746A_ABST
Abstract
Description
Technical Field
[0001] This invention relates to the field of equipment positioning, and in particular to a precise positioning method for AUVs based on intelligent noise estimation and multi-state decision-making. Background Technology
[0002] Fruit tree cultivation is an important part of the agricultural economy. However, most orchards, especially those in hilly areas, generally have unstructured operating environments characterized by sensitive soil, undulating terrain, and numerous gullies. Furthermore, modern orchards often employ high-density planting methods, leading to overlapping tree canopies and creating a closed canopy environment.
[0003] In such complex environments, traditional agricultural production methods heavily rely on manual labor, resulting in high labor intensity, low efficiency, and high management costs, making them unsuitable for the development needs of modern precision agriculture. Therefore, the introduction of automated and intelligent agricultural machinery has become an inevitable trend. While wheeled work platforms are a common solution, their inherent limitations in complex orchard terrain, such as small contact area leading to high pressure, weak obstacle-crossing and hill-climbing ability, and large turning radius resulting in poor maneuverability, have become apparent. In contrast, tracked chassis, with their low ground pressure, high traction, and superior mobility and off-road performance, are considered ideal mobile platforms for performing tasks in unstructured orchard environments, providing a reliable solution for carrying various operational modules such as spraying, harvesting, and transportation.
[0004] The core prerequisite for applying tracked chassis to automated orchard operations is achieving autonomous and precise navigation and positioning. Benefiting from the development of GNSS, IMU, and modern control theory, agricultural machinery automatic navigation technology has been widely used in open farmland environments, significantly improving the efficiency and accuracy of operations such as tilling and sowing. However, when these technologies are directly transplanted to densely planted orchard environments, new and severe technical challenges arise. The dense vegetation and overlapping canopies in orchards cause severe interference to GNSS signals, mainly manifested as signal polarization, multipath effects, and significant signal attenuation (reaching 10-30 dB). This leads to frequent jumps, drifts, and even interruptions in the positioning information output by the GNSS positioning module, resulting in a sharp decline in accuracy and reliability. Path planning and motion control based on inaccurate positioning information can easily cause vehicles to deviate from their intended trajectory, potentially leading to collisions with tree trunks, equipment damage, repetitive work, or missed tasks, severely restricting the efficiency and safety of unmanned operation platforms. Therefore, how to provide continuous, stable, and high-precision positioning information for unmanned operation platforms in environments where GNSS signals are disturbed or fail due to dense planting and canopy obstruction is a key technical challenge to overcome the current bottleneck in automated production in orchards. Summary of the Invention
[0005] In view of this, in order to solve the technical problem that existing agricultural unmanned vehicle (AUV) positioning methods have low positioning accuracy in densely planted and obstructed environments because they do not consider abnormal fluctuations in GNSS data, this invention proposes an AUV precise positioning method based on intelligent noise estimation and multi-state decision-making. This method includes the following steps: First, relevant GNSS data and IMU attitude data are acquired. Based on the UKF framework, an intelligent estimation mechanism for observation noise based on real-time residual diagnosis is introduced. By dynamically adjusting the observation noise covariance, the weight of abnormal or poor-quality observation data in the fusion process is adaptively reduced, thus completing the data fusion.
[0006] In some embodiments, the method also includes enabling the AUV to be matched with actual, multi-step agricultural operations to achieve precise execution of complex task sequences.
[0007] Based on the above scheme, this invention provides an AUV precise positioning method based on intelligent noise estimation and multi-state decision-making, introducing an adaptive noise estimation mechanism based on observation residual sequence covariance matching. This mechanism enables the system to have real-time self-diagnostic capabilities for GNSS signals, monitors the consistency between GNSS observations and system predictions online, and automatically reduces the weight of obstructed signals, effectively avoiding positioning result drift caused by single-point abnormal data. When a decrease in GNSS signal quality is detected, leading to an abnormal increase in the prediction-observation deviation vector, the system automatically increases the corresponding noise covariance, thereby intelligently reducing the weight of the poor observation data in the fusion update and effectively suppressing the contamination of positioning results by abnormal data. Conversely, when the signal returns to normal, its high-precision information is fully utilized to correct the cumulative drift of the IMU, thereby fundamentally enhancing the reliability, fault tolerance, and robustness of the entire positioning system in harsh environments. Attached Figure Description
[0008] Figure 1 This is a flowchart of the steps of an AUV precise positioning method based on intelligent noise estimation and multi-state decision-making according to the present invention. Figure 2 This is a specific embodiment of the present invention, showing the position noise variance response of the adaptive R to the observed residual sequence; Figure 3 This is a flowchart of the adaptive unscented Kalman filter algorithm according to a specific embodiment of the present invention; Figure 4 This is a flowchart of the controller in a specific embodiment of the present invention. 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] It should be noted that, for ease of description, only the parts relevant to the invention are shown in the accompanying drawings. Unless otherwise specified, the embodiments and features described in this application can be combined with each other.
[0011] It should be understood that the terms "system," "apparatus," "unit," and / or "module" used in this application are a method of distinguishing different components, elements, parts, sections, or assemblies at different levels. However, if other terms can achieve the same purpose, they may be replaced by other expressions.
[0012] As indicated in this application and claims, unless the context clearly indicates otherwise, the words "a," "an," "a," and / or "the" are not specifically singular and may include the plural. Generally, the terms "comprising" and "including" only indicate the inclusion of expressly identified steps and elements, which do not constitute an exclusive list, and the method or apparatus may also include other steps or elements. An element defined by the phrase "comprising an..." does not exclude the presence of other identical elements in the process, method, product, or apparatus that includes the element.
[0013] In the description of the embodiments of this application, "a plurality of" refers to two or more. The terms "first" and "second" are used for descriptive purposes only and should not be construed as indicating or implying relative importance or implicitly specifying the number of indicated technical features. Thus, a feature defined as "first" or "second" may explicitly or implicitly include one or more of that feature.
[0014] Furthermore, flowcharts are used in this application to illustrate the operations performed by the system according to embodiments of this application. It should be understood that the preceding or following operations are not necessarily performed precisely in sequence. Instead, the steps can be processed in reverse order or simultaneously. Additionally, other operations can be added to these processes, or one or more steps can be removed from them.
[0015] Reference Figure 1 This is a flowchart illustrating an optional example of the AUV precise positioning method based on intelligent noise estimation and multi-state decision-making proposed in this invention. This method can be applied to computer devices, and the precise positioning method proposed in this embodiment may include, but is not limited to, the following steps: Step S1: Obtain initial GNSS data and attitude information; Step S2: Dynamically adjust the trust weights of GNSS observations based on the adaptive unscented Kalman filter method, and fuse the initial GNSS data and the attitude information to obtain the final GNSS data.
[0016] In some feasible embodiments, step S2 specifically includes: Because the digital models of GNSS and IMU are often nonlinear to varying degrees, especially when using the three-axis acceleration and angular velocity measured by the IMU to infer position, velocity, and attitude, various nonlinear calculations are often required. Therefore, this application uses UKF with UT (Underlying Technology) for information fusion, directly transmitting the probability distribution of the state through a set of deterministically sampled Sigma points to improve positioning accuracy and robustness.
[0017] The system's state vector Defined as a 7-dimensional vector, it contains the carrier's two-dimensional position and two-dimensional velocity in the ENU coordinate system, as well as the zero bias of the IMU's three-axis accelerometer. Its specific form is: in, To convert GNSS two-dimensional position information to the ENU coordinate system, For GNSS two-dimensional velocity information, Zero bias for the IMU triaxial accelerometer.
[0018] State transition model It describes the dynamic changes in the system state and is a nonlinear function, with the specific form as follows: in, It is a state transition matrix, used to determine the state from the previous time step. and control input (Accelerometer and gyroscope data input from the IMU) Predict the state at the current moment. It is a time interval. It is process noise, and its covariance matrix is To reasonably characterize the process noise, the process noise covariance matrix was optimized based on experimental results. Set as a fixed diagonal matrix, specifically in the form of: in, It is position process noise. It is speed process noise. It is random walk noise due to acceleration bias.
[0019] The core of UKF lies in using unit time (UT) to approximate the probability distribution of states. Generating Sigma points is a crucial step, and these points are directly propagated through a nonlinear function to approximate the posterior probability distribution of the states with higher accuracy. This paper employs the Merwe Scaled Sigma Points (MSSP) strategy, introducing scaling parameters to generate a set of Sigma points that accurately capture the mean and covariance of the states, thereby improving the algorithm's accuracy and robustness. Scaling parameters... for: in, These are the adjustment parameters for MSSP. Control the distribution width of the Sigma points, setting it to 0.1. The auxiliary scaling parameter is set to -1.0. The dimension of the state vector is set to 7.
[0020] Subsequently, a Sigma point set is generated. and their corresponding mean weights Covariance weights : in This is an MSSP adjustment parameter used to incorporate higher-order moment information. For a Gaussian distribution, setting it to 2.0 is optimal.
[0021] The generated Sigma point set is directly substituted into the nonlinear process model formula (2) for propagation: By weighted summing of the propagated Sigma points, the prior state estimate is obtained. And the prior covariance matrix P{k|k-1}: Observation vector The GNSS provides position and velocity information in ENU coordinates, specifically in the following format: Since the observation vector is a subset of the state vector, the observation model is linear, and the corresponding elements can be directly extracted from the state vector through the observation matrix H: in, It is the observation matrix. It is observation noise, and its covariance matrix is ; Initial state covariance matrix Initialized as a diagonal matrix to reflect the uncertainty of the initial state, and optimized experimentally, its specific form is as follows: in, It is the initial variance of the position. It is the initial variance of velocity. It is the initial variance of the acceleration bias.
[0022] In practical applications of adaptive fusion positioning using multi-source heterogeneous sensors, the observation quality of GNSS is not static; it is affected by various factors such as satellite geometric distribution, multipath effects, and signal obstruction. Therefore, unlike traditional fixed noise models, the core of this invention lies in introducing an online noise covariance self-calibration module. This module monitors the statistical characteristics of the prediction-observation bias vector in real time, reverse-diagnoses the instantaneous reliability of the GNSS signal, and dynamically reconstructs the observation noise matrix, thereby achieving an intelligent fusion effect that suppresses inferior data and enhances superior data.
[0023] Figure 2 The response characteristics of the adaptive R to the observation residual sequence are shown. When the observation residual sequence is close to zero, the total position noise variance reaches its baseline minimum, approximately 0.8 m², reflecting the highest confidence level set by the algorithm for GNSS under ideal observation conditions (i.e., when the IMU prediction matches the GNSS measurement height). Conversely, when the magnitude of the observation residual sequence increases, this deviation is attributed to the increased measurement noise, and the estimated value of the noise variance is rapidly and non-linearly increased. Furthermore, the baseline height and curvature of the surface are jointly determined by the forgetting factor lambda in formula (17) and the smoothing factor alpha in formula (20) of the algorithm. For example, a smaller lambda or alpha makes the surface more sensitive to changes in the observation residual sequence (the surface is steeper), and vice versa. The lambda=0.98 and alpha=0.90 selected in this paper achieve a good balance between suppressing outliers and maintaining stability, and its performance is visually verified by this figure. The above mechanism effectively suppresses the excessive influence of outlier measurements on state estimation by dynamically adjusting the Kalman gain, thereby significantly enhancing the robustness of the algorithm and its performance in non-ideal measurement environments.
[0024] This invention employs a dual-mode noise processing strategy that combines static modeling of model noise with dynamic identification of observation noise. The algorithm flowchart is as follows: Figure 3 As shown.
[0025] Observation residual sequence Defined as actual GNSS observations Compared with observations based on IMU predictions The residuals directly reflect the degree of mismatch between the system model and the actual measurements, specifically in the form of: in, For the predicted state It is obtained through propagation via the nonlinear observation function h.
[0026] Ideally, if the model is accurate and the noise parameters Q and R are set correctly, the observed residual sequence... It should be a Gaussian white noise sequence with zero mean, whose theoretical covariance is: in, It is the prediction observation covariance matrix derived from the uncertainty of IMU prediction. It is the observation noise covariance caused by the uncertainty of GNSS measurements.
[0027] The core idea of adaptive adjustment is to estimate the actual covariance by statistically analyzing the actually calculated observed residual sequence. and compare it with theoretical covariance Compare. Utilize The relationship can be used to deduce the relationship with The estimate.
[0028] To estimate the covariance of the observed residual sequence online To avoid storing large amounts of historical data, this paper adopts a commonly used method based on the forgetting factor. (0 < Recursive estimation method for <= 1): in, It is the estimated value from the previous moment, the forgetting factor. By controlling for the influence of historical data on the current estimate, a value close to 1 indicates a greater emphasis on the historical average, resulting in smoother changes but slower adaptation. In this study, It was set to 0.98. Initialize to The zero matrix.
[0029] According to equation (16) and the covariance matching principle, the variance obtained from the prediction uncertainty of the IMU model can be subtracted from the total observation residual variance. The remainder should be the measurement noise of the GNSS itself. Therefore, the observation noise covariance is... It can be estimated as follows: Considering It must be positive semidefinite, and it is usually assumed that the east and north noise of GNSS are uncorrelated (i.e., (a diagonal matrix), for The correction process involves first extracting the diagonal elements, then setting any diagonal elements smaller than a preset threshold to that threshold to ensure numerical stability and reflect the physical lower limit of noise. The corrected diagonal matrix is denoted as... .
[0030] in, for The i-th diagonal element, The threshold for diagonal elements is set to 1e-6.
[0031] To prevent The residual sequence changes drastically with instantaneous fluctuations, so a smoothing factor is introduced. (0 <= < 1) To Update: Smoothing factor The speed and stability of updates were controlled; values closer to 1 indicate slower, smoother updates. In this paper... The value is set to 0.90. Through this series of steps, the filter can dynamically adjust its trust weight for GNSS observations based on the real-time GNSS signal quality, thereby achieving robust and accurate adaptive fusion with the IMU.
[0032] In the UKF framework based on intelligent noise estimation, the updated observation noise covariance matrix is embedded in the core of the measurement update process. This matrix, after exponentially weighting the real-time estimated noise through a smoothing factor, serves as a dynamic parameter in calculating the theoretical covariance matrix of the innovation. When GNSS signal obstruction leads to an increase in observation noise covariance, the system automatically reduces the Kalman gain, thereby decreasing the reliance on current GNSS observation data in state updates and placing greater trust in the IMU's predicted trajectory. Conversely, when GNSS signal quality recovers, the system correspondingly increases the observation weights, using accurate satellite positioning data to correct the IMU's cumulative drift. This dynamic weight adjustment mechanism enables the multi-source sensor fusion system to adapt to environmental changes and maintain stable and reliable high-precision positioning output in complex scenarios such as dense canopy environments.
[0033] In some feasible embodiments, it also includes: State estimation is performed based on the final GNSS data to obtain the current state information; Based on the comparison between the current status information and the target information, and combined with the preset task logic, control instructions are generated.
[0034] Specifically, considering the discrete and sequential nature of orchard operations, this invention abandons the traditional continuous path tracking paradigm and innovatively designs a multimodal control state machine based on discrete event triggering. This state machine integrates real-time sensor data processing, geodetic coordinate system calculation, state machine-driven task decision-making, and the generation and transmission of underlying motion control commands, based on the spatial relationship between the vehicle and the target point and the task sequence. It autonomously and seamlessly switches between multiple modes such as cruise approach, precise stopping, stationary operation, and in-situ turning, achieving deep matching between navigation control and agricultural processes. Its core theory and workflow are as follows: Figure 4 As shown.
[0035] Using GNSS data from adaptive high-precision positioning as system input, the system performs state estimation after parsing to determine the platform's current position and orientation. The current state is compared with the preset target state to calculate distance and heading deviations. Based on these errors and preset task logic, the system decides and plans the next action, finally translating the decision into specific control commands (speed and steering values), encoding them, and sending them to the underlying motion controller via serial port. This cycle repeats continuously at a high frequency, constantly correcting the platform's trajectory until all preset tasks are completed.
[0036] The distance between two points is calculated using the Haversine Formula, which is used to calculate the great circle distance (shortest distance) between two points on the Earth's surface. This formula maintains high numerical accuracy when dealing with small distances. Azimuth is the angle between a line of direction from the current point to the target point and true north (clockwise is positive, 0-360°): In this embodiment, the task logic is as follows: the system calculates the distance deviation and heading angle offset between the vehicle's current position and the target route point in real time as key deviation data, and drives the built-in multi-state task logic to switch automatically. When the distance is less than the threshold, it enters the "arrival waiting" state and sends a stop command. After the waiting period, if it is determined that a turn is required, it enters the "turn execution" state. At this time, the control target is switched from position tracking to heading angle tracking. The differential steering command makes the vehicle rotate to the target heading in place. The entire process is completely decided by the real-time deviation and the preset state machine logic to generate specific control commands at each moment, realizing the intelligent conversion from continuous positioning to discrete operation actions.
[0037] An AUV precise positioning system based on intelligent noise estimation and multi-state decision-making is provided for executing the AUV precise positioning method based on intelligent noise estimation and multi-state decision-making as described above, comprising: The data acquisition unit is used to execute step S1; The update unit is used to perform step S2.
[0038] The content of the above method embodiments is applicable to this system embodiment. The specific functions implemented in this system embodiment are the same as those in the above method embodiments, and the beneficial effects achieved are also the same as those achieved in the above method embodiments.
[0039] A precise positioning device for AUVs based on intelligent noise estimation and multi-state decision-making: At least one processor; At least one memory for storing at least one program; When the at least one program is executed by the at least one processor, the at least one processor implements the AUV precise positioning method based on intelligent noise estimation and multi-state decision-making as described above.
[0040] The content of the above method embodiments is applicable to the device embodiments. The specific functions implemented by the device embodiments are the same as those of the above method embodiments, and the beneficial effects achieved are also the same as those achieved by the above method embodiments.
[0041] A storage medium storing processor-executable instructions, which, when executed by a processor, are used to implement the AUV precise positioning method based on intelligent noise estimation and multi-state decision-making as described above.
[0042] The content of the above method embodiments is applicable to this storage medium embodiment. The specific functions implemented in this storage medium embodiment are the same as those in the above method embodiments, and the beneficial effects achieved are also the same as those achieved in the above method embodiments.
[0043] The above is a detailed description of the preferred embodiments of the present invention. However, the present invention is not limited to the embodiments described. Those skilled in the art can make various equivalent modifications or substitutions without departing from the spirit of the present invention. All such equivalent modifications or substitutions are included within the scope defined by the claims of this application.
Claims
1. A precise positioning method for AUVs based on intelligent noise estimation and multi-state decision-making, characterized in that, Includes the following steps: Acquire initial GNSS data and attitude information; The trust weights of GNSS observations are dynamically adjusted based on the adaptive unscented Kalman filter method, and the initial GNSS data and the attitude information are fused to obtain the final GNSS data.
2. The AUV precise positioning method based on intelligent noise estimation and multi-state decision-making according to claim 1, characterized in that, Also includes: State estimation is performed based on the final GNSS data to obtain the current state information.
3. The AUV precise positioning method based on intelligent noise estimation and multi-state decision-making according to claim 2, characterized in that, Also includes: The current status information is compared with the target information, and deviation data is generated. Based on the deviation data and the preset task logic, control commands are generated.
4. The AUV precise positioning method based on intelligent noise estimation and multi-state decision-making according to claim 2, characterized in that, The step of dynamically adjusting the trust weights of GNSS observations based on the adaptive unscented Kalman filter method and fusing the initial GNSS data and the attitude information specifically includes: Calculate the observation residual sequence based on the actual observations in the initial GNSS data and the IMU predicted observations in the attitude information; The observed residual sequence is statistically analyzed, and the covariance of the actual observed residual sequence is estimated by combining the forgetting factor. Calculate the observation noise covariance based on the actual observation residual sequence covariance and the predicted observation covariance matrix; A smoothing factor is introduced to update the observation noise covariance, resulting in the updated observation noise covariance. UKF updates are performed based on the updated observation noise covariance, and data fusion is performed based on the UKF framework.
5. The AUV precise positioning method based on intelligent noise estimation and multi-state decision-making according to claim 4, characterized in that, The estimation formula for the covariance of the actual observed residual sequence is expressed as follows: in, Indicates the first The actual observed residual sequence covariance of the step Indicates the forgetting factor, Indicates the first The actual observed residual sequence covariance of the step This represents the observed residual sequence.
6. The AUV precise positioning method based on intelligent noise estimation and multi-state decision-making according to claim 4, characterized in that, The smoothing factor introduced to update the observation noise covariance is expressed by the following formula: in, Indicates the first The observation noise covariance of the step, Represents the smoothing factor. Indicates the first The observation noise covariance of the step, This represents the corrected diagonal matrix. express The One diagonal element, This represents an estimate of the observation noise covariance, describing the uncertainty of sensor observations. This represents the threshold value for diagonal elements.
7. An AUV precise positioning system based on intelligent noise estimation and multi-state decision-making, characterized in that, An AUV precise positioning method based on intelligent noise estimation and multi-state decision-making as described in claim 1 includes: The data acquisition unit is used to acquire initial GNSS data and attitude information. The update unit dynamically adjusts the trust weights of GNSS observations based on the adaptive unscented Kalman filter method, and fuses the initial GNSS data and the attitude information to obtain the final GNSS data.