A combined navigation and positioning system and method for underwater defect detection robots for dams

Abstract

The present application discloses a combined navigation and positioning system and method for a dam underwater defect detection robot, belonging to the field of underwater navigation and positioning technology. The system includes: an inertial navigation system, a Doppler log and a depth meter, which are respectively used to obtain navigation parameter information, bottom velocity information and carrier depth information of the detection robot; an ultra-short baseline positioning system and a global navigation satellite system, which are respectively used to obtain the relative distance information between the detection robot and a buoy or a shore base and the absolute position information of the buoy or the shore base; a Doppler log rotating device, which is used to control the rotation of the Doppler log; a dead reckoning system hierarchical fusion center, which is used to perform data integration processing and obtain corresponding navigation and positioning parameters; and a combined navigation system total fusion center, which is used to perform data fusion processing and obtain combined navigation parameter information of the detection robot. High-precision and long-duration navigation and positioning of the detection robot is achieved.

Description

Technical Field

[0001] The present application relates to a combined navigation and positioning system and method for a dam underwater defect detection robot, belonging to the technical field of underwater navigation and positioning. Background Art

[0002] With the widespread application of underwater robots in various civil and military fields, underwater unmanned inspection technology using underwater robots as carriers has gradually become one of the most effective methods in the field of dam inspection in addition to diver inspection. At present, underwater unmanned inspection technology mainly uses unmanned remote-controlled underwater robots (ROVs) as carriers, which scan the underwater dam surface by carrying relevant equipment to achieve underwater environment construction and dam defect detection. In this process, the first problem to be solved is the navigation and positioning of underwater robots.

[0003] Underwater navigation technology is one of the core and key technologies of underwater robots. Due to the complexity of the underwater environment, the limited information transmission method and transmission distance, etc., underwater navigation technology has always been a scientific research problem in the development of underwater robots. Compared with the open ocean environment, the narrower underwater environment and more complex hydraulic structures in the dam reservoir area undoubtedly increase the difficulty of underwater navigation and positioning in the dam reservoir area.

[0004] In the prior art, ROVs achieve accurate navigation and positioning underwater mainly by relying on inertial navigation systems, Doppler logs, ultra-short baseline positioning systems, depth gauges, global positioning systems and other equipment. In practical applications, on the one hand, in the navigation system with the common SINS / DVL combined navigation as the core, it is limited by the current environment that foreign countries restrict domestic research institutions from purchasing high-precision, large-range DVLs, and the domestic DVLs are difficult to meet the combined navigation and positioning of the detection robot and the underwater defect positioning accuracy due to their small applicable water depth range for the bottom tracking working mode, large blind area, and low measurement accuracy for the water tracking working mode. On the other hand, in addition to the vertical dam surface, there are still a large number of sloped dam surfaces at different angles. How to adapt to the dam surfaces with different inclination angles so that the DVL is always in a high-precision bottom tracking working mode is a difficult point in the research on high-precision navigation and positioning of the dam underwater defect detection robot. Summary of the invention

[0005] The purpose of this application is to provide a combined navigation and positioning system and method for a dam underwater defect detection robot, which has strong versatility and high navigation and positioning accuracy, effectively solving the shortcomings of domestic DVLs, such as a small applicable water depth range, a large blind area, and low measurement accuracy in the water tracking working mode, and realizing high-precision, long-flight navigation and positioning of the detection robot.

[0006] To achieve the above objectives, the first aspect of the present application provides a dam underwater defect detection robot combined navigation and positioning system, comprising:

[0007] An inertial navigation system is arranged on the detection robot and is used to obtain navigation parameter information of the detection robot, wherein the navigation parameter information includes position information, speed information and attitude angle information;

[0008] A Doppler speed meter is arranged on the detection robot and is used to obtain the bottom speed information of the detection robot;

[0009] A depth meter, arranged on the detection robot, for obtaining carrier depth information of the detection robot;

[0010] An ultra-short baseline positioning system, comprising an acoustic transducer and an acoustic transponder, wherein the acoustic transducer is arranged on a buoy or a shore base, and the acoustic transponder is arranged on the detection robot, and is used to obtain relative distance information between the detection robot and the buoy or the shore base;

[0011] Global Navigation Satellite System, deployed on buoys or shore bases, used to obtain the absolute position information of buoys or shore bases;

[0012] A Doppler speed meter rotating device, comprising a rotating mechanism bracket and an electric indexing plate, wherein the rotating mechanism bracket is used to fix the Doppler speed meter, and the electric indexing plate is used to control the rotating mechanism bracket to rotate the Doppler speed meter;

[0013] a dead reckoning system hierarchical fusion center, connected to the inertial navigation system, the Doppler log and the depth meter signals respectively, for performing data integration processing on the navigation parameter information, the bottom velocity information and the carrier depth information respectively and obtaining corresponding navigation positioning parameters;

[0014] The total fusion center of the combined navigation system is respectively connected to the dead reckoning system hierarchical fusion center, the ultra-short baseline positioning system and the global navigation satellite system signal, and is used to perform data fusion processing on each of the navigation positioning parameters, the relative distance information and the absolute position information to obtain the combined navigation parameter information of the detection robot.

[0015] In one embodiment, the inertial navigation system includes: a startup switching module, used to switch the startup mode of the inertial navigation system to a dock alignment startup mode or a sea alignment startup mode; wherein the dock alignment startup mode uses preset longitude and latitude information as initial information and obtains navigation parameter information of the detection robot; the sea alignment startup mode uses the longitude and latitude information of GPS positioning as initial information and obtains navigation parameter information of the detection robot.

[0016] In one embodiment, the Doppler log includes: a mode switching module, which is used to switch the working mode of the Doppler log to a bottom tracking working mode or a water tracking working mode.

[0017] The second aspect of the present application provides a combined navigation and positioning method for a dam underwater defect detection robot, based on the combined navigation and positioning system for a dam underwater defect detection robot as described in any embodiment of the first aspect of the present application, comprising:

[0018] S100 controls the detection robot to be in a stationary state in the water below the buoy or the shore base, and performs an alignment operation on the inertial navigation system;

[0019] S200 controls the Doppler log to rotate through the Doppler log rotating device according to the dam surface inclination angle information, so that the Doppler log is in a bottom tracking working mode, measures the bottom velocity information and performs error calibration on the Doppler log;

[0020] S300: After the error calibration is completed, the detection robot is controlled to drive to the dam surface area to be detected of the dam, and when the detection robot navigates in the dam surface area to be detected, the navigation parameter information of the detection robot is obtained in real time through the inertial navigation system, and the carrier depth information of the detection robot is obtained in real time through the depth gauge;

[0021] S400 acquires the absolute position information of the buoy or shore base through the global navigation satellite system, and acquires the relative distance information between the detection robot and the buoy or shore base in real time through the ultra-short baseline positioning system;

[0022] S500 performs data fusion processing on the bottom velocity information, the absolute position information, the relative distance information, the navigation parameter information and the carrier depth information based on a multi-scale federal Kalman filtering method to obtain combined navigation parameter information of the detection robot.

[0023] In one implementation, in step S100, the aligning operation on the inertial navigation system includes:

[0024] Establishing a first coordinate system with the center of the buoy or shore base as the origin, and obtaining the coordinates of the origin of the first coordinate system;

[0025] A second coordinate system is established with the buoyancy center of the detection robot as the origin, and the origin coordinates of the second coordinate system are obtained according to the relative distance information obtained by the ultra-short baseline positioning system and the navigation parameter information obtained by the inertial navigation system;

[0026] An alignment operation is performed according to the origin coordinates of the first coordinate system and the origin coordinates of the second coordinate system.

[0027] In one embodiment, in step S200, controlling the Doppler odometer to rotate by the Doppler odometer rotating device specifically includes:

[0028] Establishing a third coordinate system with the center of the Doppler log rotating device as the origin;

[0029] According to the third coordinate system and the dam surface inclination angle information, the Doppler odometer is controlled by the Doppler odometer rotating device to rotate the Doppler odometer in horizontal and vertical directions.

[0030] In one implementation, in step S500, the data fusion processing of the bottom velocity information, the absolute position information, the relative distance information, the navigation parameter information and the carrier depth information includes:

[0031] Obtaining the position coordinates of the detection robot according to the absolute position information and the relative distance information;

[0032] The position coordinates, the navigation parameter information, the bottom speed information and the carrier depth information are further fused to obtain the combined navigation parameter information of the detection robot.

[0033] In one embodiment, before step S500, the method further includes: constructing system state equations corresponding to the Doppler odometer, the inertial navigation system, the global navigation satellite system, the ultra-short baseline positioning system, and the depth meter, respectively, and establishing corresponding measurement equations according to each of the system state equations;

[0034] Step S500 specifically includes, according to the Kalman filtering theory, performing Kalman filtering on the bottom velocity information, the absolute position information, the relative distance information, the navigation parameter information and the carrier depth information through each of the state joint equations and each of the measurement equations to obtain corresponding navigation and positioning parameters, performing multi-scale decomposition on each of the navigation and positioning parameters, extracting effective information at different scales and fusing them to obtain the combined navigation parameter information of the detection robot.

[0035] The third aspect of the present application provides an electronic device, comprising: a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor implements the steps of the first aspect or any one of the embodiments of the first aspect when executing the computer program.

[0036] A fourth aspect of the present application provides a computer-readable storage medium, wherein the computer-readable storage medium stores a computer program, and when the computer program is executed by a processor, the steps in the first aspect or any implementation of the first aspect are implemented.

[0037] As can be seen from the above, the present application provides a combined navigation and positioning system and method for a dam underwater defect detection robot, which controls the Doppler odometer to rotate through a Doppler odometer rotating device to realize its bottom tracking working mode, effectively solving the shortcomings of the domestic DVL, such as a small applicable water depth range, a large blind area, and low measurement accuracy in the water tracking working mode. In addition, the absolute position information of the global navigation satellite system is combined with the relative distance information between the detection robot and the buoy or shore base measured by the ultra-short baseline positioning system, and is further integrated with the navigation and positioning parameters output by the inertial navigation system, Doppler odometer and depth meter combination, to realize high-precision and long-flight navigation and positioning of the detection robot, and meet the high-precision navigation and positioning requirements of the dam underwater defect detection robot. BRIEF DESCRIPTION OF THE DRAWINGS

[0038] In order to more clearly illustrate the technical solutions in the embodiments of the present application, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present application. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying any creative work.

[0039] Figure 1 A schematic diagram of the structure of a combined navigation and positioning system for underwater defect detection robots for dams provided in an embodiment of the present application;

[0040] Figure 2 A data structure diagram of a dam underwater defect detection robot combined navigation and positioning system provided in an embodiment of the present application;

[0041] Figure 3 A flowchart of a combined navigation and positioning method for a dam underwater defect detection robot provided in an embodiment of the present application. DETAILED DESCRIPTION

[0042] In the following description, specific details such as specific system structures, technologies, etc. are provided for the purpose of illustration rather than limitation, so as to provide a thorough understanding of the embodiments of the present application. However, it should be clear to those skilled in the art that the present application may also be implemented in other embodiments without these specific details. In other cases, detailed descriptions of well-known systems, devices, circuits, and methods are omitted to prevent unnecessary details from obstructing the description of the present application.

[0043] It should be understood that when used in this specification and the appended claims, the term "comprising" indicates the presence of described features, integers, steps, operations, elements and / or components, but does not exclude the presence or addition of one or more other features, integers, steps, operations, elements, components and / or groups thereof.

[0044] It should also be understood that the terms used in this application specification are only for the purpose of describing specific embodiments and are not intended to limit the application. As used in this application specification and the appended claims, the singular forms "a", "an" and "the" are intended to include plural forms unless the context clearly indicates otherwise.

[0045] The following is a clear and complete description of the technical solutions in the embodiments of the present application in conjunction with the drawings in the embodiments of the present application. Obviously, the described embodiments are only part of the embodiments of the present application, not all of the embodiments. Based on the embodiments in the present application, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of this application.

[0046] In the following description, many specific details are set forth to facilitate a full understanding of the present application, but the present application may also be implemented in other ways different from those described herein, and those skilled in the art may make similar generalizations without violating the connotation of the present application. Therefore, the present application is not limited to the specific embodiments disclosed below.

[0047] Embodiment 1

[0048] The present application embodiment provides a dam underwater defect detection robot combined navigation and positioning system, such as Figure 1 As shown, the system includes: inertial navigation system, Doppler log, depth meter, ultra-short baseline positioning system, global navigation satellite system, Doppler log rotating device, buoy, dead reckoning system hierarchical fusion center, integrated navigation system total fusion center;

[0049] Wherein, the inertial navigation system is arranged on the detection robot and is used to obtain navigation parameter information of the detection robot, and the navigation parameter information includes position information, speed information and attitude angle information;

[0050] A Doppler velocity log (DVL), arranged on the detection robot, for obtaining bottom velocity information of the detection robot;

[0051] A depth meter, arranged on the detection robot, for obtaining carrier depth information of the detection robot;

[0052] An ultra-short baseline positioning system (USBL), comprising an acoustic transducer and an acoustic transponder, wherein the acoustic transducer is arranged on a buoy or a shore base, and the acoustic transponder is arranged on the detection robot, and is used to obtain relative distance information between the detection robot and the buoy or the shore base;

[0053] Global Navigation Satellite System (GNSS), deployed on buoys or shore bases, used to obtain the absolute position information of the buoys or shore bases;

[0054] A Doppler speed meter rotating device, comprising a rotating mechanism bracket and an electric indexing plate, wherein the rotating mechanism bracket is used to fix the Doppler speed meter, and the electric indexing plate is used to control the rotating mechanism bracket to rotate the Doppler speed meter;

[0055] a dead reckoning system hierarchical fusion center, connected to the inertial navigation system, the Doppler log and the depth meter signals respectively, for performing data integration processing on the navigation parameter information, the bottom velocity information and the carrier depth information respectively and obtaining corresponding navigation positioning parameters;

[0056] The total fusion center of the combined navigation system is respectively connected to the dead reckoning system hierarchical fusion center, the ultra-short baseline positioning system and the global navigation satellite system signal, and is used to perform data fusion processing on each of the navigation positioning parameters, the relative distance information and the absolute position information to obtain the combined navigation parameter information of the detection robot.

[0057] Optionally, the inertial navigation system includes: a startup switching module, used to switch the startup mode of the inertial navigation system to a dock alignment startup mode or a sea alignment startup mode; wherein the dock alignment startup mode uses preset longitude and latitude information as initial information and obtains navigation parameter information of the detection robot; the sea alignment startup mode uses GPS-positioned longitude and latitude information as initial information and obtains navigation parameter information of the detection robot. The GPS-positioned longitude and latitude information can be obtained through the global navigation satellite system or in other ways, which is not limited here.

[0058] In one embodiment, the inertial navigation system is a strapdown inertial navigation system (SINS), which can adopt the dock alignment start mode or the offshore alignment start mode according to the actual needs of the application scenario. Because of its autonomy, it serves as the central navigation system of the detection robot and provides comprehensive navigation parameter information, mainly including position (longitude, latitude, depth), speed (longitudinal speed, lateral speed, vertical speed), attitude angle (heading angle, pitch angle, roll angle), depth and other information.

[0059] Optionally, the Doppler log includes: a mode switching module, used to switch the working mode of the Doppler log to a bottom tracking working mode or a water tracking working mode.

[0060] In one embodiment, DVL is an underwater acoustic autonomous navigation sensor based on the Doppler effect. Depending on the actual needs of the application scenario, its working mode can adopt a bottom tracking working mode or a water tracking working mode. It will provide the detection robot's bottom speed (longitudinal speed, lateral speed, vertical speed) information. Through this low-speed information, the arm error and proportional error of DVL relative to the SINS center can be eliminated.

[0061] In one embodiment, the depth meter is an underwater pressure sensor, which provides carrier depth information based on pressure-water depth data conversion.

[0062] In one embodiment, the USBL working principle is to use phase difference or phase comparison method to measure the phase difference between the acoustic array units, and measure the propagation time of the sound wave in the water, calculate the relative distance between the acoustic array and the detection robot to determine the relative distance between the detection robot and the buoy or shore base.

[0063] In one embodiment, GNSS is a global positioning system (GPS), including a GPS antenna and a signal receiver. It has the characteristics of all-weather, wide coverage, and high accuracy, and will provide absolute position information of the GPS antenna. However, since radio cannot be transmitted in water, GPS mainly exists in the form of auxiliary navigation to eliminate the accumulated error of inertial navigation drift of the detection robot.

[0064] In one embodiment, the Doppler odometer rotating device is a mechanical structure mainly equipped with a rotating mechanism bracket and an electric dividing plate, which can rotate the DVL with two degrees of freedom so that the DVL is always in a bottom tracking working mode and ensures that the DVL speed measurement accuracy is not affected by the inclination angle of the dam surface.

[0065] In one embodiment, the buoy is a buoyancy device carrying equipment, and the equipment carried is mainly a GPS antenna and a signal receiver and a USBL acoustic transducer. When the GPS antenna and the signal receiver and the USBL acoustic transducer are arranged on the buoy, the GPS antenna and the signal receiver will provide the absolute position information of the buoy (including longitude and latitude), and the USBL acoustic transducer cooperates with the acoustic transponder carried on the detection robot to provide the relative distance information between the detection robot and the buoy. In practical applications, the relative distance information of the USBL array can be corrected by the absolute position information of the GPS, and the two can be integrated and unified into the same coordinate system to obtain the absolute position information of the detection robot, and then output to the total fusion center of the combined navigation system through the umbilical cable and the acoustic beacon, or the absolute position information and the relative distance information can be directly output to the total fusion center of the combined navigation system through the umbilical cable and the acoustic beacon, and then the data is integrated through the total fusion center of the combined navigation system to obtain the absolute position information of the detection robot, which is not limited here.

[0066] In one embodiment, if Figure 1 and 2 As shown, the dead reckoning system hierarchical fusion center is the first-level fusion information center of the integrated navigation system (i.e., the dam underwater defect detection robot integrated navigation and positioning system of the embodiment of the present application), which uses the SINS position and attitude angle information ( Figure 2 speed 1, position 1, attitude 1, and depth 1), DVL speed information ( Figure 2 Medium speed 2) and depth gauge depth information ( Figure 2 The medium depth 2) is used as input for data integration processing, and outliers are eliminated and tracks are extracted according to the outlier processing algorithm. Then the data enters the navigation data filter with the Kalman filter as the core for further data processing, and various navigation and positioning parameters are output to the overall fusion center of the integrated navigation system.

[0067] In one implementation, the total fusion center of the integrated navigation system is the secondary fusion information center of the integrated navigation system. In view of the multi-scale characteristics of the measurement information caused by the different sampling frequencies and resolutions of the sensor data in the integrated navigation system, the multi-scale analysis based on wavelet theory is performed on the measurement signals of each sensor with multi-scale characteristics. The uncertainty of the observation data under the dam environment, data distortion, the relationship between the filtering algorithm and the parameters and scales are fully considered. The corresponding data or signals are decomposed at multiple scales, and effective information is extracted at different scales and then fused. Figure 1 and 2 As shown, the absolute position information of the detection robot is obtained according to the absolute position information of the buoy and the relative position distance information between the detection robot and the buoy ( Figure 2 The attitude 1, speed 1 and speed 2, position 1 and position 2, depth 1 and depth 2 are decomposed at multiple scales, and effective information is extracted at different scales and fused through a navigation data filter with the Kalman filter as the core to obtain the final combined navigation parameter information, including more accurate navigation position (longitude, latitude, depth), speed (longitudinal speed, lateral speed, vertical speed), attitude angle (heading angle, pitch angle, roll angle), depth and other parameter information.

[0068] In one embodiment, the combined navigation system can be connected to the shore-based system, control and operation system signals respectively to achieve information interaction between systems, and organize the output of corresponding combined navigation parameters according to actual detection conditions and the needs of other systems so that other systems can perform corresponding underwater operations accordingly.

[0069] As can be seen from the above, the embodiment of the present application provides a combined navigation and positioning system for a dam underwater defect detection robot, which controls the Doppler odometer to rotate through a Doppler odometer rotating device to realize its bottom tracking working mode, effectively solving the shortcomings of the domestic DVL, such as a small applicable water depth range, a large blind area, and low measurement accuracy in the water tracking working mode. In addition, the absolute position information of the global navigation satellite system is combined with the relative distance information between the detection robot and the buoy or shore base measured by the ultra-short baseline positioning system, and is further integrated with the various navigation and positioning parameters output by the inertial navigation system, Doppler odometer and depth meter combination, to realize high-precision and long-flight navigation and positioning of the detection robot, and meet the high-precision navigation and positioning requirements of the dam underwater defect detection robot.

[0070] Embodiment 2

[0071] The present application embodiment provides a combined navigation and positioning method for a dam underwater defect detection robot, based on the combined navigation and positioning system for a dam underwater defect detection robot as described in any implementation manner of the first embodiment, the method comprising:

[0072] S100 controls the detection robot to be in a stationary state in the water below the buoy or the shore base, and performs an alignment operation on the inertial navigation system;

[0073] In one embodiment, the specific process of the combined navigation and positioning method of the dam underwater defect detection robot is as follows: Figure 3 As shown, after the detection robot is launched into the water, it is fixed under the buoy. The buoy and the robot are stationary in the water. The SINS working mode is selected and waits for 30 minutes until the inertial navigation alignment is completed.

[0074] Optionally, in step S100, the alignment operation of the inertial navigation system includes: establishing a first coordinate system with the center of the buoy or shore base as the origin, and obtaining the origin coordinates of the first coordinate system; establishing a second coordinate system with the buoyancy center of the detection robot as the origin, and obtaining the origin coordinates of the second coordinate system according to the relative distance information obtained by the ultra-short baseline positioning system and the navigation parameter information obtained by the inertial navigation system; and performing an alignment operation according to the origin coordinates of the first coordinate system and the origin coordinates of the second coordinate system.

[0075] In one embodiment, the first coordinate system n 0 The origin is the center of the buoy, and the coordinates of the origin are East is x n Axis, north is y n Axis, celestial direction is z n The navigation coordinate system of the axis, where λ is the latitude, φ is the longitude, and τ is the depth.

[0076] If the SINS working mode is selected as the dock alignment start mode, the second coordinate system n 1 Taking the center position of SINS as the initial coordinate origin, the initial origin coordinates are determined according to the preset longitude and latitude information: East is x n Axis, north is y n Axis, celestial direction is z n Axis. Or the second coordinate system n 1 To detect the robot's center of buoyancy O b is the initial coordinate origin, and the horizontal axis of the robot is detected to point to the right as x b Axis, the vertical axis points forward as y b Axis, vertical axis pointing upward is z b If the SINS working mode is selected as the offshore alignment start mode, the second coordinate system n 1 The SINS center position is regarded as the initial coordinate origin, and the latitude and longitude information output by GPS is used as the initial information origin coordinates. East is x n Axis, north is y n Axis, celestial direction is z n Axis. Or the second coordinate system n 1 To detect the robot's center of buoyancy O b is the initial coordinate origin, and the horizontal axis of the robot is detected to point to the right as x b Axis, the vertical axis points forward as y b Axis, vertical axis pointing upward is z b In the alignment phase, the detection robot is placed under the buoy, and the distance between the center of the detection robot and the center of the buoy is That is, the USBL measurement value at this time The attitude angle measured by SINS is (α 0 ,β 0 ,γ 0 ), then the initial position coordinates of the detection robot are The initial speed is The initial attitude angle is Where α is the pitch angle, β is the roll angle, and γ is the heading angle.

[0077] S200: After the inertial navigation is aligned, the dam surface inclination angle information of the dam can be obtained according to the known dam surface drawing, and the Doppler speed meter is controlled to rotate by the Doppler speed meter rotating device so that the Doppler speed meter is in a bottom tracking working mode, and the bottom speed information is measured and the error of the Doppler speed meter is calibrated;

[0078] Optionally, in step S200, controlling the Doppler speed meter to rotate by the Doppler speed meter rotating device specifically includes: establishing a third coordinate system with the center of the Doppler speed meter rotating device as the origin; and controlling the Doppler speed meter to rotate horizontally and vertically by the Doppler speed meter rotating device according to the third coordinate system and the dam surface inclination angle information.

[0079] In one embodiment, the third coordinate system R 0 The horizontal axis of the Doppler speedometer rotating device points to the right as x R Axis, the vertical axis points forward as y R Axis, vertical axis pointing upward is z R According to the known dam surface drawings, the dam surface inclination angle information is obtained to control the rotating mechanism to rotate the DVL in two degrees of freedom. First, the horizontal azimuth electric indexing plate (i.e., around the z axis) is controlled. R The DVL array heading angle is rotated to the direction of the dam surface for detection. Then the vertical azimuth electric indexing plate (i.e., around the x axis) is controlled. R The DVL array is rotated to be parallel to the detection dam surface by the longitudinal inclination angle (axis). The horizontal and vertical rotation angles are θ 1 and θ 2 , then the transformation matrices are

[0080]

[0081] S300: After the error calibration is completed, the detection robot is controlled to drive to the dam surface area to be detected of the dam, and when the detection robot navigates in the dam surface area to be detected, the navigation parameter information of the detection robot is obtained in real time through the inertial navigation system, and the carrier depth information of the detection robot is obtained in real time through the depth gauge;

[0082] S400 obtains the absolute position information of the buoy or shore base through the global navigation satellite system according to the actual detection status of the detection robot The relative distance information between the detection robot and the buoy or shore base is obtained in real time through the ultra-short baseline positioning system.

[0083] In one embodiment, according to the Kalman filtering theory, it is necessary to establish a system mathematical model, that is, a system state equation and a measurement equation to perform the Kalman filtering process. SINS is used as the reference public navigation system in the integrated navigation system, so the embodiment of the present application uses its state quantity as the system state quantity, and its state equation as the system state equation, and constructs the federal filtering system equation based on the navigation parameter information output by SINS. Specifically, the system state equations corresponding to the Doppler odometer, the inertial navigation system, the global navigation satellite system, the ultra-short baseline positioning system and the depth meter are constructed respectively. Among them, the SINS state equation can be determined by the error model of SINS as:

[0084]

[0085] In the formula, the state quantity The 18 parameters represent latitude change, longitude change, depth change, east speed, north speed, celestial speed, pitch angle, roll angle, heading angle, gyro drift random constant error x-axis component, gyro drift random constant error y-axis component, gyro drift random constant error z-axis component, gyro drift Markov process error x-axis component, gyro drift Markov process error y-axis component, gyro drift Markov process error z-axis component, accelerometer error x-axis component, accelerometer error y-axis component, accelerometer error z-axis component; F sins is the SINS state transfer matrix, W sins is the noise vector of the SINS process.

[0086] Similarly, the state equations of other navigation equipment systems can be obtained:

[0087] DVL state equation:

[0088] GPS state equation:

[0089] USBL state equation:

[0090] According to the above system state equations, the federal filter system equation can be obtained:

[0091]

[0092] Further, the corresponding measurement equations are established according to the state equations of each system, such as position measurement equations, velocity measurement equations, attitude measurement equations, depth measurement equations, etc., which usually take the difference between the observed value and the last observed value as the observed value. Taking the position parameter information as an example, the position measurement equation is:

[0093]

[0094] In the formula, ζ 1 , 2 , 3 are the observed noise in latitude, longitude and depth directions respectively.

[0095] S500 performs data fusion processing on the bottom velocity information, the absolute position information, the relative distance information, the navigation parameter information and the carrier depth information based on a multi-scale federal Kalman filtering method to obtain combined navigation parameter information of the detection robot.

[0096] Optionally, in step S500, the multi-scale federal Kalman filtering method includes: according to Kalman filtering theory, Kalman filtering is performed on the bottom velocity information, the absolute position information, the relative distance information, the navigation parameter information and the carrier depth information through each of the state joint equations and each of the measurement equations to obtain corresponding navigation and positioning parameters, multi-scale decomposition is performed on each of the navigation and positioning parameters, and effective information is extracted at different scales and then fused to obtain the combined navigation parameter information of the detection robot.

[0097] In one embodiment, according to the theory of federated Kalman filtering, it mainly includes the following processes: information allocation, time update, measurement update and global information fusion. Among them, information allocation: this process is implemented between each sub-wave filter and the main filter, and the state estimate of the sub-filter, the covariance of the state estimate and the process information are allocated according to the allocation principle; time update: the state estimate of the subsystem and the covariance of the state estimate are transferred according to the state transfer matrix of the system to complete the time update process of the Kalman filter. In this process, each sub-filter and the main filter are performed independently; measurement update: according to the measurement information of the navigation subsystem, each sub-filter completes the correction of the state estimate value and the estimated covariance obtained by the time update.

[0098] Optionally, in step S500, the data fusion processing of the bottom velocity information, the absolute position information, the relative distance information, the navigation parameter information and the carrier depth information includes: obtaining the position coordinates of the detection robot according to the absolute position information and the relative distance information in step S400; performing multi-scale federal Kalman filtering data fusion according to the position coordinates and the bottom velocity information in step S200 to obtain the position parameter information of the detection robot; and outputting more accurate combined navigation parameter information after fusing the position parameter information, navigation parameter information, carrier depth information and other sensor information through multi-scale federal Kalman filtering, including navigation position (longitude, latitude, depth), speed (longitudinal speed, lateral speed, vertical speed), attitude angle (heading angle, pitch angle, roll angle) and other parameter information.

[0099] In one implementation, taking the data fusion of the position parameter information and navigation parameter information of the detection robot as an example, the specific process is as follows: The initial position coordinates of the detection robot are The initial speed is The initial attitude angle is According to the absolute position coordinates of the buoy And the relative distance coordinates between the detection robot and the buoy are measured as The current position coordinates of the detection robot can be obtained Then output information with SINS Further fusion processing is performed to output the detection robot navigation position parameters Output speed parameters at the same time Attitude angle parameters After further fusion with other sensor information data in the same way, more accurate combined navigation parameter information can be obtained.

[0100] As can be seen from the above, the embodiment of the present application provides a combined navigation and positioning method for a dam underwater defect detection robot, which has strong versatility and high navigation and positioning accuracy, so that the Doppler odometer is in the bottom tracking working mode, effectively solving the shortcomings of the domestic DVL, such as the small applicable water depth range, large blind area range, and low measurement accuracy in the water tracking working mode. In addition, the absolute position information of the global navigation satellite system is combined with the relative distance information between the detection robot and the buoy or shore base measured by the ultra-short baseline positioning system, and is further integrated with the navigation and positioning parameters output by the inertial navigation system, Doppler odometer and depth meter combination, to achieve high-precision and long-flight navigation and positioning of the detection robot, and meet the high-precision navigation and positioning requirements of the dam underwater defect detection robot.

[0101] Embodiment 3

[0102] An embodiment of the present application provides an electronic device, which includes a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the memory is used to store software programs and modules, and the processor executes various functional applications and data processing by running the software programs and modules stored in the memory. The memory and the processor are connected via a bus. Specifically, the processor implements any step in the first embodiment above by running the computer program stored in the memory.

[0103] It should be understood that in the embodiments of the present application, the processor referred to may be a central processing unit (CPU), and the processor may also be other general-purpose processors, digital signal processors (DSP), application-specific integrated circuits (ASIC), field-programmable gate arrays (FPGA) or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. A general-purpose processor may be a microprocessor or the processor may also be any conventional processor, etc.

[0104] The memory may include a read-only memory, a flash memory, and a random access memory, and provides instructions and data to the processor. A part or all of the memory may also include a nonvolatile random access memory.

[0105] It should be understood that the above-mentioned integrated module / unit, if implemented in the form of a software functional unit and sold or used as an independent product, can be stored in a computer-readable storage medium. Based on such an understanding, the present application implements all or part of the processes in the above-mentioned embodiment method, and can also be completed by instructing the relevant hardware through a computer program. The above-mentioned computer program can be stored in a computer-readable storage medium, and the computer program can implement the steps of the above-mentioned various method embodiments when executed by the processor. Among them, the above-mentioned computer program includes computer program code, and the above-mentioned computer program code can be in source code form, object code form, executable file or some intermediate form. The above-mentioned computer-readable medium may include: any entity or device capable of carrying the above-mentioned computer program code, recording medium, U disk, mobile hard disk, disk, optical disk, computer memory, read-only memory (ROM, Read-Only Memory), random access memory (RAM, Random Access Memory), electric carrier signal, telecommunication signal and software distribution medium. It should be noted that the content contained in the above-mentioned computer-readable storage medium can be appropriately increased or decreased according to the requirements of legislation and patent practice in the jurisdiction.

[0106] The technicians in the relevant field can clearly understand that for the convenience and simplicity of description, only the division of the above-mentioned functional units and modules is used as an example for illustration. In practical applications, the above-mentioned function allocation can be completed by different functional units and modules as needed, that is, the internal structure of the above-mentioned device can be divided into different functional units or modules to complete all or part of the functions described above. The functional units and modules in the embodiment can be integrated in a processing unit, or each unit can exist physically separately, or two or more units can be integrated in one unit. The above-mentioned integrated unit can be implemented in the form of hardware or in the form of software functional units. In addition, the specific names of the functional units and modules are only for the convenience of distinguishing each other, and are not used to limit the scope of protection of this application. The specific working process of the units and modules in the above-mentioned system can refer to the corresponding process in the aforementioned method embodiment, which will not be repeated here.

[0107] It should be noted that the methods and detailed examples provided in the above embodiments can be combined with the devices and equipment provided in the embodiments, and references can be made to each other, and no further details will be given.

[0108] Those of ordinary skill in the art will appreciate that the units and algorithm steps of each example described in conjunction with the embodiments disclosed herein can be implemented in electronic hardware, or a combination of computer software and electronic hardware. Whether these functions are performed in hardware or software depends on the specific application and design constraints of the technical solution. Professional and technical personnel can use different methods to implement the described functions for each specific application, but such implementation should not be considered to be beyond the scope of this application.

[0109] In the embodiments provided in the present application, it should be understood that the disclosed apparatus / terminal equipment and method can be implemented in other ways. For example, the apparatus / equipment embodiments described above are only schematic, for example, the division of the above modules or units is only a logical function division, and in actual implementation, other division methods can be used, for example, multiple units or components can be combined or integrated into another system, or some features can be ignored or not executed.

[0110] The above embodiments are only used to illustrate the technical solutions of the present application, rather than to limit them. Although the present application has been described in detail with reference to the above embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the above embodiments, or make equivalent replacements for some of the technical features therein. These modifications or replacements do not deviate the essence of the corresponding technical solutions from the spirit and scope of the technical solutions of the embodiments of the present application, and should all be included in the protection scope of the present application.

Claims

1. A combined navigation and positioning system for underwater defect detection robots in dams. It is characterized in that include: An inertial navigation system is arranged on the detection robot and is used to obtain navigation parameter information of the detection robot, wherein the navigation parameter information includes position information, speed information and attitude angle information; A Doppler speed meter is arranged on the detection robot and is used to obtain the bottom speed information of the detection robot; A depth meter, arranged on the detection robot, for obtaining carrier depth information of the detection robot; An ultra-short baseline positioning system, comprising an acoustic transducer and an acoustic transponder, wherein the acoustic transducer is arranged on a buoy or a shore base, and the acoustic transponder is arranged on the detection robot, and is used to obtain relative distance information between the detection robot and the buoy or the shore base; Global Navigation Satellite System, deployed on buoys or shore bases, used to obtain the absolute position information of buoys or shore bases; A Doppler speed meter rotating device, comprising a rotating mechanism bracket and an electric indexing plate, wherein the rotating mechanism bracket is used to fix the Doppler speed meter, and the electric indexing plate is used to control the rotating mechanism bracket to rotate the Doppler speed meter; a dead reckoning system hierarchical fusion center, connected to the inertial navigation system, the Doppler log and the depth meter signals respectively, for performing data integration processing on the navigation parameter information, the bottom velocity information and the carrier depth information respectively and obtaining corresponding navigation positioning parameters; The total fusion center of the combined navigation system is respectively connected to the dead reckoning system hierarchical fusion center, the ultra-short baseline positioning system and the global navigation satellite system signal, and is used to perform data fusion processing on each of the navigation positioning parameters, the relative distance information and the absolute position information to obtain the combined navigation parameter information of the detection robot.

2. The dam underwater defect detection robot combined navigation and positioning system according to claim 1, It is characterized in that The inertial navigation system comprises: a startup switching module, used to switch the startup mode of the inertial navigation system to a dock alignment startup mode or a sea alignment startup mode; Wherein, the dock alignment start mode uses preset longitude and latitude information as initial information and obtains navigation parameter information of the detection robot; The offshore alignment start mode uses the latitude and longitude information of GPS positioning as initial information and obtains the navigation parameter information of the detection robot.

3. The dam underwater defect detection robot combined navigation and positioning system according to claim 1 or 2, It is characterized in that The Doppler log comprises: a mode switching module, which is used to switch the working mode of the Doppler log to a bottom tracking working mode or a water tracking working mode.

4. A combined navigation and positioning method for underwater defect detection robots for dams, based on the combined navigation and positioning system for underwater defect detection robots for dams as claimed in any one of claims 1 to 3, It is characterized in that include: S100 controls the detection robot to be in a stationary state in the water below the buoy or the shore base, and performs an alignment operation on the inertial navigation system; S200 controls the Doppler log to rotate through the Doppler log rotating device according to the dam surface inclination angle information, so that the Doppler log is in a bottom tracking working mode, measures the bottom velocity information and performs error calibration on the Doppler log; S300: After the error calibration is completed, the detection robot is controlled to drive to the dam surface area to be detected of the dam, and when the detection robot navigates in the dam surface area to be detected, the navigation parameter information of the detection robot is obtained in real time through the inertial navigation system, and the carrier depth information of the detection robot is obtained in real time through the depth gauge; S400 acquires the absolute position information of the buoy or shore base through the global navigation satellite system, and acquires the relative distance information between the detection robot and the buoy or shore base in real time through the ultra-short baseline positioning system; S500 performs data fusion processing on the bottom velocity information, the absolute position information, the relative distance information, the navigation parameter information and the carrier depth information based on a multi-scale federal Kalman filtering method to obtain combined navigation parameter information of the detection robot.

5. The combined navigation and positioning method of the dam underwater defect detection robot according to claim 4, It is characterized in that In step S100, the aligning operation of the inertial navigation system includes: Establishing a first coordinate system with the center of the buoy or shore base as the origin, and obtaining the coordinates of the origin of the first coordinate system; A second coordinate system is established with the buoyancy center of the detection robot as the origin, and the origin coordinates of the second coordinate system are obtained according to the relative distance information obtained by the ultra-short baseline positioning system and the navigation parameter information obtained by the inertial navigation system; An alignment operation is performed according to the origin coordinates of the first coordinate system and the origin coordinates of the second coordinate system.

6. The combined navigation and positioning method of the dam underwater defect detection robot according to claim 4, It is characterized in that In step S200, controlling the Doppler odometer to rotate by the Doppler odometer rotating device specifically includes: Establishing a third coordinate system with the center of the Doppler log rotating device as the origin; According to the third coordinate system and the dam surface inclination angle information, the Doppler odometer is controlled by the Doppler odometer rotating device to rotate the Doppler odometer in horizontal and vertical directions.

7. The combined navigation and positioning method of the dam underwater defect detection robot according to claim 4, It is characterized in that In step S500, the data fusion processing of the bottom velocity information, the absolute position information, the relative distance information, the navigation parameter information and the carrier depth information includes: Obtaining the position coordinates of the detection robot according to the absolute position information and the relative distance information; The position coordinates, the navigation parameter information, the bottom speed information and the carrier depth information are further fused to obtain the combined navigation parameter information of the detection robot.

8. The combined navigation and positioning method for underwater defect detection robots for dams according to claim 4, It is characterized in that Before step S500, the following steps are also included: Respectively constructing system state equations corresponding to the Doppler odometer, the inertial navigation system, the global navigation satellite system, the ultra-short baseline positioning system and the depth gauge, and respectively establishing corresponding measurement equations according to each of the system state equations; Step S500 specifically includes, according to Kalman filtering theory, performing Kalman filtering on the bottom velocity information, the absolute position information, the relative distance information, the navigation parameter information and the carrier depth information through each of the system state equations and each of the measurement equations to obtain corresponding navigation and positioning parameters, performing multi-scale decomposition on each of the navigation and positioning parameters, extracting effective information at different scales and fusing them to obtain the combined navigation parameter information of the detection robot.

9. An electronic device, include: A memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor implements the steps of the method according to any one of claims 4 to 8 when executing the computer program.

10. A computer-readable storage medium storing a computer program. It is characterized in that When the computer program is executed by a processor, the steps of the method according to any one of claims 4 to 8 are implemented.

Images