A State Estimation Method Based on Adaptive Weights for Fusion of RTK and IMU

Abstract

This invention discloses an adaptive weighted RTK-IMU fusion state estimation method. Based on an adaptive Kalman filter framework, this method establishes an observable state estimation system using two RTK-GPS antennas and one IMU device. By constructing an appropriate antenna layout, the line connecting the two GPS antennas is not parallel to the acceleration vector, thus satisfying the observability condition and effectively compensating for attitude errors caused by gyroscope drift. Compared to traditional single-GPS fusion schemes, this invention achieves high-precision 3D attitude and position estimation without requiring additional attitude measurement devices such as magnetic compasses. The method introduces an adaptive covariance estimation strategy based on a sliding window to evaluate the GPS measurement noise level in real time, assigning higher weights to the fusion process only when GPS information is reliable, effectively improving system robustness. It is applicable to unmanned surface vessel navigation tasks in various aquatic environments and has broad engineering application prospects.

Description

Technical Field

[0001] This invention relates to the field of navigation and state estimation technology in autonomous surface navigation systems, and in particular to an RTK and IMU fusion method for high-precision navigation of unmanned vessels in complex environments. Background Technology

[0002] With the development of intelligent ship and unmanned system technologies, the fusion navigation method based on GNSS (Global Navigation Satellite System) and IMU (Inertial Measurement Unit) has become a key technology for unmanned surface platforms to achieve autonomous positioning and attitude estimation. Currently, RTK (Real-Time Kinematic) technology, as a high-precision positioning method, can provide centimeter-level position information; while IMU can provide continuous acceleration and angular velocity measurements at high frequencies, facilitating high dynamic estimation in a short time.

[0003] However, in practical applications, the performance of RTK systems is highly dependent on environmental conditions. Factors such as occlusion and signal multipath effects can lead to a decrease in RTK positioning accuracy or even data failure. While IMUs offer high short-term accuracy, their attitude and position estimation results are easily affected by noise and drift during long-term operation, resulting in cumulative errors. Traditional fusion navigation systems typically employ fixed weights or Kalman filtering methods based on prior covariance, which struggle to guarantee system stability and estimation accuracy when faced with dynamically changing multi-source observation quality.

[0004] Furthermore, existing research often uses a single RTK antenna fused with an IMU, which only provides limited heading estimation capabilities and makes it difficult to achieve complete three-dimensional attitude observation in the absence of magnetic field information or when GPS heading is unstable. While a dual-antenna RTK structure can theoretically enhance system observability through baseline vector constraints, it still faces technical bottlenecks such as multi-sensor error modeling and fixed fusion strategies.

[0005] Therefore, there is an urgent need for a state estimation method that integrates dual RTK and IMU, which can automatically adjust the fusion weights when the observation quality changes, improve the accuracy and robustness of attitude and position estimation, and adapt to the navigation needs of unmanned vessels in complex water environments. Summary of the Invention

[0006] To achieve the above objectives, this invention provides a state estimation method for RTK and IMU fusion based on adaptive weights. It uses an adaptive weighting mechanism to dynamically adjust the fusion ratio of multiple sensors, thereby achieving accurate and reliable navigation state estimation in various environments.

[0007] The technical solution of this invention is: a state estimation method based on adaptive weights for RTK and IMU fusion, which includes the following steps:

[0008] S1. Deploy two RTK-GPS antennas and an IMU on the unmanned vessel body, ensuring that the IMU coincides with the origin of the unmanned vessel's coordinate system, and at the same time ensuring that the line connecting the position coordinates of the two RTK-GPS antennas is not parallel to the direction of the sum of the measured inertia and gravitational acceleration.

[0009] S2. By constructing a fusion observation model of RTK-GPS and IMU data, the acceleration, angular velocity and other data in the IMU coordinate system are transformed to the inertial coordinate system using a rotation matrix. At the same time, by establishing the GPS observation equation, the RTK-GPS measurement values ​​are combined with the dynamic information of the IMU to accurately estimate the state of the target object.

[0010] S3. Initialize the system. When the unmanned vessel is stationary, obtain the baseline vector in the inertial coordinate system through the position information of the two GPS antennas. The accelerometer provides the initial acceleration. Combine the cross product of the two to form a three-dimensional vector basis in the inertial system. Establish the rotation relationship between the two coordinate systems. Orthogonalize and repair the initial rotation matrix through singular value decomposition. Finally, obtain the attitude matrix for Kalman filter initialization.

[0011] S4. After successfully initializing the attitude matrix, as the system changes and GPS measurement noise fluctuates, the system needs to make more accurate adjustments to the GPS data. In the dynamic estimation process, the residual error is calculated through the innovative sequence in Kalman filtering, and the GPS noise covariance matrix is ​​dynamically updated using the sliding window method.

[0012] S5. After obtaining the GPS noise covariance, the continuous-time state of the system is discretized to construct a state transition matrix. The process noise covariance matrix is ​​dynamically adjusted in combination with IMU measurement data to improve the system's adaptability to changes in the external environment. On this basis, the Kalman gain is calculated by the residuals of prediction and observation, and then the system state and its covariance are iteratively updated to finally achieve accurate estimation and output of state variables such as attitude, position, and velocity.

[0013] Compared with the prior art, the present invention has the following beneficial effects:

[0014] 1. This invention, by fusing dual RTK-GPS and IMU data and employing an adaptive weighting mechanism to dynamically adjust the sensor fusion ratio, achieves high-precision and robust navigation and positioning under various environmental conditions. Compared to traditional single GPS or IMU fusion schemes, this invention effectively addresses issues such as RTK signal instability and IMU drift, improving system stability in complex environments.

[0015] 2. This invention combines the IMU noise dynamic estimation algorithm in EKF-LIO to optimize the covariance matrix of IMU measurements. By innovating and optimizing the IMU residuals, this invention can effectively suppress noise and drift in IMU measurements, ensuring the stability and accuracy of the system in complex and dynamic environments. Attached Figure Description

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

[0017] Figure 1 This is a flowchart of the adaptive weighted RTK and IMU fusion state estimation method according to an embodiment of the present invention. Detailed Implementation

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

[0019] In one specific embodiment, such as Figure 1 As shown, this invention provides a state estimation method based on adaptive weights for RTK and IMU fusion, comprising the following steps:

[0020] The first step is to deploy two RTK-GPS antennas and an IMU on the unmanned vessel. Ensure that the line connecting the position coordinates of the two RTK-GPS antennas is not parallel to the direction of the sum of the measured inertia and gravitational acceleration. The two antennas should be distributed in a suitable position on the hull as much as possible, ensuring that the distance between them is large enough to provide high-precision positioning data through differential positioning. At the same time, ensure that the IMU coincides with the origin of the unmanned vessel's coordinate system, so that the IMU coordinate system replaces the hull coordinate system.

[0021] The second step is to construct a state observation model, establish observation equations using RTK-GPS measurements and IMU data, and use a rotation matrix to transform sensor data in the IMU coordinate system to the inertial coordinate system.

[0022] Specifically: First, let the coordinates of the IMU coordinate system in the inertial coordinate system be... , The output of GPS measurements can be used to obtain the GPS measurement model: .

[0023] in, This represents the rotation matrix from the IMU coordinate system to the inertial coordinate system. This represents the position of the GPS antenna in the IMU coordinate system and is a constant vector.

[0024] Then, the state vector of the unmanned vessel is defined as: .

[0025] in Represents the attitude of the unmanned vessel; it is the vector part of the quaternion. Indicates the location of the unmanned vessel; Indicates the speed of the unmanned vessel; This indicates the bias value of the gyroscope.

[0026] The observation model is set as follows: .

[0027] in , , This represents the GPS measurement noise, and its covariance is... Since the observation model function is nonlinear, it needs to be linearized in Kalman filtering. The core of the linearization process is to find the partial derivatives of the observation function with respect to the state variables. Considering the small changes in the quaternion estimates, the observation matrix can be derived: .

[0028] in .

[0029] Deterministic errors (including scaling factor and bias) can be eliminated through the sensor's internal processor or an external calibration process, assuming these errors in the accelerometer have already been compensated for. Let ω represent the angular velocity of the unmanned surface vessel. Then, the relationship between the time derivative of the quaternion and the angular velocity is as follows: .

[0030] The relationship between angular rate and the measured rate of the gyroscope is as follows: .

[0031] in, It's the output of the gyroscope. It is the gyroscope bias vector. It is the angular random walk noise of the gyroscope rate.

[0032] The time derivative of the gyroscope bias is traditionally modeled using a random walk model. .

[0033] in, It is rate random walk noise.

[0034] The output equation of the accelerometer is: .

[0035] The continuous-time dynamics of the entire system are represented by the following state equations: .

[0036] Where, vector ,vector .

[0037] In order to use the extended Kalman filter, it is necessary to modify the nonlinear function. Linearization yields a linearized model of the continuous system: .

[0038] The third step, after establishing the state estimation model, is to initialize the system. When the unmanned vessel is stationary, the baseline vector in the inertial coordinate system is obtained through the position information of the two GPS antennas, the initial acceleration is provided by the accelerometer, and the cross product of these two accelerations forms a three-dimensional vector basis in the inertial frame. The rotation relationship between the two coordinate systems is established, and the initial rotation matrix is ​​orthogonally repaired through singular value decomposition. Finally, the attitude matrix that can be used for Kalman filter initialization is obtained.

[0039] Specifically: when the unmanned vessel is initially stationary, the GPS receiver provides the absolute position information of the two antennas in the inertial coordinate system; the accelerometer in the IMU provides the initial acceleration reading under the action of gravity.

[0040] A three-dimensional vector basis is constructed by combining the baseline vector between the two GPS antennas (i.e., the coordinate difference between the two antennas) with the acceleration vector and their cross product, forming three sets of linearly independent vectors, to represent the spatial configuration in the inertial coordinate system. Simultaneously, based on the unmanned surface vessel (USV) structure, the directions of the GPS baseline and initial acceleration are known in the USV coordinate system (IMU coordinate system). Therefore, a three-dimensional basis in the USV coordinate system can also be constructed.

[0041] By utilizing the relationship between these two sets of vector bases, the initial rotation transformation of the unmanned surface vessel's coordinate system relative to the inertial coordinate system can be solved. To eliminate the cumulative bias caused by sensor measurement errors, singular value decomposition (SVD) is used to orthogonally repair the calculated rotation matrix, thereby obtaining an effective and physically feasible initial attitude matrix. This matrix can then be used to calculate the initial attitude quaternion form of the unmanned surface vessel for Kalman filter initialization.

[0042] The fourth step, during the dynamic estimation process, involves calculating the residual error using the innovative sequence in Kalman filtering and dynamically updating the GPS noise covariance matrix using the sliding window method. Specifically, this involves calculating the residual error at each time step by real-time evaluation of the GPS measurement residuals. Based on these errors, the covariance matrix is ​​dynamically updated, and the system can adaptively adjust its dependence on GPS data. First, the GPS residual error is calculated: .

[0043] Then, the variance of the residuals is used to update the covariance matrix of the GPS measurements. The specific update formula is as follows: .

[0044] in .

[0045] Furthermore, the residual error is smoothed using a sliding window method, which further improves the estimation accuracy of the covariance matrix. The formula is as follows: .

[0046] To more accurately estimate the covariance of GPS noise, a recursive update method is introduced, which dynamically adjusts the covariance matrix using the following formula:

[0047] .

[0048] when Smaller than window size At that time, the covariance estimate is updated using a weighted average. The updated weights vary with... Gradually increase; when Greater than or equal to At that time, the system uses the GPS measurement residuals within the sliding window to update the covariance matrix.

[0049] The fifth step is to discretize the state of the continuous system, construct the state transition matrix, and perform selective correction on the state vector and covariance through Kalman gain, finally outputting the state estimation result of the unmanned ship.

[0050] Specifically: Since the system changes continuously over time, it needs to be discretized to adapt to a discrete-time Kalman filter. By integrating the continuous state transitions, the discrete-time state transition matrix can be obtained. This matrix is ​​used to update the state estimate in each filtering step. The formula for calculating the state transition matrix is: .

[0051] Based on Rodrigues' formula in the theory of rigid body rotation, the following covariance matrix is ​​obtained using the first-order approximation of matrix exponents: .

[0052] in , This is an auxiliary matrix related to angular velocity, containing sine and cosine terms:

[0053] ;

[0054] .

[0055] To optimize the entire data fusion process and ensure the system maintains high accuracy and stability in dynamically changing environments, an IMU noise dynamic estimation algorithm proposed in EKF-LIO is used to dynamically measure and adjust the covariance matrix of the IMU measurement noise. Specifically, by estimating the innovation and residuals of IMU measurements, the process noise covariance of the IMU is updated using the following formula: .

[0056] in, , It is the Jacobian matrix of the IMU process model. It is an IMU measurement error.

[0057] In the Kalman filter estimation process, previous state estimates are needed to predict the current state. By propagating the previous state estimates and adding current process noise, the current state prediction can be obtained: .

[0058] Next, the covariance matrix needs to be updated to reflect changes in system uncertainty: .

[0059] After obtaining the predicted state, an observation update is required. Kalman gain matrix. It is calculated based on the difference between observed and predicted data and used to update the system state. By calculating the residual between the predicted and actual observed values, the system state can be adjusted to reduce errors. The formula for calculating the Kalman gain matrix is: .

[0060] The state update formula is: .

[0061] The covariance update formula is: .

[0062] Finally, the system state needs to be updated and output, and the state vector is divided into... ,in The two parts are updated and calculated separately, and the final state estimation result is output: .

[0063] .

[0064] The foregoing has shown and described the basic principles, main features, and advantages of the present invention. Those skilled in the art should understand that the present invention is not limited to the above embodiments. The embodiments and descriptions in the specification are merely preferred examples and are not intended to limit the invention. Various changes and modifications can be made to the invention without departing from its spirit and scope, and all such changes and modifications fall within the scope of the present invention as claimed. The scope of protection of the present invention is defined by the appended claims and their equivalents.

Claims

1. A state estimation method based on adaptive weights for RTK and IMU fusion, characterized in that, The method includes the following steps: S1. Deploy two RTK-GPS antennas and an IMU on the unmanned vessel body, so that the IMU coincides with the origin of the unmanned vessel coordinate system, and at the same time, make the line connecting the position coordinates of the two RTK-GPS antennas non-parallel to the direction of the sum of the measured inertia and gravitational acceleration. S2. By constructing a fusion observation model of RTK-GPS and IMU data, the acceleration and angular velocity data in the IMU coordinate system are transformed into the inertial coordinate system using a rotation matrix. At the same time, by establishing the GPS observation equation, the RTK-GPS measurement values ​​are combined with the dynamic information of the IMU to accurately estimate the state of the target object. S3. Initialize the system. When the unmanned vessel is stationary, obtain the baseline vector in the inertial coordinate system through the position information of the two GPS antennas. The accelerometer provides the initial acceleration. Combine the cross product of the two to form a three-dimensional vector basis in the inertial system. Establish the rotation relationship between the two coordinate systems. Orthogonalize and repair the initial rotation matrix through singular value decomposition. Finally, obtain the attitude matrix for Kalman filter initialization. S4. To make more accurate adjustments to the GPS data, during the dynamic estimation process, the residual error is calculated through the innovative sequence in the Kalman filter, and the GPS noise covariance matrix is ​​dynamically updated using the sliding window method. S5. After obtaining the GPS noise covariance, the continuous-time state of the system is discretized to construct a state transition matrix. The process noise covariance matrix is ​​dynamically adjusted in combination with IMU measurement data. The Kalman gain is calculated through the residuals of prediction and observation. The system state and its covariance are then iteratively updated to achieve accurate estimation and output of the state variables.

2. The RTK and IMU fusion state estimation method based on adaptive weights according to claim 1, characterized in that, The specific steps of step S2 are as follows: S21. Let the coordinates of the IMU coordinate system in the inertial coordinate system be... , The output of the GPS measurement is represented, resulting in the GPS measurement model: , in, This represents the rotation matrix from the IMU coordinate system to the inertial coordinate system. This indicates the position of the GPS antenna in the IMU coordinate system; S22. The state vector of the unmanned vessel is defined as: , in Indicates the attitude of the unmanned vessel; It is the vector part of the quaternion; Indicates the location of the unmanned vessel; Indicates the speed of the unmanned vessel; This indicates the bias value of the gyroscope; S23, The observation model is set as follows: , in , , This represents the GPS measurement noise, and its covariance is... Since the observation model function is nonlinear, it needs to be linearized in Kalman filtering, and the observation matrix is ​​derived as follows: , in ; S24. Deterministic errors are eliminated through the sensor's internal processor or an external calibration process. Let ω represent the angular velocity of the unmanned surface vessel. Then, the relationship between the time derivative of the quaternion and the angular velocity is as follows: , The relationship between angular rate and the measured rate of the gyroscope is as follows: , in, It's the output of the gyroscope. It is the gyroscope bias vector. It is the angular random walk noise of the gyroscope rate; The time derivative of the gyroscope bias is traditionally modeled using a random walk model. , in, It is rate random walk noise; The output equation of the accelerometer is: , The continuous-time dynamics of the entire system are represented by the following state equations: , Where, vector ,vector , This indicates the extraction of the vector portion of the quaternion; For nonlinear functions Linearization yields a linearized model of the continuous system: .

3. The RTK and IMU fusion state estimation method based on adaptive weights according to claim 1, characterized in that, The specific steps of step S3 are as follows: S31. When the unmanned vessel is initially stationary, the GPS receiver provides the absolute position information of the two antennas in the inertial coordinate system; the accelerometer in the IMU provides the initial acceleration reading under the action of gravity. S32. By constructing three sets of linearly independent vectors, consisting of the baseline vector and acceleration vector between two GPS antennas and their cross product, a three-dimensional vector basis is constructed to represent the spatial configuration in the inertial coordinate system. At the same time, based on the unmanned ship structure, a three-dimensional basis in the unmanned ship coordinate system is constructed. S33. Using the relationship between these two sets of vector bases, the initial rotation transformation of the unmanned vessel's coordinate system relative to the inertial coordinate system is solved. Singular value decomposition is used to orthogonally repair the calculated rotation matrix, thereby obtaining an effective and physically feasible initial attitude matrix. This matrix is ​​then used to calculate the initial attitude quaternion form of the unmanned vessel for use in the initialization of the Kalman filter.

4. The RTK and IMU fusion state estimation method based on adaptive weights according to claim 1, characterized in that, The specific steps of step S4 are as follows: S41. By evaluating the GPS measurement residuals in real time, calculate the residual error at each moment. Based on these errors, the covariance matrix is ​​dynamically updated, and the system can adaptively adjust its dependence on GPS data. First, the GPS residual error is calculated: ， S42. Then, the variance of the residuals is used to update the covariance matrix of the GPS measurements. The specific update formula is as follows: , in , S43. In addition, the residual error is smoothed using the sliding window method, as shown in the following formula: , S44. Introduce a recursive update method to dynamically adjust the covariance matrix using the following formula: ， when Smaller than window size At that time, the covariance estimate is updated using a weighted average method, with the updated weights varying accordingly. Gradually increase; when Greater than or equal to At that time, the system uses the GPS measurement residuals within the sliding window to update the covariance matrix.

5. The RTK and IMU fusion state estimation method based on adaptive weights according to claim 1, characterized in that, The specific steps of step S5 are as follows: S51. By integrating the continuous state transitions, the discrete-time state transition matrix is ​​obtained. This matrix is ​​used to update the state estimate in each filtering step. The formula for calculating the state transition matrix is: , Based on Rodrigues' formula in the theory of rigid body rotation, the following covariance matrix is ​​obtained using the first-order approximation of matrix exponents: , in , This is an auxiliary matrix related to angular velocity, containing sine and cosine terms: ， ； S52. By estimating the innovation and residuals of IMU measurements, the process noise covariance of the IMU is updated using the following formula: , in, , It is the Jacobian matrix of the IMU process model. It is the IMU measurement error; By propagating the previous state estimate and adding current process noise, the state prediction for the current moment is obtained: ; S53. Update the covariance matrix: , After obtaining the predicted state, an observation update is performed, using the Kalman gain matrix. It is calculated based on the difference between observed and predicted data and is used to update the system state. By calculating the residual between the predicted and actual observed values, the system state is adjusted. The formula for calculating the Kalman gain matrix is: , The state update formula is: , The covariance update formula is: , Finally, the system state is updated and output, and the state vector is divided into... ,in The two parts are updated and calculated separately, and the final state estimation result is output: ， 。 6. The adaptive weight-based RTK and IMU fusion state estimation method according to any one of claims 1-5, characterized in that, The method can maintain high-precision navigation estimation at low speeds or when stationary.

7. The RTK and IMU fusion state estimation method based on adaptive weights according to claim 3, characterized in that, The state prediction model used in the Kalman filter considers various nonlinear characteristics of the dynamic system and linearizes the nonlinear function through extended Kalman filtering.

8. The RTK and IMU fusion state estimation method based on adaptive weights according to claim 4, characterized in that, The sliding window method is used to evaluate the residual error of GPS data in real time and adjust the weights of the covariance matrix according to the fluctuation of the error to achieve dynamically optimized data fusion.

9. The RTK and IMU fusion state estimation method based on adaptive weights according to claim 3, characterized in that, The baseline vector between the two GPS antennas is the coordinate difference between the two antennas.

10. The RTK and IMU fusion state estimation method based on adaptive weights according to claim 2, characterized in that, The deterministic error includes the scaling factor and the bias.

Images