An underwater robot variable parameter active disturbance rejection trajectory tracking control method

Abstract

The present application relates to the field of underwater robot automatic control, and aims to provide a kind of underwater robot variable parameter self-disturbance trajectory tracking control method.It includes: the dynamics model of underwater robot is established, four degrees of freedom extended state observer is designed based on dynamics model, the estimated value of corresponding motion parameter is calculated using each degree of freedom extended state observer respectively, error calculation is carried out, and real-time parameter value is obtained using adaptive function calculation;According to the trajectory tracking control law obtained by calculation, the state and output of each extended state observer are updated, and the underwater robot is controlled.The present application is based on active disturbance rejection control algorithm, can effectively estimate and compensate internal and external disturbances of system, is very suitable for the control problem of underwater robot nonlinear and uncertain system.Even in complex underwater environment, efficient and stable trajectory tracking effect can be achieved;Using adaptive parameter adjustment eliminates manual adjustment work and improves the performance of control system.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of underwater robot automatic control, and particularly relates to a variable parameter active disturbance rejection trajectory tracking control method for underwater robots. BACKGROUND

[0002] An underwater robot is an autonomous or remotely controlled device used in underwater environments, and is widely used in fields such as ocean exploration, environmental monitoring, resource development, military tasks and scientific research. Due to the high nonlinearity, strong coupling and multiple disturbances of the underwater robot system, how to achieve high-precision trajectory tracking is a difficult problem. For underwater robots, trajectory tracking needs to consider the water dynamics and nonlinear effects caused by its own motion as well as external disturbances such as water flow. These factors increase the difficulty of control, and require the control algorithm to have strong robustness and adaptability.

[0003] Active disturbance rejection control is a modern control method that improves the anti-disturbance ability of a system by estimating and compensating for internal and external disturbances. It does not rely on the accurate mathematical model of the controlled object, but estimates the system state and disturbance in real time through an extended state observer, and eliminates the influence 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 active disturbance rejection control uses fixed parameters, and relying on manual adjustment of parameters will bring a large amount of work. Moreover, the optimal parameter value will change in different scenarios, resulting in poor control effect.

[0004] Therefore, it is of great engineering value and significance to design a method of adaptively adjusting parameters, combined with the active disturbance rejection control method to improve the robustness and precision of trajectory tracking control, thereby promoting the development and application of underwater robot technology. SUMMARY

[0005] The technical problem to be solved by the present application is to overcome the shortcomings in the prior art and provide a variable parameter active disturbance rejection trajectory tracking control method for underwater robots.

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

[0007] A variable parameter active disturbance rejection trajectory tracking control method for underwater robots is provided, comprising the following steps:

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

[0009] (2) designing 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 according to the expected trajectory in the inertial coordinate system;

[0011] (4) Obtain the motion parameters of the underwater robot in the carrier coordinate system at the current time, including position, heading, speed and angular velocity, and then use the extended state observer of each degree of freedom to calculate the estimated value of the corresponding motion parameter;

[0012] (5) Based on the obtained estimated value, the error is calculated, and the real-time parameter value of each degree of freedom extended state observer considering the estimation result of the disturbance is obtained by using the adaptive function calculation;

[0013] (6) According to the motion parameters of the target tracking point at the current time and the real-time parameter value 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;

[0014] (7) Determine whether the target tracking point at the current time is the last target tracking point in the target tracking sequence, if yes, the tracking is completed, if not, the next target tracking point is updated, and the operations of steps (4)-(6) are repeated.

[0015] The application also provides a computer device, comprising: at least one processor, and a memory in communication connection with 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 to make the at least one processor execute the underwater robot variable parameter active disturbance trajectory tracking control method.

[0016] The application further provides a computer readable storage medium, which stores computer instructions, and the computer instructions are used to make the computer execute the underwater robot variable parameter active disturbance trajectory tracking control method.

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

[0018] 1. The application is based on the active disturbance control algorithm, can effectively estimate and compensate internal and external disturbances of the system, and is very suitable for the control problem of the nonlinear and uncertain system of the underwater robot. Even in a complex underwater environment, efficient and stable trajectory tracking effect can be achieved.

[0019] 2. The application proposes a new adaptive parameter adjustment method, so that the parameters of the core module extended state observer can be automatically adjusted in real time according to the estimation effect, the complex work of manual parameter adjustment is avoided, and the performance of the whole control system is improved. DETAILED DESCRIPTION

[0020] Figure 1 is the algorithm flowchart of the application.

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

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

[0023] First of all, it should be pointed out that the present application relates to underwater robot navigation technology, which is an application of computer technology in the field of automatic driving technology. In the implementation process of the present application, the application of multiple software function modules will be involved. The applicant believes that, after carefully reading the application file and accurately understanding the implementation principle and purpose of the present application, and in combination with the existing known technology, the skilled person in the art can certainly implement the present application by using the software programming skills mastered. Whatever mentioned in the application file of the present application belongs to this category, and the applicant will not list them one by one.

[0024] First part of the implementation scheme of the present application

[0025] The underwater robot variable parameter active disturbance rejection trajectory tracking control method described in the present application comprises the following steps:

[0026] 1. Establishing a dynamic model of the underwater robot; specifically including:

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

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

[0029]

[0030] Wherein, m is the mass, I z is the moment of inertia, is the added 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 in each degree of freedom in the carrier coordinate system, d r is the disturbance torque; τ x , τ y , τz are control forces on each degree of freedom, respectively r are control moments; a dot above a variable or symbol indicates the derivative of the variable or symbol with respect to time.

[0031] 2. Designing four degrees of freedom extended state observers based on the dynamic model;

[0032] The four degrees of freedom extended state observers are used in the application, which correspond to four degrees of freedom in the dynamic model, specifically forward movement degree of freedom, lateral movement degree of freedom, vertical movement degree of freedom and turning movement degree of freedom of the underwater robot, wherein:

[0033] (a) The extended state observer O1 corresponding to the forward movement degree of freedom has the following state space equation:

[0034]

[0035] wherein, is an estimated value of x, is an estimated value of u, and the superscript T is a transpose symbol; is an estimated value of the disturbance ;

[0036] θ1 is a parameter of the extended state observer O1; the input of the extended state observer O1 is u1=[τ x ,x] T , and the output is h1;

[0037] (b) The extended state observer O2 corresponding to the lateral movement degree of freedom has the following state space equation:

[0038]

[0039] wherein, is an estimated value of y, is an estimated value of v;

[0040] is an estimated value of the disturbance ;

[0041] θ2 is a parameter of the extended state observer O2; the input of the extended state observer O2 is u2=[τ y ,y] T , and the output is h2;

[0042] (c) The extended state observer O3 corresponding to the vertical movement degree of freedom has the following state space equation:

[0043]

[0044] wherein, is an estimate of z, is an estimate of w;

[0045] is an estimate of the disturbance ;

[0046] θ3 is a parameter of the extended state observer O3;

[0047] The input of the extended state observer O3 is u3 = [τ z , z] T , and the output is h3;

[0048] (d) the extended state observer O4 corresponding to the steering motion degree of freedom, whose state space equation is as follows:

[0049]

[0050] wherein, is an estimate of ψ, is an estimate of r;

[0051] is an estimate of the disturbance ;

[0052] θ4 is a parameter of the extended state observer O4;

[0053] The input of the extended state observer O4 is u4 = [τ r , ψ] T , and the output is h4.

[0054] 3. Obtain the target tracking sequence in the carrier coordinate system according to the expected trajectory in the inertial coordinate system; specifically comprising:

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

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

[0057]

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

[0059]

[0060] (c) Set the sampling period as T, discretize the target trajectory to get the target tracking sequence Specifically as follows:

[0061]

[0062] (d) As above, discretize the expected heading and expected heading angular velocity to get the target heading sequence ψ d [k] and target heading angular velocity sequence r d [k], specifically 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 d [k], y d [k], z d [k] in the carrier coordinate system according to the following formula:

[0066]

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

[0068]

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

[0070] (a) Use the positioning sensor carried by the underwater robot to obtain the current position data x, y, z, use the velocity sensor to obtain the current velocity 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) Use the output of the extended state observer O1 to get the estimated value of x and the estimated value of u Use the output of the extended state observer O2 to get the estimated value of y and the estimator of v Using the output of the extended state observer O3, an estimate of z is obtained. and the estimator of w An estimate of ψ is obtained using the output of the extended state observer O4. and the estimator of r

[0072] 5. Based on the obtained estimates, error calculation is performed, and an adaptive function is used to calculate the real-time parameter values ​​of the extended state observer for each degree of freedom, taking into account the estimation results of the disturbance; specifically including:

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

[0074]

[0075]

[0076]

[0077]

[0078]

[0079]

[0080]

[0081]

[0082] (b) The formula for calculating the parameter θ1 of the extended state observer O1 is:

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

[0084] Where, θ 1l and θ 1u Let θ be the upper and lower bound parameters, and satisfy 1 < θ 1l <θ 1u c1 > 0 is a control parameter;

[0085] (c) The formula for calculating the parameter θ2 of the extended state observer O2 is:

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

[0087] where θ 2l and θ 2u are upper and lower bound parameters, and satisfy 1 < θ 2l < θ 2u ; c2 > 0 is a control parameter;

[0088] (d) The parameter θ3 of the extended state observer O3 is calculated according to the following formula:

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

[0090] where θ 3l and θ 3u are upper and lower bound parameters, and satisfy 1 < θ 3l < θ 3u ; c3 > 0 is a control parameter;

[0091] (e) The parameter θ4 of the extended state observer O4 is calculated according to the following formula:

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

[0093] where θ 4l and θ 4u are upper and lower bound parameters, and satisfy 1 < θ 4l < θ 4u ; c4 > 0 is a control parameter;

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

[0095] 6. According to the motion parameters of the target tracking point at the current time and the real-time parameter values of the extended state observers, the trajectory tracking control law is calculated; the states and outputs of the extended state observers are updated, and the underwater robot is controlled; specifically including:

[0096] (a) The target position x d , y d , z dtarget velocity u d , v d , w d and target heading angle ψ d and target heading angle rate r d ;

[0097] (b) obtaining an estimated value of the disturbance f1 by using the output of the extended state observer O1 obtaining an estimated value of the disturbance f2 by using the output of the extended state observer O2 obtaining an estimated value of the disturbance f3 by using the output of the extended state observer O3 obtaining an estimated value of the disturbance f4 by using the output of the extended state observer O4

[0098] (c) obtaining the control law by calculating according to the following formula:

[0099]

[0100] wherein τ x , τ y , τ z and τ r are control values to be applied to the actuators of the underwater robot; μ1, μ2, μ3, μ4 are all parameters with values greater than 0 and not more than 100, which are preset and adjusted according to actual control effect;

[0101] (d) setting the input u1 = [τ x , x] T of the extended state observer O1 at the current time, updating the state and output by using the state space equation (2.1) thereof; setting the input u2 = [τ y , y] T of the extended state observer O2 at the current time, updating the state and output by using the state space equation (2.2) thereof; setting the input u3 = [τ z , z] T of the extended state observer O3 at the current time, updating the state and output by using the state space equation (2.3) thereof; setting the input u4 = [τ r , ψ] T of the extended state observer O4 at the current time, updating the state and output by using the state space equation (2.4) thereof;

[0102] (e) applying the control values τ x , τ y , τ z and τ r to the actuators, so as to adjust the running trajectory of the underwater robot according to the control law.

[0103] 7. Determine whether the target tracking point at the current time is the last target tracking point in the target tracking sequence. If yes, complete the tracking; if no, update the next target tracking point, and repeat the operations of steps (4)-(6).

[0104] A specific implementation example of the second part

[0105] The underwater robot variable parameter active disturbance trajectory tracking control method provided in the example specifically includes the following seven steps:

[0106] First, establish the dynamics model of the underwater robot. The established dynamics model is:

[0107]

[0108] Figure 2 Fig. is a structural diagram of the underwater robot, and the right side of the figure shows the layout direction of the six thrusters of the underwater robot. The labels in the right figure respectively denote different thruster numbers.

[0109] As Figure 3 shown, 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 angle velocity is r. In the above dynamics equation, m is the mass, I z is the rotational inertia; is the added 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, and can also be obtained by measurement. d x , d y , d z , d r are the disturbance forces and torques in each degree of freedom, which will be part of the total disturbance and will be estimated in the algorithm. τ x , τ y , τ z , τ r are the control forces and torques in each degree of freedom.

[0110] In combination with Figure 3 the force analysis diagram, the relationship between these quantities and the thruster thrust can be obtained as follows:

[0111]

[0112] where F1, F2, F3, F4, F5, F6 are the thrusts of the six thrusters in the thruster layout shown in Fig. 1, and L is the length of the force arm. Figure 2

[0113] Secondly, based on the established dynamic model, the extended state observers for each degree of freedom are designed. Specifically, it includes:

[0114] (a) The extended state observer O1 corresponding to the first degree of freedom, whose state space equation is as follows:

[0115]

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

[0117] is the estimated value of the disturbance ;

[0118] θ1 is the parameter of the extended state observer O1; the input of the extended state observer O1 is u1 = [τ x , x] T , and the output is h1.

[0119] (b) The extended state observer O2 corresponding to the second degree of freedom, whose state space equation is as follows:

[0120]

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

[0122] is the estimated value of the disturbance ;

[0123] θ2 is the parameter of the extended state observer O2; the input of the extended state observer O2 is u2 = [τ y , y] T , and the output is h2.

[0124] (c) The extended state observer O3 corresponding to the third degree of freedom, whose state space equation is as follows:

[0125]

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

[0127] is the estimated value of the disturbance ;

[0128] θ3 is the parameter of the extended state observer O3;

[0129] The input of the extended state observer O3 is u3 = [τ z , z] T , and the output is h3.

[0130] (d) The extended state observer O4 corresponding to the fourth degree of freedom, whose state space equation is as follows:

[0131]

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

[0133] is the estimated value of the disturbance ;

[0134] θ4 is the parameter of the extended state observer O4;

[0135] The input of the extended state observer O4 is u4 = [τ r , ψ] T , and the output is h4.

[0136] Thirdly, design the extended state observers of each degree of freedom based on the established dynamic model. Specifically, it includes:

[0137] (a) In this embodiment, the target trajectory in the inertial coordinate system is a three-dimensional spiral, i.e. First, calculate the expected heading ψ d (t) of the underwater robot, and the calculation process is as follows:

[0138]

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

[0140]

[0141] (c) In this experiment, the sampling period is set to T = 0.05 s, and the target trajectory sequence is obtained by discretizing the target trajectory as follows:

[0142]

[0143] (d) As above, discretize the expected heading and expected heading angular velocity to get 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, and the calculation formula is as follows:

[0147]

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

[0149]

[0150] Fourth step, get the position, heading, velocity and angular velocity of the underwater robot in the carrier coordinate system at the current time, and the estimated value of each degree of freedom extended state observer for these physical quantities. Specifically, it includes:

[0151] (a) Use the positioning sensor carried by the underwater robot to obtain the current position data x, y, z, use the velocity sensor to obtain the current velocity 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) The output of the extended state observer O1 can get the estimated value of x and the estimated value of u The output of the extended state observer O2 can get the estimated value of y and the estimated value of v The output of the extended state observer O3 can get the estimated value of z and the estimated value of w The output of the extended state observer O4 can get the estimated value of ψ and the estimated value of r

[0153] Fifth step, calculate the estimation error according to the data of the last step, and use the adaptive function to calculate the real-time parameter value of each degree of freedom extended state observer. Specifically, it includes the following steps:

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

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

[0156] The parameters θ1 = θ of the extended state observer O1 1l +(θ 1u -θ 1l )tanh(c1|ε x +ε u |). Where θ 1l and θ 1u The upper and lower bounds are defined, and 1 < θ. 1l <θ 1u c1 > 0 is a control parameter; these three parameters need to be set manually according to the control law.

[0157] The parameters of the extended state observer O2 are θ2 = θ 2l +(θ 2u -θ 2l )tanh(c2|ε y +ε v ). Where θ 2l and θ 2u The upper and lower bounds are defined, and 1 < θ. 2l <θ 2u c2 > 0 is a control parameter; these three parameters need to be set manually according to the control law.

[0158] The parameters θ3 = θ of the extended state observer O3 3l +(θ 3u -θ 3l )tanh(c3|ε z +ε w ). Where θ 3l and θ 3u The upper and lower bounds are defined, and 1 < θ. 3l <θ 3u c3 > 0 is a control parameter; these three parameters need to be set manually according to the control law.

[0159] The parameters θ4 = θ of the extended state observer O4 4l +(θ 4u -θ4l )tanh(c4|ε ψ +ε r ). Where θ 4l and θ 4u The upper and lower bounds are defined, and 1 < θ. 4l <θ 4u c4 > 0 is a control parameter; these three parameters need to be set manually according to the control law.

[0160] The sixth step involves acquiring the target tracking point at the current moment and the estimation results of the disturbance from each extended state observer, calculating the trajectory tracking control law, updating the state and output of each extended state observer, and implementing control over the underwater robot. This specifically includes the following steps:

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

[0162] (b) The output of the extended observer O1 is used to obtain an estimate of the disturbance f1. The output of the extended observer O2 is used to obtain an estimate of the disturbance f2. An estimate of the disturbance f3 is obtained using the output of the extended observer O3. An estimate of the disturbance f4 is obtained using the output of the extended observer O4.

[0163] (c) The control law can be calculated as follows:

[0164]

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

[0166] For this implementation example, it is also necessary to calculate the specific thrust value of each thruster. Based on the relationship between the control quantity obtained in the first step and the thrust of each thruster, the equation can be solved to obtain the thrust value as follows:

[0167]

[0168] (d) Set the input u1 = [τ] of the extended observer O1 at the current time. x ,x] T, update the state and output by using its state equation (2.1); set the input u2 = [τ y ,y] T , update the state and output by using its state equation (2.2); set the input u3 = [τ z ,z] T , update the state and output by using its state equation (2.3); set the input u4 = [τ r ,ψ] T , update the state and output by using its state equation (2.4).

[0169] (e) apply the control quantities τ x , τ y , τ z and τ r to the actuators, so as to adjust the operating trajectory of the underwater robot according to the control law.

[0170] Step 7: judge whether it is the last target tracking point. If yes, complete the tracking; otherwise, update the target tracking point and repeat the above steps 4 to 6.

[0171] The embodiment example elaborates the implementation steps of the present application, a variable parameter active disturbance trajectory tracking control method for underwater robots, in combination with a specific underwater robot. The present application 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, the complex work of manual parameter adjustment is eliminated, and on the other hand, the performance of the entire control system is greatly improved. The underwater robot can achieve efficient and stable trajectory tracking effect in complex underwater environment.

[0172] Finally, it should be noted that: the above embodiments are only used to illustrate the technical solutions of the present application, and not to limit them; although the present application has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that: it can still modify the technical solutions recorded in the foregoing embodiments, or make equivalent replacement to part or all of the technical features; and these modifications or replacements do not make the essence of the corresponding technical solutions deviate from the scope of the technical solutions of the embodiments of the present application.

Claims

1. A variable parameter active disturbance rejection trajectory tracking control method for an underwater robot, characterized in that, The method comprises the following steps: (1) establishing a dynamic model of the underwater robot; (2) designing four-degree-of-freedom extended state observers based on the dynamic model; (3) obtaining a target tracking sequence in the carrier coordinate system according to an expected trajectory in the inertial coordinate system; (4) obtaining motion parameters of the underwater robot in the carrier coordinate system at the current time, including position, heading, velocity and angular velocity; then calculating the estimated values of the corresponding motion parameters by using the extended state observers of each degree of freedom respectively; (5) calculating the error based on the obtained estimated values, and using an adaptive function to calculate the real-time parameter values of the extended state observers of each degree of freedom considering the estimated results of the disturbance; (6) calculating a trajectory tracking control law according to the motion parameters of the target tracking point at the current time and the real-time parameter values of the extended state observers, updating the states and outputs of the extended state observers, and implementing control on the underwater robot; The calculation formula of the control law is as follows: wherein τ x , τ y , τ z , τ r are control quantities to be applied to the actuators of the underwater robot; m is the mass; I z is the moment of inertia; is the added mass coefficient; μ1, μ2, μ3, μ4 are parameters with values greater than 0 and not exceeding 100, which are preset and adjusted according to the actual control effect; x d , y d , z d , ψ d are the target position and target heading angle at the current time; are the position and heading angle estimates of the underwater robot; u d , v d , w d , r d are the target velocity and target heading angular velocity; are the velocity and heading angular velocity estimates of the underwater robot in each direction; is the estimate of the disturbance quantity; (7) judging whether the target tracking point at the current time is the last target tracking point in the target tracking sequence, if yes, completing the tracking; if not, updating the next target tracking point and repeating the operations of steps (4)-(6).

2. The method of claim 1, wherein, In the step (1), the dynamic model of the underwater robot is established by the following steps: (a) assuming that the position of the underwater robot in the carrier coordinate system is [x, y, z], the velocity is [u, v, w], the heading is ψ, and the heading angular velocity is r; (b) establishing the dynamic model of the underwater robot as shown in the following formula: where m is the mass, I z is the moment of inertia, is the added mass coefficient, X u , Y v , Z w , N r is the linear damping coefficient, X |u|u , Y v|v , Z |w|w , N |r|r is the nonlinear damping coefficient, which are the intrinsic properties of the underwater robot; W and B are the gravity and buoyancy forces acting on the underwater robot, respectively; d x , d y , d z are the disturbance forces acting on each degree of freedom in the body-fixed frame, d r is the disturbance torque; τ x , τ y , τ z are the control forces acting on each degree of freedom, τ r is the control torque; a dot above a variable or symbol indicates the derivative of the variable or symbol with respect to time.

3. The method of claim 1, wherein, The four-degree-of-freedom extended state observers in the step (2) correspond to the four degrees of freedom in the dynamic model, specifically the forward motion degree of freedom, the lateral motion degree of freedom, the vertical motion degree of freedom and the turning motion degree of freedom of the underwater robot, wherein: (a) the extended state observer O1 corresponding to the forward motion degree of freedom has the following state space equation: wherein is an estimate of x, is an estimate of u, with the superscript T denoting the transpose symbol; an estimate of the disturbance quantity ; Θ1 is a parameter of the expansion state observer Ol; The input of the extended state observer O1 is u1 = [τ x , x] T , and the output is h1; (b) the extended state observer O2 corresponding to the lateral motion degree of freedom has the following state space equation: wherein is an estimate of y, is an estimate of v; an estimate of the disturbance quantity ; θ2 is a parameter of the extended state observer O2; the input of the extended state observer O2 is u2 = [τ y ,y] T , and the output is h2; (c) the extended state observer O3 corresponding to the vertical motion degree of freedom has the following state space equation: wherein is an estimate of z, is an estimate of w; an estimate of the disturbance quantity ; θ3 is a parameter of the extended state observer O3; the input of the extended state observer O3 is u3 = [τ z ,z] T , and the output is h3; (d) the extended state observer O4 corresponding to the turning motion degree of freedom has the following state space equation: wherein is an estimate of ψ, is an estimate of r; an estimate of the disturbance quantity ; θ4 is a parameter of the expansion state observer O4; The input of the extended state observer O4 is u4 = [τ r , ψ] T , and the output is h4.

4. The method of claim 1, wherein, In the step (3), the target tracking sequence in the carrier coordinate system is obtained by the following steps: (a) the target trajectory in the inertial coordinate system is set as First, the expected heading ψ of the underwater robot is calculated by the following equation d (t): (b) Then the expected velocity of the underwater robot is calculated as follows and the expected angular velocity of the heading r(t): (c) set the sampling period as T, and discretize the target trajectory to obtain a target tracking sequence The specific process is as follows: (d) As above, discretize the expected heading and expected heading rate to obtain a sequence of target headings ψ d [k] and a sequence of target heading rates 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) The target position sequence x in the carrier coordinate system is calculated according to the following formula d [k], y d [k], z d [k]: (f) The target velocity sequence u in the vehicle coordinate system is calculated as follows d [k], v d [k], w d [k]:

5. The method of claim 1, wherein, In the step (4), the position, heading, velocity and angular velocity of the underwater robot in the carrier coordinate system at the current time, and the estimated values of these physical quantities by the extended state observers of each degree of freedom are obtained by the following steps: (a) obtaining the current position data x, y, z by using the positioning sensor carried by the underwater robot, the current velocity data u, v, w by using the velocity sensor, the current heading angle data ψ by using the compass data, and the current heading angular velocity data r by using the angular velocity sensor; (b) using the output of the extended state observer O1 to obtain an estimate of x and an estimate of u using the output of the extended state observer O2 to obtain an estimate of y and an estimate of v using the output of the extended state observer O3 to obtain an estimate of z and an estimate of w using the output of the extended state observer O4 to obtain an estimate of ψ and an estimate of r 6. The method of claim 1, wherein, In the step (5), the estimated error is calculated in the following manner, and the real-time parameter values of the extended state observers of each degree of freedom are obtained by using the adaptive function: (a) first calculating the error of the estimated quantities obtained by the output results of the extended state observers of each degree of freedom: (b) the parameter θ1 of the extended state observer O1 is calculated according to the following formula: θ1= θ 1l + (θ 1u - θ 1l ) tanh(c1|ε x + ε u |) (5.9) where θ 1l and θ 1u are upper and lower bound parameters, respectively, and satisfy 1 < θ 1l < θ 1u ; and c1 > 0 is a control parameter. (c) the parameter θ2 of the extended state observer O2 is calculated according to the following formula: θ2= θ 2l + (θ 2u - θ 2l ) tanh(c2|ε y + ε v |) (5.10) where θ 2l and θ 2u are upper and lower bound parameters, respectively, and satisfy 1 < θ 2l < θ 2u ; c2 > 0 is a control parameter; (d) the parameter θ3 of the extended state observer O3 is calculated according to the following formula: θ3= θ 3l + (θ 3u - θ 3l ) tanh(c3|ε z + ε w |) (5.11) where θ 3l and θ 3u are upper and lower bound parameters, respectively, and satisfy 1 < θ 3l < θ 3u ; c3 > 0 is a control parameter; (e) the parameter θ4 of the extended state observer O4 is calculated according to the following formula: θ4= θ 4l + (θ 4u - θ 4l ) tanh(c4|ε ψ + ε r |) (5.12) where θ 4l and θ 4u are upper and lower bound parameters, respectively, and satisfy 1 < θ 4l < θ 4u ; c4 > 0 is a control parameter; The parameters and control parameters are set according to the inertia properties of the underwater robot and the actual control effect.

7. The method of claim 1, wherein, The step (6) specifically comprises: (a) obtaining the target position x d , y d , z d , target velocity u d , v d , w d and target heading angle ψ d and target heading angle rate r d at the current time from the tracking sequence calculated in step (3) (b) obtaining an estimate of the disturbance f1 using the output of the extended state observer O1 obtaining an estimate of the disturbance f2 using the output of the extended state observer O2 obtaining an estimate of the disturbance f3 using the output of the extended state observer O3 obtaining an estimate of the disturbance f4 using the output of the extended state observer O4 (c) Set the input u1 = [τ] of the extended state observer O1 at the current time. x ,x] T The state and output are updated using its state-space equation (2.1); the input u2 = [τ] of the extended state observer O2 at the current time is set. y ,y] T The state and output are updated using its state-space equation (2.2); the input u3 = [τ] of the extended state observer O3 at the current time is set. z ,z] T The state and output are updated using its state-space equation (2.3); the input u4 = [τ] of the extended state observer O4 at the current time is set. r ,ψ] T The state and output are updated using its state-space equation (2.4); (d) applying control quantities τ x , τ y , τ z , and τ r to the effectors, thereby adjusting the operational trajectory of the underwater vehicle in accordance with the control law.

8. A computer device, comprising: comprises: At least one processor, and a memory connected in communication with 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 to enable the at least one processor to perform the underwater robot variable parameter active disturbance trajectory tracking control method in 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 perform the underwater robot variable parameter active disturbance trajectory tracking control method in any one of claims 1 to 7.

Images