A multi-mode trajectory tracking method for a ship cleaning robot

Abstract

The application provides a multi-mode trajectory tracking method of a ship cleaning robot, and relates to the field of automatic control of ship robots, and specifically comprises: establishing a multi-mode switching model of the ship cleaning robot and performing state estimation on the robot. The task scene characteristics and the robot state estimation are combined, a dynamic event trigger is designed, and mode switching rules and a guard condition set corresponding to the dynamic event trigger are established. Mode switching decisions and feasible region checks are performed to obtain a switching signal. The target mode corresponding to the switching signal is subjected to a feasible region check, and whether a buffer trajectory is generated to guide the entry into the feasible region is determined according to the feasible region check result; when the check result is infeasible, an MPC buffer trajectory with a smoothing constraint is constructed based on the target state, and the system switching is performed after the target state enters the feasible region range. The technical scheme of the application overcomes the problems in the prior art that the ship cleaning robot cannot adapt to multiple motion states and the mode switching process is prone to jitter.

Description

Technical Field

[0001] This invention relates to the field of automatic control of marine robots, and specifically to a multi-mode trajectory tracking method for a marine cleaning robot. Background Technology

[0002] With the rapid development of international trade and the marine economy, global shipping demand continues to grow. As the main carriers of intercontinental transport, ships require regular and efficient cleaning of their hull surfaces to ensure safe navigation and extend their lifespan. Currently, the mainstream method of hull cleaning still relies on manual operation, specifically involving divers or snorkeling personnel using tools such as hammers, shovels, and high-pressure spray guns to directly scrape or wash away dirt from the hull surface. However, this traditional cleaning method has significant drawbacks: firstly, it involves high labor intensity for workers and relatively low overall cleaning efficiency; secondly, some stubborn dirt is difficult to remove completely, and the complex underwater environment poses significant safety hazards. Based on these issues, upgrading underwater hull cleaning from traditional manual operation to mechatronic equipment has become an inevitable requirement for industry development.

[0003] In the underwater field of marine cleaning robots, their autonomous cleaning capabilities still need further improvement due to limitations in underwater operating environments and rising maintenance costs. There is an urgent need for a trajectory tracking method for marine cleaning robots capable of completing point-to-point autonomous cleaning tasks throughout the entire process, thereby revolutionizing operational modes. Marine cleaning robots operate in three states: floating, crawling, and a hybrid transition. Traditional trajectory tracking methods cannot simultaneously meet the trajectory tracking requirements under multiple modes. Furthermore, because marine cleaning robots often operate in severe waves and complex flow fields, fixed geometric distance thresholds cannot balance safety and efficiency, easily leading to frequent motor reversals and vibrations, or even forced switching before physical adhesion conditions are met, causing the robot to fall or collide.

[0004] Therefore, there is a need for a multi-mode trajectory tracking method that can adapt to various motion states of ship cleaning robots, and has high accuracy and stability. Summary of the Invention

[0005] The main objective of this invention is to provide a multi-mode trajectory tracking method for ship cleaning robots, in order to solve the problems in the prior art that it cannot adapt to the various motion states of ship cleaning robots and that the mode switching process is prone to shaking.

[0006] To achieve the above objectives, the present invention provides a multi-mode trajectory tracking method for a ship cleaning robot, specifically including the following steps:

[0007] S1. Establish a multi-mode switching model for the ship cleaning robot and estimate the robot's state. The multi-mode switching model includes: crawling dynamics model, swimming nonlinear model and hybrid transition model.

[0008] S2, combining the characteristics of the work scenario and robot state estimation, designs dynamic event triggers, including no trigger, soft trigger and hard trigger, and establishes mode switching rules and guard condition sets corresponding to the dynamic event triggers.

[0009] S3 performs mode switching decisions and feasible region checks to obtain switching signals.

[0010] S4. Perform a feasible region check on the target mode corresponding to the switching signal. Determine whether to generate a buffer trajectory to guide the target mode into the feasible region based on the feasible region check result. If the check result is infeasible, construct a model predictive control (MPC) buffer trajectory with smoothing constraints based on the target state. After the target state enters the feasible region, execute the system switch.

[0011] Furthermore, step S1 specifically includes the following steps:

[0012] S1.1, Establish a multi-mode switching model for ship cleaning robots. for:

[0013] ;

[0014] in, Let be the system state vector. To control the input amount, The dynamic equations of the subsystem are... To switch signals, As a subsystem, It is a positive integer.

[0015] S1.2, Robot's 6-axis orientation and position speed quantity and force The definition is as follows:

[0016] ;

[0017] in, Let these be the coordinates of the robot along each axis in the coordinate system. These are the orientation angles of the robot's axes. Let represent the linear velocity of the robot in each direction. Let be the angular velocities of the robot in all directions. Force is applied to the robot's 6 axes.

[0018] To calculate attitude and position, the following vector is defined:

[0019] ;

[0020] in, For position matrix, The attitude matrix; The linear velocity of the ROV (Return on Vehicle) movement. The angular velocity of the ROV. The force matrix of the robot, This is the torque matrix of the robot.

[0021] Furthermore, step S1 also includes the following steps:

[0022] S1.3, Construct the crawling dynamics model: The dynamic constraints of the ship cleaning robot under the crawling dynamics model are:

[0023] ;

[0024] ;

[0025] ;

[0026] in, The distance between the centers of the two tracks on either side of the robot; The radius of the track drive wheel; The lateral distance between the robot's centroid and the center of the robot assembly; , These are the angular velocities of the left and right tracks, respectively. , These are the slip ratios of the left and right tracks, respectively. The angle between the robot's heading and longitudinal direction. for The derivative, These are the current robot position coordinates. , The derivative of .

[0027] The saturation constraint of the tracked actuator of the ship cleaning robot under the crawling dynamics subsystem model is [- , ], This is the angular velocity threshold.

[0028] S1.4, Construct a nonlinear floating model: , The expression is shown as:

[0029] ;

[0030] in, , These are the robot's rigid body mass and added mass matrices, respectively. It is the centripetal force and Coriolis force matrix. It is a hydrodynamic resistance term. It is resilience. It is the thrust of the propulsion unit. It is a vector composed of linear velocity and angular velocity. yes The derivative of .

[0031] S1.5, Construct a hybrid transition model: The basic dynamic model is consistent with the nonlinear model of the robot, and the following constraints are added: the robot's bow vector and the ship's surface normal vector are within ±6°, and the position constraint is obtained based on the ship's surface position information.

[0032] Furthermore, step S1 also includes the following steps:

[0033] S1.6, Perform initialization settings. Based on the constraints and the robot's multi-mode switching model, generate the initial feasible region using the convex set projection method. Initialization settings include: initializing the current state. Enter robot hardware parameters and initialize prediction interval. Initialize the model prediction parameter matrix R, P, Q, where R is the weight for adjusting the control quantity, P is the weight for adjusting the end state error, and Q is the weight for adjusting the state error. Set actuator saturation constraints, including: maximum thrust of the propeller and maximum speed of the track; set constraint buffer intervals; and set control smoothness constraints.

[0034] S1.7 performs outlier removal and filtering on the collected sensor data, completes environmental modeling by combining the start and end points of the operation with the three-dimensional surface map of the ship's hull, and uses the Kalman filter method to estimate the state of the ship cleaning robot.

[0035] Furthermore, step S2 specifically includes the following steps:

[0036] S2.1, set the hard trigger condition set, including: task-driven, security-driven, feasibility-driven and health-driven.

[0037] S2.2, If the hard trigger condition set is met, then execute step S3.

[0038] S2.3 If the hard trigger condition set is not met, then determine whether the following conditions are met: tracking error < soft threshold and error change acceleration < soft threshold. If met, continue to determine whether to execute the complete control sequence. If so, solve the MPC and obtain the next control sequence. If the complete control sequence is not executed, maintain the current mode and do not solve the MPC. Continue to execute the current control sequence and continue to determine whether the hard trigger condition set is met.

[0039] S2.4 If the tracking error is not less than the soft threshold and the error change acceleration is less than the soft threshold, then continue to determine whether the following conditions are met: soft threshold < tracking error < hard threshold and soft threshold < error change acceleration < hard threshold. If these conditions are met, then re-solve the MPC to obtain the next control sequence, and continue to determine whether the hard trigger condition set is met.

[0040] Furthermore, task-driven categories include: when the distance from the robot to the hull surface is less than the task threshold, it is preparing to transition to the wall-adhering state; when the distance from the robot to the hull surface is greater than the detachment threshold and the safe distance is greater than the minimum safe distance threshold, it indicates that the robot has safely detached from the hull and is preparing to switch to floating mode; when the distance from the robot to the hull surface is less than or equal to the wall-adhering adsorption trigger threshold, it indicates that the robot has entered the adsorption range and is preparing to switch to adsorption operation mode; when the current cleaning task segment is completed, the robot system is preparing to switch to the next task segment or return to a safe state; and when the task scheduling system actively requests a mode switch for multi-task collaboration or task replanning scenarios.

[0041] Safety-driven actions include: the ambient water flow speed exceeding the preset safe flow rate limit, indicating that it affects the robot's stability or operational safety; the adsorption force reaching the saturation limit, indicating that the adsorption system has failed or is overloaded, posing a risk of detachment; and the robot is predicted to collide with an obstacle in less than the minimum safe time threshold, prompting immediate obstacle avoidance or emergency braking measures.

[0042] Feasibility-driven categories include: the predicted future state has entered the feasible region of the target mode, indicating that it is safe to switch to the target mode; the constraint margin of the future control input is less than or equal to zero, indicating that the control quantity is about to reach saturation or exceed the limit, and the control strategy should be adjusted; the distance between the predicted state and the target feasible region is less than or equal to zero, indicating that the state has entered or is about to enter the feasible region, and the switch should be executed.

[0043] Health-related issues include: failure of key sensors, affecting state estimation and control; actuator jamming, affecting motion control; and control algorithm calculation time exceeding the maximum allowable delay, affecting real-time control performance.

[0044] Furthermore, the specific steps of the mode switching decision in step S3 include the following:

[0045] S3.1, Calculate the dynamic adsorption trigger threshold :

[0046] ;

[0047] in, This is the static reference distance. For real-time linear velocity, For pitch angle, , , These are the weighting constants for velocity, attitude, and credibility, respectively. This represents the feasibility of the robot system.

[0048] S3.2, Determine: Whether the distance D_surface to the hull surface is greater than the adsorption trigger threshold. If it is true, the status flag is in the floating cruise state. If it is not true, the status flag is in the wall-attaching transition state. Then, continue to determine whether the following conditions are met: hull distance ≤ attachment distance D_attach, attitude error angle ≤ attitude error angle threshold θ_tol, and normal velocity ≤ normal velocity threshold Vn_tol. If these conditions are met, the status flag is in the wall-attaching transition state. Continue to determine whether there is abnormal detachment, abnormal energy consumption, excessive external disturbance, or completion of the cleaning task. If these conditions are met, the status flag is in the disengagement transition state. Continue to determine whether the following conditions are met: the track stops or completely detaches, and D_surface > D_attach. If these conditions are met, the status flag is in the floating cruise state. If these conditions are not met, the status flag is in the disengagement transition state.

[0049] S3.3 If the following conditions are not met: hull distance ≤ D_attach, attitude error ≤ θ_tol, and normal velocity ≤ Vn_tol, then the state flag position is the wall-attached transition state.

[0050] Furthermore, the feasible region check in step S3 specifically includes:

[0051] The controller output is solved by the crawling dynamics model. If the controller output is abnormal, it means that it is not within the feasible region. If it is not within the feasible region, a buffer trajectory is generated so that the robot can reach the interior of the feasible region and then the state switch is performed. If it is within the feasible region, a switching signal is output and step S4 is executed.

[0052] Furthermore, step S4 specifically includes the following steps:

[0053] S4.1 Determine whether the switching signal is outside the feasible domain and inside the extended shell. If so, perform system switching to floating mode, crawling mode or transition state. During the execution, new hard trigger DET is blocked. Solve the MPC optimization problem after the switching model to obtain the output trajectory until the cleaning task is completed.

[0054] S4.2, If the switching signal is not outside the feasible domain and within the extended shell, then initialize the current state. Target state The constraints include: actuator saturation constraints, control smoothness constraints, predicted state sequences, and an MPC performance function that considers smoothness. :

[0055] ;

[0056] in, , These represent the current time and the prediction step size, respectively. The preset interval length; , , These are the weight matrices for state deviation, control input magnitude, and control increment, respectively. It is a cost-weight matrix. They are respectively The state and thrust output at any given moment The current state. For the target state, For thrust output.

[0057] S4.3, solve the MPC performance function, find the optimal output and execute the optimal output, determine whether the robot has entered the feasible region, if so, execute the system switch, during the execution, block the new DET hard trigger, and solve the MPC optimization problem after the switch model to obtain the output trajectory, until the cleaning task is completed.

[0058] S4.4, If the robot does not enter the feasible region, continue to check if the maximum number of executions has been reached. If so, perform a safe retreat and set a new retreat point. The retreat point satisfies: D_surface ≥ D_safe_recover, | ≤θ_recover、 ≤v_recover; D_safe_recover is the safe recovery distance, θ_recover is the attitude angle constraint, v_recover is the velocity constraint;

[0059] S4.5 If the robot has not entered the feasible region and has not reached the maximum number of executions, then continue to step S4.2 to initialize the current state. Target state and constraint parameters.

[0060] The present invention has the following beneficial effects:

[0061] This invention improves algorithm computational efficiency by constructing dynamic event triggers to perform pre-switching checks for high-frequency systems. Switching rules are formulated for constrained operational scenarios involving complex underwater flow fields and uncertain environmental disturbances, significantly enhancing the algorithm's environmental adaptability. By checking the feasible region and generating buffer trajectories, the risk of infeasibility during model switching is effectively avoided. Compared to ordinary multi-model fusion methods, this invention offers structural advantages and provides key technical support for the intelligentization of ship operation and maintenance systems. Attached Figure Description

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

[0063] Figure 1 A flowchart of a multi-mode trajectory tracking method for a ship cleaning robot according to the present invention is shown.

[0064] Figure 2 The coordinate system of motion for the ship cleaning robot is shown.

[0065] Figure 3 The diagram illustrates the states of a ship cleaning robot under different modes using the method provided by the present invention. Detailed Implementation

[0066] The technical solution of the present invention will now be clearly and completely described with reference to the accompanying drawings. Obviously, the described embodiments are only some, not all, of the embodiments of the present invention. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0067] like Figure 1 The multi-mode trajectory tracking method for a ship cleaning robot shown includes the following steps:

[0068] S1. Establish a multi-mode switching model for the ship cleaning robot and estimate the robot's state. The multi-mode switching model includes: crawling dynamics model, swimming nonlinear model and hybrid transition model.

[0069] S2, combining the characteristics of the work scenario and robot state estimation, designs dynamic event triggers, including: no trigger, soft trigger, and hard trigger, and establishes mode switching rules and guard condition sets corresponding to the dynamic event triggers. No trigger occurs when the deviation is less than the soft threshold and the control sequence has not been completed; the instructions from the previous moment are used, saving computational resources. Soft trigger occurs when the deviation is between the soft and hard thresholds; only the MPC within the current mode is re-solved. Hard trigger immediately forces intervention when any of the conditions for task-driven, safety-driven, feasibility-driven, or health-driven scenarios are met. When the tracking error < soft threshold and the error change acceleration < soft threshold, no trigger occurs. When the soft threshold < tracking error < hard threshold and the soft threshold < error change acceleration < hard threshold, it is a soft trigger. The hard and soft thresholds are different thresholds for tracking error and tracking error change acceleration.

[0070] S3 performs mode switching decisions and feasible region checks to obtain switching signals.

[0071] S4 performs a feasible region check on the target mode (i.e., floating mode, crawling mode, or transition mode) corresponding to the switching signal. The result of the feasible region check determines whether to generate a buffer trajectory to guide the system into the feasible region. If the check result is infeasible, an MPC buffer trajectory with smooth constraints is constructed based on the target state. Once the target state enters the feasible region, the system switches. That is, it switches to: floating mode (suspended swimming), crawling mode (tracked attachment), or transition state. Directly switching from floating mode (which does not contact the hull) to crawling mode can cause abrupt changes in the dynamic model, resulting in the initial state not being in the new feasible region, abrupt changes in control input, and instantaneous instability in trajectory tracking. At the same time, crawling mode has Z-direction position constraints and the attitude must be aligned with the normal to the hull surface; otherwise, collisions and adhesion failures may occur.

[0072] The technical solution of this invention can improve the accuracy, stability and real-time performance of trajectory tracking in the constrained multimodal system of ship cleaning robots, and solve the technical problems of switching jitter and infeasibility risks of traditional trajectory methods in complex hydrodynamic disturbances and uncertain environments.

[0073] Specifically, step S1 includes the following steps:

[0074] S1.1, Establish a multi-mode switching model for ship cleaning robots. for:

[0075] ;

[0076] in, Let be the system state vector. To control the input amount, The dynamic equations for the subsystems are as follows: the floating mode (suspended swimming) subsystem, the crawling mode (track attachment) subsystem, or the transition state subsystem. To switch signals, it determines at time... Which subsystem to use? As a subsystem, It is a positive integer.

[0077] S1.2, Robot's 6-axis orientation and position speed quantity and force The definition is as follows:

[0078] ;

[0079] in, Let these be the coordinates of the robot along each axis in the coordinate system. These are the orientation angles of the robot's axes. The robot's heading angle (rad). Let represent the linear velocity of the robot in each direction. Robot's turning angular velocity (rad / s) Let be the angular velocities of the robot in all directions. Force is applied to the robot's 6 axes.

[0080] like Figure 2 As shown, E is the constructed coordinate system. To calculate the attitude and position, the following vector is defined:

[0081] ;

[0082] The spatial position and attitude of the robot in the ROV inertial coordinate system are represented in matrix form as follows: , For position matrix, The attitude matrix; the linear velocity and angular velocity of the robot in the carrier coordinate system are expressed in matrix form as follows: , The linear velocity of the ROV (Return on Vehicle) movement. Let be the angular velocity of the ROV. The matrix expression of forces and moments in the vehicle coordinate system is as follows: , The force matrix of the robot, Here is the moment matrix for the robot. All motion parameters are assumed to rotate about the positive axes using the right-hand rule.

[0083] Specifically, step S1 further includes the following steps:

[0084] S1.3, Construct the crawling dynamics model: The dynamic constraints of the ship cleaning robot under the crawling dynamics model are:

[0085] ;

[0086] ;

[0087] ;

[0088] in, The distance between the centers of the two tracks on either side of the robot (m); The radius of the track drive wheel is (m). The lateral distance (m) between the robot's centroid and the center of the robot assembly. , These are the angular velocities of the left and right tracks, respectively (rad / s). , These are the slip ratios of the left and right tracks, respectively. The angle between the robot's heading and longitudinal direction. for The derivative, These are the current robot position coordinates. , The derivative of .

[0089] The saturation constraint of the tracked actuator of the ship cleaning robot under the crawling dynamics model is [- , ], This is the angular velocity threshold.

[0090] S1.4, Construct a nonlinear floating model: , The expression is shown as:

[0091] ;

[0092] in, , These are the robot's rigid body mass and added mass matrices, respectively. It is the centripetal force and Coriolis force matrix. It is a hydrodynamic resistance term. It is resilience. It is the thrust of the propulsion unit. It is a vector composed of linear velocity and angular velocity. yes The derivative of .

[0093] The expression, when fully expanded, is shown in the following formula:

[0094] ;

[0095] ;

[0096] ;

[0097] ;

[0098] ;

[0099] ;

[0100] ;

[0101] ;

[0102] ;

[0103] ;

[0104] ;

[0105] ;

[0106] in, It's about the quality of the robot. For the robot's feature length, , , For the robot to circle , , Moment of inertia of the three axes Let the coordinates be the center of the floating center; These are hydrodynamic parameters, which can be measured through a water tank experiment.

[0107] , This is the derivative of the corresponding parameter. These are the current robot position coordinates. , , The derivative of .

[0108] S1.5, Construct a hybrid transition model: The basic dynamic model is consistent with the nonlinear model of the robot, and the following constraints are added: the robot's bow vector and the ship's surface normal vector are within ±6°, and the position constraint is obtained based on the ship's surface position information.

[0109] Specifically, step S1 further includes the following steps:

[0110] S1.6, Perform initialization settings. Based on the constraints and the robot's multi-mode switching model, generate the initial feasible region using the convex set projection method. The constraint convex sets are shown in Table 1. Initialization settings include: initializing the current state. Enter robot hardware parameters and initialize prediction interval. Initialize the model prediction parameter matrix R, P, Q, where R is the weight for adjusting the control quantity, P is the weight for adjusting the end state error, and Q is the weight for adjusting the state error. Set actuator saturation constraints, including: maximum thrust of the propeller and maximum speed of the track; set constraint buffer intervals; and set control smoothness constraints.

[0111] Table 1. Constraint convex sets consisting of constraints, state constraints, and actuator constraints for each subsystem's dynamic model.

[0112]

[0113] Initial control sequence Apply to a multi-mode switching model to generate a predicted state sequence. Then, convex set projection calculation is performed on the predicted sequence to determine whether each predicted "state-control" is within the constraint convex set in Table 1. If not, projection correction is performed. The set of discrete points of all "state-control quantities" that satisfy the constraints after correction constitutes the minimum convex set, thus obtaining the feasible region under the prediction of the model.

[0114] S1.7 involves outlier removal and filtering of the collected sensor data, combined with the start and end points of the operation and a 3D surface map of the hull to complete environmental modeling, and using Kalman filtering for state estimation of the ship cleaning robot. Specifically, this involves using an inertial navigation system, Doppler log, depth gauge, rangefinder, and temperature and humidity sensors to collect real-time operational data of the robot, including its 6-DOF spatial position and attitude, linear velocity, angular velocity, the normal distance D_surface from the robot's bottom to the hull surface, hull surface coordinate data, and environmental disturbance flow velocity. Outlier removal and filtering are then performed on the collected sensor data, combined with the start and end points of the operation and a 3D surface map of the hull to complete environmental modeling and state estimation of the ship cleaning robot.

[0115] Specifically, based on the real-time data status dynamic event trigger determination, it is divided into three categories: no trigger, soft trigger, and hard trigger. First, it is determined whether the current motion state quantity and safety parameters trigger the hard trigger condition set. Under hard trigger, the mode switching process is directly entered. Then, the trajectory tracking error is determined, and whether the error change rate is between the hard and soft thresholds. Under soft trigger, the controller maintains the trajectory within the current mode and directly resolves the MPC. Finally, the trajectory tracking error is determined, and whether the error change rate is less than the soft threshold. If it is satisfied, the controller maintains the current mode and executes the control sequence sequentially. After the control sequence is completed, the next round of MPC solution is performed. Step S2 specifically includes the following steps:

[0116] S2.1, set the hard trigger condition set, including: task-driven, security-driven, feasibility-driven and health-driven.

[0117] S2.2, If the hard trigger condition set is met, then execute step S3.

[0118] S2.3 If the hard trigger condition set is not met, then determine whether the following conditions are met: tracking error < soft threshold and error change acceleration < soft threshold. If met, continue to determine whether to execute the complete control sequence. If so, solve the MPC and obtain the next control sequence. If the complete control sequence is not executed, maintain the current mode and do not solve the MPC. Continue to execute the current control sequence and continue to determine whether the hard trigger condition set is met.

[0119] S2.4 If the tracking error is not less than the soft threshold and the error change acceleration is less than the soft threshold, then continue to determine whether the following conditions are met: soft threshold < tracking error < hard threshold and soft threshold < error change acceleration < hard threshold. If these conditions are met, then re-solve the MPC to obtain the next control sequence, and continue to determine whether the hard trigger condition set is met.

[0120] Specifically, task-driven modes include: when the distance from the robot to the hull surface is less than the task threshold, the robot is preparing to transition to a wall-adhering state; when the distance from the robot to the hull surface is greater than the detachment threshold and the safe distance is greater than the minimum safe distance threshold, the robot has safely detached from the hull and is preparing to switch to floating mode; when the distance from the robot to the hull surface is less than or equal to the wall-adhering adsorption trigger threshold, the robot has entered the adsorption range and is preparing to switch to adsorption operation mode; when the current cleaning task segment is completed, the robot system is preparing to switch to the next task segment or return to a safe state; and when the task scheduling system actively requests a mode switch for multi-task collaboration or task replanning scenarios.

[0121] Safety-driven actions include: when the ambient water flow speed exceeds the preset safe flow rate limit, it indicates that the robot's stability or operational safety is affected; when the adsorption force reaches the saturation limit, it indicates that the adsorption system has failed or is overloaded, posing a risk of detachment; and when it is predicted that the collision time between the robot and an obstacle (such as a ship hull or underwater structure) is less than the minimum safe time threshold, obstacle avoidance or emergency braking measures should be taken immediately.

[0122] Feasibility-driven categories include: the predicted future state has entered the feasible region of the target mode, indicating that it is safe to switch to the target mode; the constraint margin of the future control input is less than or equal to zero, indicating that the control quantity is about to reach saturation or exceed the limit, and the control strategy should be adjusted; the distance between the predicted state and the target feasible region is less than or equal to zero, indicating that the state has entered or is about to enter the feasible region, and the switch should be executed.

[0123] Health-related factors include: failure of key sensors, such as inertial navigation, depth gauge, and distance sensors, which affect state estimation and control; jamming of actuators, such as jamming or abnormal response of actuators like thrusters and tracks, which affect motion control; and control algorithm calculation time exceeding the maximum allowable delay, which affects real-time control performance.

[0124] Specifically, step S3, the mode switching decision, includes the following steps:

[0125] S3.1, Calculate the dynamic adsorption trigger threshold :

[0126] ;

[0127] in, This is the static reference distance. For real-time linear velocity, For pitch angle, , , These are the weighting constants for velocity, attitude, and credibility, respectively. The feasibility of a robot system reflects the quality of sensor data.

[0128] S3.2, Determine: Whether the distance D_surface to the hull surface is greater than the adsorption trigger threshold. If it is true, the status flag is in the floating cruise state. If it is not true, the status flag is in the wall-attaching transition state. Then, continue to determine whether the following conditions are met: hull distance ≤ attachment distance D_attach, attitude error angle ≤ attitude error angle threshold θ_tol, and normal velocity ≤ normal velocity threshold Vn_tol. If these conditions are met, the status flag is in the wall-attaching transition state. Continue to determine whether there is abnormal detachment, abnormal energy consumption, excessive external disturbance, or completion of the cleaning task. If these conditions are met, the status flag is in the disengagement transition state. Continue to determine whether the following conditions are met: the track stops or completely detaches, and D_surface > D_attach. If these conditions are met, the status flag is in the floating cruise state. If these conditions are not met, the status flag is in the disengagement transition state.

[0129] S3.3 If the following conditions are not met: hull distance ≤ D_attach, attitude error ≤ θ_tol, and normal velocity ≤ Vn_tol, then the state flag position is the wall-attached transition state.

[0130] Specifically, the feasible region check examines whether the MPC algorithm can function correctly after mode switching. Because the floating mode and crawling mode use different models, control parameters (velocity or angular velocity) in one mode may cause the controller to malfunction in the other. Therefore, a feasible region check is necessary.

[0131] The feasible region check in step S3 is as follows:

[0132] The controller output is solved by crawling dynamics model. If the controller output is abnormal, it means it is not within the feasible region.

[0133] If the robot is outside the feasible region, a buffer trajectory needs to be generated to allow it to reach the feasible region before performing a state switch to achieve a safe mode switch. If the robot is within the feasible region, a switching signal is output, and step S4 is executed. The buffer trajectory is obtained by solving the MPC performance function using a model to obtain the optimal output.

[0134] Specifically, step S4 includes the following steps:

[0135] S4.1 determines whether the switching signal is within the extended shell outside the feasible region. If so, the system switches to floating mode (suspended movement), crawling mode (track attachment), or a transitional state. During execution, new hard triggers (DETs) are blocked, and the MPC optimization problem after the switching model is solved to obtain the output trajectory until the cleaning task is completed. Here, the feasible region refers to the area within which the robot can only take values ​​in a certain state on a plane due to certain constraints. The extended shell refers to the area that expands outward from the feasible region. Because there is an error between the feasible region check model and the actual feasible region of the robot, a conservative, larger area is adopted to avoid the feasible region mismatch problem caused by model error.

[0136] S4.2, If the switching signal is not outside the feasible domain and within the extended shell, then initialize the current state. Target state The constraints include actuator saturation constraints and control smoothness constraints. A predicted state sequence is generated, comprising position, attitude, velocity, and angular velocity. An MPC performance function considering smoothness is then established. :

[0137] ;

[0138] The first term is the error term, the second term is the energy consumption term, the third term is the input term, and the fourth term is the terminal constraint term. , These represent the current time and the prediction step size, respectively. The preset interval length; , , These are the weight matrices for state deviation, control input magnitude, and control increment (smoothness), respectively. It includes a cost-weighted matrix to ensure that the state converges to the target feasible region. They are respectively The state and thrust output at any given moment The current state. For the target state, For thrust output.

[0139] The smoothness-considered MPC (Multi-Process Control) is used for transition guidance, allowing the robot state to converge into the feasible region, thus enhancing the feasibility and safety of mode switching. If the buffer trajectory generation reaches the maximum number of executions, a yield point is set, and a safe yield procedure is executed.

[0140] S4.3 solves the MPC performance function to obtain the optimal output and executes it. It then determines whether the robot has entered the feasible region; if so, it performs a system switch. During execution, new DET hard triggers are disabled, and the MPC optimization problem after the model switch is solved to obtain the output trajectory, until the cleaning task is completed. The optimal output is defined differently in different modes: in floating mode, the output is the underwater thruster thrust; in crawling mode, the output is the track rotation speed.

[0141] S4.4, If the robot does not enter the feasible region, continue to check if the maximum number of executions has been reached. If so, perform a safe retreat and set a new retreat point. The retreat point satisfies: D_surface ≥ D_safe_recover, | ≤θ_recover、 ≤v_recover; D_safe_recover is the safe recovery distance, θ_recover is the attitude angle constraint, and v_recover is the velocity constraint.

[0142] S4.5 If the robot has not entered the feasible region and has not reached the maximum number of executions, then continue to step S4.2 to initialize the current state. Target state and constraint parameters.

[0143] like Figure 3 As shown, the method provided by this invention can track the trajectory of a robot in three modes. The robot is capable of autonomous adsorption and mode switching, and can perform hull cleaning work along the desired path.

[0144] This invention organically combines a dynamic event triggering mechanism, mode switching guard logic, and buffered trajectory generation MPC, providing a systematic solution to the problems existing in the field of trajectory tracking for ship cleaning robots, such as "high computational overhead, uncontrollable mode switching impact, single switching conditions, failure of feasible out-of-domain switching, neglect of disturbances, and energy consumption factors."

[0145] Replacing fixed-cycle optimization with dynamic event triggering significantly reduces the frequency of computation and execution mechanism actions, extending hardware lifespan and reducing energy consumption while maintaining trajectory accuracy.

[0146] A buffer trajectory is generated. Before mode switching, a constrained short-time-domain smooth transition trajectory is planned through MPC to ensure the continuity of attitude and force during the switching process, avoid shocks, and ensure the robust feasible region for the end to enter the new mode. This eliminates the instability and infeasibility problems caused by direct switching in the past.

[0147] The switching conditions are multi-factor and dynamic, using not only position, attitude, and speed, but also external disturbance assessment and energy consumption status. Dynamic thresholds and hysteresis strategies are used to prevent jittery switching and misjudgment.

[0148] The disturbance and energy consumption closed-loop is integrated into the decision-making process, which can adaptively adjust the triggering strategy in the case of strong disturbance or low power environment, thereby improving the task completion rate and operation safety under harsh conditions.

[0149] This invention not only improves the robustness and safety of trajectory tracking and mode switching, but also significantly improves task execution efficiency and system endurance in multi-mode, multi-task, and complex environments, solving a core problem that has long existed in existing technologies but has not been effectively addressed.

[0150] Of course, the above description is not intended to limit the present invention, and the present invention is not limited to the examples given above. Any changes, modifications, additions or substitutions made by those skilled in the art within the scope of the present invention should also fall within the protection scope of the present invention.

Claims

1. A multi-mode trajectory tracking method for a ship cleaning robot, characterized in that, Specifically, the steps include the following: S1. Establish a multi-mode switching model for the ship cleaning robot and estimate the robot's state. The multi-mode switching model includes: crawling dynamics model, swimming nonlinear model and hybrid transition model. S2. Combining the characteristics of the work scenario and the robot state estimation, design dynamic event triggers, including: no trigger, soft trigger and hard trigger, and establish a set of mode switching rules and guard conditions corresponding to the dynamic event triggers; S3, perform mode switching decision and feasible region check to obtain switching signal; S4. Perform a feasible region check on the target mode corresponding to the switching signal. Determine whether to generate a buffer trajectory to guide the target mode into the feasible region based on the feasible region check result. If the check result is infeasible, construct a model predictive control (MPC) buffer trajectory with smoothing constraints based on the target state. Perform system switching after the target state enters the feasible region. The specific steps of the S3 mode switching decision include the following: S3.1, Calculate the dynamic adsorption trigger threshold : ； in, This is the static reference distance. For real-time linear velocity, For pitch angle, , , These are the weighting constants for velocity, attitude, and credibility, respectively. For the feasibility of the robot system; S3.2, Determine: Is the distance to the hull surface D_surface greater than the adsorption trigger threshold? If yes, the status flag is in the floating cruise state. If no, the status flag is in the wall-attaching transition state. Then, continue to determine if the following conditions are met: hull distance ≤ attachment distance D_attach, attitude error angle ≤ attitude error angle threshold θ_tol, and normal velocity ≤ normal velocity threshold Vn_tol. If these conditions are met, the status flag is in the wall-attaching transition state. Continue to determine if there is abnormal detachment, abnormal energy consumption, excessive external disturbance, or completion of the cleaning task. If these conditions are met, the status flag is in the detachment transition state. Continue to determine if the following conditions are met: tracks stop or completely detach, and D_surface > D_attach. If these conditions are met, the status flag is in the floating cruise state. If these conditions are not met, the status flag is in the detachment transition state. S3.3 If the following conditions are not met: hull distance ≤ D_attach and attitude error ≤ θ_tol and normal velocity ≤ Vn_tol, then the state flag position is the wall-attached transition state; The feasible region check in step S3 is as follows: The controller output is solved by the crawling dynamics model. If the controller output is abnormal, it means that it is not within the feasible region. If it is not within the feasible region, a buffer trajectory is generated so that the robot can reach the interior of the feasible region and then the state switch is performed. If it is within the feasible region, a switching signal is output and step S4 is executed.

2. The multi-mode trajectory tracking method for a ship cleaning robot according to claim 1, characterized in that, Step S1 specifically includes the following steps: S1.1, Establish a multi-mode switching model for ship cleaning robots. for: ； in, Let be the system state vector. To control the input amount, The dynamic equations of the subsystem are... To switch signals, As a subsystem, It is a positive integer; S1.2, Robot's 6-axis orientation and position speed quantity and force The definition is as follows: ； in, Let these be the coordinates of the robot along each axis in the coordinate system. These are the orientation angles of the robot's axes. Let represent the linear velocity of the robot in each direction. Let be the angular velocities of the robot in all directions. Force is applied to the robot's 6 axes; To calculate attitude and position, the following vector is defined: ； in, For position matrix, This is the attitude matrix; The linear velocity of the ROV (Return on Vehicle) movement. The angular velocity of the ROV. The force matrix of the robot, This is the torque matrix of the robot.

3. The multi-mode trajectory tracking method for a ship cleaning robot according to claim 2, characterized in that, Step S1 also includes the following steps: S1.3, Construct the crawling dynamics model: The dynamic constraints of the ship cleaning robot under the crawling dynamics model are: ； ； ； in, The distance between the centers of the two tracks on either side of the robot; The radius of the track drive wheel; The distance between the robot's centroid and the center of the robot assembly is the lateral distance. , These are the angular velocities of the left and right tracks, respectively. , These are the slip ratios of the left and right tracks, respectively. The angle between the robot's heading and longitudinal direction. for The derivative of These are the current robot position coordinates. , The derivative; The saturation constraint of the tracked actuator of the ship cleaning robot under the crawling dynamics subsystem model is [- , ], Angular velocity threshold; S1.4, Construct a nonlinear floating model: , The expression is shown as: ； in, , These are the robot's rigid body mass and added mass matrices, respectively. It is the centripetal force and Coriolis force matrix. It is a hydrodynamic resistance term. It is resilience. It is the thrust of the propulsion unit. It is a vector composed of linear velocity and angular velocity. yes The derivative; S1.5, Construct a hybrid transition model: The basic dynamic model is consistent with the nonlinear model of the robot, and the following constraints are added: the robot's bow vector and the ship's surface normal vector are within ±6°, and the position constraint is obtained based on the ship's surface position information.

4. The multi-mode trajectory tracking method for a ship cleaning robot according to claim 3, characterized in that, Step S1 also includes the following steps: S1.6, Perform initialization settings. Based on the constraints and the robot's multi-mode switching model, generate the initial feasible region using the convex set projection method. Initialization settings include: initializing the current state. Enter robot hardware parameters and initialize prediction interval. Initialize the model prediction parameter matrix R, P, Q, where R is the weight for adjusting the control quantity, P is the weight for adjusting the end state error, and Q is the weight for adjusting the state error. Set actuator saturation constraints, including: maximum thrust of the propeller and maximum speed of the track; set constraint buffer intervals; and set control smoothness constraints. S1.7 performs outlier removal and filtering on the collected sensor data, completes environmental modeling by combining the start and end points of the operation with the three-dimensional surface map of the ship's hull, and uses the Kalman filter method to estimate the state of the ship cleaning robot.

5. The multi-mode trajectory tracking method for a ship cleaning robot according to claim 1, characterized in that, Step S2 specifically includes the following steps: S2.1, Set the hard trigger condition set, including: task-driven, security-driven, feasibility-driven and health-driven; S2.2, If the hard trigger condition set is met, then execute step S3; S2.3 If the hard trigger condition set is not met, then determine whether the following conditions are met: tracking error < soft threshold and error change acceleration < soft threshold. If met, continue to determine whether to execute the complete control sequence. If so, solve the MPC and obtain the next control sequence. If the complete control sequence is not executed, maintain the current mode and do not solve the MPC. Continue to execute the current control sequence and continue to determine whether the hard trigger condition set is met. S2.4 If the tracking error is not less than the soft threshold and the error change acceleration is less than the soft threshold, then continue to determine whether the following conditions are met: soft threshold < tracking error < hard threshold and soft threshold < error change acceleration < hard threshold. If these conditions are met, then re-solve the MPC to obtain the next control sequence, and continue to determine whether the hard trigger condition set is met.

6. The multi-mode trajectory tracking method for a ship cleaning robot according to claim 5, characterized in that, Task-driven scenarios include: when the robot's distance from the hull surface is less than the task threshold, it prepares to transition to a wall-adhering state; when the robot's distance from the hull surface is greater than the detachment threshold and the safe distance is greater than the minimum safe distance threshold, it indicates that the robot has safely detached from the hull and is preparing to switch to floating mode; when the robot's distance from the hull surface is less than or equal to the wall-adhering adsorption trigger threshold, it indicates that the robot has entered the adsorption range and is preparing to switch to adsorption operation mode; when the current cleaning task segment is completed, the robot system prepares to switch to the next task segment or return to a safe state; and when the task scheduling system actively requests a mode switch for multi-task collaboration or task replanning scenarios. Safety-driven actions include: the ambient water flow speed exceeds the preset safe flow rate limit, indicating that it affects the stability of the robot or the safety of operation; the adsorption force reaches the saturation limit, indicating that the adsorption system has failed or is overloaded, and there is a risk of falling off; if the collision time between the robot and the obstacle is predicted to be less than the minimum safe time threshold, obstacle avoidance or emergency braking measures should be taken immediately. Feasibility-driven categories include: the predicted future state has entered the feasible region of the target mode, indicating that it is safe to switch to the target mode; the constraint margin of the future control input is less than or equal to zero, indicating that the control quantity is about to reach saturation or exceed the limit, and the control strategy should be adjusted; the distance between the predicted state and the target feasible region is less than or equal to zero, indicating that the state has entered or is about to enter the feasible region, and the switch should be executed. Health-related issues include: failure of key sensors, affecting state estimation and control; actuator jamming, affecting motion control; and control algorithm calculation time exceeding the maximum allowable delay, affecting real-time control performance.

7. The multi-mode trajectory tracking method for a ship cleaning robot according to claim 1, characterized in that, Step S4 specifically includes the following steps: S4.1 Determine whether the switching signal is outside the feasible domain and inside the extended shell. If so, perform system switching to floating mode, crawling mode or transition state. During the execution, block new hard triggers (DETs), solve the MPC optimization problem after switching model, and obtain the output trajectory until the cleaning task is completed. S4.2, If the switching signal is not outside the feasible domain and within the extended shell, then initialize the current state. Target state The constraints include: actuator saturation constraints, control smoothness constraints, predicted state sequences, and an MPC performance function that considers smoothness. : ； in, , These represent the current time and the prediction step size, respectively. The preset interval length; , , These are the weight matrices for state deviation, control input magnitude, and control increment, respectively. It is a cost-weight matrix. They are respectively The state and thrust output at any given moment The current state. For the target state, For thrust output; S4.3, solve the MPC performance function, find the optimal output and execute the optimal output, determine whether the robot has entered the feasible region, if so, execute the system switch, during the execution, block the new DET hard trigger, and solve the MPC optimization problem after the switch model to obtain the output trajectory until the cleaning task is completed; S4.4, if the robot does not enter the feasible region, continue to check if the maximum number of executions has been reached. If so, perform a safe retreat and set a new retreat point. The retreat point satisfies: D_surface ≥ D_safe_recover, | ≤θ_recover、 ≤v_recover; D_safe_recover is the safe recovery distance, θ_recover is the attitude angle constraint, v_recover is the velocity constraint; S4.5 If the robot has not entered the feasible region and has not reached the maximum number of executions, then continue to step S4.2 to initialize the current state. Target state and constraint parameters.

Images