Anti-ocean current trajectory tracking control method and system of jet arm submarine cable laying robot

Abstract

The invention discloses an anti-ocean-current trajectory tracking control method and system for a jet-arm submarine cable laying robot. The method comprises the steps that an anti-ocean-current motion model of the jet-arm submarine cable laying robot is constructed; acquiring a position error and a course tracking error according to a given reference trajectory based on the ocean current resistance motion model, and constructing a preset time controller; according to the preset time, the controller obtains a virtual controller of the jet arm seabed cable laying robot so as to define a longitudinal speed error and an angular speed error; constructing a parameter adaptive law according to the longitudinal velocity error and the angular velocity error; and constructing a disturbance observer based on the virtual controller and the parameter adaptive law to obtain an actual controller for ocean current trajectory tracking of the jet arm submarine cable laying robot. Aiming at the problem that an existing submarine cable laying robot cannot realize accurate trajectory tracking in a complex submarine environment, a more perfect ocean current resistance motion model is established, and a disturbance observer is introduced to estimate disturbance in real time so as to realize accurate control; a preset time theory is adopted, so that the system can be converged to a preset time, and the robustness is better; the disturbance observer is used to more accurately compensate the interference of ocean current of the seabed, and the anti-interference capability of the submarine cable laying robot is improved.

Description

Technical Field

[0001] The present invention relates to the technical field of submarine cable-laying robots, and in particular to an anti-ocean current trajectory tracking control method and system of a jet arm submarine cable-laying robot. Background Art

[0002] In recent years, tracked robots have shown great application potential in many fields such as crop transportation, open-pit mining, oil exploration and submarine cable laying. In particular, with the continuous deepening of my country's marine exploration, submarine cable-laying robots with tracked characteristics play an important role in marine operations. At the same time, submarine cable-laying robots have a high degree of uncertainty in the complex marine operating environment and are difficult to model. In this case, the dynamic and nonlinear interaction between the submarine cable-laying robot and its surrounding environment makes it a challenge to ensure accurate trajectory tracking.

[0003] At present, although submarine cable-laying robots have many advantages in completing actual marine tasks, the existing technology still has the following problems: In the existing research on submarine cable-laying robots, since the movement of submarine cable-laying robots is subject to many uncertain factors, it is difficult to accurately model their motion characteristics on the seabed, especially the special state of trenching movement; at the same time, since it is difficult to obtain information on the seabed operating environment in practice, most of the existing control methods ignore the environmental disturbance factors, resulting in unsatisfactory trajectory tracking control effects of submarine cable-laying robots in practical applications. To solve these problems, it is necessary to establish a more complete kinematic and dynamic model, and introduce a disturbance observer to compensate for the influence of ocean currents on submarine cable-laying robots in real time to achieve precise control. In addition, most of the existing trajectory tracking control methods do not consider the expected effect within the predetermined time, while the predetermined time control can improve the convergence speed and robustness of the system, so that the submarine cable-laying robot can achieve precise trajectory tracking in a complex seabed environment. Summary of the invention

[0004] The present invention provides an anti-ocean current trajectory tracking control method and system for a jet arm submarine cable laying robot to overcome the above technical problems.

[0005] In order to achieve the above object, the technical solution of the present invention is:

[0006] A method for tracking and controlling the anti-ocean current trajectory of a jet arm submarine cable laying robot comprises the following steps:

[0007] S1: Construct the anti-ocean current motion model of the jet arm submarine cable laying robot;

[0008] S2: Based on the anti-ocean current motion model, the position error and heading tracking error are obtained according to the given reference trajectory, and a predetermined time controller is constructed based on the position error and heading tracking error;

[0009] S3: obtaining a virtual controller of the jet arm submarine cable laying robot according to a predetermined time controller;

[0010] And the virtual controller includes a longitudinal velocity virtual controller and an angular velocity virtual controller;

[0011] And define the longitudinal velocity error and angular velocity error according to the virtual controller;

[0012] S4: construct a parameter adaptive law based on the longitudinal velocity error and the angular velocity error;

[0013] The parameter adaptive law includes a longitudinal velocity adaptive law and an angular velocity adaptive law;

[0014] S5: A disturbance observer is constructed based on a virtual controller and a parameter adaptation law to obtain an actual controller for current-resistant trajectory tracking of a jet-arm submarine cable-laying robot.

[0015] Furthermore, the anti-ocean current motion model of the jet arm submarine cable laying robot constructed in S1 is:

[0016]

[0017] Where: η=[x,y,θ] T represents the position-deflection angle vector of the jet arm submarine cable laying robot; x, y represent the horizontal and vertical coordinates of the two-dimensional coordinate position of the submarine cable laying robot; θ represents the deflection angle; v = [v, ω] Τ represents the velocity vector in the coordinate system of the jet arm submarine cable laying robot; v represents the longitudinal velocity; ω represents the angular velocity; τ=[τ v ,τ ω ] T , and τ v With τ ω represents the actual control signal of the jet arm submarine cable laying robot in the longitudinal and rotational directions respectively; F = [F v ,F ω ] T ,and f L With f R They represent the resistance on the left and right tracks of the jet arm submarine cable laying robot respectively; F d represents the suspension rope force on the robot; represents the angle between the robot suspension rope and the direction of motion; Fcos(α1+α2) represents the component of the reaction force of the nozzle on the robot in the direction of motion of the robot; α1 represents the angle between the nozzle and the spray arm; α2 represents the angle between the spray arm and the direction of motion; F D Indicates drag resistance; F ω =-f R b+f L Bμ , and b represents the distance from the track center to the robot geometric center; M μ Indicates the steering resistance torque; D = [D v D ω ] T , and D v With D ω represents the disturbance of the submarine current and satisfies and Respectively represent D v With D ω The upper bound of; J(θ), A, B, C represent the parameter matrix; m, I ω They represent the total mass and the moment of inertia of the submarine cable-laying robot underwater respectively; μ L Indicates the positive friction coefficient between the left track and the soil; μ R represents the positive friction coefficient between the right track and the soil; L and R represent the left and right tracks respectively; I r represents the track moment of inertia; k r represents the traction coefficient; r represents the rotation radius of the track driving wheel, and -Av+BF represents the uncertainty in the anti-ocean current motion model.

[0018] Furthermore, the S2 specifically includes the following steps:

[0019] S21: Based on the anti-ocean current motion model, according to the given reference trajectory η d Get the position error, where η d =[x d ,y d ,θ d ] T ;

[0020] And the position error is

[0021]

[0022] e x =xx d , e y =yy d

[0023] Where: e x Indicates the horizontal coordinate error of the position; x d represents the horizontal coordinate of the expected position; e y Indicates the position ordinate error; y d Indicates the expected position ordinate; e z Indicates position error;

[0024] And obtain the heading tracking error based on the position error, and its expression is:

[0025] eθ =θ-θ d

[0026] Where: θ d Represents the horizontal coordinate error e x and position ordinate error e y The ideal heading angle associated with this d =atan2(e y ,e x ); atan2 represents the inverse tangent function;

[0027] S22: Construct error constraints based on position error and heading tracking error, and its expression is:

[0028]

[0029] Where: Ω j (t) represents the boundary function to be set, and j = z, θ;

[0030] S23: Construct a scheduled time performance function based on the error constraint condition, and design the boundary function as follows:

[0031]

[0032] Where: j (t) represents the scheduled time performance function; ρ j∞ represents the convergence accuracy; n represents the order of the system; number; σ ej , ρ j0 ,ρ j∞ represents the design parameters required according to actual needs, and σ ej >0, ρ j0 >ρ j∞ >0;

[0033] S24: Construct a logarithmic barrier function based on the designed boundary function, and its expression is

[0034]

[0035] Where: γ z represents an intermediate variable and γ θ represents an intermediate variable and ln(·) represents the natural logarithm function; and s z represents the barrier function with respect to the position error; s θ represents the obstacle function with respect to heading tracking error;

[0036] S25: Derivation of the logarithmic barrier function is used to obtain the predetermined time controller, which is expressed as

[0037]

[0038] Where: φ q With χ q Indicates the scheduled time control signal; φ z ,x z Indicates the distance to the predetermined time signal; φ θ ,x θ Indicates the angle predetermined time signal.

[0039] Furthermore, the S3 specifically includes the following steps:

[0040] S31: Based on the anti-ocean current motion model of the jet arm submarine cable laying robot, a virtual controller of the jet arm submarine cable laying robot is constructed according to the logarithmic obstacle function and the predetermined time controller;

[0041] And the virtual controller includes a longitudinal velocity virtual controller and an angular velocity virtual controller;

[0042] The expression of the longitudinal velocity virtual controller is:

[0043]

[0044] The expression of the angular velocity virtual controller is:

[0045]

[0046] Where: and represents a virtual control signal; Represents x d ,y d The first derivative of

[0047] S32: Perform first-order low-pass filtering on the virtual controller to obtain the first-order filter output error; and define the longitudinal velocity error and angular velocity error of the jet arm submarine cable laying robot according to the first-order filter output error, and its expression is:

[0048]

[0049] s v =v-α fv

[0050] s ω =ω-α fω

[0051] Where: s fv With s fωRepresents the output error of the first-order filter; α fv With α fω represents the filtered signal; and Represents a virtual control signal; s v ,s ω Represent the longitudinal velocity error and angular velocity error respectively;

[0052] And the expression for first-order low-pass filtering of the virtual controller is:

[0053]

[0054] Where: α cv ,α cω Respectively represent the input of the first-order low-pass filter in the longitudinal velocity direction and the angular velocity direction; α fω (0),α cω (0) indicates α cv ,α cω The initial value of cv The filtering time constant; κ2 represents α cω The filter time constant; Represents α fv The first derivative of Represents α fω The first derivative of .

[0055] Furthermore, the S4 specifically includes the following steps:

[0056] Approximation of the uncertainty term λ in the anti-ocean current motion model based on fuzzy logic system and λ=-Av+BF,λ=[λ v λ ω ] T , where λ v represents the unknown function term about the longitudinal velocity; λ ω Represents the unknown function term about angular velocity;

[0057] The expression of the unknown function term in the anti-ocean current motion model approximated by the fuzzy logic system is:

[0058]

[0059] Where: W v * represents the adaptive parameter W v The optimal parameter vector of ω * represents the adaptive parameter W ω The optimal parameter vector of ; Indicates is a continuous function of a variable; Indicates is a continuous function of a variable; and represents the variables related to the longitudinal velocity v and the angular velocity ω; ε v With ε ω represents the approximation error and satisfies and and represents a positive constant; and Represent the basis functions of the fuzzy logic system;

[0060] S42: constructing a parameter adaptive law according to the longitudinal velocity error and the angular velocity error in combination with step S41; the parameter adaptive law includes a longitudinal velocity adaptive law and an angular velocity adaptive law;

[0061] The expression of the longitudinal velocity adaptation law is:

[0062]

[0063] The expression of the angular velocity adaptation law is:

[0064]

[0065] Where: represents the parameter adaptation law and i=v,ω;δ v ,δ ω represents the design parameters and δ v 〉0,δ ω >0;Γ v ,Γ ω represents the designed control gain matrix; W v * An estimated value of W ω * The estimated value of .

[0066] Furthermore, the S5 specifically includes the following steps:

[0067] S51: Construction of disturbance observer based on virtual controller and parameter adaptation law;

[0068] The expression of the disturbance observer is:

[0069]

[0070] Where: v With ξ ω Represents the observation state; r v With r ω Respectively represent the unknown disturbances in the longitudinal velocity direction and the angular velocity direction; and Respectively represent the unknown interference r v With r ω The estimated value of k rv With k rω represents a positive design constant; and Represents ξ v With ξ ω The first derivative of ; T represents transpose;

[0071] S52: Based on the disturbance observer, a practical controller for tracking the anti-ocean current trajectory of the jet-arm submarine cable-laying robot is constructed, and its expression is

[0072]

[0073] Where: τ v ,τ ω represents the actual control signal for anti-ocean current trajectory tracking; k v ,k ω Represents design parameters.

[0074] An anti-ocean current trajectory tracking control system for a jet arm submarine cable laying robot, comprising a model creation module, a virtual controller module, an adaptive law module, a disturbance observer module and a predetermined time module;

[0075] The model creation module is used to construct an anti-ocean current motion model of the jet arm submarine cable laying robot;

[0076] The predetermined time module is used to obtain the position error and the heading tracking error according to the given reference trajectory based on the anti-ocean current motion model, and to construct a predetermined time controller based on the position error and the heading tracking error;

[0077] The virtual controller module is used to obtain a virtual controller of the jet arm submarine cable laying robot according to a predetermined time controller based on an anti-ocean current motion model;

[0078] And the virtual controller includes a longitudinal velocity virtual controller and an angular velocity virtual controller;

[0079] And define the longitudinal velocity error and angular velocity error according to the virtual controller;

[0080] The adaptive law module is used to construct a parameter adaptive law based on the anti-ocean current motion model according to the longitudinal velocity error and the angular velocity error;

[0081] The parameter adaptive law includes a longitudinal velocity adaptive law and an angular velocity adaptive law;

[0082] The disturbance observer module is used to construct a disturbance observer according to a virtual controller and a parameter adaptive law, and to construct an actual controller for anti-ocean current trajectory tracking of a jet arm submarine cable-laying robot based on the disturbance observer.

[0083] Compared with the prior art, the anti-ocean current trajectory tracking control method and system of the jet arm submarine cable laying robot of the present invention has the following beneficial effects:

[0084] First, in the existing research methods of submarine cable laying robots, the modeling of submarine cable laying robots is not perfect. The present invention combines the trenching characteristics of the submarine cable laying robot with the characteristics of the crawler mobile robot to complete the modeling of the anti-ocean current motion model of the jet arm submarine cable laying robot, which makes it easier to realize the trajectory tracking control of the submarine cable laying robot.

[0085] Second, the present invention takes into account the impact of interference factors such as seabed currents on the seabed cable-laying robot and designs a disturbance observer. The use of the disturbance observer will more accurately compensate for the interference of seabed currents, improve the anti-interference ability of the seabed cable-laying robot, and improve the practical application value of the seabed cable-laying robot.

[0086] Third, most of the control algorithms of submarine cable laying robots can achieve the expected control objectives, but they cannot be fully guaranteed to be completed within the preset time range. Therefore, the present invention uses the predetermined time theory to enable the system to converge to the preset time, with better robustness, so that the submarine cable laying robot trajectory tracking can achieve the expected control effect within the preset time. BRIEF DESCRIPTION OF THE DRAWINGS

[0087] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the following briefly introduces the drawings required for use in the embodiments or the description of the prior art. Obviously, the drawings described below are some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative labor.

[0088] Figure 1 This is a flow chart of the anti-ocean current trajectory tracking control method of the jet arm submarine cable laying robot of the present invention;

[0089] Figure 2 It is a two-dimensional plane diagram of the trajectory tracking of the submarine cable laying robot in the actual task in this embodiment;

[0090] Figure 3 is a relative distance error curve diagram between the submarine cable laying robot and the tracking trajectory in this embodiment;

[0091] Figure 4is a curve diagram of the relative angle error between the submarine cable laying robot and the tracking trajectory in this embodiment;

[0092] Figure 5 is the longitudinal input signal τ of the submarine cable laying robot in this embodiment v Graph of

[0093] Figure 6 is the rotation direction input signal τ of the submarine cable laying robot in this embodiment ω Graph of

[0094] Figure 7 This is a block diagram of the anti-ocean current trajectory tracking control system of the jet arm submarine cable laying robot in this embodiment. DETAILED DESCRIPTION

[0095] In order to make the purpose, technical solution and advantages of the embodiments of the present invention clearer, the technical solution in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0096] This embodiment provides a method for tracking and controlling the anti-ocean current trajectory of a jet arm submarine cable laying robot. Figure 1 As shown, the specific steps include:

[0097] S1: Construct the anti-ocean current motion model of the jet arm submarine cable laying robot;

[0098] Specifically, the input signal of the submarine cable laying robot is set as the actual control signal τ, and the output signal is set as the position information η and the longitudinal velocity information v of the submarine cable laying robot. Then, the anti-ocean current motion model of the jet arm submarine cable laying robot is constructed as follows:

[0099]

[0100] Where: η=[x,y,θ] T represents the position-deflection angle vector of the jet arm submarine cable laying robot; x, y represent the horizontal and vertical coordinates of the two-dimensional coordinate position of the submarine cable laying robot; θ represents the deflection angle; v = [v, ω] Τ represents the velocity vector in the coordinate system of the jet arm submarine cable laying robot; v represents the longitudinal velocity; ω represents the angular velocity; τ=[τ v ,τ ω ] T , and τ v With τ ω represents the actual control signal of the jet arm submarine cable laying robot in the longitudinal and rotational directions respectively; F = [Fv ,F ω ] T ,and f L With f R They represent the resistance on the left and right tracks of the jet arm submarine cable laying robot respectively; F d represents the suspension rope force on the robot; represents the angle between the robot suspension rope and the direction of motion; Fcos(α1+α2) represents the component of the reaction force of the nozzle on the robot in the direction of motion of the robot; α1 represents the angle between the nozzle and the spray arm; α2 represents the angle between the spray arm and the direction of motion; F D Indicates drag resistance; F ω =-f R b+f L B μ , and b represents the distance from the track center to the robot geometric center; M μ Indicates the steering resistance torque; D = [D v D ω ] T , and D v With D ω represents the disturbance of the submarine current and satisfies and Respectively represent D v With D ω The upper bound of; J(θ), A, B, C represent the parameter matrix; m, I ω They represent the total mass and the moment of inertia of the submarine cable-laying robot underwater respectively; μ L Indicates the positive friction coefficient between the left track and the soil; μ R represents the positive friction coefficient between the right track and the soil; L and R represent the left and right tracks respectively; I r represents the track moment of inertia; k r represents the traction coefficient; r represents the rotation radius of the track drive wheel, and -Av+BF represents the uncertainty term in the anti-ocean current motion model;

[0101] S2: Based on the anti-ocean current motion model, the position error and heading tracking error are obtained according to the given reference trajectory, and a predetermined time controller is constructed based on the position error and heading tracking error;

[0102] The specific steps include:

[0103] S21: Based on the anti-ocean current motion model, according to the given reference trajectory η d Get the position error, where η d =[x d ,y d ,θ d ] T ;

[0104] And the position error is

[0105]

[0106] e x =xx d , e y =yy d

[0107] Where: e x Indicates the horizontal coordinate error of the position; x d represents the horizontal coordinate of the expected position; e y Indicates the position ordinate error; y d Indicates the vertical coordinate of the expected position; e z Indicates position error;

[0108] And obtain the heading tracking error based on the position error, and its expression is:

[0109] e θ =θ-θ d

[0110] Where: θ d Represents the horizontal coordinate error e x and position ordinate error e y The ideal heading angle associated with this d =atan2(e y ,e x ); atan2 represents the inverse tangent function;

[0111] S22: Construct error constraints based on position error and heading tracking error, and its expression is:

[0112]

[0113] Where: Ω j (t) represents the boundary function to be set, and j = z, θ;

[0114] S23: Construct a scheduled time performance function based on the error constraint condition, and design the boundary function as follows:

[0115]

[0116] Where: j (t) represents the scheduled time performance function; ρ j∞ represents the convergence accuracy; n represents the order of the system; number; σ ej , ρ j0 ,ρ j∞represents the design parameters required according to actual needs, and σ ej >0, ρ j0 >ρ j∞ >0;

[0117] S24: Construct a logarithmic barrier function based on the designed boundary function, and its expression is

[0118]

[0119] Where: γ z represents an intermediate variable and γ θ represents an intermediate variable and ln(·) represents the natural logarithmic function; s θ For example, if and only if e θ = 0, s θ =0; when s θ →-∞, When θ →-∞,e θ →- Ω θ (t); therefore, the azimuth angle tracking error e θ will converge to a compact set containing the origin within; and z represents the barrier function with respect to the position error; s θ represents the obstacle function with respect to heading tracking error;

[0120] S25: Derivation of the logarithmic barrier function is used to obtain the predetermined time controller, which is expressed as

[0121]

[0122] Where: φ q With χ q Indicates the scheduled time control signal; φ z ,x z Indicates the distance to the predetermined time signal; φ θ ,x θ Indicates the angle predetermined time signal;

[0123] S3: obtaining a virtual controller of the jet arm submarine cable laying robot according to a predetermined time controller, and defining a longitudinal velocity error and an angular velocity error according to the virtual controller;

[0124] And the virtual controller includes a longitudinal velocity virtual controller and an angular velocity virtual controller;

[0125] The specific steps include:

[0126] S31: Based on the anti-ocean current motion model of the jet arm submarine cable laying robot, a virtual controller of the jet arm submarine cable laying robot is constructed according to the logarithmic obstacle function and the predetermined time controller;

[0127] And the virtual controller includes a longitudinal velocity virtual controller and an angular velocity virtual controller;

[0128] The expression of the longitudinal velocity virtual controller is:

[0129]

[0130] The expression of the angular velocity virtual controller is:

[0131]

[0132] Where: and represents a virtual control signal; Represents x d ,y d The first derivative of

[0133] S32: Perform first-order low-pass filtering on the virtual controller to obtain the first-order filter output error; and define the longitudinal velocity error and angular velocity error of the jet arm submarine cable laying robot according to the first-order filter output error, and its expression is:

[0134]

[0135] s v =v-α fv

[0136] s ω =ω-α fω

[0137] Where: s fv With s fω Represents the output error of the first-order filter; α fv With α fω represents the filtered signal; and Represents a virtual control signal; s v ,s ω Represent the longitudinal velocity error and angular velocity error respectively;

[0138] And the expression for first-order low-pass filtering of the virtual controller is:

[0139]

[0140] Where: α cv ,α cωRespectively represent the input of the first-order low-pass filter in the longitudinal velocity direction and the angular velocity direction; α fω (0),α cω (0) indicates α cv ,α cω The initial value of cv The filtering time constant; κ2 represents α cω The filter time constant; Represents α fv The first derivative of Represents α fω The first derivative of s fv and fω The derivation yields:

[0141]

[0142] Where: and Denotes that the independent variable is Δ v and Δ ω A continuous function of and represents a positive constant;

[0143] This embodiment also includes known variations for conducting simulation experiments, as shown below:

[0144] According to the anti-ocean current motion model, the position error and heading tracking error are derived, and the derivative formula is:

[0145]

[0146] Based on the derivative formula combined with the scheduled time controller, we can get

[0147]

[0148] S4: construct a parameter adaptive law based on the longitudinal velocity error and the angular velocity error;

[0149] The parameter adaptive law includes a longitudinal velocity adaptive law and an angular velocity adaptive law;

[0150] The specific steps include:

[0151] Approximation of the uncertainty term λ in the anti-ocean current motion model based on fuzzy logic system and λ=-Av+BF,λ=[λ v λ ω ] T , where λ v represents the unknown function term about the longitudinal velocity; λ ω Represents the unknown function term about angular velocity;

[0152] The expression of the unknown function term in the anti-ocean current motion model approximated by the fuzzy logic system is:

[0153]

[0154] Where: W v * represents the adaptive parameter W v The optimal parameter vector of ω * represents the adaptive parameter W ω The optimal parameter vector of ; Indicates is a continuous function of a variable; Indicates is a continuous function of a variable; and represents the variables related to the longitudinal velocity v and the angular velocity ω; ε v With ε ω represents the approximation error and satisfies and and represents a positive constant; and Represent the basis functions of the fuzzy logic system;

[0155] S42: constructing a parameter adaptive law according to the longitudinal velocity error and the angular velocity error in combination with step S41; the parameter adaptive law includes a longitudinal velocity adaptive law and an angular velocity adaptive law;

[0156] The expression of the longitudinal velocity adaptation law is:

[0157]

[0158] The expression of the angular velocity adaptation law is:

[0159]

[0160] Where: represents the parameter adaptation law and i=v,ω;δ v ,δ ω represents the design parameters and δ v >0,δ ω >0;Γ v ,Γ ω represents the designed control gain matrix; W v * An estimated value of W ω * An estimated value of

[0161] S5: Construct a disturbance observer based on the virtual controller and the parameter adaptation law to obtain the actual controller for the anti-current trajectory tracking of the jet-arm submarine cable-laying robot;

[0162] The specific steps include:

[0163] S51: Construction of disturbance observer based on virtual controller and parameter adaptation law;

[0164] The expression of the disturbance observer is:

[0165]

[0166] Where: v With ξ ω Represents the observation state; r v With r ω Respectively represent the unknown disturbances in the longitudinal velocity direction and the angular velocity direction; and Respectively represent the unknown interference r v With r ω The estimated value of k rv With k rω represents a positive design constant; and Represents ξ v With ξ ω The first derivative of ; T represents transpose;

[0167] S52: Based on the disturbance observer, a practical controller for tracking the anti-ocean current trajectory of the jet-arm submarine cable-laying robot is constructed, and its expression is

[0168]

[0169] Where: τ v ,τ ω represents the actual control signal for anti-ocean current trajectory tracking; k v ,k ω Represents design parameters.

[0170] In summary, the beneficial effects of this embodiment are:

[0171] First, in the existing research methods of submarine cable laying robots, the modeling of submarine cable laying robots is not perfect. This embodiment combines the trenching characteristics of the submarine cable laying robot and the characteristics of the tracked mobile robot to complete the modeling. The precise modeling of kinematics and dynamics makes it easier to realize the trajectory tracking control of the submarine cable laying robot.

[0172] Second, in this embodiment, the influence of interference factors such as seabed currents on the submarine cable-laying robot is taken into consideration, and a disturbance observer is designed to improve the practical application value of the submarine cable-laying robot. Therefore, it is more universal and convincing than previous studies.

[0173] Third, most of the control algorithms of submarine cable laying robots can achieve the expected control objectives, but they cannot be completely guaranteed to be completed within the preset time range. Therefore, in this embodiment, the predetermined time theory is used to enable the submarine cable laying robot trajectory tracking to achieve the expected control effect within the predetermined time.

[0174] At the same time, based on the same invention essence, the present invention also provides an anti-ocean current trajectory tracking control system for a jet arm submarine cable laying robot, such as Figure 7 As shown, it includes a model creation module, a virtual controller module, an adaptive law module, a disturbance observer module and a predetermined time module;

[0175] The predetermined time module is used to obtain the position error and the heading tracking error according to the given reference trajectory based on the anti-ocean current motion model, and to construct a predetermined time controller based on the position error and the heading tracking error;

[0176] The virtual controller module is used to obtain a virtual controller of the jet arm submarine cable laying robot according to a predetermined time controller based on an anti-ocean current motion model;

[0177] And the virtual controller includes a longitudinal velocity virtual controller and an angular velocity virtual controller;

[0178] And define the longitudinal velocity error and angular velocity error according to the virtual controller;

[0179] The adaptive law module is used to construct a parameter adaptive law based on the anti-ocean current motion model according to the longitudinal velocity error and the angular velocity error;

[0180] The parameter adaptive law includes a longitudinal velocity adaptive law and an angular velocity adaptive law;

[0181] The disturbance observer module is used to construct a disturbance observer according to a virtual controller and a parameter adaptive law, and to construct an actual controller for anti-ocean current trajectory tracking of a jet arm submarine cable-laying robot based on the disturbance observer.

[0182] In a specific embodiment, the connection relationship of each module is as follows: the output end of the model creation module is connected to the input end of the predetermined time module, the virtual controller module and the adaptive law module respectively;

[0183] The output end of the predetermined time module is connected to the input end of the virtual controller module;

[0184] The output end of the virtual controller module is connected to the input end of the adaptive law module and the input end of the disturbance observer module respectively;

[0185] The output end of the adaptive law module is connected to the input end of the disturbance observer module, and the output end of the disturbance observer module is connected to the control input end of the jet arm submarine cable laying robot;

[0186] The model creation module is used to construct an anti-ocean current motion model of the jet arm submarine cable laying robot;

[0187] In the trajectory tracking control of the submarine cable laying robot in this embodiment, the given reference trajectory, reference distance, reference relative azimuth angle signal and the position vector η of the submarine cable laying robot are transmitted to the predetermined time module, and the predetermined time module analyzes and calculates the input information to obtain the predetermined time parameter φ q and χ q (q=z,θ), and transmit it to the virtual controller module. At the same time, the velocity vector υ information of the submarine cable laying robot is transmitted to the virtual controller module, and the virtual controller module analyzes and calculates the input information to obtain the virtual control signal of the virtual controller and The virtual controller module will receive the virtual control signal and At the same time, the velocity vector υ information of the submarine cable laying robot is transmitted to the adaptive law module, which analyzes and calculates the input information to obtain the adaptive parameters It is sent to the disturbance observer module, which analyzes and processes the input signal from the outside to obtain the actual control signal τ v and τ ω , and finally the disturbance observer module transmits the input actual control signal to the submarine cable laying robot. The design goal of this embodiment is to enable the submarine cable laying robot trajectory tracking control system to effectively overcome the ocean current interference after introducing the disturbance observer and the predetermined time control method, and to further achieve the submarine cable laying robot collision avoidance and connectivity maintenance after combining with the specified performance theory.

[0188] The simulation results of this embodiment are as follows Figures 2 to 6 shown. Figure 2 It is a two-dimensional plane trajectory tracking curve diagram of the submarine cable laying robot according to the actual task requirements. It can be seen from the graph that the submarine cable laying robot is constrained in the specified area; Figure 3 It is a relative distance error curve between the submarine cable laying robot and the following track. From the analysis of the graph, it can be seen that the relative distance error can converge to near zero within the predetermined time; Figure 4It is a curve diagram of the relative angle error between the submarine cable laying robot and the following trajectory. From the analysis of the graph, it can be seen that the relative angle error can converge to near zero within the predetermined time; Figures 5 and 6 They represent the longitudinal input signal τ of the submarine cable laying robot respectively. v and the rotation direction input signal τ ω From the curve graph, it can be seen that the introduced disturbance observer can overcome the disturbance effect and limit the control input to a specified range; from the above simulation result graphs and the obtained tables, it can be seen that the designed submarine cable-laying robot trajectory tracking controller can eliminate the disturbance effect of ocean currents and reach the stability requirements within the predetermined time, and finally realize the expected submarine cable-laying robot trajectory tracking task requirements.

[0189] 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 and controlling the anti-ocean current trajectory of a jet arm submarine cable laying robot, characterized in that: The specific steps include: S1: Construct the anti-ocean current motion model of the jet arm submarine cable laying robot; S2: Based on the anti-ocean current motion model, the position error and heading tracking error are obtained according to the given reference trajectory, and a predetermined time controller is constructed based on the position error and heading tracking error; S3: obtaining a virtual controller of the jet arm submarine cable laying robot according to a predetermined time controller; And the virtual controller includes a longitudinal velocity virtual controller and an angular velocity virtual controller; And define the longitudinal velocity error and angular velocity error according to the virtual controller; S4: construct a parameter adaptive law based on the longitudinal velocity error and the angular velocity error; The parameter adaptive law includes a longitudinal velocity adaptive law and an angular velocity adaptive law; S5: A disturbance observer is constructed based on a virtual controller and a parameter adaptation law to obtain an actual controller for current-resistant trajectory tracking of a jet-arm submarine cable-laying robot.

2. The anti-ocean current trajectory tracking control method of a jet arm submarine cable laying robot according to claim 1 is characterized in that: The anti-ocean current motion model of the jet arm submarine cable laying robot constructed in S1 is: Where: η=[x,y,θ] T represents the position-deflection angle vector of the jet arm submarine cable laying robot; x, y represent the horizontal and vertical coordinates of the two-dimensional coordinate position of the submarine cable laying robot; θ represents the deflection angle; υ=[v,ω] Τ represents the velocity vector in the coordinate system of the jet arm submarine cable laying robot; v represents the longitudinal velocity; ω represents the angular velocity; τ=[τ v ,τ ω ] T , and τ v With τ ω represents the actual control signal of the jet arm submarine cable laying robot in the longitudinal and rotational directions respectively; F = [F v ,F ω ] T ,and f L With f R They represent the resistance on the left and right tracks of the jet arm submarine cable laying robot respectively; F d represents the suspension rope force on the robot; represents the angle between the robot suspension rope and the direction of motion; Fcos(α1+α2) represents the component of the reaction force of the nozzle on the robot in the direction of motion of the robot; α1 represents the angle between the nozzle and the spray arm; α2 represents the angle between the spray arm and the direction of motion; F D Indicates drag resistance; F ω=-f R b+f L B μ , and b represents the distance from the track center to the robot geometric center; M μ Indicates the steering resistance torque; D = [D v D ω ] T , and D v With D ω represents the disturbance of the submarine current and satisfies and Respectively represent D v With D ω The upper bound of; J(θ), A, B, C represent the parameter matrix; m, I ω They represent the total mass and the moment of inertia of the submarine cable-laying robot underwater respectively; μ L Indicates the positive friction coefficient between the left track and the soil; μ R represents the positive friction coefficient between the right track and the soil; L and R represent the left and right tracks respectively; I r represents the track moment of inertia; k r represents the traction coefficient; r represents the rotation radius of the track driving wheel, and -Aυ+BF represents the uncertainty in the anti-ocean current motion model.

3. The anti-ocean current trajectory tracking control method of a jet arm submarine cable laying robot according to claim 2 is characterized in that: The S2 specifically includes the following steps: S21: Based on the anti-ocean current motion model, according to the given reference trajectory η d Get the position error, where η d =[x d ,y d ,θ d ] T ; And the position error is and x =xx d ,and y =yy d Where: e x Indicates the horizontal coordinate error of the position; x d represents the horizontal coordinate of the expected position; e y Indicates the position ordinate error; y d Indicates the vertical coordinate of the expected position; e z Indicates position error; And obtain the heading tracking error based on the position error, and its expression is e θ =θ-θ d Where: θ d Represents the horizontal coordinate error e x and position ordinate error e y The ideal heading angle associated with this d =atan2(e y ,e x ); atan2 represents the inverse tangent function; S22: Construct error constraints based on position error and heading tracking error, and its expression is: Where: Ω j (t) represents the boundary function to be set, and j = z, θ; S23: Construct a scheduled time performance function based on the error constraint condition, and design the boundary function as follows: Where: j (t) represents the scheduled time performance function; ρ j∞ represents the convergence accuracy; n represents the order of the system; number; σ ej , ρ j0 ,ρ j∝ represents the design parameters required according to actual needs, and σ ej >0, ρ j0 >ρ j∞ >0; S24: Construct a logarithmic barrier function based on the designed boundary function, and its expression is Where: γ z represents an intermediate variable and γ θ represents an intermediate variable and ln(·) represents the natural logarithm function; and s z represents the barrier function with respect to the position error; s θ represents the obstacle function with respect to heading tracking error; S25: Derivation of the logarithmic barrier function is used to obtain the predetermined time controller, which is expressed as Where: φ q With χ q Indicates the scheduled time control signal; φ z ,x z Indicates the distance to the predetermined time signal; φ θ ,x θ Indicates the angle predetermined time signal.

4. The anti-ocean current trajectory tracking control method of a jet arm submarine cable laying robot according to claim 3 is characterized in that: The S3 specifically includes the following steps: S31: Based on the anti-ocean current motion model of the jet arm submarine cable laying robot, a virtual controller of the jet arm submarine cable laying robot is constructed according to the logarithmic obstacle function and the predetermined time controller; And the virtual controller includes a longitudinal velocity virtual controller and an angular velocity virtual controller; The expression of the longitudinal velocity virtual controller is: The expression of the angular velocity virtual controller is: Where: and represents a virtual control signal; Represents x d ,y d The first derivative of S32: Perform first-order low-pass filtering on the virtual controller to obtain the first-order filter output error; and define the longitudinal velocity error and angular velocity error of the jet arm submarine cable laying robot according to the first-order filter output error, and its expression is: s v =v-a fv s ω =ω-a fω Where: s fv With s fω Represents the output error of the first-order filter; α fv With α fω represents the filtered signal; and Represents a virtual control signal; s v ,s ω They represent the longitudinal velocity error and angular velocity error respectively; And the expression for first-order low-pass filtering of the virtual controller is: α fv (0)＝α cv (0)； α fω (0)＝α cω (0) Where: α cv ,α cω Respectively represent the input of the first-order low-pass filter in the longitudinal velocity direction and the angular velocity direction; α fω (0),α cω (0) indicates α cv ,α cω The initial value of cv The filtering time constant; κ2 represents α cω The filter time constant; Represents α fv The first derivative of Represents α fω The first derivative of .

5. The anti-ocean current trajectory tracking control method of a jet arm submarine cable laying robot according to claim 4 is characterized in that: The S4 specifically comprises the following steps: S41: Approximating the uncertainty term λ in the anti-ocean current motion model based on the fuzzy logic system and λ=-Aυ+BF,λ=[λ v λ ω ] T , where λ v represents the unknown function term about the longitudinal velocity; λ ω Represents the unknown function term about angular velocity; The expression of the unknown function term in the anti-ocean current motion model approximated by the fuzzy logic system is: Where: W v * represents the adaptive parameter W v The optimal parameter vector of ω * represents the adaptive parameter W ω The optimal parameter vector of ; Indicates is a continuous function of a variable; Indicates is a continuous function of a variable; and represents the variables related to the longitudinal velocity v and the angular velocity ω; ε v With ε ω represents the approximation error and satisfies and and represents a positive constant; and Represent the basis functions of the fuzzy logic system; S42: constructing a parameter adaptive law according to the longitudinal velocity error and the angular velocity error in combination with step S41; the parameter adaptive law includes a longitudinal velocity adaptive law and an angular velocity adaptive law; The expression of the longitudinal velocity adaptation law is: The expression of the angular velocity adaptation law is: Where: represents the parameter adaptation law and i=v,ω;δ v ,δ ω represents the design parameters and δ v >0,δ ω >0;Γ v ,Γ ω represents the designed control gain matrix; W v * An estimated value of W ω * The estimated value of .

6. The anti-ocean current trajectory tracking control method of the jet arm submarine cable laying robot according to claim 5 is characterized in that: The S5 specifically includes the following steps: S51: Construction of disturbance observer based on virtual controller and parameter adaptation law; The expression of the disturbance observer is: Where: v With ξ ω Represents the observation state; r v With r ω Respectively represent the unknown disturbances in the longitudinal velocity direction and the angular velocity direction; and Respectively represent the unknown interference r v With r ω The estimated value of k rv With k rω represents a positive design constant; and Represents ξ v With ξ ω The first derivative of ; T represents transpose; S52: Based on the disturbance observer, a practical controller for tracking the anti-ocean current trajectory of the jet-arm submarine cable-laying robot is constructed, and its expression is Where: τ v ,τ ω represents the actual control signal for anti-ocean current trajectory tracking; k v ,k ω Represents design parameters.

7. A system based on the anti-ocean current trajectory tracking control method of the jet arm submarine cable laying robot according to claim 1, characterized in that: It includes a model creation module, a virtual controller module, an adaptive law module, a disturbance observer module and a predetermined time module; The model creation module is used to construct an anti-ocean current motion model of the jet arm submarine cable laying robot; The predetermined time module is used to obtain the position error and the heading tracking error according to the given reference trajectory based on the anti-ocean current motion model, and to construct a predetermined time controller based on the position error and the heading tracking error; The virtual controller module is used to obtain a virtual controller of the jet arm submarine cable laying robot according to a predetermined time controller based on an anti-ocean current motion model; And the virtual controller includes a longitudinal velocity virtual controller and an angular velocity virtual controller; And define the longitudinal velocity error and angular velocity error according to the virtual controller; The adaptive law module is used to construct a parameter adaptive law based on the anti-ocean current motion model according to the longitudinal velocity error and the angular velocity error; The parameter adaptive law includes a longitudinal velocity adaptive law and an angular velocity adaptive law; The disturbance observer module is used to construct a disturbance observer according to a virtual controller and a parameter adaptive law, and to construct an actual controller for anti-ocean current trajectory tracking of a jet arm submarine cable-laying robot based on the disturbance observer.

Images