A positioning system and method for submersible robots based on multi-source sonar positioning information fusion
By fusing positioning data using a multi-source sonar positioning system and a complementary filtering algorithm, the problem of positioning difficulties for submersible robots in closed liquid environments was solved, achieving high-precision positioning of submersible robots.
Patent Information
- Application Number
- CN202310848497.1
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2023-07-11
- Publication Date
- 2026-11-10
- Estimated Expiration
- 2043-07-11
AI Technical Summary
Existing technologies struggle to achieve high-precision positioning of submersible robots in enclosed liquid environments, especially in large storage tanks where positioning errors are significant. Furthermore, conventional methods are severely affected by noise and other errors, leading to positioning difficulties and ambiguity.
A multi-source sonar positioning system, including an ultra-short baseline positioning device and a long baseline positioning device, is used in conjunction with a positioning beacon. The positioning data is fused through a complementary filtering algorithm to achieve precise positioning of the submersible robot.
This improved the accuracy and stability of the submersible robot's positioning, reduced the impact of noise, and ensured high-precision positioning in large storage tanks.
Smart Images

Figure CN117055052B_ABST
Abstract
Description
Technical Field
[0001] This invention mainly relates to the field of submersible robot positioning technology, specifically to a submersible robot positioning system and method based on multi-source sonar positioning information fusion. Background Technology
[0002] Storage tanks are extremely common storage devices in the oil and petrochemical industry. Due to the complexity of the internal media composition, the tank bottom plate is highly susceptible to corrosion, leading to equipment failure. To address this issue, the tank bottom plate is typically inspected and its condition assessed periodically. However, because the tank bottom plate is often covered by various oil products, conventional inspection methods are extremely difficult to implement smoothly. This usually requires opening the tank and shutting down for maintenance, which is time-consuming, labor-intensive, and increases costs.
[0003] Currently, online inspection methods are relatively mature. The development of online inspection robots for tank bottom plates, utilizing magnetic flux leakage, acoustic emission, and guided wave detection technologies combined with intelligent robots, is on the agenda. However, due to the unique environment—robots operate in enclosed liquid environments—a submersible positioning method is urgently needed to help obtain the robot's real-time position and status information in this invisible environment. Most underwater positioning products currently on the market cannot meet the positioning needs of robots in this environment. Existing technologies, for example, require the installation of four ultrasonic sensors along the robot's central axis. Due to the specific nature of this positioning method, the positions of these four sensors need to be precisely determined; otherwise, even small deviations in installation angle or position will significantly affect the positioning results. Furthermore, the positioning error increases with the increase in tank diameter, making this method only suitable for small oil tanks. Another example is installing the positioning receiver transducer outside the tank. This method causes the acoustic signal to travel through two different media—the tank wall and the oil inside—during propagation. The different propagation characteristics of acoustic signals in different media, and the complex effects of acoustic signals passing through heterogeneous interfaces, increase the difficulty of positioning and affect positioning accuracy. Meanwhile, using the signal arrival time difference method to achieve robot localization will result in multiple values, meaning the results are ambiguous, and there may be no solution under conditions of high noise and error. Summary of the Invention
[0004] The technical problem to be solved by the present invention is to provide a submersible robot positioning system and method based on multi-source sonar positioning information fusion, which addresses the shortcomings of the prior art.
[0005] The technical solution of the present invention to solve the above-mentioned technical problems is as follows: a submersible robot positioning system based on multi-source sonar positioning information fusion, comprising an ultra-short baseline positioning device, a long baseline positioning device, a positioning beacon and an external control device, wherein the ultra-short baseline positioning device is installed on the top of the tank, the long baseline positioning device is installed on the outer wall of the tank, and the positioning beacon is installed on the body of the submersible robot.
[0006] The positioning beacon is used to transmit positioning sound source signals to the ultra-short baseline positioning device and the long baseline positioning device;
[0007] The ultra-short baseline positioning device is used to perform positioning calculations on the submersible robot based on the positioning sound source signal to obtain first positioning data.
[0008] The long baseline positioning device is used to perform positioning calculations on the submersible robot based on the positioning sound source signal to obtain second positioning data.
[0009] The external control device is used to optimize the first positioning data and the second positioning data using a complementary filtering algorithm to obtain the final positioning result.
[0010] Another technical solution to the above-mentioned technical problem to be solved by the present invention is as follows: A method for positioning a submersible robot based on multi-source sonar positioning information fusion, comprising the following steps:
[0011] The positioning beacon transmits positioning sound source signals to the ultra-short baseline positioning device and the long baseline positioning device;
[0012] The ultra-short baseline positioning device performs positioning calculations on the submersible robot based on the positioning sound source signal to obtain the first positioning data;
[0013] The long baseline positioning device performs positioning calculations on the submersible robot based on the positioning sound source signal to obtain second positioning data;
[0014] The external control device uses a complementary filtering algorithm to optimize the first positioning data and the second positioning data to obtain the final positioning result.
[0015] The beneficial effects of this invention are as follows: by installing an ultra-short baseline positioning device and a long baseline positioning device on the storage tank and installing a positioning beacon on the body of the submersible robot, the submersible robot is positioned to obtain different positioning data. The two positioning data are then optimized by a complementary filtering algorithm to eliminate errors, thereby realizing the fusion of multi-source sonar positioning information, making the positioning results more accurate, and reducing the impact of noise. Attached Figure Description
[0016] Figure 1This is a schematic diagram of the submersible robot positioning system provided in an embodiment of the present invention;
[0017] Figure 2 A schematic diagram illustrating the principle of ultra-short baseline positioning provided in an embodiment of the present invention;
[0018] Figure 3 This is a flowchart illustrating the positioning method for a submersible robot provided in an embodiment of the present invention.
[0019] Figure 4 This is a flowchart illustrating a program example of a submersible robot positioning method provided in an embodiment of the present invention.
[0020] In the attached diagram, the component names represented by each label are as follows:
[0021] 1. Tank top; 2. Ultra-short baseline positioning device; 3. Long baseline positioning device; 4. Positioning beacon; 5. Submersible robot. Detailed Implementation
[0022] The principles and features of the present invention are described below with reference to the accompanying drawings. The examples given are only for explaining the present invention and are not intended to limit the scope of the present invention.
[0023] The submersible robot positioning system designed in this invention employs acoustic positioning technology, fusing ultra-short baseline and long baseline methods to obtain the submersible robot's position. Before positioning, a positioning beacon 4 is installed on the submersible robot's body. An ultra-short baseline positioning device is positioned on the top of the tank to receive beacon positioning information, while a long baseline positioning device is installed outside the tank to simultaneously locate the robot's positioning beacon 4. This achieves correction and compensation for the submersible robot's position, thereby reducing positioning errors. The submersible robot positioning system, which integrates multi-sensor information from ultra-short baseline and long baseline methods, ensures excellent positioning accuracy.
[0024] The following is a detailed description through examples:
[0025] Example 1:
[0026] like Figure 1 As shown, a submersible robot positioning system based on multi-source sonar positioning information fusion includes an ultra-short baseline positioning device 2, a long baseline positioning device 3, a positioning beacon 4, and an external control device. The ultra-short baseline positioning device 2 is installed on the top 1 of the storage tank, the long baseline positioning device 3 is installed on the outer wall of the storage tank, and the positioning beacon 4 is installed on the body of the submersible robot 5.
[0027] The positioning beacon 4 is used to transmit positioning sound source signals to the ultra-short baseline positioning device 2 and the long baseline positioning device 3;
[0028] The ultra-short baseline positioning device 2 is used to perform positioning calculations on the submersible robot 5 based on the positioning sound source signal to obtain first positioning data.
[0029] The long baseline positioning device 3 is used to perform positioning calculations on the submersible robot 5 based on the positioning sound source signal to obtain second positioning data.
[0030] The external control device is used to optimize the first positioning data and the second positioning data using a complementary filtering algorithm to obtain the final positioning result.
[0031] All components inside the tank in this positioning system are designed to be explosion-proof, providing excellent explosion-proof and flameproof capabilities.
[0032] It should be understood that the ultra-short baseline positioning device 2 is an underwater positioning technology that is widely used in marine production and development, such as marine oil exploration and development and marine salvage. The long baseline positioning device 3 is a physical performance testing instrument used in the fields of basic disciplines of information science and systems science, engineering and technology science, related engineering and technology of information and systems science, and related engineering and technology of natural science.
[0033] Specifically, the ultra-short baseline positioning device 2 and the long baseline positioning device 3 can be electrically connected to the external control device of the tank via wireless or wired means to transmit data.
[0034] In the above embodiments, an ultra-short baseline positioning device 2 and a long baseline positioning device 3 are installed on the storage tank, and a positioning beacon 4 is installed on the body of the submersible robot 5 to locate the submersible robot 5, respectively, to obtain different positioning data. The two positioning data are then optimized by a complementary filtering algorithm to eliminate errors, thereby realizing the fusion of multi-source sonar positioning information, making the positioning results more accurate, and reducing the impact of noise.
[0035] Based on the above embodiments, the ultra-short baseline positioning device 2 is also used to perform data compensation processing on the initial data before positioning calculation;
[0036] The long baseline positioning device 3 is also used to perform data compensation processing on the initial data before positioning calculation.
[0037] In the above embodiments, the ultra-short baseline positioning device 2 and the long baseline positioning device 3 correct and compensate the initial data, thereby reducing the positioning error.
[0038] like Figure 2 As shown, specifically, the ultra-short baseline positioning device 2 includes an ultra-short baseline transducer array and a first positioning device; the ultra-short baseline transducer array includes three ultra-short baseline transducers, which are installed in an isosceles right triangle at the top 1 of the storage tank.
[0039] The first positioning device is used for:
[0040] An xoy coordinate axis is established with the ultra-short baseline transducer A1 as the origin. Ultra-short baseline transducers A2 and A3 are located on the x-axis, forming an isosceles right triangle. The positioning beacon 4 is located at point S with coordinates (x1, y1, z1), where z1 represents the tank depth, and S1 is the projection of S onto the xoy plane. Based on spatial geometry, the phase difference expression, the θ angle expression, and the horizontal slant range of the positioning beacon 4 are obtained for the ultra-short baseline transducer receiving the positioning sound source signal. The phase difference expression is:
[0041]
[0042] in, The phase difference between the location sound source signals received by the ultra-short baseline transducer A1 and the ultra-short baseline transducer A2 is given. Let d be the phase difference between the location sound source signals received by the ultra-short baseline transducer A3 and the ultra-short baseline transducer A2, where d is the baseline length and d≤λ / 2, α is the angle between S and the x-axis, and β is the angle between S and the y-axis.
[0043] The expression for angle θ is:
[0044]
[0045] Where θ is the angle between S1 and the x-axis;
[0046] The horizontal slant distance of the positioning beacon 4 is:
[0047]
[0048] The first positioning data of the positioning beacon 4 is obtained based on the phase difference expression, the θ angle expression, and the horizontal slant distance of the positioning beacon 4. The first positioning data is:
[0049]
[0050] in, c is the speed of sound in oil, and Δt is the time difference between the transmission of the positioning sound source signal from positioning beacon 4 and the reception of the positioning sound source signal by the ultra-short baseline transducer.
[0051] Specifically, the long baseline positioning device 3 includes a long baseline transducer array and a second positioning device; the long baseline transducer array includes four long baseline transducers; the four long baseline transducers are installed on the outer wall of the storage tank at intervals and around the perimeter.
[0052] The second positioning device is used for:
[0053] Let (X) i ,Y i Z i Let be the coordinates of the i-th transducer, where i = 1, 2, 3, 4. Then the distance between the i-th transducer and the positioning beacon 4 is:
[0054]
[0055] Taking the first long-baseline transducer as the reference transducer, the distance difference between the positioning beacon 4 and the j-th transducer and the distance to the reference transducer is:
[0056]
[0057] Where j = 2, 3, 4, v is the propagation speed of the localization sound source signal in the medium, and R j τ is the distance between the j-th transducer and the positioning beacon 4. j.1 The time difference between the arrival of the positioning sound source signal at the reference transducer and the arrival at the j-th transducer;
[0058] The distance between the i-th transducer and the positioning beacon 4 and the distance difference between the positioning sound source and the j-th transducer and the reference transducer are used to form a system of equations. Solving the system of equations yields the sound source coordinates as (x2, y2, z2), where z2 is the depth of the storage tank.
[0059] Specifically, the external control device is used for:
[0060] The first and second positioning data are optimized using a complementary filtering formula to obtain the final positioning result. The complementary filtering formula is as follows:
[0061] x = μx1 + (1-μ)x2
[0062] y = νy1 + (1 - ν)y2,
[0063] Where (x,y,z) is the final positioning result, z coordinate is the tank depth, (x1,y1) is the x and y coordinates in the first positioning data, (x2,y2) is the x and y coordinates in the second positioning data, μ and ν are weighting coefficients, and 0 < μ,ν < 1.
[0064] Specifically, the external control device is also used for:
[0065] The final positioning result is sent to the ultra-short baseline positioning device 2 and the long baseline positioning device 3 for updating the positioning data.
[0066] In the above embodiments, the ultra-short baseline positioning device 2 and the long baseline positioning device 3 update the positioning data, enabling them to output more accurate and stable positioning data in the future.
[0067] The specific operation process is as follows:
[0068] Before operation, the submersible robot 5 has an ultra-short baseline transducer array and a long baseline transducer array installed at corresponding positions on its hull. The submersible robot 5 and its components communicate with the outside world via cables. The submersible robot 5 is lowered into the tank through the manhole 1 on the top of the tank. The ultra-short baseline transducer array is installed on the floating roof of the tank and interacts with the positioning beacon 4 via acoustic pulse signals (i.e., positioning sound source signals). The long baseline transducer array is installed on the outer wall of the tank. Due to the characteristics of long baseline positioning technology, the positions of its transducers need to be precisely determined to achieve consistency with the ultra-short baseline positioning coordinates. Long baseline positioning also uses acoustic pulse signals (i.e., positioning sound source signals) to interact with the positioning beacon 4.
[0069] The submersible robot 5 is lowered into the storage tank using a hook. After reaching the bottom of the tank, its initial position is recorded, thus calibrating the positioning system. During operation, the positioning system designed in this invention can update the robot's position information in real time and visualize its position and speed in the display and control section.
[0070] Example 2:
[0071] like Figure 3 As shown, a method for locating a submersible robot based on multi-source sonar positioning information fusion includes the following steps:
[0072] The positioning beacon transmits positioning sound source signals to the ultra-short baseline positioning device and the long baseline positioning device;
[0073] The ultra-short baseline positioning device performs positioning calculations on the submersible robot based on the positioning sound source signal to obtain the first positioning data;
[0074] The long baseline positioning device performs positioning calculations on the submersible robot based on the positioning sound source signal to obtain second positioning data;
[0075] The external control device uses a complementary filtering algorithm to optimize the first positioning data and the second positioning data to obtain the final positioning result.
[0076] Specifically, the ultra-short baseline positioning device 2 includes an ultra-short baseline transducer array and a first positioning device; the ultra-short baseline transducer array includes three ultra-short baseline transducers, which are installed in an isosceles right triangle at the top of the storage tank.
[0077] The first positioning device includes the following steps:
[0078] An xoy coordinate axis is established with the ultra-short baseline transducer A1 as the origin. Ultra-short baseline transducers A2 and A3 are located on the x-axis, forming an isosceles right triangle. The positioning beacon is located at point S with coordinates (x1, y1, z1), where z1 represents the tank depth, and S1 is the projection of S onto the xoy plane. Based on spatial geometry, the phase difference expression, the θ angle expression, and the horizontal slant range of the positioning beacon are obtained for the ultra-short baseline transducers receiving the positioning sound source signal. The phase difference expression is:
[0079]
[0080] in, The phase difference between the location sound source signals received by the ultra-short baseline transducer A1 and the ultra-short baseline transducer A2 is given. Let d be the phase difference between the location sound source signals received by the ultra-short baseline transducer A3 and the ultra-short baseline transducer A2, where d is the baseline length and d≤λ / 2, α is the angle between S and the x-axis, and β is the angle between S and the y-axis.
[0081] The expression for angle θ is:
[0082]
[0083] Where θ is the angle between S1 and the x-axis;
[0084] The horizontal slant distance of the positioning beacon is:
[0085]
[0086] The first positioning data of the positioning beacon is obtained based on the phase difference expression, the θ angle expression, and the horizontal slant distance of the positioning beacon. The first positioning data is:
[0087]
[0088] in, c is the speed of sound in oil, and Δt is the time difference between the transmission of the positioning sound source signal from the positioning beacon and the reception of the positioning sound source signal by the ultra-short baseline transducer.
[0089] Specifically, the long baseline positioning device includes a long baseline transducer array and a second positioning device; the long baseline transducer array includes four long baseline transducers; the four long baseline transducers are installed on the outer wall of the storage tank at intervals and around the perimeter.
[0090] The second positioning device includes the following steps:
[0091] Let (X) i ,Y i Z i Let be the coordinates of the i-th transducer, where i = 1, 2, 3, 4. Then the distance between the i-th transducer and the positioning beacon is:
[0092]
[0093] Using the first long-baseline transducer as the reference transducer, the distance difference between the positioning beacon and the j-th transducer and the distance to the reference transducer is:
[0094]
[0095] Where j = 2, 3, 4, v is the propagation speed of the localization sound source signal in the medium, and R j τ is the distance between the j-th transducer and the positioning beacon. j.1 The time difference between the arrival of the positioning sound source signal at the reference transducer and the arrival at the j-th transducer;
[0096] The distance between the i-th transducer and the positioning beacon, and the distance difference between the positioning sound source and the j-th transducer and the reference transducer are used to form a system of equations. Solving the system of equations yields the sound source coordinates as (x2, y2, z2), where z2 is the depth of the storage tank.
[0097] Specifically, it also includes the following steps:
[0098] The external control device sends the final positioning result to the ultra-short baseline positioning device and the long baseline positioning device for updating the positioning data.
[0099] like Figure 4 As shown, the following is a program example of a submersible robot localization method:
[0100] Program begins;
[0101] Ultra-short baseline positioning devices and long baseline positioning devices collect raw data (i.e., initial data);
[0102] Compensate the original data;
[0103] Perform a preliminary alignment operation, which includes:
[0104] The coarse alignment operation determines whether it is complete. If it is complete, fine alignment is performed. If the coarse alignment is not complete, the process returns and restarts.
[0105] Determine whether the fine alignment operation is complete. If yes, complete the initial alignment operation; otherwise, return and restart the program.
[0106] The process of performing integrated navigation after the initial alignment operation is completed is as follows:
[0107] Ultra-short baseline positioning and long baseline positioning are performed to obtain first positioning information and second positioning information. However, there are certain errors in the first positioning information and second positioning information. In order to eliminate the errors, the first positioning data and the second positioning data are optimized by the complementary filtering algorithm through the external control device (i.e., positioning error evaluation) to obtain the final positioning result, and the final positioning result (i.e., navigation information output) is output.
[0108] The final positioning result is fed back to the ultra-short baseline positioning device and the long baseline positioning device for positioning data update; the integrated navigation process ends.
[0109] The program has ended.
[0110] This invention installs an ultra-short baseline positioning device and a long baseline positioning device on the storage tank, and installs a positioning beacon on the body of the submersible robot to locate the submersible robot, obtaining different positioning data. The two positioning data are then optimized by a complementary filtering algorithm to eliminate errors, realizing the fusion of multi-source sonar positioning information, making the positioning results more accurate and reducing the impact of noise.
[0111] It should be noted that, in this document, relational terms such as "first" and "second" are used only to distinguish one entity or operation from another, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Furthermore, the terms "comprising," "including," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such process, method, article, or apparatus.
[0112] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the protection scope of the present invention.
Claims
1. A submersible robot positioning system based on multi-source sonar positioning information fusion, characterized in that, It includes an ultra-short baseline positioning device, a long baseline positioning device, a positioning beacon, and an external control device. The ultra-short baseline positioning device is installed on the top of the storage tank, the long baseline positioning device is installed on the outer wall of the storage tank, and the positioning beacon is installed on the body of the submersible robot. The positioning beacon is used to transmit positioning sound source signals to the ultra-short baseline positioning device and the long baseline positioning device; The ultra-short baseline positioning device is used to perform positioning calculations on the submersible robot based on the positioning sound source signal to obtain first positioning data. The long baseline positioning device is used to perform positioning calculations on the submersible robot based on the positioning sound source signal to obtain second positioning data. The external control device is used to optimize the first positioning data and the second positioning data using a complementary filtering algorithm to obtain the final positioning result; The external control device is specifically used for: The first and second positioning data are optimized using a complementary filtering formula to obtain the final positioning result. The complementary filtering formula is as follows: , in, For the final location result, The coordinates represent the depth of the storage tank. x 1, y 1) represents the x and y coordinates in the first positioning data. x 2, y 2) These are the x and y coordinates in the second positioning data. These are the weighting coefficients, and ; The external control device is also used for: The final positioning result is sent to the ultra-short baseline positioning device and the long baseline positioning device for updating the positioning data.
2. The submersible robot positioning system based on multi-source sonar positioning information fusion according to claim 1, characterized in that, The ultra-short baseline positioning device is also used to perform data compensation processing on the initial data before positioning calculation; The long baseline positioning device is also used to perform data compensation processing on the initial data before positioning calculation.
3. The submersible robot positioning system based on multi-source sonar positioning information fusion according to claim 1, characterized in that, The ultra-short baseline positioning device includes an ultra-short baseline transducer array and a first positioning device; the ultra-short baseline transducer array includes three ultra-short baseline transducers, which are installed in an isosceles right triangle at the top of the storage tank. The first positioning device is used for: An xoy coordinate axis is established with the ultra-short baseline transducer A1 as the origin. Ultra-short baseline transducer A2 is located on the x-axis, and ultra-short baseline transducer A3 is located on the y-axis, forming an isosceles right triangle. The positioning beacon is located at point S with coordinates... , The coordinates represent the tank depth, and S1 is the projection of S onto the xoy plane. Based on spatial geometry, the phase difference expression for the location sound source signal received by the ultra-short baseline transducer is obtained. The angular expression and the horizontal slant range of the positioning beacon, and the phase difference expression are: , in, The phase difference between the location sound source signals received by the ultra-short baseline transducer A1 and the ultra-short baseline transducer A2 is given. The phase difference between the location sound source signals received by ultra-short baseline transducers A3 and A2 is used to determine the phase difference between them. The baseline length, and , Let S be the angle between the x-axis and the x-axis. The angle between the S-axis and the y-axis; The The angle expression is: , in, Let S1 be the angle between S1 and the x-axis; The horizontal slant distance of the positioning beacon is: ; According to the phase difference expression, the The first positioning data of the positioning beacon is obtained by combining the angle expression and the horizontal slant distance of the positioning beacon. The first positioning data is: , in, c is the speed of sound in oil. The time difference between the transmission of the positioning sound source signal from the positioning beacon and the reception of the positioning sound source signal by the ultra-short baseline transducer.
4. The submersible robot positioning system based on multi-source sonar positioning information fusion according to claim 3, characterized in that, The long baseline positioning device includes a long baseline transducer array and a second positioning device; the long baseline transducer array includes four long baseline transducers; the four long baseline transducers are installed on the outer wall of the storage tank at intervals and around the perimeter. The second positioning device is used for: set up( , , ) is the first i Given the coordinates of the i-th transducer, where i = 1, 2, 3, 4, then the i-th... i The distance between each transducer and the positioning beacon is: , Using the first long-baseline transducer as the reference transducer, the positioning beacon is then directed to the first... j The distance difference between each transducer and the reference transducer is: , in, j =2,3,4 To locate the propagation speed of the sound source signal in the medium, For the first j The distance between each transducer and the positioning beacon. To locate the sound source, a positioning sound source signal is transmitted to the reference transducer and to the first... j Time difference between transducers; The first i The distance between the transducer and the positioning beacon, and the distance from the positioning sound source to the first... j The distance differences between each transducer and the reference transducer form a system of equations. Solving this system of equations yields the coordinates of the sound source. , The coordinates represent the depth of the storage tank.
5. A method for locating a submersible robot based on multi-source sonar positioning information fusion, applied to the submersible robot positioning system based on multi-source sonar positioning information fusion as described in any one of claims 1 to 4, characterized in that, Includes the following steps: The positioning beacon transmits positioning sound source signals to the ultra-short baseline positioning device and the long baseline positioning device; The ultra-short baseline positioning device performs positioning calculations on the submersible robot based on the positioning sound source signal to obtain the first positioning data; The long baseline positioning device performs positioning calculations on the submersible robot based on the positioning sound source signal to obtain second positioning data; The external control device uses a complementary filtering algorithm to optimize the first positioning data and the second positioning data to obtain the final positioning result.
6. The submersible robot localization method based on multi-source sonar localization information fusion according to claim 5, characterized in that, The ultra-short baseline positioning device includes an ultra-short baseline transducer array and a first positioning device; the ultra-short baseline transducer array includes three ultra-short baseline transducers, which are installed in an isosceles right triangle at the top of the storage tank. The first positioning device includes the following steps: An xoy coordinate axis is established with the ultra-short baseline transducer A1 as the origin. Ultra-short baseline transducer A2 is located on the x-axis, and ultra-short baseline transducer A3 is located on the y-axis, forming an isosceles right triangle. The positioning beacon is located at point S with coordinates... , The coordinates represent the tank depth, and S1 is the projection of S onto the xoy plane. Based on spatial geometry, the phase difference expression for the location sound source signal received by the ultra-short baseline transducer is obtained. The angular expression and the horizontal slant range of the positioning beacon, and the phase difference expression are: , in, The phase difference between the location sound source signals received by the ultra-short baseline transducer A1 and the ultra-short baseline transducer A2 is given. The phase difference between the location sound source signals received by ultra-short baseline transducers A3 and A2 is used to determine the phase difference between them. The baseline length, and , Let S be the angle between the x-axis and the x-axis. The angle between the S-axis and the y-axis; The The angle expression is: , in, Let S1 be the angle between S1 and the x-axis; The horizontal slant distance of the positioning beacon is: ; According to the phase difference expression, the The first positioning data of the positioning beacon is obtained by combining the angle expression and the horizontal slant distance of the positioning beacon. The first positioning data is: , in, c is the speed of sound in oil. The time difference between the transmission of the positioning sound source signal from the positioning beacon and the reception of the positioning sound source signal by the ultra-short baseline transducer.
7. The submersible robot localization method based on multi-source sonar localization information fusion according to claim 5, characterized in that, The long baseline positioning device includes a long baseline transducer array and a second positioning device; the long baseline transducer array includes four long baseline transducers; the four long baseline transducers are installed on the outer wall of the storage tank at intervals and around the perimeter. The second positioning device includes the following steps: set up( , , ) is the first i Given the coordinates of the i-th transducer, where i = 1, 2, 3, 4, then the i-th... i The distance between each transducer and the positioning beacon is: , Using the first long-baseline transducer as the reference transducer, the positioning beacon is then directed to the first... j The distance difference between each transducer and the reference transducer is: , in, j =2,3,4 To locate the propagation speed of the sound source signal in the medium, For the first j The distance between each transducer and the positioning beacon. To locate the sound source, a positioning sound source signal is transmitted to the reference transducer and to the first... j Time difference between transducers; The first i The distance between the transducer and the positioning beacon, and the distance from the positioning sound source to the first... j The distance differences between each transducer and the reference transducer form a system of equations. Solving this system of equations yields the coordinates of the sound source. , The coordinates represent the depth of the storage tank.
8. The submersible robot localization method based on multi-source sonar localization information fusion according to claim 5, characterized in that, It also includes the following steps: The external control device sends the final positioning result to the ultra-short baseline positioning device and the long baseline positioning device for updating the positioning data.
Citation Information
Patent Citations
Underwater robot positioning method based on sonar image
CN115272217A
Underwater acoustic positioning system and method
US20080037370A1
