Combined navigation method of adaptive extended state observer Kalman filtering based on sea condition estimation

Abstract

The invention discloses an integrated navigation method for adaptive extended state observer Kalman filtering based on sea condition estimation, belongs to the technical field of navigation, and realizes accurate state estimation under different sea conditions by evaluating sea condition disturbance in real time and adaptively adjusting ESO bandwidth and a Kalman filtering process noise matrix. Compared with a traditional method, the problem that filter parameters are obtained by depending on prior knowledge or empirical values in the traditional method is solved. The method comprises the following specific steps: firstly, calculating a sea condition standard deviation by using a ship rolling angular velocity, and dividing sea condition grades according to the standard deviation; then, dynamically adjusting ESO bandwidth and a Kalman filter process noise matrix according to the sea condition grade; secondly, estimating disturbance by using ESO and performing compensation; finally, IMU and GNSS data are fused through an adaptive Kalman filter, the ship state is accurately estimated, and the navigation precision is further optimized through a feedback closed loop.

Description

Technical Field

[0001] This invention belongs to the field of navigation technology, specifically relating to a combined navigation method based on sea state estimation, adaptive extended state observer, and Kalman filter. Background Technology

[0002] As humanity advances in ocean exploration, ship navigation technology continues to improve. Modern navigation has evolved from the traditional stage, primarily providing heading information, to a stage where it can provide real-time, high-precision, continuous navigation information such as position, velocity, and attitude. In recent years, to adapt to the complex marine environment and diverse marine operational needs, research in the field of unmanned surface vessels (USVs) has received widespread attention. USV systems involve multiple aspects, including control systems, communication systems, and navigation systems. Among these, the navigation system is a crucial component of USV navigation and guidance research, and a key part of achieving autonomous or semi-autonomous navigation. Due to the complex and ever-changing navigation environment, USVs using INS / GPS integrated navigation systems have developed rapidly, while also placing higher demands on the accuracy and stability of this navigation system.

[0003] While sea state awareness-based adaptive integrated navigation methods offer significant advantages in improving ship navigation accuracy and robustness, several challenges remain. First, the accuracy and real-time performance of sea state awareness are affected by noise in ship motion data, potentially leading to inaccurate sea state assessments and impacting parameter adjustment effectiveness. Second, adaptive parameter adjustment strategies rely on preset rules, which may not adapt to extreme or unknown sea states, necessitating further optimization. Furthermore, suppressing high-frequency noise and ensuring accurate disturbance estimation are also challenging, especially under adverse sea conditions. The data synchronization issues and real-time requirements of multi-sensor fusion also increase algorithm complexity, potentially causing system response delays. Finally, although adaptive adjustment can cope with different sea states, system stability and robustness still need to be strengthened under extreme conditions to avoid over-adjustment or instability. Summary of the Invention

[0004] To address the problems in the existing technologies, this invention provides a combined navigation method based on sea state estimation and adaptive extended state observer (ESO) Kalman filtering. First, the ship's roll rate is monitored in real time by a sea state sensing module, and its standard deviation is calculated to assess the disturbance intensity of the current sea state, classifying the sea state into three levels: calm, moderate, and severe. Then, based on the sea state level, the system dynamically adjusts the bandwidth parameter β of the ESO. 02The process noise covariance matrix Q of the Kalman filter enables the system to optimize response speed and model uncertainty tolerance under different sea states. Based on this, the ESO module estimates and compensates for unmodeled disturbances in the system in real time, reducing the impact of the external environment on navigation accuracy. Next, an adaptively adjusted Kalman filter is used to fuse IMU and GNSS observation data to accurately estimate the ship's attitude, speed, and position. Finally, the system outputs the final navigation state and feeds back sea state information and disturbance estimates to the sea state perception and parameter adjustment module, forming a closed-loop feedback mechanism to further optimize parameter adjustments and improve the system's adaptability and accuracy. This method can effectively cope with complex and harsh sea state environments, providing more stable and accurate positioning services in ship navigation.

[0005] To achieve the above technical objectives, this invention provides a combined navigation method based on sea state estimation, an adaptive extended state observer, and Kalman filtering, comprising:

[0006] S1, using a sliding window to calculate the standard deviation of the roll angle angular velocity. Real-time statistics are collected to assess the intensity of current sea state disturbances.

[0007] S2, dynamically adjust the bandwidth β of the Extended State Observer (ESO) based on the sea state level. 02 The process noise Q of the Kalman filter KF;

[0008] S3 uses an adaptive extended state observer to estimate external disturbances in the vertical and attitude channels and compensates for non-modeling terms in the INS dynamic model in real time.

[0009] S4 integrates GNSS and IMU information and uses an adaptive process noise model for state prediction and updating, achieving high-precision estimation and error suppression of key navigation states such as attitude, velocity, and position.

[0010] S5 outputs the fused navigation results and feeds back the disturbance estimate and sea state registration information to the adaptive module to build an online closed-loop adjustment mechanism.

[0011] In the steps described above, S1 involves using a sliding window to calculate the standard deviation of the roll angle angular velocity.

[0012]

[0013] In the formula Let represent the roll angular velocity at the i-th sampling time.

[0014] This represents the average roll rate within the window.

[0015] N represents the length of the sliding window.

[0016] k represents the k-th time.

[0017] S1 describes real-time statistical analysis to perceive the intensity of current sea state disturbances and classify sea state registration into:

[0018]

[0019] In the formula: Level represents the sea state level.

[0020] θ1 and θ2 are empirically set thresholds, with θ1 = 0.005 rad / s and θ2 = 0.015 rad / s.

[0021] The bandwidth β of the extended state observer (ESO) dynamically adjusted according to sea state level, as described in S2, is... 02 The process noise Q of the Kalman filter KF:

[0022] Sea state rating <![CDATA[β 02 ]]> Q Calm (Level 1) 200 <![CDATA[[1×10 -8 ,1×10 -8 ,1×10 -7 ,1×10 -6 ,1×10 -6 ,1×10 -6 ]]]> Intermediate (Level 2) 300 <![CDATA[[5×10 -8 ,5×10 -8 ,5×10 -7 ,5×10 -6 ,5×10 -6 ,5×10 -6 ]]]> Terrible (Level 2) 400 <![CDATA[[1×10 -7 ,1×10 -7 ,1×10 -6 ,1×10 -5 ,1×10 -5 ,1×10 -5 ]]]>

[0023] Where: β 02 This represents the ESO bandwidth parameter, which controls the dynamic response speed of the observer;

[0024] Q represents the noise covariance matrix of the Kalman filtering process, corresponding to states such as attitude angle, angular velocity, and velocity.

[0025] An adaptive extended state observer is used to estimate external disturbances in the vertical and attitude channels, and non-modeling terms in the INS dynamic model are compensated in real time. A third-order linear ESO model is adopted.

[0026]

[0027] In the formula: y represents the system input quantity;

[0028] z1 represents the system state estimate;

[0029] z2 represents the first derivative estimate

[0030] z3 represents the extended state of ESO and is used as a disturbance compensation input;

[0031] β1, β2, β3 represent parameters that are related to the ESO bandwidth parameters.

[0032] The relationship between β1, β2, β3 and the ESO bandwidth parameters described in S3 is as follows:

[0033]

[0034] S4 describes the fusion of GNSS and IMU information, utilizing an adaptive process noise model for state prediction and updating. This enables high-precision estimation and error suppression of key navigation states such as attitude, velocity, and position. The Kalman filtering process is as follows:

[0035]

[0036] In the formula: This represents the state estimate at the current moment;

[0037] F represents the state transition matrix;

[0038] H represents the observation matrix;

[0039] Q k This represents the process noise covariance matrix adjusted for sea state levels.

[0040] R represents the observation noise covariance matrix;

[0041] P k / k This represents the state covariance matrix at the current moment;

[0042] P k / k-1 This represents the estimated state covariance matrix;

[0043] K represents the gain matrix;

[0044] z k This represents the observation at time k.

[0045] The S5 system outputs fused navigation results and feeds back disturbance estimates and sea state registration information to the adaptive module to construct an online closed-loop adjustment mechanism. The fused navigation results output by S5 are as follows:

[0046]

[0047] In the formula: φ, θ, and ψ represent pitch, roll, and yaw angles, respectively;

[0048] v x ,v y ,v z These represent the velocities in the x, y, and z directions, respectively.

[0049] x, y, z represent the positions in the x, y, and z directions, respectively.

[0050] Beneficial effects: This invention provides a combined navigation method based on sea state estimation and adaptive extended state observer Kalman filtering, which has the following advantages compared with the prior art:

[0051] 1. This invention, based on sea state perception and adaptive parameter adjustment, dynamically adjusts the bandwidth of the Extended State Observer (ESO) and the process noise matrix of the Kalman filter by real-time evaluation of the standard deviation of the ship's roll rate. This solves the problem of relying on prior knowledge or empirical values ​​to obtain filter parameters in traditional methods. By adaptively adjusting parameters, this method can optimize the system's response speed and robustness under different sea states, significantly improving the navigation accuracy and stability of ships in complex sea states.

[0052] 2. This invention, based on sea state perception and adaptive filtering technology, solves the problem of response lag or oscillation divergence caused by fixed parameters in traditional ship motion control systems under complex sea conditions. By sensing the ship's motion state in real time and dynamically adjusting control parameters, it can effectively cope with model mismatch caused by the time-varying characteristics of the wave spectrum, significantly improving the system's adaptability to different sea conditions. This method can be extended to dynamic positioning systems for offshore platforms to solve the problem of thrust distribution optimization under irregular wave action;

[0053] 4. To verify the effectiveness of the algorithm, different sea state data were processed, and the sea state perception-based adaptive Kalman filtering technique was compared with the traditional adaptive Kalman filtering to obtain the RMS value of the error. The method proposed in this invention improves the position navigation accuracy by 72.7%. Attached Figure Description

[0054] Figure 1 This is a flowchart of a combined navigation method based on sea state estimation, adaptive extended state observer, and Kalman filter according to the present invention.

[0055] Figure 2 The position result is the traditional adaptive Kalman filter combined navigation result of this invention;

[0056] Figure 3 The position results of the adaptive extended state observer Kalman filter combined navigation based on sea state estimation of the present invention;

[0057] Figure 4 The RMS plot of the position error result and error of the conventional adaptive Kalman filter integrated navigation of the present invention is shown.

[0058] Figure 5 This is an RMS plot of the position error results and errors of the adaptive extended state observer Kalman filter combined navigation based on sea state estimation according to the present invention. Detailed Implementation

[0059] The present invention will now be described in detail with reference to the accompanying drawings.

[0060] like Figure 1As shown, a combined navigation method based on sea state estimation, adaptive extended state observer, and Kalman filter includes the following steps:

[0061] Step 1: Calculate the standard deviation of the roll angle angular velocity using a sliding window. Real-time statistics are collected to assess the intensity of current sea state disturbances.

[0062] Step 2: Dynamically adjust the bandwidth β of the Extended State Observer (ESO) based on the sea state level. 02 The process noise Q of the Kalman filter KF;

[0063] Step 3: Use the adaptive extended state observer to estimate the external disturbances in the vertical and attitude channels and compensate for the non-modeling terms in the INS dynamic model in real time.

[0064] Step 4: Integrate GNSS and IMU information, and use an adaptive process noise model for state prediction and updating to achieve high-precision estimation and error suppression of key navigation states such as attitude, velocity, and position;

[0065] Step 5: Output the fused navigation results and feed the disturbance estimate and sea state registration information back to the adaptive module to build an online closed-loop adjustment mechanism.

[0066] like Figure 2 As shown, traditional adaptive Kalman filtering is used for integrated navigation to obtain the position result of the vehicle. The horizontal axis represents time and the vertical axis represents the position result.

[0067] like Figure 3 As shown, the combined navigation method of adaptive extended state observer Kalman filter based on sea state estimation is used to obtain the position result of the vehicle. The horizontal axis represents time and the vertical axis represents the position result.

[0068] like Figure 4 As shown, the position result of the integrated navigation using traditional adaptive Kalman filtering is subtracted from the reference true value to obtain the RMS value of the error. The horizontal axis represents time, and the vertical axis represents the position error. The RMS value is 0.22m.

[0069] like Figure 5 As shown, a combined navigation method based on sea state estimation, adaptive extended state observer, and Kalman filter is adopted, and the difference between the Kalman filter and the reference true value is calculated to obtain the RMS value of the error. The horizontal axis represents time, and the vertical axis represents the position error. The RMS value is 0.06m.

[0070] In this embodiment, the above method can be implemented using the following scheme:

[0071] The standard deviation of roll angle angular velocity is calculated using a sliding window.

[0072]

[0073] In the formula Let represent the roll angular velocity at the i-th sampling time.

[0074] This represents the average roll rate within the window.

[0075] N represents the length of the sliding window.

[0076] k represents the k-th time.

[0077] Real-time statistics are performed to detect the intensity of current sea state disturbances, and sea state registration is divided into:

[0078]

[0079] In the formula: Level represents the sea state level.

[0080] θ1 and θ2 are empirically set thresholds, with θ1 = 0.005 rad / s and θ2 = 0.015 rad / s.

[0081] The bandwidth β of the Extended State Observer (ESO) is dynamically adjusted according to the sea state level. 02 The process noise Q of the Kalman filter KF:

[0082] Sea state rating <![CDATA[β 02 ]]> Q Calm (Level 1) 200 <![CDATA[[1×10 -8 ,1×10 -8 ,1×10 -7 ,1×10 -6 ,1×10 -6 ,1×10 -6 ]]]> Intermediate (Level 2) 300 <![CDATA[[5×10 -8 ,5×10 -8 ,5×10 -7 ,5×10 -6 ,5×10 -6 ,5×10 -6 ]]]> Terrible (Level 2) 400 <![CDATA[[1×10 -7 ,1×10 -7 ,1×10 -6 ,1×10 -5 ,1×10 -5 ,1×10 -5 ]]]>

[0083] Where: β 02 This represents the ESO bandwidth parameter, which controls the dynamic response speed of the observer;

[0084] Q represents the noise covariance matrix of the Kalman filtering process, corresponding to states such as attitude angle, angular velocity, and velocity.

[0085] An adaptive extended state observer is used to estimate external disturbances in the vertical and attitude channels, and non-modeling terms in the INS dynamic model are compensated in real time. A third-order linear ESO model is adopted.

[0086]

[0087] In the formula: y represents the system input quantity;

[0088] z1 represents the system state estimate;

[0089] z2 represents the first derivative estimate

[0090] z3 represents the extended state of ESO and is used as a disturbance compensation input;

[0091] β1, β2, β3 represent parameters that are related to the ESO bandwidth parameters.

[0092] The relationship between β1, β2, β3 and the ESO bandwidth parameters is as follows:

[0093]

[0094] By fusing GNSS and IMU information and utilizing an adaptive process noise model for state prediction and updating, high-precision estimation and error suppression of key navigation states such as attitude, velocity, and position are achieved. The Kalman filtering process is as follows:

[0095]

[0096] In the formula: This represents the state estimate at the current moment;

[0097] F represents the state transition matrix;

[0098] H represents the observation matrix;

[0099] Q k This represents the process noise covariance matrix adjusted for sea state levels.

[0100] R represents the observation noise covariance matrix;

[0101] P k / k This represents the state covariance matrix at the current moment;

[0102] P k / k-1 This represents the estimated state covariance matrix;

[0103] K represents the gain matrix;

[0104] z k This represents the observation at time k.

[0105] The system outputs the fused navigation results and feeds back the disturbance estimate and sea state registration information to the adaptive module to construct an online closed-loop adjustment mechanism. The output fused navigation results are as follows:

[0106]

[0107] In the formula: φ, θ, and ψ represent pitch, roll, and yaw angles, respectively;

[0108] v x ,v y ,v z These represent the velocities in the x, y, and z directions, respectively.

[0109] x, y, z represent the positions in the x, y, and z directions, respectively.

[0110] Based on the obtained error images and the RMS values ​​corresponding to different methods, it can be seen that the combined navigation method of adaptive extended state observer Kalman filter based on sea state estimation described in this invention can effectively improve the accuracy and reliability of heave measurement.

[0111] Those skilled in the art will clearly understand that the technical solutions in the embodiments of the present invention can be implemented using computer programs and their corresponding general-purpose hardware platforms. Based on this understanding, the technical solutions in the embodiments of the present invention, or the parts that contribute to the prior art, can be embodied in the form of computer programs, i.e., software products. These computer program software products can be stored in a storage medium and include several instructions to cause a device containing a data processing unit (which may be a personal computer, server, microcontroller, or network device, etc.) to execute the methods described in various embodiments or certain parts of the embodiments of the present invention.

[0112] The above description is merely a preferred embodiment of the present invention. It should be noted that those skilled in the art can make various improvements and modifications without departing from the principles of the present invention, and these improvements and modifications should also be considered within the scope of protection of the present invention. All components not explicitly stated in this embodiment can be implemented using existing technology.

Claims

1. A combined navigation method based on sea state estimation, adaptive extended state observer, and Kalman filter, characterized in that, Includes the following steps: S1, the standard deviation of the roll angular velocity is calculated using a sliding window. Real-time statistics are collected to assess the intensity of current sea state disturbances. S2, dynamically adjusts the bandwidth β of the Extended State Observer (ESO) based on sea state level. 02 The process noise Q of the Kalman filter KF; S3 uses an adaptive extended state observer to estimate external disturbances in the vertical and attitude channels and compensates for non-modeling terms in the INS dynamic model in real time. S4 integrates GNSS and IMU information and uses an adaptive process noise model for state prediction and updating, achieving high-precision estimation and error suppression of key navigation states such as attitude, velocity, and position. S5 outputs the fused navigation results and feeds back the disturbance estimate and sea state registration information to the adaptive module to build an online closed-loop adjustment mechanism.

2. The integrated navigation method based on sea state estimation, adaptive extended state observer, and Kalman filter according to claim 1, characterized in that, The standard deviation of roll angle angular velocity calculated using a sliding window, as described in S1, is... In the formula Represents the roll angular velocity at the i-th sampling time; This represents the average roll rate within the window; N: Represents the length of the sliding window; k: indicates the k-th time.

3. The integrated navigation method based on sea state estimation, adaptive extended state observer, and Kalman filter according to claim 1, characterized in that, S1 describes real-time statistical analysis to perceive the intensity of current sea state disturbances and classify sea state registration into: In the formula: Level represents the sea state level; θ1 and θ2 are empirically set thresholds, with θ1 = 0.005 rad / s and θ2 = 0.015 rad / s.

4. The integrated navigation method based on sea state estimation, adaptive extended state observer, and Kalman filter according to claim 1, characterized in that, The bandwidth β of the extended state observer (ESO) dynamically adjusted according to sea state level, as described in S2, is... 02 The process noise Q of the Kalman filter KF: Where: β 02 This represents the ESO bandwidth parameter, which controls the dynamic response speed of the observer; Q represents the noise covariance matrix of the Kalman filtering process, corresponding to states such as attitude angle, angular velocity, and velocity.

5. The integrated navigation method based on sea state estimation, adaptive extended state observer, and Kalman filter according to claim 1, characterized in that, S3 describes using an adaptive extended state observer to estimate external disturbances in the vertical and attitude channels, and to compensate for non-modeling terms in the INS dynamic model in real time, employing a third-order linear ESO model: In the formula: y represents the system input quantity; z1 represents the system state estimate; z2 represents the first derivative estimate; z3 represents the extended state of ESO and is used as a disturbance compensation input; β1, β2, β3 represent parameters that are related to the ESO bandwidth parameters.

6. The method for studying ship heave measurement based on a time-delay-free complementary bandpass filter according to claim 1, characterized in that: The relationship between β1, β2, β3 and the ESO bandwidth parameters described in S3 is as follows:

7. The integrated navigation method based on sea state estimation, adaptive extended state observer, and Kalman filter according to claim 1, characterized in that, S4 describes the fusion of GNSS and IMU information, utilizing an adaptive process noise model for state prediction and updating. This enables high-precision estimation and error suppression of key navigation states such as attitude, velocity, and position. The Kalman filtering process is as follows: P k / k-1 =FP k-1 / k-1 F T +Q k K k =P k / k-1 H T (HP k / k-1 H T +R) -1 P k / k =(I-K k H)P k / k-1 In the formula: This represents the state estimate at the current moment; F represents the state transition matrix; H represents the observation matrix; Q k This represents the process noise covariance matrix adjusted for sea state levels. R represents the observation noise covariance matrix; P k / k This represents the state covariance matrix at the current moment; P k / k-1 This represents the estimated state covariance matrix; K represents the gain matrix; z k This represents the observation at time k.

8. The integrated navigation method based on sea state estimation, adaptive extended state observer, and Kalman filter according to claim 1, characterized in that, The S5 system outputs fused navigation results and feeds back disturbance estimates and sea state registration information to the adaptive module to construct an online closed-loop adjustment mechanism. The fused navigation results output by S5 are as follows: In the formula: φ, θ, and ψ represent pitch, roll, and yaw angles, respectively; v x ,v y ,v z These represent the velocities in the x, y, and z directions, respectively. x, y, z represent the positions in the x, y, and z directions, respectively.

9. A combined navigation method based on sea state estimation, adaptive extended state observer, and Kalman filter according to claims 1-8, characterized in that, The invented methods include: First, the ship's roll rate is monitored in real time using the sea state sensing module, and its standard deviation is calculated to assess the disturbance intensity of the current sea state, classifying the sea state into three levels: calm, moderate, and severe. Then, based on the sea state level, the system dynamically adjusts the bandwidth parameter β of the ESO (Electronic State Oscillator). 02 The process noise covariance matrix Q of the Kalman filter enables the system to optimize response speed and model uncertainty tolerance under different sea states. Based on this, the ESO module estimates and compensates for unmodeled disturbances in the system in real time, reducing the impact of the external environment on navigation accuracy. Next, an adaptively adjusted Kalman filter is used to fuse IMU and GNSS observation data to accurately estimate the ship's attitude, speed, and position. Finally, the system outputs the final navigation state and feeds back sea state information and disturbance estimates to the sea state perception and parameter adjustment module, forming a closed-loop feedback mechanism to further optimize parameter adjustments and improve the system's adaptability and accuracy. This method can effectively cope with complex and harsh sea state environments, providing more stable and accurate positioning services in ship navigation.

Images