RTK (Real Time Kinematic) and IMU (Inertial Measurement Unit) fusion state estimation method based on adaptive weight

Abstract

The invention discloses an RTK (Real Time Kinematic) and IMU (Inertial Measurement Unit) fusion state estimation method based on adaptive weight. According to the method, an observable state estimation system is established by using two RTK-GPS antennas and an IMU device based on a self-adaptive Kalman filtering framework. By constructing a proper antenna layout, the connecting line for connecting the two GPS antennas is not parallel to the acceleration vector, so that the observability condition is met, and the attitude error caused by gyroscope drift is effectively compensated. Compared with a traditional single-GPS fusion scheme, high-precision estimation of the three-dimensional attitude and position can be achieved without an additional attitude measurement device such as a magnetic compass, a self-adaptive covariance estimation strategy based on a sliding window is introduced, the GPS measurement noise level is evaluated in real time, the fusion process is endowed with a high weight only when GPS information is reliable, and the fusion precision is improved. The method effectively improves the robustness of the system, is suitable for unmanned ship navigation tasks in various water area environments, and has a wide engineering application prospect.

Description

Technical Field

[0001] The present invention relates to the technical field of navigation and state estimation in a surface autonomous navigation system, and in particular to an RTK and IMU fusion method for high-precision navigation of unmanned ships in complex environments. Background Art

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

[0003] However, in practical applications, the performance of RTK systems is highly dependent on environmental conditions. Factors such as occlusion and signal multipath can lead to reduced RTK positioning accuracy or even data failure. While IMUs offer high short-term accuracy, their attitude and position estimates are susceptible to noise and drift over long periods of operation, resulting in cumulative errors. Traditional fusion navigation systems typically employ fixed-weight or prior-covariance-based Kalman filtering, which struggles to maintain system stability and estimation accuracy in the face of dynamic changes in the quality of multi-source observations.

[0004] Furthermore, existing research often uses a single RTK antenna integrated with an IMU, which only provides limited heading estimation capabilities and makes it difficult to achieve complete 3D attitude observation without magnetic field information or when GPS heading is unstable. While a dual-antenna RTK architecture 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 weight when the observation quality changes, improve the accuracy and robustness of attitude and position estimation, and adapt to the navigation needs of unmanned ships in complex water environments. Summary of the Invention

[0006] Based on the above objectives, the present invention provides an RTK and IMU fusion state estimation method based on adaptive weights, which adopts an adaptive weight mechanism to dynamically adjust the multi-sensor fusion ratio to achieve accurate and reliable navigation state estimation in various environments.

[0007] The technical solution of the present invention is: an RTK and IMU fusion state estimation method based on adaptive weights, the method comprising the following steps: S1. Deploy two RTK-GPS antennas and an IMU on the UAV, ensuring that the IMU and the origin of the UAV coordinate system coincide with each other, and 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 inertial and gravitational accelerations. S2. By building a fusion observation model of RTK-GPS and IMU data, the acceleration, angular velocity and other data in the IMU coordinate system are converted to the inertial coordinate system using the rotation matrix. At the same time, by establishing the GPS observation equation, the RTK-GPS measurement value is combined with the dynamic information of the IMU to accurately estimate the state of the target object; S3. Initialize the system. When the UAV is stationary, obtain the baseline vector in the inertial coordinate system using the position information of the two GPS antennas. The accelerometer provides the initial acceleration, and the three-dimensional vector basis in the inertial coordinate system is constructed by cross-product. The rotation relationship between the two coordinate systems is established, and the initial rotation matrix is ​​orthogonalized and repaired through singular value decomposition. Finally, the attitude matrix used for Kalman filter initialization is obtained. S4. After the attitude matrix is ​​successfully initialized, as the system changes and the GPS measurement noise fluctuates, the system needs 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, the state transfer matrix is ​​constructed, and the process noise covariance matrix is ​​dynamically adjusted in combination with the IMU measurement data to improve the system's adaptability to changes in the external environment. On this basis, the Kalman gain is calculated through the residual of prediction and observation, and the system state and its covariance are iteratively updated, ultimately achieving accurate estimation and output of state quantities such as attitude, position, and velocity.

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

[0009] 1. This invention fuses dual RTK-GPS and IMU data and uses an adaptive weighting mechanism to dynamically adjust the sensor fusion ratio, enabling high-precision and robust navigation and positioning in a variety of environmental conditions. Compared to traditional single-GPS or IMU fusion solutions, this invention effectively addresses issues such as RTK signal instability and IMU drift, improving system stability in complex environments.

[0010] 2. This invention combines the dynamic IMU noise estimation algorithm in EKF-LIO to optimize the covariance matrix of IMU measurements. By innovating and optimizing the IMU residual, this invention effectively suppresses noise and drift in IMU measurements, ensuring system stability and accuracy in complex and dynamic environments. BRIEF DESCRIPTION OF THE DRAWINGS

[0011] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the following briefly introduces the drawings required for use in the embodiments. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying any creative work.

[0012] Figure 1 This is a flow chart of a method for estimating the state of RTK and IMU fusion based on adaptive weights according to an embodiment of the present invention. DETAILED DESCRIPTION

[0013] The technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, rather than all the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative work are within the scope of protection of the present invention.

[0014] In a specific embodiment, Figure 1 As shown, the present invention provides an RTK and IMU fusion state estimation method based on adaptive weights, comprising the following steps:

[0015] The first step is to deploy two RTK-GPS antennas and 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 inertial and gravitational accelerations. The two antennas should be distributed as much as possible at appropriate positions on the hull, and ensure that the distance between them is large enough to provide high-precision positioning data through differential means. At the same time, ensure that the IMU coincides with the origin of the unmanned vessel's coordinate system, and then replace the hull coordinate system with the IMU coordinate system.

[0016] The second step is to build a state observation model, establish observation equations through RTK-GPS measurement values ​​and IMU data, and use the rotation matrix to convert the sensor data in the IMU coordinate system to the inertial coordinate system.

[0017] Specifically: First, let the coordinates of the IMU coordinate system in the inertial coordinate system be , Representing the output of GPS measurement, the GPS measurement model can be obtained: .

[0018] in, Represents the rotation matrix from the IMU coordinate system to the inertial coordinate system, Indicates the position of the GPS antenna in the IMU coordinate system, which is a constant vector.

[0019] Then, the state vector of the unmanned ship is defined as: .

[0020] in Represents the attitude of the unmanned ship; it is the vector part of the quaternion; Indicates the position of the unmanned vessel; Indicates the speed of the unmanned ship; Indicates the bias value of the gyroscope.

[0021] The observation model is set as: .

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

[0023] in .

[0024] Deterministic errors (including scale factor and bias) can be removed by the sensor's internal processor or by an external calibration process, assuming the accelerometer has been compensated for these errors. Represents the angular velocity of the unmanned ship, then the relationship between the time derivative of the quaternion and the angular velocity is: .

[0025] The relationship between angular rate and gyroscope measurement rate is: .

[0026] in, is the gyroscope output, is the gyroscope bias vector, is the angular random walk noise of the gyro rate.

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

[0028] in, is the rate random walk noise.

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

[0030] The continuous-time dynamics of the entire system is represented by the following state equation: .

[0031] Among them, the vector ,vector .

[0032] In order to use the extended Kalman filter, the nonlinear function Linearization obtains the linearized model of the continuous system: .

[0033] After establishing the state estimation model, the system is initialized. When the UAV is stationary, the position information of the two GPS antennas is used to obtain the baseline vector in the inertial coordinate system. The accelerometer provides the initial acceleration, and the cross product of these two accelerations forms a three-dimensional vector basis in the inertial coordinate system. The rotation relationship between the two coordinate systems is established, and the initial rotation matrix is ​​orthogonalized and repaired through singular value decomposition. Ultimately, the attitude matrix is ​​obtained for initializing the Kalman filter.

[0034] Specifically: When the unmanned ship 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.

[0035] By taking the baseline vector between the two GPS antennas (i.e., the coordinate difference between the two antennas) and the acceleration vector, and then cross-producting them to form three sets of linearly independent vectors, we construct a three-dimensional vector basis representing the spatial configuration in the inertial coordinate system. Furthermore, given the structure of the unmanned vessel, the GPS baseline and the direction of the initial acceleration are known in the unmanned vessel coordinate system (IMU coordinate system). Therefore, a three-dimensional basis in the unmanned vessel coordinate system can also be constructed.

[0036] The relationship between these two vector bases allows the initial rotation transformation of the UAV coordinate system relative to the inertial coordinate system to be determined. To eliminate the accumulated bias introduced by sensor measurement errors, singular value decomposition (SVD) is used to restore the orthogonality of the calculated rotation matrix, resulting in a valid and physically feasible initial attitude matrix. This matrix is ​​then used to calculate the UAV's initial attitude quaternion form for initializing the Kalman filter.

[0037] Step 4: In 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. Specifically: by evaluating the GPS measurement residual in real time, the residual error at each moment is calculated. , and dynamically update the covariance matrix based on these errors. The system can adaptively adjust its dependence on GPS data. First, calculate the GPS residual error: .

[0038] The variance of the residuals is then used to update the covariance matrix of the GPS measurements , the specific update formula is: .

[0039] in .

[0040] In addition, the residual error is smoothed by the sliding window method to further improve the estimation accuracy of the covariance matrix. The formula is as follows: .

[0041] In order to estimate the covariance of GPS noise more accurately, a recursive update method is introduced to dynamically adjust the covariance matrix through the following formula: .

[0042] when Smaller than window size When , the covariance estimate is updated by weighted averaging. The updated weights are Gradually increase; when Greater than or equal to When , the system uses the GPS measurement residuals within the sliding window to update the covariance matrix.

[0043] The fifth step is to discretize the continuous system state, construct the state transfer matrix, perform iterative correction on the state vector and covariance through the Kalman gain, and finally output the state estimation result of the unmanned ship.

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

[0045] According to the Rodrigues formula in rigid body rotation theory, the following covariance matrix is ​​obtained using the first-order approximation form of the matrix exponential: .

[0046] in , is the auxiliary matrix related to angular velocity, containing sine and cosine terms:

[0047] ;

[0048] .

[0049] In order to optimize the entire data fusion process and ensure that the system can maintain high accuracy and stability in a dynamically changing environment, the 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 residual of the IMU measurement, the process noise covariance of the IMU is updated using the following formula: .

[0050] in, , is the Jacobian matrix of the IMU process model, is the IMU measurement error.

[0051] In the estimation process of the Kalman filter, it is necessary to use the previous state estimate to predict the current state. By propagating the previous state estimate and adding the current process noise, the state prediction at the current moment can be obtained: .

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

[0053] After obtaining the predicted state, the observation update is required. Kalman gain matrix It is calculated based on the difference between the observed data and the predicted data and is used to update the system state. By calculating the residual between the predicted value and the actual observed value, the system state can be adjusted to reduce the error. The Kalman gain matrix is ​​calculated as follows: .

[0054] The state update formula is: .

[0055] The covariance update formula is: .

[0056] Finally, the state of the system needs to be updated and output, and the state vector is divided into ,in , update the two parts separately and output the final state estimation results: .

[0057] .

[0058] The above shows and describes 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 above embodiments and descriptions are merely preferred examples of the present invention and are not intended to limit the present invention. Various changes and improvements may be made to the present invention without departing from the spirit and scope of the present invention. Such changes and improvements fall within the scope of the present invention. The scope of protection claimed in the present invention is defined by the appended claims and their equivalents.

Claims

1. The RTK and IMU fusion state estimation method based on adaptive weights is characterized by: The method comprises the following steps: S1. Deploy two RTK-GPS antennas and an IMU on the UAV, aligning the IMU with the origin of the UAV coordinate system and ensuring that the line connecting the two RTK-GPS antenna position coordinates is not parallel to the sum of the measured inertial and gravitational accelerations. S2. By building a fusion observation model of RTK-GPS and IMU data, the acceleration and angular velocity data in the IMU coordinate system are converted to the inertial coordinate system using the rotation matrix. At the same time, by establishing the GPS observation equation, the RTK-GPS measurement value is combined with the dynamic information of the IMU to accurately estimate the state of the target object; S3. Initialize the system. When the UAV is stationary, obtain the baseline vector in the inertial coordinate system using the position information of the two GPS antennas. The accelerometer provides the initial acceleration, and the three-dimensional vector basis in the inertial coordinate system is constructed by cross-product. The rotation relationship between the two coordinate systems is established, and the initial rotation matrix is ​​orthogonalized and repaired through singular value decomposition. Finally, the attitude matrix used for Kalman filter initialization is obtained. S4. To make more accurate adjustments to 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, the state transfer matrix is ​​constructed, and the process noise covariance matrix is ​​dynamically adjusted in combination with the IMU measurement data. The Kalman gain is calculated through the residual of the prediction and observation, and the system state and its covariance are iteratively updated to finally achieve accurate estimation and output of the state quantity.

2. The RTK and IMU fusion state estimation method based on adaptive weights according to claim 1, characterized in that: The specific operation steps of step S2 are: S21. Assume that the coordinates of the IMU coordinate system in the inertial coordinate system are , Represents the output of GPS measurement and obtains the GPS measurement model: , in, Represents the rotation matrix from the IMU coordinate system to the inertial coordinate system, Indicates the position of the GPS antenna in the IMU coordinate system; S22. Define the state vector of the unmanned ship as: , in Indicates the attitude of the unmanned vessel; is the vector part of the quaternion; Indicates the position of the unmanned vessel; Indicates the speed of the unmanned ship; Indicates the bias value of the gyroscope; S23, the observation model is set to: , in , , represents the GPS measurement noise, and its covariance is , since the observation model function is nonlinear, it needs to be linearized in the Kalman filter to derive the observation matrix: , in ; S24, deterministic error is eliminated by the sensor's internal processor or external calibration process. Represents the angular velocity of the unmanned ship, then the relationship between the time derivative of the quaternion and the angular velocity is: , The relationship between angular rate and gyroscope measurement rate is: , in, is the gyroscope output, is the gyroscope bias vector, is the angular random walk noise of the gyro rate; The time derivative of the gyroscope bias is traditionally modeled using a random walk model: , in, is the rate random walk noise; The output equation of the accelerometer is: , The continuous-time dynamics of the entire system is represented by the following state equation: , Among them, the vector ,vector , Indicates the vector part of the quaternion to be extracted; For nonlinear functions Linearization obtains the 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 operation steps of step S3 are: S31. When the UAV 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. Construct a three-dimensional vector basis by taking the baseline vector between the two GPS antennas and the acceleration vector, and their cross product to form three sets of linearly independent vectors, representing the spatial configuration in the inertial coordinate system. Simultaneously, construct a three-dimensional basis in the unmanned ship coordinate system based on the unmanned ship structure. S33. Utilize the relationship between these two sets of vector bases to solve the initial rotation transformation of the unmanned ship coordinate system relative to the inertial coordinate system, and use singular value decomposition to repair the orthogonality of the calculated rotation matrix to obtain an effective and physically feasible initial attitude matrix, which is then used to calculate the initial attitude quaternion form of the unmanned ship for use in Kalman filter initialization.

4. The method for estimating the state of RTK and IMU fusion based on adaptive weights according to claim 1, characterized in that: The specific operation steps of step S4 are: S41, through the real-time evaluation of GPS measurement residuals, calculate the residual error at each moment , and dynamically update the covariance matrix based on these errors. The system can adaptively adjust its dependence on GPS data. First, calculate the GPS residual error: ， S42, then, use the variance of the residual to update the covariance matrix of the GPS measurement , the specific update formula is: , in , S43. In addition, the residual error is smoothed by the sliding window method, and the formula is as follows: , S44. Introduce a recursive update method to dynamically adjust the covariance matrix using the following formula: , when Smaller than window size When , the covariance estimate is updated by weighted averaging, and the updated weights are updated as Gradually increase; when Greater than or equal to When , the system uses the GPS measurement residuals within the sliding window to update the covariance matrix.

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

7. The method for estimating the state of RTK and IMU fusion based on adaptive weights according to claim 3, characterized in that: The state prediction model used by the Kalman filter takes into account various nonlinear characteristics of the dynamic system and linearizes the nonlinear function through the extended Kalman filter.

8. The method for estimating the state of RTK and IMU fusion 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 weight of the covariance matrix according to the fluctuation of the error to achieve dynamic optimized data fusion.

9. The method for estimating the state of RTK and IMU fusion 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 method for estimating the state of RTK and IMU fusion based on adaptive weights according to claim 2, characterized in that: The deterministic error includes a scale factor and an offset.

Images