A speed measurement method for dam inspection robot and DVL rotation mechanism

Abstract

The present invention proposes a dam inspection robot speed measurement method and a DVL rotation mechanism, which belong to the field of underwater robot navigation. The problem that DVL speed measurement easily enters blind spots and has reduced accuracy is solved. A dam inspection robot speed measurement method includes the following steps: S1, robot launch, before the robot is launched, a carrier coordinate system is established and the robot is suspended near the water area of ​​the dam surface to be measured, and the upper computer starts the DVL acoustic array and turns on all sensors; S2, inertial navigation system alignment; S3, DVL adjustment, the upper computer makes the DVL acoustic array speed measurement mode always bottom speed measurement; S4, inertial navigation system coordinate system adjustment, according to the rotation angle of the two degrees of freedom of the DVL acoustic array, the speed transmission of the DVL acoustic array after rotation is kept the same as the inertial navigation system coordinate system; S5, DVL acoustic array error calibration; S6, dam detection, detection is completed. It is mainly used for dam speed measurement.

Description

Technical Field

[0001] The invention belongs to the field of underwater robot navigation, and in particular relates to a speed measurement method of a dam inspection robot and a DVL rotating mechanism. Background Art

[0002] my country has been engaged in the research and development of underwater robots since the 1980s. In recent years, with the development of robot technology and the improvement of detection technology, underwater robots have been widely used in underwater structure detection. At present, underwater robots have become the most promising underwater detection tools in the intelligent detection of water conservancy projects, with a large room for development, and have very important application value for the prevention and reinforcement of water conservancy projects and the daily safety management of dams.

[0003] For underwater intelligent inspection robots for large hydropower stations, accurate underwater navigation and positioning provide the position and posture information of the carrier, which is the technical basis for completing defect positioning and motion control. Due to the complexity and unknown nature of the working environment of underwater robots for dam inspection, traditional combined navigation and positioning methods are difficult to meet the task requirements of high-precision defect detection in various working conditions on the vertical surface, slope surface and horizontal surface of the dam body and the switching of the carrier's working mode. In view of the high-precision combined navigation and positioning requirements, it is particularly important to study the speed measurement method suitable for dam surfaces with different slopes.

[0004] At present, the mainstream underwater robot speed measurement equipment is the strapdown inertial navigation system (SINS) and the Doppler velocimeter (DVL). The inertial navigation system outputs the speed of the carrier in three directions along the inertial navigation coordinate axis in real time through the built-in accelerometer. However, due to the characteristics of low-speed nonlinear motion of the carrier under the maintenance condition of the hydropower station dam, the speed output by the inertial navigation system will accumulate errors over time, which is difficult to meet the needs of high-precision speed measurement. The Doppler velocimeter is an acoustic speed measurement device that continuously emits sound wave signals of a certain frequency through an acoustic array. When the signal propagates to the solid surface and then reflects back to the array, the real-time speed of the carrier is calculated using the Doppler effect. Compared with the inertial navigation system, the speed output by the Doppler velocimeter is more accurate, but due to the limitations of its own acoustic principle and equipment materials, the Doppler velocimeter has a blind spot in the speed measurement range. In order to meet the task requirements of fine detection of the dam surface with different slopes of the hydropower station dam, the carrier needs to be observed close to the ground. At this time, the Doppler velocimeter can easily enter the blind spot, which greatly reduces the speed measurement accuracy. Therefore, how to design a high-precision speed measurement method that meets the needs of various working conditions is an urgent problem to be solved. Summary of the invention

[0005] In view of this, the present invention aims to propose a dam inspection robot speed measurement method and a DVL rotation mechanism to solve the problem that DVL speed measurement easily enters a blind spot and reduces accuracy.

[0006] To achieve the above object, according to one aspect of the present invention, a dam inspection robot speed measurement method is provided, comprising the following steps:

[0007] S1. Robot deployment. Before the robot is deployed, the carrier coordinate system is established and the robot is suspended near the water area of ​​the dam surface to be measured. After the robot enters the water, the host computer starts the DVL acoustic array and turns on all sensors;

[0008] S2, INS alignment, the host computer controls the INS to select the alignment mode as dock alignment or offshore alignment to establish the navigation coordinate system, the navigation mode is the combined navigation of the INS and the DVL acoustic array, and waits until the INS alignment is completed;

[0009] S3, DVL adjustment. The host computer obtains dam surface information according to the known dam surface drawings, controls the DVL rotating mechanism to rotate with two degrees of freedom, controls the horizontal electric indexing plate to rotate the heading angle of the DVL acoustic array to the direction of the dam surface to be detected, and controls the vertical electric indexing plate to rotate the DVL acoustic array pitch angle to be parallel to the dam surface to ensure that the DVL acoustic array speed measurement mode is always bottom speed measurement.

[0010] S4, inertial navigation system coordinate system adjustment, according to the rotation angle of the two degrees of freedom of the DVL acoustic array, change the velocity heading angle and pitch angle information of the rotation matrix of the combined navigation coordinate system of the inertial navigation system and the DVL acoustic array, so that the velocity transmission after the rotation of the DVL acoustic array remains the same as the inertial navigation system coordinate system;

[0011] S5. DVL acoustic array error calibration: according to the requirements of the integrated navigation of the inertial navigation system and the DVL acoustic array, the host computer of the inertial navigation system is used to calibrate the error of the DVL acoustic array;

[0012] S6. For dam inspection, after completing the slope dam surface inspection, turn off the DVL acoustic array and other sensors, and the inspection is completed.

[0013] Furthermore, the carrier coordinate system in step S1 takes the robot centroid as the origin O v , with the robot's horizontal axis as x v Axis, right is positive; the robot's vertical axis is y v Axis, forward is positive; the robot vertical axis is z v Axis, pointing upwards is positive.

[0014] Furthermore, the inertial navigation system in step S2 is a strapdown inertial navigation system (SINS).

[0015] Furthermore, the dock alignment method in step S2 is to establish the navigation coordinate system by taking the SINS center position as the initial coordinate origin, and using the latitude and longitude information output by the global positioning system as the initial information origin coordinates. East is x n Axis, north is y n Axis, celestial direction is z n The navigation coordinate system is established by the maritime alignment method: the center position of the SINS is regarded as the initial coordinate origin, and the latitude and longitude information output by the global positioning system is used as the initial information origin coordinates. East is x n Axis, north is y n Axis, celestial direction is z n Axis establishes the navigation coordinate system.

[0016] Furthermore, after the inertial navigation system is aligned in step S2, the DVL acoustic array provides the carrier coordinate system x for the inertial navigation system. v Axis, y v Axis and z v The speed information in the three directions of the axis is obtained. After the speed information is fused through Kalman filtering data, the inertial navigation system performs dead reckoning based on the speed information and attitude angle information of the DVL acoustic array at this time to obtain the specific position information of the robot.

[0017] Furthermore, in step S3, the DVL acoustic array velocity measurement mode is always set to bottom velocity measurement in the following manner: a rotating coordinate system R is established with the intersection of the horizontal electric indexing plate and the vertical electric indexing plate rotation center axis as the origin, and the horizontal electric indexing plate rotation axis as the z axis. r Axis, pointing to the sky is positive, the vertical electric indexing plate rotation axis is x r Axis, right is positive, and x r Axis and z r The vertical direction of the axis is y r Axis, forward is positive.

[0018] Furthermore, the DVL rotating mechanism operates according to the dam surface information, so that the DVL acoustic array first rotates around z r The axis rotates by an angle of θ1 to the dam surface direction, and then rotates around x r The axis rotates the DVL acoustic array Until it is parallel to the dam surface, so that the sound waves emitted by the DVL acoustic array always remain perpendicular to the dam surface.

[0019] Furthermore, the rotation matrix in step S4 is

[0020] Furthermore, the error calibration method in S5 includes the following steps:

[0021] S51, the robot sails to point A and takes position, keeping the speed constant. When it sails to point A1, the bottom tracking calibration test officially begins. Point A to point A1 is the heading and speed stability range;

[0022] S52. The robot sails straight from point A1 at a constant speed of 2 knots, maintaining a stable heading, to point B. The speed calibration interval from point A1 to point B is no less than 4 kilometers.

[0023] S53, after reaching point B, the robot starts to turn 180° and takes position at point B1, keeping the speed constant. Point B1 to point B is the speed stability range;

[0024] S54. The robot sails straight from point B at a constant speed of 2 knots, maintaining a stable heading, to point A1. The speed calibration interval from point B to point A1 is no less than 4 kilometers.

[0025] S55, read the data and process it to complete the odometer calibration.

[0026] According to another aspect of the present invention, a DVL rotating mechanism using the above-mentioned dam inspection robot speed measurement method is provided, comprising:

[0027] A rotating mechanism bracket, used for connecting with the robot;

[0028] Horizontal electric indexing plate, the fixed end is connected to the rotating mechanism bracket and the rotating plate surface is kept horizontally arranged;

[0029] The fixed end of the vertical electric indexing plate is connected to the rotating plate of the horizontal electric indexing plate, and the rotating plate of the vertical electric indexing plate is kept in a vertical arrangement;

[0030] The DVL acoustic array is connected to the vertical electric indexing plate.

[0031] Compared with the prior art, the present invention has the following beneficial effects:

[0032] 1. Through the rotation adjustment of DVL, DVL can accurately measure the speed of the vertical surface, slope surface and horizontal surface of the dam body, meeting the task requirements of high-precision underwater positioning and navigation of the robot;

[0033] 2. By combining the navigation of DVL and inertial navigation system, adjusting and unifying the coordinate system, and calibrating the error, the combined navigation mode can realize the high-precision speed measurement of DVL at all times. BRIEF DESCRIPTION OF THE DRAWINGS

[0034] The accompanying drawings constituting a part of the present invention are used to provide a further understanding of the present invention. The exemplary embodiments of the present invention and their descriptions are used to explain the present invention and do not constitute an improper limitation of the present invention. In the accompanying drawings:

[0035] Figure 1 This is a flow chart of a dam inspection robot speed measurement method according to the present invention;

[0036] Figure 2 is a top view of the DVL rotating mechanism of the present invention;

[0037] Figure 3 It is a front view of the DVL rotating mechanism of the present invention;

[0038] Figure 4 is a side view of the DVL rotating mechanism of the present invention;

[0039] Figure 5 is a three-dimensional diagram of the DVL rotating mechanism of the present invention;

[0040] Figure 6 A schematic diagram of the working condition of horizontal plane detection by a dam detection robot speed measurement method according to the present invention;

[0041] Figure 7 The present invention is a schematic diagram of the working conditions of the dam inspection robot speed measurement method for vertical surface and slope surface inspection.

[0042] Rotating mechanism bracket 1; horizontal electric indexing plate 2; DVL acoustic array fixing part 3; DVL acoustic array 4; indexing plate connecting part 5; vertical electric indexing plate 6. DETAILED DESCRIPTION

[0043] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely explain the technical solutions in the embodiments of the present invention. It should be noted that the embodiments of the present invention and the features in the embodiments can be combined with each other without conflict, and the described embodiments are only part of the embodiments of the present invention, not all of the embodiments.

[0044] Referring to the accompanying drawings, this embodiment is described. According to one aspect of the present invention, a dam inspection robot speed measurement method is provided, comprising the following steps:

[0045] S1. Robot deployment. Before the robot is deployed, a carrier coordinate system is established and the robot is suspended near the water area of ​​the dam surface to be tested. After the robot enters the water, the host computer starts the DVL acoustic array 4 and turns on all sensors; the carrier coordinate system takes the robot centroid as the origin O v , with the robot's horizontal axis as x v Axis, right is positive; the robot's vertical axis is y v Axis, forward is positive; the robot vertical axis is z v Axis, pointing up is positive;

[0046] S2, INS alignment, the host computer controls the INS to select the alignment mode as dock alignment or offshore alignment to establish the navigation coordinate system, the navigation mode is the combined navigation of the INS and DVL acoustic array 4, wait for 30 minutes until the INS alignment is completed; the INS is the strapdown inertial navigation system (SINS);

[0047] S3, DVL adjustment, the upper computer obtains the dam surface slope information according to the known dam surface drawings, controls the DVL rotating mechanism to rotate with two degrees of freedom, controls the horizontal electric indexing plate 2 to rotate the heading angle of the DVL acoustic array 4 to the direction of the detected dam surface, and controls the vertical electric indexing plate 6 to rotate the longitudinal inclination angle of the DVL acoustic array 4 to be parallel to the detected dam surface, so that the speed measurement mode of the DVL acoustic array 4 is always the bottom speed measurement;

[0048] S4, inertial navigation system coordinate system adjustment, according to the rotation angle of the two degrees of freedom of the DVL acoustic array 4, change the velocity heading angle and pitch angle information of the rotation matrix of the combined navigation coordinate system of the inertial navigation system and the DVL acoustic array 4, so that the velocity transmission of the DVL acoustic array 4 after rotation remains the same as the inertial navigation system coordinate system;

[0049] S5, DVL acoustic array 4 error calibration, according to the requirements of the combined navigation of the inertial navigation system and the DVL acoustic array 4, the host computer of the inertial navigation system is used to calibrate the error of the DVL acoustic array 4;

[0050] The error calibration method includes the following steps:

[0051] S51, the robot sails to point A and takes position, keeping the speed constant. When it sails to point A1, the bottom tracking calibration test officially begins. Point A to point A1 is the heading and speed stability range;

[0052] S52. The robot sails straight from point A1 at a constant speed of 2 knots, maintaining a stable heading, to point B. The speed calibration interval from point A1 to point B is no less than 4 kilometers.

[0053] S53, after reaching point B, the robot starts to turn 180° and takes position at point B1, keeping the speed constant. Point B1 to point B is the stable speed range;

[0054] S54. The robot sails straight from point B at a constant speed of 2 knots, maintaining a stable heading, to point A1. The speed calibration interval from point B to point A1 is no less than 4 kilometers.

[0055] S55, read the data and process it to complete the odometer calibration.

[0056] S6. For dam inspection, after completing the slope dam surface inspection, turn off the DVL acoustic array 4 and other sensors, and the inspection is completed.

[0057] In this embodiment, the dock alignment method in step S2 is to establish the navigation coordinate system by taking the center position of the SINS as the initial coordinate origin, and using the latitude and longitude information output by the global positioning system as the initial information origin coordinates. East is x n Axis, north is y n Axis, celestial direction is z n The navigation coordinate system is established by the maritime alignment method: the center position of the SINS is regarded as the initial coordinate origin, and the latitude and longitude information output by the global positioning system is used as the initial information origin coordinates. East is x n Axis, north is y n Axis, celestial direction is z n Axis establishes the navigation coordinate system.

[0058] In this embodiment, after the inertial navigation system is aligned in step S2, the DVL acoustic array 4 provides the carrier coordinate system x for the inertial navigation system. v Axis, y v Axis and z v The speed information in the three directions of the axis is obtained. After the speed information is fused through Kalman filtering data, the inertial navigation system performs dead reckoning based on the speed information and attitude angle information of the DVL acoustic array 4 at this time to obtain the specific position information of the robot.

[0059] In this embodiment, the method of making the speed measurement mode of the DVL acoustic array 4 always the bottom speed measurement in step S3 is: a rotating coordinate system R is established with the intersection of the rotation center axis of the horizontal electric indexing plate 2 and the vertical electric indexing plate 6 as the origin, and the rotation axis of the horizontal electric indexing plate 2 as the z axis. r Axis, pointing to the sky is positive, the vertical electric indexing plate 6 rotation axis is x r Axis, right is positive, and x r Axis and z r The vertical direction of the axis is y r Axis, forward is positive.

[0060] In this embodiment, the DVL rotating mechanism operates according to the dam surface information, so that the DVL acoustic array 4 first rotates around z r The axis rotates by an angle of θ1 to the dam surface direction, and then rotates around x r The axis rotates the DVL acoustic array 4 Until it is parallel to the dam surface, so that the sound waves emitted by the DVL acoustic array 4 always remain perpendicular to the dam surface.

[0061] In this embodiment, the rotation matrix in step S4 is The velocity information obtained by the DVL acoustic array 4 is transformed by the velocity conversion matrix and then input into the inertial navigation system, so that the direction of the velocity information is always consistent with the dead reckoning direction of the inertial navigation system.

[0062] According to another aspect of the present invention, a DVL rotating mechanism using the above-mentioned dam inspection robot speed measurement method is provided, comprising:

[0063] The rotating mechanism bracket 1 is used to connect with the robot and has an overall frame structure; the rotating mechanism bracket 1 is made of aluminum alloy profiles and provides protection for the DVL acoustic array 4 to prevent the DVL acoustic array 4 from being damaged due to the robot touching the bottom.

[0064] The fixed end of the horizontal electric indexing plate 2 is connected to the rotating mechanism bracket 1 through the indexing plate connecting piece 5, and the rotating plate surface is kept horizontally arranged; the indexing plate connecting piece 5 is made of 5052 aluminum alloy by turning.

[0065] The fixed end of the vertical electric indexing plate 6 is connected to the rotating plate of the horizontal electric indexing plate 2, and the rotating plate surface of the vertical electric indexing plate 6 remains vertically arranged; both the horizontal electric indexing plate 2 and the vertical electric indexing plate 6 adopt the RC01RA60 high-precision electric rotating table, and the rotating plates are all 60mm, which are used to control the heading angle and pitch angle of the DVL acoustic array 4.

[0066] The DVL acoustic array 4 uses a HEU-600 split DVL for high-precision speed measurement of the robot body, which is connected to the rotating disk of the vertical electric indexing disk 6.

[0067] In the above description, the sensors, controllers and control programs that may be used are all existing technologies and will not be described in detail here.

[0068] The embodiments of the present invention disclosed above are only used to help illustrate the present invention. The embodiments do not describe all the details in detail, nor do they limit the invention to the specific implementation methods described. According to the content of this specification, many modifications and changes can be made. This specification selects and specifically describes these embodiments in order to better explain the principles and practical applications of the present invention, so that those skilled in the art can understand and use the present invention well.

Claims

1. A dam inspection robot speed measurement method, characterized in that: The following steps are involved: S1. Launching the robot. Before launching the robot, a carrier coordinate system is established and the robot is suspended near the water area of ​​the dam surface to be measured. After the robot enters the water, the upper computer starts the DVL acoustic array (4) and turns on all sensors. S2, aligning the inertial navigation system, by controlling the inertial navigation system through the host computer to select the alignment mode as dock alignment or offshore alignment to establish a navigation coordinate system, the navigation mode is the combined navigation of the inertial navigation system and the DVL acoustic array (4), and wait until the inertial navigation system alignment is completed; S3, DVL adjustment, the upper computer obtains dam surface information according to the known dam surface drawings, controls the DVL rotating mechanism to rotate with two degrees of freedom, controls the horizontal electric indexing plate (2) to rotate the heading angle of the DVL acoustic array (4) to the direction of the detected dam surface, and controls the vertical electric indexing plate (6) to rotate the longitudinal inclination angle of the DVL acoustic array (4) to be parallel to the detected dam surface, so that the speed measurement mode of the DVL acoustic array (4) is always the bottom speed measurement; S4, adjusting the coordinate system of the inertial navigation system, according to the rotation angle of the two degrees of freedom of the DVL acoustic array (4), changing the velocity heading angle and pitch angle information of the rotation matrix of the combined navigation coordinate system of the inertial navigation system and the DVL acoustic array (4), so that the velocity transmission of the DVL acoustic array (4) after rotation remains the same as the coordinate system of the inertial navigation system; S5, DVL acoustic array (4) error calibration, according to the requirements of the combined navigation of the inertial navigation system and the DVL acoustic array (4), the error calibration of the DVL acoustic array (4) is performed using the host computer of the inertial navigation system; S6. After the slope and dam surface detection work is completed, the DVL acoustic array (4) and other sensors are turned off, and the detection is completed; In step S3, the speed measurement mode of the DVL acoustic array (4) is always set to the bottom speed measurement mode by: establishing a rotating coordinate system R with the intersection of the rotation center axes of the horizontal electric indexing plate (2) and the vertical electric indexing plate (6) as the origin, and taking the rotation axis of the horizontal electric indexing plate (2) as the z axis. r The axis is pointed to the sky and the rotation axis of the vertical electric indexing plate (6) is x. r Axis, right is positive, and x r Axis and z r The vertical direction of the axis is y r Axis, forward is positive.

2. A dam inspection robot speed measurement method according to claim 1, characterized in that: The carrier coordinate system in step S1 takes the robot's centroid as its origin. v , with the robot's horizontal axis as x v Axis, right is positive; the robot's vertical axis is y v Axis, forward is positive; the robot vertical axis is z v Axis, pointing upwards is positive.

3. A dam inspection robot speed measurement method according to claim 1 or 2, characterized in that: The inertial navigation system in step S2 is a strapdown inertial navigation system (SINS).

4. A dam inspection robot speed measurement method according to claim 3, characterized in that: The method of establishing the navigation coordinate system in the dock alignment method in step S2 is: considering the center position of the SINS as the initial coordinate origin, and using the latitude and longitude information output by the global positioning system as the initial information origin coordinates East is x n Axis, north is y n Axis, celestial direction is z n The navigation coordinate system is established by the maritime alignment method: the center position of the SINS is regarded as the initial coordinate origin, and the latitude and longitude information output by the global positioning system is used as the initial information origin coordinates. East is x n Axis, north is y n Axis, celestial direction is z n Axis establishes the navigation coordinate system.

5. A dam inspection robot speed measurement method according to claim 2, characterized in that: After the inertial navigation system is aligned in step S2, the DVL acoustic array (4) provides the carrier coordinate system x for the inertial navigation system. v Axis, y v Axis and z v The speed information in the three directions of the axis is obtained. After the speed information is fused through Kalman filtering, the inertial navigation system performs dead reckoning based on the speed information and attitude angle information of the DVL acoustic array (4) at this time to obtain the specific position information of the robot.

6. A dam inspection robot speed measurement method according to claim 1, characterized in that: The DVL rotating mechanism moves according to the dam surface information, so that the DVL acoustic array (4) first rotates around z r The axis rotates by an angle of θ1 to the dam surface direction, and then rotates around x r The axis rotates the DVL acoustic array (4) until it is parallel to the dam surface, so that the sound waves emitted by the DVL acoustic array (4) always remain perpendicular to the dam surface.

7. A dam inspection robot speed measurement method according to claim 1, characterized in that: The rotation matrix in step S4 is:

8. The dam inspection robot speed measurement method according to claim 1 is characterized by: The error calibration method in S5 comprises the following steps: S51, the robot sails to point A and takes position, keeping the speed constant. When it sails to point A1, the bottom tracking calibration test officially begins. Point A to point A1 is the heading and speed stability range; S52. The robot sails straight from point A1 at a constant speed of 2 knots, maintaining a stable heading, to point B. The speed calibration interval from point A1 to point B is no less than 4 kilometers. S53, after reaching point B, the robot starts to turn 180° and takes position at point B1, keeping the speed constant. Point B1 to point B is the speed stability range; S54. The robot sails straight from point B at a constant speed of 2 knots, maintaining a stable heading, to point A1. The speed calibration interval from point B to point A1 is no less than 4 kilometers. S55, read the data and process it to complete the odometer calibration.

9. A DVL rotating mechanism using a dam inspection robot speed measurement method as claimed in any one of claims 1 to 8, characterized in that: include: A rotating mechanism bracket (1), used for connecting with the robot; A horizontal electric indexing plate (2), the fixed end of which is connected to the rotating mechanism bracket (1) and the rotating plate surface is kept horizontally arranged; A vertical electric indexing plate (6), the fixed end of which is connected to the rotating plate of the horizontal electric indexing plate (2), and the rotating plate surface of the vertical electric indexing plate (6) is kept vertically arranged; The DVL acoustic array (4) is connected to the rotating disk of the vertical electric indexing disk (6).

Images