ROV opening hole pose anti-disturbance control system and method based on long baseline integrated navigation

Abstract

The application discloses a ROV opening position anti-disturbance control system and method based on long baseline integrated navigation, and the system comprises a long baseline positioning auxiliary module, a disturbance observation and control module and an execution module; the long baseline positioning auxiliary module obtains an absolute position through a transponder array, obtains a relative position through a strapdown inertial navigation system and a Doppler velocity meter, and outputs a position through an adaptive unscented Kalman filter; in the disturbance observation and control module, an extended state observer takes the fused position as input to observe total disturbance, a sliding mode controller generates control force based on position deviation and corrects feedforward through disturbance estimation, and a thruster dynamic compensation unit compensates cavitation and dead zone characteristics; the execution module distributes the compensated control force to a vector thruster through a construction-analysis method. The application solves the problems of navigation accuracy and frequency contradiction in deep opening operation, observer miscompensation and instability caused by non-ideal characteristics of a thruster.

Description

Technical Field

[0001] This invention relates to the fields of deep-sea emergency rescue, underwater robot control and marine engineering technology, and in particular to a disturbance-resistant control system and method for ROV opening posture based on long baseline integrated navigation. Background Technology

[0002] As global maritime transport and offshore oil and gas development expand into deeper waters, shipwrecks are becoming increasingly frequent. Residual fuel oil and liquid hazardous chemicals within the wreck compartments have become a major threat to marine ecosystems. Deep-water recovery of residual liquid from sunken ships requires drilling operations on the hull of the shipwreck at depths of 300 to 6000 meters under high pressure, low visibility, and strong ocean currents. This involves using a remotely operated vehicle (ROV) carrying drilling equipment. During the drilling process, the drill bit generates a violent impact reaction force when it contacts and penetrates the hull's steel plates. This, combined with external disturbances such as unpredictable ocean currents and changes in umbilical cable towing force, places extremely high demands on the attitude stability of the ROV.

[0003] Current remotely operated vehicle (ROV) attitude control systems typically employ a strapdown inertial navigation system (SINS) combined with a Doppler velocimeter to acquire high-frequency relative attitude data. However, SINS suffers from inherent cumulative drift, and positioning errors can accumulate to the meter level over extended operation. Long-baseline underwater acoustic positioning systems can provide centimeter-level absolute geodetic position, but their data update rate is typically only 1 Hz, making them unsuitable for direct high-frequency feedback control. Furthermore, at great depths, cavitation occurs during high-speed thruster rotation, leading to a sharp decrease in thrust. Dead zones caused by static friction exist in the low-speed range, rendering low-thrust commands ineffective.

[0004] The technical problems with the existing technology are as follows: a single navigation system cannot simultaneously meet the dual requirements of centimeter-level positioning accuracy and hundred-hertz-level update frequency for deep-water drilling operations; when the control system uses an extended state observer to estimate and compensate for external disturbances, if the observer input contains the cumulative drift error of the strapdown inertial navigation system, it will misjudge the positioning drift as an external disturbance and generate incorrect compensation output; at the same time, the cavitation decay and low-speed dead zone characteristics of the thruster make the control commands unable to be effectively executed under extreme deep-water conditions, resulting in instability of the drilling attitude. Summary of the Invention

[0005] In view of the above problems, a disturbance rejection control system and method for ROV aperture pose based on long baseline integrated navigation is proposed to overcome or at least partially solve the above problems. Specifically:

[0006] A disturbance rejection control system for ROV opening pose based on long baseline integrated navigation includes:

[0007] The long baseline positioning assistance module includes at least three transponders, a long baseline positioning terminal, a strapdown inertial navigation system, a Doppler velocimeter, and an adaptive unscented Kalman filter. The long baseline positioning terminal is used to receive transponder signals and calculate the absolute geodetic position of the remotely operated vehicle (ROV). The strapdown inertial navigation system and the Doppler velocimeter are used to acquire the relative pose data of the ROV relative to the initial navigation alignment point. The adaptive unscented Kalman filter is used to fuse the absolute geodetic position and relative pose data and output the fused pose.

[0008] The disturbance observation and control module includes an extended state observer, a sliding mode controller, and a thruster dynamic compensation unit. The extended state observer takes the fused pose as input and is used to perform lumped observation of external disturbances and unmodeled dynamics, and outputs disturbance estimates. The sliding mode controller generates a nominal control force based on the deviation between the desired pose and the fused pose, and uses the disturbance estimates for feedforward compensation correction. The thruster dynamic compensation unit is used to perform nonlinear compensation for the cavitation attenuation characteristics and low-speed dead zone characteristics of the corrected control force.

[0009] The execution module includes at least six vector-arranged thrusters and a thrust distribution unit. The thrust distribution unit uses a constructive-analytical method to distribute the control force compensated by the thruster dynamic compensation unit to at least six vector-arranged thrusters.

[0010] Optionally, the adaptive unscented Kalman filter performs Sigma point sampling on the state vector through UT transform. The state vector consists of the position and velocity of the remotely operated vehicle, the attitude of the remotely operated vehicle, the gyroscope drift of the strapdown inertial navigation system, and the accelerometer zero bias. The adaptive unscented Kalman filter drives the state prediction with relative pose data, uses the absolute position of the ground as the measurement value, and performs measurement update after time registration of the absolute position of the ground with the state prediction at the corresponding time using delayed measurement update technology.

[0011] Optionally, the extended state observer adopts a third-order extended state observer structure, with the fused pose as input. The disturbance estimates output by the extended state observer lumped together represent the changes in ocean current force, borehole impact reaction force, umbilical cable drag force, and unmodeled dynamic characteristics.

[0012] Optionally, the thruster dynamic compensation unit calculates the cavitation number in real time based on the thruster speed and ambient pressure. When the cavitation number is lower than a preset threshold, the thruster dynamic compensation unit generates a thrust attenuation compensation amount based on the pre-stored cavitation number-thrust attenuation characteristic curve. When the thrust command of the thruster is lower than a preset dead zone threshold, the thruster dynamic compensation unit injects a high-frequency flutter signal.

[0013] Optionally, when any thruster fails, the thrust distribution unit sets the upper limit of the thruster to zero through the fault detection logic, and re-solves the thrust distribution scheme through the construction-analysis method. During the re-solution process, the weight of the degree of freedom corresponding to the axial thrust of the opening is set to the highest.

[0014] A disturbance-resistant control method for ROV aperture pose based on long baseline integrated navigation includes:

[0015] The relative pose data of the remotely operated vehicle (ROV) relative to the initial navigation alignment point is obtained by a strapdown inertial navigation system and a Doppler velocimeter. The absolute ground position of the ROV is obtained by a long baseline positioning array. An adaptive unscented Kalman filter is used to fuse the relative pose data and the absolute ground position to obtain the fused pose.

[0016] The fused pose input is used to expand the state observer to obtain perturbation estimates for external disturbances and unmodeled dynamics.

[0017] Based on the deviation between the desired pose and the fused pose, a nominal control force is generated by the sliding mode controller. The nominal control force is then corrected by feedforward compensation using the disturbance estimate. Finally, the dynamic compensation unit of the thruster performs nonlinear compensation on the corrected control force for cavitation attenuation characteristics and low-speed dead zone characteristics to obtain the compensated control force.

[0018] The compensated control force is distributed to at least six vector-arranged thrusters and executed using a constructive-analytical method.

[0019] At preset time intervals, the absolute geodetic position of the remotely operated vehicle is obtained through a long baseline positioning array, and the absolute geodetic position is input into an adaptive unscented Kalman filter to correct the cumulative drift of the strapdown inertial navigation system.

[0020] Optionally, when the bottom tracking signal of the Doppler velocimeter is lost or the output data of the Doppler velocimeter is abnormal, the adaptive unscented Kalman filter identifies the failure state through residual monitoring and switches to a combined mode in which the state prediction is performed recursively by the strapdown inertial navigation system and the measurement update is provided by the absolute position of the ground.

[0021] Optionally, the control law of the sliding mode controller includes an equivalent control term, a switching control term, and a disturbance compensation term. The switching control term adopts a combined approach law, and the approach rate of the combined approach law is positively correlated with the deviation between the desired pose and the fused pose.

[0022] Optionally, the construction-analytical method establishes and solves the mapping relationship between the thrust and control force vectors of each thruster when the thruster is not saturated, and switches to the construction method when the thruster is saturated, setting the weights of the degrees of freedom corresponding to the axial thrust of the opening and the anti-overturning moment to the highest.

[0023] Optionally, the preset time interval is not less than 15 seconds. For each preset level decrease in the gyroscope drift rate of the strapdown inertial navigation system, the preset time interval increases by one step value. For each preset level decrease in the operating water depth of the remotely operated unmanned underwater vehicle, the preset time interval increases by one step value.

[0024] This invention fuses the absolute geodetic position obtained by a long-baseline positioning array with the relative pose data obtained by a strapdown inertial navigation system and a Doppler velocimeter using an adaptive unscented Kalman filter. The resulting fused pose outputs a fused pose with centimeter-level accuracy and 100Hz-level frequency, solving the technical problem that a single navigation system cannot simultaneously meet the high-precision and high-frequency pose measurement requirements of deep-sea drilling operations. The extended state observer uses the fused pose as input instead of directly using the original inertial navigation data containing accumulated drift. This ensures that external disturbances and unmodeled dynamic lumped observations are not affected by the drift error of the strapdown inertial navigation system, thus providing accurate disturbance feedforward compensation for the sliding mode controller and preventing misjudgment of positioning drift as external disturbance, leading to incorrect compensation. The thruster dynamic compensation unit performs nonlinear compensation for cavitation attenuation characteristics and low-speed dead zone characteristics, ensuring that control force commands can still be effectively executed by the thrusters under deep-sea and high-pressure conditions. Combined with the constructivist-analytical method, the compensated control force is distributed to multiple vector-arranged thrusters, guaranteeing the pose stability of the remotely operated vehicle (ROV) during the drilling process. Attached Figure Description

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

[0026] Figure 1 This is a schematic diagram of the overall structure of an ROV aperture pose anti-disturbance control system based on long baseline integrated navigation provided by an embodiment of the present invention;

[0027] Figure 2 This is a data fusion flowchart of the long baseline positioning assistance module provided in an embodiment of the present invention;

[0028] Figure 3 This is a block diagram of the combined control of the extended state observer and sliding mode controller provided in an embodiment of the present invention;

[0029] Figure 4 This is a flowchart of the dynamic compensation unit for the thruster provided in an embodiment of the present invention;

[0030] Figure 5 This is a flowchart of an ROV aperture pose anti-disturbance control method based on long baseline integrated navigation provided by an embodiment of the present invention. Detailed Implementation

[0031] To make the above-mentioned objects, features, and advantages of the present invention more apparent and understandable, the present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments. Obviously, the described embodiments are only some, not all, of the embodiments of the present invention. All other embodiments obtained by those skilled in the art based on the embodiments of the present invention without creative effort are within the scope of protection of the present invention.

[0032] Reference Figures 1 to 5 This invention provides a disturbance rejection control system and method for ROV aperture pose based on long baseline integrated navigation, specifically:

[0033] A disturbance rejection control system for ROV opening pose based on long baseline integrated navigation includes:

[0034] The long baseline positioning assistance module includes at least three transponders, a long baseline positioning terminal, a strapdown inertial navigation system, a Doppler velocimeter, and an adaptive unscented Kalman filter. The long baseline positioning terminal is used to receive transponder signals and calculate the absolute geodetic position of the remotely operated vehicle (ROV). The strapdown inertial navigation system and the Doppler velocimeter are used to acquire the relative pose data of the ROV relative to the initial navigation alignment point. The adaptive unscented Kalman filter is used to fuse the absolute geodetic position and relative pose data and output the fused pose.

[0035] The disturbance observation and control module includes an extended state observer, a sliding mode controller, and a thruster dynamic compensation unit. The extended state observer takes the fused pose as input and is used to perform lumped observation of external disturbances and unmodeled dynamics, and outputs disturbance estimates. The sliding mode controller generates a nominal control force based on the deviation between the desired pose and the fused pose, and uses the disturbance estimates for feedforward compensation correction. The thruster dynamic compensation unit is used to perform nonlinear compensation for the cavitation attenuation characteristics and low-speed dead zone characteristics of the corrected control force.

[0036] The execution module includes at least six vector-arranged thrusters and a thrust distribution unit. The thrust distribution unit uses a constructive-analytical method to distribute the control force compensated by the thruster dynamic compensation unit to at least six vector-arranged thrusters.

[0037] Specifically, the long baseline positioning assistance module includes at least three transponders, a long baseline positioning terminal, a strapdown inertial navigation system, a Doppler velocimeter, and an adaptive unscented Kalman filter.

[0038] At least three transponders were deployed on the seabed around the wreck to form a long baseline positioning array. The operating frequency range of each transponder was selected from 8kHz to 16kHz, and the built-in battery pack supported continuous operation for more than 72 hours. The deployment points of the transponders were selected according to the principle of optimal geometric accuracy attenuation factor to ensure that the transponder array covered the predetermined opening operation area and that the transponders were not in a collinear or nearly collinear positional relationship. After the transponders were deployed, the coordinates of each transponder in the geodetic coordinate system were measured by the mother ship's ultra-short baseline positioning system to establish an absolute coordinate system benchmark.

[0039] The long baseline positioning terminal is mounted on the remotely operated vehicle (ROV). It transmits interrogation signals to each transponder on the seabed according to a preset interrogation cycle. After receiving the interrogation signal, each transponder returns a response signal after a fixed delay. The long baseline positioning terminal measures the round-trip propagation time of the interrogation signal from transmission to reception of the response signals from each transponder. Combined with underwater sound velocity profile data, it calculates the slant distance between the ROV and each transponder. After obtaining the slant distance measurements from at least three transponders, it uses the principle of spherical intersection to calculate the three-dimensional absolute position of the ROV in the geodetic coordinate system. The single positioning accuracy is within 0.1m.

[0040] The strapdown inertial navigation system and Doppler velocimeter are mounted on the remotely operated vehicle (ROV). After the ROV descends to the operating depth, the strapdown inertial navigation system is activated for initial alignment, establishing an initial alignment point as a reference for relative attitude data. After initial alignment, the gyroscope and accelerometer inside the strapdown inertial navigation system continuously output angular rate and specific force data. Through mechanical arrangement algorithms, the changes in position, velocity, and attitude of the ROV relative to the initial alignment point are recursively calculated by time integration. The Doppler velocimeter emits acoustic pulses to the seabed and receives bottom tracking echoes. The velocity vector of the ROV relative to the seabed is measured using the Doppler frequency shift principle. The output data of the strapdown inertial navigation system and the Doppler velocimeter are locally fused through a combined navigation filter to output the relative attitude data of the ROV relative to the initial alignment point.

[0041] An adaptive unscented Kalman filter is used to fuse absolute geodetic position and relative pose data and output a fused pose. The absolute geodetic position comes from the solution results of a long baseline positioning terminal, while the relative pose data comes from the output of a strapdown inertial navigation system and a Doppler velocimeter combined navigation system. The two types of data differ in terms of update rate, reference frame, and error characteristics. The adaptive unscented Kalman filter performs optimal fusion of the two types of data through a recursive processing framework of state prediction and measurement update.

[0042] The disturbance observation and control module includes an extended state observer, a sliding mode controller, and a thruster dynamic compensation unit.

[0043] The extended state observer takes the fused pose output from the adaptive unscented Kalman filter as input. The fused pose combines the absolute accuracy of long baseline positioning with the high-frequency response characteristics of strapdown inertial navigation systems. The extended state observer performs lumped observations on external disturbances and unmodeled dynamics in the dynamic model of the remotely operated vehicle (ROV). External disturbances include ocean current forces, impact reaction forces during drilling operations, and changes in umbilical cable drag forces. Unmodeled dynamics include nonlinear hydrodynamic effects and friction forces that are not accurately modeled in the ROV's dynamic model. The extended state observer lumps the above factors into a total disturbance state variable, estimates it in real time, and outputs the disturbance estimate.

[0044] The sliding mode controller generates a nominal control force based on the deviation between the desired pose and the fused pose. The desired pose is the preset position and attitude of the opening based on the coordinates and direction of the opening point on the wreck hull. The sliding surface is composed of a linear combination of the deviation and its rate of change. The control law drives the system state to move towards the sliding surface and slides along the sliding surface to the equilibrium point after reaching the sliding surface. The sliding mode controller also uses the disturbance estimate output by the extended state observer to perform feedforward compensation correction on the nominal control force. The disturbance estimate is superimposed on the control force command in a feedforward manner, generating a corresponding compensation force before the external disturbance acts on the remotely operated unmanned underwater vehicle body.

[0045] The thruster dynamic compensation unit receives the control force after feedforward compensation correction and performs nonlinear compensation for the cavitation attenuation characteristics of the thruster under deep high-pressure environment. The cavitation attenuation characteristics refer to the thrust attenuation caused by the formation and collapse of cavitation bubbles when the local pressure on the surface of the thruster propeller blade drops below the saturated vapor pressure of seawater. The thruster dynamic compensation unit also performs nonlinear compensation for the low-speed dead zone characteristics of the thruster caused by static friction and insufficient starting torque under small thrust command.

[0046] The execution module includes at least six vector-arranged thrusters and a thrust distribution unit.

[0047] Vector arrangement refers to the installation of the thrust axes of each thruster on the remotely operated underwater vehicle (ROV) body at a preset spatial angle, enabling the thruster assembly to generate all six degrees of freedom control forces and moments, including longitudinal force, lateral force, vertical force, roll moment, pitch moment, and yaw moment. The thrust distribution unit receives the control forces compensated by the thruster dynamic compensation unit and uses a constructive-analytical method to distribute the compensated control forces to at least six vector-arranged thrusters. The compensated control forces are vectors containing the desired forces and moments of the aforementioned six degrees of freedom. The thrust distribution unit converts the components of this vector into thrust commands for each thruster.

[0048] During the drilling operation, at preset time intervals, the long baseline positioning terminal performs an interrogation and slant range measurement process to obtain the current absolute position of the remotely operated vehicle on the ground. This absolute position on the ground is then used as a measurement value to input into an adaptive unscented Kalman filter to correct the cumulative drift of the strapdown inertial navigation system.

[0049] In one or more embodiments of the present invention, the adaptive unscented Kalman filter performs Sigma point sampling on the state vector through UT transformation. The state vector consists of the position and velocity of the remotely operated vehicle (ROV), the attitude of the ROV, the gyroscope drift of the strapdown inertial navigation system, and the accelerometer zero bias. The adaptive unscented Kalman filter drives state prediction with relative pose data, uses the absolute position of the ground as the measurement value, and performs measurement update after time registration of the absolute position of the ground with the state prediction at the corresponding time using delayed measurement update technology.

[0050] The core function of the adaptive unscented Kalman filter is to fuse the absolute geodetic position calculated by the long baseline positioning terminal with the relative pose data jointly output by the strapdown inertial navigation system and the Doppler velocimeter, outputting a fused pose that combines absolute accuracy with high-frequency response characteristics. To achieve this fusion, the adaptive unscented Kalman filter uses relative pose data to drive recursive calculations in the state prediction stage, and uses the absolute geodetic position as the measurement value to correct the state prediction results in the measurement update stage. It also employs a delayed measurement update technique to handle the inherent delay of long baseline positioning.

[0051] The adaptive unscented Kalman filter defines a 15-dimensional state vector, consisting of: the 3D position, 3D velocity, and 3D attitude angles of the remotely operated vehicle (ROV) in the navigation coordinate system; the constant drift of the 3-axis gyroscopes in the strapdown inertial navigation system (SINS); and the constant zero bias of the 3-axis accelerometers. Position is the 3D coordinates of the ROV in the geographic coordinate system; velocity is the 3D velocity component of the ROV in the geographic coordinate system; attitude is the three attitude angles of the ROV relative to the geographic coordinate system; gyroscope drift is the constant drift of the 3-axis gyroscopes within the SINS; and accelerometer zero bias is the constant zero bias of the 3-axis accelerometers within the SINS. Incorporating gyroscope drift and accelerometer zero bias into the state vector for online estimation allows the adaptive unscented Kalman filter to correct sensor errors in the SINS with each measurement update, thereby suppressing the accumulation of errors over time in pure inertial recursion. The noise covariance matrix of the state vector is initially set based on the sensor performance parameters of the strapdown inertial navigation system and the Doppler velocimeter.

[0052] After obtaining the state vector and its error covariance matrix, the adaptive unscented Kalman filter uses the UT transform to generate a set of Sigma sampling points. The basic principle of the UT transform is to select a finite number of deterministic sampling points near the current estimated value of the state vector, according to the distribution characteristics of the error covariance matrix, and assign each sampling point a corresponding weight. The parameters of the UT transform are set as follows: the primary scaling factor is 0.5, the secondary scaling factor is 2, and the center point weight adjustment factor is 0. The Sigma sampling points generated by the UT transform are propagated nonlinearly through the state transition function one by one to obtain the propagated sampling point set. Then, the propagated sampling points are weighted and summed to obtain the state prediction value and its prediction error covariance matrix. The input of the state transition function is the estimated value of the state vector at the current time and the angular rate and specific force measurements output by the strapdown inertial navigation system. The state transition function is established according to the mechanical arrangement equations of the strapdown inertial navigation system, describing the evolution of the position, velocity, attitude, gyroscope drift, and accelerometer zero bias of the remotely operated underwater vehicle over time. The state prediction process is driven by the relative pose data jointly output by the strapdown inertial navigation system and the Doppler velocimeter. Specifically, the output data of the combined navigation of the strapdown inertial navigation system and the Doppler velocimeter is used as the driving force for state prediction, and the pose state of the remotely operated unmanned underwater vehicle is recursively calculated at a high frequency to maintain a prediction update rate of 100Hz.

[0053] The long baseline positioning terminal (LBS) is mounted on a remotely operated vehicle (ROV) and transmits interrogation signals to various transponders on the seabed according to a preset interrogation cycle. Each transponder, upon receiving the interrogation signal, returns a response signal after a fixed delay. The LBS measures the round-trip propagation time of the interrogation signal from transmission to reception of each transponder's response signal, and calculates the slant distance between the ROV and each transponder using underwater sound velocity profile data. Once the slant distance measurements from at least three transponders are obtained, the LBS calculates the ROV's three-dimensional absolute position in the geodetic coordinate system using the principle of spherical intersection. This geodetic absolute position is used as the measurement value for an adaptive unscented Kalman filter. The typical data update rate for LBS positioning is 1 Hz, with a single positioning accuracy within 0.1 m. There is an inherent delay of approximately 1 second between transmitting the interrogation signal and calculating the geodetic absolute position. This inherent delay arises from the propagation time of the acoustic signal in the water, the transponder response delay, and the signal processing time.

[0054] When the long baseline positioning terminal obtains a new absolute geodetic position measurement, the adaptive unscented Kalman filter does not directly update the current state estimate with this absolute geodetic position. Instead, it uses a delayed measurement update technique. If the current state estimate were updated directly with the received absolute geodetic position, a temporal mismatch would occur between the measured value and the predicted state, since the measured value actually corresponds to the position of the remotely operated underwater vehicle approximately 1 second ago, rather than the current position.

[0055] The delayed measurement update technique can be implemented as follows: After each state prediction, the adaptive unscented Kalman filter stores the predicted state value, error covariance matrix, and corresponding timestamp in a buffer. The buffer is organized in a first-in-first-out queue, storing a historical state sequence whose length covers the inherent delay of long-baseline positioning. When the long-baseline positioning terminal calculates the absolute geodetic position measurement, this measurement value carries a timestamp corresponding to the time of the interrogation signal transmission. The adaptive unscented Kalman filter retrieves the historical state prediction matching the timestamp of the measurement value from the buffer, establishing a correspondence between the measurement value and the historical state. The filter uses the Sigma point propagation method of the UT transform to calculate the predicted measurement value corresponding to the historical state. The actual measurement value is compared with the predicted measurement value to obtain the measurement residual. Based on the measurement residual and the Kalman gain, the historical state is corrected to obtain the posterior state estimate for that historical moment. Subsequently, the filter uses the state transition function to propagate the posterior state estimate of the historical moment forward along the time axis, successively passing through each subsequent state prediction moment in the buffer, and passing the correction effect to the current moment, completing the update of the entire buffer state sequence, and finally outputting the fused pose of the current moment after time registration and measurement correction.

[0056] The adaptive unscented Kalman filter calculates a residual sequence after each measurement update, where the residual is the difference between the actual and predicted measurement values. The filter performs sliding window statistics on the residual sequence, calculating the mean and covariance of the residuals. Based on the difference between the residual statistical characteristics and the theoretical expected values, it adjusts the measurement noise covariance matrix and the process noise covariance matrix online. When the residual statistical characteristics indicate that the actual measurement noise is less than the preset noise model, the filter decreases the value of the measurement noise covariance matrix to enhance the confidence in the measurement values ​​and accelerate the convergence speed of state estimation. When the residual statistical characteristics indicate that the actual measurement noise is greater than the preset noise model, the filter increases the value of the process noise covariance matrix to enhance the confidence in the state prediction and suppress the interference of measurement anomalies on state estimation. Through this online adaptive adjustment mechanism, the adaptive unscented Kalman filter can adjust the filtering parameters according to the noise level changes of the sensor in the actual underwater environment, enabling the output frequency of the fused pose to reach 100Hz and maintaining the positioning accuracy within 0.1m.

[0057] In one or more embodiments of the present invention, the extended state observer adopts a third-order extended state observer structure, with the fused pose as input, and the disturbance estimates output by the extended state observer ensemble characterize the changes in ocean current force, borehole impact reaction force, umbilical cable drag force, and unmodeled dynamic characteristics.

[0058] The extended state observer adopts a third-order extended state observer structure, using the fused pose output from an adaptive unscented Kalman filter as the input signal. The fused pose combines the absolute accuracy of long baseline positioning with the high-frequency response characteristics of strapdown inertial navigation systems, with an output frequency of 100Hz and a positioning accuracy within 0.1m, providing continuous, stable, and drift-free pose feedback for the extended state observer.

[0059] The extended state observer describes the motion of the remotely operated underwater vehicle (ROV) in a single degree of freedom as a third-order system, with its three state variables corresponding to position estimation, velocity estimation, and total disturbance estimation, respectively. The state equation of the extended state observer is as follows:

[0060]

[0061]

[0062]

[0063]

[0064] Where y is the input fused pose, z1 is the state estimate of the ROV's position by the extended state observer, z2 is the state estimate of the ROV's velocity by the extended state observer, z3 is the state estimate of the total disturbance by the extended state observer, e is the observation error between the position estimate and the fused pose, ż1 is the first derivative of z1 with respect to time, ż2 is the first derivative of z2 with respect to time, ż3 is the first derivative of z3 with respect to time, u is the control force output by the sliding mode controller, b0 is the gain coefficient of the control force with respect to acceleration, β1, β2, and β3 are the three gain parameters of the extended state observer, fal(e, α, δ) is a nonlinear function, α1 and α2 are the exponential parameters of the curvature of the control gain change in the nonlinear function, and δ is the transition threshold between the linear and nonlinear intervals. The above state equations construct an independent extended state observer for each degree of freedom of the ROV, i.e., six sets of parameter-independent third-order extended state observers are set for the six degrees of freedom motion.

[0065] The nonlinear function fal(e,α,δ) is defined as follows: when the absolute value of the observation error e is greater than the transition threshold δ, fal(e,α,δ) = |e|^α·sign(e); when the absolute value of the observation error e is less than or equal to the transition threshold δ, fal(e,α,δ) = e / δ^(1-α). This nonlinear function provides a relatively small feedback gain when the observation error is large, avoiding high-frequency oscillations in the extended state observer output; and provides a relatively large feedback gain when the observation error is small, accelerating the tracking speed of the extended state observer for state changes and disturbance changes. The parameter α takes a value between 0 and 1, and the parameter δ is set according to the sensor noise level and the system discrete sampling period.

[0066] The three gain parameters β1, β2, and β3 of the extended state observer are tuned using the pole placement method based on the observer bandwidth. All three poles of the extended state observer are placed at the observer bandwidth ω0, meaning the characteristic polynomial of the extended state observer is (s+ω0). 3 The triple root form is used to determine the relationship between the gain parameter and the observer bandwidth: β1=3ω0, β2=3ω0 2 β3=ω0 3 The observer bandwidth ω0 is selected based on the system control bandwidth, generally taking 3 to 5 times the control bandwidth, so that the dynamic response speed of the extended state observer is faster than the response speed of the sliding mode controller, thereby ensuring that the disturbance estimate can be reflected in a timely manner after the external disturbance changes, providing real-time and effective disturbance information for the feedforward compensation of the sliding mode controller.

[0067] The advantage of using fused pose as input for extended state observers is that if traditional extended state observers directly use the raw output of strapdown inertial navigation systems as input, due to the inherent sensor drift and accumulated errors in strapdown inertial navigation systems, as the operation time increases, the drift component contained in its position output will be misjudged as external environmental disturbances by the extended state observer, resulting in deviations in the disturbance estimation value. Consequently, the feedforward compensation of the sliding mode controller introduces an erroneous compensation force opposite to the direction of the actual disturbance, which in turn exacerbates the pose instability of the control system.

[0068] In this system, the fused pose output by the adaptive unscented Kalman filter has eliminated the cumulative drift of the strapdown inertial navigation system through periodic correction of the long baseline absolute position. The input signal received by the extended state observer does not contain drift error, and its output disturbance estimate can more accurately reflect the real external disturbance and unmodeled dynamics.

[0069] The lumped disturbance estimate z3 output by the extended state observer characterizes the ocean current force, borehole impact reaction force, umbilical cable drag force variation and unmodeled dynamic characteristics applied to the remotely operated underwater vehicle.

[0070] Ocean current force refers to the hydrodynamic force exerted on the remotely operated vehicle (ROV) by the flow of seawater at the operating depth. Its magnitude and direction vary slowly over time with changes in the marine environment. Drilling impact reaction force is the periodic impact force generated when the drill bit of the drilling rig contacts and penetrates the steel plate of the wreck hull. Its frequency is related to the drill bit rotation speed, and its amplitude is related to the steel plate thickness and material hardness. Umbilical cable towing force variation refers to the time-varying towing force generated by the umbilical cable connecting the mother ship and the ROV due to the swaying of the ocean current and the heave of the mother ship, transmitted to the ROV through the umbilical cable connection point. Unmodeled dynamic characteristics include uncertain parameters of the ROV's hydrodynamic coefficients, the nonlinear mapping relationship between thruster thrust and commands, and changes in inertial tensor caused by changes in the configuration of the cooperative system—factors not accurately modeled in the dynamic model.

[0071] The aforementioned disturbances and uncertainties coexist and are coupled with each other in actual operations. Instead of decomposing and identifying them, the extended state observer treats them as a ensemble total disturbance state variable z3 for unified real-time estimation. Through a nonlinear feedback mechanism, it converges to the neighborhood of the true total disturbance within a finite time.

[0072] The total disturbance estimate z3 output by the extended state observer is fed into the sliding mode controller and superimposed on the control force command as a disturbance compensation term in a feedforward manner. This enables the control system to generate the corresponding compensation force before the external disturbance acts on the remotely operated unmanned underwater vehicle, thereby reducing the switching gain requirement of the sliding mode control and reducing the chattering amplitude of the control output.

[0073] In one or more embodiments of the present invention, the thruster dynamic compensation unit calculates the cavitation number in real time based on the thruster speed and ambient pressure. When the cavitation number is lower than a preset threshold, the thruster dynamic compensation unit generates a thrust attenuation compensation amount according to the pre-stored cavitation number-thrust attenuation characteristic curve. When the thrust command of the thruster is lower than a preset dead zone threshold, the thruster dynamic compensation unit injects a high-frequency flutter signal.

[0074] The thruster dynamic compensation unit receives the control force generated by the sliding mode controller and corrected by feedforward compensation of the disturbance estimate. It then performs nonlinear compensation on the corrected control force for cavitation attenuation characteristics and low-speed dead zone characteristics. The deep-sea, high-pressure environment has two effects on the thrust output characteristics of the thruster: in the high-speed range of the thruster, cavitation occurs when the local pressure on the propeller blade surface drops below the saturated vapor pressure of seawater, leading to a sharp decrease in thrust; in the low-speed range of the thruster, insufficient static friction and starting torque cause a dead zone, resulting in no response from the thruster under small thrust commands. The thruster dynamic compensation unit compensates for these two non-ideal characteristics separately.

[0075] The thruster dynamic compensation unit calculates the cavitation number in real time based on the thruster speed and ambient pressure. The formula for calculating the cavitation number is σ = (Penv -P v ) / (0.5ρn 2 D 2 ), where P env Environmental pressure, measured by a depth sensor mounted on the remotely operated underwater vehicle, increases with increasing operating water depth; P v ρ is the saturated vapor pressure of seawater at the current temperature; n is the density of seawater; D is the current rotational speed of the propeller; and D is the diameter of the propeller. The cavitation number characterizes the margin between the minimum pressure on the propeller blade surface and the saturated vapor pressure of seawater; a lower cavitation number indicates a greater likelihood of cavitation. The propeller dynamic compensation unit has a preset cavitation threshold σth. When the cavitation number calculated in real-time is lower than this preset threshold, the propeller dynamic compensation unit determines that the propeller has entered a cavitation state and thrust attenuation compensation is required.

[0076] The thruster dynamic compensation unit internally stores a cavitation number-thrust attenuation characteristic curve pre-calibrated through hydrodynamic experiments. The calibration process for this characteristic curve involves measuring the ratio of the actual output thrust to the theoretical open-water thrust under different combinations of environmental pressure and rotational speeds, using this ratio as the thrust attenuation coefficient η(σ), thus establishing the correspondence between the thrust attenuation coefficient and the cavitation number. During compensation, the thruster dynamic compensation unit performs linear interpolation on this characteristic curve based on the real-time calculated cavitation number σ to obtain the thrust attenuation coefficient η(σ) corresponding to the current operating condition. The thrust attenuation coefficient η(σ) ranges from 0 to 1; when no cavitation occurs in the thruster, η(σ) is close to 1, and gradually decreases as the degree of cavitation increases. The thruster dynamic compensation unit generates a thrust attenuation compensation amount based on the thrust attenuation coefficient, calculated as ΔT = T. cmd ·(1 / η(σ)-1), where T cmd This is the corrected thrust command value. The thrust attenuation compensation amount ΔT is superimposed on the original thrust command, so that the actual thrust output of the thruster is close to the thrust value before cavitation occurs, thereby compensating for the thrust attenuation caused by cavitation.

[0077] When the thruster operates in the low-speed range, the thruster dynamic compensation unit compensates for the low-speed dead zone characteristics. Insufficient static friction and starting torque in the low-speed range cause the thruster to not generate thrust output when the absolute value of the thrust command is lower than a preset dead zone threshold. The thruster dynamic compensation unit continuously performs dead zone detection on the corrected control force. When the absolute value of a thrust command for a certain thruster is lower than the preset dead zone threshold Td, the thrust command is not executed directly. Instead, a high-frequency, low-amplitude flutter signal is superimposed on the thrust command. The flutter signal takes the form of T... comp =T cmdThe equation is +A·sin(ωt), where A is the flutter amplitude, ω is the flutter angular frequency, and t is time. The flutter amplitude A is slightly larger than the dead zone threshold Td, allowing the thrust command after superimposing the flutter signal to cross the dead zone threshold within a short time, thus breaking the resistance of static friction to the thruster's start-up process. The flutter angular frequency ω is selected near the thruster's mechanical response cutoff frequency, higher than the thruster control closed-loop bandwidth but not exceeding the highest input frequency allowed by the thruster drive controller, so that the flutter signal can effectively trigger the thruster response without causing significant fluctuations in the thruster's macroscopic thrust output.

[0078] After the thruster dynamic compensation unit performs cavitation attenuation compensation and low-speed dead zone compensation on the corrected control force, it outputs the compensated control force. The compensated control force includes the desired force and moment components of six degrees of freedom: longitudinal force, lateral force, vertical force, roll moment, pitch moment, and yaw moment. This compensated control force enters the execution module, where the thrust distribution unit uses a construction-analytical method to distribute it to the thrusters of each vector arrangement.

[0079] In one or more embodiments of the present invention, when any thruster fails, the thrust distribution unit sets the upper limit of the thrust of the thruster to zero through the fault detection logic, and re-solves the thrust distribution scheme through the construction-analysis method. During the re-solution process, the weight of the degree of freedom corresponding to the axial thrust of the opening is set to the highest.

[0080] Thrusters operate continuously for extended periods in the high-pressure environment of the deep sea, and individual unit failures may occur due to factors such as motor winding overheating, driver communication interruption, mechanical jamming, or power supply anomalies. When any thruster fails, if the thrust distribution unit still sends thrust commands to the failed thruster according to the conventional calculation results of the thrust distribution matrix, that thruster will be unable to generate the desired thrust. This uncompensated thrust loss will directly cause the resultant force and resultant torque acting on the remotely operated underwater vehicle (ROV) to deviate from the output expectation of the control law, resulting in attitude instability. The thrust distribution unit is equipped with fault detection logic to monitor the operating status of each thruster in real time and reconstruct the thrust distribution scheme accordingly.

[0081] The fault detection logic comprehensively determines whether the thruster is in a faulty state based on multiple dimensions, including the status words periodically reported by the thruster drive controller, the readings of the motor winding temperature sensor, the deviation between the thruster speed feedback value and the command value, and the communication heartbeat signal. The thruster drive controller sends a data message containing the status word, current speed, winding temperature, and fault code to the thrust distribution unit at preset communication intervals. If the thrust distribution unit does not receive a valid data message from a thruster within several consecutive communication cycles, it determines that the thruster's communication is interrupted.

[0082] The thrust allocation unit compares the thruster speed feedback value with the expected speed corresponding to the current thrust command. If the speed deviation exceeds a preset deviation threshold for a period longer than a preset time window, the thruster is deemed to be experiencing a drive malfunction. The thrust allocation unit monitors the motor winding temperature sensor readings. If the winding temperature exceeds a preset temperature protection threshold, the thruster is deemed to have an over-temperature fault. Once the fault detection logic determines that a thruster has malfunctioned, the thrust allocation unit sets the thrust limit of that thruster to zero. That is, the maximum output thrust of that thruster is set to zero in the thrust constraint conditions corresponding to that thruster, and no further thrust commands are allocated to that thruster during subsequent thrust allocation calculations.

[0083] After setting the upper limit of the thrust of the faulty thruster to zero, the thrust distribution unit calls the construction-analytical method again to solve for the thrust distribution scheme.

[0084] The solution process of the construction-analytical method is divided into two branches: analytical solution for unsaturated conditions and construction solution for saturated conditions. When the thrust capacity of the remaining healthy thrusters cannot meet the full requirements of the six degrees of freedom control force due to thruster failure, the construction solution branch is automatically entered.

[0085] The thrust distribution unit first establishes a mapping relationship from the thrust of each thruster to the six-degree-of-freedom control force vector. The number of rows in the mapping matrix equals the number of control degrees of freedom (6), and the number of columns equals the number of currently healthy thrusters. Each element in the mapping matrix is ​​determined by the thruster's installation angle and installation position coordinates through geometric projection relationships. Specifically, for the i-th thruster, its thrust contribution coefficient to the longitudinal force is equal to the cosine of the angle between the thruster's thrust axis and the longitudinal axis of the remotely operated underwater vehicle (ROV) multiplied by the sign factor of the thrust direction; its contribution coefficient to the vertical force is equal to the cosine of the angle between the thrust axis and the vertical axis multiplied by the sign factor; and its contribution coefficient to the roll moment is equal to the projection component of the thrust vector of the thruster's thrust vector onto the ROV's center of buoyancy on the roll axis. The remaining degrees of freedom follow the same principle.

[0086] During the solution construction process, the thrust distribution unit introduces a weighting matrix, which is a diagonal matrix. The j-th element wj on the diagonal corresponds to the weighting coefficient of the j-th control degree of freedom in the thrust distribution optimization. The thrust distribution unit sets the weighting coefficient of the degree of freedom corresponding to the axial thrust of the opening to be greater than the weighting coefficients of other degrees of freedom.

[0087] Specifically, if the drilling operation direction is the longitudinal axis of the remotely operated vehicle (ROV), the weighting coefficient for the longitudinal force corresponding to the degree of freedom is set to the highest value, and the weighting coefficients for the lateral force, vertical force, roll moment, pitch moment, and yaw moment corresponding to the degrees of freedom decrease sequentially. The specific values ​​of the weighting coefficients are preset according to the drilling operation process requirements. The ratio of the weighting coefficient for the axial thrust of the drilling to the weighting coefficients of other degrees of freedom is not less than a preset weighting ratio threshold. This ratio threshold is determined through a trade-off analysis between the drilling thrust requirement and the attitude maintenance accuracy requirement.

[0088] The thrust allocation unit constructs an optimization objective function using weighted least squares. The optimization objective is to minimize the sum of the quadratic forms of the thrust commands of each thruster and the weighted quadratic forms of the control force allocation residuals for each degree of freedom. Constraints include that the thrust commands of each thruster do not exceed the maximum output thrust of a healthy thruster, and the rate of change of the thrust commands does not exceed the dynamic response rate limit of the thruster. The thrust allocation unit uses a sequential quadratic programming algorithm to solve the constrained optimization problem to obtain the thrust commands of each healthy thruster.

[0089] After the thrust limit of the faulty thruster is set to zero and the weighted re-solution is performed using the construction-analytical method, the thrust distribution scheme output by the thrust distribution unit prioritizes maintaining the distribution requirement of the axial thrust of the hole opening under the condition of limited thruster capacity. It allows the control accuracy of secondary degrees of freedom such as roll moment and yaw moment to be reduced within a certain range, thereby ensuring that the remotely operated unmanned submersible can still maintain the core attitude stability required for hole opening operation under the condition of single thruster failure, and the hole opening operation is not interrupted.

[0090] A disturbance-resistant control method for ROV aperture pose based on long baseline integrated navigation includes:

[0091] The relative pose data of the remotely operated vehicle (ROV) relative to the initial navigation alignment point is obtained by a strapdown inertial navigation system and a Doppler velocimeter. The absolute ground position of the ROV is obtained by a long baseline positioning array. An adaptive unscented Kalman filter is used to fuse the relative pose data and the absolute ground position to obtain the fused pose.

[0092] The fused pose input is used to expand the state observer to obtain perturbation estimates for external disturbances and unmodeled dynamics.

[0093] Based on the deviation between the desired pose and the fused pose, a nominal control force is generated by the sliding mode controller. The nominal control force is then corrected by feedforward compensation using the disturbance estimate. Finally, the dynamic compensation unit of the thruster performs nonlinear compensation on the corrected control force for cavitation attenuation characteristics and low-speed dead zone characteristics to obtain the compensated control force.

[0094] The compensated control force is distributed to at least six vector-arranged thrusters and executed using a constructive-analytical method.

[0095] At preset time intervals, the absolute geodetic position of the remotely operated vehicle is obtained through a long baseline positioning array, and the absolute geodetic position is input into an adaptive unscented Kalman filter to correct the cumulative drift of the strapdown inertial navigation system.

[0096] In the specific implementation process, after the remotely operated vehicle (ROV) descends to the operating depth, it first performs integrated navigation initialization. The strapdown inertial navigation system is activated for initial alignment, which lasts for at least 15 minutes, establishing the initial alignment point as a reference benchmark for relative pose data.

[0097] After the initial alignment is completed, the gyroscope and accelerometer inside the strapdown inertial navigation system continuously output angular rate and specific force data. Through mechanical arrangement algorithm, the system integrates over time and recursively calculates the changes in position, velocity, and attitude of the remotely operated vehicle relative to the initial alignment point.

[0098] The Doppler velocimeter emits acoustic pulses toward the seabed and receives bottom-tracking echoes. It uses the Doppler frequency shift principle to measure the velocity vector of the remotely operated vehicle (ROV) relative to the seabed. The output data of the strapdown inertial navigation system and the Doppler velocimeter are partially fused through a combined navigation filter to output the relative pose data of the ROV relative to the initial navigation alignment point.

[0099] The long-baseline positioning terminal transmits interrogation signals to at least three transponders deployed on the seabed around the shipwreck according to a preset interrogation cycle. Each transponder receives the interrogation signal and returns a response signal after a fixed delay. The long-baseline positioning terminal measures the round-trip propagation time of the interrogation signal from transmission to reception of each transponder's response signal. Combining this with underwater sound velocity profile data, the slant distance between the remotely operated vehicle (ROV) and each transponder is calculated. After obtaining the slant distance measurements from at least three transponders, the spherical intersection principle is used to calculate the ROV's three-dimensional absolute position in the geodetic coordinate system. An adaptive unscented Kalman filter is then used to fuse the relative pose data and the geodetic absolute position to output the fused pose.

[0100] The pose input is integrated into the extended state observer. For each degree of freedom of motion of the remotely operated vehicle (ROV), an independent third-order extended state observer is constructed to perform real-time lumped observations of external disturbances and unmodeled dynamics, outputting disturbance estimates. The disturbance estimates output by the extended state observer lumpedly represent the changes in ocean current forces, borehole impact reactions, umbilical cable drag forces, and unmodeled dynamic characteristics.

[0101] Based on the deviation between the desired pose and the fused pose, a nominal control force is generated using a sliding mode controller. The desired pose is the pre-set position and attitude of the opening based on the coordinates and orientation of the opening on the wreck hull. The control law of the sliding mode controller consists of three parts: an equivalent control term, a switching control term, and a disturbance compensation term. The equivalent control term is calculated based on the nominal dynamic model of the remotely operated vehicle (ROV); the switching control term uses a combined approach law, with the approach rate adaptively adjusted according to the magnitude of the current pose deviation; the disturbance compensation term uses the disturbance estimate output by the extended state observer and is superimposed onto the control force command in a feedforward manner. After feedforward compensation correction of the nominal control force using the disturbance estimate, the corrected control force is obtained.

[0102] The corrected control force is fed into the thruster dynamic compensation unit. This unit calculates the cavitation number in real time based on the thruster speed and ambient pressure. When the cavitation number is below a preset cavitation threshold, it performs linear interpolation based on a pre-stored cavitation number-thrust attenuation characteristic curve to obtain the thrust attenuation coefficient, generating a thrust attenuation compensation amount which is then superimposed on the original thrust command. When the absolute value of the thrust command is below a preset dead zone threshold, a high-frequency flutter signal is superimposed on the thrust command. This flutter signal causes the thruster to maintain a small-amplitude reciprocating motion during the reception of small thrust commands, eliminating the obstruction of static friction on the thrust build-up process. After completing the nonlinear compensation for the cavitation attenuation characteristics and low-speed dead zone characteristics, the thruster dynamic compensation unit obtains the compensated control force.

[0103] A constructive-analytical method is used to distribute the compensated control force to at least six vector-arranged thrusters and execute it. The thrust distribution unit detects whether each thruster is in a saturated state. When all thrusters are not saturated, an analytical method is used to establish a mapping matrix from the thrust of each thruster to the 6-DOF control force vector and solve for the thrust command. When it is detected that the thrust command of a thruster exceeds its maximum output thrust, the constructive method is switched to solve again. The weighting coefficients of the axial thrust and anti-overturning moment corresponding to the degrees of freedom in the allocation weight matrix are set to be greater than those of other degrees of freedom. The objective function is constructed using weighted least squares and solved using a sequential quadratic programming algorithm. After receiving the thrust command, each thruster drives the propeller to rotate and generate thrust, so that the cooperative body consisting of the remotely operated underwater vehicle and the liquid pumping and opening mechanism maintains attitude stability during the opening process.

[0104] During the drilling operation, the absolute geodetic position of the remotely operated vehicle (ROV) is acquired at preset time intervals using a long-baseline positioning array. This absolute geodetic position is then input into an adaptive unscented Kalman filter to correct the accumulated drift of the strapdown inertial navigation system (SINS). The preset time interval is adjusted based on the gyro drift rate of the SINS and the current operating water depth, with a correction interval of no less than 15 seconds. This interval can be appropriately extended as the gyro drift rate decreases or the operating water depth decreases.

[0105] In one or more embodiments of the present invention, when the bottom tracking signal of the Doppler velocimeter is lost or the output data of the Doppler velocimeter is abnormal, the adaptive unscented Kalman filter identifies the failure state through residual monitoring and switches to a combined mode in which the state prediction is performed recursively by the strapdown inertial navigation system and the measurement update is provided by the absolute position of the ground.

[0106] During drilling operations, the Doppler velocimeter emits acoustic pulses towards the seabed and measures the velocity vector of the remotely operated vehicle (ROV) relative to the seabed using bottom-tracking echoes. When the turbidity of the operating water is high, or the seabed consists of soft mud or a thick layer of suspended sediment, the acoustic pulses are scattered or absorbed by suspended particles in the water before reaching the seabed. This causes the bottom-tracking echo signal strength to attenuate below the sensitivity of the Doppler velocimeter receiver, resulting in the loss of the Doppler velocimeter's bottom-tracking signal. When the ROV navigates to areas with drastically undulating seabed topography or areas where strong and weak acoustic reflective layers alternate, the Doppler velocimeter's bottom-tracking algorithm may lock onto the wrong reflective layer, causing abnormal jumps in the output velocity data. This manifests as velocity differences between adjacent measurement cycles exceeding the normal kinematic constraints.

[0107] The adaptive unscented Kalman filter continuously calculates the residual sequence after each measurement update. The residual is the difference between the actual and predicted measurement values. The filter performs sliding window statistics on the residual sequence, with the sliding window length covering several measurement update cycles, calculating the mean and covariance matrix of the residual sequence within the window. The filter presets a residual statistical threshold range, which is obtained offline based on the statistical characteristics of the residual sequence under normal operating conditions of the Doppler velocimeter. This threshold range includes an upper limit threshold for the residual mean and an upper limit threshold for the trace of the residual covariance matrix. When the mean of the residual sequence within the sliding window exceeds the upper limit threshold, or the trace of the residual covariance matrix exceeds the upper limit threshold, the adaptive unscented Kalman filter determines that the statistical characteristics of the current measurement data have deviated from the normal model, identifying the Doppler velocimeter as malfunctioning.

[0108] After the adaptive unscented Kalman filter detects a Doppler velocimeter failure, it automatically switches to a pure inertial navigation system with extended baseline combination mode. In this combination mode, the adaptive unscented Kalman filter stops incorporating Doppler velocimeter velocity data into the state prediction process. The state prediction stage relies solely on the angular rate and specific force data output by the strapdown inertial navigation system for recursive calculations. The gyroscope inside the strapdown inertial navigation system outputs the three-axis angular rates of the carrier coordinate system relative to the inertial coordinate system, and the accelerometer outputs the three-axis specific force in the carrier coordinate system. The angular rate is integrated using an attitude quaternion update algorithm to obtain the attitude quaternion at the current moment. The specific force in the carrier coordinate system is then converted to the navigation coordinate system using the attitude quaternion. After subtracting the gravitational acceleration component, the linear acceleration in the navigation coordinate system is obtained. The linear acceleration is integrated once to obtain the velocity increment, and the velocity is integrated again to obtain the position increment. Thus, the current position, velocity, and attitude of the remotely operated underwater vehicle are recursively obtained.

[0109] Because of the lack of absolute velocity observations provided by a Doppler velocimeter to dampen the velocity error divergence of the strapdown inertial navigation system, the position and velocity errors derived purely by inertial recursion accumulate over time, and the accumulation rate is determined by the zero-bias instability of the gyroscope and the zero-bias stability of the accelerometer.

[0110] To suppress error accumulation from pure inertial recursion, the adaptive unscented Kalman filter continues to receive geodetic absolute position measurements from the long-baseline positioning terminal at preset time intervals in the pure inertial navigation plus long-baseline combination mode. The long-baseline positioning terminal performs an interrogation and slant range measurement process every preset time interval to calculate the current geodetic absolute position of the remotely operated vehicle (ROV), which is marked with a timestamp corresponding to the interrogation signal transmission time. The adaptive unscented Kalman filter employs a delayed measurement update technique, retrieving historical state predictions matching the timestamp of the measurement values ​​from the buffer, establishing a correspondence between the measurement values ​​and historical states, calculating the measurement residuals and Kalman gain, correcting the states at historical moments, and propagating the correction results forward to the current moment through the state transition matrix.

[0111] Since the correction time interval of long baseline positioning does not exceed the preset upper limit, the pure inertial recursive cumulative position error corresponding to this time interval is less than the maximum position deviation allowed by the hole opening operation. After each long baseline correction, the fused pose is restored to the accuracy level of long baseline positioning, and the extended state observer and sliding mode controller can continue to work on the basis of accurate pose feedback.

[0112] When the Doppler velocimeter's bottom tracking signal is recovered and the adaptive unscented Kalman filter detects that the statistical characteristics of the residual sequence have returned to within the preset threshold range for several consecutive sliding windows, the filter determines that the Doppler velocimeter has returned to normal working status, automatically switches back to the normal strapdown inertial navigation and Doppler velocimeter combined navigation mode, and reintroduces the speed data of the Doppler velocimeter into the state prediction process.

[0113] In one or more embodiments of the present invention, the control law of the sliding mode controller includes an equivalent control term, a switching control term, and a disturbance compensation term. The switching control term adopts a combined approach law, and the approach rate of the combined approach law is positively correlated with the deviation between the desired pose and the fused pose.

[0114] The control law of a sliding mode controller consists of three superimposed parts: an equivalent control term, a switching control term, and a disturbance compensation term. Its mathematical expression is u=u eq +u sw +u comp , where u eq As an equivalent control term, u sw To switch control items, u comp This is a disturbance compensation term.

[0115] The equivalent control term is calculated based on the nominal dynamic model of the remotely operated vehicle (ROV). The nominal dynamic model describes the mapping relationship between the rigid body motion of the ROV in the underwater environment and the thrust of the propeller and hydrodynamic forces, including the inertia matrix, Coriolis centripetal force matrix, hydrodynamic damping matrix, and restoring force vector. The equivalent control term is calculated as follows: under the constraint that the derivative of the sliding surface s is zero, the matrix parameters of the nominal dynamic model are substituted to solve for the reference control force required to maintain the system state sliding along the sliding surface. The equivalent control term guarantees that, under ideal conditions with no external disturbances and accurate model parameters, the system state converges to the equilibrium point along the sliding surface once it reaches the sliding surface.

[0116] The switching control term employs a combined reaching law, whose reaching rate is positively correlated with the deviation between the desired pose and the fused pose. The sliding surface s is composed of a linear combination of pose deviations and their rates of change, i.e., s = ė + λ·e, where e is the deviation between the desired pose and the fused pose, ė is the rate of change of the deviation, and λ is a diagonal positive definite matrix. The diagonal elements of λ are the design parameters of the sliding surface for each degree of freedom. By adjusting λ, the time constant for the convergence of the system state along the sliding surface can be configured.

[0117] When the pose deviation e is large, the system state is far from the sliding surface. The combined approaching law generates a large approaching rate to drive the system state to quickly approach the sliding surface, shortening the time required for the system state to reach the sliding surface. When the pose deviation e is small, the system state is close to or within the neighborhood of the sliding surface. The combined approaching law automatically reduces the approaching rate to reduce the chattering amplitude of the control output and avoid high-frequency oscillation of the control signal caused by excessive switching gain.

[0118] The positive correlation between the approach rate and the deviation of the combined approach law is realized by introducing a gain scheduling function with the norm of the sliding surface s as the independent variable into the approach law. This gain scheduling function takes a larger value when the norm of s is large and a smaller value when the norm of s is small, so that the magnitude of the switching control term decreases smoothly as the distance between the system state and the sliding surface decreases.

[0119] The disturbance compensation term directly references the total disturbance estimate z3 output by the extended state observer and superimposes it into the control force command in a feedforward manner. In the sliding mode control framework, the gain of the switching control term needs to be greater than the upper bound of the external disturbance and the unmodeled dynamic lumped sum to ensure the reachability and existence of the sliding mode motion. The larger the upper bound of the disturbance, the greater the required switching gain, and a large switching gain is the main source of chattering. The disturbance compensation term introduces the total disturbance estimated in real time by the extended state observer into the control law in a feedforward form, so that the control law only needs the switching control term to overcome the disturbance estimation error and the unmodeled residual, rather than overcoming all the disturbance. The more accurate the disturbance estimate, the smaller the residual that the switching control term needs to overcome, and the smaller the switching gain can be set, thereby reducing the chattering amplitude.

[0120] After the superposition of the above three control terms, the sliding mode controller generates a nominal control force driven by the deviation between the desired pose and the fused pose. After feedforward compensation correction using the disturbance estimate, the corrected control force is output.

[0121] In one or more embodiments of the present invention, the construction-analytical method establishes and solves the mapping relationship between the thrust and control force vectors of each thruster when the thruster is not saturated, and switches to the construction method when the thruster is saturated, setting the weights of the degrees of freedom corresponding to the axial thrust of the opening and the anti-overturning moment to the highest.

[0122] After receiving the compensated control force, the thrust distribution unit invokes the constructive-analytical method to solve for the thrust commands of each thruster. The execution of the constructive-analytical method is divided into two branches: unsaturated operating condition and saturated operating condition. The thrust distribution unit compares the thrust commands of each thruster obtained by the analytical method with the maximum output thrust of that thruster under the current operating condition. When all thrust commands do not exceed the maximum output thrust of the corresponding thruster, it is determined to be an unsaturated operating condition. When the thrust command of any thruster exceeds its maximum output thrust, it is determined to be a saturated operating condition.

[0123] The maximum output thrust of the propeller under current operating conditions is not a fixed value, but a function of the operating water depth and propeller speed. As the operating water depth increases and the environmental pressure increases, the open-water efficiency curve of the propeller propeller shifts downward overall, and the maximum output thrust at the same speed is lower than in shallow water conditions. The thrust distribution unit uses the current water depth measured in real time by the depth sensor to look up the current maximum output thrust value of each propeller in a pre-stored table of propeller thrust characteristic data corresponding to different water depths, which serves as the upper limit benchmark for saturation determination.

[0124] Under unsaturated operating conditions, the thrust distribution unit is solved analytically. The analytical method treats the mapping relationship between the 6-DOF desired control force vector and the thrust of each thruster as a system of linear equations. The objective function is the sum of the quadratic forms of the thrust commands of each thruster. Under the condition that the mapping relationship serves as an equality constraint, the analytical solution is directly obtained using the Lagrange multiplier method. Each element in the mapping matrix corresponds to the projection coefficients of the unit vector of the thrust axis direction and the position vector of each thruster in the 6 control degrees of freedom directions. Specifically, for the i-th thruster, its thrust axis direction unit vector is determined by its installation deflection angle and installation tilt angle: the installation deflection angle is the angle between the projection of the thrust axis onto the horizontal plane of the remotely operated vehicle (ROV) and the longitudinal axis; the installation tilt angle is the angle between the thrust axis and the horizontal plane. Both the deflection angle and the tilt angle are based on the dimensions of the thruster mounting base design drawings. The position vector is the vector pointing from the ROV's center of buoyancy to the thrust application point of the thruster. The coordinates of the center of buoyancy are calculated by 3D modeling software based on the volume and mass distribution of the various components of the ROV. The thrust distribution unit substitutes the thrust axis direction unit vector and position vector into a pre-established offline mapping matrix expression, which is then directly called during each solution.

[0125] Under saturation conditions, the thrust allocation unit switches to a constructive method. The constructive method sets the weighting coefficients in the weighting matrix corresponding to the top thrust degree of freedom and the anti-overturning moment degree of freedom along the opening axis to the highest value. The diagonal elements of the weighting matrix are preset to a fixed proportional relationship according to the opening operation process requirements: if the opening axis is the longitudinal direction of the remotely operated vehicle (ROV), the weighting coefficient for the longitudinal force degree of freedom takes the preset highest value, the weighting coefficients for the roll and pitch moments degrees of freedom take the second highest value, and the weighting coefficients for the lateral, vertical, and bow moment degrees of freedom take lower values. During optimization, the thrust allocation unit uses the current thrust command as the initial iteration point. In each iteration, it constructs a quadratic programming subproblem with the weighting coefficients as penalty weights. Solving the subproblem yields the search direction. After determining the step size through line search, the thrust command is updated. This process is repeated until the change in thrust command between two adjacent iterations is less than a preset convergence threshold. When the iterative solution reaches the preset maximum number of iterations, the iteration terminates even if the convergence condition is not met, and the current optimal feasible solution is output as the thrust allocation result to each thruster.

[0126] In one or more embodiments of the present invention, the preset time interval is not less than 15 seconds, and the preset time interval increases by one step value for each preset level decrease in the gyroscope drift rate of the strapdown inertial navigation system, and the preset time interval increases by one step value for each preset level decrease in the operating water depth of the remotely operated unmanned underwater vehicle.

[0127] During the borehole drilling operation, the adaptive unscented Kalman filter uses the absolute geodetic position calculated by the long baseline positioning terminal as the measurement value to correct the drift error accumulated by the strapdown inertial navigation system. The preset time interval is no less than 15 seconds. When the gyroscope drift rate of the strapdown inertial navigation system is greater than or equal to the preset upper limit of drift rate and the operating water depth is greater than or equal to the preset upper limit of water depth, the preset time interval is set to 15 seconds. The preset upper limit of drift rate is the maximum drift rate allowed by the nominal gyroscope zero-bias stability index of the strapdown inertial navigation system, and the preset upper limit of water depth is the maximum operating water depth specified in the borehole drilling task profile. Both are preset during the system initialization phase based on sensor parameters and task parameters.

[0128] The dynamic adjustment of the preset time interval is based on two variables: the gyro drift rate of the strapdown inertial navigation system and the operating water depth of the remotely operated underwater vehicle (ROV). The gyro drift rate is divided into several preset levels, each corresponding to a drift rate range. These ranges are non-overlapping and arranged in order of drift rate magnitude, with equal differences between the drift rate thresholds of adjacent levels. The gyro drift rate levels are defined as follows: the upper limit of the preset drift rate is used as the upper bound of the highest level, and the drift rate ranges for each level are defined in descending order of drift rate with equal increments. Similarly, the operating water depth is divided into several preset levels, each corresponding to a water depth range. These ranges are non-overlapping and arranged in order of water depth magnitude, with equal differences between the water depth thresholds of adjacent levels. The operating water depth levels are defined as follows: the upper limit of the preset water depth is used as the upper bound of the highest level, and the water depth ranges for each level are defined in descending order of water depth with equal increments.

[0129] The adjustment rule for the preset time interval can be as follows: take 15s, which corresponds to the highest level of gyroscope drift rate and the highest level of operating water depth, as the baseline value. For each preset level decrease in gyroscope drift rate, the preset time interval increases by one step value; for each preset level decrease in operating water depth, the preset time interval increases by one step value.

[0130] The step size can be determined as follows: Under static base conditions, measure the time it takes for the pure inertial recursive position error of the strapdown inertial navigation system to reach the maximum allowable position deviation for hole opening operations at drift rates corresponding to two adjacent gyroscope drift rate levels. Use the difference between these two time durations as the step size between adjacent levels. Similarly, measure the difference in error propagation characteristics of the strapdown inertial navigation system under environmental pressure corresponding to two adjacent depth levels within a pressure chamber. Use the difference in allowable recursive time at adjacent depth levels as a reference value for the step size between depth levels. The step size is determined and stored in the control computer during the system parameter calibration phase and can be directly retrieved during online operation.

[0131] When both the gyro drift rate and the operating water depth decrease simultaneously, their step values ​​are accumulated and included in the preset time interval, with an upper limit of 60 seconds. When the gyro drift rate or operating water depth changes across levels, the changed gyro drift rate level or operating water depth level is read by the thrust distribution unit before the start of the next correction cycle. The preset time interval is recalculated based on the level, and long baseline positioning correction is performed within the correction cycle using the updated preset time interval.

[0132] This invention fuses the absolute geodetic position obtained by a long-baseline positioning array with the relative pose data obtained by a strapdown inertial navigation system and a Doppler velocimeter using an adaptive unscented Kalman filter. The resulting fused pose outputs a fused pose with centimeter-level accuracy and 100Hz-level frequency, solving the technical problem that a single navigation system cannot simultaneously meet the high-precision and high-frequency pose measurement requirements of deep-sea drilling operations. The extended state observer uses the fused pose as input instead of directly using the original inertial navigation data containing accumulated drift. This ensures that external disturbances and unmodeled dynamic lumped observations are not affected by the drift error of the strapdown inertial navigation system, thus providing accurate disturbance feedforward compensation for the sliding mode controller and preventing misjudgment of positioning drift as external disturbance, leading to incorrect compensation. The thruster dynamic compensation unit performs nonlinear compensation for cavitation attenuation characteristics and low-speed dead zone characteristics, ensuring that control force commands can still be effectively executed by the thrusters under deep-sea and high-pressure conditions. Combined with the constructivist-analytical method, the compensated control force is distributed to multiple vector-arranged thrusters, guaranteeing the pose stability of the remotely operated vehicle (ROV) during the drilling process.

[0133] The above is the overall concept of the present invention. For ease of understanding, the present invention also provides the following embodiments:

[0134] This embodiment covers water depths from 300m to 6000m and is suitable for positional stability control during the process of remotely operated unmanned underwater vehicles carrying a liquid extraction and drilling integrated machine to drill holes in the hull of sunken ships.

[0135] System Configuration

[0136] In this embodiment, the long-baseline positioning array consists of a redundant configuration of four seabed transponders. The transponders operate at frequencies ranging from 8kHz to 16kHz, achieving a positioning accuracy better than 0.05m at a water depth of 6000m. Each transponder's built-in battery pack allows for continuous operation for over 72 hours. The long-baseline positioning terminal onboard the remotely operated vehicle (ROV) is compatible with both long-baseline and ultra-short-baseline modes. The positioning update rate is 1Hz in long-baseline mode and 10Hz in ultra-short-baseline mode. The strapdown inertial navigation system uses fiber optic gyroscopes with a drift not exceeding 0.01° / h and an accelerometer bias not exceeding 1mg, achieving a data update rate of 200Hz. The Doppler velocimeter is a phased-array Doppler velocimeter with a maximum bottom tracking depth of 6000m, a velocity measurement accuracy of 0.5% of the velocity plus or minus 1mm / s, and a data update rate of 10Hz. The propulsion system consists of eight vector-arranged thrusters, four horizontally and four vertically. Each thruster has a maximum thrust of no less than 200 kgf at a water depth of 6000 m, and is powered by 3000 V DC. The control computer is an embedded industrial PC with an Intel Core i7 CPU, 16 GB of memory, and supports CAN bus, RS485, and Ethernet communication interfaces.

[0137] Transponder deployment

[0138] After the mother ship arrives at the operational area, it first uses multibeam sonar to precisely locate and 3D model the sunken ship, obtaining its specific position, attitude, and surrounding seabed topography data. Based on the size of the sunken ship and the predetermined drilling range, four deployment points are selected within a radius of 100m to 200m around the sunken ship. The deployment points are selected according to the principle of optimal geometric accuracy attenuation factor to ensure that the transponder array can completely cover the operational area and that the transponders are not collinear or nearly collinear. The transponders are then deployed one by one to the seabed using a deep-water deployment device. The precise coordinates of each transponder are measured using the ultra-short baseline positioning system on the mother ship, establishing the absolute coordinate system of the long baseline positioning array.

[0139] Integrated navigation initialization

[0140] After the remotely operated underwater vehicle (ROV) descends to the vicinity of the shipwreck, it activates the strapdown inertial navigation system for initial alignment, which takes no less than 15 minutes. Once the initial alignment is complete, the Doppler velocimeter begins outputting bottom tracking velocity data, and the long baseline positioning terminal begins receiving transponder signals and calculating the ROV's absolute geodetic position.

[0141] The initialization parameters of the adaptive unscented Kalman filter are set as follows: the state vector is 15-dimensional, including the 3D position, 3D velocity, and 3D attitude angles of the remotely operated vehicle (ROV) in the geographic coordinate system, as well as the 3-axis gyroscope drift and 3-axis accelerometer zero bias of the strapdown inertial navigation system. The UT transform parameters are set as follows: the primary scaling factor is 0.5, the secondary scaling factor is 2, and the center point weight adjustment factor is 0. The noise covariance matrix of the adaptive unscented Kalman filter is initially set based on the zero-bias stability index of the fiber optic gyroscope, the zero-bias index of the accelerometer, the velocity measurement accuracy index of the Doppler velocimeter, and empirical values. During actual operation, the adaptive unscented Kalman filter adjusts the measurement noise covariance matrix and the process noise covariance matrix online through the measured updated residual sequence to ensure that the statistical model of the filter matches the noise characteristics of the actual sensors in the underwater environment.

[0142] Hole drilling operation control

[0143] The hole-making process is divided into four stages: approach stage, alignment stage, hole-making stage, and exit stage. The control strategies for each stage are configured according to the characteristics of each stage.

[0144] During the approach phase, the remotely operated vehicle (ROV) carrying the integrated pumping and drilling unit moves from its current position towards the point above the drilling site. Trajectory tracking is the primary control objective during this phase, and the sliding mode controller employs a large approach law gain to achieve rapid tracking response to the target trajectory. Because the ROV's speed is relatively low during the approach phase, the thrusters typically do not enter a cavitation state, and the thruster dynamic compensation unit temporarily disables cavitation compensation during this phase.

[0145] During the alignment phase, the remotely operated underwater vehicle (ROV) decelerates and precisely adjusts its attitude to align the working surface of the integrated drilling machine with the opening point on the wreck hull, keeping the attitude deviation within ±10mm. In this phase, the control objective switches from trajectory tracking to fixed-point adjustment, and the sliding mode controller switches to a smaller control gain to avoid attitude overshoot. Thrust commands during alignment primarily involve minor corrections. The thruster's dynamic compensation unit activates low-speed dead-zone compensation, superimposing a high-frequency flutter signal onto the small thrust commands. This ensures the thruster maintains a micro-reciprocating motion during the reception of small thrust commands, eliminating the obstacle of static friction to thrust buildup and guaranteeing the effective execution of small thrust commands.

[0146] During the drilling phase, the drill bit contacts the hull of the sunken ship and begins penetration. The drilling impact reaction force, as a periodic external disturbance, is applied to the collaborative system consisting of the remotely operated vehicle (ROV) and the integrated pumping and drilling mechanism. The expansion state observer monitors this impact disturbance in real time by fusing high-frequency changes in attitude, and the output disturbance estimate is fed forward to compensate for the control force command of the sliding mode controller, achieving rapid suppression of the impact reaction force. Cavitation compensation is activated during this phase: the drilling operation requires the thruster to provide a stable axial thrust to maintain the contact pressure between the drill bit and the hull. At high speeds, the thruster may enter a cavitation state, leading to thrust attenuation. The cavitation identification submodule of the thruster dynamic compensation unit calculates the cavitation number in real time based on the current thruster speed and the ambient pressure obtained by the depth sensor. When the cavitation number is lower than the preset cavitation threshold, the cavitation compensation submodule performs linear interpolation according to the pre-stored cavitation number-thrust attenuation characteristic curve to obtain the thrust attenuation coefficient. The thrust command is amplified according to the reciprocal of the thrust attenuation coefficient, so that the actual thrust output of the thruster approaches the thrust value when cavitation has not occurred.

[0147] During the exit phase, the drilling operation is completed, and the drill bit is withdrawn from the wreck hull. At this stage, the control system gradually reduces the axial thrust, the sliding mode controller resumes trajectory tracking mode, and the remotely operated unmanned submersible carrying the drilling machine leaves the drilling point.

[0148] Long baseline periodic correction implementation

[0149] Throughout the entire drilling process, a long baseline periodic calibration is performed at preset time intervals, with each calibration interval being no less than 15 seconds. This interval can be appropriately extended based on the gyro drift rate of the strapdown inertial navigation system and the current operating water depth, but the maximum extension is no more than 60 seconds. The single calibration process is as follows: The long baseline positioning terminal transmits an interrogation signal to the seabed transponders. Each transponder receives the interrogation signal and returns a response signal after a fixed delay. The long baseline positioning terminal measures the round-trip propagation time and calculates the current absolute geodetic position of the remotely operated vehicle (ROV). The adaptive unscented Kalman filter uses the absolute geodetic position as the measurement value. Due to the inherent delay of approximately 1 second between transmitting the interrogation signal and calculating the absolute geodetic position, the adaptive unscented Kalman filter employs a delayed measurement update technique. It performs time registration between the measured value and the corresponding state prediction before performing a measurement update to correct the accumulated drift error of the strapdown inertial navigation system. After calibration, the fused pose is restored to the accuracy level of the long baseline positioning.

[0150] Special working condition handling

[0151] When the turbidity of the operating waters is high or the seabed sediment type causes the Doppler velocimeter to lose its bottom tracking signal, the Doppler velocimeter output data is interrupted or abnormal velocity data jumps occur. The adaptive unscented Kalman filter, through statistical monitoring of the residual sequence via a sliding window, automatically identifies the Doppler velocimeter failure state when the residual exceeds a preset threshold range, and switches to a pure inertial navigation system combined with a long baseline. In this combined mode, state prediction relies solely on gyroscope and accelerometer data from the strapdown inertial navigation system for recursive calculations, and measurement updates are still performed by the long baseline positioning terminal providing the absolute geodetic position at preset time intervals. Because the correction interval corresponding to the correction frequency of long baseline positioning is much smaller than the time scale of the strapdown inertial navigation system accumulating drift to unacceptable accuracy under uncorrected conditions, the system can maintain effective positioning at the minute level in this fault-tolerant mode, sufficient to support personnel in handling anomalies or for remotely operated vehicles to autonomously return to the repeater.

[0152] When a thruster experiences a fault such as motor winding overheating or driver communication interruption, the thrust distribution unit resets the thrust limit of that thruster to zero through fault detection logic and re-solves the thrust distribution scheme using a constructive-analytical method. During the re-solution process, the constructive method sets the weighting coefficient of the degree of freedom corresponding to the axial thrust of the opening in the distribution weight matrix to be greater than that of other degrees of freedom. Under thruster capacity-constrained conditions, priority is given to ensuring the distribution requirements of the axial thrust of the opening, allowing secondary degrees of freedom to reduce control accuracy within a certain range, ensuring uninterrupted opening operations.

[0153] Implementation effect

[0154] This embodiment was tested and verified in actual sea trials at a water depth of 1500m. Regarding positioning accuracy, the root mean square error of the fused attitude positioning under the traditional control method is ±0.35m, while the root mean square error under the method of this invention is ±0.06m. Regarding aperture attitude deviation, the aperture attitude deviation under the traditional control method is ±18mm, while the aperture attitude deviation under the method of this invention is ±4.2mm. Regarding impact disturbance response time, the response time under the traditional control method is 0.8s, while the response time under the method of this invention is 0.2s. Regarding thrust error under cavitation conditions, the thrust error under the traditional control method is as high as 40%, while the thrust error under the method of this invention is less than 8%. Regarding positioning maintenance time after Doppler velocimeter failure, the maintenance time under the traditional control method is less than 30s, while the maintenance time under the method of this invention exceeds 300s. Test data shows that the method of this invention is superior to the traditional control method in terms of positioning accuracy, attitude stability, anti-disturbance response speed, thruster adaptability, and fault tolerance.

[0155] The above provides a detailed description of the ROV aperture pose anti-disturbance control system and method based on long baseline integrated navigation. Specific examples are used to illustrate the principle and implementation of the invention. The above description of the embodiments is only for the purpose of helping to understand the method and core idea of ​​the invention. At the same time, for those skilled in the art, there will be changes in the specific implementation and application scope based on the idea of ​​the invention. Therefore, the content of this specification should not be construed as a limitation of the invention.

Claims

1. A disturbance rejection control system for ROV opening posture based on long baseline integrated navigation, characterized in that, The system includes: The long baseline positioning assistance module includes at least three transponders, a long baseline positioning terminal, a strapdown inertial navigation system, a Doppler velocimeter, and an adaptive unscented Kalman filter. The long baseline positioning terminal is used to receive transponder signals and calculate the absolute geodetic position of the remotely operated vehicle (ROV). The strapdown inertial navigation system and the Doppler velocimeter are used to acquire the relative pose data of the ROV relative to the initial navigation alignment point. The adaptive unscented Kalman filter is used to fuse the absolute geodetic position and the relative pose data and output the fused pose. The disturbance observation and control module includes an extended state observer, a sliding mode controller, and a thruster dynamic compensation unit. The extended state observer takes the fused pose as input and performs lumped observation of external disturbances and unmodeled dynamics, outputting a disturbance estimate. The sliding mode controller generates a nominal control force based on the deviation between the desired pose and the fused pose, and performs feedforward compensation correction using the disturbance estimate. The thruster dynamic compensation unit performs nonlinear compensation for the cavitation attenuation characteristics and low-speed dead zone characteristics of the corrected control force. The execution module includes at least six vector-arranged thrusters and a thrust distribution unit. The thrust distribution unit uses a constructive-analytical method to distribute the control force compensated by the thruster dynamic compensation unit to the at least six vector-arranged thrusters.

2. The system according to claim 1, characterized in that, The adaptive unscented Kalman filter performs Sigma point sampling on the state vector through UT transform. The state vector consists of the position and velocity of the remotely operated vehicle (ROV), the attitude of the ROV, the gyroscope drift of the strapdown inertial navigation system, and the accelerometer bias. The adaptive unscented Kalman filter drives state prediction with the relative pose data, uses the absolute ground position as the measurement value, and performs measurement update after time registration of the absolute ground position with the state prediction at the corresponding time using delayed measurement update technology.

3. The system according to claim 2, characterized in that, The extended state observer adopts a third-order extended state observer structure, with the fused pose as input. The disturbance estimates output by the extended state observer ensemble characterize the changes in ocean current force, borehole impact reaction force, umbilical cable drag force, and unmodeled dynamic characteristics.

4. The system according to claim 3, characterized in that, The thruster dynamic compensation unit calculates the cavitation number in real time based on the thruster speed and ambient pressure. When the cavitation number is lower than a preset threshold, the thruster dynamic compensation unit generates a thrust attenuation compensation amount according to the pre-stored cavitation number-thrust attenuation characteristic curve. When the thrust command of the thruster is lower than a preset dead zone threshold, the thruster dynamic compensation unit injects a high-frequency flutter signal.

5. The system according to claim 4, characterized in that, When any thruster fails, the thrust distribution unit sets the upper limit of the thruster to zero through the fault detection logic, and re-solves the thrust distribution scheme through the construction-analysis method. During the re-solution process, the weight of the degree of freedom corresponding to the axial thrust of the opening is set to the highest.

6. A method for disturbance-resistant control of ROV opening pose based on long baseline integrated navigation, characterized in that, The method includes: The relative pose data of the remotely operated vehicle (ROV) relative to the initial navigation alignment point is obtained by a strapdown inertial navigation system and a Doppler velocimeter. The absolute ground position of the ROV is obtained by a long baseline positioning array. The fused pose is obtained by fusing the relative pose data and the absolute ground position using an adaptive unscented Kalman filter. The fused pose is input into the extended state observer to obtain perturbation estimates for external disturbances and unmodeled dynamics; Based on the deviation between the desired pose and the fused pose, a nominal control force is generated by the sliding mode controller. The nominal control force is then corrected by feedforward compensation using the disturbance estimate. Finally, the thruster dynamic compensation unit performs nonlinear compensation on the corrected control force for cavitation attenuation characteristics and low-speed dead zone characteristics to obtain the compensated control force. The compensated control force is distributed to at least six vector-arranged thrusters using a constructive-analytical method and then executed. At preset time intervals, the absolute geodetic position of the remotely operated vehicle is obtained through the long baseline positioning array, and the absolute geodetic position is input into the adaptive unscented Kalman filter to correct the cumulative drift of the strapdown inertial navigation system.

7. The method according to claim 6, characterized in that, When the bottom tracking signal of the Doppler velocimeter is lost or the output data of the Doppler velocimeter is abnormal, the adaptive unscented Kalman filter identifies the failure state through residual monitoring and switches to a combined mode in which the state prediction is recursively performed by the strapdown inertial navigation system and the measurement update is provided by the absolute ground position.

8. The method according to claim 7, characterized in that, The control law of the sliding mode controller includes an equivalent control term, a switching control term, and a disturbance compensation term. The switching control term adopts a combined approach law, and the approach rate of the combined approach law is positively correlated with the deviation between the desired pose and the fused pose.

9. The method according to claim 8, characterized in that, The construction-analytical method establishes and solves the mapping relationship between the thrust and control force vectors of each thruster when the thruster is not saturated. When the thruster is saturated, it switches to the construction method and sets the weights of the degrees of freedom corresponding to the axial thrust of the opening and the anti-overturning moment to the highest.

10. The method according to claim 9, characterized in that, The preset time interval is not less than 15 seconds. For each preset level decrease in the gyroscope drift rate of the strapdown inertial navigation system, the preset time interval increases by one step value. For each preset level decrease in the operating water depth of the remotely operated unmanned underwater vehicle, the preset time interval increases by one step value.

Images