A method for identifying parameters of underwater robot vertical motion model

Abstract

The present invention discloses a method for identifying parameters of a vertical plane motion model of an underwater robot, comprising: S1, establishing a vertical plane motion model of the underwater robot according to the motion characteristics of the underwater robot; S2, discretizing the vertical plane motion model to obtain a discretized vertical plane motion model; S3, performing differential processing on the discretized vertical plane motion model, and obtaining a parameter identification model based on the vertical plane motion model after differential processing; S4, performing parameter identification based on a least square method of multiple new information and the parameter identification model to identify model parameters; S5, verifying the identified model parameters using the discretized vertical plane motion model, outputting an identification result if the error is less than or equal to a preset error, and returning to S2 if the error is greater than the preset error. The least square identification method based on multiple new information adopted by the present invention improves the rapidity and accuracy of identifying parameters.

Description

Technical Field

[0001] The invention belongs to the technical field of underwater robot model building and hydrodynamic parameter calculation, and in particular relates to a method for identifying parameters of an underwater robot vertical plane motion model. Background Art

[0002] The rich resources of the ocean are of great significance to the economic and social development of mankind. Underwater robots will be important tools and means for human exploration and utilization of the ocean at present and in the future. Establishing an accurate motion model is the basis for underwater robot state monitoring, motion control, trajectory tracking, fault diagnosis and fault-tolerant system design and development, and system identification is an important method for establishing underwater robot models.

[0003] The least squares method (LS) is the most commonly used parameter identification method, but LS is a one-time algorithm, which means that LS can only perform offline parameter identification. Although the calculation principle of LS is relatively simple, it requires batch processing of a large amount of observation data, which makes the LS algorithm have high requirements on computing equipment. The underwater environment in which underwater robots perform tasks is a time-varying and complex environment. The underwater robot itself is also a highly complex system, which makes some hydrodynamic parameters of underwater robots not constant. Therefore, it is of great significance to use online identification algorithms for parameter identification and real-time reconstruction of underwater robot motion model parameters. Recursive least squares identification method (RLS) is an important online identification algorithm. It uses new data to make corrections based on the original parameter estimation, solving the problem that LS cannot identify parameters in real time and LS has low computational efficiency due to the need to batch process a large amount of data. However, RLS can only correct the parameter estimation value of the previous moment according to the current moment observation data, that is, the new information, and calculate the parameter estimation value of the current moment based on a single new information. Therefore, RLS only uses each observation data once, and the data utilization rate is low. Based on the multi-innovation theory, the multi-innovation least squares identification algorithm (MILS) expands the scalar innovation into an innovation vector, and uses the innovation vector to correct the parameter estimate at the previous moment to obtain the parameter estimate at the current moment. Compared with RLS, which only uses one innovation to correct the parameter estimate, MILS uses multiple innovations of the innovation vector to correct the parameter estimate, which can make more full use of multiple historical observation data to improve data utilization, thereby improving the accuracy and convergence speed of identification.

[0004] The vertical motion model of underwater robots is the basis for navigation depth control. The conventional method for identifying the parameters of underwater robot motion model is to use a decoupling method to calculate each degree of freedom separately, that is, only one motion model in a motion direction is in an identifiable state at a time. Although this can simplify the calculation difficulty, the depth control of underwater robots is a vertical motion with multiple degrees of freedom. The decoupling method is not suitable for real-time identification of the navigation depth control of underwater robots. Therefore, a method for identifying the parameters of the vertical motion model of underwater robots based on the multi-innovation least squares identification algorithm (MILS) is proposed. Summary of the invention

[0005] In order to solve the above technical problems, the present invention proposes a method for identifying parameters of a vertical motion model of an underwater robot. According to the vertical motion model of the underwater robot, the multi-innovation least square method is used to identify the hydrodynamic parameters of the underwater robot.

[0006] To achieve the above object, the present invention provides a method for identifying parameters of a vertical motion model of an underwater robot, comprising:

[0007] S1. Establish a vertical motion model of the underwater robot based on the motion characteristics of the underwater robot;

[0008] S2, discretizing the vertical plane motion model to obtain a discretized vertical plane motion model;

[0009] S3, performing differential processing on the discretized vertical plane motion model, and obtaining a parameter discrimination model based on the vertical plane motion model after the differential processing;

[0010] S4, performing parameter identification based on the least square method of multiple innovations and the parameter identification model to identify model parameters;

[0011] S5. Use the discretized vertical plane motion model to verify the identified model parameters. If the error is less than or equal to the preset error, output the identification result. If the error is greater than the preset error, return to S2.

[0012] Optionally, establishing the vertical plane motion model of the underwater robot according to the motion characteristics of the underwater robot in S1 includes: performing linearization processing according to the motion characteristics of the underwater robot to establish the vertical plane motion model of the underwater robot:

[0013]

[0014] Among them, θ is the pitch of the underwater robot, ζ is the navigation depth, q is the pitch angular velocity, w is the vertical velocity, δ is the input rudder angle, V is the straight-line speed, M q ,Z wis the parameter to be identified, m,I y are the underwater robot shell parameters that can be directly measured, is the pitch angular velocity derivative, is the derivative of the pitch angle, is the vertical velocity derivative, is the navigation depth derivative, x R is the distance from the rudder to the coordinate origin, C LS is the lift coefficient, ρ is the density of water, and S is the cross-sectional area of ​​the rudder.

[0015] Optionally, before discretizing the vertical plane motion model, the method includes: converting the vertical plane motion model into a state space equation by taking the underwater robot's pitch and navigation depth as output items and the input rudder angle as input item:

[0016]

[0017] in, is the derivative of the state vector x, state quantity x = [q θ w ζ] T , the output vector y = [θ ζ] T , input vector u = δ, δ is the rudder angle, A, B, C are the coefficient matrices

[0018]

[0019] Optionally, discretizing the vertical plane motion model in S2 to obtain the discretized vertical plane motion model includes:

[0020] By taking a first-order approximation to describe Discretize:

[0021] x(k+1)=Φx(k)+Δu(k)

[0022] Among them, x(k) and x(k+1) are the state items at time k and k+1 respectively, Φ is the coefficient matrix, Δ is the coefficient matrix, and u(k) is the input item at time k. A is the state transfer matrix of the system, h is the time step, I is the unit matrix, B is the input matrix of the system, and the first-order approximation is used to describe it, Φ≈I+Ah, Δ≈Bh;

[0023] Obtaining a unit forward shift operator, and obtaining a new discretized model based on the unit forward shift operator;

[0024] By substituting the new discretized model into y=Cx, the discretized vertical motion model is obtained:

[0025]

[0026] Among them, θ(k) and ζ(k) are the trim and navigation depth respectively, α ij , β ij are the parameters to be identified including the hydrodynamic coefficients of the underwater robot.

[0027] Optionally, the method for performing differential processing on the discretized vertical plane motion model in S3 is:

[0028]

[0029] Among them, θ(k-1), θ(k-2), θ(k-3) are the trims at different times, δ(k-1), δ(k-2), δ(k-3), δ(k-4) are the input rudder angles at different times, ζ(k-1), ζ(k-2), ζ(k-3), ζ(k-4) are the navigation depths at different times.

[0030] Optionally, in S3, obtaining a parameter identification model based on the vertical plane motion model after differential processing includes:

[0031] The vertical motion model after the difference processing is converted into a matrix form:

[0032]

[0033] in, is the information vector, λ is the parameter vector to be identified;

[0034] A noise term is added to the vertical plane motion model after the difference processing in matrix form to obtain the parameter identification model.

[0035] Optionally, in S4, the parameter identification model is identified by using a least square method based on multiple innovations, and the identified model parameters include:

[0036] Based on the parameter identification model and the multi-innovation identification theory, an innovation scalar is obtained, and the innovation scalar is expanded to obtain an innovation vector;

[0037] Based on the principle of the innovation vector and the recursive least squares identification method, the least squares method of multiple innovations is obtained to perform parameter identification on the parameter identification model.

[0038] Optionally, the least square method of multiple innovations is specifically:

[0039]

[0040] L(t)=P(t-1)Φ(p,t)[I+Φ T (p,t)P(t-1)Φ(p,t)] -1

[0041] P(t)=[IL(t)Φ T (p,t)]P(t-1),P(0)=p0I

[0042]

[0043] Y(p,t)=[y(t),y(t-1),…,y(t-p+1)] T

[0044] in, It is divided into the estimated value of θ at time (t-1) and t, Y(p,t) is the stacking vector, Φ(p,t) is the information matrix, p0 is a large constant, I is the unit vector, is the gain vector, P(t-1), Divided into (t-1) and t time covariance matrix or covariance matrix.

[0045] Optionally, verifying the identified model parameters using the discretized vertical plane motion model in S5 includes:

[0046] Inputting the identified model parameters into the discretized vertical plane motion model to obtain a prediction result, wherein the prediction result includes a predicted trim angle of an actual trim angle and a predicted navigation depth of an actual navigation depth;

[0047] The identified model parameters are verified by comparing the predicted results with the actual results.

[0048] Technical effect of the present invention: The parameter identification method based on the multi-new information least squares algorithm adopted by the present invention is used to identify the hydrodynamic parameters of the vertical motion model of the underwater robot. It only needs to measure the signals of the underwater robot's longitudinal tilt, depth and straight-line speed and the rudder angle command issued to realize the parameter identification of the vertical motion model. The required measured physical quantities are relatively small, which reduces the requirements of the underwater robot for the onboard sensors. The multi-new information-based least squares identification method adopted by the present invention uses multiple new information quantities to correct the identification parameters, which increases the utilization rate of the data and improves the rapidity and accuracy of the identification parameters. BRIEF DESCRIPTION OF THE DRAWINGS

[0049] The drawings constituting a part of the present application are used to provide a further understanding of the present application. The illustrative embodiments and descriptions of the present application are used to explain the present application and do not constitute an improper limitation on the present application. In the drawings:

[0050] Figure 1 This is a flow chart of a method for identifying parameters of a vertical motion model of an underwater robot according to an embodiment of the present invention;

[0051] Figure 2This is a parameter update calculation diagram based on the multi-innovation least squares algorithm according to an embodiment of the present invention. DETAILED DESCRIPTION

[0052] It should be noted that, in the absence of conflict, the embodiments and features in the embodiments of the present application can be combined with each other. The present application will be described in detail below with reference to the accompanying drawings and in combination with the embodiments.

[0053] It should be noted that the steps shown in the flowcharts of the accompanying drawings can be executed in a computer system such as a set of computer executable instructions, and that, although a logical order is shown in the flowcharts, in some cases, the steps shown or described can be executed in an order different from that shown here.

[0054] like Figure 1 As shown, this embodiment provides a method for identifying parameters of a vertical motion model of an underwater robot, including:

[0055] Step 1: Establish a vertical motion model of the underwater robot based on the vertical motion characteristics of the underwater robot.

[0056] Step 2: Discretize and differentiate the motion model of the underwater robot to obtain a parameter identification model.

[0057] Step 3: According to the parameter identification model, the least squares algorithm based on multiple innovations is used to identify the model parameters.

[0058] Step 4: Based on the identified coupling parameters, separate the various hydrodynamic parameters to obtain a complete underwater robot motion model.

[0059] In step 1, the vertical motion of the underwater robot is considered. According to the motion characteristics of the underwater robot, linearization is performed to obtain the following motion equation:

[0060]

[0061] Among them, θ is the pitch of the underwater robot, ζ is the navigation depth, q is the pitch angular velocity, w is the vertical velocity, δ is the input rudder angle, V is the straight-line speed, M q ,Z w is the parameter to be identified, m,I y etc. are the underwater robot shell parameters that can be directly measured. is the pitch angular velocity derivative, is the derivative of the pitch angle, is the vertical velocity derivative, is the navigation depth derivative, x R is the distance from the rudder to the coordinate origin, C LS is the lift coefficient, ρ is the density of water, and S is the cross-sectional area of ​​the rudder.

[0062] In step 2, the motion model of the underwater robot is discretized, and the processing process is as follows:

[0063] First, take the pitch and depth as output items and the rudder angle as input item, and write the vertical motion model of the underwater robot in the above equation into the form of the following state space equation:

[0064]

[0065] in, is the derivative of the state vector x, state quantity x = [q θ w ζ] T , the output vector y = [θ ζ] T , input vector u = δ (δ is the rudder angle), A, B, C are coefficient matrices

[0066]

[0067] Pair The model after discretization is:

[0068] x(k+1)=Φx(k)+Δu(k)

[0069] in, Δ=A -1 (Φ-I)B, the first-order approximation is used to describe the system model, that is: Φ≈I+Δh, Δ≈Bh. x(k) and x(k+1) are the state items at time k and k+1 respectively, Φ is the coefficient matrix, Δ is the coefficient matrix, and u(k) is the input item at time k. Δ=A -1 (Φ-I)B, A is the state transfer matrix of the system, h is the time step, I is the identity matrix, and B is the input matrix of the system.

[0070] Define the unit shift operator z: zx(k)=x(k+1)

[0071] Then the formula x(k+1)=Φx(k)+Δu(k) can be transformed into:

[0072] zx(k)=Φx(k)+Δu(k)

[0073] Then we can get

[0074] x(k)=(zI-Φ) -1 Δu(k)

[0075] Substituting the above equation into y=Cx, we can get the discretized vertical motion equation of the underwater robot with pitch θ and depth ζ as output terms and rudder angle δ as input term:

[0076]

[0077] Among them, θ(k) and ζ(k) are the trim and navigation depth respectively, α ij ,β ij (i=1, 2; j=1, 2, 3, 4) are the parameters to be identified including the hydrodynamic coefficients of the underwater robot, and i and j are the subscripts of the parameters to be identified (where i=1, 2; j=1, 2, 3, 4).

[0078] Then, the above discrete equation is subjected to difference processing, and we can get

[0079]

[0080] Among them, θ(k-1), θ(k-2), θ(k-3) are the trims at different times, δ(k-1), δ(k-2), δ(k-3), δ(k-4) are the input rudder angles at different times, ζ(k-1), ζ(k-2), ζ(k-3), ζ(k-4) are the navigation depths at different times.

[0081] Let θ(k) = θ(t), ζ(k) = ζ(t), then the above equation can be written as

[0082]

[0083] Among them, y1(t)=θ(t), y2(t)=ζ(t)

[0084]

[0085]

[0086] λ1=[α 11 α 12 α 13 β 11 β 12 β 13 ] T

[0087] λ2=[α 21 α 22 α 23 α 24 β 21 β 22 β 23 β 24 ] T

[0088] Rewrite the above formula into the following general matrix form

[0089]

[0090] in,

[0091] In order to avoid using the ideal output as the observed output, a noise term v(t) is added, as follows:

[0092]

[0093] in is the information vector, and λ is the parameter vector to be identified.

[0094] In step 3, the least squares algorithm based on multiple innovations is used to identify the parameters. The identification algorithm is as follows:

[0095] Defining the innovation scalar And the scalar innovation Expanded to the innovation vector:

[0096]

[0097] Where, the positive integer p represents the length of the new information, and

[0098] In general, the parameter estimate at time (t-1) is Than the estimated value at the previous (ti) time (i=2,3,4,…,p-1) is closer to the true parameter θ. Therefore, the innovation vector can be reasonably taken as

[0099]

[0100] Define the information matrix Φ(p,t) and the stacking vector Y(p,t) as:

[0101]

[0102]

[0103] Then the innovation vector can be expressed as

[0104]

[0105] According to the multi-innovation theory and the principle of the recursive least squares identification method, the multi-innovation-based least squares algorithm (MILS algorithm) can be obtained as follows:

[0106]

[0107] L(t)=P(t-1)Φ(p,t)[I+Φ T (p,t)P(t-1)Φ(p,t)] -1

[0108] P(t)=[IL(t)ΦT (p,t)]P(t-1),P(0)=p0I

[0109]

[0110] Y(p,t)=[y(t),y(t-1),…,y(t-p+1)] T

[0111] Where I is a unit vector and p0 is a large constant, such as p0 = 10 6 , is the estimated value of θ at time t, is the gain vector, is the covariance matrix or covariance matrix, and p≥1 is the innovation length. When p=1, the MILS algorithm degenerates into the standard RLS algorithm.

[0112] In step 4, the parameter α is identified by the MILS algorithm. ij ,β ij (i=1,2;j=1,2,3,4), the identified parameter α ij , β ij Substitute the following transfer function model of the underwater robot motion equation to obtain the pitch angle and depth The forecast results

[0113]

[0114] The above formula is the vertical motion model of the underwater robot in step 2. Adding superscripts to the output items on the left indicates the prediction results. is the predicted trim angle of the actual trim angle θ, is the predicted navigation depth of the actual navigation depth ζ.

[0115] The underwater robot forecast results and Compare with the test data θ and ζ, and calculate the estimated error. If the error meets the accuracy requirement, output the identification result; otherwise, return to step 2 to continue the identification step, such as Figure 2 As shown. Among them, the estimation error is defined as: (e θ 、e ζ are the prediction errors of the trim angle and navigation depth respectively), e θ 、e ζ Set the accuracy according to actual requirements (such as setting the error e θ 、e ζ ≤15%)

[0116] The above are only preferred specific implementations of the present application, but the protection scope of the present application is not limited thereto. Any changes or substitutions that can be easily thought of by a person skilled in the art within the technical scope disclosed in the present application should be included in the protection scope of the present application. Therefore, the protection scope of the present application should be based on the protection scope of the claims.

Claims

1. A method for identifying parameters of a vertical motion model of an underwater robot, characterized in that: include: S1. Establish a vertical motion model of the underwater robot based on the motion characteristics of the underwater robot; S2, discretizing the vertical plane motion model to obtain a discretized vertical plane motion model; S3, performing differential processing on the discretized vertical plane motion model, and obtaining a parameter discrimination model based on the vertical plane motion model after the differential processing; S4, performing parameter identification based on the least square method of multiple innovations and the parameter identification model to identify model parameters; S5, using the discretized vertical plane motion model to verify the identified model parameters, if the error is less than or equal to the preset error, output the identification result, if the error is greater than the preset error, return to S2; In S1, establishing a vertical plane motion model of the underwater robot according to the motion characteristics of the underwater robot includes: performing linearization processing according to the motion characteristics of the underwater robot to establish the vertical plane motion model of the underwater robot: Among them, θ is the pitch of the underwater robot, ζ is the navigation depth, q is the pitch angular velocity, w is the vertical velocity, δ is the input rudder angle, V is the straight-line speed, M q ,Z w is the parameter to be identified, m,I y are the underwater robot shell parameters that can be directly measured, is the pitch angular velocity derivative, is the derivative of the pitch angle, is the vertical velocity derivative, is the navigation depth derivative, x R is the distance from the rudder to the coordinate origin, C LS is the lift coefficient, ρ is the density of water, and S is the cross-sectional area of ​​the rudder; In S2, the vertical plane motion model is discretized, and obtaining the discretized vertical plane motion model includes: By taking a first-order approximation to describe Discretize: x(k+1)=Φx(k)+Δu(k) Among them, x(k) and x(k+1) are the state items at time k and k+1 respectively, Φ is the coefficient matrix, Δ is the coefficient matrix, and u(k) is the input item at time k. A is the state transfer matrix of the system, h is the time step, I is the unit matrix, B is the input matrix of the system, and the first-order approximation is used to describe it, Φ≈I+Ah, Δ≈Bh; Obtaining a unit forward shift operator, and obtaining a new discretized model based on the unit forward shift operator; By substituting the new discretized model into y=Cx, the discretized vertical motion model is obtained: Among them, θ(k) and ζ(k) are the trim and navigation depth respectively, α ij ,β ij are the parameters to be identified including the hydrodynamic coefficients of the underwater robot.

2. The method for identifying parameters of a vertical motion model of an underwater robot according to claim 1, characterized in that: Before discretizing the vertical plane motion model, the method includes: converting the vertical plane motion model into a state space equation by using the underwater robot's pitch and navigation depth as output items and the input rudder angle as input items: in, is the derivative of the state vector x, state quantity x = [q θ w ζ] T , the output vector y = [θ ζ] T , input vector u = δ, δ is the rudder angle, A, B, C are coefficient matrices; 3. The method for identifying parameters of a vertical motion model of an underwater robot according to claim 1, characterized in that: The method for performing differential processing on the discretized vertical motion model in S3 is: Among them, θ(k-1), θ(k-2), θ(k-3) are the trims at different times, δ(k-1), δ(k-2), δ(k-3), δ(k-4) are the input rudder angles at different times, ζ(k-1), ζ(k-2), ζ(k-3), ζ(k-4) are the navigation depths at different times.

4. The method for identifying parameters of a vertical motion model of an underwater robot according to claim 1, characterized in that: In S3, based on the vertical motion model after differential processing, the parameter identification model is obtained including: The vertical motion model after the difference processing is converted into a matrix form: in, is the information vector, λ is the parameter vector to be identified; A noise term is added to the vertical plane motion model after the difference processing in matrix form to obtain the parameter identification model.

5. The method for identifying parameters of a vertical motion model of an underwater robot according to claim 4, characterized in that: In S4, the parameter identification model is identified by using a least square method based on multiple innovations, and the model parameters identified include: Based on the parameter identification model and the multi-innovation identification theory, an innovation scalar is obtained, and the innovation scalar is expanded to obtain an innovation vector; Based on the principle of the innovation vector and the recursive least squares identification method, the least squares method of multiple innovations is obtained to perform parameter identification on the parameter identification model.

6. The method for identifying parameters of a vertical motion model of an underwater robot according to claim 1, characterized in that: The least square method of multiple innovations is specifically: L(t)=P(t-1)Φ(p,t)[I+Φ T (p,t)P(t-1)Φ(p,t)] -1 P(t)=[I-L(t)Φ T (p,t)]P(t-1),P(0)=p0I Y(p,t)=[y(t),y(t-1),…,y(t-p+1)] T in, It is divided into the estimated value of θ at time (t-1) and t, Y(p,t) is the stacking vector, Φ(p,t) is the information matrix, p0 is a large constant, I is the unit vector, is the gain vector, P(t-1), Divided into (t-1) and t time covariance matrix or covariance matrix.

7. The method for identifying parameters of a vertical motion model of an underwater robot according to claim 1, characterized in that: The step of verifying the identified model parameters by using the discretized vertical motion model in S5 includes: Inputting the identified model parameters into the discretized vertical plane motion model to obtain a prediction result, wherein the prediction result includes a predicted trim angle of an actual trim angle and a predicted navigation depth of an actual navigation depth; The identified model parameters are verified by comparing the predicted results with the actual results.

Images