CN110334411A - A kind of underwater robot kinetic parameters discrimination method based on Huber M estimation - Google Patents
A kind of underwater robot kinetic parameters discrimination method based on Huber M estimation Download PDFInfo
- Publication number
- CN110334411A CN110334411A CN201910518943.6A CN201910518943A CN110334411A CN 110334411 A CN110334411 A CN 110334411A CN 201910518943 A CN201910518943 A CN 201910518943A CN 110334411 A CN110334411 A CN 110334411A
- Authority
- CN
- China
- Prior art keywords
- underwater robot
- huber
- estimation
- model
- dynamic model
- 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.)
- Pending
Links
- 238000012850 discrimination method Methods 0.000 title abstract 2
- 238000000034 method Methods 0.000 claims abstract description 56
- 238000005070 sampling Methods 0.000 claims abstract description 14
- 239000011159 matrix material Substances 0.000 claims description 40
- 239000013598 vector Substances 0.000 claims description 17
- 230000005484 gravity Effects 0.000 claims description 16
- 230000001133 acceleration Effects 0.000 claims description 14
- 238000006243 chemical reaction Methods 0.000 claims description 7
- 239000012530 fluid Substances 0.000 claims description 6
- 230000008878 coupling Effects 0.000 claims description 3
- 238000010168 coupling process Methods 0.000 claims description 3
- 238000005859 coupling reaction Methods 0.000 claims description 3
- 238000004088 simulation Methods 0.000 description 22
- 230000008569 process Effects 0.000 description 12
- 238000010586 diagram Methods 0.000 description 8
- 230000008859 change Effects 0.000 description 5
- 230000000694 effects Effects 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000013459 approach Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000009795 derivation Methods 0.000 description 1
- 238000002474 experimental method Methods 0.000 description 1
- 238000013488 ordinary least square regression Methods 0.000 description 1
- 230000000750 progressive effect Effects 0.000 description 1
- 239000013535 sea water Substances 0.000 description 1
- 230000009466 transformation Effects 0.000 description 1
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 description 1
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
- G06F17/10—Complex mathematical operations
- G06F17/11—Complex mathematical operations for solving equations, e.g. nonlinear equations, general mathematical optimization problems
- G06F17/13—Differential equations
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
- G06F17/10—Complex mathematical operations
- G06F17/15—Correlation function computation including computation of convolution operations
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
- G06F17/10—Complex mathematical operations
- G06F17/16—Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F30/00—Computer-aided design [CAD]
- G06F30/20—Design optimisation, verification or simulation
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F2119/00—Details relating to the type or aim of the analysis or the optimisation
- G06F2119/06—Power analysis or power optimisation
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Mathematical Physics (AREA)
- Theoretical Computer Science (AREA)
- Mathematical Analysis (AREA)
- Mathematical Optimization (AREA)
- Pure & Applied Mathematics (AREA)
- Computational Mathematics (AREA)
- Data Mining & Analysis (AREA)
- General Engineering & Computer Science (AREA)
- Software Systems (AREA)
- Databases & Information Systems (AREA)
- Algebra (AREA)
- Computing Systems (AREA)
- Operations Research (AREA)
- Computer Hardware Design (AREA)
- Evolutionary Computation (AREA)
- Geometry (AREA)
- Manipulator (AREA)
Abstract
The invention discloses a kind of underwater robot kinetic parameters discrimination methods based on Huber M estimation, comprising the following steps: 1) establishes underwater human occupant dynamic model；2) according to the model of foundation, the kinetic parameter of identification needed for determining；3) least square method of recursion based on Huber loss function is used, the estimates of parameters of underwater human occupant dynamic model is recognized；4) estimates of parameters of the underwater human occupant dynamic model obtained according to identification, updates the current state value of underwater robot；5) step 3) is repeated to 4), obtaining the estimates of parameters of each sampling instant, and is averaged as identification result.The method of the present invention still can be stable under " outlier " noise circumstance pick out parameter to be identified, improving identification precision and improves robustness.
Description
Technical Field
The invention relates to a model parameter identification method, in particular to an underwater robot dynamics model parameter identification method based on Huber M estimation.
Background
The underwater interference is very complex, simple Gaussian noise cannot completely describe the complex noise generated by water flow, the mean value of the noise cannot be guaranteed to be zero, and the standard deviation cannot be constant. Many studies show that the least square method has a relatively obvious effect under simple gaussian noise interference, but if a certain proportion of 'outliers' (noise outliers with a very large standard deviation) appear in interference noise, the ordinary least square method or the recursive least square method is still adopted to identify model parameters at the moment, and the condition that an estimation result is inaccurate or even diverged occurs, which directly causes the robustness of model identification to be deteriorated.
Disclosure of Invention
The invention aims to solve the technical problem of providing an underwater robot dynamics model parameter identification method based on Huber M estimation aiming at the defects in the prior art.
The technical scheme adopted by the invention for solving the technical problems is as follows: a method for identifying parameters of an underwater robot dynamic model based on Huber M estimation comprises the following steps:
1) establishing an original 6-degree-of-freedom coupling dynamic model of the underwater robot, simplifying the model, and decoupling to obtain a simplified single-degree-of-freedom dynamic model of the underwater robot required by algorithm identification;
2) determining a kinetic parameter theta to be identified according to the established model;
3) identifying a parameter estimation value of a dynamic model of the underwater robot by adopting a recursive least square method based on a Huber loss function;
4) updating the current state value of the underwater robot according to the parameter estimation value of the underwater robot dynamic model obtained by identification;
5) and (4) repeating the steps 3) to 4) to obtain the parameter estimation value of each sampling moment, and taking the average value as the identification result.
According to the scheme, in the step 1), the simplified single-degree-of-freedom dynamic model of the underwater robot is as follows:
wherein,
M＝MRB+MA；
MRB＝diag{m 0 m 0 0 Iz}；
D(v)＝diag{Xu+Xu|u||u| 0 Zw+Zw|w||w| 0 0 Nr+Nr|r||r|}；
g(η)＝[0 0 0 -16 0 0]T
where M is a mass and inertia matrix comprising rigid body masses and an inertia matrix MRBHydrodynamic additional mass matrix MA(ii) a m is the mass of the underwater robot; i is an inertia item of the underwater robot,
Izthe inertial term of the underwater robot in the z direction;representing a first direction;represents a longitudinal direction;
represents the normal direction;are linear and angular acceleration vectors; v is the linear velocity and angular velocity vector of the underwater robot under the motion coordinate system; g (η) is a restoring force (moment) vector generated by gravity and buoyancy; d (v) is a fluid resistance matrix; tau is the resultant force and resultant moment vector of the propeller and the underwater robot subjected to underwater interference, and J (eta) is a conversion matrix between a fixed coordinate system and a moving coordinate system.
According to the scheme, a recursive least square method based on a Huber loss function is adopted in the step 3) to identify the parameter estimation value of the underwater robot dynamic model,
the estimation formula of the recursive least square method based on the Huber loss function for identifying the parameters is as follows:
robust gain K of recursive least square method based on Huber loss functionHuber(k) The following were used:
in the formula, u (e)k) Is a robust factor of the Huber method.
According to the scheme, the value of the adjusting parameter delta of the robust factor is 1.345.
The invention has the following beneficial effects:
the Huber loss function is fused into the identification algorithm applied to common noise interference, so that the algorithm can still stably identify the parameters to be identified in a 'outlier' (noise outlier with extremely large standard deviation) noise environment, and has higher identification precision and robustness.
Drawings
The invention will be further described with reference to the accompanying drawings and examples, in which:
FIG. 1 is a flow chart of a method of an embodiment of the present invention;
FIG. 2 is a diagram of the heading degree of freedom model input thrust in accordance with an embodiment of the present invention;
FIG. 3 is a schematic diagram of a Simulink simulation system of a heading degree of freedom dynamic model according to an embodiment of the invention;
FIG. 4 is a one-dimensional acceleration and one-dimensional velocity curve of the heading degree of freedom according to the embodiment of the present invention;
FIG. 5 is a graph of longitudinal degree of freedom model input thrust in accordance with an embodiment of the present invention;
FIG. 6 is a graph of longitudinal degree of freedom one-dimensional acceleration versus one-dimensional velocity in accordance with an embodiment of the present invention;
FIG. 7 is a graph of the forward degree of freedom model input thrust for an embodiment of the present invention;
FIG. 8 is a schematic diagram of a Simulink simulation system of a progressive degree of freedom kinetic model according to an embodiment of the present invention;
FIG. 9 is a graph of one-dimensional acceleration versus one-dimensional velocity for the forward degree of freedom of the embodiment of the present invention;
FIG. 10 is a graph of the input thrust of the horizontal hydrodynamic model in accordance with an embodiment of the present invention;
FIG. 11 is a schematic diagram of a horizontal plane hydrodynamic model Simulink simulation system according to an embodiment of the invention;
FIG. 12 is a one-dimensional acceleration versus one-dimensional velocity graph of a horizontal hydrodynamic model in accordance with an embodiment of the present invention;
FIG. 13 is a schematic diagram of a change process of estimation errors of a heading dynamics model containing "outlier" Gaussian noise according to an embodiment of the present invention;
FIG. 14 is a schematic diagram of the process of estimating error variation of the longitudinal dynamical model containing Gaussian noise with "outlier" according to the embodiment of the present invention;
FIG. 15 is a schematic diagram of the process of estimating the error change of the forward dynamics model with Gaussian noise including "outliers" according to the embodiment of the present invention;
FIG. 16 is a schematic diagram of the process of changing the estimation error of the horizontal plane model with "outlier" Gaussian noise according to the embodiment of the present invention.
Detailed Description
In order to make the objects, technical solutions and advantages of the present invention more apparent, the present invention is further described in detail with reference to the following embodiments. It should be understood that the specific embodiments described herein are merely illustrative of the invention and are not intended to limit the invention.
As shown in fig. 1, a method for identifying parameters of a dynamics model of an underwater robot based on Huber M estimation includes the following steps:
1. establishing a basic model:
A. underwater robot dynamics model establishment
After establishing a corresponding coordinate system and establishing a mutual conversion relation, the underwater robot 6-degree-of-freedom dynamic model can be described as follows according to a Newton-Euler equation of motion of a rigid body:
where M is a mass and inertia matrix comprising rigid body masses and an inertia matrix MRBHydrodynamic additional mass matrix MA；Are linear and angular acceleration vectors; v is the linear velocity and angular velocity vector of the underwater robot under the motion coordinate system; c (v) is a general Ke's and centripetal force matrix including a rigid Ke's and centripetal force matrix C of the underwater robotRB(v) And from the additional mass inertia matrix MAInduced similar coriolis force matrix CA(v) (ii) a D (v) is a fluid resistance matrix comprising a linear resistance coefficient DLAnd secondary resistance DQ(ii) a g (η) is a restoring force (moment) vector generated by gravity and buoyancy; tau is the resultant force and resultant moment vector of the propeller and the underwater robot subjected to underwater interference; j (eta) is a conversion matrix between the fixed coordinate system and the moving coordinate system. In addition, each vector and matrix satisfies the following relationship:
1) the mass and inertia matrix satisfies:
M＝MRB+MA (1.3)
wherein M is the mass of the underwater robot, I is the inertia item of the underwater robot, and the rigid inertia matrix MRBIs unique and satisfies the following formula:
and the additional mass matrix MAWill vary with the shape of the underwater robot, but the parameters of the matrix are constant when the underwater robot is submerged.
2) The Coriolis and centripetal force matrices satisfy:
C(v)＝CRB(v)+CA(v) (1.7)
C(v)＝-CT(v) (1.8)
CRB(v)＝-CT RB(v) (1.9)
if the origin of the motion coordinate system is on the gravity center of the underwater robot, the underwater robot rigid body Coriolis and centripetal force matrix CRB(v) Can be described as:
similarly, a Coriolis-like force matrix C resulting from the additional mass inertia matrixA(v) Can be described as:
in the formula:
3) the fluid resistance matrix satisfies:
D(v)＝diag{DL+DQ|v|} (1.18)
in the formula:
DL＝diag{Xu Yv Zw Kp Mq Nr} (1.19)
DL＝diag{Xu|u| Yv|v| Zw|w| Kp|p| Mq|q| Nr|r|} (1.20)
4) the restoring force (moment) vector generated by gravity and buoyancy satisfies the following conditions:
if W is gravity, B is buoyancy, rGIs center of gravity, rBThe center of gravity is r in the motion coordinate systemG＝[xG yGzG]＝[0 0 0](ii) a The floating center has a coordinate r in a motion systemB＝[xB yB zB](ii) a The gravity and buoyancy generated restoring force (moment) vector can be described as:
5) the transformation matrix may be described as follows:
conversion matrix J for converting linear speed from moving coordinate system to fixed coordinate system1Comprises the following steps:
conversion matrix J for converting angular velocity from moving coordinate system to fixed coordinate system2Comprises the following steps:
the conversion matrixes for converting the fixed coordinate system of the linear velocity and the angular velocity into the moving coordinate system are corresponding inverse matrixes respectively.
B. Simplification of underwater robot dynamics model
1) Simplification of the Mass and inertia matrices
The physical appearance structure of the underwater robot is symmetrical about three tangent planes, and the gravity center of the underwater robot is the origin of the motion coordinate system, so the rigid mass and the inertia matrix MRBAnd hydrodynamic additional mass matrix MACan be simplified as follows:
MRB＝diag{m 0 m 0 0 Iz} (1.24)
2) simplification of the Coriolis and centripetal force matrices
Since the underwater robot has a slow navigation speed within 1 section, the coriolis and the centripetal force can be directly eliminated, so that c (v) is 0, and the dynamic model is simplified as follows:
3) simplification of the fluid resistance matrix
Also due to the symmetry and the position of the center of gravity of the underwater robot, the fluid resistance matrix is simplified as follows:
4) simplification of gravity and buoyancy vectors
Neglecting the movement of the underwater robot in the direction of the transverse moving degree of freedom, and simultaneously controlling the movement of the underwater robot in the direction of the longitudinal and transverse moving degrees of freedom by the centralizing moment generated by the buoyancy and gravity of the underwater robot, so that the transverse moving angle and the longitudinal moving angle are kept minimum. Under the characteristics, the gravity center coordinate of the underwater robot is rG＝[0 0 0]The floating center coordinate is rB＝[0 0 -0.1]TI.e. the centre of buoyancy is also on the z-axis. The gravity of the underwater robot is 1166 newtons, and the buoyancy of the underwater robot is 1182 newtons. The gravity and buoyancy generated restoring force (moment) vectors are therefore directly described as:
g(η)＝[0 0 0 -16 0 0]T (1.28)
C. propeller dynamics model
The invention adopts a main thruster forward rotation model, and describes a thruster dynamics model of the underwater robot when the control voltage is unchanged as follows:
here, the control voltage V needs to be adjusted in an actual model according to different robots and dynamic models.
From the knowledge of the relevant system identification, the input optimum angular frequency is
When calculating the time constant of the dynamic model linearization system of the heading degree of freedom, the control voltage is 10V, the input torque is 3.44 N.m, then the Runge-Kutta method is adopted to solve the differential equation, and the one-dimensional angular velocity stable value xi is obtained in the obtained solution00.4719 rad/s. Calculating the time constant and the optimum angular frequency of
In order to ensure that the propeller of the underwater robot does not suddenly rotate reversely in the process of sailing, when the input is the optimal frequency, the input voltage is set to have the following form:
V＝10+5 sin 0.23t (1.32)
if the sampling interval is set to be 1s, sampling is carried out for 350 times in 350s, and the output thrust curve is shown in FIG. 2;
in order to observe the direct and build aspects, the simulation system is built directly by an integrator, and meanwhile, in order to adapt to the sampling times of 350 times, the simulation time length is 350s, and the step length is 1 s. The curve of the input thrust of the heading degree of freedom model is shown in figure 3; the curve of one-dimensional speed and one-dimensional acceleration under the thrust input shown in fig. 3 is obtained by simulation of a heading degree of freedom dynamic model Simulink and is shown in fig. 4.
The longitudinal degree of freedom dynamic model is the same as the heading degree of freedom dynamic model, simplified formulas are adopted, the parameter true value is substituted, and after noise is added, the longitudinal degree of freedom dynamic model is obtained
In the formula,is corresponding to one-dimensional acceleration of degree of freedom, xi is corresponding to one-dimensional speed of degree of freedom, v is noise signal, tauξCorresponding to the degree of freedom thrust (moment).
Longitudinal freedom degree dynamic model in xi0Linearization, when the control voltage is constant at 10V, the input thrust is 3.44N, the Runge-Kutta method is adopted to solve the differential equation, and a one-dimensional speed stable value xi is obtained in the solution00.0951 m/s. Substituting the calculated time constant and the optimum angular frequency to
So as to simulate the degree of freedom of heading, set the input voltage as
V＝10+5 sin 0.12t (1.35)
If the sampling interval is 1s, 350 times of sampling in 350s, the output thrust curve is shown in FIG. 5
The thrust signal shown in figure 5 was simulated in Simulink as an input. In order to observe the direct and build aspects, the simulation system is built directly by an integrator, and meanwhile, in order to adapt to the sampling times of 350 times, the simulation time length is 350s, and the step length is 1 s. The longitudinal degree of freedom dynamic model Simulink simulation system has the same heading degree of freedom, and specific parameters are modified according to the longitudinal degree of freedom dynamic model.
The longitudinal degree of freedom dynamics model Simulink is simulated to obtain a curve of one-dimensional speed and one-dimensional acceleration under thrust input as shown in FIG. 5, which is shown in FIG. 6
In order to further explain the generality of the algorithm, on the basis that the two models with the same simplified process exist in the foregoing, a certain underwater robot forward degree-of-freedom model with slightly different modeling and simplified processes is selected for supplementary explanation. According to the derivation of this document, the model of the dynamics of the underwater robot with three degrees of freedom of forward, heave and yaw neglecting linear resistance is described as follows (into which truth values have been substituted)
In the formula,acceleration (angular acceleration) of three degrees of freedom of forward movement, heave movement and bow turning of the underwater robot is respectively obtained; u, w and r are the speeds (angular speeds) of three degrees of freedom of the underwater robot for advancing, heaving and turning; fx、Fz、TzRespectively, the forces (moments) in the corresponding directions.
The model equation of each degree of freedom is obtained by expansion as follows
For the convenience of simulation identification, the text selects a forward freedom model for simulation
When the control voltage is constant at 10V, the input thrust is 3.44N, the differential equation is solved by the Runge-Kutta method, and a one-dimensional velocity stable value u is obtained from the obtained solution00.37 m/s. Substitute for Chinese traditional medicineCalculating the time constant and inputting the optimum angular frequency of
Thus, let the input voltage be
V＝10+5 sin 0.17t (1.40)
If the sampling interval is set to be 1s, sampling is carried out for 350 times in 350s, and the output thrust curve is shown in FIG. 7; the Simulink simulation system of the forward degree of freedom dynamic model is shown in FIG. 8; the curve of one-dimensional acceleration and one-dimensional velocity of the forward degree of freedom is shown in fig. 9.
To further illustrate the generality of the algorithm, on the basis of the three single-degree-of-freedom dynamic models to be identified, a coupling model of an underwater robot is selected for identification, and the horizontal plane longitudinal model of the underwater robot is described as follows
In the formula, m is the mass of the underwater robot, and the true value is 150 kg; rho is the density of the seawater, and the true value is 1000kg/m3(ii) a L is the length of the underwater robot, and the true value is 1.2 m; u, g and r are respectively the linear velocity and the angular velocity of the underwater robot under the moving coordinate system; t isxInputting thrust for the system; x'uu、X′gg、Xgr、X′rrThe true values of the original parameters to be identified are-0.1250, -0.13853, -0.067593, 0.06690 and-0.03340, respectively.
Substituting the truth value into the equation and simplifying to obtain a model as described below
The model shown in the formula can be used for Simulink simulation, and the model is corrected into a model when the model parameter identification is simulated
When the underwater robot navigates forward, there are only small disturbances in the lateral and yaw directions, so here a small random number is used instead. When the control voltage is constant at 10V, the input thrust is 3.44N, the differential equation is solved by adopting a Runge-Kutta method under the assumption that the transverse speed g is 0.1m/s and the yaw rate r is 0.1rad/s, and a one-dimensional speed stable value u is obtained in the obtained solution00.2154 m/s. So that the linearized system time constant and the input optimum angular frequency are
If the sampling interval is set to be 1s, sampling is performed for 350 times in 350s, and the output thrust curve is shown in fig. 10;
the thrust signal shown in figure 10 was simulated in Simulink as an input. The simulation time length is 350s, and the step length is 1 s. The horizontal plane hydrodynamic model Simulink simulation system is obtained as shown in fig. 11; the one-dimensional acceleration and one-dimensional velocity curve of the hydrodynamic model is shown in fig. 12.
Introduction of the Huber loss function
To integrate the Huber loss function into the Recursive Least Squares algorithm, consider the k-times estimation criterion function before the following Recursive Least Squares (RLS) estimation method
So that it is obtained in a recursion form
Note the bookThenI.e. the k-th estimated residual. Then there is
Wherein ρ (e)k) Namely a Huber loss function, the specific form is as follows:
wherein δ is a tuning parameter. The influence function obtained by differentiating the formula (2.3) is
For minimum value, there are
Order to
Order to
Λ＝diag[u(ek)] (2.7)
Can be deduced from the formulae (2.5), (2.6) and (2.7)
By
Is obtained by a differential equation
Substituted by formula (2.8) to obtain
Get it solved
Compared with an estimation formula of a weighted least square method, the estimation formula can find that after the Huber loss function is introduced, the weight matrix evolves into a form shown in a formula (2.11), and if the weight matrix of the observed data is an identity matrix I, the weight matrix in the estimation formula is a diagonal matrix related to the estimation residual error.
After a weighted estimation formula of the Huber loss function is deduced and introduced, the deduction process of the Recursive Least square method is repeated, and the Recursive Least square method can be integrated into the Recursive of the Recursive Least square method gain K (k), so that the Recursive Least square method (H _ RLS) robust gain based on the Huber loss function can be obtained as follows
In the formula u (e)k) Namely the robust factor of the Huber method.
The adjusting parameter delta in the Huber loss function and the robust factor thereof is the key for the Huber method to realize file estimation, and when delta → ∞, the Huber recursive least square method is converted into a common recursive least square method; when δ → 0, the Huber recursive least squares method evolves to an absolute value estimation method. Typically, when the noise model is gaussian, taking δ to 1.345 can result in a relative efficiency of 95%. The noise distribution of the invention is Gaussian distribution, so the value of the adjusting parameter is 1.345.
Simulation experiment
In order to enable the identification algorithm to obtain reasonable input signals, the experiment simulates the dynamics model of the underwater robot in Simulink, and the simulation result is used as algorithm input. In order to illustrate the robustness of the algorithm, a Recursive Least Square (RLS) algorithm and an H _ RLS algorithm are selected to be used for identifying and simulating a dynamic model of the underwater robot under two noise environments of common Gaussian noise and Gaussian noise containing a field value, and the performance of the H _ RLS algorithm is verified.
Underwater robot dynamics model simulation under Gaussian noise containing outlier
In order to observe the effect of the Huber method in the identification of the noise model containing the outlier, the outlier noise is added into the common Gaussian noise in the simulation. The strategy of adding the noise of the 'outlier' is that a noise outlier signal with the standard deviation of 300 is additionally added into a noise signal with the noise amplitude of below 0.1, the simulation sampling frequency is 300 times, and meanwhile, in order to eliminate the influence caused by random numbers, the final result of the simulation is the average value after 100 rounds.
Under the environment of Gaussian signals with 'outliers', the identification results of the two algorithms RLS and H _ RLS are greatly different. The heading dynamics model estimation error change process is as shown in figure 13; the longitudinal dynamics model estimation error variation process is shown in FIG. 14; the process of the estimation error change of the forward dynamics model is shown in FIG. 15; the process of the change of the horizontal plane model estimation error is shown in fig. 16.
Although both methods eventually yield estimation results, the algorithm is not subject to error due to changes in the noise environment. However, it is not easy to find that the estimation of the RLS algorithm on a plurality of parameters within the simulation times of the RLS algorithm cannot be stabilized at a certain value, and the estimation absolute error also cannot be stabilized and tends to 0; the final estimation result shows that the estimation error of the RLS algorithm for most parameters is extremely large, and the estimation result of individual parameters even shows complete error. The inverse H _ RLS algorithm shows the superiority of the inverse H _ RLS algorithm in robustness under the noise environment, the estimation quickly stabilizes near the true value after the estimation starts, the estimation absolute error also quickly approaches to 0, the relative error of the final estimation value is still within +/-2%, and the estimation relative error of most parameters is within +/-1%.
Heading dynamics model identification result (with wild value Gaussian noise)
Longitudinal dynamics model identification result (with wild value Gaussian noise)
Forward dynamics model identification results (with wild value Gaussian noise)
Horizontal plane model identification result (Gauss noise with 'outlier')
It will be understood that modifications and variations can be made by persons skilled in the art in light of the above teachings and all such modifications and variations are intended to be included within the scope of the invention as defined in the appended claims.
Claims (4)
1. A Huber M estimation-based underwater robot dynamic model parameter identification method is characterized by comprising the following steps:
1) establishing an original 6-degree-of-freedom coupling dynamic model of the underwater robot, simplifying the model, and decoupling to obtain a simplified single-degree-of-freedom dynamic model of the underwater robot required by algorithm identification;
2) determining a kinetic parameter theta to be identified according to the established model;
3) identifying a parameter estimation value of a dynamic model of the underwater robot by adopting a recursive least square method based on a Huber loss function;
4) updating the current state value of the underwater robot according to the parameter estimation value of the underwater robot dynamic model obtained by identification;
5) and (4) repeating the steps 2) to 4) to obtain the parameter estimation value of each sampling moment, and taking the average value as the identification result.
2. The Huber M estimation-based underwater robot kinetic model parameter identification method as claimed in claim 1, wherein in the step 1), the simplified underwater robot single-degree-of-freedom kinetic model is:
wherein,
M＝MRB+MA；
MRB＝diag{m 0 m 0 0 Iz}；
D(v)＝diag{Xu+Xu|u||u| 0 Zw+Zw|w||w| 0 0 Nr+Nr|r||r|}；
g(η)＝[0 0 0 -16 0 0]T
where M is a mass and inertia matrix comprising rigid body masses and an inertia matrix MRBHydrodynamic additional mass matrix MA(ii) a m is the mass of the underwater robot; i is an inertia term of the underwater robot, IzThe inertial term of the underwater robot in the z direction;
representing a first direction;represents a longitudinal direction;represents the normal direction;are linear and angular acceleration vectors; v is the linear velocity and angular velocity vector of the underwater robot under the motion coordinate system; g (η) is a restoring force (moment) vector generated by gravity and buoyancy; d (v) is a fluid resistance matrix; tau is the resultant force and resultant moment vector of the propeller and the underwater robot subjected to underwater interference, and J (eta) is a conversion matrix between a fixed coordinate system and a moving coordinate system.
3. The Huber M estimation-based underwater robot dynamic model parameter identification method as claimed in claim 1, wherein the step 3) adopts a recursive least square method based on a Huber loss function to identify the parameter estimation value of the underwater robot dynamic model,
the estimation formula of the recursive least square method based on the Huber loss function for identifying the parameters is as follows:
robust gain K of recursive least square method based on Huber loss functionHuber(k) The following were used:
in the formula, u (e)k) Is a robust factor of the Huber method.
4. The Huber M estimation-based underwater robot dynamics model parameter identification method of claim 3, wherein an adjustment parameter δ of the robust factor takes a value of 1.345.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910518943.6A CN110334411A (en) | 2019-06-16 | 2019-06-16 | A kind of underwater robot kinetic parameters discrimination method based on Huber M estimation |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910518943.6A CN110334411A (en) | 2019-06-16 | 2019-06-16 | A kind of underwater robot kinetic parameters discrimination method based on Huber M estimation |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN110334411A true CN110334411A (en) | 2019-10-15 |
Family
ID=68140652
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201910518943.6A Pending CN110334411A (en) | 2019-06-16 | 2019-06-16 | A kind of underwater robot kinetic parameters discrimination method based on Huber M estimation |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN110334411A (en) |
Cited By (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112487555A (en) * | 2020-11-29 | 2021-03-12 | 西北工业大学 | Dimensionless resistance coefficient identification method for water-air amphibious submersible |
| CN113189865A (en) * | 2021-01-27 | 2021-07-30 | 上海电气智能康复医疗科技有限公司 | Rehabilitation robot control method, system, equipment and medium based on kinetic parameter identification |
| CN113341373A (en) * | 2021-05-31 | 2021-09-03 | 中国电子科技集团公司第三十六研究所 | Positioning method and device and electronic equipment |
| CN114675644A (en) * | 2022-03-21 | 2022-06-28 | 中国科学院沈阳自动化研究所 | Underwater robot vertical plane motion control method based on parameter identification |
| CN114912280A (en) * | 2022-05-19 | 2022-08-16 | 深圳市人工智能与机器人研究院 | System identification method for omnidirectional common ship body and related equipment |
| CN115167486A (en) * | 2022-07-19 | 2022-10-11 | 中国科学院沈阳自动化研究所 | Online hydrodynamic parameter identification method suitable for underwater robot |
| CN115167486B (en) * | 2022-07-19 | 2024-10-18 | 中国科学院沈阳自动化研究所 | Online hydrodynamic parameter identification method suitable for underwater robot |
Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103034123A (en) * | 2012-12-11 | 2013-04-10 | 中国科学技术大学 | Dynamic model parameter identification based parallel robot control method |
| CN108714896A (en) * | 2018-05-28 | 2018-10-30 | 华南理工大学 | A kind of robot flexibility torque feed forward control method based on kinetics of deformable bodies model |
-
2019
- 2019-06-16 CN CN201910518943.6A patent/CN110334411A/en active Pending
Patent Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103034123A (en) * | 2012-12-11 | 2013-04-10 | 中国科学技术大学 | Dynamic model parameter identification based parallel robot control method |
| CN108714896A (en) * | 2018-05-28 | 2018-10-30 | 华南理工大学 | A kind of robot flexibility torque feed forward control method based on kinetics of deformable bodies model |
Non-Patent Citations (2)
| Title |
|---|
| 刘亚龙: "多水下航行器协同定位算法研究", 《中国博士学位论文全文数据库工程科技Ⅱ辑》 * |
| 朱红坤等: "基于多传感器递推总体最小二乘融合的水下机器人动力学模型参数辨识", 《船舶力学》 * |
Cited By (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112487555A (en) * | 2020-11-29 | 2021-03-12 | 西北工业大学 | Dimensionless resistance coefficient identification method for water-air amphibious submersible |
| CN112487555B (en) * | 2020-11-29 | 2024-01-09 | 西北工业大学 | Non-dimensional resistance coefficient identification method for water-air amphibious submersible |
| CN113189865A (en) * | 2021-01-27 | 2021-07-30 | 上海电气智能康复医疗科技有限公司 | Rehabilitation robot control method, system, equipment and medium based on kinetic parameter identification |
| CN113189865B (en) * | 2021-01-27 | 2024-03-26 | 上海际知医疗科技有限公司 | Rehabilitation robot control system based on dynamic parameter identification |
| CN113341373A (en) * | 2021-05-31 | 2021-09-03 | 中国电子科技集团公司第三十六研究所 | Positioning method and device and electronic equipment |
| CN113341373B (en) * | 2021-05-31 | 2024-05-14 | 中国电子科技集团公司第三十六研究所 | Positioning method and device and electronic equipment |
| CN114675644A (en) * | 2022-03-21 | 2022-06-28 | 中国科学院沈阳自动化研究所 | Underwater robot vertical plane motion control method based on parameter identification |
| CN114912280A (en) * | 2022-05-19 | 2022-08-16 | 深圳市人工智能与机器人研究院 | System identification method for omnidirectional common ship body and related equipment |
| CN115167486A (en) * | 2022-07-19 | 2022-10-11 | 中国科学院沈阳自动化研究所 | Online hydrodynamic parameter identification method suitable for underwater robot |
| CN115167486B (en) * | 2022-07-19 | 2024-10-18 | 中国科学院沈阳自动化研究所 | Online hydrodynamic parameter identification method suitable for underwater robot |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110334411A (en) | A kind of underwater robot kinetic parameters discrimination method based on Huber M estimation | |
| CN108803632B (en) | Water surface unmanned ship all-state constraint trajectory tracking control method based on saturation compensation technology | |
| CN111736600B (en) | Track tracking control method for unmanned surface vehicle under time-lag asymmetric time-varying all-state constraint | |
| CN111650948B (en) | Quick tracking control method for horizontal plane track of benthonic AUV | |
| CN111736617B (en) | Track tracking control method for preset performance of benthonic underwater robot based on speed observer | |
| CN109100939B (en) | Input saturation considered water surface unmanned ship all-state constraint trajectory tracking control method | |
| CN104898688B (en) | The adaptive anti-interference System with Sliding Mode Controller of UUV four-degree-of-freedom dynamic positionings and control method | |
| CN108008628A (en) | A kind of default capabilities control method of uncertain drive lacking unmanned boat system | |
| CN113238567B (en) | Benthonic AUV weak buffeting integral sliding mode point stabilizing control method based on extended state observer | |
| CN112947505B (en) | Multi-AUV formation distributed control method based on reinforcement learning algorithm and unknown disturbance observer | |
| CN114115262B (en) | Multi-AUV actuator saturation cooperative formation control system and method based on azimuth information | |
| CN112631316B (en) | Limited time control method of variable-load quad-rotor unmanned aerial vehicle | |
| CN109946976B (en) | Wide-navigational-speed AUV motion control method | |
| CN112965371A (en) | Water surface unmanned ship track rapid tracking control method based on fixed time observer | |
| CN114442640A (en) | Track tracking control method for unmanned surface vehicle | |
| CN113608534A (en) | Unmanned ship tracking control method and system | |
| CN107608210B (en) | Input saturated spacecraft attitude terminal sliding mode tracking control method | |
| Jing et al. | Self-tuning adaptive active disturbance rejection pitch control of a manta-ray-like underwater glider | |
| CN113110512B (en) | Benthonic AUV self-adaptive trajectory tracking control method for weakening unknown interference and buffeting influence | |
| Wang et al. | Adaptive fuzzy sliding mode controller for dynamic positioning of FPSO vessels | |
| CN115686034B (en) | Unmanned submarine trajectory tracking control method considering failure of speed sensor | |
| CN112506052B (en) | Control method for resisting rotational interference of holder of underwater archaeological robot | |
| CN113406884B (en) | Sliding mode self-adaptation-based multi-point mooring system positioning control method | |
| Ngongi et al. | Design of generalised predictive controller for dynamic positioning system of surface ships | |
| Yu | Saturated backstepping control for boat with disturbance estimator |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| RJ01 | Rejection of invention patent application after publication | ||
| RJ01 | Rejection of invention patent application after publication |
Application publication date: 20191015 |