An underwater robot motion active disturbance rejection control method, system, medium and device

Abstract

The present application relates to the technical field of robot control, and provides a kind of underwater robot motion active disturbance rejection control method, system, medium and equipment, comprising: based on motion state data and environmental disturbance data, the total disturbance of underwater robot is estimated by dynamic gain adaptation extended state observer, and disturbance compensation is generated according to total disturbance;Combining control target value and disturbance compensation, the control signal of each propeller is calculated by active disturbance rejection control law;Based on control signal, the action of each propeller of underwater robot is driven, and the posture adjustment and position control of underwater robot are realized;The working current of propeller and the flow velocity of environment are obtained, the current motion condition of robot is judged, and the observation gain of extended state observer and control law feedback coefficient are automatically adjusted.The response speed is improved while ensuring the stability of control, and different motion conditions are adapted.

Description

TECHNICAL FIELD

[0001] The present application belongs to the technical field of robot control, and particularly relates to a motion active disturbance rejection control method and system for underwater robots, a medium and equipment. BACKGROUND

[0002] The statements in this section merely provide background information related to the present application and do not necessarily constitute the prior art.

[0003] As important equipment for exploring the ocean and developing marine resources, the motion control performance of underwater robots directly determines the efficiency and accuracy of task execution. However, the underwater environment has strong disturbances (such as strong water flow disturbance, wave disturbance), nonlinearity, and parameter time-varying characteristics, which bring many challenges to the motion control of underwater robots.

[0004] Most existing underwater robot motion control methods use traditional proportional-integral-derivative (PID) control technology. Although this technology has the advantages of simple structure and easy implementation, its control accuracy decreases significantly when facing complex underwater disturbances and changes in robot parameters, making it difficult to meet the needs of high-precision tasks. Some advanced control techniques such as adaptive control and sliding mode control have been tried, but adaptive control is highly dependent on the model and lacks adaptability in highly uncertain underwater environments, while sliding mode control is prone to chattering, which can exacerbate thruster wear and shorten equipment life.

[0005] Currently, active disturbance rejection control technology has been gradually applied to underwater robot motion control due to its advantages of not relying on accurate mathematical models and strong disturbance suppression capabilities. However, related technical solutions are not yet mature and still have many problems to be solved.

[0006] On the one hand, the observation gain of the traditional extended state observer is mostly a fixed value, which cannot match the real-time changes in environmental disturbance intensity and thruster load, and is prone to disturbance observation response lag, making it difficult to accurately capture the total disturbance in scenarios such as sudden changes in water flow and thruster overload.

[0007] On the other hand, the adaptability of control parameters to underwater robot hardware characteristics (such as thruster output characteristics and sensor sampling frequency) is insufficient, often leading to a mismatch between control signals and thruster actual output, causing slow response speed, large overshoot, and other problems, ultimately making it difficult to achieve stable and reliable motion control in complex underwater environments. SUMMARY

[0008] To solve the technical problems in the background art, the present application provides an underwater robot motion active disturbance rejection control method, system, medium and equipment, which estimates and compensates total disturbance through an extended state observer, effectively improves the anti-interference ability of the robot in a complex underwater environment, and is provided with a working condition self-adaptive adjustment unit, which can automatically adjust control parameters according to the motion state and the working state of the propeller, improve the response speed while ensuring the control stability, and adapt to different motion working conditions.

[0009] To achieve the above-mentioned purpose, the present application adopts the following technical solutions:

[0010] The first aspect of the present application provides an underwater robot motion active disturbance rejection control method, which comprises:

[0011] Obtaining motion state data, environmental disturbance data and control target value of the underwater robot;

[0012] Based on the motion state data and the environmental disturbance data, estimating the total disturbance of the underwater robot through a dynamic gain adaptive extended state observer, generating a disturbance compensation amount according to the total disturbance, combining the control target value and the disturbance compensation amount, and calculating the control signal of each propeller through an active disturbance rejection control law;

[0013] Based on the control signal, driving each propeller of the underwater robot to act, so as to realize the posture adjustment and position control of the underwater robot;

[0014] Obtaining the working current of the propeller and the flow velocity of the environment, judging the current motion working condition of the robot, and automatically adjusting the observation gain of the extended state observer and the feedback coefficient of the control law.

[0015] Further, the adjustment formula of the control law feedback coefficient is: ; wherein, and is the control law feedback coefficient, and is the initial feedback coefficient of the control law, and the working condition level index , is the flow velocity at time k, is the maximum flow resistance of the underwater robot, and I(k) is the working current of the propeller at time k, is the rated current of the propeller, is the motion state error at time k, is the motion state error threshold.

[0016] Further, it further comprises: if a propeller fails, the control signals of the remaining propellers are redistributed through an optimization algorithm, and the objective function of the optimization algorithm is: ; wherein, u is the control signal of the remaining propeller,​ The thrust matrix after the fault. For the desired torque / force vector of the underwater robot, Let J be the disturbance compensation torque / force vector, and J be the Jacobian matrix of the underwater robot. To increase the total disturbance after the fault equivalent disturbance, λ is the smoothing weight coefficient, and u(k-1) is the control signal at the moment before the fault.

[0017] Furthermore, the fault is determined by the operating current of the thruster.

[0018] Furthermore, the extended state observer employs a dynamic observation gain:

[0019] ;

[0020] Where k is the control cycle number. The gain is the dynamic observation gain at time k. For the initial observation gain, This is the water flow disturbance adjustment coefficient. Let k be the water flow velocity. To maximize the robot's resistance to flow, This is the thruster current adjustment coefficient. Let k be the real-time operating current of the thruster. For the rated current of the thruster, Let k be the motion state error at time k. This is the motion state error threshold.

[0021] Furthermore, the motion state data includes attitude angle, angular velocity, acceleration, depth, and velocity.

[0022] Furthermore, the environmental disturbance data includes water flow velocity and wave parameters.

[0023] A second aspect of the present invention provides an underwater robot motion disturbance rejection control system, comprising:

[0024] The perception module is configured to acquire motion state data, environmental disturbance data, and control target values ​​of the underwater robot.

[0025] The extended state observation module is configured to: estimate the total disturbance of the underwater robot based on motion state data and environmental disturbance data through a dynamically gain-adapted extended state observer; generate a disturbance compensation amount based on the total disturbance; and calculate the control signals of each thruster by combining the control target value and the disturbance compensation amount through an active disturbance rejection control law.

[0026] The drive module is configured to drive the movements of each thruster of the underwater robot based on control signals, thereby realizing the attitude adjustment and position control of the underwater robot.

[0027] The working condition self-adaptive adjustment module is configured to: acquire the working current of the thruster and the water flow speed of the environment, judge the current motion working condition of the robot, and automatically adjust the observation gain of the extended state observer and the feedback coefficient of the control law.

[0028] A third aspect of the present application provides a computer readable storage medium, which stores a computer program, and the program is executed by a processor to implement the steps of the underwater robot motion active disturbance rejection control method.

[0029] A fourth aspect of the present application provides a computer device, which comprises a computer readable storage medium, a processor and a computer program stored on the computer readable storage medium and executable on the processor, and the processor executes the program to implement the steps of the underwater robot motion active disturbance rejection control method.

[0030] Compared with the prior art, the present application has the following beneficial effects:

[0031] The present application innovatively proposes an underwater robot motion active disturbance rejection control method, which estimates and compensates the total disturbance through an extended state observer, effectively improves the anti-interference ability of the robot in a complex underwater environment, and is provided with a working condition self-adaptive adjustment unit, which can automatically adjust the control parameters according to the motion state and the working state of the thruster, thereby improving the response speed while ensuring the control stability and adapting to different motion working conditions.

[0032] The present application innovatively proposes a fault-tolerant distribution and disturbance compensation cooperative mechanism, which has a thruster fault-tolerant function, can redistribute the control signals of the remaining thrusters when a thruster fails, ensures that the robot continues to complete the motion control, and improves the reliability of the robot. BRIEF DESCRIPTION OF DRAWINGS

[0033] The drawings constituting a part of the specification of the present application are used to provide a further understanding of the present application, and the illustrative embodiments of the present application and the description thereof are used to explain the present application, and do not constitute an improper limitation on the present application.

[0034] Figure 1 is a flowchart of an underwater robot motion active disturbance rejection control method of the first embodiment of the present application;

[0035] Figure 2 is a partial structure perspective view of the underwater robot of the first embodiment of the present application;

[0036] Figure 3 is a partial structure sectional view of the underwater robot of the first embodiment of the present application;

[0037] Figure 4 is a protective assembly structure schematic view of the first embodiment of the present application;

[0038] Figure 5 is a structural left view of the underwater robot of the embodiment one of the present application;

[0039] Figure 6 is a structural schematic view of a computer device of the embodiment four of the present application;

[0040] Wherein, 1, robot body, 2, propeller, 3, external protection plate, 4, side buffer spring, 5, rubber support pad, 6, anti-collision plate, 7, anti-collision pad, 8, upper buffer spring, 9, movable baffle, 10, top plate, 11, top protection plate, 12, through slot, 13, arc-shaped baffle. DETAILED DESCRIPTION

[0041] In order to make the purpose, technical scheme and advantages of the embodiments of the present application more clear, the technical scheme in the embodiments of the present application will be described clearly and completely below with reference to the drawings in the embodiments of the present application.

[0042] It should be pointed out that the following detailed description is exemplary and is intended to provide further explanation of the present application. Unless otherwise specified, all technical and scientific terms used herein have the same meaning as generally understood by those skilled in the art to which the present application belongs.

[0043] Embodiment one

[0044] The embodiment provides a motion active disturbance rejection control method for an underwater robot.

[0045] The motion active disturbance rejection control method for an underwater robot provided by the embodiment is suitable for an underwater robot, such as Figure 2 and Figure 3 As shown in the drawings, the underwater robot comprises a robot body 1, two symmetrical propellers 2 are installed on the rear side of the robot body 1, external protection plates 3 are attached on the left and right sides of the robot body 1, side buffer springs 4 are installed outside the external protection plates 3, rubber support pads 5 are installed on the inner side of the side buffer springs 4, anti-collision plates 6 are installed outside the rubber support pads 5 and the side buffer springs 4, and anti-collision pads 7 are bonded outside the anti-collision plates 6, as shown in the drawings. Figure 4 and Figure 5

[0046] Anti-collision plates 6 are installed on the upper end of the external protection plates 6, upper buffer springs 8 are installed on the outer end of the anti-collision plates 6, movable baffles 9 are installed on the upper end of the two anti-collision plates 6, a top protection plate 11 is installed on the top of the external protection plates 3, a top plate 10 is installed on the top of the external protection plates 3, a through slot 12 is formed in the middle of the top protection plate 11, an arc-shaped baffle 13 is installed on the right side of the top of the robot body 1, and sensors and a main machine are arranged below the arc-shaped baffle 13.

[0047] ​When the side of the robot is impacted, the anti-collision pad 7 is extruded against the anti-collision plate 6, and the anti-collision plate 6 is extruded against the rubber supporting pad 5 and the side buffer spring 4, thereby buffering and protecting the side of the robot, and the movable baffle 9 at the upper end of the side of the robot can extrude the upper buffer spring 8, and the movable baffle 9 drives the top plate 10 to move, the through slot 12 reserved at the top of the robot is used for the movement of the robot, the top protection plate 11 made of rubber material can protect the top of the robot, and the arc-shaped baffle 13 protects the detection part (sensor and host computer) of the robot, so that the normal use and work of the robot can be ensured.

[0048] The host computer comprises a perception module, a disturbance rejection control module and a driving module. The output end of the perception module is bidirectionally electrically connected to the disturbance rejection control module, the output end of the disturbance rejection control module is bidirectionally electrically connected to the driving module, and the output end of the driving module is bidirectionally electrically connected to an upper computer interaction module. The disturbance rejection control module adopts an embedded controller based on an ARM Cortex-M7 kernel, and is internally provided with an extended state observer and a working condition self-adaptive adjustment unit. The working condition self-adaptive adjustment unit can adjust the observation gain and the control law parameters according to the motion state data.

[0049] The sensor comprises an inertial measurement unit, a depth sensor, a Doppler speedometer and an environmental disturbance monitoring assembly. The inertial measurement unit is a six-axis inertial measurement unit (IMU) with a sampling frequency of not less than 200 Hz. The environmental disturbance monitoring assembly comprises a water flow speed sensor and a wave sensor.

[0050] The underwater robot motion disturbance rejection control method provided in the embodiment comprises the following steps as shown in Figure 1 The steps are as follows:

[0051] S1, initialization setting: after power-on, the perception module, the disturbance rejection control module and the driving module are self-checked, the upper computer interaction module sets the initial posture and initial depth of the robot, and the disturbance rejection control module loads the initial control parameters.

[0052] S2, data acquisition and preprocessing: the perception module starts each sensor, and collects the motion state data (posture angle, angular velocity, acceleration, depth and speed) and the environmental disturbance data (water flow speed and wave parameters) of the robot in real time, and realizes the process of multi-sensor acquisition + filtering + time synchronization through state modeling, observation fusion and strong tracking filtering.

[0053] In the embodiment, the synchronization-filter integration is adopted, the time synchronization deviation is included in the filtering model, and real-time estimation and correction are performed to avoid the error accumulation of the traditional synchronization-first-filtering. The specific steps are as follows:

[0054] (1) State modeling aims to incorporate time synchronization deviations into the states that need to be tracked, while describing how these states change over time and the impact of synchronization deviations on measurements.

[0055] Attitude angles acquired by IMU Taking the fusion with the depth z acquired by the depth sensor as an example, the time synchronization deviation is... Incorporate into the state vector, and pass through the angular velocity term. , depth change rate This reflects the impact of synchronization deviation on the measurement of physical quantities, such as synchronization deviation. This can lead to errors in attitude angle measurement. .

[0056] Define the state vector at time k: ;in, The attitude angle (the robot's tilt angle) measured by the IMU at time k. Let k be the angular velocity (rate of change of attitude angle) measured by the IMU at time k. Let k be the depth measured by the depth sensor (the depth of the robot underwater). Let k be the rate of change of depth (the speed at which depth changes). This refers to the time synchronization deviation between the IMU and the depth sensor.

[0057] Define a state equation to describe the state at time k-1. How to transform the state at time k? : ,in:

[0058] A is the state transition matrix (including the dynamic model of synchronization deviation): ;in, Indicates the control period. This represents the angular velocity at time k-1. This represents the rate of change of depth at time k-1;

[0059] B is the input matrix (control signal mapping): Where B is used to map the control signal u(k-1) to the state change, and k u and k z It is the gain coefficient of the control signal;

[0060] The process noise vector (including physical quantities and synchronization deviation noise): , The random errors of the sensor itself (such as the measurement noise of the IMU and the fluctuation of synchronization deviation) follow a normal distribution with a mean of 0 and a variance matrix of Q. The values ​​in Q are the variances of the noise in each state (the smaller the value, the smaller the noise).

[0061] (2) Observation fusion.

[0062] The observation vector Z(k) contains the raw measurements of the IMU and the depth sensor, while incorporating the observation information of the synchronization bias, to realize multi-source information fusion: ; wherein, is the raw attitude angle directly measured by the IMU (unmodified error), is the raw depth directly measured by the depth sensor (unmodified error), is the synchronization bias measurement calculated by the time stamp (such as the time stamp difference of the data of the two sensors).

[0063] Observation equation: describes the mapping relationship between the true state and the measurement data: , wherein:

[0064] is the observation matrix: , used to map the 5-dimensional true state X(k) into the 3-dimensional measurement Z(k);

[0065] is the observation noise vector: , the random error in the measurement process (such as sensor reading fluctuation), which follows a normal distribution with mean 0 and variance matrix R, and the values in R are the variances of each measurement noise.

[0066] (3) Strong tracking filter solution.

[0067] The strong tracking fading factor is used for filtering, and the steps are as follows:

[0068] First, prior state estimation: according to the final estimated value at k-1 time , combined with the state transition matrix A and the control signal u(k 1), the state at k time is predicted (theoretical estimate) : ;

[0069] Then, the prior covariance matrix: the uncertainty of the predicted estimate: ; wherein, represents the confidence of the state estimate (the smaller the value, the higher the confidence); λ(k) is the fading factor, and the core function is dynamic adjustment - if there is disturbance (such as sudden change of water flow), λ will increase, and the weight of historical data will be reduced, so that the filter can adapt to changes faster; wherein, the fading factor λ(k) calculation (quantify disturbance intensity, dynamically adjust filter gain): , the numerator is the measurement Z(k) and the predicted value The difference (residual error) between the predicted value and the measured value, and the denominator is the theoretical variance of the noise. If the residual error is large (indicating that the disturbance is large), λ will be greater than 1, enhancing the tracking ability of the filter. If the residual error is small, λ = 1, maintaining normal filtering;

[0070] Then, the Kalman gain: the weight of the predicted value and the measured value is balanced: ; Kalman gain The larger the value, the more the measured value Z(k) is trusted; Kalman gain The smaller the value, the more the predicted value is trusted , automatically balancing the weight of the two; wherein R is the variance matrix of the measurement noise;

[0071] Then, the posterior state estimation (filter output): ; using the Kalman gain , the predicted value is corrected according to the residual error of the measured value and the predicted value , to obtain the final estimated value at time k ;

[0072] Finally, the posterior covariance matrix: ; wherein I is the identity matrix, and the updated P(k|k) reflects the uncertainty of the final estimated value, providing a basis for the next filtering (at time k+1).

[0073] S3, motion instruction receiving and analyzing: the upper computer interaction module sends a target motion instruction, and the active disturbance rejection control module receives the target motion instruction and analyzes it to obtain control target values of each motion degree of freedom.

[0074] Among them, the upper computer interaction module is connected with the underwater robot through the underwater acoustic communication module or the wired communication module, and has the functions of motion instruction sending, state monitoring display and emergency control.

[0075] S4, extended state observation and disturbance compensation: based on the preprocessed motion state data and environmental disturbance data, the active disturbance rejection control module estimates the total disturbance (including nonlinear dynamics, external disturbance, internal disturbance) of the underwater robot through the dynamic gain adaptive extended state observer (ESO), and generates a disturbance compensation amount according to the total disturbance.

[0076] The initial control parameters loaded by the active disturbance rejection control module include the initial observation bandwidth of the extended state observer, the initial control bandwidth of the control law, and the nonlinear feedback coefficient, and the initial control parameters are determined based on the nominal dynamics model of the underwater robot and the pool pre-experiment data.

[0077] Among them, the dynamic gain adaptive extended state observer is used to estimate the total disturbance (nonlinear dynamics, external flow disturbance, internal mechanical disturbance, etc.) in the robot motion as an extended state variable to estimate in real time, providing a basis for subsequent disturbance compensation.

[0078] The ESO complete state equation of the single degree of freedom (such as depth, heading) motion of the underwater robot is:

[0079] ;

[0080] Wherein, is the actual motion state (such as actual depth, actual heading angle) of the robot, is the rate of change of the motion state (such as the rate of change of depth, the angular velocity of the heading), that is, the derivative of the actual state is equal to the rate of change; The change (acceleration level) of is composed of three parts: the total disturbance estimated by the ESO , is the control gain, is the basic quantity of the control signal), and the external disturbance (such as the additional disturbance caused by the water flow); The derivative of the total disturbance is the update rule of the estimated value of the total disturbance, which is dynamically adjusted through the observation error and the nonlinear function fal, , , , is the observation gain (the dynamic observation gain is the dynamic observation gain), and y is the measurement feedback value (such as the depth and heading angle measured by the sensor) of the robot.

[0081] In this embodiment, the ESO (extended state observer) gain is used to synchronize the environmental disturbance intensity and the propeller load state, and the formula is as follows:

[0082] ;

[0083] Wherein, k is the control cycle number, is the ESO dynamic observation gain at time k, is the initial observation gain of the ESO, is the flow disturbance adjustment coefficient, is the real-time flow velocity (component along the motion direction) at time k, is the maximum flow resistance of the robot, is the propeller current adjustment coefficient, is the real-time working current of the propeller at time k, is the rated current of the propeller, is the motion state error at time k, is the motion state error threshold; the flow disturbance adaptation item : the stronger the flow is, the closer to the maximum flow resistance ), the larger this term is, the more sensitive the ESO is to the water flow disturbance; the propeller load adaptation term : the closer the propeller current I(k) is to the rated current I rated (the heavier the load is, ≥ 0.8), this term is activated and increased, to boost the disturbance change under heavy load; the control error smoothing term : is the motion state error (target value - actual value), e th is the error threshold, and the gain is smoothly transitioned through the hyperbolic tangent function tanh: when < (the error is small, and the control effect is good), the tanh value is small, and the gain decreases with the decrease of the error, avoiding the amplification of measurement noise; when > (the error is large, and it needs to be quickly adjusted), the tanh value approaches 1, and the gain is linearly increased, and the ESO quickly captures strong disturbances.

[0084] This embodiment adopts multi-dimensional coupling adjustment, and for the first time, it integrates the water flow speed (environmental disturbance), propeller current I (hardware load), and motion error (control effect), solves the problem that the traditional single error adjustment cannot adapt to complex underwater working conditions, and adopts the three-dimensional coupling adjustment logic of propeller current + motion error + environmental disturbance to ensure the balance between control stability and response speed under complex underwater working conditions (such as strong water flow and heavy propeller load).

[0085] Among them, the fal function is the nonlinear observation core of ESO, which avoids oscillation through the characteristics of fast response in large error and stable accuracy in small error, and the fal function is defined as:

[0086] ;

[0087] Among them: e = x1 y, represents the observation error, which is the difference between the actual state and the measured value y; α ∈ (0, 1) is a nonlinear coefficient, which balances the response speed and stability; δ > 0 is a linear interval threshold, which is set based on the control accuracy requirement of underwater robots.

[0088] S5, control quantity calculation and output: the active disturbance rejection control module combines the control target value (such as the set depth 10 m and the set heading 0°) and the disturbance compensation amount (the total disturbance compensation amount estimated by ESO), and calculates the pulse width modulation (PWM) control signal of each propeller through the active disturbance rejection control law. The control signal is transmitted to the drive module.

[0089] The control law needs to combine the control target value with the total disturbance estimated by ESO. The equivalent control signal u for the thruster is generated, as shown in the following formula:

[0090] ;

[0091] in, The rate of change of the target value. This represents the robot's actual motion state. This represents the rate of change of the robot's actual motion state. State error (actual state) Relative to the target state (deviation) Error of the rate of change of state (rate of change of target value) Rate of change of actual state (difference) and For the control law feedback coefficient, and For feedback nonlinear coefficients, The threshold for the linear interval of the fal function. Based on the control signal, The total disturbance estimated by ESO, U(k) is the control gain coefficient, and u(k) is the equivalent control signal of the thruster.

[0092] S6. Thruster Drive and Status Feedback: The drive module receives PWM control signals to drive each thruster to perform actions, thereby achieving robot posture adjustment and position control. At the same time, it collects the operating current of the thrusters and the water flow speed data of the environment and feeds them back to the active disturbance rejection control module.

[0093] If the thruster feedback data shows that a certain thruster is faulty, the active disturbance rejection control module automatically redistributes the PWM control signals of the remaining thrusters. The motion control is fault-tolerant through redundant drive. The target motion commands include position commands (3D coordinates), attitude commands (roll angle, pitch angle, yaw angle) and velocity commands (linear velocity, angular velocity). During the analysis process, a smooth transition target value is generated through trajectory planning algorithm to avoid sudden changes in control quantity.

[0094] In this embodiment, after a thruster malfunctions, the total disturbance increment changes (e.g., torque loss of the malfunctioning thruster, increased water flow interference). This embodiment innovatively proposes a fault-tolerant allocation + disturbance compensation coordination mechanism to ensure the reliability of robot motion when a thruster malfunctions. Specifically, this includes:

[0095] (1) Propeller fault determination: Set clear threshold through the working current of the propeller (the easiest hardware indicator to monitor) to avoid misjudgment or missed judgment and provide accurate basis for subsequent fault-tolerant mechanism triggering.

[0096] Propeller fault determination condition (quantitative threshold to ensure fault-tolerant triggering accuracy):

[0097] ;

[0098] wherein, is the real-time working current of propeller j at time k; 0.1A is the minimum current threshold (current close to 0, indicating that the propeller is not powered on, the line is disconnected or the motor is stuck, and cannot output thrust); is the rated current of the propeller (current exceeds 1.5 times the rated value, indicating that the propeller is overloaded, short-circuited or mechanically faulty, and may be burned out, which needs to be determined as a fault and stopped relying on).

[0099] (2) Fault equivalent disturbance modeling: convert the thrust loss into a disturbance.

[0100] Pre-fault ESO total disturbance observation formula: ; wherein, is the total disturbance estimated by ESO under normal working conditions, is the rate of change of motion state, is the propeller control input, is the external disturbance at time k.

[0101] When the propeller fails, the total disturbance increases the fault equivalent disturbance , and the complete correction formula is: ; wherein, is the corrected total disturbance after the fault, is the total disturbance estimated by ESO before the fault, is the fault equivalent disturbance, is the column vector corresponding to the fault propeller in the thrust matrix T, is the PWM signal of the fault propeller at the previous time before the fault, and Δt is the control period.

[0102] This embodiment first converts the historical control signal of the fault propeller into an equivalent disturbance , solving the problem of incomplete disturbance estimation after the fault.

[0103] (3) Target function with disturbance compensation: optimize the control signal of the remaining propeller.

[0104] Based on the corrected total disturbance , the target function with disturbance compensation is designed, and the PWM signal u of the remaining thruster is allocated by an optimization algorithm.

[0105] The target function with disturbance compensation is:

[0106] ;

[0107] The constraint condition is the physical constraint of the PWM signal:

[0108] , wherein is the post-fault thrust matrix; is the desired torque / force vector of the robot (such as the lift required to maintain the depth and the torque required to maintain the heading); is the disturbance compensation torque / force vector, J is the Jacobian matrix of the robot, which is calculated based on the size of the robot; the smoothing constraint avoids sudden changes in the PWM signal after the failure (protects the remaining thrusters and prolongs the service life), and λ is the smoothing weight coefficient (set according to the equipment tolerance, the larger λ is, the smoother the signal is); u(k-1) is the control signal vector at the previous time before the failure.

[0109] In the target function of the embodiment, the makes the remaining thrusters bear the desired torque / force and the fault disturbance compensation at the same time, ensuring control accuracy.

[0110] S7, working condition adaptive adjustment: the motion state data of the self-disturbance control module is judged according to the thruster feedback data and the sensing module, the observation gain of the extended state observer and the feedback coefficient of the control law are automatically adjusted according to the current motion condition of the robot, the control performance is optimized, and the above steps are repeated to realize real-time and continuous motion control of the underwater robot until the task end instruction or emergency stop instruction is sent by the upper computer interaction module.

[0111] As an implementation mode, the specific way of working condition adaptive adjustment is: when the thruster working current exceeds 80% of the rated current or the motion state error is greater than the preset threshold, the self-disturbance control module reduces the control bandwidth and increases the observation gain; when the error is less than the threshold and the current is stable, the control bandwidth is increased to improve the response speed.

[0112] As an implementation mode, the embodiment innovatively defines the working condition level index L(k) to realize dynamic weight adjustment of focusing on accuracy in stable working conditions and focusing on response in disturbance working conditions, so that the feedback coefficient of the control law needs to be adaptively switched according to the working condition level.

[0113] The working condition level index L(k) is:

[0114] ;​

[0115] Adjusting the feedback coefficient of the control law dynamically based on L(k) 、 :

[0116] ;

[0117] ;

[0118] wherein, and are the initial feedback coefficients of the control law, is the real-time water flow speed (component along the motion direction of the robot) at k, is the maximum flow resistance capability of the robot (maximum tolerable water flow speed along the motion direction), and I(k) is the real-time working current of the thruster at k, is the rated current of the thruster, is the motion state error at k, is the motion state error threshold.

[0119] In this embodiment, when the complex working condition (e.g., strong water flow + high load + large error) is met, the feedback coefficient is increased by 30% (to enhance the position tracking capability), and decreased by 20% (to avoid speed overshoot); when the stable working condition is met, the feedback coefficient returns to the initial value (to focus on control accuracy); and Working condition level constraint: , to avoid excessive adjustment of the feedback coefficient leading to control instability, and to achieve a balance between stability and response speed.

[0120] This embodiment effectively improves the anti-interference capability of the robot in a complex underwater environment by using the active disturbance rejection control algorithm, estimating and compensating the total disturbance through the extended state observer, and providing a working condition adaptive adjustment unit that can automatically adjust the control parameters according to the motion state and the working state of the thruster, thereby ensuring control stability while improving response speed, adapting to different motion conditions, and having thruster fault tolerance function. When a thruster fails, the control signals of the remaining thrusters can be redistributed to ensure that the robot continues to complete motion control, thereby improving the reliability of the robot. The perception module uses multi-sensor fusion, time synchronization, and noise suppression processing to ensure the accuracy and effectiveness of the data, thereby providing a foundation for high-precision control.

[0121] Embodiment Two

[0122] The underwater robot motion active disturbance rejection control system provided in this embodiment comprises:

[0123] The underwater robot motion active disturbance rejection control system provided in this embodiment comprises:

[0124] ​The perception module is configured to acquire motion state data, environmental disturbance data, and control target values ​​of the underwater robot.

[0125] The extended state observation module is configured to: estimate the total disturbance of the underwater robot based on motion state data and environmental disturbance data through a dynamically gain-adapted extended state observer; generate a disturbance compensation amount based on the total disturbance; and calculate the control signals of each thruster by combining the control target value and the disturbance compensation amount through an active disturbance rejection control law.

[0126] The drive module is configured to drive the movements of each thruster of the underwater robot based on control signals, thereby realizing the attitude adjustment and position control of the underwater robot.

[0127] The adaptive adjustment module is configured to: acquire the working current of the thruster and the water flow speed in the environment, determine the current motion condition of the robot, and automatically adjust the observation gain and control law feedback coefficient of the extended state observer.

[0128] It should be noted that each module in this embodiment corresponds one-to-one with each step in Embodiment 1, and their specific implementation processes are the same, so they will not be repeated here.

[0129] Example 3

[0130] This embodiment provides a computer-readable storage medium storing a computer program that, when executed by a processor, implements the steps of the underwater robot motion active disturbance rejection control method described in Embodiment 1 above.

[0131] Example 4

[0132] This embodiment provides a computer device, such as... Figure 6 As shown, the system includes a computer-readable storage medium 1003, a processor 1001, a communication interface 1002, and a computer program stored on the computer-readable storage medium 1003 and executable on the processor 1001. The processor 1001, communication interface 1002, and computer-readable storage medium 1003 can be connected via a bus or other means. The communication interface 1002 is used to receive and transmit data. When the processor 1001 executes the program, it implements the steps of the underwater robot motion active disturbance rejection control method described in Embodiment 1 above.

[0133] The above description is merely a preferred embodiment of the present invention and is not intended to limit the invention. Various modifications and variations can be made to the present invention by those skilled in the art. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

Claims

1. A method for active disturbance rejection control of underwater robot motion, characterized in that, include: Acquire motion state data, environmental disturbance data, and control target values ​​of the underwater robot; Based on motion state data and environmental disturbance data, the total disturbance of the underwater robot is estimated by an extended state observer with dynamic gain adaptation, and a disturbance compensation amount is generated based on the total disturbance. Combining the control target value and the disturbance compensation amount, the control signal of each thruster is calculated by an active disturbance rejection control law. Based on control signals, the underwater robot's thrusters are driven to move, thereby achieving attitude adjustment and position control of the underwater robot; The robot obtains the operating current of the thruster and the water flow speed in the environment to determine the current motion condition of the robot and automatically adjusts the observation gain of the extended state observer and the control law feedback coefficient; it defines the working condition level index so that the control law feedback coefficient adapts to the working condition level. If a thruster fails, the control signals of the remaining thrusters are redistributed through an optimization algorithm. Disturbance compensation torque / force vector is added to the objective function so that the remaining thrusters can simultaneously bear the desired torque / force and fault disturbance compensation. The adjustment formula for the control law feedback coefficient is: ; ;in, and For the control law feedback coefficient, and The initial feedback coefficient of the control law, and the operating condition level index. , Let k be the water flow velocity at time k. I(k) represents the maximum current resistance capability of the underwater robot, and I(k) represents the operating current of the thruster at time k. The rated current of the thruster, Let k be the motion state error at time k. The threshold value for motion state error; The objective function of the optimization algorithm is: Where u is the control signal for the remaining thrusters. The thrust matrix after the fault. For the desired torque / force vector of the underwater robot, Let J be the disturbance compensation torque / force vector, and J be the Jacobian matrix of the underwater robot. To increase the total disturbance after the fault equivalent disturbance, λ is the smoothing weight coefficient, and u(k-1) is the control signal at the moment before the fault. The extended state observer employs dynamic observation gain: ； Where k is the control cycle number. The gain is the dynamic observation gain at time k. For the initial observation gain, This is the water flow disturbance adjustment coefficient. Let k be the water flow velocity. To maximize the robot's resistance to flow, This is the thruster current adjustment coefficient. Let k be the real-time operating current of the thruster. For the rated current of the thruster, Let k be the motion state error at time k. This is the motion state error threshold.

2. The underwater robot motion active disturbance rejection control method as described in claim 1, characterized in that, The fault is determined by the operating current of the thruster.

3. The underwater robot motion active disturbance rejection control method as described in claim 1, characterized in that, The motion state data includes attitude angle, angular velocity, acceleration, depth, and velocity.

4. The underwater robot motion active disturbance rejection control method as described in claim 1, characterized in that, The environmental disturbance data includes water flow velocity and wave parameters.

5. A motion disturbance rejection control system for an underwater robot, characterized in that, include: The perception module is configured to acquire motion state data, environmental disturbance data, and control target values ​​of the underwater robot. The extended state observation module is configured to: estimate the total disturbance of the underwater robot based on motion state data and environmental disturbance data through a dynamically gain-adapted extended state observer; generate a disturbance compensation amount based on the total disturbance; and calculate the control signals of each thruster by combining the control target value and the disturbance compensation amount through an active disturbance rejection control law. The drive module is configured to drive the movements of each thruster of the underwater robot based on control signals, thereby realizing the attitude adjustment and position control of the underwater robot. The adaptive adjustment module is configured to: acquire the working current of the thruster and the water flow speed in the environment, determine the current motion condition of the robot, automatically adjust the observation gain and control law feedback coefficient of the extended state observer; and define the working condition level index so that the control law feedback coefficient switches adaptively with the working condition level. If a thruster fails, the control signals of the remaining thrusters are redistributed through an optimization algorithm. Disturbance compensation torque / force vector is added to the objective function so that the remaining thrusters can simultaneously bear the desired torque / force and fault disturbance compensation. The adjustment formula for the control law feedback coefficient is: ; ;in, and For the control law feedback coefficient, and The initial feedback coefficient of the control law, and the operating condition level index. , Let k be the water flow velocity at time k. I(k) represents the maximum current resistance capability of the underwater robot, and I(k) represents the operating current of the thruster at time k. The rated current of the thruster, Let k be the motion state error at time k. The threshold value for motion state error; The objective function of the optimization algorithm is: Where u is the control signal for the remaining thrusters. The thrust matrix after the fault. For the desired torque / force vector of the underwater robot, Let J be the disturbance compensation torque / force vector, and J be the Jacobian matrix of the underwater robot. To increase the total disturbance after the fault equivalent disturbance, λ is the smoothing weight coefficient, and u(k-1) is the control signal at the moment before the fault. The extended state observer employs dynamic observation gain: ； Where k is the control cycle number. The gain is the dynamic observation gain at time k. For the initial observation gain, This is the water flow disturbance adjustment coefficient. Let k be the water flow velocity. To maximize the robot's resistance to flow, This is the thruster current adjustment coefficient. Let k be the real-time operating current of the thruster. For the rated current of the thruster, Let k be the motion state error at time k. This is the motion state error threshold.

6. The underwater robot motion self-disturbance rejection control system as described in claim 5, characterized in that, The fault is determined by the operating current of the thruster.

7. The underwater robot motion self-disturbance rejection control system as described in claim 5, characterized in that, The motion state data includes attitude angle, angular velocity, acceleration, depth, and velocity.

8. The underwater robot motion self-disturbance rejection control system as described in claim 5, characterized in that, The environmental disturbance data includes water flow velocity and wave parameters.

9. A computer-readable storage medium having a computer program stored thereon, characterized in that, When the program is executed by the processor, it implements the steps of the underwater robot motion active disturbance rejection control method as described in any one of claims 1-4.

10. A computer device comprising a computer-readable storage medium, a processor, and a computer program stored on the computer-readable storage medium and executable on the processor, characterized in that, When the processor executes the program, it implements the steps of the underwater robot motion active disturbance rejection control method as described in any one of claims 1-4.

Images