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 PDF

Info

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
Application number
CN202010959787.XA
Other languages
Chinese (zh)
Inventor
陈涛
芦泽辉
范云垚
严浙平
王宏健
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Harbin Engineering University
Original Assignee
Harbin Engineering University
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Harbin Engineering University filed Critical Harbin Engineering University
Priority to CN202010959787.XA priority Critical patent/CN112082558A/en
Publication of CN112082558A publication Critical patent/CN112082558A/en
Pending legal-status Critical Current

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

UUV submarine topography tracking path rolling generation method based on polynomial fitting
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
Figure BDA0002680079300000021
Step three: solving the vertical plane coordinate (x) of the UUV under a fixed coordinate systemuuv,zuuv)。
Step four: detecting point sets from terrain
Figure BDA0002680079300000022
And solving a local topographic point set A.
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 set
Figure BDA0002680079300000031
The method for calculating the local topographic point set A comprises the following steps:
step four (a): detecting point sets from terrain
Figure BDA0002680079300000032
Extracting effective topographic survey point set
Figure BDA0002680079300000033
The extraction method comprises the following steps: collecting landform detection points
Figure BDA0002680079300000034
Measured 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 set
Figure BDA0002680079300000035
Wherein the landform detection point set
Figure BDA0002680079300000036
Comprises the following steps:
Figure BDA0002680079300000037
Figure BDA0002680079300000038
Figure BDA0002680079300000039
Figure BDA00026800793000000310
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
Figure BDA00026800793000000311
Figure BDA00026800793000000312
Figure BDA00026800793000000313
m represents the number of terrain points obtained by the active probe beam.
Step four (B): detecting a set of points from an effective terrain
Figure BDA00026800793000000314
And 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
Figure BDA00026800793000000315
(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
Figure BDA0002680079300000041
(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
Za＝[za1 za2 … zam]T,
Figure BDA0002680079300000042
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 sonar
Figure BDA0002680079300000051
Comprises the following steps:
Figure BDA0002680079300000052
wherein
Figure BDA0002680079300000053
And the information of the terrain detection points detected by the ith beam is represented.
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
Figure BDA0002680079300000061
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 terrain
Figure BDA0002680079300000062
And solving a local topographic point set A, wherein the solving method comprises the following steps:
step four (a): detecting point sets from terrain
Figure BDA0002680079300000063
Extracting effective topographic survey point setThe extraction method comprises collecting landform detection points
Figure BDA0002680079300000065
In diExtracting the topographic point obtained by the effective detection beam not equal to 0 to obtain
Figure BDA0002680079300000066
Wherein
Figure BDA0002680079300000067
Figure BDA0002680079300000071
m represents the number of terrain points obtained by the active probe beam.
Step four (B): detecting a set of points from an effective terrain
Figure BDA0002680079300000072
And 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
Figure BDA0002680079300000073
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
Figure BDA0002680079300000074
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
Za＝[za1 za2 … zam]T,
Figure BDA0002680079300000075
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
Figure FDA0002680079290000011
Step three: solving the vertical plane coordinate (x) of the UUV under a fixed coordinate systemuuv,zuuv)；
Step four: detecting point sets from terrain
Figure FDA0002680079290000012
Solving a local topographic point set A;
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 set
Figure FDA0002680079290000028
The method for calculating the local topographic point set A comprises the following steps:
step four (a): detecting point sets from terrain
Figure FDA0002680079290000029
Extracting effective topographic survey point set
Figure FDA0002680079290000021
The extraction method comprises the following steps: collecting landform detection points
Figure FDA00026800792900000210
Measured 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 set
Figure FDA0002680079290000022
Wherein the landform detection point set
Figure FDA00026800792900000211
Comprises the following steps:
Figure FDA0002680079290000023
(i＝1,2,…n)，
Figure FDA00026800792900000212
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
Figure FDA0002680079290000024
(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 terrain
Figure FDA0002680079290000025
The 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
Figure FDA0002680079290000026
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
Figure FDA0002680079290000027
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
Za＝[za1 za2 … zam]T,
Figure FDA0002680079290000031
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.
CN202010959787.XA 2020-09-14 2020-09-14 UUV submarine topography tracking path rolling generation method based on polynomial fitting Pending CN112082558A (en)

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)

Citations (18)

* Cited by examiner, † Cited by third party
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

Patent Citations (18)

* Cited by examiner, † Cited by third party
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)

* Cited by examiner, † Cited by third party
Title
TAO CHEN ET AL.: "Online Receding Horizon Trajectory Planning", 《PROCEEDINGS OF THE 39TH CHINESE CONTROL CONFERENCE》 *
李雪佳等: "基于三次多项式拟合三角函数的地理空间距离计算算法", 《计算机测量与控制》 *
武芳: "《空间数据库原理》", 31 May 2017, 武汉大学出版社 *
白国星等: "基于可变预测时域及速度的车辆路径跟踪控制", 《中国机械工程》 *
鄢泓哲: "多波束测深系统关键技术研究", 《中国优秀硕士学位论文全文数据库（电子期刊）基础科学辑》 *

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
CN113093159B (en) Multi-beam sounding error improved model design method
CN111273298B (en) Underwater acoustic target positioning and tracking method based on wave glider networking technology
CN108562287A (en) A kind of Terrain-aided Underwater Navigation based on adaptively sampled particle filter
Jacobi et al. Multi sensor underwater pipeline tracking with AUVs
CN111751856B (en) Accurate positioning method for submarine ground reference point based on PPP technology
CN109738902B (en) High-precision autonomous acoustic navigation method for underwater high-speed target based on synchronous beacon mode
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
Nakatani et al. Terrain based localization for pinpoint observation of deep seafloors
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
CN111220146B (en) Underwater terrain matching and positioning method based on Gaussian process regression learning
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
CN112082558A (en) UUV submarine topography tracking path rolling generation method based on polynomial fitting
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
CN108761467A (en) A kind of underwater map constructing method of three-dimensional based on Forward-looking Sonar
Wang et al. Measurement error analysis of multibeam echosounder system mounted on the deep-sea autonomous underwater vehicle
CN116699581B (en) Submarine topography measurement method and device based on deep sea submersible

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