State estimation and control method and system for autonomous marine vehicle

Abstract

The invention provides a state estimation and control method and system for an autonomous marine vehicle, and relates to the technical field of autonomous navigation of autonomous marine vehicles, and the method comprises the steps: constructing a state augmentation estimation model; the method comprises the following steps: constructing mixed layered distribution of Gaussian distribution, Pearson type VI I distribution and inverse Weishat distribution, and carrying out dynamic modeling on non-stationary heavy tail measurement noise; performing state estimation through a variational Bayesian unscented Kalman filtering algorithm based on the mixed hierarchical distribution in combination with adaptive parameter selection; and designing an adaptive filtering state feedback trajectory tracking controller according to a state estimation result, generating an actual control command with output saturation constraint, and performing trajectory tracking control. According to the scheme of the invention, the technical problems of filtering divergence caused by non-stationary heavy tail measurement noise caused by a complex marine environment and reduced state feedback control precision caused by measurement error coupling are solved. The scheme of the invention is suitable for AUVs and ASVs of various scales, and has good practical application value.

Description

Technical Field

[0001] The present invention relates to the field of autonomous navigation technology for autonomous ocean vehicles, and in particular to a state estimation and control method and system for autonomous ocean vehicles. Background Art

[0002] Autonomous ocean vehicles (AUVs, ASVs) are essential marine operating platforms, playing an increasingly crucial role in marine resource exploration, environmental monitoring, underwater engineering maintenance, and other fields. Their intelligent navigation and control systems rely heavily on the accuracy of state estimation methods and control strategies. In complex marine environments, sensors are subject to turbulent disturbances and electronic noise, resulting in widespread non-Gaussian noise contamination in measurement data, severely limiting the accuracy of track tracking during AUV operations.

[0003] Autonomous ocean vehicles exhibit complex dynamics and high-dimensional nonlinearities. Existing filtering methods for nonlinear systems, such as the EKF, UKKF, and CKF algorithms and their variants, rely on two assumptions: first, that noise follows a Gaussian distribution; and second, that the noise covariance matrix can be accurately represented. In complex ocean environments, sensor output data is subject to non-Gaussian noise, accompanied by non-stationary, heavy-tailed characteristics, and outliers, due to factors such as turbulence and device drift. These assumptions often fail to accurately simulate the characteristics of real-world noise, leading to the accumulation of measurement errors and, in turn, compromising the positioning accuracy of the vehicle.

[0004] Existing trajectory tracking control strategies typically use sensor measurements directly as state feedback, ignoring the dynamic impact of measurement errors and lacking a collaborative estimation-control optimization mechanism. When sensor data is disrupted by outliers, control commands are generated based on the distorted state, resulting in reduced trajectory tracking accuracy and making it difficult to safely and stably complete maritime operations. Summary of the Invention

[0005] The purpose of the present invention is to provide a state estimation and control method and system for an autonomous ocean vehicle, so as to solve the problem mentioned in the above background technology that the existing trajectory tracking control strategy uses sensor measurement values as state feedback, which is difficult to solve the problem of non-stationary heavy-tailed measurement noise caused by complex ocean environment leading to filtering divergence and measurement error coupling causing decreased state feedback control accuracy.

[0006] To achieve the above-mentioned objectives, the present invention provides the following technical solutions: a state estimation and control method for an autonomous ocean vehicle, comprising the following steps: constructing a state augmented estimation model; constructing a mixed stratified distribution of Gaussian distribution, Pearson-type VII distribution, and inverse Wishart distribution to dynamically model non-stationary heavy-tailed measurement noise; performing state estimation through a variational Bayesian unscented Kalman filtering algorithm based on the mixed stratified distribution combined with adaptive parameter selection; designing an adaptive filtering state feedback trajectory tracking controller based on the state estimation results, and generating an actual control command with output saturation constraint to perform trajectory tracking control.

[0007] Optionally, the step of constructing a state augmented estimation model specifically includes: constructing an augmented state estimation model of an autonomous ocean vehicle system based on a state augmented vector, a state transfer function and a measurement function; wherein the state augmented vector includes a vehicle state vector, a vehicle parameter vector, and an external unknown interference vector; the state transfer function and the measurement function are affected by the characteristics of the autonomous ocean vehicle dynamics model and the actual control command with output saturation constraints.

[0008] Optionally, the steps of constructing a mixed hierarchical distribution of Gaussian distribution, Pearson type VII distribution, and inverse Wishart distribution to dynamically model the non-stationary heavy-tailed measurement noise specifically include: introducing a judgment factor that obeys the β-Bernoulli distribution, determining whether the current measurement is a normal value or an outlier, and using Gaussian distribution to model normal noise based on the judgment result, and using Pearson type VII distribution to model heavy-tailed measurement noise; selecting inverse Wishart distribution as the prior distribution of the measurement noise covariance, adding a fading factor to transfer the posterior distribution of the inverse Wishart parameter, and reflecting the time-varying characteristics of the measurement noise covariance matrix.

[0009] Optionally, the step of constructing a mixed stratified distribution of Gaussian distribution, Pearson type VII distribution, and inverse Wishart distribution to dynamically model the non-stationary heavy-tailed measurement noise also includes: in the process of using Pearson type VII distribution to model the heavy-tailed measurement noise, using two different gamma distributions to model the location parameters and scale parameters in the Pearson type VII distribution to dynamically adjust the degree of noise heaviness.

[0010] Optionally, the step of performing state estimation through a variational Bayesian unscented Kalman filtering algorithm based on the mixed hierarchical distribution combined with adaptive parameter selection specifically includes: using a variational Bayesian unscented Kalman filtering algorithm to infer and update the joint posterior probability density of all iteratively solved latent variable sets to perform high-precision smooth estimation of the position, attitude and velocity of the autonomous ocean vehicle, and correcting the noise covariance estimation matrix before each iterative update; wherein, the corrected noise covariance estimation matrix corrects the expectation of the noise covariance inverse matrix in the previous iterative step through the dynamic expected value of the judgment factor and the mixed probability variable in the previous iterative step; the latent variable set includes a state augmentation vector, a judgment factor, a mixed probability variable, auxiliary parameters, location parameters and scale parameters of a Pearson type VII distribution, and a measurement noise covariance matrix.

[0011] Optionally, the adaptive filtering state feedback trajectory tracking controller is designed and an actual control command with an output saturation constraint is generated. The steps of performing trajectory tracking control specifically include: calculating the tracking error based on the state estimation result and the given trajectory, constructing an asymmetric predefined time performance function, designing a kinematic level virtual control law and adaptive law, a dynamic level adaptive law, designing an asymmetric actuator saturation constraint function and a dynamic compensation mechanism variable, and generating an actual control command with an output saturation constraint.

[0012] Optionally, the asymmetric predefined time performance function is a time-related piecewise function. When the current moment is less than the preset convergence time, the asymmetric predefined time performance function is an exponential decay curve. The tracking error convergence time can be adjusted by controlling the decay rate coefficient. When the current moment reaches the preset convergence time, the tracking error enters a stable state and converges to the predefined asymmetric constraint set.

[0013] Optionally, the parameters in the kinematic level adaptive law and the dynamic level adaptive law are both single parameters.

[0014] Optionally, the asymmetric actuator saturation constraint function converts the ideal control command into an actual executable command that satisfies the actuator saturation constraint, ensuring a smooth transition of the control command near the saturation boundary and avoiding sudden changes; the dynamic compensation mechanism variable is used to compensate for the control error caused by the actuator saturation constraint.

[0015] On the other hand, the present invention also provides a state estimation and control system for an autonomous ocean vehicle, including: a model construction module for constructing a state augmentation estimation model; a noise dynamic modeling module for constructing a mixed hierarchical distribution of Gaussian distribution, Pearson type VII distribution, and inverse Wishart distribution, and dynamically modeling non-stationary heavy-tailed measurement noise; a state estimation module for performing state estimation through a variational Bayesian unscented Kalman filtering algorithm based on the mixed hierarchical distribution combined with adaptive parameter selection; a trajectory tracking control module for designing an adaptive filtering state feedback trajectory tracking controller based on the state estimation results, and generating an actual control command with output saturation constraints to perform trajectory tracking control.

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

[0017] The present invention constructs a hybrid stratified distribution of Gaussian, Pearson-type VII, and inverse Wishart distributions to dynamically model nonstationary heavy-tailed measurement noise. Conventional Gaussian noise is processed using a Gaussian distribution, while heavy-tailed noise is modeled using a Pearson-type VII distribution. Two different gamma distributions are used to adjust the position and scale parameters of the Pearson-type VII distribution, adaptively adjusting the degree of heavy tails and adapting to time-varying noise characteristics. The inverse Wishart distribution, combined with a fading factor, dynamically updates the noise covariance matrix to adapt to time-varying noise characteristics. This hybrid stratified distribution noise dynamic modeling approach is effective in complex ocean environments. Based on this hybrid stratified distribution combined with adaptive parameter selection, a variational Bayesian unscented Kalman filter algorithm is used to infer and update the joint posterior probability density of all iteratively solved latent variables to achieve high-precision smoothed estimation of the position, attitude, and velocity of an autonomous ocean vehicle. Based on the state estimation results, adaptive filtered state feedback trajectory tracking control is performed. An asymmetric predefined time performance function is used to control the tracking error convergence time and convergence range. Combined with an asymmetric actuator saturation constraint function and dynamic compensation mechanism variables, actual control commands with output saturation constraints are generated, enabling high-precision tracking. The present invention effectively solves the problems of filtering divergence caused by non-stationary heavy-tailed measurement noise caused by complex ocean environments and reduced state feedback control accuracy caused by measurement error coupling. BRIEF DESCRIPTION OF THE DRAWINGS

[0018] Figure 1 Schematic diagram of the process steps of the present invention.

[0019] Figure 2 Schematic diagram of the method of the present invention.

[0020] Figure 3 Schematic diagram of dynamic modeling of non-stationary heavy-tailed measurement noise according to the method of the present invention.

[0021] Figure 4 This is an effect diagram of the method of the present invention being applied to a 76.2m scale ASV.

[0022] Figure 5 This is a diagram showing the velocity estimation effect of the method of the present invention applied to a 76.2m-scale ASV.

[0023] Figure 6 This is an effect diagram of the method of the present invention being applied to a 2m scale AUV.

[0024] Figure 7 This is a diagram showing the speed estimation effect of the method of the present invention applied to a 2m-scale AUV.

[0025] Figure 8 Schematic diagram of the system structure of the present invention.

[0026] In the figure: 10-model building module, 20-noise dynamic modeling module, 30-state estimation module, 40-trajectory tracking control module. DETAILED DESCRIPTION

[0027] The following will provide a clear and complete description of the solutions of the present invention in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, rather than all the embodiments.

[0028] It should be noted that the terms "first", "second", etc. in the specification and claims of the present application and the above-mentioned drawings are used to distinguish similar objects and are not necessarily used to describe a specific order or sequential order. It should be understood that the data used in this way can be interchanged where appropriate, so that the embodiments of the present application described here. In addition, the terms "including" and "having" and any of their variations are intended to cover non-exclusive inclusions. For example, a process, method, system, product or device that includes a series of steps or units is not necessarily limited to those steps or units clearly listed, but may include other steps or units that are not clearly listed or inherent to these processes, methods, products or devices.

[0029] It will be understood by those skilled in the art that, unless expressly stated otherwise, the singular forms "a", "an", "said" and "the" used herein may also include the plural forms. It should be further understood that the term "comprising" used in the specification of this application refers to the presence of features, integers, steps, operations, elements and / or components, but does not exclude the presence or addition of one or more other features, integers, steps, operations, elements, components and / or groups thereof. It should be understood that when we refer to an element as being "connected" or "coupled" to another element, it may be directly connected or coupled to the other element, or there may be intermediate elements. In addition, "connected" or "coupled" as used herein may include wireless connections or wireless couplings. The term "and / or" used herein includes all or any units and all combinations of one or more associated listed items.

[0030] It will be understood by those skilled in the art that, unless otherwise defined, all terms (including technical and scientific terms) used herein have the same meaning as commonly understood by those skilled in the art to which this application belongs. It should also be understood that terms such as those defined in common dictionaries should be understood to have meanings consistent with their meanings in the context of the prior art and will not be interpreted in an idealized or overly formal sense unless specifically defined as herein.

[0031] It should be understood that the sequence numbers and sizes of the steps in this embodiment do not imply the order of execution. The order of execution of each process is determined by its function and internal logic, and should not constitute any limitation on the implementation process of the embodiment of this application.

[0032] It should be noted that, in the absence of conflict, the embodiments and features of the embodiments in this application can be combined with each other. The present application will be described in detail below with reference to the accompanying drawings and in combination with the embodiments.

[0033] Please refer to Figure 1-Figure 3 The present invention provides a state estimation and control method for an autonomous ocean vehicle, comprising the following steps:

[0034] S1. Construct a state augmentation estimation model.

[0035] Specifically, the present application is applied to autonomous ocean vehicles, and a state augmentation estimation model is constructed based on a state augmentation vector, a state transfer function, and a measurement function.

[0036] S2. Construct a mixed stratified distribution of Gaussian distribution, Pearson type VII distribution, and inverse Wishart distribution to dynamically model non-stationary heavy-tailed measurement noise.

[0037] Specifically, conventional noise is modeled using Gaussian distribution, heavy-tailed noise is modeled using Pearson-type VII distribution, and the noise covariance matrix is adaptively estimated using inverse Wishart distribution. This mixed hierarchical distribution noise dynamic modeling method can effectively adapt to complex ocean environments and achieve efficient characterization and adaptive filtering of complex ocean environment noise.

[0038] S3. Based on the mixed hierarchical distribution and adaptive parameter selection, state estimation is performed through a variational Bayesian unscented Kalman filter algorithm.

[0039] Specifically, based on the mixed hierarchical distribution combined with adaptive parameter selection, the joint posterior probability density of all iteratively solved latent variable sets is updated through variational Bayesian unscented Kalman filtering algorithm inference to perform high-precision smooth estimation of the position, attitude and velocity of the autonomous ocean vehicle.

[0040] S4. Based on the state estimation results, an adaptive filtering state feedback trajectory tracking controller is designed, and an actual control command with output saturation constraint is generated to perform trajectory tracking control.

[0041] Specifically, based on the state estimation results, an adaptive filtering state feedback trajectory tracking controller is designed, and actual control commands with output saturation constraints are generated to perform trajectory tracking control and achieve high-precision tracking.

[0042] The present invention realizes high-precision estimation of the state of autonomous ocean vehicles by integrating state augmentation estimation model modeling, mixed noise distribution modeling and variational Bayesian unscented Kalman filtering algorithm; combines asymmetric predefined time performance function, single-parameter adaptive law, asymmetric actuator saturation constraint function, and dynamic compensation mechanism variables to perform adaptive filtering state feedback control on autonomous ocean vehicles, effectively suppressing sensor measurement errors and external interference, improving the trajectory tracking robustness of the vehicle in complex ocean environments, ensuring the real-time and stability of control instructions, and effectively solving the problems of non-stationary heavy-tailed measurement noise caused by complex ocean environments leading to filter divergence and measurement error coupling leading to decreased state feedback control accuracy.

[0043] In some embodiments, the step of constructing a state augmented estimation model specifically includes: constructing an augmented state estimation model of an autonomous ocean vehicle system based on a state augmentation vector, a state transfer function, and a measurement function; wherein the state augmentation vector includes a vehicle state vector, a vehicle parameter vector, and an external unknown interference vector; the state transfer function and the measurement function are affected by the characteristics of the autonomous ocean vehicle dynamics model and the actual control command with output saturation constraints.

[0044] Specifically, the augmented state estimation model of the autonomous ocean vehicle system is constructed as follows:

[0045] ζ k =f(ζ k-1 ,u k-1 )+w k

[0046] ξ k =h(ζ k ,u k )+v k

[0047]

[0048] Where, ζ k and ζ k-1 is the state augmented vector at time k and time k-1; ξ k is the measured value at the kth moment; vector is the spacecraft related parameter, u kis the actual control command with output saturation constraint, d k is composed of external unknown interference; h(·) and f(·) are the state transfer function and measurement function, which are affected by the characteristics of the autonomous ocean vehicle dynamics model and the actual control command with output saturation constraint; Ξ d and Γ d is the external unknown interference transmission matrix; w k and v k are process noise and measurement noise respectively, measurement noise v k Affected by sensor acquisition and information transmission, it is non-Gaussian distribution noise and has non-stationary heavy-tail characteristics.

[0049] The state augmentation vector ζ k The state of the ocean autonomous vehicle x k , aircraft related parameters Unknown external interference constitutes d k ; The state of the ocean autonomous vehicle mainly includes its position, heading angle, linear velocity, angular velocity, attitude, etc., which are collected by the GPS / GNSS / IMU / AIS and other equipment carried by the ocean autonomous vehicle; the vehicle-related parameters are composed of the relevant parameters of the vehicle inertia and fluid dynamic additional inertia, Coriolis matrix and hydrodynamic damping parameter matrix.

[0050] The present invention constructs a state augmented vector that includes the vehicle state, related parameters and external interference, comprehensively considers the characteristics of the dynamic model and the output saturation constraint, and enhances the integrity and adaptability of the model; by constructing an augmented state estimation model for the autonomous ocean vehicle system, it effectively estimates unknown interference including external disturbances, thereby significantly suppressing the impact of these unknown interferences on the state estimation, and providing high-precision state result input for subsequent control.

[0051] In some embodiments, the steps of constructing a mixed hierarchical distribution of Gaussian distribution, Pearson type VII distribution, and inverse Wishart distribution to dynamically model non-stationary heavy-tailed measurement noise specifically include: introducing a judgment factor that obeys the β-Bernoulli distribution, determining whether the current measurement is a normal value or an outlier, and using Gaussian distribution to model normal noise based on the judgment result, and using Pearson type VII distribution to model heavy-tailed measurement noise; selecting inverse Wishart distribution as the prior distribution of measurement noise covariance, adding a fading factor to transfer the posterior distribution of the inverse Wishart parameter, and reflecting the time-varying characteristics of the measurement noise covariance matrix.

[0052] Specifically, a judgment factor that obeys the β-Bernoulli distribution is introduced Determine whether the current sensor measurement data is a normal value or an outlier; use Gaussian and Pearson type VII distributions to model normal noise and heavy-tailed measurement noise respectively.

[0053] When the judgment factor is 1, Gaussian distribution is used to model normal noise, which can be expressed as:

[0054]

[0055] Where, Normal noise, Its probability density follows a Gaussian distribution R k is the covariance matrix;

[0056] When the judgment factor is 0, the Pearson type VII distribution is used to model the heavy-tailed measurement noise, which can be expressed as:

[0057]

[0058] Where, v k is the heavy-tailed measurement noise, Its probability density follows the Pearson type VII distribution φ k and are the location and scale parameters of the Pearson type VII distribution; R k is the measurement noise covariance matrix, γ k is an auxiliary parameter. When , the Pearson type VII distribution will degenerate into the Student t distribution;

[0059] The inverse Wishart distribution and its related parameter posterior distribution can be expressed as:

[0060]

[0061] Where, ξ 1:k-1 Represents the measurement value of the iterative process from time 1 to k-1; and is the predicted value of auxiliary parameters; is the parameter estimate at the k-1th moment; m is the measurement dimension; ρ is the fading factor 0<ρ≤1, and the parameter B satisfies is the inverse Wishart distribution.

[0062] This application adopts the β-Bernoulli distribution to dynamically distinguish normal noise from outliers, and combines the mixed modeling of Gaussian distribution (normal noise) and Pearson type VII distribution (heavy-tailed noise) to effectively characterize the characteristics of non-stationary, Gaussian and non-Gaussian mixed measurement noise; introduces the inverse Wishart distribution to estimate the noise covariance and describe the time-varying nature of noise, which significantly improves the flexibility and accuracy of noise modeling and reduces the interference of abnormal noise on state estimation.

[0063] In some embodiments, the step of constructing a mixed hierarchical distribution of Gaussian distribution, Pearson type VII distribution, and inverse Wishart distribution to dynamically model the non-stationary heavy-tailed measurement noise also includes: in the process of using Pearson type VII distribution to model the heavy-tailed measurement noise, two different gamma distributions are used to model the location parameters and scale parameters in the Pearson type VII distribution to dynamically adjust the degree of noise heaviness.

[0064] Specifically, the two different gamma distributions are:

[0065] P(φ k )=G(φ k ;a k ,b k );

[0066]

[0067] Where, φ k Expressed as a position parameter, P(φ k ) is its probability density; is represented as a scale parameter, Its probability density; G(φ k ) is a gamma distribution; a k , b k , c k , d k The corresponding auxiliary parameters are given respectively.

[0068] The posterior probability density function equation of non-stationary heavy-tailed measurement noise can be expressed as:

[0069]

[0070] Where, Represented as normal noise, obeying Gaussian distribution N(·); v k PTV(·) is the heavy-tailed measurement noise that follows a Gaussian distribution; κ k is a mixed probability variable; R k is the measurement noise covariance matrix; φ k Expressed as positional parameters, Expressed as a scale parameter, all obey the gamma distribution G(·), a k , b k , c k , d k The corresponding auxiliary parameters are given respectively.

[0071] This application dynamically adjusts the position and scale parameters of the Pearson type VII distribution through the double gamma distribution, thereby achieving adaptive adjustment of the degree of noise heaviness; enhancing the algorithm's ability to respond to sudden interference or environmental changes, and further improving the accuracy and stability of state estimation.

[0072] In some embodiments, the step of performing state estimation through a variational Bayesian unscented Kalman filtering algorithm based on the mixed hierarchical distribution combined with adaptive parameter selection specifically includes: using a variational Bayesian unscented Kalman filtering algorithm to infer and update the joint posterior probability density of all iteratively solved latent variable sets to perform high-precision smooth estimation of the position, attitude, and velocity of the autonomous ocean vehicle, and correcting the noise covariance estimation matrix before each iterative update; wherein, the corrected noise covariance estimation matrix corrects the expectation of the noise covariance inverse matrix in the previous iterative step through the dynamic expected value of the judgment factor and the mixed probability variable in the previous iterative step; the latent variable set includes a state augmented vector, a judgment factor, a mixed probability variable, auxiliary parameters, location parameters and scale parameters of a Pearson type VII distribution, and a measurement noise covariance matrix.

[0073] Specifically, a variational Bayesian unscented Kalman filtering state estimation method based on a mixed stratified distribution of Gaussian distribution, Pearson type VII distribution, and inverse Wishart distribution is designed; it should be noted that, combined with the position, speed, heading information, angular velocity, attitude and other information of the ocean autonomous vehicle collected by GPS / GNSS / IMU / AIS and other equipment carried by the ocean autonomous vehicle, the outliers caused by the influence of non-stationary heavy-tailed measurement noise are filtered, and the state of the autonomous ocean vehicle is estimated with high precision, and the information is transmitted to the adaptive filtering state feedback controller.

[0074] The variational Bayesian unscented Kalman filtering method based on a mixed hierarchical distribution of Gaussian distribution, Pearson type VII distribution, and inverse Wishart distribution utilizes a variational Bayesian unscented Kalman filtering algorithm to infer and update all sets of latent variables to be iteratively calculated.

[0075] The hidden variable set is The state of the autonomous vehicle is augmented by the vector ζ k , judgment factor Mixed probability variable κ k , auxiliary parameter γ k and the location parameter φ of the Pearson type VII distribution k and scale parameters Measurement noise covariance matrix R k constitute.

[0076] The latent variable set Θ k The joint posterior probability density will be based on the measurement value ξ of the iterative process from time 1 to k-1 1:k , by minimizing the joint posterior probability density of the set of latent variables and the probability density P(Θ k |ξ 1:k ) is achieved by the KL divergence between .

[0077]

[0078] Where, Θ k is a set of latent variables; is the set of latent variables Θ k The joint posterior probability density, P(Θ k |ξ 1:k ) is the probability density;

[0079] The optimal solution is

[0080] Where, is an element in the latent variable set, is the hidden variable set except All elements except For irrelevant constants;

[0081] The latent variable set Θ k The joint posterior probability density of is decomposed and solved iteratively Approximation:

[0082]

[0083] Where, ξ 1:k-1 is the measured value of the iterative process from time 1 to k-1; ξ k is the measured value at the kth moment; k and ζ k-1 is the state augmentation vector at time k and time k-1; f(·) and h(·) are the state transfer function and measurement function; is the judgment factor; k is an auxiliary parameter; φ k and are the location and scale parameters of the Pearson type VII distribution, both of which obey the gamma distribution G(·), a k , b k , c k , d k The corresponding auxiliary parameters are respectively: k is a mixed probability variable, obeying Beta distribution Be(·), e k , f k The corresponding auxiliary parameters are respectively: R k is the measurement noise covariance matrix; Q k is the process noise covariance matrix; IW(·) is the inverse Wishart distribution, and is the relevant parameter of the inverse Wishart distribution;

[0084] The variational Bayesian unscented Kalman filter state estimation method based on a mixed hierarchical distribution of Gaussian distribution, Pearson type VII distribution, and inverse Wishart distribution is as follows:

[0085] Predict the system augmented state variables through unscented transformation and state transfer function f(·) and its covariance matrix in, are 2n+1 Sigma points generated by the unscented transformation, and is the unscented transformation weight vector, Q k-1 is the process noise covariance matrix at the k-1th moment.

[0086] In a possible implementation, the unscented transformation needs to ensure the positive definiteness of the covariance matrix when generating Sigma points, and a square root filtering method will be used.

[0087] At the beginning of the VB iteration, the variables involved in the latent variable set are initialized and calculated.

[0088] The initialization related parameters, including the expected position and scale parameters E 0 [φ k ]=a k / b k and Auxiliary parameter expectations Judgment factor expectations Mixed probability variable expectation E 0 [lnκ k ]=Ψ(e0)-Ψ(1) and E 0 [ln(1-κ k )]=Ψ(1-e0)-Ψ(1), the expected value of the inverse of the measurement noise covariance matrix Determine the number of iterations to be 10;

[0089] Update the predicted measurement vector and its initial covariance matrix State measurement cross covariance matrix P ζξ,k|k-1 .

[0090]

[0091] in, The 2n+1 Sigma points generated by the unscented transformation are passed through and control the input u k Substitute the measurement function h(·) together to produce.

[0092] The measurement function h(·) is influenced by the actual control command generated by the adaptive filtered state feedback controller.

[0093] Corrected measurement noise covariance estimation matrix The joint posterior probability density of all latent variables is updated using variational Bayesian unscented Kalman filter inference.

[0094] The modified noise covariance estimation matrix is composed of the judgment factors Expected, auxiliary parameter γ k The expectation and noise covariance estimation matrix expectation are calculated as follows:

[0095]

[0096] Where, is the covariance estimation matrix updated for the jth iteration, The expectation of the inverse matrix of the covariance estimate updated for the j-1th iteration, is the expected judgment factor of the j-1th iteration update, E j-1 [γ k ] is the expectation of the auxiliary variable updated in the j-1th iteration.

[0097] In one possible implementation, in order to solve the mutual coupling problem of variational parameters, the fixed point iteration method is used to iteratively solve the joint posterior probability density of all variables in the latent variable set.

[0098] Update the gain matrix Measurement covariance matrix State error covariance matrix and state estimates

[0099]

[0100] Where, and is the measurement covariance updated at the jth and j-1th iterations, is the covariance estimation matrix updated for the jth iteration, is the gain matrix updated for the jth time, P ζξ,k|k-1 is the state-measurement cross-covariance matrix, is the j-th updated state error covariance matrix, Update the state estimate at time k for the jth iteration, Update the state prediction value at time k for the jth iteration, ξ k is the measured value at time k, is the predicted measurement value at time k.

[0101] Update and calculate the judgment factor expect:

[0102]

[0103] Where, E j [l k ] is the expected judgment factor of the jth iteration update, P j (l k =1) is the probability density of the jth iteration update when the judgment factor is 1, P j (l k =0) is the probability density of the jth iterative update when the judgment factor is 0, is a constant variable, E j-1 [lnκ k ] and E j-1 [ln(1-κ k )] is the mixed probability variable κ k and 1-κ k Find the expected j-1th iteration update result after logarithmization, E j-1 [lnγ k ] is the auxiliary parameter γ k Find the expected update result of the j-1th iteration after logarithmization, m is the design parameter, The expectation of the inverse matrix of the covariance estimate updated for the j-1th iteration, is the residual information updated in the jth iteration, From the measured value ξ k and the state estimate at time k updated in the jth iteration Calculated, is the design parameter.

[0104] Update and calculate the mixing probability variable κ k expect:

[0105]

[0106] Where, E j-1 [lnκ k ] and E j-1 [ln(1-κ k )] is the mixed probability variable κ k and 1-κ k Find the expected j-1th iteration update result after logarithmization, Ψ(·) is the logarithmic derivative of the gamma function, and Auxiliary parameters of the mixed probability variable updated for the j-1th iteration.

[0107] Update and calculate auxiliary parameters γ k and the location parameter of the Pearson type VII distribution and scale parameters and their expectations:

[0108]

[0109] Where E j [γ k ] is the auxiliary parameter expectation updated in the jth iteration, is the residual information updated in the jth iteration, and is the location parameter and scale parameter of the Pearson type VII distribution updated at the jth iteration, E j [φ k ]and Update the expected position and scale parameters for the jth iteration, and their auxiliary parameters are a k , b k , c k , d k Initialize variables for them respectively.

[0110] Update distribution auxiliary parameters and

[0111]

[0112] Where, The expectation of the inverse matrix of the covariance estimate updated for the j-1th iteration, and is the estimated value of the auxiliary parameter of the inverse Wissart distribution at the kth moment updated in the jth iteration, and is the predicted value of the auxiliary parameter of the inverse Wishart distribution at time k, m is the design parameter, is the residual information updated in the jth iteration, E j [γ k ] is the auxiliary parameter expectation updated in the jth iteration, is the expected judgment factor updated in the jth iteration.

[0113] After the iteration is completed, the state estimate is passed to the adaptive feedback controller and the process returns to step (1) and continues until the operation is completed.

[0114] The operation termination condition is whether the functional of the j-1th iterative update state estimate and the jth iterative update state estimate error converges to a predetermined threshold.

[0115] This application is based on the variational Bayesian unscented Kalman filter (VBUKF) algorithm to iteratively update the set of latent variables (including state augmentation vectors, judgment factors, mixed probability variables, auxiliary parameters, location parameters and scale parameters of the Pearson type VI I distribution, and measurement noise covariance matrix). By minimizing the KL divergence, the joint posterior probability density is optimized to achieve joint estimation of multi-source uncertainty (state, noise, interference), significantly improving the filtering algorithm's ability to estimate time-varying noise and ensuring the convergence and accuracy of state estimation.

[0116] In some embodiments, the steps of designing an adaptive filtering state feedback trajectory tracking controller and generating an actual control command with an output saturation constraint for trajectory tracking control specifically include: calculating the tracking error based on the state estimation result and the given trajectory, constructing an asymmetric predefined time performance function, designing a kinematic level virtual control law and adaptive law, a dynamic level adaptive law, designing an asymmetric actuator saturation constraint function and a dynamic compensation mechanism variable, generating an actual control command with an output saturation constraint, and performing trajectory tracking control.

[0117] Specifically, based on the state estimation results, an adaptive filtering state feedback trajectory tracking controller is designed, and actual control commands with output saturation constraints are generated to perform trajectory tracking control. It should be noted that the autonomous ocean vehicle can be controlled so that its trajectory tracking converges to the asymmetric constraint boundary within a predefined time.

[0118] The steps for designing an adaptive filtering state feedback trajectory tracking controller for an autonomous ocean-going vehicle are as follows:

[0119] Combined with the autonomous vehicle state estimation result η s and the given track η d , calculate the tracking error η e (t) = η s -η d .

[0120] The state estimation result of the autonomous vehicle is obtained by the state estimation method, accompanied by an unknown sensitivity error, which can be expressed as η s =σ1(t)η, where σ1=diag(σ 1,1 ,σ 1,2 ,σ 1,i ) is the measurement sensitivity error, satisfying condition, and σ 1,i are the upper and lower bounds of the i-th state error, is the derivative of the i-th state error, is the boundary of the ith state error derivative, and η is the ideal state based on the control command output.

[0121] Design a symmetric predefined time performance function to ensure that the tracking error converges to the asymmetric constraint bound -δ within the predefined time. m,i h i (t)<η e,i (t)<δ M,i h i (t).

[0122] The asymmetric predefined time performance function PFTPF is:

[0123]

[0124] in is the predefined time; h0 is the initial value, satisfying ι≥2 and λ are positive design parameters; h i (t) is the asymmetric predefined time performance function corresponding to the i-th state, corresponding to δ m,i and δ M,i are the upper and lower convergence boundaries of the i-th state of the tracking error; η e,i (t) is η e The i-th state variable.

[0125] It should be noted that the convergence speed and convergence range of different state variables in the tracking error can be controlled by adjusting the predefined time and the upper and lower convergence boundaries of the tracking error to achieve the required preset performance. e (t) To meet the above conditions, the following nonlinear transformation η needs to be introduced e,i (t) = h i (t)ρ i (μ i ),in is a strictly monotonically increasing function, μ i is an auxiliary variable.

[0126] Combined with ρ i (μ i ) and PFTPF transform the trajectory tracking error state i into the corresponding stabilization error The p i (μ i ) is the inverse functional of Among them, ρ i is the strictly monotonically increasing function corresponding to the i-th state of the tracking error, η e,i (t) is the tracking error of the i-th state, δ m,i and δ M,i are the upper and lower convergence boundaries of the i-th state of the tracking error, h i (t) is the asymmetric predefined time performance function corresponding to the i-th state.

[0127] Combined with the model uncertainty of the marine autonomous vehicle, the virtual control law and single-parameter adaptive law at the kinematic level and the dynamic adaptive law of the marine autonomous vehicle are designed respectively.

[0128] The model uncertainty of the marine autonomous vehicle is caused by the external ocean environment disturbance and the internal model uncertainty. It should be noted that the model uncertainty of marine autonomous vehicles of different scales is also different, and the size of external disturbances in different ocean environments is also different. That is, the method of the present invention is applicable to marine autonomous vehicle models of different scales, such as Figure 4 ASV of 76.2m in size, Figure 6 AUV in the mid-2m scale.

[0129] Design the virtual control law α at the kinematic level:

[0130]

[0131] And a single-parameter adaptive law at the kinematic level, whose derivative is:

[0132]

[0133] in, Adaptive single parameter estimation, Positive definite design matrix; R T (ψ s ) is the transpose of the transformation matrix; c i >0 is the controller parameter; z is the stabilization error given by z i constitute; is the uncertainty term at the kinematic level, which is determined by the tracking error η e , given track η d and its derivatives; are the coefficients of the uncertainty parameters at the kinematic level, is the inverse of the measurement sensitivity error, is the derivative of the measurement sensitivity error, Ψ is given by Composition, ρ i is a strictly monotonically increasing function corresponding to the i-th state of the tracking error; Φ is given by Composition, h i is the asymmetric predefined time performance function corresponding to the i-th state, h i is its derivative.

[0134] It should be noted that It is to find the maximum value of all parameters of the uncertain terms at the kinematic level, so the adaptive law at the kinematic level here has the characteristic of a single parameter.

[0135] The RBF neural network is used to reconstruct the model uncertainty at the dynamic level:

[0136] H(Z2)=W T ξ(Z2)+ε;

[0137] in, The autonomous ocean vehicle speed and virtual control law derivatives are composed; ε is the RBF neural network approximation error; W is the weight vector of the RBF neural network, and ξ(·) is the neural network basis function.

[0138] Design a single-parameter adaptive law at the dynamics level, whose derivative is:

[0139]

[0140] in, Estimation of a single adaptive parameter at the dynamical level, c i >0 is the controller parameter; is the model uncertainty term at the dynamic level, z is the stabilization error, ξ(Z2) is the output of the neural network basis function, are the variables of the dynamic compensation mechanism designed as follows; θ2 = max{||W||,||ε+d||,||Ψσ1R(ψ)||,||κ1||} are the coefficients of the uncertain parameters at the dynamic level; e2 = v-α is the tracking error at the dynamic level, which is obtained from the virtual control law at the kinematic level and the speed of the autonomous ocean vehicle.

[0141] It should be noted that θ2 is the maximum value of all parameters of uncertainties at the dynamic level, so the adaptive law at the dynamic level here has the characteristic of a single parameter.

[0142] Combining the asymmetric actuator saturation constraint function and the dynamic compensation mechanism, an actual control command with output constraints is derived. It should be noted that the actual control command is affected by the actuator asymmetric saturation constraint.

[0143] Design asymmetric actuator saturation constraint function:

[0144]

[0145] Where τ is the ideal control command, satisfy is the actual upper limit of the actuator; τ=max{τ min ,τ(kT)+r min T}<0 is the actual lower bound of the actuator; τ max is the maximum output of the actuator; τ min is the minimum output of the actuator; are auxiliary parameters of the asymmetric actuator saturation constraint function, and tanh(·) is the hyperbolic tangent function.

[0146] Design dynamic compensation mechanism variables Its derivative is:

[0147]

[0148] Where κ1 and ρ are auxiliary parameters of the dynamic compensation mechanism. M is the additional inertia matrix of the autonomous ocean vehicle composed of inertia and fluid dynamics.

[0149] Based on the adaptive parameter estimation and dynamic compensation mechanism variables at the dynamic level, the actual control commands with output constraints are derived:

[0150]

[0151] Where K2 is the positive definite design matrix; c i >0 is the controller parameter; e2 is the dynamic level tracking error, Adaptive parameter estimation for the dynamics level.

[0152] The actual control command is passed to the execution module and the estimation filter module in the ocean vehicle to perform trajectory tracking control on the autonomous ocean vehicle. At the same time, the program is returned to the first step to continue executing until the operation is completed. The running effect of the method is as follows: Figure 4-Figure 7 shown.

[0153] The operation termination condition is that the tracking error of the autonomous ocean-going vehicle converges to the asymmetric constraint bound within a predetermined time.

[0154] This application combines the asymmetric predefined time performance function PFTPF to design a trajectory tracking strategy that takes into account both convergence speed and convergence constraint boundaries, achieving fast and smooth trajectory tracking to meet the complex mission requirements of ocean vehicles.

[0155] In some embodiments, the asymmetric predefined time performance function is a time-related piecewise function. When the current moment is less than the preset convergence time, the asymmetric predefined time performance function is an exponential decay curve. The tracking error convergence time can be adjusted by controlling the decay rate coefficient. When the current moment reaches the preset convergence time, the tracking error enters a stable state and converges to the predefined asymmetric constraint set.

[0156] The asymmetric predefined time performance function designed in this application is segmented, including an exponential decay segment and a steady-state segment, which realizes the dynamic adjustment of the tracking error: the initial accelerated convergence shortens the adjustment time, and the later stable tracking ensures that the error continues to meet the asymmetric constraints, thereby improving the flexibility of the control strategy and task adaptability.

[0157] In some embodiments, the parameters in the kinematic-level adaptive law and the dynamic-level adaptive law are both single parameters.

[0158] Specifically, the kinematic level adaptive law and the dynamic level adaptive law both have the characteristics of a single parameter, which is convenient for controller calculation. This application adopts a single parameter adaptive law instead of multi-parameter joint optimization, which greatly reduces the computational complexity and is suitable for embedded control systems of ocean vehicles with limited computing resources.

[0159] In some embodiments, the asymmetric actuator saturation constraint function converts the ideal control command into an actual executable command that satisfies the actuator saturation constraint, ensuring a smooth transition of the control command near the saturation boundary and avoiding sudden changes; the dynamic compensation mechanism variable is used to compensate for the control error caused by the actuator saturation constraint.

[0160] Specifically, by designing an asymmetric actuator saturation constraint function and a dynamic compensation mechanism, smooth mapping of control instructions is achieved, oscillation and overshoot are suppressed when approaching the saturation boundary, and physical wear of the actuator is reduced.

[0161] On the other hand, please refer to Figure 8 The present invention also provides a state estimation and control system for an autonomous ocean vehicle, including: a model construction module 10, used to construct a state augmentation estimation model; a noise dynamic modeling module 20, used to construct a mixed hierarchical distribution of Gaussian distribution, Pearson type VII distribution, and inverse Wishart distribution, and dynamically model the non-stationary heavy-tailed measurement noise; a state estimation module 30, used to perform state estimation through a variational Bayesian unscented Kalman filter algorithm based on the mixed hierarchical distribution combined with adaptive parameter selection; a trajectory tracking control module 40, which designs an adaptive filtering state feedback trajectory tracking controller according to the state estimation result, and generates an actual control command with output saturation constraint to perform trajectory tracking control.

[0162] If the integrated unit is implemented in the form of a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present invention, or the part that contributes to the prior art, or all or part of the technical solution can be embodied in the form of a software product. The computer software product is stored in a storage medium and includes several instructions for enabling a computer device (which can be a personal computer, server, or network device, etc.) to execute all or part of the steps of the methods of each embodiment of the present invention. The aforementioned storage medium includes various media that can store program codes, such as a USB flash drive, a mobile hard disk, a read-only memory (ROM), a random access memory (RAM), a magnetic disk, or an optical disk.

[0163] Those skilled in the art will appreciate that all or part of the processes in the above-described method embodiments can be implemented by instructing related hardware through a computer program. The computer program can be stored in a non-volatile computer-readable storage medium. When executed, the computer program can include the processes of the above-described method embodiments. Any reference to memory, storage, database, or other media used in the embodiments provided by the present invention can include non-volatile and / or volatile memory. Non-volatile memory can include read-only memory (ROM), programmable ROM (PROM), electrically programmable ROM (EPROM), electrically erasable programmable ROM (EEPROM), or flash memory. Volatile memory can include random access memory (RAM) or external cache memory. By way of illustration and not limitation, RAM is available in various forms, such as static RAM (SRAM), dynamic RAM (DRAM), synchronous DRAM (SDRAM), double data rate SDRAM (DDRSDRAM), enhanced SDRAM (ESDRAM), synchronous link DRAM (SLDRAM), RAMbus direct RAM (RDRAM), direct RAMbus dynamic RAM (DRDRAM), and RAMbus dynamic RAM (RDRAM).

[0164] The above are merely embodiments of the present invention and are not intended to limit the patent scope of the present invention. Any equivalent transformations made using the contents of the present invention's description and drawings, or directly or indirectly applied in related technical fields, are also included in the patent protection scope of the present invention.

Claims

1. A state estimation and control method for an autonomous ocean vehicle, characterized in that the steps include: Construct a state augmentation estimation model; Construct a mixed stratified distribution of Gaussian distribution, Pearson type VII distribution, and inverse Wishart distribution to dynamically model non-stationary heavy-tailed measurement noise; Based on the mixed hierarchical distribution combined with adaptive parameter selection, state estimation is performed using a variational Bayesian unscented Kalman filter algorithm; According to the state estimation results, an adaptive filtering state feedback trajectory tracking controller is designed, and the actual control command with output saturation constraint is generated to perform trajectory tracking control.

2. The state estimation and control method of an autonomous ocean vehicle according to claim 1, characterized in that: The steps of constructing the state augmentation estimation model specifically include: Based on the state augmentation vector, state transfer function and measurement function, an augmented state estimation model for autonomous ocean vehicle system is constructed; The state augmentation vector includes the aircraft state vector, the aircraft parameter vector, and the external unknown interference vector; The state transfer function and the measurement function are affected by the characteristics of the autonomous ocean vehicle dynamics model and the actual control command with output saturation constraints.

3. The state estimation and control method of an autonomous ocean vehicle according to claim 1, characterized in that: The steps of constructing a mixed stratified distribution of Gaussian distribution, Pearson type VII distribution, and inverse Wishart distribution to dynamically model the non-stationary heavy-tailed measurement noise specifically include: A judgment factor that obeys the β-Bernoulli distribution is introduced to determine whether the current measurement is a normal value or an outlier. Based on the judgment result, Gaussian distribution is used to model normal noise, and Pearson type VII distribution is used to model heavy-tailed measurement noise. The inverse Wishart distribution is selected as the prior distribution of the measurement noise covariance, and the fading factor is added to transfer the posterior distribution of the inverse Wishart parameter to reflect the time-varying characteristics of the measurement noise covariance matrix.

4. The state estimation and control method of an autonomous ocean vehicle according to claim 3, characterized in that: The step of constructing a mixed stratified distribution of Gaussian distribution, Pearson type VII distribution, and inverse Wishart distribution to dynamically model the non-stationary heavy-tailed measurement noise also includes: in the process of using Pearson type VII distribution to model the heavy-tailed measurement noise, two different gamma distributions are used to model the location parameters and scale parameters in the Pearson type VII distribution to dynamically adjust the degree of noise heaviness.

5. The state estimation and control method of an autonomous ocean vehicle according to claim 4, characterized in that: The step of performing state estimation based on the mixed hierarchical distribution combined with adaptive parameter selection by using a variational Bayesian unscented Kalman filter algorithm specifically includes: The variational Bayesian unscented Kalman filter algorithm is used to infer and update the joint posterior probability density of all latent variable sets obtained by iterative solution to obtain high-precision smooth estimation of the position, attitude, and velocity of the autonomous ocean vehicle, and the noise covariance estimation matrix is corrected before each iterative update. The modified noise covariance estimation matrix modifies the expectation of the noise covariance inverse matrix in the previous iterative step by the dynamic expected value of the judgment factor and the mixed probability variable in the previous iterative step; The latent variable set includes a state augmentation vector, a judgment factor, a mixed probability variable, an auxiliary parameter, a location parameter and a scale parameter of a Pearson type VII distribution, and a measurement noise covariance matrix.

6. The state estimation and control method of an autonomous ocean vehicle according to claim 5, characterized in that: The steps of designing an adaptive filtering state feedback trajectory tracking controller and generating an actual control command with output saturation constraints to perform trajectory tracking control specifically include: Based on the state estimation results and the given trajectory, the tracking error is calculated, an asymmetric predefined time performance function is constructed, the kinematic level virtual control law and adaptive law, the dynamic level adaptive law are designed, the asymmetric actuator saturation constraint function and dynamic compensation mechanism variables are designed, and the actual control command with output saturation constraint is generated for trajectory tracking control.

7. The state estimation and control method of an autonomous ocean vehicle according to claim 6, characterized in that: The asymmetric predefined time performance function is a time-related piecewise function. When the current moment is less than the preset convergence time, the asymmetric predefined time performance function is an exponential decay curve. The tracking error convergence time can be adjusted by controlling the decay rate coefficient. When the current moment reaches the preset convergence time, the tracking error enters a stable state and converges to the predefined asymmetric constraint set.

8. The state estimation and control method of an autonomous ocean vehicle according to claim 6, characterized in that: The parameters in the kinematic level adaptive law and the dynamic level adaptive law are both single parameters.

9. The state estimation and control method of an autonomous ocean vehicle according to claim 6, characterized in that: The asymmetric actuator saturation constraint function converts the ideal control command into an actual executable command that satisfies the actuator saturation constraint, ensuring that the control command achieves a smooth transition near the saturation boundary and avoids sudden changes; The dynamic compensation mechanism variables are used to compensate for control errors caused by actuator saturation constraints.

10. A state estimation and control system for an autonomous ocean vehicle, characterized in that: include: Model building module, used to build state augmentation estimation model; Noise dynamic modeling module, used to construct mixed stratified distributions of Gaussian distribution, Pearson type VII distribution, and inverse Wishart distribution, and to dynamically model non-stationary heavy-tailed measurement noise; A state estimation module, configured to perform state estimation using a variational Bayesian unscented Kalman filter algorithm based on the hybrid hierarchical distribution combined with adaptive parameter selection; The trajectory tracking control module is used to design an adaptive filtering state feedback trajectory tracking controller based on the state estimation results, and generate actual control commands with output saturation constraints to perform trajectory tracking control.

Images