An underwater spherical robot trajectory control method and system

Abstract

The application provides a kind of liquid under spherical robot trajectory control method and system, belong to liquid environment spherical inspection robot trajectory tracking control technical field, method includes: based on the pose tracking error of spherical robot reference pose and actual pose, generate including the outer ring tracking instruction of expected speed vector;Based on the speed tracking error of actual speed vector and expected speed vector obtained by four degrees of freedom corresponding observation channel, and generate inner ring basic thrust vector;Speed tracking error and error change rate are input interval two fuzzy reasoning ware, obtain the interval activation intensity output disturbance estimation torque of fuzzy rule under each degree of freedom;The disturbance estimation torque is superimposed to inner ring basic thrust vector, obtain actual control torque, drive spherical robot in liquid three-dimensional trajectory chattering-free tracking.The application can enhance the anti-interference ability of control system, improve the disturbance estimation accuracy, guarantee the smooth and accurate trajectory tracking, adapt to a variety of industrial liquid operation scene.

Description

Technical Field

[0001] This invention relates to the field of trajectory tracking and control technology for spherical inspection robots in liquid environments, specifically to a trajectory control method and system for an underwater spherical robot. Background Technology

[0002] Spherical robots, with their fully symmetrical streamlined structure and zero turning radius, possess irreplaceable application value in confined liquid environments such as water, seawater, transformer insulating oil, and industrial coolant. Typical applications include nuclear power plant fuel pools, underwater abandoned mines, industrial cooling pools, chemical storage tanks, and underwater abandoned buildings, and they are particularly suitable for inspecting the internal windings, core, and tap changers of oil-immersed transformers. Precise three-dimensional trajectory tracking control is the core technology for their autonomous and sophisticated operations, directly determining the robot's operational accuracy and task reliability. Addressing their strongly coupled and highly nonlinear dynamic characteristics, backstepping control (BS), a mature nonlinear control method, has been widely applied in trajectory tracking control through hierarchical recursive construction of Lyapunov functions.

[0003] However, pure backstepping control is highly dependent on accurate hydrodynamic mathematical models. In real liquid environments, unmodeled dynamics such as time-varying flow field disturbances and model parameter perturbations can directly lead to rapid divergence in pose tracking errors, failing to meet the high-precision tracking requirements under complex conditions. To address the problem of insufficient disturbance rejection capability, the industry often introduces sliding mode control (SMC) or Type I fuzzy disturbance observer (T1-FDOB) for optimization, but existing solutions still have significant shortcomings: First, the inherent high-frequency chattering of the control quantity in sliding mode control can significantly shorten the lifespan of the thruster, violating engineering physical execution constraints; Second, T1-FDOB can only describe system uncertainty through a single, definite membership degree. Under extreme conditions such as strong time-varying water flow or sudden step disturbances due to thruster failure, the disturbance estimation accuracy drops drastically, resulting in insufficient robustness; Third, existing control schemes do not have customized designs for the differentiated characteristics of the robot's four degrees of freedom (forward, backward, lateral, heave, and yaw), making them prone to imbalance problems such as overcompensation in the heave channel or jumps in the control quantity in the yaw channel.

[0004] In summary, the main problems with existing trajectory tracking control schemes for spherical robots are: insufficient anti-disturbance capability, error divergence, and control quantity chattering and insufficient estimation accuracy under extreme conditions. Summary of the Invention

[0005] In view of this, it is necessary to provide a trajectory control method and system for underwater spherical robots to solve the problems of insufficient anti-disturbance capability, error divergence, and control quantity chattering and insufficient estimation accuracy in the existing technology.

[0006] To address the aforementioned technical problems, in a first aspect, the present invention provides a trajectory control method for an underwater spherical robot, comprising: The pose tracking error is determined based on the obtained reference pose and actual pose of the spherical robot, and an outer loop tracking command including the desired velocity vector is generated based on the pose tracking error. Observation channels are set up for the four degrees of freedom of forward and backward movement, lateral movement, heave and yaw, and the velocity tracking error between the actual velocity vector and the desired velocity vector after the spherical robot responds to the outer loop tracking command is obtained based on the observation channels. An inner loop basic thrust vector is generated based on the pose tracking error and the velocity tracking error. The error change rate of the velocity tracking error is obtained, and the velocity tracking error and the error change rate are input into the interval type II fuzzy inferencer to obtain the interval activation intensity of the fuzzy rule under each degree of freedom, and the disturbance estimation torque of each degree of freedom is determined based on the interval activation intensity. The estimated disturbance torques of each degree of freedom are superimposed on the inner ring basic thrust vector to obtain the actual control torque, which drives the spherical robot to perform three-dimensional trajectory tracking in the liquid without jitter.

[0007] In one possible implementation, determining the pose tracking error based on the acquired reference pose and actual pose of the spherical robot, and generating an outer-loop tracking command including a desired velocity vector based on the pose tracking error, includes: The pose tracking error is determined based on the deviation between the reference pose and the actual pose, and the time derivative of the pose tracking error is calculated to obtain the dynamic equation of the kinematic error. The kinematic Lyapunov function is constructed according to Lyapunov's second method. The kinematic Lyapunov function includes a square term of the three-dimensional spatial position deviation and a potential energy-like term of the yaw angle deviation. Based on the designed virtual velocity control law, a kinematic Lyapunov function whose time derivative satisfies the semi-negative definite condition is obtained. The time derivative of the kinematic Lyapunov function is then substituted into the kinematic error dynamic equation, and the desired velocity vector is output as the tracking command for the outer loop output.

[0008] In one possible implementation, the square term of the three-dimensional spatial position deviation includes half of the square of the position error of the three degrees of freedom of advance / retreat, lateral movement, and heave. The potential energy term of the yaw angle deviation is the difference between the constant 1 and the cosine value of the yaw angle deviation.

[0009] In one possible implementation, the step of obtaining the velocity tracking error between the actual velocity vector and the desired velocity vector of the spherical robot after responding to the outer ring tracking command based on the observation channel, and generating the inner ring basic thrust vector based on the pose tracking error and the velocity tracking error, includes: The difference between the actual velocity vector and the desired velocity vector in the body coordinate system is taken as the velocity tracking error; A four-degree-of-freedom nonlinear dynamic model is constructed, encompassing forward and backward movement, lateral movement, heave, and yaw. The left-hand side of the dynamic model includes the product of the total inertia matrix and the actual velocity, the product of the Coriolis force and centrifugal force matrix and the actual velocity, the product of the hydrodynamic damping matrix and the actual velocity, and the restoring torque of gravity and buoyancy. The right-hand side includes the generalized thrust vector and a comprehensive disturbance term that incorporates external ocean current disturbances and unmodeled dynamics. The time derivative of the velocity tracking error is calculated and substituted into the nonlinear dynamic model to obtain the inner loop dynamic error evolution equation; Construct a total composite Lyapunov function comprising the kinematic Lyapunov function and the quadratic form of velocity error, wherein the quadratic form of velocity error uses the total inertia matrix as the quadratic form matrix; The Lyapunov derivative of the closed-loop system is obtained by taking the time derivative of the total composite Lyapunov function; a dynamic backstepping control law is designed and substituted into the inner-loop dynamic error evolution equation; combined with the time derivative of the Lyapunov of the closed-loop system, the basic thrust vector is output; the dynamic backstepping control law includes a feedforward cancellation term and a feedback stabilization term. The feedforward cancellation term is used to neutralize the nonlinearity of the dynamic system moving in the liquid, and the feedback stabilization term is used to push the motion system state of the spherical robot to slide down along the negative gradient direction.

[0010] In one possible implementation, after acquiring the velocity tracking error and its filtered rate of change through each observation channel, the method further includes: The velocity tracking error and its filtered rate of change are normalized to dimensionless variables within the standard domain by using a quantization factor matrix.

[0011] In one possible implementation, obtaining the error change rate of the velocity tracking error, inputting the velocity tracking error and the error change rate into an interval type II fuzzy inferencer to obtain the interval activation intensity of the fuzzy rule under each degree of freedom includes: Construct an interval type II fuzzy set bounded by upper and lower membership functions, wherein the uncertain footprint is composed of the area bounded by the upper and lower membership functions; An IF-THEN fuzzy rule table is established for each degree of freedom. The fuzzy rule table includes multiple fuzzy rules. The antecedent of each rule consists of the dimensionless velocity tracking error obtained by normalization and the rate of change of the filtered error, and the consequent is the corresponding perturbation estimation output. Substitute each of the preceding inputs into the corresponding upper and lower membership functions to obtain the upper and lower membership degrees of each preceding. The upper limit activation strength and lower limit activation strength of each rule are determined based on the upper and lower membership degrees of each predecessor, thus obtaining the interval activation strength of each rule; the lower limit activation strength is the product of the lower membership degrees of each predecessor, and the upper limit activation strength is the product of the upper membership degrees of each predecessor.

[0012] In one possible implementation, the interval type-2 fuzzy set of the uncertain footprint is constructed by an expansion model, as shown in the following equation:

[0013] in, Let i be the type II fuzzy set of the i-th interval. The input variable is the dimensionless velocity tracking error or the rate of change of the filtered error. For the universe of discourse of fuzzy input variables, For fuzzy sets In the input Membership function at position, For fuzzy sets In the input The subordinate membership function at that location.

[0014] In one possible implementation, determining the perturbation estimation torque for each degree of freedom based on the interval activation intensity includes: The interval activation intensity is reduced using the Carnick-Mendel iterative algorithm, the interval intensity is sorted in ascending order and the switching point is searched, and the left and right endpoints of the centroid are output. The dimensionless estimate is obtained by averaging the left and right endpoints. The dimensionless estimate is mapped to the actual thrust space by a pre-calibrated physical inverse normalization factor to obtain the original disturbance estimate torque. The original disturbance estimate torque is corrected by using the maximum thrust limit of the thruster, and the corrected torque is constrained within the physical output range of the thruster to output the disturbance estimate torque for each degree of freedom.

[0015] In one possible implementation, the step of superimposing the estimated disturbance torques of each degree of freedom onto the base thrust vector to obtain the actual control torque, driving the spherical robot to perform jitter-free tracking of its three-dimensional trajectory in a liquid, includes: The estimated perturbation torques of each degree of freedom are aggregated through each channel to obtain the total estimated perturbation vector; The total estimated disturbance vector is superimposed onto the inner loop basic thrust vector in a negative feedforward manner to construct an updated model of the composite control law and obtain the actual control torque. Obtain the set upper bound of the observation error and extract the global minimum gain parameter of the controller; The critical boundary is determined based on the upper bound of the observation error, the global minimum gain parameter, and the preset scaling constant. When the state norm of the comprehensive error exceeds the critical boundary, the Lyapunov derivative of the closed-loop system is forced to be negative definite, ensuring that the trajectory tracking error converges to the bounded neighborhood. The robot is driven by the actual control torque to achieve three-dimensional trajectory jitter-free tracking under time-varying flow field disturbances, model parameter perturbations, and sudden thruster failure disturbances in the liquid.

[0016] Secondly, the present invention also provides a trajectory control system for an underwater spherical robot, comprising: The spherical robot body; An inertial measurement unit, installed inside the spherical robot body, is used to measure the robot's actual pose in the inertial coordinate system in real time; A velocity sensor, installed inside the spherical robot body, is used to measure the robot's actual velocity vector in the body coordinate system in real time. Multiple thrusters are distributed on the spherical robot body to drive the robot's movement according to the received actual control torque; An embedded controller is connected to the inertial measurement unit, the velocity sensor, and the multiple thrusters, respectively; the embedded controller includes: The dual-loop backstepping control basic unit is used to determine the pose tracking error based on the acquired reference pose and actual pose of the spherical robot, and to generate an outer loop tracking command including the desired velocity vector based on the pose tracking error; observation channels are set for the four degrees of freedom of forward / backward, lateral, heave, and yaw respectively, and the velocity tracking error between the actual velocity vector and the desired velocity vector of the spherical robot after responding to the outer loop tracking command is obtained based on the observation channels, and an inner loop basic thrust vector is generated based on the pose tracking error and the velocity tracking error; The channel-specific interval type II fuzzy disturbance observation unit is used to obtain the error change rate of the velocity tracking error, input the velocity tracking error and the error change rate into the interval type II fuzzy inference unit, obtain the interval activation intensity of the fuzzy rule under each degree of freedom, and determine the disturbance estimation torque of each degree of freedom based on the interval activation intensity. The feedforward compensation fusion unit is used to superimpose the estimated disturbance torque of each degree of freedom onto the inner loop basic thrust vector to obtain the actual control torque, which drives the spherical robot to perform jitter-free tracking of its three-dimensional trajectory in the liquid.

[0017] The beneficial effects of this invention are as follows: The underwater spherical robot trajectory control method provided by this invention first determines the pose tracking error based on the obtained reference pose and actual pose of the spherical robot, and generates an outer loop tracking command including the desired velocity vector based on the pose tracking error. Observation channels are set up for the four degrees of freedom of advance / retreat, lateral movement, heave, and yaw. Based on the observation channels, the velocity tracking error between the actual velocity vector and the desired velocity vector after the spherical robot responds to the outer loop tracking command is obtained. An inner loop basic thrust vector is generated based on the pose tracking error and the velocity tracking error. This can achieve order reduction and decoupling of the nonlinear system, weaken the dependence on the accurate hydrodynamic model, offset the nonlinearity of underwater motion and eliminate coupling interference, and ensure the smooth convergence of the trajectory tracking error. Secondly, the error rate of change of the velocity tracking error is obtained. The velocity tracking error and error change rate are input into the interval type II fuzzy inference engine to obtain the interval activation intensity of the fuzzy rules under each degree of freedom. Based on the interval activation intensity, the disturbance estimation torque of each degree of freedom is determined. This can adapt to the differentiated motion characteristics of the robot with multiple degrees of freedom, suppress high-frequency noise from the sensor, accurately estimate the comprehensive disturbance caused by time-varying flow field and model parameter perturbation, and improve the robustness against disturbances. Finally, the disturbance estimation torque of each degree of freedom is superimposed on the inner loop basic thrust vector to obtain the actual control torque, which drives the spherical robot to achieve underwater three-dimensional trajectory jitter-free tracking. It can compensate for unknown disturbances in real time, eliminate high-frequency jitter of traditional control to protect the thruster hardware, and suppress low-cost sensor integral drift. It can achieve high-precision and high-stability trajectory tracking control under complex water flow and thruster failure conditions. Attached Figure Description

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

[0019] Figure 1 A schematic flowchart of an embodiment of the trajectory control method for an underwater spherical robot provided by the present invention; Figure 2 This is a schematic flowchart of an embodiment of the invention for generating outer loop tracking instructions; Figure 3 A schematic diagram of an embodiment of the process for generating the basic thrust vector provided by the present invention; Figure 4 A schematic flowchart of an embodiment of the present invention for determining the interval activation intensity of fuzzy rules under each degree of freedom; Figure 5 A schematic flowchart of an embodiment of the interval-type fuzzy inference mechanism provided by the present invention; Figure 6A schematic flowchart of an embodiment of the interval type II fuzzy inference mechanism provided by the present invention; Figure 7 A schematic flowchart illustrating an embodiment of the present invention for determining the disturbance estimation torque of each degree of freedom based on the interval activation intensity; Figure 8 A schematic flowchart of an embodiment of the fuzzy observer provided by the present invention; Figure 9 Provided by the present invention Figure 1 A schematic diagram of an embodiment of S104; Figure 10 A schematic flowchart of another embodiment of the trajectory control method for an underwater spherical robot provided by the present invention; Figure 11 A schematic diagram of an embodiment of the spiral trajectory tracking curve under water flow disturbance provided by the present invention; Figure 12 A schematic diagram of an embodiment of the three-dimensional error curve for spiral trajectory tracking under water flow disturbance provided by the present invention; Figure 13 This is a schematic diagram of an embodiment of the underwater spherical robot trajectory control system provided by the present invention. Detailed Implementation

[0020] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only a part of the embodiments of the present invention, and not all of them. All other embodiments obtained by those skilled in the art based on the embodiments of the present invention without creative effort are within the scope of protection of the present invention.

[0021] In the description of the embodiments of the present invention, unless otherwise stated, "multiple" means two or more. "And / or" describes the relationship between related objects, indicating that there can be three relationships. For example, A and / or B can represent three situations: A exists alone, A and B exist simultaneously, and B exists alone.

[0022] The terms "first," "second," etc., used in the embodiments of this invention are for descriptive purposes only and should not be construed as indicating or implying their relative importance or implicitly specifying the number of technical features indicated. Therefore, a technical feature defined with "first" or "second" may explicitly or implicitly include at least one of that feature.

[0023] In this document, the term "embodiment" means that a particular feature, structure, or characteristic described in connection with an embodiment may be included in at least one embodiment of the invention. The appearance of this phrase in various places throughout the specification does not necessarily refer to the same embodiment, nor is it a separate or alternative embodiment mutually exclusive with other embodiments. It will be explicitly and implicitly understood by those skilled in the art that the embodiments described herein can be combined with other embodiments.

[0024] Before demonstrating the embodiments, the following terms will be explained.

[0025] Spherical robot: A mobile robot with a spherical shape. In this invention, it specifically refers to a spherical robot that can perform inspection tasks in a liquid environment.

[0026] Trajectory tracking control: a technique in the field of control that enables the actual motion trajectory of a controlled object to follow a preset reference trajectory. In this invention, it is used to control the motion path of a spherical robot.

[0027] Backstepping control is a recursive control design method based on Lyapunov stability theory. It is suitable for strictly feedback nonlinear systems and achieves system stability and trajectory tracking by progressively constructing virtual control laws.

[0028] This invention provides a trajectory control method and system for an underwater spherical robot, which will be described below.

[0029] Figure 1 This is a schematic flowchart of an embodiment of the trajectory control method for an underwater spherical robot provided by the present invention, as shown below. Figure 1 As shown, the trajectory control method for an underwater spherical robot includes: S101. Determine the pose tracking error based on the obtained reference pose and actual pose of the spherical robot, and generate an outer loop tracking command including the desired velocity vector based on the pose tracking error. S102. Set up observation channels for the four degrees of freedom of forward and backward movement, lateral movement, heave and yaw. Based on the observation channels, obtain the velocity tracking error between the actual velocity vector and the expected velocity vector after the spherical robot responds to the outer ring tracking command. Generate the inner ring basic thrust vector based on the pose tracking error and velocity tracking error. S103. Obtain the error change rate of the velocity tracking error, input the velocity tracking error and the error change rate into the interval type II fuzzy inferencer, obtain the interval activation intensity of the fuzzy rule under each degree of freedom, and determine the disturbance estimation torque of each degree of freedom based on the interval activation intensity. S104. The estimated disturbance torques of each degree of freedom are superimposed onto the inner ring basic thrust vector to obtain the actual control torque, which drives the spherical robot to perform three-dimensional trajectory tracking in the liquid without jitter.

[0030] It should be noted that the trajectory control method for underwater spherical robots provided by this invention can be applied to various liquid environments in industrial inspection scenarios, including but not limited to water, power transformer oil, and other conventional and special liquid media. It can stably achieve high-precision trajectory tracking in different industrial liquid environments and has strong versatility and scenario adaptability. However, when spherical robots operate in different industrial liquid environments, there are extreme working conditions. Extreme working conditions refer to special working states where the robot deviates from a stable operating state, external interference is severe, and operating conditions are harsh. Specifically, these may include strong water flow impact, sudden changes in medium flow velocity, turbulent disturbances inside the oil, and fluctuations in the thruster output during robot operation.

[0031] In summary, the underwater spherical robot trajectory control method provided in this embodiment of the invention first determines the pose tracking error based on the acquired reference pose and actual pose of the spherical robot, and generates an outer loop tracking command including the desired velocity vector based on the pose tracking error. Observation channels are set up for the four degrees of freedom (forward / backward, lateral, heave, and yaw). Based on the observation channels, the velocity tracking error between the actual velocity vector and the desired velocity vector after the spherical robot responds to the outer loop tracking command is obtained. An inner loop basic thrust vector is generated based on the pose tracking error and the velocity tracking error. This method can achieve order reduction and decoupling of the nonlinear system, weaken the dependence on the precise hydrodynamic model, offset underwater motion nonlinearity and eliminate coupling interference, ensuring smooth convergence of the trajectory tracking error. Secondly, the error rate of change of the velocity tracking error is obtained. By inputting the velocity tracking error and error change rate into the interval type II fuzzy inference engine, the interval activation intensity of the fuzzy rules under each degree of freedom is obtained. Based on the interval activation intensity, the disturbance estimation torque of each degree of freedom is determined. This can adapt to the differentiated motion characteristics of the robot's multiple degrees of freedom, suppress high-frequency noise from the sensor, accurately estimate the comprehensive disturbance caused by time-varying flow field and model parameter perturbation, and improve the robustness against disturbances. Finally, the disturbance estimation torque of each degree of freedom is superimposed on the inner loop basic thrust vector to obtain the actual control torque, which drives the spherical robot to achieve underwater three-dimensional trajectory jitter-free tracking. It can compensate for unknown disturbances in real time, eliminate high-frequency jitter of traditional control to protect the thruster hardware, and suppress low-cost sensor integral drift. It achieves high-precision and high-stability trajectory tracking control under complex water flow and thruster failure conditions.

[0032] In some embodiments of the present invention, the pose tracking error is determined based on the obtained reference pose and actual pose of the spherical robot, and an outer loop tracking command including the desired velocity vector is generated based on the pose tracking error, such as... Figure 2 As shown, it includes: S201. Determine the pose tracking error based on the deviation between the reference pose and the actual pose, and obtain the dynamic equation of the kinematic error by calculating the time derivative of the pose tracking error. S202. Construct the kinematic Lyapunov function according to Lyapunov's second method. The kinematic Lyapunov function includes the square term of the three-dimensional spatial position deviation and the potential energy-like term of the yaw angle deviation. S203. Based on the designed virtual velocity control law, the kinematic Lyapunov function whose time derivative satisfies the semi-negative definite condition is obtained. The time derivative of the kinematic Lyapunov function is substituted into the dynamic equation of kinematic error, and the desired velocity vector is output as the tracking command of the outer loop output.

[0033] This embodiment establishes the coordinate transformation relationship between the inertial coordinate system and the body coordinate system for attitude and velocity, accurately constructing a nonlinear geometric mapping between position deviation and real-time body velocity. By calculating the attitude tracking error and deriving the dynamic equation of kinematic error, the dynamic evolution law of attitude deviation is accurately characterized. A kinematic Lyapunov function containing the square term of the three-dimensional spatial position deviation and the potential energy term of the yaw angle deviation is constructed, which has strict positive definiteness and can effectively quantify the tracking deviation in all dimensions. Based on the designed virtual velocity control law, a kinematic Lyapunov function whose time derivative satisfies the semi-negative definite condition is obtained. The time derivative of the kinematic Lyapunov function is substituted into the dynamic equation of kinematic error, and the desired velocity vector is output as the tracking command for the outer loop output. This can drive the attitude tracking error to converge smoothly and asymptotically, reliably achieving outer loop attitude stabilization, laying the foundation for subsequent inner loop velocity tracking and high-precision trajectory control of the whole machine.

[0034] In some embodiments of the present invention, the square term of the three-dimensional spatial position deviation includes half of the square of the position error of the three degrees of freedom of advance / retreat, lateral movement, and heave; The potential energy term for yaw angle deviation is the difference between the constant 1 and the cosine value of the yaw angle deviation.

[0035] It should be noted that, in order to address the challenges of strongly coupled and highly nonlinear trajectory tracking faced by spherical underwater robots in complex and confined waters, this invention proposes a kinematic and dynamic dual-loop backstepping control architecture based on Lyapunov stability theory. This architecture employs a "hierarchical recursive" mathematical approach to decompose the complex control objective of the high-order nonlinear system into two levels: outer-loop pose stabilization and inner-loop velocity tracking.

[0036] In some embodiments of the present invention Figure 1 The specific steps for S101 and S102 are as follows: First, establish a global navigation reference, defining the generalized pose vector in the inertial coordinate system as follows: The generalized velocity vector in the body coordinate system is The desired reference trajectory is set as follows. The pose tracking error in the inertial coordinate system is defined as follows: Taking the time derivative of both sides of the pose error equation, we can obtain the dynamic equation of the system's kinematic error, as shown in Equation 1: (1) in, This is the Jacobian matrix for the four-degree-of-freedom kinematic coordinate transformation relationship. This step precisely establishes the nonlinear geometric mapping relationship between the time evolution rate of the global position deviation and the real-time velocity of the local body. To address the aforementioned multi-dimensional pose tracking errors... Able to converge smoothly and asymptotically to zero, this invention constructs the first scalar energy function, namely the kinematic Lyapunov function, based on Lyapunov's second method. Its mathematical form is shown in Equation 2: (2) This construction incorporates profound physical considerations. The first three terms quantify the squared Euclidean distance of the positional deviation in three-dimensional space, while the last term cleverly transforms the nonlinear yaw angle deviation into a potential energy-like metric with a globally unique minimum. This applies if and only if the tracking error is zero. It satisfies the strict positive definiteness requirement. For By differentiating along the time trajectory and substituting into formula (1), the core objective of the backstepping outer loop design is to find a set of virtual velocities in the body coordinate system. As a virtual control law for the system, the energy decay satisfies the semi-negative definite condition. Combining the projection characteristics of the rotation matrix, this invention designs a kinematic virtual control law, as shown in Equation 3: (3) in It is a positive definite gain diagonal matrix. This indicates that the system calculates the gain by feeding back the current pose error and combining it with the derivative of the target trajectory. It can serve as a key bridge connecting kinematics and dynamics, enabling the planning of optimal dynamic velocity targets for the underlying hardware propulsion system.

[0037] In some embodiments of the present invention, the velocity tracking error between the actual velocity vector and the desired velocity vector of the spherical robot after responding to the outer ring tracking command is obtained based on the observation channel, and the inner ring basic thrust vector is generated based on the pose tracking error and the velocity tracking error, such as... Figure 3 As shown, it includes: S301. The difference between the actual velocity vector and the desired velocity vector in the body coordinate system is taken as the velocity tracking error. S302. Construct a four-degree-of-freedom nonlinear dynamic model for forward and backward movement, lateral movement, heave, and yaw. The left-hand side of the dynamic model includes the product of the total inertia matrix and the actual velocity, the product of the Coriolis force and centrifugal force matrix and the actual velocity, the product of the hydrodynamic damping matrix and the actual velocity, and the gravity and buoyancy restoring torque. The right-hand side includes the generalized thrust vector and a comprehensive disturbance term that incorporates external ocean current disturbances and unmodeled dynamics. S303. Calculate the time derivative of the speed tracking error and substitute it into the nonlinear dynamic model to obtain the inner loop dynamic error evolution equation; S304. Construct a total composite Lyapunov function that includes the kinematic Lyapunov function and the quadratic form of velocity error, wherein the quadratic form of velocity error uses the total inertia matrix as the quadratic form matrix. S305. By taking the time derivative of the total composite Lyapunov function, the Lyapunov derivative of the closed-loop system is obtained; a dynamic backstepping control law is designed and substituted into the inner-loop dynamic error evolution equation. Combining the time derivative of the Lyapunov of the closed-loop system, the basic thrust vector is output; the dynamic backstepping control law includes a feedforward cancellation term and a feedback stabilization term. The feedforward cancellation term is used to neutralize the nonlinearity of the dynamic system moving in the liquid, and the feedback stabilization term is used to push the motion system state of the spherical robot to slide down along the negative gradient direction.

[0038] This embodiment defines the speed tracking error as the difference between the actual speed and the desired speed, and constructs a four-degree-of-freedom nonlinear dynamic model for forward / backward movement, lateral movement, heave, and yaw. This model fully characterizes the robot's inertia, Coriolis force, hydrodynamic damping, gravity buoyancy, and the combined dynamic disturbances from external ocean currents and unmodeled forces. The inner-loop dynamic error evolution equation is derived, accurately reflecting the dynamic changes in speed error. A composite Lyapunov function, including kinematic Lyapunov functions and quadratic forms of speed error, is constructed to achieve global energy management of pose and speed deviations. A dynamic backstepping control law, including feedforward cancellation and feedback stabilization terms, is designed. The feedforward term cancels the nonlinearity of the underwater motion system, and the feedback stabilization term drives the system state to converge along the negative gradient, outputting a stable basic thrust vector. This lays a solid foundation for subsequent disturbance observation compensation, improving system anti-disturbance and trajectory tracking accuracy, and enhancing the inner-loop control.

[0039] In some embodiments of the present invention, after acquiring the velocity tracking error and its filtered rate of change through each observation channel, the method further includes: The velocity tracking error and its rate of change after filtering are normalized to dimensionless variables within the standard domain by using a quantization factor matrix.

[0040] It should be noted that after entering the inner dynamic loop (velocity tracking layer), due to the inherent mechanical inertia and hydrodynamic damping of the spherical underwater robot, its actual speed... Unable to achieve virtual desired speed The instantaneous, error-free jump. This inherent dynamic delay introduces velocity tracking errors in the body coordinate system. Its definition is shown in formula (4). (4) When further combined with the four-degree-of-freedom nonlinear dynamics model of the spherical underwater robot, we can obtain Equation 5: (5) in, To include the total inertia matrix that covers the added mass, Characterizing the Coriolis force and centrifugal force matrices generated by rotation. Represents the hydrodynamic damping matrix. The restoring torque of gravity and buoyancy, The generalized thrust vector is the final output of the system. It is a comprehensive disturbance term that integrates external ocean current disturbances and unmodeled dynamics. Differentiating equation (4) and substituting it into equation (5), we obtain the inner loop dynamic error evolution equation, as shown in equation (6): (6) To achieve the dissipation of total system energy and the stabilization of velocity error, this invention addresses the kinematic outer ring. Based on this, a total composite Lyapunov function containing the quadratic term of the system's "pseudo-kinetic energy" was constructed. Equation 7 illustrates its structure: (7) Due to the inertia matrix Strict positive definiteness allows for a global coordination of the "positional deviation potential energy" of the outer ring and the "velocity deviation kinetic energy" of the inner ring. To calculate the time derivative and force it to satisfy the semi-negative definite condition, and to completely eliminate the cross-coupling terms caused by the nesting of inner and outer loops, thus avoiding their interference, this invention carefully designs a backstepping control law for the dynamic layer. As shown in Equation 8: (8) in This is the positive definite gain matrix of the dynamic controller. The construction logic of this control law is divided into two parts: the first part is the "feedforward cancellation term," which uses prior hydrodynamic parameters to calculate the torque opposite to drag and centripetal force, accurately "neutralizing" the nonlinearity of the underwater motion system; the second part is the "feedback stabilization term," which introduces an error proportional term to push the system state down along the negative gradient direction. Substituting Equation 8 back into the inner-loop dynamic error dynamic equation 6, we can obtain the Lyapunov derivative of the closed-loop system, as shown in Equation 9: (9) This step sharply reveals the inherent limitations of pure backstepping control inventions. Under ideal, undisturbed conditions (i.e....) This derivative is strictly non-positive, which mathematically guarantees the asymptotic convergence of the closed-loop error under the nominal model. However, with the introduction of underwater environmental disturbances, a non-negligible disturbance coupling term exists at the end of the equation. When faced with intense time-varying combined disturbances in real, complex sea conditions, the stringent condition of non-positive derivatives will be violated. Mathematically, this forces the closed-loop system to fail to maintain asymptotic stability, instead degenerating into steady-state errors or even causing trajectory divergence. Therefore, in order to completely eliminate the influence of the remaining uncertainties in Equation 9, it is necessary to introduce an Interval Type II Fuzzy Disturbance Observer (IT2-FDOB) into the control architecture for online evaluation and feedforward compensation of nonlinear disturbances, thus laying a physical foundation for achieving highly robust trajectory tracking.

[0041] In this embodiment, the acquisition speed is tracked according to the degree of freedom. A first-order low-pass filter is introduced to smooth the error change rate, which can suppress the amplification of high-frequency noise from the sensor during the differentiation process. Then, the error with physical dimensions and the error change rate are normalized to dimensionless variables in the standard domain of discourse through the quantization factor matrix, providing standardized feature input for subsequent interval type II fuzzy inference, and ensuring the inference accuracy and reliability of fuzzy disturbance observation.

[0042] The dual-loop backstepping control architecture reduces the order of a complex nonlinear system and decouples it into an outer-loop pose stabilization and an inner-loop velocity tracking system through a "layered recursion." This accurately neutralizes the nonlinear terms of underwater motion, eliminates cross-coupling, and ensures the smooth asymptotic convergence of trajectory tracking errors under the nominal model, laying a solid foundation for high-precision trajectory control.

[0043] In some embodiments of the present invention, the error rate of change of the velocity tracking error is obtained, and the velocity tracking error and the error rate of change are input into an interval type II fuzzy inference engine to obtain the interval activation intensity of the fuzzy rule under each degree of freedom, such as... Figure 4 As shown, it includes: S401. Construct a type II fuzzy set of intervals bounded by upper and lower membership functions for an uncertain footprint. The uncertain footprint is composed of the area bounded by the upper and lower membership functions. S402. Establish an IF-THEN fuzzy rule table for each degree of freedom. The fuzzy rule table includes multiple fuzzy rules. The antecedent of each rule consists of the dimensionless velocity tracking error obtained by normalization and the rate of change of the filtered error. The consequent is the corresponding disturbance estimation output. S403. Substitute each predecessor input into the corresponding upper membership function and lower membership function to obtain the upper membership degree and lower membership degree of each predecessor. S404. Determine the upper limit activation strength and lower limit activation strength of each rule based on the upper and lower membership degrees of each antecedent, and obtain the interval activation strength of each rule; the lower limit activation strength is the product of the lower membership degrees of each antecedent, and the upper limit activation strength is the product of the upper membership degrees of each antecedent.

[0044] This embodiment constructs a type-2 fuzzy set of intervals bounded by upper and lower membership functions to form an uncertain footprint. This set can absorb the uncertainty of time-varying disturbances and enhance the robustness of the inference process to unknown interferences. An IF-THEN fuzzy rule table is established for each degree of freedom. The normalized velocity tracking error and the rate of change of the filtered error are used as the antecedents, and the disturbance estimation is used as the consequent. This adapts to the differentiated disturbance characteristics of each degree of freedom and accurately establishes the mapping relationship between error and disturbance. The interval activation intensity is obtained by calculating the upper and lower limit activation intensity of each rule, which defines the effective inference interval and avoids the interference of single extreme value data. This provides a stable and reliable inference foundation for subsequent type reduction and disturbance estimation torque output.

[0045] In some embodiments of the present invention Figure 5 This is a schematic flowchart of an embodiment of the interval type-1 fuzzy inference mechanism provided by the present invention; the precise input is first fuzzified into a type-1 fuzzy set, a type-1 output fuzzy set is inferred according to the rule base, and finally the precise output is obtained by defuzzification.

[0046] In some embodiments of the present invention Figure 6 This is a schematic flowchart of an embodiment of the interval type II fuzzy inference mechanism provided by the present invention; the precise input is first fuzzified into a type II fuzzy set, and a type II output fuzzy set is inferred according to the rule base. It needs to be converted into a type I fuzzy set by a type reducer, and then defuzzified to obtain the precise output.

[0047] The difference between type I fuzzy inference and type II fuzzy inference lies in the following: Type I fuzzy inference uses type I fuzzy sets with fixed membership degrees, has a simple process without a reduction step, but is weakly adaptable to strong time-varying and non-Gaussian disturbances and lacks robustness; Type II fuzzy inference uses type II fuzzy sets with uncertain intervals, has an additional reduction step, and can absorb unknown disturbances such as flow field and noise through the uncertain intervals, resulting in significantly stronger anti-disturbance and robustness under complex working conditions.

[0048] In some embodiments of the present invention, the interval type-II fuzzy set of the uncertain footprint is constructed by an expansion model, which is shown in Equation 10 below: (10) in, Let i be the type II fuzzy set of the i-th interval. The input variable is the dimensionless velocity tracking error or the rate of change of the filtered error. For the universe of discourse of fuzzy input variables, For fuzzy sets In the input Membership function at position, For fuzzy sets In the input The subordinate membership function at that location.

[0049] In some embodiments of the present invention, the perturbation estimation torque for each degree of freedom is determined based on the interval activation intensity, such as... Figure 7 As shown, it includes: S701. The interval activation intensity is reduced by the Karnike-Mendel iterative algorithm, the interval intensity is sorted in ascending order and the switching point is searched, and the left and right endpoints of the centroid are output. S702. Take the average of the left and right endpoints to obtain the dimensionless estimate; S703. The dimensionless estimate is mapped to the actual thrust space by a pre-calibrated physical inverse normalization factor to obtain the original disturbance estimate torque. S704. The original disturbance estimate torque is corrected twice by using the maximum thrust limit of the thruster. The corrected torque is constrained within the physical output range of the thruster, and the disturbance estimate torque of each degree of freedom is output.

[0050] This embodiment employs the Carnik-Mendel iterative algorithm to perform interval activation intensity reduction processing. By sorting the interval intensity in ascending order and retrieving the switching points to solve for the left and right endpoints of the centroid, the distribution characteristics of time-varying perturbations can be completely preserved, improving the numerical stability and dynamic smoothness of the algorithm and avoiding the defect of losing nonlinear characteristics in traditional defuzzification algorithms. Secondly, the mean value of the left and right endpoints of the centroid is obtained to obtain dimensionless estimates, and the output results of interval type II fuzzy inference are regularized, providing standardized and reliable pre-data for subsequent physical dimension restoration and perturbation torque correction. Thirdly, with the help of a pre-calibrated physical inverse normalization factor, the dimensionless estimates are mapped to the actual thrust space to complete the dimension restoration and obtain the original perturbation estimated torque. Finally, the original torque is corrected a second time by limiting the maximum thrust of the thruster, strictly constraining the output within the physical output range of the thruster, preventing the generation of false compensation torques that exceed the hardware limits, ensuring the accuracy of perturbation reconstruction while ensuring the safe operation of the robot hardware, and outputting perturbation estimated torques for each degree of freedom that can be directly used for feedforward compensation.

[0051] In some embodiments of the present invention Figure 8 This is a schematic flowchart of an embodiment of the fuzzy observer provided by the present invention; the speed tracking error is taken as input and processed in two ways: one way is directly quantized and normalized to obtain the dimensionless value of the error; the other way is to first calculate the error change rate by difference, filter and smooth it, and then quantize and normalize it to obtain the dimensionless value of the change rate; the two signals are input into fuzzy inference together and output the disturbance estimation torque of each degree of freedom.

[0052] It should be noted that traditional Type-1 fuzzy logic systems (FLS) and conventional disturbance observers typically assume that the control rules and membership functions have uniquely defined and precise boundaries. This assumption often fails in complex aquatic environments where underwater flow fields are constantly changing and hydrodynamic parameters are subject to drastic perturbations. When underwater robots encounter sudden lateral thrust from water currents or a sharp drop in thrust from the thruster, they are prone to generating complex, non-Gaussian distributed disturbances. This causes the Type-1 fuzzy observer to experience high-frequency jumps at fixed rule boundaries, leading to a surge in outliers in the control torque.

[0053] Therefore, in some embodiments of the present invention, an adaptive dynamic evaluation mechanism based on physical uncertainty characteristics is introduced. This mechanism dynamically estimates and compensates for the remaining unknown composite disturbance vector in Equation 9 by constructing an "Interval Type II Fuzzy Perturbation Observer (IT2-FDOB)" online. First, based on the aforementioned dynamic backstepping inner loop, the velocity tracking error of each degree of freedom is determined. This invention extracts the dynamic physical characteristics of the current organism's state. To avoid the sensor's high-frequency noise being amplified infinitely during the differential step, the present invention extracts the error rate of change. At this point, a first-order low-pass filter is introduced for smoothing preprocessing. Next, by introducing a quantization factor matrix, the velocity error and its rate of change, which have actual physical dimensions, are mapped to dimensionless variables within the standard universe of discourse, as shown in Equations 11 and 12: (11) (12) in, Representing different degrees of freedom of control, The rate of change of the filtered error. These are the normalized input gains for the corresponding channels. These two formulas directly reflect the dynamic geometric contact state of the deviation between the system's current actual velocity and the desired velocity, providing standardized feature inputs for subsequent fuzzy inference.

[0054] Next, for the normalized input state, an uncertainty footprint (FOU) expansion model based on interval type II fuzzy sets is constructed, the mathematical form of which is shown in Equation 10. The physical meaning of Equation 10 is that, unlike type I fuzzy systems which give a single, definite membership degree, interval type II fuzzy systems... and This forms a buffer zone with an "uncertain footprint" of area. When external flow field disturbances cause... When drastic fluctuations occur, this area characteristic limits the system's sensitivity to input disturbances to a baseline interval; as the uncertainty of the time-varying flow field increases, the fault-tolerant coverage of the FOU automatically absorbs high-frequency disturbances. This forces the fuzzy rule network to automatically reduce the response weight to extreme transient deviation values, thereby achieving robust feature extraction that is "fault-tolerant without losing accuracy." To completely eliminate the impact of unreliable single rule spikes on the overall estimation results, this invention designs a dual-interval inference gating mechanism based on the strength of the upper and lower boundaries. After input fuzzification is completed, logical inference is performed using the IF-THEN rule table. For the first... Each rule is used to calculate its activation intensity at the lower and upper limits, as shown in Equations 13 and 14, respectively: (13) (14) Here, F1 is the fuzzy set of the first variable, and F2 is the fuzzy set of the second variable. This interval activation mechanism defines the "most conservative estimate" and "most aggressive estimate" of the compensation torque required in the current state at the logical level, cutting off the direct transmission path of single extreme value data.

[0055] After completing the interval rule strength evaluation and integration, a type-reduction step must be performed to obtain the final clear output. It is worth noting that, to prevent the traditional centroid method from losing crucial nonlinear distribution characteristics due to simple averaging when processing interval sets, this invention abandons the standard simple unfuzzing formula and instead employs the Karnik-Mendel (KM) iterative algorithm for accurate reconstruction of interval centroid boundaries. By sorting the interval strengths in ascending order and searching for switching points, the left endpoint of the output centroid is calculated. and right endpoint As shown in Equation 15: (15) Although this KM iterative form slightly increases the computational load, it ensures that the output after order reduction fully retains the symmetric positive definite probability distribution characteristics of the time-varying perturbation, significantly improving the dynamic smoothness and numerical stability of the algorithm when faced with complex step perturbations on embedded platforms. Subsequently, the accurate output is obtained by averaging the left and right endpoints. Finally, to address the dimensionless estimates generated by the model reduction, this invention introduces a physical hard constraint reduction module at the observer output. This module utilizes a pre-calibrated physical denormalization factor. By numerically constraining and scaling the fuzzy domain to the real, limited physical torque and thrust space, the final perturbation estimate torque vectors for each degree of freedom are obtained. As shown in Formula 16: (16) This correction, based on proportional mapping and absolute threshold limiting, utilizes prior physical knowledge of the maximum transient output of the underlying thruster motor to perform secondary correction on the fuzzy observer output. It fundamentally eliminates estimations that violate physical constraints (such as generating spurious compensation torques exceeding motor limits) that might arise from unconstrained optimization of the mathematical model. This achieves high-precision reconstruction of unknown disturbances while maximizing the operational safety of the underwater robot hardware. The total estimated disturbance vector is generated from all channels. This will be directly fed into the composite control law in the next section to counteract nonlinear disturbances.

[0056] In some embodiments of the present invention, the estimated disturbance torques of each degree of freedom are superimposed onto the basic thrust vector to obtain the actual control torque, which drives the spherical robot to perform jitter-free tracking of its three-dimensional trajectory in a liquid, such as... Figure 9 As shown, it includes: S901. The estimated disturbance moments of each degree of freedom are aggregated through each channel to obtain the total estimated disturbance vector; S902. The total estimated disturbance vector is superimposed onto the inner loop basic thrust vector in a negative feedforward manner to construct an updated model of the composite control law and obtain the actual control torque. S903. Obtain the set upper bound of the observation error and extract the global minimum gain parameter of the controller; S904. Determine the critical boundary based on the upper bound of the observation error, the global minimum gain parameter, and the preset scaling constant. S905. When the state norm of the comprehensive error exceeds the critical boundary, the Lyapunov derivative of the closed-loop system is forced to be negative definite to ensure that the trajectory tracking error converges to the bounded neighborhood. S906: The robot achieves three-dimensional trajectory jitter-free tracking under time-varying flow field disturbances, model parameter perturbations, and sudden thruster failure disturbances by driving the robot with actual control torque.

[0057] This embodiment aggregates the estimated moments of disturbances in each degree of freedom into a total estimated disturbance vector. By superimposing this vector onto the basic thrust vector, a composite control law is constructed to obtain the actual control moment. This can offset the combined disturbances caused by the flow field, model perturbations, and faults in real time. By combining the upper bound of the observation error, the global minimum gain parameter, and the scaling constant to set the critical boundary, the Lyapunov derivative is forced to be negative definite when the error exceeds the limit, so that the trajectory tracking error converges to a stable bounded neighborhood. At the same time, the high-frequency chattering of traditional robust control is eliminated, enabling the spherical robot to stably achieve high-precision, chatter-free underwater three-dimensional trajectory tracking under time-varying flow field disturbances, model parameter perturbations, and sudden thruster failures.

[0058] It should be noted that, since standard backstepping controllers (BS) typically rely heavily on accurate nominal hydrodynamic models, this mechanism often fails in complex underwater flow environments with unmodeled dynamics. To forcibly eliminate steady-state errors caused by external disturbances, traditional robust control often introduces high-gain switching terms (such as the sign function term in sliding mode variable structure control), but this easily leads to high-frequency chattering of control commands, severely violating the physical execution constraints and lifespan requirements of underwater thruster motors. Therefore, this invention introduces a composite control architecture based on fuzzy logic feedforward compensation, dynamically adjusting the thrust output weights by online fusion of disturbance estimates from observers.

[0059] In some embodiments of the present invention, firstly, the nominal backstepping control law obtained based on the aforementioned kinematic and dynamic hierarchical calculations is... The combined disturbance estimation vector is obtained by combining the output of the Interval Type II Fuzzy Disturbance Observer (IT2-FDOB). The update model of the composite control law is constructed as shown in Equation 17: (17) Its significance lies in: total control output Driven by two collaborative components, it relies on a nominal model for basic feedback stabilization on one hand, and directly cancels out the unknown combined disturbances brought about by the flow field through a feedforward channel on the other. Specifically, for any given... The observation compensation torque of the 1 degree of freedom can be clearly expressed as a nonlinear fuzzy mapping of the input error state, as shown in Equation 18: (18) in To output the inverse normalization scaling factor, This represents a type II fuzzy logic inference operator. This forces the underlying controller to automatically reduce its dependence on fixed nominal parameters, transforming the complex nonlinear compensation problem into a smooth optimization within a fuzzy rule space. Notably, to prevent high-frequency chattering in the propeller motor due to high-frequency switching, this invention abandons the conservative high-gain control method that forcibly wraps the estimated residuals, instead employing this smooth mapping mechanism for torque compensation. While this theoretically allows for minor oscillations in the zero-point neighborhood of the residuals, it significantly improves the output smoothness and numerical stability of the control algorithm during long-term operation on the hardware platform.

[0060] To thoroughly verify the safety of the aforementioned composite control mechanism under complex disturbances, this invention designs a global stability verification model based on Lyapunov theory. Substituting Equation 17 of the composite control law back into the system's total dynamic energy equation, the derivative evolution of the closed-loop system along the time trajectory is calculated, as shown in Equation 19: (19) in This represents the residual of the observer's estimation of the actual total disturbance. At the physical level, no matter how strong the approximation capability of a fuzzy system is, it will always have an extremely small residual error due to the limitation of the universe of discourse. Based on the universal approximation property of fuzzy logic, this invention sets the boundedness premise of the observation error, that is, the existence of a minimal positive constant. Make At the statistical and global analysis level, a comprehensive error state vector is defined. It includes all pose and velocity deviation components and extracts the global minimum gain parameter of the controller. Subsequently, a constant is introduced. After algebraically decomposing and scaling the negative definite term of the derivative, the corrected energy dissipation rate is shown in Equation 20: (20) Finally, to address the problem of unconstrained divergence of errors that may occur in closed-loop systems under unknown residual disturbances, this invention introduces a hard convergence mechanism of eventually uniformly bounded (UUB) at the filtering analysis end using formula (20).

[0061] The physical gating condition of the formula is that when the state norm of the comprehensive error exceeds a specific critical boundary, i.e. When, the derivative It is forcibly constrained to be strictly less than zero. Mathematically, this ensures that the damping stabilizing force of the controller absolutely overwhelms the destructive divergent force of the residuals, forcibly clamping the system's motion estimate to a value that is... Within the safe, confined physical boundary of the space, this correction utilizes prior knowledge of stability to theoretically correct the closed-loop performance of the system, fundamentally eliminating the possibility of trajectory puncture or divergence that may occur when underwater robots face sudden thruster failures or extreme time-varying ocean currents, thus achieving highly robust three-dimensional spatial trajectory tracking.

[0062] In some embodiments of the present invention Figure 10 This is a schematic flowchart of another embodiment of the trajectory control method for an underwater spherical robot provided by the present invention; as shown below. Figure 10 As shown, this invention employs composite control, specifically: (1) Using the double-loop backstepping control theory, the control is decomposed into kinematic outer loop pose stabilization and dynamic inner loop velocity tracking, and a basic framework that does not depend on the exact model is constructed. (2) To address the error divergence problem, a parallel interval type II fuzzy perturbation observer with multiple channels is constructed. The uncertainty footprint domain is customized by combining the four degrees of freedom differential characteristics to achieve high-precision online estimation of the comprehensive perturbation. (3) To address the chattering and insufficient estimation accuracy of control variables under extreme conditions, a smooth feedforward compensation mechanism based on fuzzy rules is designed, and a composite control law is generated by combining Lyapunov theory.

[0063] This invention effectively suppresses low-cost sensor integral drift, strong time-varying flow field interference, and sudden thruster failure disturbances without increasing expensive hardware load, and eliminates high-frequency chattering in traditional robust control, ultimately achieving highly robust millimeter-level three-dimensional trajectory tracking control for spherical underwater robots in complex working conditions.

[0064] In some embodiments of the present invention Figure 11 This is a schematic diagram of an embodiment of the spiral trajectory tracking curve under water flow disturbance provided by the present invention; wherein, the legend and method correspond to: pure backstepping control (BS), type I fuzzy disturbance observation backstepping control (T1F-DOB+BS), and the interval type II fuzzy disturbance observation backstepping control of the present invention (IT2F-DOB+BS). As can be seen from the time history curves of each degree of freedom and the three-dimensional trajectory, the trajectory of the method of the present invention closely matches the reference trajectory, with a smaller deviation than the BS and T1F-DOB+BS methods, and can still accurately reproduce the target spiral trajectory under water flow disturbance.

[0065] In some embodiments of the present invention Figure 12 This is a schematic diagram of an embodiment of the three-dimensional error curve for spiral trajectory tracking under water flow disturbance provided by the present invention; the pure BS method has drastic error fluctuations and high peak values; the T1F-DOB+BS method has some error suppression but still has obvious fluctuations; the three-dimensional error of the method of the present invention is always maintained at an extremely low level with almost no obvious fluctuations, indicating that its ability to suppress water flow disturbance is significantly stronger, and it achieves higher accuracy and more stable trajectory tracking.

[0066] Figures 11 to 12 This paper presents a comparison of the trajectory tracking performance of the proposed IT2F-DOB+BS composite control algorithm with that of traditional backstepping control (BS) and a type-1 fuzzy observer compensation algorithm (T1F-DOB+BS) under strong water flow disturbance. The results are derived from three-dimensional simulations including four-degree-of-freedom time response and spatial trajectory. Figure 12 As can be seen, even when the traditional BS algorithm suffers from significant trajectory deviation and oscillations (red dashed line) due to its over-reliance on an accurate model, the fused trajectory (green solid line) using the algorithm of this invention still closely follows the reference path. Especially under conditions of continuous spiral ascent and sharp, large-angle turns, this algorithm, through the design of the uncertain footprint (FOU) of the interval type II fuzzy set, effectively absorbs the non-Gaussian abrupt flow field disturbances and generates smooth feedforward compensation in real time, eliminating the command chattering generated by traditional robust control during high-frequency switching. Comparison data for other curve algorithms are shown in Table 1.

[0067] Table 1: RMSE results of BS, T1F-DOB+BS and IT2F-DOB+BS algorithms

[0068] It should be noted that in Table 1, "Improvement Rate 1" represents the percentage improvement of T1F-DOB+BS compared to BS, and "Improvement Rate 2" represents the percentage improvement of IT2F-DOB+BS compared to BS.

[0069] As shown in Table 1, the pure backstepping control has the highest error level. The error of the type I fuzzy disturbance observation-assisted control scheme is reduced. However, the interval type II fuzzy disturbance observation backstepping control method of the present invention has the lowest error under all test trajectories, showing better trajectory tracking accuracy and disturbance resistance robustness, and verifying the significant advantages of the present invention in complex underwater trajectory control scenarios.

[0070] Compared to existing technologies, this invention features customized decoupling for the differentiated characteristics of each degree of freedom in a spherical robot. This not only significantly improves trajectory smoothness and millimeter-level tracking accuracy under extreme conditions of model perturbations and thruster failures, but also effectively suppresses integral drift of low-cost sensors. It also verifies the superior robustness of the composite control architecture to strong time-varying disturbances in complex and confined waters. While ensuring the safe operation of the thruster hardware, this invention fundamentally balances high control precision with system stability, thereby significantly enhancing the precision operation capabilities and mission reliability of spherical underwater robots under complex conditions.

[0071] To better implement the trajectory control method for the underwater spherical robot in this embodiment of the invention, based on the trajectory control method for the underwater spherical robot, correspondingly, as follows: Figure 13 As shown, this embodiment of the invention also provides a trajectory control system for an underwater spherical robot. The spherical robot trajectory control system 1300 includes: Spherical robot body 1301; An inertial measurement unit 1302 is installed inside the spherical robot body and is used to measure the robot's actual pose in the inertial coordinate system in real time. The speed sensor 1303 is installed inside the spherical robot body and is used to measure the robot's actual velocity vector in the body coordinate system in real time. Multiple thrusters 1304 are distributed on the spherical robot body to drive the robot's movement according to the received actual control torque; The embedded controller 1305 is connected to the inertial measurement unit, the velocity sensor, and multiple thrusters; the embedded controller includes: The dual-loop backstepping control basic unit 1306 is used to determine the pose tracking error based on the acquired reference pose and actual pose of the spherical robot, and to generate an outer loop tracking command including the desired velocity vector based on the pose tracking error; observation channels are set for the four degrees of freedom of forward / backward, lateral, heave, and yaw, and the velocity tracking error between the actual velocity vector and the desired velocity vector after the spherical robot responds to the outer loop tracking command is obtained based on the observation channels, and an inner loop basic thrust vector is generated based on the pose tracking error and the velocity tracking error; The channel-specific interval type II fuzzy disturbance observation unit 1307 is used to obtain the error change rate of the velocity tracking error. The velocity tracking error and the error change rate are input into the interval type II fuzzy inferencer to obtain the interval activation intensity of the fuzzy rule under each degree of freedom, and the disturbance estimation torque of each degree of freedom is determined based on the interval activation intensity. The feedforward compensation fusion unit 1308 is used to superimpose the disturbance estimation torque of each degree of freedom onto the inner loop basic thrust vector to obtain the actual control torque, which drives the spherical robot to perform jitter-free tracking of the three-dimensional trajectory in the liquid.

[0072] In the underwater spherical robot trajectory control system provided in this embodiment, the inertial measurement unit and velocity sensor provide pose and velocity feedback; the dual-loop backstepping control unit generates the basic thrust; the channel-specific type II fuzzy disturbance observation unit estimates the disturbance online; and the feedforward compensation fusion unit merges the two into a control torque-driven thruster. Through the coordinated efforts of these modules, while ensuring nominal trajectory tracking performance, the system effectively improves disturbance rejection robustness, achieving high-precision, jitter-free three-dimensional trajectory tracking in a liquid environment.

[0073] The above provides a detailed description of the trajectory control method and system for underwater spherical robots provided by this invention. Specific examples have been used to illustrate the principles and implementation methods of this invention. The descriptions of the above embodiments are only for the purpose of helping to understand the method and core ideas of this invention. At the same time, for those skilled in the art, there will be changes in the specific implementation methods and application scope based on the ideas of this invention. Therefore, the content of this specification should not be construed as a limitation of this invention.

Claims

1. A trajectory control method for an underwater spherical robot, characterized in that, include: The pose tracking error is determined based on the obtained reference pose and actual pose of the spherical robot, and an outer loop tracking command including the desired velocity vector is generated based on the pose tracking error. Observation channels are set up for the four degrees of freedom of forward and backward movement, lateral movement, heave and yaw. Based on the observation channels, the actual velocity vector and the velocity tracking error of the spherical robot after responding to the outer ring tracking command are obtained. The inner ring basic thrust vector is generated based on the pose tracking error and the velocity tracking error. The error change rate of the velocity tracking error is obtained, and the velocity tracking error and the error change rate are input into the interval type II fuzzy inferencer to obtain the interval activation intensity of the fuzzy rule under each degree of freedom, and the disturbance estimation torque of each degree of freedom is determined based on the interval activation intensity. The estimated disturbance torques of each degree of freedom are superimposed on the inner ring basic thrust vector to obtain the actual control torque, which drives the spherical robot to perform three-dimensional trajectory tracking in the liquid without jitter.

2. The trajectory control method for an underwater spherical robot according to claim 1, characterized in that, The process of determining the pose tracking error based on the acquired reference pose and actual pose of the spherical robot, and generating an outer-loop tracking command including the desired velocity vector based on the pose tracking error, includes: The pose tracking error is determined based on the deviation between the reference pose and the actual pose, and the time derivative of the pose tracking error is calculated to obtain the dynamic equation of the kinematic error. The kinematic Lyapunov function is constructed according to Lyapunov's second method. The kinematic Lyapunov function includes a square term of the three-dimensional spatial position deviation and a potential energy-like term of the yaw angle deviation. Based on the designed virtual velocity control law, a kinematic Lyapunov function whose time derivative satisfies the semi-negative definite condition is obtained. The time derivative of the kinematic Lyapunov function is then substituted into the kinematic error dynamic equation, and the desired velocity vector is output as the tracking command for the outer loop output.

3. The trajectory control method for an underwater spherical robot according to claim 2, characterized in that, The square term of the three-dimensional spatial position deviation includes half of the square of the position error of the three degrees of freedom of advance, retreat, lateral movement, and heave; The potential energy term of the yaw angle deviation is the difference between the constant 1 and the cosine value of the yaw angle deviation.

4. The trajectory control method for an underwater spherical robot according to claim 1, characterized in that, The process of obtaining the velocity tracking error between the actual velocity vector and the desired velocity vector of the spherical robot after responding to the outer ring tracking command based on the observation channel, and generating the inner ring basic thrust vector based on the pose tracking error and the velocity tracking error, includes: The difference between the actual velocity vector and the desired velocity vector in the body coordinate system is taken as the velocity tracking error; A four-degree-of-freedom nonlinear dynamic model is constructed, encompassing forward and backward movement, lateral movement, heave, and yaw. The left-hand side of the dynamic model includes the product of the total inertia matrix and the actual velocity, the product of the Coriolis force and centrifugal force matrix and the actual velocity, the product of the hydrodynamic damping matrix and the actual velocity, and the restoring torque of gravity and buoyancy. The right-hand side includes the generalized thrust vector and a comprehensive disturbance term that incorporates external ocean current disturbances and unmodeled dynamics. The time derivative of the velocity tracking error is calculated and substituted into the nonlinear dynamic model to obtain the inner loop dynamic error evolution equation; Construct a total composite Lyapunov function comprising the kinematic Lyapunov function and the quadratic form of velocity error, wherein the quadratic form of velocity error uses the total inertia matrix as the quadratic form matrix; The Lyapunov derivative of the closed-loop system is obtained by taking the time derivative of the total composite Lyapunov function; a dynamic backstepping control law is designed and substituted into the inner-loop dynamic error evolution equation; combined with the time derivative of the Lyapunov of the closed-loop system, the basic thrust vector is output; the dynamic backstepping control law includes a feedforward cancellation term and a feedback stabilization term. The feedforward cancellation term is used to neutralize the nonlinearity of the dynamic system moving in the liquid, and the feedback stabilization term is used to push the motion system state of the spherical robot to slide down along the negative gradient direction.

5. The trajectory control method for an underwater spherical robot according to claim 1, characterized in that, After acquiring the velocity tracking error and its filtered rate of change through each observation channel, the method further includes: The velocity tracking error and its filtered rate of change are normalized to dimensionless variables within the standard domain by using a quantization factor matrix.

6. The trajectory control method for an underwater spherical robot according to claim 5, characterized in that, The step of obtaining the rate of change of the velocity tracking error, inputting the velocity tracking error and the rate of change of the error into an interval type II fuzzy inference engine, and obtaining the interval activation intensity of the fuzzy rule under each degree of freedom includes: Construct an interval type II fuzzy set bounded by upper and lower membership functions, wherein the uncertain footprint is composed of the area bounded by the upper and lower membership functions; An IF-THEN fuzzy rule table is established for each degree of freedom. The fuzzy rule table includes multiple fuzzy rules. The antecedent of each rule consists of the dimensionless velocity tracking error obtained by normalization and the rate of change of the filtered error, and the consequent is the corresponding perturbation estimation output. Substitute each of the preceding inputs into the corresponding upper and lower membership functions to obtain the upper and lower membership degrees of each preceding. The upper limit activation strength and lower limit activation strength of each rule are determined based on the upper and lower membership degrees of each predecessor, thus obtaining the interval activation strength of each rule; the lower limit activation strength is the product of the lower membership degrees of each predecessor, and the upper limit activation strength is the product of the upper membership degrees of each predecessor.

7. The trajectory control method for an underwater spherical robot according to claim 6, characterized in that, The interval type-II fuzzy set of the uncertain footprint is constructed by an expansion model, which is shown in the following equation: in, Let i be the type II fuzzy set of the i-th interval. The input variable is the dimensionless velocity tracking error or the rate of change of the filtered error. For the universe of discourse of fuzzy input variables, For fuzzy sets In the input Membership function at position, For fuzzy sets In the input The subordinate membership function at that location.

8. The trajectory control method for an underwater spherical robot according to claim 1, characterized in that, The determination of the perturbation estimation torque for each degree of freedom based on the activation intensity of the interval includes: The interval activation intensity is reduced using the Carnick-Mendel iterative algorithm, the interval intensity is sorted in ascending order and the switching point is searched, and the left and right endpoints of the centroid are output. The dimensionless estimate is obtained by averaging the left and right endpoints. The dimensionless estimate is mapped to the actual thrust space by a pre-calibrated physical inverse normalization factor to obtain the original disturbance estimate torque. The original disturbance estimate torque is corrected by using the maximum thrust limit of the thruster, and the corrected torque is constrained within the physical output range of the thruster to output the disturbance estimate torque for each degree of freedom.

9. The trajectory control method for an underwater spherical robot according to claim 1, characterized in that, The step of superimposing the estimated disturbance torques of each degree of freedom onto the inner ring base thrust vector to obtain the actual control torque, driving the spherical robot to perform jitter-free tracking of its three-dimensional trajectory in the liquid, includes: The estimated perturbation torques of each degree of freedom are aggregated through each channel to obtain the total estimated perturbation vector; The total estimated disturbance vector is superimposed onto the basic thrust vector in a negative feedforward manner to construct an updated model of the composite control law and obtain the actual control torque. Obtain the set upper bound of the observation error and extract the global minimum gain parameter of the controller; The critical boundary is determined based on the upper bound of the observation error, the global minimum gain parameter, and the preset scaling constant. When the state norm of the comprehensive error exceeds the critical boundary, the Lyapunov derivative of the closed-loop system is forced to be negative definite, ensuring that the trajectory tracking error converges to the bounded neighborhood. The robot is driven by the actual control torque to achieve three-dimensional trajectory jitter-free tracking under time-varying flow field disturbances, model parameter perturbations, and sudden thruster failure disturbances in the liquid.

10. A trajectory control system for an underwater spherical robot, characterized in that, include: The spherical robot body; An inertial measurement unit, installed inside the spherical robot body, is used to measure the robot's actual pose in the inertial coordinate system in real time; A velocity sensor, installed inside the spherical robot body, is used to measure the robot's actual velocity vector in the body coordinate system in real time. Multiple thrusters are distributed on the spherical robot body to drive the robot's movement according to the received actual control torque; An embedded controller is connected to the inertial measurement unit, the velocity sensor, and the multiple thrusters, respectively. The embedded controller includes: The dual-loop backstepping control basic unit is used to determine the pose tracking error based on the acquired reference pose and actual pose of the spherical robot, and to generate an outer loop tracking command including the desired velocity vector based on the pose tracking error; observation channels are set for the four degrees of freedom of forward / backward, lateral, heave, and yaw respectively, and the velocity tracking error between the actual velocity vector and the desired velocity vector of the spherical robot after responding to the outer loop tracking command is obtained based on the observation channels, and an inner loop basic thrust vector is generated based on the pose tracking error and the velocity tracking error; The channel-specific interval type II fuzzy disturbance observation unit is used to obtain the error change rate of the velocity tracking error, input the velocity tracking error and the error change rate into the interval type II fuzzy inference unit, obtain the interval activation intensity of the fuzzy rule under each degree of freedom, and determine the disturbance estimation torque of each degree of freedom based on the interval activation intensity. The feedforward compensation fusion unit is used to superimpose the estimated disturbance torque of each degree of freedom onto the inner loop basic thrust vector to obtain the actual control torque, which drives the spherical robot to perform jitter-free tracking of its three-dimensional trajectory in the liquid.

Images