Ship robust heave estimation method based on spatio-temporal feature perception factor graph optimization

Abstract

The application discloses a ship robust heave estimation method based on a space-time feature perception factor graph optimization, relates to the field of marine engineering state estimation, and is used for estimating the heave displacement of a ship and comprises the following steps: constructing a nonlinear observation model and outputting a posture rotation matrix; calculating a vertical motion acceleration, mapping GNSS antenna elevation to equivalent vertical observation at a reference point, and acquiring GNSS vertical observation information; constructing a space-time joint adaptive weight, and dynamically modulating a GNSS observation information matrix by using the space-time joint adaptive weight; calculating kinematic residual error and GNSS observation residual error, combining the kinematic residual error and the GNSS observation residual error, constructing a joint optimization objective function, and iteratively solving; after iterative convergence, extracting the optimal state of the window center ephemeris as the current heave displacement estimation value, and mapping and outputting the final heave result according to the lever arm of a task mounting point. The application eliminates the phase delay of traditional high-pass filtering, and improves the precision and robustness of ship heave estimation.

Description

Technical Field

[0001] This invention relates to the field of marine engineering state estimation, and more particularly to the field of ship heave estimation technology, specifically to a robust ship heave estimation method based on spatiotemporal feature perception factor map optimization. Background Technology

[0002] Accurate heave estimation is fundamental to ensuring the safe operation of modern marine engineering, autonomous surface vessels (MASS), and unmanned surface vessels (USVs). Heave motion directly affects the safety of carrier-based aircraft takeoffs and landings, the accuracy of shipborne weapon launches, the design of heave compensation devices for drilling platforms, and the survivability of unmanned vessels in harsh sea conditions. Currently, the combination of multi-antenna global navigation satellite systems (GNSS) and inertial measurement units (IMUs) has become the mainstream navigation solution due to its ability to provide high-frequency, continuous three-dimensional pose. Accurately separating the true vertical displacement from the coupled three-dimensional motion requires compensating for gravity projection and the antenna lever arm effect.

[0003] Traditional heave estimation systems often employ frequency-domain-based high-pass filtering (HPF) and bandpass / complementary filtering techniques. However, these methods are sensitive to the frequency characteristics of environmental waves and are prone to phase delays or signal distortion when dealing with long-period swells. To construct estimators that incorporate more complete dynamic characteristics, sliding window factor graph optimization (FGO) is widely used. Factor graph optimization represents state variables as nodes and constraints as edges, solving for the optimal state estimate through nonlinear optimization. However, in actual surface operations, transient maneuvers of the vessel can cause fluctuations in the quality of multi-antenna GNSS observation signals. Small attitude calculation disturbances can be nonlinearly amplified through the physical lever arm between the GNSS antenna and the IMU, transforming into equivalent observation biases in the vertical channel. Traditional graph optimization models, using static or pre-set observation covariance matrices, may incorrectly absorb degraded observations, causing the overall smooth trajectory to shift towards contaminated nodes. Furthermore, the Markov property assumptions of traditional filtering methods cannot trace historical information, and performing only a first-order Taylor expansion cannot obtain the optimal state estimate.

[0004] Therefore, there is an urgent need for a high-precision, robust heave estimation method that can adaptively identify observation quality degradation, suppress lever arm coupling error propagation, and maintain phase delay-free operation across the entire frequency band under complex sea conditions. Summary of the Invention

[0005] The purpose of this invention is to provide a robust heave estimation method for ships based on spatiotemporal feature-aware factor graph optimization, so as to solve the problem of heave estimation divergence caused by attitude lever arm coupling in the traditional factor graph optimization under severe sea conditions.

[0006] To address the above objectives, this invention provides a robust ship heave estimation method based on spatiotemporal feature-aware factor graph optimization, comprising: S1. Use a multi-antenna GNSS array to acquire baseline observations, construct a nonlinear observation model, perform attitude calculation on the nonlinear observation model, obtain the spatial attitude uncertainty, and output the attitude rotation matrix. S2. Based on the attitude rotation matrix, the specific force of the inertial measurement unit is projected to the vertical direction and gravity is subtracted to obtain the vertical motion acceleration. Based on the rigid lever arm between the GNSS antenna and the IMU, the elevation of the GNSS antenna is mapped to the equivalent vertical observation at the reference point to obtain GNSS vertical observation information. S3. Calculate the spatial robustness weight based on the spatial attitude uncertainty, calculate the temporal adaptive weight based on the changes in the angular velocity and specific force of the inertial measurement unit, fuse the spatial robustness weight and the temporal adaptive weight to obtain the spatiotemporal joint adaptive weight, and use the spatiotemporal joint adaptive weight to dynamically modulate the GNSS observation information matrix. S4. Construct the kinematic residual of the inertial measurement unit using vertical motion acceleration, construct the GNSS observation residual using GNSS vertical observation information and dynamically modulated GNSS observation information matrix, combine the kinematic residual and GNSS observation residual, construct a joint optimization objective function for iterative solution, and update the window state and prior information using Shur complement. S5. After the iteration converges, extract the optimal state of the window center epoch as the current heave displacement estimate, and output the final heave result based on the lever arm mapping of the task installation point.

[0007] In S1, the nonlinear observation model is: ; In the formula, Indicates epoch index, Indicates the first The actual observation vector of an epoch. Indicates the first The three-dimensional attitude angle of the carrier in the epoch; Indicates the attitude angle The rotation matrix constructed from the carrier system to the navigation system, This represents the pre-calibrated rigid body reference vector within the load system. Indicates the first The observation noise vector of an epoch; The nonlinear observation model is solved using the least squares algorithm. After outputting the attitude rotation matrix, the posterior unit weight variance is extracted as the scalar of the spatial attitude uncertainty for the current epoch. ; In the formula, Indicates the first Spatial attitude uncertainty of an epoch, For the least squares solution, the first The residual vector of an epoch. Indicates the first The observation weight matrix of the epoch, Indicates the first The system's degrees of freedom in an epoch. It represents the transpose of a matrix or vector.

[0008] In S2, the vertical acceleration is: ; In the formula, Indicates time, express The vertical acceleration at time t, This represents the vertical selection vector. express The attitude rotation matrix at time step; express The three-dimensional specific force vector measured by the inertial measurement unit at any given time. Represents gravitational acceleration; Equivalent GNSS vertical observations are obtained through a nonlinear mapping relationship between the total altitude position of the GNSS antenna phase center in the navigation system and the actual heave displacement lever arm of the ship's center of mass: ; In the formula, express The total altitude position of the GNSS antenna phase center in the navigation system at any given time. This represents the actual heave displacement of the ship's center of mass. This represents the three-dimensional static lever arm vector between the GNSS antenna mounting point and the geometric center of the inertial measurement unit in the carrier system.

[0009] In S3, the spatial robustness weight is: ; ; ; ; ; In the formula, Indicates the first Spatial robustness weights of epochs To find the minimum value function, This represents the scaling factor that controls the divergence boundary of the control space. Represents absolute value. Indicates the first Local robustness scale of an era express The local median within the time window. , Indicates intermediate calculation parameters. To obtain the median function, This represents the epoch index within the time window. Indicates the first Spatial attitude uncertainty of an epoch, Indicates a local time window. To find the maximum value function, The minimum variance lower bound constant is set to prevent the scale from being zero; The time-domain adaptive weights are: ; ; ; In the formula, Indicates the first Time-domain adaptive weights of the epoch. This represents an exponential function with base e. To control the exponential decay coefficient of the time-domain weight decay rate, Indicates the first The adaptive update of the dynamic maneuver variance threshold for each epoch. Indicates intermediate calculation parameters. Indicates the length of the local time window. Indicates the first The angular velocity vector output by the epoch inertial observation unit. Describing the vector norm, The statistical mean of the norm of the angular velocity vector within the time window. This is a summation function.

[0010] The spatiotemporal joint adaptive weights are: ; In the formula, Indicates the first Spatiotemporal joint adaptive weights of epochs To ensure the minimum non-zero weights for factor graph connectivity; The GNSS observation information matrix after spatiotemporal joint adaptive weight dynamic modulation is as follows: ; In the formula, Indicates the modulated first GNSS observation information matrix of the epoch.

[0011] In S4, the state vector of a single epoch within a local time window is defined. The state vector includes the heave displacement of the ship's center of mass, the vertical velocity of the ship's center of mass, and the zero bias of the vertical accelerometer of the ship's center of mass. ; In the formula, Indicates the first The state vector of an epoch, Indicates the first The heave displacement of the ship's center of mass during an epoch. Indicates the first The vertical velocity of the ship's center of mass at an epoch. Indicates the first The vertical accelerometer at the ship's center of mass at the epoch has zero bias. By using midpoint integration, the kinematic residuals of the inertial measurement unit between adjacent states are constructed using vertical motion acceleration: ; ; ; In the formula, Indicates the first Era and the First Kinematic residuals between epochs Indicates the first The time corresponding to an epoch, Indicates the time interval between adjacent epochs. Indicates intermediate calculation parameters; The GNSS observation residuals are: ; In the formula, Indicates the first GNSS observation residuals at each epoch.

[0012] The joint optimization objective function is: ; ; ; ; ; In the formula, This represents the epoch index within the time window. This represents the optimal estimate of the set of state variables within the current sliding window. For historically marginalized a priori residuals, For the IMU kinematic factor cost term, For GNSS observation factor cost term, This represents the marginalized prior residual. Represents the prior information matrix, Indicates the first Era and the First Kinematic residuals between epochs Indicates the first The covariance matrix of the kinematic factors of the inertial measurement unit at each epoch. This represents the weighted Mahalanobis distance of the information matrix. This represents the set of states to be estimated within the window.

[0013] The analytical Jacobian matrices of the kinematic residuals and GNSS observation residuals are: ; ; ; In the formula, , These are the kinematic residuals relative to , Jacobian matrix, For GNSS observation residuals relative to Jacobian matrix; By sparsely assembling the residuals, Jacobian matrix, and information matrix of each factor, a normal equation is established: ; In the formula, For approximate Hessian matrix, Adjust the increment for the state. The residual gradient vector; Sparse Cholesky decomposition is used to solve for the state correction increment, and the window state is updated accordingly: ; In the formula, Indicates the index of the number of nonlinear iterations. Indicates the first The next iteration , Indicates the first The next iteration .

[0014] When a new state node enters the window, the approximate Hessian matrix is ​​divided into blocks according to the state to be marginalized and the state to be retained: ; ; In the formula, This represents the Hessian matrix block corresponding to the state to be marginalized. This represents the Hessian matrix block corresponding to the preserved state. , This is for cross-blocking.

[0015] Window Center Era Index for: ; In the formula, This is the floor function; from Extract the optimal state of the central epoch. : ; In the formula, This represents the optimal heave displacement. Indicates the optimal vertical velocity. This indicates the optimal vertical accelerometer zero bias; The estimated heave of the ship's center of mass is as follows: ; When it is necessary to output the heave at the task mounting point, mapping is performed using a rigid body lever arm: ; In the formula, Output the vertical position rise and fall of the task assignment point. In the context of the loading system, the lever arm vector points from the ship's center of mass to the mission installation point. This represents the attitude rotation matrix at the center moment.

[0016] Compared with the prior art, the present invention has the following advantages:

[0017] This invention replaces traditional frequency domain high-pass filtering with full-band sliding window state-space estimation, and solves the zero bias of the IMU's Z-axis accelerometer as an unknown in real time in the ENU coordinate system, fundamentally eliminating the phase delay in long-period surge estimation and possessing the ability to backtrack and correct trajectories under highly dynamic abrupt changes.

[0018] This invention couples the posterior spatial residuals calculated by multi-antenna GNSS with the macroscopic maneuver variance extracted by IMU. The constructed DIMW physical mechanism can accurately identify the observation degradation induced by attitude perturbations and lever arm amplification, and implement "soft cut-off" isolation, effectively solving the problem of attitude-gravity coupling error contamination.

[0019] The Jacobian matrix analyzed in this invention avoids the massive computational load of numerical difference, ensuring the algorithm's high-frequency real-time response capability and numerical stability under continuous wave impact.

[0020] This invention uses the Shure complement marginalization mechanism to maintain a constant computational cost for the sliding window while fully preserving the constraints of historical observation information, ensuring that the optimized trajectory of the current window remains continuous and consistent with the historical global trajectory. Attached Figure Description

[0021] Figure 1 The equivalent GNSS observation error curve magnified by the projection of the lever arm is provided for the present invention;

[0022] Figure 2 The absolute estimation error comparison results provided for this invention;

[0023] Figure 3 This is a comparison diagram of the local heave estimation trajectories improved by the present invention;

[0024] Figure 4 for Figure 3 The corresponding local estimation error comparison chart. Detailed Implementation

[0025] To make the objectives, technical solutions, and advantages of this invention clearer, the technical solutions of this invention are described clearly and completely below. Obviously, the described embodiments are only some, not all, of the embodiments of this invention. All other embodiments obtained by those skilled in the art based on the embodiments of this invention without creative effort are within the scope of protection of this invention.

[0026] The robust heave estimation method for ships based on spatiotemporal feature-aware factor map optimization (RFGO-DIMW) includes: S1. Use a multi-antenna GNSS array to acquire baseline observations, construct a nonlinear observation model, perform attitude calculation on the nonlinear observation model, obtain the spatial attitude uncertainty, and output the attitude rotation matrix. S2. Based on the attitude rotation matrix, the specific force of the inertial measurement unit is projected to the vertical direction and gravity is subtracted to obtain the vertical motion acceleration. Based on the rigid lever arm between the GNSS antenna and the IMU, the elevation of the GNSS antenna is mapped to the equivalent vertical observation at the reference point to obtain GNSS vertical observation information. S3. Calculate the spatial robustness weight based on the spatial attitude uncertainty, calculate the temporal adaptive weight based on the changes in the angular velocity and specific force of the inertial measurement unit, fuse the spatial robustness weight and the temporal adaptive weight to obtain the spatiotemporal joint adaptive weight, and use the spatiotemporal joint adaptive weight to dynamically modulate the GNSS observation information matrix. S4. Construct the kinematic residual of the inertial measurement unit using vertical motion acceleration, construct the GNSS observation residual using GNSS vertical observation information and dynamically modulated GNSS observation information matrix, combine the kinematic residual and GNSS observation residual, construct a joint optimization objective function for iterative solution, and update the window state and prior information using Shur complement. S5. After the iteration converges, extract the optimal state of the window center epoch as the current heave displacement estimate, and output the final heave result based on the lever arm mapping of the task installation point.

[0027] In S1, the nonlinear observation model is: ; In the formula, Indicates epoch index, Indicates the first The actual observation vector of an epoch. Indicates the first The three-dimensional attitude angle of the carrier in the epoch; Indicates the attitude angle The rotation matrix constructed from the carrier system to the navigation system, This represents the pre-calibrated rigid body reference vector within the load system. Indicates the first The observation noise vector of an epoch; The nonlinear observation model is solved using the least squares algorithm. After outputting the attitude rotation matrix, the posterior unit weight variance is extracted as the scalar of the spatial attitude uncertainty for the current epoch. ; In the formula, Indicates the first Spatial attitude uncertainty of an epoch, For the least squares solution, the first The residual vector of an epoch. Indicates the first The observation weight matrix of the epoch, Indicates the first The system's degrees of freedom in an epoch. It represents the transpose of a matrix or vector.

[0028] In S2, the vertical acceleration is: ; In the formula, Indicates time, express The vertical acceleration at time t, This represents the vertical selection vector. express The attitude rotation matrix at time step; express The three-dimensional specific force vector measured by the inertial measurement unit at any given time. Represents gravitational acceleration; Equivalent GNSS vertical observations are obtained through a nonlinear mapping relationship between the total altitude position of the GNSS antenna phase center in the navigation system and the actual heave displacement lever arm of the ship's center of mass: ; In the formula, express The total altitude position of the GNSS antenna phase center in the navigation system at any given time. This represents the actual heave displacement of the ship's center of mass. This represents the three-dimensional static lever arm vector between the GNSS antenna mounting point and the geometric center of the inertial measurement unit in the carrier system.

[0029] In S3, the spatial robustness weight is: ; ; ; ; ; In the formula, Indicates the first Spatial robustness weights of epochs To find the minimum value function, This represents the scaling factor that controls the divergence boundary of the control space. Represents absolute value. Indicates the first Local robustness scale of an era express The local median within the time window. , Indicates intermediate calculation parameters. To obtain the median function, This represents the epoch index within the time window. Indicates the first Spatial attitude uncertainty of an epoch, Indicates a local time window. To find the maximum value function, The minimum variance lower bound constant is set to prevent the scale from being zero; The time-domain adaptive weights are: ; ; ; In the formula, Indicates the first Time-domain adaptive weights of the epoch. This represents an exponential function with base e. To control the exponential decay coefficient of the time-domain weight decay rate, Indicates the first The adaptive update of the dynamic maneuver variance threshold for each epoch. Indicates intermediate calculation parameters. Indicates the length of the local time window. Indicates the first The angular velocity vector output by the epoch inertial observation unit. Describing the vector norm, The statistical mean of the norm of the angular velocity vector within the time window. This is a summation function.

[0030] The spatiotemporal joint adaptive weights are: ; In the formula, Indicates the first Spatiotemporal joint adaptive weights of epochs To ensure the minimum non-zero weights for factor graph connectivity; The GNSS observation information matrix after spatiotemporal joint adaptive weight dynamic modulation is as follows: ; In the formula, Indicates the modulated first GNSS observation information matrix of the epoch.

[0031] In S4, the state vector of a single epoch within a local time window is defined. The state vector includes the heave displacement of the ship's center of mass, the vertical velocity of the ship's center of mass, and the zero bias of the vertical accelerometer of the ship's center of mass. ; In the formula, Indicates the first The state vector of an epoch, Indicates the first The heave displacement of the ship's center of mass during an epoch. Indicates the first The vertical velocity of the ship's center of mass at an epoch. Indicates the first The vertical accelerometer at the ship's center of mass at the epoch has zero bias. By using midpoint integration, the kinematic residuals of the inertial measurement unit between adjacent states are constructed using vertical motion acceleration: ; ; ; In the formula, Indicates the first Era and the First Kinematic residuals between epochs Indicates the first The time corresponding to an epoch, Indicates the time interval between adjacent epochs. Indicates intermediate calculation parameters; The GNSS observation residuals are: ; In the formula, Indicates the first GNSS observation residuals at each epoch.

[0032] The joint optimization objective function is: ; ; ; ; ; In the formula, This represents the epoch index within the time window. This represents the optimal estimate of the set of state variables within the current sliding window. For historically marginalized a priori residuals, For the IMU kinematic factor cost term, For GNSS observation factor cost term, This represents the marginalized prior residual. Represents the prior information matrix, Indicates the first Era and the First Kinematic residuals between epochs Indicates the first The covariance matrix of the kinematic factors of the inertial measurement unit at each epoch. This represents the weighted Mahalanobis distance of the information matrix. This represents the set of states to be estimated within the window.

[0033] The analytical Jacobian matrices of the kinematic residuals and GNSS observation residuals are: ; ; ; In the formula, , These are the kinematic residuals relative to , Jacobian matrix, For GNSS observation residuals relative to Jacobian matrix; By sparsely assembling the residuals, Jacobian matrix, and information matrix of each factor, a normal equation is established: ; In the formula, For approximate Hessian matrix, Adjust the increment for the state. The residual gradient vector; Sparse Cholesky decomposition is used to solve for the state correction increment, and the window state is updated accordingly: ; In the formula, Indicates the index of the number of nonlinear iterations. Indicates the first The next iteration , Indicates the first The next iteration .

[0034] When a new state node enters the window, the approximate Hessian matrix is ​​divided into blocks according to the state to be marginalized and the state to be retained: ; ; In the formula, This represents the Hessian matrix block corresponding to the state to be marginalized. This represents the Hessian matrix block corresponding to the preserved state. , This is for cross-blocking.

[0035] Window Center Era Index for: ; In the formula, This is the floor function; from Extract the optimal state of the central epoch. : ; In the formula, This represents the optimal heave displacement. Indicates the optimal vertical velocity. This indicates the optimal vertical accelerometer zero bias; The estimated heave of the ship's center of mass is as follows: ; When it is necessary to output the heave at the task mounting point, mapping is performed using a rigid body lever arm: ; In the formula, Output the vertical position rise and fall of the task assignment point. In the context of the loading system, the lever arm vector points from the ship's center of mass to the mission installation point. This represents the attitude rotation matrix at the center moment.

[0036] Indicates from the carrier coordinate system ( From the navigation coordinate system ( The rotation matrix of the navigation system is equivalent to the attitude rotation matrix at the corresponding moment. The navigation system uses the Northeast-Eastern-Upper (ENU) coordinate system, with the vertical direction upwards as... The positive direction of the axis, with the origin of the carrier coordinate system located at the geometric center of the IMU and fixed to the hull. The attitude rotation matrix describes the coordinate transformation relationship from the carrier coordinate system to the navigation coordinate system. It serves as a "bridge" connecting IMU measurements and vertical observations of the navigation system, projecting the specific force measured by the IMU onto the vertical direction of the navigation system and projecting the GNSS lever arm vector onto the vertical direction to compensate for the lever arm effect. It is also used to ultimately map the heave of the reference point to the mission mounting point. Its matrix elements are uniquely determined by the three attitude angles of the hull (heading angle, pitch angle, and roll angle).

[0037] Spatial attitude uncertainty is a quantitative indicator used to evaluate the reliability of epoch-based multi-antenna GNSS attitude calculation results. Specifically, it is defined as the posterior unit-weighted standard deviation after least-squares solution convergence.

[0038] A multi-antenna GNSS array refers to a Global Navigation Satellite System (GNSS) antenna installed at different locations on the ship's hull (usually three or four). These antennas have precisely calibrated spatial geometric positions (i.e., known reference vectors). By simultaneously receiving satellite signals and utilizing the carrier phase difference observations between each antenna, the ship's three-dimensional attitude angles (heading angle, pitch angle, and roll angle) can be directly calculated without relying on external attitude sensors. The multi-antenna GNSS array not only provides a high-precision attitude rotation matrix, but also evaluates the spatial quality of the attitude calculation at the current epoch, i.e., spatial attitude uncertainty, through the residual statistics (posterior unit weight variance) after least squares calculation. This provides a spatial dimension quality perception basis for the calculation of spatiotemporal joint adaptive weights, thereby achieving accurate identification and suppression of attitude-lever arm coupling errors.

[0039] An IMU (Inertial Measurement Unit) is an inertial sensor that integrates a three-axis accelerometer and a three-axis gyroscope to measure the angular velocity and specific force of a carrier in three-dimensional space. Its working principle is based on Newtonian mechanics. The gyroscope measures the angular velocity of the carrier's rotation around the three axes by sensing the Coriolis effect, while the accelerometer measures the specific force (i.e., non-gravitational acceleration, including the resultant force of kinetic acceleration and gravitational acceleration) of the carrier in the three axes by sensing the inertial force on the mass block. Installed inside the ship's hull, the IMU features high-frequency output (typically 100 Hz to 1000 Hz) and short-term high accuracy, enabling continuous capture of the ship's intense vertical maneuvers.

[0040] The three-dimensional specific force provided by the IMU, after being projected onto the vertical direction of the navigation system via the attitude matrix and subtracting gravity, yields the vertical acceleration used to construct the IMU kinematic factors and calculate the kinematic residuals. Meanwhile, the three-dimensional angular velocity provided by the IMU is used to calculate the ship's overall maneuverability, driving the dynamic adjustment of the time-domain adaptive weights. This allows the continuity of state prediction to be maintained through inertial information even when multi-antenna GNSS observations degrade. The physical quantity output by the accelerometer is "specific force," i.e., the non-gravitational external force per unit mass, rather than direct acceleration. The true vertical acceleration is obtained only after subtracting the gravitational component.

[0041] Mahalanobis distance is a weighted distance metric that assigns different weights to the components of the residual based on the uncertainty of the observations. The information matrix-weighted Mahalanobis distance is defined as follows: ; In the joint optimization objective function, the prior residual and prior information matrix Obtained from the marginalization results of the previous sliding window. After the current sliding window completes the linearization of the historical prior factors, IMU kinematic factors, and GNSS observation factors, the residuals, Jacobian matrices, and weight matrices of each factor are sparsely assembled to obtain the approximate Hessian matrix and residual gradient vector.

[0042] When a new state node enters the sliding window, the historical state about to be removed from the window is designated as the state to be marginalized, and the state that needs to be retained in the next sliding window is designated as the retained state. The approximate Hessian matrix and residual gradient vector are then divided into blocks according to the states to be marginalized and the retained states. After eliminating the states to be marginalized using Schur complement, the prior information matrix acting on the retained states is obtained. And the corresponding prior gradient information.

[0043] The prior residual is constructed based on the prior information matrix, prior gradient information, and the linearized points of the state preserved during marginalization. This leads to the formation of a priori cost terms for historical marginalization: ; Through the above marginalization process, the historical IMU observation information and GNSS observation information that have been removed from the sliding window are compressed into prior constraints and continue to participate in the joint optimization of the next sliding window, without having to retain all historical state nodes.

[0044] in, and Used to characterize the prior constraints passed from the historical window to the current window and Used to construct the kinematic constraints of the IMU within the current window. and These three types of constraints are used to form the GNSS observation constraints within the current window, and together they constitute the joint optimization objective function.

[0045] Pre-calibrated rigid body reference vectors under the load system In a statically moored state, high-precision total stations or laser trackers are used to directly measure the relative spatial positions between the phase centers of the GNSS antennas mounted on the hull, or to perform high-precision baseline calculations through differential GNSS static observations. Since all antennas are fixed to the same rigid hull, their relative positions remain constant in the carrier coordinate system; hence, this is called a "pre-calibrated rigid body reference vector." The rigid body reference vector, as a known input in the baseline observation equation, forms an observation-prediction relationship with the navigation system baseline vector actually observed by GNSS. This allows the attitude rotation matrix to be solved through least-squares iteration, thereby achieving high-precision determination of the hull's three-dimensional attitude angles. The calibration accuracy determines the upper limit of the attitude calculation accuracy. In engineering practice, the calibration error is usually controlled within the millimeter level by taking the average of multiple measurements or by joint adjustment.

[0046] Scale factor controlling the spatial divergence boundary This is used to determine at what extent attitude uncertainty deviates from the normal level before weighting begins. The method for determining this is as follows: under known good observation conditions (such as a stationary ship or uniform straight-line navigation), statistically analyze the distribution characteristics of spatial attitude uncertainty and calculate the typical maximum value. Take 1.5 to 2 times the typical maximum value, with an initial value of 2.0 being preferred.

[0047] Observation weight matrix The initial value used to reflect the prior accuracy of GNSS vertical observations is the variance of the statistical observation residuals, which is obtained by collecting GNSS vertical observation data under static conditions (the ship is fixed to the dock) with the known true elevation as a reference. This variance is then used as the initial observation weight matrix.

[0048] Exponential decay coefficient controlling the decay rate of time-domain weights This reflects the sensitivity of the control weight to decay as the maneuver intensity increases. The larger the value, the smoother the decay. The exponential decay coefficient that controls the decay rate of the time domain weight is determined based on the statistical characteristics of the IMU angular velocity and specific force under typical operating sea conditions.

[0049] Minimum nonzero weight This ensures that even if the observation quality at a certain epoch is extremely poor, the weights will not drop to zero, thus maintaining the topological connectivity of the factor graph and the positive definiteness of the Hessian matrix, in a double-precision floating-point arithmetic environment. Usually taken to Magnitude.

[0050] The sliding window length is equal to the number of epochs within the window, used to determine the number of historical state nodes to be retained simultaneously in the factor graph optimization, and is jointly determined by the IMU sampling frequency and the expected surge period.

[0051] Figure 1 The equivalent GNSS observation error curve, magnified by the projection of the rigid lever arm provided by this invention, shows the equivalent GNSS vertical observation error formed by the projection of attitude error onto the rigid lever arm between the GNSS antenna and the IMU. The horizontal axis represents time in seconds, and the vertical axis represents the equivalent GNSS vertical observation error in meters. When the ship's roll and pitch angles change with wave excitation, small disturbances in the attitude calculation are projected onto the vertical direction of the navigation coordinate system through the rigid lever arm between the GNSS antenna and the IMU, thus forming an additional vertical observation error. Figure 1 It is evident that the equivalent observation error exhibits significant time-varying characteristics with changes in ship attitude, and increases markedly during periods of more intense ship motion. This indicates that attitude uncertainty and lever arm projection effect jointly affect the reliability of GNSS vertical observations.

[0052] Figure 2 The comparison results of absolute heave estimation errors between the standard factor map optimization method (FGO) and the method of this invention (RFGO-DIMW) are shown. The horizontal axis represents time in seconds, and the vertical axis represents the absolute error between the estimated heave value and the reference heave value in meters. The standard factor map optimization method uses a fixed or preset GNSS observation information matrix. Figure 1 As the projection error of the lever arm increases, contaminated GNSS vertical observations are easily introduced directly into the optimization process, leading to increased estimation errors. The method of this invention constructs a spatiotemporal joint adaptive weight based on spatial attitude uncertainty and IMU motion characteristics, and dynamically reduces the constraint strength of abnormal GNSS observation factors. Figure 2It is evident that the absolute estimation error of the method of the present invention is lower than that of the standard factor graph optimization method, indicating that the method of the present invention can effectively suppress the influence of attitude-lever arm coupling error on the heave estimation result.

[0053] Figure 3 The comparison of reference heave, pure GNSS heave results, standard factor map optimized heave results, and the heave results of the method of this invention within a local time period is shown. The horizontal axis represents time in seconds, and the vertical axis represents the ship's heave displacement in meters. Pure GNSS results are easily affected by vertical observation noise and attitude lever arm projection errors; although the standard factor map optimization method can smooth GNSS observations using IMU kinematic constraints, it may still produce local amplitude deviations or trajectory offsets when abnormal GNSS observations enter the window. The method of this invention reduces the impact of low-confidence observations on the joint optimization results by dynamically modulating the GNSS observation information matrix. Figure 3 It can be seen that the local heave trajectory obtained by the method of the present invention is in good agreement with the reference heave trajectory in terms of the trend of change, the position of the peak and trough and the vibration phase.

[0054] Figure 4 It shows the relationship with Figure 3 The heave estimation errors of pure GNSS, the standard factor map optimization method, and the method of this invention are compared within the same local time period. The horizontal axis represents time in seconds, and the vertical axis represents the difference between the estimated heave value and the reference heave value for each method in meters. The pure GNSS error exhibits significant fluctuations due to observation noise and lever arm projection effects. The standard factor map optimization method can reduce some random errors, but local errors still increase when GNSS observation quality degrades. The estimation error of the method of this invention is mainly distributed near zero, and the error fluctuation amplitude is smaller than that of the pure GNSS and standard factor map optimization methods. These results demonstrate that the method of this invention can maintain the continuity of the heave state using IMU kinematic constraints when GNSS vertical observations are contaminated, and improve the accuracy and robustness of heave estimation through spatiotemporal joint adaptive weighting.

[0055] The above embodiments are only used to illustrate the technical solutions of the present invention, and are not intended to limit it. Although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some or all of the technical features. Such modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the scope of the technical solutions of the embodiments of the present invention.

Claims

1. A robust heave estimation method for ships based on spatiotemporal feature-based perceptual factor graph optimization, characterized in that, include: S1. Use a multi-antenna GNSS array to acquire baseline observations, construct a nonlinear observation model, perform attitude calculation on the nonlinear observation model, obtain the spatial attitude uncertainty, and output the attitude rotation matrix. S2. Based on the attitude rotation matrix, the specific force of the inertial measurement unit is projected to the vertical direction and gravity is subtracted to obtain the vertical motion acceleration. Based on the rigid lever arm between the GNSS antenna and the IMU, the elevation of the GNSS antenna is mapped to the equivalent vertical observation at the reference point to obtain GNSS vertical observation information. S3. Calculate the spatial robustness weight based on the spatial attitude uncertainty, calculate the temporal adaptive weight based on the changes in the angular velocity and specific force of the inertial measurement unit, fuse the spatial robustness weight and the temporal adaptive weight to obtain the spatiotemporal joint adaptive weight, and use the spatiotemporal joint adaptive weight to dynamically modulate the GNSS observation information matrix. S4. Construct the kinematic residual of the inertial measurement unit using vertical motion acceleration, construct the GNSS observation residual using GNSS vertical observation information and dynamically modulated GNSS observation information matrix, combine the kinematic residual and GNSS observation residual, construct a joint optimization objective function for iterative solution, and update the window state and prior information using Shur complement. S5. After the iteration converges, extract the optimal state of the window center epoch as the current heave displacement estimate, and output the final heave result based on the lever arm mapping of the task installation point.

2. The robust ship heave estimation method based on spatiotemporal feature perceptual factor graph optimization according to claim 1, characterized in that, In S1, the nonlinear observation model is: ； In the formula, Indicates epoch index, Indicates the first The actual observation vector of an epoch. Indicates the first The three-dimensional attitude angle of the carrier in the epoch; Indicates the attitude angle The rotation matrix constructed from the carrier system to the navigation system, This represents the pre-calibrated rigid body reference vector within the load system. Indicates the first The observation noise vector of an epoch; The nonlinear observation model is solved using the least squares algorithm. After outputting the attitude rotation matrix, the posterior unit weight variance is extracted as the scalar of the spatial attitude uncertainty for the current epoch. ； In the formula, Indicates the first Spatial attitude uncertainty of an epoch, For the least squares solution, the first The residual vector of an epoch. Indicates the first The observation weight matrix of the epoch, Indicates the first The system's degrees of freedom in an epoch. It represents the transpose of a matrix or vector.

3. The robust ship heave estimation method based on spatiotemporal feature perceptual factor map optimization according to claim 2, characterized in that, In S2, the vertical acceleration is: ； In the formula, Indicates time, express The vertical acceleration at time t, This represents the vertical selection vector. express The attitude rotation matrix at time step; express The three-dimensional specific force vector measured by the inertial measurement unit at any given time. Represents gravitational acceleration; Equivalent GNSS vertical observations are obtained through a nonlinear mapping relationship between the total altitude position of the GNSS antenna phase center in the navigation system and the actual heave displacement lever arm of the ship's center of mass: ； In the formula, express The total altitude position of the GNSS antenna phase center in the navigation system at any given time. This represents the actual heave displacement of the ship's center of mass. This represents the three-dimensional static lever arm vector between the GNSS antenna mounting point and the geometric center of the inertial measurement unit in the carrier system.

4. The robust ship heave estimation method based on spatiotemporal feature perceptual factor graph optimization according to claim 3, characterized in that, In S3, the spatial robustness weight is: ； ； ； ； ； In the formula, Indicates the first Spatial robustness weights of epochs To find the minimum value function, This represents the scaling factor that controls the divergence boundary of the control space. Represents absolute value. Indicates the first Local robustness scale of an era express The local median within the time window. , Indicates intermediate calculation parameters. To obtain the median function, This represents the epoch index within the time window. Indicates the first Spatial attitude uncertainty of an epoch, Indicates a local time window. To find the maximum value function, The minimum variance lower bound constant is set to prevent the scale from being zero; The time-domain adaptive weights are: ； ； ； In the formula, Indicates the first Time-domain adaptive weights of the epoch. This represents an exponential function with base e. To control the exponential decay coefficient of the time-domain weight decay rate, Indicates the first The adaptive update of the dynamic maneuver variance threshold for each epoch. Indicates intermediate calculation parameters. Indicates the length of the local time window. Indicates the first The angular velocity vector output by the epoch inertial observation unit. Describing the vector norm, The statistical mean of the norm of the angular velocity vector within the time window. This is a summation function.

5. The robust heave estimation method for ships based on spatiotemporal feature perceptual factor graph optimization according to claim 4, characterized in that, The spatiotemporal joint adaptive weights are: ； In the formula, Indicates the first Spatiotemporal joint adaptive weights of epochs To ensure the minimum non-zero weights for factor graph connectivity; The GNSS observation information matrix after spatiotemporal joint adaptive weight dynamic modulation is as follows: ； In the formula, Indicates the modulated first GNSS observation information matrix of the epoch.

6. The robust heave estimation method for ships based on spatiotemporal feature perceptual factor graph optimization according to claim 5, characterized in that, In S4, the state vector of a single epoch within a local time window is defined. The state vector includes the heave displacement of the ship's center of mass, the vertical velocity of the ship's center of mass, and the zero bias of the vertical accelerometer of the ship's center of mass. ； In the formula, Indicates the first The state vector of an epoch, Indicates the first The heave displacement of the ship's center of mass during an epoch. Indicates the first The vertical velocity of the ship's center of mass at an epoch. Indicates the first The vertical accelerometer at the ship's center of mass at the epoch has zero bias. By using midpoint integration, the kinematic residuals of the inertial measurement unit between adjacent states are constructed using vertical motion acceleration: ； ； ； In the formula, Indicates the first Era and the First Kinematic residuals between epochs Indicates the first The time corresponding to an epoch, Indicates the time interval between adjacent epochs. Indicates intermediate calculation parameters; The GNSS observation residuals are: ； In the formula, Indicates the first GNSS observation residuals at each epoch.

7. The robust ship heave estimation method based on spatiotemporal feature perceptual factor graph optimization according to claim 6, characterized in that, The joint optimization objective function is: ； ； ； ； ； In the formula, This represents the epoch index within the time window. This represents the optimal estimate of the set of state variables within the current sliding window. For historically marginalized a priori residuals, For the IMU kinematic factor cost term, For GNSS observation factor cost term, This represents the marginalized prior residual. Represents the prior information matrix, Indicates the first Era and the First Kinematic residuals between epochs Indicates the first The covariance matrix of the kinematic factors of the inertial measurement unit at each epoch. This represents the weighted Mahalanobis distance of the information matrix. This represents the set of states to be estimated within the window.

8. The robust ship heave estimation method based on spatiotemporal feature perceptual factor graph optimization according to claim 7, characterized in that, The analytical Jacobian matrices of the kinematic residuals and GNSS observation residuals are: ； ； ； In the formula, , These are the kinematic residuals relative to , Jacobian matrix, For GNSS observation residuals relative to Jacobian matrix; By sparsely assembling the residuals, Jacobian matrix, and information matrix of each factor, a normal equation is established: ； In the formula, For approximate Hessian matrix, Adjust the increment for the state. The residual gradient vector; Sparse Cholesky decomposition is used to solve for the state correction increment, and the window state is updated accordingly: ； In the formula, Indicates the index of the number of nonlinear iterations. Indicates the first The next iteration , Indicates the first The next iteration .

9. The robust ship heave estimation method based on spatiotemporal feature perceptual factor graph optimization according to claim 8, characterized in that, When a new state node enters the window, the approximate Hessian matrix is ​​divided into blocks according to the state to be marginalized and the state to be retained: ； ； In the formula, This represents the Hessian matrix block corresponding to the state to be marginalized. This represents the Hessian matrix block corresponding to the preserved state. , This is for cross-blocking.

10. The robust ship heave estimation method based on spatiotemporal feature perceptual factor graph optimization according to claim 9, characterized in that, Window Center Era Index for: ； In the formula, This is the floor function; from Extract the optimal state of the central epoch. : ; In the formula, This represents the optimal heave displacement. Indicates the optimal vertical velocity. This indicates the optimal vertical accelerometer zero bias; The estimated heave of the ship's center of mass is as follows: ； When it is necessary to output the heave at the task mounting point, mapping is performed using a rigid body lever arm: ； In the formula, Output the vertical position rise and fall of the task assignment point. In the context of the loading system, the lever arm vector points from the ship's center of mass to the mission installation point. This represents the attitude rotation matrix at the center moment.

Images