CN116295511A - Robust initial alignment method and system for pipeline submerged robot - Google Patents
Robust initial alignment method and system for pipeline submerged robot Download PDFInfo
- Publication number
- CN116295511A CN116295511A CN202211623456.4A CN202211623456A CN116295511A CN 116295511 A CN116295511 A CN 116295511A CN 202211623456 A CN202211623456 A CN 202211623456A CN 116295511 A CN116295511 A CN 116295511A
- Authority
- CN
- China
- Prior art keywords
- robot
- underwater
- attitude angle
- pipeline
- underwater robot
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 57
- 230000003287 optical effect Effects 0.000 claims abstract description 27
- 238000004364 calculation method Methods 0.000 claims abstract description 14
- 230000001133 acceleration Effects 0.000 claims abstract description 12
- 238000005259 measurement Methods 0.000 claims description 20
- 238000001514 detection method Methods 0.000 claims description 10
- 238000004590 computer program Methods 0.000 claims description 9
- 238000004422 calculation algorithm Methods 0.000 claims description 8
- 230000000694 effects Effects 0.000 claims description 3
- 238000001914 filtration Methods 0.000 claims description 3
- 238000013507 mapping Methods 0.000 claims description 3
- 239000010865 sewage Substances 0.000 abstract description 3
- 239000011159 matrix material Substances 0.000 description 21
- 230000008859 change Effects 0.000 description 9
- 230000008569 process Effects 0.000 description 6
- 230000036541 health Effects 0.000 description 3
- 230000010354 integration Effects 0.000 description 3
- 230000009466 transformation Effects 0.000 description 3
- 238000004458 analytical method Methods 0.000 description 2
- 230000008901 benefit Effects 0.000 description 2
- 230000007547 defect Effects 0.000 description 2
- VNWKTOKETHGBQD-UHFFFAOYSA-N methane Chemical compound C VNWKTOKETHGBQD-UHFFFAOYSA-N 0.000 description 2
- 230000000007 visual effect Effects 0.000 description 2
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 description 2
- WHXSMMKQMYFTQS-UHFFFAOYSA-N Lithium Chemical compound [Li] WHXSMMKQMYFTQS-UHFFFAOYSA-N 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000007797 corrosion Effects 0.000 description 1
- 238000005260 corrosion Methods 0.000 description 1
- 230000001808 coupling effect Effects 0.000 description 1
- 238000000354 decomposition reaction Methods 0.000 description 1
- 230000007123 defense Effects 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 238000005286 illumination Methods 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 229910052744 lithium Inorganic materials 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 239000003345 natural gas Substances 0.000 description 1
- 239000013307 optical fiber Substances 0.000 description 1
- 239000003208 petroleum Substances 0.000 description 1
- 238000005070 sampling Methods 0.000 description 1
- 239000004065 semiconductor Substances 0.000 description 1
- 239000000126 substance Substances 0.000 description 1
- 238000012546 transfer Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
- G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices
Abstract
The invention relates to a robust initial alignment method and a system for a pipeline submarine robot, comprising the following steps: acquiring the moving direction of the underwater robot; calculating the optical flow speed of the underwater robot based on the moving direction of the underwater robot and estimating the underwater operation speed of the underwater robot under the machine body coordinate system; calculating an initial attitude angle of the underwater robot based on the optical flow speed and the underwater operation speed of the underwater robot; acquiring real-time attitude angle information; and feeding the real-time attitude angle information back to the initial attitude angle calculation to obtain accurate initial attitude angle information. According to the invention, a robust initial alignment method is adopted, and the accurate initial attitude information of the submerged robot is obtained by combining the moving image, the acceleration, the angular velocity and the real-time velocity information, so that the influence of the sewage environment of the pipeline on the initial alignment is reduced, and the accurate initial alignment of the movable base of the strapdown inertial navigation is realized.
Description
Technical Field
The invention relates to the field of urban pipeline automatic detection, in particular to a robust initial alignment method and system for a pipeline submerged robot.
Background
The pipeline is used as a transportation means, is widely applied to a plurality of fields such as petroleum, chemical industry, national defense, natural gas, pollution discharge and the like, is greatly convenient for human communities, and brings great economic benefit. However, as the service time of the pipeline increases, a great amount of sticky dirt adheres to the inner wall of the pipeline, and in addition, the inner wall of the pipeline inevitably breaks, misplaces and collapses under the coupling action of factors such as corrosion, heavy pressure and the like, so that the factors not only influence the transportation efficiency, but also bring potential safety hazards and even generate serious economic loss. Although storms are natural disasters, if a high-standard drainage infrastructure is constructed and the blocked pipeline is inspected and cleaned in time, the loss of property can be reduced to the minimum, and the casualties can be avoided. Therefore, the pipeline is comprehensively maintained and troubleshooted by a timely and effective method, the running safety of the pipeline is improved, the national economic loss can be reduced, the stability of the ecological environment is ensured, and the method has important significance for sustainable development strategy in China.
For the work of underground pipeline health state detection, because of the specificity and complexity of the environment, the current industry mainly adopts a robot loaded with a camera to collect the visual information in the pipeline, and transmits the video data to an intelligent detection platform system, and a professional engineer checks and interprets the visual information, so that the detection of the pipeline health state is completed. Although the carrying of the camera can help to judge the health state of the pipeline, the three-dimensional position information of the defect on the pipeline section cannot be positioned. In order to obtain three-dimensional position information of a defect on a pipe section, a scholars proposes an inertial navigation positioning method, a Strapdown Inertial Navigation System (SINS) is used for directly fixing a gyroscope and an accelerometer on a carrier, real-time measurement is carried out on three-axis angular velocity and three-axis acceleration information of an operation carrier by utilizing inertial sensitive devices such as the gyroscope and the accelerometer, and navigation information such as the gesture, the speed and the position of the motion carrier is obtained through high-speed integration by combining initial inertial information of the operation carrier. The strapdown inertial navigation system does not depend on external information or radiate energy to the outside during operation, is not easy to interfere and damage, and has the advantages of high data updating rate, comprehensive data, high short-time positioning accuracy and the like.
SINS, as a dead reckoning navigation method, has a performance that depends largely on the accuracy of the initial attitude angle (or initial attitude matrix), which is determined by a so-called initial alignment process. Generally, the alignment process includes two stages, coarse alignment and fine alignment. The coarse alignment requires providing a generally known initial pose matrix in a short time, and then, in the fine alignment stage, correcting the initial pose matrix calculated in the coarse alignment stage using a kalman filter technique. Therefore, the coarse alignment is used as the premise of the fine alignment, and the high-performance coarse alignment method can effectively improve the precision and convergence rate of the fine alignment. However, the internal space of the pipeline is narrow, the flow speed of water is time-varying, the obstacles are numerous, the submarine is difficult to keep still for a long time in the initial alignment process, the rotation angular velocity of the earth sensed by the gyroscope is easily covered by the movement angular velocity of the machine body, the initial alignment error of the traditional analytical method is too large or even unusable, and the initial alignment based on an inertia system has better capability of resisting angular shaking interference.
However, the algorithm of initial alignment of the inertial system is adopted to obtain the ground speed of the submarine, and the traditional video speed measurement algorithm comprises a background difference method, a frame difference method, an optical flow method and the like. The background difference method cannot be well adapted to scene change, and the frame difference method cannot completely extract states of all relevant feature points, so that a pure background image is obtained, the detection result is inaccurate, and the target analysis and the speed detection are not facilitated. The optical flow method can support the movement of the camera, has stronger distinguishing capability for the detection of multiple moving targets, and can completely reflect the movement information and detect the related targets from the background.
Although the optical flow method can be used for calculating the ground speed of the submarine, the optical flow method is easily influenced by illumination, object shielding or image noise, if the water quality in the drainage pipe network is turbid and blackened, the measurement result has larger error, and therefore the initial alignment accuracy is influenced.
Disclosure of Invention
The invention aims to provide a robust initial alignment method and a system for a pipeline submersible vehicle robot, which can reduce the influence of a pipeline sewage environment on initial alignment and improve the initial alignment precision of inertial navigation.
In order to achieve the above purpose, the technical scheme provided by the invention is as follows:
a robust initial alignment method for a pipeline submerged robot, comprising the steps of:
step one, acquiring the moving direction of the underwater robot;
calculating the optical flow speed of the underwater robot based on the moving direction of the underwater robot and estimating the underwater operation speed of the underwater robot under a machine body coordinate system;
step three, calculating an initial attitude angle of the underwater robot based on the underwater operation speed of the underwater robot;
step four, obtaining real-time attitude angle information;
and step five, feeding back the real-time attitude angle information in the step four to the step three to obtain accurate initial attitude angle information.
In order to optimize the technical scheme, the specific measures adopted further comprise:
a camera is mounted in front of the underwater vehicle, and a lens of the camera is always maintained in the movement direction of the underwater vehicle; the underwater robot is provided with a main control board; the inertial measurement unit IMU is integrated with the triaxial accelerometer and the triaxial gyroscope in the submarine robot and is used for measuring the acceleration and angular velocity information of the submarine robot; the underwater robot is provided with a DVL sensor, and the real-time speed of the underwater robot is measured based on the Doppler effect.
In the first step, the main control board collects moving image data of the outside world through the camera module, and the moving direction of the submarine robot is determined by combining the relation between the sports field and the optical flow field and adopting the principle of the main moving direction.
Analyzing frame data of the moving image at different moments, extracting corner information of the moving image based on a Harris corner detection algorithm, constructing a moving image pyramid model according to the corner information, calculating the optical flow speed of the submerged robot by using a Lucas-Kanade optical flow method, and estimating the submerged running speed of the submerged robot under a machine body coordinate system in a coordinate mapping mode.
And thirdly, constructing an inertial navigation coarse alignment equation, and calculating an initial attitude angle of the underwater robot by using an initial alignment QMETHod method based on quaternion by combining the obtained underwater operation speed and the physical quantity of the acceleration and the angular speed output by the inertial measurement unit IMU after carrying out numerical discretization.
And step four, deriving a nonlinear Kalman filtering state equation and an observation equation by combining the submarine operation speed under the submarine robot body coordinate system output by the DVL sensor, and estimating real-time attitude errors and zero offset of the gyroscope by using the Kalman filter to obtain real-time attitude angle accurate information.
Further, an inertial measurement unit IMU adopts a laser strapdown inertial navigation system; the zero bias stability of the triaxial gyroscope and the triaxial accelerometer are respectively 0.01 degree/h and 10 degree -5 g, to meet the requirement of pipeline real-time positioning; the DVL sensor employs an a50 acoustic doppler log.
The invention also protects a robust initial alignment system for a pipeline submerged robot, comprising:
the moving direction acquisition module is used for acquiring the moving direction of the underwater robot;
the speed calculation module is used for calculating the optical flow speed of the underwater robot based on the moving direction of the underwater robot and estimating the underwater operation speed of the underwater robot under the machine body coordinate system;
the initial attitude angle calculation module is used for calculating the initial attitude angle of the underwater robot based on the underwater operation speed of the underwater robot and is used for calculating the initial attitude angle of the underwater robot;
the accurate initial attitude angle acquisition module is used for acquiring real-time attitude angle information, feeding the real-time attitude angle information back to the initial attitude angle calculation module, returning to the operation executed by the initial attitude angle calculation module, and calculating to obtain the accurate initial attitude angle information;
the invention also protects an electronic device comprising: the system comprises a memory, a processor and a computer program stored on the memory and capable of running on the processor, wherein the processor realizes the robust initial alignment method for the pipeline submerged robot when executing the computer program.
The invention also protects a computer readable storage medium storing a computer program for causing a computer to execute the robust initial alignment method for a pipeline submerged robot.
Compared with the prior art, the invention has the beneficial effects that:
according to the invention, a robust initial alignment method is adopted, and the motion image, the acceleration and the angular velocity and the real-time velocity information acquired by the underwater vehicle are acquired through the camera, the inertial measurement unit IMU and the DVL sensor respectively, so that the accurate initial attitude information of the underwater vehicle is obtained, the influence of the sewage environment of the pipeline on the initial alignment is reduced, and the accurate initial alignment of the movable base of the strapdown inertial navigation is realized.
Drawings
FIG. 1 is a schematic diagram of the overall structure of the present invention;
FIG. 2 is a flow chart of detecting the speed of a submerged robot in combination with an optical flow technique in accordance with the present invention;
FIG. 3 is a flow chart of the inertial navigation coarse alignment procedure of the present invention;
fig. 4 is a flowchart of a robust initial alignment method after combining DVL outputs in the present invention.
In the figure: 1. propeller 2, DVL sensor, 3, main control board, 4, inertial measurement unit IMU,5, camera, 6, battery compartment, 7, sonar.
Detailed Description
The above-described matters of the present invention will be further described in detail by way of examples, but it should not be construed that the scope of the above-described subject matter of the present invention is limited to the following examples, and all techniques realized based on the above-described matters of the present invention are within the scope of the present invention.
The pipeline submarine robot structure adopted by the invention is shown in fig. 1, and the initial alignment system mainly comprises: the device comprises a propeller 1, a DVL sensor 2, a main control board 3, an inertial measurement unit IMU4, a camera 5, a battery compartment 6 and a sonar 7, wherein the inertial measurement unit IMU4 and the main control board 3 are fixed in a sub-gram force tube and are connected in a USB mode, and a triaxial accelerometer and a triaxial gyroscope are integrated in the IMU and are used for measuring acceleration and angular velocity information of a carrier; the camera 6 is fixed in front of the underwater vehicle through a bracket, and the lens of the camera is always maintained in the movement direction of the underwater vehicle; the pipeline submarine robot is also provided with a DVL sensor, and the real-time speed of the submarine is measured based on the Doppler effect; a high-capacity lithium battery is mounted in the battery compartment 6 and is used as an electric power source of the submerged robot; the main control board 3 collects moving images, acceleration and angular velocity amounts and real-time velocity information of the underwater vehicle through the camera, the inertial measurement unit IMU and the DVL sensor respectively, and then the robust initial alignment method provided by the patent is combined, so that accurate initial attitude information of the underwater vehicle is obtained.
In one embodiment, the invention provides a robust initial alignment method for a pipeline submerged robot, comprising the steps of:
step one, a main control board collects moving image data of the outside world through a camera module, and a moving main direction principle is adopted to determine the moving direction of the submarine robot by combining the relation between a sports field and an optical flow field;
analyzing frame data of the moving image at different moments, extracting corner information of the moving image based on a Harris corner detection algorithm, constructing a moving image pyramid model according to the corner information, calculating the optical flow speed of the submerged robot by using a Lucas-Kanade optical flow method, and estimating the submerged running speed of the submerged robot under a machine body coordinate system in a coordinate mapping mode, wherein the operation speed is shown in FIG. 2;
constructing an inertial navigation coarse alignment equation, after performing numerical discretization, calculating an initial attitude angle of the underwater vehicle by using an initial alignment QMETHod method based on quaternion according to the underwater vehicle running speed obtained in the second step and the physical quantity of acceleration and angular speed output by the inertial measurement unit IMU, as shown in fig. 3;
step four, combining the submarine operation speed under the submarine robot body coordinate system output by the DVL sensor, deducing a nonlinear Kalman filtering state equation and an observation equation, estimating real-time attitude errors and zero offset of a gyroscope by using a Kalman filter, and obtaining real-time attitude angle accurate information, as shown in figure 4;
and step five, feeding back the real-time attitude angle accurate information in the step four to the step three, and calculating the initial attitude angle again so as to calculate the accurate initial attitude information.
In the second step, based onThe pyramid-type Lucas-Kanade optical flow algorithm calculates the corresponding optical flow speed, and converts the optical flow speed in the image into the actual moving speed of the underwater vehicle, thereby obtaining the underwater operation speed of the underwater robot under the machine body coordinate system
In the third step, the specific steps for constructing the inertial navigation coarse alignment equation are as follows:
and rewriting differential equations of the inertial navigation system by combining a gesture decomposition technology as follows:
where t represents time, represents the derivative of the state,a rotation change matrix representing a navigation system, +.>Representing an initial gesture transformation matrix->Representing a rotational change matrix of the carrier coordinate system, f b Indicating specific force->Projection of the rotation speed of the earth in the navigation coordinate system, < >>Representing the projection of the deflection of the navigational coordinate system relative to the earth coordinate system under the navigational coordinate system, v n G represents the speed of the submarine in the geographic coordinate system n Is the gravitational acceleration.
The finishing equation (0-2) can be obtained
A gesture change matrix representing navigation system change along with time, tau is an integral variable, and formulas (0-3), (1-4) and (0-5) are obtained inertial navigation rough alignment equations, wherein alpha (t) m ) And beta (t) m ) Are all at time interval t m ,t](t m ∈[0,t]) Vector observations constructed using a generalized velocity integration formula.
In the third step, the method for calculating the initial attitude angle of the underwater robot comprises the following steps:
discretizing the formulas (0-4) and (0-5), combining the fractional integration, and discretizing the formulas as follows
Wherein t is j =jT s ,t=t k =kT s ,T s The time of the sampling is indicated and,representing the modified transformation matrix by the Kalman filter,/for the matrix>From t j A change in attitude between time τ;
where the summation computation starts with the symbol m, k denotes the time of day,representing the navigation system from t 0 To t m The amount of change in the time of day,representing the navigation system from t 0 To t k The amount of change in time; based on equations (0-3), (0-6), (0-7), the speed of the camera calculated by the optical flow algorithm is +.>Substituting the angular velocity and the acceleration vector output by the inertial system, and obtaining an initial attitude matrix with errors by using a QMETHod method>
In the fourth step, the method for obtaining the accurate information of the real-time attitude angle comprises the following steps:
the following form of state equation is constructed:
using the output of the DVL sensor, the following observation equation is constructed:
Z k =H k X k (0-9)
wherein:
representing the angular velocity, eta measured by a gyroscope gu Representing process noise variable, X k A state at time k; />Representing the amount of change in posture +.>Representing an initial pose matrix;
combining equations (0-8) - (0-11) and executing a Kalman filter to estimate an attitude error phi and a gyroscope zero offset epsilon b The execution flow of the Kalman filter comprises two stages of time updating and measurement updating:
and (5) updating time:
and (5) measurement and update:
in the middle ofRepresents the optimal state at time k, P k State covariance matrix of k moment phi k/k-1 Representing a one-step prediction matrix->State value representing time k-1, +.>Representing one-step prediction state, P k-1 Representing the state covariance matrix at time k-1, P k|k-1 Representing a one-step prediction covariance matrix, Q k-1 Process noise covariance matrix Γ representing time k-1 k-1 Representing a process noise transfer matrix; h k Represent the measurement noise matrix, R k Represents the measurement noise matrix at the moment k, Z k Represents the measurement value at time K, E represents the identity matrix, K k Representing the calculated kalman filter coefficients; p (P) k The state covariance matrix at time k is represented.
The invention combines the output of the DVL sensor to construct the robust Kalman filter, thereby correcting the initial alignment result and finally realizing the SINS initial alignment problem in the severe pipeline environment.
Attitude error phi estimated by Kalman filter and zero offset epsilon of gyroscope b Kalman filter corrected transformation matrix that helps to improve the existence of errorsAnd is fed back to the initial gesture matrix with errorIn the calculation of (2), more accurate initial attitude information is obtained, so that accurate initial alignment of the movable base of the strapdown inertial navigation is realized.
In another embodiment, the present invention provides a robust initial alignment system for a pipeline submerged robot, comprising:
the moving direction acquisition module is used for acquiring the moving direction of the underwater robot;
the speed calculation module is used for calculating the optical flow speed of the underwater robot based on the moving direction of the underwater robot and estimating the underwater operation speed of the underwater robot under the machine body coordinate system;
the initial attitude angle calculation module is used for calculating the initial attitude angle of the underwater robot based on the underwater operation speed of the underwater robot and is used for calculating the initial attitude angle of the underwater robot;
the accurate initial attitude angle acquisition module is used for acquiring real-time attitude angle information, feeding the real-time attitude angle information back to the initial attitude angle calculation module, returning to the operation executed by the initial attitude angle calculation module, and calculating to obtain the accurate initial attitude angle information;
in another embodiment, the present invention provides an electronic device, including: the system comprises a memory, a processor and a computer program stored on the memory and capable of running on the processor, wherein the processor realizes the robust initial alignment method for the pipeline submerged robot when executing the computer program.
In another embodiment, the invention provides a computer readable storage medium storing a computer program for causing a computer to perform the robust initial alignment method for a pipeline submerged robot described above.
In the embodiments disclosed herein, a computer storage medium may be a tangible medium that can contain, or store a program for use by or in connection with an instruction execution system, apparatus, or device. The computer storage medium may include, but is not limited to, an electronic, magnetic, optical, electromagnetic, infrared, or semiconductor system, apparatus, or device, or any suitable combination of the foregoing. More specific examples of a computer storage medium would include one or more wire-based electrical connections, a portable computer diskette, a hard disk, a Random Access Memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or flash memory), an optical fiber, a portable compact disc read-only memory (CD-ROM), an optical storage device, a magnetic storage device, or any suitable combination of the foregoing.
Those of ordinary skill in the art will appreciate that the various illustrative elements and algorithm steps described in connection with the embodiments disclosed herein may be implemented as electronic hardware or combinations of computer software and electronic hardware. Whether such functionality is implemented as hardware or software depends upon the particular application and design constraints imposed on the solution. Skilled artisans may implement the described functionality in varying ways for each particular application, but such implementation decisions should not be interpreted as causing a departure from the scope of the present application.
The present invention is not limited to the preferred embodiments, and any simple modification, equivalent replacement, and improvement made to the above embodiments by those skilled in the art without departing from the technical scope of the present invention, will fall within the scope of the present invention.
Claims (10)
1. A robust initial alignment method for a pipeline submerged robot is characterized in that: the method comprises the following steps:
step one, acquiring the moving direction of the underwater robot;
calculating the optical flow speed of the underwater robot based on the moving direction of the underwater robot and estimating the underwater operation speed of the underwater robot under a machine body coordinate system;
step three, calculating an initial attitude angle of the underwater robot based on the underwater operation speed of the underwater robot;
step four, obtaining real-time attitude angle information;
and step five, feeding back the real-time attitude angle information in the step four to the step three to obtain accurate initial attitude angle information.
2. The robust initial alignment method for a pipeline submersible robot of claim 1, wherein: a camera is mounted in front of the underwater vehicle, and a lens of the camera is always maintained in the movement direction of the underwater vehicle; the underwater robot is provided with a main control board; the inertial measurement unit IMU is integrated with the triaxial accelerometer and the triaxial gyroscope in the submarine robot and is used for measuring the acceleration and angular velocity information of the submarine robot; the underwater robot is provided with a DVL sensor, and the real-time speed of the underwater robot is measured based on the Doppler effect.
3. The robust initial alignment method for a pipeline submersible robot of claim 2, wherein: in the first step, the main control board collects moving image data of the outside world through the camera module, and the moving direction of the submarine robot is determined by combining the relation between the sports field and the optical flow field and adopting the principle of the main moving direction.
4. A robust initial alignment method for a pipeline submersible robot according to claim 3, wherein: analyzing frame data of the moving image at different moments, extracting corner information of the moving image based on a Harris corner detection algorithm, constructing a moving image pyramid model according to the corner information, calculating the optical flow speed of the submerged robot by using a Lucas-Kanade optical flow method, and estimating the submerged running speed of the submerged robot under a machine body coordinate system in a coordinate mapping mode.
5. The robust initial alignment method for a pipeline submersible robot of claim 2, wherein: and thirdly, constructing an inertial navigation coarse alignment equation, and calculating an initial attitude angle of the underwater robot by using an initial alignment QMETHod method based on quaternion by combining the obtained underwater operation speed and the physical quantity of the acceleration and the angular speed output by the inertial measurement unit IMU after carrying out numerical discretization.
6. The robust initial alignment method for a pipeline submersible robot of claim 2, wherein: and step four, deriving a nonlinear Kalman filtering state equation and an observation equation by combining the submarine operation speed under the submarine robot body coordinate system output by the DVL sensor, and estimating real-time attitude errors and zero offset of the gyroscope by using the Kalman filter to obtain real-time attitude angle accurate information.
7. The robust initial alignment method for a pipeline submersible robot of claim 2, wherein: inertial measurement unit IMU acquisitionUsing a laser strapdown inertial navigation system; the zero bias stability of the triaxial gyroscope and the triaxial accelerometer are respectively 0.01 degree/h and 10 degree -5 g; the DVL sensor employs an a50 acoustic doppler log.
8. A robust initial alignment system for a pipeline submerged robot, comprising:
the moving direction acquisition module is used for acquiring the moving direction of the underwater robot;
the speed calculation module is used for calculating the optical flow speed of the underwater robot based on the moving direction of the underwater robot and estimating the underwater operation speed of the underwater robot under the machine body coordinate system;
the initial attitude angle calculation module is used for calculating the initial attitude angle of the underwater robot based on the underwater operation speed of the underwater robot and is used for calculating the initial attitude angle of the underwater robot;
the accurate initial attitude angle acquisition module is used for acquiring real-time attitude angle information, feeding the real-time attitude angle information back to the initial attitude angle calculation module, returning to the operation executed by the initial attitude angle calculation module, and calculating to obtain the accurate initial attitude angle information.
9. An electronic device, comprising: memory, a processor and a computer program stored on the memory and executable on the processor, which processor, when executing the computer program, implements a robust initial alignment method for a pipeline submersible robot according to any of claims 1-7.
10. A computer readable storage medium storing a computer program for causing a computer to perform the robust initial alignment method for a pipeline submerged robot according to any of claims 1-7.
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202211623456.4A CN116295511B (en) | 2022-12-16 | 2022-12-16 | Robust initial alignment method and system for pipeline submerged robot |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202211623456.4A CN116295511B (en) | 2022-12-16 | 2022-12-16 | Robust initial alignment method and system for pipeline submerged robot |
Publications (2)
Publication Number | Publication Date |
---|---|
CN116295511A true CN116295511A (en) | 2023-06-23 |
CN116295511B CN116295511B (en) | 2024-04-02 |
Family
ID=86776838
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202211623456.4A Active CN116295511B (en) | 2022-12-16 | 2022-12-16 | Robust initial alignment method and system for pipeline submerged robot |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN116295511B (en) |
Cited By (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN116592896A (en) * | 2023-07-17 | 2023-08-15 | 山东水发黄水东调工程有限公司 | Underwater robot navigation positioning method based on Kalman filtering and infrared thermal imaging |
CN116954225A (en) * | 2023-07-28 | 2023-10-27 | 南京安透可智能系统有限公司 | System and method for avoiding obstacle of submarine in urban pipeline environment based on multi-beam sonar |
CN116958439A (en) * | 2023-07-28 | 2023-10-27 | 南京安透可智能系统有限公司 | Pipeline three-dimensional reconstruction method based on multi-sensor fusion in full water environment |
Citations (14)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US6094163A (en) * | 1998-01-21 | 2000-07-25 | Min-I James Chang | Ins alignment method using a doppler sensor and a GPS/HVINS |
US20070280507A1 (en) * | 2006-06-01 | 2007-12-06 | Beddhu Murali | Apparatus and Upwind Methods for Optical Flow Velocity Estimation |
CN105021212A (en) * | 2015-07-06 | 2015-11-04 | 中国人民解放军国防科学技术大学 | Initial orientation information assisted rapid transfer alignment method for autonomous underwater vehicle |
CN106989744A (en) * | 2017-02-24 | 2017-07-28 | 中山大学 | A kind of rotor wing unmanned aerial vehicle autonomic positioning method for merging onboard multi-sensor |
CN108592951A (en) * | 2018-05-30 | 2018-09-28 | 中国矿业大学 | A kind of coalcutter inertial navigation Initial Alignment Systems and method based on optical flow method |
CN109141475A (en) * | 2018-09-14 | 2019-01-04 | 苏州大学 | Initial Alignment Method between a kind of DVL auxiliary SINS robust is advanced |
CN109443379A (en) * | 2018-09-28 | 2019-03-08 | 东南大学 | A kind of underwater anti-shake dynamic alignment methods of the SINS/DVL of deep-sea submariner device |
CN110031882A (en) * | 2018-08-02 | 2019-07-19 | 哈尔滨工程大学 | A kind of outer measurement information compensation method based on SINS/DVL integrated navigation system |
CN110873577A (en) * | 2019-12-02 | 2020-03-10 | 中国人民解放军战略支援部队信息工程大学 | Underwater quick-acting base alignment method and device |
CN111536969A (en) * | 2020-04-16 | 2020-08-14 | 哈尔滨工程大学 | Small-diameter pipeline robot positioning method based on initial attitude angle self-alignment |
KR102159937B1 (en) * | 2020-07-07 | 2020-09-25 | 한화시스템 주식회사 | Unmanned submersible with navigation initial alignment function |
CN111750863A (en) * | 2020-06-18 | 2020-10-09 | 哈尔滨工程大学 | Navigation system error correction method based on auxiliary position of sea pipe node |
JP2020169953A (en) * | 2019-04-05 | 2020-10-15 | 株式会社Ihi | Method for calibrating inertia navigation device |
CN115031727A (en) * | 2022-03-31 | 2022-09-09 | 哈尔滨工程大学 | Initial alignment method of Doppler assisted strapdown inertial navigation system based on state transformation |
-
2022
- 2022-12-16 CN CN202211623456.4A patent/CN116295511B/en active Active
Patent Citations (14)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US6094163A (en) * | 1998-01-21 | 2000-07-25 | Min-I James Chang | Ins alignment method using a doppler sensor and a GPS/HVINS |
US20070280507A1 (en) * | 2006-06-01 | 2007-12-06 | Beddhu Murali | Apparatus and Upwind Methods for Optical Flow Velocity Estimation |
CN105021212A (en) * | 2015-07-06 | 2015-11-04 | 中国人民解放军国防科学技术大学 | Initial orientation information assisted rapid transfer alignment method for autonomous underwater vehicle |
CN106989744A (en) * | 2017-02-24 | 2017-07-28 | 中山大学 | A kind of rotor wing unmanned aerial vehicle autonomic positioning method for merging onboard multi-sensor |
CN108592951A (en) * | 2018-05-30 | 2018-09-28 | 中国矿业大学 | A kind of coalcutter inertial navigation Initial Alignment Systems and method based on optical flow method |
CN110031882A (en) * | 2018-08-02 | 2019-07-19 | 哈尔滨工程大学 | A kind of outer measurement information compensation method based on SINS/DVL integrated navigation system |
CN109141475A (en) * | 2018-09-14 | 2019-01-04 | 苏州大学 | Initial Alignment Method between a kind of DVL auxiliary SINS robust is advanced |
CN109443379A (en) * | 2018-09-28 | 2019-03-08 | 东南大学 | A kind of underwater anti-shake dynamic alignment methods of the SINS/DVL of deep-sea submariner device |
JP2020169953A (en) * | 2019-04-05 | 2020-10-15 | 株式会社Ihi | Method for calibrating inertia navigation device |
CN110873577A (en) * | 2019-12-02 | 2020-03-10 | 中国人民解放军战略支援部队信息工程大学 | Underwater quick-acting base alignment method and device |
CN111536969A (en) * | 2020-04-16 | 2020-08-14 | 哈尔滨工程大学 | Small-diameter pipeline robot positioning method based on initial attitude angle self-alignment |
CN111750863A (en) * | 2020-06-18 | 2020-10-09 | 哈尔滨工程大学 | Navigation system error correction method based on auxiliary position of sea pipe node |
KR102159937B1 (en) * | 2020-07-07 | 2020-09-25 | 한화시스템 주식회사 | Unmanned submersible with navigation initial alignment function |
CN115031727A (en) * | 2022-03-31 | 2022-09-09 | 哈尔滨工程大学 | Initial alignment method of Doppler assisted strapdown inertial navigation system based on state transformation |
Non-Patent Citations (4)
Title |
---|
SHUANG ZHANG; ROBERT L. STEVENSON: "Inertia Sensor Aided Alignment for Burst Pipeline in Low Light Conditions", 2018 25TH IEEE INTERNATIONAL CONFERENCE ON IMAGE PROCESSING, 6 September 2018 (2018-09-06) * |
杜红松;: "双轴调制激光惯导系统组合点校技术研究", 光学与光电技术, vol. 14, no. 04, 31 August 2016 (2016-08-31) * |
杨理践;李晖;靳鹏;高松巍;: "管道地理位置测量系统的动态初始对准方法", 沈阳工业大学学报, vol. 37, no. 06, 30 November 2015 (2015-11-30) * |
许永强,苑艳,华习超,等: "一种基于LDV/INS组合的车载高程计", 光学与光电技术, vol. 16, no. 5, 31 October 2018 (2018-10-31) * |
Cited By (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN116592896A (en) * | 2023-07-17 | 2023-08-15 | 山东水发黄水东调工程有限公司 | Underwater robot navigation positioning method based on Kalman filtering and infrared thermal imaging |
CN116592896B (en) * | 2023-07-17 | 2023-09-29 | 山东水发黄水东调工程有限公司 | Underwater robot navigation positioning method based on Kalman filtering and infrared thermal imaging |
CN116954225A (en) * | 2023-07-28 | 2023-10-27 | 南京安透可智能系统有限公司 | System and method for avoiding obstacle of submarine in urban pipeline environment based on multi-beam sonar |
CN116958439A (en) * | 2023-07-28 | 2023-10-27 | 南京安透可智能系统有限公司 | Pipeline three-dimensional reconstruction method based on multi-sensor fusion in full water environment |
CN116958439B (en) * | 2023-07-28 | 2024-02-23 | 南京安透可智能系统有限公司 | Pipeline three-dimensional reconstruction method based on multi-sensor fusion in full water environment |
CN116954225B (en) * | 2023-07-28 | 2024-03-05 | 南京安透可智能系统有限公司 | System and method for avoiding obstacle of submarine in urban pipeline environment based on multi-beam sonar |
Also Published As
Publication number | Publication date |
---|---|
CN116295511B (en) | 2024-04-02 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN116295511B (en) | Robust initial alignment method and system for pipeline submerged robot | |
CN109991636A (en) | Map constructing method and system based on GPS, IMU and binocular vision | |
CN104655131B (en) | Inertial navigation Initial Alignment Method based on ISTSSRCKF | |
CN108731670A (en) | Inertia/visual odometry combined navigation locating method based on measurement model optimization | |
CN108731672B (en) | Coal mining machine attitude detection system and method based on binocular vision and inertial navigation | |
CN108955675A (en) | A kind of underground piping track detection system and method based on inertia measurement | |
CN114526745B (en) | Drawing construction method and system for tightly coupled laser radar and inertial odometer | |
CN110260885B (en) | Satellite/inertia/vision combined navigation system integrity evaluation method | |
CN107909614A (en) | Crusing robot localization method under a kind of GPS failures environment | |
CN109540135A (en) | The method and device that the detection of paddy field tractor pose and yaw angle are extracted | |
CN112967392A (en) | Large-scale park mapping and positioning method based on multi-sensor contact | |
CN110412596A (en) | A kind of robot localization method based on image information and laser point cloud | |
CN113739795A (en) | Underwater synchronous positioning and mapping method based on polarized light/inertia/vision combined navigation | |
CN115685292B (en) | Navigation method and device of multi-source fusion navigation system | |
CN104280024B (en) | Device and method for integrated navigation of deepwater robot | |
CN113639722B (en) | Continuous laser scanning registration auxiliary inertial positioning and attitude determination method | |
CN112197765B (en) | Method for realizing fine navigation of underwater robot | |
CN113029173A (en) | Vehicle navigation method and device | |
CN112729283A (en) | Navigation method based on depth camera/MEMS inertial navigation/odometer combination | |
CN116164747B (en) | Positioning and navigation method and system for underwater robot | |
CN115356965A (en) | Loose coupling actual installation data acquisition device and data processing method | |
CN116124161A (en) | LiDAR/IMU fusion positioning method based on priori map | |
CN115574815A (en) | Non-visual environment navigation system, method, computer equipment and storage medium | |
Ben et al. | System reset of strapdown INS for pipeline inspection gauge | |
CN116958439B (en) | Pipeline three-dimensional reconstruction method based on multi-sensor fusion in full water environment |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |