Underwater robot laying system for fire rescue

Abstract

The invention relates to the field of water rescue equipment control, and discloses an underwater robot laying system for fire rescue, which comprises a data acquisition module used for acquiring operation data in the laying process of an underwater robot and carrying out time synchronization and preprocessing; constructing a fusion module, and constructing a linear variable parameter prediction model to describe the dynamics of the hoisting system; the rolling optimization module is used for resolving to obtain a control instruction for the winch and the suspension arm in each control period; the control execution module is used for driving the winch and the suspension arm to perform corresponding actions; and the safety monitoring module is used for judging safety indexes in the laying process in real time. According to the method, synchronization and fusion preprocessing is carried out on multi-source heterogeneous operation data, a linear variable parameter prediction model capable of being self-adaptive to cable length and ocean current changes is established, dynamic constraints fusing ship motion disturbance and real-time obstacle avoidance requirements are generated, and optimal tracking control over a predetermined space path in a dynamic environment is achieved.

Description

Technical Field

[0001] This invention relates to the field of water rescue equipment control technology, specifically to a water rescue underwater robot deployment system. Background Technology

[0002] With the increasing demand for marine rescue, underwater exploration, and operations, underwater robots have become key equipment for performing tasks such as deep-sea search and rescue and target salvage. Their operational efficiency highly depends on the safe and precise deployment process from the mother ship to the underwater target point. This process is usually completed using shipborne cranes, A-frames, L-frames, or specialized deployment winches.

[0003] Existing deployment systems generally adopt a manual operation and instrument monitoring mode based on operator experience. Operators need to keep a close eye on various instrument data and manually adjust the equipment operating parameters based on their personal experience. The underwater robot is first lifted smoothly off the deck, and then slowly lowered into the water after passing over the ship's side. This system relies on the operator's real-time reaction and experience judgment and lacks the ability to adapt to changes in the environment. In emergency mission scenarios such as fire rescue, the marine environment is highly dynamic and uncertain. Ships are easily affected by wind and waves, resulting in continuous attitude disturbances. These disturbances are directly transmitted to the slings, causing the deployed underwater robot to swing violently and complexly. This not only causes the deployment path to deviate from the preset trajectory but also reduces the efficiency of the operation. Summary of the Invention

[0004] To address the shortcomings of existing technologies, this invention provides a deployment system for underwater robots used in fire and rescue operations. This system solves the problem that existing deployment systems for underwater robots used in fire and rescue operations often result in violent and complex swinging of the deployed underwater robot, causing the deployment path to deviate from the preset trajectory and reducing operational efficiency.

[0005] To achieve the above objectives, the present invention provides the following technical solution: a fire rescue underwater robot deployment system, comprising:

[0006] The data acquisition module is used to acquire operational data during the deployment of the underwater robot, and to perform time synchronization and preprocessing to form preprocessed data. The operational data includes ship motion data, marine environment data, deployment device status data, and underwater environment perception data.

[0007] A fusion module is constructed to build a linear variable parameter prediction model based on preprocessed data to describe the dynamics of the launching system, and to fuse the ship motion data with underwater environmental perception data to generate dynamic constraints.

[0008] The rolling optimization module is used to construct and solve the finite-time domain control problem based on the linear variable parameter prediction model and the dynamic constraints in each control cycle, and to obtain the control commands for the winch and boom.

[0009] The control execution module is used to drive the winch and boom to perform corresponding actions according to the control instructions, so that the underwater robot is deployed along a predetermined spatial path;

[0010] The safety monitoring module is used to determine the safety indicators during the deployment process in real time, and to trigger an emergency control strategy when the safety indicators exceed preset values.

[0011] By adopting the above technical solution, a linear variable parameter prediction model that can adapt to changes in cable length and ocean current is established by synchronizing and fusing multi-source heterogeneous operating data. Dynamic constraints that integrate ship motion disturbances and real-time obstacle avoidance requirements are generated. By solving the constrained finite-time domain optimal control problem online in each control cycle, optimal tracking control of the predetermined spatial path in a dynamic environment is achieved. Combined with feedforward compensation to actively offset the influence of ship motion, the problem of existing underwater robot deployment systems for fire rescue, which easily cause the deployed underwater robot to sway violently and complexly, resulting in the deployment path deviating from the preset trajectory and reducing operational efficiency is solved.

[0012] Preferably, the time synchronization and preprocessing specifically includes the following steps:

[0013] The system acquires ship motion data, marine environment data, deployment device status data, and underwater environment perception data during the deployment of underwater robots to form operational data.

[0014] The running data is timestamped and parsed, then filtered, denoised, and standardized to form preprocessed data that is time-synchronized and scaled uniformly.

[0015] Preferably, the construction of the linear variable parameter prediction model to describe the dynamics of the hoisting system includes the following steps:

[0016] Based on the dynamic analysis of the hoisting system, a state vector is defined that includes the underwater robot's position deviation, velocity deviation, cable swing angle, and swing angle angular velocity.

[0017] Define a control input vector that includes the winch speed increment, boom azimuth rate increment, and pitch rate increment;

[0018] Establish a discrete-time state-space equation with real-time cable length and real-time ocean current velocity as scheduling parameters;

[0019] Based on the real-time acquired cable length and the real-time ocean current velocity, the coefficient matrix of the state-space equation is updated to form a linear variable parameter prediction model.

[0020] Preferably, updating the coefficient matrix of the state-space equation includes the following steps:

[0021] Based on the real-time length of the cable and the real-time ocean current speed, the corresponding model update parameters are obtained by querying the preset parameter scheduling table.

[0022] The parameters are updated using the model, and the corresponding coefficient elements in the state-space equation are calculated and replaced.

[0023] The updated state-space equations are used as the linear variable parameter prediction model for the current control cycle.

[0024] Preferably, the process of generating dynamic constraints by fusing the ship motion data and underwater environment perception data includes the following steps:

[0025] Based on historical and current ship motion data, a ship motion disturbance sequence for a future time domain is calculated using a time series prediction model.

[0026] Based on the underwater environment perception data, the distance between each point on the predetermined spatial reference trajectory and the obstacle is calculated;

[0027] Based on the distance, the current speed of the underwater robot, and the ocean current speed, the radius of the safe corridor corresponding to each point of the reference trajectory is dynamically calculated.

[0028] Based on the safety corridor radius, a dynamic safety boundary that varies with time and space is generated, and the dynamic safety boundary and the ship motion disturbance sequence are used together as dynamic constraints.

[0029] Preferably, the dynamic calculation of the safety corridor radius corresponding to each point of the reference trajectory includes the following steps:

[0030] Obtain the preset basic safety radius, preset minimum safety radius, and preset influence coefficients of the current point on the reference trajectory;

[0031] Obtain the distance between the current point and the obstacle, the current speed of the underwater robot, and the real-time ocean current speed;

[0032] Based on the basic safety radius, the reduction determined by multiplying the distance, the current speed of the underwater robot, and the real-time ocean current speed by the corresponding influence coefficients is subtracted sequentially;

[0033] The calculation result is compared with the minimum safe radius, and the larger of the two values ​​is taken as the safe corridor radius of the current point.

[0034] Preferably, the calculation to obtain control commands for the winch and boom includes the following steps:

[0035] Within each control cycle of the rolling optimization module, the objective function is to minimize the trajectory tracking error and control increment change in the future prediction time domain.

[0036] Using the linear variable parameter prediction model as equality constraints, the physical and control increment limits of the winch and boom as inequality constraints, and the dynamic safety boundary in the dynamic constraints as output soft constraints, a quadratic programming problem is constructed.

[0037] The embedded quadratic programming solver is invoked to solve the quadratic programming problem in real time, thereby obtaining the control increment sequence;

[0038] The first control increment is extracted from the control increment sequence, and the control command of the previous moment is combined with the feedforward compensation amount calculated based on the real-time ship motion angular velocity to generate the control command of the current period.

[0039] Preferably, the step of calling the embedded quadratic programming solver to solve the quadratic programming problem in real time includes the following steps:

[0040] The objective function, the equality constraints, the inequality constraints, and the output soft constraints are converted into a standard quadratic programming form.

[0041] The embedded quadratic programming solver is initialized and iteratively solved within a preset time. If the solution is successfully solved within the preset time, a control increment sequence is output. If the solution times out or fails, a degradation control strategy is triggered.

[0042] Preferably, driving the winch and boom to perform corresponding actions includes the following steps:

[0043] The control commands are analyzed to obtain the winch target speed, boom target azimuth velocity, and boom target pitch velocity.

[0044] Calculate the boom motion feedforward compensation amount to counteract the effects of hull rotation based on the real-time angular velocity of the ship.

[0045] The feedforward compensation for boom motion is superimposed on the azimuth angular velocity and the pitch angular velocity of the boom target, respectively, to form the compensated boom control command;

[0046] The target speed of the winch and the compensated boom control command are converted into drive signals to control the winch and the boom to perform their functions.

[0047] Preferably, triggering the emergency control strategy when the safety indicator exceeds a preset value includes the following steps:

[0048] Continuously read the real-time measurements of cable tension and cable swing angle, and the distance between the underwater robot and obstacles from the status data of the deployment device;

[0049] Continuously monitor the solver status of the rolling optimization module;

[0050] If the cable tension exceeds the corresponding preset safety threshold, or the cable swing angle exceeds the corresponding preset safety threshold, or the distance is lower than the corresponding preset safety threshold, or the solver status is timeout failure, then it is determined that the safety index has exceeded the limit, and the preset emergency control strategy is triggered.

[0051] This invention provides an underwater robot deployment system for fire and rescue operations. It offers the following advantages:

[0052] 1. This invention establishes a linear variable parameter prediction model that can adapt to changes in cable length and ocean current by synchronizing and fusing multi-source heterogeneous operational data, and generates dynamic constraints that integrate ship motion disturbances and real-time obstacle avoidance requirements. By solving the constrained finite-time domain optimal control problem online in each control cycle, optimal tracking control of a predetermined spatial path in a dynamic environment is achieved. Combined with feedforward compensation to actively offset the influence of ship motion, this invention solves the problem that existing underwater robot deployment systems for fire rescue are prone to causing severe and complex swaying of the deployed underwater robot, which not only causes the deployment path to deviate from the preset trajectory but also reduces operational efficiency.

[0053] 2. This invention establishes a linear variable parameter prediction model with cable length and ocean current speed as scheduling parameters, enabling the system dynamics description to match the current operating conditions in real time. Simultaneously, based on this model and dynamic constraints that integrate ship motion disturbance prediction and real-time environmental perception, the optimal control problem is solved online in each control cycle. This allows the control system to proactively compensate for disturbances caused by ship random motion and dynamically plan safe paths to avoid obstacles. As a result, the underwater robot can be deployed stably and accurately along complex spatial paths, improving the deployment path tracking accuracy and adaptive capability under complex dynamic sea conditions.

[0054] 3. This invention uses a linear variable parameter prediction model to dynamically update its coefficients based on real-time measurements of cable length and ocean current velocity, ensuring that the dynamic model always closely reflects the current physical characteristics of the system. Combined with a closed-loop mechanism of prediction-optimization-feedback, it can re-plan the control sequence in the future short time domain based on the latest state and disturbance information, and has a certain tolerance for model errors and unmodeled dynamics. This allows the system to maintain good control performance when facing strong time-varying factors such as continuous changes in cable length and sudden changes in ocean currents, thus improving its adaptability and stability in non-stationary operating environments. Attached Figure Description

[0055] Figure 1 This is an architectural diagram of an underwater robot deployment system for fire rescue proposed in this invention;

[0056] Figure 2 This is a flowchart illustrating a method for deploying an underwater robot for fire rescue, as proposed in an embodiment of the present invention. Detailed Implementation

[0057] 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 embodiments of the present invention, and not all embodiments. 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.

[0058] Example 1:

[0059] In a first embodiment of the present invention, the present invention provides a deployment system for underwater robots used in fire rescue, such as... Figure 1 As shown, it includes:

[0060] The data acquisition module is used to acquire operational data during the deployment of the underwater robot, and to perform time synchronization and preprocessing to form preprocessed data. The operational data includes ship motion data, marine environment data, deployment device status data, and underwater environment perception data.

[0061] Further, time synchronization and preprocessing are performed, specifically including the following steps:

[0062] The system acquires ship motion data, marine environment data, deployment device status data, and underwater environment perception data during the deployment of underwater robots to form operational data.

[0063] The running data is timestamped and parsed, then filtered, denoised, and standardized to form preprocessed data that is time-synchronized and scaled uniformly.

[0064] Specifically, the data acquisition module performs time synchronization and preprocessing to ensure effective fusion of multi-source heterogeneous data and the reliability of subsequent intelligent control. Generally, raw signals from ship motion sensors, ship hydrological and meteorological observation sensors, deployment device sensors, and underwater robot-borne detection equipment differ in transmission protocols, sampling frequencies, and physical dimensions.

[0065] The data acquisition process simultaneously activates the acquisition threads of each sensor. Ship motion data may include roll angle, pitch angle, and heave displacement provided by the inertial measurement unit; ocean environment data may include ocean current profile velocity provided by the acoustic Doppler current profiler; deployment device status data may include cable length fed back by the encoder, boom angle fed back by the tilt sensor, and cable tension fed back by the tension sensor; underwater environment perception data may include obstacle point cloud information transmitted in real time by the forward-looking sonar of the underwater robot.

[0066] In the timestamp alignment and data packet parsing steps, the system uses a fixed period as a time window to parse and extract data packets from sensors that have the same timestamp or whose timestamps fall within the same window. For data with different sampling rates, a linear interpolation method is used to resample them to a unified control cycle timing sequence, thereby forming a strictly time-synchronized original data sequence.

[0067] Next, filtering and noise reduction are performed. For the low-frequency drift commonly found in ship motion signals, a first-order high-pass filter can be used to eliminate it; for high-frequency electronic noise, a low-pass filter with a cutoff frequency set according to the target signal bandwidth can be used. Outliers in sonar point cloud data can be filtered out using statistical distance methods. ,in, For the first Each sensor in time The original data, The filter function selected based on the characteristics of the sensor signal. For the corresponding set of filter parameters, This is the signal after noise reduction.

[0068] Finally, standardization is performed. The denoised data from each channel is subtracted from its long-term statistical mean and divided by its standard deviation, thereby eliminating the numerical scale differences between different physical dimensions such as angle, length, force, and velocity, transforming them to near a uniform, dimensionless standard normal distribution. The calculation can be expressed as: ,here, and The first The mean and standard deviation of each sensor signal obtained statistically during the initial calibration phase or the previous operating cycle. That is, the standardized data, the final output preprocessed data set is strictly synchronized in time, clean and effective in frequency band, and standardized and uniform in numerical scale.

[0069] A fusion module is constructed to build a linear variable parameter prediction model based on preprocessed data to describe the dynamics of the launching system, and to generate dynamic constraints by fusing ship motion data and underwater environmental perception data.

[0070] Furthermore, a linear variable parameter prediction model is constructed to describe the dynamics of the hoisting system, including the following steps:

[0071] Based on the dynamic analysis of the hoisting system, a state vector is defined that includes the underwater robot's position deviation, velocity deviation, cable swing angle, and swing angle angular velocity.

[0072] Define a control input vector that includes the winch speed increment, boom azimuth rate increment, and pitch rate increment;

[0073] Establish a discrete-time state-space equation with real-time cable length and real-time ocean current velocity as scheduling parameters;

[0074] Based on the real-time cable length and real-time ocean current velocity, the coefficient matrix of the state-space equation is updated to form a linear variable parameter prediction model.

[0075] Furthermore, updating the coefficient matrix of the state-space equations includes the following steps:

[0076] Based on the real-time cable length and real-time ocean current speed, query the preset parameter scheduling table to obtain the corresponding model update parameters;

[0077] Use the model to update parameters, calculate and replace the corresponding coefficient elements in the state-space equations;

[0078] The updated state-space equations are used as the linear variable parameter prediction model for the current control cycle.

[0079] Furthermore, dynamic constraints are generated by integrating ship motion data and underwater environment perception data, including the following steps:

[0080] Based on historical and current ship motion data, a time series prediction model is used to calculate the ship motion disturbance sequence in a future time domain.

[0081] Based on underwater environmental perception data, the distance between each point on the predetermined spatial reference trajectory and the obstacle is calculated;

[0082] Based on the distance, the current speed of the underwater robot, and the ocean current speed, the radius of the safe corridor corresponding to each point on the reference trajectory is dynamically calculated.

[0083] Based on the safety corridor radius, a dynamic safety boundary that varies with time and space is generated, and the dynamic safety boundary and the ship motion disturbance sequence are used together as dynamic constraints.

[0084] Furthermore, the dynamic calculation of the safety corridor radius corresponding to each point on the reference trajectory includes the following steps:

[0085] Obtain the preset basic safety radius, preset minimum safety radius, and preset influence coefficients of the current point on the reference trajectory;

[0086] Obtain the distance between the current point and the obstacle, the current speed of the underwater robot, and the real-time ocean current speed;

[0087] Based on the basic safety radius, the reduction is successively subtracted by the amount determined by multiplying the distance, the current speed of the underwater robot, and the real-time ocean current speed by the corresponding influence coefficients;

[0088] The calculated result is compared with the minimum safe radius, and the larger of the two values ​​is taken as the safe corridor radius of the current point.

[0089] Specifically, the fusion module creates a predictive model that can accurately describe the dynamics of the time-varying hoisting system and generates safety constraints that adapt to the dynamic environment, which forms the basis for subsequent real-time optimization control.

[0090] Specifically, based on the dynamic analysis of the hoisting system, a state vector is defined, which is constructed to contain ten state variables, and its mathematical representation is as follows: ,in, These represent the positional deviations of the underwater robot relative to the desired reference trajectory in the longitudinal, lateral, and vertical directions, respectively, within the ship's coordinate system. This represents the corresponding linear velocity deviation. and These represent the azimuth angle of the cable in the horizontal plane and the pitch angle relative to the vertical plane, respectively. and This represents the angular velocity of the two swing angles mentioned above. The definition of this state vector fully describes the system's pose, motion, and swing dynamics in space.

[0091] The corresponding control input vector is defined as: , This represents the increment of the winch's wire-laying and unlaying speed relative to the previous moment. and These represent the increments of the boom's azimuth rotation rate and pitch rate, respectively. This definition directly maps the actuator's control variables to the model input.

[0092] System dynamics is abstracted into a discrete-time state-space equation with key operating condition parameters as scheduling variables: Among them, the scheduling parameter vector , This is the real-time cable length. Real-time ocean current velocity. System matrix. Control input matrix and perturbation input matrix All are scheduling parameters The function. Measurable disturbance. This information originates from the predicted movement of ships.

[0093] When updating the model, a pre-set parameter scheduling table is queried based on the real-time acquired cable length and ocean current velocity. This table stores matrix coefficient values ​​or correction coefficients obtained through offline system identification or computational fluid dynamics simulation under different operating conditions. The corresponding elements in the state-space equations are updated using the queried parameters, ensuring that the prediction model always matches the current physical configuration and environmental loads, thus forming an adaptive linear variable parameter prediction model.

[0094] In the dynamic constraint generation section, firstly, based on historical ship motion data, an autoregressive model is used to predict the sequence of roll, pitch, and heave disturbances over several future control cycles, serving as feedforward information. Simultaneously, based on sonar sensing data from the underwater robot, the dynamic constraints at each discrete point on the reference trajectory are calculated. Euclidean distance to the nearest obstacle .

[0095] The calculation of the dynamic safety corridor radius is a key indicator of environmental adaptability, and its formulaic expression is as follows: ,in, It is the minimum safe radius to ensure basic operating space. The basic design radius under interference-free conditions. This is the current velocity vector of the underwater robot. For real-time ocean current velocity vectors, the coefficients are... , , These are preset weighting coefficients, used to adjust the influence of obstacle proximity, self-movement speed, and ocean current intensity on the contraction of the safety boundary. It is a very small positive constant used to prevent calculation overflow when the distance is zero. This formula enables the safety boundary to dynamically and elastically expand and contract according to environmental threats and motion states: the closer to the obstacle, the faster the speed, and the stronger the current, the narrower the safety corridor, and vice versa, but it is always no less than... .

[0096] Ultimately, the safety corridor radii at all trajectory points constitute a pipe-shaped dynamic safety boundary that varies spatially and temporally. This dynamic safety boundary, together with the aforementioned ship motion disturbance sequence, forms a complete set of dynamic constraints, providing the rolling optimization module with mathematical constraints that both reflect environmental disturbances and ensure obstacle avoidance safety. This enables the control system to intelligently perceive and adapt to the complex dynamic marine environment.

[0097] The rolling optimization module is used to construct and solve the finite-time domain control problem based on the linear variable parameter prediction model and dynamic constraints in each control cycle, and to obtain the control commands for the winch and boom.

[0098] Furthermore, the control commands for the winch and boom are calculated, including the following steps:

[0099] Within each control cycle of the rolling optimization module, the objective function is to minimize the trajectory tracking error and control increment change in the future prediction time domain.

[0100] A quadratic programming problem is constructed using a linear variable parameter prediction model as equality constraints, physical and control increment limits of the winch and boom as inequality constraints, and dynamic safety boundaries in the dynamic constraints as output soft constraints.

[0101] The embedded quadratic programming solver is invoked to solve the quadratic programming problem in real time, and the control increment sequence is obtained.

[0102] The first control increment is extracted from the control increment sequence, and the control command of the previous moment is combined with the feedforward compensation amount calculated based on the real-time ship motion angular velocity to generate the control command of the current period.

[0103] Furthermore, the embedded quadratic programming solver is invoked to solve the quadratic programming problem in real time, including the following steps:

[0104] The objective function, equality constraints, inequality constraints, and output soft constraints are converted into a standard quadratic programming form.

[0105] Initialize the embedded quadratic programming solver and iterate within a preset time. If the solution is successfully solved within the preset time, output the control increment sequence. If the solution times out or fails, trigger the degradation control strategy.

[0106] Specifically, the rolling optimization module solves a finite-time optimal control problem online in each control cycle, balancing trajectory tracking accuracy, control smoothness, and dynamic safety, and outputs the current optimal control command in real time.

[0107] At the start of each control cycle, the module first constructs an objective function, which aims to minimize the deviation between the system behavior and the expected behavior over a future predicted time period. The objective function consists of a weighted sum of three terms: The first term penalizes the tracking error between the predicted output and the reference trajectory. Here, the scalar... The vector represents the length of the prediction time domain, i.e., the number of control cycles predicted forward. Representative based on the current Future predicted by time information The system output at any given time is typically the position coordinates of the underwater robot, in vector form. Then it means Expected reference position at time, matrix It is a positive definite or semi-positive definite weight matrix used to adjust the relative importance of tracking accuracy in different output directions.

[0108] The second penalty control measures the magnitude of input variation, aiming to smooth the winch and boom movements and reduce mechanical shock. (Scalar) Represents the length of the control time domain, typically less than or equal to the prediction time domain. ,vector Representing the future The increment of the control input relative to the previous moment, i.e., the increment of the winch speed and the increment of the boom angular velocity. Matrix It is a positive definite weight matrix used to adjust the degree of suppression of changes in the control quantity.

[0109] The third term is the relaxation penalty term, used to handle the output soft constraint of the dynamic safety boundary. (Scalar) A sufficiently large penalty weighting coefficient. Scalar In order to be in The slack variable represents the amount of violation of the safety corridor constraint at any given time. Introducing this variable allows for a safe optimal solution that allows for minor and controllable violations, even when the optimization problem becomes unsolvable under strict constraints due to sudden environmental changes, thus enhancing the robustness of the algorithm.

[0110] Next, a set of constraints is constructed for the problem. Equality constraints are provided by the linear variable parameter prediction model provided by the aforementioned fusion module, which describes the dynamic relationship between the system state and the input. Inequality constraints include physical limits of the actuators, such as the maximum winch deployment speed, the maximum boom rotation angular velocity, and the maximum allowable range of control increments between adjacent control cycles. The output constraint is the dynamic safety boundary, which requires that the predicted underwater robot position trajectory must not exceed the elastic pipe defined by the safety corridor radius. This constraint is reflected in the objective function as a soft constraint through the aforementioned relaxation variables.

[0111] Subsequently, the module transforms the constrained optimization problem into a standard quadratic programming form, expands the terms in the objective function, substitutes the state-space equations, and finally organizes it into a standard form of a quadratic objective function for the future control increment sequence and a series of linear equality and inequality constraints.

[0112] Then, the embedded dedicated quadratic programming solver is invoked for real-time solution. This solver is optimized for the embedded platform to achieve fast convergence with limited computing resources. The system sets a time limit for the solver, for example, not exceeding 80% of the control cycle length. If the optimal solution is successfully found within this time limit, the optimal control increment sequence for the future control time domain is output. If the solution times out or fails due to numerical problems, a preset degraded control strategy is immediately triggered, such as switching to a computationally simple proportional-derivative controller to ensure the basic safety and controllability of the system.

[0113] Finally, from the successfully solved optimal control increment sequence, only the first element, i.e. the control increment that should be executed immediately in the current control cycle, is extracted. This increment is added to the control command actually applied at the previous moment to obtain the preliminary control command for the current cycle. On this basis, a feedforward compensation amount calculated based on the real-time ship motion angular velocity is further superimposed. This feedforward compensation amount aims to actively offset the displacement of the lifting point caused by the ship's roll and pitch motion. Its calculation depends on the ship's kinematic model. Finally, the comprehensive command after feedforward compensation is sent to the control execution module to drive the winch and boom to coordinate their movements. Thus, under the constraints of ship disturbance and complex underwater environment, the underwater robot can achieve accurate, smooth, and safe tracking of the predetermined spatial path.

[0114] The control execution module is used to drive the winch and boom to perform corresponding actions according to control commands, so that the underwater robot can be deployed along a predetermined spatial path.

[0115] Furthermore, the drive winch and boom perform corresponding actions, including the following steps:

[0116] Analyze the control commands to obtain the winch target speed, boom target azimuth velocity, and boom target pitch velocity;

[0117] Calculate the boom motion feedforward compensation amount to counteract the effects of hull rotation based on the real-time angular velocity of the ship.

[0118] The feedforward compensation for boom motion is superimposed on the azimuth angular velocity and pitch angular velocity of the boom target, respectively, to form the compensated boom control command.

[0119] The winch target speed and the compensated boom control command are converted into drive signals to control the winch and boom to perform their functions.

[0120] Specifically, the control execution module converts the high-level control commands calculated by the rolling optimization module into physical actions of the underlying actuators, and introduces feedforward compensation to actively suppress the adverse effects of hull movement, thereby ultimately achieving precise spatial path tracking of the underwater robot.

[0121] First, the system receives digital control commands from the rolling optimization module. These commands are data structures that encapsulate the desired actions of each actuator within the current control cycle. The parsing step involves extracting three key control setpoints from this data structure: the winch target line winding speed, and the line release / rewinding speed. Target azimuth rotational velocity of the boom and the target pitch rate of the boom These setpoints are theoretically optimal values ​​calculated based on the system dynamics model and optimization objectives.

[0122] However, under dynamic sea conditions, the mother ship's own motion will be affected by the hoisting points, adding an unwanted disturbance motion. To counteract this effect, the module calculates the feedforward compensation amount in parallel based on real-time ship motion sensor data. This calculation establishes the geometric and kinematic relationship between the ship's rotational motion and the crane's reverse compensation motion required to keep the hoisting points stationary in inertial space or moving along the desired trajectory.

[0123] The calculation of the feedforward compensation is based on the following kinematic principle. Let the real-time monitored ship rotational angular velocity be represented as a vector in the ship's coordinate system. These correspond to the angular velocities of roll, pitch, and sway, respectively. The position vector of the boom head, i.e., the cable connection point, relative to the ship's center of rotation, is denoted as . To counteract the linear velocity of the lifting points caused by the ship's rotation This requires the boom to generate a compensating velocity that is equal in magnitude but opposite in direction to the velocity.

[0124] Therefore, the required boom compensation angular velocity Jacobian matrix of the boom mechanism The pseudo-inverse can be used to approximate the solution, and the relationship can be expressed as: ,in, This refers to the calculated feedforward angular velocity of the boom's azimuth and pitch directions. It is a Jacobian matrix that describes the relationship between the linear velocity of the boom head and the angular velocity of the boom joints. Its specific form depends on the boom configuration, such as a typical A-frame or knuckle boom crane. This indicates its false rebellion.

[0125] After obtaining the feedforward compensated angular velocity, the module executes the instruction superposition step. The calculated azimuth compensated angular velocity is then... The target azimuth angular velocity obtained from analysis Perform algebraic addition to form the final azimuth axis control command. Similarly, this forms the final pitch axis control command. Winch target speed It is then used directly as the final instruction because it is less affected by the rotation of the hull.

[0126] Finally, the drive signal is converted and output. The final control command of the digital quantity is then processed. The data is converted into corresponding physical drive signals by the data conversion unit. For an electric winch, this signal may be an analog voltage or pulse frequency command sent to the servo driver; for a hydraulically driven boom, it may be a current signal sent to the proportional valve controller. These drive signals ultimately act on the winch motor and the boom hydraulic actuator, causing them to generate corresponding speeds and angular velocities, thereby collaboratively completing the precise and stable deployment of the underwater robot in space. By introducing feedforward compensation, the system can proactively counteract measurable disturbances in hull motion, improving the accuracy and stability of path tracking in dynamic environments such as waves.

[0127] The safety monitoring module is used to judge the safety indicators during the deployment process in real time and trigger emergency control strategies when the safety indicators exceed the preset values.

[0128] Furthermore, when safety indicators exceed preset values, an emergency control strategy is triggered, including the following steps:

[0129] Continuously read the real-time measurements of cable tension and cable swing angle, and the distance between the underwater robot and obstacles from the status data of the deployment device;

[0130] Continuously monitor the solver status of the rolling optimization module;

[0131] If the cable tension exceeds the corresponding preset safety threshold, or the cable swing angle exceeds the corresponding preset safety threshold, or the distance is lower than the corresponding preset safety threshold, or the solver status is timeout failure, it is determined that the safety index has exceeded the limit, triggering the preset emergency control strategy.

[0132] Specifically, the safety monitoring module performs real-time diagnosis of key physical quantities and control logic status during the deployment process, and triggers a preset emergency response mechanism when a potential hazard is detected.

[0133] Real-time measurements from underlying sensors are periodically read via a dedicated interface, primarily including cable tension reflecting structural loads. The cable azimuth swing angle characterizing system stability With pitch and sway angle And the distance between the underwater robot and the nearest obstacle, indicating the risk of obstacle avoidance. Meanwhile, the module continuously monitors the running status of the quadratic programming solver in the rolling optimization module. .

[0134] Each safety indicator has a corresponding dynamically adjustable safety threshold. The module performs real-time comparisons and logical judgments, the logic of which can be formally represented as follows: , where variables For overall alarm signs, Represents a logical OR operation. Once... When set, the module immediately interrupts the normal control command flow and executes the corresponding emergency strategy according to the type of out-of-limit indicator.

[0135] The correspondence between emergency strategies and indicators is as follows: If the cable tension exceeds the limit, the winch is instructed to enter passive cable-laying mode to unload the load; if the swing angle exceeds the limit, the boom is instructed to move in the opposite direction to implement active damping; if the obstacle avoidance distance exceeds the limit, all actuators are instructed to brake and the underwater robot to levitate; if the solver times out, it immediately switches to the preset degraded controller. Emergency commands have the highest priority, triggering high-level alarms and recording event data during execution. The system can only return to the main control loop after all indicators have returned to normal and have been confirmed by the operator.

[0136] Example 2:

[0137] In a second embodiment of the present invention, the present invention provides a method for deploying an underwater robot for fire rescue, such as... Figure 2 As shown, it includes the following steps:

[0138] The system acquires operational data during the deployment of underwater robots, performs time synchronization and preprocessing to form preprocessed data. The operational data includes ship motion data, marine environment data, deployment device status data, and underwater environment perception data.

[0139] Based on preprocessed data, a linear variable parameter prediction model is constructed to describe the dynamics of the launching system, and dynamic constraints are generated by integrating ship motion data and underwater environmental perception data.

[0140] Within each control cycle, based on the linear variable parameter prediction model and dynamic constraints, a finite-time domain control problem is constructed and solved to obtain control commands for the winch and boom.

[0141] According to the control command, the winch and boom are driven to perform corresponding actions, so that the underwater robot is deployed along the predetermined spatial path;

[0142] The system can assess safety indicators during the deployment process in real time and trigger emergency control strategies when the safety indicators exceed preset values.

[0143] In a near-shore fire rescue mission, an underwater robot needed to be rapidly deployed to the suspected distressed area. The sea conditions were complex, with irregular waves causing continuous ship swaying, low underwater visibility, and the potential presence of unknown obstacles. Traditional deployment methods relying on operator experience were risky and inefficient, making accurate and safe deployment difficult under dynamic disturbances. This invention addresses this complex scenario through an intelligent closed-loop system of perception, decision-making, and control. To solve these problems, a fire rescue underwater robot deployment method provided by this invention was adopted, the process of which is as follows: Figure 2 As shown. The specific implementation process of this method is as follows:

[0144] First, ship roll / pitch data, ocean current speed, cable length and tension, boom angle and underwater sonar detection data are acquired and preprocessed simultaneously to form a unified data stream after time alignment and noise reduction.

[0145] Next, based on the preprocessed data, a linear variable parameter prediction model for the launching system is established, using real-time cable length and ocean current velocity as scheduling parameters. Simultaneously, the predicted ship motion disturbances are fused with the dynamically calculated safe corridor radius based on sonar data to generate a dynamic constraint set that includes feedforward disturbances and elastic spatial boundary constraints.

[0146] Then, within each control cycle, based on the updated prediction model and dynamic constraints, a quadratic programming problem is constructed with tracking error and control smoothness as objectives and model equations and dynamic safety boundaries as constraints. The embedded solver is then called to perform real-time rolling optimization to obtain the current optimal winch speed and boom angular velocity commands.

[0147] Subsequently, before the command is executed, the feedforward compensation is calculated based on the real-time ship angular velocity and superimposed on the boom control command to actively counteract the influence of the ship's motion. The final drive signal is then sent to the winch and boom servo mechanism to drive the underwater robot to move along the predetermined spatial path.

[0148] Finally, through an independent safety monitoring thread, the cable tension, swing angle, obstacle avoidance distance, and solver status are continuously checked. When any safety indicator exceeds the limit, the corresponding emergency strategy is immediately triggered and executed until the risk is eliminated, thereby ensuring the safety of the operation throughout the process.

[0149] Although embodiments of the invention have been shown and described, it will be understood by those skilled in the art that various changes, modifications, substitutions and alterations can be made to these embodiments without departing from the principles and spirit of the invention, the scope of which is defined by the appended claims and their equivalents.

Claims

1. A system for deploying underwater robots for fire and rescue operations, characterized in that, include: The data acquisition module is used to acquire operational data during the deployment of the underwater robot, and to perform time synchronization and preprocessing to form preprocessed data. The operational data includes ship motion data, marine environment data, deployment device status data, and underwater environment perception data. A fusion module is constructed to build a linear variable parameter prediction model based on preprocessed data to describe the dynamics of the launching system, and to fuse the ship motion data with underwater environmental perception data to generate dynamic constraints. The rolling optimization module is used to construct and solve the finite-time domain control problem based on the linear variable parameter prediction model and the dynamic constraints in each control cycle, and to obtain the control commands for the winch and boom. The control execution module is used to drive the winch and boom to perform corresponding actions according to the control instructions, so that the underwater robot is deployed along a predetermined spatial path; The safety monitoring module is used to determine the safety indicators during the deployment process in real time, and to trigger an emergency control strategy when the safety indicators exceed preset values.

2. The underwater robot deployment system for fire rescue according to claim 1, characterized in that: The time synchronization and preprocessing process specifically includes the following steps: The system acquires ship motion data, marine environment data, deployment device status data, and underwater environment perception data during the deployment of underwater robots to form operational data. The running data is timestamped and parsed, then filtered, denoised, and standardized to form preprocessed data that is time-synchronized and scaled uniformly.

3. The underwater robot deployment system for fire rescue according to claim 1, characterized in that: The construction of a linear variable parameter prediction model to describe the dynamics of the hoisting system includes the following steps: Based on the dynamic analysis of the hoisting system, a state vector is defined that includes the underwater robot's position deviation, velocity deviation, cable swing angle, and swing angle angular velocity. Define a control input vector that includes the winch speed increment, boom azimuth rate increment, and pitch rate increment; Establish a discrete-time state-space equation with real-time cable length and real-time ocean current velocity as scheduling parameters; Based on the real-time acquired cable length and the real-time ocean current velocity, the coefficient matrix of the state-space equation is updated to form a linear variable parameter prediction model.

4. The underwater robot deployment system for fire rescue according to claim 3, characterized in that: Updating the coefficient matrix of the state-space equation includes the following steps: Based on the real-time length of the cable and the real-time ocean current speed, the corresponding model update parameters are obtained by querying the preset parameter scheduling table. The parameters are updated using the model, and the corresponding coefficient elements in the state-space equation are calculated and replaced. The updated state-space equations are used as the linear variable parameter prediction model for the current control cycle.

5. A fire rescue underwater robot deployment system according to claim 1, characterized in that: The process of generating dynamic constraints by fusing the ship motion data and underwater environment perception data includes the following steps: Based on historical and current ship motion data, a ship motion disturbance sequence for a future time domain is calculated using a time series prediction model. Based on the underwater environment perception data, the distance between each point on the predetermined spatial reference trajectory and the obstacle is calculated; Based on the distance, the current speed of the underwater robot, and the ocean current speed, the radius of the safe corridor corresponding to each point on the reference trajectory is dynamically calculated. Based on the safety corridor radius, a dynamic safety boundary that varies with time and space is generated, and the dynamic safety boundary and the ship motion disturbance sequence are used together as dynamic constraints.

6. A fire rescue underwater robot deployment system according to claim 5, characterized in that: The dynamic calculation of the safety corridor radius corresponding to each point of the reference trajectory includes the following steps: Obtain the preset basic safety radius, preset minimum safety radius, and preset influence coefficients of the current point on the reference trajectory; Obtain the distance between the current point and the obstacle, the current speed of the underwater robot, and the real-time ocean current speed; Based on the basic safety radius, the reduction determined by multiplying the distance, the current speed of the underwater robot, and the real-time ocean current speed by the corresponding influence coefficients is subtracted sequentially; The calculation result is compared with the minimum safe radius, and the larger of the two values ​​is taken as the safe corridor radius of the current point.

7. A fire rescue underwater robot deployment system according to claim 1, characterized in that: The calculation yields control commands for the winch and boom, including the following steps: Within each control cycle of the rolling optimization module, the objective function is to minimize the trajectory tracking error and control increment change in the future prediction time domain. Using the linear variable parameter prediction model as equality constraints, the physical and control increment limits of the winch and boom as inequality constraints, and the dynamic safety boundary in the dynamic constraints as output soft constraints, a quadratic programming problem is constructed. The embedded quadratic programming solver is invoked to solve the quadratic programming problem in real time, thereby obtaining the control increment sequence; The first control increment is extracted from the control increment sequence, and the control command of the previous moment is combined with the feedforward compensation amount calculated based on the real-time ship motion angular velocity to generate the control command of the current period.

8. A fire rescue underwater robot deployment system according to claim 7, characterized in that: The step of calling the embedded quadratic programming solver to solve the quadratic programming problem in real time includes the following steps: The objective function, the equality constraints, the inequality constraints, and the output soft constraints are converted into a standard quadratic programming form. The embedded quadratic programming solver is initialized and iteratively solved within a preset time. If the solution is successfully solved within the preset time, a control increment sequence is output. If the solution times out or fails, a degradation control strategy is triggered.

9. A fire rescue underwater robot deployment system according to claim 1, characterized in that: The process of driving the winch and boom to perform corresponding actions includes the following steps: The control commands are analyzed to obtain the winch target speed, boom target azimuth velocity, and boom target pitch velocity. Calculate the boom motion feedforward compensation amount to counteract the effects of hull rotation based on the real-time angular velocity of the ship. The feedforward compensation for boom motion is superimposed on the azimuth angular velocity and the pitch angular velocity of the boom target, respectively, to form the compensated boom control command; The target speed of the winch and the compensated boom control command are converted into drive signals to control the winch and the boom to perform their functions.

10. A fire rescue underwater robot deployment system according to claim 1, characterized in that: The emergency control strategy triggered when the safety indicator exceeds a preset value includes the following steps: Continuously read the real-time measurements of cable tension and cable swing angle, and the distance between the underwater robot and obstacles from the status data of the deployment device; Continuously monitor the solver status of the rolling optimization module; If the cable tension exceeds the corresponding preset safety threshold, or the cable swing angle exceeds the corresponding preset safety threshold, or the distance is lower than the corresponding preset safety threshold, or the solver status is timeout failure, then it is determined that the safety index has exceeded the limit, and the preset emergency control strategy is triggered.

Images