CN111025229B - Underwater robot pure orientation target estimation method - Google Patents
Underwater robot pure orientation target estimation method Download PDFInfo
- Publication number
- CN111025229B CN111025229B CN201911314472.3A CN201911314472A CN111025229B CN 111025229 B CN111025229 B CN 111025229B CN 201911314472 A CN201911314472 A CN 201911314472A CN 111025229 B CN111025229 B CN 111025229B
- Authority
- CN
- China
- Prior art keywords
- target
- underwater robot
- robot
- estimation
- information
- 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.)
- Active
Links
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S3/00—Direction-finders for determining the direction from which infrasonic, sonic, ultrasonic, or electromagnetic waves, or particle emission, not having a directional significance, are being received
- G01S3/80—Direction-finders for determining the direction from which infrasonic, sonic, ultrasonic, or electromagnetic waves, or particle emission, not having a directional significance, are being received using ultrasonic, sonic or infrasonic waves
- G01S3/802—Systems for determining direction or deviation from predetermined direction
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B63—SHIPS OR OTHER WATERBORNE VESSELS; RELATED EQUIPMENT
- B63C—LAUNCHING, HAULING-OUT, OR DRY-DOCKING OF VESSELS; LIFE-SAVING IN WATER; EQUIPMENT FOR DWELLING OR WORKING UNDER WATER; MEANS FOR SALVAGING OR SEARCHING FOR UNDERWATER OBJECTS
- B63C11/00—Equipment for dwelling or working underwater; Means for searching for underwater objects
- B63C11/52—Tools specially adapted for working underwater, not otherwise provided for
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
Abstract
The invention discloses a pure azimuth target estimation method for an underwater robot, which is characterized in that in the process of passive sonar detection of the underwater robot, target pure azimuth information provided by passive sonar is utilized, and a graph optimization-based estimation module and a path planning module for ensuring observability are combined, so that the nonlinear estimation of a pure azimuth target is realized on the basis of high-precision dead reckoning of the robot. The invention can utilize continuous observation information to realize the optimal estimation in a time window in the process of executing a detection task by the underwater robot, fully utilize the mobility of the underwater robot, enhance the observability in the detection process and meet the robustness requirement of the target detection process.
Description
Technical Field
The invention belongs to the field of underwater robot application, relates to a pure azimuth target estimation method for an underwater robot, and particularly relates to a pure azimuth target estimation method for an underwater robot based on graph optimization.
Background
The underwater robot provides a new platform and a new form for underwater target detection tasks, and underwater target detection is an important application direction in the field of underwater robots. However, underwater target detection can only acquire limited target azimuth information, which brings great difficulty to target position estimation.
The method for estimating the pure azimuth target of the underwater robot comprises the steps of utilizing pure azimuth information of the target provided by passive sonar, combining an estimation module based on graph optimization and a path planning module for ensuring observability, establishing a position estimation error graph model on the basis of a high-precision dead reckoning result of the robot, and realizing the nonlinear estimation of the pure azimuth target through a nonlinear optimization algorithm.
Disclosure of Invention
Aiming at the prior art, the technical problem to be solved by the invention is to provide a pure azimuth target estimation method of an underwater robot, which improves the robustness of a target estimation algorithm and achieves the purpose of target position estimation by establishing a target estimation graph model. The method solves the pure direction estimation problem of the underwater robot in the underwater passive target detection process, and realizes the position estimation of the underwater target.
In order to solve the technical problem, the invention provides a pure azimuth target estimation method for an underwater robot, which comprises the following steps:
step 1: starting a target detection task, planning a detection path, and starting navigation of the underwater robot;
step 2: the underwater robot acquires speed information by using a Doppler velocimeter, obtains a speed value of the underwater robot under a geodetic coordinate system, and inputs the speed value into a robot dead reckoning module;
and step 3: the underwater robot acquires attitude information by using an attitude sensor, acquires an attitude value of the underwater robot in a geodetic coordinate system, and inputs the attitude information into a robot dead reckoning module;
and 4, step 4: the robot dead reckoning module adopts a dead reckoning algorithm according to the input speed information and the input attitude information to complete the estimation of the position of the robot per se, and iteratively updates variable nodes of the underwater robot position factor graph according to the output position information;
and 5: the target position information is used as a variable node, and a factor node of a factor graph is formed according to the target position information, the robot position information and the direction information of the target relative to the robot, which is acquired by the passive sonar detection module;
and 6: solving the maximum posterior estimation of the factor graph by utilizing nonlinear least squares, acquiring target position information and realizing the correction of the target position;
and 7: and judging whether the estimated target position is converged, if so, ending the target detection task, otherwise, returning to the step 2, updating the detection path according to the current position estimation information, and continuously executing the detection task.
The invention also includes:
1. and the dead reckoning module completes the calculation of the position and the posture of the underwater robot by utilizing the Doppler velocimeter and the posture sensor and combining a Kalman filter.
2. And the passive sonar detection module completes the azimuth estimation of the target noise according to the measurement information of the target noise by using the multi-element sonar array, and provides target observation data for the graph optimization estimation process.
The invention has the beneficial effects that: the method utilizes a graph optimization method to enhance the robustness in the estimation process, and combines a path planning algorithm to improve the observability of the track process, thereby providing a new means for the passive target detection of the underwater robot.
The invention can realize the optimal estimation in a time window by utilizing continuous observation information in the process of executing the detection task by the underwater robot, fully utilize the mobility of the underwater robot, enhance the observability in the detection process and meet the robustness requirement of the target detection process. The method has clear logic and simple practice.
In the process of passive sonar detection of the underwater robot, target pure azimuth information provided by the passive sonar is utilized, and nonlinear estimation of a pure azimuth target is realized on the basis of high-precision dead reckoning of the robot by combining an estimation module based on graph optimization and a path planning module for ensuring observability. The invention establishes a target estimation method based on a graph model, provides a new means for pure orientation estimation, and meets the robustness requirement of underwater target estimation.
Drawings
FIG. 1 is a block diagram of a target estimation system for an underwater robot;
FIG. 2 is a diagram of a target estimation map model architecture;
FIG. 3 is a flow chart of a target estimation method.
Detailed Description
The present invention will be described in further detail below with reference to the accompanying drawings.
With reference to fig. 1, the underwater robot target estimation system has the following structure:
as shown in fig. 1, the underwater robot target estimation system structure includes a map optimization estimation module, a path planning module, a passive sonar detection module, a dead reckoning module, and the like. The underwater robot establishes a graph model of target estimation by using target pure azimuth information provided by the passive sonar and azimuth estimation information based on the dead reckoning module, completes nonlinear optimal estimation by combining an estimation module based on graph optimization, and realizes position estimation of a pure azimuth target.
The graph optimization estimation module: and establishing a nonlinear optimization target by using the azimuth measurement information of the target and the position information of the underwater robot and adopting a factor graph optimization method to realize the optimal estimation of the target position.
And the path planning module plans a curve motion track required by the detection task according to the target position estimation, ensures the observability of the target detection process and optimizes the detection angle of the passive sonar detection module.
Passive sonar detection module: and realizing the azimuth estimation of the target noise by using the multivariate sonar array according to the measurement information of the target noise, and providing target observation data for the graph optimization estimation process.
Dead reckoning module: and the position and the posture of the underwater robot are calculated by utilizing the Doppler velocimeter and the posture sensor and combining a Kalman filter.
The nonlinear estimation module based on graph optimization utilizes a factor graph to realize mathematical abstraction of a pure azimuth target estimation problem of the underwater robot, establishes an optimization equation aiming at an azimuth angle and completes optimal estimation of a target position. The effective detection angle of the passive sonar detection module is-45 degrees, and the object confidence exceeding the detection angle is low, so that the passive sonar detection module cannot be used as effective measurement data. The position and attitude information of the underwater robot calculated by the dead reckoning module has higher accuracy, and the position and attitude accuracy requirements in the target estimation process can be met. The path planning module generates a navigation motion track which can ensure observability of the nonlinear estimation module and improve the detection confidence of the passive sonar through a planning method.
With reference to fig. 2, a model structure of an underwater robot target estimation diagram is introduced:
variable XtRepresenting the self position information of the robot at the time t, and Tar is the position estimation information of the target, namely the variable node of the factor graph according to XtAnd with Tar, acquiring the prediction information of the target azimuth, and combining the observation data of the target azimuth, wherein black solid points represent factor relations among variables, namely factor nodes of a factor graph, and represent the deviation between observation and prediction.
With reference to fig. 3, a flow of the underwater robot pure orientation target estimation method is introduced:
1. starting a target detection task, planning a detection path, and starting navigation of the underwater robot;
2. the underwater robot acquires speed information by using a Doppler velocimeter, obtains a speed value of the underwater robot under a geodetic coordinate system, and inputs the speed value into a robot dead reckoning module;
3. the underwater robot acquires attitude information by using an attitude sensor, acquires an attitude value of the underwater robot in a geodetic coordinate system, and inputs the attitude information into a robot dead reckoning module;
4. the robot dead reckoning module adopts a dead reckoning algorithm according to the input speed information and the attitude information to realize the estimation of the self position of the robot, and establishes/updates variable nodes of the underwater robot position factor graph according to the output position information;
5. taking the target position information as a variable node, and forming a factor node of a factor graph according to the target position information, the robot position information and the azimuth information of the target relative to the robot, which is acquired by the passive detection sonar;
6. solving the maximum posterior estimation of the factor graph by utilizing nonlinear least squares to obtain target position information and realize the correction of the target position;
7. and (3) judging whether the estimated target position is converged, if so, finishing the target detection task, otherwise, returning to the step (2), updating the detection path according to the current position estimation information, and continuously executing the detection task.
Claims (3)
1. A pure azimuth target estimation method of an underwater robot is characterized by comprising the following steps:
step 1: starting a target detection task, planning a detection path, and starting navigation of the underwater robot;
step 2: the underwater robot acquires speed information by using a Doppler velocimeter, obtains a speed value of the underwater robot under a geodetic coordinate system, and inputs the speed value into a robot dead reckoning module;
and step 3: the underwater robot acquires attitude information by using an attitude sensor, acquires an attitude value of the underwater robot in a geodetic coordinate system, and inputs the attitude information into a robot dead reckoning module;
and 4, step 4: the robot dead reckoning module adopts a dead reckoning algorithm according to the input speed information and the input attitude information to complete the estimation of the position of the robot per se, and iteratively updates variable nodes of the underwater robot position factor graph according to the output position information;
and 5: the target position information is used as a variable node, and a factor node of a factor graph is formed according to the target position information, the robot position information and the direction information of the target relative to the robot, which is acquired by the passive sonar detection module;
step 6: solving the maximum posterior estimation of the factor graph by utilizing nonlinear least squares to obtain target position information and realize the correction of the target position;
and 7: and judging whether the estimated target position is converged, if so, ending the target detection task, otherwise, returning to the step 2, updating the detection path according to the current estimated target position information, and continuing to execute the detection task.
2. The underwater robot pure orientation object estimation method according to claim 1, characterized in that: and the dead reckoning module completes the calculation of the position and the posture of the underwater robot by utilizing the Doppler velocimeter and the posture sensor and combining a Kalman filter.
3. The underwater robot pure orientation object estimation method according to claim 1, characterized in that: and the passive sonar detection module completes the azimuth estimation of the target noise according to the measurement information of the target noise by using the multi-element sonar array, and provides target observation data for the graph optimization estimation process.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201911314472.3A CN111025229B (en) | 2019-12-19 | 2019-12-19 | Underwater robot pure orientation target estimation method |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201911314472.3A CN111025229B (en) | 2019-12-19 | 2019-12-19 | Underwater robot pure orientation target estimation method |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN111025229A CN111025229A (en) | 2020-04-17 |
| CN111025229B true CN111025229B (en) | 2022-11-01 |
Family
ID=70210684
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201911314472.3A Active CN111025229B (en) | 2019-12-19 | 2019-12-19 | Underwater robot pure orientation target estimation method |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN111025229B (en) |
Families Citing this family (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112445243B (en) * | 2020-11-09 | 2022-02-11 | 中国科学院沈阳自动化研究所 | Submarine target searching method suitable for autonomous underwater robot |
| CN114580615B (en) * | 2022-03-04 | 2022-09-23 | 哈尔滨工程大学 | Distributed small platform underwater pure orientation positioning method based on neural network |
| CN115616602B (en) * | 2022-10-14 | 2023-08-18 | 哈尔滨工程大学 | Course determination method of observer optimal maneuver strategy based on passive sonar pure azimuth positioning detection pre-tracking algorithm |
Citations (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN2788131Y (en) * | 2005-04-08 | 2006-06-14 | 哈尔滨工程大学 | Mini-type underwater self-combination navigation equipment |
| CN107167126A (en) * | 2017-03-31 | 2017-09-15 | 大鹏高科（武汉）智能装备有限公司 | A kind of autonomous type underwater robot Combinated navigation method and system |
| CN107561544A (en) * | 2016-06-30 | 2018-01-09 | 天津广深科技有限公司 | A kind of detection method of underwater robot |
| CN108318034A (en) * | 2018-01-09 | 2018-07-24 | 浙江大学 | A kind of AUV based on sonar map times depressed place air navigation aid |
| CN108332756A (en) * | 2018-02-12 | 2018-07-27 | 西北工业大学 | A kind of submarine navigation device co-located method based on topology information |
| JP2018122762A (en) * | 2017-02-02 | 2018-08-09 | 株式会社Ihi | Underwater Search Method and Underwater Search System |
| CN108415445A (en) * | 2018-02-09 | 2018-08-17 | 西北工业大学 | A kind of submarine navigation device positioning robust Optimal methods based on switch constraint |
| CN109211240A (en) * | 2018-09-01 | 2019-01-15 | 哈尔滨工程大学 | A kind of monocular vision submarine navigation device navigator fix bearing calibration |
| CN109725650A (en) * | 2019-03-08 | 2019-05-07 | 哈尔滨工程大学 | A kind of AUV barrier-avoiding method under intensive obstacle environment |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| TW201043995A (en) * | 2009-06-03 | 2010-12-16 | Ralink Technology Corp | Method and apparatus of positioning for a wireless communication system |
-
2019
- 2019-12-19 CN CN201911314472.3A patent/CN111025229B/en active Active
Patent Citations (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN2788131Y (en) * | 2005-04-08 | 2006-06-14 | 哈尔滨工程大学 | Mini-type underwater self-combination navigation equipment |
| CN107561544A (en) * | 2016-06-30 | 2018-01-09 | 天津广深科技有限公司 | A kind of detection method of underwater robot |
| JP2018122762A (en) * | 2017-02-02 | 2018-08-09 | 株式会社Ihi | Underwater Search Method and Underwater Search System |
| CN107167126A (en) * | 2017-03-31 | 2017-09-15 | 大鹏高科（武汉）智能装备有限公司 | A kind of autonomous type underwater robot Combinated navigation method and system |
| CN108318034A (en) * | 2018-01-09 | 2018-07-24 | 浙江大学 | A kind of AUV based on sonar map times depressed place air navigation aid |
| CN108415445A (en) * | 2018-02-09 | 2018-08-17 | 西北工业大学 | A kind of submarine navigation device positioning robust Optimal methods based on switch constraint |
| CN108332756A (en) * | 2018-02-12 | 2018-07-27 | 西北工业大学 | A kind of submarine navigation device co-located method based on topology information |
| CN109211240A (en) * | 2018-09-01 | 2019-01-15 | 哈尔滨工程大学 | A kind of monocular vision submarine navigation device navigator fix bearing calibration |
| CN109725650A (en) * | 2019-03-08 | 2019-05-07 | 哈尔滨工程大学 | A kind of AUV barrier-avoiding method under intensive obstacle environment |
Non-Patent Citations (8)
| Title |
|---|
| 《A factoring algorithm for probabilistic localization in Underwater Sensor Networks》;Salwa Abougamila;《2017 IEEE International Conference on Communications (ICC)》;20170731;全文 * |
| 《UUV水下定位方法的研究现状与进展》;严浙平;《哈尔滨工程大学学报》;20170731;第38卷(第7期);全文 * |
| 《基于单信标纯方位测量的AUV水下定位方法》;李佳橦;《水下无人系统学报》;20180831;第26卷(第4期);全文 * |
| 《基于因子图的AUV多传感器组合导航算法》;马晓爽;《中国惯性技术学报》;20190831;第27卷(第4期);全文 * |
| 《基于因子图的无人机全员导航关键技术研究》;王慧哲;《导航与控制》;20170430;第16卷(第2期);全文 * |
| 《基于多源信息融合的多AUV协同导航方法研究》;党媛芳;《中国优秀博硕士学位论文全文数据库(硕士)》;20180615;I140-320 * |
| 《复杂环境下多目标定位与跟踪策略研究》;郭婧;《中国优秀博硕士学位论文全文数据库(硕士)》;20180215;I136-1143 * |
| Underwater inspection using sonar-based volumetric submaps;Pedro V. Teixeira;《2016 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)》;20161201;全文 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN111025229A (en) | 2020-04-17 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN108731670B (en) | Inertial/visual odometer integrated navigation positioning method based on measurement model optimization | |
| CN111025229B (en) | Underwater robot pure orientation target estimation method | |
| Li et al. | Deep sensor fusion between 2D laser scanner and IMU for mobile robot localization | |
| Jiancheng et al. | Study on innovation adaptive EKF for in-flight alignment of airborne POS | |
| CN108387236B (en) | Polarized light SLAM method based on extended Kalman filtering | |
| CN106772524B (en) | A kind of agricultural robot integrated navigation information fusion method based on order filtering | |
| Mu et al. | End-to-end navigation for autonomous underwater vehicle with hybrid recurrent neural networks | |
| CN108362288B (en) | Polarized light SLAM method based on unscented Kalman filtering | |
| Siegwart et al. | Autonomous mobile robots | |
| CN113763548B (en) | Vision-laser radar coupling-based lean texture tunnel modeling method and system | |
| Yousuf et al. | Information fusion of GPS, INS and odometer sensors for improving localization accuracy of mobile robots in indoor and outdoor applications | |
| CN110967017B (en) | Cooperative positioning method for rigid body cooperative transportation of double mobile robots | |
| CN116429116A (en) | Robot positioning method and equipment | |
| Moore et al. | Simultaneous local and global state estimation for robotic navigation | |
| Herath et al. | Fusion-dhl: Wifi, imu, and floorplan fusion for dense history of locations in indoor environments | |
| CN109443355B (en) | Visual-inertial tight coupling combined navigation method based on self-adaptive Gaussian PF | |
| CN109443353B (en) | Visual-inertial tight coupling combined navigation method based on fuzzy self-adaptive ICKF | |
| CN109387198A (en) | A kind of inertia based on sequential detection/visual odometry Combinated navigation method | |
| Malagon-Soldara et al. | Mobile robot localization: A review of probabilistic map-based techniques | |
| CN114046800B (en) | High-precision mileage estimation method based on double-layer filtering frame | |
| CN115984463A (en) | Three-dimensional reconstruction method and system suitable for narrow roadway | |
| CN115451946A (en) | Indoor pedestrian positioning method combining MEMS-IMU and Wi-Fi | |
| CN111025269B (en) | Underwater robot sensor installation deviation estimation method | |
| CN114415655A (en) | Inspection robot navigation control method based on improved SLAM | |
| CN113689501A (en) | Double-machine cooperative target machine positioning and tracking control method based on convergence point |
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 |