Underwater robot trajectory tracking method and device, electronic equipment and medium

Abstract

The embodiment of the invention provides an underwater robot trajectory tracking method and device, electronic equipment and a medium, and the method comprises the steps: building a dynamics and kinematics model related to an underwater robot, and enabling the dynamics and kinematics model to comprise parameter uncertainty and propeller output uncertainty; according to the dynamics and kinematics models, determining a disturbance observer which realizes fixed-time convergence of observation errors and is used for compensating external disturbance; according to the disturbance observer, determining a super-spiral sliding mode control law which is converged at fixed time and is used for trajectory tracking control; and performing trajectory tracking of the underwater robot based on the super-spiral sliding mode control law. According to the embodiment of the invention, a controller and a state observer are designed, so that the underwater robot can realize rapid, robust and accurate trajectory tracking under the conditions of modeling uncertainty, ocean current disturbance and propeller faults.

Description

Technical Field

[0001] This document relates to the field of underwater robot control technology, and in particular to an underwater robot trajectory tracking method, device, electronic equipment and medium. Background Art

[0002] In today's era, people's exploration of the ocean and demand for resources have reached an unprecedented level. The in-depth exploration and utilization of marine resources has become a common goal of countries around the world. Autonomous Underwater Vehicles (AUVs) play an increasingly critical role in marine activities due to their flexibility and versatility.

[0003] In order to perform tasks in complex and changing marine environments, AUVs need to have efficient perception and positioning capabilities, accurate planning and decision-making mechanisms, and robust control strategies. Among them, motion control, as the basis for executing various decisions, is the core of AUV performance. Trajectory tracking is a core issue in AUV control research. It requires the robot's control system to be able to design appropriate control laws and drive actuators based on the dynamic trajectory provided by the planning system to ensure that the AUV can accurately and timely track the trajectory.

[0004] In the face of modeling uncertainty, unknown currents, waves and other disturbances, and thruster failures, the AUV's motion control system must have excellent anti-interference capabilities and fast response speeds, while also improving tracking accuracy, accelerating error convergence, and enhancing system robustness. The convergence of existing finite-time sliding modes depends on the initial state, and sliding mode control is prone to chattering in practical applications. Summary of the invention

[0005] The purpose of the present invention is to provide an underwater robot trajectory tracking method, device, electronic equipment and medium, aiming to solve the above-mentioned problems in the prior art.

[0006] The present invention provides a method for tracking a trajectory of an underwater robot, comprising:

[0007] Establishing a dynamics and kinematics model of the underwater robot, wherein the dynamics and kinematics model includes parameter uncertainty and thruster output uncertainty;

[0008] A disturbance observer for compensating external disturbances that achieves fixed-time convergence of observation errors according to the dynamic and kinematic models;

[0009] Determining a super-helical sliding mode control law for trajectory tracking control with fixed-time convergence according to the disturbance observer;

[0010] The underwater robot trajectory tracking is performed based on the super-helical sliding mode control law.

[0011] The present invention provides an underwater robot trajectory tracking device, comprising:

[0012] An establishment module is used to establish a dynamics and kinematics model of the underwater robot, wherein the dynamics and kinematics model includes parameter uncertainty and thruster output uncertainty;

[0013] A disturbance observer module, for determining an observation error according to the dynamic and kinematic models to achieve a disturbance observer for compensating external disturbances with fixed time convergence;

[0014] A super-helical sliding mode control law module, used for determining a super-helical sliding mode control law for trajectory tracking control that converges in a fixed time according to the disturbance observer;

[0015] A tracking module is used to track the trajectory of the underwater robot based on the super-helical sliding mode control law.

[0016] An embodiment of the present invention further provides an electronic device, comprising: a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the computer program implements the steps of the underwater robot trajectory tracking method when executed by the processor.

[0017] An embodiment of the present invention further provides a computer-readable storage medium, on which a program for implementing information transmission is stored, and when the program is executed by a processor, the steps of the above-mentioned underwater robot trajectory tracking method are implemented.

[0018] By adopting the embodiment of the present invention, a disturbance observer with fixed-time convergence characteristics is proposed for factors such as ocean current disturbances, modeling uncertainty and thruster failure, and the observer can observe speed and disturbance. On the basis of the observer, a super-helical sliding mode controller is further proposed. The traditional super-helical sliding mode controller converges in finite time, and the convergence ability depends on the initial state of the underwater robot. The convergence speed, accuracy and robustness are not as good as fixed-time convergence. The super-helical sliding mode control law of the present invention has a fixed-time convergence characteristic, and the convergence does not depend on the initial state. It can also further improve the rapid response of the underwater robot and the robustness to external interference and internal parameter changes, and enhance the suppression of thruster output jitter. This scheme can realize fast and robust trajectory tracking of underwater robots. BRIEF DESCRIPTION OF THE DRAWINGS

[0019] In order to more clearly illustrate one or more embodiments of this specification or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments recorded in this specification. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative labor.

[0020] Figure 1 is a flow chart of an underwater robot trajectory tracking method according to an embodiment of the present invention;

[0021] Figure 2 is a schematic diagram of the arrangement of the underwater robot thruster according to an embodiment of the present invention;

[0022] Figure 3 is a schematic diagram of the trajectory tracking result of an underwater robot according to an embodiment of the present invention;

[0023] Figure 4 is a diagram of position changes of each channel of the lower robot trajectory tracking according to an embodiment of the present invention;

[0024] Figure 5 is a position error diagram of the underwater robot trajectory tracking according to an embodiment of the present invention;

[0025] Figure 6 It is an output diagram of each thruster of the underwater robot trajectory tracking according to an embodiment of the present invention;

[0026] Figure 7 is a schematic diagram of an underwater robot trajectory tracking device according to an embodiment of the present invention;

[0027] Figure 8 is a schematic diagram of an electronic device according to an embodiment of the present invention. DETAILED DESCRIPTION

[0028] In order to enable those skilled in the art to better understand the technical solutions in one or more embodiments of this specification, the following will be combined with the drawings in one or more embodiments of this specification to clearly and completely describe the technical solutions in one or more embodiments of this specification. Obviously, the described embodiments are only part of the embodiments of this specification, not all of the embodiments. Based on one or more embodiments of this specification, all other embodiments obtained by ordinary technicians in this field without creative work should fall within the scope of protection of this document.

[0029] Method Embodiment

[0030] According to an embodiment of the present invention, a method for tracking a trajectory of an underwater robot is provided. Figure 1 FIG. 1 is a flow chart of a method for tracking a trajectory of an underwater robot according to an embodiment of the present invention. Figure 1As shown, the underwater robot trajectory tracking method according to an embodiment of the present invention specifically includes:

[0031] Step S101, establishing a dynamics and kinematics model of the underwater robot, wherein the dynamics and kinematics model contains parameter uncertainty and propeller output uncertainty; specifically including:

[0032] Based on the Fossen outline six-degree-of-freedom nonlinear dynamic model shown in Formula 1 and Formula 2, considering the inertia matrix shown in Formula 3 and its uncertainty, the Coriolis centripetal force matrix shown in Formula 4 and its uncertainty, the fluid damping matrix shown in Formula 5 and its uncertainty, and the restoring force matrix shown in Formula 6 and its uncertainty, the dynamic and kinematic models are expressed as shown in Formula 7 and Formula 8:

[0033]

[0034]

[0035]

[0036]

[0037]

[0038]

[0039]

[0040]

[0041] in, represents the derivative of the AUV position and attitude vector, η represents the AUV position and attitude vector, J represents the conversion matrix between the inertial coordinate system and the body coordinate system, v represents the derivative of the AUV linear velocity and angular velocity vector, represents the linear velocity and angular velocity vector of the AUV, represents the nominal inertia matrix, ΔM represents the parameter uncertainty of the inertia matrix, represents the nominal Coriolis centripetal force matrix, ΔC(v) represents the parameter uncertainty of the Coriolis centripetal force matrix, represents the nominal fluid damping matrix, ΔD(v) represents the parameter uncertainty of the fluid damping matrix, represents the nominal restoring force matrix, Δg(η) represents the parameter uncertainty of the nominal restoring force matrix, τ=Bu is the force generated by the AUV actuator, is the thrust conversion matrix of the propeller, ΔB is the uncertainty of the matrix, which is used to characterize the fault. represents the nominal thruster thrust conversion matrix, τd is the external disturbance, The model uncertainties of the corresponding matrices are ΔΜ η , ΔC η , ΔD η , Δg η , F can be regarded as a collection of various uncertainties, Represents the second-order derivative of the AUV position and attitude vector.

[0042] Step S102, determining a disturbance observer for compensating external disturbances with fixed-time convergence of observation error according to the dynamic and kinematic models; specifically comprising:

[0043] Define z1(t) and For v and M -1 τ d The estimation error is defined as e1(t) = v-z1, e2(t) = M -1 τ d -z2, then the disturbance observer is expressed as formula 9:

[0044]

[0045] Among them, k1>0, k2>0, 0 <p1<1,q1> 1 is the observer parameter, select appropriate parameters, and perturb τ d It is observed at a fixed time, z1(t) represents the observer's estimate of the AUV's linear velocity and angular velocity vector, z2(t) represents the estimate of the disturbance force in the inertial system, v represents the AUV's linear velocity and angular velocity vector, γ represents a constant greater than 0, sign represents the sign function, z1 represents z1(t) at that moment, M -1 represents the inverse of the inertia matrix, e1(t) represents the velocity observation error, e2(t) represents the disturbance observation error, z2 represents z2(t) at that moment, represents the derivative of z1 with respect to time, Cv represents the Coriolis centripetal force matrix multiplied by velocity, Dv represents the fluid damping force multiplied by velocity, g represents the restoring force matrix, express sign(e1), express sign(e2), represents the time derivative of z2, and sign(e1) represents the sign of the velocity observation error.

[0046] Step S103, determining a super-helical sliding mode control law for trajectory tracking control with fixed-time convergence according to the disturbance observer; specifically comprising:

[0047] The trajectory tracking error is defined as η e =η-η d, η d and is the expected trajectory in the inertial coordinate system given for the AUV tracking task, the sliding mode variable s is a designable item, and the super-helical sliding mode scheme with fixed time convergence satisfies Formula 10:

[0048]

[0049] Among them, λ1>0, λ2>0, α>0, p>1 are controller parameters; represents the time derivative of the sliding mode variable s, sign(s) represents the sign of the sliding mode variable s, represents the time derivative of ν, ν is Intermediate variables during design;

[0050] Assumptions Then the control law of the super-helical sliding mode controller is as shown in formula 11:

[0051]

[0052] Among them, the controller parameters λ1, λ2, α, p>1, c are positive constants, z2 is the disturbance observer, ij d represents the second-order derivative of the desired trajectory in the inertial coordinate system, t represents time, represents the derivative of the coordinate transformation matrix, v represents the AUV velocity vector, C η Denotes the Coriolis force matrix represented by the pose vector after coordinate transformation, D η represents the fluid damping matrix represented by the pose vector after coordinate transformation, g η represents the restoring force vector, and η represents the AUV position and attitude vector.

[0053] Step S104, performing underwater robot trajectory tracking based on the super-helical sliding mode control law.

[0054] The above technical solution solves the problem that the existing AUV trajectory tracking control method considers fewer uncertain factors, the convergence characteristics depend on the initial state of the robot, and the jitter problem of the sliding mode control, and then proposes a super-helical sliding mode control scheme with fixed-time convergence, in which the disturbance observer with fixed-time convergence can also observe the speed, and the super-helical sliding mode controller with fixed-time convergence can resist jitter.

[0055] In order to make the above-mentioned purposes, features and advantages of the embodiments of the present invention more obvious and easy to understand, the specific implementation of the present invention is described in detail below in conjunction with the accompanying drawings. Although certain embodiments of the present invention are shown in the accompanying drawings, the present invention can be implemented in various forms on different types of underwater robot systems, and therefore should not be interpreted as being limited to the embodiments described here. The embodiments provided are intended to explain the application process of the present invention. The drawings and embodiments of the present invention are only for exemplary purposes and are not intended to limit the scope of protection of the present invention.

[0056] An underwater robot trajectory tracking method provided by an embodiment of the present invention, wherein the underwater robot propeller is arranged as follows Figure 2 As shown, it includes: step S1, establishing a dynamic and kinematic model for underwater robot control, wherein the model includes parameter uncertainty and propeller output uncertainty. The dynamics of the underwater robot should take into account factors such as fluid resistance, buoyancy, inertia, etc. in the underwater environment, and select the coordinates, velocity, acceleration, etc. of the underwater robot as state variables. Optionally, various external interference forces that may be encountered by the underwater robot during the execution of the task, such as flow, wave, surge, etc., are considered, and the actuator failure of the underwater robot is considered and incorporated into the mathematical model.

[0057] Step S2, design a disturbance observer with fixed-time convergence. The disturbance observer is used to compensate for external disturbances, and selecting appropriate disturbance observer parameters can enable the observation error to achieve fixed-time convergence. The observed external disturbance needs to satisfy the Lipschitz condition, and the first-order derivative of the disturbance is bounded. It should be clear that in order not to limit the scope of protection of the present invention, S2 is not a mandatory step. Step S2 is to better compensate for the impact of external disturbances on the controller and speed up the convergence of the tracking error. In addition, in actual work, the speed of some underwater robots is usually difficult to obtain accurately due to factors such as sensors, and the designed observer can observe speed information.

[0058] Step S3, design a fixed-time convergence super-helical sliding mode controller to achieve trajectory tracking control of the underwater robot. According to the dynamic model and trajectory tracking error, the designed sliding mode variable s must satisfy the formula:

[0059]

[0060] Among them, λ1>0, λ2>0, α>0, p>1 are controller parameters.

[0061] The whole process from step S1 to S3 is described in detail below, and the stability analysis and simulation results are also given.

[0062] First, the parameters of this embodiment are described. Based on the inertial coordinate system o-xyz, the position and attitude vector of the AUV are expressed as Where x, y, z∈R represent the position coordinates, θ, ψ∈R represent the attitude, i.e., roll, pitch and pitch. Based on the body coordinate system O1-X1Y1Z1, the linear velocity and angular velocity vector v=[u,ν,w,p,q,r] of the spacecraft are represented. T , where u, ν, w are the linear velocities of surge, sway and heave respectively, and p, q, r represent the angular velocities of rotation around its three axes in the body coordinate system, namely the heel angular velocity, pitch angular velocity, and rotation (or yaw) angular velocity.

[0063] Embodiment step S1:

[0064]

[0065] The transformation matrix of the two coordinate systems J(η)∈R 6×6 is defined as:

[0066]

[0067]

[0068]

[0069] J2(η) will have a singular problem when the pitch θ=±π / 2rad, which does not occur in this embodiment. According to Fossen's derivation:

[0070]

[0071] in, is an inertia matrix that includes the extra mass, is the nominal inertia matrix, and ΔM is the unmodeled characteristic, i.e., the error between the model and the AUV model in the real world. It consists of two parts, namely the inertial mass matrix M RB and the additional mass matrix M A :

[0072]

[0073]

[0074] where x G ,y G , z G are the distances between the center of gravity and the origin on the x, y, and z axes respectively.

[0075] The Coriolis centripetal force matrix of the AUV, C(v)∈R 6×6 , Similarly, ΔC(v) is the error between the model and the AUV model in the real world, and the nominal Coriolis centripetal force matrix is It consists of two parts, Among them C A It is expressed as:

[0076]

[0077] in:

[0078]

[0079] Coriolis centripetal force matrix C RB It can be expressed as:

[0080]

[0081] in:

[0082]

[0083]

[0084]

[0085] Fluid damping matrix D(v)∈R 6×6 , ΔD(v) is the unmodeled uncertainty in the damping matrix. The nominal fluid damping matrix D(v) is composed of the linear damping term D l and the nonlinear damping term D n (v) the composition, Where D l and D n (v) are respectively expressed as:

[0086] D l =diag(X u ,Y v ,Z w ,U p ,V q ,W r )

[0087] D n (v) = diag(X u|u| |u|,Y v|v| |v|,Z w|w| |w|,U p|p| |p|,V q|q| |q|,W r|r| |r|)

[0088] The restoring force vector is the combined effect of gravity W and buoyancy B. Similarly, Δg(η) is the error between the unmodeled dynamics, i.e., the center of buoyancy coordinates and the actual x B ,y B , zB is the buoyancy center coordinate of the AUV, the nominal restoring force vector It is expressed as:

[0089]

[0090] The force generated by the AUV actuator τ = [τ u ,τ ν ,τ w ,τ p ,τ q ,τ r ] T , τ=Bu, u is the thrust output vector of the propeller, B is the thrust conversion matrix, and ΔB is the uncertainty of this matrix. In addition, considering external ocean currents, surges, internal waves and other disturbances, these disturbance forces are defined as τ d , τ d =[d u ,d v ,d w ,d p ,d q ,d r ] T .

[0091] The AUV is a spherical eight-thruster overdriven AUV. The specific physical parameters are as follows: The rotational inertia of the main axis is Among them, the radius of AUV is r0=0.31m, the mass of AUV is m0=125kg, zG=0.05m, ρ=1000kg / m3, the average density of AUV is ρv=965kg / m3, g=9.81m / s2, the secondary damping coefficient of translation is dt1=148N(s / m2), the linear damping coefficient of translation is dt2=100N(s / m2), the secondary damping coefficient of rotation is d1=280Ns2 / m, the linear damping coefficient of rotation is d2=230Ns2 / m. a=sin(π / 4), the distance from the center of ODIN to the vertical thrust is L=0.381m, and the radial distance from the navigation center to the horizontal thrust center is Lz=0.508m. After substituting into each matrix, we have:

[0092]

[0093]

[0094]

[0095]

[0096]

[0097] Step S2:

[0098] Design the disturbance observer. According to the assumption of disturbance, the external disturbance satisfies the Lipschitz condition ||M -1 τ d (t)||≤δ(t-t0), and the first-order derivative of the disturbance is bounded, satisfying ||D d (t)||≤δ, δ>0 is a positive constant. In the actual operation of AUV, its speed is usually difficult to obtain. Define z1(t) and For v and M -1 τ d The estimation error is defined as e1(t) = v-z1, e2(t) = M -1 τ d -z2. The fixed-time disturbance observer can be expressed as:

[0099]

[0100] Among them, k1>0, k2>0, 0 <p1<1,q1> 1 Select appropriate parameters for the observer parameters, and the disturbance τ d When observed at a fixed time, the observation error e2 will converge to 0 at a fixed time. The upper bound of the convergence time is T1:

[0101]

[0102] Among them, γ, δ, δ1 are given constants, 0 <m c <1 is a constant, k1, k2, q1, q2 are observer design parameters.

[0103] The stability of the observer is proved as follows. First, according to the definition, we can derive e1(t) and e2(t):

[0104]

[0105] e 1,i >0, which means that for t>0, in e 1,i Before the sign of the 1,i (t)) maintains the same 2,i (t)) is the opposite. In this case, we have:

[0106]

[0107] Where e1 = [e 1,1 ,e 1,2 ,e 1,3 ,e 1,4 ,e 1,5 ,e 1,6 ] T , e2=[e 2,1 ,e 2,2,e 2,3 ,e 2,4 ,e 2,5 ,e 2,6 ] T , i=1,2,...,6. The following three steps will prove the convergence of e2. The first step is to consider ||e1(0)||>δ1, where δ1 is a given constant:

[0108]

[0109]

[0110] If for i = 1, 2, ..., 6, e 1,i Does not cross the zero axis (i.e. e 1,i is not 0), then It is especially pointed out that in the formula, must be non-positive; moreover, because And k1>0, so we can immediately infer the formula:

[0111]

[0112] therefore, The second term in the last line of the equation must be negative, which leads to the inequality:

[0113]

[0114] Since q1>1, we can get the relationship:

[0115]

[0116] So further:

[0117]

[0118] According to the above inequality, when the value of ||e1(t)|| decreases and reaches ||e1(T 11 )||=δ1, we have:

[0119]

[0120] The first step is in ||e1(T 11 )||=δ1>0 ends, if ||e1(T 11 )||<δ1, the first step will not be executed.

[0121] The second step is to 11 , ||e1(t)|| continues to decrease to 0, that is, ||e1(T 12 )||=0. Note And k2 > 0, the following inequality can also be obtained:

[0122]

[0123] Since 0 < p1 < 1, rewrite the above formula as:

[0124]

[0125] So there is:

[0126]

[0127] Since ||e1|| continuously decreases to ||e1(T 12 )|| = 0, the following can be obtained:

[0128]

[0129] The second step ends when ||e1(T 12 )|| = 0, and ||e2(T 12 )|| can be calculated to satisfy the formula:

[0130] In the third step, when t > T 12 , the convergence time of e2 should satisfy the formula:

[0131]

[0132] Among them, 0 < m c < 1 is a constant, Q.E.D.

[0133] Step S3:

[0134] Consider the dynamic equation:

[0135]

[0136] Similarly, define the trajectory tracking error as η e = η - η d , η d is the desired trajectory in the inertial coordinate system given by the AUV tracking task. B is the thrust allocation matrix, u is the thruster output vector, τ d is the hydrodynamic disturbance force, Μ η , C η , D η , g η are the dynamic matrices corresponding to the AUV.

[0137] For the super-twisting scheme with fixed-time convergence, the sliding mode variable s should satisfy the formula:

[0138]

[0139] Where λ1>0, λ2>0, α>0, p>1 are controller parameters, and c is a positive constant

[0140] Design a simple sliding surface:

[0141]

[0142] The derivative of the sliding surface is:

[0143]

[0144] Substituting the requirements of the sliding mode variables and the dynamic model, the control law is obtained:

[0145]

[0146] Among them, the controller parameters λ1, λ2, α, p>1, c are positive constants, and z2 is the disturbance observer designed in step S2.

[0147] Prove the stability of the controller and give the Lyapunov function:

[0148]

[0149] Substituting into the control law and taking the derivative we get:

[0150]

[0151] Since the observer converges in fixed time T1, that is, z2 = Μ η -1 τ d -e2, e2=0,(t≥T1), the above formula can be rewritten as:

[0152]

[0153] According to Basin's derivation, the above formula meets the fixed time convergence requirement, and the convergence time satisfies:

[0154] T3≤T1+T2

[0155] Where T1 is the upper bound of the convergence time of the fixed time observer, see step S2, and T2 is as follows:

[0156]

[0157] Among them, the controller parameters λ1, λ2, α, p>1, c are positive constants, ε>0, M=α+L, m=α-L, h(λ1)=1 / λ1+(2e / mλ1) 1 / 3 , e is the base of the natural logarithm. L is the upper bound of the external disturbance, and the control gain satisfies:

[0158] α>L

[0159] λ1h -1 (λ1)>M

[0160] And the minimum value of T2(ε) is time to achieve.

[0161] Example simulation settings and results:

[0162] like Figure 3-Figure 6 As shown, the disturbance observer parameters are set to k1=0.5, k2=0.5, p1=0.8, q1=2, γ=0.05, and the controller parameters are set to λ1=3, λ2=0.8, p=1.5, c=2, α=1.5.

[0163] The simulation takes into account model uncertainty and thruster failure. When considering the uncertainty of the AUV's dynamic model, the original model parameters are multiplied by a coefficient, assuming that there is a 30% uncertainty in the dynamic model. That is, the nominal model matrices calculated by the controller are multiplied by 0.7. g η =0.7g η In the case of thruster failure, the failure of a thruster is divided into two failure cases, partial failure and complete failure. In the case of partial failure, the thruster can still work partially, but cannot exert the maximum power and reach the maximum thrust. In the case of complete failure, the thruster cannot rotate and the thrust is 0.

[0164] Thruster failure conditions are:

[0165]

[0166] Simulated external disturbance τ d =[d u ,d v ,d w ,d p ,d q ,d r ] T :.

[0167]

[0168] d p =0.1d u d q =0.1d v d r =0.1d w

[0169] In summary, with the help of the technical solution of the embodiment of the present invention, a disturbance observer with fixed-time convergence characteristics is proposed for the influencing factors such as ocean current disturbances, modeling uncertainty and thruster failure. The observer can observe speed and disturbance. On the basis of this observer, a super-helical sliding mode controller is further proposed. The traditional super-helical sliding mode controller converges in finite time, and the convergence ability depends on the initial state of the underwater robot. The convergence speed, accuracy and robustness are not as good as fixed-time convergence. The super-helical sliding mode control law of the present invention has a fixed-time convergence characteristic, and the convergence does not depend on the initial state. It can also further improve the rapid response of the underwater robot and the robustness to external interference and internal parameter changes, and enhance the suppression of thruster output jitter. This scheme can realize fast and robust trajectory tracking of underwater robots.

[0170] Device Example 1

[0171] According to an embodiment of the present invention, a trajectory tracking device for an underwater robot is provided. Figure 7 Schematic diagram of an underwater robot trajectory tracking device according to an embodiment of the present invention. Figure 7 As shown, the underwater robot trajectory tracking device according to an embodiment of the present invention specifically includes:

[0172] The establishment module 70 is used to establish a dynamic and kinematic model of the underwater robot, wherein the dynamic and kinematic model contains parameter uncertainty and propeller output uncertainty; the establishment module 70 is specifically used to:

[0173] Based on the Fossen outline six-degree-of-freedom nonlinear dynamic model shown in Formula 1 and Formula 2, considering the inertia matrix shown in Formula 3 and its uncertainty, the Coriolis centripetal force matrix shown in Formula 4 and its uncertainty, the fluid damping matrix shown in Formula 5 and its uncertainty, and the restoring force matrix shown in Formula 6 and its uncertainty, the dynamic and kinematic models are expressed as shown in Formula 7 and Formula 8:

[0174]

[0175]

[0176]

[0177]

[0178]

[0179]

[0180]

[0181]

[0182] in, represents the derivative of the AUV position and attitude vector, η represents the AUV position and attitude vector, J represents the conversion matrix between the inertial coordinate system and the body coordinate system, v represents the derivative of the AUV linear velocity and angular velocity vector, represents the linear velocity and angular velocity vector of the AUV, represents the nominal inertia matrix, ΔM represents the parameter uncertainty of the inertia matrix, represents the nominal Coriolis centripetal force matrix, ΔC(v) represents the parameter uncertainty of the Coriolis centripetal force matrix, represents the nominal fluid damping matrix, ΔD(v) represents the parameter uncertainty of the fluid damping matrix, represents the nominal restoring force matrix, Δg(η) represents the parameter uncertainty of the nominal restoring force matrix, τ=Bu is the force generated by the AUV actuator, is the thrust conversion matrix of the propeller, ΔB is the uncertainty of the matrix, which is used to characterize the fault. represents the nominal thruster thrust conversion matrix, τ d is the external disturbance, The model uncertainties of the corresponding matrices are ΔΜ η , ΔC η , ΔD η , Δg η , F can be regarded as a collection of various uncertainties, Represents the second-order derivative of the AUV position and attitude vector.

[0183] The disturbance observer module 72 is used to determine the disturbance observer for compensating external disturbances according to the dynamic and kinematic models to achieve fixed-time convergence of the observation error; specifically, to:

[0184] Define z1(t) and For v and M -1 τ d The estimation error is defined as e1(t) = v-z1, e2(t) = M -1 τ d -z2, then the disturbance observer is expressed as formula 9:

[0185]

[0186] Among them, k1>0, k2>0, 0 <p1<1,q1> 1 is the observer parameter, select appropriate parameters, and perturb τ dIt is observed at a fixed time, z1(t) represents the observer's estimate of the AUV's linear velocity and angular velocity vector, z2(t) represents the estimate of the disturbance force in the inertial system, v represents the AUV's linear velocity and angular velocity vector, γ represents a constant greater than 0, sign represents the sign function, z1 represents z1(t) at that moment, M -1 represents the inverse of the inertia matrix, e1(t) represents the velocity observation error, e2(t) represents the disturbance observation error, z2 represents z2(t) at that moment, represents the derivative of z1 with respect to time, Cv represents the Coriolis centripetal force matrix multiplied by velocity, Dv represents the fluid damping force multiplied by velocity, g represents the restoring force matrix, express express represents the time derivative of z2, and sing(e1) represents the sign of the velocity observation error.

[0187] The super-helical sliding mode control law module 74 is used to determine a super-helical sliding mode control law for trajectory tracking control with fixed-time convergence according to the disturbance observer; specifically, it is used to:

[0188] The trajectory tracking error is defined as η e =η-η d , η d and is the expected trajectory in the inertial coordinate system given for the AUV tracking task, the sliding mode variable s is a designable item, and the super-helical sliding mode scheme with fixed time convergence satisfies Formula 10:

[0189]

[0190] Among them, λ1>0, λ2>0, α>0, p>1 are controller parameters; represents the time derivative of the sliding mode variable s, sign(s) represents the sign of the sliding mode variable s, represents the time derivative of ν, ν is Intermediate variables during design;

[0191] Assumptions Then the control law of the super-helical sliding mode controller is as shown in formula 11:

[0192]

[0193] Among them, the controller parameters λ1, λ2, α, p>1, c are positive constants, z2 is the disturbance observer, ij d represents the second-order derivative of the desired trajectory in the inertial coordinate system, t represents time, represents the derivative of the coordinate transformation matrix, v represents the AUV velocity vector, C η Denotes the Coriolis force matrix represented by the pose vector after coordinate transformation, D η represents the fluid damping matrix represented by the pose vector after coordinate transformation, g η represents the restoring force vector, and η represents the AUV position and attitude vector.

[0194] The tracking module 76 is used to track the trajectory of the underwater robot based on the super-helical sliding mode control law.

[0195] The embodiment of the present invention is a device embodiment corresponding to the above method embodiment. The specific operations of each module can be understood by referring to the description of the method embodiment, which will not be repeated here.

[0196] Device Example 2

[0197] An embodiment of the present invention provides an electronic device, such as Figure 8 As shown, it includes: a memory 80, a processor 82, and a computer program stored in the memory 80 and executable on the processor 82. When the computer program is executed by the processor 82, the steps described in the method embodiment are implemented.

[0198] Device Example 3

[0199] An embodiment of the present invention provides a computer-readable storage medium, on which a program for implementing information transmission is stored. When the program is executed by the processor 82, the steps described in the method embodiment are implemented.

[0200] The computer-readable storage medium in this embodiment includes, but is not limited to, ROM, RAM, magnetic disk or optical disk, etc.

[0201] 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 method for tracking underwater robot trajectory, characterized in that: include: Establishing a dynamics and kinematics model of the underwater robot, wherein the dynamics and kinematics model includes parameter uncertainty and thruster output uncertainty; A disturbance observer for compensating external disturbances that achieves fixed-time convergence of observation errors according to the dynamic and kinematic models; Determining a super-helical sliding mode control law for trajectory tracking control with fixed-time convergence according to the disturbance observer; The underwater robot trajectory tracking is performed based on the super-helical sliding mode control law.

2. The method according to claim 1, characterized in that The establishment of the dynamics and kinematics model of the underwater robot specifically includes: Based on the Fossen outline six-degree-of-freedom nonlinear dynamic model shown in Formula 1 and Formula 2, considering the inertia matrix shown in Formula 3 and its uncertainty, the Coriolis centripetal force matrix shown in Formula 4 and its uncertainty, the fluid damping matrix shown in Formula 5 and its uncertainty, and the restoring force matrix shown in Formula 6 and its uncertainty, the dynamic and kinematic models are expressed as shown in Formula 7 and Formula 8: in, represents the derivative of the AUV position and attitude vector, η represents the AUV position and attitude vector, J represents the conversion matrix between the inertial coordinate system and the body coordinate system, v represents the derivative of the AUV linear velocity and angular velocity vector, represents the linear velocity and angular velocity vector of the AUV, represents the nominal inertia matrix, ΔM represents the parameter uncertainty of the inertia matrix, represents the nominal Coriolis centripetal force matrix, ΔC(v) represents the parameter uncertainty of the Coriolis centripetal force matrix, represents the nominal fluid damping matrix, ΔD(v) represents the parameter uncertainty of the fluid damping matrix, represents the nominal restoring force matrix, Δg(η) represents the parameter uncertainty of the nominal restoring force matrix, τ=Bu is the force generated by the AUV actuator, is the thrust conversion matrix of the propeller, ΔB is the uncertainty of the matrix, which is used to characterize the fault. represents the nominal thruster thrust conversion matrix, τ d is the external disturbance, The model uncertainties of the corresponding matrices are ΔΜ η , ΔC η , ΔD η , Δg η , F can be regarded as a collection of various uncertainties, Represents the second-order derivative of the AUV position and attitude vector.

3. The method according to claim 1, characterized in that The disturbance observer for compensating external disturbances, which determines the observation error according to the dynamic and kinematic models and achieves fixed time convergence, specifically includes: Define z1(t) and For v and M -1 τ d The estimation error is defined as e1(t) = v-z1, e2(t) = M -1 τ d -z2, then the disturbance observer is expressed as formula 9: Among them, k1>0, k2>0, 0 <p1<1,q1> 1 is the observer parameter, select appropriate parameters, and perturb τ d It is observed at a fixed time, z1(t) represents the observer's estimate of the AUV's linear velocity and angular velocity vector, z2(t) represents the estimate of the disturbance force in the inertial system, v represents the AUV's linear velocity and angular velocity vector, γ represents a constant greater than 0, sign represents the sign function, z1 represents z1(t) at that moment, M -1 represents the inverse of the inertia matrix, e1(t) represents the velocity observation error, e2(t) represents the disturbance observation error, z2 represents z2(t) at that moment, represents the derivative of z1 with respect to time, Cv represents the Coriolis centripetal force matrix multiplied by velocity, Dv represents the fluid damping force multiplied by velocity, g represents the restoring force matrix, express express represents the time derivative of z2, and sign(e1) represents the sign of the velocity observation error.

4. The method according to any one of claims 1 to 3, characterized in that: The super-helical sliding mode control law for trajectory tracking control that converges in fixed time according to the disturbance observer specifically includes: The trajectory tracking error is defined as η e =η-η d , η d and is the expected trajectory in the inertial coordinate system given for the AUV tracking task, the sliding mode variable s is a designable item, and the super-helical sliding mode scheme with fixed time convergence satisfies Formula 10: Among them, λ1>0, λ2>0, α>0, p>1 are controller parameters; represents the time derivative of the sliding mode variable s, sign(s) represents the sign of the sliding mode variable s, represents the time derivative of ν, ν is Intermediate variables during design; Assumptions Then the control law of the super-helical sliding mode controller is as shown in formula 11: Among them, the controller parameters λ1, λ2, α, p>1, c are positive constants, z2 is the disturbance observer, represents the second-order derivative of the desired trajectory in the inertial coordinate system, t represents time, represents the derivative of the coordinate transformation matrix, v represents the AUV velocity vector, C η Denotes the Coriolis force matrix represented by the pose vector after coordinate transformation, D η represents the fluid damping matrix represented by the pose vector after coordinate transformation, g η represents the restoring force vector, and η represents the AUV position and attitude vector.

5. An underwater robot trajectory tracking device, characterized in that: include: An establishment module is used to establish a dynamics and kinematics model of the underwater robot, wherein the dynamics and kinematics model includes parameter uncertainty and thruster output uncertainty; A disturbance observer module, for determining an observation error according to the dynamic and kinematic models to achieve a disturbance observer for compensating external disturbances with fixed time convergence; A super-helical sliding mode control law module, used for determining a super-helical sliding mode control law for trajectory tracking control that converges in a fixed time according to the disturbance observer; A tracking module is used to track the trajectory of the underwater robot based on the super-helical sliding mode control law.

6. The device according to claim 5, characterized in that The establishment module is specifically used for: Based on the Fossen outline six-degree-of-freedom nonlinear dynamic model shown in Formula 1 and Formula 2, considering the inertia matrix shown in Formula 3 and its uncertainty, the Coriolis centripetal force matrix shown in Formula 4 and its uncertainty, the fluid damping matrix shown in Formula 5 and its uncertainty, and the restoring force matrix shown in Formula 6 and its uncertainty, the dynamic and kinematic models are expressed as shown in Formula 7 and Formula 8: in, represents the derivative of the AUV position and attitude vector, η represents the AUV position and attitude vector, J represents the conversion matrix between the inertial coordinate system and the body coordinate system, v represents the derivative of the AUV linear velocity and angular velocity vector, represents the linear velocity and angular velocity vector of the AUV, represents the nominal inertia matrix, ΔM represents the parameter uncertainty of the inertia matrix, represents the nominal Coriolis centripetal force matrix, ΔC(v) represents the parameter uncertainty of the Coriolis centripetal force matrix, represents the nominal fluid damping matrix, ΔD(v) represents the parameter uncertainty of the fluid damping matrix, represents the nominal restoring force matrix, Δg(η) represents the parameter uncertainty of the nominal restoring force matrix, τ=Bu is the force generated by the AUV actuator, is the thrust conversion matrix of the propeller, ΔB is the uncertainty of the matrix, which is used to characterize the fault. represents the nominal thruster thrust conversion matrix, τ d is the external disturbance, The model uncertainties of the corresponding matrices are ΔΜ η , ΔC η , ΔD η , Δg η , F can be regarded as a collection of various uncertainties, Represents the second-order derivative of the AUV position and attitude vector.

7. The device according to claim 5, characterized in that The disturbance observer module is specifically used for: Define z1(t) and For v and M -1 τ d The estimation error is defined as e1(t) = v-z1, e2(t) = M -1 τ d -z2, then the disturbance observer is expressed as formula 9: Among them, k1>0, k2>0, 0 <p1<1,q1> 1 is the observer parameter, select appropriate parameters, and perturb τ d It is observed at a fixed time, z1(t) represents the observer's estimate of the AUV's linear velocity and angular velocity vector, z2(t) represents the estimate of the disturbance force in the inertial system, v represents the AUV's linear velocity and angular velocity vector, γ represents a constant greater than 0, sign represents the sign function, z1 represents z1(t) at that moment, M -1 represents the inverse of the inertia matrix, e1(t) represents the velocity observation error, e2(t) represents the disturbance observation error, z2 represents z2(t) at that moment, represents the derivative of z1 with respect to time, Cv represents the Coriolis centripetal force matrix multiplied by velocity, Dv represents the fluid damping force multiplied by velocity, g represents the restoring force matrix, express express represents the time derivative of z2, and sign(e1) represents the sign of the velocity observation error.

8. The device according to any one of claims 5 to 7, characterized in that The super-helical sliding mode control law is specifically used for: The trajectory tracking error is defined as η e =η-η d , η d and is the expected trajectory in the inertial coordinate system given for the AUV tracking task, the sliding mode variable s is a designable item, and the super-helical sliding mode scheme with fixed time convergence satisfies Formula 10: Among them, λ1>0, λ2>0, α>0, p>1 are controller parameters; represents the time derivative of the sliding mode variable s, sign(s) represents the sign of the sliding mode variable s, represents the time derivative of ν, ν is Intermediate variables during design; Assumptions Then the control law of the super-helical sliding mode controller is as shown in formula 11: Among them, the controller parameters λ1, λ2, α, p>1, c are positive constants, z2 is the disturbance observer, represents the second-order derivative of the desired trajectory in the inertial coordinate system, t represents time, represents the derivative of the coordinate transformation matrix, v represents the AUV velocity vector, C η Denotes the Coriolis force matrix represented by the pose vector after coordinate transformation, D η represents the fluid damping matrix represented by the pose vector after coordinate transformation, g η represents the restoring force vector, and η represents the AUV position and attitude vector.

9. An electronic device, characterized in that: include: A memory, a processor, and a computer program stored in the memory and executable on the processor, wherein when the computer program is executed by the processor, the steps of the underwater robot trajectory tracking method according to any one of claims 1 to 4 are implemented.

10. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores an implementation program for information transmission, and when the program is executed by the processor, the steps of the underwater robot trajectory tracking method according to any one of claims 1 to 4 are implemented.

Images