A self-disturbance thrust control method of a net cleaning robot

Abstract

This invention belongs to the field of intelligent control technology for underwater robots, specifically disclosing a proactive disturbance rejection thrust control method for a net-cleaning robot. Addressing the issues of low inertia and weak anti-interference capability in lightweight cleaning robots, and the problems of response lag, attitude instability, and high-frequency tremors inherent in traditional control methods, this method first establishes a "lumped-total disturbance" dynamic model coupling current disturbances, water jet reaction forces, and nonlinear contact forces of the flexible net. Then, it constructs an extended state observer incorporating adaptive dynamic gain and input delay compensation to achieve advanced online estimation of the motion state and total disturbances. Next, it uses a fractional-order super-helical sliding mode control law for feedforward cancellation to calculate the desired spatial control torque. Finally, it constructs a quadratic programming energy consumption optimization model considering thrust saturation and motors to achieve optimal thrust allocation. This invention transforms passive feedback into active disturbance rejection, significantly improving the stability and control accuracy of robot operations.

Description

TECHNICAL FIELD

[0001] The application belongs to the technical field of intelligent control of underwater robots, and particularly relates to a self-disturbance rejection thrust control method of a net cleaning robot. BACKGROUND

[0002] With the increasing attention of the country to the high-quality development of marine economy, the fishing nets and net cages in the deep sea and the near shore have become an important part of the construction of the "blue granary" in China. However, the breeding nets are immersed in seawater for a long time, and are prone to attach algae, shellfish and barnacles and other fouling organisms, resulting in clogging of the net holes and poor water flow, which seriously affects the survival rate and growth rate of the breeding organisms. Therefore, regular cleaning of the nets has become a necessity for the operation and maintenance of the marine ranch.

[0003] Traditional net cleaning mainly relies on manual diving operations or large and heavy underwater cleaning equipment. Manual cleaning has the problems of high safety risk and low efficiency; and the existing heavy underwater cleaning robots are often large in size and high in cost, and need to be deployed with the help of a large crane, which cannot meet the needs of small and medium-sized fish farmers in the near shore with a large area and a large number of breeding nets. Therefore, a lightweight underwater net cleaning robot with the advantages of easy deployment and low cost has become an important trend in the current industry.

[0004] However, the lightweight design not only reduces the difficulty of launching and recovering the equipment, but also significantly reduces the mass and moment of inertia of the robot in water, and the anti-interference ability is significantly reduced. In actual operation, the motion control of the lightweight net cleaning robot faces extremely complex and strongly coupled disturbances, mainly in the following three aspects: first, the external environmental disturbance is strong. There are complex and variable ocean currents in the marine environment, and the lightweight robot is prone to six-degree-of-freedom pose drift under the impact of strong ocean currents, resulting in deviation of the cleaning trajectory from the predetermined route. Second, the internal operation coupling disturbance is large. When the robot performs cleaning operation, the high-pressure water jet will generate a large transient reaction torque; at the same time, the breeding nets are mostly flexible materials, which sway with the water flow, and the robot will generate nonlinear ejection force and friction force when it contacts with the nets. Third, the traditional control strategy has a lag. The current attitude and thrust control of underwater robots mostly uses traditional PID error feedback control or thrust distribution algorithm based on state feedback. This kind of passive feedback control algorithm depends on the actual deviation of the pose or contact force, and then calculates and outputs the compensation thrust. When facing the above-mentioned sudden ocean current impact or transient impact caused by the start and stop of the cleaning disc, the traditional algorithm has a serious lag in response, which is prone to cause high-frequency collision and vibration (chattering) between the robot and the flexible net, and even cause the robot to lose stability and overturn, which not only reduces the cleaning efficiency, but also greatly increases the risk of scratching the net.

[0005] Therefore, existing passive feedback control methods are insufficient to meet the high-precision and stable control requirements of lightweight mesh cleaning robots in complex sea conditions and flexible contact environments. The industry urgently needs a control method that can proactively sense and compensate for multi-source coupled disturbances to ensure the safe and efficient operation of lightweight equipment in highly disturbed environments. Summary of the Invention

[0006] The technical problems to be solved: Addressing the weaknesses of existing lightweight underwater cleaning robots in complex sea conditions, the response lag in traditional passive error feedback control, and the inability to cope with strong ocean currents, attitude instability, and high-frequency oscillations (tremors) caused by the strong coupling between the reaction force of high-pressure water jets and the nonlinear contact force of flexible mesh, this invention provides a self-disturbance rejection thrust control method for mesh cleaning robots. This method significantly improves the operational stability and control accuracy of lightweight equipment in flexible contact environments by treating multi-source complex disturbances as a unified "lumped disturbance" for online estimation and advance compensation.

[0007] Technical Solution: To achieve the above objectives, this invention provides a self-disturbance-resistant thrust control method for a mesh cleaning robot, characterized by comprising the following steps:

[0008] Step 1: Modeling and Integrating Disturbances. Obtain the robot's current and desired position and orientation, and combine all external forces such as ocean currents, water jet reaction forces, and netting friction into a "total disturbance." Establish a nonlinear model based on the robot's own mass.

[0009] Step 2: Real-time Estimation of Disturbance (ESO). Construct an Extended State Observer (ESO), input the current pose information, and estimate the robot's motion state and the total disturbance value in real time online.

[0010] Step 3: Calculate the required control torque. Compare the desired pose with the estimated state, and calculate the initial control input using nonlinear feedback. Introduce the disturbance estimate from Step 2 for feedforward cancellation to obtain the total control torque required to maintain stability.

[0011] Step 4: Optimize thrust distribution. Based on the thruster layout, establish an optimization model with the goal of minimizing error and energy consumption. Under physical constraints such as thrust magnitude and rate of change, use an algorithm to calculate the optimal thrust command for each thruster.

[0012] Step 5: Issue and execute cyclically. Send the optimal command to each thruster drive module, outputting the corresponding thrust to complete disturbance rejection control. Then enter the next cycle and repeat Step 1.

[0013] Beneficial effects: Compared with the prior art, the present invention has the following beneficial effects:

[0014] Transforming "passive feedback" into "active disturbance rejection" eliminates response lag: Traditional PID or quadratic programming methods require thrust compensation only after the robot's attitude deviates. This invention introduces an Extended State Observer (ESO) to package unmodeled internal dynamics such as complex ocean currents and jet reaction forces, along with external disturbances, into a "lumped disturbance" for real-time observation and proactive compensation. Compared to traditional PID control, attitude deviation is reduced by approximately 78.2%, and stable convergence speed is increased by 4.0 times. Thrust adjustment is completed before disturbances cause severe attitude deviations, completely solving the problem of lightweight robots being prone to tremors and instability when operating on flexible mesh.

[0015] Reduced model dependence and extremely robust: This invention does not require pre-establishing extremely accurate ocean current hydrodynamic models and flexible net deformation models (which is almost impossible to achieve in real marine environments). Instead, it treats these strongly coupled nonlinear terms as lumped disturbances for estimation. This greatly reduces the control algorithm's dependence on precise mathematical models and improves the robot's adaptability to different sea conditions and nets with varying degrees of fouling.

[0016] Balancing disturbance rejection and energy consumption to extend operating time: After calculating the desired torque required for disturbance rejection compensation, this invention further combines the thruster layout matrix with physical constraints to dynamically distribute thrust. While ensuring the robot's posture stability against disturbances, it effectively avoids excessive or abrupt changes in thrust output from the thrusters, reducing energy loss and mechanical wear in the propulsion system, thus meeting the practical needs of low-power operation for lightweight equipment. Attached Figure Description

[0017] Figure 1 This is a flowchart of an active disturbance rejection control thrust algorithm in one embodiment of the present invention.

[0018] Figure 2 This is a three-view drawing of an embodiment of the present invention, showing the specific locations of the various thrusters.

[0019] Figure 3 This is an overall view of one embodiment of the present invention, which allows a direct view of the specific positions of each thruster and shows that they are symmetrically distributed.

[0020] Figure 4 The diagram shows the coordinate system and force / torque decomposition of the thruster of the present invention, illustrating how different thrusters control the direction of motion (forward, backward, translation) and attitude adjustment (yaw, pitch, roll) of the equipment when they are working.

[0021] Figure 5 The graph shows a comparison of the attitude error time curves between the traditional PID control and the ADRC+QP control of this invention. The intuitive simulation data demonstrates that the ADRC+QP control algorithm of this invention is superior.

[0022] Figure 6 This is a comparison chart of high-frequency tremor quantification during the net-attaching operation phase of the present invention. It shows that during the net-attaching operation phase of the underwater robot, the "ADRC+QP" algorithm of the present invention can significantly suppress the tremor phenomenon of the controller compared with the "traditional PID", making the actuator run more smoothly. Detailed Implementation

[0023] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be further described in detail below with reference to the accompanying drawings and specific embodiments. In the following description, the same numbers and letters in different drawings represent the same or similar elements. The singular forms "an embodiment," "described," and "the formula" used in this invention and the appended claims are also intended to include the plural forms. It should also be understood that the term "and / or" as used herein refers to and includes any or all possible combinations of one or more associated listed items. It should be understood that the described embodiments are only some, not all, embodiments of this invention, and the specific embodiments described herein are only for explaining the invention and are not intended to limit the scope of protection of this invention.

[0024] Example 1: This invention provides a self-disturbance rejection thrust control method for a mesh cleaning robot, characterized by the following steps:

[0025] Step 1: Establish a dynamic model of the mesh cleaning robot including lumped disturbances. Obtain the current pose and desired pose information of the mesh cleaning robot; couple the external ocean current disturbances, the transient reaction forces generated by the high-pressure water jet operation, and the nonlinear rebound forces and frictional forces generated by contact with the flexible mesh during the robot's operation, defining them as the lumped disturbances acting on the robot body; based on the lumped disturbances and the robot's own rotational inertia and mass, establish a nonlinear dynamic model.

[0026] Step 2: Construct an Extended State Observer (ESO) for perturbation estimation. Based on the nonlinear dynamic model, an Extended State Observer is designed to address the unmeasurable variables and lumped perturbations in the system state. Using the current pose information collected by sensors as input, the Extended State Observer performs real-time online estimation of the robot's motion state and the lumped perturbations, outputting state estimates and lumped perturbation estimates.

[0027] Step 3: Calculate the desired spatial control torque based on the active disturbance rejection mechanism. The difference between the desired pose information and the state estimate output by the extended state observer is calculated, and the initial feedback control quantity is obtained using a nonlinear error feedback control law (NLSEF). The lumped disturbance estimate is introduced into the control closed loop as a feedforward compensation term, which is used to compensate for the initial feedback control quantity, thus calculating the desired spatial control torque required for the net cleaning robot to maintain stable net-attaching operation.

[0028] Step 4: Dynamic thrust allocation considering physical constraints. Obtain the thruster layout matrix of the mesh cleaning robot. Construct a dynamic thrust allocation optimization model with the joint optimization objectives of minimizing thrust allocation error and minimizing the total energy consumption of the thruster system. Input the desired spatial control torque, thrust amplitude saturation constraints, and thrust change rate constraints of the thrusters as constraints into the dynamic thrust allocation optimization model. Solve the model using an optimization algorithm (such as quadratic programming) to obtain the optimal thrust allocation command for each thruster.

[0029] Step 5: Execute control commands. Send the optimal thrust allocation command to each underlying thruster drive module of the mesh cleaning robot, control the thrusters to output the corresponding thrust, complete the self-disturbance rejection control of the mesh cleaning robot, and return to step 1 in the next control cycle.

[0030] Example 2: This example is a further detailed description based on Example 1, and its working process is as follows: Figure 1 As shown.

[0031] Furthermore, the robot dynamics model described in step 1 of Example 1 refers to the nonlinear dynamics model of the lightweight mesh cleaning robot under complex sea conditions. A six-degree-of-freedom nonlinear dynamics model of the underwater robot, including lumped disturbances, is established. The system's state equations and kinematic transformation equations are as follows:

[0032]

[0033]

[0034] This is the nonlinear dynamic equations for underwater robots based on the Newton-Euler equations. It describes how various forces acting on the robot (including hydrodynamic forces, thrust, and disturbances) are converted into motion (acceleration and velocity). The inertia matrix includes the added mass. This is the Coriolis force matrix (Coriolis force and centripetal force matrix). Here is the fluid damping matrix. It is the restoring force vector (the force and torque generated by gravity and buoyancy). The pose vector (the robot's position and attitude angles in the geodetic coordinate system), and the velocity vector... Let be the linear and angular velocity vectors of the robot in its body coordinate system. The desired control torque vector generated for the thruster system, The thruster configuration layout matrix depends on the layout architecture of the eight thrusters.

[0035] Furthermore, according to the appendix Figure 2 The defined positive directions are as follows: the X-axis direction is the center pointing directly forward (i.e., towards P1, P2, P8 in the top view); the Y-axis direction is the center pointing directly to the right (i.e., towards P2, P4, P6 in the top view); and the positive Z-axis direction is vertically downward from the center. The thrust distribution matrix can then be obtained. It is The matrix is ​​used to control the thrust of the eight thrusters. Mapped to the robot's spatial control forces / torques in 6 degrees of freedom According to the appendix Figure 4 As can be seen from the thruster coordinate system and force / torque decomposition diagram, for each thruster The first three rows (forces) are the force vectors themselves, and the last three rows (torques) are obtained through cross product. The calculation shows that:

[0036]

[0037] according to Figure 2 The layout of the thrusters is determined by substituting the extracted parameters. =0.4, =0.4, =0.05, thrust distribution matrix It can be represented by the following symbol matrix:

[0038]

[0039] The Roll angle (roll around the X-axis) is mainly caused by the left-right differential movement of P1-P4; meanwhile, P7 and P8 are not on the same horizontal plane as the center of gravity. =0.05), which will also produce a weak roll coupling torque. Pitch (pitch about the Y-axis): mainly generated by the forward and backward differential of P1-P4; similarly, P5 and P6 will also produce a weak pitch coupling torque. Yaw (yaw about the Z-axis): generated by the left and right differential of horizontal thrusters P5 and P6, or the forward and backward differential of P7 and P8.

[0040] Furthermore, the nonlinear dynamic model includes a fluid damping matrix. Considering the highly nonlinear damping experienced by lightweight robots during underwater movement, the fluid damping matrix... This can be expanded to be a superposition of linear damping and nonlinear (quadratic) damping:

[0041]

[0042]

[0043]

[0044] in, It is a linear damping coefficient matrix, which mainly plays a dominant role when the robot is crawling at low speed; , Equations represent the linear hydrodynamic derivatives for each degree of freedom. It is a second-order nonlinear damping coefficient matrix. Since underwater cleaning robots often need to resist strong ocean currents and jet reaction forces, their relative water flow velocity is relatively high, and the second-order damping cannot be ignored. The values ​​represent the second hydrodynamic derivatives for each degree of freedom. The dynamic modeling steps provide the complete structure of the damping matrix, highlighting the rigor of the nonlinear dynamic model and also demonstrating why traditional linear control methods fail.

[0045] Furthermore, the nonlinear dynamic model includes lumped perturbations. ,in .in For ocean current disturbance torque, The transient reaction torque generated by the high-pressure water flow The nonlinear force and torque generated by the contact of the flexible mesh clothing The model does not model dynamics. The total lumped disturbance... It is used to bear and centrally characterize the sum of unmodeled dynamics such as external ocean current disturbances, high-pressure water jet reaction forces, and nonlinear rebound torques generated by contact with flexible mesh.

[0046] Furthermore, for the aforementioned nonlinear dynamic model, in order to facilitate the design of the Active Disturbance Rejection Controller (ADRC), the nonlinear dynamic model can also be transformed into a second-order affine nonlinear system, and the system state equation can be abstractly represented as:

[0047]

[0048] This formula is a simplified representation of the state equation. Defined as the "lumped total disturbance" of the system, it incorporates the unknown nonlinear couplings within the model ( All external disturbances (ocean currents, jets, netting forces) are packaged into a unified unknown function. It is the system control gain constant. For the desired three-dimensional control torque (i.e. (equivalent value).

[0049] Furthermore, regarding the lumped disturbance... Including the transient reaction force and torque generated by high-pressure water flow To address the reaction force generated by the high-pressure water jet, a known feedforward model is constructed:

[0050]

[0051]

[0052]

[0053] in It is the density of seawater. It is the cross-sectional area of ​​the cleaning nozzle. It is the high-pressure water flow rate output by the water pump. Jacobian transformation matrix from the point of jet application to the robot's center of mass. and These are the current horizontal deflection angle and pitch angle of the high-pressure water jet gun, respectively. Using these variables, the magnitude of the water jet thrust generated at the nozzle can be calculated. The calculated Introducing the system as a known feedforward term strips away the drastic jumps in the total disturbance, significantly reducing the bandwidth pressure on the observer.

[0054] Furthermore, regarding the lumped disturbance... Including nonlinear forces and torques generated by contact with flexible mesh. Considering the flexible material properties of the mesh, it can be equivalent to a "nonlinear variable stiffness damping" model:

[0055]

[0056] in and The deformation of the mesh exerted by the robot in the normal direction and its rate of change are given. The nonlinear stiffness coefficient varies with deformation (the deeper the compression, the tighter the mesh, and the stiffness increases exponentially). This is the damping coefficient of the netting and the surrounding water. This formula is used as the lumped disturbance of the system. The physical boundary conditions indicate that the ESO used in this invention is not blindly observed, but designed for these specific high-frequency nonlinear physical quantities, and has a very strong specificity.

[0057] In one embodiment, the construction of the extended state observer (ESO) is used for online estimation of the aforementioned "lumped disturbance." This is for robot attitude angle control (in terms of pitch angle). (For example), Expanding into a new state of the system This is the theoretical basis of Active Disturbance Rejection Control (ADRC). It transforms a second-order system (position and velocity) by incorporating "lumped disturbances" into its control. "Consider it as a new unknown state (expanded state)" Forcibly upgraded to a third-order system:

[0058] Design of Extended State Observer (ESO):

[0059]

[0060] Furthermore, the extended state observer is further designed as a linear extended state observer (ESO):

[0061]

[0062] in and For attitude and angular velocity, The control gain constant, For control input, i.e., the control force / torque that the algorithm expects to output (as mentioned above) (They are equivalent). In the formula described above... It is the deviation between the observer's estimated value and the sensor's actual measurement value. It is the actual measurement value of the sensor. , , These are stater pairs , , State estimate, These are observer gain (bandwidth) parameters, which determine how quickly the observer tracks the real data. Larger values ​​result in faster tracking but also greater sensitivity to noise. Here... This refers to the "total disturbance". The real-time estimate. This is achieved by adjusting the observer bandwidth. , , Preferably, =400, =4000, =24000, making It can sense the impact of ocean currents and jets in advance.

[0063] Furthermore, the linear extended state observer (ESO) is prone to peak overshoot when faced with a sudden "step-type" strong impact such as the sudden start and stop of a high-pressure water jet. Therefore, the traditional linear extended state observer is optimized into a continuously differentiable nonlinear extended state observer (NLESO). A smooth, nonlinear continuous function is defined below:

[0064]

[0065] Expand the "collective perturbation" into a new state variable. By constructing a discretized NLESO, the state update equation is obtained:

[0066]

[0067] in, This refers to the actual pose state measured by the sensor (such as IMU) at the current moment. and This represents the observer's tracking estimate of the robot's actual pose and velocity. The most critical variable is the real-time online estimate of the "total lumped disturbance" by the observer. It is the discrete sampling step size of the control system. For nonlinear exponential parameters (usually taken as...) =0.5, =0.25, =0.125), As the limit of the linear region width, preferred =0.02. (Use) The function replaces the conventional linear coefficients, which makes the gain of the observer decrease when the error is large (to prevent the system from diverging due to the impact of the water jet) and increase when the error is small (to eliminate steady-state chatter caused by the flexible netting), which greatly improves the robustness of the lightweight robot and the stability of the system.

[0068] Furthermore, for the extended state observer (NLESO) of the continuously differentiable nonlinear function, dynamic high gain and input delay compensation are introduced to reconstruct the original NLESO formula, resolving the contradiction between signal transmission delay and error convergence speed in lightweight robots. The adaptive dynamic gain update law is as follows:

[0069]

[0070] in, It is a dynamic gain adjustment factor calculated in real time. , It is the constant magnitude of the base gain and the variable gain. The decay rate coefficient, It is the absolute value of the observer's estimation error of the pose. When the error is large, Approaching (To prevent system divergence); when the error is small, It increases rapidly (improving steady-state accuracy).

[0071] Furthermore, based on the adaptive dynamic gain update law, a variable gain ESO observer equation with delay compensation is constructed:

[0072]

[0073] in, It is an input delay compensation item. A new dynamic gain adjustment factor has been added to address the system dead time delay of robot umbilical cable transmission and motor response. It can effectively solve the problem of control command lag caused by excessively long cables during deep-water operations, which is something that ordinary ESOs do not have.

[0074] Example 3: This example is based on Example 1 and Example 2 and is described in further detail.

[0075] In one embodiment, the net cleaning robot is a lightweight underwater cleaning robot. This inevitably leads to the problem of "high-frequency oscillation" in lightweight robots on flexible nets, and traditional PID control is prone to failure due to integral term saturation. To address this issue, a super-twisting sliding mode is introduced to replace the traditional PID controller in generating the initial control input. The pose tracking error is defined as... ( , To determine the desired pose and velocity, a fractional-order superhelical sliding mode control law (FO-STSM) is constructed, which greatly enhances the robot's transient response capability when facing ocean currents (i.e., "current resistance" is faster).

[0076]

[0077] Superspiral sliding mode control law (combination of disturbance rejection compensation and fractional-order control law)

[0078]

[0079]

[0080] in, It is a designed sliding surface. and Design positive constants for the sliding surface, and optimize accordingly. =8, =5, It is a fractional calculus operator. The order is fractional (usually 0 < 1). <1), Introducing a fractional operator is equivalent to adding a "historical memory" effect, which makes the system more predictive of the rebound of the flexible netting. For active disturbance rejection control and disturbance compensation, real-time feedforward offsetting is performed on the system disturbance based on the estimated value, thereby transforming the nonlinear disturbed system into a linear decoupled system. This is achieved using ESO observations. The lumped disturbance estimate is directly fed forward to compensate for the disturbance in the control law, thereby simplifying the complex nonlinear disturbed system into a linear system with integrator series. It is the final desired control torque (including the desired torque for error feedback and disturbance compensation). The initial feedback control torque before compensation is calculated by the superhelical sliding mode algorithm. The superhelical sliding mode control law... , It is a superspiral control gain, preferably. =12, =8. The introduction of this term enables continuous control signals. The exponent of the superspiral reaching law is preferred. =0.5 (0 < <1), which can effectively eliminate the "chattering" phenomenon of traditional sliding mode control, and the added For equivalent control compensation, it can be canceled out by the terms generated by differentiating fractional sliding surfaces.

[0081] Furthermore, the superspiral sliding mode control law includes fractional-order calculus operators. The fractional-order calculus operator Using the Caputo definition of the fractional derivative, its form is as follows, for continuously differentiable functions... ,That order derivative It can be defined as:

[0082]

[0083] Where, it is known that 0 < <1 (preferred) =0.75), take the integer m=1, It is the Gamma integral function. It is a function of The conventional derivative of order 1, the formula described therein, can accurately define how "historical memory" is traversed along the timeline. The above is obtained through the integral kernel function It is a weighted average of past state errors.

[0084] Example 4: This example is a detailed explanation of step 4 based on Examples 1, 2, and 3.

[0085] Underwater robots typically consume a large amount of electricity during operation, thus requiring special optimization of energy consumption. A quadratic programming (QP) energy consumption optimization model was constructed for this purpose:

[0086]

[0087]

[0088] Constraints:

[0089]

[0090] Furthermore, the aforementioned super-helical sliding mode control law has yielded the desired control torque required by the robot as a whole. This force is then further allocated to specific thrusters, down to the exact amount of force (in N) applied to each thruster. Considering the limited battery power of lightweight devices, this invention constructs a quadratic programming (QP) energy consumption optimization model. The formula is a thrust allocation objective function with thruster rate-of-change constraints. This formula aims to distribute this total torque across the robot's eight thrusters, minimizing the total thruster energy consumption while ensuring correct posture. It minimizes the objective function. This represents the thrust command vector for each thruster. It is a positive definite diagonal weight matrix, and at the same time The matrix is ​​extremely important; it penalizes drastic changes in thrust. This is the thruster energy consumption weight matrix, a diagonal matrix used to measure the power consumption weight of different thrusters. It is a vector of slack variables, representing the small errors allowed in thrust distribution, to prevent the equations from becoming unsolvable due to overly rigid constraints. It is the error penalty weight matrix, used to restrict slack variables. Don't make it too big.

[0091] Furthermore, for the aforementioned quadratic programming (QP) energy consumption optimization model, it incorporates... Its constraints are specifically designed to accommodate the characteristics of the miniature thruster motors in lightweight underwater robots. When lightweight robots are subjected to high-pressure water jets, a sudden and drastic change in motor speed can lead to excessively high current peaks that burn out the circuitry or cause mechanical resonance. This design protects the robot's "hybrid power supply system" hardware, effectively extending the lifespan of the motors and batteries.

[0092] Furthermore, for the aforementioned quadratic programming (QP) energy consumption optimization model, where It is basic. Thrust rate of change penalty matrix , Corresponding to the objective function It is used to punish drastic changes in thrust, with the aim of protecting the motor and preventing high-frequency chatter. Adaptive stability penalty term ( (The identity matrix). When the robot is operating stably, At this point, it is mainly affected by the fundamental matrix. Constraints, when the robot experiences high-frequency flutter while operating on a flexible network ( (significantly increased), matrix As the overall size increases, the algorithm will forcefully limit or "lock down" or "smooth out" thrust changes in the thrusters. (That is, limiting drastic changes in thruster force), thereby sacrificing extremely small instantaneous tracking accuracy in exchange for absolute anti-shake and motor protection for the lightweight underwater robot.

[0093] Furthermore, the aforementioned quadratic programming (QP) energy consumption optimization model includes... In one embodiment, step 2, the sensor acquires current pose information, and the measured data fluctuates in real time. To determine the authenticity and reliability of the data, a real-time attitude fluctuation evaluation index based on data analysis is introduced (introducing stability constraints):

[0094]

[0095]

[0096] in, This is a real-time attitude fluctuation assessment index (the larger the value, the more the robot is shaking). It is a sliding integral time window (e.g., taking data from the nearest 0.5 seconds). It is the real-time angular acceleration measured by the IMU. It is the acceleration fluctuation coefficient extracted by data analysis. High-frequency tremor is defined as a non-zero mean high-frequency pulsation of angular acceleration, which can be characterized in real time by the standard deviation (or variance) of the signal. This represents the total number of discrete data points acquired by the IMU within the sliding integration time window (e.g., 0.5 seconds). For the first The actual angular acceleration values ​​at each sampling time, as mentioned above , representing the average angular acceleration within that time window. This calculation clarifies the specific calculation method for the "acceleration fluctuation coefficient extracted from data analysis," making the triggering mechanism of the adaptive stability penalty term in the dynamic thrust allocation model clearer.

[0097] Furthermore, for the aforementioned quadratic programming (QP) energy consumption optimization model, yes The energy consumption weight matrix, , corresponding to the objective function The term is used to penalize the thrust amplitude of the thrusters, with the aim of reducing the total energy consumption of the system. Since the eight thrusters have the same model and power, they can be represented as an identity matrix multiplied by a constant coefficient, i.e. During operation, certain thrusters (such as those responsible for cleaning the surface above and below) are more prone to overheating due to overload. Their respective weights can be increased individually. This forces the optimization algorithm to use the thruster as little as possible during allocation, thus balancing the load.

[0098] Furthermore, for the aforementioned quadratic programming (QP) energy consumption optimization model, the relaxation penalty matrix... It is A positive definite diagonal matrix, This term corresponds to the objective function. Under constraints In some cases, forcibly requiring equality and satisfying both the upper and lower limits of motor thrust can sometimes lead to no solution. Introducing slack variables can help. It can be guaranteed that the quadratic programming algorithm always has a solution, and The matrix is ​​used to penalize this error. Among them... ... Error penalty weights are assigned to the six degrees of freedom: forward, lateral, diving, rolling, pitch, and yaw. This ensures the robot can accurately track the desired torque calculated by the active disturbance rejection controller. Therefore, slack variables must be as small as possible. The element values ​​in the matrix must be much larger than and The element values ​​in the matrix (usually differing) arrive (Order of magnitude). Specifically, in lightweight mesh washing operations, to prevent capsizing, "rolling" is used. ")" and "pitch up ( The penalty weight for ")" should usually be set to the highest, meaning that it is better to sacrifice some forward or downward thrust in order to ensure horizontal stability of the attitude.

[0099] Preferably, considering the total power threshold limitation of the power supply system for lightweight robots, a real-time global power saturation constraint is introduced into the constraints of the quadratic programming (QP) optimization model:

[0100]

[0101]

[0102] in For the first Each thruster outputs thrust The instantaneous electrical power required at that time The thrust-power index is defined as follows: [Value range to be filled in] The preferred value is 1.5; Let be the thrust-power conversion constant of the thruster. This represents the maximum total power that the battery management system (BMS) is allowed to output at the current moment. To mitigate the rigid power consumption of high-pressure cleaning pumps and other electronic equipment, the aforementioned constraints ensure that the thrust demand calculated by the underlying control commands under extreme disturbances (such as simultaneous ocean current and jet reaction forces) will not cause a sudden drop in bus voltage or a system power outage and restart, thus guaranteeing the survivability of lightweight equipment in complex sea conditions.

[0103] Furthermore, according to the appendix Figure 5 A comparative experiment was conducted on the proposed solution and the traditional PID control solution using the MATLAB / Simulink simulation platform. The simulation object was a mass m = 8 kg with a rotational inertia. A lightweight mesh cleaning robot with a weight of 0.32 kg·m² and a sampling frequency of 100 Hz. (Attached) Figure 6 Two control methods were derived from the planetary time flow chart of the roll angle under the combined disturbance condition. At t=2s, a 0.8 m / s lateral current impact was applied; at t=5s, a high-pressure jet (Q=18 L / min) was initiated. Figure 5 It can be seen that the maximum roll angle of traditional PID control under ocean current impact reaches 8.34°, and the settling time is 3.6s; while the maximum roll angle of the ADRC+QP control method of this invention only reaches 1.82°, the settling time is reduced to 0.9s, the maximum attitude angle decay rate reaches 78.2%, and the stable convergence speed is improved by 4.0 times, which fully verifies the strong robustness of this invention to ocean current disturbance and jet impact.

[0104] Furthermore, according to Figure 6 The results show a comparison of the tremor solutions for two control methods during the stable operation phase of net application (t=8s~16s), including normalized bar charts of various indicators and time-domain waveforms of roll angle acceleration. Figure 6 As can be seen from the bar chart, the ADRC+QP control method of this invention achieves good results in terms of the cylindrical root mean square value (RMS) of roll angle and attitude rotation evaluation index. In terms of the four indicators—mean, shudder frequency, and thrust fluctuation standard deviation—the performance was reduced by 84.2%, 84.6%, 83.9%, and 78.1% respectively compared to traditional PID control. Figure 6The time-domain waveform shows that under traditional PID control, the roll angle acceleration exhibits obvious high-frequency oscillations (RMS=2.41 rad / s², with a dominant frequency of approximately 11.2 Hz). However, the method of this invention utilizes an adaptive attitude fluctuation evaluation mechanism. By dynamically adjusting the QP weight matrix R, the high-frequency fluctuation amplitude was suppressed to RMS=0.38rad / s², reducing the high-frequency fluctuation amplitude by 84.2% and significantly improving the stability of the robot's net-laying operation.

[0105] In summary, this invention belongs to the field of intelligent control technology for underwater robots. Addressing the challenges of lightweight cleaning robots in complex sea conditions—such as low mass and moment of inertia, weak anti-interference capabilities, and the lag, attitude instability, and high-frequency oscillations inherent in traditional passive feedback control algorithms like PID—this invention proposes an active disturbance rejection control scheme. This method treats complex disturbances such as external ocean currents, high-pressure water jet reaction forces, and nonlinear contact forces of flexible netting as a unified "lumped disturbance" for online estimation and proactive compensation, transforming passive control into active control and significantly improving the control accuracy and operational stability of the equipment under strong disturbance environments.

[0106] The lumped perturbation described in one embodiment The term "optimization algorithm" should be interpreted in the broadest sense. It encompasses not only external ocean current disturbances, transient reaction forces from high-pressure water jets, and nonlinear forces from flexible netting contact, as explicitly listed in the specification, but also any internal nonlinear strongly coupled terms not precisely modeled in the dynamic model, model parameter uncertainties, sensor measurement noise, mechanical vibration, signal transmission and actuator dead-zone delays, and other unknown environmental dynamic disturbances present in actual underwater operations. While the energy consumption optimization algorithm described herein uses a quadratic programming algorithm as an example for thrust allocation in a preferred embodiment, its essence lies in achieving optimal thrust allocation under multiple objectives and constraints. Those skilled in the art should understand that any mathematical method or intelligent control algorithm capable of achieving similar constraint solutions, including but not limited to sequential quadratic programming (SQP), interior-point methods, genetic algorithms, particle swarm optimization algorithms, and artificial neural networks, should be considered equivalent replacements for the "optimization algorithm" of this invention. The thruster configuration layout and matrix values ​​mentioned in this invention are merely examples of preferred schemes; any technical solution based on the same control mechanism and using changes in the number or spatial distribution of thrusters for equivalent replacement falls within the scope of protection of this invention. The specific mathematical function form and specific parameter values ​​described in the embodiments (such as...) , , , Values, fractional order Superspiral exponent Other variable gain functions with smooth, continuous, and nonlinear saturation characteristics may be used, or equivalent replacements such as integer-order conventional control laws or traditional sliding surfaces may be used, or control parameters may be adjusted or replaced, in order to highlight the rigor and optimal control effect of the present invention. However, if they do not deviate from the core idea of ​​self-disturbance rejection and active cancellation of the present invention, they are all included within the protection scope of the present invention.

[0107] The above description is merely a preferred embodiment of a self-disturbance rejection thrust control method for a mesh cleaning robot. The scope of protection for this method is not limited to the above embodiments. This method is also applicable to similar lightweight underwater operation scenarios such as underwater inspection robots and underwater welding robots. All technical solutions falling under this concept are within the scope of protection of this invention. It should be noted that for those skilled in the art, any improvements and variations made without departing from the principles of this invention should also be considered within the scope of protection of this invention.

Claims

1. A method for self-disturbance-resistant thrust control of a mesh cleaning robot, characterized in that, Includes the following steps: Step 1: Establish a dynamic model: Obtain the current pose information and desired pose information of the mesh cleaning robot; define the multi-source disturbances experienced by the robot during operation as the total lumped disturbance acting on the robot body; based on the total lumped disturbance, the robot's own rotational inertia and mass, establish a nonlinear dynamic model; Step 2: Construct an extended state observer for perturbation estimation: Design an extended state observer based on the nonlinear dynamic model; using the current pose information collected by the sensor as input, perform real-time online estimation of the robot's real-time motion state and the total lumped perturbation through the extended state observer, and output the state estimate and the lumped perturbation estimate. Step 3: Calculate the desired spatial control torque: Subtract the desired pose information from the state estimate and use the feedback control law to calculate the initial feedback control quantity; introduce the lumped disturbance estimate as a feedforward compensation term into the control closed loop to compensate for the initial feedback control quantity, and calculate the desired spatial control torque required for the net cleaning robot to maintain stable net-attaching operation. Step 4: Dynamic thrust allocation: Obtain the thruster layout matrix of the mesh cleaning robot, and construct a dynamic thrust allocation optimization model with the joint optimization objectives of minimizing thrust allocation error and minimizing the total energy consumption of the thruster system; input the desired spatial control torque and the physical constraints of the thrusters into the dynamic thrust allocation optimization model, and solve it using an optimization algorithm to obtain the optimal thrust allocation command for each thruster; Step 5: Send out the execution command: Send the optimal thrust allocation command to each underlying thruster drive module, control the thrusters to output the corresponding thrust, complete the self-disturbance rejection control of the mesh cleaning robot, and return to step 1 in the next control cycle.

2. The self-disturbance rejection thrust control method for a mesh cleaning robot according to claim 1, characterized in that, The total disturbances mentioned in step 1 include external ocean current disturbances, transient reaction forces generated by high-pressure water jet operations, nonlinear rebound forces and frictional forces generated by contact with the flexible netting, and unmodeled dynamics within the system.

3. The self-disturbance rejection thrust control method for a mesh cleaning robot according to claim 1, characterized in that, The nonlinear dynamic model mentioned in step 1 is specifically a six-degree-of-freedom nonlinear dynamic model of an underwater robot, whose system state equations and kinematic transformation equations are expressed as follows: In the formula, The inertia matrix includes the added mass; The matrix represents the Coriolis force and the centripetal force. Here is the fluid damping matrix; This is the vector of the restoring forces generated by gravity and buoyancy. This is the pose vector in the geodetic coordinate system; This is the velocity vector in the body coordinate system; The desired control torque vector generated for the propulsion system; For the propulsion configuration layout matrix; This represents the total disturbance.

4. The self-disturbance rejection thrust control method for a mesh cleaning robot according to claim 3, characterized in that, The fluid damping matrix Expanded as a superposition of linear damping and quadratic nonlinear damping: In the formula, This is a linear damping coefficient matrix, which plays a dominant role when the robot moves at low speeds. This is a quadratic nonlinear damping coefficient matrix, used to characterize nonlinear damping under high relative water flow velocities.

5. The self-disturbance rejection thrust control method for the mesh cleaning robot according to claim 2, characterized in that, The lumped disturbance satisfy: In the formula, For ocean current disturbance torque; The transient reaction torque generated by the high-pressure water flow; Nonlinear force and torque generated by contact with the flexible mesh; The model does not model dynamics.

6. The self-disturbance rejection thrust control method for a mesh cleaning robot according to claim 2, characterized in that, A feedforward model is constructed to address the transient reaction force generated by high-pressure water jets: In the formula, The density of seawater, This is the cross-sectional area of ​​the cleaning nozzle. This refers to the high-pressure water flow rate output by the water pump. Let be the Jacobian transformation matrix from the point of impact of the jet to the robot's center of mass; To solve for the transient reaction torque generated by the high-pressure water flow, it is introduced into the system as a known feedforward term to remove the jump part in the total lumped disturbance.

7. The self-disturbance rejection thrust control method for a mesh cleaning robot according to claim 2, characterized in that, The nonlinear rebound force and friction force generated in contact with the flexible mesh are equivalent to a nonlinear variable stiffness damping model: In the formula, and The figures represent the compression deformation of the mesh by the robot in the normal direction and its rate of change, respectively. It is a nonlinear stiffness coefficient that varies with deformation and increases exponentially with the depth of compression; The damping coefficient of the netting and the surrounding water; This is the torque transformation matrix from the contact force to the robot's center of mass.

8. The self-disturbance-resistant thrust control method for a mesh cleaning robot according to claim 1, characterized in that, The extended state observer mentioned in step 2 is a continuously differentiable nonlinear function extended state observer (NLESO), which uses a smooth nonlinear continuous function. Instead of conventional linear coefficients, the The function is defined as: In the formula, The deviation between the observer's estimate and the actual measurement. For nonlinear exponential parameters, This represents the width limit of the linear region.

9. The self-disturbance rejection thrust control method for a mesh cleaning robot according to claim 8, characterized in that, The extended state observer of the continuously differentiable nonlinear function also incorporates an adaptive dynamic gain update law and input delay compensation. The reconstructed variable gain observer equation is as follows: In the formula, This represents the actual pose state measured by the sensor at the current moment. and These are the tracking estimates for the actual pose and velocity, respectively. This is a real-time online estimate of the "total ensemble perturbation"; This refers to the discrete sampling step size of the control system. , , For observer gain parameters; This is an approximate constant for the system control gain; For the input delay compensation term, where This is the system dead zone delay time; The adaptive dynamic gain adjustment factor is defined as: . in, and The constant magnitudes of the base gain and the variable gain, This is the decay rate coefficient.

10. The self-disturbance-resistant thrust control method for a mesh cleaning robot according to claim 1, characterized in that, The feedback control law mentioned in step 3 adopts the fractional-order superspiral sliding mode control law (FO-STSM), and the pose tracking error is defined as... The constructed fractional-order sliding surface is as follows: The desired spatial control torque required to maintain stable wire mesh application is calculated as follows: In the formula, and Design positive constants for the sliding surface; For fractional calculus operators, It is a fractional order; For the initial feedback control torque, , Gain is controlled by superhelix; For the exponential term of the superhelical approach law; This is an equivalent control compensation item; This is the estimated value of the lumped disturbance; This is the control gain constant.

11. The self-disturbance rejection thrust control method for a mesh cleaning robot according to claim 1, characterized in that, The dynamic thrust allocation optimization model described in step 4 is an energy consumption optimization model based on quadratic programming (QP), and its objective function and constraints are constructed as follows: In the formula, For each thruster, the thrust command vector is used. This is the thruster energy consumption weight matrix; This represents the change in thrust. A vector of slack variables; This is the error penalty weight matrix; For the propulsion configuration layout matrix; To control the torque in the desired space; The dynamically adjusted positive definite diagonal weight matrix is ​​defined as follows: in, The penalty matrix for the rate of change of basic thrust; This is an adaptive stability penalty term. This is a real-time attitude fluctuation assessment index calculated using the angular acceleration and acceleration fluctuation coefficient acquired by the IMU within a sliding time window.

12. The self-disturbance rejection thrust control method for a mesh cleaning robot according to claim 10, characterized in that, The constraints of the quadratic programming optimization model also include real-time global power saturation constraints: In the formula, For the first Each thruster outputs thrust The instantaneous electrical power required at that time; The thrust-power conversion constant of the propulsion unit; Thrust-power index; This represents the maximum total power that the battery management system is allowed to output at the current moment. This is for the rigid power consumption of high-pressure cleaning water pumps and other electronic equipment.

13. The self-disturbance rejection thrust control method for the mesh cleaning robot according to claim 1, characterized in that, The thruster layout matrix B is a 6×8 matrix used to map the thrust vectors of the eight thrusters into spatial control torques for the robot in six degrees of freedom. The eight thrusters are symmetrically distributed, including four horizontal thrusters, where P5 and P6 control the forward and backward directions, and P7 and P8 control the left and right directions, and four vertical thrusters, P1, P2, P3, and P4 control the up and down directions. Each element in the thrust distribution matrix B is determined by the installation position and orientation of each thruster in the robot's body coordinate system through the force-torque cross product relationship.

Images