Underwater Bionic Robot Body and Arm Coordinated Operation Method Based on Dynamic Surface Control

Abstract

The present invention discloses a method for coordinated operation of an underwater bionic robot body and its arm based on dynamic surface control, including the following steps: Step 1, obtain the pose and velocity of the robot body, and design a bionic robot body pose controller based on the dynamic surface control method; Step 2, establish the dynamic model of the underwater manipulator arm, and take the disturbance generated by the movement of the underwater manipulator arm as a feedforward compensation term; add the feedforward compensation term to the output of the body pose controller to obtain the total control force; Step 3, construct a fuzzy rule mapping model for the collision wave, convert the total control force into the motion parameters of the bionic undulating fin thruster, and realize the coordinated operation of the bionic robot body and its arm by controlling the motion of the bionic undulating fin thruster.

Description

Technical Field

[0001] The present invention belongs to the technical field of robot control, and particularly relates to a method for coordinated operation of an underwater bionic robot body and an arm based on dynamic surface control. Background Art

[0002] Underwater operation robots have important application values in aspects such as underwater specimen sampling, lost object salvage, and underwater facility installation. In the applications of these actual scenarios, when an underwater operation robot performs autonomous operations, the manipulator needs to perform operations such as grasping, flipping, transporting, and placing underwater items. Since the system composed of the underwater robot body and the manipulator has redundant degrees of freedom, and the underwater operation robot itself has non-linear and strong coupling characteristics, and because there are complex and uncertain disturbances in the actual underwater environment, there are still many challenges for the underwater operation robot to solve autonomous operation tasks. In order to study the coordinated control between the bionic robot body and the underwater manipulator and achieve autonomous underwater operation in a suspended state, the present invention proposes a method for coordinated operation of an underwater bionic robot body and an arm based on dynamic surface control. Summary of the Invention

[0003] The object of the present invention is to provide a method for coordinated operation of an underwater bionic robot body and an arm based on dynamic surface control, which specifically includes the following steps:

[0004] Step 1: Obtain the pose and velocity of the robot body, and design a bionic robot body pose controller based on the dynamic surface control method;

[0005] Step 2: Based on computational fluid dynamics, obtain the hydrodynamic characteristics of the underwater manipulator, establish a dynamic model of the underwater manipulator, use the disturbance generated by the movement of the underwater manipulator as the feedforward compensation amount of the underwater manipulator joint movement control system, and finally obtain the total control force by adding it to the output of the bionic robot body pose controller, so as to control the manipulator;

[0006] Step 3: Construct a fuzzy rule mapping model for the collision wave, convert the total control force into the motion parameters of the bionic undulating fin thruster, realize the motion control of the bionic undulating fin thruster, and through the motion control of the bionic undulating fin thruster, realize the coordinated operation of the bionic robot body and the arm. Thus, the design of the underwater autonomous operation control method based on dynamic surface control is completed.

[0007] Further, the pose of the robot body in Step 1 refers to the pose vector η of the robot in the world coordinate system, which is expressed as:

[0008] η = [x y ψ] T

[0009] Among them, x represents the longitudinal displacement position of the robot in the world coordinate system, y represents the lateral displacement position of the robot in the world coordinate system, and ψ represents the yaw angle of the robot in the world coordinate system;

[0010] The said velocity refers to the velocity vector of the robot in the world coordinate system It is defined as follows:

[0011]

[0012] Among them, J(η) is the rotation transformation matrix from the body coordinate system of the robot to the world coordinate system, ν = [u v r] T represents the velocity vector of the robot in the body coordinate system, u represents the longitudinal velocity, v represents the lateral velocity, and r represents the yaw velocity;

[0013] Let x1 = η, x2 = v, and design the said body pose controller as:

[0014]

[0015] Among them, M represents the inertia matrix of the robot, M -1 represents the inverse matrix of the inertia matrix, C RB (v) represents the rigid body Coriolis force and centripetal force matrix of the robot, τ h ′(v)x2 represents the hydrodynamic force and moment of the robot excluding the additional inertia matrix, τ r represents the control force and moment vector of the robot, τ r = [τ rx τ ry τ rψ , τ rx represents the longitudinal control force, τ ry represents the lateral control force, τ rψ represents the yaw control moment.

[0016] Define the first error surface z1:

[0017] z1 = x1 - η d (6)

[0018] Among them, η d represents the desired pose of the robot in the world coordinate system;

[0019] Taking the derivative of the above formula, we can get:

[0020]

[0021] Define the second error surface z2:

[0022] z2 = x2 - a2 (8)

[0023] where a2 represents the state variable.

[0024] By defining a low-pass filter with a time constant of e2, a2 can be obtained:

[0025]

[0026] where e2 > 0 represents the time constant of the low-pass filter, and r2 represents the virtual control input of the system:

[0027]

[0028] where k1 > 2 represents the design parameter;

[0029] Taking the derivative of the second error surface z2 gives:

[0030]

[0031] Making:

[0032]

[0033] where k2 > 0 represents the design parameter.

[0034] Finally, the control law obtained is:

[0035]

[0036] Furthermore, the pose η of the robot body described in step 1 refers to the pose x of the robot body obtained by using a differential tracker with median filtering p ;

[0037] The differential tracker with median filtering is expressed as follows:

[0038]

[0039] where a matrix M is defined s0 , a signal window vector for median filtering operation, k represents the current step of the discrete system, and n MF represents the window length of median filtering, x p0 represents the original state of the robot body, x p represents the state of the robot body after median filtering processing, x TD_1 represents the state of the robot system tracked by MF-TD, x TD_2 represents the differential value of the state of the robot body tracked by the differential tracker MF-TD with median filtering, h TD represents the filtering factor, r TD represents the speed factor, T TD represents the tracking step size; sort(·) represents the sorting function, used for the matrix Ms0 Sort the elements in s and place the elements in the matrix M in ascending order. fhan(·) represents the calculation function of the differential tracker TD

[0040] Furthermore, step 1 also includes using the Lyapunov stability criterion to prove the stability of the body pose controller. The specific steps are as follows:

[0041] Select the Lyapunov function:

[0042]

[0043]

[0044] V1 and V2 represent scalar functions respectively. represents the transpose of the first error surface z1. represents the transpose of the second error surface z2, and p2 is the filter error. represents the transpose of p2.

[0045] Finally, it is obtained that V2 is convergent, all signals in the closed-loop system are bounded, and the body pose controller is stable.

[0046] Furthermore, in step 2, establish the dynamic model of the underwater manipulator, and use the disturbance generated by the movement of the underwater manipulator as the feedforward compensation amount for the underwater manipulator joint motion control system;

[0047] The dynamic model of the underwater manipulator is specifically expressed as:

[0048] The relationship between the force and torque between two adjacent joints is as follows:

[0049]

[0050] Define that the i-th coordinate system is the position where the i-th joint is located, where i f i and i t i represent the dynamic force and dynamic torque of the i-th joint in the i-th coordinate system respectively. i+1 f i+1 and i+1 t i+1 represent the dynamic force and dynamic torque of the (i + 1)-th joint in the (i + 1)-th coordinate system respectively. i R i+1 represents the rotation matrix of the (i + 1)-th joint. i F i and i T represent the generalized force and generalized torque of the i-th joint in the i-th coordinate system respectively. iP i,i+1 Represents the transformation vector from joint i to i + 1.

[0051] Among them, the generalized force of the i-th joint in the i-th coordinate system i F i and moment i T are described as:

[0052]

[0053] Among them, i F I,i Represents the inertial force of the i-th joint in the i-th coordinate system;

[0054] i T I,i Represents the moment of the i-th joint in the i-th coordinate system;

[0055] i F H,i and i T H,i Respectively represent the hydrodynamic force and hydrodynamic moment of the i-th joint in the i-th coordinate system;

[0056] i P i,c Represents the transformation vector from the center of gravity of the i-th joint to the i-th joint coordinate system.

[0057] Among them, the inertial force i F I,i and moment i T I,i Are described as:

[0058]

[0059] Among them, M i Represents the mass of the i-th joint, i v c,i Represents the linear velocity of the i-th joint in the i-th coordinate system, I i Represents the moment of inertia of the i-th joint, i w i Represents the angular velocity of the i-th joint in the i-th coordinate system;

[0060] Obtain the feedforward compensation amount τ of the disturbance generated during the movement of the underwater manipulator m , the feedforward compensation amount τ m Includes the generalized force i F i and moment i T.

[0061] The generalized forces include inertial forces, hydrodynamic forces, and frictional forces. Since the frictional forces are relatively small compared to the inertial forces and hydrodynamic forces, the frictional forces are ignored. Therefore, the feedforward compensation amount τ after ignoring the frictional forces m includes inertial forces i F I,i and inertial torques i T I,i , hydrodynamic forces i F H,i and hydrodynamic torques i T H,i .

[0062] Furthermore, in step 3, a fuzzy rule mapping model of the collision wave is constructed, and the total control force τ a is converted into the motion parameters Q of the bionic undulating fin propeller as follows:

[0063] The total control force τ a includes the longitudinal control force τ ax , the lateral control force τ ay and the yaw control torque τ aψ ;

[0064] The membership function is used to perform fuzzy processing on the longitudinal control force τ ax , the lateral control force τ ay and the yaw control torque τ aψ respectively, and they are converted into fuzzy linguistic values. The set of fuzzy linguistic values is {NB, NS, Z, PS, PB}; in the present invention, the membership function adopts a triangular function;

[0065] where NS represents negative small, Z represents zero, PS represents positive small, and PB represents positive large;

[0066] The fuzzy rule mapping model of the collision wave is as follows: The control parameters of the bionic undulating fins on both sides satisfy the fuzzy rule tables, including Table 1 and Table 2. In Table 1, it includes the control parameters of the bionic undulating fins on both sides corresponding to the fuzzy linguistic values of the yaw control torque τ aψ . In Table 2, it includes the control parameters of the bionic undulating fins on both sides corresponding to the fuzzy linguistic values of the longitudinal control force τ ax , the fuzzy linguistic values of the lateral control force τ ay jointly.

[0067] In the angle control stage, find the control parameters of the bionic undulating fins on both sides corresponding to the fuzzy linguistic values of the yaw control torque τ aψ in Table 1;

[0068] In the position control stage, find the control parameters of the bionic undulating fins on both sides corresponding to the fuzzy linguistic values of the longitudinal control force τ ax , the fuzzy linguistic values of the lateral control force τ ayThe control parameters of the bionic undulating fins on both sides corresponding to the fuzzy linguistic values. Through the control of the control parameters of the bionic undulating fins on both sides, the coordinated control operation of the bionic robot body and the arm is realized.

[0069] Advantageous effects: In the coordinated operation of the bionic robot body and the arm based on dynamic surface control described in the present invention, how to obtain a stable and smooth robot state is the most cumbersome part of the research work. When the robot operates in the actual underwater environment, the vision system is often affected by impurities and waves in the water, resulting in recognition errors. This error is specifically reflected in the system state, showing that the obtained robot state contains noise interference in the form of steps or pulses. To obtain the speed state of the robot, a Tracking Differentiator (TD) is usually used to process the pose state of the robot. The traditional tracking differentiator is vulnerable to noise when tracking the system state, especially pulse noise, which will cause great interference to the tracking differentiator. To solve the above problems, the present invention proposes a tracking differentiator with median filtering to suppress the system state noise to obtain a stable and smooth robot motion state, designs a bionic robot body pose controller based on the dynamic surface control method, and proves the stability of the body pose controller using the Lyapunov stability criterion. The system composed of the underwater robot body and the manipulator has redundant degrees of freedom, and the underwater operation robot itself has non-linear and strong coupling characteristics. The movement of the underwater robot manipulator will generate disturbances. The present invention obtains the hydrodynamic characteristics of the underwater manipulator based on computational fluid dynamics, establishes a dynamic model of the underwater manipulator, and uses the disturbance generated by the movement of the underwater manipulator as a feed-forward compensation amount. Brief Description of the Drawings

[0070] Figure 1 is a schematic diagram comparing the tracking effects of TD and MF-TD according to an embodiment of the present invention;

[0071] Figure 2 is a schematic diagram of the coordinate system definition of the undulating fin propulsion underwater operation robot according to an embodiment of the present invention;

[0072] Figure 3 is a coordinate diagram established at each joint position;

[0073] Figure 4 is a schematic diagram of the water force of the waist joint under different rotation directions and rotation speeds;

[0074] Figure 5 is a schematic diagram of the water force on the manipulator when the waist joint rotates according to an embodiment of the present invention;

[0075] Figure 6 is a schematic diagram of the collision wave of the bionic undulating fin at t = 0 according to an embodiment of the present invention;

[0076] Figure 7 Membership function graph of the fuzzy inference system for the coordinated control of the bionic robot body and the underwater manipulator;

[0077] Figure 8 It is a schematic diagram of the first sequence of video screenshots of the underwater autonomous assembly operation experiment according to an embodiment of the present invention;

[0078] Figure 9 It is a schematic diagram of the second sequence of video screenshots of the underwater autonomous assembly operation experiment according to an embodiment of the present invention; Detailed implementation manners

[0079] Step 1: A tracking differentiator with median filtering is proposed to obtain the pose and velocity of the robot body, and a pose controller for the bionic robot body is designed based on the dynamic surface control method; and the stability of the control method is proved using the Lyapunov stability criterion; specifically including:

[0080] Since the traditional tracking differentiator is vulnerable to noise when tracking the system state, especially impulse noise will have a greater interference on the tracking differentiator. The present invention proposes a tracking differentiator with median filtering (MF-TD). The MF-TD is used to filter the pose state of the robot, which can effectively suppress noise and smooth the drastic fluctuations of the robot state, so as to obtain the actual pose η and actual velocity of the robot body

[0081] The tracking differentiator with median filtering is expressed as follows:

[0082]

[0083] where k represents the current step of the discrete system, n MF represents the window length of the median filtering, x p0 represents the original system state, x p represents the system state after median filtering, x TD_1 represents the system state tracked by MF-TD, x TD_2 represents the differential value of the system state tracked by MF-TD, h TD represents the filtering factor, r TD represents the velocity factor, T TD represents the tracking step; sort(·) represents the sorting function, which can sort the elements in the matrix M s0 and place the elements in ascending order in the matrix M s , and fhan(·) represents the calculation function of TD.

[0084] Figure 1 Describes the comparison of the tracking effects of the yaw angle and yaw angular velocity obtained by the robot vision system using TD (Differential Tracker) and MF-TD. Figure 1 In (a), it is the tracking curve of the original yaw angle. It can be seen that there is obvious pulse noise in the original robot body state between 30 - 40 s. At this time, there is obvious jitter in the tracking of the TD for the system state, while the MF-TD adopted in the present invention has a better noise suppression effect, and the tracking system state curve is smoother. Figure 1 In (b), it is the tracking curve of the differential value of the original yaw angle. It can be seen that MF-TD has better noise suppression ability than TD.

[0085] The motion of the undulating fin propelled underwater operation robot only considers three degrees of freedom: lateral translation, longitudinal translation, and yaw, as Figure 2 shown, where O E -X E Y E represents the world coordinate system, and O V -X V Y V represents the body-fixed coordinate system of the undulating fin propelled underwater operation robot. Therefore, the velocity vector of the robot in the world coordinate system is defined as follows:

[0086]

[0087] where J(η) is the rotation transformation matrix from the body-fixed coordinate system of the robot to the world coordinate system, ν = [u v r] T represents the velocity vector of the robot in the body-fixed coordinate system, u represents the longitudinal translation velocity, v represents the lateral translation velocity, and r represents the yaw velocity; η = [x y ψ] T represents the pose vector of the robot in the world coordinate system.

[0088] According to Newton-Euler dynamics, the robot dynamics model can be expressed as:

[0089]

[0090] where M RB represents the rigid body inertia matrix of the robot, C RB (v) represents the rigid body Coriolis force and centripetal force matrix of the robot, v represents the velocity vector of the robot in the body-fixed coordinate system, represents the acceleration vector of the robot in the body-fixed coordinate system, represents the hydrodynamic force and moment vector, τ r = [τ rx τ ry τ rψrepresents the control force and torque vector of the robot.

[0091] Furthermore, it can be obtained that:

[0092]

[0093] where M represents the inertia matrix of the robot, and τ′ h (v)v represents the hydrodynamic force and torque of the robot excluding the additional inertia matrix.

[0094] The design of the body pose controller is as follows:

[0095] Select x1 = η, x2 = v. According to the previous equations (2) and (4), we can obtain:

[0096]

[0097] Define the first error surface z1:

[0098] z1 = x1 - η d (6)

[0099] where η d represents the desired pose of the robot in the world coordinate system.

[0100] Taking the derivative of the above equation, we can obtain:

[0101]

[0102] Define the second error surface z2:

[0103] z2 = x2 - a2 (8)

[0104] where a2 represents the state variable.

[0105] By defining a low-pass filter with a time constant of e2, a2 can be obtained:

[0106]

[0107] where e2 > 0 represents the time constant of the low-pass filter, and r2 represents the virtual control input of the system:

[0108]

[0109] where k1 > 2 represents the design parameter.

[0110] According to equations (5) and (7), taking the derivative of the second error surface z2 gives:

[0111]

[0112] Let:

[0113]

[0114] where \(k_2>0\) represents a design parameter.

[0115] Finally, the control law can be obtained as:

[0116]

[0117] C RB (v) represents the rigid body Coriolis force and centripetal force matrix of the robot, M represents the inertia matrix of the robot, \(a_2\) represents the state variable of the system, \(x_2\) represents the velocity vector of the robot, \(\tau\) h ′(v)\(x_2\) represents the hydrodynamic force and moment of the robot excluding the additional inertia matrix, and \(k_2>0\) represents a design parameter.

[0118] Combining the position information \(\eta\) of the end effector calculated by the underwater manipulator kinematics m and the velocity of the end effector to obtain the feedback quantity of the body pose controller. Subtracting the feedback quantity \(\eta\) m from the desired pose \(\eta\) of the robot in the world coordinate system d to obtain the first error surface \(z_1\). Taking the first error surface \(z_1\) and the derivative of the first error surface \(z_1\) as the input of the body pose controller, and finally obtaining the control law \(\tau\) through the dynamic surface controller r to complete the design of the body pose controller.

[0119] Conduct a stability analysis of the bionic robot body pose controller through the Lyapunov stability criterion, and select a Lyapunov function for verification:

[0120]

[0121]

[0122] \(V_1\) and \(V_2\) represent scalar functions, represents the transpose of the first error surface \(z_1\), represents the transpose of the second error surface \(z_2\), \(p_2\) is the filter error, represents the transpose of \(p_2\).

[0123] Finally, it is obtained that \(V_2\) is convergent, all signals in the closed-loop system are bounded, and the system is stable.

[0124] Step 2: Based on computational fluid dynamics, the hydrodynamic characteristics of the underwater manipulator are obtained, and the dynamic model of the underwater manipulator is established. The disturbances generated by the movement of the underwater manipulator are used as the feedforward compensation quantity for the joint motion control system of the underwater manipulator; specifically including:

[0125] In the dynamic modeling of a robotic arm, the robotic arm is usually regarded as a multi-link mechanism. Therefore, the additional coupling effects caused by the joint movements of the robotic arm can be transmitted to the robot body through the chain rule. The relationship between the forces and torques between two adjacent joints is as follows:

[0126]

[0127] Define that the i-th coordinate system is the position where the i-th joint is located, where i f i and i t i respectively represent the dynamic force and dynamic torque of the i-th joint in the i-th coordinate system. i+1 f i+1 and i+1 t i+1 respectively represent the dynamic force and dynamic torque of the (i + 1)-th joint in the (i + 1)-th coordinate system. i R i+1 represents the rotation matrix of the (i + 1)-th joint. i F i and i T respectively represent the generalized force and generalized torque of the i-th joint in the i-th coordinate system. i P i,i+1 represents the transformation vector from joint i to i + 1.

[0128] The generalized force includes inertial force, hydrodynamic force, and frictional force. Since the formation reason of the frictional force is relatively complex and relatively small, the frictional force is ignored in this section. In summary, the generalized force i F i and torque i T can be described as:

[0129]

[0130] where, i F I,i represents the inertial force of the i-th joint in the i-th coordinate system;

[0131] i T I,i represents the inertial torque of the i-th joint in the i-th coordinate system;

[0132] i F H,i and i T H,i respectively represent the hydrodynamic force and hydrodynamic torque of the i-th joint in the i-th coordinate system;

[0133] i P i,cRepresents the transformation vector from the center of gravity of the $i$-th joint to the coordinate system of the $i$-th joint.

[0134] Inertial force i F I,i And moment i T I,i Can be described as:

[0135]

[0136] Where, $M$ i Represents the mass of the $i$-th joint, i $v$ c,i Represents the linear velocity of the $i$-th joint in the $i$-th coordinate system, $I$ i Represents the moment of inertia of the $i$-th joint, i $w$ i Represents the angular velocity of the $i$-th joint in the $i$-th coordinate system.

[0137] According to the above equations (16), (17), and (18), the feedforward compensation amount $\tau$ of the disturbance generated during the movement of the underwater manipulator can finally be obtained. The feedforward compensation amount of the disturbance generated during the movement of the underwater manipulator includes the forces and moments of hydrodynamic force and inertial force. By adding it to the output $\tau$ m of the pose controller of the bionic robot body, the total control force $\tau$ r can finally be obtained. a .

[0138] The water force acting on the manipulator during movement is relatively complex. Using computational fluid dynamics technology to obtain its water force is a practical and convenient method. Considering that the underwater robot is in a state of balance between gravity and buoyancy in the vertical direction, the present invention only considers the hydrodynamic forces in the $X$ and $Y$ directions. The manipulator of the undulating fin propulsion underwater operation robot has 5 joints, namely the waist joint, shoulder joint, elbow joint, wrist pitch joint, and wrist rotation joint. As Figure 3 shown, $e$, $\theta_2$, $\theta_3$, $\theta_4$, and $\theta_5$ are the rotation angles of the waist joint, shoulder joint, elbow joint, wrist pitch joint, and wrist rotation joint respectively. The water force acting on the underwater manipulator joint movement mainly depends on the rotation angle, rotation direction, and rotation speed of the joint. By setting different rotation directions and rotation speeds for each joint of the underwater manipulator in FLUENT, the water force model of each joint of the underwater manipulator can be simulated.

[0139] Figure 4 Shows the water force conditions of the waist joint under different rotation directions and rotation speeds. In Figure 4 the (a) and (b), the rotation direction of the waist joint is positive, and the rotation angle is from $0^{\circ}$ to $360^{\circ}$. In Figure 4 (c) and (d), the rotation direction of the waist joint is positive, and the rotation angle is from $360^{\circ}$ to $0^{\circ}$. FromFigure 4 It can be seen that as the rotation speed of the waist joint increases from 5° / s to 50° / s, the maximum value of the water force increases from 0.4 N to 10.6 N. From the simulation results, it can be known that the range of the water force acting on the manipulator during the rotation of the waist joint is in the interval [-10.6, 10.6] N.

[0140] Figure 5 The water force conditions of the above joints at different rotation speeds are shown. At Figure 5 (a), as the shoulder joint rotates from -20° to 20°, the water force in the X direction is always negative and its absolute value continuously increases. At Figure 5 (b), as the elbow joint rotates from -200° to -90°, the water force in the X direction first decreases from a positive value to 0 and then becomes negative and its absolute value continuously increases; as the elbow joint rotates from -90° to 20°, the water force in the X direction first decreases from the negative maximum value to 0 and then becomes positive and its absolute value continuously increases. At Figure 5 (c), as the wrist pitch joint rotates from -90° to 0°, the water force in the X direction decreases from the negative maximum value to 0; as the wrist pitch joint rotates from 0° to 90°, the water force in the X direction increases from 0 to the positive maximum value. At Figure 5 (d), the water force in the Y direction acting on the wrist rotation joint during rotation has a Figure 5 similar trend to (b), but the absolute value of the water force is smaller.

[0141] The absolute values of the water forces of the above four joints all increase with the increase of the joint rotation speed, and the ranges of the water forces during the rotation of the shoulder joint, elbow joint, wrist pitch joint and wrist rotation joint are in the intervals [-9.8, 9.8] N, [-8.3, 8.3] N, [-2.4, 2.4] N, [-0.2, 0.2] N respectively. So far, the water forces acting on the underwater manipulator during the rotation of each joint have been obtained.

[0142] When the actual pose η and actual speed of the bionic robot body meet the grasping or placing conditions (for example, the robot grabs or places building blocks underwater through the manipulator), through the inverse kinematics of the underwater manipulator, the rotation angles θ of each joint of the underwater manipulator can be obtained according to the desired end effector pose i so as to control the joints of the underwater manipulator. At this time, according to the dynamic model of the underwater manipulator of the present invention, the disturbance τ generated by the underwater manipulator joints during rotation to the bionic robot body can be obtained m and used as the feed-forward compensation amount of the human body pose controller. By adding it to the output τ r of the bionic robot body pose controller, the total control force τ a can be finally obtained.

[0143] Step 3, a fuzzy rule mapping model of the collision wave is constructed, and the total control force τ a is converted into the motion parameters of the bionic undulating fin thruster, specifically including:

[0144] The collision wave of the bionic undulating fin is composed of two sinusoidal waves propagating in opposite directions, and the meeting point of the two sinusoidal waves is the collision position. The description function of the collision wave is as follows:

[0145]

[0146] where β icps (t, x f ) represents the angular value of the bionic undulating fin at position x at time t f , A icps represents the amplitude of the undulation angle, f icps represents the frequency of the collision wave, λ icps represents the wavelength of the collision wave, m icps represents the collision position.

[0147] Figure 6 Fig. shows the schematic diagram of the bionic undulating fin generating a collision wave. By selecting appropriate control parameters, the bionic undulating fin can generate continuously variable longitudinal and lateral forces when moving with the collision wave.

[0148] Design the fuzzy rule mapping model of the collision wave. In the coordinated control framework of the bionic robot body and the underwater manipulator designed in the present invention, it is necessary to control three states: the yaw angle ψ, the longitudinal position x, and the lateral position y. Since the bionic undulating fin propels the underwater operation robot is an underactuated system, the pose controller of the robot body in the present invention is divided into two stages: angle control and position control. In the angle control stage, the control parameters of the bionic undulating fins on both sides satisfy the fuzzy rule table 1. In the position control stage, the control parameters of the bionic undulating fins on both sides satisfy the fuzzy rule table 2. In Table 1 and Table 2, NB represents negative large, NS represents negative small, Z represents zero, PS represents positive small, and PB represents positive large.

[0149] Table 1

[0150]

[0151] Table 2

[0152]

[0153] In Table 1 and Table 2, the longitudinal control force satisfies τ ax ∈[-8, 8] N, the lateral control force satisfies τ ay ∈[-6, 6] N, the yaw control torque satisfies τ aψ ∈[-10, 10] N·m, and the control parameter Q of the bionic undulating fin:

[0154]

[0155] Among them, the first row and the second row are the control parameters of the bionic undulating fin thrusters on the left and right sides respectively. The control parameters are, in sequence: the frequency of the collision wave, the collision position, the amplitude, and the wavelength.

[0156] f icps_L represents the frequency of the left collision wave, m icps_L represents the left collision position, A icps_L represents the left amplitude, λ icps_L represents the left wavelength;

[0157] f icps_R represents the frequency of the right collision wave, m icps_R represents the right collision position, A icps_R represents the right amplitude, λ icps_R represents the right wavelength;

[0158] Among them, the frequency of the left collision wave of the bionic undulating fin satisfies f icps_L ∈[0, 1.8] Hz, and the frequency of the right collision wave satisfies f icps_R ∈[0, 1.8] Hz;

[0159] It should be noted specifically that in Tables 1 and 2, the frequency parameters of the collision waves are expressed in fuzzy language, while the remaining control parameters are all described using specific numerical values. The longitudinal movement control force τ ax , the lateral movement control force τ ay and the yaw control moment τ aψ have a fuzzy language value set of {NB, NS, Z, PS, PB}. The fuzzy language values of the frequency f icps of the bionic undulating fin are all {Z, PS, PM, PB}, and their membership functions are as Figure 7 shown.

[0160] Import the above fuzzy rules into the Fuzzy Logic Designer module of Matlab, and finally obtain a fuzzy rule mapping model. The inputs of the model are the longitudinal movement control force τ ax , the lateral movement control force τ ay and the yaw control moment τ aψ , and the output of the model is all the control parameters Q of the bionic undulating fin. Thus, the design of the underwater autonomous operation control method based on dynamic surface control is completed.

[0161] In order to verify the effectiveness of the coordinated control method of the bionic robot body and the underwater manipulator, an underwater autonomous assembly operation experiment was carried out in an indoor pool, Figure 8 and Figure 9Presents a sequence of video screenshots of the underwater autonomous assembly operation experiment. The entire experimental process includes the stages of autonomous grasping, autonomous transportation, and autonomous construction of 4 building blocks (IDs: 17, 18, 19, 20).

[0162] In Figure 8 (a)-(c), the undulating fin propulsion underwater operation robot conducts autonomous grasping operation on the building block with ID 17. Among them Figure 8 (a) shows that the robot adjusts its pose by controlling the bionic undulating fins according to the recognized pose information of the building block Figure 8 (b) shows that the robotic arm is conducting grasping operation on the building block Figure 8 (c) shows that the robotic arm has completed the grasping operation on the building block. In Figure 8 (d)-(e), the undulating fin propulsion underwater operation robot conducts autonomous transportation operation on the building block with ID 17. The robot adjusts the frequencies of the bionic undulating fins on both sides according to the recognized pose information of the tracking path to achieve tracking control of the path, and transports the building block from area A to area B. In Figure 8 (f)-(h), the undulating fin propulsion underwater operation robot conducts autonomous construction operation on the building block with ID 17. Among them Figure 8 (f) shows that the robot adjusts its pose by controlling the bionic undulating fins according to the recognized pose information of the marker board Figure 8 (g) shows that the robotic arm is conducting construction operation on the building block Figure 8 (h) shows that the robotic arm has completed the construction operation on the building block

[0163] In addition Figure 8 (i)-(p), Figure 9 (a)-(h) and Figure 9 (i)-(p) respectively show that the undulating fin propulsion underwater operation robot conducts autonomous grasping, autonomous transportation, and autonomous construction operations on the building blocks with IDs 18, 19, and 20. During the entire underwater autonomous assembly operation experiment, the bionic propulsion underwater operation robot has successfully completed the grasping process of 4 building blocks, can transport the building blocks to the miscellaneous area through path tracking, and can place the building blocks at the pre-designed designated positions with a small error, and finally completed the building block construction operation. As can be seen from Figure 9 (p), the finally constructed shape of the building blocks is the pre-designed "square" shape, meeting the task requirements of the building block assembly autonomous operation

[0164] Aiming at the problems of object grasping and construction of bionic propulsion underwater operation robots, the present invention proposes a bionic robot body and water. The method includes three parts: a posture controller for the bionic robot body based on dynamic surface control, a feedforward compensation for the disturbance of the underwater manipulator, and a collision wave parameter mapping. First, a differential tracker with median filtering is proposed to suppress the system state noise to obtain a stable and smooth robot motion state. Based on the dynamic surface control method, a posture controller for the bionic robot body is designed, and the stability of the body posture controller is proved using the Lyapunov stability criterion. Second, based on computational fluid dynamics, the hydrodynamic characteristics of the underwater manipulator are obtained, a dynamic model of the underwater manipulator is established, and the disturbance generated by the movement of the underwater manipulator is used as a feedforward compensation for the body posture controller. Third, the propulsion mechanism of the collision wave is analyzed, a fuzzy rule mapping model of the collision wave is constructed, and the motion control of the bionic undulating fin thruster is realized. Finally, an underwater autonomous operation experiment is carried out, and the experimental results verify the effectiveness of the coordinated operation method of the present invention.

Claims

1. A method for coordinated operation of an underwater bionic robot body and an arm based on dynamic surface control, characterized in that: The steps include: Step 1, obtaining the posture and speed of the robot body, and designing the posture controller of the bionic robot body based on the dynamic surface control method; Step 2: Establish the dynamic model of the underwater manipulator and use the disturbance generated by the movement of the underwater manipulator as the feedforward compensation τ m ; The dynamic model of the underwater manipulator is specifically expressed as: The relationship between the force and torque between two adjacent joints is as follows: Definition: The i-th coordinate system is the location of the i-th joint, where i f i and i t i They represent the dynamic force and dynamic torque of the ith joint in the ith coordinate system, i+1 f i+1 and i+1 t i+1 They represent the dynamic force and dynamic torque of the i+1th joint in the i+1th coordinate system, respectively. i R i+1 represents the rotation matrix of the i+1th joint, i F i and i T represents the generalized force and generalized moment of the ith joint in the ith coordinate system, i P i,i+1 Represents the transformation vector from joint i to i+1; Among them, the generalized force of the i-th joint in the i-th coordinate system is i F i and torque i T is described as: in, i F I,i represents the inertia force of the i-th joint in the i-th coordinate system; i T I,i represents the torque of the i-th joint in the i-th coordinate system; i F H,i and i T H,i denote the hydrodynamic force and hydrodynamic moment of the ith joint in the ith coordinate system, respectively; i P i,c represents the transformation vector from the center of gravity of the i-th joint to the i-th joint coordinate system; Among them, the inertia force i F I,i and torque i T I,i Described as: Among them, M i represents the mass of the i-th joint, i v c,i represents the linear velocity of the ith joint in the ith coordinate system, I i represents the moment of inertia of the ith joint, i w i represents the angular velocity of the i-th joint in the i-th coordinate system; Get the feedforward compensation τ of the disturbance generated when the underwater manipulator moves m , the feedforward compensation τ m Including general force i F i and torque i T; the feedforward compensation τ m Adding the output of the body posture controller to obtain a total control force; Step 3, constructing a fuzzy rule mapping model of the collision wave, converting the total control force into the motion parameters of the bionic undulating fin thruster, and realizing the motion control of the bionic undulating fin thruster.

2. The method for coordinated operation of an underwater bionic robot body and an arm based on dynamic surface control according to claim 1, characterized in that: The posture of the robot body in step 1 refers to the posture vector η of the robot in the world coordinate system, which is expressed as: η=[xy ψ] T Where x represents the longitudinal position of the robot in the world coordinate system, y represents the lateral position of the robot in the world coordinate system, and ψ represents the bow angle of the robot in the world coordinate system. The speed refers to the velocity vector η of the robot in the world coordinate system, which is defined as follows: Where J(η) is the rotation transformation matrix from the robot body coordinate system to the world coordinate system, ν = [uvr] T represents the velocity vector of the robot in the body coordinate system, u represents the longitudinal velocity, v represents the lateral velocity, and r represents the bowing velocity; Let x1 = η, x2 = v, and design the body posture controller as: Where M represents the inertia matrix of the robot, M -1 represents the inverse matrix of the inertia matrix, C RB (v) represents the rigid body Coriolis force and centripetal force matrix of the robot, τ h ′(v)x2 represents the hydrodynamic force and torque of the robot without the additional inertia matrix, τ r represents the control force and torque vector of the robot, τ r =[τ rx τ ry τ rψ ],τ rx represents the longitudinal control force, τ ry represents the lateral control force, τ rψ It represents the bow control torque; Define the first error surface z1: z1=x1-η d (6) Among them, η d Represents the desired position of the robot in the world coordinate system; Taking the derivative of the above formula, we can get: Define the second error surface z2: z2=x2-a2 (8) Where a2 represents the state variable; By defining a low-pass filter with a time constant of e2, we get a2: Among them, e2>0 represents the time constant of the low-pass filter, and r2 represents the virtual control input of the system: Where k1>2 represents the design parameter; Taking the derivative of the second error surface z2, we get: make: Where k2>0 represents the design parameter; The final control law is:

3. The method for coordinated operation of an underwater bionic robot body and an arm based on dynamic surface control according to claim 2, characterized in that: The position and posture η of the robot body in step 1 refers to the position and posture x of the robot body obtained by using a differential tracker with median filtering. p ; The differential tracker with median filtering is expressed as follows: Among them, define a matrix M s0 , the signal window vector for median filtering operation, k represents the current step of the discrete system, n MF Indicates the window length of the median filter, x p0 represents the original robot body state, x p represents the robot body state after median filtering, x TD_1 represents the robot system state tracked by MF-TD, x TD_2 represents the differential value of the robot body state tracked by the differential tracker MF-TD with median filtering, h TD represents the filtering factor, r TD represents the speed factor, T TD represents the tracking step size; sort(·) represents the sorting function used to sort the matrix M s0 Sort the elements in and place them in the matrix M in ascending order s In , fhan(·) represents the calculation function of the differential tracker TD.

4. The method for coordinated operation of an underwater bionic robot body and an arm based on dynamic surface control according to claim 1, characterized in that: Step 1 also includes using the Lyapunov stability criterion to prove the stability of the body posture controller. The specific steps are as follows: Choose a Lyapunov function: V1 and V2 represent scalar functions, represents the transpose of the first error surface z1, represents the transpose of the second error surface z2, p2 is the filter error, represents the transpose of p2; Finally, V2 is converged, all signals in the closed-loop system are bounded, and the body posture controller is stable.

5. The method for coordinated operation of an underwater bionic robot body and an arm based on dynamic surface control according to claim 1, characterized in that: The generalized force includes inertial force, hydrodynamic force and friction force. Since the friction force is smaller than the inertial force and hydrodynamic force, the friction force is ignored. The feedforward compensation amount τ m Including inertia i F I,i , Moment of inertia i T I,i , Hydrodynamics i F H,i and hydrodynamic torque i T H,i .

6. The method for coordinated operation of an underwater bionic robot body and an arm based on dynamic surface control according to claim 1, characterized in that: In step 3, a fuzzy rule mapping model of collision waves is constructed, and the total control force τ a Transformed into the motion parameter Q of the bionic wave fin thruster, it is as follows: Total control force τ a Including longitudinal control force τ ax , lateral control force τ ay and the bow control torque τ aψ ; Control force for longitudinal movement τ ax , lateral control force τ ay and the bow control torque τ aψ Fuzzy processing is performed respectively and converted into fuzzy language values. The set of fuzzy language values ​​is {NB, NS, Z, PS, PB}; Among them, NS means negative small, Z means zero, PS means positive small, and PB means positive large; The fuzzy rule mapping model of the collision wave is: the control parameters of the bionic wave fins on both sides satisfy the fuzzy rule table, including Table 1 and Table 2. Table 1 includes the bow control torque τ aψ The control parameters of the bionic wave fins on both sides corresponding to the fuzzy language value of , including the longitudinal control force τ in Table 2 ax The fuzzy language value and lateral control force τ ay The fuzzy language values ​​of the control parameters of the bionic wave fins on both sides correspond to each other; The control of the bionic wave fin thruster is divided into two stages: angle control and position control; In the angle control stage, find the bow control torque τ in Table 1 aψ The fuzzy language value corresponds to the control parameters of the bionic wave fins on both sides; In the position control stage, find the longitudinal control force τ in Table 2 ax The fuzzy language value and lateral control force τ ay The fuzzy language values ​​correspond to the control parameters of the bionic undulating fins on both sides.

7. The method for coordinated operation of an underwater bionic robot body and an arm based on dynamic surface control according to claim 6, characterized in that: Specifically: the membership function is used to control the longitudinal control force τ ax , lateral control force τ ay and the bow control torque τ aψ Fuzzy processing is performed separately.

8. The method for coordinated operation of an underwater bionic robot body and an arm based on dynamic surface control according to claim 7, characterized in that: The membership function is a triangular function.

Images