Benthic submersible crawl mission control method and system

Abstract

This invention proposes a multi-task control method and system for bottom-dwelling submersibles, including: introducing additional mass and nonlinear hydrodynamic damping to modify the underwater dynamic equations, constructing a global generalized Jacobian mapping matrix, and mapping the thruster and mechanical legs to the total spin of the center of mass control; acquiring the real-time state and desired commands of the submersible, dividing the tasks into three priorities: rigid constraints, posture tracking, and energy consumption optimization, and generating characteristic parameter matrices at each level after linearizing the friction cone constraints; performing quadratic programming solutions by null space projection according to priority, accumulating the results of each level to the optimal solution and updating the null space basis matrix; constructing a fluid external dynamic pressure model to generate fine-tuning torque, which is mapped to the thruster subspace in the previous null space to compensate for the fuselage attitude; through global Jacobian mapping and hierarchical quadratic programming, multiple conflicting tasks such as anti-slip, tracking, and energy consumption can be completed collaboratively without interference, improving the stability and safety of the integrated swimming and crawling operation of the bottom-dwelling submersible.

Description

Technical Field

[0001] This invention relates to the field of motion control technology for deep-sea robots, and in particular to a multi-task control method and system for bottom-diving submersibles. Background Technology

[0002] Bottom-diving vehicles are a type of deep-sea operational equipment that combines underwater movement and seabed crawling capabilities. By integrating two heterogeneous drive devices—vector thrusters and multi-jointed robotic legs—they can perform sophisticated tasks such as sampling, drilling, and observation in various modes, including hovering, bottom-sitting, and slope crawling. They are one of the core pieces of equipment for deep-sea mineral resource exploration and scientific research. However, thrusters and robotic legs have fundamental differences in their physical characteristics. Thrusters provide spatial spin, while robotic legs provide contact reaction forces. These correspond to two completely different mathematical descriptions: rigid body dynamics and contact dynamics. This heterogeneity poses a severe challenge to the coordinated control in integrated swimming and crawling operations.

[0003] Existing deep-sea equipment control methods typically decouple thrust distribution from mechanical leg force control into independent modules, lacking effective means to integrate these two types of heterogeneous actuators into a unified control framework. When a submersible simultaneously performs multiple constraint tasks such as attitude tracking and foot anti-slip in complex sea conditions, conflicts arise between these tasks. Attitude tracking requires the fuselage to adjust its attitude, anti-slip constraints restrict the direction of foot force, and thrust distribution aims for optimal energy consumption. Traditional weighted single-layer quadratic programming methods rely on empirical weights to coordinate task priorities, which can easily trigger anti-slip boundaries due to compromises and interference between tasks in extreme sea conditions, leading to fuselage instability or even equipment damage, thus reducing the stability and safety of the submersible's integrated swimming and crawling operations. Summary of the Invention

[0004] In view of this, the present invention proposes a multi-task control method and system for bottom-dwelling submersibles that crawls and swims. By constructing a global generalized Jacobian mapping, the thruster and mechanical legs are unified into the same control framework. Furthermore, a task priority mechanism is established using hierarchical quadratic programming, enabling multiple conflicting tasks to be completed collaboratively without interference, thereby improving the stability and safety of the integrated crawling and swimming operation of the bottom-dwelling submersible.

[0005] The technical solution of the present invention is implemented as follows: In a first aspect, the present invention provides a multi-task control method for a benthic submersible's crawling, comprising the following steps: S1. By introducing an additional mass matrix and a nonlinear hydrodynamic damping spindle, the underwater dynamic equation of the submersible is established. A global generalized Jacobian mapping matrix is ​​constructed to map the thrust output of the thruster and the contact reaction force at the end of the mechanical leg into the total dynamic control spindle of the underwater dynamic equation. S2, obtain the real-time state of the submersible fed back by the state observer, the expected pose and acceleration commands issued by the navigation behavior layer, and the hardware physical limit parameters. According to the control priority, the task is divided into an absolutely rigid constraint set, a main task set for operation tracking, and a redundant zero space optimization set. The nonlinear friction cone constraint is linearized into a set of linear inequalities, and the characteristic parameter matrix set corresponding to each level is generated. S3. Based on the feature parameter matrix group of each level, the quadratic programming solution is performed step by step in the order of priority from high to low using the null space projection method. The solution of each level is performed in the null space of the previous level. The solution results are projected and accumulated to the cumulative optimal solution. After each level is completed, the null space basis matrix is ​​updated to obtain the optimal solution of the system's generalized control input. S4. Construct a fluid external dynamic pressure model modulated by the fuselage attitude, generate the desired fine-tuning torque, map the desired fine-tuning torque to the thruster subspace in the null space left by the previous mission, generate the null space control compensation increment of the thruster system to adjust the fuselage attitude.

[0006] Based on the above technical solutions, preferably, step S1 includes the following sub-steps: S11, Establish a generalized control input vector, which includes the desired thrust output vector of the thruster and the three-dimensional contact reaction force vector at the end of the mechanical leg; S12, establish the underwater dynamic equation of the submersible. The underwater dynamic equation is the sum of the rigid body mass inertia matrix and the additional mass matrix multiplied by the acceleration vector, plus the Coriolis and centripetal force matrix multiplied by the velocity vector, plus the nonlinear hydrodynamic damping screw, plus the restoring force and restoring torque screw. The result is equal to the total dynamic control screw acting on the center of mass. S13, construct a global generalized Jacobian mapping matrix, which includes a thruster static configuration mapping matrix and a mechanical leg smooth contact state mapping matrix; S14 multiplies the generalized control input vector with the global generalized Jacobian mapping matrix to obtain the total dynamic control spin, which maps the thrust output of the thruster and the contact reaction force at the end of the mechanical leg into the total dynamic control spin towards the center of mass.

[0007] Based on the above technical solutions, preferably, step S13, which involves constructing the global generalized Jacobian mapping matrix, includes the following steps: S131, obtain the thrust direction spatial unit vector and installation position vector of each thruster, use the thrust direction spatial unit vector of each thruster to form the upper part of the thrust mapping, use the cross product of the three-dimensional position vector of each thruster installation position and the thrust direction spatial unit vector to form the lower part of the force matrix mapping, and combine them to obtain the thruster static configuration mapping matrix. S132, obtain the number of mechanical legs currently in contact and the position vector of each mechanical leg foot end, multiply the contact force vector of each mechanical leg foot end by the corresponding contact probability factor to form the upper part of the thrust mapping, multiply the cross product of the position vector of each mechanical leg foot end and the contact force vector by the corresponding contact probability factor to form the lower part of the torque mapping, and combine to obtain the smooth contact state mapping matrix of the mechanical leg. S133, the static configuration mapping matrix of the thruster and the smooth contact state mapping matrix of the mechanical leg are spliced ​​together in the horizontal direction to obtain the global generalized Jacobian mapping matrix.

[0008] Based on the above technical solutions, preferably, the contact probability factor is determined as follows: the normal contact force measured by the six-axis force sensor at the foot end is obtained, the normal contact force is subtracted from the preset mechanical dead zone threshold and multiplied by the preset scaling factor, the negative of the product is taken as the exponent of the natural constant e, the reciprocal of the exponential function is calculated and 1 is added, and the reciprocal of the sum is taken as the contact probability factor at the foot end of the current mechanical leg.

[0009] Based on the above technical solutions, preferably, step S2 includes the following sub-steps: S21, acquire the real-time pose and velocity of the submersible fed back by the state observer, the desired pose and acceleration commands issued by the navigation behavior layer, the contact state measured by the foot force sensor, and the hardware physical limit parameters. S22 divides the task into three priority sets. The first priority is the set of absolutely rigid constraints, including the upper and lower limits of thrust output of the thruster and the minimum value constraint of the normal force at the end of the mechanical leg. The second priority is the main task set of operation tracking, which maps the deviation between the desired pose and acceleration tracking into equality constraints based on the global generalized Jacobian matrix. The third priority is the set of redundant null space optimization, where the optimization objective is the sum of the quadratic energy consumption cost function of the generalized control input vector and the dynamic seal pressure difference control cost. The energy consumption cost is used to optimize the thrust distribution efficiency, and the dynamic seal pressure difference control cost is used to actively adjust the fuselage pitch angle to reduce the seal pressure difference. S23, reduce the dimension of the nonlinear friction cone constraint to a regular polygonal pyramid, decouple the regular polygonal pyramid into multiple sets of linear absolute value inequalities, and establish the linearized friction cone constraint; S24, the first priority thrust extreme value constraint, foot end normal force constraint and linearized friction cone constraint are used as inequality safety constraints, the second priority pose tracking equation is used as the equation task objective, and the third priority quadratic energy consumption cost function is used as the optimization objective. The corresponding feature parameter matrix groups for each level are generated respectively. The feature parameter matrix groups include the equation task objective feature matrix and the target expectation vector, the inequality safety constraint feature matrix and the safety boundary vector.

[0010] Based on the above technical solutions, preferably, step S3 includes the following sub-steps: S31, set the zero vector as the initial reference solution, the identity matrix as the initial null space basis matrix, and obtain the feature parameter matrix set for each level; S32, For the first priority task, perform a quadratic programming solution in the initial null space basis matrix space. The objective function of the quadratic programming is the sum of the first-level equation task objective feature matrix multiplied by the current cumulative optimal solution and the null space basis matrix multiplied by the decision variables, minus half of the norm square of the objective expectation vector, plus half of the regularization damping term of the decision variables. The constraint condition of the quadratic programming is that the sum of the first-level inequality safety constraint feature matrix multiplied by the current cumulative optimal solution and the null space basis matrix multiplied by the decision variables is less than or equal to the safety boundary vector. Solving for the first-level optimal control step size, the first-level optimal control step size is projected onto the initial null space basis matrix and accumulated to the initial reference solution to obtain the first-level cumulative optimal solution. S33. Based on the product of the first-level equation task feature matrix and the initial null space basis matrix, calculate the generalized pseudo-inverse of the product. Multiply the initial null space basis matrix by the identity matrix, subtract the generalized pseudo-inverse, and then multiply by the difference of the product to obtain the second-level usable null space basis matrix. S34, For the second priority task, take the first-level cumulative optimal solution as the current cumulative optimal solution and the second-level available null space basis matrix as the current null space basis matrix, perform the second-level quadratic programming solution to obtain the second-level optimal control step size, project the second-level optimal control step size onto the second-level available null space basis matrix and accumulate it to the first-level cumulative optimal solution to obtain the second-level cumulative optimal solution, and update the null space basis matrix according to the method in step S33 based on the product of the second-level equation task feature matrix and the second-level available null space basis matrix to obtain the third-level available null space basis matrix; S35. For the third priority task, take the second-level cumulative optimal solution as the current cumulative optimal solution and the third-level available null space basis matrix as the current null space basis matrix, and perform the third-level quadratic programming solution to obtain the third-level optimal control step size. Project the third-level optimal control step size onto the third-level available null space basis matrix and accumulate it to the second-level cumulative optimal solution to obtain the optimal solution of the system's generalized control input.

[0011] Based on the above technical solutions, preferably, step S4 includes the following sub-steps: S41. Obtain the absolute hydrostatic pressure in the deep sea, the density of the deep sea water, the current pitch angle of the fuselage, the current velocity vector amplitude of the submersible, and the real-time pressure inside the cabin volume compensator. Establish a local fluid external dynamic pressure model modulated by the pitch angle of the fuselage. Calculate the real-time sum of the hydrostatic and dynamic pressures in the local dynamic sealing area outside the fuselage by adding half of the density of the deep sea water to the absolute hydrostatic pressure in the deep sea, multiplying it by the fluid dynamic pressure distribution coefficient, and then multiplying it by the square of the current absolute velocity vector amplitude of the submersible. S42, calculate the square of the difference between the real-time sum of dynamic and static fluid pressure and the real-time pressure inside the cabin and the optimal safe working pressure difference benchmark value, obtain the partial derivative of the square with respect to the pitch angle, and use the partial derivative multiplied by the negative of the preset proportional feedback gain as the desired pitch fine-tuning differential torque. S43, obtain the sub-block matrix of the thruster in the null-space basis matrix after the second-level task update, obtain the row vector of the fuselage pitch degree of freedom in the static configuration mapping matrix of the thruster, multiply the row vector by the generalized pseudo-inverse of the sub-block matrix and then by the desired pitch fine-tuning differential torque to obtain the null-space control compensation increment of the thruster system. S44 superimposes the zero-space control compensation increment onto the original control command of the thruster system, enabling the thruster to generate differential thrust while maintaining the original trajectory tracking, so as to adjust the fuselage pitch angle.

[0012] Secondly, the present invention also provides a multi-task control system for a benthic submersible crawling, implemented using a multi-task control method for a benthic submersible crawling, comprising: The global mapping module is used to introduce the additional mass matrix and nonlinear hydrodynamic damping spindle, establish the underwater dynamic equation of the submersible, construct the global generalized Jacobian mapping matrix, and map the thrust output of the thruster and the contact reaction force at the end of the mechanical leg into the total dynamic control spindle of the underwater dynamic equation. The multi-task hierarchical module is used to acquire the real-time status of the submersible fed back by the state observer, the expected pose and acceleration commands issued by the navigation behavior layer, and the hardware physical limit parameters. According to the control priority, the tasks are divided into an absolute rigid constraint set, a main task set for operation tracking, and a redundant null space optimization set. The nonlinear friction cone constraint is linearized into a set of linear inequalities, and the characteristic parameter matrix set corresponding to each level is generated. The planning and solving module is used to perform secondary planning and solving step by step according to the feature parameter matrix group of each level, in order of priority from high to low, using the null space projection method. The solution of each level is performed in the null space of the previous level. The solution results are projected and accumulated to the cumulative optimal solution. After each level is completed, the null space basis matrix is ​​updated to obtain the optimal solution of the generalized control input of the system. The space control module is used to construct a fluid external dynamic pressure model modulated by the fuselage attitude, generate the desired fine-tuning torque, map the desired fine-tuning torque to the thruster subspace in the null space left by the previous mission, and generate the null space control compensation increment of the thruster system to adjust the fuselage attitude.

[0013] Thirdly, the present invention also provides a terminal device, the terminal device comprising: a memory, a processor, and a benthic submersible crawling multi-task control method program stored in the memory and executable on the processor, the benthic submersible crawling multi-task control method program being configured to implement the steps of the benthic submersible crawling multi-task control method.

[0014] Fourthly, the present invention also provides a computer-readable storage medium storing a program for a multi-task control method for a benthic submersible crawling, wherein the program, when executed, implements the multi-task control method for a benthic submersible crawling.

[0015] The multi-task control method and system for bottom-diving submersibles of the present invention have the following advantages over the prior art: (1) By constructing a global generalized Jacobi mapping, the two heterogeneous actuators, the thruster and the mechanical leg, are unified into the total spin of the center of mass dynamics control. The hierarchical quadratic programming is used to establish three strict non-interference priority sets: anti-slip hard constraints, pose tracking and energy consumption optimization. The zero-space projection is used to solve the problem step by step, so that the three conflicting tasks of anti-slip constraints, pose tracking and energy consumption optimization are completed collaboratively under the strict priority framework. This eliminates the mutual compromise and interference between tasks, avoids the risk of triggering the instability of the anti-slip boundary under extreme sea conditions, and improves the stability and safety of the bottom submersible's swimming and crawling integrated operation. (2) By establishing a generalized control input vector that includes the thrust output of the thruster and the contact reaction force of the mechanical leg foot end, and combining the static configuration mapping matrix of the thruster and the smooth contact state mapping matrix of the mechanical leg to form a global generalized Jacobian mapping matrix, the two heterogeneous actuators, the thruster and the mechanical leg, are uniformly expressed in the centroid dynamic space, which effectively eliminates the cooperative control barrier caused by the difference in physical characteristics between the two types of actuators. At the same time, the underwater additional mass matrix and the nonlinear hydrodynamic damping spindle are introduced to correct the deep-sea fluid effect, so that the control model can accurately match the deep-sea hydrodynamic environment. The Sigmoid contact probability function is used to realize the smooth evolution of the Jacobian matrix when the swimming and crawling modes are switched, which effectively avoids the impact and oscillation of the control system caused by hard mode switching, so that the whole body control algorithm always has good convergence characteristics and control stability in the extreme fluid environment of the deep sea. (3) By dividing the tasks into three strictly non-interfering priority sets according to physical hard constraints, job tracking and redundancy optimization, and combining the null space projection technology, the results of high priority tasks are absolutely frozen and low priority tasks are optimized in the residual null space. The null space cascade step-by-step solution and generalized pseudo-inverse projection basis update mechanism are adopted to completely avoid the task compromise interference caused by empirical weight parameter tuning under extreme working conditions. At the same time, the second-order cone nonlinear friction constraint is reduced to a linear absolute value inequality system to adapt to high-frequency real-time solution. When there is no feasible solution in a certain layer, the target of that layer is directly discarded and the results of the previous layer are maintained. Flexible dimensionality reduction and smooth degradation are achieved, ensuring the basic safety control capability and high-frequency real-time solution requirements under extreme working conditions. Attached Figure Description

[0016] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0017] Figure 1 This is a flowchart of the multi-task control method for the bottom-diving submersible crawling in this invention; Figure 2 This is a schematic diagram of the collaborative control architecture of the multi-task control method for bottom-diving submersibles in this invention. Detailed Implementation

[0018] The technical solutions of the present invention will be clearly and completely described below with reference to the embodiments of the present invention. Obviously, the described embodiments are only a part of the embodiments of the present invention, and not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those of ordinary skill in the art without creative effort are within the scope of protection of the present invention.

[0019] like Figure 1 and Figure 2 As shown, in a first aspect, a multi-task control method for a bottom-diving submersible's crawling movement according to the present invention includes the following steps: S1. By introducing an additional mass matrix and a nonlinear hydrodynamic damping spindle, the underwater dynamic equation of the submersible is established. A global generalized Jacobian mapping matrix is ​​constructed to map the thrust output of the thruster and the contact reaction force at the end of the mechanical leg into the total dynamic control spindle of the underwater dynamic equation. In the integrated swimming and crawling operation of deep-sea submersibles, the thruster and the robotic legs belong to completely heterogeneous execution spaces. The thruster generates generalized forces and torques at the center of mass of the submersible, while the contact forces at the foot end of the robotic legs generate reaction forces and torques acting on the fuselage. The two have different physical dimensions, different points of application, and different mathematical models. If these two types of actuators are handled separately, the thrust distribution and leg force control will be isolated from each other in the control architecture, and they cannot be optimized in coordination when there are task conflicts. This step constructs a global generalized Jacobian mapping matrix to uniformly map the outputs of the two types of heterogeneous actuators to the total spin of the center of mass dynamics control, so that the subsequent hierarchical quadratic programming can coordinate the collaborative control of the two types of actuators under a unified physical framework.

[0020] Step S1 includes the following sub-steps: S11, Establish a generalized control input vector, which includes the desired thrust output vector of the thruster and the three-dimensional contact reaction force vector at the end of the mechanical leg; This step establishes the system's unified generalized control input variable U: ; In the formula, U For the unified generalized control input variable vector of the system, u thr ∈R 8 Let the desired thrust output column vector of the 8 vector thrusters be the first... j The output of each thruster is denoted as u thr,j , f leg ∈R 3k for k The three-dimensional contact reaction force column vector at the foot of the mechanical leg, where k The number of mechanical legs currently in contact (0≤ k ≤4), No. i The vector of the contact reaction force at the foot end of the contact leg is denoted as f c,i =[ f x,i , f y,i , f z,i ] T , f x,i and f y,i Shear force in the horizontal direction. f z,i For the normal contact force, the dimension of the generalized control input vector varies with the number of contact legs. k Dynamic changes k When the value is 0, it corresponds to the pure floating mode, where only the thruster is active.k When =4 corresponds to the quadrupedal crawling mode (the thruster and the mechanical leg work together), this unified representation incorporates the output variables of the two types of heterogeneous actuators into the same vector space, establishing a mathematical foundation for subsequent global mapping.

[0021] S12, establish the underwater dynamic equation of the submersible. The underwater dynamic equation is the sum of the rigid body mass inertia matrix and the additional mass matrix multiplied by the acceleration vector, plus the Coriolis and centripetal force matrix multiplied by the velocity vector, plus the nonlinear hydrodynamic damping screw, plus the restoring force and restoring torque screw. The result is equal to the total dynamic control screw acting on the center of mass. This step establishes a complete dynamic model of the submersible in the underwater environment. Unlike traditional land robots, underwater submersibles are significantly affected by added mass effects and nonlinear hydrodynamic damping, and these two aspects must be accurately modeled.

[0022] The underwater dynamics equations of the submersible are expressed as follows: ; In the formula, M rb ∈R 6×6 M is the space rigid body mass-inertia matrix of the submersible, containing information about the submersible's own mass and moment of inertia. It is determined by the submersible's mass distribution and is a constant matrix. a ∈R 6×6 The additional mass inertia matrix is ​​unique to deep-water environments and is used to characterize the reverse fluid inertial drag generated when a submersible accelerates and pulls the surrounding water along with it. Its value is determined by the geometry and direction of motion of the submersible and can be obtained through computational fluid dynamics simulation or experimental determination. Let be the acceleration vector of the submersible's center of mass in the inertial frame, containing three linear acceleration components and three angular acceleration components. Let be the velocity vector of the submersible's center of mass in the inertial frame. The Coriolis and centripetal force matrix is ​​determined by the submersible's velocity. Modulation reflects the inertial coupling effect in a rotating coordinate system, D(v rel (Based on the relative velocity v of the ocean current) rel The nonlinear hydrodynamic damping spinor, comprising linear and quadratic damping terms, reflects the drag effect of deep-sea currents on the motion of submersibles. g ( η The restoring force and restoring moment spin, generated by the hydrodynamic properties of gravity and buoyancy, depend on the attitude angle of the submersible. η When the submersible deviates from its balanced attitude, a restoring torque is generated. τ com The total dynamic control force and total torque spinor acting on the center of mass of the submersible, including the resultant force and resultant torque in three directions, is the final convergence point of the output of all heterogeneous actuators.

[0023] The essential difference between the underwater dynamics equation and the land robot dynamics equation lies in two modifications: First, the introduction of the added mass matrix. Land robots do not need to consider this effect, while underwater submersibles have a significantly increased equivalent inertia due to the need to push against the surrounding water. Second, the nonlinear hydrodynamic damping spindle. Damping of land robots is usually negligible, while underwater submersibles are affected by ocean currents, and the damping force is proportional to the square of the velocity, becoming one of the dominant forces in high-velocity environments.

[0024] S13, construct a global generalized Jacobian mapping matrix, which includes a thruster static configuration mapping matrix and a mechanical leg smooth contact state mapping matrix; This step maps the outputs of two heterogeneous actuators, the thruster and the robotic leg, to the total spin of the center-of-mass dynamics control, using the global generalized Jacobian mapping matrix J. g It connects to the generalized control input. U The expression for the bridge with the centroid controlling the spinor is: ; In the formula, B thr This is the static configuration mapping matrix of the thruster. This is the smooth contact state mapping matrix for the mechanical leg.

[0025] Step S13, which involves constructing the global generalized Jacobian mapping matrix, includes the following steps: S131, obtain the thrust direction spatial unit vector and installation position vector of each thruster, use the thrust direction spatial unit vector of each thruster to form the upper part of the thrust mapping, use the cross product of the three-dimensional position vector of each thruster installation position and the thrust direction spatial unit vector to form the lower part of the force matrix mapping, and combine them to obtain the thruster static configuration mapping matrix. The expression for the thruster static configuration mapping matrix is: ; In the formula, n t,j For the first j The thrust direction of a thruster is a unit vector in space, describing the direction of the thrust, and is determined by the fixed installation angle of the thruster. r t,j To point from the submersible's center of mass to the first j The three-dimensional position vector of each thruster installation location describes the lever arm of the thruster relative to the center of mass. r t,j × n t,j The cross product of the position vector and the thrust direction vector represents the first... jThe thrust generated by each thruster produces a torque on the center of mass.

[0026] S132, obtain the number of mechanical legs currently in contact and the position vector of each mechanical leg foot end, multiply the contact force vector of each mechanical leg foot end by the corresponding contact probability factor to form the upper part of the thrust mapping, multiply the cross product of the position vector of each mechanical leg foot end and the contact force vector by the corresponding contact probability factor to form the lower part of the torque mapping, and combine to obtain the smooth contact state mapping matrix of the mechanical leg. The expression for the smooth contact state mapping matrix of the robotic leg is: ; In the formula, α i ( t ) is ∈[0,1] and is the first i The contact probability factor at the foot of the mechanical leg is determined by the value measured by a six-axis force sensor at the foot. Sigmoid The function is calculated in real time, approaching 0 when the foot is not in contact and approaching 1 when the foot is reliably in contact; I 3×3 As a three-dimensional identity matrix, the force vector at the foot contact point is mapped to the force acting on the center of mass. p c,i For the first i The three-dimensional position vector of the foot end of the robotic leg within the mechanical system is obtained in real time from the joint angles through the forward kinematics of the robotic leg; (p c,i ×) is the antisymmetric matrix form of the foot position vector, used to calculate the torque generated by the foot contact force about the center of mass. p c,i × f c,i This represents the torque contribution of the foot contact force to the center of mass.

[0027] S133, the static configuration mapping matrix of the thruster and the smooth contact state mapping matrix of the mechanical leg are spliced ​​together in the horizontal direction to obtain the global generalized Jacobian mapping matrix.

[0028] The contact probability factor is determined as follows: the normal contact force measured by the six-axis force sensor at the foot is obtained, the normal contact force is subtracted from the preset mechanical dead zone threshold and then multiplied by the preset scaling factor. The negative of the product is taken as the exponent of the natural constant e. The reciprocal of the exponential function is calculated and 1 is added. The reciprocal of the sum is then taken as the contact probability factor at the foot of the current mechanical leg.

[0029] The contact probability factor is determined as follows: ; In the formula, α i ( t ) is the first i The contact probability factor at the foot end of the mechanical leg. F z,i(t) represents the normal contact force measured by the six-axis force sensor at the foot. A positive value indicates a compressive force on the foot, and a negative value indicates a tensile force. F th To determine the mechanical dead zone threshold for contact, when the normal force is less than this threshold, it is considered noise or false contact. k c The scaling factor determines the smoothing slope of the switching; a larger value results in a faster switching, while a smaller value results in a smoother switching.

[0030] The mathematical properties of the sigmoid function ensure continuous change of the contact probability factor, avoiding abrupt control changes caused by hard threshold switching. As the foot gradually contacts the seabed, the normal contact force... F z,i (t) gradually increases from zero, α i ( t As the robot smoothly ascends from 0 to 1, its contribution gradually takes over; when the foot leaves the ground, α i ( t As the mechanical leg smoothly decreases from 1 to 0, its contribution gradually disappears. This smoothing law design ensures that the number of columns in the global generalized Jacobian mapping matrix does not change abruptly during the switching of crawling modes, thus avoiding control oscillations.

[0031] S14, multiply the generalized control input vector by the global generalized Jacobian mapping matrix to obtain the total dynamic control spin, which maps the thrust output of the thruster and the contact reaction force at the end of the mechanical leg into the total dynamic control spin towards the center of mass. The expression is: .

[0032] Step S1 establishes a generalized control input vector that includes the expected thrust output of the thruster and the contact reaction force at the end of the mechanical leg, incorporating the outputs of the two types of heterogeneous actuators into a unified variable space. By introducing an additional mass matrix to correct the deep-sea additional mass effect and introducing a nonlinear hydrodynamic damping spinor to correct the ocean current resistance, the underwater dynamic equations accurately reflect the physical effects of the deep-sea environment on the submersible's motion. By combining the thrust direction of the thruster with the installation position vector to construct the static configuration mapping matrix of the thruster, and combining the contact force at the end of the mechanical leg with the position vector and multiplying it by the Sigmoid contact probability factor to construct a smooth contact state mapping matrix, the two matrices are horizontally spliced ​​to obtain the global generalized Jacobian mapping matrix. Finally, the generalized control input vector is mapped to the total spinor of the center-of-mass dynamic control. The Sigmoid contact probability factor makes the dimension of the Jacobian matrix evolve smoothly during the switching between swimming and crawling modes, effectively avoiding control oscillations caused by hard switching.

[0033] S2, obtain the real-time state of the submersible fed back by the state observer, the expected pose and acceleration commands issued by the navigation behavior layer, and the hardware physical limit parameters. According to the control priority, the task is divided into an absolutely rigid constraint set, a main task set for operation tracking, and a redundant zero space optimization set. The nonlinear friction cone constraint is linearized into a set of linear inequalities, and the characteristic parameter matrix set corresponding to each level is generated. Bottom-diving vehicles face multiple conflicting constraints in swimming and crawling integrated operations: they must strictly lock the fuselage attitude (tracking task), ensure that the feet do not slip (anti-slip constraint), and minimize thruster energy consumption (optimization task). Traditional weighted single-layer QP combines the three types of tasks into the same cost function, relying on empirical weights to coordinate priorities. Under extreme sea conditions, this is prone to triggering the anti-slip boundary due to mutual compromise and interference between tasks. This step establishes a strict physical priority dictionary to stratify tasks according to their importance, ensuring that high-priority tasks are executed with absolute priority.

[0034] Step S2 includes the following sub-steps: S21, acquire the real-time pose and velocity of the submersible fed back by the state observer, the desired pose and acceleration commands issued by the navigation behavior layer, the contact state measured by the foot force sensor, and the hardware physical limit parameters. It should be noted that the real-time status of the submersible is the attitude vector and velocity vector of the submersible's center of mass in the inertial frame, which are fed back in real time by the state observer; the expected commands are the expected fuselage attitude commands and acceleration commands issued by the navigation behavior layer, which represent the target attitude and acceleration that the submersible should achieve in the current control cycle; the contact status is the normal contact force measured in real time by the six-axis force sensors at the foot of each mechanical leg, which is used to calculate the contact probability factor and determine the foot contact status. The hardware physical limit parameters include the upper and lower limits of the thrust output of each thruster, as well as the Coulomb friction coefficient of the seabed geology.

[0035] The above data constitutes all the input information for the subsequent task layering and constraint generation. Among them, the pose tracking deviation is calculated based on the difference between the expected pose issued by the navigation behavior layer and the pose vector of the submersible's center of mass in the inertial frame, and the velocity tracking deviation is calculated based on the difference between the acceleration command and the velocity vector.

[0036] S22 divides the task into three priority sets. The first priority is the set of absolutely rigid constraints, including the upper and lower limits of thrust output of the thruster and the minimum value constraint of the normal force at the end of the mechanical leg. The second priority is the main task set of operation tracking, which maps the deviation between the desired pose and acceleration tracking into equality constraints based on the global generalized Jacobian matrix. The third priority is the set of redundant null space optimization, where the optimization objective is the sum of the quadratic energy consumption cost function of the generalized control input vector and the dynamic seal pressure difference control cost. The energy consumption cost is used to optimize the thrust distribution efficiency, and the dynamic seal pressure difference control cost is used to actively adjust the fuselage pitch angle to reduce the seal pressure difference. This step constructs three strictly non-interfering priority mapping sets based on control importance. The definitions of each priority set are as follows: Rank 1 set of absolutely rigid constraints: Handles benthic stability and hardware security boundaries; these are the physical hard constraints that the system must absolutely satisfy. Includes: Thrust extreme value constraints: u min ≤ u thr ≤ u max To ensure that the thrust command of each thruster does not exceed its physical output capacity, and to avoid thruster saturation or damage; Anti-slip ground clearance constraints: f z,i ≥ f min Ensure that the normal contact force of each contact leg is not less than the preset minimum value to prevent the foot from lifting off the ground or slipping, which could cause the machine to become unstable. f min Typical values ​​range from 20 to 50 N, depending on the seabed conditions and the weight of the fuselage.

[0037] Rank 2 task tracking master set: Handling fuselage kinematics servo control is the core control objective for the submersible to complete its operational tasks. Combining hydrodynamic feedforward, the pose tracking deviation is mapped to a global generalized Jacobian J... g Standard algebraic equation: ; In the formula, K p , K d These are the proportional and differential gain matrices, used to adjust the stiffness and damping characteristics of pose tracking.

[0038] Rank 3 Redundancy Zero-Space Optimization Set: Utilizing the system's high redundancy, a quadratic cost function is constructed in the zero space to optimize energy consumption and hardware lifetime without interfering with the core tasks of Rank 1 and Rank 2. ; In the formula, U For the generalized control input matrix, W energy Let λ be the system's global energy consumption weight diagonal matrix. p For the dynamic seal pressure differential control penalty weight, P seal ( U P represents the real-time dynamic pressure build-up value generated inside the joint cavity due to thermal expansion under the current drive input. refThe objective function is designed to determine the optimal internal and external pressure difference required to resist the hydrostatic pressure of the deep-sea environment. It comprises two terms: the first is the quadratic energy consumption cost of the generalized control input vector, used to optimize thrust distribution efficiency, enabling high-energy-consuming thrusters to automatically reduce their output, with redundant thrust being handled by high-efficiency thrusters; the second is the dynamic seal pressure difference control cost, used to actively adjust the fuselage pitch angle to reduce the seal pressure difference. This constraint ensures fuselage stability while automatically reducing the output of high-energy-consuming thrusters and adjusting joint heat generation to actively maintain the isobaric balance of the dynamic seals.

[0039] S23, reduce the dimension of the nonlinear friction cone constraint to a regular polygonal pyramid, decouple the regular polygonal pyramid into multiple sets of linear absolute value inequalities, and establish the linearized friction cone constraint; This step linearizes the extremely time-consuming second-order conical nonlinear friction constraint to adapt it to the high-frequency QP real-time solution requirements of the underlying microprocessor.

[0040] The frictional constraint when the foot contacts the seabed is mathematically expressed as a second-order cone constraint: ; In the formula, f x , f y This represents the horizontal shear force component of the contact force at the foot. f z For normal contact force, μ The coefficient of friction is Coulomb. This constraint is a second-order cone programming constraint, and its direct solution involves a large amount of computation, which is difficult to meet the requirements of high-frequency real-time control.

[0041] To adapt to high-frequency QP solvers, this invention reduces the dimension of the inscribed friction cone to a regular m-sided pyramid (typically m=4), decoupling it into a system of linear absolute value inequalities. When m=4, the linearization constraint of the inscribed regular quadrilateral pyramid is: ; This linearization process transforms the nonlinear second-order cone constraint into two sets of linear absolute value inequalities, which can be directly incorporated into the linear constraint matrix of QP. The inscribed regular quadrilateral pyramid is a conservative approximation of the original friction cone. All solutions that satisfy the linearization constraints must satisfy the original friction cone constraints, but the converse is not true. Although this conservative approximation sacrifices some of the available friction force space, it gains the feasibility of high-frequency real-time solution, which is an acceptable trade-off in engineering practice.

[0042] S24, the first priority thrust extreme value constraint, foot end normal force constraint and linearized friction cone constraint are used as inequality safety constraints, the second priority pose tracking equation is used as the equation task objective, and the third priority quadratic energy consumption cost function is used as the optimization objective. The corresponding feature parameter matrix groups for each level are generated respectively. The feature parameter matrix groups include the equation task objective feature matrix and the target expectation vector, the inequality safety constraint feature matrix and the safety boundary vector.

[0043] This step encapsulates the hierarchical tasks and linearization constraints defined in S22 and S23 into a standardized set of characteristic parameter matrices for use by the null space HQP cascade solver in step S3.

[0044] The feature parameter matrix group {A} corresponding to each level p ,b p C p ,d p The hierarchical indexes p=1,2,3 are defined as follows: First priority: Inequality safety constraint feature matrix C1 and safety boundary vector d1, including all linear inequalities of thrust extreme value constraint, foot end normal force minimum value constraint and linearized friction cone constraint; Second priority: the task objective feature matrix (i.e., the global generalized Jacobian matrix) and the objective expectation vector (i.e., the right-hand side of the Rank2 equality constraint), and the corresponding regularization weight matrix; Third priority: The optimization objective is the quadratic energy consumption cost function of the generalized control input vector and the dynamic sealing pressure difference regulation cost.

[0045] Step S2 establishes complete input data by acquiring feedback from the state observer and instructions from the navigation behavior layer. Tasks are hierarchically layered according to three strict priorities: physical hard constraints, job tracking, and redundancy optimization, ensuring that the execution of high-priority tasks is not interfered with by low-priority tasks. By reducing the dimension of the nonlinear friction cone constraint to a regular quadrilateral pyramid and decoupling it into a linear absolute value inequality, the computationally complex second-order cone constraint is transformed into a linear constraint acceptable to QP, significantly reducing the computational load. Finally, the corresponding equality task target feature matrix and target expectation vector, inequality safety constraint feature matrix and safety boundary vector are generated for each level, providing structured input data for the null space HQP cascade solution in step S3, enabling subsequent solutions to strictly guarantee the absolute priority of physical hard constraints without relying on empirical weights.

[0046] S3. Based on the feature parameter matrix group of each level, the quadratic programming solution is performed step by step in the order of priority from high to low using the null space projection method. The solution of each level is performed in the null space of the previous level. The solution results are projected and accumulated to the cumulative optimal solution. After each level is completed, the null space basis matrix is ​​updated to obtain the optimal solution of the system's generalized control input. Step S3 includes the following sub-steps: S31, set the zero vector as the initial reference solution, the identity matrix as the initial null space basis matrix, and obtain the feature parameter matrix set for each level; This step initializes the cascade solver. The initial reference solution U0=0, Z0=I, is set as the zero vector, indicating that no task has been executed yet. The initial null basis matrix Z0 is set as the identity matrix I, indicating that the entire control space is fully available for the first-level task.

[0047] It should be noted that, for the first p The ultimate mission, in the null space Z left over from its superior mission. p-1 Within, with the optimal step size v p Construct a constrained quadratic programming problem for the decision variables: ; In the formula, v p For the first p The optimal control step size, A, is solved within the currently available null space for the level task. p ,b p For the first p The task objective feature matrix and objective expectation vector are expressed as an equation; C p ,d p For the first p Level inequality safety constraint characteristic matrix and safety boundary vector; U p-1 Z is the cumulative optimal solution that has been obtained and is absolutely fixed at the previous level. p-1 The null basis matrix output by the previous level task serves to ensure that the optimization action at this level is strictly projected onto a dimension that does not affect the previous level at all. W p A positive definite regularized damping weight matrix is ​​set to prevent singular divergence of the solution.

[0048] If the process successfully solves for the optimal step size of the current level, then the global optimal solution of the system is cumulatively updated, expressed as: ; In the formula, U p For the updated number pIf the optimal solution of the generalized control input in the multi-stage cumulative system is found to be 'Infeasible' (no feasible solution conflict) at the p-th stage, then the optimization objective of that stage is discarded directly, and U is maintained. p =U p-1 This enables flexible dimensionality reduction and smooth degradation. After calculating the control variables for this level, the motion space already occupied by this level must be stripped away, and a completely new zero-space projection basis Z available for the next level must be calculated. p The expression is: ; In the formula, Z p To provide the null projection basis for the next level of execution, (.) + For the Moore-Penrose generalized pseudoinverse calculated based on singular value decomposition, I−(A p Z p−1 ) + (A p Z p−1 () is the null space orthogonal projection operator, whose physical essence is to include all operators that would cause the current A to be orthogonal. p The motion degree-of-freedom vectors that cause deviations in the task are completely filtered out from the feasible region of the system.

[0049] Null space projection operator Z p−1 The column vectors span a subspace, and any vector Z in this subspace... p−1 v p In A p−1 The result under the mapping is zero, meaning it has no impact on the completion status of the previous level task. Therefore, when the first level task... p When a task of level 1 seeks optimization within its null space, regardless of v p Regardless of the value chosen, the execution results of the preceding tasks remain unchanged. This ensures that the results of high-priority tasks are strictly frozen and unaffected by any low-priority optimizations.

[0050] S32, For the first priority task, perform a quadratic programming solution in the initial null space basis matrix space. The objective function of the quadratic programming is the sum of the first-level equation task objective feature matrix multiplied by the current cumulative optimal solution and the null space basis matrix multiplied by the decision variables, minus half of the norm square of the objective expectation vector, plus half of the regularization damping term of the decision variables. The constraint condition of the quadratic programming is that the sum of the first-level inequality safety constraint feature matrix multiplied by the current cumulative optimal solution and the null space basis matrix multiplied by the decision variables is less than or equal to the safety boundary vector. Solving for the first-level optimal control step size, the first-level optimal control step size is projected onto the initial null space basis matrix and accumulated to the initial reference solution to obtain the first-level cumulative optimal solution. S33. Based on the product of the first-level equation task feature matrix and the initial null space basis matrix, calculate the generalized pseudo-inverse of the product. Multiply the initial null space basis matrix by the identity matrix, subtract the generalized pseudo-inverse, and then multiply by the difference of the product to obtain the second-level usable null space basis matrix. S34, For the second priority task, take the first-level cumulative optimal solution as the current cumulative optimal solution and the second-level available null space basis matrix as the current null space basis matrix, perform the second-level quadratic programming solution to obtain the second-level optimal control step size, project the second-level optimal control step size onto the second-level available null space basis matrix and accumulate it to the first-level cumulative optimal solution to obtain the second-level cumulative optimal solution, and update the null space basis matrix according to the method in step S33 based on the product of the second-level equation task feature matrix and the second-level available null space basis matrix to obtain the third-level available null space basis matrix; S35. For the third priority task, take the second-level cumulative optimal solution as the current cumulative optimal solution and the third-level available null space basis matrix as the current null space basis matrix, and perform the third-level quadratic programming solution to obtain the third-level optimal control step size. Project the third-level optimal control step size onto the third-level available null space basis matrix and accumulate it to the second-level cumulative optimal solution to obtain the optimal solution of the system's generalized control input.

[0051] Step S3 uses a stepwise QP solution through null space projection to ensure that the first priority rigid constraint is satisfied first and strictly frozen in the entire control space. The second priority pose tracking task is solved in the null space after removing the first-level occupied dimension. The third priority energy optimization task is optimized in the residual null space after removing the first two levels of occupied dimensions. Each level of task does not interfere with the others. When a level reports Infeasible, the target of that level is directly discarded while the results of the previous level are maintained, thus achieving flexible degradation. Finally, the optimal solution of the system generalized control input that takes into account anti-slip hard constraints, pose tracking and energy optimization and strictly satisfies the priority order is obtained, avoiding the instability of the anti-slip boundary caused by compromise interference between tasks in the traditional weighted method.

[0052] S4. Construct a fluid external dynamic pressure model modulated by the fuselage attitude, generate the desired fine-tuning torque, map the desired fine-tuning torque to the thruster subspace in the null space left by the previous mission, generate the null space control compensation increment of the thruster system to adjust the fuselage attitude.

[0053] This step, executed by the space control module, addresses the issue of pressure differential overload in localized dynamic sealing cavities during high-speed ascent and descent. When a deep-sea submersible descends or ascends at high speed, the hydrodynamic pressure on localized areas of the external dynamic seal changes with the pitch angle. If the pressure difference exceeds the seal's tolerance range, it can lead to seal failure or even seawater infiltration into the hull. This step generates differential thrust within zero space to adjust the fuselage pitch angle, actively altering the fluid angle of attack to regulate the dynamic pressure distribution, thereby reducing the peak pressure differential.

[0054] This step does not interfere with the execution of Rank1 and Rank2 tasks, and all actions are strictly constrained within the zero space remaining from the first two levels of tasks.

[0055] Step S4 includes the following sub-steps: S41. Obtain the absolute hydrostatic pressure in the deep sea, the density of the deep sea water, the current pitch angle of the fuselage, the current velocity vector amplitude of the submersible, and the real-time pressure inside the cabin volume compensator. Establish a local fluid external dynamic pressure model modulated by the pitch angle of the fuselage. Calculate the real-time sum of the hydrostatic and dynamic pressures in the local dynamic sealing area outside the fuselage by adding half of the density of the deep sea water to the absolute hydrostatic pressure in the deep sea, multiplying it by the fluid dynamic pressure distribution coefficient, and then multiplying it by the square of the current absolute velocity vector amplitude of the submersible. This step constructs a local fluid external dynamic pressure model modulated by the fuselage pitch angle θ. When the submersible is ascending and descending at high speed, the fluid pressure borne by the local area of ​​the external dynamic seal of the fuselage consists of two parts: hydrostatic pressure and dynamic pressure. The dynamic pressure part is closely related to the fuselage pitch angle. The pitch angle changes the angle of attack of the fluid, which in turn changes the flow distribution and affects the local dynamic pressure of the sealing area.

[0056] The calculation formula for the external pressure field model is: ; In the formula, P ext ( θ , v This represents the sum of real-time hydrodynamic and static pressures in a localized area of ​​the external dynamic seal of the machine body. P static The absolute hydrostatic pressure in the deep sea is obtained by a high-precision depth gauge, reflecting the environmental hydrostatic pressure at the current depth; ρ sea The density of deep-sea water is determined by a combination of seawater temperature, salinity, and depth. C p ( θ ( ) is the current pitch angle of the fuselage θ The modulated fluid dynamic pressure distribution coefficient reflects the dynamic pressure distribution characteristics of the sealed local area under different pitch angles. It is obtained through offline computational fluid dynamics simulation and is pre-set in the controller in the form of an interpolation table. v This represents the amplitude of the submersible's current absolute velocity vector, obtained by a Doppler velocimeter.

[0057] S42, calculate the square of the difference between the real-time sum of dynamic and static fluid pressure and the real-time pressure inside the cabin and the optimal safe working pressure difference benchmark value, obtain the partial derivative of the square with respect to the pitch angle, and use the partial derivative multiplied by the negative of the preset proportional feedback gain as the desired pitch fine-tuning differential torque. This step is based on the external pressure field model to solve the gradient of the pressure difference deviation with respect to the pitch angle, and then generates the desired fine-tuning pitch torque. Its physical logic is: when the external pressure deviates from the target value, the system generates a torque command that causes the pitch angle to change in the direction of reducing the deviation.

[0058] The differential pressure deviation is defined as: ; In the formula, P int For the real-time pressure inside the cabin volume compensator, Δ P target The optimal safe operating pressure difference reference value for the dynamic seal Glyd ring is determined by the seal specifications and deep-sea operation experience.

[0059] The goal of differential pressure control is to minimize the square of the differential pressure deviation, with the desired pitch fine-tuning differential torque Δ. τ pitch The following is obtained by taking the partial derivative of the square of the deviation with respect to the pitch angle: ; In the formula, k θ It is a positive proportional feedback gain for mapping pressure gradient to torque, used to convert the deviation gradient into torque command amplitude. When the external pressure is high, the gradient is positive, generating negative torque to reduce the fuselage pitch angle, thereby changing the dynamic pressure distribution and reducing the external pressure; when the external pressure is low, the gradient is negative, generating positive torque to increase the fuselage pitch angle and increase the external pressure.

[0060] S43, obtain the sub-block matrix of the thruster in the null-space basis matrix after the second-level task update, obtain the row vector of the fuselage pitch degree of freedom in the static configuration mapping matrix of the thruster, multiply the row vector by the generalized pseudo-inverse of the sub-block matrix and then by the desired pitch fine-tuning differential torque to obtain the null-space control compensation increment of the thruster system. This step maps the desired pitch fine-tuning torque into the null space of the thruster system. Since this action is a Rank 3 task with the lowest priority, it must be strictly projected into the null space remaining after the first two tasks have been fully processed to ensure that the differential action of the thruster will not interfere with the heading and position tracking of the fuselage.

[0061] The formula for calculating the thrust zero-space compensation increment is: ; In the formula, This is the additional speed compensation command vector output by the thruster system in null space. This increment will absolutely not cause any interference to the main heading of the fuselage. This refers to the sub-block matrix of the corresponding thruster extracted from the null-space basis matrix Z2 after the second-level update calculated in step S3. For the thruster global Jacobian matrix B thr The single row vector in the middle corresponds specifically to the pitch degree of freedom of the fuselage.

[0062] S44 superimposes the zero-space control compensation increment onto the original control command of the thruster system, enabling the thruster to generate differential thrust while maintaining the original trajectory tracking, thereby adjusting the fuselage pitch angle. The expression is: ; In the formula, To finally issue complete thrust commands to all eight thrusters, This refers to the original thrust command generated by the Rank2 pose tracking task in step S3.

[0063] Through this superposition, the eight thrusters, while maintaining the original X / Y / Z position tracking and heading control, will utilize their redundancy to generate an additional set of purely internally canceling differential thrust. Some thrusters increase thrust, while others decrease thrust, causing the resultant force and resultant moment to cancel each other out in the horizontal and heading directions, producing only a net pitching moment. This net pitching moment forces the fuselage to produce a small pitch angle Δ. θ By utilizing the angle of attack of the fluid to change the dynamic pressure distribution coefficient C p ( θ This actively reduces the peak pressure difference at the dynamic sealing surface during high-speed descent. Since this compensation amount is strictly constrained within the null space of the first two mission stages, its generation does not affect the anti-slip constraint of Rank 1 or the pose tracking accuracy of Rank 2.

[0064] In this embodiment, a local fluid external dynamic pressure model modulated by the fuselage pitch angle is constructed to sense the real-time external pressure in the dynamic sealing area. The deviation between the external pressure and the internal pressure and the optimal pressure difference benchmark is calculated. The desired pitch fine-tuning torque is generated by multiplying the gradient of the square of the deviation with respect to the pitch angle by the proportional feedback gain. The torque is mapped to the thrust sub-block matrix in the null space left over from the first two missions. The null space control compensation increment of the thrust system is obtained through generalized pseudo-inverse calculation and superimposed on the original thrust command. This allows the eight thrusters to generate additional pure internally canceled differential thrust while maintaining the original trajectory tracking benchmark. This forces the fuselage to generate a small pitch angle to change the fluid angle of attack and dynamic pressure distribution. During high-speed ascent and descent, the peak value of the dynamic sealing pressure difference is actively reduced, and the Rank1 anti-slip constraint and Rank2 attitude tracking accuracy are not interfered with at all times.

[0065] To verify the effectiveness and robustness of the control method of the present invention, experiments were conducted under the following six typical benthic operating conditions.

[0066] Example 1: Smooth Sampling from Hovering Drill Holes Under Strong Ocean Currents In a strong ocean current environment, the submersible needs to remain hovered to complete drilling and sampling operations. The fuselage is subjected to continuous ocean current disturbances. Rank 1 forcibly ensures that the mechanical legs do not slip and ensures stable contact at the foot tip; Rank 2 ensures 1mm level anti-disturbance locking of the fuselage to maintain drilling accuracy; Rank 3 automatically distributes excess thrust to counteract ocean current interference, and this thrust distribution does not interfere with the normal force of the mechanical legs, solving the problem of oscillation control by the leg thrust. Experimental results show that the submersible's attitude deviation is less than 1mm under the scouring of a 1.5 knot ocean current, and there is no slippage at the foot tip of the mechanical legs.

[0067] Example 2: Climbing on an irregular hydrothermal rock wall with an extremely steep slope During the crawling operation on an irregular hydrothermal rock wall with an ultra-sharp 35° slope, the submersible needs to maintain stable movement by adhering to the inclined surface. The linearized friction cone constraint active drive HQP in Rank1 distributes a normal thrust of up to 500N to press against the rock wall, enabling the submersible to exhibit a gecko-like adhesion and crawling effect, ensuring sufficient normal contact force at the feet on the inclined rock wall to maintain friction margin. Experimental results show that the submersible crawls stably on the 35° slope without slippage.

[0068] Example 3: Seamless switching and shock-free transition between crawling and swimming modes When a submersible transitions from swimming mode to seabed crawling mode, it must undergo an impact process at the moment of bottom contact. At the moment of bottom contact, the contact probability factor α... i ( t The global generalized Jacobian matrix J smoothly rises from 0 to 1 within 2 seconds. g The thruster speed decreases smoothly and exponentially as it expands, and the foot contact force gradually takes over the fuselage support. This transition process is shock-free and vibration-free, achieving a seamless and shock-free switch between swim-climb modes and completely avoiding a severe crash. Example 4: Adaptive Dimensionality Reduction of Soft Mud Substrate When operating on soft mud bottoms, the feet sink into the mud and cannot provide effective shear force. The system automatically reduces the Rank1 friction coefficient threshold to μ=0.05. The HQP solver transfers the lateral position locking task to the thrust vector array. The thruster undertakes the horizontal positioning, while the mechanical legs only provide vertical support. Experimental results show that the submersible can still maintain position locking accuracy under soft mud bottoms.

[0069] Example 5: Zero-space compensation for reaction torque induced by a robotic arm When the seven-axis robotic arm grabs a 15kg rock core, it generates a huge overturning moment instantly. This disturbance is injected into the HQP solver as a feedforward term. The HQP solves the corresponding thrust differential compensation spin in zero space instantly, which cancels the disturbance of the robotic arm's reaction torque on the body attitude. The experimental results show that the body attitude remains unchanged during the robotic arm's grabbing process, maintaining the original locking accuracy.

[0070] Example 6: Adaptive pseudo-inverse reconstruction of thrust array / joint multi-source failure In an extreme failure scenario where both thrusters are stuck and the left forelimb is de-energized, the global generalized Jacobian mapping matrix J... g The corresponding columns are automatically removed, the null basis matrix Z is recalculated, the tracking accuracy of Rank2 is compromised to preserve the rigid body stability of Rank1, and the submersible enters a defensive limp state for safe recovery. Experimental results show that the system can still maintain basic safety control capabilities and achieve safe recovery under multi-source failures of the thruster array and mechanical legs.

[0071] The above six embodiments cover the most challenging typical working conditions in the integrated swimming and crawling operation of benthic submersibles, such as strong current disturbance suppression, large slope crawling, mode switching, soft mud bottom adaptation, robotic arm reaction force compensation, and multi-source fault tolerance. They verify the effectiveness, robustness, and engineering applicability of the control method of the present invention in terms of global Jacobian unified mapping, HQP hierarchical priority management, zero-space projection stepwise solution, and dynamic sealing pressure difference zero-space control.

[0072] Secondly, the present invention also provides a multi-task control system for a benthic submersible crawling, implemented using a multi-task control method for a benthic submersible crawling, comprising: The global mapping module is used to introduce the additional mass matrix and nonlinear hydrodynamic damping spindle, establish the underwater dynamic equation of the submersible, construct the global generalized Jacobian mapping matrix, and map the thrust output of the thruster and the contact reaction force at the end of the mechanical leg into the total dynamic control spindle of the underwater dynamic equation. The multi-task hierarchical module is used to acquire the real-time status of the submersible fed back by the state observer, the expected pose and acceleration commands issued by the navigation behavior layer, and the hardware physical limit parameters. According to the control priority, the tasks are divided into an absolute rigid constraint set, a main task set for operation tracking, and a redundant null space optimization set. The nonlinear friction cone constraint is linearized into a set of linear inequalities, and the characteristic parameter matrix set corresponding to each level is generated. The planning and solving module is used to perform secondary planning and solving step by step according to the feature parameter matrix group of each level, in order of priority from high to low, using the null space projection method. The solution of each level is performed in the null space of the previous level. The solution results are projected and accumulated to the cumulative optimal solution. After each level is completed, the null space basis matrix is ​​updated to obtain the optimal solution of the generalized control input of the system. The space control module is used to construct a fluid external dynamic pressure model modulated by the fuselage attitude, generate the desired fine-tuning torque, map the desired fine-tuning torque to the thruster subspace in the null space left by the previous mission, and generate the null space control compensation increment of the thruster system to adjust the fuselage attitude.

[0073] It should be noted that this system corresponds to the aforementioned multi-task control method for bottom-diving vehicles. All implementation methods in the above method embodiments are applicable to the embodiments of this system and can achieve the same technical effect.

[0074] Those skilled in the art will recognize that the units and algorithm steps of the various examples described in conjunction with the embodiments disclosed herein can be implemented in electronic hardware, or a combination of computer software and electronic hardware. Whether these functions are implemented in hardware or software depends on the specific application and design constraints of the technical solution. Those skilled in the art can use different methods to implement the described functions for each specific application, but such implementations should not be considered beyond the scope of this invention.

[0075] Those skilled in the art will understand that, for the sake of convenience and brevity, the specific working process of the system and modules described above can be referred to the corresponding process in the foregoing method embodiments, and will not be repeated here.

[0076] In the embodiments provided by this invention, it should be understood that the disclosed systems and methods can be implemented in other ways. For example, the device embodiments described above are merely illustrative. For instance, the division of units is only a logical functional division, and in actual implementation, there may be other division methods. For example, multiple units or components may be combined or integrated into another system, or some features may be ignored or not executed. Furthermore, the coupling or direct coupling or communication connection shown or discussed may be through some interfaces; the indirect coupling or communication connection between devices or units may be electrical, mechanical, or other forms.

[0077] The units described as separate components may or may not be physically separate. The components shown as units may or may not be physical units; that is, they may be located in one place or distributed across multiple network units. Some or all of the units can be selected to achieve the purpose of this embodiment according to actual needs.

[0078] In addition, the functional units in the various embodiments of the present invention can be integrated into one processing unit, or each unit can exist physically separately, or two or more units can be integrated into one unit.

[0079] If the aforementioned functions are implemented as software functional units and sold or used as independent products, they can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of this invention, essentially, or the part that contributes to the prior art, or a portion of the technical solution, can be embodied in the form of a software product. This computer software product is stored in a storage medium and includes several instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) to execute all or part of the steps of the methods described in the various embodiments of this invention. The aforementioned storage medium includes various media capable of storing program code, such as USB flash drives, portable hard drives, ROM, RAM, magnetic disks, or optical disks.

[0080] Furthermore, it should be noted that in the system and method of the present invention, it is obvious that the components or steps can be decomposed and / or recombined. These decompositions and / or recombinations should be considered equivalent solutions of the present invention. Moreover, the steps performing the above series of processes can naturally be executed in the order described, but are not necessarily required to be executed in chronological order; some steps can be executed in parallel or independently of each other. Those skilled in the art will understand that all or any step or component of the method and apparatus of the present invention can be implemented in any computing device (including processors, storage media, etc.) or network of computing devices, in hardware, firmware, software, or a combination thereof. This is something that those skilled in the art can achieve by using their basic programming skills after reading the description of the present invention.

[0081] Therefore, the object of the present invention can also be achieved by running a program or a set of programs on any computing system. The computing system can be a known general-purpose system. Therefore, the object of the present invention can also be achieved simply by providing a program product containing program code implementing the method or apparatus. That is, such a program product also constitutes the present invention, and the storage medium storing such a program product also constitutes the present invention. Obviously, the storage medium can be any known storage medium or any storage medium developed in the future. It should also be noted that in the apparatus and method of the present invention, it is obvious that the components or steps can be decomposed and / or recombined. These decompositions and / or recombinations should be considered equivalent to the present invention. Furthermore, the steps performing the above series of processes can naturally be performed in the order described, but are not necessarily required to be performed in chronological order. Some steps can be performed in parallel or independently of each other.

[0082] 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 within the protection scope of the present invention.

Claims

1. A multi-task control method for a bottom-diving submersible, characterized in that, Includes the following steps: S1. By introducing an additional mass matrix and a nonlinear hydrodynamic damping spindle, the underwater dynamic equation of the submersible is established. A global generalized Jacobian mapping matrix is ​​constructed to map the thrust output of the thruster and the contact reaction force at the end of the mechanical leg into the total dynamic control spindle of the underwater dynamic equation. S2, obtain the real-time state of the submersible fed back by the state observer, the expected pose and acceleration commands issued by the navigation behavior layer, and the hardware physical limit parameters. According to the control priority, the task is divided into an absolutely rigid constraint set, a main task set for operation tracking, and a redundant zero space optimization set. The nonlinear friction cone constraint is linearized into a set of linear inequalities, and the characteristic parameter matrix set corresponding to each level is generated. S3. Based on the feature parameter matrix group of each level, the quadratic programming solution is performed step by step in the order of priority from high to low using the null space projection method. The solution of each level is performed in the null space of the previous level. The solution results are projected and accumulated to the cumulative optimal solution. After each level is completed, the null space basis matrix is ​​updated to obtain the optimal solution of the system's generalized control input. S4. Construct a fluid external dynamic pressure model modulated by the fuselage attitude, generate the desired fine-tuning torque, map the desired fine-tuning torque to the thruster subspace in the null space left by the previous mission, generate the null space control compensation increment of the thruster system to adjust the fuselage attitude.

2. The multi-task control method for bottom-diving submersible crawling as described in claim 1, characterized in that, Step S1 includes the following sub-steps: S11, Establish a generalized control input vector, which includes the desired thrust output vector of the thruster and the three-dimensional contact reaction force vector at the end of the mechanical leg; S12, establish the underwater dynamic equation of the submersible. The underwater dynamic equation is the sum of the rigid body mass inertia matrix and the additional mass matrix multiplied by the acceleration vector, plus the Coriolis and centripetal force matrix multiplied by the velocity vector, plus the nonlinear hydrodynamic damping screw, plus the restoring force and restoring torque screw. The result is equal to the total dynamic control screw acting on the center of mass. S13, construct a global generalized Jacobian mapping matrix, which includes a thruster static configuration mapping matrix and a mechanical leg smooth contact state mapping matrix; S14 multiplies the generalized control input vector with the global generalized Jacobian mapping matrix to obtain the total dynamic control spin, which maps the thrust output of the thruster and the contact reaction force at the end of the mechanical leg into the total dynamic control spin towards the center of mass.

3. The multi-task control method for bottom-diving submersible crawling as described in claim 2, characterized in that, Step S13, which involves constructing the global generalized Jacobian mapping matrix, includes the following steps: S131, obtain the thrust direction spatial unit vector and installation position vector of each thruster, use the thrust direction spatial unit vector of each thruster to form the upper part of the thrust mapping, use the cross product of the three-dimensional position vector of each thruster installation position and the thrust direction spatial unit vector to form the lower part of the force matrix mapping, and combine them to obtain the thruster static configuration mapping matrix. S132, obtain the number of mechanical legs currently in contact and the position vector of each mechanical leg foot end, multiply the contact force vector of each mechanical leg foot end by the corresponding contact probability factor to form the upper part of the thrust mapping, multiply the cross product of the position vector of each mechanical leg foot end and the contact force vector by the corresponding contact probability factor to form the lower part of the torque mapping, and combine to obtain the smooth contact state mapping matrix of the mechanical leg. S133, the static configuration mapping matrix of the thruster and the smooth contact state mapping matrix of the mechanical leg are spliced ​​together in the horizontal direction to obtain the global generalized Jacobian mapping matrix.

4. The multi-task control method for bottom-diving submersibles as described in claim 3, characterized in that, The contact probability factor is determined as follows: the normal contact force measured by the six-axis force sensor at the foot is obtained, the normal contact force is subtracted from the preset mechanical dead zone threshold and then multiplied by the preset scaling factor. The negative of the product is taken as the exponent of the natural constant e. The reciprocal of the exponential function is calculated and 1 is added. The reciprocal of the sum is then taken as the contact probability factor at the foot of the current mechanical leg.

5. The multi-task control method for bottom-diving submersibles as described in claim 4, characterized in that, Step S2 includes the following sub-steps: S21, acquire the real-time pose and velocity of the submersible fed back by the state observer, the desired pose and acceleration commands issued by the navigation behavior layer, the contact state measured by the foot force sensor, and the hardware physical limit parameters. S22 divides the task into three priority sets. The first priority is the set of absolutely rigid constraints, including the upper and lower limits of thrust output of the thruster and the minimum value constraint of the normal force at the end of the mechanical leg. The second priority is the main task set for operation tracking, which maps the desired pose and acceleration tracking deviation to an equality constraint based on the global generalized Jacobian matrix; the third priority is the redundant null space optimization set, whose optimization objective is the sum of the quadratic energy consumption cost function of the generalized control input vector and the dynamic seal pressure difference control cost. The energy consumption cost is used to optimize the thrust distribution efficiency, and the dynamic seal pressure difference control cost is used to actively adjust the fuselage pitch angle to reduce the seal pressure difference. S23, reduce the dimension of the nonlinear friction cone constraint to a regular polygonal pyramid, decouple the regular polygonal pyramid into multiple sets of linear absolute value inequalities, and establish the linearized friction cone constraint; S24, the first priority thrust extreme value constraint, foot end normal force constraint and linearized friction cone constraint are used as inequality safety constraints, the second priority pose tracking equation is used as the equation task objective, and the third priority quadratic energy consumption cost function is used as the optimization objective. The corresponding feature parameter matrix groups for each level are generated respectively. The feature parameter matrix groups include the equation task objective feature matrix and the target expectation vector, the inequality safety constraint feature matrix and the safety boundary vector.

6. The multi-task control method for bottom-diving submersible crawling as described in claim 5, characterized in that, Step S3 includes the following sub-steps: S31, set the zero vector as the initial reference solution, the identity matrix as the initial null space basis matrix, and obtain the feature parameter matrix set for each level; S32, For the first priority task, perform a quadratic programming solution in the initial null space basis matrix space. The objective function of the quadratic programming is the sum of the first-level equation task objective feature matrix multiplied by the current cumulative optimal solution and the null space basis matrix multiplied by the decision variables, minus half of the norm square of the objective expectation vector, plus half of the regularization damping term of the decision variables. The constraint condition of the quadratic programming is that the sum of the first-level inequality safety constraint feature matrix multiplied by the current cumulative optimal solution and the null space basis matrix multiplied by the decision variables is less than or equal to the safety boundary vector. Solving for the first-level optimal control step size, the first-level optimal control step size is projected onto the initial null space basis matrix and accumulated to the initial reference solution to obtain the first-level cumulative optimal solution. S33. Based on the product of the first-level equation task feature matrix and the initial null space basis matrix, calculate the generalized pseudo-inverse of the product. Multiply the initial null space basis matrix by the identity matrix, subtract the generalized pseudo-inverse, and then multiply by the difference of the product to obtain the second-level usable null space basis matrix. S34, For the second priority task, take the first-level cumulative optimal solution as the current cumulative optimal solution and the second-level available null space basis matrix as the current null space basis matrix, perform the second-level quadratic programming solution to obtain the second-level optimal control step size, project the second-level optimal control step size onto the second-level available null space basis matrix and accumulate it to the first-level cumulative optimal solution to obtain the second-level cumulative optimal solution, and update the null space basis matrix according to the method in step S33 based on the product of the second-level equation task feature matrix and the second-level available null space basis matrix to obtain the third-level available null space basis matrix; S35. For the third priority task, take the second-level cumulative optimal solution as the current cumulative optimal solution and the third-level available null space basis matrix as the current null space basis matrix, and perform the third-level quadratic programming solution to obtain the third-level optimal control step size. Project the third-level optimal control step size onto the third-level available null space basis matrix and accumulate it to the second-level cumulative optimal solution to obtain the optimal solution of the system's generalized control input.

7. The multi-task control method for bottom-diving submersibles as described in claim 6, characterized in that, Step S4 includes the following sub-steps: S41. Obtain the absolute hydrostatic pressure in the deep sea, the density of the deep sea water, the current pitch angle of the fuselage, the current velocity vector amplitude of the submersible, and the real-time pressure inside the cabin volume compensator. Establish a local fluid external dynamic pressure model modulated by the pitch angle of the fuselage. Calculate the real-time sum of the hydrostatic and dynamic pressures in the local dynamic sealing area outside the fuselage by adding half of the density of the deep sea water to the absolute hydrostatic pressure in the deep sea, multiplying it by the fluid dynamic pressure distribution coefficient, and then multiplying it by the square of the current absolute velocity vector amplitude of the submersible. S42, calculate the square of the difference between the real-time sum of dynamic and static fluid pressure and the real-time pressure inside the cabin and the optimal safe working pressure difference benchmark value, obtain the partial derivative of the square with respect to the pitch angle, and use the partial derivative multiplied by the negative of the preset proportional feedback gain as the desired pitch fine-tuning differential torque. S43, obtain the sub-block matrix of the thruster in the null-space basis matrix after the second-level task update, obtain the row vector of the fuselage pitch degree of freedom in the static configuration mapping matrix of the thruster, multiply the row vector by the generalized pseudo-inverse of the sub-block matrix and then by the desired pitch fine-tuning differential torque to obtain the null-space control compensation increment of the thruster system. S44 superimposes the zero-space control compensation increment onto the original control command of the thruster system, enabling the thruster to generate differential thrust while maintaining the original trajectory tracking, so as to adjust the fuselage pitch angle.

8. A multi-task control system for a bottom-diving submersible, implemented using the multi-task control method for a bottom-diving submersible as described in any one of claims 1-7, characterized in that, include: The global mapping module is used to introduce the additional mass matrix and nonlinear hydrodynamic damping spindle, establish the underwater dynamic equation of the submersible, construct the global generalized Jacobian mapping matrix, and map the thrust output of the thruster and the contact reaction force at the end of the mechanical leg into the total dynamic control spindle of the underwater dynamic equation. The multi-task hierarchical module is used to acquire the real-time status of the submersible fed back by the state observer, the expected pose and acceleration commands issued by the navigation behavior layer, and the hardware physical limit parameters. According to the control priority, the tasks are divided into an absolute rigid constraint set, a main task set for operation tracking, and a redundant null space optimization set. The nonlinear friction cone constraint is linearized into a set of linear inequalities, and the characteristic parameter matrix set corresponding to each level is generated. The planning and solving module is used to perform secondary planning and solving step by step according to the feature parameter matrix group of each level, in order of priority from high to low, using the null space projection method. The solution of each level is performed in the null space of the previous level. The solution results are projected and accumulated to the cumulative optimal solution. After each level is completed, the null space basis matrix is ​​updated to obtain the optimal solution of the generalized control input of the system. The space control module is used to construct a fluid external dynamic pressure model modulated by the fuselage attitude, generate the desired fine-tuning torque, map the desired fine-tuning torque to the thruster subspace in the null space left by the previous mission, and generate the null space control compensation increment of the thruster system to adjust the fuselage attitude.

9. A terminal device, characterized in that, The terminal device includes: a memory, a processor, and a benthic submersible crawling multi-task control method program stored in the memory and executable on the processor, the benthic submersible crawling multi-task control method program being configured to implement the steps of the benthic submersible crawling multi-task control method as described in any one of claims 1 to 7.

10. A computer-readable storage medium, characterized in that, The storage medium stores a program for a multi-task control method for bottom-diving vehicle crawling, which, when executed, implements the multi-task control method for bottom-diving vehicle crawling as described in any one of claims 1-7.

Images