Water surface cleaning robot motion planning and control system based on ADRC

Abstract

The invention belongs to the technical field of water surface cleaning robot control, and discloses a water surface cleaning robot motion planning and control system based on ADRC. The system comprises a multi-dimensional parameter acquisition unit, a coupling parameter calculation unit, a self-adaptive ADRC control unit and a propeller driving execution unit, the multi-dimensional parameter acquisition unit acquires related parameters of a water body and a robot, and the coupling parameter calculation unit generates various correction parameters; the self-adaptive ADRC control unit executes control logic according to the correction parameters and generates a driving signal, and the propeller drives the execution unit to execute driving processing; the coupling parameter calculation unit adopts a specific formula to generate fluid resistance correction parameters; the problem that an existing system is poor in control adaptability is solved, and stable motion control over the robot in the complex water area is achieved.

Description

Technical Field

[0001] This invention belongs to the field of water surface cleaning robot control technology, specifically relating to a motion planning and control system for a water surface cleaning robot based on ADRC. Background Technology

[0002] Existing motion control systems for water surface cleaning robots mostly employ traditional ADRC (Advanced Dynamic Control Reduction) schemes. These schemes only consider the influence of water flow velocity and robot load on motion control, neglecting the coupling effect between water medium characteristics and robot attitude parameters. In real-world complex aquatic environments, changes in water turbidity and salinity alter fluid resistance characteristics, and dynamic variations in wave periods and robot roll angles generate additional attitude disturbances. Traditional ADRC schemes use fixed values ​​for the extended state observer gain and nonlinear state error feedback coefficient, which cannot be dynamically adjusted based on these coupling factors. This results in lag in disturbance observation, poor control logic adaptability, and the robot is prone to trajectory deviation and motion instability, making it difficult to meet the cleaning operation requirements in complex aquatic environments.

[0003] Based on the above problems, there is an urgent need for a motion control system that can adapt to multiple coupling factors and improve control adaptability. Summary of the Invention

[0004] The purpose of this invention is to overcome the shortcomings of existing technologies and proposes a motion planning and control system for a water surface cleaning robot based on ADRC (Adaptive Dynamic Control and Reduction). This system includes a multi-dimensional parameter acquisition unit, a coupled parameter calculation unit, an adaptive ADRC control unit, and a thruster drive execution unit. The multi-dimensional parameter acquisition unit is used to acquire water turbidity parameters, water salinity parameters, water flow velocity parameters, robot load parameters, wave period parameters, and robot roll angle parameters. The coupled parameter calculation unit is used to generate fluid resistance correction parameters based on the water turbidity parameters, water salinity parameters, and water flow velocity parameters. It is also used to generate extended state observer perturbation observation gain parameters based on the fluid resistance correction parameters and robot load parameters, and further to generate extended state observer perturbation observation gain parameters. The parameters, wave period parameters, and robot roll angle parameters are used to generate nonlinear state error feedback coefficient parameters. The adaptive ADRC control unit includes a tracking differentiator unit, an extended state observer unit, and a nonlinear state error feedback unit. The extended state observer unit performs perturbation observation processing using the extended state observer perturbation observation gain parameter, and the nonlinear state error feedback unit performs error feedback processing using the nonlinear state error feedback coefficient parameters. The adaptive ADRC control unit is used to generate drive control signals based on the output signals of the tracking differentiator unit, the extended state observer unit, and the nonlinear state error feedback unit. The thruster drive execution unit is used to execute the motion drive processing of the water surface cleaning robot based on the drive control signals.

[0005] Preferably, the multi-dimensional parameter acquisition unit includes a water medium acquisition subunit, a motion posture acquisition subunit, and a load detection subunit; the water medium acquisition subunit is used to acquire water turbidity parameters, water salinity parameters, and water flow velocity parameters; the motion posture acquisition subunit is used to acquire wave period parameters and robot roll angle parameters; and the load detection subunit is used to acquire robot load parameters; the water medium acquisition subunit, motion posture acquisition subunit, and load detection subunit all establish data transmission connections with the coupling parameter calculation unit.

[0006] More preferably, the coupling parameter calculation unit performs parameter generation processing in the following order: first, it generates fluid resistance correction parameters based on the water turbidity parameters, water salinity parameters, and water flow velocity parameters transmitted by the water medium acquisition subunit; then, it generates extended state observer disturbance observation gain parameters based on the fluid resistance correction parameters and the robot load parameters transmitted by the load detection subunit; and finally, it generates nonlinear state error feedback coefficient parameters based on the extended state observer disturbance observation gain parameters, the wave period parameters transmitted by the motion attitude acquisition subunit, and the robot roll angle parameters.

[0007] More preferably, the adaptive ADRC control unit includes a tracking differentiator unit for receiving water flow velocity parameters transmitted by the water medium acquisition subunit and wave period parameters transmitted by the motion attitude acquisition subunit; the tracking differentiator unit performs tracking transition process parameter adjustment processing according to the water flow velocity parameters and wave period parameters, and transmits the adjusted tracking transition process parameters as the output signal of the tracking differentiator unit to the signal integration node of the adaptive ADRC control unit.

[0008] More preferably, the coupling parameter calculation unit generates the fluid resistance correction parameters using the fluid resistance coupling correction coefficient formula, which is: ; In the formula, λ is the fluid resistance correction parameter, α is the turbidity influence coefficient, T is the water turbidity parameter, β is the salinity influence coefficient, S is the water salinity parameter, γ is the water flow velocity influence coefficient, v is the water flow velocity parameter, g is the gravitational acceleration parameter, and d is the propeller blade diameter parameter.

[0009] More preferably, when the coupling parameter calculation unit generates the expansion state observer disturbance observation gain parameter, it combines the fluid resistance correction parameter, robot load parameter, water density parameter, and thruster blade diameter parameter, and obtains the expansion state observer disturbance observation gain parameter through a specific calculation logic; the calculation logic is constructed based on the coupling relationship between fluid resistance and load to ensure that the expansion state observer disturbance observation gain parameter is adapted to the actual disturbance intensity.

[0010] More preferably, when the coupling parameter calculation unit generates the nonlinear state error feedback coefficient parameter, it combines the extended state observer disturbance observation gain parameter, robot roll angle parameter, wave period parameter, running time parameter, thruster rotational inertia parameter, and extended state observer basic bandwidth parameter, and obtains the nonlinear state error feedback coefficient parameter through a specific calculation logic; the calculation logic takes into account the coordinated adaptation of attitude disturbance and observation accuracy.

[0011] More preferably, the extended state observer unit included in the adaptive ADRC control unit receives the extended state observer disturbance observation gain parameters transmitted by the coupling parameter calculation unit; the extended state observer unit substitutes the extended state observer disturbance observation gain parameters into the disturbance observation logic to perform disturbance observation processing, and transmits the output signal after disturbance observation processing to the signal integration node of the adaptive ADRC control unit.

[0012] More preferably, the adaptive ADRC control unit includes a nonlinear state error feedback unit that receives nonlinear state error feedback coefficient parameters transmitted by the coupling parameter calculation unit; the nonlinear state error feedback unit substitutes the nonlinear state error feedback coefficient parameters into the error feedback logic to perform error feedback processing, and transmits the output signal after error feedback processing to the signal integration node of the adaptive ADRC control unit.

[0013] More preferably, the signal integration node of the adaptive ADRC control unit integrates the output signal of the tracking differentiator unit, the output signal of the extended state observer unit, and the output signal of the nonlinear state error feedback unit to generate a drive control signal; the adaptive ADRC control unit transmits the drive control signal to the thruster drive execution unit, and the thruster drive execution unit performs thruster speed adjustment processing and thruster direction adjustment processing according to the drive control signal.

[0014] Technical effects: This invention captures coupling parameters between the water body and the robot through a multi-dimensional parameter acquisition unit, generates adaptive correction parameters through a coupling parameter calculation unit, and dynamically adjusts the control logic based on the correction parameters. This solves the problem of poor control adaptability and motion instability caused by the inability of fixed parameters to adapt to multiple coupling factors in existing systems. Its innovation lies in the deep coupling of water medium, load, attitude parameters and ADRC control, realizing dynamic adaptation of control parameters and ensuring stable robot movement in complex water environments. Attached Figure Description

[0015] Figure 1 This is a connection block diagram of the main module of the motion planning and control system for the water surface cleaning robot based on ADRC in this application; Figure 2 This is a connection block diagram of the multi-dimensional parameter acquisition unit submodule in this application; Figure 3 A flowchart for generating parameters of the coupling parameter calculation unit in this application; Figure 4 This is a block diagram of the internal structure of the adaptive ADRC control unit in this application; Figure 5 This is a block diagram of the control execution of the thruster drive execution unit in this application. Detailed Implementation

[0016] 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 embodiments. It should be understood that the specific embodiments described herein are merely illustrative and not intended to limit the invention.

[0017] Technical problems with existing technologies: The existing motion control system of water surface cleaning robots adopts traditional ADRC control, which only considers water flow velocity and robot load, and does not take into account the coupling effect of water turbidity, salinity and robot roll angle and wave period. The ADRC related parameters are fixed, resulting in lag in disturbance observation, poor control adaptability, and easy trajectory deviation of the robot.

[0018] Based on this, please refer to Figures 1-5This embodiment provides a motion planning and control system for a water surface cleaning robot based on ADRC, including: a multi-dimensional parameter acquisition unit, a coupling parameter calculation unit, an adaptive ADRC control unit, and a thruster drive execution unit. The multi-dimensional parameter acquisition unit is used to acquire water turbidity parameters, water salinity parameters, water flow velocity parameters, robot load parameters, wave period parameters, and robot roll angle parameters. The coupling parameter calculation unit is used to generate fluid resistance correction parameters based on the water turbidity parameters, water salinity parameters, and water flow velocity parameters. The coupling parameter calculation unit is also used to generate an expanded state observer perturbation observation gain parameter based on the fluid resistance correction parameters and the robot load parameters. The parameters, wave period parameters, and robot roll angle parameters are used to generate nonlinear state error feedback coefficient parameters. The adaptive ADRC control unit includes a tracking differentiator unit, an extended state observer unit, and a nonlinear state error feedback unit. The extended state observer unit performs perturbation observation processing using the extended state observer perturbation observation gain parameter. The nonlinear state error feedback unit performs error feedback processing using the nonlinear state error feedback coefficient parameters. The adaptive ADRC control unit is used to generate drive control signals based on the output signals of the tracking differentiator unit, the extended state observer unit, and the nonlinear state error feedback unit. The thruster drive execution unit is used to execute motion drive processing for the water surface cleaning robot based on the drive control signals.

[0019] It is worth mentioning that the multi-dimensional parameter acquisition unit adopts a multi-sensor distributed layout, with all sensors using industrial-grade waterproof components. The acquisition frequency is uniformly set to 20Hz to ensure the real-time and synchronous nature of parameter acquisition. All acquired parameters are transmitted in digital signal form via a CAN bus at a baud rate of 500kbps, avoiding attenuation and interference caused by analog signal transmission. The coupled parameter calculation unit uses an STM32H743 embedded microcontroller as its core hardware carrier. This controller is equipped with a double-precision floating-point arithmetic unit, enabling high-speed and accurate calculation of various parameters. The calculation unit has a built-in 128KB high-speed cache for temporarily storing the acquired raw parameters and the calculated corrected parameters. The cache uses a circular overwrite storage mechanism to ensure real-time data updates. The adaptive ADRC control unit adopts a dual-core architecture of FPGA+MCU. The FPGA chip is model XC7A35T, which is specifically used to execute the core algorithm calculations of the tracking differentiator unit, the extended state observer unit, and the nonlinear state error feedback unit. The calculation latency is controlled within 1ms. The MCU chip communicates and links with the microcontroller of the coupled parameter calculation unit, which is responsible for receiving correction parameters and sending calculation instructions to the FPGA, while integrating the output signals of each subunit. The adaptive ADRC control unit has an independent signal integration node, which is a hardware logic circuit that can realize the synchronous integration and analog-to-digital conversion of multi-channel signals with a conversion accuracy of 16 bits, ensuring the accuracy of the drive control signal. The thruster drive execution unit adopts a brushless DC servo driver, which supports dual-mode input of PWM signal and analog voltage signal. It can realize continuous adjustment of thruster speed from 0-3000r / min and switching of forward and reverse directions according to the drive control signal output by the adaptive ADRC control unit. The driver has built-in overcurrent, overvoltage, and stall protection mechanisms to ensure the stability of thruster operation. The driver and thruster are connected by a waterproof aviation plug, which is suitable for the underwater working environment of the water surface cleaning robot. All units are connected by waterproof communication cables, and the communication protocol adopts CRC16 check mechanism to ensure that there is no packet loss or error during data transmission. The multi-dimensional parameter acquisition unit, coupling parameter calculation unit and adaptive ADRC control unit are all integrated in the robot's main control box. The thruster drive execution unit is matched one-to-one with the thruster and is distributed on both sides of the robot's hull.

[0020] The existing technology has the following technical problems: the multi-dimensional parameter acquisition lacks a systematic division, and the data transmission of each acquisition module is chaotic, which makes it impossible for the coupled parameter calculation unit to efficiently obtain accurate data, affecting the accuracy of subsequent parameter calculations.

[0021] Based on this, the multi-dimensional parameter acquisition unit includes a water medium acquisition subunit, a motion posture acquisition subunit, and a load detection subunit. The water medium acquisition subunit is used to acquire water turbidity parameters, water salinity parameters, and water flow velocity parameters. The motion posture acquisition subunit is used to acquire wave period parameters and robot roll angle parameters. The load detection subunit is used to acquire robot load parameters. The water medium acquisition subunit, motion posture acquisition subunit, and load detection subunit all establish data transmission connections with the coupling parameter calculation unit.

[0022] It is worth mentioning that the water medium acquisition subunit integrates a turbidity sensor, a salinity sensor, and a Doppler flow velocity sensor. The turbidity sensor adopts the principle of scattered light detection, with a detection range covering 0-4000 NTU, and outputs a 4-20mA standard analog signal, which is converted into a digital signal by the analog-to-digital conversion module of the multi-dimensional parameter acquisition unit before transmission. The salinity sensor adopts the principle of electrode detection, converting the salinity value by detecting the conductivity of the water, with a detection accuracy of ±0.1‰, and directly outputs a digital signal. The Doppler flow velocity sensor adopts underwater ultrasonic detection, which can realize the detection of water flow velocity from 0.01-5m / s, effectively shielding the interference of water impurities, and updating the detection data in real time. The motion attitude acquisition subunit uses a MEMS attitude sensor as its core, integrating a three-axis gyroscope and a three-axis accelerometer. It calculates the robot's roll angle parameters by resolving acceleration and angular velocity data, with a detection range of -180° to +180° and a detection accuracy of ±0.05°. Simultaneously, this subunit extracts wave periodic characteristics through time-domain analysis of the roll angle parameters, generating wave period parameters without requiring an additional wave sensor, thus simplifying the hardware structure. The load detection subunit employs a pressure sensor array positioned at the bottom of the robot's waste collection bin. It calculates the load weight by detecting pressure changes in the bin. The sensor detection range is 0-50 kg with a detection accuracy of ±0.1 kg. When the load on the waste collection bin changes, the parameters can be updated within 50 ms. Each of the three sub-units has an independent microcontroller module responsible for performing preliminary filtering on the parameters it collects. A moving average filtering algorithm is used with a filtering window size of 10 to effectively remove random noise during parameter acquisition. Each sub-unit's microcontroller module establishes a point-to-point communication connection with the main controller of the coupled parameter calculation unit via an SPI bus. Each sub-unit is assigned an independent communication address. The coupled parameter calculation unit can accurately identify the parameter type transmitted by each sub-unit based on the communication address, avoiding data confusion. The parameter transmission of the three sub-units adopts a time-division synchronous mechanism. Within each 20Hz acquisition cycle, the water medium parameters, motion attitude parameters, and load parameters are transmitted sequentially, with a total transmission time of no more than 10ms, ensuring that the coupled parameter calculation unit can efficiently obtain accurate raw parameters.

[0023] The existing technology has the following technical problems: the coupling parameter calculation unit lacks a clear parameter generation order, and the generation of various correction parameters is chaotic, which makes it impossible for the adaptive ADRC control unit to obtain the appropriate control parameters in a timely manner, thus affecting the control response speed.

[0024] Based on this, the coupling parameter calculation unit performs parameter generation processing in the following order: first, it generates fluid resistance correction parameters based on the water turbidity parameters, water salinity parameters, and water flow velocity parameters transmitted by the water medium acquisition subunit; then, it generates extended state observer disturbance observation gain parameters based on the fluid resistance correction parameters and the robot load parameters transmitted by the load detection subunit; and finally, it generates nonlinear state error feedback coefficient parameters based on the extended state observer disturbance observation gain parameters, the wave period parameters transmitted by the motion posture acquisition subunit, and the robot roll angle parameters.

[0025] It is worth mentioning that the main controller of the coupled parameter calculation unit has a built-in timing logic circuit for parameter generation. This circuit solidifies the generation order of the three correction parameters through hardware logic, eliminating the need for software program scheduling and significantly improving the efficiency of parameter generation. The timing logic circuit allocates a fixed computation time slice for each parameter generation step: 2ms for the fluid resistance correction parameter, 1ms for the expansion state observer perturbation observation gain parameter, and 2ms for the nonlinear state error feedback coefficient parameter. These three time slices are sequentially connected, and the generation of all correction parameters can be completed within 5ms, which is much smaller than the 50ms interval of the 20Hz acquisition cycle, ensuring that the adaptive ADRC control unit can acquire the latest correction parameters in real time. The coupled parameter calculation unit sets an independent computation buffer for each correction parameter. The computation buffers for the fluid resistance correction parameter, expansion state observer perturbation observation gain parameter, and nonlinear state error feedback coefficient parameter are independent of each other. After the calculation of the previous parameter is completed, the result is stored in the corresponding buffer. The calculation logic of the next parameter will automatically read the result of the previous parameter from the corresponding buffer without manual intervention, realizing progressive automatic calculation of parameters. When the coupling parameter calculation unit receives new raw parameters, it first verifies the validity of the raw parameters. This verification includes checking if the parameters are within a reasonable range and if there are any errors in parameter transmission. If the verification passes, the parameter generation timing logic is initiated. If the verification fails, the corrected parameters from the previous acquisition cycle are used, and a parameter anomaly warning signal is sent to the main control system to ensure the continuity of system operation. All corrected parameters generated by the coupling parameter calculation unit are stored in 32-bit floating-point format. After storage, a parameter update command is sent to the adaptive ADRC control unit. Upon receiving the command, the adaptive ADRC control unit immediately reads the corrected parameters from the corresponding buffer of the coupling parameter calculation unit to update its own parameters. The entire parameter update process takes no more than 1ms, effectively improving the system's control response speed.

[0026] The existing technology has the following technical problems: the tracking differentiator unit does not adjust the tracking parameters in combination with the actual water area and the robot's motion parameters, resulting in a mismatch between the tracking transition process and the actual motion state, which affects the signal integration accuracy of the adaptive ADRC control unit.

[0027] Based on this, the adaptive ADRC control unit includes a tracking differentiator unit for receiving water flow velocity parameters transmitted by the water medium acquisition subunit and wave period parameters transmitted by the motion attitude acquisition subunit. The tracking differentiator unit performs tracking transition process parameter adjustment processing based on the water flow velocity parameters and wave period parameters. The tracking differentiator unit transmits the adjusted tracking transition process parameters as the tracking differentiator unit output signal to the signal integration node of the adaptive ADRC control unit.

[0028] It is worth mentioning that the tracking differentiator unit employs a discrete fast tracking differentiator algorithm. The core of this algorithm consists of two tracking transition process parameters: the tracking velocity factor and the filtering factor. The tracking differentiator unit has a built-in parameter mapping table, constructed based on extensive water test data, covering a water flow velocity range of 0.01-5 m / s and a wave period range of 1-10 s. Each range corresponds to a set of optimal tracking velocity factor and filtering factor values. When the tracking differentiator unit receives the water flow velocity and wave period parameters, it first uses an interpolation algorithm to look up the parameter mapping table. If the received parameters fall between two ranges in the mapping table, linear interpolation is used to calculate the corresponding tracking velocity factor and filtering factor, ensuring the continuity of parameter adjustment. The tracking speed factor ranges from 0.1 to 10. The higher the water flow velocity and the shorter the wave period, the larger the tracking speed factor, enabling the tracking differentiator to quickly track the robot's actual motion state. The filtering factor ranges from 0.01 to 1. The higher the water flow velocity and the shorter the wave period, the smaller the filtering factor, improving the tracking differentiator's anti-interference capability. The algorithm calculation of the tracking differentiator unit is performed by the FPGA chip of the adaptive ADRC control unit. The FPGA chip adopts a pipelined computing architecture, which can complete the adjustment of tracking transition process parameters and tracking differential calculation within 0.1ms. The calculation result is output to the signal integration node in the form of a 16-bit digital signal. The tracking differentiator unit also has a parameter smoothing adjustment mechanism. When the water flow velocity parameter and wave period parameter change abruptly, the tracking transition process parameter will not change immediately, but will be gradually adjusted using exponential smoothing with a smoothing time constant of 0.5s. This avoids oscillations in the tracking differentiator output signal due to parameter abrupt changes, further improving the signal integration accuracy of the adaptive ADRC control unit. In addition, the tracking differentiator unit will feed back the adjusted tracking transition process parameters to the coupling parameter calculation unit in real time, which will serve as a reference for verifying the validity of the parameters of the coupling parameter calculation unit, thus forming a two-way linkage of parameters.

[0029] The existing technology has the following technical problems: the fluid resistance correction parameters lack scientific calculation basis and cannot accurately reflect the coupled influence of water turbidity, salinity and water flow velocity on fluid resistance, resulting in large deviations in subsequent control parameter corrections.

[0030] Based on this, the coupling parameter calculation unit generates the fluid resistance correction parameters using the fluid resistance coupling correction coefficient formula, which is as follows: ; In the formula For fluid resistance correction parameters, The turbidity influence coefficient is... This refers to the turbidity parameter of the water body. This is the salinity influence coefficient. For water salinity parameters, The coefficient representing the influence of water flow velocity. For water flow velocity parameters, These are parameters related to gravitational acceleration. For the diameter parameters of the propeller blades.

[0031] It is worth mentioning that this formula is derived from the classical fluid dynamics theory of flow resistance. This theory shows that the resistance experienced by an object moving in a fluid is coupled with the fluid's physical properties, the object's velocity, and its dimensions. Based on this theory, this formula, for the first time, incorporates the often-neglected physical properties of water—turbidity and salinity—into the calculation system for fluid resistance. It also considers the influence of the propeller blade diameter on the water flow velocity, enabling the calculated fluid resistance correction parameters to more accurately reflect the actual water resistance characteristics. The base value 1 in the formula represents the correction coefficient corresponding to the basic fluid resistance experienced by the propeller in a clean, freshwater environment without water flow. Based on this, by superimposing the coupled effects of turbidity, salinity, and water flow velocity, precise correction of fluid resistance under different aquatic environments is achieved. As a turbidity influence coefficient, it is a dimensionless parameter with a value ranging from 0.0001 to 0.001. This coefficient is determined by the average particle size and density of suspended particles in the water body; the larger the particle size and the higher the density, the better. The larger the value, the more suitable it is for on-site calibration based on different water body types, such as river waterways. Take 0.0003, lake water area Take 0.0002, nearshore waters Take 0.0008; The turbidity parameter of the water body, measured in NTUs, is a dimensionless parameter directly detected by the turbidity sensor of the water medium acquisition subunit, reflecting the content of suspended particles in the water body. The higher the value, the more suspended particles there are in the water, and the greater the frictional resistance experienced by the propulsion unit. The salinity influence coefficient is also a dimensionless parameter, ranging from 0.5 to 2. The effect of salinity on fluid resistance is mainly reflected in the change of water density. The higher the salinity, the greater the water density, and the greater the pressure drag experienced by the thruster. The value can be determined based on the salinity range of the water body, such as freshwater areas. Take 0.5, brackish water area Take 1.2, seawater area Take 2; The salinity parameter is expressed in per mille (‰), and is dimensionless. It is detected by the salinity sensor of the water medium acquisition subunit. The formula uses... Perform a per mille conversion to ensure that the dimension of the influence of salinity is consistent with that of other parameters, thus avoiding calculation errors caused by differences in numerical magnitude. The velocity coefficient is a dimensionless parameter, ranging from 0.1 to 0.5. This coefficient is related to the number of propeller blades and the blade pitch; the more blades and the larger the pitch, the better. The larger the value of ; The water flow velocity parameter, in m / s, is detected by the Doppler water flow velocity sensor of the water medium acquisition subunit and reflects the relative motion velocity between the propeller and the water body. This is a parameter for gravitational acceleration, with dimensions in m / s². 2 A fixed value of 9.8 is taken as the basic parameter for dimensional normalization; Let be the diameter parameter of the propeller blade, in meters (m), and be the fixed structural parameter of the propeller for the water surface cleaning robot, determined according to the propeller model. The formula uses... For water flow velocity parameters Perform dimensional normalization to make The entire formula is dimensionless, ensuring that all terms are dimensionless, satisfying the principle of dimensional homogeneity and avoiding calculation errors caused by inconsistent dimensions. The calculation of this formula is performed by the floating-point unit of the STM32H743 microcontroller in the coupled parameter calculation unit. During the calculation, all parameters are used as double-precision floating-point numbers, achieving a calculation precision of 10⁻⁶. -6 This effectively reduces the correction deviation of subsequent control parameters. The innovation of this formula lies in breaking the limitation of existing technology that only considers the influence of a single water flow velocity factor on fluid resistance. It realizes the coupled calculation of multiple factors such as turbidity, salinity and water flow velocity. At the same time, through scientific dimensional normalization, it ensures the scientificity and accuracy of the calculation results, so that the fluid resistance correction parameters can be accurately matched to the actual conditions of different aquatic environments.

[0032] The existing technology has the following technical problems: the logic for generating the disturbance observation gain parameter of the extended state observer is unclear and does not take into account the coupling relationship between fluid resistance and load, which leads to a mismatch between the gain parameter and the actual disturbance intensity, affecting the disturbance observation effect.

[0033] Based on this, when the coupling parameter calculation unit generates the disturbance observation gain parameter of the expansion state observer, it combines the fluid resistance correction parameter, robot load parameter, water density parameter and thruster blade diameter parameter, and obtains the disturbance observation gain parameter of the expansion state observer through a specific calculation logic. The calculation logic is constructed based on the coupling relationship between fluid resistance and load to ensure that the disturbance observation gain parameter of the expansion state observer is adapted to the actual disturbance intensity.

[0034] It is worth mentioning that the calculation logic in the coupling parameter calculation unit used to generate the disturbance observation gain parameters of the extended state observer is based on the classical disturbance observation theory of the extended state observer. This theory shows that the disturbance observation gain of the extended state observer must be positively correlated with the actual disturbance intensity experienced by the system. The greater the disturbance intensity, the greater the observation gain needs to be to achieve rapid and accurate observation of the disturbance. This calculation logic takes fluid resistance and robot load as the core factors determining the disturbance intensity. Fluid resistance is quantified through the fluid resistance correction parameter, and robot load is quantified through the robot load parameter. At the same time, water density parameters and thruster blade diameter parameters are introduced to normalize the dimensions of the robot load parameters to ensure the dimensional homogeneity of the calculation logic. The water density parameter is calculated by the coupling parameter calculation unit based on the water salinity parameter. For every 1‰ increase in salinity, the water density increases by 0.001 kg / m³. 3The conversion formula is permanently stored in the coupling parameter calculation unit, eliminating the need for additional sensors and simplifying the hardware structure. The calculation logic is executed by the floating-point unit of the coupling parameter calculation unit. First, the fluid resistance correction parameter is multiplied by the dimensionally normalized robot load parameter to obtain a quantized disturbance intensity value. Then, this quantized value is weighted and superimposed with the base gain parameter of the extended state observer to obtain the final disturbance observation gain parameter of the extended state observer. The weighting coefficient is a dimensionless parameter with a value range of 0.01-0.1, which can be calibrated according to the robot's motion characteristics. The coupling parameter calculation unit sets a disturbance intensity threshold for this calculation logic. When the calculated quantized disturbance intensity value exceeds the threshold, the disturbance observation gain parameter of the extended state observer is limited. The upper limit of the limit is three times the base gain parameter to prevent oscillations in the extended state observer due to excessively large gain parameters, ensuring the stability of disturbance observation. Furthermore, the coupling parameter calculation unit associates and stores the generated expansion state observer disturbance observation gain parameters with the quantized disturbance intensity values ​​in real time, forming a mapping relationship between the gain parameters and the disturbance intensity. When the system encounters the same disturbance intensity again, it can directly call the corresponding gain parameters without recalculation, further improving parameter generation efficiency. The innovation of this calculation logic lies in clarifying the influence mechanism of the coupling relationship between fluid resistance and load on the expansion state observer disturbance observation gain, breaking the limitation of the existing technology that uses fixed gain parameters, enabling the gain parameters to dynamically match the actual disturbance intensity, and significantly improving the disturbance observation effect of the expansion state observer.

[0035] The existing technology has the following technical problems: the generation of nonlinear state error feedback coefficient parameters does not take into account both attitude disturbances and observation accuracy, resulting in a mismatch between error feedback processing and actual control requirements, which affects control accuracy.

[0036] Based on this, when the coupling parameter calculation unit generates the nonlinear state error feedback coefficient parameter, it combines the extended state observer disturbance observation gain parameter, robot roll angle parameter, wave period parameter, running time parameter, thruster rotational inertia parameter and extended state observer basic bandwidth parameter, and obtains the nonlinear state error feedback coefficient parameter through a specific calculation logic. The calculation logic takes into account the coordinated adaptation of attitude disturbance and observation accuracy.

[0037] It is worth mentioning that the calculation logic in the coupling parameter calculation unit used to generate the nonlinear state error feedback coefficient parameters is based on the error compensation theory of nonlinear state error feedback. This theory indicates that the error feedback coefficient needs to be compatible with the system's attitude disturbance intensity and disturbance observation accuracy. The stronger the attitude disturbance, the larger the error feedback coefficient needs to be to achieve effective compensation for the attitude disturbance; the higher the observation accuracy, the more refined the adaptability of the error feedback coefficient needs to be to fully utilize the role of observation. This calculation logic uses the robot's roll angle and wave period as the core quantitative indicators of attitude disturbance intensity, and the disturbance observation gain parameter of the extended state observer as the quantitative indicator of observation accuracy. It also introduces the running time parameter to reflect the periodicity of wave attitude disturbance, and introduces the thruster rotational inertia parameter and the extended state observer's basic bandwidth parameter for dimensional normalization processing to ensure the scientific nature of the calculation logic. The running time parameter is the system clock parameter of the coupling parameter calculation unit, updated in real time, used to capture the sinusoidal variation characteristics of the wave period, so that the generated nonlinear state error feedback coefficient parameters can be dynamically adjusted according to the periodic changes of the waves. The thruster rotational inertia parameter is a fixed structural parameter of the thruster, with dimensions in kg·m. 2The parameters are determined based on the thruster model and stored in the flash memory of the coupling parameter calculation unit. The basic bandwidth parameter of the extended state observer is a fixed performance parameter of the extended state observer, with dimensions in rad / s, determined by the design specifications of the ADRC control algorithm, and is also stored in the coupling parameter calculation unit. The calculation logic is divided into two stages. The first stage is attitude disturbance intensity quantization, which combines the robot roll angle parameter and wave period parameter with the running time parameter, and obtains the dynamic quantization value of the attitude disturbance through sine function calculation. The larger the roll angle and the shorter the wave period, the larger the dynamic quantization value of the attitude disturbance. The second stage is observation accuracy adaptation, which normalizes the dimensions of the extended state observer disturbance observation gain parameter through the thruster rotational inertia parameter and the basic bandwidth parameter of the extended state observer to obtain the observation accuracy quantization value. The larger the observation gain parameter, the larger the observation accuracy quantization value. Finally, the dynamic quantized value of the attitude disturbance and the quantized value of the observation accuracy are weighted and summed, and then superimposed with the basic coefficient parameters of the nonlinear state error feedback to obtain the final nonlinear state error feedback coefficient parameters. Both weighting coefficients are dimensionless parameters. The attitude disturbance weighting coefficient ranges from 0.2 to 0.6, and the observation accuracy weighting coefficient ranges from 0.1 to 0.3, which can be calibrated on-site according to the robot's cleaning operation scenario. The coupled parameter calculation unit sets upper and lower limits for the error feedback coefficients of this calculation logic. The lower limit is 0.5 times the basic coefficient parameters, and the upper limit is 2 times the basic coefficient parameters, avoiding overshoot or slow adjustment problems caused by excessively large or small coefficient parameters. The innovation of this calculation logic lies in the fact that it is the first time that the periodicity of the attitude disturbance and the disturbance observation accuracy are considered together, breaking the limitation of existing technologies that only consider a single error factor in the design of feedback coefficients. This allows the nonlinear state error feedback coefficient parameters to accurately match the actual control requirements, effectively improving the control accuracy of the system.

[0038] The existing technology has the following technical problems: the extended state observer unit does not effectively utilize the generated extended state observer disturbance observation gain parameters and still uses fixed observation logic, which leads to disturbance observation lag and affects the timeliness of control response.

[0039] Based on this, the extended state observer unit included in the adaptive ADRC control unit receives the extended state observer disturbance observation gain parameters transmitted by the coupling parameter calculation unit. The extended state observer unit substitutes the extended state observer disturbance observation gain parameters into the disturbance observation logic to perform disturbance observation processing. The extended state observer unit transmits the output signal after disturbance observation processing to the signal integration node of the adaptive ADRC control unit.

[0040] It is worth mentioning that the extended state observer unit adopts a second-order linear extended state observer algorithm. The core perturbation observation logic of this algorithm is determined by the observation gain matrix. The perturbation observation gain parameter of the extended state observer is the core element of this observation gain matrix, directly determining the perturbation observation speed and accuracy of the observer. The FPGA chip of the extended state observer unit has the second-order linear extended state observer's operation logic embedded in it. When it receives the extended state observer perturbation observation gain parameter transmitted by the coupling parameter calculation unit, it immediately updates the parameter to the corresponding position in the observation gain matrix. The update process takes no more than 0.05ms, realizing dynamic adjustment of the observation logic, rather than the fixed observation logic of existing technologies. The input signals of the extended state observer unit include the thruster's rotational speed feedback signal and the output command signal of the adaptive ADRC control unit. Both input signals are obtained from the signal integration node of the adaptive ADRC control unit, and the sampling frequency is consistent with that of the multi-dimensional parameter acquisition unit, which is 20Hz. The extended state observer unit separates the total disturbance experienced by the system from the input signal through disturbance observation logic, including fluid resistance disturbance, load disturbance, and attitude disturbance. The disturbance observation delay is controlled within 0.5ms, far less than the system control cycle, ensuring timely control response. The observed disturbance signal, together with the compensation signal, is transmitted as an output signal to the signal integration node. The output signal is a 16-bit digital signal, consistent with the output signal format of other sub-units, facilitating signal integration. The extended state observer unit also incorporates an observation error compensation mechanism. By calculating the deviation between the observed disturbance and the actual disturbance, a small correction is made to the disturbance observation gain parameter of the extended state observer. The correction amount is 0.1%-1% of the gain parameter, ensuring that the accuracy of disturbance observation remains at a high level. The actual disturbance is calculated by the coupling parameter calculation unit based on various original parameters and correction parameters, and transmitted to the extended state observer unit via the CAN bus. Furthermore, the extended state observer unit feeds back the observation error to the coupling parameter calculation unit in real time, serving as a reference for the coupling parameter calculation unit to adjust the calculation logic of the extended state observer's disturbance observation gain parameter, thus forming a closed-loop optimization of disturbance observation. The innovation of this unit lies in breaking the limitation of the fixed observation logic of the extended state observer, realizing the real-time updating of the observation gain parameter and the dynamic adjustment of the observation logic, effectively solving the problem of disturbance observation lag, and greatly improving the timeliness of system control response.

[0041] The existing technology has the following technical problems: the nonlinear state error feedback unit does not adjust the error feedback logic in conjunction with the generated nonlinear state error feedback coefficient parameters, resulting in insufficient accuracy of error feedback processing and affecting the control effect.

[0042] Based on this, the nonlinear state error feedback unit included in the adaptive ADRC control unit receives the nonlinear state error feedback coefficient parameters transmitted by the coupling parameter calculation unit. The nonlinear state error feedback unit substitutes the nonlinear state error feedback coefficient parameters into the error feedback logic to perform error feedback processing. The nonlinear state error feedback unit transmits the output signal after error feedback processing to the signal integration node of the adaptive ADRC control unit.

[0043] It is worth mentioning that the nonlinear state error feedback unit uses the fal function as its core error feedback logic. This function is a nonlinear continuous function that can automatically adjust the feedback strength according to the magnitude of the error, achieving high precision for small errors and rapid adjustment for large errors. The nonlinear state error feedback coefficient parameter is the shape control parameter of the fal function, directly determining the nonlinear characteristics of the function. The FPGA chip of the nonlinear state error feedback unit has the fal function's operation logic embedded in it. When it receives the nonlinear state error feedback coefficient parameter transmitted by the coupling parameter calculation unit, it immediately updates the parameter to the corresponding position of the fal function, realizing dynamic adjustment of the error feedback logic. The update process does not require interruption of the operation, ensuring the continuity of error feedback processing. The input signal of the nonlinear state error feedback unit is the deviation between the tracking signal output by the tracking differentiator unit and the robot's actual motion state signal. The actual motion state signal is obtained by comprehensive calculation from various sensors of the multi-dimensional parameter acquisition unit, including the robot's speed and heading. The calculation of the deviation signal is completed by the MCU chip of the adaptive ADRC control unit, with a calculation accuracy of 16 bits. The nonlinear state error feedback unit inputs the deviation signal into the adjusted fal function, performs nonlinear error feedback calculation, and outputs the result as an error compensation signal to the signal integration node. The calculation delay is controlled within 0.2ms to ensure real-time error feedback processing. The nonlinear state error feedback unit sets a deviation threshold. When the absolute value of the deviation signal exceeds the threshold, it automatically increases the effective value of the nonlinear state error feedback coefficient parameter to enhance the error feedback strength and achieve rapid compensation for large deviations. When the absolute value of the deviation signal is less than the threshold, it decreases the effective value of the coefficient parameter to improve the accuracy of the error feedback and achieve precise compensation for small deviations. The deviation threshold can be set according to the robot's control accuracy requirements, with a range of 0.01-0.1. Furthermore, the nonlinear state error feedback unit feeds back the compensation effect after error feedback processing to the coupling parameter calculation unit in real time. The compensation effect is quantified by the error decay rate; a higher error decay rate indicates a better compensation effect. The coupling parameter calculation unit adjusts the calculation logic of the nonlinear state error feedback coefficient parameter based on the error decay rate, forming a closed-loop optimization of the error feedback. The innovation of this unit lies in the deep integration of dynamic nonlinear state error feedback coefficient parameters with the nonlinear error feedback logic of the fal function, breaking the limitation of fixed error feedback logic in existing technologies, realizing dynamic adaptation of error feedback strength and accuracy, effectively improving the accuracy of error feedback processing, and improving the control effect of the system.

[0044] The existing technology has the following technical problems: the drive control signal generated by the adaptive ADRC control unit is not effectively transmitted to the thruster drive execution unit, or the thruster drive execution unit does not accurately execute the control command, resulting in the control logic failing to be implemented.

[0045] Based on this, the signal integration node of the adaptive ADRC control unit integrates the output signals of the tracking differentiator unit, the extended state observer unit, and the nonlinear state error feedback unit to generate a drive control signal. The adaptive ADRC control unit transmits the drive control signal to the thruster drive execution unit, which performs thruster speed adjustment and thruster direction adjustment according to the drive control signal.

[0046] It is worth mentioning that the signal integration node is a dedicated digital signal processing circuit, integrating a multi-channel signal input interface, a signal superposition operation module, a D / A conversion module, and a signal amplification module. The output signals of the tracking differentiator unit, the extended state observer unit, and the nonlinear state error feedback unit are all connected to the signal integration node through differential input interfaces. The differential input method can effectively suppress common-mode interference during signal transmission, ensuring the accuracy of the input signal. The signal superposition operation module is a hardware adder that linearly superimposes the output signals of the three sub-units. The superposition weights are determined by the design specifications of the ADRC control algorithm, which are 0.3 for the tracking differentiator unit output signal, 0.4 for the extended state observer unit output signal, and 0.3 for the nonlinear state error feedback unit output signal. The superimposed signal is a digital drive control signal with a precision of 16 bits. The D / A conversion module converts the digital drive control signal into an analog voltage signal of 0-10V with a conversion precision of 16 bits and a linearity of ±0.01%, ensuring a linear correspondence between the analog and digital signals. The signal amplification module employs an operational amplifier to amplify the 0-10V analog voltage signal, enabling it to drive the thruster drive execution unit. The amplified signal has a 200mA driving capability and includes an overvoltage protection mechanism, ensuring the output voltage does not exceed 10.5V to prevent damage to the thruster drive execution unit. The adaptive ADRC control unit and the thruster drive execution unit use a shielded cable for signal transmission, with the shield grounded to effectively shield against electromagnetic interference. The signal transmission distance can reach 10m, adapting to the hull layout of the water surface cleaning robot. Upon receiving the drive control signal, the thruster drive execution unit converts it into a PWM drive signal using an internal PID adjustment algorithm. The PWM signal has a frequency of 20kHz and a continuously adjustable duty cycle of 0-100%. The duty cycle is linearly related to the thruster's rotational speed. The positive or negative sign of the drive control signal controls the thruster's forward and reverse rotation directions: forward rotation corresponds to the robot moving forward and turning left, while reverse rotation corresponds to the robot moving backward and turning right. The thruster drive actuator unit incorporates a speed feedback module. It detects the actual thruster speed using a Hall sensor and compares it with the target speed to achieve closed-loop speed control. The speed control accuracy is ±5 r / min, ensuring precise execution of control commands. Furthermore, the thruster drive actuator unit feeds back parameters such as the thruster's actual speed, operating current, and working status to the adaptive ADRC control unit in real time. The adaptive ADRC control unit fine-tunes the drive control signal based on the feedback parameters, forming a closed-loop motion control system to ensure precise implementation of the control logic.The innovation of this part lies in the construction of a precise control mechanism that spans the entire chain from signal integration and transmission to execution. By using hardware-based signal integration nodes and closed-loop speed control, it solves the problems of distortion in drive control signal transmission and inaccurate execution instructions, ensuring the effective implementation of the entire ADRC control logic and realizing precise control of the movement of the water surface cleaning robot.

[0047] In the specific implementation process, the coupling parameter calculation unit generates the extended state observer perturbation observation gain parameters using the ESO perturbation observation gain coupling formula, which is: ; in the formula The perturbation observation gain parameter for the extended state observer has dimensions of ; The basic extended state observer gain parameter, with dimensions and Consistent; is the load resistance coupling coefficient, which is a dimensionless parameter; This is a fluid resistance correction parameter, which is a dimensionless parameter. Here are the robot's load parameters, in kg. This is a water density parameter, with dimensions in kg / m³. 3 ; Let be the diameter parameter of the propeller blade, with dimensions in meters (m). The theoretical basis of this formula is that the disturbance observation gain of the extended state observer must be positively correlated with the actual disturbance intensity experienced by the system. The main disturbances experienced by the water surface cleaning robot during its movement are fluid resistance disturbance and load inertia disturbance. These two disturbances are coupled and jointly determine the total disturbance intensity. Therefore, the formula uses the fluid resistance correction parameter and the robot load parameter as core variables to construct the coupling relationship between the gain parameter and the total disturbance intensity. In the formula... The basic extended state observer gain parameter is the basic gain designed for the extended state observer under ideal conditions of clean fresh water, no load, and no water flow. This parameter is determined by the bandwidth design index of the ADRC control algorithm and is the benchmark value of the entire gain parameter. Its value must ensure that the extended state observer has good disturbance observation performance under ideal conditions, without observation oscillation or significant observation lag. The load-resistance coupling coefficient serves as a weighting adjustment parameter for the two disturbances. It is a dimensionless parameter with a value range of 0.001-0.01. The value of this coefficient is determined by the hull characteristics and propeller performance of the water surface cleaning robot. A better streamlined hull and greater propeller thrust result in higher thrust. The smaller the value, the larger the value; conversely, the larger the value, the more likely it is to be calibrated through a water tank test. During calibration, the water environment and load weight are varied to test different values. The perturbation observation effect at the specified value is evaluated, and the value with the smallest observation error is selected as the calibration value. This is a fluid resistance correction parameter, calculated using the fluid resistance coupling correction coefficient formula mentioned earlier. It is a dimensionless parameter that accurately quantifies the intensity of fluid resistance disturbances under different aquatic environments. The larger the value, the stronger the fluid resistance disturbance, and the greater the disturbance observation gain parameter of the expansion state observer needs to be to achieve rapid observation of the fluid resistance disturbance. The load parameter, measured in kg, is obtained by the load detection subunit. It quantifies the intensity of the robot's load inertial disturbance. The greater the load, the greater the robot's motion inertia, the stronger the load inertial disturbance, and the greater the demand for the gain parameter. This is a water density parameter, with dimensions in kg / m³. 3 The coupling parameter calculation unit calculates the parameters based on the water salinity parameters. Let be the diameter parameter of the propeller blade, with dimensions in meters, and be the fixed structure parameter of the propeller. The formula uses... Robot load parameters Perform dimensional normalization to make The entire term is dimensionless, ensuring that all terms within the square brackets are dimensionless, consistent with the fundamental gain parameter. After multiplication, Dimensions and To maintain consistency and satisfy the principle of dimensional homogeneity, the logical derivation of this formula consists of three steps. The first step is to determine the quantification method of the total disturbance intensity, coupling the fluid resistance disturbance with the load inertia disturbance, and using multiplication to represent the coupling relationship between the two, thus obtaining... The first step is to initially quantify the total disturbance intensity; the second step is to normalize the dimensions of the initially quantified total disturbance intensity, through... Dimensionless total disturbance intensity is obtained by eliminating dimensions, and a load-resistance coupling coefficient is introduced at the same time. Adjusting the degree of influence of the total disturbance intensity on the gain parameter, we obtain The third step is to superimpose the dimensionless total disturbance intensity quantized value with the base value 1, and then add it to the base extended state observer gain parameter. Multiplying these parameters yields the final extended state observer perturbation observation gain parameters. The base value of 1 is added to ensure that in an ideal, undisturbed environment, equal This ensures observation performance under ideal conditions. The formula is implemented by the STM32H743 microcontroller of the coupled parameter calculation unit. The microcontroller's floating-point unit supports multi-precision floating-point multiplication and division operations. During the calculation, intermediate results are stored in double precision to avoid precision loss. The calculation time of the formula is less than 1ms, meeting real-time requirements. The core innovation of this formula lies in the first realization of coupled calculation of fluid resistance disturbance and load inertia disturbance. Through scientific dimensional normalization, a dynamic method for calculating the disturbance observation gain parameter of the extended state observer is constructed. This breaks through the technical limitations of existing technologies that use fixed gain parameters, enabling the gain parameter to accurately match the actual total disturbance intensity. This significantly improves the disturbance observation speed and accuracy of the extended state observer and effectively solves the problem of disturbance observation lag.

[0048] In the specific implementation process, the coupling parameter calculation unit generates the nonlinear state error feedback coefficient parameters using the attitude wave coupling feedback coefficient formula, which is: ; in the formula represents the nonlinear state error feedback coefficient parameter, with dimensions in rad / s; The basic nonlinear state error feedback coefficient parameters, with dimensions and Consistent; Let be the attitude angle influence coefficient, which is dimensionless and 1 / rad. Let be the robot's roll angle parameter, measured in rad; Let be the wave period parameter, with dimensions in seconds; The runtime parameter is measured in seconds (s). is the extended state observer gain coupling coefficient, and is a dimensionless parameter; The perturbation observation gain parameter for the extended state observer has dimensions of ; The moment of inertia parameter of the thruster is expressed in kg·m. 2 ; The basic bandwidth parameter of the extended state observer is expressed in rad / s. The theoretical basis of this formula is that the nonlinear state error feedback coefficient needs to simultaneously adapt to the robot's attitude perturbation characteristics and the perturbation observation accuracy of the extended state observer. The attitude perturbations experienced by the water surface cleaning robot in a wave environment exhibit significant periodicity, determined by the wave period. The perturbation observation accuracy is determined by the perturbation observation gain parameter of the extended state observer. Therefore, the formula couples the periodicity quantification index of the attitude perturbation with the observation accuracy quantification index, constructing a dynamic method for calculating the error feedback coefficient parameter. In the formula... The basic nonlinear state error feedback coefficient parameter is a fundamental coefficient designed under ideal conditions of no waves and stable attitude. Its dimension is rad / s. It is determined by the error adjustment performance index of the ADRC control algorithm. Its value must ensure that the error adjustment of the system under ideal conditions has neither overshoot nor fast adjustment speed. It is the benchmark value of the entire error feedback coefficient. The attitude angle influence coefficient is a dimensionless 1 / rad, with a value ranging from 0.1 to 1 rad. -1 This coefficient is used to adjust the influence of the robot's roll angle on the error feedback coefficient. The larger the roll angle, the stronger the attitude disturbance, and the larger the error feedback coefficient needs to be. The value is calibrated through wave tests in a water tank, where different values ​​are tested at different wave levels. To assess the error compensation effect at the given value, the value with the highest attitude stability was selected as the calibration value. The yaw angle parameter, measured in rad, is obtained by the motion attitude acquisition subunit and accurately quantifies the amplitude of the robot's attitude perturbation. The larger the absolute value, the more severe the robot's posture deviation and the stronger the posture disturbance it is subjected to. The wave period parameter, with dimensions in seconds, is extracted by the motion attitude acquisition subunit. Let be the runtime parameter, measured in seconds (s), and be the system's real-time clock parameter. The formula uses... It reflects the periodicity of wave attitude disturbances. The period of this sine function is consistent with the wave period, and the value range is from -1 to 1. It can accurately capture the changing pattern of wave attitude disturbances over time, so that the error feedback coefficient parameter can be dynamically adjusted according to the periodic changes of the waves, and realize real-time compensation for attitude disturbances. The extended state observer gain coupling coefficient is a dimensionless parameter ranging from 0.01 to 0.1. This coefficient adjusts the influence of the extended state observer's perturbation observation gain parameter on the error feedback coefficient. A larger observation gain parameter results in higher observation accuracy, requiring corresponding fine-tuning of the error feedback coefficient. The value of is determined by the observation accuracy characteristics of the extended state observer; the higher the observation accuracy, the better. The larger the value, the better. The perturbation observation gain parameter for the extended state observer has dimensions of The calculation, obtained from the ESO perturbation observation gain coupling formula mentioned above, accurately quantifies the perturbation observation accuracy of the extended state observer. The larger the value, the higher the observation accuracy, and the more refined the adaptability of the error feedback coefficient needs to be. The moment of inertia parameter of the thruster is expressed in kg·m. 2 , which are the fixed structural parameters of the thruster. The fundamental bandwidth parameter of the extended state observer, in rad / s, is determined by the design specifications of the ADRC control algorithm, and is expressed in the formula through... right Perform dimensional normalization to make The entire term is dimensionless, ensuring that all terms within the square brackets are dimensionless, and is consistent with the basic nonlinear state error feedback coefficient parameters. After multiplication, Dimensions and To maintain consistency and satisfy the principle of dimensional homogeneity, the logical derivation of this formula consists of three steps. The first step is to quantify the dynamic intensity of the attitude disturbance by multiplying the robot's roll angle parameter by a sine function reflecting the wave's periodicity to obtain the dynamic quantized value of the attitude disturbance. Then, the attitude angle influence coefficient is introduced. Adjusting its degree of influence to obtain This value can accurately capture the intensity of attitude perturbations at different times; the second step is to quantify the impact of observation accuracy on the error feedback coefficient, normalize the dimensions of the extended state observer perturbation observation gain parameter, and then introduce the extended state observer gain coupling coefficient. Adjusting its degree of influence to obtain This value accurately reflects the adaptation requirements of observation accuracy to the error feedback coefficient; the third step is to superimpose the dynamic quantization value of attitude disturbance and the quantization value of the impact of observation accuracy with the base value 1 respectively, and then with the base nonlinear state error feedback coefficient parameter. Multiplying them yields the final nonlinear state error feedback coefficient parameters. The base value of 1 is superimposed to ensure that, under ideal conditions with no attitude disturbances and observation accuracy at the basic level, equal This ensures error adjustment performance under ideal conditions. The formula is implemented by the STM32H743 microcontroller of the coupled parameter calculation unit. The microcontroller's floating-point unit supports various operations such as sine function, square root, multiplication, and addition. The sine function calculation uses the CORDIC algorithm, which has high accuracy and speed. The overall calculation time of the formula is less than 2ms, meeting the real-time requirements of the system. The core innovation of this formula lies in the first coupling calculation of the periodicity of attitude disturbance with the disturbance observation accuracy. It constructs a nonlinear state error feedback coefficient parameter calculation method that can dynamically adjust with the wave cycle, breaking the technical limitation of the existing technology that uses a fixed error feedback coefficient. This allows the error feedback coefficient to accurately match the attitude disturbance intensity and observation accuracy at different times, realizing real-time and accurate compensation for attitude disturbance, and significantly improving the motion stability and control accuracy of the water surface cleaning robot in a wave environment.

[0049] The above are merely preferred embodiments of the present invention and are not intended to limit the present invention in any other way. Any person skilled in the art may make changes or modifications to the above-disclosed technical content to create equivalent embodiments that can be applied to other fields. However, any simple modifications, equivalent changes, and modifications made to the above embodiments based on the technical essence of the present invention without departing from the scope of the present invention shall still fall within the protection scope of the present invention.

Claims

1. A motion planning and control system for a water surface cleaning robot based on ADRC, characterized in that, It includes a multi-dimensional parameter acquisition unit, a coupled parameter calculation unit, an adaptive ADRC control unit, and a thruster drive execution unit; The multi-dimensional parameter acquisition unit is used to acquire water turbidity parameters, water salinity parameters, water flow velocity parameters, robot load parameters, wave period parameters, and robot roll angle parameters. The coupling parameter calculation unit is used to generate fluid resistance correction parameters based on water turbidity, salinity, and flow velocity parameters. It is also used to generate extended state observer perturbation observation gain parameters based on the fluid resistance correction parameters and robot load parameters. Furthermore, it is used to generate nonlinear state error feedback coefficient parameters based on the extended state observer perturbation observation gain parameters, wave period parameters, and robot roll angle parameters. The adaptive ADRC control unit includes a tracking differentiator unit, an extended state observer unit, and a nonlinear state error feedback unit. The extended state observer unit performs perturbation observation processing using the extended state observer perturbation observation gain parameters, and the nonlinear state error feedback unit performs error feedback processing using the nonlinear state error feedback coefficient parameters. The adaptive ADRC control unit generates drive control signals based on the output signals of the tracking differentiator unit, the extended state observer unit, and the nonlinear state error feedback unit. The thruster drive execution unit is used to perform motion drive processing for the water surface cleaning robot according to the drive control signal.

2. The motion planning and control system for the water surface cleaning robot based on ADRC according to claim 1, characterized in that, The multi-dimensional parameter acquisition unit includes a water medium acquisition subunit, a motion attitude acquisition subunit, and a load detection subunit. The water medium acquisition subunit is used to acquire water turbidity parameters, water salinity parameters, and water flow velocity parameters. The motion posture acquisition subunit is used to acquire wave period parameters and robot roll angle parameters. The load detection subunit is used to acquire robot load parameters. The water medium acquisition subunit, motion posture acquisition subunit, and load detection subunit all establish data transmission connections with the coupling parameter calculation unit.

3. The motion planning and control system for the water surface cleaning robot based on ADRC according to claim 2, characterized in that, The coupling parameter calculation unit performs parameter generation processing in the following order: first, it generates fluid resistance correction parameters based on the water turbidity parameters, water salinity parameters, and water flow velocity parameters transmitted by the water medium acquisition subunit; then, it generates extended state observer disturbance observation gain parameters based on the fluid resistance correction parameters and the robot load parameters transmitted by the load detection subunit; and finally, it generates nonlinear state error feedback coefficient parameters based on the extended state observer disturbance observation gain parameters, the wave period parameters transmitted by the motion attitude acquisition subunit, and the robot roll angle parameters.

4. The motion planning and control system for the water surface cleaning robot based on ADRC according to claim 3, characterized in that, The adaptive ADRC control unit includes a tracking differentiator unit for receiving water flow velocity parameters transmitted by the water medium acquisition subunit and wave period parameters transmitted by the motion attitude acquisition subunit; the tracking differentiator unit performs tracking transition process parameter adjustment processing according to the water flow velocity parameters and wave period parameters, and transmits the adjusted tracking transition process parameters as the output signal of the tracking differentiator unit to the signal integration node of the adaptive ADRC control unit.

5. The motion planning and control system for the water surface cleaning robot based on ADRC according to claim 4, characterized in that, The coupling parameter calculation unit generates the fluid resistance correction parameters using the fluid resistance coupling correction coefficient formula, which is as follows: ； In the formula, λ is the fluid resistance correction parameter, α is the turbidity influence coefficient, T is the water turbidity parameter, β is the salinity influence coefficient, S is the water salinity parameter, γ is the water flow velocity influence coefficient, v is the water flow velocity parameter, g is the gravitational acceleration parameter, and d is the propeller blade diameter parameter.

6. The motion planning and control system for the water surface cleaning robot based on ADRC according to claim 5, characterized in that, When the coupling parameter calculation unit generates the disturbance observation gain parameter of the expanded state observer, it combines the fluid resistance correction parameter, robot load parameter, water density parameter, and thruster blade diameter parameter to obtain the disturbance observation gain parameter of the expanded state observer through a specific calculation logic. The calculation logic is constructed based on the coupling relationship between fluid resistance and load to ensure that the disturbance observation gain parameter of the expanded state observer is adapted to the actual disturbance intensity.

7. The motion planning and control system for the water surface cleaning robot based on ADRC according to claim 6, characterized in that, When the coupling parameter calculation unit generates the nonlinear state error feedback coefficient parameter, it combines the extended state observer disturbance observation gain parameter, robot roll angle parameter, wave period parameter, running time parameter, thruster rotational inertia parameter, and extended state observer basic bandwidth parameter to obtain the nonlinear state error feedback coefficient parameter through a specific calculation logic; the calculation logic takes into account the coordinated adaptation of attitude disturbance and observation accuracy.

8. The motion planning and control system for a water surface cleaning robot based on ADRC according to claim 7, characterized in that, The adaptive ADRC control unit includes an extended state observer unit that receives the extended state observer disturbance observation gain parameters transmitted by the coupling parameter calculation unit; the extended state observer unit substitutes the extended state observer disturbance observation gain parameters into the disturbance observation logic to perform disturbance observation processing, and transmits the output signal after disturbance observation processing to the signal integration node of the adaptive ADRC control unit.

9. The motion planning and control system for a water surface cleaning robot based on ADRC according to claim 8, characterized in that, The adaptive ADRC control unit includes a nonlinear state error feedback unit that receives nonlinear state error feedback coefficient parameters transmitted by the coupling parameter calculation unit; the nonlinear state error feedback unit substitutes the nonlinear state error feedback coefficient parameters into the error feedback logic to perform error feedback processing, and transmits the output signal after error feedback processing to the signal integration node of the adaptive ADRC control unit.

10. The motion planning and control system for a water surface cleaning robot based on ADRC according to claim 9, characterized in that, The signal integration node of the adaptive ADRC control unit integrates the output signals of the tracking differentiator unit, the extended state observer unit, and the nonlinear state error feedback unit, and generates a drive control signal. The adaptive ADRC control unit transmits the drive control signal to the thruster drive execution unit, which then performs thruster speed adjustment and thruster direction adjustment according to the drive control signal.

Images