Benthic submersible of deep sea hydrothermal sampling system and state control method

Abstract

The invention provides a deep sea hydrothermal sampling system benthonic submersible and a state control method, and the method comprises the steps: planning a motion track according to a task demand, and dynamically adjusting the control force distribution weight of a propeller and a mechanical leg system according to an error between an actual motion state and an expected value, thereby optimizing the motion performance, and improving the stability of the underwater submersible. Through thrust distribution and inverse dynamics decomposition, the thrust and rotating speed of each propeller and the joint torque of the mechanical leg are accurately controlled, through the combined action of the mechanical leg and the propeller, the pose stability of the benthonic submersible in the working process is kept, the benthonic submersible has the capabilities of resisting ocean current interference and adapting to rugged terrains, and then the position pose precision of a hydrothermal sampling system is guaranteed. The operation efficiency is ensured, a closed-loop control system fuses sensor data, the data are processed by a state observer / filter, and the actual state of the submersible is fed back in real time, so that the precision and stability of motion control are improved, and the operation capability and adaptability of the submersible in a complex environment are enhanced.

Description

Technical Field

[0001] The present invention relates to the technical field of deep-sea submersibles, and in particular to a benthic submersible for a deep-sea hydrothermal sampling system and a state control method thereof. Background Art

[0002] There are a large number of hydrothermal areas on the deep seabed. The total heat of supercritical seawater at hydrothermal vents is as high as gigawatts. Sampling hydrothermal fluids with high heat flux density and directly usable, and understanding their specific composition and characteristics, will help to further target and efficiently utilize seabed hydrothermal fluids. However, due to the accumulation of sediments from long-term eruptions, the area around the seabed hydrothermal vents is a state of rugged rocks and towering peaks, making it difficult for traditional submersibles such as remote-controlled vehicles, underwater autonomous vehicles, and manned submersibles to land on the bottom for operation. At the same time, the small diameter of the hydrothermal vents and the high temperature difference, high flow rate, and high corrosion environment make it extremely difficult to accurately deploy the sampling system to the hydrothermal vents.

[0003] Announcement No. CN118857856B discloses a deep-sea hydrothermal vent location survey device and method based on a submersible, including a main cabin with an upward opening and an overall barrel-shaped structure, a connecting piece fixed between the outer wall of the main cabin and the outer wall of the head of the submersible, and a transfer column head inserted into the side of the circumferential outer wall of the main cabin away from the connecting piece, and two fixed tubes 1 and 2 extending toward the upper and lower positions are fixed to the circumferential outer wall of the transfer column head, and a closed convex cover is provided on the top of the main cabin; the extended detection cabin will detect the heat source position at the first time, and then suck samples from different positions as needed, and the suction power is located above the suction barrel in the main cabin, and the amount of suction is carried out according to the need to avoid over-pumping caused by excessive water pressure.

[0004] The above-mentioned hydrothermal sampling system needs to maintain a stable posture during operation to ensure maximum sampling efficiency. At the same time, the hydrothermal sampling system is large in size and difficult to carry directly. At present, it is impossible to accurately deploy the hydrothermal sampling system to the hydrothermal vent and maintain a stable posture under the interference of the vent jet, thereby reducing the operating efficiency. Summary of the Invention

[0005] In view of this, the present invention proposes a benthic submersible and state control method for a deep-sea hydrothermal sampling system. By placing the hydrothermal sampler on the benthic submersible and using a multimodal motion system based on thrusters and mechanical legs, combined with real-time sensor monitoring and error feedback mechanism, the position and posture of the benthic submersible during operation is kept stable, and the submersible has the ability to resist ocean current interference and adapt to rugged terrain, thereby ensuring the position and posture accuracy of the hydrothermal sampling system and ensuring operational efficiency.

[0006] The technical solution of the present invention is implemented as follows: In a first aspect, the present invention provides a state control method for a benthic submersible of a deep-sea hydrothermal sampling system, wherein the submersible includes a propeller and mechanical legs for adjusting the submersible's posture, and the method comprises the following steps:

[0007] S1, using a path planning algorithm combined with seabed topography data to generate the expected motion trajectory of the submersible from the current position to the target hydrothermal vent, and obtain the expected motion state data of the submersible's movement trajectory;

[0008] S2 uses inertial navigation IMU, high depth gauge and DVL sensor to collect the submersible's motion state data in real time, and fuses the multi-sensor data through extended Kalman filter to obtain the submersible's actual motion state feedback;

[0009] S3, compare the actual motion state feedback of the submersible with the planned expected motion state data, and calculate the position error and attitude error;

[0010] S4, based on the position error and attitude error, the PID control algorithm and backstepping method are used to calculate the translational force and rotational torque required to eliminate the position and attitude errors;

[0011] S5, establishing an optimization objective function, allocating weights to the propulsion module and the mechanical leg module according to the horizontal force required by the submersible to eliminate position errors, and obtaining the mechanical leg force and the propulsion force;

[0012] S6, based on the number and layout of the submersible's robotic legs, performs inverse dynamics decomposition on the robotic leg forces to obtain the torque of each joint of the robotic legs, and controls the movement of the robotic legs through actuators;

[0013] S7, based on the number and layout of the submersible thrusters, establish a thrust distribution model, use the pseudo-inverse method to distribute the thrust of the thrusters, obtain the speed of each thruster, and control the thruster movement through the actuator;

[0014] S8, through the movement of mechanical legs and thrusters, collects the submersible's motion state data in real time to generate the submersible's actual motion state feedback, forming a closed-loop control.

[0015] Based on the above technical solution, preferably, the step S2 uses an inertial navigation IMU, a high depth gauge and a DVL sensor to collect the submersible motion state data in real time, and fuses the multi-sensor data through an extended Kalman filter to obtain the actual motion state feedback of the submersible, including the following sub-steps:

[0016] The motion state data of the submersible include position, velocity, attitude and sensor bias data, which can be expressed as:

[0017]

[0018] Where p n =[x,y,z] T , expressed as the position in the geodetic coordinate system; v b =[u,v,w] T , expressed as the velocity in the carrier coordinate system; θ=[φ,θ,ψ] T , expressed as attitude angle; Expressed as the IMU accelerometer bias, Represents the IMU gyroscope bias;

[0019] The state equation of the submersible's state information changing with time is established, and the expression is:

[0020]

[0021] Where x is the motion state vector of the submersible, and u is the acceleration measurement value a of the IMU. m and the angular velocity measurement ω m , w is the process noise, modeled as zero-mean Gaussian noise, and its covariance matrix is Q;

[0022] The state equation is expanded into:

[0023]

[0024] Where, It is expressed as the rotation matrix from the carrier coordinate system to the earth coordinate system, and T(θ) is the angular velocity to Euler angular rate matrix, which is expressed as:

[0025]

[0026] g n =[0,0,g] T ≈[0,0,9.81] T , represented as the gravity vector; w a is the accelerometer measurement noise, w g is the random white noise of gyroscope angular velocity; w ba is the accelerometer bias random walk noise; w bg is the gyroscope bias random walk noise;

[0027] The first-order Euler method is used to discretize the state equation, which is expressed as:

[0028]

[0029] Where Δt is the sampling time;

[0030] According to the sensor observation value and the discretized state equation, the sensor observation equation is constructed, and the expression is:

[0031] z k =h(x k )+v k

[0032] Where z k is the observed value, which represents the data actually measured by the sensor at time k; h(x k ) is the observation function, representing the state x k A deterministic function that maps to observations; v k is the observation noise, which represents the random error introduced by the sensor measurement;

[0033] According to the observation equation of the depth gauge and sensor, the observation equation of the depth gauge is obtained, which is expressed as:

[0034]

[0035] Where p z is the direct measurement value of the depth gauge, v depth is the observation noise of the depth gauge;

[0036] According to the DVL and sensor observation equation, the DVL observation equation is obtained, which is expressed as:

[0037] v d =z DVL =v b +v DVL ,v DVL ~N(0,R DVL )

[0038] Where, v b DVL directly measures the velocity value, v DVL is the observation noise of DVL;

[0039] According to the IMU and sensor observation equations, the IMU observation equation is obtained, which is expressed as:

[0040]

[0041] According to the optimal estimated state and IMU measurement value at the previous moment, the extended Kalman filter (EKF) method is used to predict the state and covariance matrix at the current moment.

[0042] When the DVL or depth gauge data is refreshed, the predicted state is corrected using the observation value and observation equation, the Kalman gain is calculated, and the state estimate and covariance matrix are updated;

[0043] When the IMU data is refreshed, the current state is predicted by the state equation without observation correction;

[0044] When the DVL or depth gauge data is refreshed, the predicted state at the current moment is fused with the observation values of the DVL and depth gauge, and the optimal estimated state information of the submersible is calculated through a filtering algorithm to obtain the actual motion state feedback of the submersible.

[0045] Based on the above technical solution, preferably, the step S3 of comparing the actual motion state feedback of the submersible with the planned expected motion state data to calculate the position error and attitude error includes the following sub-steps:

[0046] The planned expected motion state data includes an expected position and an expected posture;

[0047] The actual position feedback from the actual motion state of the submersible is compared with the expected position to calculate the position error, which is expressed as:

[0048] e p =x d -x

[0049] Where, e p is the position error vector, X d is the expected position vector, X is the real-time monitoring position vector;

[0050] The actual attitude feedback from the actual motion state of the submersible is compared with the corresponding expected attitude, and the attitude error is calculated. The expression is:

[0051] e a =θ d -θ

[0052] Where, e a is the attitude error vector, θ d is the expected posture information, and θ is the real-time monitoring posture information.

[0053] Based on the above technical solution, preferably, the step S4 includes the following sub-steps: performing preliminary adjustments based on the position and attitude errors using a PID control algorithm, and calculating the translational force and rotational torque required to eliminate the position and attitude errors using a backstepping method.

[0054] According to the position and attitude errors, PID control calculation is used to obtain the compensation control force and rotation torque, respectively. The expressions are:

[0055]

[0056] Where, To compensate for the control force, They are proportional gain, integral gain and differential gain of position control respectively; are the proportional gain, integral gain and differential gain of attitude control respectively, m is the mass of the submersible, g is the gravity acceleration vector, τ d To compensate for the rotational torque;

[0057] S32, according to the compensation control force, the backstepping method is used for dynamic calculation to obtain the compensation horizontal force, which is expressed as:

[0058]

[0059] Where, F d To compensate for the flat force, V d is the expected speed, the expected speed V d is the first-order derivative of the desired position with respect to time, V is the actual velocity; ξ p is a dummy control variable, α p is the dynamic surface attenuation coefficient.

[0060] Based on the above technical solution, preferably, the optimization objective function is established in step S5, and weights are allocated to the thruster module and the mechanical leg module according to the horizontal force required by the submersible to eliminate the position error, so as to obtain the mechanical leg force and the thruster force, including the following sub-steps:

[0061] An optimization objective function is constructed to minimize the weighted execution cost of the thrusters and mechanical legs when performing compensatory flat force. The expression is:

[0062]

[0063] Where, F leg is the mechanical leg force, F thr is the thruster force, W1 is the force mapping diagonal weight matrix of the robotic leg, W2 is the force mapping diagonal weight matrix of the thruster, and st represents the constraint condition;

[0064] Using the pseudo-inverse distribution method, the compensating horizontal force is substituted into the pseudo-inverse formula to calculate the mechanical leg force and propeller force, which are expressed as:

[0065]

[0066] F thr =F d -F leg

[0067] Where, F leg is the mechanical leg force, F thr is the thruster force.

[0068] Based on the above technical solution, preferably, step S6 includes the following sub-steps: performing inverse dynamics decomposition on the mechanical leg forces according to the number and layout of the submersible's mechanical legs to obtain the torque of each joint of the mechanical legs, and controlling the movement of the mechanical legs through actuators:

[0069] An inverse dynamics model of the robotic leg is established. Using the inverse dynamics model, the driving torque of each joint of the robotic leg is calculated. The expression is:

[0070]

[0071] Where, τ i is the driving torque of the i-th joint of the robotic leg, q is the current joint angle of the robotic leg, M i (q) is the inertia matrix of joint i, are the Coriolis force and centrifugal force terms at joint i, G i (q) is the gravity term of joint i, is the transpose of the Jacobian matrix of joint i, F leg is the mechanical leg force;

[0072] According to the driving torque τ of each joint i , generate corresponding actuator control instructions, the actuator generates corresponding driving torque according to the control instructions, drives the various joints of the mechanical leg to move, adjusts the posture and position of the mechanical leg, and realizes the control of the submarine's posture.

[0073] Based on the above technical solution, preferably, the method described in step S7 is to establish a thrust distribution model according to the number and layout of the submersible thrusters, distribute the thrust of the thrusters using a pseudo-inverse method, obtain the rotation speed of each thruster, and control the motion of the thrusters through the actuator, including the following sub-steps:

[0074] According to the number and layout of the submersible thrusters, a thrust distribution model is established, and the thruster force is substituted into the thrust model. The pseudo-inverse method is used to solve the speed command of each thruster. The expression is:

[0075] F thr =T·K t ·n 2

[0076]

[0077] Where, T represents the thruster configuration matrix, K t =diag(k t1 ,...,k tm ), K t represents the thrust coefficient matrix, n=[n1,...,n m ]T , n represents the propeller speed instruction; T + represents the pseudo-inverse matrix of the thrust configuration matrix, K t -1 represents the inverse matrix of the thrust coefficient matrix K;

[0078] The saturation limit algorithm is used to saturate the speed command of each propeller, and the speed command after saturation limit is obtained. The expression is:

[0079] n cmd =sat(n,n min ,n max )

[0080] Where n cmd is the speed command after saturation limitation, sat(·) is the speed saturation function, n min is the minimum speed limit of the propeller, n max is the maximum speed limit of the propeller;

[0081] According to the speed instruction after saturation limit, the corresponding actuator control instruction is generated. The actuator generates the corresponding speed according to the control instruction, drives the propeller to rotate to generate thrust, and adjusts the attitude and position of the submersible.

[0082] In a second aspect, the present invention further provides a benthic submersible for a deep-sea hydrothermal sampling system, which is used to execute the state control method for the benthic submersible for the deep-sea hydrothermal sampling system, comprising a hydrothermal sampling mechanism and a benthic submersible mechanism, wherein:

[0083] A circular groove is provided in the middle of the benthic submersible mechanism, and a hydrothermal sampling mechanism is arranged in the circular groove for extracting and sampling deep-sea hydrothermal fluids;

[0084] The benthic submersible mechanism includes four thrusters and four mechanical legs. The four thrusters and four mechanical legs are arranged at the four corners of the benthic submersible mechanism, and two adjacent thrusters and two adjacent mechanical legs are symmetrically arranged. The pushing direction of the thrusters is horizontally arranged with the benthic submersible mechanism; the setting direction of the mechanical legs is vertically arranged with the benthic submersible mechanism. The four thrusters and four mechanical legs are used to adjust the posture of the submersible.

[0085] On the basis of the above technical solution, preferably, the hydrothermal sampling mechanism includes an extractor, a sampling sensor, and a hydrothermal storage device, wherein the hydrothermal storage device is arranged in a circular trough for storing hydrothermal samples, and the sampling sensor is arranged in the hydrothermal storage device for testing and recording the physical and chemical properties of the hydrothermal samples; the extractor is arranged on a side of the hydrothermal storage device close to the benthic submersible mechanism, and one end of the extractor is connected to the hydrothermal storage device for extracting the hydrothermal samples into the hydrothermal storage device.

[0086] On the basis of the above technical solution, preferably, the mechanical leg includes a bearing bracket, a housing, a thigh support arm, a calf support arm, a connecting rod, a calf joint motor, a thigh joint motor and a hip joint motor, wherein,

[0087] The hip joint motor is fixed to the benthic submersible mechanism, and the output shaft of the hip joint motor is fixedly connected to the top end of the bearing bracket to drive the mechanical legs to swing in and out;

[0088] The bearing bracket has two rotating cylinder parts, the housing is rotatably connected between the two rotating cylinder parts, and the thigh joint motor is fixed to the side surface of one rotating cylinder part, and the output shaft of the thigh joint motor is fixedly connected to the housing to drive the housing to swing back and forth along the central axis of the two rotating cylinder parts;

[0089] The calf joint motor is rotatably connected to the rotating cylinder portion on the side away from the thigh joint motor, and the calf joint motor is fixedly connected to the housing. The output shaft of the calf joint motor is hinged to one end of the thigh support arm, and the other end of the thigh support arm is hinged to the calf support arm on the housing. One end of the connecting rod is hinged to the driving disk of the calf joint motor, and the other end of the connecting rod is hinged to one end of the calf support arm. The other end of the calf support arm serves as a support point between the mechanical leg and the ground.

[0090] A parallelogram is formed between the connecting rod, the calf support arm and the thigh support arm, and the calf joint motor is used to control the calf support arm to swing forward and backward through the parallelogram connecting rod.

[0091] The benthic submersible and state control method of the deep-sea hydrothermal sampling system of the present invention have the following beneficial effects compared with the prior art:

[0092] (1) The motion performance is optimized by planning the motion trajectory according to the mission requirements and dynamically adjusting the control force distribution weights of the thruster and mechanical leg system based on the error between the actual motion state and the expected value; the thrust and speed of each thruster and the joint torque of the mechanical leg are accurately controlled through thrust distribution and inverse dynamics decomposition. Through the combined action of the mechanical leg and thruster, the posture of the benthic submersible is kept stable during operation, and it has the ability to resist ocean current interference and adapt to rugged terrain, thereby ensuring the position and posture accuracy of the hydrothermal sampling system and ensuring operational efficiency.

[0093] (2) The extended Kalman filter (EKF) achieves spatiotemporal alignment and optimal fusion of multi-sensor data, and after processing by the state observer / filter, the actual state of the submersible is fed back in real time, significantly improving the accuracy and stability of motion control and enhancing the submersible's operational capability and adaptability in complex environments.

[0094] (3) By constructing the objective function, the optimal force distribution scheme can be found under the premise of satisfying the constraints, so that the total cost of the thrusters and mechanical legs in performing the compensatory flat force is minimized, thereby improving the motion control efficiency of the submersible, reducing energy consumption, and enhancing the stability of the system; and using the pseudo-inverse distribution method, the mechanical leg force and thruster force values can be obtained, thereby eliminating position errors and maintaining a stable posture. BRIEF DESCRIPTION OF THE DRAWINGS

[0095] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the following briefly introduces the drawings required for use in the embodiments or the description of the prior art. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying any creative work.

[0096] Figure 1 This is a flow chart of a state control method for a benthic submersible in a deep-sea hydrothermal sampling system according to the present invention;

[0097] Figure 2 A system block diagram of a state control method for a benthic submersible in a deep-sea hydrothermal sampling system according to the present invention;

[0098] Figure 3 A structural perspective view of a benthic submersible for a deep-sea hydrothermal sampling system according to the present invention;

[0099] Figure 4 A three-dimensional diagram of the benthic submersible mechanism of the benthic submersible for the deep-sea hydrothermal sampling system of the present invention;

[0100] Figure 5 A cross-sectional view of the hydrothermal sampling mechanism of a benthic submersible in a deep-sea hydrothermal sampling system according to the present invention;

[0101] Figure 6 A three-dimensional diagram of the mechanical leg structure of a benthic submersible for a deep-sea hydrothermal sampling system according to the present invention;

[0102] Figure 7 Schematic diagram of the movement principle of the mechanical leg and shank of the benthic submersible of the deep-sea hydrothermal sampling system of the present invention;

[0103] Figure 8 A schematic diagram of the operation flow of a benthic submersible for a deep-sea hydrothermal sampling system according to the present invention;

[0104] Figure 9 Schematic diagram of the horizontal layout of the propellers of the benthic submersible of the deep-sea hydrothermal sampling system of the present invention. DETAILED DESCRIPTION

[0105] The following will be combined with the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the embodiments described are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative efforts are within the scope of protection of the present invention.

[0106] like Figure 3 As shown, a benthic submersible of a deep-sea hydrothermal sampling system of the present invention includes a hydrothermal sampling mechanism 1 and a benthic submersible mechanism 2, wherein a circular groove 200 is opened in the middle of the benthic submersible mechanism 2, and the hydrothermal sampling mechanism 1 is arranged in the circular groove 200 for extracting and sampling deep-sea hydrothermal fluids.

[0107] It should be noted that this benthic submersible adopts a semi-enclosed symmetrical overall configuration, and the hydrothermal sampling mechanism 1 is installed at the center of the benthic submersible mechanism, which not only ensures that the core electromechanical components are away from the hydrothermal vents to avoid high-temperature damage, but also ensures the overall stability of the submersible.

[0108] like Figure 5 As shown, the hydrothermal sampling mechanism 1 in this embodiment includes an extractor 11, a sampling sensor 12, and a hydrothermal storage device 13, wherein the hydrothermal storage device 13 is arranged in a circular groove 200 for storing hydrothermal samples, and the sampling sensor 12 is arranged in the hydrothermal storage device 13 for testing and recording the physical and chemical properties of the hydrothermal samples; the extractor 11 is arranged on a side of the hydrothermal storage device 13 close to the benthic submersible mechanism 2, and one end of the extractor 11 is connected to the hydrothermal storage device 13 for extracting hydrothermal samples into the hydrothermal storage device 13.

[0109] It should be noted that the extractor 11 and the sampling sensor 12 can be remotely or autonomously controlled through the control system; the extractor 11 is made of high-temperature resistant and corrosion-resistant materials, can be directly inserted into the hydrothermal flow, and quickly collect liquid and solid material samples under high-temperature and high-pressure conditions; it can improve operational efficiency, reduce pollution risks, reduce operating costs, and promote research on deep-sea hydrothermal sampling operations and exploration and analysis.

[0110] like Figure 4 and Figure 9 As shown, the benthic submersible mechanism 2 includes four thrusters 21 and four mechanical legs 22. The four thrusters 21 and the four mechanical legs 22 are all arranged at the four corners of the benthic submersible mechanism 2, and two adjacent thrusters 21 and two adjacent mechanical legs 22 are symmetrically arranged. The pushing direction of the thruster 21 is horizontally arranged with the benthic submersible mechanism 2; the setting direction of the mechanical legs 22 is vertically arranged with the benthic submersible mechanism 2. The four thrusters 21 and the four mechanical legs 22 are used to adjust the posture of the submersible.

[0111] In addition, the benthic submersible mechanism 2 also includes a main frame, buoyancy material, an underwater communication system, an underwater navigation and positioning system, an underwater detection system, a control system and an energy system, wherein the four thrusters 21 and the four mechanical legs 22 are all arranged on the main frame, the four thrusters 21 are horizontally arranged on the main frame, and the four mechanical legs 22 are centrally symmetrically arranged at the bottom of the main frame, and the buoyancy material is arranged on the top of the main frame. Due to the huge pressure in the deep-sea environment, the role of the buoyancy material is to provide a certain buoyancy, reduce the weight of the submersible, and reduce the difficulty of the submersible sinking in the deep sea. At the same time, it also helps the submersible to adjust its posture during operation. The underwater communication system, underwater navigation and positioning system, underwater detection system, control system and energy system are all arranged on the main frame.

[0112] The benthic submersible in this embodiment adopts a multi-mode motion system that combines mechanical legs 21 and thrusters 22. Through redundant motion capability design, it has the ability to resist ocean current interference and adapt to rugged terrain, thereby ensuring the position and attitude accuracy of the hydrothermal sampling system and ensuring operational efficiency; and it is equipped with a high-precision inertial navigation and positioning system to ensure that the submersible can accurately find and approach hydrothermal vents in complex deep-sea terrain, adjust the sampling position in real time, and adapt to the hydrothermal vent environment. At the same time, it is equipped with an energy system and an underwater communication system. In order to ensure long-term operational capability, the system integrates an efficient energy management system that supports the combined use of batteries and renewable energy sources such as thermoelectric power generation and tidal power generation. The underwater communication module ensures stable data transmission between the submersible and the mother ship, realizing real-time monitoring and command reception and transmission.

[0113] like Figure 6 and Figure 7As shown, the mechanical leg 22 includes a bearing bracket 201, a shell 202, a thigh support arm 203, a calf support arm 204, a connecting rod 205, a calf joint motor 206, a thigh joint motor 207 and a hip joint motor 208, wherein the hip joint motor 208 is fixed on the benthic submersible mechanism 2, and the output shaft of the hip joint motor 208 is fixedly connected to the top of the bearing bracket 21, for driving the mechanical leg 22 to swing inward and outward; the bearing bracket 201 has two rotating cylinder parts 209, the shell 202 is rotatably connected between the two rotating cylinder parts 209, and the thigh joint motor 207 is fixed on the side of the rotating cylinder part 209 on one side, and the output shaft of the thigh joint motor 207 is fixedly connected to the shell 202, for driving the shell 202 along the central axis of the two rotating cylinder parts 209 Swing back and forth; the calf joint motor 206 is rotatably connected in the rotating cylinder portion 209 away from the side of the thigh joint motor 207, and the calf joint motor 206 is fixedly connected to the shell 202, the output shaft of the calf joint motor 206 is hinged to one end of the thigh support arm 203, and the other end of the thigh support arm 203 is hinged to the calf support arm 204 on the shell 202, one end of the connecting rod 205 is hinged to the drive disk of the calf joint motor 206, and the other end of the connecting rod 205 is hinged to one end of the calf support arm 204, and the other end of the calf support arm 204 serves as the support point of the mechanical leg and the ground; a parallelogram is formed between the connecting rod 205, the calf support arm 204 and the thigh support arm 203, and the calf support arm 204 is controlled to swing back and forth through the parallelogram connecting rod by the calf joint motor 206.

[0114] It should be noted that when the horizontal extension direction of the robotic leg 22 needs to be adjusted, the hip joint motor 208 is activated, and its output shaft drives the bearing bracket 201 to rotate. Since the other components of the robotic leg are connected to the bearing bracket 201, the robotic leg 22 as a whole can swing inward and outward relative to the benthic submersible mechanism 2. When the thigh joint motor 207 is in operation, its output shaft drives the housing 202 to swing back and forth along the central axis of the rotating cylinder 209, causing the thigh arm 203 and the shank arm 204 in the housing 202 to move accordingly, achieving forward and backward movement of the thigh portion of the robotic leg, allowing the robotic leg to adjust its position in the forward and backward direction to adapt to different terrain undulations. The shank joint motor 206 drives its output shaft to drive the connecting rod 205 to move. Because the connecting rod 205 forms a parallelogram structure with the shank arm 204 and thigh arm 203, according to the characteristics of the parallelogram, the shank arm 204 will swing back and forth with the movement of the connecting rod 205, making the movement of the shank arm 204 more stable and precise, better able to adapt to changes in terrain, and providing stable support for the robotic leg.

[0115] In addition, the hip joint motor 208, the thigh joint motor 207, and the calf joint motor 206 use an integrated motor, which is sealed with a metal shell on the outside and has two grid rings as dynamic seals at the output shaft to ensure that the motor works normally underwater and outputs a large torque; and the three joint motors are all arranged at the root to facilitate overall wiring and reduce the inertia of the mechanical leg during movement. The mechanical leg is longer and has a larger working space, which is conducive to the robot crossing rugged terrain.

[0116] like Figure 8 As shown, in this example, the workflow of the hydrothermal sampling system is as follows: the benthic submersible is deployed from the mother ship into the water, and according to the pre-detected hydrothermal vent location, guided by the high-precision navigation and positioning system, it cruises to the vicinity of the hydrothermal vent using the thruster 22, and senses the specific location of the hydrothermal vent according to the underwater detection system, and feeds back the benthic submersible status information and the hydrothermal vent location information to the mother ship host operator in real time through the underwater communication system; the benthic submersible accurately deploys the hydrothermal sampling mechanism 1 to the hydrothermal vent through remote control or automatic control, and according to the actual environment near the hydrothermal vent, the mechanical legs 21 are deployed and supported around the hydrothermal vent to support the normal operation of the hydrothermal sampling mechanism 1 at the hydrothermal vent; during the operation of the hydrothermal sampling mechanism 1, it will be disturbed by the seabed current and the hydrothermal vent jet, and the actual movement state of the benthic submersible is monitored by the navigation and positioning system as feedback, and the posture stability of the benthic submersible during operation is maintained through the combined action of the mechanical legs 21 and the thruster 22.

[0117] like Figure 1 and Figure 2 As shown, in a second aspect, the present invention further provides a state control method for a benthic submersible of a deep-sea hydrothermal sampling system, the method comprising the following steps:

[0118] S1, using a path planning algorithm combined with seabed topography data to generate the expected motion trajectory of the submersible from the current position to the target hydrothermal vent, and obtain the expected motion state data of the submersible's movement trajectory;

[0119] It should be noted that the RRT path planning algorithm is selected based on the complexity of the seabed terrain and the motion characteristics of the submersible, and seabed terrain data, including seabed depth, obstacle distribution and other information, is obtained through sensors such as sonar and lidar; the acquired terrain data is preprocessed, such as filtering, denoising, and interpolation, to improve the accuracy and availability of the data; at the same time, the terrain data is converted into a format that can be recognized by the path planning algorithm, and the processed seabed terrain data is input into the path planning algorithm for path search. The algorithm will generate one or more candidate paths based on the starting position, target position and terrain constraints of the submersible; the candidate paths are evaluated and optimized, and the optimal path is selected as the expected motion trajectory; the force conditions during the posture stabilization control of the benthic submersible are analyzed, and a submersible dynamic model is established; based on the generated expected motion trajectory and the submersible dynamic model, the expected motion state data of the submersible's movement trajectory is obtained, and the expected motion state data such as the speed, acceleration, and attitude of the submersible at different positions are extracted. This data will be used in subsequent control steps to ensure that the submersible can reach the target position according to the expected trajectory and state.

[0120] In this embodiment, the force conditions of a benthic submersible during attitude stabilization control are analyzed, and a submersible dynamic model is established. This includes defining an inertial coordinate system and a body coordinate system. The inertial coordinate system is fixed to the seabed, with the Z axis pointing vertically downward, the X axis pointing to geographic north, and the Y axis pointing to geographic east; the body coordinate system is fixed to the body's center of mass, with the X axis pointing forward, the Y axis pointing to the right, and the Z axis vertically downward.

[0121] Construct the kinematic equation for the conversion between the submersible's inertial coordinate system and the body coordinate system. The expression is:

[0122]

[0123] Where η is the generalized position vector in the inertial coordinate system, including the position and attitude information of the submersible; is the generalized velocity vector, which represents the time derivative of the generalized position vector in the inertial coordinate system, and J(η) is the kinematic transformation matrix, where

[0124]

[0125] Where, is the rotation matrix from the fuselage coordinate system to the inertial coordinate system, and T(Θ) is the attitude angular velocity conversion matrix;

[0126] Based on the inertial coordinate system and the body coordinate system, the posture stability control force of the submersible is analyzed and the submersible dynamic model is established. The expression is:

[0127]

[0128] Where M is the inertia matrix, ν is the velocity vector in the body coordinate system, C(v) is the Coriolis centripetal matrix, D(v) is the hydrodynamic damping matrix, g(η) is the restoring force matrix, and τ 腿 is the torque generated by the robotic leg, τ 推 is the torque generated by the thruster.

[0129] Where M is the inertia matrix, M=M R +M A , M R is the rigid body inertia matrix, M A is the additional mass matrix; the rigid body inertia matrix M R The expression is:

[0130]

[0131] Where m is the total mass of the robot, I3 is the 3×3 unit matrix, and I G is the moment of inertia tensor about the center of mass;

[0132] Additional mass matrix M A The expression is:

[0133]

[0134] Where, is the additional mass coefficient in each direction of linear velocity, is the additional mass coefficient in the direction of angular velocity;

[0135] v is the velocity vector in the submersible coordinate system, including the linear velocity and angular velocity of the submersible, and its expression is:

[0136]

[0137] Where, u, v, w are the linear velocities of the submersible, u is the forward velocity of the submersible, v is the lateral velocity of the submersible, and w is the vertical velocity of the submersible; p, q, r are the angular velocities of the submersible, p is the roll angular velocity of the submersible, q is the pitch angular velocity, and r is the yaw angular velocity of the submersible;

[0138] C(v) is the Coriolis centripetal matrix, which includes the Coriolis effect terms of rigid body and hydrodynamic added mass, and satisfies antisymmetry. The expression is:

[0139] C(v)=-C T (v)

[0140]

[0141] Where S(·) is the skew-symmetric matrix of vector cross product, O3 is the skew-symmetric matrix of vector cross product, v1=[u,v,w ]T ,v2=[p,q,r ] T , I G is the moment of inertia tensor about the center of mass, M R11 is the inertia of the submersible on the x-axis, M A11 is the inertial resistance of the fluid to the x-axis motion of the submersible;

[0142] D(v) is the hydrodynamic damping matrix, which is expressed as:

[0143] D(v)=diag(D u |u|,D v |v|,D w |w|,D p |p|,D q |q|,D r |r|)

[0144] Where D u ,D v ,D w is the linear velocity secondary damping coefficient, D p ,D q ,D r is the secondary damping coefficient of angular velocity;

[0145] η is the position vector in the inertial coordinate system, including the position and attitude angle of the submersible, and its expression is:

[0146]

[0147] Where x, y, z are the coordinates of the submersible position, θ, ψ are the attitude angles of the submersible;

[0148] g(η) is the restoring force matrix, which is expressed as:

[0149]

[0150] Where B is the buoyancy, mg is the gravity, and r B is the position vector of the center of buoyancy in the fuselage coordinate system, r G is the position vector of the center of gravity in the fuselage coordinate system.

[0151] The submersible has four mechanical legs, namely the left front leg, the right front leg, the left rear leg and the right rear leg. The support force at the end of the mechanical legs is mapped to the center of mass of the submersible through the Jacobian matrix to obtain the equivalent moment, which is expressed as:

[0152]

[0153] Where, J iis the Jacobian matrix of the i-th leg, which represents the mapping from joint velocity to foot velocity, F i is the foot contact force, where F1 is the left front, F2 is the right front, F3 is the left back, and F4 is the right back leg. The position of the four-legged machine system relative to the center of mass is in:

[0154]

[0155] The foot end contact force satisfies the friction force cone limitation, and the expression is:

[0156]

[0157] Where μ is the friction coefficient, f ix ,f iy ,f iz F i Components in the geodetic coordinate system;

[0158] The thrust of the propeller is mapped to the equivalent force / torque at the center of mass, expressed as:

[0159] τ 推 =B t T

[0160] Where B t is the thrust distribution matrix of the submersible, and T is the thrust of a single thruster.

[0161]

[0162] In the formula, the symbol c represents the cosine function cos(·), and the symbol s represents the sine function sin(·). h 、y h Represents the horizontal propeller and carrier coordinate system x B 、y B The moment arm, x v 、y v Represents the vertical thruster and carrier coordinate system x B 、y B The lever arm.

[0163] S2 uses inertial navigation IMU, high depth gauge and DVL sensor to collect submersible motion status data in real time, and fuses multi-sensor data through extended Kalman filter to obtain the actual motion status feedback of the submersible.

[0164] In this embodiment, step S2 includes the following sub-steps:

[0165] The motion state data of the submersible include position, velocity, attitude and sensor bias data, which can be expressed as:

[0166]

[0167] Where p n =[x,y,z] T , expressed as the position in the geodetic coordinate system; v b =[u,v,w] T , expressed as the velocity in the carrier coordinate system; θ=[φ,θ,ψ] T , expressed as attitude angle; Expressed as the IMU accelerometer bias, Represents the IMU gyroscope bias;

[0168] The state equation of the submersible's state information changing with time is established, and the expression is:

[0169]

[0170] Where x is the motion state vector of the submersible, and u is the acceleration measurement value a of the IMU. m and the angular velocity measurement ω m , w is the process noise, modeled as zero-mean Gaussian noise, and its covariance matrix is Q;

[0171] The state equation is expanded into:

[0172]

[0173] Where, It is expressed as the rotation matrix from the carrier coordinate system to the earth coordinate system, and T(θ) is the angular velocity to Euler angular rate matrix, which is expressed as:

[0174]

[0175] g n =[0,0,g] T ≈[0,0,9.81] T , represented as the gravity vector; w a is the accelerometer measurement noise, w g is the random white noise of gyroscope angular velocity; w ba is the accelerometer bias random walk noise; w bg is the gyroscope bias random walk noise;

[0176] The first-order Euler method is used to discretize the state equation, which is expressed as:

[0177]

[0178] Where Δt is the sampling time;

[0179] According to the sensor observation value and the discretized state equation, the sensor observation equation is constructed, and the expression is:

[0180] z k =h(x k )+v k

[0181] Where z k is the observed value, which represents the data actually measured by the sensor at time k; h(x k ) is the observation function, representing the state x k A deterministic function that maps to observations; v k is the observation noise, which represents the random error introduced by the sensor measurement;

[0182] According to the observation equation of the depth gauge and sensor, the observation equation of the depth gauge is obtained, which is expressed as:

[0183]

[0184] Where p z is the direct measurement value of the depth gauge, v depth is the observation noise of the depth gauge;

[0185] According to the DVL and sensor observation equation, the DVL observation equation is obtained, which is expressed as:

[0186] v d =z DVL =v b +v DVL ,v DVL ~N(0,R DVL )

[0187] Where, v b DVL directly measures the velocity value, v DVL is the observation noise of DVL;

[0188] According to the IMU and sensor observation equations, the IMU observation equation is obtained, which is expressed as:

[0189]

[0190] According to the optimal estimated state and IMU measurement value at the previous moment, the extended Kalman filter (EKF) method is used to predict the state and covariance matrix at the current moment.

[0191] When the DVL or depth gauge data is refreshed, the predicted state is corrected using the observation value and observation equation, the Kalman gain is calculated, and the state estimate and covariance matrix are updated;

[0192] When the IMU data is refreshed, the current state is predicted by the state equation without observation correction;

[0193] When the DVL or depth gauge data is refreshed, the predicted state at the current moment is fused with the observation values of the DVL and depth gauge, and the optimal estimated state information of the submersible is calculated through a filtering algorithm to obtain the actual motion state feedback of the submersible.

[0194] It should be noted that the extended Kalman filter (EKF) method is used to predict the state and covariance matrix at the current moment based on the optimal estimated state and IMU measurement value at the previous moment, where:

[0195] State prediction expression:

[0196]

[0197] Where, is the predicted state, indicating that the state at time k is based on the data at the previous time k-1. is the optimal estimated state at the previous moment, f(·) is the nonlinear state equation; u k is the control input, Δt is the time step, and 0 means ignoring process noise.

[0198] Covariance matrix prediction expression:

[0199]

[0200] Where, P k|k-1 is the prediction covariance matrix, F k-1 is the state transfer Jacobian matrix, Linearized at, the expression is:

[0201]

[0202] P k-1|k-1 is the covariance matrix of the previous moment, Q k-1 is the process noise covariance matrix, including w a 、w g 、w ba 、w bg Statistical properties.

[0203] When the DVL or depth gauge data is refreshed, the predicted state is corrected using the observation value and observation equation, the Kalman gain is calculated, and the state estimate and covariance matrix are updated;

[0204] The expression for calculating the Kalman gain is:

[0205]

[0206] Where K k is the Kalman gain, which is used to weigh the weight of prediction and observation, H k is the observation Jacobian matrix, Linearization: The expression is:

[0207]

[0208] R k is the observation noise covariance matrix.

[0209] The state estimation correction expression is:

[0210]

[0211] Where, is the modified optimal estimation state, z k is the actual observation value, h(·) is the nonlinear observation equation, is the new information, expressed as the difference between the observation and the prediction.

[0212] Covariance correction expression:

[0213] P k|k =(IK k H k )P k|k-1

[0214] Where, P k|k is the corrected covariance matrix, and I is the identity matrix.

[0215] When the IMU data is refreshed, the current state is predicted by the state equation without observation correction; the expression is:

[0216]

[0217] Where u k is the IMU input;

[0218] When the DVL or depth gauge data is refreshed, the predicted state at the current moment is fused with the observed values of the DVL and depth gauge, and the optimal estimated state information of the submersible is calculated through the filtering algorithm to obtain the actual motion state feedback of the submersible; the expression is:

[0219]

[0220] Where K k is the Kalman gain, and h(·) is the observation function.

[0221] It should be noted that the EKF achieves the spatiotemporal alignment and optimal fusion of multi-sensor data, which significantly improves the accuracy, robustness and environmental adaptability of the submersible's motion state estimation while ensuring real-time performance, providing a reliable state feedback basis for complex underwater missions.

[0222] S3, compare the actual motion state feedback of the submersible with the planned expected motion state data, and calculate the position error and attitude error.

[0223] Step S3 in this embodiment includes the following sub-steps:

[0224] The planned expected motion state data includes an expected position and an expected posture;

[0225] The actual position feedback from the actual motion state of the submersible is compared with the expected position to calculate the position error, which is expressed as:

[0226] e p =x d -x

[0227] Where, e p is the position error vector, X d is the expected position vector, X is the real-time monitoring position vector;

[0228] The actual attitude feedback from the actual motion state of the submersible is compared with the corresponding expected attitude, and the attitude error is calculated. The expression is:

[0229] e a =θ d -θ

[0230] Where, e a is the attitude error vector, θ d is the expected posture information, and θ is the real-time monitoring posture information.

[0231] S4, based on the position error and attitude error, PID control algorithm and backstepping method are used to calculate the translational force and rotational torque required to eliminate the position and attitude errors.

[0232] In this embodiment, step S4 includes the following sub-steps:

[0233] According to the position and attitude errors, PID control calculation is used to obtain the compensation control force and rotation torque, respectively. The expressions are:

[0234]

[0235] Where, To compensate for the control force, They are proportional gain, integral gain and differential gain of position control respectively; are the proportional gain, integral gain and differential gain of attitude control respectively, m is the mass of the submersible, g is the gravity acceleration vector, τ d To compensate for the rotational torque;

[0236] S32, according to the compensation control force, the backstepping method is used for dynamic calculation to obtain the compensation horizontal force, which is expressed as:

[0237]

[0238] Where, F d To compensate for the flat force, V d is the expected speed, the expected speed V d is the first-order derivative of the desired position with respect to time, V is the actual velocity; ξ p is a dummy control variable, α p is the dynamic surface attenuation coefficient.

[0239] It should be noted that by calculating the position error and attitude error, the difference between the actual motion state of the submersible and the desired motion state can be quantified; this enables the control algorithm to accurately know the gap between the current state and the desired state, thereby providing an accurate basis for subsequent control; and, the position error and attitude error are important input parameters of the PID control algorithm and backstepping method; the control algorithm calculates the control force required to eliminate the error based on the size and direction of the error, thereby driving the submersible's thrusters and mechanical legs to make adjustments, so that the submersible approaches the desired motion state.

[0240] S5, establish the optimization objective function, distribute the weights of the thruster module and the mechanical leg module according to the horizontal force required by the submersible to eliminate the position error, and obtain the mechanical leg force and the thruster force.

[0241] In this embodiment, step S5 includes the following sub-steps:

[0242] An optimization objective function is constructed to minimize the weighted execution cost of the thrusters and mechanical legs when performing compensatory flat force. The expression is:

[0243]

[0244] Where, F leg is the mechanical leg force, F thr is the thruster force, W1 is the force mapping diagonal weight matrix of the robotic leg, W2 is the force mapping diagonal weight matrix of the thruster, and st represents the constraint condition;

[0245] Using the pseudo-inverse distribution method, the compensating horizontal force is substituted into the pseudo-inverse formula to calculate the mechanical leg force and propeller force, which are expressed as:

[0246]

[0247] F thr =F d -F leg

[0248] Where, Fleg is the mechanical leg force, F thr is the thruster force.

[0249] It should be noted that by constructing the objective function, an optimal force distribution scheme can be found under the premise of satisfying the constraints, so that the total cost of the thrusters and mechanical legs in performing compensatory flat force is minimized, thereby improving the motion control efficiency of the submersible, reducing energy consumption, and enhancing the stability of the system; and by using the pseudo-inverse distribution method, the specific values of the mechanical leg force and the thruster force can be obtained; these values will be used as inputs for the subsequent control algorithm to drive the mechanical legs and thrusters to move, thereby eliminating position errors and maintaining a stable posture.

[0250] S6, according to the number and layout of the submersible's mechanical legs, performs inverse dynamics decomposition on the mechanical leg forces to obtain the torque of each joint of the mechanical legs, and controls the movement of the mechanical legs through actuators.

[0251] Step S6 includes the following sub-steps:

[0252] An inverse dynamics model of the robotic leg is established. Using the inverse dynamics model, the driving torque of each joint of the robotic leg is calculated. The expression is:

[0253]

[0254] Where, τ i is the driving torque of the i-th joint of the robotic leg, q is the current joint angle of the robotic leg, M i (q) is the inertia matrix of joint i, are the Coriolis force and centrifugal force terms at joint i, G i (q) is the gravity term of joint i, is the transpose of the Jacobian matrix of joint i, F leg is the mechanical leg force;

[0255] According to the driving torque τ of each joint i , generate corresponding actuator control instructions, the actuator generates corresponding driving torque according to the control instructions, drives the various joints of the mechanical leg to move, adjusts the posture and position of the mechanical leg, and realizes the control of the submarine's posture.

[0256] It should be noted that by establishing a support force distribution model and an inverse dynamics model of the mechanical leg, the driving torque of each joint of the mechanical leg can be accurately calculated, thereby improving the control accuracy of the submersible's attitude; reasonable support force distribution and mechanical leg motion control can maintain the submersible's stable attitude and reduce motion instability caused by external interference or internal parameter changes.

[0257] S7, based on the number and layout of the submersible thrusters, a thrust distribution model is established, the thruster force is distributed using the pseudo-inverse method, the speed of each thruster is obtained, and the thruster movement is controlled by the actuator.

[0258] In this embodiment, step S7 includes the following sub-steps:

[0259] According to the number and layout of the submersible thrusters, a thrust distribution model is established, and the thruster force is substituted into the thrust model. The pseudo-inverse method is used to solve the speed command of each thruster. The expression is:

[0260] F thr =T·K t ·n 2

[0261]

[0262] Where, T represents the thruster configuration matrix, K t =diag(k t1 ,...,k tm ), K t represents the thrust coefficient matrix, n=[n1,...,n m ] T , n represents the propeller speed instruction; T + represents the pseudo-inverse matrix of the thrust configuration matrix, K t -1 represents the inverse matrix of the thrust coefficient matrix K;

[0263] The saturation limit algorithm is used to saturate the speed command of each propeller, and the speed command after saturation limit is obtained. The expression is:

[0264] n cmd =sat(n,n min ,n max )

[0265] Where n cmd is the speed command after saturation limitation, sat(·) is the speed saturation function, n min is the minimum speed limit of the propeller, n max is the maximum speed limit of the propeller;

[0266] According to the speed instruction after saturation limit, the corresponding actuator control instruction is generated. The actuator generates the corresponding speed according to the control instruction, drives the propeller to rotate to generate thrust, and adjusts the attitude and position of the submersible.

[0267] It should be noted that by solving the thruster speed command through the pseudo-inverse method and combining it with the saturation limit algorithm, the thruster force can be reasonably distributed among the thrusters, thereby improving the control accuracy of the submarine's attitude and position. Reasonable thrust distribution and saturation limit can avoid performance degradation or failure of the thruster due to overload or underload, and enhance the stability of the system.

[0268] S8, through the movement of mechanical legs and thrusters, collects the submersible's motion state data in real time to generate the submersible's actual motion state feedback, forming a closed-loop control.

[0269] In this embodiment, the benthic submersible adopts a multimodal motion system based on thrusters and mechanical legs to achieve overall posture stability. The overall motion trajectory of the benthic submersible is planned according to the mission requirements. During the motion process, the motion state of the submersible is monitored in real time using sensors such as inertial navigation, high depth gauge, and DVL. When an error occurs between the actual motion state and the planned expected position and attitude angle, the posture controller distributes the total control force of the submersible body to the thruster system and the mechanical leg system according to the amount of error, where the distribution weight is comprehensively determined by the current motion stage and motion state of the submersible. Combined with the thruster layout of the benthic submersible, the thrust command of the thruster system is distributed to obtain The thrust and speed of each thruster are determined by the speed command sent by the electronic control system to control the movement of the thruster; combined with the layout of the benthic submersible's mechanical legs, the support force command of the mechanical leg system is decomposed by inverse dynamics to obtain the torque of each joint of the mechanical leg, and the electronic control system sends the current command of the joint to control the movement of the mechanical leg; during the operation of the thruster system and the mechanical leg system, the output data of each sensor are fused and processed by the state observer / filter to obtain the actual state feedback of the submersible, thereby forming a closed-loop control, enhancing the system's robustness and reliability, improving the load balancing capability and safety, and ensuring the submersible's ability to resist seabed disturbances and adapt to rugged terrain.

[0270] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, improvements, etc. made within the spirit and principles of the present invention should be included in the scope of protection of the present invention.

Claims

1. A state control method for a benthic submersible of a deep-sea hydrothermal sampling system, characterized by: The submersible includes a propeller and mechanical legs for adjusting the submersible's attitude, and the method includes the following steps: S1, using a path planning algorithm combined with seabed topography data to generate the expected motion trajectory of the submersible from the current position to the target hydrothermal vent, and obtain the expected motion state data of the submersible's movement trajectory; S2 uses inertial navigation IMU, high depth gauge and DVL sensor to collect the submersible's motion status data in real time, and fuses the multi-sensor data through the extended Kalman filter to obtain the submersible's actual motion status feedback; S3, compare the actual motion state feedback of the submersible with the planned expected motion state data, and calculate the position error and attitude error; S4, based on the position error and attitude error, the PID control algorithm and backstepping method are used to calculate the translational force and rotational torque required to eliminate the position and attitude errors; S5, establishing an optimization objective function, allocating weights to the propulsion module and the mechanical leg module according to the horizontal force required by the submersible to eliminate position errors, and obtaining the mechanical leg force and the propulsion force; S6, based on the number and layout of the submersible's robotic legs, performs inverse dynamics decomposition on the robotic leg forces to obtain the torque of each joint of the robotic legs, and controls the movement of the robotic legs through actuators; S7, based on the number and layout of the submersible thrusters, establish a thrust distribution model, use the pseudo-inverse method to distribute the thrust of the thrusters, obtain the speed of each thruster, and control the thruster movement through the actuator; S8, through the movement of mechanical legs and thrusters, collects the submersible's motion state data in real time to generate the submersible's actual motion state feedback, forming a closed-loop control.

2. The state control method of a benthic submersible of a deep-sea hydrothermal sampling system according to claim 1, characterized in that: Step S2 uses the inertial navigation IMU, high depth gauge and DVL sensor to collect the submersible motion state data in real time, and fuses the multi-sensor data through the extended Kalman filter to obtain the actual motion state feedback of the submersible, including the following sub-steps: The motion state data of the submersible include position, velocity, attitude and sensor bias data, which can be expressed as: Where p n =[x,y,z] T , expressed as the position in the geodetic coordinate system; v b =[u,v,w] T , expressed as the velocity in the carrier coordinate system; θ=[φ,θ,ψ] T , expressed as attitude angle; Expressed as the IMU accelerometer bias, Represents the IMU gyroscope bias; The state equation of the submersible's state information changing with time is established, and the expression is: Where x is the motion state vector of the submersible, and u is the acceleration measurement value a of the IMU. m and the angular velocity measurement ω m , w is the process noise, modeled as zero-mean Gaussian noise, and its covariance matrix is Q; The state equation is expanded into: Where, It is expressed as the rotation matrix from the carrier coordinate system to the earth coordinate system, and T(θ) is the angular velocity to Euler angular rate matrix, which is expressed as: g n =[0,0,g] T ≈[0,0,9.81] T , represented as the gravity vector; w a is the accelerometer measurement noise, w g is the random white noise of gyroscope angular velocity; w ba is the accelerometer bias random walk noise; w bg is the gyroscope bias random walk noise; The first-order Euler method is used to discretize the state equation, which is expressed as: Where Δt is the sampling time; According to the sensor observation value and the discretized state equation, the sensor observation equation is constructed, and the expression is: z k =h(x k )+v k Where z k is the observed value, which represents the data actually measured by the sensor at time k; h(x k ) is the observation function, representing the state x k A deterministic function that maps to observations; v k is the observation noise, which represents the random error introduced by the sensor measurement; According to the observation equation of the depth gauge and sensor, the observation equation of the depth gauge is obtained, which is expressed as: Where p z is the direct measurement value of the depth gauge, v depth is the observation noise of the depth gauge; According to the DVL and sensor observation equation, the DVL observation equation is obtained, which is expressed as: v d =z DVL =v b +v DVL ,v DVL ~N(0,R DVL ) Where, v b DVL directly measures the velocity value, v DVL is the observation noise of DVL; According to the IMU and sensor observation equations, the IMU observation equation is obtained, which is expressed as: According to the optimal estimated state and IMU measurement value at the previous moment, the extended Kalman filter (EKF) method is used to predict the state and covariance matrix at the current moment. When the DVL or depth gauge data is refreshed, the predicted state is corrected using the observation value and observation equation, the Kalman gain is calculated, and the state estimate and covariance matrix are updated; When the IMU data is refreshed, the current state is predicted by the state equation without observation correction; When the DVL or depth gauge data is refreshed, the predicted state at the current moment is fused with the observation values of the DVL and depth gauge, and the optimal estimated state information of the submersible is calculated through a filtering algorithm to obtain the actual motion state feedback of the submersible.

3. The state control method of a benthic submersible of a deep-sea hydrothermal sampling system according to claim 2, characterized in that: Step S3 compares the actual motion state feedback of the submersible with the planned expected motion state data to calculate the position error and attitude error, including the following sub-steps: The planned expected motion state data includes an expected position and an expected posture; The actual position feedback from the actual motion state of the submersible is compared with the expected position to calculate the position error, which is expressed as: e p =x d -x Where, e p is the position error vector, X d is the expected position vector, X is the real-time monitoring position vector; The actual attitude feedback from the actual motion state of the submersible is compared with the corresponding expected attitude, and the attitude error is calculated. The expression is: e a =θ d -θ Where, e a is the attitude error vector, θ d is the expected posture information, and θ is the real-time monitoring posture information.

4. The state control method for a benthic submersible of a deep-sea hydrothermal sampling system according to claim 3, characterized in that: Step S4, as described in the preceding, uses a PID control algorithm to perform preliminary adjustments based on the position and attitude errors, and calculates the translational force and rotational torque required to eliminate the position and attitude errors using a backstepping method, including the following sub-steps: According to the position and attitude errors, PID control calculation is used to obtain the compensation control force and rotation torque, respectively. The expressions are: Where, To compensate for the control force, They are proportional gain, integral gain and differential gain of position control respectively; are the proportional gain, integral gain and differential gain of attitude control respectively, m is the mass of the submersible, g is the gravity acceleration vector, τ d To compensate for the rotational torque; S32, according to the compensation control force, the backstepping method is used for dynamic calculation to obtain the compensation horizontal force, which is expressed as: Where, F d To compensate for the flat force, V d is the expected speed, the expected speed V d is the first-order derivative of the desired position with respect to time, V is the actual velocity; ξ p is a dummy control variable, α p is the dynamic surface attenuation coefficient.

5. The state control method of a benthic submersible of a deep-sea hydrothermal sampling system according to claim 4, characterized in that: Step S5 establishes an optimization objective function, distributes weights to the propulsion module and the mechanical leg module according to the horizontal force required by the submersible to eliminate position errors, and obtains the mechanical leg force and the propulsion force, including the following sub-steps: An optimization objective function is constructed to minimize the weighted execution cost of the thrusters and mechanical legs when performing compensatory flat force. The expression is: Where, F leg is the mechanical leg force, F thr is the thruster force, W1 is the force mapping diagonal weight matrix of the robotic leg, W2 is the force mapping diagonal weight matrix of the thruster, and st represents the constraint condition; Using the pseudo-inverse distribution method, the compensating horizontal force is substituted into the pseudo-inverse formula to calculate the mechanical leg force and propeller force, which are expressed as: F thr =F d -F leg Where, F leg is the mechanical leg force, F thr is the thruster force.

6. The state control method for a benthic submersible of a deep-sea hydrothermal sampling system according to claim 5, characterized in that: Step S6, based on the number and layout of the submersible's mechanical legs, performs inverse dynamics decomposition on the mechanical leg forces to obtain the torque of each joint of the mechanical legs, and controls the movement of the mechanical legs through actuators, including the following sub-steps: An inverse dynamics model of the robotic leg is established. Using the inverse dynamics model, the driving torque of each joint of the robotic leg is calculated. The expression is: Where, τ i is the driving torque of the i-th joint of the robotic leg, q is the current joint angle of the robotic leg, M i (q) is the inertia matrix of joint i, are the Coriolis force and centrifugal force terms at joint i, G i (q) is the gravity term of joint i, is the transpose of the Jacobian matrix of joint i, F leg is the mechanical leg force; According to the driving torque τ of each joint i , generate corresponding actuator control instructions, the actuator generates corresponding driving torque according to the control instructions, drives the various joints of the mechanical leg to move, adjusts the posture and position of the mechanical leg, and realizes the control of the submarine's posture.

7. The state control method for a benthic submersible of a deep-sea hydrothermal sampling system according to claim 5, characterized in that: Step S7, as described in the preceding, establishes a thrust distribution model based on the number and layout of the submersible's thrusters, distributes the thrust of the thrusters using a pseudo-inverse method, obtains the rotational speed of each thruster, and controls the thruster motion through the actuator, including the following sub-steps: According to the number and layout of the submersible thrusters, a thrust distribution model is established, and the thruster force is substituted into the thrust model. The pseudo-inverse method is used to solve the speed command of each thruster. The expression is: F thr =T·K t ·n 2 Where, T represents the thruster configuration matrix, K t =diag(k t1 ,...,k tm ), K t represents the thrust coefficient matrix, n=[n1,...,n m ] T , n represents the propeller speed instruction; T + represents the pseudo-inverse matrix of the thrust configuration matrix, K t -1 represents the inverse matrix of the thrust coefficient matrix K; The saturation limit algorithm is used to saturate the speed command of each propeller, and the speed command after saturation limit is obtained. The expression is: n cmd =sat(n,n min ,n max ) Where n cmd is the speed command after saturation limitation, sat(·) is the speed saturation function, n min is the minimum speed limit of the propeller, n max is the maximum speed limit of the propeller; According to the speed instruction after saturation limit, the corresponding actuator control instruction is generated. The actuator generates the corresponding speed according to the control instruction, drives the propeller to rotate to generate thrust, and adjusts the attitude and position of the submersible.

8. A benthic submersible for a deep-sea hydrothermal sampling system, characterized by: A method for controlling the state of a benthic submersible of a deep-sea hydrothermal sampling system according to any one of claims 1 to 7, comprising a hydrothermal sampling mechanism (1) and a benthic submersible mechanism (2), wherein: A circular groove (200) is provided in the middle of the benthic submersible mechanism (2), and the hydrothermal fluid sampling mechanism (1) is arranged in the circular groove (200) for extracting and sampling deep-sea hydrothermal fluids; The benthic submersible mechanism (2) comprises four thrusters (21) and four mechanical legs (22). The four thrusters (21) and the four mechanical legs (22) are all arranged at the four corners of the benthic submersible mechanism (2), and two adjacent thrusters (21) and two adjacent mechanical legs (22) are symmetrically arranged. The thrusting direction of the thrusters (21) is arranged horizontally with the benthic submersible mechanism (2); the arrangement direction of the mechanical legs (22) is arranged vertically with the benthic submersible mechanism (2). The four thrusters (21) and the four mechanical legs (22) are used to adjust the posture of the submersible.

9. The benthic submersible for deep-sea hydrothermal sampling system according to claim 8, characterized in that: The hydrothermal sampling mechanism (1) comprises an extractor (11), a sampling sensor (12), and a hydrothermal storage device (13), wherein the hydrothermal storage device (13) is arranged in a circular trough (200) for storing hydrothermal samples, and the sampling sensor (12) is arranged in the hydrothermal storage device (13) for testing and recording the physical and chemical properties of the hydrothermal samples; the extractor (11) is arranged on a side of the hydrothermal storage device (13) close to the benthic submersible mechanism (2), and one end of the extractor (11) is connected to the hydrothermal storage device (13) for extracting the hydrothermal samples into the hydrothermal storage device (13).

10. The benthic submersible for deep-sea hydrothermal sampling system according to claim 8, characterized in that: The mechanical leg (22) comprises a bearing bracket (201), a housing (202), a thigh support arm (203), a calf support arm (204), a connecting rod (205), a calf joint motor (206), a thigh joint motor (207) and a hip joint motor (208), wherein: The hip joint motor (208) is fixed on the benthic submersible mechanism (2), and the output shaft of the hip joint motor (208) is fixedly connected to the top end of the bearing bracket (21) to drive the mechanical leg (22) to swing inward and outward; The bearing bracket (201) has two rotating cylinder parts (209), the housing (202) is rotatably connected between the two rotating cylinder parts (209), and the thigh joint motor (207) is fixed on the side of the rotating cylinder part (209) on one side, and the output shaft of the thigh joint motor (207) is fixedly connected to the housing (202) for driving the housing (202) to swing back and forth along the central axis of the two rotating cylinder parts (209); The calf joint motor (206) is rotatably connected in a rotating cylinder portion (209) on a side away from the thigh joint motor (207), and the calf joint motor (206) is fixedly connected to the housing (202). The output shaft of the calf joint motor (206) is hinged to one end of the thigh support arm (203), and the other end of the thigh support arm (203) is hinged to the calf support arm (204) on the housing (202). A driving disk is fixed to the outer side of the output shaft of the calf joint motor (206), one end of the connecting rod (205) is hinged to the driving disk of the calf joint motor (206), and the other end of the connecting rod (205) is hinged to one end of the calf support arm (204), and the other end of the calf support arm (204) serves as a support point between the mechanical leg and the ground. A parallelogram is formed between the connecting rod (205), the calf support arm (204) and the thigh support arm (203), and the calf joint motor (206) controls the calf support arm (204) to swing forward and backward through the parallelogram connecting rod.

Images