CN107990896B - Path generation method based on XYZ format map - Google Patents
Path generation method based on XYZ format map Download PDFInfo
- Publication number
- CN107990896B CN107990896B CN201610944485.9A CN201610944485A CN107990896B CN 107990896 B CN107990896 B CN 107990896B CN 201610944485 A CN201610944485 A CN 201610944485A CN 107990896 B CN107990896 B CN 107990896B
- Authority
- CN
- China
- Prior art keywords
- point
- map
- map information
- coordinates
- navigation point
- 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
- 238000000034 method Methods 0.000 title claims abstract description 30
- 238000001514 detection method Methods 0.000 abstract description 2
- 238000006243 chemical reaction Methods 0.000 description 2
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 230000003252 repetitive effect Effects 0.000 description 1
Images
Classifications
-
- 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
- G01C21/203—Specially adapted for sailing ships
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Navigation (AREA)
- Geophysics And Detection Of Objects (AREA)
Abstract
The invention relates to the technical field of underwater robots, in particular to a path generation method based on an XYZ format map, which is used for realizing path planning of an autonomous underwater robot in a changing marine environment. The method comprises the following steps: reading a map file in an XYZ format to obtain map information; loading the obtained map information into map conversion software, and identifying the map information according to the map information; the coordinates of the next navigation point are planned according to the starting point of the submersible, usually the polar coordinates are used for the next navigation point, and the polar coordinates are converted into a geodetic coordinate system. Compared with a simple path planning method, the method has better performance, has more advantages in a hydrothermal detection environment, can adapt to the change of an external environment, and improves the working capacity of the AUV. The method is convenient to transplant and can be suitable for various underwater robots.
Description
Technical Field
The invention relates to the technical field of underwater robots, in particular to a path generation method based on an XYZ format map.
Background
In marine applications, underwater robots play an increasingly important role. Underwater robots are divided into two categories: one is a remote control type cabled underwater Robot (ROV) and the other is an autonomous underwater robot (AUV). The ROV needs to be supported by a mother ship on the water surface, is limited by the length of a cable, and has a limited working distance which is only hundreds of meters generally; the AUV carries energy and can be far away from the mother ship, and the movement distance reaches dozens of kilometers or even hundreds of kilometers. Therefore, the research of the AUV is more and more emphasized by various countries, and the development of the AUV represents the development direction of the underwater robot in the future. However, the AUV control method is more complicated than the ROV control method, and the AUV can exert a strong operation capability only by designing a good control method.
The AUV performs terrain surveying, and pipeline tracking, all rely on path planning by the AUV. Usually, before the AUV works, a map of the current sea area is acquired, planning is performed according to the map, and when the terrain fluctuation is not very large, a simple fixed-height mode and a fixed-depth mode can be used for navigation.
Disclosure of Invention
Aiming at the defects in the prior art, the invention aims to provide the path generation method based on the XYZ format map, so that the path planning process is simpler, and the mission executed by the AUV meets the task requirement.
The technical scheme adopted by the invention for realizing the purpose is as follows: a path generation method based on an XYZ format map comprises the following steps:
reading a map file in an XYZ format to obtain map information;
and carrying out map information identification according to the map information, planning the coordinates of the next navigation point of the submersible, and converting the polar coordinates of the navigation point into geodetic coordinates.
The map information includes longitude, latitude, depth/altitude.
The map information identification is carried out according to the map information, and the planning of the coordinates of the next navigation point of the submersible comprises the following steps:
1) according to the selected target depth, an isobath is obtained, meanwhile, rays are generated according to eight directions of true east, true south, true west, true north, southeast, northeast, southwest and northwest according to the selected preset starting point, and the gradient of the rays in each direction is calculated;
2) selecting a point with the minimum absolute value as a next navigation point according to the gradient of the ray;
3) and taking the next navigation point as a preset starting point, returning to the step 1) until the current depth reaches a threshold value or a path coverage map formed by the navigation points.
The gradient of the ray is calculated by:
if the function t is f (x, y, z) at the point (x)0,y0,z0) Can be differentiated, the function F (x) is at the point (x)0,y0,z0) The directional derivative in either direction exists, whereinEach derivative is at a point (x)0,y0,z0) The directional derivative calculation formula can be expressed as:
wherein,
wherein, gradt is called gradient, i, j, k represent unit vector on x axis, y axis, z axis respectively; e denotes the unit vector, cos α, cos β and cos γ are the directional cosines of direction l.
The step of converting the polar coordinates of the navigation point into geodetic coordinates specifically includes:
latitude and longitude coordinates (a) of navigation point1,b1) Comprises the following steps:
wherein, (a, b) is longitude and latitude coordinates of the starting point, a is longitude, b is latitude, rho is the distance from the starting point to the navigation point, and theta is an included angle between a connecting line from the starting point to the navigation point and the due north direction; r is the radius of the earth.
The invention has the following advantages and beneficial effects:
1. the model of the submersible is not sensitive, and any submersible can be completely suitable.
2. Maps of other formats may also be extended in this way, with different decoding only when the format is read.
3. Compared with a simple path planning method, the method has better performance, has more advantages in a hydrothermal detection environment, can adapt to the change of an external environment, and improves the working capacity of the AUV. The method is convenient to transplant and can be suitable for various underwater robots.
Drawings
FIG. 1 is a map displayed by a software interface according to an embodiment of the present invention;
FIG. 2 is a flow chart of the method of the present invention.
Detailed Description
The present invention will be described in further detail with reference to the accompanying drawings and examples.
The invention relates to the technical field of underwater robots, in particular to a path generation method based on an XYZ format map, which is used for realizing path planning of an autonomous underwater robot in a changing marine environment. The method comprises the following steps: reading a map file in an XYZ format to obtain map information; loading the obtained map information into map conversion software, and identifying the map information according to the map information; the coordinates of the next navigation point are planned according to the starting point of the submersible, usually the polar coordinates are used for the next navigation point, and the polar coordinates are converted into a geodetic coordinate system.
The hardware requirement of the invention is a computer, which loads a program capable of reading files in XYZ format, displays the obtained geographic information by using the program compiled by the method, and simultaneously selects a starting point, selects a path length and generates a plan.
The invention obtains the terrain information by reading the map file with XYZ format: longitude, latitude, depth/height, and automatically generating a path of the same depth/height by extracting coordinate points of the same depth or the same height, and simultaneously covering the current sea area. And viewing the map by using software, simultaneously saving the map into a required XYZ format, and loading the obtained map information into map conversion software.
A path generation method based on an XYZ format map comprises the following steps:
reading a map file in an XYZ format to obtain map information;
according to the map information, map information identification is carried out;
loading the obtained map information into map conversion software;
and planning the coordinates of the next navigation point according to the starting point of the submersible, and converting the polar coordinates of the navigation point into a geodetic coordinate system.
The topographic information includes longitude, latitude, depth/altitude.
The map information identification comprises the following steps:
according to the selected target depth, an isobath is obtained, meanwhile, rays are generated according to eight directions of true east, true south, true west, true north, southeast, northeast, southwest and northwest according to the selected preset starting point, and the gradient of the rays in each direction is calculated;
selecting a point with the minimum absolute value as a next navigation point according to the gradient of the ray;
and continuously calculating the next navigation point according to the method until the task requirement of detecting the current depth or covering the map is met.
The gradient of the ray is calculated by:
if the function t is f (x, y, z) at the point (x)0,y0,z0) Can be differentiated, the function F (x) is at the point (x)0,y0,z0) The directional derivatives in either direction exist, where each derivative is at a point (x)0,y0,z0) The directional derivative calculation formula can be expressed as:
wherein,
gradt is called gradient.
The obtained polar coordinates of the navigation point are converted into a geodetic coordinate system, which specifically comprises the following steps:
latitude and longitude coordinates (a) of the next navigation point1,b1) Comprises the following steps:
wherein, (a, b) is longitude and latitude coordinates of the starting point, a is longitude, b is latitude, rho is the distance from the starting point to the navigation point, and theta is an included angle between a connecting line from the starting point to the navigation point and the due north direction;
and (4) obtaining the longitude and latitude coordinates of all navigation points by analogy according to the formulas (2) and (3).
As shown in fig. 2, firstly initializing a certain program, loading the acquired map in XYZ format into the program, displaying the map similar to that shown in fig. 1 on a software interface, knowing the depth/height information in the current chart according to the interface, selecting a water entry point and making a gradient rule to generate a path, wherein at the moment, three conditions 1. completely realizing full coverage according to the task purpose; no repetitive route; 2. full coverage is achieved, but with coincident paths; 3. full coverage cannot be achieved; the trapping is locally minimal.
For the above three cases, the following processing is respectively performed:
case 1: the task requirements are met; no treatment is performed.
Case 2: according to the current map information; finding a starting point of the coincident path; simultaneously modifying the map information; and planning the path again according to the rules at the starting point of the overlapped path until the first condition is met.
Case 3: if the local minimum is trapped, a proper starting point needs to be selected from the global map, so that the situation that the local minimum is trapped and the global optimum is obtained is avoided. Proceed according to the rules until scenario 1 is obtained.
The method for automatically generating the path in the invention comprises a map information identification and coordinate conversion method, and the following details are described as follows:
the map information identification in the invention is that according to the existing map information, according to the selected target depth, an equal-depth line is obtained, meanwhile, according to the selected preset starting point, rays are generated according to eight directions of true east, true south, true west, true north, south east, north, south, west and north, and according to the gradient of the rays, the next navigation point is calculated.
If the function t is f (x, y, z) at the point (x)0,y0,z0) Can be differentiated, the function F (x) is at the point (x)0,y0,z0) The directional derivatives in either direction exist, where each derivative is at a point (x)0,y0,z0) The directional derivative calculation formula can be expressed as:
where α, β, γ respectively denote a rotation angle from the x-axis to the direction l, a rotation angle from the y-axis to the direction l, and a rotation angle from the z-axis to the direction l, and e denotes a unit vector.
gradt is called gradient. i is the unit vector of the x-axis; j is a unit vector on the y-axis; k is a unit vector on the z-axis. cos α, cos β and cos γ are the directional cosines of direction l.
The coordinate conversion method in the invention converts the polar coordinates into a geodetic coordinate system.
Knowing the coordinates of the starting point (a, b), a being longitude and b being latitude; the next waypoint coordinate is (ρ, θ). Rho is the distance from the starting point to the navigation point, and theta is the included angle between the connecting line from the starting point to the navigation point and the due north direction.
Let the navigation point longitude and latitude coordinate be (a)1,b1)
And obtaining all navigation points by analogy according to the formulas (2) and (3). The target depth value of the navigation point is the target depth of the starting point. R represents the earth radius.
Claims (4)
1. A path generation method based on an XYZ format map is characterized by comprising the following steps:
reading a map file in an XYZ format to obtain map information;
carrying out map information identification according to the map information, planning the coordinate of the next navigation point of the submersible, and converting the polar coordinate of the navigation point into a geodetic coordinate;
the map information identification is carried out according to the map information, and the planning of the coordinates of the next navigation point of the submersible comprises the following steps:
1) according to the selected target depth, an isobath is obtained, meanwhile, rays are generated according to eight directions of true east, true south, true west, true north, southeast, northeast, southwest and northwest according to the selected preset starting point, and the gradient of the rays in each direction is calculated;
2) selecting a point with the minimum absolute value as a next navigation point according to the gradient of the ray;
3) and taking the next navigation point as a preset starting point, returning to the step 1) until the current depth reaches a threshold value or a path coverage map formed by the navigation points.
2. The XYZ-format-based path generation method of claim 1, wherein the map information includes longitude, latitude, depth/altitude.
3. The XYZ-format-based path generation method of claim 1, wherein the gradient of the ray is calculated by the following equation:
if the function t is f (x, y, z) at the point (x)0,y0,z0) Can be differentiated, the function F (x) is at the point (x)0,y0,z0) The directional derivatives in either direction exist, where each derivative is at a point (x)0,y0,z0) The directional derivative calculation formula can be expressed as:
wherein,
wherein, gradt is called gradient, i, j, k represent unit vector on x axis, y axis, z axis respectively; e denotes the unit vector, cos α, cos β and cos γ are the directional cosines of direction l.
4. The XYZ-format-map-based path generation method according to claim 1, wherein the converting the polar coordinates of the waypoint into geodetic coordinates is specifically:
latitude and longitude coordinates (a) of navigation point1,b1) Comprises the following steps:
wherein, (a, b) is longitude and latitude coordinates of the starting point, a is longitude, b is latitude, rho is the distance from the starting point to the navigation point, and theta is an included angle between a connecting line from the starting point to the navigation point and the due north direction; r is the radius of the earth.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201610944485.9A CN107990896B (en) | 2016-10-26 | 2016-10-26 | Path generation method based on XYZ format map |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201610944485.9A CN107990896B (en) | 2016-10-26 | 2016-10-26 | Path generation method based on XYZ format map |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN107990896A CN107990896A (en) | 2018-05-04 |
| CN107990896B true CN107990896B (en) | 2021-06-01 |
Family
ID=62029010
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201610944485.9A Active CN107990896B (en) | 2016-10-26 | 2016-10-26 | Path generation method based on XYZ format map |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN107990896B (en) |
Families Citing this family (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109520505B (en) * | 2018-12-03 | 2022-11-25 | 中国北方车辆研究所 | Autonomous navigation topological map generation method |
Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101436074A (en) * | 2008-12-06 | 2009-05-20 | 中国海洋大学 | Autonomous type underwater robot by simultaneous positioning and map constructing method |
| CN103968838A (en) * | 2014-05-09 | 2014-08-06 | 哈尔滨工程大学 | Co-location method of AUVs (Autonomous Underwater Vehicles) in curvilinear motion state based on polar coordinate system |
| CN103968841A (en) * | 2014-06-03 | 2014-08-06 | 哈尔滨工程大学 | Improved fireflyalgorithm based AUV (autonomous underwater vehicle) three-dimensional track planning method |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20150134158A1 (en) * | 2013-11-12 | 2015-05-14 | Conocophillips Company | Ice alert system and method of evacuating an arctic floating platform from a hazardous ice condition |
-
2016
- 2016-10-26 CN CN201610944485.9A patent/CN107990896B/en active Active
Patent Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101436074A (en) * | 2008-12-06 | 2009-05-20 | 中国海洋大学 | Autonomous type underwater robot by simultaneous positioning and map constructing method |
| CN103968838A (en) * | 2014-05-09 | 2014-08-06 | 哈尔滨工程大学 | Co-location method of AUVs (Autonomous Underwater Vehicles) in curvilinear motion state based on polar coordinate system |
| CN103968841A (en) * | 2014-06-03 | 2014-08-06 | 哈尔滨工程大学 | Improved fireflyalgorithm based AUV (autonomous underwater vehicle) three-dimensional track planning method |
Non-Patent Citations (1)
| Title |
|---|
| 基于电子海图的智能潜水器路径规划研究;孙俊岭;《海洋工程》;20111130;第136第4段、第137页第2段 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN107990896A (en) | 2018-05-04 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN108362281B (en) | Long-baseline underwater submarine matching navigation method and system | |
| JP2018155765A (en) | Compensation of magnetic data for autonomous underwater vehicle mapping surveys | |
| Chen et al. | Review of AUV underwater terrain matching navigation | |
| CN106679662B (en) | A kind of underwater robot list beacon Combinated navigation method based on TMA technology | |
| CN210719199U (en) | Multi-equipment combined navigation system of underwater robot | |
| KR20110116842A (en) | Electronic navigational chart display method of vessels navigation system using augmented reality | |
| CN111090283B (en) | Unmanned ship combined positioning and orientation method and system | |
| CN110806209A (en) | Underwater robot multi-device combined navigation system and method | |
| CN113218386B (en) | Method and device for high-precision navigation of robot in liquid building | |
| CN107831515A (en) | Underwater Navigation method and system | |
| CN109425347A (en) | Positioning and map constructing method while a kind of unmanned boat partly latent | |
| Liu et al. | Tightly coupled modeling and reliable fusion strategy for polarization-based attitude and heading reference system | |
| CN106767749B (en) | A kind of path generating method based on GRD format map | |
| CN111091622A (en) | Unmanned aerial vehicle inspection route construction method | |
| CN107990896B (en) | Path generation method based on XYZ format map | |
| US9581695B2 (en) | Generating a map using radar data | |
| Wang et al. | Terrain matching localization for hybrid underwater vehicle in the Challenger Deep of the Mariana Trench | |
| Lager et al. | Underwater terrain navigation using standard sea charts and magnetic field maps | |
| US20170254915A1 (en) | Earth surveying with two or more marine vehicles for improved drilling applications | |
| US11977201B2 (en) | Integrated detection method of electromagnetic searching, locating and tracking for subsea cables | |
| Mindell et al. | New archaeological uses of autonomous underwater vehicles | |
| Xanthidis et al. | Towards multi-robot shipwreck mapping | |
| Pfingsthorn et al. | Full 3D navigation correction using low frequency visual tracking with a stereo camera | |
| Naus | Electronic navigational chart as an equivalent to image produced by hypercatadioptric camera system | |
| Inzartsev et al. | Integrated positioning system of autonomous underwater robot and its application in high latitudes of arctic zone |
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 |