Identification method and verification device for kinetic model parameters of underwater robot under anchoring subsurface buoy load

Abstract

The invention discloses a method for identifying kinetic model parameters of an underwater robot under anchoring subsurface buoy load. The method comprises the following steps: constructing a vertical plane kinetic model of an underwater robot-manipulator-anchoring subsurface buoy system; taking a to-be-identified parameter of the model as a state vector, constructing a state space equation, calculating a process interference residual error and a measurement interference residual error of the system at a certain moment, and obtaining a process covariance matrix and a measurement covariance matrix according to a residual error statistical rule; calculating a mahalanobis distance according to the residual error and the covariance matrix thereof, and correcting the residual error according to the mahalanobis distance; an OUKF algorithm is improved through the corrected process covariance matrix and the measurement covariance matrix; calculating a Kalman filtering gain; and calculating an output state estimation value and an output covariance matrix so as to identify parameters of the system vertical plane dynamical model. Parameters of the system dynamics model are determined through an optimized unscented Kalman filter estimator based on residual weighted statistics, and a more accurate system dynamics model can be obtained.

Description

Technical Field

[0001] The present invention relates to an underwater robot, and more particularly to a method for identifying the dynamic model parameters of an underwater robot with an anchored mooring buoy load and a verification device therefor. Background Art

[0002] The anchored mooring buoy monitoring system is a core facility of the ocean monitoring system. It realizes long-term stable data collection through anchor chains, floats, and sensor arrays (temperature, salinity, flow velocity, etc.), providing key support for coping with climate change, ecological protection, and the blue economy. However, the long-term underwater operating environment is complex, and problems such as ocean current impact, biological attachment, and equipment aging occur frequently. Therefore, an underwater robot-manipulator system is introduced. Through the collaborative technology of the manipulator and multiple sensors, the deployment accuracy, maintenance efficiency, and recovery ability under complex sea conditions of the anchored mooring buoy monitoring equipment can be significantly improved. The underwater robot, the manipulator, and the anchored mooring buoy together constitute the underwater robot-manipulator-anchored mooring buoy (UVMASB) system. The precise control of the UVMASB system depends on the multi-body dynamic model, but its strong coupling, nonlinear, and time-varying characteristics lead to insufficient model accuracy. In addition, offline identification has limitations when dealing with different types of anchored mooring buoys and lacks the ability to dynamically adjust model parameters. During cross-type operations, there are systematic deviations between the model output and the true dynamic response, resulting in poor system universality.

[0003] In the prior art, for the patent application with the application number 202410192173.1 and the title "A Method for Establishing a Dynamic Positioning Controller of an Underwater Robot in a Disturbed Environment", a non-steady numerical simulation method is used to identify the hydrodynamic parameters of the underwater robot. Then, based on the identified parameters, the observed known hydrodynamic terms and the unknown total disturbance force are compensated, and a controller is designed to achieve the motion control of the underwater robot in a disturbed environment. However, the influence of the manipulator motion on the system dynamic model is not considered during the implementation of this patent application.

[0004] For the patent application with the application number 202210276370.2 and the title "A Method for Controlling the Vertical Motion of an Underwater Robot Based on Parameter Identification", from the perspective of parameter identification, a parameter recursive damping and forgetting factor are designed, and the motions of the manipulator and the underwater robot are put into a filter together for parameter identification to achieve accurate parameter estimation of the underwater robot under the disturbance of the manipulator. However, the influence of the load factor on the parameter identification result has not been considered in this patent application.

[0005] The patent application with the application number 202410408193.8 and the title "Method for Establishing Dynamic Model of Underwater Robot Plumb Plane under Moored Buoy Load, Model Parameter Identification Experiment Method and Device" takes into account the coupling force influence of the moored buoy on the underwater robot. On this basis, a dynamic model of the underwater robot in the vertical plane under the moored buoy load is established. However, when establishing this model, only the scenario of the buoy in the steady-state pose is considered, that is, the cable connected to the buoy always remains vertical, and the influence of the transient pose of the buoy is not considered, which is significantly different from the actual working conditions. In addition, the limitations inherent in the offline identification method are not fully considered in the cross-model operation scenario. Summary of the Invention

[0006] Object of the Invention: Aiming at the above problems, the present invention provides a more accurate method for identifying the dynamic model parameters of an underwater robot under a moored buoy load.

[0007] The present invention also provides a verification device for the method for identifying the dynamic model parameters of an underwater robot under a moored buoy load.

[0008] Technical Solution: To solve the above problems, the present invention adopts a method for identifying the dynamic model parameters of an underwater robot under a moored buoy load, including the following steps:

[0009] (1) Based on the inertial coordinate system and the underwater robot body coordinate system, eliminate the motion quantities irrelevant to the underwater robot's vertical plane to obtain the vertical plane dynamic model of the underwater robot-manipulator-moored buoy UVMASB system;

[0010] (2) Take the parameters to be identified in the vertical plane dynamic model as the state vector, and construct the state space equation of the UVMASB system dynamic model, including the state equation and the measurement equation;

[0011] (3) Based on the state space equation, calculate the state estimate value and the measurement value of the UVMASB system at a certain moment, determine the process disturbance residual and the measurement disturbance residual of the UVMASB system at this moment, and obtain the process covariance matrix and the measurement covariance matrix according to the residual statistical law;

[0012] (4) Calculate the Mahalanobis distance according to the residual and its covariance matrix, and correct the residual according to the Mahalanobis distance to obtain the corrected process covariance matrix and measurement covariance matrix;

[0013] (5) Improve the prior covariance matrix based on the Sigma sampling points in the OUKF algorithm through the corrected process covariance matrix; improve the self-covariance matrix of the updated Sigma sampling points in the OUKF algorithm through the corrected measurement covariance matrix; calculate the Kalman filter gain;

[0014] (6) Calculate the output state estimate value and the output covariance matrix according to the Kalman filter gain, so as to identify the parameters of the vertical plane dynamic model of the UVMASB system online.

[0015] The present invention also adopts a verification device for the above identification method, including an underwater robot, an experimental frame, a displacement monitoring device and a device for simulating the load of an anchored mooring buoy; the device for simulating the load of the anchored mooring buoy includes a floating ball, a rope and a tension sensor; one end of the rope is connected to the floating ball, and the other end is connected to the experimental frame through the tension sensor; the underwater robot includes a manipulator, and the manipulator is fixedly connected to the rope, and the displacement monitoring device is used to monitor the displacement data of the underwater robot.

[0016] Beneficial effects: Compared with the prior art, the remarkable advantage of the present invention is that the parameters of the dynamic model of the UVMASB system are determined by an optimized unscented Kalman filter estimator based on residual weighted statistics, which solves the problem of systematic deviation of offline identification under different types of anchored mooring buoys, can obtain a more accurate dynamic model of the UVMASB system, and enhances the adaptability and universality of the model. Description of the Drawings

[0017] Figure 1 It is a schematic diagram of the coordinate system of the UVMASB system and the force analysis of the movement of the underwater floating ball in the present invention.

[0018] Figure 2 It is a schematic diagram of the flow of the method for identifying the parameters of the dynamic model in the present invention.

[0019] Figure 3 It is a schematic structure of the verification device in the present invention.

[0020] Figure 4 It is a schematic diagram of the result of identifying the parameters of the dynamic model by the verification device in the present invention.

[0021] Figure 5 It is a comparison diagram of the longitudinal displacement outputs of different identification methods under a 100 mm floating ball in the present invention.

[0022] Figure 6 It is a comparison diagram of the longitudinal displacement outputs of different identification methods under a 140 mm floating ball in the present invention.

[0023] Figure 7 It is a comparison diagram of the longitudinal displacement outputs of different identification methods under a 180 mm floating ball in the present invention. Detailed Embodiments

[0024] Embodiment 1

[0025] As Figure 1As shown, in this embodiment, the device for simulating the mooring buoy load mainly consists of a buoy, a rope, a fixed seat, etc. A manipulator is installed at the front end of the underwater robot, and the end of the manipulator holds the rope. During the movement of the underwater robot and the mooring buoy load, they affect each other.

[0026] In this embodiment, a method for identifying the dynamic model parameters of an underwater robot under a mooring buoy load includes the following steps:

[0027] The first step is to establish the UVMASB system coordinate system. As Figure 1 shown, it includes an inertial coordinate system E-ξηζ, a moving coordinate system O-xyz of the underwater robot-manipulator system, and a moving coordinate system O b -xyz.

[0028] Based on the UVMASB system coordinate system, establish the vertical plane dynamic model of the UVMASB system.

[0029] From Newton's second law and Euler's equation, the six-degree-of-freedom nonlinear dynamic equation of the UVMASB system can be obtained as follows:

[0030]

[0031] The dynamic equation of the UVMASB system with the action of the mooring buoy load force f introduced is:

[0032]

[0033] In formula (2): M RB represents the rigid body mass inertia matrix, C RB represents the rigid body centripetal force and Coriolis force matrix, M A represents the added mass inertia matrix, C A (v) represents the added mass centripetal force and Coriolis force matrix, D(v) represents the water resistance matrix, g(η) represents the restoring force matrix, v represents the velocity vector, τ represents the thruster thrust, and f represents the mooring buoy load force.

[0034] Under the action of the mooring load, the movement of the underwater robot in the ξ-η plane and the ζ-η plane is very small and can be ignored. To simplify the problem, only the movement of the UVMASB system in the ξ-ζ plane is discussed.

[0035] As Figure 1 shown, to describe the dynamic coupling effect between the mooring buoy and the underwater robot-manipulator in the UVMASB system, the cable is divided into upper and lower sections with the grasping point of the underwater robotic arm as the boundary. The length of the lower section of the cable is d, and the tension it receives is T A1 ; the length of the upper section of the cable is L2, and the tension it receives is T A2The spatial coordinate representation of the end of the manipulator in the underwater robot-manipulator motion coordinate system is (d, 0, h).

[0036] The expression for the angle α between L1 and the ξ-axis can be obtained as follows:

[0037]

[0038] In formula (3), the distance from the position where the manipulator grabs the cable to the origin of the underwater robot motion coordinate system is The angle θ0 with the x-axis of the motion coordinate system is θ0 = arctan(h / d). ξ, ζ, and θ respectively represent the longitudinal displacement, vertical displacement, and pitch angle of the UVMS in the fixed coordinate system.

[0039] The calculation process of the vertical plane mooring load force f is as follows:

[0040] The vertical plane longitudinal mooring load force f X :

[0041] f X = T A1 cos(α - θ) + T A2 cos(θ b + θ) (4)

[0042] The vertical plane vertical mooring load force f Z :

[0043] f Z = T A1 sin(α - θ) + T A2 sin(θ b + θ) (5)

[0044] The vertical plane pitching mooring load force f M :

[0045]

[0046] Therefore, the vertical plane mooring load force f is:

[0047]

[0048] In formula (7), T A1 is measured by the tension sensor, T A2 is obtained according to the transient pose of the floating ball, and θ b is the pitch angle of the floating ball in the inertial coordinate system.

[0049] Define m b as the mass of the floating ball; R as the radius of the floating ball; ρ 水 as the density of water; η as the viscosity coefficient of water; and They are respectively the longitudinal velocity, vertical velocity and pitching angular velocity of the floating ball; and re

[0050] spectively the longitudinal acceleration, vertical acceleration and pitching angular acceleration of the floating ball. Conduct a force analysis on the floating ball. As Figure 1 shown, when the floating ball moves in water under the vertical plane, it is affected by four forces: gravity (G), buoyancy (F 浮 ), the tension of the cable on it (T A2 ), and the water resistance (F D ) when moving underwater. The expressions are respectively:

[0051] G = m b g (8)

[0052] F 浮 = ρ 水 gv 排 (9)

[0053] F D = 6πηRv (10)

[0054] According to Newton's second law:

[0055] F 合 = m b a = T A2 + F D + F 浮 + G (11)

[0056] Substitute formulas (8), (9), and (10) into formula (11) to obtain:

[0057] m b a = T A2 + 6πηRv + ρ 水 gv 排 + m b g (12)

[0058] Combined with the motion state of the floating ball, after organizing formula (12), we get:

[0059]

[0060] Decompose it into motions along the longitudinal, vertical, and pitching degrees of freedom of the vertical plane, and analyze it to obtain:

[0061] (1) Analysis of the longitudinal motion of the floating ball in the vertical plane:

[0062]

[0063] (2) Analysis of the vertical motion of the floating ball in the vertical plane:

[0064]

[0065] (3) Analysis of the longitudinal tilting motion of the floating ball:

[0066]

[0067] By combining equations (14), (15), and (16), the cable tension T can be obtained as: A2 as follows:

[0068]

[0069] Finally, by organizing equation (17), the relationship between the cable tension T considering the transient pose of the floating ball and the motion parameters of the floating ball is obtained as: A2 as:

[0070]

[0071] Substituting equations (18) and (7) into equation (2) and expanding, the vertical-plane dynamic equation of the UVMASB system considering the transient pose of the floating ball is obtained as:

[0072]

[0073] The system parameters are identified by the experimental method, and the dynamic model parameters of the UVMASB system are obtained by the least squares method.

[0074] Equation (19) is decomposed into three independent equations: the longitudinal equation in the vertical plane, the vertical equation in the vertical plane, and the longitudinal tilting equation in the vertical plane. Since the identification processes of the three equations are similar, only the longitudinal equation in the vertical plane is described in detail in this embodiment.

[0075] The longitudinal equation in the vertical plane is:

[0076]

[0077] Let the coupling force on the right side of the equation be:

[0078]

[0079] The following relationship is established by the least squares method:

[0080]

[0081] According to the above equation, the least squares estimated value of θ can be obtained as:

[0082]

[0083] Step 2: Take the parameters to be identified in the dynamic model of the vertical plane as the state vector, and construct the state-space equation of the enhanced dynamic system model of the UVMASB system.

[0084] The dynamic model of the UVMASB system is a gray-box model, and there is incompatibility among the state-space dimension, the dimension of undetermined coefficients, and the input and output state-space dimensions. It is necessary to introduce additional state variables to expand the undetermined dynamic parameters into a part of the state vector.

[0085] The state-space expression of the dynamic model of the UVMASB system is:

[0086]

[0087] In formula (28), x a is the state vector; u and Y a are the control input and the measurement vector respectively; W and V are the process disturbance and the measurement disturbance respectively; f a and H a are the dynamic model function and the observation matrix respectively, and Ω is the dynamic model parameter to be identified in the UVMASB system.

[0088] Let τ + f in formula (2) be τ 总 The enhanced dynamic equation obtained by the simplified transformation can be obtained:

[0089]

[0090] According to formula (29), the enhanced dynamic model of the UVMASB system is:

[0091]

[0092] Among them, ∑F x is the total force in the longitudinal direction; ∑F z is the total force in the vertical direction; ∑M y is the pitching moment; M a is the inertia matrix of the rigid body and the added mass. The detailed expressions of ∑F x , ∑F z , ∑M y , M a are shown in formulas (31)-(34):

[0093]

[0094]

[0095]

[0096]

[0097] The observation model is as follows:

[0098] Y a = [u w q Ω] T + V (35)

[0099] In the third step, based on the residual statistical method, the process covariance matrix after residual statistics and the measurement covariance matrix

[0100] The residual statistical adaptive algorithm uses a sliding window of length N to update the residual sequence in real time and dynamically analyzes the residual characteristics through residual statistics. The specific implementation steps are as follows:

[0101] (1) Obtain the process interference and measurement interference residuals:

[0102]

[0103] In formula (36), the process interference residual ξ k is the deviation between the prior state estimate value at time k and the posterior state estimate value ; the measurement interference residual is the deviation between the measurement value Y k at time k and the measurement calculated value

[0104] (2) Obtain the process covariance matrix and the measurement covariance matrix

[0105] Based on the obtained N-dimensional process interference residual sequence at time k, construct the process interference residual mean and the corresponding process interference residual covariance matrix C ξ :

[0106]

[0107]

[0108] According to the algorithm flow of the Optimized Unscented Kalman Filter (OUKF), the expected value of the covariance matrix C ξ is:

[0109]

[0110] Substitute formula (38) into formula (39), and then obtain the process error covariance matrix after residual statistics:

[0111] ​

[0112] Similarly, the measurement error covariance matrix after residual statistical processing can be obtained:

[0113]

[0114] In formulas (37)-(41) is the mean of the process interference residuals of the N-dimensional process interference residual sequence at a certain moment, is the mean of the measurement interference residuals of the N-dimensional measurement interference residual sequence at a certain moment, ξ j is the j-th process interference residual, is the j-th measurement interference residual, Without considering the covariance propagation term of the process interference, is the corresponding state error covariance matrix, is the state covariance matrix.

[0115] Fourthly, based on the Mahalanobis distance (MD) judgment method for weighting, the process covariance matrix after residual weighting and the measurement covariance matrix

[0116] Based on the Mahalanobis distance (MD) judgment method for weighting, the data is updated in real time, and then the parameters of the UVMASB kinetic model are optimized.

[0117] Firstly, outliers are judged according to MD. MD is a method that reflects the degree of dispersion between outliers and the sample mean by calculating the data covariance distance. The corresponding calculation steps are as follows:

[0118]

[0119] where M represents the Mahalanobis distance in the MD method, h and C are the data point mean and the data point covariance matrix respectively, and the calculation process is the same as formulas (37) and (38).

[0120]

[0121] In formula (43), represents the critical value of the chi-square distribution with degrees of freedom A i and significance level α. α is used to judge the strictness of control. M 2 should not exceed the critical value of the chi-square distribution. If it exceeds, this data point is considered abnormal.

[0122] Therefore, the weight coefficients of the corresponding process interference residuals and measurement interference residuals are expressed as:

[0123]

[0124] The process covariance matrix is obtained by using the residual weighted statistical method and the measurement covariance matrix Specifically, they are shown in formulas (45) and (46):

[0125]

[0126]

[0127] In formula (46), is the inverse matrix of the measurement prediction residual

[0128] Step 5: Online identification of the UVMASB system dynamics model based on the residual weighted optimization unscented Kalman filter (RWSOUKF). The UKF algorithm is improved by the corrected process covariance matrix and measurement covariance matrix to realize the parameter estimation of the UVMASB system dynamics model. As Figure 2 shown, the specific steps are as follows:

[0129] (1) Initialization: Set the initial state estimate x0 and the initial error covariance matrix p0:

[0130] P(W)~(0,Q) P(V)~(0,R) (47)

[0131] Both the process disturbance W and the measurement disturbance V in formula (47) conform to the Gaussian normal distribution, with a mean of 0. Q and R are the covariance matrices of the process noise and the measurement noise respectively

[0132] (2) Perform the unscented transformation to generate 2n + 1 sampling points x i , that is, sigma points:

[0133]

[0134]

[0135]

[0136] (3) Substitute the generated sampling points into formula (30) and calculate the corresponding weights:

[0137]

[0138]

[0139] Where n is the dimension of the augmented state space, and λ is a scaling ratio parameter whose main role is to adjust the distribution range of the sigma points, thereby reducing the overall prediction error. A recursive tuning rule and an update descent lemma are introduced to dynamically update λ to achieve dynamic optimization of the parameter λ and reduce the system deviation caused by the accumulation of estimation errors during the algorithm update stage, thereby improving the prediction accuracy.

[0140] (4) Introduce a recursive tuning rule and an update descent lemma to find λ

[0141] The formula for the recursive tuning rule is:

[0142]

[0143] Similarly, the recursive tuning formula of λ at the k-th time step is:

[0144]

[0145] λ k represents the recursive scaling parameter at the k-th time step, T is the learning rate, and γ represents the adjustment step size.

[0146] is the rate of change of the error function J with respect to λ. According to the update descent lemma, the formula for is:

[0147]

[0148] In formula (55), is the measured calculated value at time k - 1, is the expected value of the measurement vector at the k-th time step, and are the diagonal elements of the autocovariance matrix the mean of the autocovariance matrix respectively.

[0149] (5) The prior value and its prior covariance p k|k-1 can be obtained through the updated λ:

[0150]

[0151]

[0152] (6) According to and p k|k-1 re-construct the Sigma sampling points again:

[0153]

[0154] Update the sampling points and calculate the updated sampling point set Measured calculation value of each sampling point Autocovariance matrix And cross-covariance matrix

[0155]

[0156]

[0157]

[0158]

[0159] (7) According to the autocovariance matrix And cross-covariance matrix Calculate the Kalman gain K K ; Calculate the output state estimate value according to the Kalman filter gain And output covariance matrix P k|k , The calculation formula is:

[0160]

[0161]

[0162]

[0163] Calculate the output state estimate value and output covariance matrix according to the Kalman filter gain, so as to identify the parameters of the vertical plane dynamic model of the UVMASB system.

[0164] Example 2

[0165] In this example, a verification device for the identification method of the dynamic model parameters of an underwater robot with an anchored buoy load is used to obtain data.

[0166] As Figure 3 shown, the verification device in this example consists of an underwater robot 1, a manipulator 2, and an anchored buoy equivalent device 3.

[0167] The underwater robot 1 consists of an underwater robot body 101, a depth gauge 102, and an inclinometer 103. The depth gauge 102 and the inclinometer are precisely fixed through the installation holes preset in the underwater robot body 101. The manipulator 2 consists of a mounting base 201, an underwater servo 202, a right-angle frame 203, a connecting bracket 204, and a mechanical gripper 205. The manipulator 2 is installed at the front end of the underwater robot body 101 through the mounting base 201. The underwater servo 202 is installed on the right-angle frame 203 through bolts, and the right-angle frames are connected to each other through the connecting bracket 204. The equivalent device of the mooring buoy 3 consists of a buoy 301, a cable 302, a tension sensor 303, a realization frame 304, and an industrial camera 305. The upper section of the cable 302 is connected to the buoy 301, the lower section is connected to the experimental frame 304 and is equipped with the tension sensor 303, and the industrial camera 305 is installed on the experimental frame 304 through angle codes.

[0168] The device provided in this example can obtain the data required in the dynamic identification and experimental verification process of the UVMASB system. The measured parameters and the names of the required sensors are summarized in the following table.

[0169] Table 1 Summary Table of Measured Data of UVMASB System

[0170]

[0171] As Figure 4 shown, a buoy with a radius of 140 mm is selected to conduct a propeller thrust measurement experiment. The control voltage of the propeller is changed, the thrust value data of each group are recorded, and the data of each group of sensors are recorded. Then, the measured and calculated data of each group are respectively substituted into the least-squares relationships of the vertical plane longitudinal equation, the vertical plane vertical equation, and the vertical plane trim equation. Based on the least-squares principle, the corresponding dynamic model parameters can be obtained. Figure 4 (a), (b), (c), and (d) in

[0172] respectively represent the experimental results of different dynamic model parameters.

[0173]

[0174] Therefore, the longitudinal dynamic model of the UVMASB system in the vertical plane is the following equation:

[0175]

[0176] Similarly, the vertical dynamic model of the UVMASB system in the vertical plane is the following equation:

[0177]

[0178] The pitching dynamics model of the UVMASB system in the vertical plane is the following equation:

[0179]

[0180] In this embodiment, online identification verification of the dynamics model of the UVMASB system with different float sizes is carried out. Three groups of floats with different radii of 100, 140, and 180 mm are selected. The fourth-order Runge-Kutta method is used based on Matlab to numerically integrate and solve the longitudinal displacement, that is, the longitudinal displacement outputs of four models: the output of the traditional method (M1) of the UVMASB system dynamics model through experiments; the output of the UVMASB system dynamics model considering the transient pose of the float (M2); the output of the UVMASB system dynamics model based on online identification by OUKF (OUKF); the output of the UVMASB system dynamics model based on online identification by RWSOUKF (RWSOUKF). Among them, the online identification methods of OUKF and RWSOUKF use the offline identification parameters of M2 as the initial conditions.

[0181] During the experiment, a control voltage signal of 0.6 V is applied to the vertical thruster of the UVMASB system, and control voltage signals of 0.5 V, 0.6 V, 0.7 V, and 0.8 V are applied to the main thruster respectively. The errors between the longitudinal displacement outputs (M1, M2, OUKF, RWSOUKF) of the four models and the actual measured values of the longitudinal displacement of the float are compared, and three error evaluation indexes, namely the sum of squared residuals (SSE), the mean absolute error (MAE), and the mean absolute percentage error (MAPE), are used for evaluation. The effectiveness of the RWSOUKF algorithm in online identification of the dynamics model parameters of different float UVMASB systems is verified.

[0182] According to Figure 5 the results of, where Figure 5 (a) is a schematic diagram of the results when a control voltage signal of 0.5 V is applied to the main thruster, Figure 5 (b) is a schematic diagram of the results when a control voltage signal of 0.6 V is applied to the main thruster, Figure 5 (c) is a schematic diagram of the results when a control voltage signal of 0.7 V is applied to the main thruster, Figure 5 (d) is a schematic diagram of the results when a control voltage signal of 0.8 V is applied to the main thruster. Calculate the errors between the longitudinal displacement outputs of the four models and the actual measured values under the 100 mm float. The results are shown in Table 1:

[0183] Table 1 Longitudinal displacement output errors of different identification methods under 100 mm float

[0184]

[0185] When the experimental conditions are set to a float radius of 100 mm, according to the data in Table 1:

[0186] Under the condition of main push voltage of 0.5V. In the longitudinal freedom direction, taking into account the transient posture of the float M2, the residual sum of squares, mean absolute error, and mean absolute percentage error between the model output and the actual measurement value are reduced compared with the traditional method M1. The residual sum of squares is reduced by 0.7460, with a relative reduction rate of 42.5%; the mean absolute error is reduced by 0.0296, with a relative reduction rate of 24.1%; the mean absolute percentage is reduced by 0.0679, with a relative reduction rate of 23.9%. Compared with the M2 offline identification method, the three error evaluation indicators of the OUKF online identification method have all decreased, with a relative reduction rate of 89.5% in the residual sum of squares; a relative reduction rate of 68.8% in the mean absolute error; and a relative reduction rate of 68.5% in the mean absolute percentage. Compared with the OUKF online identification method, the RWSOUKF online identification method shows better convergence, with the residual sum of squares reduced by 12.6%; the mean absolute error reduced by 11.0%; and the mean absolute percentage reduced by 8.9%.

[0187] Under the working conditions of main push voltage 0.6V, 0.7V, 0.8V. In the longitudinal freedom direction, the residual sum of squares, mean absolute error and mean absolute percentage of M2, taking into account the transient posture of the float, are reduced by 50.3%, 33.8%, 34.4%; 32.8%, 19.2%, 17.9%; 23.1%, 14.7%, 14.1% respectively compared with the traditional method M1. Compared with the M2 offline identification method, the residual sum of squares, mean square error and mean absolute error of the OUKF online identification method are reduced to varying degrees, with relative reductions of 87.9%, 67.2%, 66.1%; 89.1%, 67.8%, 66.8%; 88.1%, 66.6%, 65.5%. RWSOUKF shows better performance in online identification than OUKF method, with relative reductions of 33.8%, 19.9%, 18.4%; 25.6%, 15.1%, 13.9%; and 39.1%, 23.7%, 21.7%.

[0188] according to Figure 6 The results, among which Figure 6 (a) Schematic diagram of the result of applying a 0.5V control voltage signal to the main thruster. Figure 6 (b) Schematic diagram of the result of applying a 0.6V control voltage signal to the main thruster. Figure 6 (c) Schematic diagram of the result of applying a 0.7V control voltage signal to the main thruster. Figure 6 (d) Apply a 0.8V control voltage signal to the main thruster, and calculate the error between the longitudinal displacement output results of the four models under the 140mm float and the actual measured value. The results are shown in Table 2:

[0189] Table 2 Longitudinal Displacement Output Error of Different Identification Methods under 140mm Floating Ball

[0190]

[0191] When the experimental condition is set with a floating ball radius of 140mm, according to the data in Table 2:

[0192] Under the condition of a main propulsion voltage of 0.5V. In the longitudinal degree of freedom direction, considering the transient pose M2 of the floating ball relative to the traditional method M1, the sum of squared residuals, mean absolute error, and mean absolute percentage error between the model output and the actual measurement value all decrease. The sum of squared residuals decreases by 0.2928, and the relative decrease ratio is 55.3%; the mean absolute error decreases by 0.0187, and the relative decrease ratio is 36.8%; the mean absolute percentage decreases by 0.0671, and the relative decrease ratio is 36.3%. Compared with the M2 offline identification method, the three error evaluation indexes of the OUKF online identification method all decrease. The relative decrease ratio of the sum of squared residuals is 47.0%; the relative decrease ratio of the mean absolute error is 26.2%; the relative decrease ratio of the mean absolute percentage is 20.5%. The RWSOUKF online identification method shows better convergence compared with the OUKF online identification method. The relative decrease ratio of the sum of squared residuals is 28.3%; the relative decrease ratio of the mean absolute error is 15.6%; the relative decrease ratio of the mean absolute percentage is 13.8%.

[0193] Under the conditions of main propulsion voltages of 0.6V, 0.7V, and 0.8V. In the longitudinal degree of freedom direction, considering the transient pose M2 of the floating ball relative to the traditional method M1, the reduction amplitudes of the sum of squared residuals, mean absolute error, and mean absolute percentage are respectively 50.1%, 39.5%, 28.1%; 56.4%, 42.1%, 38.9%; 49.8%, 35.3%, 33.8%. Compared with the M2 offline identification method, the sum of squared residuals, mean square error, and mean absolute error of the OUKF online identification method all decrease to varying degrees, and the relative reduction amplitudes are 51.7%, 28.9%, 23.9%; 48.9%, 30.5%, 16.1%; 58.4%, 31.6%, 28.6%. The RWSOUKF method shows better performance in online identification compared with the OUKF method, and the relative reduction amplitudes are 34.3%, 18.9%, 19.7%; 19.7%, 11.9%, 7.1%; 0.05%, 14.1%, 6.4%.

[0194] According to Figure 7 the results, among which Figure 7 (a) is a schematic diagram of the result when a 0.5V control voltage signal is applied to the main thruster, Figure 7 (b) is a schematic diagram of the result when a 0.6V control voltage signal is applied to the main thruster, Figure 7(c) Schematic diagram of the result of applying a 0.7V control voltage signal to the main thruster, Figure 7 (d) Apply a 0.8V control voltage signal to the main thruster, and calculate the errors between the longitudinal displacement output results of the four models under the 180mm floating ball and the actual measured values. The results are shown in Table 3:

[0195] Table 3 Longitudinal displacement output errors of different identification methods under the 180mm floating ball

[0196]

[0197] When the experimental conditions are set with a floating ball radius of 180mm, according to the data in Table 3:

[0198] Under the condition of a main thrust voltage of 0.5V. In the longitudinal degree of freedom direction, considering the transient pose M2 of the floating ball relative to the traditional method M1, the sum of squared residuals, mean absolute error, and mean absolute percentage error between the model output and the actual measured values all decrease. The sum of squared residuals decreases by 0.0677, and the relative reduction ratio is 16.5%; the mean absolute error decreases by 0.0055, and the relative reduction ratio is 9.3%; the mean absolute percentage decreases by 0.0528, and the relative reduction ratio is 8.5%. Compared with the M2 offline identification method, the three error evaluation indexes of the OUKF online identification method all decrease. The relative reduction ratio of the sum of squared residuals is 88.9%; the relative reduction ratio of the mean absolute error is 66.5%; the relative reduction ratio of the mean absolute percentage is 63.9%. The RWSOUKF online identification method shows better convergence compared with the OUKF online identification method. The relative reduction of the sum of squared residuals is 34.9%; the relative reduction of the mean absolute error is 22.5%; the relative reduction of the mean absolute percentage is 20.3%.

[0199] Under the conditions of main thrust voltages of 0.6V, 0.7V, and 0.8V. In the longitudinal degree of freedom direction, considering the transient pose M2 of the floating ball relative to the traditional method M1, the reduction amplitudes of the sum of squared residuals, mean absolute error, and mean absolute percentage are respectively 25.0%, 11.3%, 12.0%; 20.2%, 10.9%, 10.5%; 40.8%, 27.6%, 23.8%. Compared with the M2 offline identification method, the sum of squared residuals, mean square error, and mean absolute error of the OUKF online identification method all decrease to varying degrees, and the relative reduction amplitudes are 87.2%, 55.0%, 51.8%; 88.5%, 65.9%, 60.5%; 85.2%, 59.4%, 54.1%. The RWSOUKF method shows better performance in online identification compared with the OUKF method, and the relative reduction amplitudes are 45.1%, 26.8%, 22.4%; 32.6%, 24.9%, 20.5%; 48.2%, 27.9%, 21.5%.

Claims

1. A method for identifying the dynamic model parameters of an underwater robot with an anchor mooring buoy payload, characterized in that It includes the following steps: (1) Based on the inertial coordinate system and the underwater robot carrier coordinate system, eliminate the motion quantities irrelevant to the vertical plane of the underwater robot to obtain the vertical plane dynamic model of the underwater robot-manipulator-anchored buoy UVMASB system; (2) Take the parameters to be identified in the vertical plane dynamic model as the state vector, and construct the state space equation of the UVMASB system dynamic model, including the state equation and the measurement equation; (3) Based on the state space equation, calculate the state estimate value and the measurement value of the UVMASB system at a certain moment, determine the process disturbance residual and the measurement disturbance residual of the UVMASB system at this moment, and obtain the process covariance matrix and the measurement covariance matrix according to the residual statistical law; (4) Calculate the Mahalanobis distance according to the residual and its covariance matrix, and correct the residual according to the Mahalanobis distance to obtain the corrected process covariance matrix and measurement covariance matrix; (5) Improve the prior covariance matrix based on Sigma sampling points in the OUKF algorithm through the corrected process covariance matrix; Improve the self-covariance matrix of the updated Sigma sampling points in the OUKF algorithm through the corrected measurement covariance matrix; Calculate the Kalman filter gain; (6) Calculate the output state estimate value and the output covariance matrix according to the Kalman filter gain, so as to online identify the parameters of the vertical plane dynamic model of the UVMASB system.

2. The identification method according to claim 1, wherein The vertical plane dynamic model of the underwater robot-manipulator-anchored buoy UVMASB system is as follows: where, M RB is the rigid body mass inertia matrix; C RB (v) is the rigid body centripetal force and Coriolis force matrix; v is the velocity vector; is the acceleration matrix; τ is the thruster thrust; g(η) is the restoring force matrix; M A is the added mass inertia matrix; C A (v) is the Coriolis force-like matrix generated by the added mass; D(v) is the fluid resistance matrix; f is the mooring buoy load force matrix.

3. The identification method according to claim 2, wherein The anchored buoy load force matrix f is: Among them, T A1 is the first-direction force exerted on the underwater robot by the simulated mooring buoy load; T A2 is the second-direction force exerted on the underwater robot by the simulated mooring buoy load; α is the direction angle of the first-direction force in the inertial coordinate system; θ0 is the direction angle in the carrier coordinate system of the distance from the action position of the simulated mooring buoy load on the underwater robot to the origin of the underwater robot; θ is the pitch angle of the underwater robot; L3 is the straight-line distance from the end of the robotic arm to the motion coordinate system of the underwater robot; θ b is the pitch angle of the float in the inertial coordinate system.

4. The identification method according to claim 3, characterized in that The second-direction force T A2 The calculation formula is as follows: where m b is the mass of the floating ball; R is the radius of the floating ball; ρ 水 is the density of water; η is the viscosity coefficient of water; and are the longitudinal velocity and the vertical velocity of the floating ball respectively; and are the longitudinal acceleration and the vertical acceleration of the floating ball respectively.

5. The identification method according to claim 4, wherein The state space equation of the UVMASB system dynamic model is: where, x a is the state vector, u is the control input, Y a is the measurement vector, W is the process disturbance, V is the measurement noise, f a is the dynamic system model function, H a is the observation matrix, and Ω is the dynamic model parameter to be identified for the UVMASB system.

6. The identification method according to claim 5, wherein The process interference residual ξ k and the measurement interference residual are calculated by the following formula: Among them, is the prior state estimate of the UVMASB system at time k, is the posterior state estimate of the UVMASB system at time k, Y k is the measurement value of the UVMASB system at time k, is the measurement calculation value of the UVMASB system at time k.

7. The identification method according to claim 6, wherein The corrected process covariance matrix and the measurement covariance matrix are as follows: Among them, is the process interference residual weight coefficient weighted by the Mahalanobis distance judgment method, is the measurement interference residual weight coefficient weighted by the Mahalanobis distance judgment method, is the mean value of the process interference residual of the N-dimensional process interference residual sequence at a certain moment, is the mean value of the measurement interference residual of the N-dimensional measurement interference residual sequence at a certain moment, ξ j is the j-th process interference residual, is the j-th measurement interference residual, does not consider the covariance propagation term of the process interference, is the corresponding state error covariance matrix, is the inverse matrix of the measurement prediction residual, is the state covariance matrix.

8. The identification method according to claim 7, wherein The specific process of step (5) is: (51) Calculate the state estimate value x0 of the state vector and the initial error covariance matrix p0; (52) Based on the sampling strategy, perform unscented transformation to generate Sigma sampling points; (53) Calculate the weights of the Sigma sampling points; (54) Calculate the prior state value and the prior covariance matrix based on the Sigma sampling points at time step k; The prior covariance matrix p k|k-1 has the following calculation formula: Among them, is the prior state value, and α (i) is the weight of the i-th Sigma sampling point, is the state vector of the i-th sampling point after unscented transformation. (54)According to the obtained prior state value and the prior covariance matrix p k|k-1 generate new Sigma sampling points; (55) Calculate the measurement prediction value by passing the new Sigma sampling points through the measurement equation, perform weighted summation on the measurement prediction value to obtain the measurement calculated value; calculate the self-covariance matrix and the cross-covariance matrix according to the measurement calculated value; The said autocovariance matrix has the following calculation formula: Among them, is the measurement prediction value of the Sigma sampling point. (56) Calculate the Kalman gain according to the self-covariance matrix and the cross-covariance matrix.

9. The identification method according to claim 8, wherein The formula for calculating the weights of the Sigma sampling points is: Among them, λ is the scaling ratio parameter, and n is the dimension of the augmented state space. Perform dynamic update on λ through the recursive tuning rule and the update descent lemma: The recursive tuning formula of λ at the k-th time step is: where, λ k represents the recursive scaling parameter at the k-th time step, T represents the learning rate, γ represents the adjustment step, is the rate of change of the error function J with respect to the change in λ; According to the update descent lemma, the rate of change of the error function J with respect to the change in λ is calculated by the formula: Among them, is the measured calculated value at time k - 1, is the expected value of the measurement vector at the k-th time step, and are the autocovariance matrix the diagonal elements of the mean of the autocovariance matrix, respectively.

10. A verification device for the identification method according to any one of claims 1 to 9, characterized in that, It includes an underwater robot (1), an experimental framework, a displacement monitoring device, and a device for simulating the load of an anchored mooring buoy; the device for simulating the load of an anchored mooring buoy includes a float (301), a rope (302), and a tension sensor; one end of the rope (302) is connected to the float (301), and the other end is connected to the experimental framework (304) through the tension sensor; the underwater robot (1) includes a manipulator, and the manipulator is fixedly connected to the rope (302), and the displacement monitoring device is used to monitor the displacement data of the underwater robot (1).

Images