Switching control method for movement modes of climbing robot

Abstract

The invention provides a conversion control method for movement modes of a climbing robot. The conversion control method focuses on improving the conversion efficiency and accuracy of an underwater robot from a swimming mode to a climbing mode in a complex environment. By defining an inertial coordinate system and a carrier coordinate system, establishing kinematics and dynamics models and designing an active disturbance rejection controller (ADRC), effective suppression of nonlinear disturbance and compensation of a wall effect are realized. A propeller propelling system and an optimized thrust distribution model are utilized, efficient and stable movement of the robot on different degrees of freedom is ensured, and compared with PID control and manual control, better control performance and higher anti-interference capacity are shown on the three dimensions of front pitch, depth and pitching.

Description

Technical Field

[0001] The invention belongs to the technical field of motion control of swimming and crawling robots, and particularly relates to a conversion control method for the motion modes of a swimming and crawling robot. Background Art

[0002] The key to ensuring the development and utilization of clean energy and marine resources lies in regularly and precisely inspecting offshore platforms such as clean energy facilities, for example, nuclear power facilities, wind power tower barrels, etc. Facing the complex structures and terrains of underwater facilities, for example, nuclear power plants and wind turbines are mostly irregular cylindrical facades, and there are some obstacles such as pipelines, brackets, fuel rods, sacrificial anode blocks, etc. on the walls. In addition, the walls of such structures are mostly made of stainless steel cladding materials, lacking ferromagnetism, or having relatively thick anti-corrosion coatings. Therefore, compared with simple swimming or crawling robots, swimming and crawling robots exhibit better environmental adaptability and mobility, ensuring the accuracy of inspection work and the flexibility of obstacle avoidance. Therefore, the main difficulty in ensuring the working efficiency and accuracy of swimming and crawling robots lies in the conversion control between the two motion modes of crawling and swimming.

[0003] The underwater working environment flow field of swimming and crawling robots is complex. When the robot performs wall-attached motion, that is, when the swimming and crawling robot performs the motion conversion control of approaching the wall, on the one hand, it is affected by the non-linear disturbance from the flow field, and on the other hand, it is affected by the disturbance generated by the robot's own motion. Especially in the area near the wall in the underwater flow field, there is a wall effect, which will produce an interference that cannot be accurately predicted in the motion control process of the swimming and crawling robot. At present, for the motion mode conversion control of wall-facing operations based on swimming and crawling robots, it is basically manually controlled, which is not only time-consuming but also has large errors, and it is difficult to avoid human operation errors caused by unexpected situations in the actual application process. Therefore, an automatic control method for the motion mode conversion of swimming and crawling robots needs to be proposed urgently. Summary of the Invention

[0004] The purpose of the embodiments of the invention is to provide a conversion control method for the motion modes of a swimming and crawling robot, so as to solve the problems that the existing conversion control method for the motion modes of a swimming and crawling robot is time-consuming and has large errors.

[0005] To solve the above technical problems, the technical solution adopted by the invention is a conversion control method for the motion modes of a swimming and crawling robot, and the steps include:

[0006] Step S1, define an inertial coordinate system and a carrier coordinate system;

[0007] Step S2, establish the kinematic conversion relationship between the fixed coordinate system and the moving coordinate system;

[0008] Step S3, establish the dynamic model of the swimming and crawling robot;

[0009] Step S4: Design the controller of the crawling and swimming robot.

[0010] Further, in S2, the conversion relationship between the fixed system and the moving system follows the following equation:

[0011]

[0012] In the formula, represents the representation of the linear velocity and angular velocity in the carrier coordinate system in the inertial coordinate system, represents the representation of the linear velocity and angular velocity in the carrier coordinate system in the carrier coordinate system, b represents the carrier coordinate system, n represents the inertial coordinate system, and J represents the reversible conversion matrix from the moving system to the fixed system.

[0013] Further, the reversible conversion matrix J from the moving system to the fixed system can be expressed as:

[0014]

[0015] J1 and J2 can be expanded respectively as:

[0016]

[0017] Integrating the above formulas, the kinematic equation of the crawling and swimming robot is obtained as follows:

[0018]

[0019] Expanding it gives:

[0020]

[0021] Among them, respectively represent the linear and angular velocity matrices of the robot in the inertial coordinate system, 0 3×3 represents a 3x3 zero matrix, J1 and J2 respectively represent the linear and angular velocity conversion matrices, V1 and V2 respectively represent the linear and angular velocity matrices of the robot in the carrier coordinate system, η1 and η2 respectively represent the position and angle matrices of the robot in the inertial coordinate system, X, Y, and Z respectively represent the positions of the robot in the x, y, and z directions in the inertial coordinate system, u, v, and w respectively represent the linear velocities of the robot along the x, y, and z axes in the carrier coordinate system, p, q, and r respectively represent the angular velocities of the robot around the x, y, and z axes in the carrier coordinate system, respectively represent the linear velocities of the robot along the x″, y″, and z″ axes in the inertial coordinate system, respectively represent the angular velocities of the robot around the x″, y″, and z″ axes in the inertial coordinate system, θ represents the angle of rotation of the carrier coordinate system around the y″ axis of the inertial coordinate system, that is, the pitch angle, represents the angle of rotation of the vehicle coordinate system about the x″ axis of the inertial coordinate system, i.e., the roll angle, and ψ represents the angle of rotation of the vehicle coordinate system about the z″ axis of the inertial coordinate system, i.e., the yaw angle.

[0022] Further, the S3 specifically includes:

[0023] S31, perform rigid body dynamics analysis on the robot and represent it in the form of a matrix:

[0024]

[0025] M RB According to the simplification conditions, it can be expanded as:

[0026]

[0027] C RB According to the simplification conditions, it can be expanded as:

[0028]

[0029] Among them, M RB is the rigid body mass matrix, C RB is the rigid body Coriolis centripetal force matrix, τ RB is the external force matrix acting on the rigid body, represents the linear acceleration or angular acceleration in the vehicle coordinate system, V represents the linear velocity or angular velocity in the vehicle coordinate system, the rigid body mass matrix M RB ∈ R 6×6 R 6×6 represents a 6×6 matrix, u, v, and w respectively represent the linear velocities of the robot in the x, y, and z directions in the vehicle coordinate system, p, q, and r respectively represent the angular velocities of the robot about the x, y, and z axes in the vehicle coordinate system, m represents the mass of the robot, I x I y I z respectively represent the moments of inertia of the robot about the x, y, and z axes in the vehicle coordinate system;

[0030] S32, perform mechanical analysis on the static force received by the crawling robot, and the restoring force matrix g(η) ∈ R 6×1 can be expanded as:

[0031]

[0032] Among them, W represents the gravity of the robot, B represents the buoyancy force received by the robot, θ and φ respectively represent the pitch angle and roll angle of the robot, x g y g z g respectively represent the coordinates of the center of gravity of the robot in the x, y, and z axis directions, x b y b, z b respectively represent the coordinates of the center of buoyancy of the robot in the x, y, and z axis directions, and R 6×1 represents a 6×1 matrix;

[0033] S33. Analyze the hydrodynamic forces acting on the crawling robot in water. The hydrodynamic forces are divided into inertial forces and viscous forces and can be expressed as:

[0034]

[0035] Among them, τ hyd represents the hydrodynamic matrix, D represents the viscous hydrodynamic matrix, and M A represents the added mass matrix, and C A represents the hydrodynamic Coriolis centripetal force matrix. M A and C A constitute the added mass coefficient matrix of the inertial hydrodynamic force, and its function is to generate a resistance opposite to the acceleration of the rigid body. represents the linear acceleration or angular acceleration in the body coordinate system, and V represents the linear velocity or angular velocity in the body coordinate system;

[0036] S34. Establish a complete thrust allocation model, and the thrust model can be expressed as:

[0037] T = K T ρc 2 d 4

[0038] Let the thrust coefficient of the propeller K = K T ρd 4 , then it can be written as:

[0039] T = Kc 2

[0040] In the formula, T is the thrust generated by the propeller thruster, and K T is the thrust coefficient of the thruster, ρ is the density of water, c is the rotational speed of the propeller, and d is the diameter of the propeller.

[0041] Furthermore, the added mass matrix M A can be expanded according to the simplified conditions as:

[0042]

[0043] The hydrodynamic Coriolis centripetal force matrix C A can be expanded according to the simplified conditions as:

[0044]

[0045] The viscous hydrodynamic matrix D can be expressed as a linear damping coefficient matrix D Land the non - linear damping coefficient matrix D NL The sum is shown in the following formula:

[0046] D = D L + D NL

[0047] In the formula, D L and D NL Can be expanded according to the simplification conditions as:

[0048]

[0049]

[0050] Where, Respectively represent the inertial hydrodynamic coefficients when the robot moves along the x, y, and z axes in the body coordinate system and the linear accelerations are respectively, Respectively represent the inertial hydrodynamic coefficients when the robot rotates around the x, y, and z axes in the body coordinate system and the angular accelerations are respectively. X u 、Y v 、Z w Respectively represent the linear damping coefficients when the linear velocities in the x, y, and z directions in the body coordinate system are u, v, and w. K p 、M q 、N r Respectively represent the linear damping coefficients when the angular velocities during rotation around the x, y, and z axes are p, q, and z respectively. X u|u| 、Y v|v| 、Z w|w| Respectively represent the non - linear damping coefficients when the linear velocities in the x, y, and z directions are u, v, and w respectively. |u| represents the absolute value of the linear velocity u, |v| represents the absolute value of the linear velocity v, and |w| represents the absolute value of the linear velocity w. K p|p| 、M q|q| 、N r|r| Respectively represent the non - linear damping coefficients when the angular velocities during the robot's rotation around the x, y, and z axes are p, q, and r respectively. |p| represents the absolute value of the angular velocity p, |q| represents the absolute value of the angular velocity q, and |r| represents the absolute value of the angular velocity. R 6×6 Represents a 6×6 matrix;

[0051] The dynamic model can be specifically expressed as:

[0052]

[0053] M = M RB + M A

[0054] C = CRB +C A

[0055] where M is the total mass matrix, which is the sum of the rigid body mass matrix M RB and the added mass matrix M A ; C is the total inertia hydrodynamic matrix, which is the sum of the rigid body Coriolis centripetal force matrix C RB and the hydrodynamic Coriolis centripetal force matrix C A ; τ represents the control force, g(η) represents the restoring force matrix with a value of zero, τ hyd represents the hydrodynamic matrix, D represents the viscous hydrodynamic matrix, τ RB represents the rigid body inertia force matrix, represents the linear acceleration or angular acceleration in the carrier coordinate system, and V represents the linear velocity or angular velocity in the carrier coordinate system.

[0056] Furthermore, the thrust distribution model matrix τ' of the crawling and swimming robot is specifically as follows:

[0057]

[0058] The thrust model is specifically expressed as:

[0059] τ1 = T1 - T2

[0060] τ3 = T3 - T4 - T5

[0061] τ5 = -T5l y +(T4 - T3)l y

[0062] where T1, T2, T3, T4, and T5 respectively represent the thrusts of the corresponding thrusters of the robot, l y is the arm length from the center of gravity of the robot to the thruster, τ1, τ3, and τ5 respectively represent the control forces in the three degrees of freedom of forward and backward movement, heave movement, and pitch movement, and τ2, τ4, and τ6 respectively represent the control forces in the three degrees of freedom of lateral movement, yaw, and roll;

[0063] Therefore, the complete thrust model matrix equation of the crawling and swimming detection robot can be obtained as follows:

[0064]

[0065] n1, n2, n3, n4, and n5 respectively represent the rotational speeds of the 5 propellers, τ1, τ2, τ3, τ4, τ5, and τ6 respectively represent the control forces in the 6 degrees of freedom in the coordinate system, and τ' represents the thrust distribution model matrix of the robot;

[0066] When the robot adjusts its attitude underwater, the gravity and buoyancy are kept in balance, i.e., W = B, and: v = 0, p = 0, r = 0, Therefore, the dynamic equations of the robot in three degrees of freedom can be simplified as follows:

[0067]

[0068] The kinematic equations of the robot in three degrees of freedom are:

[0069]

[0070] Wherein, respectively represent the linear accelerations in the x and z axis directions in the carrier coordinate system, represents the angular acceleration of the y axis in the carrier coordinate system, m represents the mass of the robot, respectively represent that the robot moves along the x and z axis directions in the carrier coordinate system and the linear accelerations are when the inertial hydrodynamic coefficients, represents that the robot moves along the y axis direction in the carrier coordinate system and the angular acceleration is when the inertial hydrodynamic coefficient, X u 、Z w respectively represent the linear damping coefficients when the linear velocities in the x and z axis directions in the carrier coordinate system are u and w respectively, M q represents the linear damping coefficient when the angular velocity of the y axis in the carrier coordinate system is q, u and w respectively represent the linear velocities of the robot along the x and z directions in the carrier coordinate system, q represents the angular velocity of the robot around the y axis in the carrier coordinate system, |u| represents the absolute value of the linear velocity u, |w| represents the absolute value of the linear velocity w, |q| represents the absolute value of the angular velocity q, respectively represent the linear velocities of the robot along the x″ and z″ axes in the inertial coordinate system, represents the angular velocity of the robot around the y″ axis in the inertial coordinate system, θ represents the pitch angle of the robot, I y represents the moment of inertia of the robot around the y axis in the carrier coordinate system.

[0071] Furthermore, the specific steps of S4 are as follows:

[0072] S41, compensate the input response value of the system through the TD structure to weaken the overshoot value. The specific calculation process of TD is as follows:

[0073]

[0074] Wherein, v0 represents the set input signal, v1(·) represents the tracking signal of the robot, v2(·) represents the differential of the tracking signal, k represents the moment, h represents the step size, Fst represents the Fst function, and c1 and c2 represent two constant parameter values;

[0075] S42. Linearize the non - linear complex unknown system into a series closed - loop system with double - integral through the ESO structure, and calculate the uncalculable external and internal disturbances in the system. The specific algorithm is as follows:

[0076] ε1 = z1(k)-y(k)

[0077]

[0078] Among them, z1(·) represents the actual measured value of the first state variable, z2(·) represents the actual measured value of the second state variable, z3(·) represents the actual measured value of the third state variable, ε1 represents the system error, y(·) represents the actual output of the system, β 01 、β 02 、β 03 represent weight coefficients, c3 represents the control gain coefficient, fal(·) represents the fal function, out(·) represents the output signal of the controller, k represents the time, h represents the step size, α1, α2 represent slope coefficients, and δ is the threshold;

[0079] S43. Through the non - linear feedback structure NLSEF, change the state variables and tracking targets of the system from the original linear weighting to non - linear combination, and compensate for the unknown disturbances of the system through non - linear functions to obtain the optimal output. The specific algorithm is as follows:

[0080] e1 = v h1 (k)-z1(k)

[0081] e2 = v h2 (k)-z2(k)

[0082] u0 = β 01 ·fal(e1,α1,δ)+β 02 ·fal(e1,α2,δ)

[0083] out(k)=u0 - z3(k) / c3

[0084] v h1 (·) represents the expected value of the first state variable, v h2 (·) represents the expected value of the second state variable, z1(·) represents the actual measured value of the first state variable, z2(·) represents the actual measured value of the second state variable, z3(·) represents the actual measured value of the third state variable, u(·) represents the output signal of the controller, β 01 、β 02represents the weight coefficient, α1 and α2 represent the slope coefficients, δ represents the threshold, c3 represents the control gain coefficient, u0 represents the initial control quantity, out(·) represents the output signal, fal(·) represents the fal function, k represents the moment, e1 represents the error between the expected value and the actual measured value of the first state variable, and e2 represents the error between the expected value and the actual measured value of the second state variable;

[0085] S44, simplify and improve the auto-disturbance rejection controller structure and algorithm of the crawling robot. The specific form of the second-order equation of the controlled object is as follows:

[0086]

[0087] Based on the second-order equation of the controlled object and two simplified mathematical model equations of the robot in three degrees of freedom, the robot motion control equation based on ADRC can be obtained, which is as follows:

[0088] The forward distance ADRC control equation of the robot is:

[0089]

[0090] The depth ADRC control equation of the robot is:

[0091]

[0092] The pitch ADRC control equation of the robot is:

[0093]

[0094] Among them, represents the second derivative of the system state variable, x h , ω, t represent the system state variable, the change rate of the state variable, the unknown disturbance quantity, and time respectively, f(·) represents a function related to the system state variable, the change rate of the state variable, the unknown disturbance quantity, and time, represents the change rate of the state variable x h1 , represents the change rate of the state variable x h2 , τ represents the control signal, x h1 represents the position state of the system, x h2 represents the speed state of the system, c3 represents the control gain coefficient, represents the acceleration of the robot along the z″ axis in the inertial coordinate system, represents the acceleration of the robot along the x″ axis in the inertial coordinate system, represents the acceleration of the robot rotating around the y″ axis in the inertial coordinate system, respectively represent the inertial hydrodynamic coefficients when the robot moves along the x and z axes in the carrier coordinate system and the linear accelerations are respectively, represents the inertial hydrodynamic coefficient when the robot moves along the y-axis in the carrier coordinate system and the angular acceleration is respectively. ω1, ω2, and ω3 are the interference amounts received by the robot in the flow field. y′ is the actual output of the system, z′ is the forward distance state of the robot, x′ is the depth state of the robot, θ' is the pitch state of the robot, and τ1, τ3, and τ5 respectively represent the control forces in the three degrees of freedom of the forward and backward movement, heave movement, and pitch movement.

[0095] The beneficial effects of the present invention are as follows:

[0096] 1. In the present invention, an overall design of the crawler-swimmer robot is carried out, the arrangement method of the thrusters and the structural configuration of the robot are planned in detail, and the spiral propeller propulsion system is used to achieve swimming control, endowing the robot with flexible crawler-swimmer dual-mode movement ability to ensure that it can efficiently and stably realize the conversion of movement modes in a complex underwater environment.

[0097] 2. The kinematics and dynamics of the crawler-swimmer robot are deeply analyzed in the present invention. By establishing an accurate mathematical model, the movement laws and mechanical behaviors of the robot in different postures and different movement modes are described in detail. On this basis, a highly optimized thrust distribution model is designed, which can dynamically adjust the forces and torques output by the thrusters to meet the conversion requirements between crawling and swimming. This not only improves the adaptability and flexibility of the robot, but also greatly enhances its stability and efficiency when performing complex tasks.

[0098] 3. According to the mathematical model and limiting conditions of the robot, the control equations of the robot in three degrees of freedom are analyzed in the present invention, and the overall controller structure is designed to obtain the crawler-swimmer mode conversion control method.

[0099] 4. The movement mode conversion control method designed based on the active disturbance rejection control in the present invention has the advantages of non-linear estimation ability, not being limited by model parameters, strong robustness, and simple algorithm structure compared with other algorithms. It can ensure that the robot safely and stably performs wall-attached movement and inspection and repair work facing the wall, and effectively cope with various interferences.

[0100] 5. In the existing conversion of robot motion modes, there are technical problems such as non-linear disturbances and wall effects that cause unpredictable interference to the motion control process of swimming robots. The present invention provides a customized control scheme for underwater robots with two motion modes, which perfectly fits the algorithm structure characteristics of the auto-disturbance rejection controller, reduces the calculation data of the intermediate complicated process, and can directly obtain the required thrust when the robot performs wall-clinging motion, balancing the softness and rapidity of the robot during the motion mode conversion control.

[0101] 6. When the distance between the robot and the wall is the set value during use, the conversion control of the swimming motion mode towards the wall can be carried out through the upper computer (such as: computer, handle, etc.). In this path, both the working efficiency problem of the robot and the safety and reliability problem of the robot during the process of fitting the wall need to be considered. The present invention balances the navigation rapidity of the swimming robot in the fitting path and the softness when fitting the wall to ensure the safety and efficiency of the robot's work. BRIEF DESCRIPTION OF THE DRAWINGS

[0102] In order to more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the following will briefly introduce the drawings required for use in the description of the embodiments or the prior art. Obviously, the following-described drawings are only some embodiments of the present invention. For those of ordinary skill in the art, without creative efforts, other drawings can also be obtained based on these drawings.

[0103] Figure 1 It is a schematic diagram of the coordinate system of the swimming robot.

[0104] Figure 2 It is a schematic diagram of the propeller arrangement of the swimming robot.

[0105] Figure 3 It is a structure diagram of the ADRC controller.

[0106] Figure 4 It is a control system architecture diagram of the swimming robot.

[0107] Figure 5 It is a process curve diagram of the front distance control by the method of the present invention.

[0108] Figure 6 It is a process curve diagram of the front distance control by the PID method.

[0109] Figure 7 It is a process curve diagram of the depth control by the method of the present invention.

[0110] Figure 8 It is a process curve diagram of the depth control by the PID method.

[0111] Figure 9It is the process curve diagram of the pitch control by the method of the present invention.

[0112] Figure 10 It is the process curve diagram of the pitch control by the PID method. Specific embodiments

[0113] Next, the technical solutions in the embodiments of the present invention will be clearly and completely described in conjunction with the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are only a part of the embodiments of the present invention, rather than all the embodiments. All other embodiments obtained by those of ordinary skill in the art based on the embodiments of the present invention without creative work shall fall within the protection scope of the present invention.

[0114] As Figures 1 to 10 shown, the embodiment of the present invention provides a conversion control method for the motion mode of a crawling and swimming robot.

[0115] Embodiment 1

[0116] The steps of a conversion control method for the motion mode of a crawling and swimming robot include:

[0117] Step S1, define an inertial coordinate system E-x″y″z″ and a carrier coordinate system O-xyz to establish a mathematical model of the robot's motion.

[0118] Among them, the inertial coordinate system is used as a fixed system to describe the absolute position and attitude of the robot. The Ez″ axis of the inertial coordinate system is perpendicular to the ground and points to the center of the earth, and the Ex″ axis and the Ey″ axis are perpendicular to each other to form the horizontal plane of the earth's surface. The carrier coordinate system is used as a moving system, which is a coordinate system that moves with the robot and is used to describe the position and attitude changes of the robot relative to itself. The origin O is located at the center of gravity of the robot. The Ox axis represents the forward direction of the robot, the Oy axis is usually the lateral direction of the robot, and the Oz axis is perpendicular to the Ox axis and the Oy axis, conforming to the right-hand rule. Through the transformation of these two coordinate systems, the motion and attitude of the robot can be accurately described.

[0119] Step S2, establish the kinematic conversion relationship between the fixed system and the moving system to completely express the kinematic model of the crawling and swimming robot. The conversion relationship follows the following equation:

[0120]

[0121] In the formula, represents the representation of the linear velocity and angular velocity in the carrier coordinate system in the inertial coordinate system, represents the representation of the linear velocity and angular velocity in the carrier coordinate system in the carrier coordinate system, b represents the carrier coordinate system, and n represents the inertial coordinate system.

[0122] The reversible conversion matrix J from the moving system to the fixed system can be expressed as:

[0123]

[0124] J1 and J2 can be expanded respectively as follows:

[0125]

[0126] By integrating the above formulas, the kinematic equation of the crawler robot is obtained as follows:

[0127]

[0128] Expanding it gives:

[0129]

[0130] Among them, respectively represent the linear and angular velocity matrices of the robot in the inertial coordinate system, 0 3×3 represents a 3x3 zero matrix, J1 and J2 respectively represent the linear and angular velocity transformation matrices, V1 and V2 respectively represent the linear and angular velocity matrices of the robot in the carrier coordinate system, η1 and η2 respectively represent the position and angle matrices of the robot in the inertial coordinate system, X, Y, and Z respectively represent the positions of the robot in the x, y, and z directions in the inertial coordinate system, u, v, and w respectively represent the linear velocities of the robot along the x, y, and z axes in the carrier coordinate system, p, q, and r respectively represent the angular velocities of the robot around the x, y, and z axes in the carrier coordinate system, respectively represent the linear velocities of the robot along the x″, y″, and z″ axes in the inertial coordinate system, respectively represent the angular velocities of the robot around the x″, y″, and z″ axes in the inertial coordinate system, θ represents the angle of rotation of the carrier coordinate system around the y″ axis of the inertial coordinate system, that is, the pitch angle, represents the angle of rotation of the carrier coordinate system around the x″ axis of the inertial coordinate system, that is, the roll angle, ψ represents the angle of rotation of the carrier coordinate system around the z″ axis of the inertial coordinate system, that is, the yaw angle.

[0131] Thus, the conversion of the linear velocity and angular velocity of the crawler robot between the fixed coordinate system and the moving coordinate system can be realized, so as to better predict the attitude transformation and trajectory movement of the robot, and provide the necessary information guarantee for the subsequent control system design.

[0132] Step S3, establish the dynamic model of the crawler robot.

[0133] S31, conduct a rigid body dynamics analysis on the robot, list the rigid body dynamics equation of the robot based on Lagrange's theorem, and represent it in matrix form as:

[0134]

[0135] In the formula, M RB is the rigid body mass matrix, representing the mass distribution of the robot; C RB is the rigid body Coriolis centripetal force matrix, representing the force caused by the rotation of the rigid body in the fixed coordinate system; τ RB is the external force matrix acting on the rigid body, represents the linear acceleration or angular acceleration in the carrier coordinate system, V represents the linear velocity or angular velocity in the carrier coordinate system, the rigid body mass matrix M RB ∈R 6 ×6 and R 6×6 represents a 6×6 matrix.

[0136] Determine the simplification conditions. Specifically: Coincide the origin O of the moving coordinate system with the center of gravity of the robot, and balance the gravity and buoyancy of the robot through weight trimming, that is, the center of gravity coincides with the center of buoyancy. Consider the robot as a rigid body that is symmetric in the front-back, left-right, and up-down directions. If the navigation speed of the robot is slow enough, the high-order fluid system coefficients can be ignored, that is, the hydrodynamic effects can be ignored.

[0137] M RB can be expanded according to the simplification conditions as:

[0138]

[0139] C RB can be expanded according to the simplification conditions as:

[0140]

[0141] where u, v, and w respectively represent the linear velocities of the robot along the x, y, and z directions in the carrier coordinate system, p, q, and r respectively represent the angular velocities of the robot around the x, y, and z axes in the carrier coordinate system, m represents the mass of the robot, and I x , I y , I z respectively represent the moments of inertia of the robot around the x, y, and z axes in the carrier coordinate system, and the moments of inertia can be obtained based on the three-dimensional model and physical object of the robot.

[0142] S32. Conduct a mechanical analysis of the static force acting on the crawling robot. The static force is the resultant force of gravity and buoyancy, and its function is to generate an immediate restoring force for the robot. The restoring force matrix g(η) ∈ R 6×1 can be expanded as:

[0143]

[0144] where W = mg, W represents the gravity of the robot, m represents the mass of the robot, and g represents the acceleration due to gravity; B represents the buoyancy force on the robot, and ρ represents the density of water. represents the volume of water displaced; θ, respectively represent the pitch angle and roll angle of the robot, and x g , y g , z g respectively represent the coordinates of the center of gravity of the robot in the x, y, and z-axis directions, and x b , y b , z b respectively represent the coordinates of the center of buoyancy of the robot in the x, y, and z-axis directions, and R 6×1 represents a 6×1 matrix. According to the simplified conditions, the subsequent calculation of the static force of the robot can be ignored.

[0145] S33. Analyze the hydrodynamic force of the crawling robot in water. When the crawling robot adjusts its attitude and tracks its trajectory in water, the fluid will generate a non-linear damping force on the robot, and the magnitude of this force is related to the structural shape and navigation speed of the robot. The hydrodynamic force is generally divided into inertial force and viscous force, and can be expressed as:

[0146]

[0147] Among them, τ hyd represents the hydrodynamic matrix, which is used to represent the total hydrodynamic effect generated by the fluid on the robot, D represents the viscous hydrodynamic matrix, and M A represents the added mass matrix, C A represents the hydrodynamic Coriolis centripetal force matrix, and M A and C A form the added mass coefficient matrix of the inertial hydrodynamic force, and its function is to generate a resistance opposite to the acceleration of the rigid body. represents the linear acceleration or angular acceleration in the body coordinate system, and V represents the linear velocity or angular velocity in the body coordinate system.

[0148] S33a. Inertial hydrodynamic force analysis.

[0149] The added mass matrix M A ∈ R 6×6 , and according to the simplified conditions, it can be expanded as:

[0150]

[0151] The hydrodynamic Coriolis centripetal force matrix C A ∈ R 6×6 , and according to the simplified conditions, it can be expanded as:

[0152]

[0153] Among them, They respectively represent the inertial hydrodynamic coefficients when the robot moves along the x, y, and z axes in the carrier coordinate system and the linear accelerations are respectively, They respectively represent the inertial hydrodynamic coefficients when the robot rotates around the x, y, and z axes in the carrier coordinate system and the angular accelerations are respectively. u, v, and w respectively represent the linear velocities of the robot along the x, y, and z directions in the carrier coordinate system, and p, q, and r respectively represent the angular velocities of the robot around the x, y, and z axes in the carrier coordinate system. R 6 ×6 represents a 6×6 matrix.

[0154] S33b, Viscous Hydrodynamic Analysis.

[0155] The viscous hydrodynamic matrix D ∈ R 6×6 can be expressed as the sum of the linear damping coefficient matrix D L and the nonlinear damping coefficient matrix D NL as shown in the following equation:

[0156] D = D L + D NL

[0157] In the equation, D L and D NL can be expanded according to the simplified conditions as:

[0158]

[0159] Among them, X u , Y v , Z w respectively represent the linear damping coefficients when the linear velocities in the x, y, and z directions in the carrier coordinate system are u, v, and w respectively. K p , M q , N r respectively represent the linear damping coefficients when the angular velocities during rotation around the x, y, and z axes are p, q, and z respectively. X u|u| , Y v|v| , Z w|w| respectively represent the nonlinear damping coefficients when the linear velocities in the x, y, and z directions are u, v, and w respectively. |u| represents the absolute value of the linear velocity u, |v| represents the absolute value of the linear velocity v, and |w| represents the absolute value of the linear velocity w. K p|p| , M q|q| , N r|r| respectively represent the nonlinear damping coefficients when the angular velocities of the robot during rotation around the x, y, and z axes are p, q, and r respectively. |p| represents the absolute value of the angular velocity p, |q| represents the absolute value of the angular velocity q, and |r| represents the absolute value of the angular velocity. R 6×6 represents a 6×6 matrix.

[0160] In summary, ignoring the interference forces that will not affect the present invention, such as umbilical cord forces, and substituting into the following formula, an analytical dynamic model is obtained:

[0161]

[0162] M = M RB + M A

[0163] C = C RB + C A

[0164] where M is the total mass matrix, which is the sum of the rigid body mass matrix M RB and the added mass matrix M A ; C is the total inertial hydrodynamic matrix, which is the sum of the rigid body Coriolis centripetal force matrix C RB and the hydrodynamic Coriolis centripetal force matrix C A ; τ represents the control force, g(η) represents the restoring force matrix, with a value of zero, τ hyd represents the hydrodynamic matrix, D represents the viscous hydrodynamic matrix, τ RB represents the rigid body inertial force matrix, represents the linear acceleration or angular acceleration in the carrier coordinate system, and V represents the linear velocity or angular velocity in the carrier coordinate system.

[0165] S34, establish a complete thrust allocation model for subsequent decoupling of the degrees of freedom of the mathematical model of the crawling and swimming robot. The crawling and swimming robot of the present invention uses a propeller thruster. Ignoring the rotational torque generated by the propeller, its thrust model can be expressed as:

[0166] T = K T ρc 2 d 4

[0167] Let the thrust coefficient of the propeller K = K T ρd 4 , then it can be written as:

[0168] T = Kc 2

[0169] Through experimental curve fitting, the thrust coefficient of the propeller K = 0.0043 is obtained, and the thrust equation of the propeller can be written as:

[0170] T = 0.0043c 2

[0171] In the formula, T is the thrust generated by the propeller thruster, K T is the thrust coefficient of the thruster, ρ is the density of water, c is the rotational speed of the propeller, and d is the diameter of the propeller.

[0172] The robot in this embodiment is equipped with 6 propeller thrusters. Among them, propellers 1, 2, and 6 are horizontally arranged. Propellers 1 and 2 are arranged along the x-axis direction of the robot and are symmetrically distributed on both sides of the x-axis of the robot. Propeller 6 is arranged along the y-axis direction of the robot and is located on the y-axis. Propellers 3, 4, and 5 are vertically arranged along the z-axis direction of the robot. Propellers 3 and 4 are located at the head end of the robot and are symmetrically distributed about the x-axis of the robot. Propeller 5 is arranged on the x-axis of the robot and is located at the tail end of the robot. As Figure 2 shown, propellers 1, 3, and 6 rotate forward, which is represented by green, and propellers 2, 4, and 5 rotate backward, which is represented by blue.

[0173] S34a, according to the distribution structure of the robot propeller thrusters, obtain the thrust distribution model matrix τ' of the robot, τ' ∈ R 6×1 as shown in the following formula:

[0174]

[0175] τ1, τ2, τ3, τ4, τ5, and τ6 respectively represent the control forces in 6 degrees of freedom in the coordinate system, and R 6×1 represents a matrix of dimension 6×1. Since this embodiment is related to the movement process of the robot adhering to the wall, only the movement of three degrees of freedom of the robot, namely the forward and backward movement, heave movement, and pitch movement, is described. The parameters of the other three degrees of freedom (lateral movement, yaw, roll) are all assumed to be 0. The thrust model is specifically expressed as:

[0176] τ1 = T1 - T2

[0177] τ3 = T3 - T4 - T5

[0178] τ5 = -T5l y +(T4 - T3)l y

[0179] where T1, T2, T3, T4, and T5 respectively represent the thrusts of the corresponding thrusters of the robot, and l y is the arm length of the force from the center of gravity of the robot to the thruster. τ1, τ3, and τ5 respectively represent the control forces in the three degrees of freedom of forward and backward movement, heave movement, and pitch movement.

[0180] Therefore, the complete thrust model matrix equation of the crawling and swimming detection robot can be obtained as follows:

[0181]

[0182] n1, n2, n3, n4, n5 represent the rotational speeds of 5 propellers respectively, τ1, τ2, τ3, τ4, τ5, τ6 represent the control forces on 6 degrees of freedom in the coordinate system respectively, and τ’ represents the thrust distribution model matrix of the robot.

[0183] S34b, when the robot adjusts its attitude underwater, make the gravity and buoyancy balanced, that is, W = B, and:

[0184]

[0185] From this, the complete dynamic equation of the robot can be obtained. Ignoring the coupling terms (the products of the velocities / angular velocities on different degrees of freedom), the dynamic equations of the robot on three degrees of freedom can be simplified as:

[0186]

[0187] The kinematic equations of the robot on three degrees of freedom are:

[0188]

[0189] Among them, represent the linear accelerations in the x and z axis directions in the carrier coordinate system respectively, represents the angular acceleration of the y axis in the carrier coordinate system, m represents the mass of the robot, represent the inertial hydrodynamic coefficients when the robot moves along the x and z axis directions in the carrier coordinate system and the linear accelerations are respectively, represents the inertial hydrodynamic coefficient when the robot moves along the y axis direction in the carrier coordinate system and the angular acceleration is respectively, X u 、Z w represent the linear damping coefficients when the linear velocities in the x and z axis directions in the carrier coordinate system are u and w respectively, M q represents the linear damping coefficient when the angular velocity of the y axis in the carrier coordinate system is q, u and w represent the linear velocities of the robot along the x and z directions in the carrier coordinate system respectively, q represents the angular velocity of the robot around the y axis in the carrier coordinate system, |u| represents the absolute value of the linear velocity u, |w| represents the absolute value of the linear velocity w, |q| represents the absolute value of the angular velocity q, represent the linear velocities of the robot along the x″ and z″ axes in the inertial coordinate system respectively, represents the angular velocity of the robot around the y″ axis in the inertial coordinate system, θ represents the pitch angle of the robot, I y represents the moment of inertia of the robot around the y axis in the carrier coordinate system, τ1, τ3, τ5 represent the control forces on the three degrees of freedom of forward / backward movement, heave movement, and pitch movement respectively.

[0190] According to the above two mathematical models, there are still three degrees of freedom (lateral translation, yaw, roll) of unknown hydrodynamic coefficient parameters, and these unknown hydrodynamic coefficients are obtained by performing fluid simulation on the robot using the STAR-CCM+ software.

[0191] Step S4: Design the controller of the crawling and swimming robot. The robot controller is an active disturbance rejection controller, which is divided into three major modules, namely: Tracking Differentiator (TD), Extended State Observer (ESO), and Nonlinear Error Feedback (NLSEF). Among them, the Tracking Differentiator is mainly used to achieve the transition of the target tracking value, and obtain the differential form of the target value by arranging the transition process; the Extended State Observer is used to estimate the total disturbance generated by the system state quantity and the error of the target tracking value; the Nonlinear Error Feedback is used to suppress and compensate the steady-state error of the system, and finally obtain the optimal output value. The specific controller structure is as Figure 3 shown.

[0192] S41: Compensate the input response value of the system through the TD structure to weaken the overshoot value. The dynamic response effect is an important factor to measure the ability of the controller. When the system receives a sudden tracking signal, in order to reach the stable state as soon as possible to ensure the rapidity of the controller, there will inevitably be a relatively obvious initial overshoot value. Weakening the overshoot value can make the system transition to the stable state more smoothly. The specific calculation process of TD is as follows:

[0193]

[0194] where, v0 represents the set input signal, v1(·) represents the tracking signal of the robot, v2(·) represents the differential of the tracking signal, k represents the moment, h represents the step size, Fst represents the Fst function, and c1, c2 represent two constant parameter values.

[0195] S42: Linearize the nonlinear and complex unknown system into a double-integral series closed-loop system through the ESO structure, and calculate the external and internal disturbances that cannot be calculated in the system, so as to perform error elimination and disturbance compensation through the NLSEF structure later. The specific algorithm is as follows:

[0196] ε1 = z1(k) - y(k)

[0197]

[0198] where, z1(·) represents the actual measured value of the first state variable, z2(·) represents the actual measured value of the second state variable, z3(·) represents the actual measured value of the third state variable, ε1 represents the system error, y(·) represents the actual output of the system, β 01 、β 02 、β 03Let \(\omega\) denote the weight coefficient, \(c_3\) denote the control gain coefficient, \(fal(\cdot)\) denote the \(fal\) function, \(out(\cdot)\) denote the output signal of the controller, \(k\) denote the time instant, \(h\) denote the step size, \(\alpha_1,\alpha_2\) denote the slope coefficients, and \(\delta\) be the threshold value.

[0199] S43. Through the nonlinear feedback structure NLSEF, the state variables of the system and the tracking target are changed from the original linear weighting to a nonlinear combination, and the unknown disturbances of the system are compensated by a nonlinear function, so as to obtain the optimal output quantity, improving the robustness and adaptability of the controller. The specific algorithm is as follows:

[0200] \(e_1 = v\) h1 (k) - z1(k)

[0201] \(e_2 = v\) h2 (k) - z2(k)

[0202] \(u_0=\beta\) 01 \(\cdot fal(e_1,\alpha_1,\delta)+\beta\) 02 \(\cdot fal(e_1,\alpha_2,\delta)\)

[0203] \(out(k)=u_0 - z3(k) / c_3\)

[0204] \(v\) h1 (\(\cdot\)) represents the expected value of the first state variable, \(v\) h2 (\(\cdot\)) represents the expected value of the second state variable, \(z1(\cdot)\) represents the actual measured value of the first state variable, \(z2(\cdot)\) represents the actual measured value of the second state variable, \(z3(\cdot)\) represents the actual measured value of the third state variable, \(u(\cdot)\) represents the output signal of the controller, \(\beta\) 01 ,\(\beta\) 02 denote the weight coefficients, \(\alpha_1,\alpha_2\) denote the slope coefficients, \(\delta\) denotes the threshold value, \(c_3\) denotes the control gain coefficient, \(u_0\) denotes the initial control quantity, \(out(\cdot)\) denotes the output signal, \(fal(\cdot)\) denotes the \(fal\) function, \(k\) denotes the time instant, \(e_1\) represents the error between the expected value and the actual measured value of the first state variable, and \(e_2\) represents the error between the expected value and the actual measured value of the second state variable.

[0205] From the above algorithm, it can be seen that the algorithm of the entire controller only requires the output signal \(u(\cdot)\) of the controlled object and the system output value \(y(\cdot)\), and the rest are only the selection and adjustment of parameters.

[0206] S44. Simplify and improve the active disturbance rejection controller structure and algorithm of the crawling robot. The design of the robot's active disturbance rejection controller is divided into three parts, which are respectively used in the front distance, depth and pitch control systems of the robot. The specific control system structure is as Figure 4As shown in the figure, in the figure, H, J, and Q are the forward distance information, depth information, and pitch information output by the swimming robot sensor respectively; τ out1 , τ out3 , τ out5 are the control forces of the robot along the x-axis, z-axis, and around the y-axis output by the active disturbance rejection control module respectively; T1, T2, T3, T4, and T5 are the thrusts of the corresponding thrusters of the robot, and the propeller speed signals of the corresponding thrusters are output through the motor driver; y' is the position and attitude sensing information of the robot output by the sensor.

[0207] According to the active disturbance rejection control theory, the specific form of the second-order equation of the controlled object (the structure and algorithm of the active disturbance rejection controller in the prior art) is as follows:

[0208]

[0209] Among them, represents the second derivative of the system state variable, x h , ω, t represent the system state variable, the change rate of the state variable, the unknown disturbance, and time respectively, and f(·) represents a function related to the system state variable, the change rate of the state variable, the unknown disturbance, and time; represents the change rate of the state variable x h1 , represents the change rate of the state variable x h2 , τ represents the control signal, x h1 represents the position state of the system, x h2 represents the speed state of the system, and c3 represents the control gain coefficient.

[0210] Based on the second-order equation of the controlled object and two simplified mathematical model equations of the robot in three degrees of freedom, the robot motion control equation based on ADRC can be obtained, which is specifically as follows:

[0211] The forward distance ADRC control equation of the robot is:

[0212]

[0213] The depth ADRC control equation of the robot is:

[0214]

[0215] The pitch ADRC control equation of the robot is:

[0216]

[0217] Among them, represents the acceleration of the robot along the z″ axis in the inertial coordinate system, represents the acceleration of the robot along the x″ axis in the inertial coordinate system, represents the acceleration of the robot rotating about the y″ axis in the inertial coordinate system, respectively represent the inertial hydrodynamic coefficients when the robot moves along the x and z axes in the carrier coordinate system and the linear accelerations are ; represents the inertial hydrodynamic coefficient when the robot moves along the y axis in the carrier coordinate system and the angular acceleration is ; ω1, ω2, and ω3 are the interference quantities received by the robot in the flow field, y′ is the actual output of the system, z′ is the forward distance state of the robot, x′ is the depth state of the robot, θ′ is the pitch state of the robot, and τ1, τ3, and τ5 respectively represent the control forces in the three degrees of freedom of forward and backward movement, heave movement, and pitch movement.

[0218] Design the control equation into the control source code of the host computer to control the robot. When the distance between the robot and the wall reaches the set value, the host computer can execute this code to complete the control process of the swimming motion mode conversion of the robot sticking to the wall.

[0219] Embodiment 2

[0220] The hardware system module of the swimming robot of the present invention includes: a main controller module, a depth data processing module, a navigation data processing module, a CAN data transceiver module, a power supply module, a thruster control module, and a laser sensing data processing module.

[0221] G1: The main controller module is responsible for receiving data from other modules, is the center of the entire system, and makes decisions and controls according to the received data, and is composed of a processing chip; the main controller module is connected to the depth data processing module, the navigation data processing module, and the CAN data transceiver module, and the main controller is connected to the depth data processing module, the navigation data processing module, and the laser sensing data processing module through the IIC bus.

[0222] G2: The depth data processing module is used to process information from the depth gauge.

[0223] G3: The navigation data processing module is used to process data from the inertial measurement unit (IMU), such as attitude, speed, acceleration and other information, and receive navigation data and parse navigation information; it is composed of a microprocessing chip and a serial port data transceiver chip.

[0224] G4: The CAN data transceiver module is used to communicate with each thruster control module, and is connected to each thruster control module through the CAN bus, and is composed of a CAN data transceiver chip.

[0225] G5: Power module, which provides power for each module. The power module includes a regulated power supply and an isolated power supply, and multiple capacitors with different capacitance values are respectively connected in parallel at the input end and the output end of the power supply. It is connected to the depth data processing module, the navigation data processing module, the main controller module, the CAN data transceiver module, and each thruster control module.

[0226] G6: Laser sensing data processing module, which completes the reception of front distance data and the analysis of front distance information, and is composed of a microprocessing chip and a serial port data transceiver chip.

[0227] G7: Sensors, including two laser sensors, an inertial measurement unit (IMU), and a depth gauge. One of the two laser sensors is installed on the front side of the robot, i.e., the traveling direction, for measuring the distance to the wall surface, and the other is placed at the bottom of the robot for measuring the distance information between the bottom and the wall during the flipping process. The two sensors can measure the entire front distance information of the robot during the climbing-swimming conversion process by being placed orthogonally. The IMU is placed in the control cabin of the robot to obtain information such as the attitude, speed, and acceleration of the robot. The depth gauge is placed in the control cabin of the robot and is connected to the control carrier board for reading the depth information of the robot.

[0228] The cooperation among each module is as follows:

[0229] Regarding the technical difficulty of the wall attachment process during the conversion of the swimming-climbing robot from the swimming mode to the climbing mode, a total motion control path is formed with the cruising state as the starting point and the wall-climbing balance state as the ending point.

[0230] The laser sensing processing module, the depth data processing module, and the navigation data processing module transmit the analyzed depth and navigation information to the main controller module through the IIC bus. The main controller module uses the active disturbance rejection motion control algorithm. Specifically, it uses the laser sensor to control the front distance x of the robot to ensure that the robot is close to the wall. At the same time, it combines the information of the IMU to ensure that the robot is parallel to the bottom or the ground. Finally, it uses the information of the depth gauge to ensure that the robot maintains a constant underwater depth, completes the underwater wall attachment motion, and thus achieves the purpose of the swimming-climbing attitude conversion.

[0231] During the motion control process, the laser sensing data processing module, depth data processing module, and navigation data processing module convey the processed data to the main controller module after data processing. The main controller module calculates the corresponding rotational speeds of each thruster using the thrust model of the active disturbance rejection motion control algorithm. After CAN data packet processing, the corresponding thruster rotational speeds are placed into the corresponding CAN data packets, and the CAN data transceiver module sends them to each thruster via the CAN bus to change the rotational speed of the thruster. During the process of the thruster changing its rotational speed for motion attitude conversion, the depth gauge, IMU, and laser sensor continue to obtain the depth, navigation information, and front distance information of the robot and transmit them to the corresponding depth data processing module, navigation data processing module, and laser sensing module. The three modules continuously transmit the data to the main controller and continue to use the active disturbance rejection algorithm to modify the rotational speed of the thruster for the attitude conversion of the robot. This process is repeated until the task of the crawling-to-swimming attitude conversion is completed.

[0232] Similarly, the process from crawling to swimming is the reverse of the above process.

[0233] Experimental verification

[0234] The crawling-to-swimming motion mode conversion control method of the present invention is compared with the existing methods using the PID algorithm and manual control in a comparative experiment. Figure 5 、 6 are the results of the front distance target control of the method of the present invention and the PID method. It can be seen that the method of the present invention has better anti-interference ability and adaptability, and higher stability. Figure 7 、 8 are the results of the depth target control of the method of the present invention and the manual control method respectively. It can be seen that the adjustment process of the method of the present invention is more stable and smooth. Figure 9 、 10 are the results of the pitch target control of the method of the present invention and the PID method respectively. It can be seen that the method of the present invention reaches the stable state more quickly. Therefore, the control effect of the method of the present invention in three degrees of freedom is excellent, and there are obvious overshoot and oscillation phenomena in the control effects of PID and manual control in three degrees of freedom. It can be known that ADRC has better anti-interference ability and adaptability. It has a better compensation effect on the influence of the wall effect, ensuring that the robot smoothly and gently performs the motion mode conversion control facing the wall.

[0235] In summary, the algorithm studied in the present invention can effectively estimate and compensate the flow field disturbance and the dynamic changes of the robot in a variable environment, fully demonstrating its high anti-interference ability and self-adaptability, and its effectiveness is verified by simulation.

[0236] Each embodiment in this specification is described in a related manner. For the same or similar parts among the embodiments, reference can be made to each other. Each embodiment focuses on the differences from other embodiments. In particular, for the system embodiment, since it is basically similar to the method embodiment, the description is relatively simple, and reference can be made to the corresponding part of the method embodiment for the related content.

[0237] The above are only the preferred embodiments of the present invention, and are not intended to limit the protection scope of the present invention. Any modifications, equivalent replacements, improvements, etc. made within the spirit and principle of the present invention are all included in the protection scope of the present invention.

Claims

1. A conversion control method for the motion mode of a crawling robot, characterized in that the steps Including: Step S1, defining an inertial coordinate system and a carrier coordinate system; Step S2, establishing a kinematic transformation relationship between the fixed coordinate system and the moving coordinate system; Step S3, establishing a dynamic model of the crawling robot; Step S4, designing a controller for the crawling robot.

2. The conversion control method for the motion mode of a crawling and swimming robot according to claim 1, characterized in that, In S2, the transformation relationship between the fixed coordinate system and the moving coordinate system follows the following equation: In the formula, represents the representation of the linear velocity and angular velocity in the carrier coordinate system in the inertial coordinate system. represents the representation of the linear velocity and angular velocity in the carrier coordinate system in the carrier coordinate system. Here, b represents the carrier coordinate system, n represents the inertial coordinate system, and J represents the reversible transformation matrix from the moving coordinate system to the fixed coordinate system.

3. The conversion control method for the movement mode of a crawling and swimming robot according to claim 2, characterized in that, The reversible transformation matrix J from the moving coordinate system to the fixed coordinate system can be expressed as: J1 and J2 can be expanded respectively as: Integrating the above formulas, the kinematic equation of the crawling robot is obtained as follows: Expanding gives: Among them, respectively represent the linear and angular velocity matrices of the robot in the inertial coordinate system, 0 3×3 represents a 3x3 zero matrix, J1 and J2 respectively represent the linear and angular velocity transformation matrices, V1 and V2 respectively represent the linear and angular velocity matrices of the robot in the carrier coordinate system, η1 and η2 respectively represent the position and angle matrices of the robot in the inertial coordinate system, X, Y, and Z respectively represent the positions of the robot in the x, y, and z directions in the inertial coordinate system, u, v, and w respectively represent the linear velocities of the robot along the x, y, and z axes in the carrier coordinate system, and p, q, and r respectively represent the angular velocities of the robot around the x, y, and z axes in the carrier coordinate system. respectively represent the linear velocities of the robot along the x″, y″, and z″ axes in the inertial coordinate system. respectively represent the angular velocities of the robot around the x″, y″, and z″ axes in the inertial coordinate system, and θ represents the angle of rotation of the carrier coordinate system around the y″ axis of the inertial coordinate system, that is, the pitch angle. represents the angle of rotation of the carrier coordinate system around the x″ axis of the inertial coordinate system, that is, the roll angle, and ψ represents the angle of rotation of the carrier coordinate system around the z″ axis of the inertial coordinate system, that is, the yaw angle.

4. The conversion control method for the motion mode of a crawling and swimming robot according to claim 1, characterized in that, S3 specifically includes: S31, performing a rigid body dynamics analysis on the robot and expressing it in matrix form as: M RB It can be expanded according to the simplification conditions as follows: C RB It can be expanded according to the simplification conditions as follows: where, M RB is the rigid body mass matrix, C RB is the rigid body Coriolis centripetal force matrix, τ RB is the external force matrix acting on the rigid body, represents the linear acceleration or angular acceleration in the carrier coordinate system, V represents the linear velocity or angular velocity in the carrier coordinate system, the rigid body mass matrix M RB ∈R 6×6 where, R 6×6 represents a 6×6 matrix, u, v, and w respectively represent the linear velocities of the robot in the x, y, and z directions in the carrier coordinate system, p, q, and r respectively represent the angular velocities of the robot about the x, y, and z axes in the carrier coordinate system, m represents the mass of the robot, I x , I y , I z respectively represent the moments of inertia of the robot about the x, y, and z axes in the carrier coordinate system; S32, conduct a mechanical analysis of the static force received by the crawling robot, and the restoring force matrix g(η) ∈ R 6×1 can be expanded as: Among them, W represents the gravity of the robot, B represents the buoyancy force on the robot, θ, respectively represent the pitch angle and roll angle of the robot, x g , y g , z g respectively represent the coordinates of the center of gravity of the robot in the x, y, and z-axis directions, x b , y b , z b respectively represent the coordinates of the center of buoyancy of the robot in the x, y, and z-axis directions, R 6×1 represents a matrix of dimension 6×1; S33, analyzing the hydrodynamic forces acting on the crawling robot during movement in water. The hydrodynamic forces are divided into inertial forces and viscous forces and can be expressed as: Among them, τ hyd represents the hydrodynamic matrix, D represents the viscous hydrodynamic matrix, M A represents the added mass matrix, C A represents the hydrodynamic Coriolis centripetal force matrix, M A and C A constitute the added mass coefficient matrix of the inertial hydrodynamic force, whose function is to generate a resistance opposite to the acceleration of the rigid body, represents the linear acceleration or angular acceleration in the body coordinate system, and V represents the linear velocity or angular velocity in the body coordinate system; S34, establishing a complete thrust distribution model, and the thrust model can be expressed as: T = K T ρc 2 d 4 Let the thrust coefficient of the propeller be \(K = K\) T \(\rho d\) 4 , then it can be written as: T = Kc 2 Where T is the thrust generated by the propeller, K T is the propeller thrust coefficient, ρ is the density of water, c is the rotational speed of the propeller, and d is the diameter of the propeller.

5. The conversion control method for the movement mode of a crawling and swimming robot according to claim 4, characterized in that The additional mass matrix M A can be expanded according to the simplification conditions as follows: The hydrodynamic Coriolis centripetal force matrix C A can be expanded according to the simplification conditions as follows: The viscous hydrodynamic matrix D can be expressed as the sum of a linear damping coefficient matrix D L and a non-linear damping coefficient matrix D NL as shown in the following equation: D = D L + D NL where D L and D NL can be expanded according to the simplification conditions as follows: Among them, respectively represent the inertial hydrodynamic coefficients when the robot moves along the x, y, and z axes in the carrier coordinate system and the linear accelerations are respectively, respectively represent the inertial hydrodynamic coefficients when the robot rotates around the x, y, and z axes in the carrier coordinate system and the angular accelerations are respectively, X u , Y v , Z w respectively represent the linear damping coefficients when the linear velocities in the x, y, and z directions in the carrier coordinate system are u, v, and w respectively. K p , M q , N r respectively represent the linear damping coefficients when the angular velocities during rotation around the x, y, and z axes are p, q, and z respectively. X u|u| , Y v|v| , Z w|w| respectively represent the non - linear damping coefficients when the linear velocities in the x, y, and z directions are u, v, and w respectively. |u| represents the absolute value of the linear velocity u, |v| represents the absolute value of the linear velocity v, and |w| represents the absolute value of the linear velocity w. K p|p| , M q|q| , N r|r| respectively represent the non - linear damping coefficients when the angular velocities of the robot during rotation around the x, y, and z axes are p, q, and r respectively. |p| represents the absolute value of the angular velocity p, |q| represents the absolute value of the angular velocity q, and |r| represents the absolute value of the angular velocity. R 6×6 represents a 6×6 matrix; The dynamic model can be specifically expressed as: M = M RB + M A C=C RB +C A where M is the total mass matrix, which is the sum of the rigid body mass matrix M RB and the added mass matrix M A ; C is the total inertia hydrodynamic matrix, which is the sum of the rigid body Coriolis centripetal force matrix C RB and the hydrodynamic Coriolis centripetal force matrix C A ; τ represents the control force, g(η) represents the restoring force matrix with a value of zero, τ hyd represents the hydrodynamic matrix, D represents the viscous hydrodynamic matrix, τ RB represents the rigid body inertia force matrix, represents the linear acceleration or angular acceleration in the carrier coordinate system, and V represents the linear velocity or angular velocity in the carrier coordinate system.

6. The conversion control method for the movement mode of a crawling and swimming robot according to claim 4, characterized in that, The thrust distribution model matrix τ' of the crawling robot is specifically as follows: The thrust model is specifically expressed as: τ1 = T1 - T2 τ3 = T3 - T4 - T5 τ5 = -T5l y +(T4 - T3)l y Among them, T1, T2, T3, T4, and T5 respectively represent the thrusts of the corresponding thrusters of the robot, and l y is the arm length from the center of gravity of the robot to the thruster. τ1, τ3, and τ5 respectively represent the control forces in the three degrees of freedom of forward and backward movement, heave movement, and pitch movement, and τ2, τ4, and τ6 respectively represent the control forces in the three degrees of freedom of lateral movement, yaw, and roll; Therefore, the complete thrust model matrix equation of the crawling detection robot can be obtained as follows: n1, n2, n3, n4, n5 respectively represent the rotational speeds of 5 propellers, τ1, τ2, τ3, τ4, τ5, τ6 respectively represent the control forces in 6 degrees of freedom in the coordinate system, and τ' represents the thrust distribution model matrix of the robot; When the robot adjusts its attitude underwater, the gravity and buoyancy are balanced, i.e., W = B, and: v = 0, p = 0, r = 0, Therefore, the dynamic equations of the robot in three degrees of freedom can be simplified as: The kinematic equations of the robot in three degrees of freedom are: Among them, respectively represent the linear accelerations in the x and z axis directions in the carrier coordinate system, represents the angular acceleration of the y axis in the carrier coordinate system, m represents the mass of the robot, respectively represent that when the robot moves in the x and z axis directions in the carrier coordinate system and the linear accelerations are respectively, the inertial hydrodynamic coefficients, represents that when the robot moves in the y axis direction in the carrier coordinate system and the angular acceleration is respectively, the inertial hydrodynamic coefficient, X u 、Z w respectively represent the linear damping coefficients when the linear velocities in the x and z axis directions in the carrier coordinate system are u and w respectively, M q represents the linear damping coefficient when the angular velocity in the y axis direction in the carrier coordinate system is q. u and w respectively represent the linear velocities of the robot in the x and z directions in the carrier coordinate system, q represents the angular velocity of the robot around the y axis in the carrier coordinate system, |u| represents the absolute value of the linear velocity u, |w| represents the absolute value of the linear velocity w, and |q| represents the absolute value of the angular velocity q. respectively represent the linear velocities of the robot along the x″ and z″ axes in the inertial coordinate system, represents the angular velocity of the robot around the y″ axis in the inertial coordinate system, θ represents the pitch angle of the robot, I y represents the moment of inertia of the robot around the y axis in the carrier coordinate system.

7. A conversion control method for the movement mode of a crawling and swimming robot according to claim 1, characterized in that, The specific steps of S4 are as follows: S41, compensating the input response value of the system through the TD structure to weaken the overshoot value. The specific calculation process of TD is as follows: Among them, v0 represents the set input signal, v1(·) represents the tracking signal of the robot, v2(·) represents the differential of the tracking signal, k represents the moment, h represents the step size, Fst represents the Fst function, and c1, c2 represent two constant parameter values; S42, linearizing the nonlinear and complex unknown system into a double-integral series closed-loop system through the ESO structure and calculating the external and internal disturbances that cannot be calculated in the system. The specific algorithm is as follows: ε1 = z1(k) - y(k) where, z1(·) represents the actual measured value of the first state variable, z2(·) represents the actual measured value of the second state variable, z3(·) represents the actual measured value of the third state variable, ε1 represents the system error, y(·) represents the actual output of the system, β 01 , β 02 , β 03 represent weight coefficients, c3 represents the control gain coefficient, fal(·) represents the fal function, out(·) represents the output signal of the controller, k represents the time instant, h represents the step size, α1, α2 represent slope coefficients, and δ is the threshold; S43, through the nonlinear feedback structure NLSEF, changing the state quantity and tracking target of the system from the original linear weighting to a nonlinear combination, and compensating for the unknown disturbances of the system through a nonlinear function to obtain the optimal output quantity. The specific algorithm is as follows: e1 = v h1 (k) - z1(k) e2 = v h2 (k) - z2(k) u0 = β 01 ·fal(e1, α1, δ) + β 02 ·fal(e1, α2, δ) out(k) = u0 - z3(k) / c3 v h1 (·) represents the expected value of the first state variable, v h2 (·) represents the expected value of the second state variable, z1(·) represents the actual measured value of the first state variable, z2(·) represents the actual measured value of the second state variable, z3(·) represents the actual measured value of the third state variable, u(·) represents the output signal of the controller, β 01 β 02 represent weight coefficients, α1, α2 represent slope coefficients, δ represents a threshold, c3 represents a control gain coefficient, u0 represents an initial control quantity, out(·) represents an output signal, fal(·) represents a fal function, k represents a moment, e1 represents the error between the expected value and the actual measured value of the first state variable, e2 represents the error between the expected value and the actual measured value of the second state variable; S44, simplifying and improving the structure and algorithm of the active disturbance rejection controller of the crawling robot. The specific form of the second-order equation of the controlled object is as follows: Based on the second-order equation of the controlled object and two simplified mathematical model equations of the robot in three degrees of freedom, the robot motion control equation based on ADRC can be obtained as follows: The forward distance ADRC control equation of the robot is: The depth ADRC control equation of the robot is: The pitch ADRC control equation of the robot is: Among them, represents the second derivative of the system state variable, x h , ω, t represent the system state variable, the rate of change of the state variable, the unknown disturbance quantity, and time respectively, and f(·) represents a function related to the system state variable, the rate of change of the state variable, the unknown disturbance quantity, and time. represents the rate of change of the state variable x h1 . represents the rate of change of the state variable x h2 . τ represents the control signal, x h1 represents the position state of the system, x h2 represents the velocity state of the system, and c3 represents the control gain coefficient. represents the acceleration of the robot along the z″ axis in the inertial coordinate system. represents the acceleration of the robot along the x″ axis in the inertial coordinate system. represents the acceleration of the robot rotating about the y″ axis in the inertial coordinate system. respectively represent the inertial hydrodynamic coefficients when the robot moves along the x and z axes in the carrier coordinate system and the linear accelerations are . represents the inertial hydrodynamic coefficient when the robot moves along the y axis in the carrier coordinate system and the angular acceleration is . ω1, ω2, ω3 are the disturbance quantities received by the robot in the flow field, y′ is the actual output of the system, z′ is the forward distance state of the robot, x′ is the depth state of the robot, θ′ is the pitch state of the robot, and τ1, τ3, τ5 represent the control forces in the three degrees of freedom of forward and backward movement, heave movement, and pitch movement respectively.

Images