Reliable omnidirectional acoustic homing guidance method for underwater robot recovery

Abstract

The application provides a reliable omnidirectional acoustic guiding method for underwater robot recovery and homing, which obtains state information and distance data of a moving mother ship through integrated acoustic ranging and communication equipment, and realizes autonomous guiding and recovery of the underwater robot to the moving mother ship. The method provided by the application can realize accurate guiding of the underwater robot to the mother ship only by measuring data of one underwater acoustic communication equipment in the case of static and motion of the mother ship.

Description

Technical Field

[0001] This invention relates to the field of guidance and positioning technology for autonomous underwater vehicles, and in particular to a reliable omnidirectional acoustic guidance method for the recovery and homing of an underwater robot. Background Technology

[0002] Autonomous underwater vehicles (AUVs) are widely used in various marine operations due to their ability to navigate autonomously and perform tasks independently. Since the endurance of AUVs is limited by battery capacity, automatic recharging is a solution to improve their mission capabilities. Furthermore, with the help of automatic recovery and guidance technology, AUVs can transmit large amounts of data at high speeds. However, after prolonged underwater navigation, a deviation in latitude and longitude reference between the AUV and the mother vessel can occur, making it impossible to accurately utilize the other's latitude and longitude information. Therefore, it is necessary to employ AUV guidance and positioning methods to eliminate this discrepancy and achieve a unified navigation reference.

[0003] Guidance algorithms are a key technology for achieving autonomous docking, making the development of underwater robot recovery and guidance technologies a key research direction both domestically and internationally. In recent years, this field has received considerable attention from the academic community. For the autonomous guidance process of two underwater robots, motion status information, including latitude, longitude, depth, speed, and heading angle, can be exchanged through underwater acoustic communication. Currently, acoustic guidance, optical guidance, and electromagnetic guidance technologies are mainly used. The docking process is clearly divided into two stages: the long-range homing stage, where the underwater robot uses an inertial navigation system and acoustic signals to locate the recovery station within a range of ten meters to a kilometer; and the close-range docking stage (meter- to ten-meter range), which requires higher-precision optical or electromagnetic guidance technologies to complete the final docking.

[0004] Acoustic guidance remains the most commonly used and preferred guidance method in underwater robot recovery missions due to its relatively long effective range. Traditional acoustic recovery guidance mainly employs long baseline (LBL) and ultra-short baseline (USBL) positioning methods.

[0005] Chinese patent publication CN113156442A discloses an underwater robot positioning method based on a long-baseline underwater acoustic system (SINS) assisted navigation. First, hydrophones are deployed and their positions calibrated. The long-baseline SINS receives signals from the underwater robot's sound source and performs generalized cross-correlation (GCL) calculations. The difference between the position coordinates calculated by the SINS and the long-baseline SINS is input into a Kalman filter for filtering. The filtering result corrects various errors in the SINS and DVL (Directed Virtual Positioning) systems, yielding accurate underwater robot position information. However, this technical solution has shortcomings. The long-baseline system requires the deployment and calibration of multiple beacons to establish positioning capability, a highly complex process that often consumes significant time and manpower to ensure accuracy. In contrast, traditional underwater robot recovery and guidance often employs the USBL (Underground Universal Positioning) method.

[0006] Chinese patent publication CN113805606B discloses a method for guiding, docking, and recovering an underwater vehicle based on ultra-short baseline acoustic positioning (USBL). The underwater robot sequentially completes waypoints using waypoint guidance. During its approach to the recovery station, it receives USBL positioning measurements and updates the station's latitude and longitude in real time by combining these measurements with the robot's own latitude and longitude. While USBL possesses strong positioning capabilities, it still has several limitations. First, the effective positioning range of USBL is not omnidirectional, covering only a small portion of the hemisphere. Second, the USBL positioning range is conical, with a typical maximum opening angle of 80°. Once the cone angle exceeds 30°, the relative slant distance error increases significantly, leading to a substantial increase in positioning error. Third, during the underwater robot's homing process, if the USBL array operates in a forward-looking mode, significant multipath effects occur in shallow water due to depth, while in deep water, the thermocline causes changes in sound velocity gradients and ray bending, resulting in a larger positioning error compared to the downward-looking mode.

[0007] To address these issues, a single acoustic transponder can be used to provide positioning information for the underwater robot. This method has low hardware requirements, eliminating the need for dedicated positioning equipment for underwater recovery guidance. Effective homing guidance can be achieved simply by adding ranging and transponder functionality to the existing underwater acoustic communication system of the underwater robot. Therefore, it is essential to research a reliable omnidirectional acoustic guidance method that can resolve the critical issue of inconsistent navigation references between the underwater robot and the mother ship during prolonged underwater navigation. Summary of the Invention

[0008] The purpose of this invention is to address the critical issue of inconsistency in navigation references between underwater robots and their mother vessels due to prolonged underwater navigation. This invention proposes a reliable omnidirectional acoustic guidance method for the recovery and retrieval of underwater robots. This method integrates acoustic ranging and communication equipment to acquire the status information and distance data of the mobile mother vessel, enabling the underwater robot to autonomously guide and recover the mobile mother vessel.

[0009] This invention is achieved through the following technical solution: This invention proposes a reliable omnidirectional acoustic guidance method for the recovery and homing of an underwater robot, the method comprising the following steps: (1) The mothership remains stationary or moves at a constant depth, orientation, and speed; (2) After the underwater robot dives and the underwater acoustic channel is established, the mother ship broadcasts its latitude, longitude, heading and depth to the underwater robot at the start of the dive. (3) The underwater robot sends a ranging signal to the mother ship and receives feedback from the mother ship to calculate the distance; after completing the first complete cycle, the acoustic communication and ranging cycle remain unchanged, and the underwater robot will rely entirely on the ranging information to achieve pure acoustic ranging guidance; (4) The underwater robot uses its own depth gauge to calculate the depth difference between the underwater robot and the mother ship. The three-dimensional ranging information is projected onto the horizontal plane; (5) During the first five communication cycles, the underwater robot remains hovering after reaching the predetermined depth; combining the ranging information of the five cycles, a circle is drawn with the current position of the underwater robot as the center and the distance measured by the integrated acoustic ranging and communication equipment as the radius, and the initial position of the mother ship is projected onto the line connecting the initial positions of the underwater robot and the mother ship to eliminate the radial error of the starting position. (6) The underwater robot combines the ranging information to correct the initial position information of the mothership and the heading of the mothership, and solves the speed of the mothership. (7) Calculate the mothership's sailing time and the estimated value of the mothership's current moment by using the underwater acoustic time interval; (8) The underwater robot combines the ranging information, uses the corrected position of the mother ship and the calculated speed of the mother ship as the initial value of the filter, and uses the unscented Kalman filter based on singular value decomposition to estimate the position of the mother ship and sails towards the estimated value. (9) When the distance between the underwater robot and the mother ship reaches the preset distance value, the underwater robot performs a turning maneuver and navigates to... point, The line connecting the point to the mothership at the initial moment forms a 90-degree angle with the line connecting the starting position of the underwater robot to the starting position of the mothership, and the distances are equal. (10) Underwater robots in By hovering at a fixed point and combining the ranging information from five cycles, a circle is drawn with the current position of the underwater robot as the center and the distance measured by the integrated acoustic ranging and communication equipment as the radius. The initial corrected position of the mother ship is then projected onto the line connecting the initial positions of the underwater robot and the mother ship, thus eliminating the lateral error of the initial position. (11) Calculate the mothership's sailing time and the estimated value of the mothership's current moment by using the underwater acoustic time interval; (12) The underwater robot combines the ranging information, replaces the current filter value with the corrected mother ship position, estimates the mother ship position using an unscented Kalman filter based on singular value decomposition, and sails toward the estimated value. (13) When the distance between the underwater robot and the mother ship is less than the set termination threshold, the underwater robot adjusts its own state to maintain the same speed and direction as the mother ship and completes the recovery and guidance task.

[0010] Furthermore, the integrated acoustic ranging and communication device adds ranging and response functions to the underwater acoustic communication device. It is installed on the back of the underwater robot and the mother ship, respectively. It is used to monitor the underwater robot when it is performing tasks on the seabed. When measuring distance, the underwater robot sends an inquiry to the integrated acoustic ranging and communication device (IARC) on the mother ship. After receiving the signal, the IARC device on the mother ship processes it and returns a response signal. The underwater robot receives the response signal and calculates the corresponding distance data.

[0011] Furthermore, in step (4), after the underwater robot establishes a communication connection with the mothership via IARC, the initial position of the mothership is transmitted through underwater acoustic signals. and heading The data is transmitted to the underwater robot for initial positioning of the mothership; the depth of the underwater robot is measured using a depth gauge, and the depth difference between the underwater robot and the mothership is calculated. Record the initial time as .

[0012] Furthermore, in step (5), the method for correcting the initial position of the mothership is as follows: the underwater robot remains in a hovering state. Receive the first distance measurement information at any time Then, calculate the current position of the underwater robot. Azimuth angle relative to the initial position of the mothership ,in Therefore, the mothership's initial correction position In the following four cycles, the corrected position of the mothership is repeatedly calculated. After receiving the fifth ranging information, the average of the five calculated corrected positions of the mothership is taken to obtain the starting corrected position of the mothership. .

[0013] Furthermore, in step (6), in At that moment, the corrected position of the mothership was calculated. At the next moment, a two-dimensional coordinate system is established with the underwater robot as the origin, and after... After the time course, the mothership was The position of the moment is The location of the underwater robot is The distance between the mothership and the underwater robot is , The inner mothership's course is Based on ranging information The mothership is definitely located centered around the underwater robot. On a circle with radius ; according to The location of the mothership can be determined as follows: The vector originating from the circle; therefore, the intersection of the vector and the circle is the position of the mothership, based on the sailing time. Then the speed of the mothership can be calculated. The calculation formula is as follows: In the actual process of solving for the mothership's velocity, two solutions may appear, one of which is incorrect and needs to be deleted. The specific method for filtering out incorrect solutions is as follows: calculate... and Two velocity solutions, one of which should be the correct solution. Since the mothership is traveling at a constant speed, it is possible to predict its speed at which it will recover. Maximum speed within a time The correct solution should satisfy ;like If the conditions are met, then the correct velocity solution is: Conversely, if If the conditions are met, then the correct velocity solution is: To prevent outliers in ranging from affecting the mothership's speed estimation, a correction is made after obtaining the mothership's initial position. The distance measurement information for the last 9 beats is used to calculate the position and speed of the mothership at the corresponding time. The two speed values, the maximum and minimum, are removed, and the average value of the remaining values ​​is taken to obtain the final calculated sailing speed of the mothership.

[0014] Furthermore, in step (8), the system state equation of the unscented Kalman filter algorithm based on singular value decomposition is: ,in Indicates time The mothership state vector, where These are the mothership's north and east positions, respectively. These are the mothership's northward and eastward speeds, respectively. , indicating the distance measurement interval; This is the state noise during mothership estimation; the observation equation is: , ,in It is observation noise. Indicates that the mothership is Location, Indicates underwater robots in The location.

[0015] Furthermore, in step (8), the method for estimating the mothership's position includes: (8-1) Initialization: ,in It is the initial state vector. It is the initial covariance matrix, set based on equipment characteristics and experience. For mathematical expectation; (8-2) Constructing Sigma points and weights: The initial step of the unscented Kalman filter (UKF) algorithm is to obtain the previous time step. covariance matrix The square root of the given value is used for Cholesky decomposition: ,in Covariance matrix The matrix after Cholesky decomposition; Cholesky decomposition requires... It must be a positive semi-definite matrix; due to the influence of unknown noise, the matrix may... Since the matrix is ​​ill-conditioned, Cholesky decomposition cannot proceed. Therefore, Singular Value Decomposition (SVD) is introduced to replace the Cholesky transformation in performing the square root operation on the matrix. The specific steps are as follows: ,in yes 3D matrix It is a diagonal matrix, and its diagonal elements are called singular values; matrix , and All are positive numbers; generated by the unscented transform sampling method based on singular value decomposition. We use Sigma points to approximate the probability distribution of the state vector:

[0016] in for The state vector at time step 1 One sampling point, The dimension of the system state vector; parameters This is a scaling parameter used to reduce the overall prediction error; the relevant parameter values ​​are... , and To account for the significance of differences between different samples, the corresponding weights for the sampling points are calculated:

[0017] in and All represent the weights of the samples, where the subscripts are... The mean, For covariance; (8-3) Time Update: The time update is a prediction step based on the current estimated state and the system model, aiming to predict the state at the next moment through the system dynamic equations; specifically:

[0018] in for Time of the first One-step prediction for each sampling point; and This is a one-step prediction of the state vector and its covariance. It is the system process noise covariance matrix; (8-4) Measurement Update: The error between the new measured value and the predicted value is used to correct the current state vector and its corresponding covariance matrix; the predicted observation mean and covariance are:

[0019] in for The first moment One-step observation predictions for Sigma points; the cross-covariance between state and observation is: ,

[0020] in The system observation noise covariance matrix; Kalman gain; and Let be the updated state vector and its covariance.

[0021] Furthermore, in step (10), step (5) is repeated to obtain the mothership's corrected starting position again. .

[0022] Further, in step (12), step (8) is repeated, and the position of the mother ship is estimated by using an unscented Kalman filter based on singular value decomposition, and the pure tracking method is used to sail towards the estimated value of the mother ship.

[0023] Furthermore, in step (13), if the distance between the underwater robot and the mother ship is less than the set distance threshold and the holding time is within 5 underwater acoustic beats, the underwater robot recovery and guidance task is considered to be completed; otherwise, it is considered to have failed.

[0024] The beneficial effects of this invention are: (1) This invention proposes a novel acoustic guidance method based on single beacon ranging, which aims to overcome the limitations of the traditional USBL system and realize reliable and omnidirectional homing guidance during the underwater robot recovery process.

[0025] (2) The method proposed in this invention integrates limited moving target information and ranging data to estimate the position of the underwater robot, thereby solving the navigation inconsistency problem and achieving effective homing guidance.

[0026] (3) The method proposed in this invention can achieve precise guidance of the underwater robot to the vicinity of the mother ship by relying solely on the measurement data of a single underwater acoustic communication device, whether the mother ship is stationary or moving. Attached Figure Description

[0027] Figure 1 This is a flowchart of the underwater robot homing guidance method based on a single beacon according to the present invention; Figure 2 This is a schematic diagram illustrating the homing guidance of the underwater robot of the present invention. Figure 3 This is a schematic diagram illustrating the correction of the mothership's initial position deviation according to the present invention; Figure 4 This is a schematic diagram illustrating the calculation of the mothership's speed according to the present invention. Detailed Implementation

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

[0029] See Figures 1-4 This invention proposes a reliable omnidirectional acoustic guidance method for the recovery and homing of an underwater robot, the method comprising the following steps: (1) The mothership remains stationary or moves at a constant depth, orientation, and speed; (2) After the underwater robot dives and the underwater acoustic channel is established, the mother ship broadcasts its latitude, longitude, heading and depth to the underwater robot at the start of the dive. (3) The underwater robot sends a ranging signal to the mother ship and receives feedback from the mother ship to calculate the distance; after completing the first complete cycle, the acoustic communication and ranging cycle remain unchanged, and the underwater robot will rely entirely on the ranging information to achieve pure acoustic ranging guidance; (4) The underwater robot uses its own depth gauge to calculate the depth difference between the underwater robot and the mother ship. The three-dimensional ranging information is projected onto the horizontal plane; (5) During the first five communication cycles, the underwater robot remains hovering after reaching the predetermined depth; combining the ranging information of the five cycles, a circle is drawn with the current position of the underwater robot as the center and the distance measured by the integrated acoustic ranging and communication equipment as the radius, and the initial position of the mother ship is projected onto the line connecting the initial positions of the underwater robot and the mother ship to eliminate the radial error of the starting position. (6) The underwater robot combines the ranging information to correct the initial position information of the mothership and the heading of the mothership, and solves the speed of the mothership. (7) Calculate the mothership's sailing time and the estimated value of the mothership's current moment by using the underwater acoustic time interval; (8) The underwater robot combines the ranging information, uses the corrected position of the mother ship and the calculated speed of the mother ship as the initial value of the filter, and uses the unscented Kalman filter based on singular value decomposition to estimate the position of the mother ship and sails towards the estimated value. (9) When the distance between the underwater robot and the mother ship reaches the preset distance value, the underwater robot performs a turning maneuver and navigates to... point, The line connecting the point to the mothership at the initial moment forms a 90-degree angle with the line connecting the starting position of the underwater robot to the starting position of the mothership, and the distances are equal. (10) Underwater robots in By hovering at a fixed point and combining the ranging information from five cycles, a circle is drawn with the current position of the underwater robot as the center and the distance measured by the integrated acoustic ranging and communication equipment as the radius. The initial corrected position of the mother ship is then projected onto the line connecting the initial positions of the underwater robot and the mother ship, thus eliminating the lateral error of the initial position. (11) Calculate the mothership's sailing time and the estimated value of the mothership's current moment by using the underwater acoustic time interval; (12) The underwater robot combines the ranging information, replaces the current filter value with the corrected mother ship position, estimates the mother ship position using an unscented Kalman filter based on singular value decomposition, and sails toward the estimated value. (13) When the distance between the underwater robot and the mother ship is less than the set termination threshold, the underwater robot adjusts its own state to maintain the same speed and direction as the mother ship and completes the recovery and guidance task.

[0030] The Integrated Acoustic Ranging and Communication (IARC) device adds ranging and response functionality to an underwater acoustic communication unit. It is mounted on both the underwater robot and the mothership, and is used for monitoring the underwater robot while it is performing tasks on the seabed. It is also commonly used as an information transmission and positioning aid. When measuring distance, the underwater robot sends a query to the IARC on the mothership. The IARC on the mothership receives and processes the signal, returning a response signal. The underwater robot then receives the response signal and calculates the corresponding distance data.

[0031] In step (4), after the underwater robot establishes an IARC communication connection with the mothership, the initial position of the mothership is transmitted via underwater acoustic signals. and heading The data is transmitted to the underwater robot for initial positioning of the mothership; the depth of the underwater robot is measured using a depth gauge, and the depth difference between the underwater robot and the mothership is calculated. Record the initial time as .

[0032] In step (5), the method for correcting the initial position of the mothership is as follows: the underwater robot remains in a hovering state. Receive the first distance measurement information at any time Then, calculate the current position of the underwater robot. Azimuth angle relative to the initial position of the mothership ,in Therefore, the mothership's initial correction position In the following four cycles, the corrected position of the mothership is repeatedly calculated. After receiving the fifth ranging information, the average of the five calculated corrected positions of the mothership is taken to obtain the starting corrected position of the mothership. .

[0033] In step (6), At that moment, the corrected position of the mothership was calculated. At the next moment, a two-dimensional coordinate system is established with the underwater robot as the origin, and after... After the time course, the mothership was The position of the moment is The location of the underwater robot is The distance between the mothership and the underwater robot is , The inner mothership's course is Based on ranging information The mothership is definitely located centered around the underwater robot. On a circle with radius ; according to The location of the mothership can be determined as follows: The vector originating from the circle; therefore, the intersection of the vector and the circle is the position of the mothership, based on the sailing time. Then the mothership speed can be calculated. The calculation formula is as follows: In the actual process of solving for the mothership's velocity, two solutions may appear, one of which is incorrect and needs to be deleted. The specific method for filtering out incorrect solutions is as follows: calculate... and Two velocity solutions, one of which should be the correct solution. Since the mothership is traveling at a constant speed, it is possible to predict its speed at which it will recover. Maximum speed within a time The correct solution should satisfy ;like If the conditions are met, then the correct velocity solution is: Conversely, if If the conditions are met, then the correct velocity solution is: To prevent outliers in ranging from affecting the mothership's speed estimation, a correction is made after obtaining the mothership's initial position. The distance measurement information for the last 9 beats is used to calculate the position and speed of the mothership at the corresponding time. The two speed values, the maximum and minimum, are removed, and the average value of the remaining values ​​is taken to obtain the final calculated sailing speed of the mothership.

[0034] In step (8), the system state equation of the unscented Kalman filter algorithm based on singular value decomposition is: ,in Indicates time The mothership state vector, where These are the mothership's north and east positions, respectively. These are the mothership's northward and eastward speeds, respectively; , indicating the distance measurement interval; This is the state noise during mothership estimation; the observation equation is: , ,in It is observation noise. Indicates that the mothership is Location, Indicates underwater robots in The location.

[0035] In step (8), the method for estimating the mothership's position includes: (8-1) Initialization: ,in It is the initial state vector. It is the initial covariance matrix, set based on equipment characteristics and experience. For mathematical expectation; (8-2) Constructing Sigma points and weights: The initial step of the unscented Kalman filter (UKF) algorithm is to obtain the previous time step. covariance matrix The square root of the given value is used for Cholesky decomposition: ,in Covariance matrix The matrix after Cholesky decomposition; Cholesky decomposition requires... It must be a positive semi-definite matrix; due to the influence of unknown noise, the matrix may... Since the matrix is ​​ill-conditioned, Cholesky decomposition cannot proceed. Therefore, Singular Value Decomposition (SVD) is introduced to replace the Cholesky transformation in performing the square root operation on the matrix. The specific steps are as follows: ,in yes 3D matrix It is a diagonal matrix, and its diagonal elements are called singular values; matrix , and All are positive numbers; generated by the unscented transform sampling method based on singular value decomposition. We use Sigma points to approximate the probability distribution of the state vector:

[0036] in for The state vector at time step 1 One sampling point, The dimension of the system state vector; parameters This is a scaling parameter used to reduce the overall prediction error; the relevant parameter values ​​are... , and To account for the significance of differences between different samples, the corresponding weights for the sampling points are calculated:

[0037] in and All represent the weights of the samples, where the subscripts are... The mean, For covariance; (8-3) Time Update: The time update is a prediction step based on the current estimated state and the system model, with the aim of predicting the state at the next moment through the system dynamic equations; specifically:

[0038] in for Time of the first One-step prediction for each sampling point; and This is a one-step prediction of the state vector and its covariance. It is the system process noise covariance matrix; (8-4) Measurement Update: The error between the new measured value and the predicted value is used to correct the current state vector and its corresponding covariance matrix; the predicted observation mean and covariance are:

[0039] in for The first moment One-step observation predictions for Sigma points; the cross-covariance between state and observation is: ,

[0040] in The system observation noise covariance matrix; Kalman gain; and Let be the updated state vector and its covariance.

[0041] In step (10), step (5) is repeated to obtain the mothership's corrected starting position again. .

[0042] In step (12), step (8) is repeated, and the position of the mother ship is estimated by using an unscented Kalman filter based on singular value decomposition, and the pure tracking method is used to sail towards the estimated value of the mother ship.

[0043] In step (13), if the distance between the underwater robot and the mother ship is less than the set distance threshold and the holding time is within 5 underwater acoustic beats, the underwater robot recovery and guidance task is considered to be completed; otherwise, it is considered to have failed.

[0044] Example like Figure 1 As shown, this invention proposes a reliable omnidirectional acoustic guidance method for the recovery and return of an underwater robot, which is achieved by integrating acoustic ranging and communication equipment.

[0045] First, the mothership plans its route according to the desired route, which must avoid obstacles. The mothership can remain stationary or move at a constant depth, direction, and speed.

[0046] After the mothership has stabilized its course, the underwater robot dives to the desired depth. Both the mothership and the underwater robot are equipped with the same type of integrated acoustic ranging and communication equipment. This integrated acoustic ranging and communication equipment adds a ranging and response function to an existing underwater acoustic communication device. When measuring distance, the underwater robot sends a query to the equipment on the mothership. The mothership equipment receives and processes this signal, returning a response signal. The underwater robot then receives the response signal and calculates the corresponding distance data.

[0047] Considering the mutual interference between sound waves, underwater acoustic communication requires 6 seconds to complete, while according to the integrated acoustic ranging and communication equipment ranging function test, acoustic ranging requires 4 seconds. At the start, the mothership broadcasts the latitude and longitude of its inertial navigation system to the underwater robot. ,course Depth measured by a depth gauge.

[0048] The underwater robot sends ranging signals to the mother vessel and receives feedback to calculate the distance. After completing the first full cycle, the acoustic communication and ranging cycle remain unchanged, and the underwater robot will rely entirely on ranging information for pure acoustic ranging guidance. The underwater robot uses its own depth gauge to calculate the depth difference between itself and the mother vessel. The three-dimensional ranging information is projected onto the horizontal plane.

[0049] During the first five communication cycles, the underwater robot hovers after reaching the predetermined depth. For example... Figure 2 As shown, Constantly using the underwater robot's current position as the center, it receives ranging information from integrated acoustic ranging and communication equipment. Draw a circle with a radius of 100 and calculate the current position of the underwater robot. Azimuth angle relative to the initial position of the mothership ,in Project the initial position of the mothership onto the line connecting the initial positions of the underwater robot and the mothership, and calculate the initial correction position of the mothership. This eliminates the radial error in the mothership's initial position. In the following four cycles, the corrected position of the mothership is repeatedly calculated. After receiving the fifth ranging information, the average of the five calculated corrected positions is taken to obtain the corrected initial position of the mothership. .

[0050] After determining the mothership's corrected starting position, a two-dimensional coordinate system is established with the underwater robot's initial position as the origin, such as... Figure 4 As shown. After After the time course, the mothership was The position of the moment is The location of the underwater robot is The distance between the mothership and the underwater robot is... , The inner mothership's course is Based on ranging information The mothership is definitely located centered around the underwater robot. On a circle with radius . The location of the mothership can be determined as follows: The vector originating from the circle. Therefore, the intersection of the vector and the circle is the position of the mothership, depending on the sailing time. Then the mothership speed can be calculated. The calculation formula is as follows: In the actual process of solving for the mothership's velocity, two solutions may appear, one of which is incorrect and needs to be discarded. The following describes a method for filtering out incorrect solutions. The calculation... and Two velocity solutions, one of which should be the correct solution. Since the mothership travels at a constant speed, its trajectory can be predicted. Maximum speed within a time The correct solution should satisfy .like If the conditions are met, then the correct velocity solution is: Conversely, if If the conditions are met, then the correct velocity solution is: To prevent outliers in ranging from affecting the mothership's speed estimation, a correction is made after obtaining the mothership's initial position. The distance measurement information from the last nine beats is used to calculate the position and speed of the mothership at the corresponding time. The two highest and lowest speed values ​​are removed, and the average of the remaining values ​​is taken to obtain the final calculated speed of the mothership.

[0051] Combining the time it takes for the underwater robot to receive the initial position of the mothership and the time it takes to calculate the mothership's motion, the current position of the mothership is calculated based on the corrected initial position and used as the initial value for filtering. An unscented Kalman filter based on singular value decomposition is then used to estimate the mothership's position, and the robot navigates towards the estimated value. The system state equation for the unscented Kalman filter algorithm based on singular value decomposition is: ,in Indicates time The mothership state vector, where These are the mothership's north and east positions, respectively. These are the mothership's northward and eastward speeds, respectively. , indicating the distance measurement interval. This is the state noise during mothership estimation. The observation equation is: , ,in It is observation noise. Indicates that the mothership is Location, Indicates underwater robots in The location.

[0052] Methods for estimating the mothership's position include: 1. Initialization: ,in It is the initial state vector. It is the initial covariance matrix, set based on equipment characteristics and experience. Let be the mathematical expectation.

[0053] 2. Constructing Sigma points and weights: The initial step of the Unscented Kalman Filter (UKF) algorithm is to obtain the previous time step. covariance matrix The conventional method to find the square root is to perform Cholesky decomposition: ,in Covariance matrix The matrix after Cholesky decomposition. Cholesky decomposition requires... It must be a positive semi-definite matrix. Due to the influence of unknown noise, the matrix may... Since the matrix is ​​ill-conditioned, Cholesky decomposition cannot proceed. Therefore, Singular Value Decomposition (SVD) is introduced to replace the Cholesky transformation in performing the square root operation on the matrix. The specific steps are as follows: ,in yes 3D matrix It is a diagonal matrix, and its diagonal elements are called singular values. Matrix , and All are positive numbers. Generated using an unscented transform sampling method based on singular value decomposition. We use Sigma points to approximate the probability distribution of the state vector:

[0054] in for The state vector at time step 1 One sampling point, The dimension of the system state vector; parameters This is a scaling parameter used to reduce the overall prediction error. A recommended value for this parameter is [value to be filled in]. , and To account for the significance of differences between different samples, the corresponding weights for the sampling points are calculated:

[0055] in and All represent the weights of the samples, where the subscripts are... The mean, For covariance.

[0056] 3. Time Update: The time update is a prediction step based on the current estimated state and the system model. Its purpose is to predict the state at the next moment using the system's dynamic equations. Specific steps:

[0057] in for Time of the first One-step prediction for each sampling point; and This is a one-step prediction of the state vector and its covariance. It is the system process noise covariance matrix.

[0058] 4. Measurement Update: The measurement update step is the core of the algorithm. It uses the error between the new measured value and the predicted value to correct the current state vector and its corresponding covariance matrix. The predicted observation mean and covariance are:

[0059] in for The first moment One-step observation predictions for Sigma points. The cross-covariance between state and observation is: ,

[0060] in The system observation noise covariance matrix; Kalman gain; and Let be the updated state vector and its covariance.

[0061] When the distance measurement between the underwater robot and the mother ship reaches the preset distance value of 50m, the underwater robot performs a turning maneuver, which involves turning 45° to the right. point, The line connecting the point to the mothership at its initial moment forms a 90-degree angle with the line connecting the starting position of the underwater robot to the starting position of the mothership, and the distances between them are equal.

[0062] Underwater robot arrives Starting from a fixed point and hovering, combining the ranging information from five cycles, a circle is drawn with the underwater robot's current position as the center and the distance measured by the integrated acoustic ranging and communication equipment as the radius. The initial corrected position of the mothership is projected onto the line connecting the initial positions of the underwater robot and the mothership. This process is repeated. Figure 4 After eliminating the lateral error of the initial position, the corrected initial position of the mothership is obtained again. Based on the time it takes for the underwater robot to receive the initial position of the mothership, the current position of the mothership is calculated using the corrected initial position. This corrected position is then replaced with the current filter value. The unscented Kalman filter based on singular value decomposition is then used to estimate the mothership's position, and the robot navigates toward the estimated value.

[0063] When the distance between the underwater robot and the mother ship is less than the set termination threshold of 8m, the underwater robot adjusts its own state to maintain the same speed and direction as the mother ship for 5 underwater acoustic beats, and completes the underwater robot recovery and guidance task.

[0064] Although the present invention has been disclosed above with reference to preferred embodiments, it is not intended to limit the present invention. Anyone skilled in the art can make various modifications and alterations without departing from the spirit and scope of the present invention. Therefore, the scope of protection of the present invention should be determined by the claims.

Claims

1. A reliable omnidirectional acoustic guidance method for the recovery and homing of an underwater robot, characterized in that, The method includes the following steps: (1) The mothership remains stationary or moves at a constant depth, orientation, and speed; (2) After the underwater robot dives and the underwater acoustic channel is established, the mother ship broadcasts its latitude, longitude, heading and depth to the underwater robot at the start of the dive. (3) The underwater robot sends a ranging signal to the mother ship and receives feedback from the mother ship to calculate the distance; after completing the first complete cycle, the acoustic communication and ranging cycle remain unchanged, and the underwater robot will rely entirely on the ranging information to achieve pure acoustic ranging guidance; (4) The underwater robot uses its own depth gauge to calculate the depth difference between the underwater robot and the mother ship. The three-dimensional ranging information is projected onto the horizontal plane; (5) During the first five communication cycles, the underwater robot remains hovering after reaching the predetermined depth; combining the ranging information of the five cycles, a circle is drawn with the current position of the underwater robot as the center and the distance measured by the integrated acoustic ranging and communication equipment as the radius, and the initial position of the mother ship is projected onto the line connecting the initial positions of the underwater robot and the mother ship to eliminate the radial error of the starting position. (6) The underwater robot combines the ranging information to correct the initial position information of the mothership and the heading of the mothership, and solves the speed of the mothership. (7) Calculate the mothership's sailing time and the estimated value of the mothership's current moment by using the underwater acoustic time interval; (8) The underwater robot combines the ranging information, uses the corrected position of the mother ship and the calculated speed of the mother ship as the initial value of the filter, and uses the unscented Kalman filter based on singular value decomposition to estimate the position of the mother ship and sails towards the estimated value. (9) When the distance between the underwater robot and the mother ship reaches the preset distance value, the underwater robot performs a turning maneuver and navigates to... point, The line connecting the point to the mothership at the initial moment forms a 90-degree angle with the line connecting the starting position of the underwater robot to the starting position of the mothership, and the distances are equal. (10) Underwater robots in By hovering at a fixed point and combining the ranging information from five cycles, a circle is drawn with the current position of the underwater robot as the center and the distance measured by the integrated acoustic ranging and communication equipment as the radius. The initial corrected position of the mother ship is then projected onto the line connecting the initial positions of the underwater robot and the mother ship, thus eliminating the lateral error of the initial position. (11) Calculate the mothership's sailing time and the estimated value of the mothership's current moment by using the underwater acoustic time interval; (12) The underwater robot combines the ranging information, replaces the current filter value with the corrected mother ship position, estimates the mother ship position using an unscented Kalman filter based on singular value decomposition, and sails toward the estimated value. (13) When the distance between the underwater robot and the mother ship is less than the set termination threshold, the underwater robot adjusts its own state to maintain the same speed and direction as the mother ship and completes the recovery and guidance task.

2. The method according to claim 1, characterized in that, The integrated acoustic ranging and communication device adds ranging and response functions to the underwater acoustic communication device. It is installed on the back of the underwater robot and the mother ship, respectively. It is used to monitor the underwater robot when it is performing tasks on the seabed. When measuring distance, the underwater robot sends an inquiry to the integrated acoustic ranging and communication device on the mother ship. After receiving the signal, the integrated acoustic ranging and communication device on the mother ship processes it and returns a response signal. The underwater robot receives the response signal and calculates the corresponding distance data.

3. The method according to claim 1, characterized in that, In step (4), after the underwater robot end and the mother ship end are connected by integrated acoustic ranging and communication equipment, the initial position of the mother ship is transmitted through underwater acoustic signals. and heading The data is transmitted to the underwater robot for initial positioning of the mothership; the depth of the underwater robot is measured using a depth gauge, and the depth difference between the underwater robot and the mothership is calculated. Record the initial time as .

4. The method according to claim 1, characterized in that, In step (5), the method for correcting the initial position of the mothership is as follows: the underwater robot remains in a hovering state. Receive the first distance measurement information at any time Then, calculate the current position of the underwater robot. Azimuth angle relative to the initial position of the mothership ,in Therefore, the mothership's initial correction position In the following four cycles, the corrected position of the mothership is repeatedly calculated. After receiving the fifth ranging information, the average of the five calculated corrected positions of the mothership is taken to obtain the starting corrected position of the mothership. .

5. The method according to claim 1, characterized in that, In step (6), At that moment, the corrected position of the mothership was calculated. At the next moment, a two-dimensional coordinate system is established with the underwater robot as the origin, and after... After the time course, the mothership was The position of the moment is The location of the underwater robot is The distance between the mothership and the underwater robot is , The inner mothership's course is Based on ranging information The mothership is definitely located centered around the underwater robot. On a circle with radius ; according to The location of the mothership can be determined as follows: The vector originating from the circle; therefore, the intersection of the vector and the circle is the position of the mothership, based on the sailing time. Then the mothership speed can be calculated. ; The calculation formula is as follows: In the actual process of solving for the mothership's velocity, two solutions may appear, one of which is incorrect and needs to be deleted. The specific method for filtering out incorrect solutions is as follows: calculate... and Two velocity solutions, one of which should be the correct solution. Since the mothership is traveling at a constant speed, it is possible to predict its speed at which it will recover. Maximum speed within a time The correct solution should satisfy ;like If the conditions are met, then the correct velocity solution is: Conversely, if If the conditions are met, then the correct velocity solution is: ; To prevent outliers in ranging from affecting the mothership's speed estimation, the corrected starting position of the mothership is obtained. The distance measurement information for the last 9 beats is used to calculate the position and speed of the mothership at the corresponding time. The two speed values, the maximum and minimum, are removed, and the average value of the remaining values ​​is taken to obtain the final calculated sailing speed of the mothership.

6. The method according to claim 1, characterized in that, In step (8), the system state equation of the unscented Kalman filter algorithm based on singular value decomposition is: ,in Indicates time The mothership state vector, where These are the mothership's north and east positions, respectively. These are the mothership's northward and eastward speeds, respectively. , indicating the distance measurement interval; This is the state noise during mothership estimation; the observation equation is: , ,in It is observation noise. Indicates that the mothership is Location, Indicates underwater robots in The location.

7. The method according to claim 1, characterized in that, In step (8), the method for estimating the mothership's position includes: (8-1) Initialization: ,in It is the initial state vector. It is the initial covariance matrix, set based on equipment characteristics and experience. For mathematical expectation; (8-2) Constructing Sigma points and weights: The initial step of the unscented Kalman filter (UKF) algorithm is to obtain the previous time step. covariance matrix The square root of the given value is used for Cholesky decomposition: ,in Covariance matrix The matrix after Cholesky decomposition; Cholesky decomposition requires... It must be a positive semi-definite matrix; due to the influence of unknown noise, the matrix... Since the matrix is ​​ill-conditioned, Cholesky decomposition cannot proceed. Therefore, Singular Value Decomposition (SVD) is introduced to replace the Cholesky transformation in performing the square root operation on the matrix. The specific steps are as follows: ,in yes 3D matrix It is a diagonal matrix, and its diagonal elements are called singular values; matrix , and All are positive numbers; generated by the unscented transform sampling method based on singular value decomposition. We use Sigma points to approximate the probability distribution of the state vector: in for The state vector at time step 1 One sampling point, The dimension of the system state vector; parameters This is a scaling parameter used to reduce the overall prediction error; the relevant parameter values ​​are... , and To account for the significance of differences between different samples, the corresponding weights for the sampling points are calculated: in and All represent the weights of the samples, where the subscripts are... The mean, For covariance; (8-3) Time Update: The time update is a prediction step based on the current estimated state and the system model, aiming to predict the state at the next moment through the system dynamic equations; specifically: in for Time of the first One-step prediction for each sampling point; and This is a one-step prediction of the state vector and its covariance. It is the system process noise covariance matrix; (8-4) Measurement Update: The error between the new measured value and the predicted value is used to correct the current state vector and its corresponding covariance matrix; the predicted observation mean and covariance are: in for The first moment One-step observation predictions for Sigma points; the cross-covariance between state and observation is: , in The system observation noise covariance matrix; Kalman gain; and Let be the updated state vector and its covariance.

8. The method according to claim 1, characterized in that, In step (10), step (5) is repeated to obtain the mothership's corrected starting position again. .

9. The method according to claim 1, characterized in that, In step (12), step (8) is repeated, and the position of the mother ship is estimated by using an unscented Kalman filter based on singular value decomposition, and the pure tracking method is used to sail towards the estimated value of the mother ship.

10. The method according to claim 1, characterized in that, In step (13), if the distance between the underwater robot and the mother ship is less than the set distance threshold and the holding time is within 5 underwater acoustic beats, the underwater robot recovery and guidance task is considered to be completed; otherwise, it is considered to have failed.

Images