A control method, device, system and storage medium of an underwater robot with an arm

Abstract

The present invention discloses a control method and device, system and storage medium for an underwater robot with an arm, including: the motion control of the robot arm adopts an improved non-singular terminal synovial film control, and the robot body adopts an integral synovial film control of an extended state observer. By adopting the technical solution of the present invention, the trajectory of the robot arm and the robot body can be controlled to complete a specified task.

Description

Technical Field

[0001] The present invention belongs to the technical field of robot main body, and in particular relates to a control method and device, a system and a storage medium of an arm-carrying underwater robot. Background Art

[0002] With the development of marine resources at home and abroad, robots are used in various submarine tasks. However, in the fields of deep-sea exploration, submarine salvage, etc., robots are difficult to meet such needs due to their own limitations. In contrast, the arm-carrying underwater robot is equipped with a two-joint robotic arm, making it the first choice for performing such tasks.

[0003] However, the problem of poor system stability of the arm-carrying underwater robot body has always existed. The land robot equipped with a robotic arm can fix the base, so only the carrying capacity of the robotic arm needs to be solved. However, when the arm-carrying robot body is working, due to the influence of the complex ocean environment, it is difficult for the unmanned boat to maintain a relatively stable posture, which can easily lead to confusion in the robot coordinate system and make it impossible to control the trajectory of the robot body to complete the specified task. Summary of the invention

[0004] The technical problem to be solved by the present invention is to provide a control method and device, system and storage medium for an arm-carrying underwater robot.

[0005] To achieve the above object, the present invention adopts the following technical solution:

[0006] A method for controlling an underwater robot with an arm, comprising the following steps:

[0007] Step S1: designing a controller of the robotic arm, including:

[0008] Step S11, based on the two-degree-of-freedom mechanical arm dynamics equation, establish the mechanical arm state equation according to the pre-selected state variables;

[0009] Step S12, reconstructing the state equation of the robot arm into a state equation containing a lumped error according to the coupling error of the robot arm;

[0010] Step S13, obtaining the robot arm control rate based on the improved non-singular terminal synovial membrane;

[0011] Step S14, obtaining the execution torque according to the robot arm control rate, and outputting the torque by the execution module to realize the trajectory tracking control of the robot arm;

[0012] Step S2: designing a controller of the robot body, including:

[0013] Step 21, observing the lumped disturbance based on the nonlinear extended state observer to obtain the observed state of the robot body; wherein the lumped disturbance is the external disturbance of the robot body and the interference caused by the operation of the robot arm;

[0014] Step 22, obtaining the robot main body control rate according to the observed state and the integral synovial control strategy;

[0015] Step 23: According to the control rate of the robot body, the actual torque of the robot body is obtained, and the propeller is used to control the navigation of the robot body according to the actual torque of the robot body.

[0016] Preferably, a two-degree-of-freedom robotic arm dynamics equation is established based on the dynamics parameters of the two-joint robotic arm.

[0017] The present invention also provides a control device for an arm-carrying underwater robot, comprising:

[0018] The first processing module is used to design the controller of the robotic arm, including:

[0019] A first calculation unit is used to establish a state equation of the manipulator according to a pre-selected state variable based on a two-degree-of-freedom manipulator dynamics equation;

[0020] The second calculation unit reconstructs the state equation of the manipulator into a state equation containing a lumped error according to the coupling error of the manipulator;

[0021] A third calculation unit is used for obtaining a control rate of the manipulator based on the improved non-singular terminal synovium;

[0022] A first control unit is used to obtain an execution torque according to the control rate of the robot arm, and the execution module outputs the execution torque to realize the trajectory tracking control of the robot arm;

[0023] The second processing module is used to design the controller of the robot body, including:

[0024] The fourth calculation unit is used to observe the lumped disturbance based on the nonlinear extended state observer to obtain the observed state of the robot body; wherein the lumped disturbance is the external disturbance of the robot body and the disturbance caused by the operation of the robot arm;

[0025] A fifth calculation unit, used for obtaining a robot main body control rate according to the observed state and the integral synovial control strategy;

[0026] The second control unit is used to obtain the actual torque of the robot body according to the control rate of the robot body, and use the thruster to control the navigation of the robot body according to the actual torque of the robot body.

[0027] Preferably, the first calculation unit establishes a two-degree-of-freedom robotic arm dynamics equation based on the dynamics parameters of the two-joint robotic arm.

[0028] The present invention also provides an arm-carrying underwater robot control system, comprising: a memory and a processor, wherein the memory stores a computer program run by the processor, and the computer program executes the arm-carrying underwater robot control method when run by the processor.

[0029] The present invention also provides a storage medium, on which a computer program is stored, and the computer program executes the control method of the arm-carrying underwater robot when running.

[0030] The motion control of the robot arm of the present invention adopts improved non-singular terminal synovial film control, and the robot body adopts integral synovial film control of the extended state observer. The trajectory of the robot arm and the robot body can be controlled to complete the specified task. The present invention makes full use of the prior information of the robot body and the robot arm for calculation, and no additional equipment needs to be installed. BRIEF DESCRIPTION OF THE DRAWINGS

[0031] In order to more clearly illustrate the embodiments of the present invention 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 embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on the provided drawings without paying creative work.

[0032] Figure 1 It is a schematic diagram of the structure of the arm-carrying underwater robot;

[0033] Figure 2 Flow chart of a control method of an underwater robot with an arm according to an embodiment of the present invention. DETAILED DESCRIPTION

[0034] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only 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.

[0035] In order to make the above-mentioned objects, features and advantages of the present invention more obvious and easy to understand, the present invention is further described in detail below with reference to the accompanying drawings and specific embodiments.

[0036] Embodiment 1:

[0037] The embodiment of the present invention provides a control method for an underwater robot with an arm, such as Figure 1As shown, the arm-carrying underwater robot includes: a robot body 1 and a robot thruster 2, a first robot arm motor 3, a second robot arm motor 5, a first robot arm connecting rod 4, and a second robot arm connecting rod 6; wherein the robot body 1 includes: a sensor module and a control module, which are used to collect the current position and speed of the robot and calculate the torque required at the current moment; the robot thruster 2 uses a motor to drive a propeller to provide thrust for turning and moving the robot body; the first robot arm motor 3 and the second robot arm motor 5 are installed at the joints of the robot arm, and are used to provide rotational movement of the two-joint robot arm; the first robot arm connecting rod 4 and the second robot arm connecting rod 6 are used to connect the various joints, and in the first robot arm connecting rod 4 and the second robot arm connecting rod 6, the base of the robot arm and the actuator at the end of the robot arm are connected through the connecting rod of the robot arm.

[0038] For the robotic arm part: the underwater two-degree-of-freedom robotic arm includes a control module and an execution module. The sensor in the control module obtains the dynamic parameters of the two-joint robotic arm. According to the dynamic parameters of the two-joint robotic arm, the dynamic equation of the two-degree-of-freedom robotic arm is established; based on the dynamic equation of the two-degree-of-freedom robotic arm, the robotic arm state equation is established according to the pre-selected state variables, and the robotic arm state equation is reconstructed into a state equation containing a lumped error according to the coupling error of the robotic arm; the robotic arm control rate is obtained based on the improved non-singular terminal synovial membrane; the execution torque is obtained by calculating the robotic arm control rate, and the execution torque is output by the execution module to realize the trajectory tracking control of the robotic arm.

[0039] For the robot body: the robot body's sensors measure the robot body's position, velocity and acceleration, and the external disturbances of the robot body and the interference caused by the operation of the robotic arm are regarded as lumped disturbances. The lumped disturbances are observed based on the nonlinear extended state observer to obtain the observed state of the robot body. According to the observed state and the integral sliding film control strategy, the robot body control rate is obtained. The control module obtains the actual torque of the robot body based on the robot body control rate, and uses the thrusters to control the navigation of the robot body according to the actual torque of the robot body.

[0040] like Figure 2 As shown, an embodiment of the present invention provides a control method for an underwater robot with an arm, comprising:

[0041] Step S1: Design the robot controller

[0042] The dynamic equation of the two-degree-of-freedom manipulator is:

[0043]

[0044] where q=[q1,q2] T represents the joint angle vector, represents the joint angular velocity vector, represents the joint angular acceleration vector, M(q)=M0(q)+ΔM(q) is the inertia matrix, where Positive symmetry, satisfying M m ≤||M0(q)||≤M n , ||·|| represents the standard Euclidean norm, M m 、M n is the bound on the Euclidean norm of M0(q), M m ,M n denote the lower and upper bounds respectively, Represents the model error caused by inaccurate modeling; is the centrifugal force-Coriolis force matrix, satisfy C m express With velocity vector The maximum possible norm of the product, is the error caused by the inability to accurately calculate the hydrodynamic parameters, g(q)=g0(q)+Δg(q) is the gravity term, g0(q)=[(m1+m2)r1gcos(q1)+m2r2g1cos(q1+q2),m2r2g1cos(q1+q2)] T Satisfies ||g(q)||≤G M , G M is the maximum limit of the gravity term, is the error caused by the change of the gravity vector g(q) at a certain position q, is an unknown bounded external disturbance, d(t) = [2sin(t) + 0.5sin(200πt), cos(2t) + 0.5sin(200πt)] T , represents the torque vector; m1 is the mass of the first robot arm link, m2 is the mass of the second robot arm link, r1 and r2 are the distances from the two links to their respective joints, J1 and J2 are the moments of inertia of the first and second robot arm links about their center of mass, g1 and g2 represent the influence of gravity on the torque generated by the first and second robot arm links.

[0045] Since the motor and control module of the robot arm have a maximum tolerance, exceeding a certain range may cause hardware damage, which means that the input cannot be increased indefinitely. In addition, to ensure that the control algorithm operates within a safe range and exceeds the instability caused by hardware limitations, the execution module saturation situation τ is designed. max =[150,100] T .

[0046] Considering the uncertainty of the robot arm as a disturbance, the state equation of the robot arm is remodeled as:

[0047]

[0048] in, is the uncertainty error, satisfying the following conditions: b0=12,b1=2.2,b2=2.8.

[0049] Define the expected trajectory q d =[q d1 ,q d2 ] T for q d1 =5-sin(t),q d2 =5+sin(t), it can be seen that the expected trajectory is second-order differentiable.

[0050] Defining Error e=[q1-q d1 ,q2-q d2 ] T

[0051] The conversion function is described as follows:

[0052] sig α (x)=|x| α sign(x)

[0053] Among them, α is a positive parameter,

[0054] In order to reduce the oscillation of the synovial surface during the arrival phase, an improved non-singular terminal synovial surface is designed:

[0055]

[0056] Among them, C1, C2, C3>0 are system parameters, 0<p<1, q>1 are known parameters, and the nonlinear function s p (x) is defined as follows:

[0057]

[0058] Among them, 0<δ≤1, compare |x| p sign(x) and s p (x) It can be seen that s p+1 (x) is a linear function when |x|<δ, and the design δ p x can avoid |x| p The first-order derivative of sign(x) appears p-1, resulting in singularity. p (x) Find the differential with respect to x

[0059]

[0060] Define it as

[0061] Taking the derivative of the sliding film surface S and multiplying it by M0(q) yields:

[0062]

[0063] Will l p Substituting into the above formula,

[0064]

[0065] Based on the stability of the Lyapunov function, the control rate of the robot arm is designed, and the Lyapunov function is given as Taking its derivative with respect to time, we get:

[0066]

[0067] Will Substitution

[0068]

[0069] The robot arm control rate is:

[0070]

[0071] Among them, S p (e), S p (e) = [s p (e1),s p (e2)] T , Sig q (e) = [sig q (e1),sig q (e2)] T , k,ξ, is a given positive parameter, Substituting τ into Solved Depend on It can be seen that V>0, the robot arm is stable;

[0072] At this point, the design of the robot arm controller is complete;

[0073] Step S2: Design the robot body controller

[0074] The dynamic equation of the robot body is:

[0075]

[0076] where η = [x, y, ψ] T Respectively represent the actual position and heading angle of the robot body in the inertial coordinate system, ν = [u, v, r] T is the actual speed of the robot body and the yaw angular velocity, is the transformation matrix between the inertial coordinate system and the earth coordinate system, is the inertia matrix of the robot body, satisfying M, I zz , They are the total mass, x-axis additional damping force parameter, y-axis additional damping force parameter, z-axis moment of inertia, and angular velocity additional damping force parameter. is the Coriolis centripetal force matrix, satisfying is the damping parameter matrix, satisfying d(t) = [d1, d2, d3] represents the time-varying unknown disturbance of the robot arm underwater, τ d =[τ du ,τ dv ,0] T Represents the coupling error between the manipulator and the robot body, satisfying |τ du |<a1,|τ dv |<a2, let a1=0.15cos(t), a2=0.2sin(t); τ2=[τ u ,0,τ r ] T is the robot main body control rate,

[0077] Observer Design Part

[0078] The external disturbance and the coupling disturbance of the manipulator are regarded as lumped disturbances, so the dynamic equation of the robot body can be written as:

[0079]

[0080] in, represents the lumped disturbance, C = J(η)M -1 Represents the controller gain part, based on which the extended state observer is designed:

[0081]

[0082] in, is the position observation error, is the observed value of η, for The observed value of is an estimate of Ξ, and γ1, γ2, and γ3 are positive diagonal matrices.

[0083] Controller design part

[0084] Design expected trajectory x d (t) = cos(t),y d (t) = sin(t)

[0085] The dynamic equation of the robot body is written as:

[0086]

[0087] Defining position observation error

[0088] in, is the velocity observation value, The expression is calculated as follows:

[0089]

[0090] Design expected speed u d ,v d :

[0091]

[0092] Where b1, b2>0 is the controller gain, defining the speed error

[0093] Design of integral synovial surface:

[0094]

[0095] Taking the derivative of s1 and s2 with respect to time, we get

[0096]

[0097] in, is the velocity observation error derivative, is the velocity observation derivative. In order to ensure that the sliding film converges to 0 in a finite time, for

[0098]

[0099] Among them, k1, k2, z1, z2 are positive parameters, and the above equations are solved to get

[0100]

[0101] At this point, the controller design of the robot body is completed.

[0102] Embodiment 2:

[0103] The embodiment of the present invention further provides a control device for an underwater robot with an arm, comprising:

[0104] The first processing module is used to design the controller of the robotic arm, including:

[0105] A first calculation unit is used to establish a state equation of the manipulator according to a pre-selected state variable based on a two-degree-of-freedom manipulator dynamics equation;

[0106] A second calculation unit is used to reconstruct the state equation of the manipulator into a state equation containing a lumped error according to the coupling error of the manipulator;

[0107] A third calculation unit is used for obtaining a control rate of the manipulator based on the improved non-singular terminal synovium;

[0108] A first control unit is used to obtain an execution torque according to the control rate of the robot arm, and the execution module outputs the execution torque to realize the trajectory tracking control of the robot arm;

[0109] The second processing module is used to design the controller of the robot body, including:

[0110] The fourth calculation unit is used to observe the lumped disturbance based on the nonlinear extended state observer to obtain the observed state of the robot body; wherein the lumped disturbance is the external disturbance of the robot body and the disturbance caused by the operation of the robot arm;

[0111] A fifth calculation unit, used for obtaining a robot main body control rate according to the observed state and the integral synovial control strategy;

[0112] The second control unit is used to obtain the actual torque of the robot body according to the control rate of the robot body, and use the thruster to control the navigation of the robot body according to the actual torque of the robot body.

[0113] As an implementation manner of the embodiment of the present invention, the first calculation unit establishes a two-degree-of-freedom robotic arm dynamics equation according to the dynamics parameters of the two-joint robotic arm.

[0114] Embodiment 3:

[0115] An embodiment of the present invention further provides an arm-carrying underwater robot control system, comprising: a memory and a processor, wherein the memory stores a computer program run by the processor, and the computer program executes an arm-carrying robot body control method when run by the processor.

[0116] Embodiment 4:

[0117] An embodiment of the present invention further provides a storage medium, on which a computer program is stored, and the computer program executes the arm-carrying robot main body control method when running.

[0118] The embodiments described above are only descriptions of the preferred embodiments of the present invention and are not intended to limit the scope of the present invention. Without departing from the design spirit of the present invention, various modifications and improvements made to the technical solutions of the present invention by ordinary technicians in this field should all fall within the protection scope determined by the claims of the present invention.

Claims

1. A control method for an underwater robot with an arm, characterized in that: The following steps are involved: Step S1: designing a controller for the robotic arm, including: Step S11, based on the two-degree-of-freedom mechanical arm dynamics equation, establish the mechanical arm state equation according to the pre-selected state variables; Step S12, reconstructing the state equation of the robot arm into a state equation containing a lumped error according to the coupling error of the robot arm; Step S13, obtaining the robot arm control rate based on the improved non-singular terminal synovial membrane; Step S14: obtaining an execution torque according to the robot arm control rate, and outputting the execution torque by the execution module to realize trajectory tracking control of the robot arm; The robot arm control rate is: Among them, S p (e),Sig q (e) S p (e) = [s p (e1),s p (e2)] T , Sig q (e) = [sig q (e1),sig q (e2)] T , k,ξ, is a positive parameter; Step S2: designing a controller of the robot body, including: Step 21, observing the lumped disturbance based on the nonlinear extended state observer to obtain the observed state of the robot body; wherein the lumped disturbance is the external disturbance of the robot body and the interference caused by the operation of the robot arm; Step 22, obtaining the robot main body control rate according to the observed state and the integral synovial control strategy; Step 23: According to the robot main body control rate, the actual torque of the robot main body is obtained, and the propeller is used to control the navigation of the robot main body according to the actual torque of the robot main body; The robot main body control rate is τ2=[τ u ,0,τ r ] T ,in, 2. The control method of the arm-carrying underwater robot according to claim 1, characterized in that: According to the dynamic parameters of the two-joint robot, the dynamic equation of the two-degree-of-freedom robot is established.

3. A control device for an underwater robot with an arm mounted thereon for realizing the control method for an underwater robot with an arm mounted thereon according to claim 1, characterized in that: include: The first processing module is used to design the controller of the robotic arm, including: A first calculation unit is used to establish a state equation of the manipulator according to a pre-selected state variable based on a two-degree-of-freedom manipulator dynamics equation; A second calculation unit is used to reconstruct the state equation of the manipulator into a state equation containing a lumped error according to the coupling error of the manipulator; A third calculation unit is used for obtaining a control rate of the manipulator based on the improved non-singular terminal synovium; A first control unit is used to obtain an execution torque according to the control rate of the robot arm, and the execution module outputs the execution torque to realize the trajectory tracking control of the robot arm; The second processing module is used to design the controller of the robot body, including: The fourth calculation unit is used to observe the lumped disturbance based on the nonlinear extended state observer to obtain the observed state of the robot body; wherein the lumped disturbance is the external disturbance of the robot body and the disturbance caused by the operation of the robot arm; A fifth calculation unit, used for obtaining a robot main body control rate according to the observed state and the integral synovial control strategy; The second control unit is used to obtain the actual torque of the robot body according to the control rate of the robot body, and use the thruster to control the navigation of the robot body according to the actual torque of the robot body.

4. The control device for the underwater robot with an arm according to claim 3, characterized in that: The first calculation unit establishes a two-degree-of-freedom mechanical arm dynamics equation according to the dynamics parameters of the two-joint mechanical arm.

5. A control system for an underwater robot with an arm, comprising: A memory and a processor, wherein the memory stores a computer program executed by the processor, and when the computer program is executed by the processor, the control method of the arm-carrying underwater robot as described in any one of claims 1-2 is executed.

6. A storage medium, characterized in that: The storage medium stores a computer program, which, when running, executes the method for controlling an arm-carrying underwater robot as described in any one of claims 1 to 2.

Images