CN112082558A - UUV submarine topography tracking path rolling generation method based on polynomial fitting - Google Patents
UUV submarine topography tracking path rolling generation method based on polynomial fitting Download PDFInfo
- Publication number
- CN112082558A CN112082558A CN202010959787.XA CN202010959787A CN112082558A CN 112082558 A CN112082558 A CN 112082558A CN 202010959787 A CN202010959787 A CN 202010959787A CN 112082558 A CN112082558 A CN 112082558A
- Authority
- CN
- China
- Prior art keywords
- uuv
- tracking
- local
- path
- terrain
- 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.)
- Pending
Links
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
-
- 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
- G01S15/00—Systems using the reflection or reradiation of acoustic waves, e.g. sonar systems
- G01S15/66—Sonar tracking systems
-
- 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
- G01S15/00—Systems using the reflection or reradiation of acoustic waves, e.g. sonar systems
- G01S15/88—Sonar systems specially adapted for specific applications
- G01S15/89—Sonar systems specially adapted for specific applications for mapping or imaging
-
- 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
- G01S15/00—Systems using the reflection or reradiation of acoustic waves, e.g. sonar systems
- G01S15/88—Sonar systems specially adapted for specific applications
- G01S15/93—Sonar systems specially adapted for specific applications for anti-collision purposes
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Computer Networks & Wireless Communication (AREA)
- Acoustics & Sound (AREA)
- Automation & Control Theory (AREA)
- Measurement Of Velocity Or Position Using Acoustic Or Ultrasonic Waves (AREA)
Abstract
The invention belongs to the field of UUV navigation motion path planning, and particularly relates to a method for generating a UUV submarine topography tracking rolling path based on polynomial fitting. The method for tracking the UUV submarine topography based on deliberate path rolling generation can realize the tracking of unknown complex submarine topography, the safety of the planned and generated tracking path is good, and the collision with the submarine topography can be avoided to the greatest extent. The invention adopts a polynomial fitting method to generate the tracking path, can still generate an ideal tracking path under the condition that sonar detection submarine topography information is incomplete, and realizes effective height-fixed tracking of UUV to the submarine topography. The method does not need prior knowledge of the submarine topography, realizes the unification of online detection, planning and tracking of the UUV on the submarine topography, has small calculated amount and good real-time performance, and is beneficial to engineering realization.
Description
Technical Field
The invention belongs to the field of UUV navigation motion path planning, and particularly relates to a method for generating a UUV submarine topography tracking rolling path based on polynomial fitting.
Background
When the UUV executes the tasks of submarine topography survey and submarine target search, in order to obtain good topography survey and target search effects, the UUV needs to adopt a navigation mode of submarine topography tracking to obtain good operation effects. In the process of tracking the submarine topography of the UUV, the UUV is required to keep constant height of the fluctuating submarine topography on one hand, and the UUV is required not to collide with the fluctuating submarine topography on the other hand. In addition, as the submarine topography is unknown, the UUV must acquire the submarine topography information on line by using the configured detection sonar, thereby realizing the submarine topography tracking based on-line detection. UUV submarine topography tracking methods are mainly divided into two categories, namely reactive topography tracking and deliberate topography tracking. The reactive terrain tracking means that the UUV directly generates a motion control instruction according to the submarine terrain information detected on line so as to realize submarine terrain tracking. Deliberate terrain tracking means that a UUV plans and generates a terrain tracking path according to seabed terrain information detected on line, and then the UUV tracks the path to realize seabed terrain tracking. Compared with reactive terrain tracking, deliberate terrain tracking can realize the tracking of more complex submarine terrain, the safety of the generated tracking path is good, and the collision with the submarine terrain can be avoided to the greatest extent.
The deliberate terrain tracking mainly solves two problems, one is how to generate a tracking path on line by utilizing detected submarine terrain information, and the other is how to generate the tracking path in a rolling manner aiming at the condition that only local submarine terrain can be obtained by online detection. The method comprises the steps of detecting submarine topography by using a forward-looking multi-beam sonar configured by a UUV, solving a local topography point set according to the detection point set, then carrying out height-setting affine on the basis of the local topography point set to form a local topography affine point set, planning and generating a local tracking path by adopting a polynomial fitting method on the basis of the local topography affine point set, carrying out local tracking on the local tracking path by the UUV, repeating the steps after the local tracking is finished, and finally realizing effective and safe tracking on the submarine topography.
Patent document No. 201410619398.7 discloses "height-fixed navigation system and navigation method for UUV to approach to seabed operation", which realizes height-fixed navigation of the seabed topography when UUV operates near the seabed, and essentially realizes seabed topography tracking. First, the present invention uses two single-beam sonars for submarine topography information detection, unlike the present invention which uses a forward looking multi-beam sonar for submarine topography information detection. Secondly, the method for realizing the UUV submarine topography tracking is different from the method, belongs to a reactive submarine topography tracking method, and realizes the submarine topography tracking by solving the height motion instruction of the UUV. The invention belongs to a deliberate submarine topography tracking method, which is characterized in that fitting rolling is carried out on detected submarine topography information to generate a local tracking path, and then a UUV continuously tracks the local tracking path to realize submarine topography tracking.
The patent document with the application number of 201910618905.8 discloses a UUV submarine topography tracking system and a tracking method based on topographic feature extraction, and submarine topography tracking is achieved. Firstly, the invention uses three single-beam sonars to carry out combined detection on submarine topographic information, which is different from the method for detecting submarine topographic information by using a forward-looking multi-beam sonar. The invention discloses a method for realizing UUV submarine topography tracking, which is different from the method, belongs to a reactive submarine topography tracking method, extracts submarine topography characteristics by using submarine topography information jointly detected by three single-beam sonars, and then generates a height motion instruction based on different submarine characteristics to realize submarine topography tracking. The invention belongs to a deliberate submarine topography tracking method, which is characterized in that fitting rolling is carried out on detected submarine topography information to generate a local tracking path, and then a UUV continuously tracks the local tracking path to realize submarine topography tracking.
Disclosure of Invention
The invention aims to provide a rolling generation method of a UUV submarine topography tracking path based on polynomial fitting, which can be applied to UUV to effectively and safely track complex submarine topography.
The purpose of the invention is realized by the following technical scheme: the method comprises the following steps:
the method comprises the following steps: and initializing parameter setting. Setting expected sailing height h of UUV submarine topography trackingp(unit: meter), setting the safe height h of UUV from the sea bottomsafe(unit: meter), XEZ of the set vertical plane fixes the longitude and latitude lon of the origin of the coordinate system0(unit: degree) and lat0(unit: degree), setting the longitude and latitude lon of the task end pointE(unit: degree) and latE(unit: degree) setting the maximum floating pitch angle thetamax(unit: degree), set the maximum tracking time T of local tracking pathmax(unit: second).
Step two: UUV terrain tracking is started, and information measurement is carried out. The UUV obtains the current longitude and latitude lon by the measurement of the configured inertial navigatoruuv、latuuv(unit: degree), current depth z is measured by UUV configured depth gaugeuuv(unit: meter), measuring by a configured attitude sensor to obtain a current pitch angle theta (unit: degree), and measuring by a configured height measuring sonar to obtain a current height h from the sea floordet(unit: meter), obtaining a landform detection point set by configured forward-looking multi-beam sonar measurement
Step three: solving the vertical plane coordinate (x) of the UUV under a fixed coordinate systemuuv,zuuv)。
Step five: and (5) carrying out height-fixing affine on the local terrain point set A, and solving a local terrain affine point set P.
Step six: a polynomial fit is made from the set of local terrain affine points P to generate a local tracking path z (x).
Step seven: and the UUV adopts a general line-of-sight path tracking control method to carry out local tracking on the generated local tracking path z (x).
Step eight: and judging whether the risk of collision with the submarine topography exists by the UUV, if so, turning to the ninth step, and if not, turning to the tenth step.
Step nine: and the UUV carries out collision avoidance response. At the moment, UUV stops path tracking control, and the maximum floating trim angle theta is usedmaxAnd (5) emergently floating to avoid collision, and then rotating to the eighth step.
Step ten: and the UUV judges whether the task end point is reached. Judging whether the current longitude and latitude of the UUV reaches the task end point longitude and latitude lonEAnd latE. And if the task end is reached, turning to the step thirteen, and otherwise, turning to the step eleven.
Step eleven: and the UUV judges whether the local path tracking is finished or not. The judgment method is that the UUV reaches the last path point of the local tracking path point set. If the local path tracking is finished, turning to the step two, and if not, turning to the step twelve.
Step twelve: the UUV determines whether the local path trace is out of time. The judgment method is that the local path tracking time of the UUV exceeds the maximum tracking time T of the specified local tracking pathmax. If yes, turning to the step two, otherwise, turning to the step seven.
Step thirteen: and finishing the UUV submarine topography tracking.
The invention relates to a rolling generation method of a UUV submarine topography tracking path based on polynomial fitting, which further comprises the following steps:
(1) vertical plane coordinate (x) of UUV in step three under fixed coordinate systemuuv,zuuv) The calculation method comprises the following steps:
xuuv=(lonuuv-lon0) 1852 · 60, wherein xuuvIn meters.
zuuvThe UUV depth is measured by a depth meter, and the current depth is measured in meters.
(2) Step four-middle terrain detection point setThe method for calculating the local topographic point set A comprises the following steps:
step four (a): detecting point sets from terrainExtracting effective topographic survey point setThe extraction method comprises the following steps: collecting landform detection pointsMeasured distance d between UUV and the submarine topographyiExtracting topographic points obtained by effective detection beams not equal to 0 to obtain an effective topographic detection point setWherein the landform detection point setComprises the following steps: the terrain detection point information of the ith wave beam of the forward-looking wave beam sonar is shown, and n represents the number of wave beams of the forward-looking multi-wave beam sonar. Effective topography detection point set m represents the number of terrain points obtained by the active probe beam.
Step four (B): detecting a set of points from an effective terrainAnd solving a local topographic point set A. The calculation method comprises the following steps:
A={A1,A2,…Amin which A isj=[xj,zj]And is provided with
(3) In the fifth step, the method for performing height-fixing affine on the local topographic point set A and calculating the local topographic affine point set P comprises the following steps:
P={P1,P2,…Pmin which P isj=[xaj,zaj]And is provided with
(4) In the sixth step, polynomial fitting is carried out on the local terrain affine point set to generate a local tracking path, and the fitting generation method comprises the following steps:
z(x)=b0+b1x+b2x2+b3x3=BT[1 x x2 x3]T
wherein: x is an element of [ min (x)a1 xa2 … xam),max(xa1 xa2 … xam)]
Β=[b0 b1 b2 b3]T=(Xa TXa)-1Xa TZa
The generated local tracking path z (x) is a continuous path in a vertical plane having the coordinate x as an independent variable and the coordinate z as a dependent variable.
(5) And step eight, the method for judging whether the collision risk with the submarine topography exists is as follows:
step eight (A) UUV on-line measuring the detection height h from the seabed according to the height sonardetAnd calculating the actual height h of the UUV from the current distance to the seabeduuvThe calculation method comprises the following steps:
huuv=hdetcosθ
step eight (B) UUV according to the actual height h from the current position to the seabeduuvAnd judging whether the risk of collision with the submarine topography exists or not, wherein the judgment conditions are as follows: if h isuuv<hsafeThere is a risk of collision, otherwise there is no risk of collision.
The invention has the beneficial effects that:
the method for tracking the UUV submarine topography based on deliberate path rolling generation can realize the tracking of unknown complex submarine topography, the safety of the planned and generated tracking path is good, and the collision with the submarine topography can be avoided to the greatest extent.
The invention adopts a polynomial fitting method to generate the tracking path, can still generate an ideal tracking path under the condition that sonar detection submarine topography information is incomplete, and realizes effective height-fixed tracking of UUV to the submarine topography.
The method does not need prior knowledge of the submarine topography, realizes the unification of online detection, planning and tracking of the UUV on the submarine topography, has small calculated amount and good real-time performance, and is beneficial to engineering realization.
Drawings
FIG. 1 illustrates a schematic view of UUV sonar seafloor detection;
FIG. 2 illustrates a schematic view of UUV submarine topography tracking path rolling generation;
FIG. 3 is a flow chart of a UUV submarine topography tracking path rolling generation method based on polynomial fitting;
figure 4 shows a flow diagram of information generated by rolling a UUV seafloor terrain tracking path based on polynomial fitting.
Detailed Description
The present invention will be described in further detail below with reference to the accompanying drawings.
With reference to fig. 1, a method for detecting the submarine topography by UUV is described.
The UUV submarine topography tracking process mainly moves on a vertical plane, so that an XEZ fixed coordinate system of the vertical plane is established as shown in figure 1, wherein the origin of the fixed coordinate system is set at the water surface, and the longitude and latitude of the origin of the fixed coordinate system are set to lon0、lat0(unit: degree), depth z0Is (unit: meter). Defining the O point as the gravity center of the UUV, and processing the UUV as a mass point, so that the coordinates of the O point represent the coordinates of the UUV. The longitude and latitude of UUV is lonuuv、latuuv(in degrees) measurable by an inertial navigator configured by a UUV. UUV depth of zuuv(unit: meter), measurable by a UUV configured depth gauge. In the figure, θ (unit: degree) represents the pitch angle of the UUV, which can be measured by an attitude sensor provided in the UUV. h isdet(unit: meter) represents the height from the seafloor measured by the UUV-equipped height-finding sonar. In the figure alphai(unit: degree) represents the included angle between the ith forward-looking multi-beam sonar wave beam of UUV configuration and the central axis of UUV, and diThe distance between the UUV and the submarine topography measured by the ith beam of the forward-looking multi-beam sonar is represented by (unit: meter), and i is 1,2, … n, and n is the number of the forward-looking multi-beam sonar beams. When the ith beam cannot detect the submarine topography, the time diWhen the beam is referred to as an invalid probe beam, otherwise d is equal to 0iThe probe beam with no equal to 0 is called the effective probe beam. Then, UUV obtains the landform detection point set through forward looking multi-beam sonarComprises the following steps:
With reference to fig. 2, the general idea of UUV seafloor terrain tracking path roll generation is described.
As shown, hpThe (unit: meter) represents the expected navigation height instruction value to the seabed which needs to be maintained in the UUV seabed terrain tracking process. UUV utilizes configured height measuring sonar to measure detection height h from seabed on linedetUUV is at h from the actual height of the sea flooruuvWhen the UUV pitch angle is 0, h isuuv=hdetH when the UUV pitch angle is not 0uuv≠hdet. The UUV utilizes a forward-looking multi-beam sonar to detect the submarine topography on line. UUV submarine topography tracking is to ensure the actual height h of the UUV from the seabed on the premise of ensuring navigation safety and avoiding collision with the submarine topographyuuvIs constantly maintained at hp. The invention adopts a deliberate terrain tracking method, namely, a UUV plans and generates a terrain tracking path and tracks the path according to seabed terrain information detected on line so as to realize seabed terrain tracking. The tracking path rolling generation means that a UUV cannot generate a global terrain tracking path at a time aiming at unknown submarine terrain, a local tracking path is continuously generated in a rolling way according to the position of the UUV and the change of the terrain, and then the UUV continuously tracks the local tracking path in the rolling way. As shown in fig. 2, a UUV firstly plans and generates a local tracking path 1 according to terrain detection information for a period of time, and then performs tracking control on the local tracking path 1, when the UUV finishes tracking the local tracking path 1 or the tracking time exceeds a specified maximum tracking time TmaxAnd detecting the terrain information again to generate a local tracking path 2, then carrying out tracking control on the local tracking path 2 by the UUV, and so on to realize the rolling generation of the UUV terrain tracking path.
With reference to fig. 2, 3 and 4, a method for generating UUV ocean bottom terrain tracking path rolling based on polynomial fitting is described.
The method comprises the following steps: and initializing parameter setting. Setting expected sailing height h of UUV submarine topography trackingp(unit: meter), setting the safe height h of UUV from the sea bottomsafe(unit: meter), XEZ of the set vertical plane fixes the longitude and latitude lon of the origin of the coordinate system0(unit: degree) and lat0(unit: degree), setting the longitude and latitude lon of the task end pointE(unit:degree) and latE(unit: degree) setting the maximum floating pitch angle thetamax(unit: degree), set the maximum tracking time T of local tracking pathmax(unit: second).
Step two: UUV terrain tracking is started, and information measurement is carried out. The UUV obtains the current longitude and latitude lon by the measurement of the configured inertial navigatoruuv、latuuvThe current depth z is measured by a UUV-equipped depth gaugeuuvMeasuring to obtain a current pitch angle theta by using a configured attitude sensor, and measuring to obtain a current height h from the sea floor by using a configured height measuring sonardetA landform detection point set is obtained by the configured foresight multibeam sonar measurement
Step three: solving the vertical plane coordinate (x) of the UUV under a fixed coordinate systemuuv,zuuv) The calculation method comprises the following steps:
xuuv=(lonuuv-lon0) 1852 · 60, wherein xuuvIn meters.
zuuvThe UUV depth is measured by a depth meter, and the current depth is measured in meters.
Step four: detecting point sets from terrainAnd solving a local topographic point set A, wherein the solving method comprises the following steps:
step four (a): detecting point sets from terrainExtracting effective topographic survey point setThe extraction method comprises collecting landform detection pointsIn diExtracting the topographic point obtained by the effective detection beam not equal to 0 to obtainWherein m represents the number of terrain points obtained by the active probe beam.
Step four (B): detecting a set of points from an effective terrainAnd solving a local topographic point set A. The calculation method comprises the following steps:
A={A1,A2,…Amin which A isj=[xj,zj]And is provided with
Step five: carrying out height-fixing affine on the local terrain point set A, and solving a local terrain affine point set P, wherein the solving method comprises the following steps:
P={P1,P2,…Pmin which P isj=[xaj,zaj]And is provided with
Step six: performing polynomial fitting on the local terrain affine point set P to generate a local tracking path z (x), wherein the fitting generation method comprises the following steps:
z(x)=b0+b1x+b2x2+b3x3=BT[1 x x2 x3]T
wherein: x is an element of [ min (x)a1 xa2 … xam),max(xa1 xa2 … xam)]
Β=[b0 b1 b2 b3]T=(Xa TXa)-1Xa TZa
The generated local tracking path z (x) is a continuous path in a vertical plane having the coordinate x as an independent variable and the coordinate z as a dependent variable.
Step seven: and the UUV adopts a general line-of-sight path tracking control method to carry out local tracking on the generated local tracking path z (x).
Step eight: and judging whether the risk of collision with the submarine topography exists by the UUV, if so, turning to the ninth step, and if not, turning to the tenth step. The method for judging whether the collision risk with the submarine topography exists is as follows:
step eight (a): UUV on-line measuring detection height h from seabed according to height measuring sonardetAnd calculating the actual height h of the UUV from the current distance to the seabeduuvThe calculation method comprises the following steps:
huuv=hdetcosθ
step eight (B): UUV according to actual height h from the current sea bottomuuvAnd judging whether the risk of collision with the submarine topography exists or not, wherein the judgment conditions are as follows: if h isuuv<hsafeThere is a risk of collision, otherwise there is no risk of collision.
Step nine: and the UUV carries out collision avoidance response. At the moment, UUV stops path tracking control, and the maximum floating trim angle theta is usedmaxAnd (5) emergently floating to avoid collision, and then rotating to the eighth step.
Step ten: and the UUV judges whether the task end point is reached. Judging whether the current longitude and latitude of the UUV reaches the task end point longitude and latitude lonEAnd latE. And if the task end is reached, turning to the step thirteen, and otherwise, turning to the step eleven.
Step eleven: and the UUV judges whether the local path tracking is finished or not. The judgment method is that the UUV reaches the last path point of the local tracking path point set. If the local path tracking is finished, turning to the step two, and if not, turning to the step twelve.
Step twelve: the UUV determines whether the local path trace is out of time. The judgment method is that the local path tracking time of the UUV exceeds the specified maximum tracking time Tmax. If yes, turning to the step two, otherwise, turning to the step seven.
Step thirteen: and finishing the UUV submarine topography tracking.
Claims (6)
1. A UUV submarine topography tracking path rolling generation method based on polynomial fitting is characterized by comprising the following steps:
the method comprises the following steps: setting initialization parameters: setting expected sailing height h of UUV submarine topography trackingp(unit: meter), setting the safe height h of UUV from the sea bottomsafe(unit: meter), XEZ of the set vertical plane fixes the longitude and latitude lon of the origin of the coordinate system0(unit: degree) and lat0(unit: degree), setting the longitude and latitude lon of the task end pointE(unit: degree) and latE(unit: degree) setting the maximum floating pitch angle thetamax(unit: degree), set the maximum tracking time T of local tracking pathmax(unit: second);
step two: and starting UUV terrain tracking, and carrying out information measurement: the UUV obtains the current longitude and latitude lon by the measurement of the configured inertial navigatoruuv、latuuv(unit: degree), current depth z is measured by UUV configured depth gaugeuuv(unit: meter), measuring by a configured attitude sensor to obtain a current pitch angle theta (unit: degree), and measuring by a configured height measuring sonar to obtain a current height h from the sea floordet(unit: meter), obtaining a landform detection point set by configured forward-looking multi-beam sonar measurement
Step three: solving the vertical plane coordinate (x) of the UUV under a fixed coordinate systemuuv,zuuv);
step five: carrying out height-fixing affine on the local terrain point set A, and solving a local terrain affine point set P;
step six: performing polynomial fitting on the local terrain affine point set P to generate a local tracking path z (x);
step seven: the UUV adopts a general line-of-sight path tracking control method to carry out local tracking on the generated local tracking path z (x);
step eight: judging whether a risk of collision with the submarine topography exists by the UUV, if so, turning to the ninth step, and if not, turning to the tenth step;
step nine: and the UUV carries out collision avoidance response: at the moment, UUV stops path tracking control, and the maximum floating trim angle theta is usedmaxStep eight, emergency floating and collision avoidance, and then turning to step eight;
step ten: the UUV judges whether the task end point is reached: judging whether the current longitude and latitude of the UUV reaches the task end point longitude and latitude lonEAnd latEIf the task end point is reached, turning to the step thirteen, otherwise, turning to the step eleven;
step eleven: and the UUV judges whether the local path tracking is finished: judging whether the UUV reaches the last path point of the local tracking path point set, if so, turning to the step II, and if not, turning to the step twelve;
step twelve: the UUV judges whether the local path tracking is overtime: the judgment method is that the local path tracking time of the UUV exceeds the maximum tracking time T of the specified local tracking pathmaxIf yes, turning to the step II, otherwise, turning to the step seven;
step thirteen: and finishing the UUV submarine topography tracking.
2. The UUV seafloor terrain tracking path rolling generation method based on polynomial fitting of claim 1, further comprising: and in the third step, the vertical plane coordinate (x) of the UUV under the fixed coordinate systemuuv,zuuv) The calculation method comprises the following steps:
xuuv=(lonuuv-lon0) 1852 · 60, wherein xuuvThe unit of (a) is a meter,
zuuvthe UUV depth is measured by a depth meter, and the current depth is measured in meters.
3. The UUV seafloor terrain tracking path rolling generation method based on polynomial fitting of claim 1, further comprising: the step four-middle terrain detection point setThe method for calculating the local topographic point set A comprises the following steps:
step four (a): detecting point sets from terrainExtracting effective topographic survey point setThe extraction method comprises the following steps: collecting landform detection pointsMeasured distance d between UUV and the submarine topographyiExtracting topographic points obtained by effective detection beams not equal to 0 to obtain an effective topographic detection point setWherein the landform detection point setComprises the following steps:(i=1,2,…n),representing forward-looking beam sonar ith beamThe information of the landform detection points, n represents the number of forward-looking multi-beam sonar beams, and the effective landform detection point set(j ≦ 1,2, … m, j ≦ i, m ≦ n), m indicating the number of landform points acquired by the active probe beam;
step four (B): detecting a set of points from an effective terrainThe method for resolving the local topographic point set A comprises the following steps:
A={A1,A2,…Amin which A isj=[xj,zj]And is provided with
4. The UUV seafloor terrain tracking path rolling generation method based on polynomial fitting of claim 1, further comprising: in the fifth step, the method for performing height-fixing affine on the local topographic point set A and calculating the local topographic affine point set P comprises the following steps:
P={P1,P2,…Pmin which P isj=[xaj,zaj]And is provided with
5. The UUV seafloor terrain tracking path rolling generation method based on polynomial fitting of claim 1, further comprising: in the sixth step, polynomial fitting is performed on the local terrain affine point set to generate a local tracking path, and the fitting generation method comprises the following steps:
z(x)=b0+b1x+b2x2+b3x3=BT[1 x x2 x3]T
wherein: x is an element of [ min (x)a1 xa2 … xam),max(xa1 xa2 … xam)]
Β=[b0 b1 b2 b3]T=(Xa TXa)-1Xa TZa
The generated local tracking path z (x) is a continuous path in a vertical plane having the coordinate x as an independent variable and the coordinate z as a dependent variable.
6. The UUV seafloor terrain tracking path rolling generation method based on polynomial fitting of claim 1, further comprising: the judgment method for whether the collision risk with the submarine topography exists in the step eight comprises the following steps:
step eight (A) UUV on-line measuring the detection height h from the seabed according to the height sonardetAnd calculating the actual height h of the UUV from the current distance to the seabeduuvThe calculation method comprises the following steps:
huuv=hdetcosθ
step eight (B) UUV according to the actual height h from the current position to the seabeduuvAnd judging whether the risk of collision with the submarine topography exists or not, wherein the judgment conditions are as follows: if h isuuv<hsafeThere is a risk of collision, otherwise there is no risk of collision.
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010959787.XA CN112082558A (en) | 2020-09-14 | 2020-09-14 | UUV submarine topography tracking path rolling generation method based on polynomial fitting |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010959787.XA CN112082558A (en) | 2020-09-14 | 2020-09-14 | UUV submarine topography tracking path rolling generation method based on polynomial fitting |
Publications (1)
Publication Number | Publication Date |
---|---|
CN112082558A true CN112082558A (en) | 2020-12-15 |
Family
ID=73737702
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202010959787.XA Pending CN112082558A (en) | 2020-09-14 | 2020-09-14 | UUV submarine topography tracking path rolling generation method based on polynomial fitting |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN112082558A (en) |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
WO2024149606A1 (en) * | 2023-01-10 | 2024-07-18 | Atlas Elektronik Gmbh | Information system for an underwater vehicle for determining navigational information by means of sonar |
Citations (18)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN101788666A (en) * | 2010-03-17 | 2010-07-28 | 上海大学 | Underwater three dimensional terrain reconstruction method based on multi-beam sonar data |
CN102446367A (en) * | 2011-09-19 | 2012-05-09 | 哈尔滨工程大学 | Method for constructing three-dimensional terrain vector model based on multi-beam sonar submarine measurement data |
CN102945045A (en) * | 2012-11-19 | 2013-02-27 | 上海大学 | Unmanned ship combined obstacle avoiding device and method based on laser sensor and forward looking sonar |
CN103529844A (en) * | 2013-10-18 | 2014-01-22 | 吴宝举 | Forward-looking sonar-based underwater robot collision avoidance method |
CN105787886A (en) * | 2014-12-22 | 2016-07-20 | 中国科学院沈阳自动化研究所 | Multi-beam image sonar-based real-time image processing method |
CN106020212A (en) * | 2016-05-12 | 2016-10-12 | 哈尔滨工程大学 | Navigation switching system of UUV during submarine topography tracking process and switching method thereof |
CN106123850A (en) * | 2016-06-28 | 2016-11-16 | 哈尔滨工程大学 | AUV prestowage multibeam sonar underwater topography mapping modification method |
CN107870621A (en) * | 2016-10-25 | 2018-04-03 | 中国科学院沈阳自动化研究所 | Autonomous underwater robot collision prevention method in unknown complex sea-floor relief environment |
CN108334677A (en) * | 2018-01-17 | 2018-07-27 | 哈尔滨工程大学 | A kind of UUV Realtime collision free planing methods based on GRU networks |
CN109213204A (en) * | 2018-10-15 | 2019-01-15 | 中国海洋大学 | AUV sub-sea floor targets based on data-driven search navigation system and method |
CN109725650A (en) * | 2019-03-08 | 2019-05-07 | 哈尔滨工程大学 | A kind of AUV barrier-avoiding method under intensive obstacle environment |
CN109738928A (en) * | 2018-12-29 | 2019-05-10 | 中国科学院长春光学精密机械与物理研究所 | The satellite imagery paths planning method of space vascular envelope Distributed Three-dimensional target |
EP3492944A1 (en) * | 2017-12-01 | 2019-06-05 | Origin Wireless, Inc. | Apparatus, systems and methods for event detection and recognition based on a wireless signal |
CN110208812A (en) * | 2019-05-21 | 2019-09-06 | 哈尔滨工程大学 | Unmanned vehicles seabed dimensional topography detection device and method partly latent |
CN110456786A (en) * | 2019-07-10 | 2019-11-15 | 哈尔滨工程大学 | UUV sea-floor relief tracking system and tracking based on Extraction of Topographic Patterns |
CN110501020A (en) * | 2019-08-28 | 2019-11-26 | 湖南大学 | A kind of multiple target three-dimensional path planning method |
CN110979605A (en) * | 2020-01-04 | 2020-04-10 | 三门梅格测量仪器有限公司 | Metal detection robot used in water |
CN111615618A (en) * | 2018-12-26 | 2020-09-01 | 百度时代网络技术(北京)有限公司 | Polynomial fitting based reference line smoothing method for high speed planning of autonomous vehicles |
-
2020
- 2020-09-14 CN CN202010959787.XA patent/CN112082558A/en active Pending
Patent Citations (18)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN101788666A (en) * | 2010-03-17 | 2010-07-28 | 上海大学 | Underwater three dimensional terrain reconstruction method based on multi-beam sonar data |
CN102446367A (en) * | 2011-09-19 | 2012-05-09 | 哈尔滨工程大学 | Method for constructing three-dimensional terrain vector model based on multi-beam sonar submarine measurement data |
CN102945045A (en) * | 2012-11-19 | 2013-02-27 | 上海大学 | Unmanned ship combined obstacle avoiding device and method based on laser sensor and forward looking sonar |
CN103529844A (en) * | 2013-10-18 | 2014-01-22 | 吴宝举 | Forward-looking sonar-based underwater robot collision avoidance method |
CN105787886A (en) * | 2014-12-22 | 2016-07-20 | 中国科学院沈阳自动化研究所 | Multi-beam image sonar-based real-time image processing method |
CN106020212A (en) * | 2016-05-12 | 2016-10-12 | 哈尔滨工程大学 | Navigation switching system of UUV during submarine topography tracking process and switching method thereof |
CN106123850A (en) * | 2016-06-28 | 2016-11-16 | 哈尔滨工程大学 | AUV prestowage multibeam sonar underwater topography mapping modification method |
CN107870621A (en) * | 2016-10-25 | 2018-04-03 | 中国科学院沈阳自动化研究所 | Autonomous underwater robot collision prevention method in unknown complex sea-floor relief environment |
EP3492944A1 (en) * | 2017-12-01 | 2019-06-05 | Origin Wireless, Inc. | Apparatus, systems and methods for event detection and recognition based on a wireless signal |
CN108334677A (en) * | 2018-01-17 | 2018-07-27 | 哈尔滨工程大学 | A kind of UUV Realtime collision free planing methods based on GRU networks |
CN109213204A (en) * | 2018-10-15 | 2019-01-15 | 中国海洋大学 | AUV sub-sea floor targets based on data-driven search navigation system and method |
CN111615618A (en) * | 2018-12-26 | 2020-09-01 | 百度时代网络技术(北京)有限公司 | Polynomial fitting based reference line smoothing method for high speed planning of autonomous vehicles |
CN109738928A (en) * | 2018-12-29 | 2019-05-10 | 中国科学院长春光学精密机械与物理研究所 | The satellite imagery paths planning method of space vascular envelope Distributed Three-dimensional target |
CN109725650A (en) * | 2019-03-08 | 2019-05-07 | 哈尔滨工程大学 | A kind of AUV barrier-avoiding method under intensive obstacle environment |
CN110208812A (en) * | 2019-05-21 | 2019-09-06 | 哈尔滨工程大学 | Unmanned vehicles seabed dimensional topography detection device and method partly latent |
CN110456786A (en) * | 2019-07-10 | 2019-11-15 | 哈尔滨工程大学 | UUV sea-floor relief tracking system and tracking based on Extraction of Topographic Patterns |
CN110501020A (en) * | 2019-08-28 | 2019-11-26 | 湖南大学 | A kind of multiple target three-dimensional path planning method |
CN110979605A (en) * | 2020-01-04 | 2020-04-10 | 三门梅格测量仪器有限公司 | Metal detection robot used in water |
Non-Patent Citations (5)
Title |
---|
TAO CHEN ET AL.: "Online Receding Horizon Trajectory Planning", 《PROCEEDINGS OF THE 39TH CHINESE CONTROL CONFERENCE》 * |
李雪佳等: "基于三次多项式拟合三角函数的地理空间距离计算算法", 《计算机测量与控制》 * |
武芳: "《空间数据库原理》", 31 May 2017, 武汉大学出版社 * |
白国星等: "基于可变预测时域及速度的车辆路径跟踪控制", 《中国机械工程》 * |
鄢泓哲: "多波束测深***关键技术研究", 《中国优秀硕士学位论文全文数据库(电子期刊)基础科学辑》 * |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
WO2024149606A1 (en) * | 2023-01-10 | 2024-07-18 | Atlas Elektronik Gmbh | Information system for an underwater vehicle for determining navigational information by means of sonar |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
Johannsson et al. | Imaging sonar-aided navigation for autonomous underwater harbor surveillance | |
Ånonsen et al. | An analysis of real-time terrain aided navigation results from a HUGIN AUV | |
Chen et al. | Review of AUV underwater terrain matching navigation | |
CN111273298B (en) | Underwater acoustic target positioning and tracking method based on wave glider networking technology | |
CN113093159B (en) | Multi-beam sounding error improved model design method | |
Jacobi et al. | Multi sensor underwater pipeline tracking with AUVs | |
CN108562287A (en) | A kind of Terrain-aided Underwater Navigation based on adaptively sampled particle filter | |
CN104390646A (en) | Position matching method for underwater vehicle terrain aided inertial navigation system | |
CN109738902B (en) | High-precision autonomous acoustic navigation method for underwater high-speed target based on synchronous beacon mode | |
CN111751856B (en) | Accurate positioning method for submarine ground reference point based on PPP technology | |
CN111220146B (en) | Underwater terrain matching and positioning method based on Gaussian process regression learning | |
CN112540371A (en) | Near-bottom multi-beam coordinate conversion processing method | |
CN113688562B (en) | Automatic shallow water multi-beam fold error correction method based on GA algorithm | |
RU2471152C1 (en) | Method of aircraft navigation | |
CN112581610B (en) | Robust optimization method and system for building map from multi-beam sonar data | |
CN112802195B (en) | Underwater robot continuous occupying and mapping method based on sonar | |
CN116699581B (en) | Submarine topography measurement method and device based on deep sea submersible | |
CN112082558A (en) | UUV submarine topography tracking path rolling generation method based on polynomial fitting | |
CN110456786B (en) | UUV submarine topography tracking system and method based on topography feature extraction | |
CN116164747B (en) | Positioning and navigation method and system for underwater robot | |
Zhang et al. | A line-surface integrated algorithm for underwater terrain matching | |
CN112082557A (en) | UUV submarine topography tracking path rolling generation method based on Bessel fitting | |
CN105425246A (en) | Method for ship-borne integrated measurement system precision calibration in water pool | |
Wang et al. | Measurement error analysis of multibeam echosounder system mounted on the deep-sea autonomous underwater vehicle | |
Huang et al. | An online calibration method for the DVL scale factor error based on a circular track |
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 | ||
RJ01 | Rejection of invention patent application after publication |
Application publication date: 20201215 |
|
RJ01 | Rejection of invention patent application after publication |