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
- model
- kinetic
- parameters
- 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 claims abstract description 8
- 238000000034 method Methods 0.000 claims abstract description 52
- 238000005070 sampling Methods 0.000 claims abstract description 9
- 239000011159 matrix material Substances 0.000 claims description 44
- 238000004422 calculation algorithm Methods 0.000 claims description 16
- 230000005484 gravity Effects 0.000 claims description 15
- 230000001133 acceleration Effects 0.000 claims description 14
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 claims description 9
- 230000007704 transition Effects 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
- 238000011156 evaluation Methods 0.000 claims 1
- 230000008859 change Effects 0.000 description 11
- 238000010586 diagram Methods 0.000 description 11
- 238000004088 simulation Methods 0.000 description 5
- 230000008569 process Effects 0.000 description 4
- 238000011105 stabilization Methods 0.000 description 3
- 230000000694 effects Effects 0.000 description 2
- 238000002474 experimental method Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 241000208340 Araliaceae Species 0.000 description 1
- 235000005035 Panax pseudoginseng ssp. pseudoginseng Nutrition 0.000 description 1
- 235000003140 Panax quinquefolius Nutrition 0.000 description 1
- 244000141353 Prunus domestica Species 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 238000006243 chemical reaction Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000009795 derivation Methods 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 235000008434 ginseng Nutrition 0.000 description 1
- 230000007257 malfunction Effects 0.000 description 1
- 230000009467 reduction Effects 0.000 description 1
- 238000012552 review Methods 0.000 description 1
- 239000013535 sea water Substances 0.000 description 1
- 238000006467 substitution reaction Methods 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 present invention relates to model parameter identification method more particularly to a kind of underwater robot based on Huber M estimation are dynamic
Mechanical model parameter identification method.
Background technique
Interfere under water extremely complex, Complex Noise caused by water flow, noise cannot be fully described in simple Gaussian noise
Mean value not can guarantee be zero, standard deviation is also impossible to permanent be constant.Many studies have shown that under simple Gauusian noise jammer,
Least square method effect can be obvious, but (standard deviation is great if occurring a certain proportion of " outlier " in interference noise
Noise outlier), the identification of model parameter, meeting are still carried out using common least square method or least square method of recursion at this time
There is the case where estimated result inaccuracy even dissipates, directly results in Model Distinguish robustness variation.
Summary of the invention
The technical problem to be solved in the present invention is that for the defects in the prior art, providing one kind and being estimated based on Huber M
The underwater robot kinetic parameters discrimination method of meter.
The technical solution adopted by the present invention to solve the technical problems is: a kind of underwater based on Huber M estimation
Human occupant dynamic model parameter identification method, comprising the following steps:
1) it establishes original underwater robot 6DOF Coupling Dynamic Model and it is simplified, decoupling acquires calculation
Simplification underwater robot single-degree-of-freedom kinetic model needed for method identification；
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 ginseng of underwater human occupant dynamic model is recognized
Number estimated value；
4) estimates of parameters of the underwater human occupant dynamic model obtained according to identification, updates the current of underwater robot
State value；
5) step 3) is repeated to 4), obtaining the estimates of parameters of each sampling instant, and is averaged as identification result.
According to the above scheme, in the step 1), simplified underwater robot single-degree-of-freedom kinetic model are as follows:
Wherein,
M=MRB+MA；
MRB=diag { 0 m of m, 00 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
Wherein, M is quality and inertial matrix, which includes Rigid Mass and inertial matrix MRB, hydrodynamic force additional mass
Matrix MA；M is the quality of underwater robot；I is the inertia item of underwater robot,
IzFor the inertia item of underwater robot on the direction z；Indicate it is first to；Indicate longitudinal；
Indicate normal direction；It is linear acceleration and angular acceleration vector；V is the line of the underwater robot under kinetic coordinate system
Speed and angular velocity vector；G (η) is restoring force (torque) vector generated by gravity and buoyancy；D (v) is fluid resistance matrix；
τ is propeller and underwater robot by the resultant force and resultant moment vector interfered in water, and J (η) is that fixed coordinate system and movement are sat
Transition matrix between mark system.
According to the above scheme, the least square method of recursion based on Huber loss function is used in the step 3), identification is underwater
The estimates of parameters of Dynamic Models of Robot Manipulators,
Wherein, the estimation formulas of the least square method of recursion based on Huber loss function of identified parameters are as follows:
The steady gain K of least square method of recursion based on Huber loss functionHuber(k) as follows:
In formula, u (ek) be Huber method conservatism factor.
According to the above scheme, the adjustment parameter δ value of the conservatism factor is 1.345.
The beneficial effect comprise that:
The present invention incorporates Huber loss function in the identification algorithm being applied under normal noise interference, so that algorithm exists
Still can be stable under " outlier " (the great noise outlier of standard deviation) noise circumstance pick out parameter to be identified, have more
High identification precision and robustness.
Detailed description of the invention
Present invention will be further explained below with reference to the attached drawings and examples, in attached drawing:
Fig. 1 is the method flow diagram of the embodiment of the present invention；
Fig. 2 is that the bow of the embodiment of the present invention inputs thrust curve figure to Degrees of Freedom Model；
Fig. 3 is the bow of the embodiment of the present invention to freedom degree kinetic model Simulink analogue system schematic diagram；
Fig. 4 is the bow of the embodiment of the present invention to the one-dimensional acceleration of freedom degree and one-dimensional speed curve diagram；
Fig. 5 is the longitudinal degress of feedom mode input thrust curve figure of the embodiment of the present invention；
Fig. 6 is the one-dimensional acceleration of the longitudinal degress of feedom and one-dimensional speed curve diagram of the embodiment of the present invention；
Fig. 7 is the advance Degrees of Freedom Model input thrust curve figure of the embodiment of the present invention；
Fig. 8 is the advance freedom degree kinetic model Simulink analogue system schematic diagram of the embodiment of the present invention；
Fig. 9 is the one-dimensional acceleration of advance freedom degree and one-dimensional speed curve diagram of the embodiment of the present invention；
Figure 10 is the horizontal plane hydrodynamic model input thrust curve figure of the embodiment of the present invention；
Figure 11 is the horizontal plane hydrodynamic model Simulink analogue system schematic diagram of the embodiment of the present invention；
Figure 12 is the one-dimensional acceleration of horizontal plane hydrodynamic model and one-dimensional speed curve diagram of the embodiment of the present invention；
Figure 13 is the bow containing " outlier " Gaussian noise of the embodiment of the present invention to kinetic model evaluated error change procedure
Schematic diagram；
Figure 14 is the Longitudinal Dynamic Model evaluated error change procedure containing " outlier " Gaussian noise of the embodiment of the present invention
Schematic diagram；
Figure 15 is the onward impulse model evaluated error change procedure containing " outlier " Gaussian noise of the embodiment of the present invention
Schematic diagram；
Figure 16 is the horizontal surface model evaluated error change procedure signal containing " outlier " Gaussian noise of the embodiment of the present invention
Figure.
Specific embodiment
In order to make the objectives, technical solutions, and advantages of the present invention clearer, with reference to embodiments, to the present invention
It is further elaborated.It should be appreciated that described herein, specific examples are only used to explain the present invention, is not used to limit
The fixed present invention.
As shown in Figure 1, a kind of underwater robot kinetic parameters discrimination method based on Huber M estimation, including
Following steps:
1. establishing basic model:
A. the foundation of underwater human occupant dynamic model
After establishing corresponding coordinate system and establishing its Conversion Relations, underwater robot 6DOF kinetic simulation
Type can be described according to the Newton-Euler equation of motion of rigid body are as follows:
In formula, M is quality and inertial matrix, which includes Rigid Mass and inertial matrix MRB, hydrodynamic force additional mass
Matrix MA；It is linear acceleration and angular acceleration vector；V is the linear velocity and angular speed of the underwater robot under kinetic coordinate system
Vector；C (v) is total Coriolis and centripetal force matrix, including underwater robot rigid body Coriolis and centripetal force Matrix CRB(v) and by adding
Mass inertia matrix MACaused similar coriolis force Matrix CA(v)；D (v) is fluid resistance matrix, including linear resistance coefficient DL
With secondary resistance DQ；G (η) is restoring force (torque) vector generated by gravity and buoyancy；τ be propeller and underwater robot by
The resultant force and resultant moment vector interfered into water；Transition matrix of the J (η) between fixed coordinate system and kinetic coordinate system.In addition,
Each vector and matrix meet following relationship:
1) quality and inertial matrix meet:
M=MRB+MA (1.3)
In formula, m is the quality of underwater robot, and I is the inertia item of underwater robot, and rigid body inertia matrix MRBParameter
It is unique, and meets following formula:
And additional mass matrix MAParameter can change with the shape of underwater robot and change, but when underwater robot is latent
The parameter of the matrix is constant when entering in water.
2) Coriolis and centripetal force matrix meet:
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 kinetic coordinate system is under water in the center of gravity of robot, underwater robot rigid body Coriolis and centripetal torque
Battle array CRB(v) it can be described as:
Likewise, the similar coriolis force Matrix C as caused by additional mass inertial matrixA(v) it can be described as:
In formula:
3) fluid resistance matrix meets:
D (v)=diag { DL+DQ|v|} (1.18)
In 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) restoring force (torque) vector that gravity and buoyancy generate meets:
If W is gravity, B is buoyancy, rGFor center of gravity, rBFor centre of buoyancy, then center of gravity is r in kinetic coordinate system coordinateG=[xG yG
zG]=[0 0 0]；Centre of buoyancy is r in kinetic system coordinateB=[xB yB zB]；Restoring force (torque) vector that gravity and buoyancy generate
It can be described as:
5) transition matrix can be described as follows:
Linear velocity is transformed into the transition matrix J of fixed coordinate system by kinetic coordinate system1Are as follows:
Angular speed is transformed into the transition matrix J of fixed coordinate system by kinetic coordinate system2Are as follows:
It is then respectively corresponding inverse that linear velocity has fixed coordinate system to be transformed into the transition matrix of kinetic coordinate system with angular speed
Matrix.
B. the simplification of underwater human occupant dynamic model
1) simplification of mass and inertial matrix
Since the physical appearance structure of the underwater robot is symmetrical about three sections, while its center of gravity is the coordinates of motion
It is origin, therefore Rigid Mass and inertial matrix MRBWith hydrodynamic force additional mass matrix MAIt can simplify are as follows:
MRB=diag { 0 m of m, 00 Iz} (1.24)
2) simplification of Coriolis and centripetal force matrix
Since the underwater robot route speed is slower, within 1 section, therefore Coriolis and centripetal force can directly be given up
It goes, therefore C (v)=0, kinetic model simplify are as follows:
3) simplification of fluid resistance matrix
Also due to the reason of underwater robot symmetry and position of centre of gravity, fluid resistance matrix reduction are as follows:
4) simplification of gravity and buoyancy vector
Ignore and moved in the traversing freedom degree direction of the underwater robot, while the underwater underwater robot shakes certainly longitudinal and transverse
As movement righting moment as caused by natural buoyancy and gravity control on degree direction, so that roll angle and pitch angle are kept most
It is small.Under these features, the barycentric coodinates of the underwater robot are rG=[0 0 0], centre of buoyancy coordinate are rB=[0 0-0.1]T,
That is centre of buoyancy is also in z-axis.The underwater robot gravity is 1166 newton, and buoyancy is 1182 newton.Therefore gravity and buoyancy generate
Restoring force (torque) vector directly describe are as follows:
G (η)=[0 0 0-16 0 0]T (1.28)
C. propeller kinetic model
The present invention rotates forward model using main thrust device, by underwater robot thruster kinetic model when control voltage is constant
It is described as follows:
Control voltage V needs to be adjusted according to different robot and kinetic model in realistic model herein.
Knowledge is recognized by related system it is found that the best angular frequency of its input is
When calculating bow to freedom degree kinetic model linearized system time constant, taking control voltage is 10V, is inputted at this time
Torque is 3.44Nm, is then solved using Runge-Kutta method to the differential equation, takes one-dimensional angular speed steady in gained solution
Definite value ξ0=0.4719rad/s.It calculates time constant and best angular frequency is
In order to guarantee that underwater robot will not invert suddenly in navigation process propeller, when input is optimum frequency, if
Input voltage has following form:
V=10+5 sin 0.23t (1.32)
If setting the sampling interval as 1s, sampled altogether in 350s 350 times, thrust output curve is as shown in Figure 2；
In order to observe directly and build aspect, analogue system directly built by integrator, while being adopted to adapt to 350 times
Sample number, a length of 350s, step-length 1s when emulating herein.Bow is as shown in Figure 3 to Degrees of Freedom Model input thrust curve；Bow is to freedom
Degree kinetic model Simulink emulates to obtain one-dimensional speed and one-dimensional accelerating curve such as Fig. 4 institute under the input of thrust shown in Fig. 3
Show.
Longitudinal degress of feedom kinetic model is identical to freedom degree kinetic model as bow, is all made of simplified style, substitutes into parameter
True value and after noise is added, obtaining longitudinal degress of feedom kinetic model is
In formula,For the corresponding one-dimensional acceleration of freedom degree, ξ is the corresponding one-dimensional speed of freedom degree, and v is noise signal, τξIt is right
Answer freedom degree thrust (torque).
Longitudinal degress of feedom kinetic model is in ξ0Place's linearisation is 10V when control voltage is constant, and inputting thrust at this time is
3.44N solves the differential equation using Runge-Kutta method, and one-dimensional velocity-stabilization value ξ is taken in gained solution0=
0.0951m/s.It substitutes into calculating time constant and best angular frequency is
Therefore analogy bow is to freedom degree, if input voltage is
V=10+5 sin 0.12t (1.35)
If setting the sampling interval as 1s, sampled altogether in 350s 350 times, thrust output curve is as shown in Figure 5
It is emulated thrust signal shown in Fig. 5 as input in Simulink.In order to observe directly and build aspect,
Analogue system is directly built by integrator, while in order to adapt to 350 sampling numbers, a length of 350s, step-length when emulating herein
1s.For longitudinal degress of feedom kinetic model Simulink analogue system with bow to freedom degree, design parameter is dynamic according to the longitudinal degress of feedom
Mechanical model modification.
Longitudinal degress of feedom kinetic model Simulink emulates to obtain one-dimensional speed under the input of thrust shown in Fig. 5 and adds with one-dimensional
Rate curve is as shown in Figure 6
The generality of algorithm in order to further illustrate the present invention, there are two the bases of the identical model of the process of simplification above
On plinth, then chooses modeling and simplify certain slightly distinguishing underwater robot advance Degrees of Freedom Model of process and remark additionally.Root
According to the derivation of the document, the underwater human occupant dynamic model in the case where ignoring linear resistance its advance, heave and turn bow
The model of three degree of freedom is described as follows and (substitutes into true value wherein)
In formula,Respectively the underwater robot advances, heaves, turning, and (angle adds for the acceleration of bow three degree of freedom
Speed)；U, w, r are respectively the speed (angular speed) that the underwater robot advances, heaves, turning bow three degree of freedom；Fx、Fz、TzPoint
It Wei not power (torque) on corresponding direction.
The model equation that expansion obtains each freedom degree is as follows
For the convenience of simulation recognition, advance Degrees of Freedom Model is chosen herein and is emulated
It is 10V when control voltage is constant, inputting thrust at this time is 3.44N, is carried out using Runge-Kutta method to the differential equation
It solves, one-dimensional velocity-stabilization value u is taken in gained solution0=0.37m/s.It substitutes into calculate time constant and input best angular frequency and is
Therefore, if input voltage is
V=10+5 sin 0.17t (1.40)
If setting the sampling interval as 1s, sampled altogether in 350s 350 times, thrust output curve is as shown in Figure 7；Advance freedom degree is dynamic
Mechanical model Simulink analogue system is as shown in Figure 8；The one-dimensional acceleration of advance freedom degree and one-dimensional rate curve such as Fig. 9 institute
Show.
In order to further explain the generality of this algorithm, it is dynamic that three single-degree-of-freedoms to be identified are had been described above above
It on the basis of mechanical model, then chooses the coupling model of certain underwater robot and is recognized, which is indulged
It is described as follows to model
In formula, m is underwater robot quality, true value 150kg；ρ is density of sea water, true value 1000kg/m3；L is water
Lower robot length, true value 1.2m；U, g, r are respectively linear velocity and angular speed of the underwater robot under moving coordinate system；Tx
Thrust is inputted for system；X′uu、X′gg、Xgr、X′rrFor original parameter to be identified, true value is respectively -0.1250, -
0.13853、-0.067593、0.06690、-0.03340。
True value is substituted into equation and abbreviation obtains model and is described as follows
It can will be used for Simulink emulation such as the formula institute representation model, and be then by Modifying model when identification of Model Parameters emulation
When underwater robot navigates by water forward, laterally there is only microvariations with yaw direction, therefore herein with lesser random
Number replaces.It is 10V when control voltage is constant, inputting thrust at this time is 3.44N, it is assumed that lateral velocity g=0.1m/s, yaw angle speed
R=0.1rad/s is spent, the differential equation is solved using Runge-Kutta method, one-dimensional velocity-stabilization value u is taken in gained solution0
=0.2154m/s.Therefore system time constant and the best angular frequency of input are after linearizing
If setting the sampling interval as 1s, sampled altogether in 350s 350 times, thrust output curve is as shown in Figure 10；
It is emulated thrust signal shown in Figure 10 as input in Simulink.A length of 350s when emulation, step-length 1s.
It is as shown in figure 11 to obtain horizontal plane hydrodynamic model Simulink analogue system；The one-dimensional acceleration of hydrodynamic model and one-dimensional speed
Curve is as shown in figure 12.
The introducing of 2.Huber loss function
In order to which Huber loss function is incorporated recursive least squares algorithm, following least square method of recursion is considered
K estimation criterion function before (Recursive Least Squares, RLS) estimation method
Therefore it obtains its recursive form and is
NoteThenAs kth time estimation residual error.Then have
In formula, ρ (ek) it is Huber loss function, concrete form is as follows:
In formula, δ is adjustment parameter.Formula (2.3) differential, which can be obtained its influence function, is
To seek its minimum, then have
It enables
It enables
Λ=diag [u (ek)] (2.7)
It can be released by formula (2.5), (2.6), (2.7)
By
Differential formulas obtains
Substitution formula (2.8)
It solves
It can be found that introducing after Huber loss function, weight matrix develops the estimation formulas of contrast weight least square method
For form shown in formula (2.11), if the weight matrix of observation data is unit matrix I, the weight matrix in estimation formulas be with
Estimate the relevant diagonal matrix of residual error.
After pushing away to introduce the Weighted estimation formula of Huber loss function, as long as repeating pushing away for least square method of recursion
Process is led, it can be dissolved into the stepping type of least square method of recursion gain K (k), can acquire and letter is lost based on Huber
Several least square method of recursion (Huber Recursive Least Squares, H_RLS) steady gains is as follows
U (e in formulak) be Huber method conservatism factor.
Adjustment parameter δ in Huber loss function and its conservatism factor is the pass that Huber method can be realized file estimation
Key, as δ → ∞, Huber least square method of recursion develops into common least square method of recursion；As δ → 0, Huber recursion is most
Small square law develops into the absolute value estimation technique.In general, taking δ=1.345 available 95% when noise model is Gaussian Profile
Relative efficiency.Noise profile of the present invention is Gaussian Profile, therefore adjustment parameter value is 1.345.
Emulation experiment
In order to enable identification algorithm obtains relatively reasonable input signal, this experiment is in Simulink to underwater robot
Kinetic model is emulated, and is inputted simulation result as algorithm.In order to illustrate the robustness of algorithm, select in normal Gaussian
Noise and containing using least square method of recursion (Recursive Least under two kinds of noise circumstances of " outlier " Gaussian noise
Squares, RLS), two kinds of algorithms of H_RLS identification and simulation is carried out to the kinetic model of underwater robot, verify H_RLS algorithm
Performance.
It is emulated containing Dynamic Models of Robot Manipulators underwater under " outlier " Gaussian noise
It is observation Huber method containing the effect in the identification of " outlier " noise model, this emulation is in normal Gaussian noise
" outlier " noise is added.The addition strategy of " outlier " noise is additionally to be added in 0.1 noise signal below in noise amplitude
The noise that standard deviation is 300 peels off signal, and emulation sampling number is 300 times, while influencing to eliminate random number bring, this
The last result of secondary emulation takes the average value after 100 wheels.
In the case where there is the gaussian signal environment of " outlier ", there is larger difference in the identification result of two kinds of algorithms of RLS, H_RLS.
Bow is to kinetic model evaluated error change procedure such as Figure 13；Longitudinal Dynamic Model evaluated error change procedure such as Figure 14 institute
Show；Onward impulse model evaluated error change procedure is as shown in figure 15；Horizontal surface model evaluated error change procedure such as Figure 16
It is shown.
Although two methods finally have the generation of estimated result, algorithm does not malfunction because of the change of noise circumstance.
But it is not difficult to find that RLS algorithm can not stablize in some value the estimation of multiple parameters in this paper simulation times, estimation is absolute
Error also can not be stable tend to 0；From the point of view of final estimated result, RLS algorithm is very big to the evaluated error of most parameters, a
Even there is full of prunes situation in the estimated result of other parameter.It reviews H_RLS algorithm and embodies it under this noise circumstance
Superiority in terms of robustness, for immediate stability near true value, estimation absolute error is also to tend to rapidly after estimation starts
0, the relative error of final estimated value is still within ± 2% and the estimation relative error of most parameters is within ± 1%.
Bow is to kinetic model identification result (contain " outlier " Gaussian noise)
Longitudinal Dynamic Model identification result (contains " outlier " Gaussian noise)
Onward impulse model identification result (contains " outlier " Gaussian noise)
Horizontal plane Model Distinguish result (contains " outlier " Gaussian noise)
It should be understood that for those of ordinary skills, it can be modified or changed according to the above description,
And all these modifications and variations should all belong to the protection domain of appended claims of the present invention.
Claims (4)
1. it is a kind of based on Huber M estimation underwater robot kinetic parameters discrimination method, which is characterized in that including with
Lower step:
1) it establishes original underwater robot 6DOF Coupling Dynamic Model and it is simplified, decoupling acquires algorithm and distinguishes
Simplification underwater robot single-degree-of-freedom kinetic model needed for knowing；
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 parameter of identification underwater human occupant dynamic model is estimated
Evaluation；
4) estimates of parameters of the underwater human occupant dynamic model obtained according to identification, updates the current state of underwater robot
Value；
5) step 2) is repeated to 4), obtaining the estimates of parameters of each sampling instant, and is averaged as identification result.
2. the underwater robot kinetic parameters discrimination method according to claim 1 based on Huber M estimation,
It is characterized in that, in the step 1), simplified underwater robot single-degree-of-freedom kinetic model are as follows:
Wherein,
M=MRB+MA；
MRB=diag { 0 m of m, 00 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
Wherein, M is quality and inertial matrix, which includes Rigid Mass and inertial matrix MRB, hydrodynamic force additional mass matrix
MA；M is the quality of underwater robot；I is the inertia item of underwater robot, IzFor the inertia item of underwater robot on the direction z；
Indicate it is first to；Indicate longitudinal；Indicate normal direction；It is linear acceleration and angular acceleration vector；V is the coordinates of motion
The linear velocity and angular velocity vector of underwater robot under system；G (η) be from the restoring force (torque) that gravity and buoyancy generate to
Amount；D (v) is fluid resistance matrix；τ is propeller and underwater robot by the resultant force and resultant moment vector interfered in water, J
The transition matrix of (η) between fixed coordinate system and kinetic coordinate system.
3. the underwater robot kinetic parameters discrimination method according to claim 1 based on Huber M estimation,
It is characterized in that, the least square method of recursion based on Huber loss function is used in the step 3), recognize underwater robot power
The estimates of parameters of model is learned,
Wherein, the estimation formulas of the least square method of recursion based on Huber loss function of identified parameters are as follows:
The steady gain K of least square method of recursion based on Huber loss functionHuber(k) as follows:
In formula, u (ek) be Huber method conservatism factor.
4. the underwater robot kinetic parameters discrimination method according to claim 3 based on Huber M estimation,
It is characterized in that, the adjustment parameter δ value of the conservatism factor is 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 (3)
| 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 |
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 (6)
| 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 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109283941A (en) | Default capabilities seabed flight node-locus tracking and controlling method based on disturbance observer | |
| CN104898688B (en) | The adaptive anti-interference System with Sliding Mode Controller of UUV four-degree-of-freedom dynamic positionings and control method | |
| CN110334411A (en) | A kind of underwater robot kinetic parameters discrimination method based on Huber M estimation | |
| CN107168312A (en) | A kind of space tracking tracking and controlling method of compensation UUV kinematics and dynamic disturbance | |
| CN109240316A (en) | Consider the seabed flight node default capabilities Trajectory Tracking Control method of propeller output saturation | |
| CN105807789A (en) | UUV control method based on T-S fuzzy observer compensation | |
| CN109507890A (en) | A kind of unmanned plane dynamic inverse generalized predictive controller based on ESO | |
| CN109343347A (en) | A kind of Trajectory Tracking Control method of seabed flight node | |
| Barisic et al. | Sigma-point Unscented Kalman Filter used for AUV navigation | |
| Karmozdi et al. | Implementation of translational motion dynamics for INS data fusion in DVL outage in underwater navigation | |
| CN112015086B (en) | Feedback control method for limited-time path tracking output of under-actuated surface ship | |
| Bryne et al. | Nonlinear observer with time-varying gains for inertial navigation aided by satellite reference systems in dynamic positioning | |
| CN109858137A (en) | It is a kind of based on the complicated maneuvering-vehicle track estimation method that can learn Extended Kalman filter | |
| CN109708670A (en) | The noise compensation method of polar region Transfer Alignment deflection deformation based on improved Sage-Husa adaptive Kalman filter | |
| CN106802143A (en) | A kind of hull deformation angle measuring method based on inertial instruments and Iterative-Filtering Scheme | |
| Li et al. | Modeling and control of tethered undersea kites | |
| CN109521798A (en) | AUV motion control method based on finite time extended state observer | |
| CN112327883B (en) | Under-actuated surface vessel path tracking control method based on extended state observer | |
| CN103616026B (en) | A kind of AUV control model based on H ∞ filtering is assisted inertial navigation Combinated navigation method | |
| CN109029499A (en) | A kind of accelerometer bias iteration optimizing estimation method based on gravity apparent motion model | |
| CN107036595A (en) | Deformation of hull angular estimation method based on interacting multiple model filters | |
| CN110186483A (en) | The method for improving inertial guidance spacecraft impact accuracy | |
| Limache et al. | Aerodynamic sensitivity theory for rotary stability derivatives | |
| Bouadi et al. | Flight path tracking based-on direct adaptive sliding mode control | |
| Harris | Model-based cooperative acoustic navigation and parameter identification for underactuated underwater vehicles |
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 |