A heave disturbance compensation method and device for navigation of a waterborne carrier

Abstract

The application provides a heave disturbance compensation method and device for navigation of a water-borne carrier. The method comprises: acquiring inertial measurement data and relative seabed state data of the water-borne carrier collected by an inertial measurement unit and a Doppler velocimeter in real time, and determining inertial navigation state data and inertial navigation real-time height values of the water-borne carrier based on the inertial measurement data; determining sea wave state parameters of a water environment in which the water-borne carrier is located based on the bottom height values and the inertial navigation real-time height values, and predicting heave displacement values and heave velocity values by using a sea wave heave disturbance model, and then determining height observation values of the water environment; and determining target heave displacement values, target heave velocity values and target navigation state data by using an extended Kalman filtering fusion method. By the above method, the influence of sea wave heave disturbance on navigation accuracy is suppressed, and the navigation accuracy and stability of the water-borne carrier in complex sea conditions are improved.

Description

Technical Field

[0001] This application relates to the field of navigation technology for waterborne vehicles, and in particular to a method and apparatus for compensating for heave disturbances in navigation for waterborne vehicles. Background Technology

[0002] In real marine environments, waterborne vehicles are often subjected to periodic heave motion by ocean waves. This heave motion introduces periodic errors through the height measurement channel of the Doppler Velocity Log (DVL). When these errors are not effectively modeled, they can lead to problems such as vertical estimation oscillations, increased filtering residuals, and decreased navigation accuracy in the integrated navigation system. In severe cases, it can even lead to decreased filtering stability, affecting the operational safety and efficiency of the waterborne vehicle.

[0003] Currently, existing navigation methods for waterborne vessels typically rely on inertial measurement units (IMUs) for state prediction and utilize velocity or altitude information from drift volume (DVL) for state correction. These methods generally assume that DVL observation errors are white noise or simple random processes, and often do not explicitly consider the correlation disturbances caused by ocean waves, leading to a significant decrease in filtering performance under varying sea conditions. Some existing technologies reduce the observation weights of DVL by amplifying the observation noise covariance to mitigate the impact of ocean waves, but this cannot accurately distinguish between effective observations and wave interference components, easily resulting in insufficient utilization of effective information and reduced overall system accuracy. Furthermore, heave information can be obtained through external sensors, but these methods require additional sensors that need to be installed and calibrated before use, increasing system cost and complexity. Summary of the Invention

[0004] In view of this, the purpose of this application is to provide a method and device for compensating for heave disturbances in navigation of a waterborne vehicle. By acquiring real-time inertial measurement data and relative seabed state data of the waterborne vehicle, the inertial navigation state data and real-time height value of the inertial navigation system are determined. Then, based on the height data, the wave state parameters are determined, and the heave displacement and velocity are predicted by the wave heave disturbance model. After constructing the height observation values, the target heave parameters and target navigation state data are obtained by fusion through extended Kalman filtering. This solves the problems of existing technologies, such as not explicitly considering wave-related disturbances, requiring additional sensor configuration, navigation error accumulation, poor filtering performance, and high system complexity. It achieves the technical effect of suppressing the impact of wave heave disturbances on navigation accuracy without increasing system cost and complexity, and improving the navigation accuracy and stability of waterborne vehicles in complex sea conditions.

[0005] This application provides a method for compensating for heave disturbances during navigation on a waterborne platform, the method comprising: During the current compensation cycle, the inertial measurement data corresponding to the waterborne vehicle is acquired in real time by the inertial measurement unit set on the waterborne vehicle, and the relative seabed state data corresponding to the waterborne vehicle is acquired in real time by the Doppler velocimeter. Based on the inertial measurement data, the inertial navigation state data and the real-time inertial navigation altitude value corresponding to the waterborne vehicle are determined. Based on the bottom height value in the relative seabed state data and the real-time height value of the inertial navigation system, the wave state parameters corresponding to the water environment in which the waterborne carrier is located are determined, and the heave displacement value and heave velocity value corresponding to the water environment are predicted using a preset wave heave disturbance model. Based on the heave displacement value, the bottom height value, and the inertial navigation real-time height value, the height observation value corresponding to the water environment is determined; Based on the inertial measurement data, the heave displacement value, the heave velocity value, the wave state parameters, the height observation value, the bottom velocity value in the relative seabed state data, and the inertial navigation state data, the target heave displacement value, target heave velocity value, and target navigation state data corresponding to the current compensation cycle are determined using the extended Kalman filter fusion method.

[0006] Furthermore, the inertial measurement data includes at least three-axis angular velocity data and three-axis acceleration data; the inertial navigation state data includes at least attitude parameters, three-dimensional velocity parameters, and three-dimensional position parameters; the determination of the inertial navigation state data and real-time altitude value of the waterborne vehicle based on the inertial measurement data includes: Based on the triaxial angular velocity data, the attitude parameters corresponding to the waterborne vehicle are determined using a preset quaternion and Euler angle update method; wherein, the attitude parameters include at least roll angle, pitch angle and heading angle. The three-axis acceleration data is transformed from the carrier coordinate system to the navigation coordinate system, and preset interference terms in the three-axis acceleration data are removed, so that the three-dimensional velocity parameters corresponding to the waterborne carrier can be obtained by numerical integration; wherein, the three-dimensional velocity parameters include at least lateral velocity value, longitudinal velocity value and vertical velocity value; The three-dimensional velocity parameters are numerically integrated to obtain the three-dimensional position parameters corresponding to the waterborne carrier; wherein the three-dimensional position parameters include at least longitude, latitude and vertical height values; The real-time altitude value of the inertial navigation system corresponding to the waterborne carrier is determined from the three-dimensional position parameters.

[0007] Furthermore, the wave state parameters include at least the wave principal period, wave principal frequency, wave significant wave height, and wave stochastic process intensity; determining the wave state parameters corresponding to the water environment in which the waterborne carrier is located, based on the bottom height value in the relative seabed state data and the real-time height value of the inertial navigation system, includes: The height difference between the bottom height value in the relative seabed state data and the real-time height value of the inertial navigation system is determined, and based on the height difference, the zero-crossing time is detected using the zero-crossing method to determine the main period of the ocean waves. The main wave frequency is determined based on the main wave period; Determine the standard deviation of the sequence corresponding to the height difference, and determine the significant wave height based on the standard deviation; The intensity value of the random process of the ocean waves is determined based on the significant wave height and the dominant wave frequency.

[0008] Furthermore, the wave heave disturbance model is constructed based on the wave spectrum model and incorporates heave displacement and heave velocity values ​​as extended state variables; the prediction of the heave displacement and heave velocity values ​​corresponding to the water environment using the preset wave heave disturbance model includes: Obtain the wave heave disturbance coefficient corresponding to the water environment; wherein, the wave heave disturbance coefficient includes at least the wave dominant frequency coefficient, the wave damping coefficient, and the process noise coefficient; Based on the wave dominant frequency coefficient, the wave damping coefficient, the process noise coefficient, and the historical target heave displacement and historical target heave velocity values ​​corresponding to the previous compensation cycle, the heave velocity value of the water environment corresponding to the current compensation cycle is determined using the heave velocity state equation in the preset wave heave disturbance model. Based on the historical target heave velocity, the heave displacement value of the water environment corresponding to the current compensation cycle is determined using the heave displacement state equation in the wave heave disturbance model.

[0009] Furthermore, determining the height observation value corresponding to the water environment based on the heave displacement value, the bottom height value, and the inertial navigation real-time height value includes: The initial altitude value of the inertial navigation system, the initial altitude value of the bottom alignment, and the historical target heave displacement value corresponding to the previous compensation cycle are obtained respectively. Based on the historical target heave displacement value and the heave displacement value, the heave compensation displacement value is determined. Determine the inertial navigation altitude difference between the real-time altitude value and the initial altitude value, and determine the bottom altitude difference between the bottom-aligned altitude value and the initial bottom-aligned altitude value; Based on the inertial navigation height difference, the bottom height difference, and the heave compensation displacement value, the height observation value corresponding to the water environment is determined.

[0010] Furthermore, the step of determining the target heave displacement value, target heave velocity value, and target navigation status data corresponding to the current compensation cycle based on the inertial measurement data, the heave displacement value, the heave velocity value, the wave state parameters, the height observation value, the bottom velocity value in the relative seabed state data, and the inertial navigation status data, using the extended Kalman filter fusion method, includes: Based on the inertial measurement data, the historical target heave displacement value and historical target heave velocity value corresponding to the previous compensation cycle, the wave state parameters and the inertial navigation state data, an extended Kalman filter time update is performed to obtain the predicted heave displacement value, predicted heave velocity value, predicted navigation state data and predicted covariance matrix corresponding to the current compensation cycle. Based on the height observation value, the bottom velocity value in the relative seabed state data, the predicted heave displacement value, the predicted heave velocity value, and the predicted navigation state data, the velocity observation residual and the height observation residual are calculated. Based on the predicted covariance matrix, the preset observation matrix, and the preset observation noise covariance matrix, the Kalman gain matrix corresponding to the current compensation period is calculated. The Kalman gain matrix is ​​weighted and fused with the velocity observation residual and the altitude observation residual to correct the predicted heave displacement value, predicted heave velocity value and predicted navigation status data, and to determine the target heave displacement value, target heave velocity value and target navigation status data corresponding to the current compensation cycle.

[0011] Furthermore, the step of determining the target heave displacement value, target heave velocity value, and target navigation status data corresponding to the current compensation cycle using an extended Kalman filter fusion method based on the inertial measurement data, the heave displacement value, the heave velocity value, the wave state parameters, the height observation value, the bottom velocity value in the relative seabed state data, and the inertial navigation status data, also includes: Based on the Kalman gain matrix, the prediction covariance matrix, and the observation matrix, the posterior covariance matrix is ​​updated to perform extended Kalman filter prediction calculation for the next compensation cycle.

[0012] This application embodiment also provides a heave disturbance compensation device for navigation of a waterborne vehicle, the device comprising: The data acquisition module is used to acquire, in the current compensation cycle, the inertial measurement data corresponding to the waterborne carrier collected in real time by the inertial measurement unit set on the waterborne carrier and the relative seabed state data corresponding to the waterborne carrier collected in real time by the Doppler velocimeter, and determine the inertial navigation state data and real-time inertial navigation height value corresponding to the waterborne carrier. The model prediction module is used to determine the wave state parameters corresponding to the water environment in which the water carrier is located based on the bottom height value in the relative seabed state data and the real-time height value of the inertial navigation system, and to predict the heave displacement value and heave velocity value corresponding to the water environment using a preset wave heave disturbance model. The height observation module is used to determine the height observation value corresponding to the water environment based on the heave displacement value, the bottom height value, and the inertial navigation real-time height value. The disturbance compensation module is used to determine the target heave displacement value, target heave velocity value, and target navigation status data corresponding to the current compensation period based on the inertial measurement data, the heave displacement value, the heave velocity value, the wave state parameters, the height observation value, the bottom velocity value in the relative seabed state data, and the inertial navigation status data, using the extended Kalman filter fusion method.

[0013] This application embodiment also provides an electronic device, including: a processor, a memory, and a bus. The memory stores machine-readable instructions executable by the processor. When the electronic device is running, the processor communicates with the memory via the bus. When the machine-readable instructions are executed by the processor, the steps of the heave disturbance compensation method for navigation of waterborne carriers described above are performed.

[0014] This application also provides a computer-readable storage medium storing a computer program, which, when executed by a processor, performs the steps of the above-described method for compensating for heave disturbances in navigation of a waterborne carrier.

[0015] The present application provides a method and apparatus for compensating for heave disturbances in navigation of a waterborne vehicle. The method includes: in the current compensation cycle, acquiring inertial measurement data corresponding to the waterborne vehicle collected in real time by an inertial measurement unit installed on the waterborne vehicle, and relative seabed state data corresponding to the waterborne vehicle collected in real time by a Doppler velocimeter; and determining inertial navigation state data and real-time inertial navigation height value corresponding to the waterborne vehicle based on the inertial measurement data; and determining wave state parameters corresponding to the water environment in which the waterborne vehicle is located based on the bottom height value in the relative seabed state data and the real-time inertial navigation height value. The system uses a preset wave heave disturbance model to predict the heave displacement and heave velocity values ​​corresponding to the water environment. Based on the heave displacement values, the bottom elevation value, and the inertial navigation real-time elevation value, it determines the elevation observation value corresponding to the water environment. Based on the inertial measurement data, the heave displacement values, the heave velocity values, the wave state parameters, the elevation observation values, the bottom elevation velocity value in the relative seabed state data, and the inertial navigation status data, it uses an extended Kalman filter fusion method to determine the target heave displacement value, target heave velocity value, and target navigation status data corresponding to the current compensation cycle.

[0016] Compared with existing technologies that predict state using inertial measurement units (IMUs) and correct state using velocity or altitude information from DVLs, or reduce the observation weight of DVLs by amplifying the observation noise covariance to mitigate the impact of ocean waves, or obtain heave information through external sensors, this new method obtains real-time IMU data and relative seabed state data from the waterborne vehicle, determines the inertial navigation state data and real-time altitude value, then determines ocean wave state parameters based on the altitude data, predicts heave displacement and velocity using an ocean wave heave disturbance model, constructs altitude observations, and fuses them using an extended Kalman filter to obtain target heave parameters and target navigation state data. This method solves the problems of existing technologies, such as not explicitly considering ocean wave-related disturbances, requiring additional sensors, navigation error accumulation, poor filtering performance, and high system complexity. It achieves the technical effect of suppressing the impact of ocean wave heave disturbances on navigation accuracy without increasing system cost and complexity, and improving the navigation accuracy and stability of waterborne vehicles in complex sea conditions.

[0017] To make the above-mentioned objectives, features and advantages of this application more apparent and understandable, preferred embodiments are described below in detail with reference to the accompanying drawings. Attached Figure Description

[0018] To more clearly illustrate the technical solutions of the embodiments of this application, the accompanying drawings used in the embodiments will be briefly introduced below. It should be understood that the following drawings only show some embodiments of this application and should not be regarded as a limitation of the scope. For those skilled in the art, other related drawings can be obtained based on these drawings without creative effort.

[0019] Figure 1 A flowchart of a heave disturbance compensation method for navigation of a waterborne carrier provided in an embodiment of this application; Figure 2 This is a schematic diagram of the effect curve of a height error comparison test provided in an embodiment of this application; Figure 3 A schematic diagram of a heave disturbance compensation device for navigation of a waterborne vehicle provided in an embodiment of this application; Figure 4 This is a schematic diagram of the structure of an electronic device provided in an embodiment of this application. Detailed Implementation

[0020] To make the objectives, technical solutions, and advantages of the embodiments of this application clearer, 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. The components of the embodiments of this application described and shown in the accompanying drawings can generally be arranged and designed in various different configurations. Therefore, the following detailed description of the embodiments of this application provided in the accompanying drawings is not intended to limit the scope of the claimed application, but merely represents selected embodiments of this application. Based on the embodiments of this application, every other embodiment obtained by those skilled in the art without inventive effort falls within the scope of protection of this application.

[0021] Research has shown that underwater unmanned platforms (such as ROVs and AUVs) have been widely used in marine exploration, resource development, and underwater operations. Their operating environments are complex and varied, especially in nearshore and medium-to-high sea states where wave action is significant, placing higher demands on the accuracy and stability of navigation systems.

[0022] Inertial navigation systems (INS) are a core component of navigation for waterborne vehicles. They offer advantages such as high autonomy and update rates, enabling them to autonomously calculate the vehicle's attitude, velocity, and position information without relying on external signals. However, they have an inherent drawback—errors accumulate over time, leading to a significant decrease in navigation accuracy after long-term operation. Therefore, INS typically requires integration with other sensors to compensate for their inherent error accumulation limitations.

[0023] Doppler velocity logs (DVLs) are commonly used sensors in INS-integrated navigation systems. They can provide information on the velocity of the vehicle relative to the seabed (i.e., bottom velocity) and the height of the vehicle relative to the seabed (i.e., bottom height). Therefore, INS / DVL integrated navigation is widely used in the navigation of marine platforms, surface ships, and underwater robots.

[0024] However, in actual marine environments, waterborne vehicles are often subjected to periodic heave motion caused by ocean waves. This heave motion introduces periodic errors through the height measurement channel of the Doppler Velocity Log (DVL). When these errors are not effectively modeled, they can lead to problems such as vertical estimation oscillations, increased filtering residuals, and decreased navigation accuracy in the integrated navigation system. In severe cases, it may even cause a decrease in filtering stability, affecting the operational safety and efficiency of the waterborne vehicle.

[0025] Currently, existing navigation methods for waterborne vehicles typically rely on inertial measurement units (IMUs) for state prediction and use velocity or altitude information from drift volume (DVL) for state correction. These methods generally assume that the observation error of DVL is white noise or a simple random process and usually do not explicitly consider the correlation disturbances caused by ocean waves, resulting in a significant decrease in filtering performance under sea conditions.

[0026] Some existing technologies reduce the observation weight of DVL by amplifying the observation noise covariance to mitigate the impact of ocean waves, but they cannot accurately distinguish between effective observations and ocean wave interference components, which can easily lead to insufficient utilization of effective information and reduce the overall accuracy of the system. In addition, heave information can be obtained through external sensors, but these methods require additional sensors, which need to be installed and calibrated before use, increasing the system cost and complexity.

[0027] Based on this, this application provides a method for compensating for heave disturbances in navigation of a waterborne vehicle. By acquiring real-time inertial measurement data and relative seabed state data of the waterborne vehicle, the inertial navigation state data and real-time height value of the inertial navigation system are determined. Then, based on the height data, the wave state parameters are determined, and the heave displacement and velocity are predicted through a wave heave disturbance model. After constructing the height observation values, the target heave parameters and target navigation state data are obtained by fusing them through an extended Kalman filter. This method solves the problems of existing technologies, such as not explicitly considering wave-related disturbances, requiring additional sensors, navigation error accumulation, poor filtering performance, and high system complexity. It achieves the technical effect of suppressing the impact of wave heave disturbances on navigation accuracy without increasing system cost and complexity, and improving the navigation accuracy and stability of waterborne vehicles in complex sea conditions.

[0028] Please see Figure 1 , Figure 1 This is a flowchart illustrating a heave disturbance compensation method for navigation of a waterborne vehicle, provided as an embodiment of this application. Figure 1 As shown in the embodiments of this application, the heave disturbance compensation method for navigation of a waterborne vehicle includes: S101. During the current compensation cycle, acquire the inertial measurement data corresponding to the waterborne carrier, which is collected in real time by the inertial measurement unit set on the waterborne carrier, and the relative seabed state data corresponding to the waterborne carrier, which is collected in real time by the Doppler velocimeter. Based on the inertial measurement data, determine the inertial navigation state data and the real-time altitude value of the inertial navigation system corresponding to the waterborne carrier.

[0029] In this embodiment, the current compensation period refers to the preset filter update period. In combination with actual engineering applications, for example, the compensation period can be set to 1Hz (i.e., a complete heave disturbance compensation process is executed once per second), which is consistent with the update frequency of the Extended Kalman Filter (EKF).

[0030] The inertial measurement unit (IMU) is the core sensing component installed on the waterborne vehicle, used to collect real-time motion state data of the waterborne vehicle. For example, its sampling frequency is set to 200Hz to ensure the real-time and continuous nature of the collected data. The Doppler velocimeter (DVL) is also installed on the waterborne vehicle, used to collect real-time motion state data of the waterborne vehicle relative to the seabed. For example, its sampling frequency is set to 5Hz, which is coordinated and adapted with the IMU sampling frequency to balance data accuracy and system computing power.

[0031] Here, in this embodiment of the application, the inertial measurement data is the raw motion state data of the waterborne vehicle collected by the inertial measurement unit, including at least three-axis angular velocity data (i.e., the rotational speed of the waterborne vehicle about the three coordinate axes x, y, and z) and three-axis acceleration data (i.e., the linear acceleration of the waterborne vehicle along the three coordinate axes x, y, and z); the relative seabed state data is the motion state data of the waterborne vehicle relative to the seabed collected by the Doppler velocimeter, including at least the bottom velocity value (i.e., the motion speed of the waterborne vehicle relative to the seabed) and the bottom height value (i.e., the vertical distance of the waterborne vehicle relative to the seabed).

[0032] In this embodiment of the application, the waterborne carrier is equipped with a navigation system. The navigation device includes an inertial measurement unit (IMU), a Doppler velocimeter (DVL), and a microcontroller (MCU). The IMU and DVL are both connected to the MCU through a serial communication interface to transmit measurement data to the MCU in real time. The MCU performs unified management, processing, and fusion calculation of the data from each sensor. The signals from each sensor are uniformly transmitted to the microcontroller, which performs unified data fusion and state estimation.

[0033] In this step, the determination of inertial navigation status data and real-time altitude value based on inertial measurement data is achieved through basic inertial navigation (INS) calculation. That is, relying solely on the three-axis angular velocity data and three-axis acceleration data collected by the inertial measurement unit, the inertial navigation status data of the water vehicle is autonomously calculated through the sequence of attitude calculation, velocity calculation, and position calculation, and the real-time altitude value of the inertial navigation is extracted from the position data (i.e., the vertical altitude of the water vehicle calculated based on inertial navigation).

[0034] In this embodiment, basic motion state data (inertial measurement data and relative seabed state data) of the waterborne vehicle are acquired to provide core input data for subsequent wave state parameter estimation, heave disturbance prediction, height observation calculation, and EKF fusion. Through basic inertial navigation calculation, the inertial navigation state data and real-time inertial navigation height value of the waterborne vehicle are obtained, providing a basic benchmark for subsequent comparison with DVL bottom height values ​​and construction of wave models.

[0035] This enables real-time and stable acquisition of motion status data of the waterborne vehicle, ensuring data synchronization and the absence of significant outliers and noise. Through basic inertial navigation calculations, it can output high-frequency inertial navigation status data and real-time inertial navigation height values ​​of the waterborne vehicle, ensuring data continuity and real-time responsiveness. This provides reliable basic data support for subsequent heave disturbance compensation processes and maintains the waterborne vehicle's autonomous navigation capability for a short period of time in the event of short-term DVL failure.

[0036] In one possible implementation of this application, in specific implementation, step S101, which determines the inertial navigation state data and real-time inertial navigation altitude value corresponding to the waterborne carrier based on the inertial measurement data, may include: S1011. Based on the triaxial angular velocity data, the attitude parameters corresponding to the waterborne carrier are determined using a preset quaternion and Euler angle update method.

[0037] The attitude parameters include at least the roll angle, pitch angle, and yaw angle.

[0038] Here, the three-axis angular velocity data is the rotational rate of the waterborne vehicle around the x, y, and z coordinate axes collected by the IMU, with the unit being rad / s. It reflects the attitude change of the waterborne vehicle. The quaternion and Euler angle update method is a commonly used attitude calculation method in inertial navigation. Quaternions are used to avoid the gimbal lock problem in the Euler angle update process, while Euler angles are used to intuitively represent the attitude of the waterborne vehicle. The combination of the two achieves accurate calculation of attitude parameters.

[0039] In the embodiments of this application, attitude parameters are the core parameters characterizing the spatial attitude of the waterborne vehicle. The roll angle is the rotation angle of the waterborne vehicle about the x-axis, the pitch angle is the rotation angle of the waterborne vehicle about the y-axis, and the heading angle is the rotation angle of the waterborne vehicle about the z-axis. The three together determine the attitude state of the waterborne vehicle in space.

[0040] For example, suppose the triaxial angular velocity data acquired by the IMU includes ω x =0.02rad / s, ω =0.01rad / s, ω z =0.03 rad / s, the initial quaternion value is preset to [1, 0, 0, 0], and the quaternion update formula is used: = 0.5×q×ω (where q is a quaternion and ω is the three-axis angular velocity vector), calculate the current quaternion, and then convert the quaternion to Euler angles using the conversion formula between quaternions and Euler angles to obtain the roll angle value of 1.15°, the pitch angle value of 0.57°, and the heading angle value of 1.72°, thus obtaining the attitude parameters of the waterborne vehicle.

[0041] S1012. The three-axis acceleration data is transformed from the carrier coordinate system to the navigation coordinate system, and preset interference terms in the three-axis acceleration data are removed, so as to obtain the three-dimensional velocity parameters corresponding to the water carrier through numerical integration.

[0042] The three-dimensional velocity parameters include at least the lateral velocity value, the longitudinal velocity value, and the vertical velocity value.

[0043] Here, the carrier coordinate system is a coordinate system fixed on the water carrier, with its x-axis pointing forward along the longitudinal direction of the carrier, its y-axis pointing to the right along the transverse direction of the carrier, and its z-axis pointing upward perpendicular to the plane of the carrier; the navigation coordinate system adopts the Northeast-Universe (ENU) coordinate system, with its x-axis pointing east, its y-axis pointing north, and its z-axis pointing to the sky. The conversion between the two is achieved through attitude parameters (Euler angles), that is, by using a rotation matrix to convert the linear acceleration data in the carrier coordinate system to the navigation coordinate system.

[0044] In this embodiment, the preset interference refers to the non-carrier motion acceleration components contained in the linear acceleration data, mainly including gravitational acceleration and Coriolis acceleration. The gravitational acceleration is along the negative z-axis of the navigation coordinate system and has a magnitude of 9.8 m / s². The Coriolis acceleration is an additional acceleration caused by the Earth's rotation and the carrier's motion, which is calculated and removed by a preset formula. Numerical integration refers to performing time integration on the converted linear acceleration data to obtain the three-dimensional velocity parameters of the carrier. The integration step size is consistent with the IMU sampling period (5 ms).

[0045] Among the three-dimensional velocity parameters, the lateral velocity value is the velocity of the watercraft along the y-axis (northward) of the navigation coordinate system, the longitudinal velocity value is the velocity of the watercraft along the x-axis (eastward) of the navigation coordinate system, and the vertical velocity value is the velocity of the watercraft along the z-axis (upward) of the navigation coordinate system. The unit of all values ​​is m / s.

[0046] For example, suppose the triaxial acceleration data acquired by the IMU is a x =0.2m / s², a =0.1m / s², a_z=9.9m / s² (in the carrier coordinate system). Using the obtained attitude parameters, a rotation matrix is ​​constructed to transform the linear acceleration data to the navigation coordinate system, resulting in a_E=0.198m / s², a_N=0.102m / s², a_U=0.1m / s² (after removing gravitational acceleration of 9.8m / s² and Coriolis acceleration of 0.002m / s²). The trapezoidal integral method is used to integrate the linear acceleration data in the navigation coordinate system (integration step size 5ms) to obtain the three-dimensional velocity parameters: lateral velocity (northward) is 0.51m / s, longitudinal velocity (eastward) is 0.99m / s, and vertical velocity (upward) is 0.005m / s.

[0047] S1013. Perform numerical integration on the three-dimensional velocity parameters to obtain the three-dimensional position parameters corresponding to the waterborne carrier.

[0048] The three-dimensional position parameters include at least longitude, latitude, and vertical altitude values.

[0049] In this step, numerical integration of the three-dimensional velocity parameters refers to integrating the lateral velocity value, longitudinal velocity value, and vertical velocity value over time with the IMU sampling period (5ms) as the integration step size, to obtain the three-dimensional position parameters of the waterborne carrier.

[0050] Among them, longitude and latitude reflect the horizontal position of the waterborne vehicle, and vertical height reflects the vertical position of the waterborne vehicle, with units of degrees (°) and meters (m), respectively.

[0051] Here, during the integration process, parameters such as the Earth's radius are used to convert the velocity integration result into latitude and longitude coordinates to ensure the accuracy of the position parameters; at the same time, the accumulated error in the integration process is initially suppressed to ensure the stability of the position parameters.

[0052] For example, assuming the obtained three-dimensional velocity parameters are: lateral velocity 0.51 m / s, longitudinal velocity 0.99 m / s, and vertical velocity 0.005 m / s, the trapezoidal integration method is used with an integration step of 5 ms. Combining the Earth's radius of 6371 km, the velocity integration results are converted into latitude and longitude to obtain the three-dimensional position parameters: longitude 116.38°, latitude 39.90°, and vertical height 10.2 m.

[0053] S1014. Determine the real-time inertial navigation height value corresponding to the waterborne carrier in the three-dimensional position parameters.

[0054] In this embodiment, the real-time inertial navigation height value is the vertical height calculated by the waterborne carrier based on inertial navigation. It is directly extracted from the obtained three-dimensional position parameters. That is, the vertical height value in the three-dimensional position parameters is the real-time inertial navigation height value, with the unit being meters (m). It is used to compare with the bottom height value to calculate the height difference and support the estimation of wave state parameters.

[0055] S102. Based on the bottom height value in the relative seabed state data and the real-time height value of the inertial navigation system, determine the wave state parameters corresponding to the water environment in which the waterborne carrier is located, and use a preset wave heave disturbance model to predict the heave displacement value and heave velocity value corresponding to the water environment.

[0056] Here, the bottom height value is the actual vertical height of the waterborne vehicle relative to the seabed collected by DVL, and the real-time height value of the inertial navigation system is the vertical height of the waterborne vehicle obtained by the INS basic calculation. The difference between the two can reflect the influence of wave heave disturbance on the two height measurement methods. Therefore, the wave state parameters can be determined based on this height difference.

[0057] Among them, wave state parameters are the core parameters characterizing the wave characteristics of the aquatic environment. They include at least the wave principal period, wave principal frequency, wave significant wave height, and wave stochastic process intensity, which can comprehensively reflect the changing characteristics of the current sea state.

[0058] In the embodiments of this application, the preset wave heave disturbance model is constructed based on the Pierson–Moskowitz wave spectrum model (a classic wave energy spectrum model that can accurately describe the energy distribution characteristics of waves). Its core is to incorporate the heave displacement value and heave velocity value as extended state variables into the state estimation system, and to model the heave motion of the water carrier caused by the waves as a time-dependent stochastic dynamic process, which can adapt to the periodic variation characteristics of the waves.

[0059] Here, the heave displacement value refers to the vertical displacement of the waterborne carrier under the action of ocean waves, and the heave velocity value refers to the vertical velocity of the waterborne carrier. Both are predicted by the ocean wave heave disturbance model. The prediction process needs to combine the ocean wave state parameters and the historical target heave displacement value and historical target heave velocity value of the previous compensation cycle to ensure the continuity and accuracy of the prediction results.

[0060] In this step, the wave characteristics of the current water environment are accurately identified, and wave-related information is extracted through height difference to determine wave state parameters. Based on the wave heave disturbance model, the heave displacement and heave velocity values ​​under the current sea state are predicted, providing wave disturbance-related data for subsequent height observation calculation and EKF fusion, realizing explicit modeling of wave heave disturbance, and solving the deficiency of existing technologies that do not consider wave-related disturbances.

[0061] In this way, wave state parameters can be estimated in real time and adaptively, adapting to changes in small, medium and large sea conditions, reducing the estimation errors of the main wave period and significant wave height, and ensuring the accuracy of the wave characteristic description. The heave displacement and heave velocity values ​​predicted by the wave heave disturbance model can accurately match the periodic variation law of the waves, providing reliable disturbance data for subsequent heave disturbance compensation, and laying the foundation for suppressing wave interference in DVL height observation.

[0062] In one possible implementation of this application, in specific implementation, the step S102 of determining the wave state parameters corresponding to the water environment where the waterborne carrier is located based on the bottom height value in the relative seabed state data and the real-time height value of the inertial navigation system may include: S1021. Determine the height difference between the bottom height value in the relative seabed state data and the real-time height value of the inertial navigation system, and based on the height difference, use the zero-crossing method to detect the moment of crossing the zero point to determine the main period of the ocean waves.

[0063] Here, the height difference refers to the difference between the bottom height value (h_DVL) collected by DVL and the real-time height value (h_INS) calculated by INS, i.e., Δh = h_INS - h_DVL. This difference can reflect the influence of wave heave disturbance on the two height measurement methods, and its variation pattern is consistent with the periodic variation of the waves.

[0064] In the embodiments of this application, the zero-crossing method is a commonly used method for detecting the periodicity of periodic signals. Its core is to detect the moment when the value changes from negative to positive (crosses zero) in the height difference sequence. The time interval between two adjacent moments of crossing zero is the half period (Tp_half) of the wave. The main period (Tp) of the wave is twice the half period, that is, Tp=2×Tp_half. The main period of the wave is the core parameter characterizing the periodicity of the wave, and its unit is seconds (s).

[0065] For example, suppose the collected height difference Δh sequence is: -0.02, -0.01, 0.01, 0.02, 0.01, -0.01, -0.02, -0.01, 0.01, 0.02; using the zero-crossing method to detect the zero-crossing time, the zero-crossing time is found to be at the 3s and 9s respectively, and the time interval between the two times is 6s (i.e., half period Tp_half=6s). Therefore, the main period of the wave Tp=2×6=12s (corresponding to the sea state scenario).

[0066] S1022. Determine the main wave frequency based on the main wave period.

[0067] Here, the dominant frequency (ω) of the ocean wave is 2π times the reciprocal of the dominant period of the ocean wave. Its calculation formula is ω=2π / Tp, and the unit is rad / s. It is used to characterize the vibration frequency of the ocean wave and is the core parameter for constructing the ocean wave heave disturbance model and calculating the intensity value of the ocean wave stochastic process.

[0068] In this embodiment of the application, in order to avoid the EKF filter oscillation caused by the rapid change of the main wave frequency, the update period of the main wave frequency is set to 60s. That is, the main wave frequency is recalculated every 60s based on the latest main wave period to ensure the stability of the parameters.

[0069] S1023. Determine the standard deviation of the sequence corresponding to the height difference, and determine the significant wave height of the sea waves based on the standard deviation.

[0070] In the embodiments of this application, the height difference corresponding sequence refers to a sequence composed of multiple height differences collected within a certain period of time. The standard deviation (σ) is a parameter that characterizes the degree of dispersion of the sequence and can reflect the fluctuation range of the height difference. The fluctuation of the height difference is mainly caused by the heave and swell of the sea waves. Therefore, the standard deviation is fixedly correlated with the effective wave height of the sea waves.

[0071] Among them, the significant wave height (Hs) refers to the statistical average of the larger wave heights in the ocean. Its calculation formula is Hs=4σ, and the unit is meters (m). This parameter can characterize the intensity of the ocean waves and is the core indicator for distinguishing between small, medium and large ocean conditions.

[0072] S1024. Based on the significant wave height and the dominant wave frequency, determine the intensity value of the random process of the waves.

[0073] In the embodiments of this application, the wave random process intensity value (σ_wave) is a parameter characterizing the random characteristics of wave heave disturbance, used to construct the process noise covariance (Q) in the wave heave disturbance model, and its calculation formula is σ_wave=(Hs / 4)×sqrt(2ω).

[0074] Among them, sqrt(·) represents the square root operation. The magnitude of this parameter is positively correlated with the significant wave height and the main wave frequency, and can adapt to real-time changes in sea conditions.

[0075] In one possible implementation of this application, in specific implementation, the step S102 of predicting the heave displacement and heave velocity values ​​corresponding to the water environment using a preset wave heave disturbance model may include: S1025. Obtain the wave heave disturbance coefficient corresponding to the water environment.

[0076] The wave heave disturbance coefficient includes at least the wave dominant frequency coefficient, the wave damping coefficient, and the process noise coefficient.

[0077] Here, the wave heave disturbance coefficient is a key parameter for constructing the wave heave disturbance model, used to characterize the dynamic characteristics of wave heave disturbance. Among them, the wave dominant frequency coefficient is the determined wave dominant frequency (ω), used to describe the vibration frequency of the wave; the wave damping coefficient (ζi) is a parameter characterizing the energy attenuation characteristics of the wave, used to adjust the attenuation rate of the heave velocity; and the process noise coefficient is the determined wave random process intensity value (σ_wave), used to characterize the random noise characteristics of wave heave disturbance.

[0078] In this embodiment, the wave heave disturbance model is constructed based on the “Pierson–Moskowitz” wave spectrum model, which supports single-frequency and dual-frequency wave modeling. In engineering, to balance the real-time performance and complexity of the system, the single-frequency wave model is preferred, and the wave damping coefficient ζi is set to 0.7. If the dual-frequency wave model is used, the damping coefficients corresponding to the two frequencies can be set to 0.55 and 0.25 respectively, depending on the different surge periods.

[0079] For example, if a single-frequency wave model is used, the obtained wave heave disturbance coefficients are: wave dominant frequency coefficient ω≈0.457rad / s, wave damping coefficient ζi=0.7, and process noise coefficient σ_wave≈0.705; if a dual-frequency wave model is used, the coefficients corresponding to the two frequencies are: ω1≈0.5rad / s, ζ1=0.55, σ_wave1≈0.6, ω2≈0.3rad / s, ζ2=0.25, and σ_wave2≈0.4.

[0080] S1026. Based on the wave dominant frequency coefficient, the wave damping coefficient, the process noise coefficient, and the historical target heave displacement value and historical target heave velocity value corresponding to the previous compensation cycle, the heave velocity value of the water environment corresponding to the current compensation cycle is determined using the heave velocity state equation in the preset wave heave disturbance model.

[0081] In this embodiment, the heave velocity state equation is one of the core equations of the wave heave disturbance model, used to describe the dynamic change of the heave velocity value, and its expression is: _wavei = -2ζiωi·v_wavei_prev –ωi²·x_wavei_prev + wi, where: _wavei is the predicted heave velocity value (i.e., heave velocity value) for the current compensation period, v_wavei_prev is the historical target heave velocity value for the previous compensation period, x_wavei_prev is the historical target heave displacement value for the previous compensation period, wi is the process noise (determined by the process noise coefficient σ_wave), ζi is the wave damping coefficient, and ωi is the wave dominant frequency coefficient.

[0082] Here, to adapt to the discretization calculation in actual engineering, the above continuous state equation is discretized, and the discretization formula is: v_wavei(k|k-1) = v_wavei_prev + (-2ζiωi·v_wavei_prev – ωi²·x_wavei_prev)·ΔT + wi; where ΔT is the compensation period (1s), and v_wavei(k|k-1) is the heave velocity value of the current compensation period.

[0083] For example, assuming the historical target heave displacement value x_wavei_prev = 0.2m, the historical target heave velocity value v_wavei_prev = 0.1m / s, the compensation period ΔT = 1s, and the wave heave disturbance coefficients ζi = 0.7, ωi ≈ 0.457rad / s, and σ_wave ≈ 0.705 (process noise wi = σ_wave × randn(), where randn() is a standard normal distribution random number, taken as 0.1 in this embodiment); substituting into the discretization formula, the heave velocity value of the current compensation period is calculated as: v_wavei(k|k-1) = 0.1 + (-2 × 0.7 × 0.457 × 0.1 – 0.457² × 0.2) × 1 + 0.1 ≈ 0.1 + (-0.064 – 0.042) + 0.1 ≈ 0.094m / s.

[0084] S1027. Based on the historical target heave velocity, the heave displacement value of the water environment corresponding to the current compensation cycle is determined using the heave displacement state equation in the wave heave disturbance model.

[0085] In this embodiment, the heave-displacement state equation is another core equation of the wave heave-displacement disturbance model, used to describe the dynamic change law of the heave-displacement value, and its expression is: _wavei = v_wavei_prev, that is, the rate of change of heave displacement is equal to the heave velocity value of the previous compensation cycle; the discretized formula is: x_wavei(k|k-1) = x_wavei_prev + v_wavei_prev·ΔT; where x_wavei(k|k-1) is the heave displacement value of the current compensation cycle, x_wavei_prev is the historical target heave displacement value of the previous compensation cycle, v_wavei_prev is the historical target heave velocity value of the previous compensation cycle, and ΔT is the compensation cycle (1s).

[0086] S103. Based on the heave displacement value, the bottom height value, and the inertial navigation real-time height value, determine the height observation value corresponding to the water environment.

[0087] In this embodiment, the altitude observation value is one of the core observation data used for extended Kalman filter fusion update. Its core function is to remove the wave heave disturbance component in the altitude observation of DVL and retain the effective observation information. The determination of the altitude observation value needs to combine the heave displacement value, the bottom height value, and the real-time altitude value of the inertial navigation system. This is achieved by constructing a compensation equation. That is, by using the difference between the real-time altitude value and the initial altitude value of the inertial navigation system, and the difference between the bottom height value and the initial bottom height value, combined with the heave displacement related compensation term, the influence of wave heave disturbance on the altitude observation is removed, and a pure altitude observation value is obtained.

[0088] Among them, the initial inertial navigation height value is the initial inertial navigation height of the waterborne vehicle before the start of the current compensation cycle; the initial bottom height value is the initial height of the waterborne vehicle relative to the seabed collected by DVL before the start of the current compensation cycle; the heave compensation displacement value is calculated based on the historical target heave displacement value of the previous compensation cycle and the currently predicted heave displacement value, and is used to compensate for the cumulative impact of wave heave disturbance.

[0089] In this step, the wave heave disturbance component in the DVL height observation is eliminated to obtain a height observation that can truly reflect the actual height of the waterborne carrier. This avoids the problem of vertical estimation oscillation and increased filtering residual caused by wave disturbance during extended Kalman filter fusion, and provides reliable vertical observation data for extended Kalman filter fusion update.

[0090] This effectively separates the effective observation components and wave interference components in DVL altitude observations, eliminates periodic observation errors caused by wave rise and fall, and ensures that altitude observations accurately reflect the actual altitude status of the waterborne vehicle. This guarantees the accuracy of subsequent extended Kalman filter fusion and avoids a decrease in navigation accuracy due to distortion of observation data.

[0091] In one possible implementation of this application, step S103 may include: S1031. Obtain the initial inertial navigation height value, the initial bottom alignment height value, and the historical target heave displacement value corresponding to the previous compensation cycle, and determine the heave compensation displacement value based on the historical target heave displacement value and the heave displacement value.

[0092] In this embodiment, the initial inertial navigation altitude value is the initial inertial navigation altitude of the waterborne vehicle before the start of the current compensation cycle, i.e., the real-time inertial navigation altitude value calculated during the first execution; the initial altitude value relative to the seabed is the initial altitude of the waterborne vehicle relative to the seabed collected by DVL before the start of the current compensation cycle, i.e., the altitude value relative to the seabed obtained during the first execution; the historical target heave displacement value is the target heave displacement value determined by extended Kalman filter fusion in the previous compensation cycle, used to calculate the heave compensation displacement value.

[0093] Here, the heave compensation displacement value (Σ^x_wavei) is the core parameter used to compensate for wave heave disturbances. It is the sum of the historical target heave displacement value of the previous compensation cycle and the heave displacement value of the current compensation cycle, that is, Σ^x_wavei = x_wavei_prev + x_wavei(k|k-1), which is used to remove the cumulative effect of wave heave disturbances in the calculation of height observation values.

[0094] For example, assuming the initial inertial navigation height h_INS_init=10.0m, the initial ground clearance height h_DVL_init=9.98m, the historical target heave displacement x_wavei_prev=0.2m in the previous compensation cycle, and the heave displacement x_wavei(k|k-1)=0.3m in the current compensation cycle; then the heave compensation displacement value is: Σ^x_wavei=0.2 + 0.3=0.5m.

[0095] S1032. Determine the inertial navigation altitude difference between the real-time altitude value and the initial altitude value, and determine the bottom altitude difference between the bottom-aligned altitude value and the initial bottom-aligned altitude value.

[0096] In this embodiment, the inertial navigation altitude difference refers to the difference between the real-time inertial navigation altitude value and the initial inertial navigation altitude value in the current compensation period, i.e., Δh_INS = h_INS - h_INS_init, which is used to reflect the altitude change of the waterborne vehicle based on inertial navigation; the bottom altitude difference refers to the difference between the bottom altitude value and the initial bottom altitude value in the current compensation period, i.e., Δh_DVL = h_DVL - h_DVL_init, which is used to reflect the altitude change of the waterborne vehicle based on DVL.

[0097] Here, the difference between the bottom height value and the initial bottom height value can further amplify the impact of wave heave disturbance, providing a basis for the calculation of subsequent height observations.

[0098] S1033. Based on the inertial navigation height difference, the bottom height difference, and the heave compensation displacement value, determine the height observation value corresponding to the water environment.

[0099] In this embodiment, the formula for calculating the altitude observation value (z_h) is: z_h = (h_INS - h_INS_init) – (h_DVL – h_DVL_init) – Σ^x_wavei, that is, z_h = Δh_INS - Δh_DVL - Σ^x_wavei. Its core function is to remove the wave heave disturbance component in the DVL altitude observation to obtain a pure altitude observation value, which is used for subsequent extended Kalman filter fusion update.

[0100] Here, the essence of the formula for determining the height observation value corresponding to the water environment is to compare the height difference of the inertial navigation system with the height difference of the bottom, and combine the heave compensation displacement value to offset the influence of wave heave disturbance on the height observation, so as to ensure that the height observation value can truly reflect the actual height change of the water carrier.

[0101] S104. Based on the inertial measurement data, the heave displacement value, the heave velocity value, the wave state parameters, the height observation value, the bottom velocity value in the relative seabed state data, and the inertial navigation state data, the target heave displacement value, target heave velocity value, and target navigation state data corresponding to the current compensation cycle are determined using the extended Kalman filter fusion method.

[0102] In this embodiment, the Extended Kalman Filter (EKF) fusion method is the core fusion algorithm of the method described in this embodiment. It can handle the state estimation problem of nonlinear systems and is adapted to the nonlinear modeling requirements of wave heave disturbance in the method described in this embodiment.

[0103] The core of this step is to take inertial measurement data, heave displacement value, heave velocity value, wave state parameters, height observation value, bottom velocity value, and inertial navigation status data as inputs, and correct the predicted heave displacement value, heave velocity value, and navigation status data through EKF time update, observation update and other processes to obtain the optimal value of the current compensation cycle (i.e. target heave displacement value, target heave velocity value, and target navigation status data).

[0104] Among them, the target heave displacement value and the target heave velocity value are the optimal estimates of the wave heave disturbance in the current compensation cycle, which are used to predict the heave disturbance in the next compensation cycle; the target navigation status data is the optimal navigation status data of the water vehicle after wave disturbance compensation, including core parameters such as attitude, velocity, and position, which can provide high-precision navigation guidance for the water vehicle.

[0105] Here, the extended Kalman filter fusion process strictly follows the standard procedure of "prediction, residual calculation, gain solution, state correction, and covariance update" to ensure the accuracy of the fusion results and the stability of the filtering system. At the same time, it dynamically adjusts the noise covariance in combination with the wave state parameters to adapt to the real-time changes in sea state.

[0106] In this embodiment, the extended Kalman filter fusion method is used to fuse multi-source input data, correct the prediction errors of heave displacement and heave velocity values, and correct the cumulative errors of inertial navigation status data, so as to obtain the optimal target heave displacement, target heave velocity and target navigation status data for the current compensation period, thereby achieving effective compensation for wave heave disturbances and improving the navigation accuracy and stability of waterborne carriers.

[0107] This effectively suppresses the impact of wave heave disturbances on the navigation system, while ensuring that the filtering system is oscillating and has small residuals, improving the system's robustness and enabling high-precision navigation of waterborne vehicles in complex sea conditions. It eliminates the need for additional sensors, reducing system cost and complexity.

[0108] In one possible implementation of this application, step S104 may include: S1041. Based on the inertial measurement data, the historical target heave displacement value and historical target heave velocity value corresponding to the previous compensation cycle, the wave state parameters and the inertial navigation state data, perform extended Kalman filter time update to obtain the predicted heave displacement value, predicted heave velocity value, predicted navigation state data and predicted covariance matrix corresponding to the current compensation cycle.

[0109] Here, the EKF time update (prediction phase) is the first step in EKF fusion. Its core is to predict the state and covariance of the current period based on the optimal state of the previous period and the current inertial measurement data. The inputs to this step include: inertial measurement data (IMU's three-axis angular velocity and three-axis acceleration), historical target heave displacement and velocity values ​​from the previous compensation period, wave state parameters (wave dominant frequency, wave stochastic process intensity, etc.), and inertial navigation state data (attitude, velocity, and position).

[0110] In this embodiment, the prediction process includes state prediction and covariance prediction: State prediction is recursively derived through the system state equation, and the system state vector is: X=[δPδvφbabgx_waveiv_wavei] (where δP is the position error, δv is the velocity error, φ is the attitude misalignment angle, ba is the accelerometer bias, bg is the gyro drift, x_wavei is the heave displacement, and v_wavei is the heave velocity); the state prediction formula is: X(k|k-1) = f(X(k-1), u), where u is the inertial measurement data, and f(·) is the state transition function; the covariance prediction formula is: P(k|k-1) = F·P(k-1)·F + Q, where P(k-1) is the posterior covariance matrix of the previous period, F is the state transition matrix, and Q is the process noise covariance matrix (determined by the intensity value σ_wave of the random wave process).

[0111] Here, the output predicted heave displacement value and predicted heave velocity value, i.e. the predicted x_wavei(k|k-1) and v_wavei(k|k-1), are the navigation state prediction values ​​obtained by recursion based on the inertial navigation state data and the state equation. The predicted covariance matrix P(k|k-1) represents the error confidence of the predicted state.

[0112] S1042. Based on the height observation value, the bottom velocity value in the relative seabed state data, the predicted heave displacement value, the predicted heave velocity value, and the predicted navigation state data, calculate the velocity observation residual and the height observation residual.

[0113] Here, observation residuals refer to the difference between actual and predicted observations, used to reflect the deviation between the predicted and actual states, including velocity observation residuals and altitude observation residuals.

[0114] Among them, the velocity observation value is the bottom velocity value collected by DVL; the predicted velocity observation value and the predicted altitude observation value are obtained by using the observation equation based on the predicted navigation status data, the predicted heave displacement value, and the predicted heave velocity value.

[0115] In this embodiment, the formula for calculating the velocity observation residual (γ_v) is: γ_v = v_DVL - h_v(X(k|k-1)), where v_DVL is the DVL bottom velocity value, and h_v(X(k|k-1)) is the predicted velocity observation value obtained based on the predicted state; the formula for calculating the altitude observation residual (γ_h) is: γ_h = z_h - h_h(X(k|k-1)), where z_h is the altitude observation value, and h_h(X(k|k-1)) is the predicted altitude observation value obtained based on the predicted state.

[0116] S1043. Based on the predicted covariance matrix, the preset observation matrix, and the preset observation noise covariance matrix, calculate the Kalman gain matrix corresponding to the current compensation period.

[0117] Here, the Kalman gain matrix (K) is a core parameter in the EKF observation update phase, used to balance the confidence of the predicted state with the reliability of the observation data. Its role is to reasonably distribute the correction effect of the observation residuals to each state variable, ensuring the accuracy of the state correction and the stability of the filtering system. The calculation of this matrix requires the combination of the prediction covariance matrix (P(k|k-1)), the preset observation matrix (H), and the preset observation noise covariance matrix (R). These three factors together determine the size of the Kalman gain matrix, which in turn affects the weight of the state correction.

[0118] The preset observation matrix (H) is a matrix used to establish the mapping relationship between the system state and the observed values. Its dimensions are determined by the dimensions of the observed values ​​and the system state. In the method described in this application embodiment, the observed values ​​include velocity observed values ​​and altitude observed values ​​(4 dimensions in total), and the system state vector is 7 dimensions. Therefore, the observation matrix H is a 4×7 matrix, which can map the 7-dimensional system state to 4-dimensional observed values. The preset observation noise covariance matrix (R) is a matrix that characterizes the noise characteristics of the observed data. Its diagonal elements are the noise variance of each observed value. In this embodiment, based on the accuracy of the DVL sensor and the intensity of sea wave interference, the R matrix is ​​set to a 4×4 diagonal matrix, with diagonal elements of [1e-4, 1e-4, 1e-4, 1e-3], which correspond to the DVL three-axis bottom velocity observation noise and altitude observation noise, respectively. It can be dynamically adjusted according to sea state changes to ensure the accuracy of observation noise modeling.

[0119] In this embodiment, the Kalman gain matrix is ​​calculated as follows: K = P(k|k-1)·H ·(H·P(k|k-1)·H + R) - ¹, where H Let H be the transpose of the observation matrix H, (·) - ¹ indicates the matrix inversion operation.

[0120] Here, the core logic of the Kalman gain matrix calculation formula is: the smaller the prediction covariance matrix P(k|k-1), the higher the confidence of the predicted state; the smaller the Kalman gain matrix, the weaker the correction effect of the observation residuals; the smaller the observation noise covariance matrix R, the higher the reliability of the observation data; the larger the Kalman gain matrix, the stronger the correction effect of the observation residuals. This trade-off achieves the optimal fusion of the predicted state and the observation data.

[0121] S1044. The Kalman gain matrix is ​​weighted and fused with the velocity observation residual and the altitude observation residual to correct the predicted heave displacement value, the predicted heave velocity value and the predicted navigation status data, and to determine the target heave displacement value, the target heave velocity value and the target navigation status data corresponding to the current compensation period.

[0122] This step is the observation update (correction phase) of EKF fusion, and it is also the core closing step of the entire heave disturbance compensation process. Its core purpose is to use the Kalman gain matrix to reasonably allocate the correction effect of velocity observation residuals and altitude observation residuals to the predicted heave displacement value, predicted heave velocity value and predicted navigation state data, so as to achieve the optimal correction of each state variable, and finally obtain the target parameters of the current compensation cycle, thus completing one heave disturbance compensation.

[0123] In this embodiment, the state correction process adopts a linear weighted fusion method, and the correction formula is divided into two parts: state correction and covariance correction. The target state vector is: X(k) = X(k|k-1) + K·γ, where X(k) is the target state vector of the current compensation period (containing the target heave displacement value, target heave velocity value, and the error corresponding to the target navigation state data), X(k|k-1) is the predicted state vector, K is the Kalman gain matrix, and γ is the observation residual vector (composed of the velocity observation residual γ_v and the altitude observation residual γ_h). The covariance correction formula is: P(k) = (I - K·H)·P(k|k-1), where I is the identity matrix, and P(k) is the posterior covariance matrix of the current compensation period, used to characterize the error confidence of the target state and provide input for the time update of the next compensation period.

[0124] Furthermore, after the correction is completed, the target heave displacement value and target heave velocity value are extracted from the target state vector X(k) and used as the historical target heave displacement value and historical target heave velocity value for the next compensation cycle, thus achieving continuity in heave disturbance prediction. At the same time, the error corresponding to the target navigation state data is fused with the inertial navigation state data to obtain the final target navigation state data (including the corrected attitude, velocity, and position parameters), providing high-precision navigation guidance for the waterborne vehicle.

[0125] Optionally, in another possible implementation of this application, step S104 further includes: S1045. Based on the Kalman gain matrix, the prediction covariance matrix, and the observation matrix, the posterior covariance matrix is ​​updated to perform extended Kalman filter prediction calculation for the next compensation cycle using the posterior covariance matrix.

[0126] This step is the closing step of the extended Kalman filter fusion process. Its core purpose is to update the posterior covariance matrix, providing core input parameters for the EKF time update of the next compensation cycle, and ensuring the continuity and stability of the filtering process.

[0127] Here, the core basis for updating the posterior covariance matrix P(k) is the Kalman gain matrix K, the prediction covariance matrix P(k|k-1) and the observation matrix H. Its update formula is consistent with the covariance correction formula, that is, P(k) = (I - K·H)·P(k|k-1), where I is the identity matrix with the same dimension as the state vector (7×7 identity matrix in this embodiment).

[0128] The above-mentioned update process essentially adjusts the error confidence of each state quantity based on the correction effect of the observed residuals, so that the posterior covariance matrix can accurately represent the actual error level of the target state, providing reliable prior error information for the next round of prediction and avoiding the decline in filtering performance caused by error accumulation.

[0129] For example, please refer to Figure 2 , Figure 2 This is a schematic diagram of the effect curve of a height error comparison test provided in an embodiment of this application. To verify the effectiveness of the method described in this application under ocean wave conditions, simulation verification was carried out. By statistically analyzing the adaptive estimation results of ocean wave parameters under different sea state conditions, the adaptability of the method described in this application to changes in sea state was verified. The ocean wave parameters obtained by adaptive estimation under different sea state conditions are shown in Table 1 below.

[0130] Table 1 Adaptive Sea State Estimation Parameters

[0131] As can be seen from Table 1 above, the method described in this application embodiment can adaptively estimate the main period and significant wave height of the ocean waves based on the changing characteristics of the height observation signal, effectively reflecting the changing pattern of ocean wave characteristics under different sea conditions. The estimation errors of Tp and Hs are both less than 2%, demonstrating high accuracy and stability.

[0132] Based on the above verification tests, such as Figure 2 As shown in the figure, a comparative test of system performance was conducted. The corresponding curves of the test results of the traditional INS / DVL integrated navigation method, the method of suppressing sea wave interference by increasing the observation noise covariance, and the method described in the embodiments of this application are shown in the figure. Figure 2 As shown in the figure; where the red dashed line corresponds to the traditional INS / DVL integrated navigation method, the blue dashed line corresponds to the method of increasing the observation noise covariance to suppress sea wave interference, and the green solid line corresponds to the method described in the embodiments of this application.

[0133] Thus, the heave disturbance compensation method for waterborne carrier navigation described in this application has significant technical advantages and practical value compared to the prior art, with the following specific beneficial effects: Firstly, it eliminates the need for additional sensors such as pressure gauges and acoustic altimeters. It utilizes the existing IMU and DVL sensors on the watercraft to achieve accurate compensation for wave heave disturbances without increasing system cost or complexity. This solves the problems of cumbersome installation, complex calibration, and increased costs caused by the need for additional sensors in existing technologies.

[0134] Secondly, by explicitly constructing a wave heave disturbance model, introducing heave displacement and heave velocity as extended state variables, and combining adaptive estimation of wave state parameters, the periodicity and randomness of wave heave disturbance are accurately characterized, overcoming the shortcomings of existing technologies that do not explicitly consider wave-related disturbances and rely solely on empirical parameter tuning.

[0135] Third, the process noise covariance matrix Q of EKF is dynamically adjusted based on the wave state parameters. Combined with the observation residuals and Kalman gain matrix, the optimal state correction is achieved, which effectively suppresses the problems of vertical estimation oscillation and increased filtering residuals, and greatly improves navigation accuracy.

[0136] Fourth, the entire compensation process adopts a closed-loop iterative design, with heave disturbance prediction and EKF fusion update forming a linkage, adapting to different sea states of small, medium and high, with strong robustness, and can meet the high-precision navigation needs of underwater unmanned platforms (ROV, AUV) and other waterborne vehicles in nearshore and complex sea states, providing reliable navigation support for marine exploration, resource development and other operations of waterborne vehicles, and has extremely strong engineering application value.

[0137] The heave disturbance compensation method for navigation of waterborne vehicles provided in this application acquires real-time inertial measurement data and relative seabed state data of the waterborne vehicle, determines the inertial navigation state data and real-time inertial navigation height value, then determines the wave state parameters based on the height data and predicts the heave displacement and velocity through a wave heave disturbance model. After constructing the height observation values, the target heave parameters and target navigation state data are obtained by fusion through extended Kalman filtering. This method solves the problems of existing technologies, such as not explicitly considering wave-related disturbances, requiring additional sensor configuration, navigation error accumulation, poor filtering performance, and high system complexity. It achieves the technical effect of suppressing the impact of wave heave disturbances on navigation accuracy without increasing system cost and complexity, and improving the navigation accuracy and stability of waterborne vehicles in complex sea conditions.

[0138] Please see Figure 3 , Figure 3 This is a schematic diagram of a heave disturbance compensation device for navigation of a waterborne vehicle, provided as an embodiment of this application. Figure 3 As shown, the heave disturbance compensation device 300 includes: The data acquisition module 310 is used to acquire, in the current compensation cycle, the inertial measurement data corresponding to the waterborne carrier collected in real time by the inertial measurement unit set on the waterborne carrier and the relative seabed state data corresponding to the waterborne carrier collected in real time by the Doppler velocimeter, and determine the inertial navigation state data and the real-time inertial navigation height value corresponding to the waterborne carrier. The model prediction module 320 is used to determine the wave state parameters corresponding to the water environment in which the water carrier is located based on the bottom height value in the relative seabed state data and the real-time height value of the inertial navigation system, and to predict the heave displacement value and heave velocity value corresponding to the water environment using a preset wave heave disturbance model. The height observation module 330 is used to determine the height observation value corresponding to the water environment based on the heave displacement value, the bottom height value and the inertial navigation real-time height value; The disturbance compensation module 340 is used to determine the target heave displacement value, target heave velocity value, and target navigation status data corresponding to the current compensation period based on the inertial measurement data, the heave displacement value, the heave velocity value, the wave state parameters, the height observation value, the bottom velocity value in the relative seabed state data, and the inertial navigation status data, using the extended Kalman filter fusion method.

[0139] Furthermore, the inertial measurement data includes at least three-axis angular velocity data and three-axis acceleration data; the inertial navigation state data includes at least attitude parameters, three-dimensional velocity parameters, and three-dimensional position parameters; when the data acquisition module 310 is used to determine the inertial navigation state data and real-time inertial navigation height value corresponding to the waterborne carrier based on the inertial measurement data, the data acquisition module 310 is used for: Based on the triaxial angular velocity data, the attitude parameters corresponding to the waterborne vehicle are determined using a preset quaternion and Euler angle update method; wherein, the attitude parameters include at least roll angle, pitch angle and heading angle. The three-axis acceleration data is transformed from the carrier coordinate system to the navigation coordinate system, and preset interference terms in the three-axis acceleration data are removed, so that the three-dimensional velocity parameters corresponding to the waterborne carrier can be obtained by numerical integration; wherein, the three-dimensional velocity parameters include at least lateral velocity value, longitudinal velocity value and vertical velocity value; The three-dimensional velocity parameters are numerically integrated to obtain the three-dimensional position parameters corresponding to the waterborne carrier; wherein the three-dimensional position parameters include at least longitude, latitude and vertical height values; The real-time altitude value of the inertial navigation system corresponding to the waterborne carrier is determined from the three-dimensional position parameters.

[0140] Furthermore, the wave state parameters include at least the wave principal period, wave principal frequency, wave significant wave height, and wave stochastic process intensity; when the model prediction module 320 determines the wave state parameters corresponding to the water environment in which the waterborne carrier is located based on the bottom height value in the relative seabed state data and the real-time height value of the inertial navigation system, the model prediction module 320 is used to: The height difference between the bottom height value in the relative seabed state data and the real-time height value of the inertial navigation system is determined, and based on the height difference, the zero-crossing time is detected using the zero-crossing method to determine the main period of the ocean waves. The main wave frequency is determined based on the main wave period; Determine the standard deviation of the sequence corresponding to the height difference, and determine the significant wave height based on the standard deviation; The intensity value of the random process of the ocean waves is determined based on the significant wave height and the dominant wave frequency.

[0141] Furthermore, the wave heave disturbance model is constructed based on the wave spectrum model and incorporates heave displacement and heave velocity values ​​as extended state variables; when the model prediction module 320 is used to predict the heave displacement and heave velocity values ​​corresponding to the water environment using the preset wave heave disturbance model, the model prediction module 320 is used to: Obtain the wave heave disturbance coefficient corresponding to the water environment; wherein, the wave heave disturbance coefficient includes at least the wave dominant frequency coefficient, the wave damping coefficient, and the process noise coefficient; Based on the wave dominant frequency coefficient, the wave damping coefficient, the process noise coefficient, and the historical target heave displacement and historical target heave velocity values ​​corresponding to the previous compensation cycle, the heave velocity value of the water environment corresponding to the current compensation cycle is determined using the heave velocity state equation in the preset wave heave disturbance model. Based on the historical target heave velocity, the heave displacement value of the water environment corresponding to the current compensation cycle is determined using the heave displacement state equation in the wave heave disturbance model.

[0142] Furthermore, when the height observation module 330 is used to determine the height observation value corresponding to the water environment based on the heave displacement value, the bottom height value, and the inertial navigation real-time height value, the height observation module 330 is used to: The initial altitude value of the inertial navigation system, the initial altitude value of the bottom alignment, and the historical target heave displacement value corresponding to the previous compensation cycle are obtained respectively. Based on the historical target heave displacement value and the heave displacement value, the heave compensation displacement value is determined. Determine the inertial navigation altitude difference between the real-time altitude value and the initial altitude value, and determine the bottom altitude difference between the bottom-aligned altitude value and the initial bottom-aligned altitude value; Based on the inertial navigation height difference, the bottom height difference, and the heave compensation displacement value, the height observation value corresponding to the water environment is determined.

[0143] Furthermore, when the disturbance compensation module 340 is used to determine the target heave displacement value, target heave velocity value, and target navigation status data corresponding to the current compensation period based on the inertial measurement data, the heave displacement value, the heave velocity value, the wave state parameters, the height observation value, the bottom velocity value in the relative seabed state data, and the inertial navigation status data, using the extended Kalman filter fusion method, the disturbance compensation module 340 is used to: Based on the inertial measurement data, the historical target heave displacement value and historical target heave velocity value corresponding to the previous compensation cycle, the wave state parameters and the inertial navigation state data, an extended Kalman filter time update is performed to obtain the predicted heave displacement value, predicted heave velocity value, predicted navigation state data and predicted covariance matrix corresponding to the current compensation cycle. Based on the height observation value, the bottom velocity value in the relative seabed state data, the predicted heave displacement value, the predicted heave velocity value, and the predicted navigation state data, the velocity observation residual and the height observation residual are calculated. Based on the predicted covariance matrix, the preset observation matrix, and the preset observation noise covariance matrix, the Kalman gain matrix corresponding to the current compensation period is calculated. The Kalman gain matrix is ​​weighted and fused with the velocity observation residual and the altitude observation residual to correct the predicted heave displacement value, predicted heave velocity value and predicted navigation status data, and to determine the target heave displacement value, target heave velocity value and target navigation status data corresponding to the current compensation cycle.

[0144] Furthermore, when the disturbance compensation module 340 is used to determine the target heave displacement value, target heave velocity value, and target navigation status data corresponding to the current compensation period based on the inertial measurement data, the heave displacement value, the heave velocity value, the wave state parameters, the height observation value, the bottom velocity value in the relative seabed state data, and the inertial navigation status data, using the extended Kalman filter fusion method, the disturbance compensation module 340 is also used to: Based on the Kalman gain matrix, the prediction covariance matrix, and the observation matrix, the posterior covariance matrix is ​​updated to perform extended Kalman filter prediction calculation for the next compensation cycle.

[0145] The heave disturbance compensation device for navigation of a waterborne vehicle provided in this application acquires real-time inertial measurement data and relative seabed state data of the waterborne vehicle, determines the inertial navigation state data and real-time inertial navigation height value, then determines the wave state parameters based on the height data and predicts the heave displacement and velocity through a wave heave disturbance model. After constructing the height observation values, the target heave parameters and target navigation state data are obtained by fusion through extended Kalman filtering. This solves the problems of existing technologies, such as not explicitly considering wave-related disturbances, requiring additional sensors, navigation error accumulation, poor filtering performance, and high system complexity. It achieves the technical effect of suppressing the impact of wave heave disturbances on navigation accuracy without increasing system cost and complexity, and improving the navigation accuracy and stability of waterborne vehicles in complex sea conditions.

[0146] Please see Figure 4 , Figure 4 This is a schematic diagram of the structure of an electronic device provided in an embodiment of this application. Figure 4 As shown, the electronic device 400 includes a processor 410, a memory 420, and a bus 430.

[0147] The memory 420 stores machine-readable instructions executable by the processor 410. When the electronic device 400 is running, the processor 410 communicates with the memory 420 via the bus 430. When the machine-readable instructions are executed by the processor 410, they can perform the operations described above. Figure 1 The steps of the heave disturbance compensation method for navigation of waterborne carriers in the method embodiment shown are described in detail in the method embodiment, and will not be repeated here.

[0148] This application also provides a computer-readable storage medium storing a computer program, which, when executed by a processor, can perform the above-described actions. Figure 1 The steps of the heave disturbance compensation method for navigation of waterborne carriers in the method embodiment shown are described in detail in the method embodiment, and will not be repeated here.

[0149] Those skilled in the art will understand that, for the sake of convenience and brevity, the specific working processes of the systems, devices, and units described above can be referred to the corresponding processes in the foregoing method embodiments, and will not be repeated here.

[0150] In the several embodiments provided in this application, it should be understood that the disclosed systems, apparatuses, and methods can be implemented in other ways. The apparatus embodiments described above are merely illustrative. For example, the division of units is only a logical functional division, and in actual implementation, there may be other division methods. Furthermore, multiple units or components may be combined or integrated into another system, or some features may be ignored or not executed. Additionally, the shown or discussed mutual couplings, direct couplings, or communication connections may be through some communication interfaces; indirect couplings or communication connections between devices or units may be electrical, mechanical, or other forms.

[0151] The units described as separate components may or may not be physically separate. The components shown as units may or may not be physical units; that is, they may be located in one place or distributed across multiple network units. Some or all of the units can be selected to achieve the purpose of this embodiment according to actual needs.

[0152] In addition, the functional units in the various embodiments of this application can be integrated into one processing unit, or each unit can exist physically separately, or two or more units can be integrated into one unit.

[0153] If the aforementioned functions are implemented as software functional units and sold or used as independent products, they can be stored in a processor-executable, non-volatile, computer-readable storage medium. Based on this understanding, the technical solution of this application, in essence, or the part that contributes to the prior art, or a portion of the technical solution, can be embodied in the form of a software product. This computer software product is stored in a storage medium and includes several instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) to execute all or part of the steps of the methods described in the various embodiments of this application. The aforementioned storage medium includes various media capable of storing program code, such as USB flash drives, portable hard drives, read-only memory (ROM), random access memory (RAM), magnetic disks, or optical disks.

[0154] Finally, it should be noted that the above-described embodiments are merely specific implementations of this application, used to illustrate the technical solutions of this application, and not to limit them. The scope of protection of this application is not limited thereto. Although this application has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that any person skilled in the art can still modify or easily conceive of changes to the technical solutions described in the foregoing embodiments, or make equivalent substitutions for some of the technical features, within the scope of the technology disclosed in this application. Such modifications, changes, or substitutions do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of this application, and should all be covered within the scope of protection of this application. Therefore, the scope of protection of this application should be determined by the scope of the claims.

Claims

1. A method for compensating for heave disturbances in navigation of a waterborne vehicle, characterized in that, The method includes: During the current compensation cycle, the inertial measurement data corresponding to the waterborne vehicle is acquired in real time by the inertial measurement unit set on the waterborne vehicle, and the relative seabed state data corresponding to the waterborne vehicle is acquired in real time by the Doppler velocimeter. Based on the inertial measurement data, the inertial navigation state data and the real-time inertial navigation altitude value corresponding to the waterborne vehicle are determined. Based on the bottom height value in the relative seabed state data and the real-time height value of the inertial navigation system, the wave state parameters corresponding to the water environment in which the waterborne carrier is located are determined, and the heave displacement value and heave velocity value corresponding to the water environment are predicted using a preset wave heave disturbance model. Based on the heave displacement value, the bottom height value, and the inertial navigation real-time height value, the height observation value corresponding to the water environment is determined; Based on the inertial measurement data, the heave displacement value, the heave velocity value, the wave state parameters, the height observation value, the bottom velocity value in the relative seabed state data, and the inertial navigation state data, the target heave displacement value, target heave velocity value, and target navigation state data corresponding to the current compensation cycle are determined using the extended Kalman filter fusion method.

2. The method according to claim 1, characterized in that, The inertial measurement data includes at least three-axis angular velocity data and three-axis acceleration data; the inertial navigation state data includes at least attitude parameters, three-dimensional velocity parameters, and three-dimensional position parameters; the determination of the inertial navigation state data and real-time altitude value of the waterborne vehicle based on the inertial measurement data includes: Based on the triaxial angular velocity data, the attitude parameters corresponding to the waterborne vehicle are determined using a preset quaternion and Euler angle update method; wherein, the attitude parameters include at least roll angle, pitch angle and heading angle. The three-axis acceleration data is transformed from the carrier coordinate system to the navigation coordinate system, and preset interference terms in the three-axis acceleration data are removed, so that the three-dimensional velocity parameters corresponding to the waterborne carrier can be obtained by numerical integration; wherein, the three-dimensional velocity parameters include at least lateral velocity value, longitudinal velocity value and vertical velocity value; The three-dimensional velocity parameters are numerically integrated to obtain the three-dimensional position parameters corresponding to the waterborne carrier; wherein the three-dimensional position parameters include at least longitude, latitude and vertical height values; The real-time altitude value of the inertial navigation system corresponding to the waterborne carrier is determined from the three-dimensional position parameters.

3. The method according to claim 1, characterized in that, The wave state parameters include at least the wave principal period, wave principal frequency, wave significant wave height, and wave stochastic process intensity; determining the wave state parameters corresponding to the water environment in which the waterborne carrier is located, based on the bottom height value in the relative seabed state data and the real-time height value of the inertial navigation system, includes: The height difference between the bottom height value in the relative seabed state data and the real-time height value of the inertial navigation system is determined, and based on the height difference, the zero-crossing time is detected using the zero-crossing method to determine the main period of the ocean waves. The main wave frequency is determined based on the main wave period; Determine the standard deviation of the sequence corresponding to the height difference, and determine the significant wave height based on the standard deviation; The intensity value of the random process of the ocean waves is determined based on the significant wave height and the dominant wave frequency.

4. The method according to claim 1, characterized in that, The wave heave disturbance model is constructed based on the wave spectrum model and incorporates heave displacement and heave velocity values ​​as extended state variables; the prediction of the heave displacement and heave velocity values ​​corresponding to the water environment using the preset wave heave disturbance model includes: Obtain the wave heave disturbance coefficient corresponding to the water environment; wherein, the wave heave disturbance coefficient includes at least the wave dominant frequency coefficient, the wave damping coefficient, and the process noise coefficient; Based on the wave dominant frequency coefficient, the wave damping coefficient, the process noise coefficient, and the historical target heave displacement and historical target heave velocity values ​​corresponding to the previous compensation cycle, the heave velocity value of the water environment corresponding to the current compensation cycle is determined using the heave velocity state equation in the preset wave heave disturbance model. Based on the historical target heave velocity, the heave displacement value of the water environment corresponding to the current compensation cycle is determined using the heave displacement state equation in the wave heave disturbance model.

5. The method according to claim 1, characterized in that, The process of determining the height observation value corresponding to the water environment based on the heave displacement value, the bottom height value, and the inertial navigation real-time height value includes: The initial altitude value of the inertial navigation system, the initial altitude value of the bottom alignment, and the historical target heave displacement value corresponding to the previous compensation cycle are obtained respectively. Based on the historical target heave displacement value and the heave displacement value, the heave compensation displacement value is determined. Determine the inertial navigation altitude difference between the real-time altitude value and the initial altitude value, and determine the bottom altitude difference between the bottom-aligned altitude value and the initial bottom-aligned altitude value; Based on the inertial navigation height difference, the bottom height difference, and the heave compensation displacement value, the height observation value corresponding to the water environment is determined.

6. The method according to claim 1, characterized in that, The process of determining the target heave displacement, target heave velocity, and target navigation status data corresponding to the current compensation cycle based on the inertial measurement data, the heave displacement value, the heave velocity value, the wave state parameters, the height observation value, the bottom velocity value in the relative seabed state data, and the inertial navigation status data, using an extended Kalman filter fusion method, includes: Based on the inertial measurement data, the historical target heave displacement value and historical target heave velocity value corresponding to the previous compensation cycle, the wave state parameters and the inertial navigation state data, an extended Kalman filter time update is performed to obtain the predicted heave displacement value, predicted heave velocity value, predicted navigation state data and predicted covariance matrix corresponding to the current compensation cycle. Based on the height observation value, the bottom velocity value in the relative seabed state data, the predicted heave displacement value, the predicted heave velocity value, and the predicted navigation state data, the velocity observation residual and the height observation residual are calculated. Based on the predicted covariance matrix, the preset observation matrix, and the preset observation noise covariance matrix, the Kalman gain matrix corresponding to the current compensation period is calculated. The Kalman gain matrix is ​​weighted and fused with the velocity observation residual and the altitude observation residual to correct the predicted heave displacement value, predicted heave velocity value and predicted navigation status data, and to determine the target heave displacement value, target heave velocity value and target navigation status data corresponding to the current compensation cycle.

7. The method according to claim 6, characterized in that, The method of determining the target heave displacement value, target heave velocity value, and target navigation status data corresponding to the current compensation cycle based on the inertial measurement data, the heave displacement value, the heave velocity value, the wave state parameters, the height observation value, the bottom velocity value in the relative seabed state data, and the inertial navigation status data, using an extended Kalman filter fusion method, further includes: Based on the Kalman gain matrix, the prediction covariance matrix, and the observation matrix, the posterior covariance matrix is ​​updated to perform extended Kalman filter prediction calculation for the next compensation cycle.

8. A heave disturbance compensation device for navigation of a waterborne vehicle, characterized in that, The heave disturbance compensation device includes: The data acquisition module is used to acquire, in the current compensation cycle, the inertial measurement data corresponding to the waterborne carrier collected in real time by the inertial measurement unit set on the waterborne carrier and the relative seabed state data corresponding to the waterborne carrier collected in real time by the Doppler velocimeter, and determine the inertial navigation state data and real-time inertial navigation height value corresponding to the waterborne carrier. The model prediction module is used to determine the wave state parameters corresponding to the water environment in which the water carrier is located based on the bottom height value in the relative seabed state data and the real-time height value of the inertial navigation system, and to predict the heave displacement value and heave velocity value corresponding to the water environment using a preset wave heave disturbance model. The height observation module is used to determine the height observation value corresponding to the water environment based on the heave displacement value, the bottom height value, and the inertial navigation real-time height value. The disturbance compensation module is used to determine the target heave displacement value, target heave velocity value, and target navigation status data corresponding to the current compensation period based on the inertial measurement data, the heave displacement value, the heave velocity value, the wave state parameters, the height observation value, the bottom velocity value in the relative seabed state data, and the inertial navigation status data, using the extended Kalman filter fusion method.

9. An electronic device, characterized in that, include: The device includes a processor, a memory, and a bus. The memory stores machine-readable instructions executable by the processor. When the electronic device is running, the processor communicates with the memory via the bus. The machine-readable instructions are executed by the processor to perform the steps of the heave disturbance compensation method for navigation of a waterborne vehicle as described in any one of claims 1 to 7.

10. A computer-readable storage medium, characterized in that, The computer-readable storage medium stores a computer program that, when executed by a processor, performs the steps of the heave disturbance compensation method for navigation of a waterborne carrier as described in any one of claims 1 to 7.

Images