Tandem multi-section structure underwater robot and multi-motion mode coordination control method

Abstract

The invention discloses a series multi-section underwater robot based on a bionics principle and a motion regulation and control method, and belongs to the technical field of underwater robots. The robot comprises a head part and a trunk part. The head part consists of a duct and a connecting mechanism thereof; the trunk part is composed of a plurality of identical joints, and each joint module is composed of a steering engine joint and a fastening assembly. Transverse swing, vertical axial swing, cross-medium motion and composite motion are achieved through a multi-mode cooperative control method; a stable swing mode is generated based on an anti-phase joint driving strategy and a cubic trajectory planning method; based on a thrust minimization control strategy of the duct propulsion system, in combination with a multi-body rigid body dynamic balance equation and phase constraint conditions of all steering engines, a phase optimization matrix for achieving cross-medium motion is established, and therefore cross-boundary mass motion is generated; a phase matrix of the steering engines is solved in real time through space coordinate mapping, and the pitching steering engine and the yaw steering engine are cooperatively controlled, so that composite motion is generated. The device has the advantages of modular structure, multiple motion modes, multiple degrees of freedom and the like, can adapt to pipelines, reef groups and water-air cross-medium scenes, and provides an innovative solution for ocean exploration and emergency rescue.

Description

Technical Field

[0001] The present invention belongs to the technical field of robot application, and in particular relates to a serial multi-segment structure underwater robot and a multi-motion mode coordinated control method thereof. Technical Background

[0002] As competition in the oceans intensifies, the acquisition of marine information and the utilization of marine resources have become a key focus. Underwater robots, as a high-tech, low-cost technology, can replace divers in deeper and wider waters, completing challenging research tasks, and have become a key technology currently relied upon in marine development.

[0003] Many research institutions at home and abroad have conducted research on underwater robots. Among them, a team from Shanghai University has developed the biomimetic underwater flexible snake-like robot "BaiLong," whose structural design closely mimics the morphology and motion characteristics of biological snakes. The robot's main body consists of a head, neck, four body segments, and a tail. The head integrates a hydraulic drive, control module, and power supply, forming an independent power unit. The neck serves as an independent drive unit, enabling the head to pivot relative to the body. The body segments utilize a grid-like water cavity structure made of silicone rubber, mimicking the mechanical properties of snakes' dense bony joints. Each segment contains bilaterally symmetrical semi-elliptical water cavities. An internal grid reinforcement structure suppresses longitudinal stretching, enabling controlled lateral bending deformation. A team from Tianjin University has proposed an efficient motion mode for an underwater snake-like robot with an integrated tail thruster (H. Shi, Y. Meng, W. Cui, M. Rao, S. Wang and Y. Xie, "Biomimetic Underwater SoftSnake Robot: Self-Motion Sensing and Online Gait Control," in IEEE Transactions on Robotics, vol. 41, pp. 1193-1210, 2025, doi: 10.1109 / TRO.2025.3530349). The robot utilizes a modular multi-jointed structure with seven connecting rod joints arranged sequentially from the head to the tail. A pump-jet propulsor is installed at the tail. The flexible tail fin optimizes hydrodynamic performance, forming a hybrid propulsion system that combines serpentine motion with propulsion. The system can achieve three motion modes: serpentine mode, propulsion mode, and hybrid mode. (Gao Ming, Kong Detian, Ren Chao, et al. Efficient motion mode of underwater snake-like robot with propeller at the tail [J]. Robot, 2023, 45(04): 462-471. DOI: 10.13973 / j.cnki.robot.220095.). Shanghai Bintong Intelligent Technology Co., Ltd. proposed a modular snake-like robot. The robot consists of a head joint, a tail joint and a universal joint. Each joint integrates a servo, a control circuit and a communication module. The head joint is equipped with a camera and a functional module. The universal joint has a built-in servo to drive the joint movement. The tail joint is connected to the power controller through a control line and supports USB cable or remote control handle control. In terms of motion mode, adjacent joints are installed in series at 90°, with a maximum rotation angle of 180°, supporting multi-modal motion such as forward and backward crawling, lateral movement, turning in place and vertical climbing. (Shanghai Bintong Intelligent Technology Co., Ltd. A snake-like bionic robot and its control system: CN202010045419.4[P]. 2025-01-21.)

[0004] So far, robots with serial structures have complex mechanical structures, high maintenance costs, and limited freedom of movement. However, a serial multi-segment underwater robot and its multi-motion mode coordinated control method have not been reported or studied. Summary of the Invention

[0005] The purpose of the present invention is to provide a serial multi-segment structure underwater robot and a multi-motion mode coordinated control method thereof, which solves the technical bottlenecks of traditional underwater robots, such as limited maneuverability, single motion mode and insufficient cross-media adaptability, through the deep integration of bionic mechanical structure and fluid dynamic regulation.

[0006] A serial multi-segment underwater robot, characterized by:

[0007] The underwater robot includes a head A and a body B; the head A is composed of a duct 1 and a duct connecting mechanism 2; the body B is composed of N sequentially connected servo joints, namely the first servo joint, the second servo joint, ..., the Nth servo joint; each of the servo joints has the same structure, consisting of a joint frame 3 and a pitch servo 4 mounted on the front end of the joint frame 3 and a yaw servo 5 mounted on the rear end; each of the pitch servo 4 and yaw servo 5 has the same structure, consisting of a servo body 6 and a short U-bracket 7 and a long U-bracket 8 mounted on both ends of the servo body 6; the long U-bracket 8 is mounted at the end of the servo output shaft, and its length direction is perpendicular to the servo output shaft;

[0008] In the initial state, the phases of the pitch servo 4 and the yaw servo 5 of the N servo joints are all zero initial phases, and the above N is a natural number between 6 and 10;

[0009] The zero initial phase is defined as the angle between the symmetry planes of the long U-shaped bracket 8 and the symmetry plane of the servo output shaft, when they coincide with the symmetry plane of the servo body 6. As viewed from the output end of the servo output shaft, the phase is defined as positive when the angle of the long U-shaped bracket 8 rotates counterclockwise relative to the zero initial phase, and negative when the angle of the long U-shaped bracket 8 rotates counterclockwise relative to the zero initial phase.

[0010] The multi-motion mode coordinated control method of the serial multi-stage structure underwater robot is characterized by including a lateral swing motion control method, a vertical axial swing control method, a cross-medium motion control method, and a composite motion control method.

[0011] The multi-motion mode coordinated control method of the serial multi-segment structure underwater robot is characterized by:

[0012] The lateral swing motion control method comprises the following steps:

[0013] Step 1. Divide the N consecutively connected servo joints in the body into 2M groups, where 2M is an integer factor of N, and the odd and even groups are arranged alternately;

[0014] Step 2: Establish the phase matrix of the yaw servo 5 of each servo joint in each odd-numbered and even-numbered servo joint groups in the lateral swing local coordinate system and the global coordinate system respectively;

[0015] Step 3: Based on the geometric topological relationship of the kinematic chain, a mapping relationship is generated between the phase of the yaw servo 5 of each servo joint in each odd-numbered and even-numbered servo joint group in the lateral swing local coordinate system and the phase in the lateral swing global coordinate system;

[0016] Step 4: In order to limit the amplitude of the lateral swing, set constraints on the phase in the local coordinate system of the lateral swing and the phase in the global coordinate system of the lateral swing;

[0017] Step 5: Generate the real-time phase matrix of each odd-numbered and even-numbered servo joint group through the cubic trajectory planning method to ensure the smoothness of the movement;

[0018] Step 6: The central controller drives all yaw servos 5 to deflect according to the real-time phase matrix, and triggers the phase reversal mechanism when the target phase is reached, and continues switching until a new instruction is received.

[0019] The multi-motion mode coordinated control method of the tandem multi-stage structure underwater robot is characterized by:

[0020] In the step 1 of the lateral swing motion control method, the first group of 2M groups of servo joints includes Z1 servo joints, the second group of servo joints includes Z2 servo joints, ..., the 2M-1 group of servo joints includes Z 2M-1 servo joints, the 2Mth servo joint group includes Z 2M servo joints;

[0021] In step 1, the odd and even groups are divided into odd and even groups;

[0022] In step 2, the lateral swing local coordinate system is defined as taking the center of mass of the yaw servo 5 as the origin, and establishing the lateral swing local coordinate system ∑ local (x, y, z), where the X-axis is defined as being parallel to the center axis of the servo joint, and the positive direction of the X-axis is away from the joint frame 3; the Z-axis is defined as being parallel to the output shaft of the yaw servo 5, and the positive direction of the Z-axis is the same as the output end of the yaw servo 5; and the Y-axis is defined as satisfying the right-hand rule.

[0023] In step 2, the phase in the lateral swing local coordinate system is defined as the angle between the positive direction of the X-axis and the symmetry plane of the long U-bracket 8, which includes the output shaft of the yaw servo 5. Furthermore, when viewed from the positive direction of the Z-axis toward the negative direction of the Z-axis, the phase is positive when the X-axis rotates in the positive direction until it is parallel to the symmetry plane of the long U-bracket 8, which includes the output shaft of the yaw servo 5, and negative when it rotates counterclockwise.

[0024] In step 2, the lateral swing global coordinate system is defined as taking the center of mass of the head A as the origin, and establishing the lateral swing global coordinate system ∑ global (x, y, z), where the X-axis is defined as the positive direction of the servo joint that passes through the center axis of the duct and points to the rear, and the Y-axis is defined as the positive direction that is perpendicular to the X-axis and points upward, ensuring that the xoy plane and the symmetry plane of the head A coincide. The Z-axis is defined to satisfy the right-hand rule.

[0025] In step 2, the phase in the lateral swing global coordinate system is defined as the angle between the positive direction of the X-axis and the center axis of the servo joint, and it is stipulated that when the positive direction of the X-axis rotates to be collinear with the center axis of the servo joint, the phase is positive when the positive direction of the X-axis rotates counterclockwise, and negative when the positive direction of the X-axis rotates counterclockwise.

[0026] In step 2, the mapping relationship between the phase of the yaw servo 5 of each servo joint in the odd-numbered servo joint group in the lateral swing local coordinate system and the phase in the lateral swing global coordinate system is:

[0027] In the lateral swing local coordinate system, all the servo joints of the 2i-1 servo joint group, namely Z 2i-1 The phase matrix of the yaw servo 5 in the servo joint is

[0028]

[0029] θ p(2i-1)k is the phase of the yaw servo 5 in the k-th servo joint in the 2i-1-th servo joint group in the lateral swing local coordinate system;

[0030] In the lateral swing global coordinate system, all the servo joints of the 2i-1 servo joint group, namely Z 2i-1 The phase matrix of the yaw servo 5 in the servo joint is

[0031]

[0032] is the phase of the yaw servo 5 in the k-th servo joint in the 2i-1-th servo joint group in the lateral swing global coordinate system;

[0033] In step 2, the mapping relationship between the phase of the yaw servo 5 of each servo joint in the even-numbered servo joint group in the lateral swing local coordinate system and the phase in the lateral swing global coordinate system is:

[0034] In the lateral swing local coordinate system, all the servo joints of the 2i servo joint group, namely Z 2i The phase matrix of the yaw servo 5 in the servo joint is

[0035]

[0036] θ p(2i)k is the phase of the yaw servo 5 in the k-th servo joint in the 2i-th servo joint group in the lateral swing local coordinate system;

[0037] In the lateral swing global coordinate system, all the servo joints of the 2i servo joint group, namely Z 2i The phase matrix of the yaw servo 5 in the servo joint is

[0038]

[0039] is the phase of the yaw servo 5 in the k-th servo joint in the 2i-th servo joint group in the lateral swing global coordinate system;

[0040] Among them, the 2i-1th group of servo joints is a group in the odd array, and the 2ith group of servo joints is a group in the even array, where i is a positive integer and i∈[1,M];

[0041] The mapping relationship between the phase of the yaw servo 5 of each servo joint in the odd-numbered servo joint group in step 3 in the lateral swing local coordinate system and the phase in the lateral swing global coordinate system is:

[0042]

[0043] in, is the phase accumulation sum of the yaw servo 5 in the first k servo joints in the 2i-1 th servo joint group in the lateral swing local coordinate system, It is the Zth in the 2i-2 group of servo joints. 2i-2 The phase of the yaw servo 5 in the servo joint, i.e. the last servo joint in the 2i-2 group, in the lateral swing global coordinate system; is the phase of the yaw servo 5 in the k-th servo joint in the 2i-1-th servo joint group in the lateral swing global coordinate system;

[0044] In step 3, the mapping relationship between the phase of the yaw servo 5 of each servo joint in the even-numbered servo joint group in the lateral swing local coordinate system and the phase in the lateral swing global coordinate system is:

[0045]

[0046] in, is the phase accumulation sum of the yaw servo 5 in the first k servo joints in the 2i-th servo joint group in the lateral swing local coordinate system, It is the Zth in the 2i-1 group of servo joints. 2i-1 The phase of the yaw servo 5 in the servo joint, i.e. the last servo joint in the group, in the lateral swing global coordinate system; is the phase of the yaw servo 5 in the k-th servo joint in the 2i-th servo joint group in the lateral swing global coordinate system;

[0047] Among them, the 2i-1th group of servo joints is a group in the odd array, and the 2ith group of servo joints is a group in the even array, where i is a positive integer and i∈[1,M];

[0048] In step 4, the constraint conditions include constraining the global phase of the last servo joint of all even-numbered servo joint groups to be equal to 0, and limiting the phase of the yaw servo 5 in all servo joints in the lateral swing local coordinate system and the phase in the lateral swing global coordinate system to be within a preset angle range, that is, θ p ∈[θ pmin ,θ pmax ];

[0049] In step 5, the cubic trajectory planning method generates the real-time phase matrix calculation process of each odd-group and even-group servo joint group as follows:

[0050] The real-time phase of the hth yaw servo 5 in the lateral swing local coordinate system at any time t is calculated by the cubic trajectory planning method:

[0051]

[0052] Among them, θ hf is the target phase of the hth yaw servo 5 in the lateral swing local coordinate system, θ h0 is the initial phase of the hth yaw servo 5 in the lateral swing local coordinate system, θ h (t) is the real-time phase of the hth yaw servo 5 in the lateral swing local coordinate system at any time t; t hf is the time when the hth yaw servo is expected to reach the target phase, which is determined according to the requirements; t is the current time, which is obtained by the timer;

[0053] In the lateral swing local coordinate system, at any time t, all the servo joints of the 2i-1th group of servo joints in the odd group, namely Z 2i-1 The real-time phase matrix of the yaw servo 5 in the servo joint is

[0054]

[0055] Among them, θ p(2i-1) (t) is the position of all the servo joints of the 2i-1th servo joint group in the lateral swing local coordinate system at any time t, i.e., Z 2i-1 The real-time phase matrix of the yaw servo 5 in the servo joint, θ p(2i-1)k (t) is the real-time phase of the yaw servo 5 in the kth servo joint in the 2i-1th servo joint group in the lateral swing local coordinate system at any time t;

[0056] In the lateral swing local coordinate system, at any time t, all the servo joints of the 2ith group of servo joints in the even group, namely Z 2i The real-time phase matrix of the yaw servo 5 in the servo joint is

[0057]

[0058] Among them, θ p(2i) (t) is the position of all the servo joints of the 2i-th servo joint group in the lateral swing local coordinate system at any time t, i.e., Z 2i-1 The real-time phase matrix of the yaw servo 5 in the servo joint, θ p(2i)k (t) is the real-time phase of the yaw servo 5 in the kth servo joint in the 2ith servo joint group in the lateral swing local coordinate system at any time t;

[0059] The specific process of the phase reversal mechanism in step 6 is as follows: the real-time phase matrix θ of the yaw servo 5 is converted to p(2i-1) (t), θ p(2i) Each element in (t) is multiplied by a scalar -1 to generate the inverse real-time phase matrix Θ p(2i-1) (t), Θ p(2i) (t);

[0060] The specific process of step 6, switching until receiving a new instruction, is as follows: the above process is executed cyclically, and the yaw servo 5 is controlled according to the real-time phase matrix θ p(2i-1) (t), θ p(2i) (t) and the inverse real-time phase matrix Θ p(2i-1) (t), Θ p(2i) (t) Deflect until the central controller receives a new action instruction.

[0061] The multi-motion mode coordinated control method of the serial multi-segment structure underwater robot is characterized by:

[0062] The vertical axial swing motion control method comprises the following steps:

[0063] Step 1. Divide the N consecutively connected servo joints in the body into 2M groups, where 2M is an integer factor of N, and the odd and even groups are arranged alternately;

[0064] Step 2: Establish the phase matrix of the pitch servo 4 of each servo joint in each odd-numbered and even-numbered servo joint groups in the vertical axial swing local coordinate system and the global coordinate system respectively;

[0065] Step 3: Generate a mapping relationship between the phase of the pitch servo 4 of each servo joint in each odd-numbered and even-numbered servo joint groups in the vertical axial swing local coordinate system and the phase in the vertical axial swing global coordinate system based on the geometric topological relationship of the kinematic chain;

[0066] Step 4: In order to limit the amplitude of the vertical axis swing, set constraints on the phase in the local coordinate system of the vertical axis swing and the phase in the global coordinate system of the vertical axis swing;

[0067] Step 5: Generate the real-time phase matrix of each odd-numbered and even-numbered servo joint group through the cubic trajectory planning method to ensure the smoothness of the movement;

[0068] Step 6: The central controller drives all pitch servos 4 to deflect according to the real-time phase matrix, and triggers the phase reversal mechanism when the target phase is reached, and continues switching until a new instruction is received.

[0069] The multi-motion mode coordinated control method of the tandem multi-stage structure underwater robot is characterized by:

[0070] In the step 1 of the vertical axial swing motion control method, the first group of 2M groups of servo joints includes Z1 servo joints, the second group of servo joints includes Z2 servo joints, ..., the 2M-1 group of servo joints includes Z 2M-1 servo joints, the 2Mth servo joint group includes Z 2M servo joints;

[0071] In step 1, the odd and even groups are divided into odd and even groups;

[0072] In step 2, the vertical axial swing local coordinate system is defined as taking the center of mass of the pitch servo 4 as the origin, and establishing the vertical axial swing local coordinate system ∑ local(x, y, z), where the X-axis is defined as being parallel to the center axis of the servo joint, and the positive direction of the X-axis is away from the joint frame 3; the Z-axis is defined as being parallel to the output shaft of the pitch servo 4, and the positive direction of the Z-axis is the same as the output end of the pitch servo 4; the Y-axis is defined as satisfying the right-hand rule;

[0073] In step 2, the phase in the vertical axial swing local coordinate system is defined as the angle between the positive direction of the X-axis and the symmetry plane of the long U-bracket 8 containing the output shaft of the pitch servo 4, and it is stipulated that when viewed from the positive direction of the z-axis toward the negative direction of the z-axis, when the positive direction of the X-axis rotates to be parallel to the symmetry plane of the long U-bracket 8 containing the output shaft of the pitch servo 4, the phase is positive when rotating counterclockwise, and negative when it is reversed;

[0074] In step 2, the vertical axial swing global coordinate system is defined as taking the center of mass of the head A as the origin, and establishing the vertical axial swing global coordinate system ∑ global (x, y, z), where the X-axis is defined as the positive direction of the servo joint that passes through the center axis of the duct and points to the rear, and the Y-axis is defined as the positive direction that is perpendicular to the X-axis and points upward, ensuring that the xoy plane and the symmetry plane of the head A coincide. The Z-axis is defined to satisfy the right-hand rule.

[0075] In step 2, the phase in the vertical axial swing global coordinate system is defined as the angle between the positive direction of the X-axis and the center axis of the servo joint, and it is stipulated that when the positive direction of the X-axis rotates to be collinear with the center axis of the servo joint, the phase is positive when the positive direction of the X-axis rotates counterclockwise, and negative when the positive direction of the X-axis rotates counterclockwise.

[0076] In step 2, the mapping relationship between the phase of the pitch servo 4 of each servo joint in the odd-numbered servo joint group in the vertical axial swing local coordinate system and the phase in the vertical axial swing global coordinate system is:

[0077] In the vertical axial swing local coordinate system, all the servo joints of the 2i-1 servo joint group, namely Z 2i-1 The phase matrix of the pitch servo 4 in the servo joint is

[0078]

[0079] θ f(2i-1)k is the phase of the pitch servo 4 in the kth servo joint in the 2i-1th servo joint group in the vertical axial swing local coordinate system;

[0080] In the vertical axial swing global coordinate system, all the servo joints of the 2i-1 servo joint group, namely Z 2i-1 The phase matrix of the pitch servo 4 in the servo joint is

[0081]

[0082] is the phase of the pitch servo 4 in the k-th servo joint in the 2i-1-th servo joint group in the vertical axial swing global coordinate system;

[0083] In step 2, the mapping relationship between the phase of the pitch servo 4 of each servo joint in the even-numbered servo joint group in the vertical axial swing local coordinate system and the phase in the vertical axial swing global coordinate system is:

[0084] In the vertical axial swing local coordinate system, all the servo joints of the 2i servo joint group, namely Z 2i The phase matrix of the pitch servo 4 in the servo joint is

[0085]

[0086] θ f(2i)k is the phase of the pitch servo 4 in the kth servo joint in the 2ith servo joint group in the vertical axial swing local coordinate system;

[0087] In the vertical axial swing global coordinate system, all the servo joints of the 2i servo joint group, namely Z 2i The phase matrix of the pitch servo 4 in the servo joint is

[0088]

[0089] is the phase of the pitch servo 4 in the k-th servo joint in the 2i-th servo joint group in the vertical axial swing global coordinate system;

[0090] Among them, the 2i-1th group of servo joints is a group in the odd array, and the 2ith group of servo joints is a group in the even array, where i is a positive integer and i∈[1,M];

[0091] The mapping relationship between the phase of the pitch servo 4 of each servo joint in the odd-numbered servo joint group in step 3 in the vertical axial swing local coordinate system and the phase in the vertical axial swing global coordinate system is:

[0092]

[0093] in, It is the phase accumulation sum of the pitch servo 4 in the first k servo joints in the 2i-1 th group of servo joints in the vertical axial swing local coordinate system, It is the Zth in the 2i-2 group of servo joints. 2i-2 The phase of the pitch servo 4 in the last servo joint in the 2i-2 group in the vertical axial swing global coordinate system; is the phase of the pitch servo 4 in the k-th servo joint in the 2i-1-th servo joint group in the vertical axial swing global coordinate system;

[0094] In step 3, the mapping relationship between the phase of the pitch servo 4 of each servo joint in the even-numbered servo joint group in the vertical axial swing local coordinate system and the phase in the vertical axial swing global coordinate system is:

[0095]

[0096] in, is the phase accumulation sum of the pitch servo 4 in the first k servo joints in the 2i-th servo joint group in the vertical axial swing local coordinate system, It is the Zth in the 2i-1 group of servo joints. 2i-1 The phase of the pitch servo 4 in the last servo joint in the group in the vertical axial swing global coordinate system; is the phase of the pitch servo 4 in the k-th servo joint in the 2i-th servo joint group in the vertical axial swing global coordinate system;

[0097] Among them, the 2i-1th group of servo joints is a group in the odd array, and the 2ith group of servo joints is a group in the even array, where i is a positive integer and i∈[1,M];

[0098] In step 4, the constraint conditions include constraining the global phase of the last servo joint of all even-numbered servo joint groups to be equal to 0, and limiting the phase of the pitch servo 4 in all servo joints in the vertical axial swing local coordinate system and the phase in the vertical axial swing global coordinate system to be within a preset angle range, that is, θ p ∈[θ pmin ,θ pmax ];

[0099] In step 5, the cubic trajectory planning method generates the real-time phase matrix calculation process of each odd-group and even-group servo joint group as follows:

[0100] The real-time phase of the h-th pitch servo 4 in the vertical axial swing local coordinate system at any time t is calculated by the cubic trajectory planning method:

[0101]

[0102] Among them, θ hf is the target phase of the h-th pitch servo 4 in the vertical axial swing local coordinate system, θ h0 is the initial phase of the h-th pitch servo 4 in the local coordinate system of vertical axial swing, θ h(t) is the real-time phase of the h-th pitch servo 4 in the vertical axial swing local coordinate system at any time t; t hf is the time when the hth pitch servo 4 is expected to reach the target phase, which is determined according to the requirements; t is the current time, which is obtained by the timer;

[0103] In the vertical axial swing local coordinate system, at any time t, all the servo joints of the 2i-1th group of servo joints in the odd group, namely Z 2i-1 The real-time phase matrix of the pitch servo 4 in the servo joint is

[0104]

[0105] Among them, θ f(2i-1) (t) is the total number of servo joints in the 2i-1th servo joint group in the vertical axial swing local coordinate system at any time t, i.e., Z 2i-1 The real-time phase matrix of the pitch servo 4 in the servo joint, θ f(2i-1)k (t) is the real-time phase of the pitch servo 4 in the kth servo joint in the 2i-1th servo joint group in the vertical axial swing local coordinate system at any time t;

[0106] In the vertical axial swing local coordinate system, at any time t, all the servo joints of the 2i-th group of servo joints in the even group, namely, Z 2i The real-time phase matrix of the pitch servo 4 in the servo joint is

[0107]

[0108] Among them, θ f(2i) (t) is the coordinate of all the servo joints of the 2i-th servo joint group in the vertical axial swing local coordinate system at any time t, i.e., Z 2i-1 The real-time phase matrix of the pitch servo 4 in the servo joint, θ f(2i)k (t) is the real-time phase of the pitch servo 4 in the kth servo joint in the 2ith servo joint group in the vertical axial swing local coordinate system at any time t;

[0109] The specific process of the phase reversal mechanism in step 6 is as follows: the real-time phase matrix θ of the pitch servo 4 is converted to f(2i-1) (t), θ f(2i) Each element in (t) is multiplied by a scalar -1 to generate the inverse real-time phase matrix Θ f(2i-1) (t), Θ f(2i) (t);

[0110] The specific process of step 6, switching until receiving a new instruction, is as follows: the above process is executed cyclically, and the pitch servo 4 is controlled according to the real-time phase matrix θ f(2i-1)(t), θ f(2i) (t) and the inverse real-time phase matrix Θ f(2i-1) (t), Θ f(2i) (t) Deflect until the central controller receives a new action instruction.

[0111] The multi-motion mode coordinated control method of the serial multi-segment structure underwater robot is characterized by:

[0112] The cross-media motion control method comprises the following steps:

[0113] Step 1. Based on the thrust minimization control strategy of the ducted propulsion system, combined with the multi-body rigid body dynamics equilibrium equations in the global coordinate system of cross-medium motion and the phase constraints of each servo, establish the phase optimization matrix of each servo in the local coordinate system of cross-medium motion to achieve cross-medium motion;

[0114] Step 2. The central controller applies a preset pulse width modulation signal to the pitch servo 4 in each servo joint, driving the long U-bracket 8 of the pitch servo 4 to complete deflection according to the phase optimization matrix generated in Step 1, thereby constructing the cross-medium motion preparation action configuration;

[0115] Step 3. Based on the phase optimization matrix constructed in Step 1, duct 1 of head A outputs a propulsion force with space vector characteristics, using the vertical component of the propulsion force to drive the system to break through the water-air interface;

[0116] Step 4. The duct propulsion force ceases to work after leaving the water, and the system is only affected by gravity, and the motion trajectory is approximately an oblique parabola; under the action of gravity, the system completes the water re-entry motion along the parabolic trajectory, thereby completing the cross-medium motion control.

[0117] The multi-motion mode coordinated control method of the serial multi-segment structure underwater robot is characterized by:

[0118] In the step 1 of the cross-medium motion control method, the cross-medium motion local coordinate system is defined as taking the mass center of the pitch servo 4 as the origin, and establishing the cross-medium motion local coordinate system ∑ local (x, y, z), where the X-axis is defined as being parallel to the center axis of the servo joint, and the positive direction of the X-axis is away from the joint frame 3; the Z-axis is defined as being parallel to the output shaft of the yaw servo 5, and the positive direction of the Z-axis is the same as the output end of the yaw servo 5; and the Y-axis is defined as satisfying the right-hand rule.

[0119] The phase in the local coordinate system of cross-media movement is defined as the angle between the positive direction of the X-axis and the symmetry plane of the long U-shaped bracket 8 that contains the output shaft of the yaw servo 5. It is stipulated that when looking from the positive direction of the z-axis to the negative direction of the z-axis, when the positive direction of the X-axis rotates to be parallel to the symmetry plane of the long U-shaped bracket 8 that contains the output shaft of the yaw servo 5, the phase is positive when rotating counterclockwise, and negative otherwise;

[0120] In step 1, the global coordinate system of cross-media movement is defined with the centroid of the m-th servo joint as the origin, and the global coordinate system ∑ global (x, y, z) is established, where m < N, and N is the number of all servo joints in the system; the positive direction of the X-axis of the global coordinate system of cross-media movement is defined as horizontally to the right, the positive direction of the Y-axis is defined as vertically upward, and the Z-axis is defined to satisfy the right-hand rule;

[0121] The phase in the global coordinate system of cross-media movement is defined as the angle between the positive direction of the X-axis and the central axis of the servo joint, and it is stipulated that when looking from the positive direction of the z-axis to the negative direction of the z-axis, when the positive direction of the X-axis rotates to be collinear with the central axis of the servo joint, the phase is positive when rotating counterclockwise, and negative otherwise;

[0122] In step 1, the establishment of the multi-body rigid dynamics equilibrium equation in the global coordinate system of cross-media movement includes the establishment of the force equilibrium equation and the process torque equation:

[0123] Establishment of the force equilibrium equation:

[0124]

[0125] Where is the sum of the weights of N servo joints, that is, all servo joints, and the weight of the head A, which is the total weight of the system; M is the total mass of the system, and the calculation formula is That is, the total mass of the system is equal to the sum of the weights of N servo joints and the weight of the head A divided by the acceleration due to gravity, F y is the vertical component of the ducted propulsion force received by the head, a y is the vertical component of the acceleration of the system from the start of movement to when it exits the water, and g is the acceleration due to gravity;

[0126] Establishment of the torque equation:

[0127]

[0128] Among them, R j is the position vector of the action point of the external force received by the j-th servo joint; specifically, when j = 0, R0 represents the position vector of the action point of the external force received by the head A; F j is the external force vector received by the j-th servo joint; specifically, when j = 0, F0 represents the external force vector received by the head A;

[0129] The process of establishing the force balance equation is as follows:

[0130] Based on the need for engineering simplification, the following assumptions are made: air resistance and mechanical transmission friction are ignored; the ducted propulsion force ceases to function after exiting the water and is only affected by gravity, and the motion trajectory is approximately an oblique parabola;

[0131] According to the law of oblique projection motion, the vertical component of the velocity of the system when it leaves the water is

[0132]

[0133] Where g is the acceleration of gravity, t is the duration of the air movement, and t can be determined according to the duration of the system's operation in the air; v y is the vertical component of the velocity of the system when it leaves the water;

[0134] Acceleration of the system in water

[0135]

[0136] Among them, a y is the vertical component of the acceleration of the system from the beginning of the movement to the time it leaves the water, and H is the vertical distance from the center of mass of the system head A to the water surface;

[0137] The process of establishing the torque equation is as follows:

[0138] In the global coordinate system of cross-medium motion, the force states of each component are as follows:

[0139] The position vector of the point where the resultant external force is applied to the j-th servo joint:

[0140]

[0141] Among them, l m is the length of the mth servo joint, where m is defined so that the center of mass of the mth servo joint is the origin of the global coordinate system for cross-medium motion; l j The length of the jth servo joint; in particular, when j = 0, l0 is the length of the head A, R j =R0, R0 represents the position vector of the point where the resultant external force is applied to the head A; is the phase of the pitch servo 4 in the j+1th servo joint in the global coordinate system of cross-medium motion, is the phase of the pitch servo 4 in the j-th servo joint in the global coordinate system of cross-medium motion;

[0142] The resultant external force vector on the j-th servo joint is:

[0143]

[0144] Fjx ,F jy are the x and y components of the net external force on the j-th servo joint except gravity, G j is the weight of the jth servo joint; in particular, when j = 0, G0 is the weight of the head A, and the formula F j =F0, F0 represents the resultant external force vector on the head A;

[0145]

[0146] Among them, F 0x ,F 0y G is the horizontal and vertical components of the duct propulsion force on the head, j is the weight of the jth servo joint;

[0147] The phase constraint conditions of each servo include constraining the phase range of each pitch servo 4 in the local coordinate system of cross-medium motion and the phase range in the global coordinate system of cross-medium motion:

[0148]

[0149] Among them, θ fj is the phase of the pitch servo 4 in the j-th servo joint in the local coordinate system of cross-medium motion, is the phase of the pitch servo 4 in the j-th servo joint in the global coordinate system of cross-medium motion;

[0150] The process of establishing the phase optimization matrix of each servo in the local coordinate system of the cross-medium motion for realizing the cross-medium motion in step 1 includes: constructing a mapping function between the phase in the local coordinate system of the cross-medium motion and the phase in the global coordinate system of the cross-medium motion, minimizing the ducted propulsion force F by numerical optimization, and obtaining the phase optimization matrix of each servo in the local coordinate system of the cross-medium motion;

[0151] The mapping function between the phase in the local coordinate system of cross-medium motion and the phase in the global coordinate system of cross-medium motion is:

[0152]

[0153] Among them, θ fi is the phase of the pitch servo 4 in the i-th servo joint in the local coordinate system of cross-medium motion, is the phase of the pitch servo 4 in the i-th servo joint in the global coordinate system of cross-medium motion;

[0154] The numerical optimization process includes defining the objective function, setting constraints, selecting the optimization algorithm, and outputting the optimized phase matrix. The specific numerical optimization process includes the following steps:

[0155] Step 1. Define the objective function: minimize the propulsion force F, that is,

[0156] Among them, θ f =[θ f1 ,θ f2 ,…,θ f(N-1) ,θ fN ] T is the phase matrix of the pitch servo 4 in the local coordinate system of cross-medium motion, is the phase of the pitch servo 4 in the mth servo joint in the global coordinate system of cross-medium motion, which is obtained based on the mapping function between the phase in the local coordinate system of cross-medium motion and the phase in the global coordinate system of cross-medium motion. The definition of m satisfies that the mth servo joint is the origin of the global coordinate system of the center of mass moving across the medium; It is the sum of the weight of N servo joints and the weight of the head A, that is, the total weight of the system; M is the total mass of the system. The calculation formula is That is, the total mass of the system is equal to the sum of the weight of the N servo joints and the weight of the head A divided by the acceleration of gravity; a y It is the vertical component of the acceleration of the system from the beginning of motion to the time it leaves the water;

[0157] Step 2. Set constraints: including nonlinear equality constraints and linear inequality constraints, where the nonlinear equality constraints are torque equations, and the linear inequality constraints are the phase ranges of each pitch servo 4 in the local coordinate system of cross-medium motion and the phase ranges in the global coordinate system of cross-medium motion;

[0158] Step 3. Select the optimization algorithm: A hybrid optimization strategy is used to achieve global convergence and improve local accuracy. In the global search phase, the initial feasible solution set is generated based on the genetic algorithm. In the local optimization phase, the sequential quadratic programming algorithm is used to perform gradient iteration to optimize the objective function.

[0159] Step 4. Output the optimized phase matrix: Obtain the phase matrix θ=[θ f1 * ,θ f2 * ,…,θ f(N-1) * ,θ fN * ] T .

[0160] The multi-motion mode coordinated control method of the serial multi-segment structure underwater robot is characterized by:

[0161] The composite motion control method comprises the following steps:

[0162] Step 1. Define the composite motion trajectory equation according to the actual project needs and project the trajectory into the polar coordinate system;

[0163] Step 2. Based on the geometric topological relationship of the kinematic chain, establish the servo phase matrix of the yaw servo 5 and the pitch servo 4 in the local coordinate system of the composite motion;

[0164] Step 3. Apply a pulse width modulation signal to the servo group through the central controller to drive the servo to deflect according to the phase matrix of the servo in the local coordinate system of the compound motion, thereby completing the compound motion control.

[0165] The multi-motion mode coordinated control method of the serial multi-segment structure underwater robot is characterized by:

[0166] In the step 1 of the composite motion control method, the composite motion trajectory equation is defined as:

[0167]

[0168] The process of projecting the trajectory to the polar coordinate system in step 1 includes:

[0169] Project the composite motion trajectory at time t onto the xoy plane and establish a polar coordinate system on the xoy plane. At any time t, the trajectory of the motion trajectory projected onto the xoy plane in the polar coordinate system is r(α, t), where α is the polar angle and r is the polar radius. The α and r corresponding to the center of mass of each servo joint are determined by the following formula:

[0170]

[0171] Project the composite motion trajectory at time t onto the xoz plane and establish a polar coordinate system on the xoz plane. At any time t, the trajectory of the motion trajectory projected onto the xoz plane in the polar coordinate system is ρ(β,t), where β is the polar angle and ρ is the polar radius. The β and ρ corresponding to the center of mass of each servo joint are determined by the following formula:

[0172]

[0173] In step 2, the composite motion local coordinate system is defined as follows: a composite motion local coordinate system is established with the center of mass of each pitch servo 4 and yaw servo 5 as the origin, wherein the X-axis is defined as being parallel to the center axis of the servo joint, and the direction away from the joint frame 3 is the positive direction of the X-axis; the Z-axis is defined as being parallel to the servo output shaft, and the positive direction of the Z-axis is the same as the direction of the output end of the servo 5; and the Y-axis is defined as satisfying the right-hand rule;

[0174] In step 2, the phase in the local coordinate system of the compound motion is defined as the angle between the positive direction of the X-axis and the symmetry plane of the long U-bracket 8 containing the servo output shaft, and it is stipulated that when viewed from the positive direction of the Z-axis toward the negative direction of the Z-axis, the phase is positive when the positive direction of the X-axis rotates until it is parallel to the symmetry plane of the long U-bracket 8 containing the servo output shaft, and negative when it rotates counterclockwise;

[0175] In step 2, the servo phase matrices of the yaw servo 5 and the pitch servo 5 in the compound motion local coordinate system are respectively:

[0176] θ p =[θ p1 ,θ p2 ,…,θ pn ,…θ p(N-1) ,θ pN ] T

[0177] Among them, θ pn is the phase of the nth yaw servo 5 in the local coordinate system of the composite motion; θ p is the phase matrix of N yaw servos 5;

[0178] θ f =[θ f1 ,θ f2 ,…,θ fn ,…,θ f(N-1) ,θ fN ] T

[0179] Among them, θ fn is the phase of the nth pitch servo 4 in the local coordinate system of the compound motion; θ f is the phase matrix of N pitch servos 4;

[0180] The phases of the nth yaw servo 5 and the nth pitch servo 4 in the compound motion local coordinate system are respectively determined by the tangent angle change at the mass center of the adjacent yaw servo 5 and the tangent angle change at the mass center of the adjacent pitch servo 4:

[0181] θ pn =φ(α n+1 ,t)-φ(α n ,t)

[0182] Among them, φ(α n+1 ,t) is the change of the tangent angle of the mass center of the n+1th yaw servo 5 with the polar angle, φ(α n ,t) is the change of the tangent angle of the mass center of the nth yaw servo 5 with the polar angle, θ pn is the phase of the nth yaw servo 5 in the local coordinate system of the composite motion;

[0183] θ fn =φ(β n+1 ,t)-φ(β n ,t)

[0184] Among them, φ(β n+1 ,t) is the change of the tangent angle of the mass center of the n+1th pitch servo 4 with the polar angle, φ(β n ,t) is the change of the tangent angle of the mass center of the nth pitch servo 4 with the polar angle, θ fn is the phase of the nth pitch servo 4 in the local coordinate system of the compound motion;

[0185] The calculation formulas for the mass center tangent angle of the nth yaw servo 5 and the mass center tangent angle of the nth pitch servo 4 are:

[0186] The calculation formula of the mass center tangent angle of the nth yaw servo 5 is:

[0187]

[0188] r(α, t) is the trajectory of the motion projected onto the xoy plane in the polar coordinate system at any time t, and r'(α, t) is the partial derivative of the radial distance with respect to the polar angle;

[0189] The calculation formula for the tangent angle of the mass center of the nth pitch servo 4 is:

[0190]

[0191] ρ(β n ,t) is the trajectory of the motion trajectory projected onto the xoz plane in the polar coordinate system at any time t, ρ'(β n ,t) is the partial derivative of radial distance with respect to polar angle;

[0192] The calculation formula of the partial derivative of the radial distance with respect to the polar angle is:

[0193]

[0194] r(α, t) is the trajectory of the motion projected onto the xoy plane in the polar coordinate system at any time t, and r'(α, t) is the partial derivative of the radial distance with respect to the polar angle;

[0195]

[0196] ρ(β n ,t) is the trajectory of the motion trajectory projected onto the xoz plane in the polar coordinate system at any time t, ρ'(β n ,t) is the partial derivative of the radial distance with respect to the polar angle.

[0197] Compared with the prior art, the present invention has the following advantages:

[0198] 1. The present invention can achieve multi-modal coordinated control of lateral swing, vertical axial swing, cross-medium movement and composite movement with multiple degrees of freedom and high movement flexibility;

[0199] 2. The present invention adopts a standardized joint module series configuration. Each module integrates 3D printed parts and waterproof steering gear components, supporting fast disassembly and maintenance, with low maintenance costs;

[0200] 3. The invention has no oil pressure and low noise propulsion with a sound intensity of ≤85dB, meeting the operation requirements in ecologically sensitive areas.

[0201] 4. The present invention has an ingenious structure, small size, easy processing, and economic feasibility, and can adapt to complex underwater environments such as reefs, narrow pipelines, and water-to-air cross-media mission requirements. BRIEF DESCRIPTION OF THE DRAWINGS

[0202] Figure 1 This is a front view of a serial multi-segment underwater robot according to the present invention;

[0203] Figure 2 This is an exploded view of the head of a serial multi-segment underwater robot according to the present invention;

[0204] Figure 3 This is an exploded view of a tandem multi-stage underwater robot servo according to the present invention;

[0205] Figure 4 It is a schematic diagram of the cross-medium motion preparation action configuration of a serial multi-segment structure underwater robot for cross-medium motion according to the present invention.

[0206] Figure 1-4 Name of the bid number: A, head; B, first joint; C, second joint; D, third joint; E, fourth joint; F, fifth joint; G, sixth joint; 3, joint; 4, pitch servo; 5, yaw servo; 6, servo body; 7, short U bracket; 8, long U bracket; 9, bolt; 10, M3 nut DETAILED DESCRIPTION

[0207] The present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments:

[0208] Combine Figure 1-4The present invention aims to provide a serial multi-stage underwater robot, characterized in that: the underwater robot includes a head A and a body B; the head A is composed of a duct 1 and a duct connection mechanism 2; the body B is composed of N sequentially connected servo joints, namely the first servo joint, the second servo joint, ..., the Nth servo joint; each of the servo joints has the same structure, consisting of a joint frame 3 and a pitch servo 4 mounted on the front end of the joint frame 3 and a yaw servo 5 mounted on the rear end; each of the pitch servo 4 and yaw servo 5 has the same structure, consisting of a servo body 6 and a short U bracket 7 and a long U bracket 8 mounted on both ends of the servo body 6; the long U bracket 8 is mounted at the end of the servo output shaft, and its length direction is perpendicular to the servo output shaft;

[0209] In the initial state, the phases of the pitch servo 4 and the yaw servo 5 of the N servo joints are all zero initial phases, and the above N is a natural number between 6 and 10;

[0210] The zero initial phase is defined as the angle between the symmetry planes of the long U-shaped bracket 8 and the symmetry plane of the servo output shaft, when they coincide with the symmetry plane of the servo body 6. As viewed from the output end of the servo output shaft, the phase is defined as positive when the angle of the long U-shaped bracket 8 rotates counterclockwise relative to the zero initial phase, and negative when the angle of the long U-shaped bracket 8 rotates counterclockwise relative to the zero initial phase.

[0211] 2. The present invention aims to provide a multi-motion mode coordinated control method for a tandem multi-stage underwater robot, characterized by: including a lateral swing motion control method, a vertical axial swing control method, a cross-medium motion control method, and a composite motion control method;

[0212] 3. The multi-motion mode coordinated control method of the tandem multi-segment structure underwater robot is characterized by:

[0213] The lateral swing motion control method comprises the following steps:

[0214] Step 1. Divide the N consecutively connected servo joints in the body into 2M groups, where 2M is an integer factor of N, and the odd and even groups are arranged alternately;

[0215] Step 2: Establish the phase matrix of the yaw servo 5 of each servo joint in each odd-numbered and even-numbered servo joint groups in the lateral swing local coordinate system and the global coordinate system respectively;

[0216] Step 3: Based on the geometric topological relationship of the kinematic chain, a mapping relationship is generated between the phase of the yaw servo 5 of each servo joint in each odd-numbered and even-numbered servo joint group in the lateral swing local coordinate system and the phase in the lateral swing global coordinate system;

[0217] Step 4: In order to limit the amplitude of the lateral swing, set constraints on the phase in the local coordinate system of the lateral swing and the phase in the global coordinate system of the lateral swing;

[0218] Step 5: Generate the real-time phase matrix of each odd-numbered and even-numbered servo joint group through the cubic trajectory planning method to ensure the smoothness of the movement;

[0219] Step 6: The central controller drives all yaw servos 5 to deflect according to the real-time phase matrix, and triggers the phase reversal mechanism when the target phase is reached, and continues switching until a new instruction is received.

[0220] 4. The multi-motion mode coordinated control method of the tandem multi-stage structure underwater robot is characterized by:

[0221] In the step 1 of the lateral swing motion control method, the first group of 2M groups of servo joints includes Z1 servo joints, the second group of servo joints includes Z2 servo joints, ..., the 2M-1 group of servo joints includes Z 2M-1 servo joints, the 2Mth servo joint group includes Z 2M servo joints;

[0222] In step 1, the odd and even groups are divided into odd and even groups;

[0223] In step 2, the lateral swing local coordinate system is defined as taking the center of mass of the yaw servo 5 as the origin, and establishing the lateral swing local coordinate system ∑ local (x, y, z), where the X-axis is defined as being parallel to the center axis of the servo joint, and the positive direction of the X-axis is away from the joint frame 3; the Z-axis is defined as being parallel to the output shaft of the yaw servo 5, and the positive direction of the Z-axis is the same as the output end of the yaw servo 5; and the Y-axis is defined as satisfying the right-hand rule.

[0224] In step 2, the phase in the lateral swing local coordinate system is defined as the angle between the positive direction of the X-axis and the symmetry plane of the long U-bracket 8, which includes the output shaft of the yaw servo 5. Furthermore, when viewed from the positive direction of the Z-axis toward the negative direction of the Z-axis, the phase is positive when the X-axis rotates in the positive direction until it is parallel to the symmetry plane of the long U-bracket 8, which includes the output shaft of the yaw servo 5, and negative when it rotates counterclockwise.

[0225] In step 2, the lateral swing global coordinate system is defined as taking the center of mass of the head A as the origin, and establishing the lateral swing global coordinate system ∑ global (x, y, z), where the X-axis is defined as the positive direction of the servo joint that passes through the center axis of the duct and points to the rear, and the Y-axis is defined as the positive direction that is perpendicular to the X-axis and points upward, ensuring that the xoy plane and the symmetry plane of the head A coincide. The Z-axis is defined to satisfy the right-hand rule.

[0226] In step 2, the phase in the lateral swing global coordinate system is defined as the angle between the positive direction of the X-axis and the center axis of the servo joint, and it is stipulated that when the positive direction of the X-axis rotates to be collinear with the center axis of the servo joint, the phase is positive when the positive direction of the X-axis rotates counterclockwise, and negative when the positive direction of the X-axis rotates counterclockwise.

[0227] In step 2, the mapping relationship between the phase of the yaw servo 5 of each servo joint in the odd-numbered servo joint group in the lateral swing local coordinate system and the phase in the lateral swing global coordinate system is:

[0228] In the lateral swing local coordinate system, all the servo joints of the 2i-1 servo joint group, namely Z 2i-1 The phase matrix of the yaw servo 5 in the servo joint is

[0229]

[0230] θ p(2i-1)k is the phase of the yaw servo 5 in the k-th servo joint in the 2i-1-th servo joint group in the lateral swing local coordinate system;

[0231] In the lateral swing global coordinate system, all the servo joints of the 2i-1 servo joint group, namely Z 2i-1 The phase matrix of the yaw servo 5 in the servo joint is

[0232]

[0233] is the phase of the yaw servo 5 in the k-th servo joint in the 2i-1-th servo joint group in the lateral swing global coordinate system;

[0234] In step 2, the mapping relationship between the phase of the yaw servo 5 of each servo joint in the even-numbered servo joint group in the lateral swing local coordinate system and the phase in the lateral swing global coordinate system is:

[0235] In the lateral swing local coordinate system, all the servo joints of the 2i servo joint group, namely Z 2i The phase matrix of the yaw servo 5 in the servo joint is

[0236]

[0237] θ p(2i)k is the phase of the yaw servo 5 in the k-th servo joint in the 2i-th servo joint group in the lateral swing local coordinate system;

[0238] In the lateral swing global coordinate system, all the servo joints of the 2i servo joint group, namely Z 2iThe phase matrix of the yaw servo 5 in the servo joint is

[0239]

[0240] is the phase of the yaw servo 5 in the k-th servo joint in the 2i-th servo joint group in the lateral swing global coordinate system;

[0241] Among them, the 2i-1th group of servo joints is a group in the odd array, and the 2ith group of servo joints is a group in the even array, where i is a positive integer and i∈[1,M];

[0242] The mapping relationship between the phase of the yaw servo 5 of each servo joint in the odd-numbered servo joint group in step 3 in the lateral swing local coordinate system and the phase in the lateral swing global coordinate system is:

[0243]

[0244] in, is the phase accumulation sum of the yaw servo 5 in the first k servo joints in the 2i-1 th servo joint group in the lateral swing local coordinate system, It is the Zth in the 2i-2 group of servo joints. 2i-2 The phase of the yaw servo 5 in the servo joint, i.e. the last servo joint in the 2i-2 group, in the lateral swing global coordinate system; is the phase of the yaw servo 5 in the k-th servo joint in the 2i-1-th servo joint group in the lateral swing global coordinate system;

[0245] In step 3, the mapping relationship between the phase of the yaw servo 5 of each servo joint in the even-numbered servo joint group in the lateral swing local coordinate system and the phase in the lateral swing global coordinate system is:

[0246]

[0247] in, is the phase accumulation sum of the yaw servo 5 in the first k servo joints in the 2i-th servo joint group in the lateral swing local coordinate system, It is the Zth in the 2i-1 group of servo joints. 2i-1 The phase of the yaw servo 5 in the servo joint, i.e. the last servo joint in the group, in the lateral swing global coordinate system; is the phase of the yaw servo 5 in the k-th servo joint in the 2i-th servo joint group in the lateral swing global coordinate system;

[0248] Among them, the 2i-1th group of servo joints is a group in the odd array, and the 2ith group of servo joints is a group in the even array, where i is a positive integer and i∈[1,M];

[0249] In step 4, the constraint conditions include constraining the global phase of the last servo joint of all even-numbered servo joint groups to be equal to 0, and limiting the phase of the yaw servo 5 in all servo joints in the lateral swing local coordinate system and the phase in the lateral swing global coordinate system to be within a preset angle range, that is, θ p ∈[θ pmin ,θ pmax ];

[0250] In step 5, the cubic trajectory planning method generates the real-time phase matrix calculation process of each odd-group and even-group servo joint group as follows:

[0251] The real-time phase of the hth yaw servo 5 in the lateral swing local coordinate system at any time t is calculated by the cubic trajectory planning method:

[0252]

[0253] Among them, θ hf is the target phase of the hth yaw servo 5 in the lateral swing local coordinate system, θ h0 is the initial phase of the hth yaw servo 5 in the lateral swing local coordinate system, θ h (t) is the real-time phase of the hth yaw servo 5 in the lateral swing local coordinate system at any time t; t hf is the time when the hth yaw servo is expected to reach the target phase, which is determined according to the requirements; t is the current time, which is obtained by the timer;

[0254] In the lateral swing local coordinate system, at any time t, all the servo joints of the 2i-1th group of servo joints in the odd group, namely Z 2i-1 The real-time phase matrix of the yaw servo 5 in the servo joint is

[0255]

[0256] Among them, θ p(2i-1) (t) is the position of all the servo joints of the 2i-1th servo joint group in the lateral swing local coordinate system at any time t, i.e., Z 2i-1 The real-time phase matrix of the yaw servo 5 in the servo joint, θ p(2i-1)k (t) is the real-time phase of the yaw servo 5 in the kth servo joint in the 2i-1th servo joint group in the lateral swing local coordinate system at any time t;

[0257] In the lateral swing local coordinate system, at any time t, all the servo joints of the 2ith group of servo joints in the even group, namely Z 2i The real-time phase matrix of the yaw servo 5 in the servo joint is

[0258]

[0259] Among them, θ p(2i) (t) is the position of all the servo joints of the 2i-th servo joint group in the lateral swing local coordinate system at any time t, i.e., Z 2i-1 The real-time phase matrix of the yaw servo 5 in the servo joint, θ p(2i)k (t) is the real-time phase of the yaw servo 5 in the kth servo joint in the 2ith servo joint group in the lateral swing local coordinate system at any time t;

[0260] The specific process of the phase reversal mechanism in step 6 is as follows: the real-time phase matrix θ of the yaw servo 5 is converted to p(2i-1) (t), θ p(2i) Each element in (t) is multiplied by a scalar -1 to generate the inverse real-time phase matrix Θ p(2i-1) (t), Θ p(2i) (t);

[0261] The specific process of step 6, switching until receiving a new instruction, is as follows: the above process is executed cyclically, and the yaw servo 5 is controlled according to the real-time phase matrix θ p(2i-1) (t), θ p(2i) (t) and the inverse real-time phase matrix Θ p(2i-1) (t), Θ p(2i) (t) Deflect until the central controller receives a new action instruction.

[0262] 5. The multi-motion mode coordinated control method of the tandem multi-segment structure underwater robot is characterized by:

[0263] The vertical axial swing motion control method comprises the following steps:

[0264] Step 1. Divide the N consecutively connected servo joints in the body into 2M groups, where 2M is an integer factor of N, and the odd and even groups are arranged alternately;

[0265] Step 2: Establish the phase matrix of the pitch servo 4 of each servo joint in each odd-numbered and even-numbered servo joint groups in the vertical axial swing local coordinate system and the global coordinate system respectively;

[0266] Step 3: Generate a mapping relationship between the phase of the pitch servo 4 of each servo joint in each odd-numbered and even-numbered servo joint groups in the vertical axial swing local coordinate system and the phase in the vertical axial swing global coordinate system based on the geometric topological relationship of the kinematic chain;

[0267] Step 4: In order to limit the amplitude of the vertical axis swing, set constraints on the phase in the local coordinate system of the vertical axis swing and the phase in the global coordinate system of the vertical axis swing;

[0268] Step 5: Generate the real-time phase matrix of each odd-numbered and even-numbered servo joint group through the cubic trajectory planning method to ensure the smoothness of the movement;

[0269] Step 6: The central controller drives all pitch servos 4 to deflect according to the real-time phase matrix, and triggers the phase reversal mechanism when the target phase is reached, and continues switching until a new instruction is received.

[0270] 6. The multi-motion mode coordinated control method of the tandem multi-stage structure underwater robot is characterized by:

[0271] In the step 1 of the vertical axial swing motion control method, the first group of 2M groups of servo joints includes Z1 servo joints, the second group of servo joints includes Z2 servo joints, ..., the 2M-1 group of servo joints includes Z 2M-1 servo joints, the 2Mth servo joint group includes Z 2M servo joints;

[0272] In step 1, the odd and even groups are divided into odd and even groups;

[0273] In step 2, the vertical axial swing local coordinate system is defined as taking the center of mass of the pitch servo 4 as the origin, and establishing the vertical axial swing local coordinate system ∑ local (x, y, z), where the X-axis is defined as being parallel to the center axis of the servo joint, and the positive direction of the X-axis is away from the joint frame 3; the Z-axis is defined as being parallel to the output shaft of the pitch servo 4, and the positive direction of the Z-axis is the same as the output end of the pitch servo 4; the Y-axis is defined as satisfying the right-hand rule;

[0274] In step 2, the phase in the vertical axial swing local coordinate system is defined as the angle between the positive direction of the X-axis and the symmetry plane of the long U-bracket 8 containing the output shaft of the pitch servo 4, and it is stipulated that when viewed from the positive direction of the z-axis toward the negative direction of the z-axis, when the positive direction of the X-axis rotates to be parallel to the symmetry plane of the long U-bracket 8 containing the output shaft of the pitch servo 4, the phase is positive when rotating counterclockwise, and negative when it is reversed;

[0275] In step 2, the vertical axial swing global coordinate system is defined as taking the center of mass of the head A as the origin, and establishing the vertical axial swing global coordinate system ∑ global (x, y, z), where the X-axis is defined as the positive direction of the servo joint that passes through the center axis of the duct and points to the rear, and the Y-axis is defined as the positive direction that is perpendicular to the X-axis and points upward, ensuring that the xoy plane and the symmetry plane of the head A coincide. The Z-axis is defined to satisfy the right-hand rule.

[0276] In step 2, the phase in the vertical axial swing global coordinate system is defined as the angle between the positive direction of the X-axis and the center axis of the servo joint, and it is stipulated that when the positive direction of the X-axis rotates to be collinear with the center axis of the servo joint, the phase is positive when the positive direction of the X-axis rotates counterclockwise, and negative when the positive direction of the X-axis rotates counterclockwise.

[0277] In step 2, the mapping relationship between the phase of the pitch servo 4 of each servo joint in the odd-numbered servo joint group in the vertical axial swing local coordinate system and the phase in the vertical axial swing global coordinate system is:

[0278] In the vertical axial swing local coordinate system, all the servo joints of the 2i-1 servo joint group, namely Z 2i-1 The phase matrix of the pitch servo 4 in the servo joint is

[0279]

[0280] θ f(2i-1)k is the phase of the pitch servo 4 in the kth servo joint in the 2i-1th servo joint group in the vertical axial swing local coordinate system;

[0281] In the vertical axial swing global coordinate system, all the servo joints of the 2i-1 servo joint group, namely Z 2i-1 The phase matrix of the pitch servo 4 in the servo joint is

[0282]

[0283] is the phase of the pitch servo 4 in the k-th servo joint in the 2i-1-th servo joint group in the vertical axial swing global coordinate system;

[0284] In step 2, the mapping relationship between the phase of the pitch servo 4 of each servo joint in the even-numbered servo joint group in the vertical axial swing local coordinate system and the phase in the vertical axial swing global coordinate system is:

[0285] In the vertical axial swing local coordinate system, all the servo joints of the 2i servo joint group, namely Z 2i The phase matrix of the pitch servo 4 in the servo joint is

[0286]

[0287] θ f(2i)k is the phase of the pitch servo 4 in the kth servo joint in the 2ith servo joint group in the vertical axial swing local coordinate system;

[0288] In the vertical axial swing global coordinate system, all the servo joints of the 2i servo joint group, namely Z 2i The phase matrix of the pitch servo 4 in the servo joint is

[0289]

[0290] is the phase of the pitch servo 4 in the k-th servo joint in the 2i-th servo joint group in the vertical axial swing global coordinate system;

[0291] Among them, the 2i-1th group of servo joints is a group in the odd array, and the 2ith group of servo joints is a group in the even array, where i is a positive integer and i∈[1,M];

[0292] The mapping relationship between the phase of the pitch servo 4 of each servo joint in the odd-numbered servo joint group in step 3 in the vertical axial swing local coordinate system and the phase in the vertical axial swing global coordinate system is:

[0293]

[0294] in, It is the phase accumulation sum of the pitch servo 4 in the first k servo joints in the 2i-1 th group of servo joints in the vertical axial swing local coordinate system, It is the Zth in the 2i-2 group of servo joints. 2i-2 The phase of the pitch servo 4 in the last servo joint in the 2i-2 group in the vertical axial swing global coordinate system; is the phase of the pitch servo 4 in the k-th servo joint in the 2i-1-th servo joint group in the vertical axial swing global coordinate system;

[0295] In step 3, the mapping relationship between the phase of the pitch servo 4 of each servo joint in the even-numbered servo joint group in the vertical axial swing local coordinate system and the phase in the vertical axial swing global coordinate system is:

[0296]

[0297] in, is the phase accumulation sum of the pitch servo 4 in the first k servo joints in the 2i-th servo joint group in the vertical axial swing local coordinate system, It is the Zth in the 2i-1 group of servo joints. 2i-1 The phase of the pitch servo 4 in the last servo joint in the group in the vertical axial swing global coordinate system; is the phase of the pitch servo 4 in the k-th servo joint in the 2i-th servo joint group in the vertical axial swing global coordinate system;

[0298] Among them, the 2i-1th group of servo joints is a group in the odd array, and the 2ith group of servo joints is a group in the even array, where i is a positive integer and i∈[1,M];

[0299] In step 4, the constraint conditions include constraining the global phase of the last servo joint of all even-numbered servo joint groups to be equal to 0, and limiting the phase of the pitch servo 4 in all servo joints in the vertical axial swing local coordinate system and the phase in the vertical axial swing global coordinate system to be within a preset angle range, that is, θ p ∈[θ pmin ,θ pmax ];

[0300] In step 5, the cubic trajectory planning method generates the real-time phase matrix calculation process of each odd-group and even-group servo joint group as follows:

[0301] The real-time phase of the h-th pitch servo 4 in the vertical axial swing local coordinate system at any time t is calculated by the cubic trajectory planning method:

[0302]

[0303] Among them, θ hf is the target phase of the h-th pitch servo 4 in the vertical axial swing local coordinate system, θ h0 is the initial phase of the h-th pitch servo 4 in the local coordinate system of vertical axial swing, θ h (t) is the real-time phase of the h-th pitch servo 4 in the vertical axial swing local coordinate system at any time t; t hf is the time when the hth pitch servo 4 is expected to reach the target phase, which is determined according to the requirements; t is the current time, which is obtained by the timer;

[0304] In the vertical axial swing local coordinate system, at any time t, all the servo joints of the 2i-1th group of servo joints in the odd group, namely Z 2i-1 The real-time phase matrix of the pitch servo 4 in the servo joint is

[0305]

[0306] Among them, θ f(2i-1)(t) is the total number of servo joints in the 2i-1th servo joint group in the vertical axial swing local coordinate system at any time t, i.e., Z 2i-1 The real-time phase matrix of the pitch servo 4 in the servo joint, θ f(2i-1)k (t) is the real-time phase of the pitch servo 4 in the kth servo joint in the 2i-1th servo joint group in the vertical axial swing local coordinate system at any time t;

[0307] In the vertical axial swing local coordinate system, at any time t, all the servo joints of the 2i-th group of servo joints in the even group, namely, Z 2i The real-time phase matrix of the pitch servo 4 in the servo joint is

[0308]

[0309] Among them, θ f(2i) (t) is the coordinate of all the servo joints of the 2i-th servo joint group in the vertical axial swing local coordinate system at any time t, i.e., Z 2i-1 The real-time phase matrix of the pitch servo 4 in the servo joint, θ f(2i)k (t) is the real-time phase of the pitch servo 4 in the kth servo joint in the 2ith servo joint group in the vertical axial swing local coordinate system at any time t;

[0310] The specific process of the phase reversal mechanism in step 6 is as follows: the real-time phase matrix θ of the pitch servo 4 is converted to f(2i-1) (t), θ f(2i) Each element in (t) is multiplied by a scalar -1 to generate the inverse real-time phase matrix Θ f(2i-1) (t), Θ f(2i) (t);

[0311] The specific process of step 6, switching until receiving a new instruction, is as follows: the above process is executed cyclically, and the pitch servo 4 is controlled according to the real-time phase matrix θ f(2i-1) (t), θ f(2i) (t) and the inverse real-time phase matrix Θ f(2i-1) (t), Θ f(2i) (t) Deflect until the central controller receives a new action instruction.

[0312] 7. The multi-motion mode coordinated control method of the tandem multi-segment structure underwater robot is characterized by:

[0313] The cross-media motion control method comprises the following steps:

[0314] Step 1. Based on the thrust minimization control strategy of the ducted propulsion system, combined with the multi-body rigid dynamics equilibrium equation in the global coordinate system of the cross-medium motion and the phase constraints of each servo, establish the phase optimization matrix of each servo for realizing cross-medium motion in the local coordinate system of the cross-medium motion.

[0315] Step 2. Apply a preset pulse width modulation signal to the pitch servo 4 in each servo joint through the central controller, and drive the long U bracket 8 of the pitch servo 4 to deflect according to the phase optimization matrix generated in Step 1, so as to construct the configuration of the cross-medium motion preparation action.

[0316] Step 3. Based on the phase optimization matrix constructed in Step 1, the ducted fan 1 of the head A outputs a propulsive force with spatial vector characteristics, and uses the vertical component of the propulsive force to drive the system to break through the water-air interface.

[0317] Step 4. The ducted propulsion force terminates after emerging from the water, and the system is only affected by gravity, and the motion trajectory is approximately a skew parabola; under the action of gravity, the system completes the water re-entry motion along the parabolic trajectory, so as to complete the cross-medium motion control.

[0318] 8. The multi-motion mode coordinated control method of the serial multi-segment structure underwater robot is characterized in that:

[0319] In Step 1 of the cross-medium motion control method steps, the local coordinate system of the cross-medium motion is defined with the centroid of the pitch servo 4 as the origin, and the local coordinate system of the cross-medium motion ∑ local (x, y, z) is established. Its X-axis direction is defined as parallel to the central axis of the servo joint, and the direction away from the joint bracket 3 is the positive direction of the X-axis; its Z-axis direction is defined as parallel to the output shaft of the yaw servo 5 and the positive direction of the Z-axis points in the same direction as the output end of the yaw servo 5; its Y-axis direction satisfies the right-hand rule.

[0320] The phase in the local coordinate system of the cross-medium motion is defined as the included angle between the positive direction of the X-axis and the symmetry plane of the long U bracket 8 containing the output shaft of the yaw servo 5. It is stipulated that when looking from the positive direction of the z-axis to the negative direction of the z-axis, when the positive direction of the X-axis rotates to be parallel to the symmetry plane of the long U bracket 8 containing the output shaft of the yaw servo 5, the phase is positive when rotating counterclockwise, and negative otherwise.

[0321] In Step 1, the global coordinate system of the cross-medium motion is defined with the centroid of the mth servo joint as the origin, and the global coordinate system of the cross-medium motion ∑ global (x, y, z) is established, where m < N, and N is the number of all servo joints of the system; the X-axis direction of the global coordinate system of the cross-medium motion is defined as the positive direction horizontally to the right, the Y-axis direction is defined as the positive direction vertically upward, and the Z-axis definition satisfies the right-hand rule.

[0322] The phase in the global coordinate system of cross-medium motion is defined as the angle between the positive direction of the X-axis and the center axis of the servo joint. It is stipulated that when looking from the positive direction of the Z-axis to the negative direction of the Z-axis, when the positive direction of the X-axis rotates to be collinear with the center axis of the servo joint, the phase is positive when rotating counterclockwise, and negative when rotating counterclockwise.

[0323] The step 1, establishing the multi-body rigid body dynamics equilibrium equation in the global coordinate system of cross-medium motion, includes establishing the force balance equation and the process torque equation:

[0324] Establishment of force balance equation:

[0325]

[0326] in It is the sum of the weight of N servo joints, i.e. the weight of all servo joints and the weight of the head A, i.e. the total weight of the system; M is the total mass of the system, which is calculated as follows: That is, the total mass of the system is equal to the sum of the weight of the N servo joints and the weight of the head A divided by the acceleration of gravity, F y is the vertical component of the duct propulsion force on the head, a y is the vertical component of the acceleration of the system from the beginning of movement to the time it leaves the water, and g is the acceleration due to gravity;

[0327] Establishment of the torque equation:

[0328]

[0329] Among them, R j is the position vector of the point where the resultant external force is applied to the j-th servo joint; in particular, when j = 0, R0 represents the position vector of the point where the resultant external force is applied to the head A; F j is the net external force vector acting on the j-th servo joint; in particular, when j = 0, F0 represents the net external force vector acting on the head A;

[0330] The process of establishing the force balance equation is as follows:

[0331] Based on the need for engineering simplification, the following assumptions are made: air resistance and mechanical transmission friction are ignored; the ducted propulsion force ceases to function after exiting the water and is only affected by gravity, and the motion trajectory is approximately an oblique parabola;

[0332] According to the law of oblique projection motion, the vertical component of the velocity of the system when it leaves the water is

[0333]

[0334] Where g is the acceleration of gravity, t is the duration of the air movement, and t can be determined according to the duration of the system's operation in the air; v y is the vertical component of the velocity of the system when it leaves the water;

[0335] Acceleration of the system in water

[0336]

[0337] Among them, a y is the vertical component of the acceleration of the system from the beginning of the movement to the time it leaves the water, and H is the vertical distance from the center of mass of the system head A to the water surface;

[0338] The process of establishing the torque equation is as follows:

[0339] In the global coordinate system of cross-medium motion, the force states of each component are as follows:

[0340] The position vector of the point where the resultant external force is applied to the j-th servo joint:

[0341]

[0342] Among them, l m is the length of the mth servo joint, where m is defined so that the center of mass of the mth servo joint is the origin of the global coordinate system for cross-medium motion; l j The length of the jth servo joint; in particular, when j = 0, l0 is the length of the head A, R j =R0, R0 represents the position vector of the point where the resultant external force is applied to the head A; is the phase of the pitch servo 4 in the j+1th servo joint in the global coordinate system of cross-medium motion, is the phase of the pitch servo 4 in the j-th servo joint in the global coordinate system of cross-medium motion;

[0343] The resultant external force vector on the j-th servo joint is:

[0344]

[0345] F jx ,F jy are the x and y components of the net external force on the j-th servo joint except gravity, G j is the weight of the jth servo joint; in particular, when j = 0, G0 is the weight of the head A, and the formula F j =F0, F0 represents the resultant external force vector on the head A;

[0346]

[0347] Among them, F 0x ,F 0y G is the horizontal and vertical components of the duct propulsion force on the head, j is the weight of the jth servo joint;

[0348] The phase constraint conditions of each servo include constraining the phase range of each pitch servo 4 in the local coordinate system of cross-medium motion and the phase range in the global coordinate system of cross-medium motion:

[0349]

[0350] Among them, θ fj is the phase of the pitch servo 4 in the j-th servo joint in the local coordinate system of cross-medium motion, is the phase of the pitch servo 4 in the j-th servo joint in the global coordinate system of cross-medium motion;

[0351] The process of establishing the phase optimization matrix of each servo in the local coordinate system of the cross-medium motion for realizing the cross-medium motion in step 1 includes: constructing a mapping function between the phase in the local coordinate system of the cross-medium motion and the phase in the global coordinate system of the cross-medium motion, minimizing the ducted propulsion force F by numerical optimization, and obtaining the phase optimization matrix of each servo in the local coordinate system of the cross-medium motion;

[0352] The mapping function between the phase in the local coordinate system of cross-medium motion and the phase in the global coordinate system of cross-medium motion is:

[0353]

[0354] Among them, θ fi is the phase of the pitch servo 4 in the i-th servo joint in the local coordinate system of cross-medium motion, is the phase of the pitch servo 4 in the i-th servo joint in the global coordinate system of cross-medium motion;

[0355] The numerical optimization process includes defining the objective function, setting constraints, selecting the optimization algorithm, and outputting the optimized phase matrix. The specific numerical optimization process includes the following steps:

[0356] Step 1. Define the objective function: minimize the propulsion force F, that is,

[0357] Among them, θ f =[θ f1 ,θ f2 ,…,θ f(N-1) ,θ fN ] T is the phase matrix of the pitch servo 4 in the local coordinate system of cross-medium motion, is the phase of the pitch servo 4 in the mth servo joint in the global coordinate system of cross-medium motion, which is obtained based on the mapping function between the phase in the local coordinate system of cross-medium motion and the phase in the global coordinate system of cross-medium motion. The definition of m satisfies that the mth servo joint is the origin of the global coordinate system of the center of mass moving across the medium; It is the sum of the weight of N servo joints and the weight of the head A, that is, the total weight of the system; M is the total mass of the system. The calculation formula is That is, the total mass of the system is equal to the sum of the weight of the N servo joints and the weight of the head A divided by the acceleration of gravity; a y It is the vertical component of the acceleration of the system from the beginning of motion to the time it leaves the water;

[0358] Step 2. Set constraints: including nonlinear equality constraints and linear inequality constraints, where the nonlinear equality constraints are torque equations, and the linear inequality constraints are the phase ranges of each pitch servo 4 in the local coordinate system of cross-medium motion and the phase ranges in the global coordinate system of cross-medium motion;

[0359] Step 3. Select an optimization algorithm: A hybrid optimization strategy is used to achieve global convergence and improve local accuracy. In the global search phase, an initial feasible solution set is generated based on a genetic algorithm. In the local optimization phase, a sequential quadratic programming algorithm (SQP, implemented by the fmincon function) is used to perform gradient iterations to optimize the objective function.

[0360] Step 4. Output the optimized phase matrix: Obtain the phase matrix θ=[θ f1 * ,θ f2 * ,…,θ f(N-1) * ,θ fN * ] T .

[0361] 9. The multi-motion mode coordinated control method of the tandem multi-segment structure underwater robot is characterized by:

[0362] The composite motion control method comprises the following steps:

[0363] Step 1. Define the composite motion trajectory equation according to the actual project needs and project the trajectory into the polar coordinate system;

[0364] Step 2. Based on the geometric topological relationship of the kinematic chain, establish the servo phase matrix of the yaw servo 5 and the pitch servo 4 in the local coordinate system of the composite motion;

[0365] Step 3. Apply a pulse width modulation signal to the servo group through the central controller to drive the servo to deflect according to the phase matrix of the servo in the local coordinate system of the compound motion, thereby completing the compound motion control.

[0366] 10. The multi-motion mode coordinated control method of the tandem multi-segment structure underwater robot is characterized by:

[0367] In the step 1 of the composite motion control method, the composite motion trajectory equation is defined as:

[0368]

[0369] The process of projecting the trajectory to the polar coordinate system in step 1 includes:

[0370] Project the composite motion trajectory at time t onto the xoy plane and establish a polar coordinate system on the xoy plane. At any time t, the trajectory of the motion trajectory projected onto the xoy plane in the polar coordinate system is r(α, t), where α is the polar angle and r is the polar radius. The α and r corresponding to the center of mass of each servo joint are determined by the following formula:

[0371]

[0372] Project the composite motion trajectory at time t onto the xoz plane and establish a polar coordinate system on the xoz plane. At any time t, the trajectory of the motion trajectory projected onto the xoz plane in the polar coordinate system is ρ(β,t), where β is the polar angle and ρ is the polar radius. The β and ρ corresponding to the center of mass of each servo joint are determined by the following formula:

[0373]

[0374] In step 2, the composite motion local coordinate system is defined as follows: a composite motion local coordinate system is established with the center of mass of each pitch servo 4 and yaw servo 5 as the origin, wherein the X-axis is defined as being parallel to the center axis of the servo joint, and the direction away from the joint frame 3 is the positive direction of the X-axis; the Z-axis is defined as being parallel to the servo output shaft, and the positive direction of the Z-axis is the same as the direction of the output end of the servo 5; and the Y-axis is defined as satisfying the right-hand rule;

[0375] In step 2, the phase in the local coordinate system of the compound motion is defined as the angle between the positive direction of the X-axis and the symmetry plane of the long U-bracket 8 containing the servo output shaft, and it is stipulated that when viewed from the positive direction of the Z-axis toward the negative direction of the Z-axis, the phase is positive when the positive direction of the X-axis rotates until it is parallel to the symmetry plane of the long U-bracket 8 containing the servo output shaft, and negative when it rotates counterclockwise;

[0376] In step 2, the servo phase matrices of the yaw servo 5 and the pitch servo 5 in the compound motion local coordinate system are respectively:

[0377] θ p =[θ p1 ,θ p2 ,…,θ pn ,…θ p(N-1) ,θ pN ]T

[0378] Among them, θ pn is the phase of the nth yaw servo 5 in the local coordinate system of the composite motion; θ p is the phase matrix of N yaw servos 5;

[0379] θ f =[θ f1 ,θ f2 ,…,θ fn ,…,θ f(N-1) ,θ fN ] T

[0380] Among them, θ fn is the phase of the nth pitch servo 4 in the local coordinate system of the compound motion; θ f is the phase matrix of N pitch servos 4;

[0381] The phases of the nth yaw servo 5 and the nth pitch servo 4 in the compound motion local coordinate system are respectively determined by the tangent angle change at the mass center of the adjacent yaw servo 5 and the tangent angle change at the mass center of the adjacent pitch servo 4:

[0382] θ pn =φ(α n+1 ,t)-φ(α n ,t)

[0383] Among them, φ(α n+1 ,t) is the change of the tangent angle of the mass center of the n+1th yaw servo 5 with the polar angle, φ(α n ,t) is the change of the tangent angle of the mass center of the nth yaw servo 5 with the polar angle, θ pn is the phase of the nth yaw servo 5 in the local coordinate system of the composite motion;

[0384] θ fn =φ(β n+1 ,t)-φ(β n ,t)

[0385] Among them, φ(β n+1 ,t) is the change of the tangent angle of the mass center of the n+1th pitch servo 4 with the polar angle, φ(β n ,t) is the change of the tangent angle of the mass center of the nth pitch servo 4 with the polar angle, θ fn is the phase of the nth pitch servo 4 in the local coordinate system of the compound motion;

[0386] The calculation formulas for the mass center tangent angle of the nth yaw servo 5 and the mass center tangent angle of the nth pitch servo 4 are:

[0387] The calculation formula of the mass center tangent angle of the nth yaw servo 5 is:

[0388]

[0389] r(α, t) is the trajectory of the motion projected onto the xoy plane in the polar coordinate system at any time t, and r'(α, t) is the partial derivative of the radial distance with respect to the polar angle;

[0390] The calculation formula for the tangent angle of the mass center of the nth pitch servo 4 is:

[0391]

[0392] ρ(β n ,t) is the trajectory of the motion trajectory projected onto the xoz plane in the polar coordinate system at any time t, ρ'(β n ,t) is the partial derivative of radial distance with respect to polar angle;

[0393] The calculation formula of the partial derivative of the radial distance with respect to the polar angle is:

[0394]

[0395] r(α, t) is the trajectory of the motion projected onto the xoy plane in the polar coordinate system at any time t, and r'(α, t) is the partial derivative of the radial distance with respect to the polar angle;

[0396]

[0397] ρ(β n ,t) is the trajectory of the motion trajectory projected onto the xoz plane in the polar coordinate system at any time t, ρ'(β n ,t) is the partial derivative of the radial distance with respect to the polar angle.

Claims

1. A tandem multi-segment underwater robot, characterized by: The underwater robot comprises a head (A) and a body (B); the head (A) is composed of a duct (1) and a duct connection mechanism (2); the body (B) is composed of N servo joints connected in sequence, namely the first servo joint, the second servo joint, ..., the Nth servo joint; each of the servo joints has the same structure, consisting of a joint frame (3) and a pitch servo (4) respectively mounted on the front end of the joint frame (3) and a yaw servo (5) at the rear end; each of the pitch servo (4) and the yaw servo (5) has the same structure, consisting of a servo body (6) and a short U bracket (7) and a long U bracket (8) mounted on both ends of the servo body (6); the long U bracket (8) is mounted at the end of the servo output shaft, and its length direction is perpendicular to the servo output shaft; In the initial state, the phases of the pitch servos (4) and the yaw servos (5) of the N servo joints are all zero initial phases, and the above N is a natural number between 6 and 10; The zero initial phase is defined as the angle between the two symmetry planes when the symmetry plane of the long U bracket (8) containing the steering gear output shaft and the symmetry plane of the steering gear body (6) containing the steering gear output shaft coincide with each other; the phase is defined as the phase when the rotation angle of the long U bracket (8) rotates counterclockwise relative to the zero initial phase is positive, and vice versa.

2. The multi-motion mode coordinated control method of a tandem multi-segment underwater robot according to claim 1, characterized in that: It includes a lateral swing motion control method, a vertical axial swing control method, a cross-medium motion control method, and a composite motion control method.

3. The multi-motion mode coordinated control method of a tandem multi-segment underwater robot according to claim 2, characterized in that: The lateral swing motion control method comprises the following steps: Step 1. Divide the N consecutively connected servo joints in the body into 2M groups, where 2M is an integer factor of N, and the odd and even groups are arranged alternately; Step 2, establishing the phase matrix of the yaw servo (5) of each servo joint in each odd-group and even-group servo joint group in the lateral swing local coordinate system and the global coordinate system respectively; Step 3, based on the geometric topological relationship of the kinematic chain, generating a mapping relationship between the phase of the yaw servo (5) of each servo joint in each odd-group and even-group servo joint group in the lateral swing local coordinate system and the phase in the lateral swing global coordinate system; Step 4: In order to limit the amplitude of the lateral swing, set constraints on the phase in the local coordinate system of the lateral swing and the phase in the global coordinate system of the lateral swing; Step 5: Generate the real-time phase matrix of each odd-numbered and even-numbered servo joint group through the cubic trajectory planning method to ensure the smoothness of the movement; Step 6: The central controller drives all yaw servos (5) to deflect according to the real-time phase matrix, and triggers the phase reversal mechanism when the target phase is reached, and continues switching until a new instruction is received.

4. The multi-motion mode coordinated control method of a tandem multi-segment structure underwater robot according to claim 3 is characterized in that : In the step 1 of the lateral swing motion control method, the first group of 2M groups of servo joints includes Z1 servo joints, the second group of servo joints includes Z2 servo joints, ..., the 2M-1 group of servo joints includes Z 2M-1 servo joints, the 2Mth servo joint group includes Z 2M servo joints; In step 1, the odd and even groups are divided into odd and even groups; In the step 2, the lateral swing local coordinate system is defined as taking the center of mass of the yaw steering gear (5) as the origin, and establishing the lateral swing local coordinate system ∑ local (x, y, z), wherein the X-axis direction is defined as being parallel to the center axis of the servo joint, and the direction away from the joint frame (3) is the positive direction of the X-axis; the Z-axis direction is defined as being parallel to the output shaft of the yaw servo (5), and the positive direction of the Z-axis is the same as the direction of the output end of the yaw servo (5); and the Y-axis direction is defined as satisfying the right-hand rule; In the step 2, the phase in the lateral swing local coordinate system is defined as the angle between the positive direction of the X-axis and the symmetry plane of the long U bracket (8) containing the output shaft of the yaw servo (5), and it is stipulated that when the positive direction of the X-axis rotates to be parallel to the symmetry plane of the long U bracket (8) containing the output shaft of the yaw servo (5), the phase is positive when rotating counterclockwise, and negative when rotating counterclockwise; In step 2, the lateral swing global coordinate system is defined as taking the center of mass of the head (A) as the origin, and establishing the lateral swing global coordinate system ∑ global (x, y, z), where the X-axis is defined as the positive direction of the X-axis, which passes through the center axis of the duct and points to the rear servo joint. The Y-axis is defined as the positive direction perpendicular to the X-axis, with the upward direction being the positive direction. The xoy plane and the symmetry plane of the head (A) coincide with each other. The Z-axis is defined to satisfy the right-hand rule. In step 2, the phase in the lateral swing global coordinate system is defined as the angle between the positive direction of the X-axis and the center axis of the servo joint, and it is stipulated that when the positive direction of the X-axis rotates to be collinear with the center axis of the servo joint, the phase is positive when the positive direction of the X-axis rotates counterclockwise, and negative when the positive direction of the X-axis rotates counterclockwise. In the step 2, the mapping relationship between the phase of the yaw servo (5) of each servo joint in the odd-numbered servo joint group in the lateral swing local coordinate system and the phase in the lateral swing global coordinate system is: In the lateral swing local coordinate system, all the servo joints of the 2i-1 servo joint group, namely Z 2i-1 The phase matrix of the yaw servo (5) in the servo joint is θ p(2i-1)k is the phase of the yaw servo (5) in the kth servo joint in the 2i-1th servo joint group in the lateral swing local coordinate system; In the lateral swing global coordinate system, all the servo joints of the 2i-1 servo joint group, namely Z 2i-1 The phase matrix of the yaw servo (5) in the servo joint is is the phase of the yaw servo (5) in the kth servo joint in the 2i-1th servo joint group in the lateral swing global coordinate system; In the step 2, the mapping relationship between the phase of the yaw servo (5) of each servo joint in the even-numbered servo joint group in the lateral swing local coordinate system and the phase in the lateral swing global coordinate system is: In the lateral swing local coordinate system, all the servo joints of the 2i servo joint group, namely Z 2i The phase matrix of the yaw servo (5) in the servo joint is θ p(2i)k is the phase of the yaw servo (5) in the kth servo joint in the 2ith servo joint group in the lateral swing local coordinate system; In the lateral swing global coordinate system, all the servo joints of the 2i servo joint group, namely Z 2i The phase matrix of the yaw servo (5) in the servo joint is is the phase of the yaw servo (5) in the kth servo joint in the 2ith servo joint group in the lateral swing global coordinate system; Among them, the 2i-1th group of servo joints is a group in the odd array, and the 2ith group of servo joints is a group in the even array, where i is a positive integer and i∈[1,M]; The mapping relationship between the phase of the yaw servo (5) of each servo joint in the odd-numbered servo joint group in step 3 in the lateral swing local coordinate system and the phase in the lateral swing global coordinate system is: in, is the phase accumulation sum of the yaw servos (5) in the first k servo joints in the 2i-1 th servo joint group in the lateral swing local coordinate system, It is the Zth in the 2i-2 group of servo joints. 2i-2 The phase of the yaw servo (5) in the servo joint, i.e. the last servo joint in the 2i-2 group, in the lateral swing global coordinate system; is the phase of the yaw servo (5) in the kth servo joint in the 2i-1th servo joint group in the lateral swing global coordinate system; In step 3, the mapping relationship between the phase of the yaw servo (5) of each servo joint in the even-numbered servo joint group in the lateral swing local coordinate system and the phase in the lateral swing global coordinate system is: in, is the phase accumulation sum of the yaw servos (5) in the first k servo joints in the 2i-th servo joint group in the lateral swing local coordinate system, It is the Zth in the 2i-1 group of servo joints. 2i-1 The phase of the yaw servo (5) in the servo joint, i.e. the last servo joint in the group, in the lateral swing global coordinate system; is the phase of the yaw servo (5) in the kth servo joint in the 2ith servo joint group in the lateral swing global coordinate system; Among them, the 2i-1th group of servo joints is a group in the odd array, and the 2ith group of servo joints is a group in the even array, where i is a positive integer and i∈[1,M]; In step 4, the constraint condition includes constraining the global phase of the last servo joint of all even-numbered servo joint groups to be equal to 0, and limiting the phase of the yaw servo (5) in all servo joints in the lateral swing local coordinate system and the phase in the lateral swing global coordinate system to be within a preset angle range, that is, θ p ∈[θ pmin ,θ pmax ]; In step 5, the cubic trajectory planning method generates the real-time phase matrix calculation process of each odd-group and even-group servo joint group as follows: The real-time phase of the hth yaw servo (5) in the lateral swing local coordinate system at any time t is calculated by the cubic trajectory planning method: Among them, θ hf is the target phase of the hth yaw servo (5) in the lateral swing local coordinate system, θ h0 is the initial phase of the hth yaw servo (5) in the local coordinate system of lateral swing, θ h (t) is the real-time phase of the hth yaw servo (5) in the lateral swing local coordinate system at any time t; t hf is the time when the hth yaw servo is expected to reach the target phase, which is determined according to the requirements; t is the current time, which is obtained by the timer; In the lateral swing local coordinate system, at any time t, all the servo joints of the 2i-1th group of servo joints in the odd group, namely Z 2i-1 The real-time phase matrix of the yaw servo (5) in the servo joint is Among them, θ p(2i-1) (t) is the position of all the servo joints of the 2i-1th servo joint group in the lateral swing local coordinate system at any time t, i.e., Z 2i-1 The real-time phase matrix of the yaw servo (5) in the servo joint, θ p(2i-1)k (t) is the real-time phase of the yaw servo (5) in the kth servo joint in the 2i-1th servo joint group in the lateral swing local coordinate system at any time t; In the lateral swing local coordinate system, at any time t, all the servo joints of the 2ith group of servo joints in the even group, namely Z 2i The real-time phase matrix of the yaw servo (5) in the servo joint is Among them, θ p(2i) (t) is the position of all the servo joints of the 2i-th servo joint group in the lateral swing local coordinate system at any time t, i.e., Z 2i-1 The real-time phase matrix of the yaw servo (5) in the servo joint, θ p(2i)k (t) is the real-time phase of the yaw servo (5) in the kth servo joint in the 2ith servo joint group in the lateral swing local coordinate system at any time t; The specific process of the phase reversal mechanism in step 6 is as follows: the real-time phase matrix θ of the yaw servo (5) is converted to p(2i-1) (t), θ p(2i) Each element in (t) is multiplied by a scalar -1 to generate the inverse real-time phase matrix Θ p(2i-1) (t), Θ p(2i) (t); The specific process of step 6, switching until receiving a new instruction, is as follows: the above process is executed cyclically, and the yaw servo (5) is controlled according to the real-time phase matrix θ p(2i-1) (t), θ p(2i) (t) and the inverse real-time phase matrix Θ p(2i-1) (t), Θ p(2i) (t) Deflect until the central controller receives a new action instruction.

5. The multi-motion mode coordinated control method of a tandem multi-segment underwater robot according to claim 2, characterized in that: The vertical axial swing motion control method comprises the following steps: Step 1. Divide the N consecutively connected servo joints in the body into 2M groups, where 2M is an integer factor of N, and the odd and even groups are arranged alternately; Step 2, establishing the phase matrix of the pitch servo (4) of each servo joint in each odd-group and even-group servo joint group in the vertical axial swing local coordinate system and the global coordinate system respectively; Step 3, based on the geometric topological relationship of the kinematic chain, a mapping relationship is generated between the phase of the pitch servo (4) of each servo joint in each odd-group and even-group servo joint group in the vertical axial swing local coordinate system and the phase in the vertical axial swing global coordinate system; Step 4: In order to limit the amplitude of the vertical axis swing, set constraints on the phase in the local coordinate system of the vertical axis swing and the phase in the global coordinate system of the vertical axis swing; Step 5: Generate the real-time phase matrix of each odd-numbered and even-numbered servo joint group through the cubic trajectory planning method to ensure the smoothness of the movement; Step 6: The central controller drives all pitch servos (4) to deflect according to the real-time phase matrix, and triggers the phase reversal mechanism when the target phase is reached, and continues switching until a new instruction is received.

6. The multi-motion mode coordinated control method of a serial multi-segment structure underwater robot according to claim 5 is characterized in that : In the step 1 of the vertical axial swing motion control method, the first group of 2M groups of servo joints includes Z1 servo joints, the second group of servo joints includes Z2 servo joints, ..., the 2M-1 group of servo joints includes Z 2M-1 servo joints, the 2Mth servo joint group includes Z 2M servo joints; In step 1, the odd and even groups are divided into odd and even groups; In the step 2, the vertical axial swing local coordinate system is defined as taking the center of mass of the pitch servo (4) as the origin, and establishing the vertical axial swing local coordinate system ∑ local (x, y, z), wherein the X-axis direction is defined as being parallel to the center axis of the servo joint, and the direction away from the joint frame (3) is the positive direction of the X-axis; the Z-axis direction is defined as being parallel to the output axis of the pitch servo (4), and the positive direction of the Z-axis is the same as the direction of the output end of the pitch servo (4); and the Y-axis direction is defined as satisfying the right-hand rule; In the step 2, the phase in the vertical axial swing local coordinate system is defined as the angle between the positive direction of the X-axis and the symmetry plane of the long U bracket (8) containing the output shaft of the pitch servo (4), and it is stipulated that when the positive direction of the X-axis rotates to be parallel to the symmetry plane of the long U bracket (8) containing the output shaft of the pitch servo (4), the phase is positive when the X-axis rotates counterclockwise, and negative when the X-axis rotates counterclockwise; In step 2, the vertical axial swing global coordinate system is defined as taking the center of mass of the head (A) as the origin, and establishing the vertical axial swing global coordinate system ∑ global (x, y, z), where the X-axis is defined as the positive direction of the X-axis, which passes through the center axis of the duct and points to the rear servo joint. The Y-axis is defined as the positive direction perpendicular to the X-axis, with the upward direction being the positive direction. The xoy plane and the symmetry plane of the head (A) coincide with each other. The Z-axis is defined to satisfy the right-hand rule. In step 2, the phase in the vertical axial swing global coordinate system is defined as the angle between the positive direction of the X-axis and the center axis of the servo joint, and it is stipulated that when the positive direction of the X-axis rotates to be collinear with the center axis of the servo joint, the phase is positive when the positive direction of the X-axis rotates counterclockwise, and negative when the positive direction of the X-axis rotates counterclockwise. In the step 2, the mapping relationship between the phase of the pitch servo (4) of each servo joint in the odd-numbered servo joint group in the vertical axial swing local coordinate system and the phase in the vertical axial swing global coordinate system is: In the vertical axial swing local coordinate system, all the servo joints of the 2i-1 servo joint group, namely Z 2i-1 The phase matrix of the pitch servo (4) in the servo joint is θ f(2i-1)k is the phase of the pitch servo (4) in the kth servo joint in the 2i-1th servo joint group in the vertical axial swing local coordinate system; In the vertical axial swing global coordinate system, all the servo joints of the 2i-1 servo joint group, namely Z 2i-1 The phase matrix of the pitch servo (4) in the servo joint is is the phase of the pitch servo (4) in the kth servo joint in the 2i-1th servo joint group in the vertical axial swing global coordinate system; In the step 2, the mapping relationship between the phase of the pitch servo (4) of each servo joint in the even-numbered servo joint group in the vertical axial swing local coordinate system and the phase in the vertical axial swing global coordinate system is: In the vertical axial swing local coordinate system, all the servo joints of the 2i servo joint group, namely Z 2i The phase matrix of the pitch servo (4) in the servo joint is θ f(2i)k is the phase of the pitch servo (4) in the kth servo joint in the 2ith servo joint group in the vertical axial swing local coordinate system; In the vertical axial swing global coordinate system, all the servo joints of the 2i servo joint group, namely Z 2i The phase matrix of the pitch servo (4) in the servo joint is is the phase of the pitch servo (4) in the kth servo joint in the 2ith servo joint group in the vertical axial swing global coordinate system; Among them, the 2i-1th group of servo joints is a group in the odd array, and the 2ith group of servo joints is a group in the even array, where i is a positive integer and i∈[1,M]; The mapping relationship between the phase of the pitch servo (4) of each servo joint in the odd-numbered servo joint group in step 3 in the vertical axial swing local coordinate system and the phase in the vertical axial swing global coordinate system is: in, is the phase accumulation sum of the pitch servos (4) in the first k servo joints in the 2i-1 th servo joint group in the vertical axial swing local coordinate system, It is the Zth in the 2i-2 group of servo joints. 2i-2 The phase of the pitch servo (4) in the servo joint, i.e. the last servo joint in the 2i-2 group, in the vertical axial swing global coordinate system; is the phase of the pitch servo (4) in the kth servo joint in the 2i-1th servo joint group in the vertical axial swing global coordinate system; In step 3, the mapping relationship between the phase of the pitch servo (4) of each servo joint in the even-numbered servo joint group in the vertical axial swing local coordinate system and the phase in the vertical axial swing global coordinate system is: in, is the phase accumulation sum of the pitch servos (4) in the first k servo joints in the 2i-th servo joint group in the vertical axial swing local coordinate system, It is the Zth in the 2i-1 group of servo joints. 2i-1 The phase of the pitch servo (4) in the last servo joint in the group in the vertical axial swing global coordinate system; is the phase of the pitch servo (4) in the kth servo joint in the 2ith servo joint group in the vertical axial swing global coordinate system; Among them, the 2i-1th group of servo joints is a group in the odd array, and the 2ith group of servo joints is a group in the even array, where i is a positive integer and i∈[1,M]; In step 4, the constraint condition includes constraining the global phase of the last servo joint of all even-numbered servo joint groups to be equal to 0, and limiting the phase of the pitch servo (4) in all servo joints in the vertical axial swing local coordinate system and the phase in the vertical axial swing global coordinate system to be within a preset angle range, that is, θ p ∈[θ pmin ,θ pmax ]; In step 5, the cubic trajectory planning method generates the real-time phase matrix calculation process of each odd-group and even-group servo joint group as follows: The real-time phase of the h-th pitch servo (4) in the vertical axial swing local coordinate system at any time t is calculated by the cubic trajectory planning method: Among them, θ hf is the target phase of the h-th pitch servo (4) in the vertical axial swing local coordinate system, θ h0 is the initial phase of the hth pitch servo (4) in the vertical axial swing local coordinate system, θ h (t) is the real-time phase of the h-th pitch servo (4) in the vertical axial swing local coordinate system at any time t; t hf is the time when the hth pitch servo (4) is expected to reach the target phase, which is determined according to the requirements; t is the current time, which is obtained by the timer; In the vertical axial swing local coordinate system, at any time t, all the servo joints of the 2i-1th group of servo joints in the odd group, namely Z 2i-1 The real-time phase matrix of the pitch servo (4) in the servo joint is Among them, θ f(2i-1) (t) is the total number of servo joints in the 2i-1th servo joint group in the vertical axial swing local coordinate system at any time t, i.e., Z 2i-1 The real-time phase matrix of the pitch servo (4) in the servo joint, θ f(2i-1)k (t) is the real-time phase of the pitch servo (4) in the kth servo joint in the 2i-1th servo joint group in the vertical axial swing local coordinate system at any time t; In the vertical axial swing local coordinate system, at any time t, all the servo joints of the 2i-th group of servo joints in the even group, namely, Z 2i The real-time phase matrix of the pitch servo (4) in the servo joint is Among them, θ f(2i) (t) is the coordinate of all the servo joints of the 2i-th servo joint group in the vertical axial swing local coordinate system at any time t, i.e., Z 2i-1 The real-time phase matrix of the pitch servo (4) in the servo joint, θ f(2i)k (t) is the real-time phase of the pitch servo (4) in the kth servo joint in the 2ith servo joint group in the vertical axial swing local coordinate system at any time t; The specific process of the phase reversal mechanism in step 6 is as follows: the real-time phase matrix θ of the pitch servo (4) is converted to f(2i-1) (t), θ f(2i) Each element in (t) is multiplied by a scalar -1 to generate the inverse real-time phase matrix Θ f(2i-1) (t), Θ f(2i) (t); The specific process of step 6, switching until receiving a new instruction, is as follows: the above process is executed cyclically, and the pitch servo (4) is controlled according to the real-time phase matrix θ f(2i-1) (t), θ f(2i) (t) and the inverse real-time phase matrix Θ f(2i-1) (t), Θ f(2i) (t) Deflect until the central controller receives a new action instruction.

7. The multi-motion mode coordinated control method of a tandem multi-segment underwater robot according to claim 2, characterized in that: The cross-media motion control method comprises the following steps: Step 1. Based on the thrust minimization control strategy of the ducted propulsion system, combined with the multi-body rigid body dynamics equilibrium equations in the global coordinate system of cross-medium motion and the phase constraints of each servo, establish the phase optimization matrix of each servo in the local coordinate system of cross-medium motion to achieve cross-medium motion; Step 2. Applying a preset pulse width modulation signal to the pitch servo (4) in each servo joint through the central controller, driving the long U bracket (8) of the pitch servo (4) to complete the deflection according to the phase optimization matrix generated in step 1, and constructing the cross-medium motion preparation action configuration; Step 3. Based on the phase optimization matrix constructed in step 1, the duct (1) of the head (A) outputs a propulsion force with space vector characteristics, and uses the vertical component of the propulsion force to drive the system to break through the water-air interface; Step 4. The duct propulsion force ceases to work after leaving the water, and the system is only affected by gravity, and the motion trajectory is approximately an oblique parabola; under the action of gravity, the system completes the water re-entry motion along the parabolic trajectory, thereby completing the cross-medium motion control.

8. The multi-motion mode coordinated control method of a tandem multi-segment underwater robot according to claim 7, characterized in that: In the step 1 of the cross-medium motion control method, the cross-medium motion local coordinate system is defined as follows: the cross-medium motion local coordinate system ∑ local (x, y, z), wherein the X-axis direction is defined as being parallel to the center axis of the servo joint, and the direction away from the joint frame (3) is the positive direction of the X-axis; the Z-axis direction is defined as being parallel to the output shaft of the yaw servo (5), and the positive direction of the Z-axis is the same as the direction of the output end of the yaw servo (5); and the Y-axis direction is defined as satisfying the right-hand rule; The phase in the local coordinate system of cross-medium motion is defined as the angle between the positive direction of the X-axis and the symmetry plane of the long U-bracket (8) containing the output shaft of the yaw servo (5), and it is stipulated that when the positive direction of the X-axis rotates to be parallel to the symmetry plane of the long U-bracket (8) containing the output shaft of the yaw servo (5), the phase is positive when rotating counterclockwise, and negative when rotating counterclockwise. In step 1, the global coordinate system for cross-medium motion is defined with the centroid of the m-th rudder joint as the origin, and the global coordinate system ∑ global (x, y, z) is established, where m < N and N is the number of all rudder joints in the system; The X-axis of the global coordinate system for cross-medium motion is defined as the horizontal rightward positive direction, the Y-axis is defined as the vertical upward positive direction, and the Z-axis is defined to satisfy the right-hand rule; The phase in the global coordinate system of cross-medium motion is defined as the angle between the positive direction of the X-axis and the center axis of the servo joint. It is stipulated that when looking from the positive direction of the Z-axis to the negative direction of the Z-axis, when the positive direction of the X-axis rotates to be collinear with the center axis of the servo joint, the phase is positive when rotating counterclockwise, and negative when rotating counterclockwise. The step 1, establishing the multi-body rigid body dynamics equilibrium equation in the global coordinate system of cross-medium motion, includes establishing the force balance equation and the process torque equation: Establishment of force balance equation: in is the sum of the weight of N servo joints, i.e., the weight of all servo joints and the weight of the head (A), i.e., the total weight of the system; M is the total mass of the system, which is calculated as follows: That is, the total mass of the system is equal to the sum of the weight of the N servo joints and the weight of the head (A) divided by the acceleration of gravity, F y is the vertical component of the duct propulsion force on the head, a y is the vertical component of the acceleration of the system from the beginning of movement to the time it leaves the water, and g is the acceleration due to gravity; Establishment of the torque equation: Among them, R j is the position vector of the point where the resultant external force is applied to the j-th servo joint; in particular, when j = 0, R0 represents the position vector of the point where the head (A) is applied to the resultant external force; F j is the net external force vector acting on the j-th servo joint. In particular, when j = 0, F0 represents the net external force vector acting on the head (A). The process of establishing the force balance equation is as follows: Based on the need for engineering simplification, the following assumptions are made: air resistance and mechanical transmission friction are ignored; the ducted propulsion force ceases to function after exiting the water and is only affected by gravity, and the motion trajectory is approximately an oblique parabola; According to the law of oblique projection motion, the vertical component of the velocity of the system when it leaves the water is Where g is the acceleration of gravity, t is the duration of the air movement, and t can be determined according to the duration of the system's operation in the air; v y is the vertical component of the velocity of the system when it leaves the water; Acceleration of the system in water Among them, a y is the vertical component of the acceleration of the system from the beginning of motion to the time it leaves the water, and H is the vertical distance from the center of mass of the system head (A) to the water surface; The process of establishing the torque equation is as follows: In the global coordinate system of cross-medium motion, the force states of each component are as follows: The position vector of the point where the resultant external force is applied to the j-th servo joint: Among them, l m is the length of the mth servo joint, where m is defined so that the center of mass of the mth servo joint is the origin of the global coordinate system for cross-medium motion; l j The length of the jth servo joint; in particular, when j = 0, l0 is the length of the head (A), R j = R0, R0 represents the position vector of the point where the resultant external force is applied to the head (A); is the phase of the pitch servo (4) in the j+1th servo joint in the global coordinate system of cross-medium motion, is the phase of the pitch servo (4) in the jth servo joint in the global coordinate system of cross-medium motion; The resultant external force vector on the j-th servo joint is: F jx ,F jy are the x and y components of the net external force on the j-th servo joint except gravity, G j is the weight of the jth servo joint; in particular, when j = 0, G0 is the weight of the head (A), and the formula F j =F0, F0 represents the resultant external force vector acting on the head (A); Among them, F 0x ,F 0y G is the horizontal and vertical components of the duct propulsion force on the head, j is the weight of the jth servo joint; The phase constraint conditions of each servo include constraining the phase range of each pitch servo (4) in the local coordinate system of cross-medium motion and the phase range in the global coordinate system of cross-medium motion: Among them, θ fj is the phase of the pitch servo (4) in the jth servo joint in the local coordinate system of cross-medium motion, is the phase of the pitch servo (4) in the j-th servo joint in the global coordinate system of cross-medium motion; The process of establishing the phase optimization matrix of each servo in the local coordinate system of the cross-medium motion for realizing the cross-medium motion in step 1 includes: constructing a mapping function between the phase in the local coordinate system of the cross-medium motion and the phase in the global coordinate system of the cross-medium motion, minimizing the ducted propulsion force F by numerical optimization, and obtaining the phase optimization matrix of each servo in the local coordinate system of the cross-medium motion; The mapping function between the phase in the local coordinate system of cross-medium motion and the phase in the global coordinate system of cross-medium motion is: Among them, θ fi is the phase of the pitch servo (4) in the i-th servo joint in the local coordinate system of cross-medium motion, is the phase of the pitch servo (4) in the i-th servo joint in the global coordinate system of cross-medium motion; The numerical optimization process includes defining the objective function, setting constraints, selecting the optimization algorithm, and outputting the optimized phase matrix. The specific numerical optimization process includes the following steps: Step 1. Define the objective function: minimize the propulsion force F, that is, Among them, θ f =[θ f1 ,θ f2 ,…,θ f(N-1) ,θ fN ] T is the phase matrix of the pitch servo (4) in the local coordinate system of cross-medium motion, is the phase of the pitch servo (4) in the mth servo joint in the global coordinate system of cross-medium motion, which is obtained from the mapping function between the phase in the local coordinate system of cross-medium motion and the phase in the global coordinate system of cross-medium motion. The definition of m satisfies that the mth servo joint is the origin of the global coordinate system of the center of mass moving across the medium; is the sum of the weight of the N servo joints and the weight of the head (A), that is, the total weight of the system; M is the total mass of the system. The calculation formula is That is, the total mass of the system is equal to the sum of the weight of the N servo joints and the weight of the head (A) divided by the acceleration of gravity; a y It is the vertical component of the acceleration of the system from the beginning of motion to the time it leaves the water; Step 2. Set constraints: including nonlinear equality constraints and linear inequality constraints, wherein the nonlinear equality constraints are torque equations, and the linear inequality constraints are the phase ranges of each pitch servo (4) in the local coordinate system of cross-medium motion and the phase ranges in the global coordinate system of cross-medium motion; Step 3. Select the optimization algorithm: A hybrid optimization strategy is used to achieve global convergence and improve local accuracy. In the global search phase, the initial feasible solution set is generated based on the genetic algorithm. In the local optimization phase, the sequential quadratic programming algorithm is used to perform gradient iteration to optimize the objective function. Step 4. Output the optimized phase matrix: Obtain the phase matrix θ=[θ f1 * ,θ f2 * ,…,θ f(N-1) * ,θ fN * ] T .

9. The multi-motion mode coordinated control method of a tandem multi-segment underwater robot according to claim 2, characterized in that: The composite motion control method comprises the following steps: Step 1. Define the composite motion trajectory equation according to the actual project needs and project the trajectory into the polar coordinate system; Step 2. Based on the geometric topological relationship of the kinematic chain, establish the servo phase matrix of the yaw servo (5) and the pitch servo (4) in the composite motion local coordinate system; Step 3. Apply a pulse width modulation signal to the servo group through the central controller to drive the servo to deflect according to the phase matrix of the servo in the local coordinate system of the compound motion, thereby completing the compound motion control.

10. The multi-motion mode coordinated control method of a tandem multi-segment underwater robot according to claim 9, characterized in that: In the step 1 of the composite motion control method, the composite motion trajectory equation is defined as: The process of projecting the trajectory to the polar coordinate system in step 1 includes: Project the composite motion trajectory at time t onto the xoy plane and establish a polar coordinate system on the xoy plane. At any time t, the trajectory of the motion trajectory projected onto the xoy plane in the polar coordinate system is r(α, t), where α is the polar angle and r is the polar radius. The α and r corresponding to the center of mass of each servo joint are determined by the following formula: Project the composite motion trajectory at time t onto the xoz plane and establish a polar coordinate system on the xoz plane. At any time t, the trajectory of the motion trajectory projected onto the xoz plane in the polar coordinate system is ρ(β,t), where β is the polar angle and ρ is the polar radius. The β and ρ corresponding to the center of mass of each servo joint are determined by the following formula: In step 2, the composite motion local coordinate system is defined as follows: taking the center of mass of each pitch servo (4) and yaw servo (5) as the origin, a composite motion local coordinate system is established, wherein the X-axis direction is defined as being parallel to the center axis of the servo joint, and the direction away from the joint frame (3) is the positive direction of the X-axis; the Z-axis direction is defined as being parallel to the servo output shaft, and the positive direction of the Z-axis is the same as the direction of the output end of the servo (5); and the Y-axis direction is defined as satisfying the right-hand rule; In the step 2, the phase in the local coordinate system of the composite motion is defined as: the angle between the positive direction of the X-axis and the symmetry plane of the long U-bracket (8) containing the servo output shaft, and it is stipulated that when the positive direction of the X-axis rotates to be parallel to the symmetry plane of the long U-bracket (8) containing the servo output shaft from the positive direction of the Z-axis to the negative direction of the Z-axis, the phase when the X-axis rotates counterclockwise is positive, and vice versa; In the step 2, the servo phase matrices of the yaw servo (5) and the pitch servo (5) in the composite motion local coordinate system are respectively: i p =[θ p1 ,i p2 ,…,θ pn ,…θ p(N-1) ,i pN ] T Among them, θ pn is the phase of the nth yaw servo (5) in the local coordinate system of the composite motion; θ p is the phase matrix of N yaw servos (5); i f =[θ f1 ,i f2 ,…,θ fn ,…,θ f(N-1) ,i fN ] T Among them, θ fn is the phase of the nth pitch servo (4) in the local coordinate system of the composite motion; θ f is the phase matrix of N pitch servos (4); The phases of the nth yaw servo (5) and the nth pitch servo (4) in the compound motion local coordinate system are respectively determined by the tangent angle change at the mass center of the adjacent yaw servo (5) and the tangent angle change at the mass center of the adjacent pitch servo (4): i pn =φ(a n+1 ,t)-φ(a n ,t) Among them, φ(α n+1 ,t) is the change of the tangent angle of the mass center of the n+1th yaw servo (5) with the polar angle, φ(α n ,t) is the change of the tangent angle of the center of mass of the nth yaw servo (5) with the polar angle, θ pn is the phase of the nth yaw servo (5) in the local coordinate system of the composite motion; i fn =φ(β n+1 ,t)-φ(β n ,t) Among them, φ(β n+1 ,t) is the change of the tangent angle of the mass center of the n+1th pitch servo (4) with the polar angle, φ(β n ,t) is the change of the tangent angle of the mass center of the nth pitch servo (4) with the polar angle, θ fn is the phase of the nth pitch servo (4) in the local coordinate system of the composite motion; The calculation formulas for the center-of-mass tangent angle of the nth yaw steering gear (5) and the center-of-mass tangent angle of the nth pitch steering gear (4) are: The formula for calculating the tangent angle of the center of mass of the nth yaw servo (5) is: r(α, t) is the trajectory of the motion projected onto the xoy plane in the polar coordinate system at any time t, and r'(α, t) is the partial derivative of the radial distance with respect to the polar angle; The formula for calculating the tangent angle of the center of mass of the nth pitch servo (4) is: ρ(β n ,t) is the trajectory of the motion trajectory projected onto the xoz plane in the polar coordinate system at any time t, ρ'(β n ,t) is the partial derivative of radial distance with respect to polar angle; The calculation formula of the partial derivative of the radial distance with respect to the polar angle is: r(α, t) is the trajectory of the motion projected onto the xoy plane in the polar coordinate system at any time t, and r'(α, t) is the partial derivative of the radial distance with respect to the polar angle; ρ(β n ,t) is the trajectory of the motion trajectory projected onto the xoz plane in the polar coordinate system at any time t, ρ'(β n ,t) is the partial derivative of the radial distance with respect to the polar angle.

Images