ADAPTIVE ROBUST ESTIMATION METHOD AND SYSTEM FOR PARAMETERS OF UNMANNED SURFACE VESSEL

Abstract

An adaptive robust estimation method and system for parameters of an unmanned surface vessel belongs to the field of automatic control. First, an augmented state estimation problem considering an external disturbance for the unmanned surface vessel is constructed; augmented state vectors include a state vector of the unmanned surface vessel, a parameter vector, and an unknown input vector; and then, depending on a real-time input vector and a measurement vector of a system, a designed adaptive robust unscented Kalman filtering method is employed to obtain model parameters of the unmanned surface vessel.

Description

TECHNICAL FIELD

The present invention relates to an adaptive robust estimation method and system for parameters of an unmanned surface vessel and belongs to the field of automatic control. Robust Kalman filtering is designed by means of a robust optimal criterion to weaken influences of outlier noises on identification results, and statistical characteristics of process noises are estimated by means of an adaptive method, thereby improving the estimation accuracy and robustness of unknown parameters of the unmanned surface vessel.

BACKGROUND

A process during which specific numerical values of model parameters of a control system are estimated based on input vectors and measurement vectors of the control system is referred to as parameter estimation. Adaptive estimation is to adaptively adjust algorithm parameters during an estimation process. However, in Kalman filtering, the estimation of statistical characteristics of noises, especially a covariance matrix of the noises, is a major problem to be taken into account during the design of an adaptive law. Robust estimation is a type of estimation algorithms with good robustness. Parameter estimation is an important technology in the field of automatic control. In engineering practice, the design of a model-based controller and the selection of its parameters can be performed on the premise that an accurate system model and model parameters of a controlled object are known. The provision of accurate model parameters will help to ensure the controller to play a good control effect. With the development of unmanned systems, the controlled object has presented higher complexity in terms of kinematic and dynamical models at present. Therefore, higher requirements have been put forward for the accuracy of parameter estimation.

With the development of the unmanned systems, unmanned surface vessels have drawn great attention in both military and civilian areas at present, and are often used in scenes such as environment monitoring, maritime search and rescue, and resource exploration. In recent years, a great deal of researches also has been carried out on parameter estimation problems of the unmanned surface vessels. For example, Parameter Estimation for Unmanned Surface Vessels Based on Quadratic Programming [Wang X, Zhao J, Geng T. Nonlinear modeling and identification of vessel based on constrained quadratic programming method [C]. 39th Chinese Control Conference (CCC). Shenyang, China; IEEE. 2020: 1028-33.], this method considers constraint conditions possibly present in a model parameter estimation problem of the unmanned surface vessels to obtain better parameter estimation results, but an optimization problem is only established for measurement noises, without considering the influences of the process noises. For example, Parameter Estimation for Unmanned Surface Vessels Based on Extended Kalman Filtering [Perera L P, Oliveira P, Guedes Soares C. System identification of nonlinear vessel steering [J]. Journal of Offshore Mechanics and Arctic Engineering, 2015, 137(3).], this method considers the process noises and the measurement noises, and the establishment of the optimization problem is more complete. However, it is difficult for the extended Kalman filtering to have good accuracy when coping with unmanned surface vessel models with strongly nonlinear dynamics. In addition, this method also does not consider the influences of outlier noises and unknown noise characteristics.

SUMMARY

Invention objective: To overcome the deficiencies in the prior art, the present invention provides an adaptive robust estimation method and system for parameters of an unmanned surface vessel, which are used for improving the robustness of estimation of model parameters of the unmanned surface vessel and implementing high-accuracy estimation of a high-dimensional process noise covariance matrix.

Technical solution: To achieve the above invention objective, an adaptive robust estimation method for parameters of an unmanned surface vessel provided by the present invention includes the following steps:

• • step 1: constructing an augmented state estimation problem considering an external disturbance for the unmanned surface vessel; augmented state vectors including a state vector of the unmanned surface vessel, a parameter vector of an unmanned surface vessel model, and an unknown input vector representing a modeling error of an unmanned surface vessel system and the external disturbance;
  • step 2: depending on a real-time input vector and a measurement vector of the unmanned surface vessel system, employing an adaptive robust unscented Kalman filtering method having process noise covariance matrix constraints and based on the combination of a maximum cross entropy criterion and a minimum mean square error criterion to obtain model parameters of the unmanned surface vessel; the input vector including a control signal from a shipborne controller, and the measurement vector including data obtained by GPS and IMU measurement;
  • a method for estimating the process noise covariance matrix including: firstly, employing an innovation-based adaptive method to estimate a noise covariance matrix _k⁰; then, introducing covariance matrix constraints according to a known structure and dividing a process noise covariance matrix _k to be estimated into a diagonal block without structure constraints and a diagonal block with structure constraints; and finally, employing KL divergence to construct and solve the following optimization problem:

${\hat{Q}}_k^{1,j_1},{\lambda }_k^{j_2}=\underset{{\hat{Q}}_k^{1,j_1},{\lambda }_k^{j_2}}{\min }{\int }_{-\infty }^{+\infty }p\left(x|{\hat{Q}}_k^0\right)\log \frac{p\left(x|{\hat{Q}}_k^0\right)}{p\left(x|{\hat{Q}}_k\right)}\mathrm{dx};$

• • where k represents the moment; _k^1,j1 represents the diagonal block without structure constraints; λ_k^j2 represents a coefficient of the diagonal block _k^2,j2 with structure constraints; _k^2,j2=λ_k^j2_k^c,j2, _k^c,j2 is a known structure; j₁=1, . . . , n₁, j₂=1, . . . , n₂ n₁ and n₂ represent the number of the diagonal block without structure constraints and the number of the diagonal block with structure constraints, respectively; p(x|_k⁰) and p(x|_k) represent probability density functions of a random variable X with the covariance matrix being _k⁰ and _k respectively and obeying zero mean Gaussian distribution.

Preferably, the noise covariance matrix estimated by the innovation-based adaptive method is expressed as:

${\hat{Q}}_k^0=\frac{1}{N}\sum _{i=k_0}^k\left({\hat{x}}_{i|i}-{\hat{x}}_{i|i-1}\right){\left({\hat{x}}_{i|i}-{\hat{x}}_{i|i-1}\right)}^T,k_0=k-N+1,k\ge N$

• • where k₀ represents an initial moment of a sample used for calculating the covariance matrix; N represents the quantity of the sample used; and {circumflex over (x)}_i|i-1 and {circumflex over (x)}_i|i are a predicted value and an estimated value of the augmented state vector at an i th moment, respectively.

Preferably, the process noise covariance matrix to be estimated is expressed as:

${\hat{Q}}_k=\mathrm{diag}\left[{\hat{Q}}_k^{1,1},...,{\hat{Q}}_k^{1,n_1},{\hat{Q}}_k^{2,1},...,{\hat{Q}}_k^{2,n_2}\right];$

• • in a case that _k^c,j2=0 is present in an actual problem, this diagonal block is removed to ensure that _k is positive definite, and _k^2,j2=0 is directly given in an estimation result.

Preferably, an optimal solution for the optimization problem constructed by employing the KL divergence is:

${\hat{Q}}_k^{1,j_1}={\hat{Q}}_k^{3,j_1},{\lambda }_k^{j_2}=\frac{1}{m_{j_2}}\mathrm{tr}\left[{\left({\hat{Q}}_k^{c,j_2}\right)}^{-1}{\hat{Q}}_k^{4,j_2}\right];$

• • where m_j2 is the dimension of _k^c,j2; _k^3,j1 is a corresponding matrix block of _k^1,j1 in _k⁰; and _k^4,j2 is a corresponding matrix block of _k^2,j2 in _k⁰.

Preferably, the augmented estimation problem constructed in step 1 employs a system equation as below:

$x_k=\left[\begin{array}{c}{f}_{\xi }\left({\xi }_{k-1},u_{k-1}\right)+{\mathrm{Gd}}_{k-1}\\ a_{k-1}\\ d_{k-1}\end{array}\right]+w_{k-1};$ $y_k=h_{\xi }\left({\xi }_k,u_k\right)+{\mathrm{Ed}}_k+v_k;$

• • where x_k and y_k are the augmented state vector and the measurement vector at a k th moment, respectively; ξ_k-1 and ξ_k are the state vectors of the unmanned surface vessel at a (k−1)th moment and the k th moment, respectively; u_k-1 and u_k are the input vectors at the (k−1)th moment and the k th moment, respectively; d_k-1 and d_k are the unknown input vectors at the (k−1)th moment and the k th moment, respectively; a_k-1 is the parameter vector at the (k−1)th moment; f_ξ represents nonlinear mapping from the state vector ξ_k-1 and the input vector u_k-1 at the (k−1)th moment to the state vector ξ_k at the k th moment, without the consideration of noises and the unknown input vectors; h_ξ represents nonlinear mapping from the state vector ξ_k and the input vector u_k at the k th moment to the measurement vector y_k, without the consideration of the noises and the unknown input vectors; w_k-1 is zero mean process noises at the (k−1)th moment; V_k is zero mean measurement noises at the k th moment; and in a case that a system sampling period is T_s, matrices G and E are expressed as:

$G=T_s\left[\begin{array}{c}{0}_{3\times 3}\\ I_3\end{array}\right],E=\left[\begin{array}{cc}{0}_{6\times 2}& 0_{6\times 1}\\ I_2& 0_{2\times 1}\end{array}\right];$

I₂ and I₃ are 2×2 and 3×3 identity matrices, respectively.

Preferably, step 2 specifically includes:

• • step 21: initializing the adaptive robust unscented Kalman filtering, including an estimated value {circumflex over (x)}_0|0 of the augmented state vector and a covariance matrix P_0|0^xx of an error thereof;
  • step 22: acquiring the input vector u_k and the measurement vector y_k of the unmanned surface vessel system;
  • step 23: predicating an augmented state vector {circumflex over (x)}_k|k-1 at the k th moment and a covariance matrix P_k|k-1^xx of a prediction error of the augmented state vector;
  • step 24: predicating a measurement vector ŷ_k|k-1 at the k th moment, a covariance matrix P_k|k-1^yy of a prediction error of the measurement vector, and a cross covariance matrix P_k|k-1^xy;
  • step 25: constructing an optimization problem employed by robust state estimation, including:
  • linearizing a measurement equation for the unmanned surface vessel system by employing results of unscented transformation as follows:

$y_k-{\hat{y}}_{k❘k-1}=H_k\left[x_k-{\hat{x}}_{k❘k-1}\right]+r_{k;}$ $H_k={\left(P_{k❘k-1}^{\mathrm{xy}}\right)}^T{\left(P_{k❘k-1}^{\mathrm{xx}}\right)}^{-1};$

• • where r_k is noises of the linearized measurement equation, having a statistical property as below:

$E\left[r_k\right]=0,{\Psi }_k=E\left[r_k{r}_k^T\right]=P_{k❘k-1}^{\mathrm{yy}}-{\left(P_{k❘k-1}^{\mathrm{xy}}\right)}^T{\left(P_{k❘k-1}^{\mathrm{xx}}\right)}^{-1}{P}_{k❘k-1}^{\mathrm{xy}};$

• • the system with the measurement equation linearized being written as:

$\left[\begin{array}{c}{\hat{x}}_{k❘k-1}\\ y_k-{\hat{y}}_{k❘k-1}+H_k{\hat{x}}_{k❘k-1}\end{array}\right]=\left[\begin{array}{c}I\\ H_k\end{array}\right]x_k+{\xi }_k;$

• • where I is an identity matrix, and the vector ξ_k has the following properties:

${\xi }_k=\left[\begin{array}{c}{\hat{x}}_{k❘k-1}-x_k\\ r_k\end{array}\right];$ $E\left[{\xi }_k{\xi }_k^T\right]=\left[\begin{array}{cc}{P}_{k❘k-1}^{\mathrm{xx}}& 0\\ 0& {\Psi }_k\end{array}\right]=\left[\begin{array}{cc}{T_k^P\left(T_k^P\right)}^T& 0\\ 0& T_k^{\psi }{\left(T_k^{\psi }\right)}^T\end{array}\right]=T_k{T}_k^T;$

• • where T_k^P, T_k^ψ, and T_k are Cholesky factors of P_k|k-1^xx, Ψ_k, and E[ξ_kξ_k^T, respectively;
  • further, the following equation being provided:

$T_k^{-1}\left[\begin{array}{c}{\hat{x}}_{k|k‐1}\\ y_k-{\hat{y}}_{k|k‐1}+H_k{\hat{x}}_{k|k‐1}\end{array}\right]=T_k^{-1}\left[\begin{array}{c}I\\ H_k\end{array}\right]x_k+T_k^{-1}{\xi }_k;$

• • this equation being rewritten as:

$Y_k=A_k{x}_k+e_k;$

• • where:

$Y_k=T_k^{-1}\left[\begin{array}{c}{\hat{x}}_{k|k‐1}\\ y_k-{\hat{y}}_{k|k‐1}+H_k{\hat{x}}_{k|k‐1}\end{array}\right],A_k=T_k^{-1}\left[\begin{array}{c}I\\ H_k\end{array}\right],e_k=T_k^{-1}{\xi }_k;$

• • combining the maximum cross entropy criterion and the minimum mean square error criterion, the former being used for processing the process noises and the latter being used for processing the measurement noises; establishing and solving the following optimization problem to obtain optimal estimation {circumflex over (x)}_k|k:

${\hat{x}}_{k|k}=\arg \underset{x_k}{\min }\left\{\sum _{i=1}^n\left[G_{\sigma }\left(0\right)-G_{\sigma }\left(e_k^i\right)\right]+w\sum _{i=n+1}^{n+m}{\left(e_k^i\right)}^2\right\};$

• • where W is a weight coefficient; n is the dimension of the augmented state vector; M is the dimension of y_k; e_kⁱ is an ith element of e_k; σ is bandwidth; and Gσ(·) is Gaussian kernel function;
  • step 26: obtaining optimal estimation of the augmented state vector according to an iteration form of robust unscented Kalman filtering;
  • step 27: estimating the process noise covariance matrix by employing an adaptive law having covariance matrix constraints; and
  • step 28: determining whether to terminate the parameter estimation; if not, returning back to step 22.

Preferably, the state vector of the unmanned surface vessel includes a center-of-mass position X, Y and a yaw angle ψ of the unmanned surface vessel under a world coordinate system, and a center-of-mass velocity U,V and a yaw velocity r of the unmanned surface vessel under a body coordinate system; and the parameter vector of the unmanned surface vessel model includes an inertia-related parameter, a damping-related parameter, and a driving force-related parameter.

Based on the same inventive concept, an adaptive robust estimation system for parameters of an unmanned surface vessel provided by the present invention includes:

• • a problem construction module, configured to construct an augmented state estimation problem considering an external disturbance for the unmanned surface vessel; augmented state vectors including a state vector of the unmanned surface vessel, a parameter vector of an unmanned surface vessel model, and an unknown input vector representing a modeling error of an unmanned surface vessel system and the external disturbance;
  • and an adaptive robust estimation module, configured to depending on a real-time input vector and a measurement vector of the unmanned surface vessel system, employ an adaptive robust unscented Kalman filtering method having process noise covariance matrix constraints and based on the combination of a maximum cross entropy criterion and a minimum mean square error criterion to obtain model parameters of the unmanned surface vessel; the input vector including a control signal from a shipborne controller, and the measurement vector including data obtained by GPS and IMU measurement; a method for estimating the process noise covariance matrix including: firstly, employing an innovation-based adaptive method to estimate a noise covariance matrix _k⁰; then, introducing covariance matrix constraints according to a known structure and dividing a process noise covariance matrix _k to be estimated into a diagonal block without structure constraints and a diagonal block with structure constraints; and finally, employing KL divergence to construct and solve the following optimization problem:

${\hat{Q}}_k^{1,j_1},{\lambda }_k^{j_2}=\underset{{\hat{Q}}_k^{1,j_1},{\lambda }_k^{j_2}}{\min }{\int }_{-\infty }^{+\infty }p\left(x|{\hat{Q}}_k^0\right)\log \frac{p\left(x|{\hat{Q}}_k^0\right)}{p\left(x|{\hat{Q}}_k\right)}\mathrm{dx}.$

Based on the same inventive concept, a computer system provided by the present invention includes a memory, a processor, and a computer program stored in the memory and capable of running on the processor; when the computer program is loaded to the processor, the steps of the adaptive robust estimation method for parameters of an unmanned surface vessel are implemented.

Beneficial effects: The adaptive robust estimation method for parameters of an unmanned surface vessel set forth by the present invention firstly transforms the model parameter estimation problem of the unmanned surface vessel into the augmented state estimation problem, and then, employs an improved adaptive robust Kalman filtering method to estimate the model parameters of the unmanned surface vessel, depending on the real-time input vector and the measurement vector of the system. By employing the improved robust estimation method combining the maximum cross entropy criterion and the minimum mean square error criterion of the present invention, the robustness to ambient outlier noises can be improved. By employing the adaptive law having covariance matrix constraints set forth by the present invention, high-accuracy estimation of the high-dimensional process noise covariance matrix can be implemented. Simulation results indicate that the present invention can implement precise estimation for the parameters of the unmanned surface vessels.

BRIEF DESCRIPTION OF THE DRAWINGS

FIG. 1 is a general flowchart of a method according to an embodiment of the present invention.

FIG. 2 is a flowchart of an adaptive robust unscented Kalman filtering method according to an embodiment of the present invention.

FIG. 3 is an error curve graph of parameters of a twin-propeller differential unmanned surface vessel estimated according to an embodiment of the present invention.

DETAILED DESCRIPTION

The technical solutions in embodiments of the present invention will be described below clearly and completely with reference to the accompanying drawings in the embodiments of the present invention. Apparently, the described embodiments are merely a part rather than all of the embodiments of the present invention. All other embodiments obtained by a person of ordinary skill in the art based on the embodiments of the present invention without creative efforts shall fall within the protection scope of the present invention.

As shown in FIG. 1, an adaptive robust estimation method for parameters of an unmanned surface vessel disclosed by the embodiments of the present invention includes: firstly, an augmented state estimation problem considering an external disturbance for the unmanned surface vessel is constructed, specifically, a system state equation is established according to kinematic and dynamical equations of the unmanned surface vessel, a system measurement equation is established according to real-time kinematic global positioning system and inertial measurement units, an unknown input of a system is introduced, and system parameters and the unknown input are augmented into a state, so as to transform a parameter estimation problem of the unmanned surface vessel into an augmented state estimation problem; then, depending on a real-time input vector and a measurement vector of an unmanned surface vessel system, an adaptive robust unscented Kalman filtering method having process noise covariance matrix constraints and based on the combination of a maximum cross entropy criterion and a minimum mean square error criterion is employed to obtain model parameters of the unmanned surface vessel. Specifically, considering that the state of the unmanned surface vessel system may be disturbed by outlier noises, a robust Kalman filtering method is designed based on the maximum cross entropy criterion and the minimum mean square error criterion; and considering that statistical characteristics of process noises of the unmanned surface vessel system are unknown and the system state dimension is relatively high, an adaptive law having process noise covariance matrix constraints is introduced to precisely estimate the process noise covariance matrix.

The implementation process of the embodiments of the present invention will be described in detail below by taking estimation for model parameters of a twin-propeller differential unmanned surface vessel as an example, which includes the following specific steps:

• • S1: construction of an augmented state estimation problem considering an external disturbance for the unmanned surface vessel.

When it is assumed that a water flow rate is zero, an unmanned surface vessel model may be expressed with kinematic and dynamical equations, respectively:

$\stackrel{.}{\eta }=R\upsilon ;$ $M\stackrel{.}{\upsilon }+C\upsilon +D\upsilon =\tau ;$

• • where η=[x, y, ψ]^T, X, Y represents a center-of-mass position of the unmanned surface vessel under a world coordinate system and ψ represents a yaw angle of the unmanned surface vessel under the world coordinate system; v=[u, v, r]^T, u, v represents a center-of-mass velocity of the unmanned surface vessel under a body coordinate system and r represents a yaw velocity of the unmanned surface vessel under the body coordinate system; R represents a rotation matrix; M is an inertial matrix; C is a Coriolis force matrix; D is a damping matrix; τ is a force borne by the unmanned surface vessel; when ambient disturbances are not considered, τ is composed of a forward thrust τ_u generated by a driving motor and a torque τ_r that is, τ=τ_u, 0, τ_r]^T:
  • the rotation matrix R, the inertial matrix M, the Coriolis force matrix C, and the damping matrix D are expressed as follows, respectively:

$R=\left[\begin{array}{ccc}\cos \psi & -s\mathrm{in}\psi & 0\\ \sin \psi & \cos \psi & 0\\ 0& 0& 1\end{array}\right];M=\left[\begin{array}{ccc}m-X_{\stackrel{.}{u}}& 0& 0\\ 0& m-Y_{\stackrel{.}{v}}& {\mathrm{mx}}_g-Y_{\stackrel{.}{r}}\\ 0& {\mathrm{mx}}_g-Y_{\stackrel{.}{r}}& I_z-N_{\stackrel{.}{r}}\end{array}\right];$ $C=\left[\begin{array}{ccc}0& 0& -c_{31}\\ 0& 0& -c_{32}\\ c_{31}& c_{32}& 0\end{array}\right];D=\left[\begin{array}{ccc}{d}_{11}& 0& 0\\ 0& d_{22}& d_{23}\\ 0& d_{32}& d_{33}\end{array}\right];$

• • where m is a mass of the unmanned surface vessel; I_z is a moment of inertia of the unmanned surface vessel; x_g is a center-of-mass coordinate; and X_{{dot over (u)}}, Y_{{dot over (v)}}, and N_{{dot over (r)}} are hydrodynamic force derivatives;
  • c₃₁, c₃₂, d₁₁, d₂₂, d₂₃, d₃₂ and d₃₃ in the Coriolis force matrix C and the damping matrix D are as follows, respectively:

$c_{31}=\left(m-Y_{\stackrel{.}{v}}\right)v+\left({\mathrm{mx}}_g-Y_{\stackrel{.}{r}}\right)r;c_{32}=-\left(m-X_{\stackrel{.}{u}}\right)u;$ $d_{11}=-X_u-X_{❘u❘u}❘u❘;d_{22}=-Y_v-Y_{❘v❘v}❘v❘;d_{23}=-Y_r-Y_{❘r❘r}❘r❘;$ $d_{32}=-N_v-N_{❘v❘v}❘v❘;d_{33}=-N_r-N_{❘r❘r}❘r❘;$

• • where X_u, X_|u|u, Y_v, Y_|v|v, Y_r, Y_|r|r, N_v, N_|v|v, N_r and N_|r|r are different types of hydrodynamic force derivatives.

With respect to a twin-propeller differential unmanned surface vessel system, a driving force thereof is generated by two (sets of) motors. Under an assumption that rotating speeds of the two (sets of) motors are in direct proportion to control signals τ₁ and τ₂ of a shipborne controller, τ_u and τ_r have the following relational expressions with respect to the control signals τ₁ and τ₂:

${\tau }_u=k_1\left(❘{\tau }_1❘{\tau }_1+❘{\tau }_2❘{\tau }_2\right)-k_2\left(❘{\tau }_1❘+❘{\tau }_2❘\right)u;$ ${\tau }_r=k_{1}l\left(❘{\tau }_1❘{\tau }_1-❘{\tau }_2❘{\tau }_2\right)-k_{2}l\left(❘{\tau }_1❘-❘{\tau }_2❘\right)u;$

• • where k₁ and k₂ are coefficients, and l is a moment arm of a torque generated by each one of the two (sets of) motors. In conjunction with the above expressions, by properly rearranging the kinematic and dynamical equations of the unmanned surface vessel, state equations of the twin-propeller differential unmanned surface vessel system may be obtained:

$\stackrel{.}{x}=u\cos \psi -v\sin \psi ;$ $\stackrel{.}{y}=u\sin \psi +v\cos \psi ;$ $\stackrel{.}{\psi }=r;$ $\stackrel{.}{u}=a_1\mathrm{vr}+a_2{r}^2+a_{5}u+a_6❘u❘u+a_{15}\left(❘{\tau }_1❘{\tau }_1+❘{\tau }_2❘{\tau }_2\right)-a_{16}\left(❘{\tau }_1❘+❘{\tau }_2❘\right)u;$ $\stackrel{.}{v}=a_3\mathrm{uv}+a_4\mathrm{ur}+a_{7}v+a_8❘v❘v+a_{9}r+a_{10}❘r❘r-a_{17}\left(❘{\tau }_1❘{\tau }_1-❘{\tau }_2❘{\tau }_2\right)+a_{18}\left(❘{\tau }_1❘-❘{\tau }_2❘\right)u;$ $\stackrel{.}{r}=\frac{a_1{a}_3}{a_2}\mathrm{uv}-a_3\mathrm{ur}+a_{11}v+a_{12}❘v❘v+a_{13}r+a_{14}❘r❘r+\frac{a_1{a}_{17}}{a_2}\left(❘{\tau }_1❘{\tau }_1-❘{\tau }_2❘{\tau }_2\right)-\frac{a_1{a}_{18}}{a_2}\left(❘{\tau }_1❘-❘{\tau }_2❘\right)u;$

• • where a₁, . . . , a₄ are inertia-related parameters, a₅, . . . ,a₁₄ are damping-related parameters, and a₁₅′ . . . a₁₈ are driving force-related parameters. Assuming that a vector a=[a₁, . . . , a₁₈]^T represents a parameter vector to be identified, the state vector of the twin-propeller differential unmanned surface vessel is selected as ξ=[x, y, ψ, u, v, r]^T and the input vector thereof is selected as u=τ₁,τ₂]^T the above equations may be denoted as a discrete state equation below:

${\xi }_k=f_{\xi }\left({\xi }_{k-1},u_{k-1}\right).$

It is assumed that the unmanned surface vessel has been equipped with a global positioning system (GPS) and an inertial measurement unit (IMU), the former is used for measuring a position X, Y and a velocity u, v of the unmanned surface vessel and the latter is used for measuring a yaw angle ψ, a yaw velocity r, and an acceleration {dot over (u)}, {dot over (v)} of the unmanned surface vessel, then the measurement equation of the system is expressed as below:

$y_k=h_{\xi }\left({\xi }_{k,}{u}_k\right)={\left[x_k,y_k,{\psi }_k,u_k,v_k,r_k,{\stackrel{.}{u}}_k,{\stackrel{.}{v}}_k\right]}^T.$

In addition, considering influences caused by unknown inputs such as a modeling error of the unmanned surface vessel system and the external disturbance to dynamics characteristics of the unmanned surface vessel, an unknown input vector d_k ∈³ is introduced. To transform the parameter estimation problem of the unmanned surface vessel into the augmented state estimation problem, the following augmented state vector is selected:

$x_k={\left[\begin{array}{ccc}{\xi }_k^T& a_k^T& d_k^T\end{array}\right]}^T.$

In this case, a system equation employed by the augmented state estimation problem may be written as:

$x_k=f\left(x_{k-1},u_{k-1}\right)+w_{k-1}=\left[\begin{array}{c}{f}_{\xi }\left({\xi }_{k-1},u_{k-1}\right)+{\mathrm{Gd}}_{k-1}\\ a_{k-1}\\ d_{k-1}\end{array}\right]+w_{k-1};$ $y_k=h\left(x_k,u_k\right)+v_k=h_{\xi }\left({\xi }_k,u_k\right)+{\mathrm{Ed}}_k+v_k;$

• • where f represents nonlinear mapping from an augmented state vector x_k-1 and an input vector u_k-1 at a (k−1)th moment to an augmented state vector x_k at a k th moment, without the consideration of noises; h represents nonlinear mapping from the augmented state vector x_k and an input vector U_k at the k th moment to a measurement vector y_k, without the consideration of noises; W_k-1 and V_k are zero mean process noises and zero mean measurement noises of covariance matrices _k-1 and R_k at the (k−1)th moment and the k th moment, respectively; and in a case that a system sampling period is T_s, matrices G and E are expressed as:

$G=T_s\left[\begin{array}{c}{0}_{3\times 3}\\ I_3\end{array}\right],E=\left[\begin{array}{cc}{0}_{6\times 2}& 0_{6\times 1}\\ I_2& 0_{2\times 1}\end{array}\right];$

S2: adaptive robust unscented Kalman filtering having process noise covariance matrix constraints is employed to obtain model parameters of the unmanned surface vessel.

S21: the adaptive robust unscented Kalman filtering is initialized.

An initial augmented state vector estimated value {circumflex over (X)}_0|0 is selected and the value should be approximate to actual initial augmented state vector, parameters and unknown input of the unmanned surface vessel. At the same time, a covariance matrix P_0|0^xx of an error of the initial augmented state vector estimated value is given.

S22: the input vector and the measurement vector of the unmanned surface vessel system are acquired.

An input vector u_k of the unmanned surface vessel system may be directly obtained from a controller, whereas a measurement vector y_k may be obtained from measurement results of the GPS and the IMU.

S23: an augmented state vector and a covariance matrix of a prediction error of the augmented state vector are predicted.

Firstly, a set of σ sample points X_k-1|k-1⁽ⁱ⁾ at the (k−1)th moment are generated:

${{\rm X}}_{k-1❘k-1}^{\left(0\right)}={\hat{x}}_{k-1❘k-1};$ ${{\rm X}}_{k-1❘k-1}^{\left(i\right)}={\hat{x}}_{k-1❘k-1}+{\left(\sqrt{{\mathrm{cP}}_{k-1❘k-1}^{\mathrm{xx}}}\right)}_i,i=1,...,n;$ ${{\rm X}}_{k-1❘k-1}^{\left(i\right)}={\hat{x}}_{k-1❘k-1}-{\left(\sqrt{{\mathrm{cP}}_{k-1❘k-1}^{\mathrm{xx}}}\right)}_{i-n},i=n+1,...,2n;$

• • where {circumflex over (x)}_k-1|k-1 is the augmented state vector estimated at the (k−1)th moment; P_k-1|k-1^xx is a covariance matrix of an error of this augmented state vector estimated value; n is the dimension of the augmented state vector; c is a constant; and (√{square root over (cP_k-1|k-1^xx)})_i represents a vector composed of an i th column of a Cholesky factor of cP_k-1|k-1^xx. Then, a predicted value X_k|k-1⁽ⁱ⁾ of each σ point is mapped by means of the state equation of the unmanned surface vessel system:

${{\rm X}}_{k❘k-1}^{\left(i\right)}=f\left({{\rm X}}_{k-1❘k-1}^{\left(i\right)},u_{k-1}\right);$

• • an augmented state vector {circumflex over (x)}_k|k-1 at the (k)th moment is predicted according to the predicted value X_k|k-1⁽ⁱ⁾ of the σ point:

${\hat{x}}_{k❘k-1}=\sum _{i=0}^{2n}{W}^{\left(i\right)}{{\rm X}}_{k❘k-1}^{\left(i\right)};$

• • where W is the weight.

Secondly, the covariance matrix P_k|k-1^xx of the error of the predicted value of the augmented state vector is calculated:

$P_{k❘k-1}^{\mathrm{xx}}=\sum _{i=0}^{2n}{W}^{\left(i\right)}\left({{\rm X}}_{k❘k-1}^{\left(i\right)}-{\hat{x}}_{k+1❘k}\right){\left({{\rm X}}_{k❘k-1}^{\left(i\right)}-{\hat{x}}_{k+1❘k}\right)}^T+Q_k.$

S24: a measurement vector, a covariance matrix of a prediction error of the measurement vector and a cross covariance matrix are predicted:

• • σ point X_k|k-1⁽ⁱ⁾ is regenerated:

${{\rm X}}_{k❘k-1}^{\left(0\right)}={\hat{x}}_{k❘k-1};$ ${{\rm X}}_{k❘k-1}^{\left(i\right)}={\hat{x}}_{k❘k-1}+{\left(\sqrt{{\mathrm{cP}}_{k❘k-1}^{\mathrm{xx}}}\right)}_i,i=1,...,n;$ ${{\rm X}}_{k❘k-1}^{\left(i\right)}={\hat{x}}_{k❘k-1}-{\left(\sqrt{{\mathrm{cP}}_{k❘k-1}^{\mathrm{xx}}}\right)}_{i-n},i=n+1,...,2n;$

• • a measurement vector γ_k|k-1⁽ⁱ⁾ predicted by each σ point X_k|k-1⁽ⁱ⁾ is calculated:

${\gamma }_{k❘k-1}^{\left(i\right)}=h\left({{\rm X}}_{k❘k-1}^{\left(i\right)},u_k\right);$

• • a measurement vector ŷ_k|k-1 predicted at the k th moment is calculated:

${\hat{y}}_{k❘k-1}=\sum _{i=0}^{2n}{W}^{\left(i\right)}{\gamma }_{k❘k-1}^{\left(i\right)};$

• • a covariance matrix P_k|k-1^yy of the prediction error of the measurement vector is calculated:

$P_{k|k-1}^{\mathrm{yy}}=\sum _{i=0}^{2n}{W}^{\left(i\right)}\left({\gamma }_{k|k-1}^{\left(i\right)}-{\hat{y}}_{k|k-1}\right){\left({\gamma }_{k|k-1}^{\left(i\right)}-{\hat{y}}_{k|k-1}\right)}^T+R_k;$

• • an cross covariance matrix P_k|k-1^yy between {circumflex over (x)}_k|k-1^xy and ŷ_k|k-1 is calculated:

$P_{k|k-1}^{\mathrm{xy}}=\sum _{i=0}^{2n}{W}^{\left(i\right)}\left({\chi }_{k|k-1}^{\left(i\right)}-{\hat{x}}_{k|k-1}\right){\left({\gamma }_{k|k-1}^{\left(i\right)}-{\hat{y}}_{k|k-1}\right)}^T.$

S25: an optimization problem employed by robust state estimation is constructed.

A measurement equation for the unmanned surface vessel system may be linearized by employing results of unscented transformation as follows:

$y_k-{\hat{y}}_{k|k-1}=H_k\left[x_k-{\hat{x}}_{k|k-1}\right]+r_k;$ $H_k={\left(P_{k|k-1}^{\mathrm{xy}}\right)}^T{\left(P_{k|k-1}^{\mathrm{xx}}\right)}^{-1};$

• • where r_k is noises of the linearized measurement equation, which have a statistical property as below:

$E\left[r_k\right]=0,{\Psi }_k=E\left[r_k{r}_k^T\right]=P_{k|k-1}^{\mathrm{yy}}-{\left(P_{k|k-1}^{\mathrm{xy}}\right)}^T{\left(P_{k|k-1}^{\mathrm{xx}}\right)}^{-1}{P}_{k|k-1}^{\mathrm{xy}}.$

The system with the measurement equation linearized may be written as:

$\left[\begin{array}{c}{\hat{x}}_{k|k-1}\\ y_k-{\hat{y}}_{k|k-1}+H_k{\hat{x}}_{k|k-1}\end{array}\right]=\left[\begin{array}{c}I\\ H_k\end{array}\right]x_k+{\xi }_k;$

• • where:

${\xi }_k=\left[\begin{array}{c}{\tilde{x}}_{k|k-1}\\ r_k\end{array}\right]=\left[\begin{array}{c}{\hat{x}}_{k|k-1}-x_k\\ r_k\end{array}\right].$

It may be verified that, the desire Eξ_k] of ξ_k has zero mean, and a covariance matric E[ξ_kξ_k^T] thereof may be transformed as:

$E\left[{\xi }_k{\xi }_k^T\right]=\left[\begin{array}{cc}{P}_{k|k-1}^{\mathrm{xx}}& 0\\ 0& {\Psi }_k\end{array}\right]=\left[\begin{array}{cc}{T_k^P\left(T_k^P\right)}^T& 0\\ 0& T_k^{\psi }{\left(T_k^{\psi }\right)}^T\end{array}\right]=T_k{T}_k^T;$

• • where T_k^P, T_k^ψ, T_k are Cholesky factors of P_k|k-1^xx, ψ_k, and E[ξ_k ξ_k^T], respectively.

Further, the following equation is provided:

$T_k^{-1}\left[\begin{array}{c}{\hat{x}}_{k|k-1}\\ y_k-{\hat{y}}_{k|k-1}+H_k{\hat{x}}_{k|k-1}\end{array}\right]=T_k^{-1}\left[\begin{array}{c}I\\ H_k\end{array}\right]x_k+T_k^{-1}{\xi }_k;$

This equation is rewritten as:

$Y_k=A_k{x}_k+e_k;$

• • where:

$Y_k=T_k^{-1}\left[\begin{array}{c}{\hat{x}}_{k|k-1}\\ y_k-{\hat{y}}_{k|k-1}+H_k{\hat{x}}_{k|k-1}\end{array}\right],A_k=T_k^{-1}\left[\begin{array}{c}I\\ H_k\end{array}\right],e_k=T_k^{-1}{\xi }_k.$

In addition, it may be verified that the mean value of a vector e_k is 0, and a covariance matrix thereof is an identity matrix. The maximum cross entropy criterion with good robustness is introduced as an optimal criterion, and is combined with the minimum mean square error criterion employed by conventional Kalman filtering. The former is used for processing the process noises possibly having outliers and the latter is used for processing the measurement noises, that is, the former is modeled with respect to the state equation and the latter is modeled with respect to the measurement equation. An optimal estimation {circumflex over (x)}_k|k integrating the robustness and accuracy may be obtained by establishing and solving the following optimization problem:

${\hat{x}}_{k|k}=\arg \underset{x_k}{\min }\left\{\sum _{i=1}^n\left[G_{\sigma }\left(0\right)-G_{\sigma }\left(e_k^i\right)\right]+w\sum _{i=n+1}^{n+m}{\left(e_k^i\right)}^2\right\};$

• • where W is a manually set weight coefficient and is used for balancing two optimal criteria; M is the dimension of the measurement vector y_k; e_kⁱ is an i th element of e_k; σ is bandwidth; and G_σ(·) is Gaussian kernel function:

$G_{\sigma }\left(e\right)=\exp \left(-\frac{e^2}{2{\sigma }^2}\right).$

S26: optimal estimation of the augmented state is obtained according to an iteration form of robust unscented Kalman filtering.

The optimal solution in S25 satisfies a fixed point equation below and needs to be iteratively solved:

${\hat{x}}_{k|k}={\left(A_k^T{W}_k{A}_k\right)}^{-1}{A}_k^T{W}_k{Y}_k;$

• • where W_k=diag (W_k^x,I_m), I_m is an m-dimensional identity matrix, and:

$W_k^x=\frac{1}{2w{\sigma }^2}\mathrm{diag}\left[G_{\sigma }\left(e_k^1\right),\dots ,G_{\sigma }\left(e_k^n\right)\right].$

A robust unscented Kalman filter iterative equation as below may be obtained by matrix inversion lemma and properly rearranging the optimal solution:

${\hat{x}}_{k|k}^{\left(t\right)}={\hat{x}}_{k|k-1}+K_k^{\left(t\right)}\left(y_k-{\hat{y}}_{k|k-1}\right);$ $K_k^{\left(t\right)}={\stackrel{\_}{P}}_{k|k-1}^{\mathrm{xx},\left(t\right)}{H_k^T\left(H_k{\stackrel{\_}{P}}_{k|k-1}^{\mathrm{xx},\left(t\right)}{H}_k^T+{\stackrel{\_}{\Psi }}_k^{\left(t\right)}\right)}^{-1};$ ${\stackrel{\_}{P}}_{k|k-1}^{\mathrm{xx},\left(t\right)}-{T_k^P\left(W_k^{x,\left(t\right)}\right)}^{-1}{\left(T_k^P\right)}^T;$ ${\stackrel{\_}{\Psi }}_k^{\left(t\right)}={\Psi }_k;$ $e_k^{\left(t\right)}=Y_k-A_k{\hat{x}}_{k|k}^{\left(t-1\right)};$

• • where the superscript (t) represents a t th iteration; {circumflex over (x)}_k|k⁽⁰⁾={circumflex over (x)}_k|k-1; and in a case that the following conditions are satisfied, let {circumflex over (x)}_k|k={circumflex over (x)}_k|k^(t) and K_k=K^t):

$\frac{{\hat{x}}_{k❘k}^{\left(t\right)}-{\hat{x}}_{k❘k}^{\left(t-1\right)}}{{\hat{x}}_{k❘k}^{\left(t-1\right)}}\le \epsilon ;$

• • where ε is a threshold. After that, a covariance matrix of a state prediction error is:

$P_{k❘k}^{\mathrm{xx}}=\left(I-K_k{H}_k\right){P_{k❘k-1}^{\mathrm{xx}}\left(I-K_k{H}_k\right)}^T+K_k{\Phi }_k{K}_k^T.$

S27: the process noise covariance matrix is estimated by employing an adaptive law having covariance matrix constraints.

On the premise of slow change of the noise covariance matrix and in order to ensure the positive definiteness of the noise covariance matrix, the innovation-based adaptive method may be employed to perform primary estimation of the noise covariance matrix:

${\hat{Q}}_k^0=\frac{1}{N}\sum _{i=k_0}^k\left({\hat{x}}_{i❘i}-{\hat{x}}_{i❘i-1}\right){\left({\hat{x}}_{i❘i}-{\hat{x}}_{i❘i-1}\right)}^T,k_0=k-N+1,k\ge N;$

• • where _k⁰ is a covariance matrix estimated by the innovation-based adaptive method; k₀ represents an initial moment of a sample used for calculating the covariance matrix; N represents the quantity of the sample used; xî_i|i-1 is a predicted value of the augmented state vector at an i th moment; and {circumflex over (x)}_i|i is an estimated value of the augmented state vector at the i th moment. An augmentation system for the unmanned surface vessel is a high-dimensional system and contains 27 dimensions of state variables. Since high-dimensional process noise covariance matrices directly calculated by using the above equation have greater errors, introducing covariance matrix constraints according to a known structure on the premise that the structure of the process noise covariance matrix is known may reduce the estimation error of the covariance matrix. It is assumed that the process noise covariance matrix to be estimated is expressed as:

${\hat{Q}}_k=\mathrm{diag}\left[{\hat{Q}}_k^{1,1},\dots ,{\hat{Q}}_k^{1,n_1},{\hat{Q}}_k^{2,1},\dots ,{\hat{Q}}_k^{2,n_2}\right];$

• • where _k^1,j1, j₁=1, . . . ,n₁ represents a diagonal block without structure constraints, which needs to be solved; the number of _k^1,j1 is n₁; _k^2,j2 represents a diagonal block with structure constraints, that is, _k^2,j2=λ_k^j2_k^c,j2,j₂=1, . . . , n₂, λ_k^j2<0; _k^c,j2 is a known structure; λ_k^j2 is a coefficient to be solved; and the number of _k^2,j2 is n₂. In addition, in a case that _k^c,j2=0 is present in an actual problem, this diagonal block is removed to ensure that _k is positive definite, and _k^2,j2=0 is directly given in an estimation result. Based on a starting point that enables _k⁰ estimated by using an unconstrained method to be the closest to zero mean Gaussian distribution constituted of _k considering constraints, the following optimization problem is constructed by employing KL divergence.

The following gives the optimization problem employed by the KL divergence-based covariance matrix constraints:

${\hat{Q}}_k^{1,j_1},{\lambda }_k^{j_2}=\underset{{\hat{Q}}_k^{1,j_1},{\lambda }_k^{j_2}}{\mathrm{argmin}}{\int }_{-\infty }^{+\infty }p\left(x|{\hat{Q}}_k^0\right)\log \frac{p\left(x❘{\hat{Q}}_k^0\right)}{p\left(x❘{\hat{Q}}_k\right)}\mathrm{dx};$

• • where p(x|_k⁰) and p(x|_k) represent probability density functions of a random variable x with the covariance matrix being _k⁰ and _k respectively and obeying zero mean Gaussian distribution:

$\begin{array}{c}p\left(x❘{\hat{Q}}_k^0\right)=\frac{1}{{\left(2\pi \right)}^{n/2}{❘{\hat{Q}}_k^0❘}^{1/2}}\exp \left(-\frac{1}{2}{x^T\left({\hat{Q}}_k^0\right)}^{-1}x\right);\\ p\left(x❘{\hat{Q}}_k\right)=\frac{1}{{\left(2\pi \right)}^{n/2}{❘{\hat{Q}}_k❘}^{1/2}}\exp \left(-\frac{1}{2}{x}^T{\hat{Q}}_k^{-1}x\right);\end{array}$

An optimal solution below may be obtained by solving the above optimization problem:

${\hat{Q}}_k^{1,j_1}={\hat{Q}}_k^{3,j_1},{\lambda }_k^{j_2}=\frac{1}{m_{j_2}}\left[{\left({\hat{Q}}_k^{c,j_2}\right)}^{-1}{\hat{Q}}_k^{4,j_2}\right];$

• • where m_j2 is the dimension of _k^c,j2; _k^3,j3 is a corresponding matrix block of _k^1,j1 in _k⁰ and _k^4,j2 is a corresponding matrix block of _k^2,j2 in _k⁰.

S28: whether to terminate parameter identification is determined; if not, return back to S22.

In this embodiment, MATLAB 2021b is used as a simulation software to compare the adaptive robust estimation method for parameters of an unmanned surface vessel of the present invention with a conventional unscented Kalman filtering method.

The settings of model parameters of a twin-propeller differential unmanned surface vessel used for simulation are shown in Table 1:

TABLE 1
Settings of Model Parameters of a Twin-propeller
Differential Unmanned Surface Vessel
Parameter Numerical Value
a1        1.3101
a2        0.0424
a3        0.0951
a4        −0.7602
a5        −0.0280
a6        −0.0514
a7        −0.3124
a8        −0.7753
a9        −0.0106
a10       −0.0945
a11       −0.2765
a12       −0.0707
a13       −0.6842
a14       −0.2343
a15       0.3876
a16       1.9380
a17       0.0238
a18       0.1189

Measurement noises used for simulation are zero mean Gaussian noises, with a covariance matrix as:

$R_k=\mathrm{diag}\left(1,1,0.7615,4,4,76.15,100,100\right)\times 10^{-4}.$

Noises experienced by the state vector of the unmanned surface vessel are composed of the zero mean Gaussian noises and outliers. A covariance matrix of the zero mean Gaussian noises is:

$Q_k^{\xi }=\mathrm{diag}\left(1,1,0.7615,25,25,3.046\right)\times 10^{-3}.$

The outliers are generated once every 10 seconds. The outliers also obey the zero mean Gaussian distribution and have a covariance matrix 10_k^ξ. The covariance of initial process noises of the adaptive robust unscented Kalman filtering and the unscented Kalman filtering is:

$Q_0=\mathrm{diag}\left(1,1,0.7615,25,25,3.046,0_{1\times 18},10,10,10\right)\times 10^{-4}.$

The system input is set as:

$u_k=\left[\begin{array}{c}0.5+0.6\sin 0.01k\\ 0.5+0.6\cos 0.01k\end{array}\right].$

The unknown input experienced by the system is set as:

$d_k=\left[\begin{array}{c}0.5\cos k-0.1\\ 0.5\sin 2k+0.1\\ 2r\mathrm{and}-1\end{array}\right];$

• • where rand represents a uniformly distributed random number in a range from 0 to 1.

The process noise covariance matrix to be estimated has the following known structure:

${\hat{Q}}_k=\mathrm{diag}\left[{\hat{Q}}_k^{2,1},{\hat{Q}}_k^{2,2},{\hat{Q}}_k^{1,1},{\hat{Q}}_k^{2,3},{\hat{Q}}_k^{1,2}\right]$

• • where _k^c,1 is a two-dimensional identity matrix, that is, _k^c,1=I₂; _k^c,2 is a one-dimensional identity matrix, that is, _k^c,2=I₁; and _k^c,3 is an 18×18-dimensional identity matrix, that is, _k^c,3=0_18×18.

Parameter settings of the adaptive robust unscented Kalman filtering method are: σ=12, ε=10⁻⁶, w=3.5×10⁻⁴, and N=200. The sampling period is Δt=0.1. Initial estimations of the augmented state vectors of the two types of unscented Kalman filtering are identical, the initial estimations of the state vector and the unknown input are set as 0, the parameters a₁, . . . , a₄, a₁₅, . . . , a₁₈ are set as 1, and the parameters a₅, . . . , a₁₄ are set as −1. The duration of simulation is 500 seconds, and 20 times of simulation are performed in total.

FIG. 3 shows an iteration curve of a parameter error Δa_k, and Δa_k is defined as:

$\Delta a_k=\frac{1}{18}\sum _{i=1}^{18}\sqrt{\frac{1}{20}\sum _{t=1}^{20}{\left(\Delta a_{i,k}^t\right)}^2};$

• • where Δa_i,k^t is an estimation error of an i th parameter at the k th moment in a t th simulation.

In FIG. 3, the solid line marked as UKF represents the error of the parameters estimated by means of the conventional unscented Kalman filtering, and the dotted line marked as ARUKF represents the error of the parameters estimated by means of the adaptive robust unscented Kalman filtering provided by the present invention. It can be seen that, the adaptive robust estimation method for parameters of an unmanned surface vessel of the present invention can implement a better estimation effect.

Based on the same inventive concept, an adaptive robust estimation system for parameters of an unmanned surface vessel provided by the embodiments of the present invention includes: a problem construction module, configured to construct an augmented state estimation problem considering an external disturbance for the unmanned surface vessel; augmented state vectors including a state vector of the unmanned surface vessel, a parameter vector of an unmanned surface vessel model, and an unknown input vector representing a modeling error of an unmanned surface vessel system and the external disturbance; and an adaptive robust estimation module, configured to depending on a real-time input vector and a measurement vector of the unmanned surface vessel system, employ an adaptive robust unscented Kalman filtering method having process noise covariance matrix constraints and based on the combination of a maximum cross entropy criterion and a minimum mean square error criterion to obtain model parameters of the unmanned surface vessel.

For a specific working process of each module described above, reference may be made to a corresponding process in the foregoing method embodiment. Details are not described herein again. The module division is merely logical function division and may be other division in actual implementation. For example, a plurality of modules may be combined or integrated into another system.

Based on the same inventive concept, a computer system disclosed by the embodiments of the present invention includes a memory, a processor, and a computer program stored in the memory and capable of running on the processor; when the computer program is loaded to the processor, the steps of the adaptive robust estimation method for parameters of an unmanned surface vessel are implemented.

A person skilled in the art may understand that, the technical solution of the present invention is essentially or a part contributing to the prior art may be embodied in a form of software products, the computer software product is stored in a storage medium and includes a plurality of instructions for causing a computer system (which may be a personal computer, a server, or a network device) to execute all or some steps of the method according to the embodiments of the present invention. The storage medium includes: various media that can store computer programs, such as a USB flash drive, a removable hard disk, a read-only memory (ROM), a random access memory (RAM), a magnetic disk, or an optical disc.

The above contents are only preferred embodiments of the present invention and are not intended to limit the present invention. Any modification, equivalent replacement, and improvement made within the spirit and principle of the present invention shall be included in the scope of the present invention.

Claims

1. An adaptive robust estimation method for parameters of an unmanned surface vessel, comprising the following steps: step 1: constructing an augmented state estimation problem considering an external disturbance for the unmanned surface vessel; augmented state vectors comprising a state vector of the unmanned surface vessel, a parameter vector of an unmanned surface vessel model, and an unknown input vector representing a modeling error of an unmanned surface vessel system and the external disturbance;step 2: depending on a real-time input vector and a measurement vector of the unmanned surface vessel system, employing an adaptive robust unscented Kalman filtering method having process noise covariance matrix constraints and based on the combination of a maximum cross entropy criterion and a minimum mean square error criterion to obtain model parameters of the unmanned surface vessel; the input vector comprising a control signal from a shipborne controller, and the measurement vector comprising data obtained by GPS and IMU measurement;a method for estimating the process noise covariance matrix comprising: firstly, employing an innovation-based adaptive method to estimate a noise covariance matrix k0; then, introducing covariance matrix constraints according to a known structure and dividing a process noise covariance matrix k to be estimated into a diagonal block without structure constraints and a diagonal block with structure constraints; and finally, employing KL divergence to construct and solve the following optimization problem:

2. The adaptive robust estimation method for parameters of an unmanned surface vessel according to claim 1, wherein the noise covariance matrix estimated by the innovation-based adaptive method is expressed as:

3. The adaptive robust estimation method for parameters of an unmanned surface vessel according to claim 1, wherein the process noise covariance matrix to be estimated is expressed as:

4. The adaptive robust estimation method for parameters of an unmanned surface vessel according to claim 1, wherein an optimal solution for the optimization problem constructed by employing the KL divergence is:

5. The adaptive robust estimation method for parameters of an unmanned surface vessel according to claim 1, wherein the augmented estimation problem constructed in step 1 employs a system equation as below:

6. The adaptive robust estimation method for parameters of an unmanned surface vessel according to claim 5, wherein step 2 specifically comprises: step 21: initializing the adaptive robust unscented Kalman filtering, comprising an estimated value {circumflex over (x)}0|0 of the augmented state vector and a covariance matrix P0|0xx of an error thereof;step 22: acquiring the input vector uk and the measurement vector yk of the unmanned surface vessel system;step 23: predicating an augmented state vector {circumflex over (x)}k|k-1 at the k th moment and a covariance matrix Pk|k-1xx of a prediction error of the augmented state vector;step 24: predicating a measurement vector ŷk|k-1 at the k th moment, a covariance matrix Pk|k-1yy of a prediction error of the measurement vector, and a cross covariance matrix Pk|k-1yy;step 25: constructing an optimization problem employed by robust state estimation, comprising:linearizing a measurement equation for the unmanned surface vessel system by employing results of unscented transformation as follows:

7. The adaptive robust estimation method for parameters of an unmanned surface vessel according to claim 1, wherein the state vector of the unmanned surface vessel comprises a center-of-mass position x, y and a yaw angle ψ of the unmanned surface vessel under a world coordinate system, and a center-of-mass velocity u, v and a yaw velocity r of the unmanned surface vessel under a body coordinate system; and the parameter vector of the unmanned surface vessel model comprises an inertia-related parameter, a damping-related parameter, and a driving force-related parameter.

8. The adaptive robust estimation method for parameters of an unmanned surface vessel according to claim 7, wherein in conjunction with kinematic and dynamical equations, state equations of a twin-propeller differential unmanned surface vessel system are obtained:

9. An adaptive robust estimation system for parameters of an unmanned surface vessel, comprising: a problem construction module, configured to construct an augmented state estimation problem considering an external disturbance for the unmanned surface vessel; augmented state vectors comprising a state vector of the unmanned surface vessel, a parameter vector of an unmanned surface vessel model, and an unknown input vector representing a modeling error of an unmanned surface vessel system and the external disturbance;and an adaptive robust estimation module, configured to depending on a real-time input vector and a measurement vector of the unmanned surface vessel system, employ an adaptive robust unscented Kalman filtering method having process noise covariance matrix constraints and based on the combination of a maximum cross entropy criterion and a minimum mean square error criterion to obtain model parameters of the unmanned surface vessel; the input vector comprising a control signal from a shipborne controller, and the measurement vector comprising data obtained by GPS and IMU measurement; a method for estimating the process noise covariance matrix comprising: firstly, employing an innovation-based adaptive method to estimate a noise covariance matrix k0; then, introducing covariance matrix constraints according to a known structure and dividing a process noise covariance matrix k to be estimated into a diagonal block without structure constraints and a diagonal block with structure constraints; and finally, employing KL divergence to construct and solve the following optimization problem:

10. A computer system, comprising a memory, a processor, and a computer program stored in the memory and capable of running on the processor, wherein when the computer program is loaded to the processor, the steps of the adaptive robust estimation method for parameters of an unmanned surface vessel according to claim 1 are implemented.