[go: up one dir, main page]

CN112945238A - Method and device for quantitatively calculating AUV water surface navigation endpoint radius threshold - Google Patents

Method and device for quantitatively calculating AUV water surface navigation endpoint radius threshold Download PDF

Info

Publication number
CN112945238A
CN112945238A CN202110204157.6A CN202110204157A CN112945238A CN 112945238 A CN112945238 A CN 112945238A CN 202110204157 A CN202110204157 A CN 202110204157A CN 112945238 A CN112945238 A CN 112945238A
Authority
CN
China
Prior art keywords
auv
path
yaw angle
radius threshold
inertial navigation
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.)
Granted
Application number
CN202110204157.6A
Other languages
Chinese (zh)
Other versions
CN112945238B (en
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.)
Boya Gongdao Beijing Robot Technology Co Ltd
Original Assignee
Boya Gongdao Beijing Robot Technology Co Ltd
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 Boya Gongdao Beijing Robot Technology Co Ltd filed Critical Boya Gongdao Beijing Robot Technology Co Ltd
Priority to CN202110204157.6A priority Critical patent/CN112945238B/en
Publication of CN112945238A publication Critical patent/CN112945238A/en
Application granted granted Critical
Publication of CN112945238B publication Critical patent/CN112945238B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

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—Instruments for performing navigational calculations specially adapted for water-borne vessels
    • 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/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • 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
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Navigation (AREA)
  • Position Fixing By Use Of Radio Waves (AREA)

Abstract

The invention relates to a method and a device for quantitatively calculating an AUV water surface navigation endpoint radius threshold value, wherein the method comprises the steps of obtaining a motion path of an AUV and establishing an east-north-sky coordinate system according to the motion path; presetting a starting point coordinate, an inertial navigation yaw angle error, an AUV speed and an iteration time interval of a path; establishing a motion state space model of the AUV according to the starting point coordinates of the path, the inertial navigation yaw angle error, the AUV speed and the iteration time interval; and determining the radius threshold of the path terminal according to the motion state space model. According to the method, the rule of setting the radius threshold is found through an optimized modeling mode, the radius threshold is determined after the inertial navigation yaw angle and the yaw angle error are determined, and whether the AUV reaches the terminal point is judged.

Description

Method and device for quantitatively calculating AUV water surface navigation endpoint radius threshold
Technical Field
The invention belongs to the technical field of unmanned underwater vehicles, and particularly relates to a method and a device for quantitatively calculating an AUV water surface navigation endpoint radius threshold.
Background
The AUV is known as an unmanned submersible and, while operating most of the time underwater, it also has little time on the surface for long endurance AUVs. In water surface navigation planning, an AUV generally uses GNSS and inertial navigation to navigate, and a route is planned on an upper computer to be executed. Because most of the environments of the sea surface do not have 4G signals or the 4G signals are weak, RTK cannot be used, only GNSS in a single-point positioning mode can be used, and the positioning accuracy is usually in the meter level. In determining whether the AUV reaches the endpoint, a circle is typically defined, and a radius threshold is used to determine whether the endpoint is reached, which is typically empirically determined.
The threshold is determined by experience, and on one hand, the threshold is determined by multiple water surface navigation tests, so that the navigation debugging time is prolonged, and a large amount of manpower and material resources are wasted; on the other hand, it is difficult to find the optimal value of the parameter by determining the threshold value empirically, and the AUV cannot exert the optimal navigation planning capability.
Disclosure of Invention
In view of the above, the present invention provides a method and a device for quantitatively calculating an AUV water surface navigation endpoint radius threshold, so as to solve the problem that the threshold determined by experience in the prior art cannot make the AUV exert the optimal navigation planning capability.
In order to achieve the purpose, the invention adopts the following technical scheme: a method for quantitatively calculating an AUV water surface navigation endpoint radius threshold value comprises the following steps:
acquiring a motion path of the AUV and establishing an east-north-sky coordinate system according to the motion path; presetting a starting point coordinate, an inertial navigation yaw angle error, an AUV speed and an iteration time interval of the path;
establishing a motion state space model of the AUV according to the starting point coordinates, the inertial navigation yaw angle error, the AUV speed and the iteration time interval of the path;
and determining a radius threshold value of the path terminal according to the motion state space model.
Further, the establishing an east-north-sky coordinate system according to the motion path includes:
establishing an east-north-sky coordinate system by taking the end point of the path as an origin, and taking the coordinates of the start point of the path as coordinates
Figure BDA0002949771320000021
Inertial navigation yaw angle thetayThe inertial navigation yaw angle error is ay(t)The yaw angle error is a function of t, the counterclockwise direction is the positive direction, and the speed is set
Figure BDA0002949771320000022
The iteration time interval Δ t is 1.
3. The method of claim 2, wherein the motion state space model of the AUV is:
Figure BDA0002949771320000023
Figure BDA0002949771320000024
converting the formulas (1) and (2) to obtain:
Figure BDA0002949771320000025
wherein,
Figure BDA0002949771320000026
the east position of the AUV at time n,
Figure BDA0002949771320000027
the north position of the AUV at time n,
Figure BDA0002949771320000028
the east position of the AUV at time n-1,
Figure BDA0002949771320000029
north orientation of AUV at time n-1, θy，n-1And the inertial navigation yaw angle of the AUV at the moment n-1.
Further, the motion state space model includes: an objective function with a minimum radius threshold as a target;
the objective function is:
Figure BDA00029497713200000210
further, the determining a radius threshold of the path terminal according to the motion state space model includes:
determining an angle value of an inertial navigation yaw angle and an error value of the inertial navigation yaw angle;
and determining a radius threshold value of the path terminal according to the angle value and the error value.
Further, a GNSS device is adopted to perform navigation and positioning on the AUV.
The embodiment of the application provides a device of quantitative calculation AUV surface of water navigation terminal radius threshold value, includes:
the acquisition module is used for acquiring a motion path of the AUV and establishing an east-north-sky coordinate system according to the motion path; presetting a starting point coordinate, an inertial navigation yaw angle error, an AUV speed and an iteration time interval of the path;
the establishing module is used for establishing a motion state space model of the AUV according to the starting point coordinates of the path, the inertial navigation yaw angle error, the AUV speed and the iteration time interval;
and the determining module is used for determining the radius threshold of the path terminal according to the motion state space model.
The embodiment of the application provides computer equipment, which comprises a processor and a memory connected with the processor;
the memory is used for storing a computer program, and the computer program is used for executing the method for quantitatively calculating the AUV water surface navigation endpoint radius threshold value provided by any one of the above embodiments;
the processor is used for calling and executing the computer program in the memory.
By adopting the technical scheme, the invention can achieve the following beneficial effects:
the invention provides a method and a device for quantitatively calculating an AUV water surface navigation endpoint radius threshold.
Drawings
In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings used in the description of the embodiments or the prior art will be briefly described below, it is obvious that the drawings in the following description are only some embodiments of the present invention, and for those skilled in the art, other drawings can be obtained according to the drawings without creative efforts.
FIG. 1 is a schematic diagram illustrating the steps of the method for quantitatively calculating the AUV water surface navigation endpoint radius threshold value according to the present invention;
FIG. 2 is a schematic diagram illustrating simulation of influence of different yaw angle errors of inertial navigation on a navigation track according to the present invention;
FIG. 3 is a schematic diagram illustrating simulation of influence of different yaw angle errors of inertial navigation on navigation convergence rate according to the present invention;
FIG. 4 is a schematic diagram illustrating simulation of the effect of GPS dynamic accuracy on navigation at different variation intervals according to the present invention;
FIG. 5 is a schematic diagram illustrating simulation of the effect of different GPS dynamic accuracies at different variation intervals on navigation according to the present invention;
FIG. 6 is a schematic diagram showing the simulation of the effect of different GPS dynamic accuracies at different variation intervals on navigation according to the present invention;
FIG. 7 is a schematic diagram of the dynamic accuracy analysis of the latitude and longitude of the GPS according to the present invention;
fig. 8 is a schematic structural diagram of the device for quantitatively calculating the radius threshold of the AUV water surface navigation endpoint according to the present invention.
Detailed Description
In order to make the objects, technical solutions and advantages of the present invention more apparent, the technical solutions of the present invention will be described in detail below. It is to be understood that the described embodiments are merely exemplary of the invention, and not restrictive of the full scope of the invention. All other embodiments, which can be derived by a person skilled in the art from the examples given herein without any inventive step, are within the scope of the present invention.
The following describes a specific method and apparatus for quantitatively calculating an end radius threshold of an AUV water surface navigation system provided in the embodiments of the present application with reference to the accompanying drawings.
As shown in fig. 1, the method for quantitatively calculating the radius threshold of the end point of the AUV water surface navigation provided in the embodiment of the present application. The method comprises the following steps:
s101, obtaining a motion path of the AUV and establishing an east-north-sky coordinate system according to the motion path; presetting a starting point coordinate, an inertial navigation yaw angle error, an AUV speed and an iteration time interval of the path;
firstly, establishing an east-north-sky coordinate system for representing the movement of the AUV, and presetting the starting point coordinates of a path, the inertial navigation yaw angle error, the speed of the AUV and an iteration time interval according to the running path of the AUV by taking the east direction on a plane as an x axis, the north direction as a y axis and the direction vertical to the x axis and the y axis as a z axis.
S102, establishing a motion state space model of the AUV according to the starting point coordinates of the path, the inertial navigation yaw angle error, the AUV speed and the iteration time interval;
and determining the motion state space model of the AUV according to the preset parameter value, wherein the motion state space model of the AUV is represented by a equation in the application.
S103, determining a radius threshold value of the path terminal according to the motion state space model.
And substituting the angle value of the inertial navigation yaw angle and the error value of the inertial navigation yaw angle in the motion state space model into the motion state space model to calculate the radius threshold.
The working principle of the method for quantitatively calculating the AUV water surface navigation end point radius threshold value is as follows: firstly, obtaining a motion path of an AUV (autonomous Underwater vehicle) and establishing an east-north-sky coordinate system according to the motion path; presetting a starting point coordinate, an inertial navigation yaw angle error, an AUV speed and an iteration time interval of the path; establishing a motion state space model of the AUV according to the starting point coordinates, the inertial navigation yaw angle error, the AUV speed and the iteration time interval of the preset path; and determining the radius threshold of the path terminal according to the motion state space model.
According to the method, the optimal model is established, the inertial navigation yaw angle and the yaw angle error are determined, and then the radius threshold is determined, so that manpower and material resources are saved, and the AUV can exert the optimal navigation planning capability.
In some embodiments, an east-north-sky coordinate system is established with the end point of the path as the origin and the start point of the path as the coordinate
Figure BDA0002949771320000051
Inertial navigation yaw angle thetayThe inertial navigation yaw angle error is ay(t)The yaw angle error is a function of t, the counterclockwise direction is the positive direction, and the speed is set
Figure BDA0002949771320000052
The iteration time interval Δ t is 1.
In this application, velocity induced by flow measurement is not taken into account
Figure BDA0002949771320000053
Under the condition of (1), the motion state space model of the AUV is as follows:
Figure BDA0002949771320000054
Figure BDA0002949771320000055
converting the formulas (1) and (2) to obtain:
Figure BDA0002949771320000056
wherein,
Figure BDA0002949771320000057
the east position of the AUV at time n,
Figure BDA0002949771320000058
the north position of the AUV at time n,
Figure BDA0002949771320000059
the east position of the AUV at time n-1,
Figure BDA00029497713200000510
north orientation of AUV at time n-1, θy，n-1And the inertial navigation yaw angle of the AUV at the moment n-1.
Preferably, the motion state space model includes: an objective function with a minimum radius threshold as a target;
the objective function is:
Figure BDA0002949771320000061
specifically, the equation (1) is converted by taking the sum of squares to obtain an equation (3), and when the radius threshold value is smaller in the application, the AUV is closer to the end point or reaches the end point.
Preferably, the determining the radius threshold of the path terminal according to the motion state space model includes:
determining an angle value of an inertial navigation yaw angle and an error value of the inertial navigation yaw angle;
and determining a radius threshold value of the path terminal according to the angle value and the error value.
It can be understood that the pair thetay，n-1In different value ranges
Figure BDA0002949771320000062
The value analysis was as follows:
according to the above formula, it can be seen that when the yaw angle is less than 90 °, the AUV can converge to the end point without error, i.e. the threshold is 0.
The application verifies the theory through simulation. As shown in fig. 2, the influence of different yaw angle errors on the navigation track is simulated by simulation software in the present application. The deviation angle error in different navigation tracks is 5 degrees or-85 degrees, the interval is 5 degrees, the deviation angle error only influences the convergence speed, theoretically, the deviation angle error is converged at the small angle of 90 degrees, and the method is consistent with the theoretical analysis.
Where the effect on convergence speed is shown in fig. 3, it can be seen that the convergence speed is slower as the yaw angle error is larger, consistent with the theoretical analysis described above.
Preferably, a GNSS device is used to navigate and locate the AUV.
In the application, the GNSS device is adopted to perform navigation positioning on the AUV so as to ensure positioning accuracy, the dynamic deviation of the GPS is firstly set as a fixed constant, and when the fixed constant is set to be 10m, the simulation result is shown in figure 4. As can be seen from fig. 4, the convergence point from the target point does not exceed 10m at the maximum, i.e., does not exceed the dynamic accuracy of the GPS. In fig. 5, the influence of different GPS dynamic accuracies on navigation is shown, the range of the GPS dynamic accuracy is 2m to 20m, the variation interval is 2m, it can be seen that the final convergence time is not influenced by the GPS dynamic accuracy, only the final convergence point is influenced, and the final convergence point is the GPS dynamic accuracy.
In practice, the dynamic accuracy of the GPS can be considered to satisfy gaussian noise, which can be obtained in the analysis of actual data. When the mean value of the gaussian noise is 10m and the standard deviation is 4m, the simulation result is shown in fig. 6.
In order to ensure the stability of the system, the 3 σ principle in the gaussian distribution can be referred to in actual value taking, when the 3 σ is set, when the target point threshold circle is detected for the first time, the probability program detection is successful at 99.76%, when the 2 σ is set, the probability program detection is successful at 95.44%, and in practice, even if the first detection is unsuccessful, the simulation program can continuously detect until convergence.
Here, if the present application is set within a time T of N · Δ T and the probability of successful detection is P, the probability of convergence (detection of arrival at the target point) within the time T is Ps＝1-(1-P)NTherefore, the probability and the time are in an exponential relationship, and the threshold value can be set to be very small in order to converge in a short time, so that the value can be flexibly selected according to the actual situation.
An analysis chart of the dynamic accuracy of the longitude and latitude of the GPS is shown in fig. 7, which includes 50000 pieces of data, and the GPS reading rate is 1Hz, so the elapsed time is 13.8 hours, the elapsed time is longer, and the reliability of the data is higher.
As can be seen from fig. 7, the GPS longitude and latitude dynamic accuracy can be roughly regarded as gaussian distribution, and from an engineering point of view, the average error value can be calculated to be 0.9m and the standard deviation is about 2.0 based on the 3 σ principle, because the dynamic accuracy is calculated above, and assuming that the actual movement speed of the AUV is about 1m/s, the following values can be taken:
meangps≥2.0m (5)
vargps≥2.0m (6)
the result shows that the dynamic position accuracy of the GPS is higher, and according to the analysis of the correlation theory, the static position accuracy of the GPS is higher than the dynamic position accuracy.
The influence of the two errors, namely the azimuth drift of the optical fiber inertial navigation and the dynamic precision of the GNSS on the determination of the water surface navigation threshold value is obtained by analyzing the AUV kinematic angle through an optimization modeling method, and the method has very high practical reference value.
As shown in fig. 8, an embodiment of the present application provides an apparatus for quantitatively calculating an ending radius threshold of an AUV water surface navigation, including:
an obtaining module 801, configured to obtain a motion path of the AUV and establish an east-north-sky coordinate system according to the motion path; presetting a starting point coordinate, an inertial navigation yaw angle error, an AUV speed and an iteration time interval of the path;
an establishing module 802, configured to establish a motion state space model of the AUV according to the start coordinates of the path, the inertial navigation yaw angle error, the speed of the AUV, and the iteration time interval;
a determining module 803, configured to determine a radius threshold of the path terminal according to the motion state space model.
The application provides a device for quantitatively calculating AUV water surface navigation endpoint radius threshold's theory of operation does: the obtaining module 801 obtains a motion path of the AUV and establishes an east-north-sky coordinate system according to the motion path; presetting a starting point coordinate, an inertial navigation yaw angle error, an AUV speed and an iteration time interval of the path; the establishing module 802 establishes a motion state space model of the AUV according to the starting point coordinates of the path, the inertial navigation yaw angle error, the AUV speed and the iteration time interval; the determining module 803 determines the radius threshold of the path termination according to the motion state space model.
The embodiment of the application provides computer equipment, which comprises a processor and a memory connected with the processor;
the memory is used for storing computer program, and the computer program is used for executing
Acquiring a motion path of the AUV and establishing an east-north-sky coordinate system according to the motion path; presetting a starting point coordinate, an inertial navigation yaw angle error, an AUV speed and an iteration time interval of the path;
establishing a motion state space model of the AUV according to the starting point coordinates, the inertial navigation yaw angle error, the AUV speed and the iteration time interval of the path;
determining a radius threshold value of a path terminal according to the motion state space model;
the processor is used to call and execute the computer program in the memory.
In summary, the invention provides a method and a device for quantitatively calculating the radius threshold of the water surface navigation endpoint of an AUV, wherein the method comprises the steps of obtaining a motion path of the AUV and establishing an east-north-sky coordinate system according to the motion path; presetting a starting point coordinate, an inertial navigation yaw angle error, an AUV speed and an iteration time interval of a path; establishing a motion state space model of the AUV according to the starting point coordinates of the path, the inertial navigation yaw angle error, the AUV speed and the iteration time interval; and determining the radius threshold of the path terminal according to the motion state space model. According to the method, the rule of setting the radius threshold is found through an optimized modeling mode, the radius threshold is determined after the inertial navigation yaw angle and the yaw angle error are determined, and whether the AUV reaches the terminal point is judged.
It is to be understood that the embodiments of the method provided above correspond to the embodiments of the apparatus described above, and the corresponding specific contents may be referred to each other, which is not described herein again.
As will be appreciated by one skilled in the art, embodiments of the present application may be provided as a method, system, or computer program product. Accordingly, the present application may take the form of an entirely hardware embodiment, an entirely software embodiment or an embodiment combining software and hardware aspects. Furthermore, the present application may take the form of a computer program product embodied on one or more computer-usable storage media (including, but not limited to, disk storage, optical storage, and the like) having computer-usable program code embodied therein.
The present application is described with reference to flowchart illustrations and/or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of the application. It will be understood that each flow and/or block of the flow diagrams and/or block diagrams, and combinations of flows and/or blocks in the flow diagrams and/or block diagrams, can be implemented by computer program instructions. These computer program instructions may be provided to a processor of a general purpose computer, special purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, create means for implementing the functions specified in the flowchart flow or flows and/or block diagram block or blocks.
These computer program instructions may also be stored in a computer-readable memory that can direct a computer or other programmable data processing apparatus to function in a particular manner, such that the instructions stored in the computer-readable memory produce an article of manufacture including instruction means which implement the function specified in the flowchart flow or flows and/or block diagram block or blocks.
These computer program instructions may also be loaded onto a computer or other programmable data processing apparatus to cause a series of operational steps to be performed on the computer or other programmable apparatus to produce a computer implemented process such that the instructions which execute on the computer or other programmable apparatus provide steps for implementing the functions specified in the flowchart flow or flows and/or block diagram block or blocks.
The above description is only for the specific embodiments of the present invention, but the scope of the present invention is not limited thereto, and any person skilled in the art can easily conceive of the changes or substitutions within the technical scope of the present invention, and all the changes or substitutions should be covered within the scope of the present invention. Therefore, the protection scope of the present invention shall be subject to the protection scope of the appended claims.

Claims (8)

1. A method for quantitatively calculating an AUV water surface navigation endpoint radius threshold value is characterized by comprising the following steps:
acquiring a motion path of the AUV and establishing an east-north-sky coordinate system according to the motion path; presetting a starting point coordinate, an inertial navigation yaw angle error, an AUV speed and an iteration time interval of the path;
establishing a motion state space model of the AUV according to the starting point coordinates, the inertial navigation yaw angle error, the AUV speed and the iteration time interval of the path;
and determining a radius threshold value of the path terminal according to the motion state space model.
2. The method of claim 1, wherein establishing an east-north-sky coordinate system from the motion path comprises:
establishing an east-north-sky with the end point of the path as the originA coordinate system, the coordinates of the starting point of the path are
Figure FDA0002949771310000011
Inertial navigation yaw angle thetayThe inertial navigation yaw angle error is ay(t)The yaw angle error is a function of t, the counterclockwise direction is the positive direction, and the speed is set
Figure FDA0002949771310000012
The iteration time interval Δ t is 1.
3. The method of claim 2, wherein the motion state space model of the AUV is:
Figure FDA0002949771310000013
Figure FDA0002949771310000014
converting the formulas (1) and (2) to obtain:
Figure FDA0002949771310000015
wherein,
Figure FDA0002949771310000016
the east position of the AUV at time n,
Figure FDA0002949771310000017
the north position of the AUV at time n,
Figure FDA0002949771310000018
the east position of the AUV at time n-1,
Figure FDA0002949771310000019
north orientation of AUV at time n-1, θy，n-1And the inertial navigation yaw angle of the AUV at the moment n-1.
4. The method of claim 3, wherein the motion state space model comprises: an objective function with a minimum radius threshold as a target;
the objective function is:
Figure FDA00029497713100000110
5. the method of claim 4, wherein determining a radius threshold for a path termination from the motion state space model comprises:
determining an angle value of an inertial navigation yaw angle and an error value of the inertial navigation yaw angle;
and determining a radius threshold value of the path terminal according to the angle value and the error value.
6. The method according to any one of claims 1 to 5,
and adopting a GNSS device to perform navigation and positioning on the AUV.
7. An apparatus for quantitatively calculating AUV water surface navigation endpoint radius threshold value, comprising:
the acquisition module is used for acquiring a motion path of the AUV and establishing an east-north-sky coordinate system according to the motion path; presetting a starting point coordinate, an inertial navigation yaw angle error, an AUV speed and an iteration time interval of the path;
the establishing module is used for establishing a motion state space model of the AUV according to the starting point coordinates of the path, the inertial navigation yaw angle error, the AUV speed and the iteration time interval;
and the determining module is used for determining the radius threshold of the path terminal according to the motion state space model.
8. A computer device, comprising: a processor, and a memory coupled to the processor;
the memory is used for storing a computer program used for executing the method for quantitatively calculating the AUV water surface navigation endpoint radius threshold value according to any one of claims 1 to 6;
the processor is used for calling and executing the computer program in the memory.
CN202110204157.6A 2021-02-24 2021-02-24 Method and device for quantitatively calculating AUV water surface navigation endpoint radius threshold Active CN112945238B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110204157.6A CN112945238B (en) 2021-02-24 2021-02-24 Method and device for quantitatively calculating AUV water surface navigation endpoint radius threshold

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110204157.6A CN112945238B (en) 2021-02-24 2021-02-24 Method and device for quantitatively calculating AUV water surface navigation endpoint radius threshold

Publications (2)

Publication Number Publication Date
CN112945238A true CN112945238A (en) 2021-06-11
CN112945238B CN112945238B (en) 2023-01-10

Family

ID=76245777

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110204157.6A Active CN112945238B (en) 2021-02-24 2021-02-24 Method and device for quantitatively calculating AUV water surface navigation endpoint radius threshold

Country Status (1)

Country Link
CN (1) CN112945238B (en)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115035183A (en) * 2022-06-07 2022-09-09 深圳市鹏龙海洋机器人科技有限公司 Method and device for quantitatively calculating AUV water surface navigation endpoint radius threshold

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
RU2345326C1 (en) * 2007-11-19 2009-01-27 Тульский государственный университет (ТулГУ) Method of correction of inertial navigating system
US20090171577A1 (en) * 2007-12-31 2009-07-02 Wavemarket, Inc. Navigation system implementing dynamic route correction
CN103017765A (en) * 2012-12-06 2013-04-03 北京遥测技术研究所 Yaw angle correction method and yaw angle correction device applied to a micro-mechanical integrated navigation system
CN103900605A (en) * 2012-12-25 2014-07-02 上海博泰悦臻电子设备制造有限公司 Detection method and device of navigation path planning and calculating reliability
JP2016085092A (en) * 2014-10-24 2016-05-19 パイオニア株式会社 Navigation device, control method, program, and storage medium
CN107664764A (en) * 2016-07-29 2018-02-06 高德信息技术有限公司 A kind of navigation object determines method and device
CN108418644A (en) * 2018-02-27 2018-08-17 南京芝麻信息科技有限公司 Signal processing method and device
CN108592939A (en) * 2018-07-11 2018-09-28 维沃移动通信有限公司 A kind of air navigation aid and terminal

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
RU2345326C1 (en) * 2007-11-19 2009-01-27 Тульский государственный университет (ТулГУ) Method of correction of inertial navigating system
US20090171577A1 (en) * 2007-12-31 2009-07-02 Wavemarket, Inc. Navigation system implementing dynamic route correction
CN103017765A (en) * 2012-12-06 2013-04-03 北京遥测技术研究所 Yaw angle correction method and yaw angle correction device applied to a micro-mechanical integrated navigation system
CN103900605A (en) * 2012-12-25 2014-07-02 上海博泰悦臻电子设备制造有限公司 Detection method and device of navigation path planning and calculating reliability
JP2016085092A (en) * 2014-10-24 2016-05-19 パイオニア株式会社 Navigation device, control method, program, and storage medium
CN107664764A (en) * 2016-07-29 2018-02-06 高德信息技术有限公司 A kind of navigation object determines method and device
CN108418644A (en) * 2018-02-27 2018-08-17 南京芝麻信息科技有限公司 Signal processing method and device
CN108592939A (en) * 2018-07-11 2018-09-28 维沃移动通信有限公司 A kind of air navigation aid and terminal

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115035183A (en) * 2022-06-07 2022-09-09 深圳市鹏龙海洋机器人科技有限公司 Method and device for quantitatively calculating AUV water surface navigation endpoint radius threshold

Also Published As

Publication number Publication date
CN112945238B (en) 2023-01-10

Similar Documents

Publication Publication Date Title
CN109807911B (en) Outdoor patrol robot multi-environment combined positioning method based on GNSS, UWB, IMU, laser radar and code disc
Wang et al. An adaptive UKF based SLAM method for unmanned underwater vehicle
CN110716222B (en) A method and system for unmanned vehicle navigation based on UAV
CN107966145B (en) AUV underwater navigation method based on sparse long baseline tight combination
CN111070205A (en) Pile alignment control method and device, intelligent robot and storage medium
CN113218421B (en) Robust self-adaptive dynamic alignment method for strapdown inertial navigation system under Beidou rejection condition
CN102162733A (en) Method for correcting autonomous underwater vehicle (AUV) dead reckoning navigation error in real time based on space vector modulation (SVM)
CN113108774B (en) Underwater robots and underwater robot navigation and positioning methods
CN115435782B (en) A method and device for anti-interference position estimation under multi-source information constraints
CN106896363A (en) A kind of submarine target active tracing track initiation method
CN115060275A (en) A method for selecting optimal navigation information for multiple inertial navigation devices
CN111174774A (en) A navigation information fusion method and system in a certain depth water level mode
CN112945238B (en) Method and device for quantitatively calculating AUV water surface navigation endpoint radius threshold
US20140180580A1 (en) Module, device and method for positioning
Bento et al. Set-membership position estimation with GNSS pseudorange error mitigation using lane-boundary measurements
CN110865338A (en) A high-precision localization method for unknown periodic acoustic beacon based on underwater mobile platform
Bingham Predicting the navigation performance of underwater vehicles
Yang et al. Robust sequential adaptive Kalman filter algorithm for ultrashort baseline underwater acoustic positioning
CN110873813B (en) Water flow velocity estimation method, integrated navigation method and device
Lager et al. Underwater terrain navigation using standard sea charts and magnetic field maps
CN110749861A (en) A three-dimensional localization method for underwater fixed targets based on multi-hypothesis depth
Huang et al. An online calibration method for the DVL scale factor error based on a circular track
CN115930904A (en) Unmanned ship reservoir area underwater topography measurement method
Khater et al. Improved Navigation and Guidance System of AUV Using Sensors Fusion.
Cao et al. Research on the Influence of Non Identical Position of Acoustic Ranging in AUV Underwater Guidance

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant