Ship attitude self-adaptive sampling stability control method and system

Abstract

The application discloses a ship attitude self-adaptive sampling stability control method and system, and belongs to the field of ship attitude control and self-adaptive sampling control. In order to solve the problems that a fixed sampling period is difficult to reflect the strength of ship attitude change in time, causes control lag, attitude estimation deviation accumulation and redundant control calculation, the application constructs an attitude disturbance representation value, dynamically adjusts a sampling period gear, carries out robust self-adaptive attitude estimation, and triggers model prediction control based on a predicted attitude deviation, so that the technical effects of timely stability control in a disturbance state and reduced redundant calculation in a smooth state are realized.

Description

Technical Field

[0001] This invention relates to the field of ship attitude control and adaptive sampling control, and in particular to a ship attitude adaptive sampling stabilization control method and system. Background Technology

[0002] During navigation, berthing, offshore operations, or dynamic positioning, the roll, pitch, yaw, and sudden acceleration changes caused by waves, steering, and load variations directly affect attitude estimation and control command generation. Existing attitude stabilization and control schemes typically collect inertial measurement unit, heading, speed, and actuator feedback data at fixed sampling periods, and perform filtering estimation and control calculations within fixed control periods.

[0003] A fixed sampling period can maintain basic control functions when attitude changes are gradual, but when attitude disturbances suddenly increase, the sampling time and control optimization time cannot be adjusted synchronously with the disturbance intensity, which can easily cause a time lag in the disturbance information entering the controller. At the same time, dynamic acceleration disturbances will affect the attitude filter update, making it difficult for the attitude estimation residuals and covariance to participate in sampling and control decisions in a timely manner.

[0004] On the other hand, under stable navigation conditions, fixed high-frequency sampling and fixed-frequency control solving will generate a large amount of redundant data and invalid control calculations, increasing the computing power burden of the controller and potentially causing unnecessary actuator instruction updates.

[0005] Therefore, there is a need for a ship attitude adaptive sampling stabilization method and system that can overcome the shortcomings of the existing technologies. Summary of the Invention

[0006] One objective of this invention is to propose a ship attitude adaptive sampling stability control method and system. Addressing the problems in existing technologies where fixed sampling periods fail to reflect the intensity of attitude changes in a timely manner, easily leading to control command lag, accumulated attitude estimation errors, and redundant sampling and ineffective control calculations during stable navigation, this invention proposes a technical solution based on adjusting the sampling period level according to the attitude disturbance characterization value, combined with robust adaptive error state Kalman filtering and event-triggered model predictive control. This invention has the technical effect of timely updating attitude estimation and control commands under disturbance conditions and reducing redundant calculations under stable conditions.

[0007] This invention provides a ship attitude adaptive sampling stabilization method, comprising: S1, collecting ship inertial measurement unit data, heading data, speed data, actuator feedback data, historical control commands, and attitude error data, and performing time alignment on the data to obtain a multi-source attitude observation sequence; S2, constructing attitude disturbance characterization values ​​based on the abrupt changes in roll angular velocity, pitch angular velocity, yaw angular velocity, angular acceleration, and linear acceleration in the multi-source attitude observation sequence, as well as the attitude estimation residual and filter covariance of the previous filtering cycle, and determining the ship's attitude state based on the attitude disturbance characterization values ​​and the set of state thresholds updated from stable navigation data; S3, based on the ship's attitude... The ship's attitude state determines the sampling period setting, which is used to adjust the sensor sampling clock. The adjusted sampling data is then input into a robust adaptive error state Kalman filter to obtain the attitude estimate, attitude estimation residual, and estimation uncertainty determined by the filter covariance. S4: Based on the attitude estimation residual, estimation uncertainty, and historical attitude deviation sequence, the attitude deviation for the next control cycle is predicted to obtain the predicted attitude deviation. S5: The predicted attitude deviation, estimation uncertainty, and actuator constraint state are input into the event-triggered model predictive control module. Based on the event triggering results, the control command is re-solved or the previous control command is maintained and smoothed.

[0008] Optionally, S1 includes:

[0009] The inertial measurement unit data, heading data, speed data, actuator feedback data, historical control commands, and attitude error data are timestamped and aligned with the ship's master control clock as a reference.

[0010] For observation data whose sampling time difference exceeds the set synchronization interval, write a "to be completed" flag;

[0011] Completed observations are generated by interpolating adjacent valid observation data;

[0012] Transform the acceleration and angular velocity in the inertial measurement unit coordinate system to the ship's coordinate system;

[0013] The conversion results are combined with actuator feedback data to form the multi-source attitude observation sequence.

[0014] Optionally, S2 includes:

[0015] The attitude disturbance characterization values ​​are constructed in the following way: using the mean and scale parameters of each observation within the stable navigation baseline window, the roll rate, pitch rate, yaw rate, angular acceleration, linear acceleration abrupt change, attitude estimation residual of the previous filtering cycle, and filter covariance of the previous filtering cycle are normalized to obtain normalized disturbance components.

[0016] The weight set is obtained by normalizing the inverse of the variance of each observation within the stable navigation baseline window;

[0017] The attitude perturbation characterization value is obtained by weighting and summing each normalized perturbation component according to the weight set.

[0018] The set of state thresholds is determined by the quantile of the attitude disturbance characterization value within the stable navigation baseline window, the threshold for the number of consecutive observations, and the hysteresis width.

[0019] Optionally, S3 includes:

[0020] The sampling period levels include a first period level, a second period level, a third period level, and a fourth period level. The sampling periods corresponding to the four sampling period levels increase and decrease according to the level number.

[0021] When the attitude disturbance characterization value crosses the adjacent state threshold and the number of consecutive observations reaches the number of consecutive observations threshold, the sampling period level is switched to the corresponding level.

[0022] When the attitude disturbance characterization value falls back, the sampling period level is switched according to the return boundary formed by the adjacent state threshold and the hysteresis width.

[0023] Furthermore, the robust adaptive error state Kalman filter generates a dynamic acceleration disturbance flag based on the deviation of the linear acceleration magnitude from the gravitational acceleration, the abrupt change in angular acceleration, and the attitude estimation residual.

[0024] The measurement noise covariance is mapped from the baseline covariance to the corrected covariance based on the dynamic acceleration disturbance flag.

[0025] The corrected covariance is input into the filtering update stage to obtain the attitude estimate, attitude estimate residual and filtering covariance, and the estimation uncertainty is determined by the trace value or diagonal component of the filtering covariance.

[0026] Furthermore, the attitude disturbance characterization value-uncertainty coupling trigger gating module receives the sampling period level determined in step S3 and synchronizes it to the sensor sampling clock, and outputs the control optimization trigger flag or control hold flag to the control optimization clock;

[0027] The sensor sampling clock performs sampling updates according to the sampling period level, and the control optimization clock only starts model predictive control solving when the control optimization trigger flag is received.

[0028] When the control optimization clock is not started, the robust adaptive error state Kalman filter still updates the attitude estimate based on the sampling data obtained from the sensor sampling clock.

[0029] Optionally, S4 includes:

[0030] The attitude estimate, attitude estimate residual, estimate uncertainty, historical control commands, and historical attitude deviation sequence formed by the attitude error data in chronological order are stored in the sliding window.

[0031] A perturbation trend vector is generated based on the residual difference of attitude estimation at adjacent time points;

[0032] The perturbation trend vector, the current attitude estimate, and the historical attitude deviation sequence are input into the short-time perturbation predictor, which outputs the predicted attitude deviation.

[0033] The parameters of the short-term disturbance predictor are recursively corrected based on the prediction error within the sliding window, and the corrected parameters are subject to the set parameter boundary constraints.

[0034] Optionally, S5 includes:

[0035] The event triggering result is determined by the attitude disturbance characterization value-uncertainty coupling triggering gating module. The gating module receives the roll angular velocity and its rate of change, pitch angular velocity and its rate of change, yaw angular velocity and its rate of change, linear acceleration abrupt change, attitude estimation residual, estimation uncertainty determined by the filter covariance, predicted attitude deviation and actuator constraint state.

[0036] Normalize each input to the stable navigation baseline;

[0037] The normalization results, estimation uncertainty, and predicted attitude deviation are mapped to a comprehensive trigger index.

[0038] When the comprehensive trigger index reaches the control trigger threshold, the predicted attitude deviation reaches the deviation threshold, or the actuator constraint state reaches the constraint trigger condition determined by the actuator amplitude boundary and the rate of change boundary, the control optimization trigger flag is output; otherwise, the control hold flag is output.

[0039] Furthermore, the actuator constraint state is determined by comparing the rudder angle, thruster thrust, and corresponding rate of change with the set amplitude boundary and the set rate of change boundary, respectively, to generate an amplitude constraint flag and a rate of change constraint flag.

[0040] When the event trigger result is a control hold flag, a smooth correction amount is generated based on the previous control command, the predicted attitude deviation direction, and the actuator constraint state.

[0041] The smoothing correction amount is superimposed on the previous control command and output after being limited by the set amplitude boundary and the set rate of change boundary.

[0042] On the other hand, the present invention also provides a ship attitude adaptive sampling stability control system, comprising:

[0043] The data acquisition and alignment module is used to acquire ship inertial measurement unit data, heading data, speed data, actuator feedback data, historical control commands, and attitude error data, and output a multi-source attitude observation sequence; the disturbance state identification module is used to construct attitude disturbance characterization values ​​and determine the ship's attitude state based on the multi-source attitude observation sequence; the sampling and filtering module is used to adjust the sensor sampling clock according to the ship's attitude state and output the attitude estimate, attitude estimate residual, and estimate uncertainty; the disturbance prediction module is used to output the predicted attitude deviation based on the attitude estimate residual, estimate uncertainty, and historical attitude deviation sequence; and the event-triggered stabilization and control module is used to output control commands based on the predicted attitude deviation, estimate uncertainty, and actuator constraint state.

[0044] The beneficial effects of this invention are:

[0045] 1. By constructing attitude disturbance characterization values ​​from multi-source attitude observation sequences, attitude estimation residuals from the previous filtering cycle, and filtering covariance, and determining the ship's attitude state based on the set of state thresholds, the sampling period can be adjusted according to the degree of attitude disturbance, reducing information lag under severe disturbances with a fixed sampling period.

[0046] 2. The measurement noise covariance is corrected by a robust adaptive error state Kalman filter based on the dynamic acceleration disturbance flag, and the estimation uncertainty is determined by the filtered covariance, so that the attitude estimation results under dynamic acceleration abrupt change can provide traceable uncertainty input for subsequent prediction and trigger control.

[0047] 3. By decoupling the sampling clock from the control optimization clock through the attitude disturbance characterization value-uncertainty coupling trigger gating module, the model predictive control solution is started when the control optimization trigger flag is met, and the previous control command is maintained or smoothly corrected when not triggered, thereby reducing redundant control calculations and reducing control command lag under stable navigation. Attached Figure Description

[0048] The accompanying drawings are provided to further illustrate the invention and form part of the specification. They are used in conjunction with embodiments of the invention to explain the invention and do not constitute a limitation thereof. In the drawings:

[0049] Figure 1 This is a flowchart of a ship attitude adaptive sampling stabilization method.

[0050] Figure 2 This is a flowchart of step S2 of the present invention, which involves constructing attitude disturbance characterization values ​​and identifying ship attitude state. Detailed Implementation

[0051] The present invention will now be described in further detail with reference to the accompanying drawings. These drawings are simplified schematic diagrams, illustrating only the basic structure of the invention, and therefore only show the components relevant to the invention.

[0052] refer to Figure 1 An adaptive sampling stabilization method for ship attitude includes: S1, collecting ship inertial measurement unit data, heading data, speed data, actuator feedback data, historical control commands, and attitude error data, and performing time alignment on the data to obtain a multi-source attitude observation sequence; S2, constructing attitude disturbance characterization values ​​based on the abrupt changes in roll angular velocity, pitch angular velocity, yaw angular velocity, angular acceleration, and linear acceleration in the multi-source attitude observation sequence, as well as the attitude estimation residual and filter covariance of the previous filtering cycle, and determining the ship's attitude state based on the attitude disturbance characterization values ​​and the set of state thresholds updated from stable navigation data; S3, determining the ship's attitude state based on the ship's attitude... The sampling period is determined by the state, and the sensor sampling clock is adjusted using the sampling period. The adjusted sampling data is then input into a robust adaptive error state Kalman filter to obtain the attitude estimate, attitude estimation residual, and estimation uncertainty determined by the filter covariance. S4: Based on the attitude estimation residual, estimation uncertainty, and historical attitude deviation sequence, the attitude deviation for the next control cycle is predicted to obtain the predicted attitude deviation. S5: The predicted attitude deviation, estimation uncertainty, and actuator constraint state are input into the event-triggered model predictive control module. Based on the event triggering results, the control command is re-solved or the previous control command is maintained and smoothed.

[0053] In this specific embodiment, S1 includes:

[0054] The ship's master clock uses a fixed reference period Generate reference time series ,in Indicates the first One alignment moment, The time interval between adjacent alignment moments is indicated; inertial measurement unit data, heading data, velocity data, actuator feedback data, historical control commands, and attitude error data all carry their own local timestamps, which are converted to timestamps consistent with those of the main control unit after entering the main control system, and are denoted as follows: The superscript IMU indicates the inertial measurement unit data source, HDG indicates the heading data source, SPD indicates the velocity data source, ACT indicates the actuator feedback data source, CMD indicates the historical control command data source, and ERR indicates the attitude error data source. The subscripts... This indicates the first... Strip sampling;

[0055] The master controller maintains a length of [length] for each data source. A circular buffer is used and written to incremented timestamps, and in each Upon arrival, retrieve the location located in each buffer. Two consecutive valid observations are denoted as "previous valid observation" and "next valid observation," respectively, and their timestamps are denoted as follows: and ,in Identify the data source;

[0056] Calculate the sampling time difference for each data source. and When either of them is greater than the set synchronization interval At that time, the data source will be at time... The observations were written to the markers to be completed. ,in Indicates the maximum allowable alignment deviation. Indicates alignment time Data source The status of completion, This indicates that something needs to be added. This indicates that no further information is needed.

[0057] when At that time, the main controller is in the maximum waiting time The system continuously waits for the arrival of the "next valid observation" to ensure that the alignment of adjacent valid observations required for interpolation is completed. This indicates the maximum processing delay that interpolation can introduce, when in Internal acquisition and After the corresponding valid observations are obtained, time-weighted linear interpolation is used to generate supplementary observations, which are then overwritten into the data source. The alignment result, while preserving This allows subsequent steps to identify and complete the location where the event occurred.

[0058] For inertial measurement unit data, an aligned acceleration vector is obtained after interpolation. With angular velocity vector ,in Indicates the alignment to the inertial measurement unit coordinate system. The column vector of accelerations along the three axes, Indicates the coordinate system of the inertial measurement unit, aligned to The three-axis angular velocity column vector is then transformed from the inertial measurement unit coordinate system to the hull coordinate system to ensure that subsequent disturbance characterization and filtering estimation are performed under the same coordinate reference. The transformation relationship is as follows:

[0059] ,

[0060] ,

[0061] ,

[0062] ;

[0063] in and These represent the inertial measurement units in... and Effective acceleration observations at the location, This represents the interpolation weight determined by the position of the alignment time between adjacent valid observations. Indicates alignment to the ship's coordinate system. The column vector of accelerations along the three axes, Indicates alignment to the ship's coordinate system. The column vector of three-axis angular velocities, This represents the direction cosine matrix from the inertial measurement unit coordinate system to the ship's coordinate system, and is a constant matrix. This matrix is ​​constructed from the fixed attitude misalignment angles obtained during installation and calibration, and stored in the main control unit. Installation and calibration continuously acquire data while the ship is stationary. The inertial measurement unit outputs data and, combined with the gravity direction in the ship's coordinate system and the sensor installation direction recorded by the main control unit, calculates the misalignment angle, thereby ensuring... Maintain consistency throughout the entire voyage;

[0064] The heading angle is obtained after aligning the heading data. ,in Indicates alignment to The heading angle scalar and its value range is After aligning the speed data, the speed scalar is obtained. ,in Indicates alignment to The sailing speed, after aligning the actuator feedback data, yields the rudder angle. Thruster thrust The previous time-to-time feedback buffer required for its rate of change, and the control command of the previous cycle obtained after aligning with the historical control commands. ,in Indicates in Output at all times and The control command vectors that are still accessible are used to align the attitude error data to obtain the attitude error. ,in Indicates alignment to The attitude error vector;

[0065] The main controller will and the set of markers to be completed for each data source. According to the unified timestamp Package output, by The increasing sequence forms a multi-source attitude observation sequence, which is then used in step S2.

[0066] In this specific embodiment, S2 includes:

[0067] The main controller at each alignment moment An attitude perturbation characterization value is constructed based on multi-source attitude observation sequences, and attitude state identification is completed.

[0068] The main control first starts with the angular velocity vector in the ship's coordinate system. Extracting the roll angular velocity from the three components Pitch angular velocity With bow roll angular velocity ,in And superscript Indicates the ship's coordinate system;

[0069] The main controller uses the reference cycle right Perform adjacent differences and divide by Obtain the angular acceleration vector And take one norm to obtain the angular acceleration scalar. ,in Indicates alignment to The column vector of triaxial angular accelerations, Describes the norm 1. Indicates the magnitude of angular acceleration;

[0070] The main control system's linear acceleration vector in the ship's coordinate system Calculate the adjacent differences and take the L2 norm to obtain the linear acceleration abrupt change. ,in Indicates alignment to The linear acceleration abrupt change scalar;

[0071] The master controller reads the attitude estimation residual vector from the buffer of the previous filtering cycle. With the filter covariance matrix And calculate the residual amplitude respectively. covariance trace value ,in This represents the magnitude scalar of the attitude estimation residual from the previous filtering cycle. This represents the source of estimation uncertainty in the previous filtering cycle. Represents the matrix trace operation;

[0072] During system initialization, set and This serves as the initial cache value for the first cycle to ensure sequence availability;

[0073] The master controller arranges the above seven disturbance-related quantities into a disturbance component vector in a fixed order. ,in Indicates alignment to The perturbation component column vector;

[0074] Main control maintenance length is Stable navigation baseline window The baseline window only receives the set of markers to be filled that satisfy the condition that the attitude state was stationary at the previous time step and that were output in step S1. All of them The samples are updated in a first-in, first-out (FIFO) manner, among which... This represents the sample set used as a statistical baseline. Indicates alignment time Data source No completion occurred; the master controller checked the baseline window. Calculate the mean of each component. With scale parameters ,in The component number is the component sequence number. Indicates the first Each component in The sample mean within, Indicates the first Each component in The sample standard deviation within and when the calculated result is less than Forced take To avoid normalization divergence;

[0075] The main controller further based on Construct weights The attitude perturbation characterization value is obtained by normalizing each component and then weighting and summing them. The calculation is as follows:

[0076] ,

[0077] ;

[0078] in Indicates alignment to The attitude perturbation characterization scalar value, Representing vectors The One portion, This represents the absolute value operation. Indicates the first The weighting coefficients of each component, Indicates the component index used for weighted normalization summation;

[0079] The main controller is based on the baseline window. Inside Constructing a set of state thresholds from the quantile values ​​of the sequence ,in Pick Inside The 0.60 quantile of the sequence, Take the 0.80 quantile. Take the 0.95 quantile. Indicates the threshold for the number of consecutive observations. Indicates the hysteresis width and is used to form the return boundary. ;

[0080] Master control state variables Indicate the ship's attitude status and complete the initial judgment according to the threshold range. Indicates the corresponding stationary state Indicates a state of slight disturbance Indicates the state corresponding to moderate disturbance , Indicates the corresponding state of strong disturbance ;

[0081] To suppress state jitter, the master controller sets counters for "upward jump changes crossing adjacent thresholds" and "downward jump changes falling back to the return boundary" and executes a continuous judgment mechanism. continuous Only when the interval condition for entering a higher state is met will it be... Update to a higher state, when continuous The next lowest return boundary is below the current state. Only then will Update to a lower state;

[0082] Main controller Continuous maintenance After a certain alignment time, use the segment that satisfies... Recalculate the sample where all values ​​are 0 , and It also overwrites and updates, making the set of state thresholds adaptively updated with the smooth navigation data and outputting the attitude state. Provided for use in step S3.

[0083] In this specific embodiment, S3 includes:

[0084] Master control receives attitude disturbance characterization values State threshold set With attitude state ,in These represent the threshold values ​​of three adjacent states that characterize the attitude perturbation. Indicates the threshold for the number of consecutive observations. Indicates the hysteresis width. It represents four attitude states: stable, slightly disturbed, moderately disturbed, and strongly disturbed.

[0085] The main controller is set with four sampling period levels. And solidify the corresponding sampling period set ,in Indicates alignment time The sampling period that takes effect afterward is in place. Indicates the first The sampling period decreases when the sampling period corresponding to the current position is satisfied and the position number increases.

[0086] The main controller maintains the gear shift counter at each alignment moment. Gear downshift counter ,in For statistics The number of times the price falls into a higher disturbance range consecutively. For statistics The number of consecutive times the value falls below the return boundary;

[0087] when From the current gear level's corresponding threshold range, cross the adjacent upper threshold and continuously When the conditions for entering a higher disturbance range are met for the second time, the main controller will... Switch to a higher setting and and Reset to zero, when pullback and continued The next lower than the return boundary corresponding to the current gear. At that time, the main controller will Switch to a lower gear and and Reset to zero;

[0088] To ensure consistency between gear position and attitude state, after completing the above counting and hysteresis determination, the main controller will adjust the gear position. With attitude state Perform a consistent mapping, the mapping relationship is as follows season season season season and with this unified As the final gear output of this cycle;

[0089] The main controller will adjust the sampling period. The timer reload value register that synchronously writes the sensor sampling clock enables the inertial measurement unit to interface with the heading data. A sampling trigger interrupt is generated once per period, where Indicates gear position The indexed sampling period, and the sampling trigger interrupt, serve as the source of the "alignment time" in step S1 to generate a new one. This causes the subsequent attitude estimation update frequency to change with the gear position;

[0090] When each sampling trigger interrupt occurs, the main controller obtains the aligned linear acceleration vector of the hull coordinate system from step S1. Angular velocity vector in the ship's coordinate system With heading angle observation ,in and superscript Indicates the ship's coordinate system. Indicates alignment to The heading angle and its range is And set the discrete time step of this filtering to be ,in This represents the time interval between two consecutive filter updates;

[0091] The master controller executes a robust adaptive error state Kalman filter, and the nominal state of the filter includes attitude quaternions. gyroscope zero bias Zero bias of accelerometer ,in This represents the attitude quaternion from the navigation coordinate system to the hull coordinate system. This represents the zero-bias column vector of the three-axis angular velocities. This represents the zero-bias column vector of acceleration along three axes, with the error state using a small angular error. With zero bias error The prediction-update process is constructed and recursively performed according to the error state Kalman filter.

[0092] In the prediction phase, the main controller uses the zero-offset angular velocity. Quaternions Perform first-order integration to obtain the predicted pose, and update using a zero-biased random walk model. and The process noise covariance is set as a constant diagonal matrix, and the diagonal elements are sequentially selected. ,in Indicates the standard deviation of angular velocity drive noise. This represents the standard deviation of the zero-bias random walk of the gyroscope. This indicates the standard deviation of the accelerometer's zero-bias random walk.

[0093] During the update phase, the master control system constructs a combined observation of "gravity direction + heading angle," specifically... Normalization yields a unit direction vector, which is then subtracted from the unit gravity direction vector calculated from the predicted attitude to form the acceleration direction observation residual. The heading angle observation is then... Subtract the predicted heading angle from the predicted attitude and normalize the angle difference to 0. The heading observation residual is generated, thus obtaining the attitude estimation residual vector. ,in This represents the column vector of the comprehensive observation residuals after the current period's filtering update, and it is cached for use in step S2.

[0094] To achieve robust adaptation, the main controller is based on a dynamic acceleration disturbance flag. Corrected measurement noise covariance, where The value is 1 if and only if the following decision process is satisfied simultaneously:

[0095] The main controller first calculates the linear acceleration modulus. and the constant of gravitational acceleration In comparison, when Time-based perturbation condition. This indicates that the acceleration magnitude deviates from the threshold.

[0096] The main controller then reads the linear acceleration abrupt change calculated in step S2. and with threshold In comparison, when The time-set disturbance condition, where The L2 norm represents the difference in linear acceleration between adjacent time points;

[0097] The main controller then reads the residual amplitude from the previous filtering cycle. and with threshold In comparison, when The time-set disturbance condition, where This represents the L2 norm of the attitude estimation residual vector from the previous cycle;

[0098] When any one of the above three disturbance conditions is set, the master control command... Otherwise ;

[0099] The master controller will measure the noise covariance from the baseline covariance. Mapping to modified covariance ,in It is a diagonal matrix and the diagonal elements are taken in sequence. This represents the standard deviation of observed noise in the acceleration direction. The standard deviation of the heading angle observation noise is represented by the following mapping relationship: in Indicates alignment to The corrected measurement noise covariance matrix, Indicates the noise scaling factor. This represents the measurement noise covariance matrix under a stable navigation baseline. This represents the perturbation scaling factor. Indicates dynamic acceleration disturbance;

[0100] The main controller will Substituting the values ​​into the filter update calculation, the Kalman gain is calculated and the error state and nominal state are updated to obtain the attitude estimate. With the filter covariance matrix ,in This represents the Euler angle attitude estimation vector output in this cycle. This represents the estimated roll angle. This represents the estimated pitch angle. This represents the estimated bow roll angle. This represents the error state estimation covariance matrix;

[0101] Main control The uncertainty of trace value calculation estimation ,in This represents the estimation uncertainty scalar used for subsequent disturbance prediction and trigger gating. Represent the trace operation, and With sampling period Output to the attitude disturbance characterization value-uncertainty coupling trigger gating module;

[0102] The gating module will The sampling clock is synchronized to the sensor sampling clock to ensure that the sampling cycle is consistent with the gear position. The control optimization trigger flag or control hold flag generated in subsequent steps is output to the control optimization clock to decouple the sampling clock from the control optimization clock. Even when the control optimization clock is not activated, the main controller still strictly updates the robust adaptive error state Kalman filter according to the sensor sampling clock trigger to continuously output the signal. and .

[0103] In this specific embodiment, S4 includes:

[0104] The main controller at each alignment moment Received attitude estimation value Attitude estimation residual vector Estimating uncertainty and the historical control commands output and cached by S1 With attitude error vector ,in Indicates the estimated value of the roll angle. Pitch angle estimate Estimated bow roll angle The composed attitude estimation vector This represents the composite observation residual vector obtained by concatenating the "gravity direction observation residual" and the "heading observation residual", with a fixed dimension. The expression is derived from the filter covariance matrix. The scalar of the estimation uncertainty determined by the trace value. This represents the control command vector that was output and accessible at the previous alignment time. Indicates alignment to The attitude error data is used as the attitude deviation and is compared with the attitude error data. Under the same Euler angle definition, the deviations of the three channels—roll, pitch, and yaw—are characterized.

[0105] The master control setup length is sliding window And save the sequence in a first-in, first-out manner. and the historical attitude deviation sequence formed in chronological order ,in Indicates at time Available short-term historical data set;

[0106] The master controller constructs a perturbation trend vector based on the residual difference of attitude estimation at adjacent time steps. ,in It represents the residual change trend vector and is used to characterize the short-term trend of disturbance enhancement or decay;

[0107] The main controller will Current attitude deviation and attitude deviation at the previous moment The input feature vectors that together constitute the short-term disturbance predictor It also employs a linear short-time disturbance predictor with recursively correctable parameters to output the predicted attitude deviation at the next alignment time. The calculation is as follows:

[0108] ;

[0109] in Represents the predicted attitude deviation vector and is related to Dimensional consistency This represents the parameter matrix of the short-time disturbance predictor, with a fixed dimension. Represents the input feature column vector. This represents the current attitude deviation vector. This represents the attitude deviation vector at the previous moment. This represents the perturbation trend vector. This represents the estimation uncertainty scalar, and the constant 1 indicates that the bias term is used to compensate for systematic bias.

[0110] Parameter matrix of short-time disturbance predictor Employs a recursive least squares update mechanism and uses only a sliding window. The latest set of "prediction errors" is used for recursive correction. Prediction error is defined as follows: ,in Indicates at time Due to actual attitude deviation Deviation from the predicted attitude output at the previous time step The error vector obtained from the difference;

[0111] The forgetting factor in recursive least squares is fixed at 1. And the parameter covariance is initialized to ,in The forgetting factor represents the decay of historical sample weights. Represents the initial value matrix of parameter covariance. Indicates and A dimensionally consistent identity matrix;

[0112] To ensure recursive stability and physical interpretability, the master controller... Each element is subject to boundary constraints and saturation pruning is performed after each recursive update, with the pruning boundary fixed at [value]. and ,in Indicates the upper limit of the magnitude of the parameter element;

[0113] The main controller will With the corresponding The results are output to step S5 for event-triggered gating and control optimization decision-making.

[0114] In this specific embodiment, S5 includes:

[0115] Attitude perturbation characterization value - uncertainty coupling triggers gating module at each alignment time Receive attitude perturbation characterization values Received estimate uncertainty With attitude estimation residual vector Receive predicted attitude deviation And read the angular velocity components of the hull coordinate system from the multi-source attitude observation sequence in step S1. and its rate of change ,in , This indicates that the current filtering time step determined in step S3 is equal to the sampling period corresponding to the sampling period level;

[0116] The gating module also reads the linear acceleration abrupt change. rudder angle with actuator feedback Thruster thrust and its rate of change ,in , ;

[0117] The gating module constructs the actuator constraint state based on the actuator amplitude boundary and rate of change boundary, specifically by... and The comparison yields the rudder angle amplitude constraint flag, and the following is a summary of the data. and The comparison yields the constraint flag for the rate of change of rudder angle, and the result is... and The thrust amplitude constraint flag is obtained by comparison, and then... and The thrust change rate constraint flag is obtained by comparison, and the actuator constraint state is set when any constraint flag is 1. Otherwise ,in Represents the actuator constraint state scalar; gating module for and Step-by-step, step S2, for the stable navigation baseline window. The statistically obtained mean and scaling parameter are normalized. The normalization method is to subtract the corresponding baseline mean from the current value, divide by the corresponding scaling parameter, and take the absolute value. The lower limit of the scaling parameter is fixed. To avoid normalization divergence;

[0118] The gating module maps the normalized result to the actuator constraint state as a comprehensive trigger index. The event triggering result is determined by a combination of a comprehensive triggering index, a predicted attitude deviation threshold, and actuator constraint triggering conditions. The comprehensive triggering index is calculated as follows: in Indicates alignment to The comprehensive trigger indicator scalar, Represents a column vector of weights and takes , The normalized input column vector, defined in a fixed order, is as follows:

[0119] ;

[0120] They represent The normalization result, They represent The normalization result, express The normalization result, Indicates residual amplitude The normalization result, Indicates the uncertainty of the estimate The normalization result, Indicates the predicted attitude deviation magnitude The normalization result, Indicates transpose;

[0121] Gating module settings control trigger threshold Deviation threshold and output control optimization trigger flag. Or control and maintain the sign , among which when Time setting ,when Time setting ,when Time setting In other cases, place ;

[0122] when The time-control optimization clock is started and the model predictive control solution is executed. The model predictive control uses a discrete linear predictive model and calculates the current state. As the initial state, where The attitude estimate is output from step S3;

[0123] The control quantity for model predictive control is defined as follows: ,in Indicates rudder angle control command, Indicates thrust control command;

[0124] The prediction step size of model predictive control is taken as follows Control step size The cost function is weighted by attitude deviation and control increment, and the attitude deviation weight matrix is ​​fixed. The control increment weight matrix is ​​fixed as follows: and the actuator amplitude boundary Boundary of the rate of change , The solution is implemented as a hard constraint using an interior-point quadratic programming solver, completing the solution within 20ms. If the solution fails, it reverts to a control-hold strategy and is then applied. ;

[0125] when The time-gating module does not initiate model predictive control solving but performs control hold and smoothing correction. Control hold is based on historical control commands. Based on the predicted attitude deviation direction, a smooth correction amount is generated. The smoothing correction amount is obtained through a fixed gain matrix. and Multiplying together yields and in This will cause the saturated component to be set to zero to avoid further triggering of constraints, and then... Overlay The control command to be output is obtained and passes through the amplitude boundaries in sequence. Boundary of the rate of change After the amplitude limiting process, the output is sent to the servo and thruster actuators, and the final output control command is cached as the historical control command for the next alignment moment for use in steps S1 and S4.

[0126] The above description is only a preferred embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any equivalent substitutions or modifications made by those skilled in the art within the scope of the technology disclosed in the present invention, based on the technical solution and inventive concept of the present invention, should be covered within the scope of protection of the present invention.

[0127] This invention establishes a closed control chain by linking attitude disturbance characterization values, estimated uncertainties, predicted attitude deviations, and actuator constraint states, thereby enabling ship attitude estimation, sampling period adjustment, and control optimization triggering. This chain can increase sampling and control response density when attitude changes are drastic, and reduce sampling and solution frequencies when attitude changes are stable.

[0128] The technical solution of this invention does not simply adjust the threshold, but sets up a coupled trigger gating relationship between the sampling clock and the control optimization clock, and determines the estimation uncertainty through filtering covariance, so that disturbance identification, attitude estimation and stability triggering share a consistent parameter source, thereby achieving more stable adaptive sampling stability control of ship attitude.

Claims

1. A ship attitude adaptive sampling stabilization method, characterized in that, include: S1. Collect ship inertial measurement unit data, heading data, speed data, actuator feedback data, historical control commands and attitude error data, and perform time alignment on the data to obtain a multi-source attitude observation sequence; S2. Based on the abrupt changes in roll rate, pitch rate, yaw rate, angular acceleration, and linear acceleration in the multi-source attitude observation sequence, as well as the attitude estimation residual and filter covariance of the previous filtering cycle, construct attitude disturbance characterization values, and determine the ship's attitude state based on the attitude disturbance characterization values ​​and the set of state thresholds updated by the stable navigation data. S3. Determine the sampling period setting based on the ship's attitude state, adjust the sensor sampling clock using the sampling period setting, and input the adjusted sampling data into the robust adaptive error state Kalman filter to obtain the attitude estimate, attitude estimation residual, and estimation uncertainty determined by the filter covariance. S4. Based on the attitude estimation residual, estimation uncertainty and historical attitude deviation sequence, predict the attitude deviation for the next control cycle to obtain the predicted attitude deviation; S5. Input the predicted attitude deviation, estimated uncertainty and actuator constraint state into the event-triggered model predictive control module, and re-solve the control command based on the event triggering result or perform hold and smooth correction on the previous control command. The event triggering result is determined by the attitude disturbance characterization value-uncertainty coupling triggering gating module. The gating module receives the roll angular velocity and its rate of change, pitch angular velocity and its rate of change, yaw angular velocity and its rate of change, linear acceleration abrupt change, attitude estimation residual, estimation uncertainty determined by the filter covariance, predicted attitude deviation, and actuator constraint state. It normalizes each input according to the stable navigation baseline. It maps the normalization result, estimation uncertainty, and predicted attitude deviation to a comprehensive triggering index. When the comprehensive triggering index reaches the control triggering threshold, the predicted attitude deviation reaches the deviation threshold, or the actuator constraint state reaches the constraint triggering condition determined by the actuator amplitude boundary and rate of change boundary, it outputs a control optimization triggering flag; otherwise, it outputs a control hold flag. The actuator constraint state is determined as follows: the rudder angle, thruster thrust, and corresponding rate of change are compared with the set amplitude boundary and the set rate of change boundary, respectively, to generate an amplitude constraint flag and a rate of change constraint flag; when the event trigger result is a control hold flag, a smoothing correction amount is generated based on the previous control command, the predicted attitude deviation direction, and the actuator constraint state; the smoothing correction amount is superimposed on the previous control command and output after being limited by the set amplitude boundary and the set rate of change boundary.

2. The ship attitude adaptive sampling stabilization method according to claim 1, characterized in that, S1 includes: Using the ship's master clock as a reference, the inertial measurement unit data, heading data, velocity data, actuator feedback data, historical control commands, and attitude error data are timestamped and aligned; observation data with sampling time differences exceeding the set synchronization interval are marked as to be supplemented; supplemented observation values ​​are generated by interpolation using adjacent valid observation data; the acceleration and angular velocity in the inertial measurement unit coordinate system are transformed to the ship's coordinate system; and the transformation results are combined with the actuator feedback data to form the multi-source attitude observation sequence.

3. The ship attitude adaptive sampling stabilization method according to claim 1, characterized in that, The attitude disturbance characterization value described in step S2 is constructed in the following way: using the mean and scale parameters of each observation within the stable navigation baseline window, the roll rate, pitch rate, yaw rate, angular acceleration, linear acceleration abrupt change, attitude estimation residual of the previous filtering cycle, and filter covariance of the previous filtering cycle are normalized to obtain the normalized disturbance component. The weight set is obtained by normalizing the inverse of the variance of each observation within the stable navigation baseline window; The attitude disturbance characterization value is obtained by weighting and summing each normalized disturbance component according to the weight set; the state threshold set is determined by the quantile value of the attitude disturbance characterization value within the stable navigation baseline window, the consecutive observation number threshold, and the hysteresis width.

4. The ship attitude adaptive sampling stabilization method according to claim 3, characterized in that, The sampling period levels mentioned in step S3 include a first period level, a second period level, a third period level, and a fourth period level. The sampling periods corresponding to the four sampling period levels increase and decrease according to the level number. When the attitude disturbance characterization value crosses the adjacent state threshold and the number of consecutive observations reaches the consecutive observation number threshold, the sampling period level is switched to the corresponding level. When the attitude disturbance characterization value falls back, the sampling period level is switched according to the return boundary formed by the adjacent state threshold and the hysteresis width.

5. The ship attitude adaptive sampling stabilization method according to claim 1, characterized in that, The robust adaptive error state Kalman filter described in step S3 generates a dynamic acceleration disturbance flag based on the deviation of the linear acceleration magnitude from the gravitational acceleration, the abrupt change in angular acceleration, and the attitude estimation residual. The measurement noise covariance is mapped from the baseline covariance to the corrected covariance based on the dynamic acceleration disturbance flag; the corrected covariance is input into the filtering update stage to obtain the attitude estimate, attitude estimate residual and filtering covariance, and the estimation uncertainty is determined by the trace value or diagonal component of the filtering covariance.

6. The ship attitude adaptive sampling stabilization method according to claim 1, characterized in that, S4 includes: storing attitude estimates, attitude estimation residuals, estimation uncertainties, historical control commands, and a historical attitude deviation sequence formed by the attitude error data in chronological order within a sliding window; generating a disturbance trend vector based on the difference of attitude estimation residuals at adjacent time points; inputting the disturbance trend vector, the current attitude estimate, and the historical attitude deviation sequence into a short-time disturbance predictor, and outputting the predicted attitude deviation; the parameters of the short-time disturbance predictor are recursively corrected according to the prediction error within the sliding window, and the corrected parameters are subject to set parameter boundary constraints.

7. The ship attitude adaptive sampling stabilization method according to claim 6, characterized in that, The attitude disturbance characterization value-uncertainty coupling trigger gating module receives the sampling period level determined in step S3 and synchronizes it to the sensor sampling clock, and outputs the control optimization trigger flag or control hold flag to the control optimization clock; the sensor sampling clock performs sampling updates according to the sampling period level, and the control optimization clock only starts model predictive control solving when it receives the control optimization trigger flag; when the control optimization clock is not started, the robust adaptive error state Kalman filter still updates the attitude estimate value according to the sampling data obtained by the sensor sampling clock.

8. A ship attitude adaptive sampling stabilization system, used to execute the ship attitude adaptive sampling stabilization method according to any one of claims 1 to 7, characterized in that, include: The data acquisition and alignment module is used to acquire ship inertial measurement unit data, heading data, speed data, actuator feedback data, historical control commands and attitude error data, and output multi-source attitude observation sequences; The disturbance state identification module is used to construct attitude disturbance characterization values ​​and determine the ship's attitude state based on the multi-source attitude observation sequence. The sampling and filtering module is used to adjust the sensor sampling clock according to the ship's attitude state and output the attitude estimate, attitude estimate residual, and estimation uncertainty; the disturbance prediction module is used to output the predicted attitude deviation based on the attitude estimate residual, estimation uncertainty, and historical attitude deviation sequence. The event-triggered stabilization module is used to output control commands based on the predicted attitude deviation, estimated uncertainty, and actuator constraint state.

Images