Underwater bionic robot body and arm coordinated operation method based on dynamic surface control

Abstract

The invention discloses an underwater bionic robot body and arm coordinated operation method based on dynamic surface control, which comprises the following steps: step 1, acquiring the pose and speed of a robot body, and designing a bionic robot body pose controller based on a dynamic surface control method; step 2, establishing a dynamic model of the underwater mechanical arm, and taking disturbance generated by movement of the underwater mechanical arm as a feedforward compensation item; adding the feedforward compensation item and the output of the body pose controller to obtain a total control force; and 3, a fuzzy rule mapping model of collision waves is constructed, the total control force is converted into motion parameters of the bionic undulating fin propeller, and through motion control over the bionic undulating fin propeller, coordinated operation of the bionic robot body and the arms is achieved.

Description

Technical Field

[0001] The invention belongs to the technical field of robot-machine control, and in particular 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 value in underwater specimen sampling, lost object salvage, underwater facility installation, etc. In the application of these practical scenarios, when the underwater operation robot performs autonomous operation, the robotic arm needs to grab, flip, carry, place and other operations on underwater objects. Because the system composed of the underwater robot body and the robotic arm has redundant degrees of freedom, and the underwater operation robot itself has nonlinear and strong coupling characteristics, and because the actual underwater environment has complex uncertain disturbances, there are still many challenges for underwater operation robots to solve autonomous operation tasks. In order to study the coordinated control between the bionic robot body and the underwater robotic arm, and realize underwater autonomous operation in a suspended state, the present invention proposes a method for coordinated operation of the underwater bionic robot body and arm based on dynamic surface control. Summary of the invention

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

[0004] 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;

[0005] 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 disturbance generated by the movement of the underwater manipulator is used as a feedforward compensation item of the underwater manipulator joint motion control system. By adding it to the output of the bionic robot body posture controller, the total control force can be finally obtained, so that the manipulator can be controlled;

[0006] Step 3, a fuzzy rule mapping model of collision waves was constructed, the total control force was converted into the motion parameters of the bionic wave fin thruster, the motion control of the bionic wave fin thruster was realized, and the coordinated operation of the bionic robot body and arm was realized through the motion control of the bionic wave fin thruster. So far, the design of the underwater autonomous operation control method based on dynamic surface control has been completed.

[0007] Furthermore, 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:

[0008] η=[xyy] T

[0009] 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 y represents the bow angle of the robot in the world coordinate system;

[0010] The speed refers to the velocity vector of the robot in the world coordinate system The definition is as follows:

[0011]

[0012] 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;

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

[0014]

[0015] 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 robot's control force and torque vector, τ r =[τ rx τ ry τ ry ],τ rx represents the longitudinal control force, τ ry represents the lateral control force, τ ry Indicates the bow control torque.

[0016] Define the first error surface z1:

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

[0018] Among them, η d Represents the desired position 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, we can obtain a2:

[0025]

[0026] Among them, 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, we get:

[0030]

[0031] make:

[0032]

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

[0034] The final control law is:

[0035]

[0036] Furthermore, the position η of the robot body in step 1 refers to the position 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] 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 Ms0 Sort the elements in and place them in the matrix M in ascending order s In the equation, 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 posture controller, and the specific steps are as follows:

[0041] Choose a Lyapunov function:

[0042]

[0043] 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.

[0044] Finally, V2 is converged, all signals in the closed-loop system are bounded, and the body posture controller is stable.

[0045] Furthermore, in step 2, 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 item of the underwater manipulator joint motion control system;

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

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

[0048]

[0049] 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.

[0050] 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:

[0051]

[0052] in, i F I,i represents the inertia force of the i-th joint in the i-th coordinate system;

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

[0054] 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;

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

[0056] Among them, the inertial force i F I,i and torque i T I,i Described as:

[0057]

[0058] 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;

[0059] 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.

[0060] 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. Therefore, the feedforward compensation after ignoring the friction force is τ mIncluding inertia i F I,i , Moment of inertia i T I,i , Hydrodynamics i F H,i and hydrodynamic torque i T H,i .

[0061] Furthermore, in step 3, a fuzzy rule mapping model of the collision wave is constructed, and the total control force τ a Transformed into the motion parameter Q of the bionic wave fin thruster, it is as follows:

[0062] Total control force τ a Including longitudinal control force τ ax , lateral control force τ ay and the bow control torque τ ay ;

[0063] The membership function is used to calculate the longitudinal control force τ ax , lateral control force τ ay and the bow control torque τ ay The fuzzy processing is performed respectively and converted into fuzzy language values, and the set of fuzzy language values ​​is {NB, NS, Z, PS, PB}; the membership function in the present invention adopts a triangular function;

[0064] Among them, NS means negative small, Z means zero, PS means positive small, and PB means positive large;

[0065] 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, two tables, Table 1 includes the bow control torque τ ay 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 ​​correspond to the control parameters of the bionic undulating fins on both sides.

[0066] In the angle control stage, find the bow control torque τ in Table 1 ay The fuzzy language value corresponds to the control parameters of the bionic wave fins on both sides;

[0067] 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 ​​of the two sides correspond to the control parameters of the bionic undulating fins. By controlling the control parameters of the bionic undulating fins on both sides, the coordinated control operation of the bionic robot body and the arm can be achieved.

[0068] Beneficial effects: In the coordinated work of the bionic robot body and arm based on dynamic surface control described in the present invention, how to obtain a stable and smooth robot state is the most tedious part of the research work. When the robot operates in an actual underwater environment, the visual system is often affected by impurities, waves, etc. in the water and produces recognition errors. This error is specifically reflected in the system state, which is manifested as the acquired robot state containing step or pulse noise interference. To obtain the speed state of the robot, a differential tracker (Tracking Differentiator, TD) is usually required to process the posture state of the robot. Traditional differential trackers are more susceptible to noise when tracking the system state, especially pulse noise will cause greater interference to the differential tracker. In response to the above problems, the present invention proposes a differential tracker with median filtering to suppress the system state noise to obtain a stable and smooth robot motion state, designs a bionic machine body posture controller based on the dynamic surface control method, and uses the Lyapunov stability criterion to prove the stability of the body posture controller. The system composed of the underwater robot body and the robotic arm has redundant degrees of freedom, and the underwater working robot itself has nonlinear and strong coupling characteristics. The movement of the underwater robot robotic arm will cause disturbances. The present invention derives the hydrodynamic characteristics of the underwater robotic arm based on computational fluid dynamics, establishes a dynamic model of the underwater robotic arm, and uses the disturbance caused by the movement of the underwater robotic arm as a feedforward compensation term. BRIEF DESCRIPTION OF THE DRAWINGS

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

[0070] Figure 2 is a schematic diagram of a coordinate system definition of an undulating fin-propelled underwater working robot according to an embodiment of the present invention;

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

[0072] Figure 4 It is a schematic diagram showing the water force on the waist joint under different rotation directions and rotation speeds;

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

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

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

[0076] Figure 8 is a schematic diagram of a video screenshot sequence 1 of an underwater autonomous assembly operation experiment according to an embodiment of the present invention;

[0077] Fig. 9 is a schematic diagram of a video screenshot sequence 2 of an underwater autonomous assembly operation experiment according to an embodiment of the present invention; DETAILED DESCRIPTION

[0078] Step 1: A differential tracker with median filtering is proposed to obtain the posture and velocity of the robot body, and a bionic robot body posture controller is designed based on the dynamic surface control method; and the stability of the control method is proved using the Lyapunov stability criterion; specifically, it includes:

[0079] Since the traditional differential tracker is more susceptible to noise when tracking the system state, especially pulse noise will cause great interference to the differential tracker. The present invention proposes a differential tracker with median filter (Tracking Differentiator with Median Filter, MF-TD), the differential tracker with median filter MF-TD is used to filter the robot's posture state, which can effectively suppress noise and smooth the violent fluctuation of the robot state, so as to obtain the actual posture η and actual speed of the robot body.

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

[0081]

[0082] Among them, k represents the current step of the discrete system, n MF Indicates the window length of the median filter, x p0 represents the original system state, x p represents the system state after median filtering, x TD_1 Indicates the system status of MF-TD tracking, 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 speed factor, T TD represents the tracking step size; sort(·) represents the sorting function, which can be 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 TD.

[0083] Figure 1 The comparison of the tracking effect of the bow angle and bow angular velocity obtained by the robot vision system using TD (differential tracker) and MF-TD is described. Figure 1(a) in the figure is the tracking curve of the original bow angle. It can be seen that the original robot body state has obvious pulse noise between 30 and 40 seconds. At this time, the TD tracking of the system state has obvious jitter. The MF-TD adopted in the present invention has better noise suppression effect and the tracked system state curve is smoother. Figure 1 (b) in the figure is the tracking curve of the differential value of the original bow angle. It can be seen that MF-TD has better noise suppression ability than TD.

[0084] The motion of the underwater robot with wave fin propulsion only considers the three degrees of freedom of lateral movement, longitudinal movement and bow shaking, such as Figure 2 As shown, O E -X E Y E The world coordinate system, O V -X V Y V represents the body coordinate system of the underwater robot propelled by the undulating fin. Therefore, the velocity vector of the robot in the world coordinate system is defined as follows:

[0085]

[0086] 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; η=[xyy] T Represents the robot's position vector in the world coordinate system.

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

[0088]

[0089] Among them, 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 coordinate system, represents the acceleration vector of the robot in the body coordinate system, represents the hydrodynamic force and moment vector, τ r =[τ rx τ ry τ ry ] represents the control force and torque vector of the robot.

[0090] Further we can get:

[0091]

[0092] Where M represents the inertia matrix of the robot, τ h ′(v)v represents the hydrodynamic force and torque of the robot without the additional inertia matrix.

[0093] The design of the body posture controller is as follows:

[0094] Select x1 = η, x2 = v, and according to the previous equations (2) and (4), we can get:

[0095]

[0096] Define the first error surface z1:

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

[0098] where η d Represents the desired position of the robot in the world coordinate system.

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

[0100]

[0101] Define the second error surface z2:

[0102] z2=x2-a2 (8)

[0103] Where a2 represents the state variable.

[0104] By defining a low-pass filter with a time constant of e2, we can obtain a2:

[0105]

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

[0107]

[0108] Where k1>2 represents the design parameter.

[0109] According to equations (5) and (7), the derivative of the second error surface z2 can be obtained:

[0110]

[0111] make:

[0112]

[0113] Where k2>0 represents the design parameter.

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

[0115]

[0116] C RB (v) represents the rigid body Coriolis force and centripetal force matrix of the robot, M represents the inertia matrix of the robot, a2 represents the state variable of the system, x2 represents the velocity vector of the robot, τ h ′(v)x2 represents the hydrodynamic force and torque of the robot excluding the additional inertia matrix, and k2>0 represents the design parameters.

[0117] The position information η of the end effector calculated by combining the kinematics of the underwater manipulator m , the speed of the end effector Get the feedback of the body posture controller, the feedback and the expected posture η of the robot in the world coordinate system d , expected speed Deviation z1, As the input of the body posture controller, the control law τ is finally obtained through the dynamic surface controller r , completing the design of the body posture controller.

[0118] The stability of the bionic robot body posture controller is analyzed by Lyapunov stability criterion, and the Lyapunov function is selected for verification:

[0119]

[0120] 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.

[0121] Finally, V2 is converged, all signals in the closed-loop system are bounded, and the system is stable.

[0122] 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, and the disturbance generated by the movement of the underwater manipulator is used as the feedforward compensation item of the underwater manipulator joint motion control system; specifically including:

[0123] In the dynamic modeling of the robot arm, the robot arm is usually regarded as a multi-link mechanism. Therefore, the additional coupling effect caused by the joint motion of the robot arm can be transferred to the robot body through the chain rule. The relationship between the force and torque between two adjacent joints is as follows:

[0124]

[0125] 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.

[0126] Generalized forces include inertial force, hydrodynamic force and friction. The formation of friction is more complicated and relatively small, so friction is ignored in this section. In summary, generalized forces i F i and torque i T can be described as:

[0127]

[0128] in, i F I,i represents the inertia force of the i-th joint in the i-th coordinate system;

[0129] i T I,i represents the inertia moment of the i-th joint in the i-th coordinate system;

[0130] 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;

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

[0132] Inertial force i F I,i and torque i T I,i It can be described as:

[0133]

[0134] 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.

[0135] According to the above equations (16), (17), and (18), the feedforward compensation value τ of the disturbance generated when the underwater manipulator moves can be finally obtained: m The feedforward compensation of the disturbance generated by the underwater manipulator movement includes the force and torque of the hydrodynamic force and inertial force. r Add together and you can get the total control force τ a .

[0136] The water force 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 gravity and buoyancy balance 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 working robot has 5 joints, namely the waist joint, shoulder joint, elbow joint, wrist pitch joint and wrist rotation joint. Figure 3 As shown in the figure, e, θ2, θ3, θ4 and θ5 are the rotation angles of the waist joint, shoulder joint, elbow joint, wrist pitch joint and wrist rotation joint respectively. The water force on the joint movement of the underwater manipulator 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.

[0137] Figure 4 The water force on the waist joint at different rotation directions and speeds is shown. Figure 4 In (a) and (b), the rotation direction of the waist joint is positive, and the rotation angle is 0° to 360°. Figure 4 In (c) and (d), the rotation direction of the waist joint is positive, and the rotation angle is 360°~0°. Figure 4 It can be seen that as the waist joint rotation speed increases from 5° / s to 50° / s, the maximum value of the water force increases from 0.4N to 10.6N. From the simulation results, it can be seen that the range of the water force on the robot arm when the waist joint rotates is [-10.6, 10.6]N.

[0138] Figure 5 The water forces acting on the above joints at different rotation speeds are shown. Figure 5 In (a), as the shoulder joint rotates from -20° to 20°, the water force in the X direction is always negative, and its absolute value increases continuously. Figure 5 In (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, then becomes a negative value and its absolute value continues to increase; as the elbow joint rotates from -90° to 20°, the water force in the X direction first decreases from a negative maximum value to 0, then becomes a positive value and its absolute value continues to increase. Figure 5 In (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. Figure 5 In (d), the Y-direction water force on the wrist revolute joint is Figure 5 (b) has a similar trend, but the absolute value of the water force is smaller.

[0139] The absolute values ​​of the water forces on the above four joints increase with the increase of the joint rotation speed, and the range of the water forces on the shoulder joint, elbow joint, wrist pitch joint and wrist rotation joint when rotating is [-9.8, 9.8] N, [-8.3, 8.3] N, [-2.4, 2.4] N, [-0.2, 0.2] N. So far, the water forces on the underwater manipulator when each joint rotates are obtained.

[0140] When the actual position η and actual speed of the bionic robot body When the grasping or placing conditions are met (for example, the robot grasps or places blocks underwater through the robotic arm), the rotation angle θ of each joint of the underwater robotic arm can be obtained according to the desired end effector posture through the inverse kinematics of the underwater robotic arm. i , thereby controlling the joints of the underwater manipulator. At this time, the disturbance τ generated by the underwater manipulator joints during the rotation of the bionic robot body can be obtained according to the dynamic model of the underwater manipulator of the present invention. m , which is used as the feedforward compensation term of the human body posture controller. r The total control force τ can be obtained by adding a .

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

[0142] The collision wave of the bionic wave fin is composed of two sine waves propagating in opposite directions. The meeting point of the two sine waves is the collision position. The description function of the collision wave is as follows:

[0143]

[0144] Among them, β icps (t,x f ) represents the bionic wave fin at time t x f The angle value of the position, A icps represents the amplitude of the fluctuation angle, f icps represents the frequency of the collision wave, λ icps represents the wavelength of the collision wave, m icps Indicates the collision position.

[0145] Figure 6 A schematic diagram of the collision wave generated by the bionic undulating fin is given. By selecting appropriate control parameters, the bionic undulating fin can generate continuously changing longitudinal and lateral forces when moving in the collision wave.

[0146] A fuzzy rule mapping model for collision waves is designed. In the coordinated control framework of the bionic robot body and the underwater manipulator designed by the present invention, it is necessary to control the three states of the bow angle y, the longitudinal position x and the lateral position y. Since the bionic undulating fin propulsion underwater working robot is an under-actuated system, the present invention divides the posture controller of the robot body 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 Tables 1 and 2, NB represents negative large, NS represents negative small, Z represents zero, PS represents positive small, and PB represents positive large.

[0147] Table 1

[0148]

[0149] Table 2

[0150]

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

[0152]

[0153] Among them, the first and second rows are the control parameters of the left and right bionic wave fin thrusters, respectively, and the control parameters are: frequency, collision position, amplitude and wavelength of the collision wave.

[0154] f icps_L represents the frequency of the left collision wave, micps_L Indicates the left collision position, A icps_L represents the amplitude on the left side, λ icps_L represents the wavelength on the left;

[0155] f icps_R represents the frequency of the right collision wave, m icps_R Indicates the right collision position, A icps_R represents the right side amplitude, λ icps_R represents the wavelength on the right;

[0156] The frequency of the left collision wave of the bionic wave fin satisfies f icps_L ∈[0,1.8]Hz, the frequency of the right collision wave satisfies f icps_R ∈[0,1.8]Hz;

[0157] It should be noted that in Tables 1 and 2, the frequency parameters of the collision wave are expressed in fuzzy language, while the other control parameters are described using specific numerical values. ax , lateral control force τ ay and the bow control torque τ ay The fuzzy language value set is {NB, NS, Z, PS, PB}, and the frequency f of the bionic wave fin is icps The fuzzy language values ​​are all {Z, PS, PM, PB}, and their membership functions are as follows Figure 7 shown.

[0158] The above fuzzy rules are imported into the Fuzzy Logic Designer module of Matlab, and finally the fuzzy rule mapping model is obtained. The input of the model is the longitudinal control force τ ax , lateral control force τ ay and the bow control torque τ ay , the output of the model is all the control parameters Q of the bionic undulating fin. So far, the design of the underwater autonomous operation control method based on dynamic surface control has been completed.

[0159] In order to verify the effectiveness of the coordinated control method between the bionic robot body and the underwater manipulator, an underwater autonomous assembly experiment was carried out in an indoor pool. Figure 8 and Fig. 9 A sequence of video screenshots of the underwater autonomous assembly experiment is given. The entire experimental process includes the autonomous grasping, autonomous transportation and autonomous construction stages of four building blocks (ID: 17, 18, 19, 20).

[0160] exist Figure 8 In (a)-(c), the undulating fin-propelled underwater robot autonomously grasps the building block with ID 17. Figure 8(a) shows that the robot adjusts its posture by controlling the bionic undulating fins according to the recognized posture information of the building blocks. Figure 8 (b) shows the robot arm grasping the building blocks. Figure 8 (c) shows the robot arm completing the grabbing of the building blocks. Figure 8 In (d)-(e), the underwater robot with undulating fins is autonomously moving the building block with ID 17. The robot adjusts the frequency of the bionic undulating fins on both sides according to the recognized tracking path posture information to achieve path tracking control and move the building block from area A to area B. Figure 8 In (f)-(h), the undulating fin-propelled underwater robot is autonomously building the building block with ID 17. Figure 8 (f) shows that the robot adjusts its posture by controlling the bionic undulating fins according to the recognized posture information of the marker plate. Figure 8 (g) shows the robot arm building blocks. Figure 8 (h) shows the robotic arm completing the building of building blocks.

[0161] besides, Figure 8 (i)-(p), Fig. 9 (a)-(h) and Fig. 9 (i)-(p) show the autonomous grasping, transporting and building of building blocks with IDs 18, 19 and 20 by the undulating fin-propelled underwater robot. During the entire underwater autonomous assembly experiment, the bionic-propelled underwater robot completed the grasping process of the four building blocks well, and was able to transport the building blocks to a miscellaneous area through path tracking, and was able to place the building blocks in the pre-designed designated position with a small error, and finally completed the building block construction operation. Fig. 9 (p) It can be seen that the shape of the building blocks finally constructed is the pre-designed "mouth" shape, which meets the task requirements of autonomous building block assembly.

[0162] Aiming at the problem of object grabbing and building of bionic propulsion underwater operation robot, the present invention proposes a bionic robot body and water. The method includes three parts: bionic robot body posture controller based on dynamic surface control, feedforward compensation of underwater manipulator disturbance and collision wave parameter mapping. Firstly, a differential tracker with median filtering is proposed to suppress the system state noise to obtain a stable and smooth robot motion state. The bionic machine body posture controller is designed based on the dynamic surface control method, and the stability of the body posture controller is proved by using the Lyapunov stability criterion. Secondly, the hydrodynamic characteristics of the underwater manipulator are obtained based on computational fluid dynamics, the dynamic model of the underwater manipulator is established, and the disturbance generated by the movement of the underwater manipulator is used as the feedforward compensation of the body posture controller. Thirdly, the propulsion mechanism of the collision wave is analyzed, the fuzzy rule mapping model of the collision wave is constructed, and the motion control of the bionic wave 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, establishing a dynamic model of the underwater manipulator, and taking the disturbance generated by the movement of the underwater manipulator as a feedforward compensation term; Adding the feedforward compensation term to 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: η=[xyy] 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 y 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 The definition is 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 τ ry ],τ rx represents the longitudinal control force, τ ry represents the lateral control force, τ ry 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: In step 2, 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 item of the underwater manipulator joint motion control system; 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.

6. According to the method of coordinated operation of underwater bionic robot body and arm based on dynamic surface control in claim 5, 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 .

7. According to the method for coordinated operation of underwater bionic robot body and arm based on dynamic surface control in claim 1, in step 3, a fuzzy rule mapping model of collision wave 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 τ ay ; Control force for longitudinal movement τ ax , lateral control force τ ay And the bow control torque τ ay Fuzzy processing is performed respectively and converted into fuzzy language values. The set of fuzzy language values ​​is {NB, NS, Z, PS, PB}; in, 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 τ ay 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 ay 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.

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: Specifically: the membership function is used to control the longitudinal control force τ ax , lateral control force τ ay And the bow control torque τ ay Fuzzy processing is performed separately.

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

Images