CN112082557A - UUV submarine topography tracking path rolling generation method based on Bessel fitting - Google Patents

UUV submarine topography tracking path rolling generation method based on Bessel fitting Download PDF

Info

Publication number
CN112082557A
CN112082557A CN202010959775.7A CN202010959775A CN112082557A CN 112082557 A CN112082557 A CN 112082557A CN 202010959775 A CN202010959775 A CN 202010959775A CN 112082557 A CN112082557 A CN 112082557A
Authority
CN
China
Prior art keywords
uuv
tracking
local
path
submarine topography
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
CN202010959775.7A
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 CN202010959775.7A priority Critical patent/CN112082557A/en
Publication of CN112082557A publication Critical patent/CN112082557A/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 Bessel 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 method adopts a Bessel fitting method to generate the tracking path, can well realize the fixed-height tracking matching of the UUV and the submarine topography, and particularly has better fixed-height tracking effect on the topography with violent change. 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 Bessel 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 Bessel 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 Bessel 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 Bessel fitting, which can be applied to the 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 discrete precision k of the local tracking path, and 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 BDA0002680079670000021
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 BDA0002680079670000022
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: and B, carrying out Bezier fitting on the local terrain affine point set P to generate a local tracking path L (t).
Step seven: discretizing the local tracking path L (t) to generate a UUV local tracking path point set pi.
Step eight: and the UUV adopts a general line-of-sight path tracking control method to sequentially perform local tracking on the generated local tracking path point set pi.
Step nine: and judging whether the risk of collision with the submarine topography exists by the UUV, if so, turning to the step ten, and otherwise, turning to the step eleven.
Step ten: 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 (unit: degree) emergency floating collision avoidance, and then the ninth step is carried out.
Step eleven: 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. If the task end point is reached, turning to the step fourteen, otherwise, turning to the step twelve.
Step twelve: and the UUV judges whether the local tracking 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 tracking path tracking is finished, turning to the step two, and if not, turning to the step thirteen.
Step thirteen: and the UUV judges whether the tracking of the local tracking path is overtime. The judgment method is that the tracking time of the local tracking path of the UUV exceeds the specified maximum tracking time Tmax. If yes, turning to step two, otherwise, turning to step eight.
Fourteen steps: and finishing the UUV submarine topography tracking.
The invention relates to a rolling generation method of a UUV submarine topography tracking path based on Bessel 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 detectionMeasuring point set
Figure BDA0002680079670000038
The method for calculating the local topographic point set A comprises the following steps:
step four (a): detecting point sets from terrain
Figure BDA0002680079670000039
Extracting effective topographic survey point set
Figure BDA0002680079670000031
The extraction method comprises the following steps: collecting landform detection points
Figure BDA00026800796700000310
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 BDA0002680079670000032
Wherein the landform detection point set
Figure BDA0002680079670000033
Comprises the following steps:
Figure BDA0002680079670000034
(i＝1,2,…n)，
Figure BDA0002680079670000035
the terrain detection point information of the ith beam of the forward-looking multi-beam sonar is shown, and n represents the number of beams of the forward-looking multi-beam sonar. Effective topography detection point set
Figure BDA0002680079670000036
(j ≦ 1,2, … m, j ≦ i, m ≦ n), m indicates the number of landform points acquired by the active probe beam.
Step four (B): detecting a set of points from an effective terrain
Figure BDA0002680079670000037
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 BDA0002680079670000041
(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 BDA0002680079670000042
(4) In the sixth step, Bessel fitting is carried out on the local terrain affine point set P to generate a local tracking path L (t), and the fitting generation method comprises the following steps:
Figure BDA0002680079670000043
Figure BDA0002680079670000044
the generated local tracking path l (t) is a continuous path in the vertical plane with t as a formalization parameter, comprising two parameterized equations x (t) and z (t).
(5) In the seventh step, discretizing the local tracking path L (t) to generate a UUV local tracking path point set pi, wherein the discretizing method comprises the following steps:
Π＝{Π1,Π2,…ΠkII, whereing＝[xg,zg]Where g is 1,2, … k, k is the local tracking path discretization precision, and a larger k indicates a finer discretization of the local tracking path l (t), and k is generally equal to or greater than 2m, and includes:
Figure BDA0002680079670000045
(6) the method for judging whether the collision risk with the submarine topography exists in the ninth step comprises the following steps:
nine (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＝hdet cosθ
step nine (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 method adopts a Bessel fitting method to generate the tracking path, can well realize fixed-height tracking matching of the UUV and the submarine topography, and particularly has good fixed-height tracking effect on the topography with violent change.
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 Bessel fitting;
figure 4 shows that an information flow graph is generated based on rolling of a bessel-fitted UUV submarine topography tracking path.
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 0 (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 BDA0002680079670000063
Comprises the following steps:
Figure BDA0002680079670000061
wherein
Figure BDA0002680079670000062
And (i-1, 2, … n) represents the information of the terrain detection point detected by the ith beam.
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 the maximum tracking time T of the specified local tracking pathmaxAnd 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 UUV seafloor terrain tracking path rolling generation method based on bezier 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 system0And lat0Setting the longitude and latitude lon of the task end pointE(unit: degree) and latE(unit: degree), setting the discrete precision k of the local tracking path, and 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 BDA00026800796700000711
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 BDA00026800796700000712
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 BDA0002680079670000071
Extracting effective topographic survey point set
Figure BDA0002680079670000072
The extraction method comprises collecting landform detection points
Figure BDA0002680079670000073
In diExtracting the topographic point obtained by the effective detection beam not equal to 0 to obtain
Figure BDA0002680079670000074
Wherein
Figure BDA0002680079670000075
(j ≦ 1,2, … m, j ≦ i, m ≦ n), m indicates the number of landform points acquired by the active probe beam.
Step four (B): detecting a set of points from an effective terrain
Figure BDA0002680079670000076
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 BDA0002680079670000077
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 BDA0002680079670000078
Step six: b, performing Bessel fitting on the local terrain affine point set P to generate a local tracking path L (t), wherein the fitting generation method comprises the following steps:
Figure BDA0002680079670000079
Figure BDA00026800796700000710
the generated local tracking path l (t) is a continuous path in the vertical plane with t as a formalization parameter, comprising two parameterized equations x (t) and z (t).
Step seven: discretizing the local tracking path L (t) to generate a UUV local tracking path point set pi, wherein the discretizing method comprises the following steps:
Π＝{Π1,Π2,…ΠkII, whereing＝[xg,zg]Where g is 1,2, … k, k is the local tracking path discretization precision, and a larger k indicates a finer discretization of the local tracking path l (t), and k is generally equal to or greater than 2m, and includes:
Figure BDA0002680079670000081
step eight: and the UUV adopts a general line-of-sight path tracking control method to sequentially perform local tracking on the generated local tracking path point set pi.
Step nine: and judging whether the risk of collision with the submarine topography exists by the UUV, if so, turning to the step ten, and otherwise, turning to the step eleven. The method for judging whether the collision risk with the submarine topography exists is as follows:
step nine (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＝hdet cosθ
step nine (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 ten: 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 turning to the ninth step.
Step eleven: and the UUV judges whether the task end point is reached. The judgment method is that the current longitude and latitude of the UUV isWhether the task destination longitude and latitude lon is reachedEAnd latE. If the task end point is reached, turning to the step fourteen, otherwise, turning to the step twelve.
Step twelve: 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 thirteen.
Step thirteen: 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 step two, otherwise, turning to step eight.
Fourteen steps: and finishing the UUV submarine topography tracking.

Claims (7)

1. A UUV submarine topography tracking path rolling generation method based on Bessel 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 discrete precision k of the local tracking path, and 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 FDA0002680079660000011
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 FDA0002680079660000012
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: b, carrying out Bessel fitting on the local terrain affine point set P to generate a local tracking path L (t);
step seven: discretizing the local tracking path L (t) to generate a UUV local tracking path point set pi;
step eight: the UUV adopts a general line-of-sight path tracking control method to sequentially perform local tracking on the generated local tracking path point set pi;
step nine: judging whether a risk of collision with the submarine topography exists by the UUV, if so, turning to the step ten, and if not, turning to the step eleven;
step ten: and the UUV carries out collision avoidance response: at the moment, UUV stops path tracking control, and the maximum floating trim angle theta is usedmaxEmergency floating and collision avoidance (unit: degree), and then turning to the ninth step;
step eleven: 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 fourteen, otherwise, turning to the step twelve;
step twelve: and the UUV judges whether the local tracking path tracking is finished: the judgment method is that the UUV reaches the last path point of the local tracking path point set,
if the local tracking path tracking is finished, turning to the step two, and if not, turning to the step thirteen;
step thirteen: the UUV judges whether the tracking of the local tracking path is overtime: the judgment method is the local tracking of UUVThe tracking time of the path exceeds a specified maximum tracking time TmaxIf yes, turning to the step two, otherwise, turning to the step eight;
fourteen steps: and finishing the UUV submarine topography tracking.
2. The Bessel fitting-based UUV submarine topography tracking path rolling generation method according to claim 1, wherein the UUV in the third step has vertical plane coordinates (x) under a fixed coordinate systemuuv,zuuv) The calculation method comprises the following steps:
xuuv＝(lonuuv-lon0) 1852 · 60, wherein xuuvThe unit of (a) is meter;
zuuvthe UUV depth is measured by a depth meter, and the current depth is measured in meters.
3. The Bessel-fitting-based UUV submarine topography tracking path rolling generation method according to claim 1, wherein the step four-intermediate-terrain detection point set
Figure FDA0002680079660000027
The method for calculating the local topographic point set A comprises the following steps:
step four (a): detecting point sets from terrain
Figure FDA0002680079660000028
Extracting effective topographic survey point set
Figure FDA0002680079660000021
The extraction method comprises the following steps: collecting landform detection points
Figure FDA0002680079660000029
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 FDA0002680079660000022
Wherein the landform detection point set
Figure FDA00026800796600000210
Comprises the following steps:
Figure FDA0002680079660000023
(i＝1,2,…n)，
Figure FDA00026800796600000211
representing the terrain detection point information of the ith wave beam of the forward-looking multi-beam sonar, representing the number of the wave beams of the forward-looking multi-beam sonar by n, and effectively detecting a point set by the terrain
Figure FDA0002680079660000024
(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 FDA0002680079660000025
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 FDA0002680079660000026
4. The rolling generation method for the UUV submarine topography tracking path based on bezier fitting according to claim 1, wherein in the fifth step, the local topography affine point set a is used for height setting affine, and the method for solving the local topography affine point set P comprises:
P＝{P1,P2,…Pmin which P isj＝[xaj,zaj]And is provided with
Figure FDA0002680079660000031
5. The rolling generation method of the UUV submarine topography tracking path based on the bezier fitting according to claim 1, wherein in step six, the local tracking path l (t) is generated by performing the bezier fitting on the local topography affine point set P, and the fitting generation method comprises:
Figure FDA0002680079660000032
Figure FDA0002680079660000033
the generated local tracking path l (t) is a continuous path in the vertical plane with t as a formalization parameter, comprising two parameterized equations x (t) and z (t).
6. The rolling generation method for the UUV submarine topography tracking path according to claim 1, wherein in the seventh step, the local tracking path l (t) is discretized to generate a UUV local tracking path point set Π, and the discretization method is as follows:
Π＝{Π1,Π2,…ΠkII, whereing＝[xg,zg]Where g is 1,2, … k, k is the local tracking path discretization precision, and a larger k indicates a finer discretization of the local tracking path l (t), and k is generally equal to or greater than 2m, and includes:
Figure FDA0002680079660000034
7. the rolling generation method for the UUV submarine topography tracking path based on Bessel fitting as claimed in claim 1, wherein the judgment method for whether the collision risk with the submarine topography exists in the ninth step is as follows:
nine (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＝hdet cosθ
step nine (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.
CN202010959775.7A 2020-09-14 2020-09-14 UUV submarine topography tracking path rolling generation method based on Bessel fitting Pending CN112082557A (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010959775.7A CN112082557A (en) 2020-09-14 2020-09-14 UUV submarine topography tracking path rolling generation method based on Bessel fitting

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010959775.7A CN112082557A (en) 2020-09-14 2020-09-14 UUV submarine topography tracking path rolling generation method based on Bessel fitting

Publications (1)

Publication Number Publication Date
CN112082557A true CN112082557A (en) 2020-12-15

Family

ID=73736775

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010959775.7A Pending CN112082557A (en) 2020-09-14 2020-09-14 UUV submarine topography tracking path rolling generation method based on Bessel fitting

Country Status (1)

Country Link
CN (1) CN112082557A (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
CN104316932A (en) * 2014-11-05 2015-01-28 哈尔滨工程大学 Height-determined sailing system and method of UUV which reaches seabed for working
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
CN108088456A (en) * 2017-12-21 2018-05-29 北京工业大学 A kind of automatic driving vehicle local paths planning method with time consistency
US20180172821A1 (en) * 2016-12-16 2018-06-21 The Government Of The United States Of America, As Represented By The Secretary Of The Navy Millimeter-Wave Terrain Aided Navigation System
CN108334677A (en) * 2018-01-17 2018-07-27 哈尔滨工程大学 A kind of UUV Realtime collision free planing methods based on GRU networks
CN108931243A (en) * 2018-04-17 2018-12-04 哈尔滨工程大学 It is a kind of complexity marine environment under the influence of the UUV paths planning method based on energy consumption and sampling quantity multiple-objection optimization
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
EP3492946A1 (en) * 2017-12-01 2019-06-05 Origin Wireless, Inc. Method, apparatus, and system for object tracking and navigation
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
CN110979605A (en) * 2020-01-04 2020-04-10 三门梅格测量仪器有限公司 Metal detection robot used in water

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
CN104316932A (en) * 2014-11-05 2015-01-28 哈尔滨工程大学 Height-determined sailing system and method of UUV which reaches seabed for working
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
US20180172821A1 (en) * 2016-12-16 2018-06-21 The Government Of The United States Of America, As Represented By The Secretary Of The Navy Millimeter-Wave Terrain Aided Navigation System
EP3492946A1 (en) * 2017-12-01 2019-06-05 Origin Wireless, Inc. Method, apparatus, and system for object tracking and navigation
CN108088456A (en) * 2017-12-21 2018-05-29 北京工业大学 A kind of automatic driving vehicle local paths planning method with time consistency
CN108334677A (en) * 2018-01-17 2018-07-27 哈尔滨工程大学 A kind of UUV Realtime collision free planing methods based on GRU networks
CN108931243A (en) * 2018-04-17 2018-12-04 哈尔滨工程大学 It is a kind of complexity marine environment under the influence of the UUV paths planning method based on energy consumption and sampling quantity multiple-objection optimization
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
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
CN110979605A (en) * 2020-01-04 2020-04-10 三门梅格测量仪器有限公司 Metal detection robot used in water

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
TAO CHEN ET AL.: "Online Receding Horizon Trajectory Planning Method for UUV Near Bottom Terrain Tracking", 《PROCEEDINGS OF THE 39TH CHINESE CONTROL CONFERENCE》 *
鄢泓哲: "多波束测深系统关键技术研究", 《中国优秀硕士学位论文全文数据库（电子期刊）基础科学辑》 *

Similar Documents

Publication Publication Date Title
EP3388327B1 (en) Route setting method for underwater vehicle, underwater vehicle optimum control method using same, and underwater vehicle
CN101436074B (en) Autonomous type underwater robot by simultaneous positioning and map constructing method
Chen et al. Review of AUV underwater terrain matching navigation
CN104390646B (en) The location matching method of underwater hiding-machine terrain aided inertial navigation system
CN111596333B (en) Underwater positioning navigation method and system
CN108562287A (en) A kind of Terrain-aided Underwater Navigation based on adaptively sampled particle filter
CN113093159B (en) Multi-beam sounding error improved model design method
Jacobi et al. Multi sensor underwater pipeline tracking with AUVs
Xu et al. A novel self-adapting filter based navigation algorithm for autonomous underwater vehicles
CN110906933A (en) AUV (autonomous Underwater vehicle) auxiliary navigation method based on deep neural network
RU2467914C1 (en) Method of ship navigability control and device to this end
RU2471152C1 (en) Method of aircraft navigation
CN113688562B (en) Automatic shallow water multi-beam fold error correction method based on GA algorithm
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
CN112581610B (en) Robust optimization method and system for building map from multi-beam sonar data
Tomczak Modern methods of underwater positioning applied in subsea mining
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
CN112082558A (en) UUV submarine topography tracking path rolling generation method based on polynomial fitting
EP4310815A1 (en) Information processing device, control method, program, and storage medium
KR102469164B1 (en) Apparatus and method for geophysical navigation of USV(Unmanned Surface Vehicles)
Wang et al. Measurement error analysis of multibeam echosounder system mounted on the deep-sea autonomous underwater vehicle

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