Underwater cable type robot positioning method and system

Abstract

The invention relates to the field of underwater positioning, and particularly discloses an underwater cable type robot positioning method and system, and the method comprises the steps: S1, enabling a robot to be placed underwater through an unmanned ship and an umbilical cable laying device; s2, continuously collecting positioning information of the robot; s3, processing the obtained positioning information, and outputting a positioning result; and S4, repeating the steps S2-S3, and continuously obtaining the position information of the robot. The method is high in positioning precision, integrates inertial navigation and ultra-short baseline positioning data, and is higher in precision than inertial navigation, an acoustic positioning method and a traditional integrated navigation method. And meanwhile, original observation is directly fused, so that the problem of error accumulation caused by firstly calculating the absolute position of the ultra-short baseline and then fusing with inertial navigation in a traditional integrated navigation method is avoided. In addition, the robustness of the method is enhanced, and when USBL signals are interrupted or partial observation is abnormal, the positioning precision can still be maintained through other observation data.

Description

Technical Field

[0001] The present invention relates to the field of underwater positioning, and specifically relates to a positioning method and system for an underwater cable - type robot. Background Art

[0002] With the rapid development of fields such as computer science and technology, sensing technology, etc., the field of robots has achieved rapid development, laying a solid foundation for the technological development of underwater robots. Underwater robots play an increasingly important role in fields such as ocean development and underwater operations, showing great potential. One of the core technologies of underwater robots is underwater positioning, and the quality of its implementation is directly related to the accuracy and efficiency of underwater path planning tasks, thereby affecting the underwater operation efficiency and safety of the robot. Since the underwater environment cannot receive GPS signals, higher requirements are imposed on underwater positioning tasks. Effective and highly accurate underwater positioning can enable underwater robots to efficiently complete tasks in complex underwater environments, avoid collisions and dangers, and improve the reliability and stability of work.

[0003] For underwater cable - type robots, there are currently three main existing positioning methods. One is the inertial navigation method, one is the acoustic positioning method, and one is the traditional integrated navigation method. The principle of the inertial positioning algorithm is to use an inertial measurement unit, which consists of various sensors such as accelerometers and gyroscopes, to measure the acceleration and angular velocity information of the underwater robot. The computer performs integral operations on these data to deduce the position, speed, and attitude changes of the robot. The acoustic positioning method relies on the propagation characteristics of sound waves in water. By accurately capturing key data such as the propagation time and phase of sound waves, the distance and angle parameters between the underwater robot and beacons or base stations at known positions can be accurately determined, and then relevant mathematical models and algorithms are used to accurately deduce the specific position of the robot underwater. The traditional integrated navigation method refers to achieving complementary advantages by combining the inertial navigation and acoustic positioning methods, and realizing data fusion by means of Kalman filtering to obtain more accurate positioning. Summary of the Invention

[0004] To solve the technical problems in the above background, the present invention provides a positioning method for an underwater cable - type robot. The steps include:

[0005] S1. Place the robot underwater through an unmanned ship and an umbilical cable deployment device;

[0006] S2. Continuously collect the positioning information of the robot. The steps include: collecting acceleration and angular velocity through an inertial measurement device, and obtaining the position update of the robot by using double integral; obtaining the position observation value of the robot by using the ultra - short baseline positioning method;

[0007] S3. Process the obtained positioning information and output the positioning result. The steps include: introducing a fading factor into the Kalman filter, dynamically adjusting the weight of historical data according to the ultra-short baseline positioning information, and when the ultra-short baseline signal disappears, increasing the fading factor to accelerate the forgetting of old data;

[0008] S4. Repeat S2 - S3 to continuously obtain the position information of the robot.

[0009] Preferably, use double integral to obtain the position update of the robot:

[0010] ,

[0011] ,

[0012] where, v(t) represents the velocity integral; r(t) represents the position integral; v 0 represents the initial velocity of the robot; v represents the velocity of the robot at time t; a represents the acceleration of the robot; r 0 represents the initial position of the robot; t represents the current time; σ and s represent the integration variables.

[0013] Preferably, set the introduced fading factor as λ, where 0 < λ ≤ 1, the process noise covariance is Q, and the observation noise covariance is R; the dynamic adjustment formula of the fading factor and the estimated values of Q and R are respectively:

[0014] ,

[0015] ,

[0016] ,

[0017] In the formula, λ k represents the dynamic adjustment value of the fading factor; Q k represents the estimated value of the noise covariance; R k represents the estimated value of the observation noise covariance; S k represents the covariance of the observation residual; represents the predicted covariance matrix; represents the updated covariance matrix; represents the lower threshold; K k represents the weight matrix of the state update; represents the process noise; y i represents the observation residual at the i-th step; N represents the sliding window length; H k describes how the state variables affect the observations; T represents the transpose of the matrix; k represents the k-th step; tr represents the trace of the matrix.

[0018] Preferably, the residual chi-square test is used to detect abnormal observations to determine whether abnormal data occurs during the positioning of the ultra-short baseline positioning. The judgment criteria are as follows:

[0019] ,

[0020] where γ k represents the standardized residual; y k represents the observation residual at the k-th step.

[0021] Preferably, the residual chi-square test is used to detect abnormal observations. If the data is valid, the positioning value is calculated according to the tightly coupled observation equation:

[0022] ,

[0023] In the formula, z k represents the positioning value obtained by fusing the original USBL data and the INS integration to obtain the position and velocity information; represents a non-linear observation function for mapping the state vector x k obtained by the INS to the observation space of the USBL; v k represents the observation noise vector.

[0024] Preferably, The expression of

[0025] ,

[0026] where r represents the straight-line distance between the robot and the ultra-short baseline base station; θ represents the azimuth angle of the robot relative to the ultra-short baseline base station on the horizontal plane; φ represents the pitch angle of the robot relative to the ultra-short baseline base station on the vertical plane; x represents the x coordinate of the robot in the unmanned ship coordinate system; x USBL represents the x coordinate of the ultra-short baseline array in the unmanned ship coordinate system; y represents the y coordinate of the robot in the unmanned ship coordinate system; y USBL represents the y coordinate of the ultra-short baseline array in the unmanned ship coordinate system; z represents the z coordinate of the robot in the unmanned ship coordinate system; z USBL represents the z coordinate of the ultra-short baseline array in the unmanned ship coordinate system.

[0027] The present invention also provides an underwater cable-type robot positioning system, which is used to implement the above method, including: a collection module, a processing module, and an update module;

[0028] The collection module is used to continuously collect the positioning information of the robot. The process includes: collecting acceleration and angular velocity through an inertial measurement device, and obtaining the position update of the robot by using double integration; obtaining the position observation value of the robot through the ultra-short baseline positioning method;

[0029] The processing module is used to process the acquired positioning information and output a positioning result. The process includes: introducing a fading factor into the Kalman filter, dynamically adjusting the weight of historical data according to the ultra-short baseline positioning information, and increasing the fading factor to accelerate the forgetting of old data when the ultra-short baseline signal disappears.

[0030] The updating module is used to receive the positioning result of the processing module and update the position information of the robot in real time.

[0031] Compared with the prior art, the beneficial effects of the present invention are as follows:

[0032] 1. High positioning accuracy, integrating data from inertial navigation and ultra-short baseline positioning, with a higher accuracy than inertial navigation, acoustic positioning methods, and traditional integrated navigation methods.

[0033] 2. Directly fuse the original observations: avoiding the error accumulation problem in the traditional integrated navigation method of first calculating the absolute position of the ultra-short baseline and then fusing it with inertial navigation.

[0034] 3. Enhanced robustness, still being able to maintain the positioning accuracy through other observation data when the USBL signal is intermittent or some observations are abnormal.

[0035] 4. Good adaptability, by dynamically adjusting the noise equation and adaptively adjusting the weights of the two positioning methods, it can better adapt to dynamic environments compared with traditional methods. BRIEF DESCRIPTION OF THE DRAWINGS

[0036] In order to more clearly illustrate the technical solutions of the present invention, the following briefly introduces the drawings required in the embodiments. Obviously, the drawings in the following description are only some embodiments of the present invention. For those of ordinary skill in the art, other drawings can be obtained based on these drawings without creative efforts.

[0037] Figure 1 It is a flowchart of the method of the present invention. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0038] The following will clearly and completely describe the technical solutions in the embodiments of the present invention with reference to the drawings in the embodiments of the present invention. Obviously, the described embodiments are only some embodiments of the present invention, rather than all embodiments. All other embodiments obtained by those of ordinary skill in the art based on the embodiments of the present invention without creative efforts belong to the scope of protection of the present invention.

[0039] First, some technical terms used in the present invention are elaborated:

[0040] USBL: Ultra-Short Baseline positioning system

[0041] INS: Inertial Navigation System,

[0042] To make the above objects, features, and advantages of the present invention more obvious and understandable, the present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments.

[0043] Embodiment 1:

[0044] As can be seen from the background art, the disadvantages of the inertial navigation method are:

[0045] 1. Errors accumulate over time. The inertial navigation system calculates speed and position by integrating acceleration. Due to measurement errors in inertial sensors such as accelerometers and gyroscopes, these errors will continuously accumulate over time, resulting in a gradual increase in the error of the navigation result.

[0046] 2. Sensitive to changes in motion state. The accuracy of the inertial navigation system is greatly affected by changes in the motion state of the carrier. Especially in high-dynamic environments, such as rapid turning, acceleration, and deceleration, the measurement errors of inertial sensors will increase significantly, thus affecting the navigation accuracy.

[0047] The disadvantages of the acoustic positioning method are:

[0048] 1. Limited positioning range. Since the propagation of sound waves in a medium attenuates with increasing distance, the effective range of acoustic positioning is limited.

[0049] 2. Larger positioning variance. There are often other environmental noises underwater, such as wave sounds and biological noises, which will interfere with acoustic signals. The greater the noise, the more it will affect the reception and transmission of acoustic signals, thus reducing the accuracy and reliability of positioning.

[0050] 3. Poor real-time performance. The propagation speed of sound waves is relatively slow, and the round-trip time between the transponder beacon and the ultra-short baseline beacon is long, resulting in poor real-time performance.

[0051] The disadvantages of traditional integrated navigation methods are:

[0052] 1. Over-reliance on acoustic positioning. Once the acoustic positioning signal is lost due to environmental reasons, the traditional integrated navigation method cannot effectively suppress the cumulative error of the inertial navigation method, resulting in a decrease in positioning accuracy.

[0053] 2. Poor adaptability. Insufficient processing of the slant range and angle observation noises of acoustic positioning, and failure to jointly optimize the two sets of data, resulting in limited positioning accuracy.

[0054] This embodiment provides an underwater cabled robot positioning method, and the steps include:

[0055] S1. Place the robot underwater through an unmanned ship and an umbilical cable deployment device.

[0056] S2. Continuously collect the positioning information of the robot.

[0057] S201. Collect the acceleration and angular velocity through an inertial measurement device and obtain the position update of the robot by using double integration. The steps include:

[0058] Under the three-dimensional inertial coordinate system, the steps to calculate the carrier velocity and position based on the IMU measurement data are as follows:

[0059] (1) Initialization, calibrate the IMU, and set the initial position vector of the underwater robot , and the initial velocity vector is , and the acceleration vector .

[0060] (2) Update the velocity according to the acceleration data. The velocity integration formula is:

[0061] .

[0062] (3) Update the position vector of the carrier according to the velocity formula. The position integration formula is:

[0063] ,

[0064] where, v(t) represents the velocity integration; r(t) represents the position integration; v 0 represents the initial velocity of the robot; v represents the velocity of the robot at time t; a represents the acceleration of the robot; r 0 represents the initial position of the robot; t represents the current time; σ and s represent the integration variables.

[0065] S202. Obtain the position observation value of the robot through the ultra-short baseline positioning method.

[0066] The surface unit is installed on the surface hull and includes a USBL transducer array (multiple hydrophones). The robot carries an acoustic transponder to respond to the signal of the USBL.

[0067] The transducer array sends an acoustic pulse signal to the underwater robot. After receiving the signal, the robot immediately returns a response signal.

[0068] The time difference of the response signals received by each hydrophone in the USBL array (because the path lengths of the sound waves reaching different hydrophones are different).

[0069] According to the time difference or phase difference, calculate the azimuth angle and pitch angle of the signal through the interference method. Calculate the slant range from the USBL array through the round-trip time of the acoustic signal and the speed of sound.

[0070] S3. Process the obtained positioning information and output the positioning result.

[0071] The steps include: introducing a fading factor to the Kalman filter, dynamically adjusting the weights of historical data according to the ultra-short baseline positioning information, and when the ultra-short baseline signal disappears, increasing the fading factor to accelerate the forgetting of old data, thereby reducing the cumulative error.

[0072] First, initialize the state, specifically including: initializing the speed and position of the robot, initializing P as a diagonal matrix, and the diagonal elements being the initial variances of the speed and position state components; initializing Q as a diagonal matrix, and the diagonal elements representing the noise intensities of each state component, including the noise variance of the accelerometer and the ocean current noise variance; initializing R as a diagonal matrix, and the diagonal elements being the noise variances of the slant range, azimuth angle, and pitch angle measurement channels of the ultra-short baseline positioning measurement.

[0073] The state after initialization includes: x 0 , P 0 , Q 0 and R 0 . Among them, x 0 is the initial state vector, including the position (x, y, z) and speed (v x , v y , v z ); P 0 represents the state covariance matrix, which is used to represent the uncertainty of the state estimation; Q 0 represents the process noise covariance matrix, which is used to describe the uncertainty of the system model; R 0 represents the observation noise covariance matrix, which describes the noise characteristics in the observation data.

[0074] After that, predict the state x and covariance P at the next moment. The inputs are the acceleration of the INS and the attitude angle updated by integrating the angular velocity. Construct a uniform motion model according to the time interval and adjust the process noise in real time according to the motion state. The specific steps are as follows:

[0075] Let the fading factor be λ(0 < λ ≤ 1), let the process noise covariance be Q, and the observation noise covariance be R. Then the dynamic adjustment formula of the fading factor in this embodiment and the estimated values of Q and R are respectively:

[0076] ,

[0077] ,

[0078] ,

[0079] In the formula, λ k represents the dynamic adjustment value of the fading factor; Qk represents the estimated value of the noise covariance; R k represents the estimated value of the observation noise covariance; S k represents the covariance of the observation residual, which is used to evaluate the quality of the current ultra-short baseline positioning observation value; represents the predicted covariance matrix, which is used to describe the state uncertainty when the current observation is not considered; represents the updated covariance matrix, which is used to describe the state uncertainty after fusing the current observation state; represents the lower threshold (0.95 is taken in this embodiment), which is used to prevent filtering oscillation caused by excessive forgetting; K k represents the weight matrix of the state update; represents the process noise; y i represents the observation residual at the i-th step; N represents the sliding window length; H k describes how the state variable affects the observation value; T represents the transpose of the matrix; k represents the k-th step; tr represents the trace of the matrix (Trace).

[0080] When the quality of the ultra-short baseline positioning observation is high, that is, when it decreases, tends to approach the lower limit, more historical information will be retained in the process noise covariance Q and the weight of the ultra-short baseline positioning observation value will be increased to smooth the trajectory; while when the ultra-short baseline positioning fails or there are large unreasonable fluctuations, that is, when it increases, when it increases, the forgetting of old data is accelerated to suppress the filtering divergence.

[0081] Q mainly describes the process noise characteristics in the positioning system, such as robot modeling error, ocean current interference, INS noise, etc. Among them, K k represents the weight matrix of the state update, that is, the Kalman gain matrix, which is used to adjust the ratio of the state estimate to the observation residual; H k describes how the state variable affects the observation value, maps the position and velocity state of the ROV to the slant range, azimuth angle and pitch angle observation values of the ultra-short baseline positioning; R describes the noise characteristics of the observation data, such as ultra-short baseline positioning measurement noise, environmental interference, ultra-short baseline positioning data anomaly or loss, etc. y i is the observation residual at the i-th step; N is the sliding window length.

[0082] Abnormal observations are detected through the residual chi-square test to judge whether there are abnormal data during the ultra-short baseline positioning. The judgment criteria are as follows:

[0083] ,

[0084] where, γ k represents the standardized residual; T represents the transpose of the matrix; if (where α is the significance level and p is the observation dimension), it is determined that the positioning value observed at this time by the ultra-short baseline positioning is abnormal data, and R is temporarily enlarged to reduce the impact; y k represents the observation residual at the k-th step.

[0085] If the data is valid, the positioning value is calculated according to the tightly coupled observation equation:

[0086] ,

[0087] In the formula, z k is an observation vector, representing the positioning value obtained after fusing the USBL raw data and the position and velocity information obtained by INS integration; is an observation function, representing mapping the state vector x k obtained by INS to the observation space of USBL, and after linearizing it, the matrix H k is obtained; v k represents the observation noise vector, which follows a Gaussian distribution with a mean of 0 and a covariance of R k .

[0088] Among them, the specific expression of the non-linear observation function is:

[0089] ,

[0090] Among them, r represents the straight-line distance between the robot and the ultra-short baseline base station; θ represents the azimuth angle of the robot relative to the ultra-short baseline base station on the horizontal plane; φ represents the pitch angle of the robot relative to the ultra-short baseline base station on the vertical plane; x represents the x coordinate of the robot in the unmanned ship coordinate system; x USBL represents the x coordinate of the ultra-short baseline array in the unmanned ship coordinate system; y represents the y coordinate of the robot in the unmanned ship coordinate system; y USBL represents the y coordinate of the ultra-short baseline array in the unmanned ship coordinate system; z represents the z coordinate of the robot in the unmanned ship coordinate system; z USBL represents the z coordinate of the ultra-short baseline array in the unmanned ship coordinate system.

[0091] Since the observation function is non-linear, it needs to be linearized in the Kalman filter to obtain the Jacobian matrix H k :

[0092] ,

[0093] Among them, x, y, and z represent the position vector (x, y, z) of the robot in the unmanned ship coordinate system.

[0094] The residual covariance is calculated according to the following formula for subsequent calculation of the Kalman gain matrix to determine the degree of trust in the observation:

[0095] ,

[0096] The two noises Q and R are updated according to the following formula:

[0097] ,

[0098] ,

[0099] Finally, the fading factor is adjusted. The Kalman gain matrix, state, and covariance are updated, and the positioning result is output.

[0100] S4. Repeat S2 - S3 to continuously obtain the position information of the robot.

[0101] The process framework of the present invention is as Figure 1 shown.

[0102] Embodiment 2:

[0103] This embodiment also provides an underwater cable - type robot positioning system, including: an acquisition module, a processing module, and an update module.

[0104] Next, in combination with this embodiment, it will be detailed how the present invention solves the technical problems in actual work.

[0105] First, the acquisition module is used to continuously acquire the positioning information of the robot. The process includes:

[0106] The acceleration and angular velocity are acquired through an inertial measurement device, and double - integral is used to obtain the position update of the robot.

[0107] The acceleration and angular velocity are acquired through an inertial measurement device, and double - integral is used to obtain the position update of the robot. The steps include:

[0108] In a three - dimensional inertial coordinate system, the steps to calculate the carrier velocity and position according to the IMU measurement data are as follows:

[0109] (1) Initialization, calibrate the IMU, and set the initial position vector of the underwater robot , the initial velocity vector is , and the acceleration vector .

[0110] (2) Update the velocity according to the acceleration data. The velocity integration formula is:

[0111] ,

[0112] (3) Update the position vector of the carrier according to the speed formula. The position integration formula is:

[0113] ,

[0114] where v(t) represents the speed integration; r(t) represents the position integration; v 0 represents the initial speed of the robot; v represents the speed of the robot at time t; a represents the acceleration of the robot; r 0 represents the initial position of the robot; t represents the current time; σ and s represent integration variables.

[0115] Obtain the position observation value of the robot through the ultra-short baseline positioning method.

[0116] The surface unit is installed on the surface hull and includes a USBL transducer array (multiple hydrophones). The robot carries an acoustic transponder and responds to the signal of the USBL.

[0117] The transducer array sends an acoustic pulse signal to the underwater robot. After receiving the signal, the robot immediately returns a response signal.

[0118] The time difference of the response signals received by each hydrophone in the USBL array (due to the different path lengths of the sound waves reaching different hydrophones).

[0119] According to the time difference or phase difference, calculate the azimuth angle and pitch angle of the signal by the interference method. Calculate the slant range from the USBL array through the round-trip time of the acoustic signal and the speed of sound.

[0120] After that, use the processing module to process the obtained positioning information and output the positioning result. The process includes: introducing a fading factor into the Kalman filter, dynamically adjusting the weight of historical data according to the ultra-short baseline positioning information, and increasing the fading factor to accelerate the forgetting of old data when the ultra-short baseline signal disappears, so as to reduce the cumulative error.

[0121] First, initialize the state, specifically including: initializing the speed and position of the robot, initializing P as a diagonal matrix, and the diagonal elements are the initial variances of the speed and position state components; initializing Q as a diagonal matrix, and the diagonal elements represent the noise intensities of each state component, including the noise variance of the accelerometer and the ocean current noise variance; initializing R as a diagonal matrix, and the diagonal elements are the noise variances of the slant range, azimuth angle, and pitch angle measurement channels of the ultra-short baseline positioning measurement.

[0122] The state after initialization includes: x 0 , P 0 , Q 0 and R 0 . Among them, x 0 is the initial state vector, including the position (x, y, z) and speed (vx , v y , v z ); P 0 represents the state covariance matrix, which is used to represent the uncertainty of state estimation; Q 0 represents the process noise covariance matrix, which is used to describe the uncertainty of the system model; R 0 represents the observation noise covariance matrix, which describes the noise characteristics in the observation data.

[0123] After that, the state x and covariance P at the next moment are predicted. The input is the acceleration of the INS and the attitude angle updated by integrating the angular velocity. According to the time interval A uniform motion model is constructed, and the process noise is adjusted in real time according to the motion state , and the specific steps are as follows:

[0124] Let the fading factor be λ (0 < λ ≤ 1), let the process noise covariance be Q, and the observation noise covariance be R. Then the dynamic adjustment formula of the fading factor in this embodiment and the estimated values of Q and R are respectively:

[0125] ,

[0126] ,

[0127] ,

[0128] In the formula, λ k represents the dynamic adjustment value of the fading factor; Q k represents the estimated value of the noise covariance; R k represents the estimated value of the observation noise covariance; S k represents the covariance of the observation residual, which is used to evaluate the quality of the current ultra-short baseline positioning observation value; represents the predicted covariance matrix, which is used to describe the state uncertainty when the current observation is not considered; represents the updated covariance matrix, which is used to describe the state uncertainty after fusing the current observation state; represents the lower threshold (0.95 is taken in this embodiment), which prevents filtering oscillation caused by excessive forgetting; K k represents the weight matrix for state update; represents the process noise; y i represents the observation residual at the i-th step; N represents the sliding window length; H k describes how the state variables affect the observation values; T represents the transpose of the matrix; k represents the k-th step; tr represents the trace of the matrix.

[0129] When the quality of the ultra-short baseline positioning observation is high, that is When it decreases, approaching the lower limit, more historical information will be retained in the process noise covariance Q and the weight of the ultra-short baseline positioning observation value will be increased to smooth the trajectory; while when the ultra-short baseline positioning fails or there are large unreasonable fluctuations, ( When it increases, increasing, then accelerating the forgetting of old data to suppress the divergence of filtering.

[0130] Q mainly describes the process noise characteristics in the positioning system, such as robot modeling error, ocean current interference, INS noise, etc. Among them, K k represents the weight matrix for state update, that is, the Kalman gain matrix, which is used to adjust the ratio of state estimation to observation residual; H k describes how the state variables affect the observation values, mapping the position and velocity states of the ROV to the slant range, azimuth angle, and pitch angle observation values of the ultra-short baseline positioning; R describes the noise characteristics of the observation data, such as ultra-short baseline positioning measurement noise, environmental interference, ultra-short baseline positioning data anomaly or loss, etc.; y i is the observation residual at the i-th step; N is the sliding window length.

[0131] Anomaly observations are detected through the residual chi-square test to determine whether there are abnormal data during ultra-short baseline positioning. The judgment criteria are as follows:

[0132] ,

[0133] where γ k represents the standardized residual; T represents the transpose of the matrix; if , (α is the significance level, p is the observation dimension), then it is determined that the positioning value observed by the ultra-short baseline positioning at this time is abnormal data, and R is temporarily amplified to reduce the impact; y k represents the observation residual at the k-th step.

[0134] If the data is valid, the positioning value is calculated according to the tightly coupled observation equation:

[0135] ,

[0136] In the formula, z k is the observation vector, representing the positioning value obtained after fusing the USBL raw data and the INS integral to obtain the position and velocity information; represents mapping the state vector x k obtained by INS to the observation space of USBL, and after linearizing it, the matrix H k is obtained; v k represents the observation noise vector, which follows a Gaussian distribution with a mean of 0 and a covariance of R k .

[0137] Among them, the non - linear observation function has the following specific expression:

[0138] ,

[0139] where r represents the straight - line distance between the robot and the ultra - short baseline base station; θ represents the azimuth angle of the robot relative to the ultra - short baseline base station on the horizontal plane; φ represents the pitch angle of the robot relative to the ultra - short baseline base station on the vertical plane; x represents the x - coordinate of the robot in the unmanned ship coordinate system; x USBL represents the x - coordinate of the ultra - short baseline array in the unmanned ship coordinate system; y represents the y - coordinate of the robot in the unmanned ship coordinate system; y USBL represents the y - coordinate of the ultra - short baseline array in the unmanned ship coordinate system; z represents the z - coordinate of the robot in the unmanned ship coordinate system; z USBL represents the z - coordinate of the ultra - short baseline array in the unmanned ship coordinate system.

[0140] Since the observation function is non - linear, it needs to be linearized in the Kalman filter to obtain the Jacobian matrix H k :

[0141] ,

[0142] where x, y, z represent the position vector (x, y, z) of the robot in the unmanned ship coordinate system.

[0143] The residual covariance is calculated according to the following formula for subsequent calculation of the Kalman gain matrix, which determines the trust level of the observation:

[0144] ,

[0145] The two noises Q and R are updated according to the following formula:

[0146] ,

[0147] ,

[0148] Finally, the fading factor is adjusted. The Kalman gain matrix, state, and covariance are updated, and the positioning result is output.

[0149] Finally, the update module receives the positioning result of the processing module and updates the position information of the robot in real - time.

[0150] The embodiments described above are only descriptions of the preferred embodiments of the present invention, and do not limit the scope of the present invention. Without departing from the design spirit of the present invention, various deformations and improvements made by those of ordinary skill in the art to the technical solutions of the present invention shall fall within the protection scope determined by the claims of the present invention.

Claims

1. A method for positioning an underwater cable-type robot, characterized in that the steps include: S1. Place the robot underwater using an unmanned ship and umbilical cable deployment device; S2. Continuously collect the positioning information of the robot, the steps include: collecting acceleration and angular velocity through an inertial measurement device, and using double integral to obtain the position update of the robot; obtaining the position observation value of the robot through an ultra-short baseline positioning method; S3. Processing the acquired positioning information and outputting the positioning result, the steps include: introducing a fading factor into the Kalman filter, dynamically adjusting the weight of historical data according to the ultra-short baseline positioning information, and increasing the fading factor to accelerate the forgetting of old data when the ultra-short baseline signal disappears; S4. Repeat S2-S3 to continuously obtain the robot's position information.

2. The underwater cable robot positioning method according to claim 1, characterized in that: Use double integration to get the robot's position update: ， ， Among them, v(t) represents velocity integral; r(t) represents position integral; v0 represents the initial velocity of the robot; v represents the velocity of the robot at time t; a represents the acceleration of the robot; r0 represents the initial position of the robot; t represents the current time; σ and s represent integral variables.

3. The underwater cable robot positioning method according to claim 1, characterized in that: The introduced fading factor is set to λ, where 0<λ≤1, the process noise covariance is Q, and the observation noise covariance is R; the dynamic adjustment formula of the fading factor and the estimated values ​​of Q and R are: ， ， ， In the formula, λ k Indicates the dynamic adjustment value of the fading factor; Q k represents the estimated value of the noise covariance; R k represents the estimated value of the observation noise covariance; S k represents the covariance of the observed residuals; represents the prediction covariance matrix; represents the updated covariance matrix; Represents the lower threshold; K k The weight matrix representing the state update; represents process noise; y i represents the observed residual of the i-th step; N represents the sliding window length; H k Describes how the state variables affect the observed values; T represents the transpose of the matrix; k represents the kth step; tr represents the trace of the matrix.

4. The underwater cable robot positioning method according to claim 3, characterized in that: The residual chi-square test is used to test abnormal observations to determine whether abnormal data occurs during positioning in ultra-short baseline positioning. The judgment criteria are as follows: ， Among them, γ k represents the standardized residual; y k represents the observed residual at the kth step.

5. The underwater cable robot positioning method according to claim 4, characterized in that: The abnormal observation is tested by residual chi-square. If the data is valid, the positioning value is calculated according to the tightly coupled observation equation: ， In the formula, z k It represents the positioning value obtained by fusing the USBL raw data and the position and velocity information obtained by INS integration; represents a nonlinear observation function, which is used to transform the state vector x obtained by INS k Mapped to the observation space of USBL; v k represents the observation noise vector.

6. The underwater cabled robot positioning method according to claim 5, characterized in that: The expression is: ， Where r represents the straight-line distance between the robot and the ultra-short baseline base station; θ represents the azimuth angle of the robot on the horizontal plane relative to the ultra-short baseline base station; φ represents the pitch angle of the robot on the vertical plane relative to the ultra-short baseline base station; x represents the x-coordinate of the robot in the unmanned ship coordinate system; x USBL represents the x-coordinate of the ultra-short baseline array in the unmanned ship coordinate system; y represents the y-coordinate of the robot in the unmanned ship coordinate system; y USBL represents the y coordinate of the ultra-short baseline array in the unmanned ship coordinate system; z represents the z coordinate of the robot in the unmanned ship coordinate system; USBL Represents the z coordinate of the ultra-short baseline array in the unmanned ship coordinate system.

7. An underwater tethered robot positioning system, the system being used to implement the method according to any one of claims 1 to 6, characterized in that: include: Acquisition module, processing module and update module; The acquisition module is used to continuously acquire the positioning information of the robot, and the process includes: acquiring acceleration and angular velocity through an inertial measurement device, and obtaining the position update of the robot by using double integral; obtaining the position observation value of the robot by an ultra-short baseline positioning method; The processing module is used to process the acquired positioning information and output the positioning result. The process includes: introducing a fading factor into the Kalman filter, dynamically adjusting the weight of historical data according to the ultra-short baseline positioning information, and increasing the fading factor to accelerate the forgetting of old data when the ultra-short baseline signal disappears; The updating module is used to receive the positioning result of the processing module and update the position information of the robot in real time.

Images