CN102862666B - Underwater robot state and parameter joint estimation method based on self-adaption unscented Kalman filtering (UKF) - Google Patents
Underwater robot state and parameter joint estimation method based on self-adaption unscented Kalman filtering (UKF) Download PDFInfo
- Publication number
- CN102862666B CN102862666B CN201110190512.5A CN201110190512A CN102862666B CN 102862666 B CN102862666 B CN 102862666B CN 201110190512 A CN201110190512 A CN 201110190512A CN 102862666 B CN102862666 B CN 102862666B
- Authority
- CN
- China
- Prior art keywords
- mrow
- msub
- mover
- state
- variance
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 52
- 238000001914 filtration Methods 0.000 title claims abstract description 24
- 238000005259 measurement Methods 0.000 claims abstract description 31
- 230000008569 process Effects 0.000 claims abstract description 26
- 239000011159 matrix material Substances 0.000 claims description 51
- 239000013598 vector Substances 0.000 claims description 36
- 238000004364 calculation method Methods 0.000 claims description 16
- 230000026676 system process Effects 0.000 claims description 9
- 230000009466 transformation Effects 0.000 claims description 9
- 230000003044 adaptive effect Effects 0.000 claims description 8
- 230000008859 change Effects 0.000 claims description 5
- 230000009897 systematic effect Effects 0.000 claims description 3
- 238000010276 construction Methods 0.000 description 3
- 238000011161 development Methods 0.000 description 3
- 238000010586 diagram Methods 0.000 description 3
- 238000012886 linear function Methods 0.000 description 3
- 238000006243 chemical reaction Methods 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 238000005070 sampling Methods 0.000 description 2
- 238000012935 Averaging Methods 0.000 description 1
- 230000009471 action Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 238000009472 formulation Methods 0.000 description 1
- 230000005484 gravity Effects 0.000 description 1
- 230000036541 health Effects 0.000 description 1
- 239000000203 mixture Substances 0.000 description 1
- 230000000737 periodic effect Effects 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 description 1
Landscapes
- Feedback Control In General (AREA)
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
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 sensorkThe 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 Pk(ii) a Where the state information of the system includes filtered pose information skAnd real-time estimated speed informationThe parameter information is a fault parameter b of the system propellerkAnd, at the same time, calculating estimated innovation information xi according to the main filterkCalculating the measurement Sq of the auxiliary filterk(ii) a Establishing a state equation and a measurement equation of the UKF auxiliary filter according to the measured value SqkAs 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 wkAnd measuring the noise vkTheir covariance matrices are respectively DwAnd Dv。
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 containedkThe system state space equation is as follows:
xk+1=f(xk,bk,uk)+wk
bk+1=fb(xk,bk,uk)+wkb
in the formula, bkIs an unknown/time-varying propeller fault parameter at time k, wkbThe system propeller fault noise vector which is a fault parameter model is zero mean Gaussian noise;
when the fault parameter bkIf the change rule is unknown, b can bekThe random drift vector, considered uncorrelated, has the recursive expression:
bk=bk-1+wbk
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:
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 modelkIs/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:
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:
constructing an auxiliary filter measurement equation in the self-adaptive UKF algorithm:
with innovation xi of the main filterkMeasurement Sq of the variance matrixkAs an observation signal of the system, the variance P is estimated in accordance with the main filter in one stepk|k-1Variance P of the main filter extension statekGain matrix K of the main filter UKFkThe measurement equations that make up the auxiliary filter are:
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 Pk-1After nonlinear colorless transformation, a group of discrete Sigma points chi are obtainedk-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 Pk|k-1To extended state meanSum variance Pk|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 sensorkMean of co-spread statesSum variance Pk|k-1And an estimated value of the observable state and a gain matrix KkCalculating and estimating to obtain a system mean valueVariance PkTo derive from it an estimate of xik(ii) a According to the new information xi of the main filterkThe measured value Sq is obtained through calculationkAs 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 stepk|k-1Extended state variance PkAnd a gain matrix KkObtaining estimated observed value after calculation and updateThe obtained one-step estimation value and observation valueMeasured value Sq of same main filterkObtaining diagonal elements of system process noise variance matrix D through calculation and estimationEstimation of elementsAnd the variance thereofWhen new observation pose information y existskThen, 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 sensorkThe 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 Pk(ii) a Where the state information of the system includes filtered pose information skAnd real-time estimated speed informationThe parameter information is a fault parameter b of the system propellerkAnd, at the same time, calculating estimated innovation information xi according to the main filterkCalculating the measurement Sq of the auxiliary filterk(ii) a For building UKF auxiliary filtersEquation of state and measurement equation based on the measured value SqkAs 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 wkAnd measuring the noise vkTheir covariance matrices are respectively DwAnd Dv。
Extending fault parameters b in reference modelkIs/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:
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:
where F (u) is the failure function of the propeller, defined as follows:
Fk(uk)=uk+Ukbk
in the above formula, ukIs a 6 x 1 dimensional motor torque vector matrix, bkIs 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 containedkThe system state space equation is as follows:
xk+1=f(xk,bk,uk)+wk
bk+1=fb(xk,bk,uk)+wbk
in the formula, bkIs the unknown/time varying propeller fault parameter vector at time k, wkbThe system propeller fault noise which is a fault parameter model is zero mean Gaussian noise;
because of the fault parameter bkIf the change rule is unknown, b can bekThe random drift vector, considered uncorrelated, has the recursive expression:
bk=bk-1+wbk
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:
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 Pk-1After nonlinear colorless transformation, a group of discrete Sigma points chi are obtainedk-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 Pk|k-1To extended state meanSum variance Pk|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 sensorkMean of co-spread statesSum variance Pk|k-1And an estimated value of the observable state and a gain matrix KkCalculating and estimating to obtain a system mean valueVariance PkEstimated values from which to derive the new information xik(ii) a According to the new information xi of the main filterkThe measured value Sq is obtained through calculationkAs 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 PxAveraging ySum variance PyThe colorless conversion steps are as follows:
generation of Sigma dots
According to the mean value of the random vector xSum variance PxA 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:
in the formula, xiThe 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 gammaiThe distribution of the non-linear function is approximated.
Calculating the mean and variance of the nonlinear function y:
in the formulaIs a weight coefficient, and
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 λ ═ α2(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:
extendedState matrixs0Representing 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, b0Fault information for a six degree of freedom thruster is presented. At the same time, setting the initial system process noise w0And measuring the noise v0,DwAnd DvProcess noise and measurement noise covariance matrices, respectively.
Obtaining the expansion state and the initial mean value of the underwater robot through filter initializationSum variance P0。
As the filtering algorithm proceeds, the mean value of the estimated filtering at the last time k-1 is obtainedSum variance Pk-1(ii) a While the system noise w of the last time k-1 is to be obtainedk-1And measuring the noise vk-1System noise wk-1And measuring the noise vk-1Is the diagonal element of the system noise variance matrix D estimated by the auxiliary filterMean value of expansion state of underwater robotSum variance Pk-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:
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:
the extended system equation formulation formed in the filtering process:
the Sigma point formula after being updated by the system equation is:
then, the mean and variance formula of the updated Sigma point is calculated as:
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:
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:
yk=xk(1:6)+vk
wherein xk(1:6) pose information of the first six dimensions representing the extended state, vkIs 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:
calculating a gain matrix of the UKF filter:
estimating the mean and variance of the system expansion state:
reading the sensor observation pose information y after data processingkAnd 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:
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:
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:
constructing an auxiliary filter measurement equation in the self-adaptive UKF algorithm:
the auxiliary filter is the innovation xi of the main filterkDiagonal elements of the variance matrixAs an observation signal of the system, the variance P is estimated in accordance with the main filter in one stepk|k-1Variance P of the main filter extension statekGain matrix K of the main filter UKFkThe observation equations that make up the auxiliary filter are:
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 stepk|k-1Extended state variance PkAnd a gain matrix KkObtaining 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 existskThen 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:
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:
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:
wherein,is a noise variance matrix in the auxiliary filter byEstimating D of the main filterwAnd set.
Of auxiliary filtersThe measurement equation is:
where vdiag represents a vector formed by taking the major diagonal elements of the matrix in brackets, letIt is possible to obtain:
wherein, BpkIs a constant value vector; HPkIs a constant matrix. If KPk∈Rmq×nqAnd, in addition,
due to QwFor diagonal matrix, HP can be obtainedk∈Rmq×nqAnd is provided with
Computing a gain matrix for the auxiliary filter:
according to the real-time information xi of the main filterkCalculating the measurement value of the auxiliary filter:
wherein,ykis 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 filterkEstimating diagonal elements of process noise variance matrix of underwater robot system based on the aboveSum varianceAs shown in the formula:
at the next moment, if there is information xik+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 sensorkThe 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 Pk(ii) a Where the state information of the system includes filtered pose information skAnd real-time estimated speed informationThe parameter information is a fault parameter b of the system propellerkAnd, at the same time, calculating estimated innovation information xi according to the main filterkCalculating the measurement Sq of the auxiliary filterk(ii) a Establishing a state equation and a measurement equation of the UKF auxiliary filter according to the measured value SqkAs 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 wkAnd measuring the noise vkTheir covariance matrices are respectively DwAnd Dv;
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 containedkThe system state space equation is as follows:
xk+1=f(xk,bk,uk)+wk
bk+1=fb(xk,bk,uk)+wkb
in the formula, bkIs an unknown/time-varying propeller fault parameter at time k, wkbSystem thruster fault noise vector as a fault parameter modelQuantity, gaussian noise with zero mean;
when the fault parameter bkIf the change rule is unknown, bkThe random drift vector, considered uncorrelated, has the recursive expression:
bk=bk-1+wbk
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:
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 modelkIs/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:
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:
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 filterkMeasurement Sq of the variance matrixkAs an observation signal of the system, the variance P is estimated in accordance with the main filter in one stepk|k-1Variance P of the main filter extension statekGain matrix K of the main filter UKFkThe measurement equations that make up the auxiliary filter are:
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 Pk-1After nonlinear colorless transformation, a group of discrete Sigma points chi are obtainedk-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 Pk|k-1To extended state meanSum variance Pk|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 sensorkMean of co-spread statesSum variance Pk|k-1And an estimated value of the observable state and a gain matrix KkCalculating and estimating to obtain a system mean valueVariance PkTo derive from it an estimate of xik(ii) a According to the new information xi of the main filterkThe measured value Sq is obtained through calculationkAs 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 stepk|k-1Extended state variance PkAnd a gain matrix KkObtaining estimated observed value after calculation and updateThe obtained one-step estimation value and observation valueMeasured value Sq of same main filterkThe 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 existskThen, the main filter pass of the next control period is continuedAnd (5) sequence steps.
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201110190512.5A CN102862666B (en) | 2011-07-08 | 2011-07-08 | Underwater robot state and parameter joint estimation method based on self-adaption unscented Kalman filtering (UKF) |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201110190512.5A CN102862666B (en) | 2011-07-08 | 2011-07-08 | Underwater robot state and parameter joint estimation method based on self-adaption unscented Kalman filtering (UKF) |
Publications (2)
Publication Number | Publication Date |
---|---|
CN102862666A CN102862666A (en) | 2013-01-09 |
CN102862666B true CN102862666B (en) | 2014-12-10 |
Family
ID=47441869
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201110190512.5A Active CN102862666B (en) | 2011-07-08 | 2011-07-08 | Underwater robot state and parameter joint estimation method based on self-adaption unscented Kalman filtering (UKF) |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN102862666B (en) |
Families Citing this family (10)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN103336525B (en) * | 2013-06-18 | 2016-09-14 | 哈尔滨工程大学 | Stochastic system high weight convenient UKF filtering method |
CN103488078B (en) * | 2013-07-18 | 2014-09-24 | 清华大学 | Excitation signal optimization method for improving closed-loop identification accuracy of power system |
CN107037821B (en) * | 2017-05-12 | 2019-10-29 | 中国人民解放军91550部队 | The estimation of underwater hiding-machine athletic posture and control method under repetitive shocks |
CN108170151B (en) * | 2017-07-24 | 2019-12-31 | 西北工业大学 | Self-adaptive motion control device and method for underwater robot |
CN109341683A (en) * | 2018-06-29 | 2019-02-15 | 中国人民解放军海军工程大学 | Course based on the bis- labels of UWB calculates and its method for analyzing performance |
CN110011879B (en) * | 2019-04-29 | 2021-01-05 | 燕山大学 | Sensor network safety real-time online monitoring system based on parallel filtering |
CN111191186B (en) * | 2020-01-07 | 2021-09-28 | 江南大学 | Multi-cell filtering method for positioning position of mobile robot in production workshop |
CN111880411B (en) * | 2020-08-12 | 2022-06-03 | 深圳职业技术学院 | Dynamic extended regression and interactive estimation method of linear system |
CN112025706B (en) * | 2020-08-26 | 2022-01-04 | 北京市商汤科技开发有限公司 | Method and device for determining state of robot, robot and storage medium |
CN112445244B (en) * | 2020-11-09 | 2022-03-04 | 中国科学院沈阳自动化研究所 | Target searching method for multiple autonomous underwater robots |
Citations (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN201362352Y (en) * | 2008-05-22 | 2009-12-16 | 上海海事大学 | Fault-tolerant control device of unmanned underwater robot sensor |
Family Cites Families (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US5355818A (en) * | 1992-08-27 | 1994-10-18 | Strait Imaging Research | Portable inspection equipment for ocean going vessels |
GB2359049A (en) * | 2000-02-10 | 2001-08-15 | H2Eye | Remote operated vehicle |
-
2011
- 2011-07-08 CN CN201110190512.5A patent/CN102862666B/en active Active
Patent Citations (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN201362352Y (en) * | 2008-05-22 | 2009-12-16 | 上海海事大学 | Fault-tolerant control device of unmanned underwater robot sensor |
Non-Patent Citations (8)
Title |
---|
主从式UUV群的随从UUV导航;翼大雄等;《仪器仪表学报》;20090630;第30卷(第6S期);期刊第631-634页 * |
基于MIT规则的自适应Unscented卡尔曼滤波及其在旋翼飞行机器人容错控制的应用;齐俊桐等;《机械工程学报》;20090430;第45卷(第4期);期刊第115-123页 * |
基于UKF的水下机器人执行器故障检测方法研究;林昌龙等;《机械设计与制作》;20110531(第5期);期刊第168-170页 * |
基于自适应UKF算法的小型水下机器人导航***;孙尧等;《自动化学报》;20110331;第37卷(第3期);期刊第342-353页 * |
孙尧等.基于自适应UKF算法的小型水下机器人导航***.《自动化学报》.2011,第37卷(第3期), * |
林昌龙等.基于UKF的水下机器人执行器故障检测方法研究.《机械设计与制作》.2011,(第5期), * |
翼大雄等.主从式UUV群的随从UUV导航.《仪器仪表学报》.2009,第30卷(第6S期), * |
齐俊桐等.基于MIT规则的自适应Unscented卡尔曼滤波及其在旋翼飞行机器人容错控制的应用.《机械工程学报》.2009,第45卷(第4期), * |
Also Published As
Publication number | Publication date |
---|---|
CN102862666A (en) | 2013-01-09 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN102862666B (en) | Underwater robot state and parameter joint estimation method based on self-adaption unscented Kalman filtering (UKF) | |
Zhao et al. | Particle filter for fault diagnosis and robust navigation of underwater robot | |
CN101871782B (en) | Position error forecasting method for GPS (Global Position System)/MEMS-INS (Micro-Electricomechanical Systems-Inertial Navigation System) integrated navigation system based on SET2FNN | |
CN112432644B (en) | Unmanned ship integrated navigation method based on robust adaptive unscented Kalman filtering | |
Alonge et al. | Trajectory tracking of underactuated underwater vehicles | |
CN103217175A (en) | Self-adaptive volume Kalman filtering method | |
CN107179693B (en) | Robust adaptive filtering and state estimation method based on Huber estimation | |
CN102795323B (en) | Unscented Kalman filter (UKF)-based underwater robot state and parameter joint estimation method | |
CN113608534B (en) | Unmanned ship tracking control method and system | |
CN105867417B (en) | A kind of UUV contragradience sliding formwork power positioning control methods that DVL tests the speed when failing | |
Rigatos | Sensor fusion-based dynamic positioning of ships using Extended Kalman and Particle Filtering | |
Demim et al. | SLAM problem for autonomous underwater vehicle using SVSF filter | |
CN108226887B (en) | Water surface target rescue state estimation method under condition of transient observation loss | |
Skriver et al. | Adaptive extended Kalman filter for actuator fault diagnosis | |
CN113587926A (en) | Spacecraft space autonomous rendezvous and docking relative navigation method | |
CN112710304A (en) | Underwater autonomous vehicle navigation method based on adaptive filtering | |
Popov et al. | Adaptive Kalman filtering for dynamic positioning of marine vessels | |
CN104331087B (en) | Robust underwater sensor network target tracking method | |
CN114139109A (en) | Target tracking method, system, equipment, medium and data processing terminal | |
Shi et al. | 2 Identification of ship maneuvering model using extended Kalman filters | |
CN112986977A (en) | Method for overcoming radar extended Kalman track filtering divergence | |
CN108710295B (en) | Robot following method based on progressive volume information filtering | |
CN111310110A (en) | Mixed state estimation method for high-dimensional coupling uncertain system | |
CN116185024A (en) | Unmanned ship heading anti-interference control method | |
CN112595319B (en) | Model self-adaptive compensation return trajectory estimation method |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
C06 | Publication | ||
PB01 | Publication | ||
C10 | Entry into substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
C14 | Grant of patent or utility model | ||
GR01 | Patent grant |