CN102795323A - Unscented Kalman filter (UKF)-based underwater robot state and parameter joint estimation method - Google Patents

Abstract

The invention discloses an unscented Kalman filter (UKF)-based underwater robot state and parameter joint estimation method. According to the method, expansion reference models of an underwater robot are established, and comprise a kinetic model of the underwater robot and a fault model of a propeller. According to pose information detected by a position sensor, the expansion reference models are subjected to on-line joint estimation through states of the underwater robot, including pose and speed, and propeller fault parameters by a UKF algorithm, and the speed information of the underwater robot and the propeller fault information are estimated in real time. The method has a high real-time property, and the states and parameters of a system can be subjected to joint estimation; and under the condition that prior information of process noise and measurement noise is known, high estimation accuracy can be achieved by the method.

Description

A kind of under-water robot state and parametric joint method of estimation based on UKF

Technical field

The present invention relates to a kind of based on colourless Kalman filtering (UKF; Unscented Kalman Filter) under-water robot state and parametric joint method of estimation, particularly a kind of under-water robot state and propelling unit fault parameter combined estimation method based on UKF.

Technical background

Along with developing rapidly of ocean exploitation cause, underwater construction and construction project are more and more, and be also increasingly high for the performance requriements of underwater effect means.Because under-water robot can be observed under water, photographs, salvaging and construction operation, therefore in ocean exploitation, is widely used.Constantly full-fledged along with under-water robot technology; Its task mission also becomes increasingly complex; The residing environment of under-water robot comprises that mainly self behavioural environment, outside natural environment, targeted environment etc. are complicated unknown under many circumstances; So improve the capacity of will of under-water robot,, can adapt to dynamic environment complicated and changeable again simultaneously to guarantee the under-water robot mission of successfully finishing the work.

Angle of rake health status determining under-water robot can be fine the task of completion expection.The under-water robot of tradition pre-programmed generally takes to throw the measures such as come-up of carrying detecting when propelling unit has fault to take place; In fact under many circumstances; The propelling unit of under-water robot is not entirely ineffective; If can be real-time pick out its loss in efficiency factor, then thrust is redistributed, under-water robot still possibly accomplished the task of expection.

Application number is that (open day: disclosed " diagnosing information fusion fault of underwater robot propeller method and device " provided according to propelling unit rotating speed of motor current signal and voltage signal and come the device to underwater robot propeller trouble diagnosing and fault size identification on March 17th, 2010) for 200810042803.8 Chinese patent file.

Summary of the invention

To the problem of above-mentioned existence, the invention provides a kind of can be in real time detection information and to the under-water robot state and the parametric joint method of estimation based on UKF of its high precision estimation automatically.

For realizing the object of the invention, the present invention adopts following scheme:

A kind of under-water robot state and parametric joint method of estimation based on UKF comprise the steps: to set up the extended reference model of under-water robot off-line; Online posture information y according to the detection of pose sensor _kAdopt UKF Filtering Estimation algorithm, the extended reference model of robot off-line is as the system state equation of expansion, to the extended mode transmission and the renewal of state of the system and propelling unit fault composition under the unfree water; Estimate the state and the parameter information of system in real time, obtain the end value of system's estimation And P _kHere the status information of system comprises through filtered posture information q _kThe velocity information of coming out with real-time estimation

Parameter information is the angle of rake fault parameter b of system _k

The formation of the extended reference model of off-line:

The kinetic model of under-water robot is represented as follows:

$M\stackrel{\·\·}{q}+C\left(\stackrel{\·}{q}\right)\stackrel{\·}{q}+D\left(\stackrel{\·}{q}\right)\stackrel{\·}{q}+G\left(q\right)=u$

In the formula; M is the inertial matrix of 6 * 6 dimensions;

comprises 6 * 6 dimension matrixes of centnifugal force and coriolis force;

is the hydrodynamic force matrix of 6 * 6 dimensions; G (q) comprises 6 * 1 dimension matrixes of gravity and buoyancy, and q and u are respectively the state and the motor torque vector of 6 * 1 dimensions;

Show as the variation that system motor torque vector u imports according to angle of rake fault, the extended reference model representation that obtains under-water robot is following:

$M\stackrel{\·\·}{q}+C\left(\stackrel{\·}{q}\right)\stackrel{\·}{q}+D\left(\stackrel{\·}{q}\right)\stackrel{\·}{q}+G\left(q\right)=F\left(u\right)$

F in the formula (u) is angle of rake failure function, defines 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 following formula, u _kBe 6 * 1 dimension motor torque vector matrixs, b _kBe 6 * 1 dimension fault parameter matrixes,

Be expressed as the k angle of rake loss in efficiency factor constantly, subscript i representes i propelling unit, when

The time, represent that i propelling unit is normal; When

The time, represent that rigid fault has taken place i propelling unit, promptly loss factor is 100%; When

The time, represent that soft fault has taken place i propelling unit, i.e. partial failure;

Described UKF Filtering Estimation algorithm is to be framework with basic Kalman filtering algorithm, carries out the recursion and the renewal of the state and the error covariance of NLS through nonlinear colourless conversion.

Adopt the implementation procedure of UKF Filtering Estimation:

To last one constantly is k-1 by by the average of estimator " priori "

With variance P _K-1The one group of discrete Sigma point X that obtains through non-linear colourless conversion _K-1, X _K-1Calculate the Sigma point that obtains dispersing after the renewal through system state equation

Right

Calculate the extended mode average of step estimation

With variance P _K|k-1Right

And P _K|k-1Carry out colourless variation again and obtain one group of discrete Sugma point X _K|k-1, X _K|k-1Through measuring the Sigma point γ that obtains dispersing after Equation for Calculating is upgraded _K|k-1, to γ _K|k-1But calculate the average of under-water robot observer state

And variance

Estimated valve; But the estimated valve and the sensor acquisition observed value y of a resulting step estimated valve and the observer state that obtains _k, estimate to obtain system's estimated value through calculating

And P _kIf new observed reading y is arranged _k, repeat above-mentioned steps.

Estimation to the under-water robot parameter is to estimate to realize that its step is following through uniting:

The associating estimation promptly adopts the same method to the state of system

With angle of rake fault parameter b _kUnite estimation simultaneously, in uniting estimation, with angle of rake fault parameter b _kAs the motion vector of system, append state vector in reality

After, form the state vector of expanding Re-use UKF and its extended reference model is estimated the state vector that is expanded Estimated result, and then can obtain propelling unit fault parameter b _kEstimated valve.

The present invention compared with prior art beneficial effect is following:

1. the present invention realizes simply, can on existing under-water robot carrier, directly use this method, need not carry out any change to the hardware system of under-water robot, also need not increase any hardware device.This invention can embed in the existing thread of under-water robot, concerning under-water robot, only is the calculated amount that has increased system.

2. method of estimation of the present invention; Be detection information, adopt the UKF algorithm, real-time estimation under-water robot state and angle of rake fault parameter information according to underwater robot sensor; These information are that the fault-tolerant control of under-water robot provides foundation; Can be real-time pick out its loss in efficiency factor, and then thrust is redistributed, still possibly accomplish the task of expection to guarantee under-water robot.

3. estimated accuracy of the present invention is high, can online state and parameter to system unite estimation.Under the known situation of process noise and the prior imformation of measuring noise, this method can reach high estimation accuracy; Because UT acts directly on the non-linear dynamic model, need not carry out linearization, so avoided the linearization error that produces in the NLS linearization procedure to it.

Description of drawings

Fig. 1 is the realization schematic diagram of this inventive method;

Fig. 2 is the Filtering Estimation procedure chart of UKF algorithm;

Fig. 3 is the estimation diagram of circuit of this invention.

The specific embodiment

Below in conjunction with accompanying drawing and embodiment the present invention program is described in further detail:

Referring to accompanying drawing 1, for the inventive method realizes schematic diagram.The extended reference model that comprises the steps: to set up the under-water robot off-line based on under-water robot state and the parametric joint method of estimation of UKF; Online posture information y according to the detection of pose sensor _kAdopt UKF Filtering Estimation algorithm, the extended reference model of robot off-line is the system state equation of expansion under the unfree water, to the extended mode transmission and the renewal of state of the system and propelling unit fault composition; The state and the parameter information of the real-time system that estimates obtain the end value that system is estimated And P _kHere the status information of system comprises through filtered posture information q _kThe velocity information of coming out with real-time estimation

Parameter information is the angle of rake fault parameter b of system _k

The formation of the extended reference model of off-line:

The kinetic model of under-water robot is represented as follows:

$M\stackrel{\·\·}{q}+C\left(\stackrel{\·}{q}\right)\stackrel{\·}{q}+D\left(\stackrel{\·}{q}\right)\stackrel{\·}{q}+G\left(q\right)=u$

In the formula; M is the inertial matrix of 6 * 6 dimensions; comprises 6 * 6 dimension matrixes of centnifugal force and coriolis force;

is the hydrodynamic force matrix of 6 * 6 dimensions; G (q) comprises 6 * 1 dimension matrixes of gravity and buoyancy, and q and u are respectively the state and the motor torque vector of 6 * 1 dimensions;

On the basis of the kinetic model of robot, show as the variation that system motor torque vector u imports according to angle of rake fault under water, the extended reference model representation that obtains under-water robot is following:

$M\stackrel{\·\·}{q}+C\left(\stackrel{\·}{q}\right)\stackrel{\·}{q}+D\left(\stackrel{\·}{q}\right)\stackrel{\·}{q}+G\left(q\right)=F\left(u\right)$

F in the formula (u) is angle of rake failure function, defines 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 following formula, u _kBe 6 * 1 dimension motor torque vector matrixs, b _kBe 6 * 1 dimension fault parameter matrixes,

Be expressed as the k angle of rake loss in efficiency factor constantly, subscript i representes i propelling unit, when

The time, represent that i propelling unit is normal; When

The time, represent that rigid fault has taken place i propelling unit, promptly loss factor is 100%; When

The time, represent that soft fault has taken place i propelling unit, i.e. partial failure;

Referring to accompanying drawing 2～3, described UKF Filtering Estimation algorithm is to be framework with basic Kalman filtering algorithm, carries out the recursion and the renewal of the state and the error covariance of NLS through nonlinear colourless conversion.

UKF Filtering Estimation implementation procedure:

To last one constantly is k-1 by by the average of estimator " priori "

With variance P _K-1The one group of discrete Sigma point X that obtains through non-linear colourless conversion _K-1, X _K-1Calculate the Sigma point that obtains dispersing after the renewal through system state equation Right

Calculate the extended mode average of step estimation

With variance P _K|k-1Right

And P _K|k-1Carry out colourless variation again and obtain one group of discrete Sigma point X _K|k-1, X _K|k-1Through measuring the Sigma point γ that obtains dispersing after Equation for Calculating is upgraded _K|k-1, to γ _K|k-1But calculate the average of under-water robot observer state

And variance

Estimated valve; But the estimated valve and the sensor acquisition observed value y of a resulting step estimated valve and the observer state that obtains _k, estimate to obtain system's estimated value through calculating And P _kIf new observed reading y is arranged _k, repeat above-mentioned steps.

The colourless conversion of NLS:

Colourless conversion is the basis that the UKF algorithm is realized, also is the essential characteristic that is different from other nonlinear filtering.Colourless conversion (UT; Unscented Transform) basic principle is the probability distribution of coming the approximate random variable with the distribution of sampling point; By last one constantly by estimator by by " priori " average and the variance of estimator, produce a collection of discrete and the sampling point that is had the equal probabilities statistical property by estimator, be called the Sigma point; According to the Sigma point after transmitting through nonlinear equation, generate the average and the variance of " posteriority ".

If known nonlinear function y=f (x), wherein

Be a random vector, its average and variance are respectively

And P _x, ask the average of y

With variance P _y, the step of its colourless conversion is following:

The generation that Sigma is ordered

Average according to random vector x With variance P _x, can construct one group about

Symmetry is asked and is distributed near its discrete Sigma point, can be designated as X _i, i=1 ..., respectively corresponding each Sigma point of 2l.Can be with the APPROXIMATE DISTRIBUTION of this group Sigma point approximate representation random vector x, specific as follows:

$\left\{\begin{array}{cc}{\mathrm{\χ}}_0=\stackrel{\‾}{x}& \\ {\mathrm{\χ}}_i={(\stackrel{\‾}{x}-\sqrt{(l+\mathrm{\λ})P_x})}_i& i=1,...,l\\ {\mathrm{\χ}}_i={(\stackrel{\‾}{x}+\sqrt{(l+\mathrm{\λ})P_x})}_{i-1}& i=l+1,...,2l\end{array}\right.$

In the formula, X _iBe the Sigma point after the colourless conversion of process; L is the dimension of the extended mode of state and noise composition, and λ is the scale parameter of control Sigma point to mean distance.With the distribution of this group Sigma point approximate representation random vector, the Sigma point has average identical with x and variance.

Calculate nonlinear function

γ _i＝g(X _i)i＝0，1，…，2l

Use γ _iThe distribution of approximate nonlinear function.

Calculate average and the variance of nonlinear function y:

$\left\{\begin{array}{c}\stackrel{\‾}{y}=\underset{i=0}{\overset{2l}{\mathrm{\Σ}}}{d}_i^m{\mathrm{\γ}}_i\\ P_y=\underset{i=0}{\overset{2l}{\mathrm{\Σ}}}{d}_i^c({\mathrm{\γ}}_i-\stackrel{\‾}{y}){({\mathrm{\γ}}_i-\stackrel{\‾}{y})}^T\end{array}\right.$

is weight coefficient in the formula, and

$\left\{\begin{array}{cc}{d}_0^m=\frac{\mathrm{\λ}}{\mathrm{\λ}+l}& \\ d_0^c=\frac{\mathrm{\λ}}{\mathrm{\λ}+l}+(1-{\mathrm{\α}}^2+\mathrm{\β})& \\ d_i^m=d_i^c=\frac{1}{2(l+\mathrm{\λ})}& i=1,...,2l\end{array}\right.$

Wherein, the scope that α control Sigma point distributes, the value zone is 0≤α≤1, generally gets 1; β is non-negative constant, and its effect is to make variance after the conversion contain the order of information of part, for Gaussian distribution β=2 is arranged; λ=α ²(l+e)-and l, wherein e is a constant, and it is relevant with the higher order term of estimation, and the higher order term that it is equal to or higher than 4 rank is its function, and the state estimation gets 0 usually, and parameter estimation is got 3-l.

Estimation to the under-water robot parameter is to estimate to realize that its step is following through uniting:

The associating estimation promptly adopts the same method to the state of system With angle of rake fault parameter b _kUnite estimation simultaneously, in uniting estimation, with angle of rake fault parameter b _kAs the motion vector of system, append state vector in reality After, form the state vector of expanding Re-use UKF and its extended reference model is estimated the state vector that is expanded Estimated result, and then can obtain propelling unit fault parameter b _kEstimated valve.

UKF essence is a kind of method of state estimation, and its estimation to the under-water robot parameter is to estimate to realize through uniting.In uniting estimation, the state and the parameter of system are mutually promoted, and have improved the accuracy of estimating.Even it is pointed out that in linear system, the combined estimation method of this state and parameter also is non-linear.

During the expansion NLS, contain the fault parameter b of the unknown/time feathering screw propeller _kSystem state equation be:

$\left\{\begin{array}{c}{x}_{k+1}=f(x_k,b_k,u_k)+w_k\\ b_{k+1}=f_b(x_k,b_k,u_k)+w_{\mathrm{kb}}\\ y_k=h(x_k,b_k)+v_k\end{array}\right.$

In the formula, b _kBe k the unknown/time feathering screw propeller fault parameter vector constantly, w _KbBeing the system noise of fault parameter model, is the Gaussian noise of zero-mean.Because fault parameter b _kChanging Pattern is unknown, then can be with b _kBe regarded as incoherent random drift vector, its recurrence expression formula is:

b _k＝b _k1+w _bk

In uniting in the estimation based on UKF; After the angle of rake fault parameter of system is appended to the time of day of system; Form the state matrix of expansion, promptly is the form that system equation is rewritten as the expanding system equation of state:

$\left\{\begin{array}{c}{x}_{k+1}^a=\tilde{f}(x_k^a,u_k)+w_k^a\\ y_k=\tilde{h}\left(x_k^a\right)+v_k\end{array}\right.$

In the formula,

is the system noise vector of expansion.

The system state equation of its expansion is estimated the state vector that is expanded with UKF

Estimated result, and then can obtain propelling unit fault parameter b _kEstimated valve.

Referring to accompanying drawing 3, be the diagram of circuit of this invention; The realization flow of this scheme in this enforcement:

The filtering initialization:

Read the state of under-water robot system and the initial value of propelling unit fault parameter, form the matrix of the initial condition of expansion, its initial condition (IC) is:

$\left\{\begin{array}{c}{\stackrel{\‾}{x}}_0=E\left[x_0\right]\\ P_0=E\left[(x_0-{\stackrel{\‾}{x}}_0){(x_0-{\stackrel{\‾}{x}}_0)}^T\right]\end{array}\right.$

The state matrix of expansion

q ₀The initial posture information of the sextuple under-water robot under the expression geodetic coordinate system,

Represent speed and angular velocity information under the sextuple under-water robot carrier coordinate system, b ₀The angle of rake failure message of expression six-freedom degree.Starter system process noise w also will be set simultaneously ₀With measurement noise v ₀, D ^wAnd D ^vBe respectively system and measure noise covariance matrix.

Obtain under-water robot extended mode and initial average through the filtering initialization

With variance P ₀, along with the carrying out of filtering algorithm, to obtain one here constantly, promptly k-1 can estimate the average of filtering

With variance P _K-1

The average that the under-water robot extended mode has been arranged

With variance P _K-1, through one group of colourless transition structure about

Symmetry and be positioned near the discrete Sigma point it, suc as formula shown in:

${\mathrm{\χ}}_{k-1}=[{\stackrel{\‾}{x}}_{k-1},{\stackrel{\‾}{x}}_{k-1}-\sqrt{(l+\mathrm{\λ})P_{k-1}},{\stackrel{\‾}{x}}_{k-1}+\sqrt{(l+\mathrm{\λ})}{P}_{k-1}]$

Wherein, l is the dimension of the extended mode of state of the system and propelling unit fault parameter composition, and λ is the scale parameter of control Sigma point to mean distance.

Discrete Sigma point calculates through system equation and upgrades the Sigma point, owing to unite estimation to the state and the angle of rake fault parameter of under-water robot, so must comprise angle of rake fault model in the system equation of under-water robot.Shown in the system equation formula of the expansion of adopting:

$M\stackrel{\·\·}{q}+C\left(\stackrel{\·}{q}\right)\stackrel{\·}{q}+D\left(\stackrel{\·}{q}\right)\stackrel{\·}{q}+G\left(q\right)=F\left(u\right)$

The Sigma point formula that calculates after upgrading through system equation is:

${\mathrm{\χ}}_{k|k-1}^{*}=f({\mathrm{\χ}}_{k-1},u_k)$

Calculating average and the formula of variance that Sigma is ordered after upgrading then is:

$\left\{\begin{array}{c}{\stackrel{\‾}{x}}_{k|k-1}=\underset{i=0}{\overset{2l}{\mathrm{\Σ}}}{d}_i^m{\mathrm{\χ}}_{i,k|k-1}^{*}\\ P_{k|k-1}=\underset{i=0}{\overset{2l}{\mathrm{\Σ}}}{d}_i^c({\mathrm{\χ}}_{i,k|k-1}^{*}-{\stackrel{\‾}{x}}_{k|k-1}){({\mathrm{\χ}}_{i,k|k-1}^{*}-{\stackrel{\‾}{x}}_{k|k-1})}^T+Q^w\end{array}\right.$

The extended mode average that the computing system Equation for Calculating is upgraded and the Sigma point of variance obtain one and go on foot state average and the variance of estimating:

${\mathrm{\χ}}_{k|k-1}=[{\stackrel{\‾}{x}}_{k|k-1},{\stackrel{\‾}{x}}_{k|k-1}-\sqrt{(l+\mathrm{\λ})P_{k|k-1}},{\stackrel{\‾}{x}}_{k|k-1}+\sqrt{(l+\mathrm{\λ})P_{k|k-1}}]$

Measure Equation for Calculating and upgrade the Sigma point:

The state average that one step was estimated is carried out colourless conversion with variance and again after the renewal of measurement Equation for Calculating, but obtains under-water robot observation bit appearance information:

Because only preceding sextuple posture information can be observed in the under-water robot extended mode, so the preceding 6 DOF that generation of last step Sigma is ordered calculates renewal.Measure shown in the following formula of equation:

y _k＝x _k(1∶6)+v _k

X wherein _kThe posture information of the preceding 6 DOF of (1: 6) expression extended mode, v _kBe incoherent white noise; Sigma point formula through measuring after Equation for Calculating is upgraded is:

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

But calculate the observer state average of measurement Equation for Calculating renewal and the Sigma point formula of variance be:

$\left\{\begin{array}{c}{\stackrel{\‾}{y}}_{k|k-1}=\underset{i=0}{\overset{2l}{\mathrm{\Σ}}}{d}_i^m{\mathrm{\γ}}_{i,k|k-1})\\ P_{{\stackrel{\‾}{y}}_k{\stackrel{\‾}{y}}_k}=\underset{i=0}{\overset{2l}{\mathrm{\Σ}}}{d}_i^c({\mathrm{\γ}}_{i,k|k-1}-{\stackrel{\‾}{y}}_{k|k-1}){({\mathrm{\γ}}_{i,k|k-1}-{\stackrel{\‾}{y}}_{k|k-1})}^T+Q^v\\ P_{{\stackrel{\‾}{x}}_k{\stackrel{\‾}{y}}_k}=\underset{i=0}{\overset{2l}{\mathrm{\Σ}}}{d}_i^c({\mathrm{\χ}}_{i,k|k-1}-{\stackrel{\‾}{x}}_{k|k-1}){({\mathrm{\γ}}_{i,k|k-1}-{\stackrel{\‾}{y}}_{k|k-1})}^T\end{array}\right.$

Calculate the gain matrix of UKF filter:

$K_k=P_{{\stackrel{\‾}{x}}_k{\stackrel{\‾}{y}}_k}{P}_{{\stackrel{\‾}{y}}_k{\stackrel{\‾}{y}}_k}^{-1}$

The average of estimating system extended mode and variance then:

Read through the sensors observe posture information y behind the data handing _k, estimate the average and the variance of under-water robot extended mode based on this, suc as formula shown in:

$\left\{\begin{array}{c}{P}_k=P_{k|k-1}-K_k{P}_{{\stackrel{\‾}{y}}_k{\stackrel{\‾}{y}}_k}{K}_k^T\\ {\stackrel{\‾}{x}}_k={\stackrel{\‾}{x}}_{k|k-1}+K_k(y_k-{\stackrel{\‾}{y}}_{k|k-1})\end{array}\right.$

At the next one constantly, promptly k+1 if new observed reading is arranged, repeats above-mentioned steps constantly.

Claims (5)

1. under-water robot state and parametric joint method of estimation based on a UKF is characterized in that comprising the steps: to set up the extended reference model of under-water robot off-line; Online posture information y according to the detection of pose sensor _kAdopt UKF Filtering Estimation algorithm, the extended reference model of robot off-line is the system state equation of expansion under the unfree water, to the extended mode transmission and the renewal of state of the system and propelling unit fault composition; The state and the parameter information of the real-time system that estimates obtain the end value that system is estimated

And P _kHere the status information of system comprises through filtered posture information q _kThe velocity information of coming out with real-time estimation

Parameter information is the angle of rake fault parameter b of system _k

2. a kind of under-water robot state and parametric joint method of estimation based on UKF according to claim 1 is characterized in that: the structure of under-water robot reference model:

The kinetic model of under-water robot is represented as follows:

$M\stackrel{\·\·}{q}+C\left(\stackrel{\·}{q}\right)\stackrel{\·}{q}+D\left(\stackrel{\·}{q}\right)\stackrel{\·}{q}+G\left(q\right)=u$

In the formula; M is the inertial matrix of 66 dimensions;

comprises 66 dimension matrixes of centnifugal force and coriolis force;

is the hydrodynamic force matrix of 66 dimensions; G (q) comprises 61 dimension matrixes of gravity and buoyancy, and q and u are respectively the state and the motor torque vector of 61 dimensions;

With the kinetic model of under-water robot, show as the variation that system motor torque vector u imports according to angle of rake fault, the extended reference model representation that obtains under-water robot is following:

$M\stackrel{\·\·}{q}+C\left(\stackrel{\·}{q}\right)\stackrel{\·}{q}+D\left(\stackrel{\·}{q}\right)\stackrel{\·}{q}+G\left(q\right)=F\left(u\right)$

F in the formula (u) is angle of rake failure function, defines 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 following formula, u _kBe 61 dimension motor torque vector matrixs, b _kBe 61 dimension fault parameter matrixes,

Be expressed as the k angle of rake loss in efficiency factor constantly, subscript i representes i propelling unit, when

The time, represent that i propelling unit is normal; When

The time, represent that rigid fault has taken place i propelling unit, promptly loss factor is 100%; When

The time, represent that soft fault has taken place i propelling unit, i.e. partial failure.

3. a kind of under-water robot state and parametric joint method of estimation according to claim 1 based on UKF; It is characterized in that: described UKF Filtering Estimation algorithm is to be framework with basic Kalman filtering algorithm, carries out the recursion and the renewal of the state and the error covariance of NLS through nonlinear colourless conversion.

4. according to claim 1 or 3 described a kind of under-water robot state and parametric joint methods of estimation, it is characterized in that, adopt the implementation procedure of UKF Filtering Estimation based on UKF:

To last one constantly is k-1 by by the average of estimator " priori "

With variance P _K-1The one group of discrete Sigma point X that obtains through non-linear colourless conversion _K-1, X _K-1Calculate the Sigma point that obtains dispersing after the renewal through system state equation

Right

Calculate the extended mode average of step estimation

With variance P _K|k-1Right And P _K|k-1Carry out colourless variation again and obtain one group of discrete Sigma point X _K|k-1, X _K|k-1Through measuring the Sigma point γ that obtains dispersing after Equation for Calculating is upgraded _K|k-1, to γ _K|k-1But calculate the average of under-water robot observer state

And variance

Estimated valve; But the estimated valve and the sensor acquisition observed value y of a resulting step estimated valve and the observer state that obtains _k, estimate to obtain system's estimated value through calculating And P _kIf new observed reading y is arranged _k, repeat above-mentioned steps.

5. according to claim 1 or 3 described a kind of under-water robot state and parametric joint methods of estimation based on UKF, it is characterized in that: the estimation to the under-water robot parameter is to estimate to come performing step following through uniting:

The associating estimation promptly adopts the same method to the state of system

With angle of rake fault parameter b _kUnite estimation simultaneously, in uniting estimation, with angle of rake fault parameter b _kAs the motion vector of system, append state vector in reality

After, form the state vector of expanding

Re-use UKF and its extended reference model is estimated the state vector that is expanded Estimated result, and then can obtain propelling unit fault parameter b _kEstimated valve.

Images