Variable-parameter active-disturbance-rejection trajectory tracking control method for underwater robot

Abstract

The invention relates to the field of automatic control of underwater robots, and aims to provide a variable-parameter active-disturbance-rejection trajectory tracking control method for an underwater robot. Comprising the following steps: establishing a kinetic model of the underwater robot, designing four-degree-of-freedom extended state observers based on the kinetic model, respectively calculating estimated values of corresponding motion parameters by using the degree-of-freedom extended state observers to carry out error calculation, and calculating by using an adaptive function to obtain real-time parameter values; and according to the calculated trajectory tracking control law, updating the state and output of each extended state observer, and controlling the underwater robot. Based on an active disturbance rejection control algorithm, the method can effectively estimate and compensate internal and external disturbances of the system, and is very suitable for the control problem of a nonlinear and uncertain system of an underwater robot. An efficient and stable trajectory tracking effect can be achieved even in a complex underwater environment; and manual adjustment work is avoided by utilizing self-adaptive parameter adjustment, and the performance of the control system is improved.

Description

Technical Field

[0001] The invention relates to the field of underwater robot automatic control, and in particular to a variable parameter self-disturbance rejection trajectory tracking control method for an underwater robot. Background Art

[0002] Underwater robots are autonomous or remotely controlled devices used in underwater environments. They are widely used in fields such as ocean exploration, environmental monitoring, resource development, military missions, and scientific research. Due to the characteristics of underwater robot systems such as high nonlinearity, strong coupling, and multiple interferences, how to achieve high-precision trajectory tracking is a difficult problem. For underwater robots, trajectory tracking needs to consider the hydrodynamic and nonlinear effects caused by their own motion as well as external interference such as water flow. These factors increase the difficulty of control and require the control algorithm to have strong robustness and adaptability.

[0003] ADRC is a modern control method that improves the anti-disturbance capability of a system by estimating and compensating for disturbances inside and outside the system. It does not rely on the precise mathematical model of the controlled object, but estimates the system state and disturbances in real time through an extended state observer, and eliminates the effects of these disturbances through feedback control. Therefore, it performs well in dealing with complex, nonlinear and uncertain systems, and is very suitable for the control problem of underwater robot trajectory tracking. However, the extended state observer in traditional ADRC uses fixed parameters, and manual adjustment of parameters will bring a large workload. In addition, the optimal parameter values ​​will change in different scenarios, resulting in poor control effects.

[0004] Therefore, designing a method for adaptively adjusting parameters combined with the anti-disturbance control method to improve the robustness and accuracy of trajectory tracking control, thereby promoting the development and application of underwater robot technology, has important engineering value and significance. Summary of the invention

[0005] The technical problem to be solved by the present invention is to overcome the deficiencies in the prior art and provide a variable parameter self-disturbance rejection trajectory tracking control method for an underwater robot.

[0006] To achieve the above purpose, the technical solution adopted by the present invention is:

[0007] A variable parameter self-disturbance rejection trajectory tracking control method for an underwater robot is provided, comprising the following steps:

[0008] (1) Establish the dynamic model of the underwater robot;

[0009] (2) Design a four-degree-of-freedom extended state observer based on the dynamic model;

[0010] (3) Obtaining a target tracking sequence in the carrier coordinate system based on the expected trajectory in the inertial coordinate system;

[0011] (4) obtaining the motion parameters of the underwater robot in the carrier coordinate system at the current moment, including position, heading, velocity and angular velocity; and then using the extended state observer of each degree of freedom to calculate the estimated values ​​of the corresponding motion parameters respectively;

[0012] (5) performing error calculation based on the obtained estimated value and using an adaptive function to calculate, thereby obtaining the real-time parameter value of the extended state observer for each degree of freedom after taking into account the estimated result of the disturbance;

[0013] (6) Calculating the trajectory tracking control law based on the motion parameters of the target tracking point and the real-time parameter values ​​of each extended state observer at the current moment; updating the state and output of each extended state observer, and controlling the underwater robot;

[0014] (7) Determine whether the target tracking point at the current moment is the last target tracking point in the target tracking sequence. If so, complete the tracking; if not, update the next target tracking point and repeat steps (4) to (6).

[0015] The present invention also provides a computer device, comprising: at least one processor, and a memory communicatively connected to the at least one processor, wherein the memory stores instructions executed by the at least one processor, and the instructions are executed by the at least one processor so that the at least one processor executes the aforementioned underwater robot variable parameter self-anti-disturbance trajectory tracking control method.

[0016] The present invention further provides a computer-readable storage medium, wherein the computer-readable storage medium stores computer instructions, and the computer instructions are used to enable the computer to execute the aforementioned underwater robot variable parameter self-disturbance rejection trajectory tracking control method.

[0017] Compared with the prior art, the present invention has the following beneficial effects:

[0018] 1. The present invention is based on the anti-disturbance control algorithm, which can effectively estimate and compensate for the internal and external disturbances of the system, and is very suitable for the control problems of nonlinear and uncertain systems of underwater robots. Even in complex underwater environments, it can achieve efficient and stable trajectory tracking effects.

[0019] 2. The present invention proposes a new adaptive parameter adjustment method, which enables the parameters of the extended state observer of the core module to be automatically adjusted in real time according to the estimated effect, eliminating the complex work of manual parameter adjustment and improving the performance of the entire control system. BRIEF DESCRIPTION OF THE DRAWINGS

[0020] Figure 1 It is the algorithm flow chart of the present invention.

[0021] Figure 2 It is a diagram of the appearance and propeller layout of the underwater robot in a specific embodiment.

[0022] Figure 3 It is a coordinate system and underwater robot force analysis diagram in a specific embodiment. DETAILED DESCRIPTION

[0023] First of all, it should be explained that the present invention relates to underwater robot navigation technology, which is an application of computer technology in the field of autonomous driving technology. In the process of implementing the present invention, the application of multiple software function modules will be involved. The applicant believes that after carefully reading the application documents and accurately understanding the implementation principle and purpose of the present invention, in combination with the existing known technology, those skilled in the art can fully use their software programming skills to implement the present invention. All those mentioned in the application documents of the present invention belong to this category, and the applicant will not list them one by one.

[0024] Part I Implementation of the Invention

[0025] The underwater robot variable parameter self-disturbance rejection trajectory tracking control method of the present invention comprises the following steps:

[0026] 1. Establish the dynamic model of the underwater robot; specifically include:

[0027] (a) Assume that in the carrier coordinate system, the position of the underwater robot is [x, y, z], the velocity is [u, v, w], the heading is ψ, and the heading angular velocity is r;

[0028] (b) Establish the dynamic model of the underwater robot as expressed by the following formula:

[0029]

[0030] Where m is mass, I z is the moment of inertia, is the additional mass coefficient, X u , Y v , Z w 、N r is the linear damping coefficient, X uu , Y vv , Z ww 、N rr is the nonlinear damping coefficient, which is the inherent property of the underwater robot; W and B are the gravity and buoyancy of the underwater robot respectively; d x d y d z are the disturbance forces on each degree of freedom in the carrier coordinate system, d r is the disturbance torque; τ x , τ y , τz are the control forces on each degree of freedom, τ r To control the torque; adding a dot on a variable or symbol indicates the time derivative of the variable or symbol.

[0031] 2. Design a four-degree-of-freedom extended state observer based on the dynamic model;

[0032] The present invention uses a four-degree-of-freedom extended state observer, which corresponds to the four degrees of freedom in the dynamic model, specifically the forward motion degree of freedom, lateral motion degree of freedom, vertical motion degree of freedom and steering motion degree of freedom of the underwater robot, where:

[0033] (a) Extended state observer O corresponding to the forward motion degree of freedom 1 , its state space equation is as follows:

[0034]

[0035] in, is the estimated value of x, is the estimated value of u, and the superscript T is the transposition symbol; is the disturbance An estimated value of

[0036] θ 1 is the extended state observer O 1 The parameters of the extended state observer O 1 The input is u 1 =[τ x ,x] T , the output is h 1 ;

[0037] (b) Extended state observer O corresponding to the lateral motion degree of freedom 2 , its state space equation is as follows:

[0038]

[0039] in, is the estimated value of y, is the estimated value of v;

[0040] is the disturbance An estimated value of

[0041] θ 2 is the extended state observer O 2 The parameters of the extended state observer O 2 The input is u2 =[τ y ,y] T , the output is h 2 ;

[0042] (c) Extended state observer O corresponding to the vertical degree of freedom 3 , its state space equation is as follows:

[0043]

[0044] in, is the estimated value of z, is the estimated value of w;

[0045] is the disturbance An estimated value of

[0046] θ 3 is the extended state observer O 3 Parameters;

[0047] The extended state observer O 3 The input is u 3 =[τ z ,z] T , the output is h 3 ;

[0048] (d) Extended state observer O corresponding to the steering freedom 4 , its state space equation is as follows:

[0049]

[0050] in, is the estimated value of ψ, is the estimated value of r;

[0051] is the disturbance An estimated value of

[0052] θ 4 is the extended state observer O 4 Parameters;

[0053] The extended state observer O 4 The input is u 4 =[τ r ,ψ] T , the output is h 4 .

[0054] 3. According to the expected trajectory in the inertial coordinate system, the target tracking sequence in the carrier coordinate system is obtained; specifically including:

[0055] (a) Assume that the target trajectory in the inertial coordinate system is

[0056] First, the expected heading ψ of the underwater robot is calculated according to the following formula: d (t):

[0057]

[0058] (b) Then calculate the expected speed of the underwater robot according to the following formula: And the expected heading angular velocity r(t):

[0059]

[0060] (c) Let the sampling period be T, discretize the target trajectory to obtain the target tracking sequence The details are as follows:

[0061]

[0062] (d) As above, the expected heading and expected heading angular velocity are discretized to obtain the target heading sequence ψ d [k] and the target heading angular velocity sequence r d [k], as follows:

[0063] ψ d [k] = ψ d (kT), k=1,2,3,… (3.4)

[0064] r d [k] = r d (kT), k=1,2,3,…… (3.5)

[0065] (e) Calculate the target position sequence x in the carrier coordinate system according to the following formula d [k], y d [k], z d [k]:

[0066]

[0067] (f) Calculate the target velocity sequence u in the carrier coordinate system according to the following formula d [k]、v d [k], w d [k]:

[0068]

[0069] 4. Obtain the motion parameters of the underwater robot in the carrier coordinate system at the current moment, including position, heading, speed and angular velocity; then use the extended state observer of each degree of freedom to calculate the estimated values ​​of the corresponding motion parameters respectively; specifically including:

[0070] (a) Use the positioning sensor carried by the underwater robot to obtain the current position data x, y, z, use the speed sensor to obtain the current speed data u, v, w, use the compass data to obtain the current heading angle data ψ, and use the angular velocity sensor to obtain the current heading angular velocity data r;

[0071] (b) Using the extended state observer O 1 The output of , we get the estimate of x and an estimate of u Using the extended state observer O 2 The output of y is obtained and an estimate of v Using the extended state observer O 3 The output of , we get the estimate of z and an estimate of w Using the extended state observer O 4 The output of , we get the estimate of ψ and an estimate of r

[0072] 5. Based on the estimated value, the error is calculated and the adaptive function is used to calculate the real-time parameter value of the extended state observer of each degree of freedom after considering the estimated result of the disturbance; specifically, it includes:

[0073] (a) First, calculate the error of the estimated quantity obtained by the output of the extended state observer for each degree of freedom:

[0074]

[0075]

[0076]

[0077]

[0078]

[0079]

[0080]

[0081]

[0082] (b) Extended State Observer O1 The parameter θ 1 The calculation formula is:

[0083] θ 1 =θ 1l +(θ 1u -θ 1l )tanh(c 1 |ε x +ε u |) (5.9)

[0084] Among them, θ 1l and θ 1u are upper and lower bound parameters, and satisfy 1<θ 1l <θ 1u ;c 1 >0 is the control parameter;

[0085] (c) Extended State Observer O 2 The parameter θ 2 The calculation formula is:

[0086] θ 2 =θ 2l +(θ 2u -θ 2l )tanh(c 2 |ε y +ε v )(5.10)

[0087] Among them, θ 2l and θ 2u are upper and lower bound parameters, and satisfy 1<θ 2l <θ 2u ;c 2 >0 is the control parameter;

[0088] (d) Extended State Observer O 3 The parameter θ 3 The calculation formula is:

[0089] θ 3 =θ 3l +(θ 3u -θ 3l )tanh(c 3 |ε z +ε w |)(5.11)

[0090] Among them, θ 3l and θ 3u are upper and lower bound parameters, and satisfy 1<θ 3l <θ 3u ;c 3 >0 is the control parameter;

[0091] (e) Extended State Observer O 4 The parameter θ 4 The calculation formula is:

[0092] θ 4 =θ 4l +(θ 4u -θ 4l )tanh(c 4 |ε ψ +ε r |)(5.12)

[0093] Among them, θ 4l and θ 4u are upper and lower bound parameters, and satisfy 1<θ 4l <θ 4u ;c 4 >0 is the control parameter;

[0094] The above parameters are set according to the inertia properties of the controlled underwater robot and the actual control effect. Generally, the greater the inertia, the greater the parameter can be set.

[0095] 6. According to the motion parameters of the target tracking point at the current moment and the real-time parameter values ​​of each extended state observer, the trajectory tracking control law is calculated; the state and output of each extended state observer are updated, and the underwater robot is controlled; specifically, the following are included:

[0096] (a) Obtain the current target position x from the tracking sequence calculated in step (3) d ,y d 、z d , target speed u d 、v d 、w d and the target heading angle ψ d and the target heading angular velocity r d ;

[0097] (b) Using the extended state observer O 1 The output of the interference f 1 Estimator Using the extended state observer O 2 The output of the interference f 2 Estimator Using the extended state observer O 3 The output of the interference f 3 Estimation of Using the extended state observer O 4 The output of the interference f 4 Estimator

[0098] (c) The control law is calculated by the following formula:

[0099]

[0100] Among them, τ x , τ y , τ z and τ r is the control quantity to be applied to the underwater robot actuator; μ 1 , μ 2 , μ 3 , μ 4 These are all parameters with values ​​greater than 0 and not exceeding 100, and their values ​​are preset and adjusted according to the actual control effect;

[0101] (d) Setting the extended state observer O 1 The current input u 1 =[τ x ,x] T , using its state space equation (2.1) to update the state and output; set the extended state observer O 2 The current input u 2 =[τ y ,y] T , using its state space equation (2.2) to update the state and output; set the extended state observer O 3 The current input u 3 =[τ z ,z] T , using its state space equation (2.3) to update the state and output; set the extended state observer O 4 The current input u 4 =[τ r ,ψ] T , using its state space equation (2.4) to update the state and output;

[0102] (e) The control quantity τ x , τ y , τ z and τ r It is applied to the actuator to adjust the trajectory of the underwater robot according to the control law.

[0103] 7. Determine whether the target tracking point at the current moment is the last target tracking point in the target tracking sequence. If so, complete the tracking; if not, update the next target tracking point and repeat steps (4) to (6).

[0104] Part II: A specific implementation example

[0105] The underwater robot variable parameter self-disturbance rejection trajectory tracking control method provided in this example includes the following seven steps:

[0106] The first step is to establish the dynamic model of the underwater robot. The established dynamic model is:

[0107]

[0108] Figure 2 The figure in the middle is a schematic diagram of the structure of the underwater robot. The right side of the figure shows the layout direction of the six thrusters of the underwater robot. The marks in the right figure refer to different thruster numbers.

[0109] like Figure 3 As shown in the figure, the position of the underwater robot in the carrier coordinate system is [x, y, z], and the velocity is [u, v, w]; the heading of the underwater robot is ψ, and the heading angular velocity is r. In the above dynamic equation, m is the mass, I z is the moment of inertia; is the additional mass coefficient; X u , Y v , Z w 、N r is the linear damping coefficient, X uu , Y vv , Z ww 、N rr is the nonlinear damping coefficient, which is the inherent property of the underwater robot and can be obtained through experiments and measurements. W and B are the gravity and buoyancy of the underwater robot, respectively, which can also be obtained through measurements. x d y d z d r is the disturbance force and torque on each degree of freedom, which will be considered as part of the total disturbance and estimated in the algorithm. x , τ y , τ z , τ r are the control forces and moments on each degree of freedom.

[0110] Combination Figure 3 From the force analysis diagram, we can get the relationship between these quantities and the thrust of the propeller as follows:

[0111]

[0112] Among them, F 1 ,F 2 ,F 3 ,F 4 ,F 5 ,F 6 They are Figure 2Thrust of six thrusters in the thruster layout shown, where L is the length of the lever arm.

[0113] The second step is to design the extended state observer for each degree of freedom based on the established dynamic model. Specifically, it includes:

[0114] (a) Extended state observer O corresponding to the first degree of freedom 1 , its state space equation is as follows:

[0115]

[0116] in is the estimated value of x, is the estimated value of u;

[0117] is the disturbance An estimated value of

[0118] θ 1 is the extended state observer O 1 The parameters of the extended state observer O 1 The input is u 1 =[τ x ,x] T , the output is h 1 .

[0119] (b) Extended state observer O corresponding to the second degree of freedom 2 , its state space equation is as follows:

[0120]

[0121] in is the estimated value of y, is the estimated value of v;

[0122] is the disturbance estimated value of );

[0123] θ 2 is the extended state observer O 2 The parameters of the extended state observer O 2 The input is u 2 =[τ y ,y] T , the output is h 2 .

[0124] (c) Extended state observer O corresponding to the third degree of freedom 3 , its state space equation is as follows:

[0125]

[0126] in is the estimated value of z, is the estimated value of w;

[0127] is the disturbance An estimated value of

[0128] θ 3 is the extended state observer O 3 Parameters;

[0129] The extended state observer O 3 The input is u 3 =[τ z ,z] T , the output is h 3 .

[0130] (d) Extended state observer O corresponding to the fourth degree of freedom 4 , its state space equation is as follows:

[0131]

[0132] in is the estimated value of ψ, is the estimated value of r;

[0133] is the disturbance An estimated value of

[0134] θ 4 is the extended state observer O 4 Parameters;

[0135] The extended state observer O 4 The input is u 4 =[τ r ,ψ] T , the output is h 4 .

[0136] The third step is to design the extended state observer for each degree of freedom based on the established dynamic model. Specifically, it includes:

[0137] (a) In this implementation example, the target trajectory in the inertial coordinate system is assumed to be a three-dimensional spiral line, that is, First, calculate the expected heading ψ of the underwater robot d (t), the calculation process is as follows:

[0138]

[0139] (b) Then calculate the expected speed of the underwater robot And the expected heading angular velocity r(t), the calculation process is as follows:

[0140]

[0141] (c) In this experimental example, the sampling period is set to T = 0.05s, and the target trajectory is discretized to obtain the target trajectory sequence as follows:

[0142]

[0143] (d) As above, the expected heading and expected heading angular velocity are discretized to obtain the target heading sequence and target heading angular velocity sequence as follows:

[0144] ψ d [k] = ψ d (kT), k=1,2,3,……

[0145] r d [k] = r d (kT), k=1,2,3,……

[0146] (e) Calculate the target position sequence in the carrier coordinate system. The calculation formula is as follows:

[0147]

[0148] (f) Calculate the target velocity sequence in the carrier coordinate system. The calculation formula is as follows:

[0149]

[0150] The fourth step is to obtain the current position, heading, speed and angular velocity of the underwater robot in the carrier coordinate system, as well as the estimated values ​​of these physical quantities of the extended state observer of each degree of freedom. Specifically including:

[0151] (a) Use the positioning sensor carried by the underwater robot to obtain the current position data x, y, z, use the speed sensor to obtain the current speed data u, v, w, use the compass data to obtain the current heading angle data ψ, and use the angular velocity sensor to obtain the current heading angular velocity data r.

[0152] (b) Using the extended state observer O 1 The output of can get the estimate of x and an estimate of u Using the extended state observer O 2The output of can get the estimate of y and an estimate of v Using the extended state observer O 3 The output of can be used to obtain an estimate of z and an estimate of w Using the extended state observer O 4 The output of can be used to obtain an estimate of ψ and an estimate of r

[0153] The fifth step is to calculate the estimated error based on the data from the previous step, and use the adaptive function to calculate the real-time parameter value of the extended state observer for each degree of freedom. Specifically, it includes the following steps:

[0154] (a) First, calculate the estimated error of each data. The estimated error of x is The estimated error of y is The estimated error of z is The estimated error of ψ is The estimated error of u is The estimated error of v is The estimated error of w is The estimated error of r is

[0155] (b) Next, calculate the parameters of the extended state observer for each degree of freedom.

[0156] Extended State Observer 1 The parameter θ 1 =θ 1l +(θ 1u -θ 1l )tanh(c 1 |ε x +ε u |). where θ 1l and θ 1u are upper and lower bounds, and satisfy 1<θ 1l <θ 1u ;c 1 >0 is the control parameter; these three parameters need to be set manually according to the control law.

[0157] Extended State Observer 2 The parameter θ 2 =θ 2l +(θ 2u -θ 2l )tanh(c 2 |ε y +ε v ). where θ 2l and θ 2uare upper and lower bounds, and satisfy 1<θ 2l <θ 2u ;c 2 >0 is the control parameter; these three parameters need to be set manually according to the control law.

[0158] Extended State Observer 3 The parameter θ 3 =θ 3l +(θ 3u -θ 3l )tanh(c 3 |ε z +ε w ). where θ 3l and θ 3u are upper and lower bounds, and satisfy 1<θ 3l <θ 3u ;c 3 >0 is the control parameter; these three parameters need to be set manually according to the control law.

[0159] Extended State Observer 4 The parameter θ 4 =θ 4l +(θ 4u -θ 4l )tanh(c 4 |ε ψ +ε r ). where θ 4l and θ 4u are upper and lower bounds, and satisfy 1<θ 4l <θ 4u ;c 4 >0 is the control parameter; these three parameters need to be set manually according to the control law.

[0160] The sixth step is to obtain the target tracking point at the current moment and the estimation results of the disturbance amount of each extended state observer, calculate the trajectory tracking control law, update the state and output of each extended state observer, and control the underwater robot. Specifically, it includes the following steps:

[0161] (a) Get the current target position x from the tracking sequence calculated in the third step d ,y d , z d , target speed u d , v d , w d and the target heading angle ψ d and the target heading angular velocity r d .

[0162] (b) Using the extended observer O 1 The output of the interference f 1Estimator Using the extended observer O 2 The output of the interference f 2 Estimator Using the extended observer O 3 The output of the interference f 3 Estimator Using the extended observer O 4 The output of the interference f 4 Estimator

[0163] (c) From this, the control law can be calculated as follows:

[0164]

[0165] where μ 1 , μ 2 , μ 3 , μ 4 Both are parameters greater than 0 and need to be set manually according to the control law.

[0166] For this implementation example, the specific thrust value of each thruster needs to be calculated. According to the relationship between the control amount obtained in the first step and the thrust of each thruster, the thrust value can be obtained by solving the equation as follows:

[0167]

[0168] (d) Setting the extended observer O 1 The current input u 1 =[τ x ,x] T , use its state equation (2.1) to update the state and output; set the extended observer O 2 The current input u 2 =[τ y ,y] T , use its state equation (2.2) to update the state and output; set the extended observer O 3 The current input u 3 =[τ z ,z] T , use its state equation (2.3) to update the state and output; set the expanded observer O 4 The current input u 4 =[τ r ,ψ] T , using its state equation (2.4) to update the state and output.

[0169] (e) The control quantity τ x , τ y , τ zand τ r It is applied to the actuator to adjust the trajectory of the underwater robot according to the control law.

[0170] Step 7: Determine whether it is the last target tracking point. If so, the tracking is completed; otherwise, the target tracking point is updated and the above steps 4 to 6 are repeated.

[0171] This implementation example elaborates the implementation steps of the present invention - a variable parameter self-disturbance rejection trajectory tracking control method for an underwater robot by combining a specific underwater robot. The present invention designs an adaptive parameter adjustment method so that the parameters of the extended state observer module can be automatically adjusted in real time according to the state estimation effect of the underwater robot. On the one hand, it eliminates the complicated work of manual parameter adjustment, and on the other hand, it greatly improves the performance of the entire control system. It can enable the underwater robot to achieve efficient and stable trajectory tracking effect in a complex underwater environment.

[0172] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit it. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the aforementioned embodiments, or replace some or all of the technical features therein with equivalents. However, these modifications or replacements do not cause the essence of the corresponding technical solutions to deviate from the scope of the technical solutions of the embodiments of the present invention.

Claims

1. A variable parameter auto-disturbance rejection trajectory tracking control method for an underwater robot, characterized in that: The following steps are involved: (1) Establish the dynamic model of the underwater robot; (2) Design a four-degree-of-freedom extended state observer based on the dynamic model; (3) Obtaining a target tracking sequence in the carrier coordinate system based on the expected trajectory in the inertial coordinate system; (4) obtaining the motion parameters of the underwater robot in the carrier coordinate system at the current moment, including position, heading, velocity and angular velocity; and then using the extended state observer of each degree of freedom to calculate the estimated values ​​of the corresponding motion parameters respectively; (5) performing error calculation based on the obtained estimated value and using an adaptive function to calculate, thereby obtaining the real-time parameter value of the extended state observer for each degree of freedom after taking into account the estimated result of the disturbance; (6) Calculate the trajectory tracking control law based on the motion parameters of the target tracking point and the real-time parameter values ​​of each extended state observer at the current moment; Update the state and output of each extended state observer and control the underwater robot; (7) Determine whether the target tracking point at the current moment is the last target tracking point in the target tracking sequence. If so, complete the tracking; if not, update the next target tracking point and repeat steps (4) to (6).

2. The method according to claim 1, characterized in that In step (1), the dynamic model of the underwater robot is established according to the following steps: (a) Assume that in the carrier coordinate system, the position of the underwater robot is [x, y, z], the velocity is [u, v, w], the heading is ψ, and the heading angular velocity is r; (b) Establish the dynamic model of the underwater robot as expressed by the following formula: Where m is mass, I z is the moment of inertia, is the additional mass coefficient, X u , Y v , Z w 、N r is the linear damping coefficient, X uu , Y vv , Z ww 、N rr is the nonlinear damping coefficient, which is the inherent property of the underwater robot; W and B are the gravity and buoyancy of the underwater robot respectively; d x ,d y ,d z are the disturbance forces on each degree of freedom in the carrier coordinate system, d r is the disturbance torque; τ x , τ y , τ z are the control forces on each degree of freedom, τ r To control the torque; adding a dot on a variable or symbol indicates the time derivative of the variable or symbol.

3. The method according to claim 1, characterized in that The four degrees of freedom extended state observers described in step (2) correspond to the four degrees of freedom in the dynamic model, specifically the forward motion degree of freedom, lateral motion degree of freedom, vertical motion degree of freedom and steering motion degree of freedom of the underwater robot, wherein: (a) The state space equation of the extended state observer O1 corresponding to the forward motion degree of freedom is as follows: in, is the estimated value of x, is the estimated value of u, and the superscript T is the transposition symbol; is the disturbance An estimated value of θ1 is the parameter of the extended state observer O1; the input of the extended state observer O1 is u1 = [τ x ,x] T , the output is h1; (b) The state space equation of the extended state observer O2 corresponding to the lateral motion degree of freedom is as follows: in, is the estimated value of y, is the estimated value of v; is the disturbance An estimated value of θ2 is the parameter of the extended state observer O2; the input of the extended state observer O2 is u2 = [τ y ,y] T , the output is h2; (c) The state space equation of the extended state observer O3 corresponding to the vertical motion degree of freedom is as follows: in, is the estimated value of z, is the estimated value of w; is the disturbance An estimated value of θ3 is the parameter of the extended state observer O3; The input of the extended state observer O3 is u3 = [τ z ,z] T , the output is h3; (d) The state space equation of the extended state observer O4 corresponding to the steering motion degree of freedom is as follows: in, is the estimated value of ψ, is the estimated value of r; is the disturbance An estimated value of θ4 is the parameter of the extended state observer O4; The input of the extended state observer O4 is u4 = [τ r ,ψ] T , the output is h4.

4. The method according to claim 1, characterized in that: In step (3), the target tracking sequence in the carrier coordinate system is obtained by following the steps below: (a) Assume that the target trajectory in the inertial coordinate system is First, the expected heading ψ of the underwater robot is calculated according to the following formula: d (t): (b) Then calculate the expected speed of the underwater robot according to the following formula: And the expected heading angular velocity r(t): (c) Let the sampling period be T, discretize the target trajectory to obtain the target tracking sequence The details are as follows: (d) As above, the expected heading and expected heading angular velocity are discretized to obtain the target heading sequence ψ d [k] and the target heading angular velocity sequence r d [k], as follows: ψ d [k]=ψ d (kT),k=1,2,3,… (3.4) r d [k]=r d (kT),k=1,2,3,… (3.5) (e) Calculate the target position sequence x in the carrier coordinate system according to the following formula d [k], y d [k], z d [k]: (f) Calculate the target velocity sequence u in the carrier coordinate system according to the following formula d [k]、v d [k], w d [k]:

5. The method according to claim 1, characterized in that In step (4), the position, heading, velocity and angular velocity of the underwater robot in the carrier coordinate system at the current moment, as well as the estimated values ​​of these physical quantities of the extended state observers of each degree of freedom are obtained according to the following steps: (a) Use the positioning sensor carried by the underwater robot to obtain the current position data x, y, z, use the speed sensor to obtain the current speed data u, v, w, use the compass data to obtain the current heading angle data ψ, and use the angular velocity sensor to obtain the current heading angular velocity data r; (b) Using the output of the extended state observer O1, we can obtain an estimate of x: and an estimate of u Using the output of the extended state observer O2, we can get an estimate of y and an estimate of v Using the output of the extended state observer O3, we can get an estimate of z and an estimate of w Using the output of the extended state observer O4, we can get an estimate of ψ and an estimate of r 6. The method according to claim 1, characterized in that In step (5), the estimation error is calculated in the following manner, and then the adaptive function is used to calculate the real-time parameter value of the extended state observer for each degree of freedom: (a) First, calculate the error of the estimated quantity obtained by the output of the extended state observer for each degree of freedom: (b) The calculation formula of the parameter θ1 of the extended state observer O1 is: θ1=θ 1l +(θ 1u -θ 1l )tanh(c1|ε x +e u |) (5.9) Among them, θ 1l and θ 1u are upper and lower bound parameters, and satisfy 1<θ 1l <θ 1u ; c1>0 is the control parameter; (c) The calculation formula of the parameter θ2 of the extended state observer O2 is: θ2=θ 2l +(θ 2u -θ 2l )tanh(c2|ε y +e v |) (5.10) Among them, θ 2l and θ 2u are upper and lower bound parameters, and satisfy 1<θ 2l <θ 2u ; c2>0 is the control parameter; (d) The calculation formula of the parameter θ3 of the extended state observer O3 is: θ3=θ 3l +(θ 3u -θ 3l )tanh(c3|e z +e w |) (5.11) Among them, θ 3l and θ 3u are upper and lower bound parameters, and satisfy 1<θ 3l <θ 3u ; c3>0 is the control parameter; (e) The calculation formula of the parameter θ4 of the extended state observer O4 is: θ4=θ 4l +(θ 4u -θ 4l )tanh(c4|e ψ +e r |) (5.12) Among them, θ 4l and θ 4u are upper and lower bound parameters, and satisfy 1<θ 4l <θ 4u ; c4>0 is the control parameter; The above-mentioned parameters and control parameters are set according to the inertial properties of the controlled underwater robot and the actual control effect.

7. The method according to claim 1, characterized in that The step (6) specifically comprises: (a) Obtain the current target position x from the tracking sequence calculated in step (3) d ,y d 、z d , target speed u d 、v d 、w d and the target heading angle ψ d and the target heading angular velocity r d ; (b) Using the output of the extended state observer O1, we can obtain an estimate of the disturbance f1: The output of the extended state observer O2 is used to obtain the estimate of the disturbance f2 The output of the extended state observer O3 is used to obtain the estimate of the disturbance f3 The output of the extended state observer O4 is used to obtain the estimate of the disturbance f4 (c) The control law is calculated by the following formula: Among them, τ x , τ y , τ z and τ r is the control quantity to be applied to the actuator of the underwater robot; μ1, μ2, μ3, and μ4 are all parameters with values ​​greater than 0 and less than 100, and their values ​​are preset and adjusted according to the actual control effect; (d) Set the input u1 of the extended state observer O1 at the current moment = [τ x ,x] T , using its state space equation (2.1) to update the state and output; set the input u2 of the extended state observer O2 at the current moment = [τ y ,y] T , using its state space equation (2.2) to update the state and output; set the input u3 of the extended state observer O3 at the current moment = [τ z ,z] T , using its state space equation (2.3) to update the state and output; set the input u4 of the extended state observer O4 at the current moment = [τ r ,ψ] T , using its state space equation (2.4) to update the state and output; (e) The control quantity τ x , τ y , τ z and τ r It is applied to the actuator to adjust the trajectory of the underwater robot according to the control law.

8. A computer device, characterized in that: include: At least one processor, and a memory communicatively connected to the at least one processor, wherein the memory stores instructions executed by the at least one processor, and the instructions are executed by the at least one processor so that the at least one processor executes the underwater robot variable parameter self-disturbance rejection trajectory tracking control method according to any one of claims 1 to 7.

9. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores computer instructions, and the computer instructions are used to enable the computer to execute the underwater robot variable parameter self-disturbance rejection trajectory tracking control method according to any one of claims 1 to 7.

Images