An Underwater Cabled Robot Positioning Method and System

Abstract

The present invention relates to the field of underwater positioning, and specifically discloses an underwater cabled robot positioning method and system. The method steps include: S1. Placing the robot underwater through an unmanned ship and an umbilical cable deployment device; S2. Continuously collecting the positioning information of the robot; S3. Processing the obtained positioning information and outputting a positioning result; S4. Repeating S2-S3 to continuously obtain the position information of the robot. The positioning of the present invention has high accuracy, integrates data from inertial navigation and ultra-short baseline positioning, and has higher accuracy than inertial navigation, acoustic positioning methods, and traditional integrated navigation methods. At the same time, it directly integrates 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. In addition, the robustness of the present invention is enhanced, and the positioning accuracy can still be maintained through other observation data when the USBL signal is intermittent or some observations are abnormal.

Description

Technical Field

[0001] The present invention relates to the field of underwater positioning, and particularly 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 robotics 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 enables 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. Then, using relevant mathematical models and algorithms, the specific position of the robot underwater can be accurately deduced. 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 - mentioned 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 increasing the fading factor to accelerate the forgetting of old data when the ultra-short baseline signal disappears.

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

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

[0010] ,

[0011] ,

[0012] where v(t) represents the velocity integral; r(t) represents the 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 the integration variables.

[0013] Preferably, the introduced fading factor is set 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 length of the sliding window; H k describes how the state variables affect the observed values; T represents the transpose of the matrix; k represents the k-th step; tr represents the trace of the matrix.

[0018] Preferably, residual chi-square test is used to detect abnormal observations to determine whether there are abnormal data in 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, 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 INS to the observation space of USBL; v k represents the observation noise vector.

[0024] Preferably, The expression of is:

[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 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 fusing 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, being more adaptable to dynamic environments than traditional methods. BRIEF DESCRIPTION OF THE DRAWINGS

[0036] In order to more clearly illustrate the technical solutions of the present invention, the drawings required for use in the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention, and those of ordinary skill in the art can also obtain other drawings 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 technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the drawings in the embodiments of the present invention. Obviously, the described embodiments are only a part of the embodiments of the present invention, rather than all the embodiments. All other embodiments obtained by those of ordinary skill in the art based on the embodiments of the present invention without creative efforts shall fall within the protection scope 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. Underwater is often accompanied by other environmental noises, 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 the traditional integrated navigation method 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 errors 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 cable-type robot positioning method, and the steps include:

[0055] S1. Use an unmanned ship and an umbilical cable deployment device to place the robot underwater.

[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] In 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; 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 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 sound speed.

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

[0071] The steps include: introducing a fading factor into 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: x0, P0, Q0, and R0. Among them, x0 is the initial state vector, including the position (x, y, z) and speed (v x , v y , v z ); P0 represents the state covariance matrix, used to represent the uncertainty of the state estimate; Q0 represents the process noise covariance matrix, used to describe the uncertainty of the system model; R0 represents the observation noise covariance matrix, describing 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. According to the time interval construct a uniform motion model and adjust the process noise in real time according to the motion state , and 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; 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, 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 without considering the current observation; represents the updated covariance matrix, which is used to describe the state uncertainty after fusing the current observation state; represents the lower threshold (taking 0.95 in this embodiment), which prevents the 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 variable affects the observed 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 observation quality of the ultra-short baseline positioning is high, that is decreases, approaches the lower limit, more historical information will be retained in the process noise covariance Q and the weight of the ultra-short baseline positioning observed value will be increased to smooth the trajectory; while when the ultra-short baseline positioning fails or there are large unreasonable fluctuations, that is increases, increases, then the acceleration of forgetting old data is used 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 for 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 observed value, maps the position and velocity state of the ROV to the slant range, azimuth angle and pitch angle observed values of the ultra-short baseline positioning; R describes the noise characteristics of the observed 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 determine whether there are abnormal data in the ultra-short baseline positioning during positioning. The judgment criteria are as follows:

[0083] ,

[0084] Among them, γ 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.

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

[0086] ,

[0087] where z k is an observation vector representing the positioning value obtained after fusing the USBL raw data and the INS integrated position and velocity information; is an observation function representing the state vector x obtained by INS k mapped to the observation space of USBL, and after linearization, 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] 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.

[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] where x, y, and z represent the position vector (x, y, z) of the robot in the unmanned ship coordinate system.

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

[0095] ,

[0096] Update the two noises Q and R according to the following formula:

[0097] ,

[0098] ,

[0099] Finally, adjust the fading factor. Update the Kalman gain matrix, state, and covariance, and output the positioning result.

[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: a collection module, a processing module, and an update module.

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

[0105] First, use the collection module to continuously collect the positioning information of the robot. The process includes:

[0106] Collect acceleration and angular velocity through an inertial measurement device, and use double - integral to obtain the position update of the robot.

[0107] Collect acceleration and angular velocity through an inertial measurement device, and use double - integral 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 velocity formula. The position integration formula is:

[0113] ,

[0114] Among them, v(t) represents the velocity integral; r(t) represents the 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 the integration variables.

[0115] The position observation value of the robot is obtained 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 to respond 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, the azimuth angle and elevation angle of the signal are calculated by the interference method. The slant range from the USBL array is calculated by the round-trip time of the acoustic signal and the sound speed.

[0120] After that, the processing module processes the obtained positioning information and outputs 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 when the ultra-short baseline signal disappears, increasing the fading factor to accelerate forgetting of old data, thereby reducing the cumulative error.

[0121] First, initialize the state, specifically including: initializing the velocity and position of the robot, initializing P as a diagonal matrix, and the diagonal elements are the initial variances of the velocity 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 elevation angle measurement channels of the ultra-short baseline positioning measurement.

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

[0123] Next, 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 without considering the current observation; 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) to prevent 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 length of the sliding window; 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.

[0129] When the quality of the ultra-short baseline positioning observation is high, that is decreases, approaches 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, ([ increases, increases, then the old data is forgotten faster to suppress the filtering divergence.

[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 observed values, mapping the position and velocity states of the ROV to the slant range, azimuth, and pitch angle observations of the ultra-short baseline positioning; R describes the noise characteristics of the observed 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 in the ultra-short baseline positioning during 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 integrated position and velocity information; represents mapping the state vector x k obtained by the INS to the observation space of the USBL, and after linearization, 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 specific expression of the non-linear observation function is:

[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 to determine the confidence 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 above-described embodiments 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 should fall within the protection scope determined by the claims of the present invention.

Claims

1. An underwater cable - type robot positioning method, characterized in that the steps Including: S1. Lower the robot underwater through an unmanned vessel and an umbilical cable deployment device. S2. Continuously collect the positioning information of the robot. The steps include: collect acceleration and angular velocity through an inertial measurement device, and use double integration to obtain the position update of the robot; obtain the position observation value of the robot through the ultra-short baseline positioning method. S3. Process the obtained positioning information and output the positioning result. The steps include: introduce a fading factor into the Kalman filter, dynamically adjust the weight of historical data according to the ultra-short baseline positioning information, and when the ultra-short baseline signal disappears, increase the fading factor to accelerate the forgetting of old data; 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: ， ， ， where λ 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 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 observations; T represents the transpose of the matrix; k represents the k-th step; tr represents the trace of the matrix; Detect abnormal observations through residual chi-square test. If the data is valid, calculate the positioning value according to the tightly coupled observation equation: ， where z k represents the positioning value obtained after integrating the original data of the integrated ultra-short baseline positioning system and the inertial navigation system to obtain the position and velocity information; represents a non-linear observation function used to map the state vector x obtained by the inertial navigation system k to the observation space of the ultra-short baseline positioning system; v k represents the observation noise vector; The expression of ， 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; S4. Repeat S2 - S3 to continuously obtain the position information of the robot.

2. The underwater cable - type robot positioning method according to claim 1, characterized in that, Obtain the position update of the robot by using double integration: ， ， Where, v(t) represents the velocity integration; r(t) represents the position integration; 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 the integration variables.

3. The underwater cable - type robot positioning method according to claim 1, characterized in that, Detect abnormal observations through residual chi-square test to determine whether abnormal data appears during 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 k-th step.

4. An underwater cable - type robot positioning system, which is used to implement the method described in any one of claims 1 - 3, and is characterized in that, Including: A collection module, a processing module, and an update module; The collection module is used to continuously collect the positioning information of the robot. The process includes: collect acceleration and angular velocity through an inertial measurement device, and use double integration to obtain the position update of the robot; obtain the position observation value of the robot through the ultra-short baseline positioning method. The processing module is used to process the obtained positioning information and output the positioning result. The process includes: introduce a fading factor into the Kalman filter, dynamically adjust the weight of historical data according to the ultra-short baseline positioning information, and when the ultra-short baseline signal disappears, increase the fading factor to accelerate the forgetting of old data. The update module is used to receive the positioning result of the processing module and update the position information of the robot in real time.

Images