CN102862666B - Underwater robot state and parameter joint estimation method based on self-adaption unscented Kalman filtering (UKF) - Google Patents

Abstract

The invention discloses an underwater robot state and parameter joint estimation method based on self-adaption unscented Kalman filtering (UKF). The method comprises building an expanding reference model of an underwater robot, and enabling the reference model to have a dynamical model of the underwater robot and a fault model of a thruster; adopting a main filter of the self-adaption UKF to deliver and update expanding states composed of poses and speed of the underwater robot state and faults of the thruster according to pose information detected by a position sensor, and timely estimating speed information of the underwater robot and fault message of the thruster; and simultaneously adopting an accessory filter of the self-adaption UKF to timely estimate noise information of a system according to innovation information of the main filter. The underwater robot state and parameter joint estimation method has good instantaneity, can conduct joint estimation on states and parameters of the system, and can achieve high estimation accuracy when priori information of process noise and measurement noise is unknown.

Description

Underwater robot state and parameter joint estimation method based on self-adaptive UKF

Technical Field

The invention relates to an underwater robot state and parameter joint estimation method based on self-adaptive colorless Kalman filter (UKF), in particular to an underwater robot state and propeller fault parameter joint estimation method based on self-adaptive UKF.

Technical Field

With the rapid development of ocean development industry, underwater construction and construction projects are more and more, and the performance requirements on underwater action means are higher and higher. Underwater robots are widely used in ocean development because they can perform observation, photography, salvage, and construction work under water. The health of the thruster determines whether the underwater robot can perform the intended task well. The conventional pre-programmed underwater robot generally adopts the measures of load rejection, floating and the like when detecting that a propeller has a fault, and in many cases, the propeller of the underwater robot does not completely fail, and if the efficiency loss factor of the underwater robot can be identified in real time and then the thrust is redistributed, the underwater robot can still possibly complete the expected task.

Under the condition that noise information of the underwater robot system is known, a good estimation effect can be achieved by adopting the UKF algorithm, and when the noise information of the system is unknown, if the difference between the set noise parameter and the actual noise is too large, biased estimation and even divergence of a filtering algorithm can be caused, so that the filtering algorithm which is more suitable for estimating the noise information of the system is designed and adopted to achieve real-time estimation of the noise information of the underwater robot system, and the good estimation effect can be achieved under the condition that the noise information is unknown.

Disclosure of Invention

Aiming at the existing problems, the invention provides the underwater robot state and parameter joint estimation method based on the adaptive UKF, which can estimate the propeller fault information and the system noise information in real time, has good practicability and high estimation precision

In order to realize the purpose of the invention, the invention adopts the following scheme:

a state and parameter joint estimation method of an underwater robot based on self-adaptive UKF is characterized by comprising the following steps:

modeling a propeller fault on the basis of an underwater robot dynamics model to obtain an offline extended reference model; on-line pose information y detected by pose sensor_kThe method adopts a main filter of a self-adaptive UKF as a standard UKF filtering estimation algorithm, combines an off-line extended reference model of the underwater robot as an extended system state equation, transmits and updates an extended state formed by a system state and a propeller fault, estimates the state and parameter information of the system in real time, and obtains a mean value of result values estimated by the systemSum variance P_k(ii) a Where the state information of the system includes filtered pose information s_kAnd real-time estimated speed informationThe parameter information is a fault parameter b of the system propeller_kAnd, at the same time, calculating estimated innovation information xi according to the main filter_kCalculating the measurement Sq of the auxiliary filter_k(ii) a Establishing a state equation and a measurement equation of the UKF auxiliary filter according to the measured value Sq_kAs an observation signal of the system, the noise information of the underwater robot system is estimated in real time by adopting an auxiliary filter KF of the self-adaptive UKF to obtain the system noiseDiagonal elements of acoustic covariance matrix DHezhong FangWhere the noise information includes process noise w_kAnd measuring the noise v_kTheir covariance matrices are respectively D^wAnd D^v。

And (3) a main filter expansion system equation forming process:

when the nonlinear system is expanded, the fault parameter b of the unknown/time-varying propeller is contained_kThe system state space equation is as follows:

x_k+1＝f(x_k，b_k，u_k)+w_k

b_k+1＝f_b(x_k，b_k，u_k)+w_kb

in the formula, b_kIs an unknown/time-varying propeller fault parameter at time k, w_kbThe system propeller fault noise vector which is a fault parameter model is zero mean Gaussian noise;

when the fault parameter b_kIf the change rule is unknown, b can be_kThe random drift vector, considered uncorrelated, has the recursive expression:

b_k＝b_k-1+w_bk

based on the joint estimation of the self-adaptive UKF, the fault parameters of the system propeller are added to the real state of the system to form an extended state matrix, namelyThe system equation is formed into an extended system state equation as follows:

$x_{k+1}^a=\tilde{f}(x_k^a,u_k)+w_k^a$

in the formula,for an extended systematic process noise vector, whereinIn order for the system process to noise the vector,to estimate a propeller fault noise vector.

Extending fault parameters b in reference model_kIs/are as followsAnd expressing the efficiency loss factor of the ith propeller at the moment k, and taking a numerical value of 0-1 to express the failure degree of the propeller fault.

And (3) constructing an auxiliary filter state equation in the self-adaptive UKF algorithm:

noise variance matrix diagonal elements from underwater robot systemThe state equation of the auxiliary filter can be expressed as:

<math> <mrow> <msub> <mover> <mi>q</mi> <mo>&OverBar;</mo> </mover> <mi>k</mi> </msub> <mo>=</mo> <msub> <mi>f</mi> <mi>q</mi> </msub> <mrow> <mo>(</mo> <msub> <mover> <mi>q</mi> <mo>&OverBar;</mo> </mover> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> <mo>+</mo> <msub> <mi>w</mi> <msub> <mi>q</mi> <mi>k</mi> </msub> </msub> </mrow> </math>

whereinThe system noise vector in the auxiliary filter is zero mean Gaussian white noise;

due to the fact thatIs unknown, it can be considered as a noise-driven uncorrelated random drift vector, so the state equation of the auxiliary filter can be obtained:

<math> <mrow> <msub> <mover> <mi>q</mi> <mo>&OverBar;</mo> </mover> <mi>k</mi> </msub> <mo>=</mo> <msub> <mover> <mi>q</mi> <mo>&OverBar;</mo> </mover> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>+</mo> <msub> <mi>w</mi> <msub> <mi>q</mi> <mi>k</mi> </msub> </msub> <mo>.</mo> </mrow> </math>

constructing an auxiliary filter measurement equation in the self-adaptive UKF algorithm:

with innovation xi of the main filter_kMeasurement Sq of the variance matrix_kAs an observation signal of the system, the variance P is estimated in accordance with the main filter in one step_k|k-1Variance P of the main filter extension state_kGain matrix K of the main filter UKF_kThe measurement equations that make up the auxiliary filter are:

<math> <mrow> <mover> <mi>S</mi> <mo>&OverBar;</mo> </mover> <msub> <mi>q</mi> <mi>k</mi> </msub> <mo>=</mo> <mi>g</mi> <mrow> <mo>(</mo> <msub> <mover> <mi>q</mi> <mo>&OverBar;</mo> </mover> <mi>k</mi> </msub> <mo>)</mo> </mrow> <mo>=</mo> <mi>vdiag</mi> <mo>[</mo> <msup> <mrow> <mo>(</mo> <msubsup> <mi>K</mi> <mi>k</mi> <mi>T</mi> </msubsup> <msub> <mi>K</mi> <mi>k</mi> </msub> <mo>)</mo> </mrow> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> <msubsup> <mi>K</mi> <mi>k</mi> <mi>T</mi> </msubsup> <mrow> <mo>(</mo> <msub> <mi>P</mi> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>-</mo> <msub> <mi>P</mi> <mi>k</mi> </msub> <mo>)</mo> </mrow> <msub> <mi>K</mi> <mi>k</mi> </msub> <msup> <mrow> <mo>(</mo> <msubsup> <mi>K</mi> <mi>k</mi> <mi>T</mi> </msubsup> <msub> <mi>K</mi> <mi>k</mi> </msub> <mo>)</mo> </mrow> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> <mo>]</mo> </mrow> </math>

where vdiag is a vector of the main diagonal elements of the matrix within the middle brackets.

And the implementation process of the main filter on the extended state equation filtering estimation comprises the following steps:

mean of estimated quantities of random vectors at last momentSum variance P_k-1After nonlinear colorless transformation, a group of discrete Sigma points chi are obtained_k-1Calculating and updating the Sigma point through a system state equation, and then calculating and updating an extended state mean value of the Sigma pointSum variance P_k|k-1To extended state meanSum variance P_k|k-1Calculating to obtain a state mean value and a variance of one-step estimation; and performing colorless transformation and calculation and update by using a measurement equation on the state mean value and the variance estimated in one step to obtain the mean value of the observable state of the underwater robotIts varianceAnd covarianceAn estimated value of (d); acquisition of observable posture information y by reading sensor_kMean of co-spread statesSum variance P_k|k-1And an estimated value of the observable state and a gain matrix K_kCalculating and estimating to obtain a system mean valueVariance P_kTo derive from it an estimate of xi_k(ii) a According to the new information xi of the main filter_kThe measured value Sq is obtained through calculation_kAs the observed signal of the auxiliary filter, and then follows the auxiliary filter program step.

The auxiliary filter KF carries out a filtering estimation process on the system process noise:

estimating the diagonal elements of the system noise variance matrix of the auxiliary filter at the last momentAnd the variance thereofState equation pair using auxiliary filterAndobtaining diagonal elements after calculation updatingAnd the variance thereofThe one-step estimation value of (1); using the measurement equation of the auxiliary filter to determine the variance P of the primary filter in one step_k|k-1Extended state variance P_kAnd a gain matrix K_kObtaining estimated observed value after calculation and updateThe obtained one-step estimation value and observation valueMeasured value Sq of same main filter_kObtaining diagonal elements of system process noise variance matrix D through calculation and estimationEstimation of elementsAnd the variance thereofWhen new observation pose information y exists_kThen, the main filter program step of the next control period is continued.

Compared with the prior art, the invention has the following beneficial effects:

1. the method is simple to realize, can be directly used on the existing underwater robot carrier, does not need to change the hardware system of the underwater robot, and does not need to increase any hardware equipment. The invention can be embedded into the existing thread of the underwater robot, and for the underwater robot, the calculated amount of the system is only increased.

2. The estimation method of the invention adopts the self-adaptive UKF algorithm to estimate the state of the underwater robot and the fault parameter information of the propeller in real time according to the detection information of the underwater robot sensor, the information provides a basis for the fault-tolerant control of the underwater robot, the efficiency loss factor can be identified in real time, and then the thrust is redistributed to ensure that the underwater robot can still possibly complete the expected task.

3. The method has high estimation precision, and can carry out joint estimation on the state and the parameters of the system on line. Since the UT directly acts on the nonlinear dynamical model and does not need to be linearized, the linearization error generated in the linearization process of the nonlinear system is avoided.

4. Under the condition that the noise prior information of the system comprises process noise and measurement noise, if the set noise and the actual noise have large deviation, the adoption of the UKF filtering estimation algorithm can generate biased error or divergence of the filtering algorithm, and the self-adaptive UKF algorithm can estimate the noise information of the system in real time due to the auxiliary filter and can converge quickly, so that higher estimation precision can be achieved.

Drawings

FIG. 1 is a schematic diagram of an implementation of the method of the present invention;

fig. 2 is a flow chart of the filtering estimation of the present invention.

Detailed Description

The invention is described in further detail below with reference to the following figures and examples:

referring to the attached figures 1-2, a schematic diagram of the implementation of the method and a filtering estimation flow diagram thereof are shown. A state and parameter joint estimation method of an underwater robot based on self-adaptive UKF is characterized by comprising the following steps:

modeling a propeller fault on the basis of an underwater robot dynamics model to obtain an offline extended reference model; on-line pose information y detected by pose sensor_kThe method adopts a main filter of a self-adaptive UKF as a standard UKF filtering estimation algorithm, combines an off-line extended reference model of the underwater robot as an extended system state equation, transmits and updates an extended state formed by a system state and a propeller fault, estimates the state and parameter information of the system in real time, and obtains a mean value of result values estimated by the systemSum variance P_k(ii) a Where the state information of the system includes filtered pose information s_kAnd real-time estimated speed informationThe parameter information is a fault parameter b of the system propeller_kAnd, at the same time, calculating estimated innovation information xi according to the main filter_kCalculating the measurement Sq of the auxiliary filter_k(ii) a For building UKF auxiliary filtersEquation of state and measurement equation based on the measured value Sq_kAs an observation signal of the system, the noise information of the underwater robot system is estimated in real time by adopting an auxiliary filter KF of the self-adaptive UKF to obtain a diagonal element of a system noise covariance matrix DSum varianceAn estimate of (d); where the noise information includes process noise w_kAnd measuring the noise v_kTheir covariance matrices are respectively D^wAnd D^v。

Extending fault parameters b in reference model_kIs/are as followsAnd expressing the efficiency loss factor of the ith propeller at the moment k, and taking a numerical value of 0-1 to express the failure degree of the propeller fault.

And (3) an off-line extended reference model building process:

the dynamical model of the underwater robot is represented as follows:

<math> <mrow> <mi>M</mi> <mover> <mi>s</mi> <mrow> <mo>&CenterDot;</mo> <mo>&CenterDot;</mo> </mrow> </mover> <mo>+</mo> <mi>C</mi> <mrow> <mo>(</mo> <mover> <mi>s</mi> <mo>&CenterDot;</mo> </mover> <mo>)</mo> </mrow> <mover> <mi>s</mi> <mo>&CenterDot;</mo> </mover> <mo>+</mo> <mi>D</mi> <mrow> <mo>(</mo> <mover> <mi>s</mi> <mo>&CenterDot;</mo> </mover> <mo>)</mo> </mrow> <mover> <mi>s</mi> <mo>&CenterDot;</mo> </mover> <mo>+</mo> <mi>G</mi> <mrow> <mo>(</mo> <mi>s</mi> <mo>)</mo> </mrow> <mo>=</mo> <mi>u</mi> </mrow> </math>

in the formula, M is an inertia matrix of 6 x 6 dimensions,a 6 x 6 dimensional matrix containing centrifugal and coriolis forces,a hydrodynamic matrix of dimensions 6 × 6, g(s) a 6 × 1 dimensional matrix containing gravity and buoyancy, s and u being state and motor torque vectors of dimensions 6 × 1, respectively;

according to the fact that the fault of the propeller represents the change of the torque vector input of the system motor, an extended reference model of the underwater robot is obtained and expressed as follows:

<math> <mrow> <mi>M</mi> <mover> <mi>s</mi> <mrow> <mo>&CenterDot;</mo> <mo>&CenterDot;</mo> </mrow> </mover> <mo>+</mo> <mi>C</mi> <mrow> <mo>(</mo> <mover> <mi>s</mi> <mo>&CenterDot;</mo> </mover> <mo>)</mo> </mrow> <mover> <mi>s</mi> <mo>&CenterDot;</mo> </mover> <mo>+</mo> <mi>D</mi> <mrow> <mo>(</mo> <mover> <mi>s</mi> <mo>&CenterDot;</mo> </mover> <mo>)</mo> </mrow> <mover> <mi>s</mi> <mo>&CenterDot;</mo> </mover> <mo>+</mo> <mi>G</mi> <mrow> <mo>(</mo> <mi>s</mi> <mo>)</mo> </mrow> <mo>=</mo> <mi>F</mi> <mrow> <mo>(</mo> <mi>u</mi> <mo>)</mo> </mrow> </mrow> </math>

where F (u) is the failure function of the propeller, defined as follows:

F_k(u_k)＝u_k+U_kb_k

$b_k=\left[\begin{array}{c}{b}_k^1\\ b_k^2\\ .\\ .\\ .\\ b_k^6\end{array}\right]$

in the above formula, u_kIs a 6 x 1 dimensional motor torque vector matrix, b_kIs a 6 multiplied by 1 dimension propeller fault parameter matrix,expressed as the efficiency loss factor of the propeller at time k, the superscript i denotes the ith propeller, whenWhen, the ith propeller is normal; when in useWatch, clockIndicating that the ith propeller has a hard fault, namely the loss factor is 100%; when in useWhen the pressure exceeds the preset value, the soft failure of the ith propeller is indicated, namely, the partial failure occurs.

And (3) forming a main filter extended system state equation:

when the nonlinear system is expanded, the fault parameter b of the unknown/time-varying propeller is contained_kThe system state space equation is as follows:

x_k+1＝f(x_k，b_k，u_k)+w_k

b_k+1＝f_b(x_k，b_k，u_k)+w_bk

in the formula, b_kIs the unknown/time varying propeller fault parameter vector at time k, w_kbThe system propeller fault noise which is a fault parameter model is zero mean Gaussian noise;

because of the fault parameter b_kIf the change rule is unknown, b can be_kThe random drift vector, considered uncorrelated, has the recursive expression:

b_k＝b_k-1+w_bk

in the joint estimation based on the adaptive UKF, the fault parameters of the system propeller are added to the real state of the system to form an extended state matrix, namelyThe system equations are organized into extended system state equations of the form:

$x_{k+1}^a=\tilde{f}(x_k^a,u_k)+w_k^a$

in the formula,for an extended systematic process noise vector, whereinIn order for the system process to noise the vector,representing the noise vector of the estimated thruster.

And the implementation process of the main filter on the extended state equation filtering estimation comprises the following steps:

mean of estimated quantities of random vectors at last momentSum variance P_k-1After nonlinear colorless transformation, a group of discrete Sigma points chi are obtained_k-1Calculating and updating the Sigma point through a system state equation, and then calculating and updating an extended state mean value of the Sigma pointSum variance P_k|k-1To extended state meanSum variance P_k|k-1Calculating to obtain a state mean value and a variance of one-step estimation; the mean value and variance of the state estimated in one step are then transformed by a colorless way and measured by a measurement equationCalculating and updating to obtain the average value of the observable state of the underwater robotIts varianceAnd covarianceAn estimated value of (d); acquisition of observable posture information y by reading sensor_kMean of co-spread statesSum variance P_k|k-1And an estimated value of the observable state and a gain matrix K_kCalculating and estimating to obtain a system mean valueVariance P_kEstimated values from which to derive the new information xi_k(ii) a According to the new information xi of the main filter_kThe measured value Sq is obtained through calculation_kAs the observed signal of the auxiliary filter, and then follows the auxiliary filter program step.

Referring to fig. 2, the adaptive filter estimation algorithm is based on the basic kalman filter algorithm, and performs recursion and update of the state and error covariance of the nonlinear system through nonlinear colorless transformation.

Colorless transformation of nonlinear systems:

the colorless transformation is the basis for realizing the self-adaptive UKF algorithm and is the essential characteristic different from other nonlinear filtering. The fundamental principle of the colorless Transform (UT) is to approximate the probability distribution of a random variable with the distribution of sampling points, generate a batch of discrete sampling points with the same probability statistical properties as the estimated quantity, called Sigma points, from the "a priori" mean and variance of the estimated quantity, and generate the mean and variance of the "a posteriori" from the Sigma points passed through the nonlinear equation.

If the non-linear function y ═ j (x) is known, whereIs a random vector whose mean and variance are respectivelyAnd P_xAveraging ySum variance P_yThe colorless conversion steps are as follows:

generation of Sigma dots

According to the mean value of the random vector xSum variance P_xA set of rules can be constructedThe symmetric distribution of discrete Sigma points around it can be recorded as χ_iI 1, …, 2l correspond to the respective Sigma points. The approximate distribution of the random vector x can be approximated by this set of Sigma points as follows:

<math> <mrow> <msub> <mi>&chi;</mi> <mn>0</mn> </msub> <mo>=</mo> <mover> <mi>x</mi> <mo>&OverBar;</mo> </mover> </mrow> </math>

<math> <mrow> <msub> <mi>&chi;</mi> <mi>i</mi> </msub> <mo>=</mo> <mover> <mi>x</mi> <mo>&OverBar;</mo> </mover> <mo>-</mo> <msub> <mrow> <mo>(</mo> <msqrt> <mrow> <mo>(</mo> <mi>l</mi> <mo>+</mo> <mi>&lambda;</mi> <mo>)</mo> </mrow> <msub> <mi>P</mi> <mi>x</mi> </msub> </msqrt> <mo>)</mo> </mrow> <mi>i</mi> </msub> <mi>i</mi> <mo>=</mo> <mn>1</mn> <mo>,</mo> <mo>.</mo> <mo>.</mo> <mo>.</mo> <mo>,</mo> <mi>l</mi> </mrow> </math>

<math> <mrow> <msub> <mi>&chi;</mi> <mi>i</mi> </msub> <mo>=</mo> <mover> <mi>x</mi> <mo>&OverBar;</mo> </mover> <mo>+</mo> <msub> <mrow> <mo>(</mo> <msqrt> <mrow> <mo>(</mo> <mi>l</mi> <mo>+</mo> <mi>&lambda;</mi> <mo>)</mo> </mrow> <msub> <mi>P</mi> <mi>x</mi> </msub> </msqrt> <mo>)</mo> </mrow> <mrow> <mi>i</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mi>i</mi> <mo>=</mo> <mi>l</mi> <mo>+</mo> <mn>1</mn> <mo>,</mo> <mo>.</mo> <mo>.</mo> <mo>.</mo> <mo>,</mo> <mn>2</mn> <mi>l</mi> </mrow> </math>

in the formula, x_iThe Sigma points are subjected to colorless conversion; l is the dimension of the extended state of the state and noise components, and λ is the scale constant that controls the distance from the Sigma point to the mean. The distribution of the random vectors is approximated by this set of Sigma points, which have the same mean and variance as x.

Computing a non-linear function

γ_i＝j(χ_i)i＝0，1，...，2l

By gamma_iThe distribution of the non-linear function is approximated.

Calculating the mean and variance of the nonlinear function y:

<math> <mrow> <mover> <mi>y</mi> <mo>&OverBar;</mo> </mover> <mo>=</mo> <munderover> <mi>&Sigma;</mi> <mrow> <mi>i</mi> <mo>=</mo> <mn>0</mn> </mrow> <mrow> <mn>2</mn> <mi>l</mi> </mrow> </munderover> <msubsup> <mi>d</mi> <mi>i</mi> <mi>m</mi> </msubsup> <msub> <mi>&gamma;</mi> <mi>i</mi> </msub> </mrow> </math>

<math> <mrow> <msub> <mi>P</mi> <mi>y</mi> </msub> <mo>=</mo> <munderover> <mi>&Sigma;</mi> <mrow> <mi>i</mi> <mo>=</mo> <mn>0</mn> </mrow> <mrow> <mn>2</mn> <mi>l</mi> </mrow> </munderover> <msubsup> <mi>d</mi> <mi>i</mi> <mi>c</mi> </msubsup> <mrow> <mo>(</mo> <msub> <mi>&gamma;</mi> <mi>i</mi> </msub> <mo>-</mo> <mover> <mi>y</mi> <mo>&OverBar;</mo> </mover> <mo>)</mo> </mrow> <msup> <mrow> <mo>(</mo> <msub> <mi>&gamma;</mi> <mi>i</mi> </msub> <mo>-</mo> <mover> <mi>y</mi> <mo>&OverBar;</mo> </mover> <mo>)</mo> </mrow> <mi>T</mi> </msup> </mrow> </math>

in the formulaIs a weight coefficient, and

<math> <mrow> <msubsup> <mi>d</mi> <mn>0</mn> <mi>m</mi> </msubsup> <mo>=</mo> <mfrac> <mi>&lambda;</mi> <mrow> <mi>&lambda;</mi> <mo>+</mo> <mi>l</mi> </mrow> </mfrac> </mrow> </math>

<math> <mrow> <msubsup> <mi>d</mi> <mn>0</mn> <mi>c</mi> </msubsup> <mo>=</mo> <mfrac> <mi>&lambda;</mi> <mrow> <mi>&lambda;</mi> <mo>+</mo> <mi>l</mi> </mrow> </mfrac> <mo>+</mo> <mrow> <mo>(</mo> <mn>1</mn> <mo>-</mo> <msup> <mi>&alpha;</mi> <mn>2</mn> </msup> <mo>+</mo> <mi>&beta;</mi> <mo>)</mo> </mrow> </mrow> </math>

<math> <mrow> <msubsup> <mi>d</mi> <mi>i</mi> <mi>m</mi> </msubsup> <mo>=</mo> <msubsup> <mi>d</mi> <mi>i</mi> <mi>c</mi> </msubsup> <mo>=</mo> <mfrac> <mn>1</mn> <mrow> <mn>2</mn> <mrow> <mo>(</mo> <mi>l</mi> <mo>+</mo> <mi>&lambda;</mi> <mo>)</mo> </mrow> </mrow> </mfrac> <mi>i</mi> <mo>=</mo> <mn>1</mn> <mo>,</mo> <mo>.</mo> <mo>.</mo> <mo>.</mo> <mo>,</mo> <mn>2</mn> <mi>l</mi> </mrow> </math>

wherein, alpha controls the distribution range of Sigma points, the value area is more than or equal to 0 and less than or equal to 1, and 1 is generally selected; beta is a non-negative constant, the function of which is to make the transformed variance contain partial high-order information, and beta is 2 for Gaussian distribution; filter parameter λ ═ α²(l + e) -l, where e is a constant, which is related to the higher order term of the estimate, which is equal to or higher than the higher order term of order 4 as a function thereof, and usually the state estimate takes 0 and the parameter estimate takes 3-l.

Referring to fig. 2, a flow chart of the invention is shown; the implementation flow of the scheme in the embodiment is as follows:

the self-adaptive UKF main filter-standard UKF realizes the flow of the extended state equation filtering estimation: and (3) filter initialization:

reading the initial values of the state of the underwater robot system and the propeller fault parameters to form an expanded initial state matrix, wherein the initial conditions are as follows:

<math> <mrow> <msub> <mover> <mi>x</mi> <mo>&OverBar;</mo> </mover> <mn>0</mn> </msub> <mo>=</mo> <mi>E</mi> <mo>[</mo> <msub> <mi>x</mi> <mn>0</mn> </msub> <mo>]</mo> </mrow> </math>

<math> <mrow> <msub> <mi>P</mi> <mn>0</mn> </msub> <mo>=</mo> <mi>E</mi> <mo>[</mo> <mrow> <mo>(</mo> <msub> <mi>x</mi> <mn>0</mn> </msub> <mo>-</mo> <msub> <mover> <mi>x</mi> <mo>&OverBar;</mo> </mover> <mn>0</mn> </msub> <mo>)</mo> </mrow> <msup> <mrow> <mo>(</mo> <msub> <mi>x</mi> <mn>0</mn> </msub> <mo>-</mo> <msub> <mover> <mi>x</mi> <mo>&OverBar;</mo> </mover> <mn>0</mn> </msub> <mo>)</mo> </mrow> <mi>T</mi> </msup> <mo>]</mo> </mrow> </math>

extendedState matrixs₀Representing the initial pose information of the six-dimensional underwater robot under the geodetic coordinate system,representing velocity and angular velocity information under a six-dimensional underwater robot carrier coordinate system, b₀Fault information for a six degree of freedom thruster is presented. At the same time, setting the initial system process noise w₀And measuring the noise v₀，D^wAnd D^vProcess noise and measurement noise covariance matrices, respectively.

Obtaining the expansion state and the initial mean value of the underwater robot through filter initializationSum variance P₀。

As the filtering algorithm proceeds, the mean value of the estimated filtering at the last time k-1 is obtainedSum variance P_k-1(ii) a While the system noise w of the last time k-1 is to be obtained_k-1And measuring the noise v_k-1System noise w_k-1And measuring the noise v_k-1Is the diagonal element of the system noise variance matrix D estimated by the auxiliary filterMean value of expansion state of underwater robotSum variance P_k-1Constructing a set of relationships by means of a colorless transformationA discrete Sigma point that is symmetric and located in its vicinity, as shown by:

<math> <mrow> <msub> <mi>&chi;</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>=</mo> <mo>[</mo> <msub> <mover> <mi>x</mi> <mo>&OverBar;</mo> </mover> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>,</mo> <msub> <mover> <mi>x</mi> <mo>&OverBar;</mo> </mover> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>-</mo> <msqrt> <mrow> <mo>(</mo> <mi>l</mi> <mo>+</mo> <mi>&lambda;</mi> <mo>)</mo> </mrow> <msub> <mi>P</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> </msqrt> <mo>,</mo> <msub> <mover> <mi>x</mi> <mo>&OverBar;</mo> </mover> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>+</mo> <msqrt> <mrow> <mo>(</mo> <mi>l</mi> <mo>+</mo> <mi>&lambda;</mi> <mo>)</mo> </mrow> <msub> <mi>P</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> </msqrt> <mo>]</mo> </mrow> </math>

wherein l is the dimension of an expansion state formed by the system state and the propeller fault parameters, and lambda is a scale constant for controlling the distance from a Sigma point to a mean value.

The discrete Sigma point is calculated and updated through a system equation, and a fault model of the propeller is required to be included in the system equation of the underwater robot due to the joint estimation of the state of the underwater robot and the fault parameters of the propeller. Namely, the system equation formula of the extended reference model is shown as follows:

<math> <mrow> <mi>M</mi> <mover> <mi>s</mi> <mrow> <mo>&CenterDot;</mo> <mo>&CenterDot;</mo> </mrow> </mover> <mo>+</mo> <mi>C</mi> <mrow> <mo>(</mo> <mover> <mi>s</mi> <mo>&CenterDot;</mo> </mover> <mo>)</mo> </mrow> <mover> <mi>s</mi> <mo>&CenterDot;</mo> </mover> <mo>+</mo> <mi>D</mi> <mrow> <mo>(</mo> <mover> <mi>s</mi> <mo>&CenterDot;</mo> </mover> <mo>)</mo> </mrow> <mover> <mi>s</mi> <mo>&CenterDot;</mo> </mover> <mo>+</mo> <mi>G</mi> <mrow> <mo>(</mo> <mi>s</mi> <mo>)</mo> </mrow> <mo>=</mo> <mi>F</mi> <mrow> <mo>(</mo> <mi>u</mi> <mo>)</mo> </mrow> </mrow> </math>

the extended system equation formulation formed in the filtering process:

$x_{k+1}^a=\tilde{f}(x_k^a,u_k)+w_k^a$

the Sigma point formula after being updated by the system equation is:

<math> <mrow> <msubsup> <mi>&chi;</mi> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mo>*</mo> </msubsup> <mo>=</mo> <mi>f</mi> <mrow> <mo>(</mo> <msub> <mi>&chi;</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>,</mo> <msub> <mi>u</mi> <mi>k</mi> </msub> <mo>)</mo> </mrow> </mrow> </math>

then, the mean and variance formula of the updated Sigma point is calculated as:

<math> <mrow> <msub> <mover> <mi>x</mi> <mo>&OverBar;</mo> </mover> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>=</mo> <munderover> <mi>&Sigma;</mi> <mrow> <mi>i</mi> <mo>=</mo> <mn>0</mn> </mrow> <mrow> <mn>2</mn> <mi>l</mi> </mrow> </munderover> <msubsup> <mi>d</mi> <mi>i</mi> <mi>m</mi> </msubsup> <msubsup> <mi>&chi;</mi> <mrow> <mi>i</mi> <mo>,</mo> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mo>*</mo> </msubsup> </mrow> </math>

<math> <mrow> <msub> <mi>P</mi> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>=</mo> <munderover> <mi>&Sigma;</mi> <mrow> <mi>i</mi> <mo>=</mo> <mn>0</mn> </mrow> <mrow> <mn>2</mn> <mi>l</mi> </mrow> </munderover> <msubsup> <mi>d</mi> <mi>i</mi> <mi>c</mi> </msubsup> <mrow> <mo>(</mo> <msubsup> <mi>&chi;</mi> <mrow> <mi>i</mi> <mo>,</mo> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mo>*</mo> </msubsup> <mo>-</mo> <msub> <mover> <mi>x</mi> <mo>&OverBar;</mo> </mover> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> <msup> <mrow> <mo>(</mo> <msubsup> <mi>&chi;</mi> <mrow> <mi>i</mi> <mo>,</mo> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mo>*</mo> </msubsup> <mo>-</mo> <msub> <mover> <mi>x</mi> <mo>&OverBar;</mo> </mover> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> <mi>T</mi> </msup> <msup> <mrow> <mo>+</mo> <mi>Q</mi> </mrow> <mi>w</mi> </msup> </mrow> </math>

calculating the updated Sigma point of the extended state mean and variance by a calculation system equation to obtain the state mean and variance estimated in one step:

<math> <mrow> <msub> <mi>&chi;</mi> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>=</mo> <mo>[</mo> <msub> <mover> <mi>x</mi> <mo>&OverBar;</mo> </mover> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>,</mo> <msub> <mover> <mi>x</mi> <mo>&OverBar;</mo> </mover> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>-</mo> <msqrt> <mrow> <mo>(</mo> <mi>l</mi> <mo>+</mo> <mi>&lambda;</mi> <mo>)</mo> </mrow> <msub> <mi>P</mi> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> </msqrt> <mo>,</mo> <msub> <mover> <mi>x</mi> <mo>&OverBar;</mo> </mover> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>+</mo> <msqrt> <mrow> <mo>(</mo> <mi>l</mi> <mo>+</mo> <mi>&lambda;</mi> <mo>)</mo> </mrow> <msub> <mi>P</mi> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> </msqrt> <mo>]</mo> </mrow> </math>

the measurement equation calculates the updated Sigma point:

and (3) performing colorless transformation and measurement equation calculation updating on the state mean value and the variance estimated in the one step to obtain the observable positioning attitude information of the underwater robot:

and because only the pose information of the front six dimensions in the extended state of the underwater robot can be observed, the front six dimensions of the Sigma point generated in the previous step are calculated and updated. The measurement equation is shown in the following formula:

y_k＝x_k(1:6)+v_k

wherein x_k(1:6) pose information of the first six dimensions representing the extended state, v_kIs uncorrelated white noise; the Sigma point formula after calculation and update by the measurement equation is as follows:

γ_k|k-1＝h(χ_k|k-1)

the Sigma point formula for calculating the updated observable state mean and variance of the measurement equation is calculated as:

<math> <mrow> <msub> <mover> <mi>y</mi> <mo>&OverBar;</mo> </mover> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>=</mo> <munderover> <mi>&Sigma;</mi> <mrow> <mi>i</mi> <mo>=</mo> <mn>0</mn> </mrow> <mrow> <mn>2</mn> <mi>l</mi> </mrow> </munderover> <msubsup> <mi>d</mi> <mi>i</mi> <mi>m</mi> </msubsup> <msub> <mi>&gamma;</mi> <mrow> <mi>i</mi> <mo>,</mo> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> </mrow> </math>

<math> <mrow> <msub> <mi>P</mi> <mrow> <msub> <mover> <mi>y</mi> <mo>&OverBar;</mo> </mover> <mi>k</mi> </msub> <msub> <mover> <mi>y</mi> <mo>&OverBar;</mo> </mover> <mi>k</mi> </msub> </mrow> </msub> <mo>=</mo> <munderover> <mi>&Sigma;</mi> <mrow> <mi>i</mi> <mo>=</mo> <mn>0</mn> </mrow> <mrow> <mn>2</mn> <mi>l</mi> </mrow> </munderover> <msubsup> <mi>d</mi> <mi>i</mi> <mi>c</mi> </msubsup> <mrow> <mo>(</mo> <msub> <mi>&gamma;</mi> <mrow> <mi>i</mi> <mo>,</mo> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msub> <mrow> <mo>-</mo> <mover> <mi>y</mi> <mo>&OverBar;</mo> </mover> </mrow> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> <msup> <mrow> <mo>(</mo> <msub> <mi>&gamma;</mi> <mrow> <mi>i</mi> <mo>,</mo> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>-</mo> <msub> <mover> <mi>y</mi> <mo>&OverBar;</mo> </mover> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> <mi>T</mi> </msup> <mo>+</mo> <msup> <mi>Q</mi> <mi>v</mi> </msup> </mrow> </math>

<math> <mrow> <msub> <mi>P</mi> <mrow> <msub> <mover> <mi>x</mi> <mo>&OverBar;</mo> </mover> <mi>k</mi> </msub> <msub> <mover> <mi>y</mi> <mo>&OverBar;</mo> </mover> <mi>k</mi> </msub> </mrow> </msub> <mo>=</mo> <munderover> <mi>&Sigma;</mi> <mrow> <mi>i</mi> <mo>=</mo> <mn>0</mn> </mrow> <mrow> <mn>2</mn> <mi>l</mi> </mrow> </munderover> <msubsup> <mi>d</mi> <mi>i</mi> <mi>c</mi> </msubsup> <mrow> <mo>(</mo> <msub> <mi>&chi;</mi> <mrow> <mi>i</mi> <mo>,</mo> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msub> <mrow> <mo>-</mo> <mover> <mi>x</mi> <mo>&OverBar;</mo> </mover> </mrow> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> <msup> <mrow> <mo>(</mo> <msub> <mi>&gamma;</mi> <mrow> <mi>i</mi> <mo>,</mo> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>-</mo> <msub> <mover> <mi>y</mi> <mo>&OverBar;</mo> </mover> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> <mi>T</mi> </msup> </mrow> </math>

calculating a gain matrix of the UKF filter:

<math> <mrow> <msub> <mi>K</mi> <mi>k</mi> </msub> <mo>=</mo> <msub> <mi>P</mi> <mrow> <msub> <mover> <mi>x</mi> <mo>&OverBar;</mo> </mover> <mi>k</mi> </msub> <msub> <mover> <mi>y</mi> <mo>&OverBar;</mo> </mover> <mi>k</mi> </msub> </mrow> </msub> <msubsup> <mi>P</mi> <mrow> <msub> <mover> <mi>y</mi> <mo>&OverBar;</mo> </mover> <mi>k</mi> </msub> <msub> <mover> <mi>y</mi> <mo>&OverBar;</mo> </mover> <mi>k</mi> </msub> </mrow> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msubsup> </mrow> </math>

estimating the mean and variance of the system expansion state:

reading the sensor observation pose information y after data processing_kAnd estimating the mean value and the variance of the expansion state of the underwater robot on the basis of the above parameters, wherein the formula is as follows:

<math> <mrow> <msub> <mi>P</mi> <mi>k</mi> </msub> <mo>=</mo> <msub> <mi>P</mi> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>-</mo> <msub> <mi>K</mi> <mi>k</mi> </msub> <msub> <mi>P</mi> <mrow> <msub> <mover> <mi>y</mi> <mo>&OverBar;</mo> </mover> <mi>k</mi> </msub> <msub> <mover> <mi>y</mi> <mo>&OverBar;</mo> </mover> <mi>k</mi> </msub> </mrow> </msub> <msubsup> <mi>K</mi> <mi>k</mi> <mi>T</mi> </msubsup> </mrow> </math>

<math> <mrow> <msub> <mover> <mi>x</mi> <mo>&OverBar;</mo> </mover> <mi>k</mi> </msub> <mo>=</mo> <msub> <mover> <mi>x</mi> <mo>&OverBar;</mo> </mover> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>+</mo> <msub> <mi>K</mi> <mi>k</mi> </msub> <mrow> <mo>(</mo> <msub> <mi>y</mi> <mi>k</mi> </msub> <mo>-</mo> <msub> <mover> <mi>y</mi> <mo>&OverBar;</mo> </mover> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> </mrow> </math>

at the next instant, i.e. at instant k +1, the auxiliary filter program step is continued if there is a new measurement.

And (3) constructing an auxiliary filter state equation in the self-adaptive UKF algorithm:

noise variance matrix D diagonal element of underwater robot systemThe state equation of the auxiliary filter can be expressed as:

<math> <mrow> <msub> <mover> <mi>q</mi> <mo>&OverBar;</mo> </mover> <mi>k</mi> </msub> <mo>=</mo> <msub> <mi>f</mi> <mi>q</mi> </msub> <mrow> <mo>(</mo> <msub> <mover> <mi>q</mi> <mo>&OverBar;</mo> </mover> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> <mo>+</mo> <msub> <mi>w</mi> <msub> <mi>q</mi> <mi>k</mi> </msub> </msub> </mrow> </math>

whereinThe system noise in the auxiliary filter is zero mean Gaussian white noise which is zero mean Gaussian white noise;

due to the fact thatIs unknown, it can be considered as a noise-driven uncorrelated random drift vector, so the state equation of the auxiliary filter can be obtained:

<math> <mrow> <msub> <mover> <mi>q</mi> <mo>&OverBar;</mo> </mover> <mi>k</mi> </msub> <mo>=</mo> <msub> <mover> <mi>q</mi> <mo>&OverBar;</mo> </mover> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>+</mo> <msub> <mi>w</mi> <msub> <mi>q</mi> <mi>k</mi> </msub> </msub> <mo>.</mo> </mrow> </math>

constructing an auxiliary filter measurement equation in the self-adaptive UKF algorithm:

the auxiliary filter is the innovation xi of the main filter_kDiagonal elements of the variance matrixAs an observation signal of the system, the variance P is estimated in accordance with the main filter in one step_k|k-1Variance P of the main filter extension state_kGain matrix K of the main filter UKF_kThe observation equations that make up the auxiliary filter are:

<math> <mrow> <mover> <mi>S</mi> <mo>&OverBar;</mo> </mover> <msub> <mi>q</mi> <mi>k</mi> </msub> <mo>=</mo> <mi>g</mi> <mrow> <mo>(</mo> <msub> <mover> <mi>q</mi> <mo>&OverBar;</mo> </mover> <mi>k</mi> </msub> <mo>)</mo> </mrow> <mo>=</mo> <mi>vdiag</mi> <mo>[</mo> <msup> <mrow> <mo>(</mo> <msubsup> <mi>K</mi> <mi>k</mi> <mi>T</mi> </msubsup> <msub> <mi>K</mi> <mi>k</mi> </msub> <mo>)</mo> </mrow> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> <msubsup> <mi>K</mi> <mi>k</mi> <mi>T</mi> </msubsup> <mrow> <mo>(</mo> <msub> <mi>P</mi> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>-</mo> <msub> <mi>P</mi> <mi>k</mi> </msub> <mo>)</mo> </mrow> <msub> <mi>K</mi> <mi>k</mi> </msub> <msup> <mrow> <mo>(</mo> <msubsup> <mi>K</mi> <mi>k</mi> <mi>T</mi> </msubsup> <msub> <mi>K</mi> <mi>k</mi> </msub> <mo>)</mo> </mrow> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> <mo>]</mo> </mrow> </math>

where vdiag is a vector of the main diagonal elements of the matrix within the middle brackets.

The auxiliary filter KF carries out a filtering estimation process on the process noise of the system:

estimating the diagonal elements of the system noise variance matrix of the auxiliary filter at the last momentAnd the variance thereofState equation pair using auxiliary filterAndobtaining diagonal elements after calculation updatingAnd the variance thereofThe one-step estimation value of (1); using the measurement equation of the auxiliary filter to determine the variance P of the primary filter in one step_k|k-1Extended state variance P_kAnd a gain matrix K_kObtaining estimated observed value after calculation and updateThe obtained one-step estimation value and observation valueThe measured value of the same main filter is calculated and estimated to obtain the estimated value of diagonal elements of the system process noise variance matrix DAnd the variance thereofWhen new observation pose information y exists_kThen continue the next oneAnd controlling the periodic main filter program step.

With reference to figure 2 of the drawings,

implementation procedure of adaptive UKF auxiliary filter KF (taking process noise of estimation system as example):

and (3) filter initialization:

reading initial values and variances of diagonal elements of a process noise variance matrix of the underwater robot system, wherein the initial conditions are as follows:

<math> <mrow> <msub> <mover> <mi>q</mi> <mo>&OverBar;</mo> </mover> <mn>0</mn> </msub> <mo>=</mo> <mi>E</mi> <mo>[</mo> <msub> <mi>q</mi> <mn>0</mn> </msub> <mo>]</mo> </mrow> </math>

<math> <mrow> <msub> <mi>P</mi> <msub> <mi>q</mi> <mn>0</mn> </msub> </msub> <mo>=</mo> <mi>E</mi> <mo>[</mo> <mrow> <mo>(</mo> <msub> <mi>q</mi> <mn>0</mn> </msub> <mo>-</mo> <msub> <mover> <mi>q</mi> <mo>&OverBar;</mo> </mover> <mn>0</mn> </msub> <mo>)</mo> </mrow> <msup> <mrow> <mo>(</mo> <msub> <mi>q</mi> <mn>0</mn> </msub> <mo>-</mo> <msub> <mover> <mi>q</mi> <mo>&OverBar;</mo> </mover> <mn>0</mn> </msub> <mo>)</mo> </mrow> <mi>T</mi> </msup> <mo>]</mo> </mrow> </math>

obtaining initial values of diagonal elements of process noise variance matrix of underwater robot system through filtering initializationAnd its varianceAs the filtering algorithm proceeds, it is here obtained that the last time instant k-1 is estimatable to be filteredSum variance

Process noise variance matrix diagonal elements from underwater robot systemThe state equation of the auxiliary filter can be expressed as:

<math> <mrow> <msub> <mover> <mi>q</mi> <mo>&OverBar;</mo> </mover> <mi>k</mi> </msub> <mo>=</mo> <msub> <mi>f</mi> <mi>q</mi> </msub> <mrow> <mo>(</mo> <msub> <mover> <mi>q</mi> <mo>&OverBar;</mo> </mover> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> <mo>+</mo> <msub> <mi>w</mi> <msub> <mi>q</mi> <mi>k</mi> </msub> </msub> </mrow> </math>

whereinRepresenting zero mean white gaussian noise for system noise in the auxiliary filter;

due to the fact thatIs unknown, and can be regarded as a noise-driven uncorrelated random drift vector, so that the diagonal elements of the noise variance matrix D of the underwater robot system estimated at the previous momentAnd its varianceThe formula updated by the calculation of the auxiliary filter state equation is:

<math> <mrow> <msub> <mover> <mi>q</mi> <mo>&OverBar;</mo> </mover> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>=</mo> <msub> <mover> <mi>q</mi> <mo>&OverBar;</mo> </mover> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>+</mo> <msub> <mi>w</mi> <msub> <mi>q</mi> <mi>k</mi> </msub> </msub> </mrow> </math>

$P_{q_{k|k-1}}=P_{q_{k-1}}+Q_q^w;$

wherein,is a noise variance matrix in the auxiliary filter byEstimating D of the main filter^wAnd set.

Of auxiliary filtersThe measurement equation is:

<math> <mrow> <mover> <mi>S</mi> <mo>&OverBar;</mo> </mover> <msub> <mi>q</mi> <mi>k</mi> </msub> <mo>=</mo> <mi>g</mi> <mrow> <mo>(</mo> <msub> <mover> <mi>q</mi> <mo>&OverBar;</mo> </mover> <mi>k</mi> </msub> <mo>)</mo> </mrow> <mo>=</mo> <mi>vdiag</mi> <mo>[</mo> <msup> <mrow> <mo>(</mo> <msubsup> <mi>K</mi> <mi>k</mi> <mi>T</mi> </msubsup> <msub> <mi>K</mi> <mi>k</mi> </msub> <mo>)</mo> </mrow> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> <msubsup> <mi>K</mi> <mi>k</mi> <mi>T</mi> </msubsup> <mrow> <mo>(</mo> <msub> <mi>P</mi> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>-</mo> <msub> <mi>P</mi> <mi>k</mi> </msub> <mo>)</mo> </mrow> <msub> <mi>K</mi> <mi>k</mi> </msub> <msup> <mrow> <mo>(</mo> <msubsup> <mi>K</mi> <mi>k</mi> <mi>T</mi> </msubsup> <msub> <mi>K</mi> <mi>k</mi> </msub> <mo>)</mo> </mrow> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> <mo>]</mo> </mrow> </math>

where vdiag represents a vector formed by taking the major diagonal elements of the matrix in brackets, letIt is possible to obtain:

<math> <mrow> <mover> <mi>S</mi> <mo>&OverBar;</mo> </mover> <msub> <mi>q</mi> <mi>k</mi> </msub> <mo>=</mo> <mi>vdiag</mi> <mo>[</mo> <msub> <mi>KP</mi> <mi>k</mi> </msub> <mrow> <mo>(</mo> <munderover> <mi>&Sigma;</mi> <mrow> <mi>i</mi> <mo>=</mo> <mn>0</mn> </mrow> <mrow> <mn>2</mn> <mi>n</mi> </mrow> </munderover> <msubsup> <mi>d</mi> <mi>i</mi> <mi>c</mi> </msubsup> <mrow> <mo>(</mo> <msubsup> <mi>&chi;</mi> <mrow> <mi>i</mi> <mo>,</mo> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mo>*</mo> </msubsup> <mo>-</mo> <msub> <mover> <mi>x</mi> <mo>&OverBar;</mo> </mover> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> <msup> <mrow> <mo>(</mo> <msubsup> <mi>&chi;</mi> <mrow> <mi>i</mi> <mo>,</mo> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mo>*</mo> </msubsup> <mo>-</mo> <msub> <mover> <mi>x</mi> <mo>&OverBar;</mo> </mover> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> <mi>T</mi> </msup> <mo>+</mo> <msup> <mi>Q</mi> <mi>w</mi> </msup> <msub> <mrow> <mo>-</mo> <mi>P</mi> </mrow> <mi>k</mi> </msub> <mo>)</mo> </mrow> <mo>]</mo> </mrow> </math>

<math> <mrow> <mo>=</mo> <mi>vdiag</mi> <mo>[</mo> <msub> <mi>KP</mi> <mi>k</mi> </msub> <mrow> <mo>(</mo> <munderover> <mi>&Sigma;</mi> <mrow> <mi>i</mi> <mo>=</mo> <mn>0</mn> </mrow> <mrow> <mn>2</mn> <mi>n</mi> </mrow> </munderover> <msubsup> <mi>d</mi> <mi>i</mi> <mi>c</mi> </msubsup> <mrow> <mo>(</mo> <msubsup> <mi>&chi;</mi> <mrow> <mi>i</mi> <mo>,</mo> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mo>*</mo> </msubsup> <mo>-</mo> <msub> <mover> <mi>x</mi> <mo>&OverBar;</mo> </mover> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> <msup> <mrow> <mo>(</mo> <msubsup> <mi>&chi;</mi> <mrow> <mi>i</mi> <mo>,</mo> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mo>*</mo> </msubsup> <mo>-</mo> <msub> <mover> <mi>x</mi> <mo>&OverBar;</mo> </mover> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> <mi>T</mi> </msup> <mo>-</mo> <msub> <mi>P</mi> <mi>k</mi> </msub> <mo>)</mo> </mrow> <mo>]</mo> <mo>+</mo> <mi>vdiag</mi> <mrow> <mo>(</mo> <msub> <mi>KP</mi> <mi>k</mi> </msub> <msup> <mi>Q</mi> <mi>w</mi> </msup> <msubsup> <mi>KP</mi> <mi>k</mi> <mi>T</mi> </msubsup> <mo>)</mo> </mrow> </mrow> </math>

$={\mathrm{Bp}}_k+\mathrm{vdiag}\left({\mathrm{KP}}_k{Q}^w{\mathrm{KP}}_k^T\right)$

<math> <mrow> <mo>=</mo> <msub> <mi>Bp</mi> <mi>k</mi> </msub> <mo>+</mo> <msub> <mi>HP</mi> <mi>k</mi> </msub> <mo>&CenterDot;</mo> <msub> <mi>q</mi> <mi>k</mi> </msub> </mrow> </math>

wherein, Bp_kIs a constant value vector; HP_kIs a constant matrix. If KP_k∈R^mq×nqAnd, in addition,

due to Q^wFor diagonal matrix, HP can be obtained_k∈R^mq×nqAnd is provided with

Computing a gain matrix for the auxiliary filter:

<math> <mrow> <msub> <mi>K</mi> <msub> <mi>q</mi> <mi>k</mi> </msub> </msub> <mo>=</mo> <msub> <mi>P</mi> <msub> <mi>q</mi> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> </msub> <mo>&CenterDot;</mo> <msubsup> <mi>HP</mi> <mi>k</mi> <mi>T</mi> </msubsup> <msup> <mrow> <mo>(</mo> <msub> <mi>HP</mi> <mi>k</mi> </msub> <mo>&CenterDot;</mo> <msub> <mi>P</mi> <msub> <mi>q</mi> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> </msub> <mo>&CenterDot;</mo> <msubsup> <mi>HP</mi> <mi>k</mi> <mi>T</mi> </msubsup> <mo>+</mo> <msubsup> <mi>Q</mi> <mi>q</mi> <mi>v</mi> </msubsup> <mo>)</mo> </mrow> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> </mrow> </math>

according to the real-time information xi of the main filter_kCalculating the measurement value of the auxiliary filter:

<math> <mrow> <msub> <mi>Sq</mi> <mi>k</mi> </msub> <mo>=</mo> <mi>g</mi> <mrow> <mo>(</mo> <msub> <mi>q</mi> <mi>k</mi> </msub> <mo>)</mo> </mrow> <mo>=</mo> <mi>vdiag</mi> <mrow> <mo>(</mo> <mfrac> <mn>1</mn> <mi>N</mi> </mfrac> <munderover> <mi>&Sigma;</mi> <mrow> <mi>i</mi> <mo>=</mo> <mi>k</mi> <mo>-</mo> <mi>N</mi> <mo>+</mo> <mn>1</mn> </mrow> <mi>k</mi> </munderover> <msub> <mi>&xi;</mi> <mi>k</mi> </msub> <msubsup> <mi>&xi;</mi> <mi>k</mi> <mi>T</mi> </msubsup> <mo>)</mo> </mrow> </mrow> </math>

wherein,y_kis the observation pose information of the main filter at the moment k,is the mean value of observable states。

Estimating diagonal elements and variance of a process noise variance matrix of the underwater robot system at the moment k:

reading the measurement Sq of the auxiliary filter_kEstimating diagonal elements of process noise variance matrix of underwater robot system based on the aboveSum varianceAs shown in the formula:

<math> <mrow> <msub> <mi>P</mi> <msub> <mi>q</mi> <mi>k</mi> </msub> </msub> <mo>=</mo> <mrow> <mo>(</mo> <mi>I</mi> <mo>-</mo> <msub> <mi>K</mi> <msub> <mi>q</mi> <mi>k</mi> </msub> </msub> <mo>&CenterDot;</mo> <msub> <mi>HP</mi> <mi>k</mi> </msub> <mo>)</mo> </mrow> <msub> <mi>P</mi> <msub> <mi>q</mi> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> </msub> </mrow> </math>

<math> <mrow> <msub> <mover> <mi>q</mi> <mo>&OverBar;</mo> </mover> <mi>k</mi> </msub> <mo>=</mo> <msub> <mover> <mi>q</mi> <mo>&OverBar;</mo> </mover> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>+</mo> <msub> <mi>K</mi> <msub> <mi>q</mi> <mi>k</mi> </msub> </msub> <mrow> <mo>(</mo> <msub> <mi>Sq</mi> <mi>k</mi> </msub> <mo>-</mo> <msub> <mrow> <mover> <mi>S</mi> <mo>&OverBar;</mo> </mover> <mi>q</mi> </mrow> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> </mrow> </math>

at the next moment, if there is information xi_k+1Repeating the step of the auxiliary filter; and when the pose information of the new observed value is read, continuing the next control period program step.

Claims (6)

1. A state and parameter joint estimation method of an underwater robot based on self-adaptive UKF is characterized by comprising the following steps:

modeling a propeller fault on the basis of an underwater robot dynamics model to obtain an offline extended reference model; on-line pose information y detected by pose sensor_kThe method adopts a main filter of a self-adaptive UKF as a standard UKF filtering estimation algorithm, combines an off-line extended reference model of the underwater robot as an extended system state equation, and transmits and combines an extended state formed by a system state and a propeller faultUpdating, estimating the state and parameter information of the system in real time to obtain the mean value of the estimated result value of the systemSum variance P_k(ii) a Where the state information of the system includes filtered pose information s_kAnd real-time estimated speed informationThe parameter information is a fault parameter b of the system propeller_kAnd, at the same time, calculating estimated innovation information xi according to the main filter_kCalculating the measurement Sq of the auxiliary filter_k(ii) a Establishing a state equation and a measurement equation of the UKF auxiliary filter according to the measured value Sq_kAs an observation signal of the system, the noise information of the underwater robot system is estimated in real time by adopting an auxiliary filter KF of the self-adaptive UKF to obtain a diagonal element of a system noise covariance matrix DSum varianceAn estimate of (d); where the noise information includes process noise w_kAnd measuring the noise v_kTheir covariance matrices are respectively D^wAnd D^v；

And (3) a main filter expansion system equation forming process:

when the nonlinear system is expanded, the fault parameter b of the unknown/time-varying propeller is contained_kThe system state space equation is as follows:

x_k+1＝f(x_k,b_k,u_k)+w_k

b_k+1＝f_b(x_k,b_k,u_k)+w_kb

in the formula, b_kIs an unknown/time-varying propeller fault parameter at time k, w_kbSystem thruster fault noise vector as a fault parameter modelQuantity, gaussian noise with zero mean;

when the fault parameter b_kIf the change rule is unknown, b_kThe random drift vector, considered uncorrelated, has the recursive expression:

b_k＝b_k-1+w_bk

based on the joint estimation of the self-adaptive UKF, the fault parameters of the system propeller are added to the real state of the system to form an extended state matrix, namelyThe system equation is formed into an extended system state equation as follows:

$x_{k+1}^a=\tilde{f}(x_k^a,u_k)+w_k^a$

in the formula,for an extended systematic process noise vector, whereinIn order for the system process to noise the vector,to estimate a propeller fault noise vector.

2. According to the claimsSolving 1 the underwater robot state and parameter joint estimation method based on the self-adaptive UKF is characterized in that: extending fault parameters b in reference model_kIs/are as followsAnd expressing the efficiency loss factor of the ith propeller at the moment k, and taking a numerical value between 0 and 1 to express the failure degree of the propeller fault.

3. The underwater robot state and parameter joint estimation method based on the adaptive UKF as claimed in claim 1, wherein: and (3) constructing an auxiliary filter state equation in the self-adaptive UKF algorithm:

noise variance matrix diagonal elements from underwater robot systemThe state equation of the auxiliary filter can be expressed as:

<math> <mrow> <msub> <mover> <mi>q</mi> <mo>&OverBar;</mo> </mover> <mi>k</mi> </msub> <mo>=</mo> <msub> <mi>f</mi> <mi>q</mi> </msub> <mrow> <mo>(</mo> <msub> <mover> <mi>q</mi> <mo>&OverBar;</mo> </mover> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> <mo>+</mo> <msub> <mi>w</mi> <msub> <mi>q</mi> <mi>k</mi> </msub> </msub> </mrow> </math>

whereinThe system noise vector in the auxiliary filter is zero mean Gaussian white noise;

due to the fact thatIs unknown and is considered as a noise-driven uncorrelated random drift vector, so the state equation of the auxiliary filter can be obtained:

<math> <mrow> <msub> <mover> <mi>q</mi> <mo>&OverBar;</mo> </mover> <mi>k</mi> </msub> <mo>=</mo> <msub> <mover> <mi>q</mi> <mo>&OverBar;</mo> </mover> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>+</mo> <msub> <mi>w</mi> <msub> <mi>q</mi> <mi>k</mi> </msub> </msub> <mo>.</mo> </mrow> </math>

4. the underwater robot state and parameter joint estimation method based on the adaptive UKF as claimed in claim 1, wherein: constructing an auxiliary filter measurement equation in the self-adaptive UKF algorithm:

with innovation xi of the main filter_kMeasurement Sq of the variance matrix_kAs an observation signal of the system, the variance P is estimated in accordance with the main filter in one step_k|k-1Variance P of the main filter extension state_kGain matrix K of the main filter UKF_kThe measurement equations that make up the auxiliary filter are:

<math> <mrow> <msub> <mover> <mi>S</mi> <mo>&OverBar;</mo> </mover> <msub> <mi>q</mi> <mi>k</mi> </msub> </msub> <mo>=</mo> <mi>g</mi> <mrow> <mo>(</mo> <msub> <mover> <mi>q</mi> <mo>&OverBar;</mo> </mover> <mi>k</mi> </msub> <mo>)</mo> </mrow> <mo>=</mo> <mi>vdiag</mi> <mo>[</mo> <msup> <mrow> <mo>(</mo> <msubsup> <mi>K</mi> <mi>k</mi> <mi>T</mi> </msubsup> <msub> <mi>K</mi> <mi>k</mi> </msub> <mo>)</mo> </mrow> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> <msubsup> <mi>K</mi> <mi>k</mi> <mi>T</mi> </msubsup> <mrow> <mo>(</mo> <msub> <mi>P</mi> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>-</mo> <msub> <mi>P</mi> <mi>k</mi> </msub> <mo>)</mo> </mrow> <msub> <mi>K</mi> <mi>k</mi> </msub> <msup> <mrow> <mo>(</mo> <msubsup> <mi>K</mi> <mi>k</mi> <mi>T</mi> </msubsup> <msub> <mi>K</mi> <mi>k</mi> </msub> <mo>)</mo> </mrow> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> <mo>]</mo> </mrow> </math>

where vdiag is a vector made up of the major diagonal elements of the matrix within the middle brackets.

5. The underwater robot state and parameter joint estimation method based on the adaptive UKF as claimed in claim 1, wherein: and the implementation process of the main filter on the extended state equation filtering estimation comprises the following steps:

mean of estimated quantities of random vectors at last momentSum variance P_k-1After nonlinear colorless transformation, a group of discrete Sigma points chi are obtained_k-1Calculating and updating the Sigma point through a system state equation, and then calculating and updating an extended state mean value of the Sigma pointSum variance P_k|k-1To extended state meanSum variance P_k|k-1Calculating to obtain a state mean value and a variance of one-step estimation; and performing colorless transformation and calculation and update by using a measurement equation on the state mean value and the variance estimated in one step to obtain the mean value of the observable state of the underwater robotVariance (variance)And covarianceAn estimated value of (d); acquisition of observable posture information y by reading sensor_kMean of co-spread statesSum variance P_k|k-1And an estimated value of the observable state and a gain matrix K_kCalculating and estimating to obtain a system mean valueVariance P_kTo derive from it an estimate of xi_k(ii) a According to the new information xi of the main filter_kThe measured value Sq is obtained through calculation_kAs the observed signal of the auxiliary filter, and then follows the auxiliary filter program step.

6. The underwater robot state and parameter joint estimation method based on the adaptive UKF as claimed in claim 1 or 5, wherein: the auxiliary filter KF carries out a filtering estimation process on the system process noise:

last time estimation to obtain auxiliary filterDiagonal elements of the system noise variance matrixSum varianceState equation pair using auxiliary filterAndobtaining diagonal elements after calculation updatingSum varianceThe one-step estimation value of (1); using the measurement equation of the auxiliary filter to determine the variance P of the primary filter in one step_k|k-1Extended state variance P_kAnd a gain matrix K_kObtaining estimated observed value after calculation and updateThe obtained one-step estimation value and observation valueMeasured value Sq of same main filter_kThe calculated estimation value is used for obtaining the estimation value of the diagonal element of the system process noise variance matrix DSum varianceWhen new observation pose information y exists_kThen, the main filter pass of the next control period is continuedAnd (5) sequence steps.