Underwater thruster thrust distribution control method and system based on QP optimization

Abstract

The invention discloses an underwater propeller thrust distribution control method and system based on QP optimization, which are used for avoiding redundant power consumption caused by improper distribution under redundant propeller configuration and prolonging underwater operation time and endurance mileage of a robot. The method comprises the steps that a preset attitude control instruction of the underwater robot and actual attitude information fed back by a current sensor are acquired, total disturbance acting on the underwater robot is estimated in real time based on the active disturbance rejection control principle, and expected total control force and total control torque instructions are calculated; constructing a quadratic programming problem based on the total control force and the total control moment instruction; solving a quadratic programming problem to obtain a first thrust instruction of each propeller after optimization; whether a propeller fault exists or not is diagnosed in real time, and when the fault is detected, a control efficiency matrix or a physical limitation constraint in the quadratic programming problem is dynamically adjusted; and solving again based on the adjusted quadratic programming problem to obtain a second thrust instruction of each propeller under fault tolerance.

Description

Technical Field

[0001] The present application relates to the technical field of underwater thruster control, and particularly to a method and system for underwater thruster thrust allocation control based on QP optimization. Background Art

[0002] Underwater robots, such as autonomous underwater vehicles (AUVs) and remotely operated vehicles (ROVs), are playing an increasingly important role in fields such as ocean exploration, resource development, and underwater operations. To enable these robots to have sufficient degrees of freedom of movement to adapt to complex underwater environments and achieve flexible and precise control performance, multiple (usually 4 to 8) thrusters are typically configured for them. This redundantly configured propulsion system is the key physical basis for achieving multi-degree-of-freedom motion control, attitude adjustment, and dynamic positioning.

[0003] In an underwater robot control system, thrust allocation is to reasonably distribute the desired total control force and torque calculated by the upper-layer motion controller to each independent thruster for execution. In the prior art, the pseudo-inverse method is generally used for thrust allocation, decomposing the six-degree-of-freedom total control command into thrust commands for each thruster, so as to drive the robot to complete a predetermined motion task.

[0004] Although traditional thrust allocation algorithms can distribute the total control force / torque generated by the upper-layer controller to each thruster, they generally do not consider the problem of optimal energy consumption, resulting in unnecessary energy consumption under redundant configurations, significantly shortening the underwater operation time and endurance of the robot. In addition, these traditional methods generally lack effective fault diagnosis and fault-tolerant control mechanisms. Once one or some thrusters fail (such as jamming, damage, or efficiency degradation), if the system cannot detect and adjust the thrust allocation strategy in time, the robot is likely to experience a sharp decline in control performance or even complete loss of control, leading to mission failure. Summary of the Invention

[0005] The present application provides a method and system for underwater thruster thrust allocation control based on QP optimization, which is used to avoid redundant power consumption caused by improper allocation under redundant thruster configurations and extend the underwater operation time and endurance of the robot.

[0006] The first aspect of the present application provides a method for underwater thruster thrust allocation control based on QP optimization, including: Obtaining a preset attitude control command of the underwater robot and actual attitude information feedback by the current sensor, estimating the total disturbance acting on the underwater robot in real time based on the active disturbance rejection control principle, and calculating the desired total control force and total control torque commands; For multiple thrusters configured on the underwater robot, a quadratic programming problem is constructed based on the total control force and the total control moment command. The objective function of the quadratic programming problem is used to minimize the total energy consumption of the multiple thrusters, and the constraint conditions of the quadratic programming problem at least include a balance constraint defined based on a control effectiveness matrix and a physical limit constraint on the thrust of each thruster; Solve the quadratic programming problem to obtain the first thrust command for each optimized thruster, and output the first thrust command to the actuators of each thruster; Diagnose in real time whether there is a thruster failure, and dynamically adjust the control effectiveness matrix or the physical limit constraint in the quadratic programming problem when a failure is detected; Re-solve based on the adjusted quadratic programming problem to obtain the second thrust command for each thruster under fault tolerance, and replace the first thrust command with the second thrust command and output it to the actuators of each thruster.

[0007] Optionally, the objective function is specifically designed as: ; where f is an n×1 vector used to represent the thrust generated by each of the n thrusters; f T represents the transpose of the vector f; W e is an n×n diagonal positive definite weight matrix, and the diagonal elements therein are used to reflect the energy consumption characteristics when each thruster generates a unit thrust; W s is an n×n identity matrix; is a regularization term used to ensure the uniqueness and smoothness of the solution of the objective function.

[0008] Optionally, the balance constraint is: ; where B(α) is the control effectiveness matrix, α is used to represent the installation angle, position and current working state of each thruster; τdes is the desired total control force and total control moment command.

[0009] Optionally, the real-time diagnosis of whether there is a thruster failure includes: Real-time collect and record the working state parameters of the multiple thrusters, and continuously compare the working state parameters with the corresponding healthy state baseline model. The working state parameters include at least one of motor drive current, motor terminal voltage, thruster rotor speed, thruster output thrust estimate, and thruster housing vibration signal; When the comparison result indicates that the deviation between the working state parameters and the healthy state baseline model continuously exceeds the preset fault judgment threshold, it is determined that the corresponding thruster has failed; Dynamically adjusting the control effectiveness matrix or the physical limit constraint in the quadratic programming problem when a fault is detected includes: When a fault is detected, evaluating the impact of the fault on the thruster effectiveness, determining the degree of effectiveness attenuation, and dynamically adjusting the control effectiveness matrix or the physical limit constraint in the quadratic programming problem according to the degree of effectiveness attenuation.

[0010] Optionally, evaluating the impact of the fault on the thruster effectiveness, determining the degree of effectiveness attenuation, and dynamically adjusting the control effectiveness matrix or the physical limit constraint in the quadratic programming problem according to the degree of effectiveness attenuation includes: When a fault is detected, for the target thruster determined to have a fault, determining the effectiveness attenuation factor of the target thruster by analyzing the operating state parameters of the target thruster, where the effectiveness attenuation factor is used to quantify the performance degradation amplitude of the target thruster; Multiplying the column vector corresponding to the target thruster in the control effectiveness matrix by the effectiveness attenuation factor, or adjusting the thrust amplitude of the target thruster according to the effectiveness attenuation factor.

[0011] Optionally, the real-time estimation of the total disturbance acting on the underwater robot based on the active disturbance rejection control principle includes: Combining the internal unmodeled dynamics and external environmental disturbances received by the underwater robot as the total disturbance, and defining the total disturbance as the extended state variable; Operating a predefined extended state observer according to the extended state variable, the attitude control instruction, and the actual attitude information to perform real-time online estimation of the extended state variable, so as to obtain an estimated value of the total disturbance.

[0012] Optionally, the method further includes: Using the historical thrust instruction sequence of the underwater robot, the actual motion response sequence feedback by the sensor, and the dynamic model to iteratively calibrate and update the control effectiveness matrix through an online parameter identification algorithm.

[0013] A second aspect of the present application provides a system for underwater thruster thrust allocation control based on QP optimization, including: An acquisition unit, configured to acquire the preset attitude control instruction of the underwater robot and the actual attitude information currently feedback by the sensor, estimate the total disturbance acting on the underwater robot in real time based on the active disturbance rejection control principle, and calculate the desired total control force and total control moment instructions; A building unit is configured to construct a quadratic programming problem for multiple thrusters configured on the underwater robot based on the total control force and the total control torque command. The objective function of the quadratic programming problem is used to minimize the total energy consumption of the multiple thrusters. The constraint conditions of the quadratic programming problem at least include a balance constraint defined based on a control effectiveness matrix and a physical limit constraint on the thrust of each thruster. A first solving unit is configured to solve the quadratic programming problem to obtain a first thrust command for each optimized thruster and output the first thrust command to the actuator of each thruster. A diagnosis unit is configured to diagnose in real time whether there is a thruster failure and dynamically adjust the control effectiveness matrix or the physical limit constraint in the quadratic programming problem when a failure is detected. A second solving unit is configured to re-solve based on the adjusted quadratic programming problem to obtain a second thrust command for each thruster under fault tolerance and output the second thrust command to replace the first thrust command to the actuator of each thruster.

[0014] A third aspect of the present application provides a device for underwater thruster thrust allocation control based on QP optimization. The device includes: A processor, a memory, an input / output unit, and a bus; The processor is connected to the memory, the input / output unit, and the bus; The memory stores a program, and the processor calls the program to execute the method for underwater thruster thrust allocation control based on QP optimization in the first aspect and any optional one in the first aspect.

[0015] A fourth aspect of the present application provides a computer-readable storage medium. A program is stored on the computer-readable storage medium, and when the program is executed on a computer, it executes the method for underwater thruster thrust allocation control based on QP optimization in the first aspect and any optional one in the first aspect.

[0016] From the above technical solutions, it can be seen that the present application has the following advantages: After obtaining the attitude control requirements of the underwater robot and calculating the desired total control force and total control torque commands through the active disturbance rejection control principle, for multiple thrusters configured on the underwater robot, a quadratic programming problem is constructed based on the total control force and total control torque commands. The objective function of this quadratic programming problem is used to minimize the total energy consumption of the multiple thrusters, which means that when solving the thrust allocation scheme, the core orientation of the algorithm is to find a combination of thruster thrusts that can meet the control commands and minimize the overall energy consumption. In addition, through the monitoring and diagnosis of the working states of each thruster, when a thruster fails, its performance degradation degree is evaluated, and the core parameters in the quadratic programming problem are actively adjusted and re-solved to obtain a thruster thrust combination considering the real-time fault state to replace the original command.

[0017] This method with the direct optimization goal of minimizing energy consumption can effectively avoid redundant power consumption caused by improper allocation under redundant thruster configurations, significantly reduce the ineffective energy consumption of the underwater robot during task execution, and thus effectively extend the underwater operation time and endurance of the robot. Through the closed-loop fault diagnosis and fault tolerance adjustment capabilities, it ensures that the underwater robot can still maintain its control stability and task execution ability to the greatest extent when encountering sudden situations where some thrusters fail, greatly enhancing the reliability and environmental adaptability of the underwater robot. Brief Description of the Drawings

[0018] In order to more clearly illustrate the technical solutions in this application, the drawings required for the description of the embodiments will be briefly introduced below. Obviously, the drawings in the following description are only some embodiments of this application. For those of ordinary skill in the art, without creative efforts, other drawings can be obtained based on these drawings.

[0019] Figure 1 Schematic flowchart of an embodiment of the method for underwater thruster thrust allocation control based on QP optimization provided by this application; Figure 2 Schematic flowchart of another embodiment of the method for underwater thruster thrust allocation control based on QP optimization provided by this application; Figure 3 Schematic structural diagram of an embodiment of the system for underwater thruster thrust allocation control based on QP optimization provided by this application; Figure 4 Schematic structural diagram of another embodiment of the system for underwater thruster thrust allocation control based on QP optimization provided by this application; Figure 5 Schematic structural diagram of an embodiment of the device for underwater thruster thrust allocation control based on QP optimization provided by this application. Detailed Description of the Embodiments

[0020] The present application provides a method and system for underwater thruster thrust allocation control based on QP optimization, which is used to avoid redundant power consumption caused by improper allocation under redundant thruster configurations and extend the underwater operation time and endurance of the robot.

[0021] Please refer to Figure 1 , Figure 1 which is an embodiment of the method for underwater thruster thrust allocation control based on QP optimization provided by the present application. The method includes: 101. Obtain the preset attitude control instruction of the underwater robot and the actual attitude information feedback by the current sensor, estimate the total disturbance acting on the underwater robot in real time based on the active disturbance rejection control principle, and calculate the desired total control force and total control torque instructions; Step 101 is the starting link of the entire thrust allocation control method. Its core purpose is to provide a clear and disturbance-compensated control target for subsequent thrust allocation, that is, to calculate the desired total control force and total control torque instructions. This control target is used to indicate how much resultant force and resultant torque the underwater robot needs to generate to accurately track the preset instruction and resist external and internal disturbances. In order to enable the underwater robot to move along a predetermined trajectory or attitude, it is necessary to first obtain the control target (preset attitude control instruction) and the current actual state of the robot (actual attitude information feedback by the sensor). The difference between the two is the basis for forming feedback control. Specifically, first obtain the preset attitude control instruction of the underwater robot. This part of the instruction is usually generated by the task planning system or the manual remote control operating system according to the task requirements. The instruction content usually includes the desired position (for example, x, y, z in the earth coordinate system), attitude (for example, Euler angles representing direction such as roll angle Φ, pitch angle θ, yaw angle ψ or quaternion), and may include the desired speed and acceleration. In addition, it is also necessary to obtain the actual attitude information feedback by the current sensor. The underwater robot is equipped with a variety of sensors to sense its own state and the environment, including but not limited to inertial measurement units, depth sensors, Doppler log, acoustic positioning systems, etc.

[0022] However, due to the complex and variable underwater environment, there are unknown external disturbances such as ocean currents and waves. At the same time, the mathematical model of the underwater robot itself may also be inaccurate. These factors will seriously affect the control accuracy. Therefore, in this embodiment, the active disturbance rejection control principle (ADRC) is adopted to regard these internal and external disturbances as a "total disturbance" and perform real-time estimation and compensation, so as to improve the robustness and control accuracy of the system. The total disturbance specifically includes the sum of all factors that affect the robot's motion and are not accurately described in the nominal model of the controller, such as environmental forces / moments such as water currents and waves, the uncertainty of the robot's own hydrodynamic parameters, unmodeled high-order dynamics, and the deviation of the thruster thrust model. By adding the estimated total disturbance to the control command calculation, it is possible to know in advance and cancel the influence of the upcoming disturbance, so that the robot can more accurately track the desired attitude, showing stronger disturbance rejection performance, and finally calculating the desired total control force and total control moment commands.

[0023] 102. For multiple thrusters configured on the underwater robot, a quadratic programming problem is constructed based on the total control force and total control moment commands. The objective function of the quadratic programming problem is used to minimize the total energy consumption of the multiple thrusters. The constraint conditions of the quadratic programming problem at least include the balance constraint defined based on the control effectiveness matrix and the physical limit constraint of the thrust of each thruster. Since an underwater robot is usually configured with multiple thrusters, there are multiple thrust combinations that can generate the same resultant force and resultant moment. What the upper-layer controller calculates are the overall desired total control force and total control moment commands for the underwater robot, while the actuators are individual thrusters. There must be an intermediate link to decompose this overall command into the specific thrust magnitudes of each thruster. The endurance of the underwater robot is limited by the energy it carries. Under the redundant thruster configuration, even if different thrust allocation schemes can produce the same control effect, their energy consumption may vary greatly. Therefore, it is necessary to find a set of thrust commands for each thruster that can not only meet the control requirements but also minimize the total energy consumption through mathematical optimization methods.

[0024] Specifically, to achieve the above goals, a quadratic programming (QP) problem is selected to be constructed in this embodiment. A QP problem usually includes three core parts: decision variables, objective function, and constraint conditions. Among them, the decision variables are the unknowns to be solved in the QP problem. The decision variables are specifically the thrust values that each thruster configured on the underwater robot will generate. The objective function is used to minimize the total energy consumption of the multiple thrusters. The total energy consumption specifically refers to the sum of the energies consumed by all thrusters when generating the allocated thrusts. The constraint conditions are the equalities or inequalities that the decision variables must satisfy, which jointly define the range of feasible solutions. The constraint conditions specifically include: the balance constraint defined based on the control effectiveness matrix and the physical limit constraint of each thruster determined by factors such as its physical structure and motor power.

[0025] The main function of the control effectiveness matrix is to linearly map or transform a vector containing the magnitudes of the thrusts independently generated by each thruster into a vector representing the resultant force and resultant moment acting on the six degrees of freedom in the robot's body coordinate system. This control effectiveness matrix specifically quantifies the contribution of each thruster to the movement of the robot in six directions: forward, lateral, vertical, roll, pitch, and yaw, when generating a unit thrust, due to its specific installation position and thrust action direction on the robot. For a system with n thrusters that needs to be controlled in m degrees of freedom (usually m = 6 for an underwater robot), the control effectiveness matrix is typically an m×n matrix. Each column of this matrix uniquely corresponds to a thruster. For example, the j-th column of the matrix is an m-dimensional vector, and its elements detail the specific values of the forces and torques generated by the j-th thruster on the m degrees of freedom of the robot when outputting a unit of its own thrust.

[0026] 103. Solve the quadratic programming problem to obtain the first thrust commands for each optimized thruster, and output the first thrust commands to the actuators of each thruster; After constructing the quadratic programming problem, it is necessary to solve this quadratic programming problem to obtain the thrust commands that minimize the sum of the energies consumed by all thrusters when generating the allocated thrusts under the premise of satisfying all constraint conditions, that is, the first thrust commands. Specifically, an appropriate QP solver and algorithm can be selected according to the specific computing platform, the required solution speed, and the availability of the solver library. After receiving the problem parameters, the QP solver will execute its internal numerical optimization algorithm. This process is usually iterative, and the algorithm starts from an initial point and gradually searches according to certain rules (such as combining Newton's method, gradient method, etc. with constraint handling strategies) until the convergence conditions are met (for example, the degree of satisfaction of the KKT conditions, the change in the objective function value is small enough, or the number of iterations reaches the upper limit). When the QP solver converges successfully, its output is the optimal first thrust commands. Subsequently, the first thrust commands are sent to the corresponding actuators of each thruster on the underwater robot, and these actuators then convert these numerical thrust commands into actual physical drive signals to generate the required thrust.

[0027] In some specific embodiments, an RL agent can be selected to solve the quadratic programming problem, that is, transform the QP solution into an RL problem. After sufficient offline training, the online decision-making process of the RL agent can be simplified to a forward propagation calculation through a neural network. This method is faster than the iterative traditional QP solver. Before actual deployment, the RL agent will experience a large number of QP problem instances with different parameters in a simulation environment. The goal of the RL agent's learning is to find a set of thruster thrusts that can not only minimize the total energy consumption as much as possible (i.e., optimize the objective function of the QP problem) but also strictly satisfy all constraint conditions. During the training process, a reward mechanism is used to guide the learning. If the thrust plan given by the agent can well meet the low energy consumption and all constraints, it will receive a high reward; otherwise, if the energy consumption is high or the constraints are violated, it will receive a negative reward. By continuously adjusting its internal policy network, the RL agent can learn how to obtain a higher cumulative reward, thus mastering an effective method for quickly solving the QP problem.

[0028] 104. Real-time diagnose whether there is a thruster fault, and dynamically adjust the control effectiveness matrix or physical limit constraints in the quadratic programming problem when a fault is detected; The thruster is a key execution component of the underwater robot, and its fault directly affects the robot's motion control ability. Therefore, during the actual operation of the underwater robot, it is also necessary to real-time monitor the working state parameters of multiple thrusters to diagnose whether there is a thruster fault. Real-time monitoring and early diagnosis can timely detect problems, prevent the deterioration of faults or trigger chain reactions, thereby improving the reliability and operation safety of the overall system. When a thruster fails and causes a decline in effectiveness, its thrust generation ability has deviated from the model in the initial design or healthy state. If the control system still makes thrust allocation decisions based on outdated or inaccurate information, it will not be physically effectively executed, and may even exacerbate system instability or cause further damage. Therefore, in this embodiment, once a certain thruster is diagnosed as having a fault, it is necessary to dynamically adjust the control effectiveness matrix or physical limit constraints in the quadratic programming problem. By dynamically adjusting these core parameters, the mathematical model on which the subsequent thrust allocation calculation is based can truthfully reflect the current true working ability of the propulsion system including the impact of the fault.

[0029] 105. Resolve the adjusted quadratic programming problem again to obtain the second thrust commands of each thruster under fault tolerance, and replace the first thrust commands with the second thrust commands and output them to the actuators of each thruster.

[0030] After a fault is diagnosed and the core parameters in the quadratic programming problem are adjusted, the updated QP model that can accurately reflect the current fault state is used to calculate a new set of thrust allocation schemes, that is, the second thrust command, and the second thrust command is used to replace the original first thrust command calculated under fault-free conditions, so as to actually respond to the fault and maintain or restore the control of the underwater robot. The second thrust command is the optimal solution for the current fault situation. It takes into account which thrusters have reduced efficiency or completely failed, and based on this, reallocates the thrust. Therefore, while meeting the control requirements, it still follows the principle of minimizing energy consumption. This enables the underwater robot to still be able to utilize the remaining resources in an optimal way to execute control tasks after a partial failure occurs in the propulsion system, thus significantly enhancing the robustness and mission success rate of the system.

[0031] In this embodiment, after obtaining the attitude control requirements of the underwater robot and calculating the desired total control force and total control torque commands through the active disturbance rejection control principle, for the multiple thrusters configured on the underwater robot, a quadratic programming problem is constructed based on the total control force and total control torque commands. The objective function of this quadratic programming problem is used to minimize the total energy consumption of the multiple thrusters, which means that when solving the thrust allocation scheme, the core orientation of the algorithm is to find a combination of thruster thrusts that can meet the control commands while minimizing the overall energy consumption. In addition, through the monitoring and diagnosis of the working states of each thruster, when a thruster fails, its performance degradation degree is evaluated, and the core parameters in the quadratic programming problem are actively adjusted and re-solved to obtain a thruster thrust combination considering the real-time fault state to replace the original command.

[0032] This method with minimizing energy consumption as the direct optimization objective can effectively avoid redundant power consumption caused by improper allocation under redundant thruster configurations, significantly reducing the ineffective energy consumption of the underwater robot during task execution, thereby effectively extending the underwater operation time and endurance of the robot. Through the closed-loop fault diagnosis and fault tolerance adjustment capabilities, it ensures that the underwater robot can still maintain its maximum control stability and task execution ability when encountering sudden situations of partial thruster failures, greatly enhancing the reliability and environmental adaptability of the underwater robot.

[0033] The following provides a detailed description of the method for underwater thruster thrust allocation control based on QP optimization provided by this application. Please refer to Figure 2 , Figure 2 which is another embodiment of the method for underwater thruster thrust allocation control based on QP optimization provided by this application. The method includes: 201. Obtain the preset attitude control instruction of the underwater robot and the actual attitude information feedback by the current sensor, estimate the total disturbance acting on the underwater robot in real time based on the active disturbance rejection control principle, and calculate the desired total control force and total control torque instruction. In this embodiment, step 201 is similar to step 101 of the foregoing embodiment, and will not be elaborated here.

[0034] In some specific embodiments, the process of estimating the total disturbance acting on the underwater robot in real time based on the active disturbance rejection control principle specifically includes the following steps: Combine the internal unmodeled dynamics and external environmental disturbances suffered by the underwater robot as the total disturbance, and define the total disturbance as the extended state variable; run a predefined extended state observer according to the extended state variable, attitude control instruction and actual attitude information to perform real-time online estimation of the extended state variable, so as to obtain the estimated value of the total disturbance.

[0035] The underwater robot faces a wide variety of disturbance sources and it is difficult to accurately model them. For example, the direction and speed of the water flow are time-varying, the hydrodynamic parameters of the robot itself may change with the speed or due to attachments, and the internal friction characteristics may also change. Trying to separately model and measure each kind of uncertainty is very difficult or even impractical, and traditional control methods usually have difficulty dealing with unknown or rapidly changing disturbances. Therefore, by regarding the total disturbance as an additional state of the system, that is, the extended state variable, then an observer theory can be used to design a special extended state observer ESO to estimate this extended state variable, so as to realize the real-time online estimation of the total disturbance. The predefined extended state observer is a specially designed state observer, and its structure and parameters are preset according to the nominal model of the underwater robot and the desired observer performance.

[0036] Specifically, the total disturbance consists of internal unmodeled dynamics and external environmental disturbances. Among them, internal unmodeled dynamics refer to the characteristics of the underwater robot itself that cannot be fully and accurately described or are ignored when constructing the mathematical model of the underwater robot, such as the change of hydrodynamic coefficients at different speeds, the change of mass and inertia caused by load changes, the nonlinear characteristics of thrusters, and the errors caused by model simplification. External environmental disturbances refer to various forces and torques that come from the external environment of the robot and act on it. The most typical ones are the impact of ocean currents, the disturbance of waves, and the change of water flow caused by complex terrain. The comprehensive impact of all the above internal unmodeled dynamics and external environmental disturbances on the system dynamics, as well as any other factors that are not explicitly modeled, are uniformly regarded as a lumped unknown disturbance acting on a specific channel of the system. In order to estimate this unknown disturbance, it can be regarded as a new and unknown state variable of the system and added to the state equation of the original system to form an extended state space model. Thereafter, using the predefined extended state observer (ESO), according to the known control input and the current system state, predict what state the underwater robot should be in. Then compare this "predicted state" with the "actual state" feedback by the sensor. The difference between the two largely reflects the immediate impact of the "total disturbance". The ESO continuously adjusts its estimated values of the original state of the robot and the "extended state" (i.e., the total disturbance) through its internal correction mechanism (such as a specific gain function or algorithm), with the goal of making the system behavior estimated by the observer as consistent as possible with the actually observed behavior. After the above real-time online estimation process, the ESO can finally output the estimated value of the total disturbance.

[0037] Through the total disturbance value estimated by the extended state observer, the controller can know in advance which unmodeled dynamics and external disturbances are currently trying to interfere with the normal operation of the system. Therefore, when calculating the desired total control force and total control torque commands, the total disturbance value will be taken into consideration, and a compensation control amount with the same magnitude and opposite direction will be generated to offset various internal and external disturbances acting on the underwater robot in real time.

[0038] 202. For multiple thrusters configured on the underwater robot, a quadratic programming problem is constructed based on the total control force and total control torque commands. The objective function of the quadratic programming problem is used to minimize the total energy consumption of the multiple thrusters. The constraint conditions of the quadratic programming problem at least include the balance constraint defined based on the control effectiveness matrix and the physical limit constraint of the thrust of each thruster; In this embodiment, step 202 is similar to step 102 in the foregoing embodiment, and will not be elaborated here. The following will introduce the objective function and constraint conditions of the quadratic programming problem in detail: In this embodiment, the objective function is specifically designed as: ; Among them, $\mathbf{f}$ is an $n\times1$ vector, which is used to represent the thrust generated by each of the $n$ thrusters and is the core variable to be solved in the QP problem; $\mathbf{f}$ T represents the transpose of the vector $\mathbf{f}$; $\mathbf{W}$ e is an $n\times n$ diagonal positive definite weight matrix, and the diagonal elements therein are used to reflect the energy consumption characteristics when each thruster generates a unit thrust; a diagonal matrix means that all other elements are zero except for the elements on the main diagonal. The diagonal property means that the thrust $\mathbf{f}$ generated by each thruster i contributes independently and is weighted to the total energy consumption, without being directly cross - affected by the thrusts of other thrusters. Therefore, its weight can be set independently according to the characteristics of each thruster (such as energy consumption efficiency).

[0039] In the case where $\mathbf{W}$ e is a diagonal matrix, the quadratic - form energy consumption term can be simplified to the weighted sum of the squares of each thrust: ; where $\mathbf{f}$ i is the thrust of the $i$-th thruster, and $w$ ei is the $i$-th element on the main diagonal of $\mathbf{W}$ e . By minimizing this quadratic - form energy consumption term, the goal is to preferentially use thrusters with higher energy efficiency (corresponding to smaller weight values in $\mathbf{W}$ e ), or to generally reduce the sum of the squares of the thrusts, so as to achieve the goal of minimizing the total energy consumption.

[0040] $\mathbf{W}$ s is an $n\times n$ identity matrix, and $\lambda\mathbf{I}$ is the regularization term with the regularization coefficient $\lambda$ introduced. This regularization term is used to ensure the uniqueness and smoothness of the solution of the objective function. This is because in some cases, relying only on the energy consumption term and the constraint conditions may not be sufficient to determine a unique thrust allocation scheme. Adding a strictly convex regularization term can ensure that the entire objective function is strictly convex, so as to ensure that if the QP problem has a feasible solution, its optimal solution is unique. This term also helps to penalize overly large thrust values, making the thrust allocation scheme tend to use smaller and more evenly distributed thrusts, which may bring smoother control outputs and help to improve the numerical conditions of the optimization problem and avoid extreme solutions.

[0041] In this embodiment, the balance constraint is: ; Among them, B(α) is the control effectiveness matrix, and α is used to represent the installation angles, positions, and current working states of each thruster; τdes is the desired total control force and total control moment command. The composition of the control effectiveness matrix depends on the parameter α, which includes the installation angles, positions, and current working states of each thruster. The installation angles and positions determine the force and moment components decomposed into each degree of freedom in the robot body coordinate system when each thruster generates a unit thrust. The current working state can reflect whether the thruster is healthy and whether there is performance decay. This means that B(α) is not fixed but can be dynamically adjusted according to the real-time health status of the thruster.

[0042] The fundamental role of this balance constraint is to ensure that no matter how the optimization process selects the thrust combination f of each thruster, after being transformed by the control effectiveness matrix B(α), its combined effect must be exactly equal to the total control force and total control moment τdes desired by the upper-layer controller.

[0043] In addition, the quadratic programming problem also involves physical limit constraints on the thrust of each thruster, specifically including the upper and lower limits of the thrust amplitude of each thruster (maximum forward thrust, minimum thrust, or maximum reverse thrust) and the thrust change rate constraint. Among them, the upper and lower limits of the thrust amplitude are usually determined by factors such as the motor power of the thruster, propeller design, structural strength, and prevention of cavitation. The thrust change rate constraint is to protect the motor and driver and avoid adverse hydrodynamic effects. Both of these constraints are linear, jointly ensuring that while the optimization algorithm pursues the lowest energy consumption, the output thrust command can be safely and stably achieved physically by each thruster.

[0044] 203. Solve the quadratic programming problem to obtain the first thrust command of each optimized thruster, and output the first thrust command to the actuator of each thruster; In this embodiment, step 203 is similar to step 103 in the foregoing embodiment, and will not be elaborated here.

[0045] 204. Real-time collect and record the working state parameters of multiple thrusters, and continuously compare the working state parameters with the corresponding health state baseline model; 205. When the comparison result indicates that the deviation between the working state parameters and the health state baseline model continuously exceeds the preset fault judgment threshold, determine that the corresponding thruster has failed; For the health diagnosis of thrusters, it is necessary to collect and record the working state parameters of multiple thrusters when needed. The working state parameters specifically refer to the physical quantities reflecting the health of the thrusters, including at least one of the motor drive current, motor terminal voltage, thruster rotor speed, estimated thruster output thrust, and thruster housing vibration signal. Next, continuously compare these real-time collected working state parameters with the corresponding health state baseline model. The health state baseline model is a model established through a large number of experimental data collections, statistical analyses, or physical principle-based modeling when the thruster is confirmed to be in a fault-free and healthy operating state. It can depict the normal fluctuation range or expected behavior pattern of each thruster parameter under different working conditions. Ideally, each thruster should have its personalized baseline model, or use a general model and then adjust it according to the individual differences of the thrusters, because even thrusters of the same model may have slight performance differences.

[0046] In each control cycle or data sampling cycle, it is necessary to compare the latest obtained real-time working state parameters with the normal values defined by the health state baseline model. This comparison can take various forms. For example, directly compare whether the instantaneous value exceeds the preset normal threshold range, or calculate the residual between the real-time data and the model prediction value, or analyze the similarity or distance between the real-time data characteristics and the health mode characteristics. The specific form is not limited here. When one or some of the working state parameters from a specific thruster simultaneously meet the two conditions of exceeding the preset fault judgment threshold and being persistent, it can be determined that the corresponding thruster has a fault. At this time, it is necessary to continue to execute step 206.

[0047] 206. When a fault is detected, evaluate the impact of the fault on the thruster efficiency, determine the degree of efficiency decay, and dynamically adjust the control efficiency matrix or physical limit constraint in the quadratic programming problem according to the degree of efficiency decay; When a fault is detected, the impact of all faults on thruster performance is not the same. Some may cause a slight decrease in efficiency, while others may lead to a complete failure of the thruster. Therefore, it is necessary to first evaluate the impact of the fault on thruster efficiency to determine the degree of efficiency decay. Accurately determining the degree of efficiency decay is the basis for subsequent adjustment of the control strategy to achieve optimal compensation, avoiding improper control caused by overestimating or underestimating the fault impact. Therefore, it is also necessary to dynamically adjust the control efficiency matrix or physical limit constraints in the quadratic programming problem according to the degree of efficiency decay. The adjustment of the control efficiency matrix is because this matrix directly describes how much force and torque each thruster can contribute to the overall robot when generating a unit of thrust. When the efficiency of a certain thruster decays, its corresponding influence in the control efficiency matrix needs to be adjusted downward according to the degree of decay. The adjustment of the physical limit constraints is because some faults may directly change the actual output range of the thruster. For example, for a completely failed thruster, both its upper and lower output limits should be set to zero, or for a partially damaged thruster, its maximum thrust may be less than that in the healthy state. Through these adjustments, the QP model changes from a model describing a healthy system to a model describing the system in the current specific fault state.

[0048] It should be noted that this adjustment occurs in real time. Once the fault is determined and the degree of efficiency decay is analyzed, the parameters of the QP model should be immediately updated accordingly so that the latest model can be used for thrust allocation in the next control cycle.

[0049] Specifically, an efficiency decay factor (a value between 0 and 1, where 1 represents completely healthy and 0 represents completely failed) can be used to represent the degree of efficiency decay. That is, when a fault is detected, for the target thruster determined to have a fault, the efficiency decay factor of the target thruster is determined by analyzing the working state parameters of the target thruster. The efficiency decay factor is used to quantify the performance degradation of the target thruster; multiply the column vector corresponding to the target thruster in the control efficiency matrix by the efficiency decay factor, or adjust the thrust amplitude of the target thruster according to the efficiency decay factor.

[0050] Since each column of the control effectiveness matrix represents the forces and moments generated by a thruster on the six degrees of freedom of the underwater robot under unit thrust, by multiplying the column vector corresponding to the target thruster in the control effectiveness matrix by the effectiveness decay factor, the contribution of the faulty thruster in the QP model can be directly reduced. When solving the QP, it will be sensed that even if the faulty thruster outputs the same self-thrust as when it is healthy, the effective control effect it can provide for the overall robot has been reduced proportionally, and the dependence on this thruster will naturally be reduced. Adjusting the thrust amplitude of the target thruster according to the effectiveness decay factor means directly modifying the physical thrust upper and lower limits of the faulty thruster in the QP constraint conditions. If the effectiveness decay factor of a thruster is 0 (completely failed), then both its thrust upper and lower limits should be set to 0. This adjustment directly limits the thrust range that the QP optimizer can allocate to the faulty thruster, ensuring that the allocation result does not exceed the physical limit that the thruster can actually reach in the faulty state.

[0051] These two adjustment methods can be used alone or in combination, depending on the physical manifestation of the fault and how to most accurately reflect the fault impact in the QP model. For example, for a completely failed thruster, usually both methods are adopted (setting the corresponding column of the control effectiveness matrix to zero and setting the thrust amplitude to zero). For the case where the motor power is partially damaged but still can stably output a low thrust, it may be more focused on adjusting the upper limit of the thrust amplitude and supplemented by the adjustment of the control effectiveness matrix.

[0052] 207. Resolve the adjusted quadratic programming problem again to obtain the second thrust commands of each thruster under fault tolerance, and replace the first thrust commands with the second thrust commands and output them to the actuators of each thruster; In this embodiment, step 207 is similar to step 105 of the foregoing embodiment, and will not be elaborated here.

[0053] 208. Use the historical thrust command sequence of the underwater robot, the actual motion response sequence feedback by the sensor, and the dynamic model to iteratively calibrate and update the control effectiveness matrix through an online parameter identification algorithm.

[0054] The control efficiency matrix initially set may be based on theoretical calculations or experimental data under ideal conditions, and may deviate from the actual situation after the robot is actually deployed. In addition, the performance of the thruster may gradually change over time. For example, the efficiency slowly decreases due to slight wear, aging of components, attachment of marine organisms, etc., resulting in the control efficiency matrix initially set not being able to reflect the actual operating conditions. The core goal of step 208 is to make the control efficiency matrix no longer a fixed parameter matrix, but to be able to continuously self-calibrate and dynamically update according to the performance data of the underwater robot in actual operation. A more accurate control efficiency matrix that better reflects the current real performance of the thruster can directly improve the accuracy and efficiency of the subsequent secondary planning thrust distribution, thereby improving the control performance and energy utilization of the entire underwater robot.

[0055] First, the system collects two sets of key sequence data in real time. The first set is the historical thrust command sequence, which refers to the command values ​​actually sent to each thruster in the recent period. The second set is the actual motion response sequence fed back by the sensor. This information includes the actual acceleration, angular acceleration, velocity, and position attitude of the robot, which is usually collected by inertial measurement units, Doppler odometers and other equipment. In order to accurately separate the part contributed by the thruster from these complex motion responses, the system also uses a preset underwater robot dynamics model. This dynamics model describes the robot's mass, inertia, hydrodynamic damping, and restoring torque. With the help of this model, the influence of non-thruster factors, such as residual hydrodynamic force or gravitational buoyancy after compensating for water flow disturbances, can be deducted from the observed total motion response, so as to estimate the actual motion effect generated by the thruster force, that is, the actual combined control force and combined control torque. Next, through the online parameter identification algorithm, the control effectiveness matrix estimation value of the current version is used, combined with the input thrust command, to predict the motion response that the robot should theoretically produce. The algorithm then compares this predicted motion response with the actual observed motion response to form a prediction error or cost function. The goal of an online parameter identification algorithm, such as recursive least squares, gradient descent, or a Kalman filter-based estimation algorithm, is to minimize this prediction error by systematically adjusting the values ​​of the elements in the control effectiveness matrix. This adjustment process is iterative, meaning that the algorithm is not calibrated all at once. It makes small, continuous corrections to the elements of the control effectiveness matrix at each new data batch or time step based on the latest error information.

[0056] Finally, through continuous iteration and calibration of the online parameter identification algorithm, the system will obtain an updated control effectiveness matrix. Compared with the initial version or the previous version, this updated control effectiveness matrix can more accurately reflect the true control effectiveness of each thruster under the current operating conditions, because it incorporates the performance data during the actual operation of the robot. Once the control effectiveness matrix is updated, this new version of the matrix will replace the old version and be supplied for subsequent thrust allocation steps, such as when constructing a quadratic programming problem. Through this continuous online learning and self-correction mechanism, the control effectiveness matrix can dynamically adapt to the gradual changes in thruster performance, such as wear or the influence of attachments, as well as the subtle influence of environmental factors that are not accurately modeled, thus ensuring that thrust allocation decisions are always based on the system model closest to the current physical reality.

[0057] Please refer to Figure 3 , Figure 3 which is an embodiment of the underwater thruster thrust allocation control system based on QP optimization provided by this application. The system includes: An acquisition unit 301, configured to acquire the preset attitude control instruction of the underwater robot and the actual attitude information feedback by the current sensor, estimate the total disturbance acting on the underwater robot in real time based on the active disturbance rejection control principle, and calculate the desired total control force and total control torque instructions; A construction unit 302, configured to construct a quadratic programming problem for multiple thrusters configured on the underwater robot based on the total control force and total control torque instructions. The objective function of the quadratic programming problem is used to minimize the total energy consumption of the multiple thrusters. The constraint conditions of the quadratic programming problem at least include the balance constraint defined based on the control effectiveness matrix, and the physical limit constraint of the thrust of each thruster; A first solving unit 303, configured to solve the quadratic programming problem to obtain the optimized first thrust instructions of each thruster, and output the first thrust instructions to the actuators of each thruster; A diagnosis unit 304, configured to diagnose in real time whether there is a thruster failure, and dynamically adjust the control effectiveness matrix or the physical limit constraint in the quadratic programming problem when a failure is detected; A second solving unit 305, configured to re-solve based on the adjusted quadratic programming problem to obtain the second thrust instructions of each thruster under fault tolerance, and replace the first thrust instructions with the second thrust instructions and output them to the actuators of each thruster.

[0058] The following will detail the underwater thruster thrust allocation control system based on QP optimization provided by this application. Please refer to Figure 4 , Figure 4 which is another embodiment of the underwater thruster thrust allocation control system based on QP optimization provided by this application. The system includes: An acquisition unit 401, configured to acquire a preset attitude control instruction of the underwater robot and actual attitude information feedback by a current sensor, estimate in real time a total disturbance acting on the underwater robot based on the active disturbance rejection control principle, and calculate a desired total control force and total control torque instruction; A construction unit 402, configured to construct a quadratic programming problem for a plurality of thrusters configured on the underwater robot based on the total control force and total control torque instructions, where an objective function of the quadratic programming problem is used to minimize the total energy consumption of the plurality of thrusters, and constraint conditions of the quadratic programming problem at least include a balance constraint defined based on a control effectiveness matrix and a physical limit constraint of the thrust of each thruster; A first solving unit 403, configured to solve the quadratic programming problem to obtain first thrust instructions of the optimized respective thrusters, and output the first thrust instructions to actuators of the respective thrusters; A diagnosis unit 404, configured to diagnose in real time whether there is a thruster fault, and dynamically adjust the control effectiveness matrix or the physical limit constraint in the quadratic programming problem when a fault is detected; A second solving unit 405, configured to re-solve based on the adjusted quadratic programming problem to obtain second thrust instructions of the respective thrusters under fault tolerance, and replace the first thrust instructions with the second thrust instructions and output them to the actuators of the respective thrusters.

[0059] Optionally, the objective function is specifically designed as: ; where f is an n×1 vector, used to represent the thrust generated by each of the n thrusters; f T represents the transpose of the vector f; W e is an n×n diagonal positive definite weight matrix, and the diagonal elements therein are used to reflect the energy consumption characteristics when each thruster generates a unit thrust; W s is an n×n identity matrix; is a regularization term, used to ensure the uniqueness and smoothness of the solution of the objective function.

[0060] Optionally, the balance constraint is: ; where B(α) is a control effectiveness matrix, and α is used to represent the installation angle, position and current working state of each thruster; τdes is the desired total control force and total control torque instruction.

[0061] Optionally, the diagnosis unit 304 is specifically configured to: Collect and record the working state parameters of multiple thrusters in real time, and continuously compare the working state parameters with the corresponding health state baseline model. The working state parameters include at least one of motor drive current, motor terminal voltage, thruster rotor speed, estimated thruster output thrust, and thruster housing vibration signal. When the comparison result indicates that the deviation between the working state parameters and the health state baseline model continuously exceeds the preset fault judgment threshold, it is determined that the corresponding thruster has a fault. Dynamically adjust the control effectiveness matrix or physical limit constraints in the quadratic programming problem when a fault is detected, including: Evaluate the impact of the fault on the thruster effectiveness when a fault is detected, determine the degree of effectiveness attenuation, and dynamically adjust the control effectiveness matrix or physical limit constraints in the quadratic programming problem according to the degree of effectiveness attenuation.

[0062] Optionally, the diagnosis unit 304 is further specifically configured to: When a fault is detected, for the target thruster determined to have a fault, determine the effectiveness attenuation factor of the target thruster by analyzing the working state parameters of the target thruster. The effectiveness attenuation factor is used to quantify the performance degradation amplitude of the target thruster. Multiply the column vector corresponding to the target thruster in the control effectiveness matrix by the effectiveness attenuation factor, or adjust the thrust amplitude of the target thruster according to the effectiveness attenuation factor.

[0063] Optionally, the acquisition unit 301 is specifically configured to: Combine the internal unmodeled dynamics and external environmental disturbances received by the underwater robot as the total disturbance, and define the total disturbance as the extended state variable. Run a predefined extended state observer based on the extended state variable, attitude control instruction, and actual attitude information to perform real-time online estimation of the extended state variable, so as to obtain an estimated value of the total disturbance.

[0064] Optionally, the system further includes: An update unit 406, configured to iteratively calibrate and update the control effectiveness matrix through an online parameter identification algorithm by using the historical thrust command sequence of the underwater robot, the actual motion response sequence feedback by the sensor, and the dynamic model.

[0065] In the system of this embodiment, the functions of each unit correspond to the steps in the foregoing Figure 1 or Figure 2 The method embodiments shown will not be elaborated here.

[0066] This application also provides a device for underwater thruster thrust allocation control based on QP optimization. Please refer to Figure 5 , Figure 5An embodiment of the device for underwater thruster thrust allocation control based on QP optimization provided by this application, the device includes: A processor 501, a memory 502, an input / output unit 503, and a bus 504; The processor 501 is connected to the memory 502, the input / output unit 503, and the bus 504; The memory 502 stores a program, and the processor 501 calls the program to execute any one of the methods for underwater thruster thrust allocation control based on QP optimization as described above.

[0067] This application also relates to a computer-readable storage medium, on which a program is stored. When the program runs on a computer, it causes the computer to execute any one of the methods for underwater thruster thrust allocation control based on QP optimization as described above.

[0068] Those skilled in the art can clearly understand that for the convenience and simplicity of description, the specific working processes of the above-described systems, devices, and units can refer to the corresponding processes in the foregoing method embodiments, and will not be elaborated herein.

[0069] In several embodiments provided by this application, it should be understood that the disclosed systems, devices, and methods can be implemented in other ways. For example, the device embodiments described above are merely illustrative. For example, the division of units is only a logical function division. In actual implementation, there may be other division methods. For example, multiple units or components can be combined or integrated into another system, or some features can be ignored or not executed. Another point is that the displayed or discussed couplings or direct couplings or communication connections to each other can be through some interfaces. The indirect couplings or communication connections of devices or units can be in electrical, mechanical, or other forms.

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

[0071] In addition, the functional units in each embodiment of this application can be integrated into one processing unit, or each unit can exist physically alone, or two or more units can be integrated into one unit. The above-mentioned integrated units can be implemented in the form of hardware or in the form of software functional units.

[0072] If the integrated unit is implemented in the form of a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of this application, in essence, or the part that contributes to the prior art, or all or part of this 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 for causing a computer device (which may be a personal computer, a server, or a network device, etc.) to execute all or part of the steps of the methods of the various embodiments of this application. The foregoing storage medium includes: various media that can store program codes, such as USB flash drives, mobile hard disks, read-only memories (ROMs), random access memories (RAMs), magnetic disks, or optical discs.

Claims

1. A method for thrust allocation control of an underwater thruster based on QP optimization, characterized in that The method includes: Obtaining the preset attitude control instruction of the underwater robot and the actual attitude information feedback by the current sensor, estimating the total disturbance acting on the underwater robot in real time based on the active disturbance rejection control principle, and calculating the desired total control force and total control torque instructions; For multiple thrusters configured on the underwater robot, constructing a quadratic programming problem based on the total control force and the total control torque instructions, where the objective function of the quadratic programming problem is used to minimize the total energy consumption of the multiple thrusters, and the constraint conditions of the quadratic programming problem at least include the balance constraint defined based on the control effectiveness matrix and the physical limit constraint of the thrust of each thruster; Solving the quadratic programming problem to obtain the optimized first thrust instruction of each thruster, and outputting the first thrust instruction to the actuators of each thruster; Diagnosing in real time whether there is a thruster fault, and dynamically adjusting the control effectiveness matrix or the physical limit constraint in the quadratic programming problem when a fault is detected; Based on the adjusted quadratic programming problem, re-solving to obtain the second thrust instruction of each thruster under fault tolerance, and replacing the first thrust instruction with the second thrust instruction and outputting it to the actuators of each thruster.

2. The method for thrust allocation control of an underwater thruster based on QP optimization according to claim 1, characterized in that, The objective function is specifically designed as: ； Among them, f is an n×1 vector, used to represent the thrust generated by each of the n thrusters; f T represents the transpose of the vector f; W e is an n×n diagonal positive definite weight matrix, and the diagonal elements therein are used to reflect the energy consumption characteristics when each thruster generates a unit thrust; W s is an n×n identity matrix; is a regularization term, used to ensure the uniqueness and smoothness of the solution of the objective function.

3. The method for underwater thruster thrust allocation control based on QP optimization according to claim 1, characterized in that The balance constraint is: ； Where B(α) is the control effectiveness matrix, and α is used to represent the installation angle, position and current working state of each thruster; τdes is the desired total control force and total control torque instructions.

4. The method for thrust allocation control of an underwater thruster based on QP optimization according to claim 1, wherein The diagnosing in real time whether there is a thruster fault includes: Collecting and recording the working state parameters of the multiple thrusters in real time, and continuously comparing the working state parameters with the corresponding healthy state baseline model, where the working state parameters include at least one of the motor drive current, the motor terminal voltage, the thruster rotor speed, the estimated thruster output thrust, and the thruster housing vibration signal; When the comparison result indicates that the deviation between the working state parameters and the healthy state baseline model continuously exceeds the preset fault judgment threshold, it is determined that the corresponding thruster has a fault; The dynamically adjusting the control effectiveness matrix or the physical limit constraint in the quadratic programming problem when a fault is detected includes: Evaluating the impact of the fault on the thruster effectiveness when a fault is detected, determining the effectiveness attenuation degree, and dynamically adjusting the control effectiveness matrix or the physical limit constraint in the quadratic programming problem according to the effectiveness attenuation degree.

5. The method for underwater thruster thrust allocation control based on QP optimization according to claim 4, characterized in that The evaluating the impact of the fault on the thruster effectiveness when a fault is detected, determining the effectiveness attenuation degree, and dynamically adjusting the control effectiveness matrix or the physical limit constraint in the quadratic programming problem according to the effectiveness attenuation degree includes: When a fault is detected, for the target thruster determined to have a fault, determining the effectiveness attenuation factor of the target thruster by analyzing the working state parameters of the target thruster, where the effectiveness attenuation factor is used to quantify the performance degradation amplitude of the target thruster; Multiply the column vector corresponding to the target thruster in the control effectiveness matrix by the effectiveness decay factor, or adjust the thrust amplitude of the target thruster according to the effectiveness decay factor.

6. The method for thrust allocation control of an underwater thruster based on QP optimization according to any one of claims 1 to 5, characterized in that, The real-time estimation of the total disturbance acting on the underwater robot based on the active disturbance rejection control principle includes: Combining the internal unmodeled dynamics and external environmental disturbances suffered by the underwater robot as the total disturbance, and defining the total disturbance as the extended state variable; According to the extended state variable, the attitude control instruction, and the actual attitude information, run a predefined extended state observer to perform real-time online estimation of the extended state variable to obtain an estimated value of the total disturbance.

7. The method for thrust allocation control of an underwater thruster based on QP optimization according to any one of claims 1 to 5, characterized in that, The method further includes: Using the historical thrust command sequence of the underwater robot, the actual motion response sequence feedback by the sensor, and the dynamic model, iteratively calibrate and update the control effectiveness matrix through an online parameter identification algorithm.

8. A system for thrust allocation control of an underwater thruster based on QP optimization, characterized in that, The system includes: An acquisition unit for acquiring the preset attitude control instruction of the underwater robot and the actual attitude information currently feedback by the sensor, estimating the total disturbance acting on the underwater robot in real time based on the active disturbance rejection control principle, and calculating the desired total control force and total control torque instructions; A construction unit for constructing a quadratic programming problem for multiple thrusters configured on the underwater robot based on the total control force and the total control torque instructions. The objective function of the quadratic programming problem is used to minimize the total energy consumption of the multiple thrusters. The constraint conditions of the quadratic programming problem at least include the balance constraint defined based on the control effectiveness matrix and the physical limit constraint of the thrust of each thruster; A first solution unit for solving the quadratic programming problem to obtain the optimized first thrust instruction of each thruster and outputting the first thrust instruction to the actuator of each thruster; A diagnosis unit for diagnosing in real time whether there is a thruster fault, and dynamically adjusting the control effectiveness matrix or the physical limit constraint in the quadratic programming problem when a fault is detected; A second solution unit for re-solving based on the adjusted quadratic programming problem to obtain the second thrust instruction of each thruster under fault tolerance and outputting the second thrust instruction to replace the first thrust instruction to the actuator of each thruster.

9. An apparatus for thrust allocation control of an underwater thruster based on QP optimization, characterized in that, The device includes: A processor, a memory, an input / output unit, and a bus; The processor is connected to the memory, the input / output unit, and the bus; The memory stores a program, and the processor calls the program to execute the method for underwater thruster thrust allocation control based on QP optimization as described in any one of claims 1 to 7.

10. A computer-readable storage medium, characterized in that, A program is stored on the computer-readable storage medium, and when the program is executed on a computer, it executes the method for underwater thruster thrust allocation control based on QP optimization as described in any one of claims 1 to 7.

Images