CN111928850A - Combined navigation method of autonomous underwater robot suitable for environment under polar ice frame - Google Patents
Combined navigation method of autonomous underwater robot suitable for environment under polar ice frame Download PDFInfo
- Publication number
- CN111928850A CN111928850A CN202010198740.6A CN202010198740A CN111928850A CN 111928850 A CN111928850 A CN 111928850A CN 202010198740 A CN202010198740 A CN 202010198740A CN 111928850 A CN111928850 A CN 111928850A
- Authority
- CN
- China
- Prior art keywords
- autonomous underwater
- underwater robot
- beacon
- time
- point
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 38
- 239000013598 vector Substances 0.000 claims description 27
- 238000005259 measurement Methods 0.000 claims description 22
- 239000011159 matrix material Substances 0.000 claims description 20
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 claims description 20
- 238000004364 calculation method Methods 0.000 claims description 16
- 238000006073 displacement reaction Methods 0.000 claims description 2
- 230000000977 initiatory effect Effects 0.000 claims 1
- 238000011835 investigation Methods 0.000 description 2
- 241000282414 Homo sapiens Species 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 230000004927 fusion Effects 0.000 description 1
- 238000005457 optimization Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 238000012876 topography Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
- G01C21/203—Specially adapted for sailing ships
Abstract
The invention relates to the technical field of underwater integrated navigation, in particular to an integrated navigation method of an autonomous underwater robot in an environment under an ice frame of an autonomous underwater robot in polar region, aiming at solving the problem of accumulated navigation errors of the autonomous underwater robot with long navigation range under polar region, realizing that the autonomous underwater robot carries an acoustic navigation beacon autonomously, the autonomous underwater robot lays the acoustic navigation beacon autonomously, lays a three-dimensional space position of the acoustic beacon autonomously, and finally carries out single-beacon integrated navigation according to a beacon calibration result, thereby solving the problem of accumulated offset of the navigation position of the autonomous underwater robot under ice long-time remote condition. The method is mainly used for solving the problem of position calibration of the beacon distributed under the ice, and after the beacon is calibrated successfully, the autonomous underwater robot under the ice is navigated according to the beacon calibration position and single beacon ranging.
Description
Technical Field
The invention relates to the technical field of underwater integrated navigation, in particular to an integrated navigation method of an autonomous underwater robot (AUV for short) in an environment under an arctic ice frame.
Background
In the process of oceanographic engineering and oceanographic science investigation, the autonomous underwater robot plays an increasingly important role, and the observation of an under-polar ice-ocean system by using the autonomous underwater robot to replace human beings in an polar region environment is a hotspot of the research on important engineering equipment supporting the polar region scientific investigation at present. The observation platform aiming at the polar region ice frame-ocean system can be divided into various platforms such as a space-based platform, an ice-based platform, an autonomous underwater robot and the like. The space-based observation platform comprises satellite remote sensing and airplane mounted observation equipment and the like, and has the advantages that high-efficiency large-area observation is realized, and the limitation is that the fine structure in the ice rack cannot be obtained; the ice-based observation platform mainly comprises a radar, a hot water drill and the like, and has the advantages of capability of observing an ice frame, but the limitations are low space coverage rate and insufficient observation flexibility. Compared with the two types of platforms, the autonomous underwater robot platform is a high-efficiency ice moving platform, can reach the inside of an ice rack for observation and can flexibly move and observe in a large range, so that the autonomous underwater robot becomes important ocean equipment for polar ice ocean observation more and more. The autonomous underwater robot needs to survey and draw the shape of an ice rack, the topography of the sea bottom under the ice rack and observe the marine water body under the ice rack for a long time under the ice rack, wherein the navigation system is used for accurately guiding the autonomous underwater robot to reach an observation target position, realizing high-precision position matching of observation data and an observation space and ensuring the quality of marine observation data under the ice rack, so that the navigation problem under the ice rack is a key technical problem for the autonomous underwater robot to be applied under the ice rack. The difficulty of navigation under the polar ice frame is that the deviation angle error of the deep-water compass in the polar region is amplified along with the increase of the latitude, namely the course precision of the deep-water compass is reduced along with the increase of the latitude, and the reduction of the course precision further reduces the position precision of autonomous navigation dead reckoning, so that the realization of high-precision navigation positioning in a long-time and long-distance range under the polar ice frame environment is an important technical challenge of an underwater navigation system. The external auxiliary positioning of the traditional underwater combined navigation system mainly adopts an acoustic positioning mode, and the underwater acoustic positioning system can be divided into a long baseline, a short baseline and an ultra-short baseline according to different distances of acoustic baselines. The base line length of the long base line is generally 100-6000 meters, more than 3 long base line positioning beacons are arranged on a mother ship, then the mother ship navigates around the long base line acoustic positioning beacon on the water surface, navigation calibration is carried out on the position of the long base line positioning beacon by using a large-scale ultra-short base line and a differential GPS carried by the mother ship, finally, the autonomous underwater robot navigates in the long base line array, and the position accuracy of the combined navigation system is improved by continuously fusing long base line ranging information. The length of the base line of the short base line is generally 1 to 50 meters, the short base line beacon is generally carried on a mother ship, the autonomous underwater robot sails near the mother ship, and the short base line ranging information is continuously fused to improve the position accuracy of the integrated navigation system. The length of the basic line of the ultra-short basic line is less than 1 meter, the ultra-short basic line matrix is generally carried on a mother ship, the autonomous underwater robot sails in the ultra-short basic line coverage range of the mother ship, and the position accuracy of the combined navigation system is improved by data fusion ultra-short basic line azimuth/distance measurement. Under the environment of the polar ice frame, particularly the thickness of the Antarctic ice frame reaches up to kilometer, a mother ship cannot reach the upper part of the ice frame to use a short baseline and an ultra-short baseline to carry out acoustic auxiliary positioning on an autonomous underwater robot under the ice frame, cannot lay a long baseline positioning beacon above the ice frame, cannot carry out position calibration on the laid beacon, therefore, the autonomous underwater robot is required to autonomously carry the beacon to the sea below the ice frame from the opening under the ice frame, autonomously lay the beacon to the seabed, autonomously calibrate the position of the beacon, correct the self navigation error by measuring the distance between the autonomous underwater robot and the beacon, the method is characterized in that how to calibrate the beacon position by using the autonomous underwater robot is a key step of polar region under-ice combined navigation, and the calibration precision directly influences the external measurement precision of the autonomous underwater robot so as to determine the navigation precision of the autonomous underwater robot for under-ice operation. In recent years, researchers have made a certain progress in the aspect of position calibration of the beacon arrangement without depending on the working condition of the mother ship, but the platform is still unmanned ship with real-time receiving GPS signal, autonomous underwater robot with GPS beacon floating up to water surface, the autonomous underwater robot in the polar ice environment does not have the objective condition of receiving GPS positioning, and the autonomous navigation precision of the autonomous underwater robot under the polar ice frame is obviously reduced, therefore, a new autonomous underwater robot single-beacon combined navigation method suitable for the polar ice frame environment is needed, the pure distance indirect observation of the beacon and the autonomous underwater robot is utilized, the moving vector path calculated by the navigation sensor in the autonomous underwater robot is introduced into an observation equation, the maneuvering path of observation is planned through maneuvering element analysis, and the beacon calibration precision and the observability degree are improved.
Disclosure of Invention
The invention relates to the technical field of underwater integrated navigation, in particular to an integrated navigation method of an autonomous underwater robot (AUV) in an environment under an polar ice frame, which aims at solving the problem of accumulated navigation errors of the autonomous underwater robot with a long navigation range under the polar ice frame, realizes that the autonomous underwater robot autonomously carries an acoustic navigation beacon, autonomously arranges the acoustic navigation beacon, autonomously calibrates the three-dimensional spatial position of the acoustic beacon, and finally performs single-beacon integrated navigation according to the beacon calibration result, thereby solving the problem of accumulated offset of the navigation position of the autonomous underwater robot under the ice frame in a long-time and remote manner. The invention discloses a single-beacon combined navigation method of an autonomous underwater robot under polar ice, which is mainly used for solving the problem of accurately calibrating beacons of the autonomous underwater robot under polar ice. The invention comprises the following steps: after the autonomous underwater robot is arranged under the ice rack, the autonomous underwater robot improves observability of a beacon calibration equation by using self dead reckoning position and ranging information of the autonomous underwater robot and a beacon through a maneuvering navigation method of the autonomous underwater robot, introduces adjacent time sequence ranging information to recur to improve calibration calculation efficiency, and obtains high-precision beacon position estimation. And finally, substituting the calibration result into a single beacon combined navigation filter to realize high-precision navigation of the autonomous underwater robot navigating under the ice frame environment. The method can effectively solve the navigation problem of the autonomous underwater robot under the ice frame environment, adopts a maneuvering navigation optimization observability strategy and a moving vector path strategy, improves the observability of the system, saves the time cost of navigation calculation, and has strong engineering application value; the method is convenient to transplant, has strong expansibility, and is also suitable for the combined navigation application field of polar ice gliders, polar cable remote control underwater robots and the like.
The technical scheme adopted by the invention for realizing the purpose is as follows:
the combined navigation method of the autonomous underwater robot suitable for the environment under the polar ice frame comprises the following steps:
1) the autonomous underwater robot carries out maneuvering ranging on the beacon according to the constraint condition of the planned track;
2) calculating an initial position and variance estimate for the beacon;
3) calculating a measured Jacobian matrix of the autonomous underwater robot at the current moment;
4) calculating the position estimation and the variance estimation of the beacon at the current moment according to the Jacobian matrix;
5) judging whether an ending condition is met or not according to the position estimation and the variance estimation of the beacon at the current moment, and if so, recording the position of the beacon at the current moment; otherwise, returning to the step 3);
6) and the underwater robot navigates according to the recorded beacon position as a reference point.
The constraint conditions of the planned trajectory are as follows:
where ρ isi,i+1Represents point XiTo point Xi+1Position vector of (1), pi+1,i+2Represents point Xi+1To point Xi+2Position vector of (1), pcIs the linearized decision constant and fabs is the absolute value.
The step 2) comprises the following steps:
solving for initial position estimates for beacons
The method specifically comprises the following steps:
wherein the time of 1 st receiving the ranging signal is t1Autonomous underwater robot at time t1Is at a position X1＝(x1,y1,z1)TAnd similarly defining the position of the autonomous underwater robot at the 2 nd, 3 rd, 4 th and 5 th time ranging moments as X2、X3、X4、X5They are obtained by direct measurement of a Doppler log, a deep water compass and a depth meter carried by an autonomous underwater robot, R1、R2、R3、R4、R5Respectively representing autonomous underwater vehicles at time t1、t2、t3、t4、t5Ranging from beacons, obtained by direct measurement by a range finder, H0And Y is an intermediate variable;
P0indicating initial position estimate of beacon
The variance estimation and calculation method comprises the following steps:
wherein, XLThe three-dimensional position of the autonomous underwater robot when the autonomous underwater robot lays down the beacon is represented, and E () represents a mathematically expected calculation symbol.
The measured Jacobian matrix is as follows:
Hk＝[ρi-4,i-3 ρi-3,i-2 ρi-2,i-1 ρi-1,i]T
wherein HkFor autonomous underwater robot at time tkMeasurement of the Jacobian matrix, rhoi-4,i-3Represents point Xi-4To point Xi-3The moving vector diameter of (1); rhoi-3,i-2Represents point Xi-3To point Xi-2The moving vector diameter of (1); rhoi-2,i-1Represents point Xi-2To point Xi-1The moving vector diameter of (1); rhoi-1,iRepresents point Xi-1To point XiThe displacement of (2) is the dynamic vector diameter.
ρi-4,i-3、ρi-2,i-1、ρi-3,i-2、ρi-1,iThe calculation method comprises the following steps:
wherein u (t) represents the navigation speed of the autonomous underwater robot at the moment t, and is directly measured by a Doppler log carried by the autonomous underwater robot; psi (t) and theta (t) respectively represent a course angle and a pitch angle of the autonomous underwater robot at the moment t, and are directly measured by a deep-water compass carried by the autonomous underwater robot; z is a radical ofi、zi-1、zi-2、zi-3、zi-4Respectively representing autonomous underwater vehicles at time tiTime ti-1Time ti-2Time ti-3Time ti-4The depth of (2) is directly measured by a depth sensor carried by the autonomous underwater robot.
The step 4) comprises the following steps:
calculating the beacon at time tkVariance estimate P ofkAnd location estimation
The calculation method comprises the following steps:
wherein, PkIndicating that the beacon is at time tkThe estimate of the variance of (a) is,
indicating that the beacon is at time tkThe location estimate of (2); pk-1Indicating that the beacon is at time tk-1The estimate of the variance of (a) is,
indicating that the beacon is at time tk-1The location estimate of (2); kkIndicating that the beacon is at time tkEstimated gain of, YkRepresenting a column vector consisting of beacon ranges at different times, RkRepresents t introduced by autonomous underwater robot due to body navigation motion in the beacon calibration processkThe noise is measured at the time of the measurement,
is an intermediate variable, Pk-1Representing the previous time tk-1Estimate of variance of u (t)k) Indicating autonomous underwater vehicle at tkThe navigation speed at the moment is directly measured by a Doppler log carried by the autonomous underwater robot, psi (t)k) And θ (t)k) Respectively representing autonomous underwater vehicles at time tkThe course angle and the longitudinal inclination angle of the underwater robot are directly measured by a deepwater compass carried by the autonomous underwater robot,
is a matrix of deep water compass and Doppler log, dialog (r.) representing a diagonal matrix operator, σuIs the speed measurement accuracy, sigma, of a Doppler logψIs the course accuracy of the deepwater compass,
representing the measurement variance, R, of a rangefinder carried by the autonomous Underwater roboti、Ri-1、Ri-2、Ri-3、Ri-4Respectively representing the distance measurement of the beacon and the autonomous underwater robot, which are provided by a distance measuring instrument carried by the autonomous underwater robot, I represents an identity matrix which is a constant, and k represents the time tkThe time index of (c).
When k is 0, i.e. the current time is the initial time, PkAnd
position estimates respectively equal to the initial time
Sum variance estimate P0。
The end conditions are as follows:
wherein | · | purple2The 2-norm operator is represented by a 2-norm operator,
indicating the time t fromk-20To time tkEstimate 2 norm of the expectation, αcIs the beacon calibration accuracy threshold, k represents the time tkFor is a key representing the desired calculation range.
Step 6) comprises the following steps:
according to tkDistance between autonomous underwater robot and beacon calibration position provided by time range finder
And navigating the autonomous underwater robot by combining the dead reckoning position of the autonomous underwater robot.
The invention has the following beneficial effects and advantages:
1. compared with the traditional combined navigation method, the method uses the autonomous underwater robot to autonomously calibrate the beacon under ice, and overcomes the defect that the traditional combined navigation method only depends on ship-based calibration and has limited use environment;
2. aiming at the problem of high calculation space and time cost of the traditional beacon calibration method, the maneuvering navigation strategy and the moving vector path model are combined, and the instantaneity and reliability of the autonomous underwater robot combined navigation method applied under the ice rack are improved.
3. The method has wide application range, and can be applied to the combined navigation of the autonomous underwater robot under the ice frame environment, and also can be applied to the combined navigation of the polar ice glider and the polar cable remote control underwater robot.
Drawings
FIG. 1 is a schematic composition of the present invention;
fig. 2 is a flow chart of the present invention.
Detailed Description
The present invention will be described in further detail with reference to the accompanying drawings and examples.
The whole system comprises an autonomous underwater robot, a Doppler log, a deep water compass, a depth meter, a distance meter and a beacon. The autonomous underwater robot is a body for carrying a Doppler log, a deep water compass, a depth meter and a distance meter, and is also a carrying tool for carrying beacons. The Doppler log is used for measuring the navigation speed of the autonomous underwater robot; the deep water compass is used for measuring the attitude angle of the autonomous underwater robot; the deep water meter is used for measuring the depth of the autonomous underwater robot; the distance meter is used for measuring the distance from the autonomous underwater robot to the beacon; the beacon is initially carried by the autonomous underwater robot, is separated from the autonomous underwater robot after reaching a predetermined area, is laid on the seabed under an ice shelf, and is used for providing an acoustic positioning response signal to the autonomous underwater robot, and the system composition is shown in fig. 1.
The whole system works according to the following procedures:
for convenience of the following description, definitions are given here for part symbol variables: defining autonomous underwater robots respectively at time tiTime ti-1Time ti-2Time ti-3When the distance measurement between the beacon and the beacon is received, R is used for the distance measurement corresponding to each timei、Ri-1、Ri-2、Ri-3Wherein i represents a time sequence number of ranging; definition of XiRepresenting autonomous underwater vehicle at time tiThree-dimensional spatial position of (2), Xi＝(xi,yi,zi)TWherein x isi,，yi,，ziRespectively representing a north position, an east position and a depth; definition of pi-1,iRepresents point Xi-1To point XiThe position vector of (2).
Fig. 2 shows a flow chart of the present invention.
Firstly, maneuvering distance measurement is carried out on the beacon according to the planned track
After the autonomous underwater robot autonomously lays the beacon, the autonomous underwater robot dynamically navigates and measures the distance of the beacon. The autonomous underwater robot lays a beacon at a preset position, and performs maneuvering navigation by taking the beacon as a center and simultaneously measures the distance of the beacon, wherein the maneuvering navigation track is required to meet maneuvering observable conditions, and the basis for judging the maneuvering observability is that the following judging formula is met:
wherein p is definedi,i+1Represents point XiTo point Xi+1A position vector of (a); rhoi+1,i+2Represents point Xi+1To point Xi+2A position vector of (a); rhoi+2,i+3Represents point Xi+2To point Xi+3Position vector of (1), pcIs a linearized decision constant, pcAn empirical value of 0.9 may be taken from engineering. The physical meaning of the discriminant is that when the neighboring position vectors satisfy the discriminant, then the neighboring position vectors are linearly independent. Therefore, in the practical engineering application process, the motor navigation is recommended to adopt polygonal tracks (the number of edges is not less than 5), circular, elliptical, Arabic numeral 8-shaped and other planned tracks, and the track gaugeThe three-dimensional distance from the scratch point to the initial placement position of the beacon is between 100 meters and 600 meters.
Second, calculate initial position and variance estimates for the beacon
Definition of
An initial position estimate, which represents the three-dimensional position of the beacon, is the unknown to be solved for, and is obtained by solving the following system of equations:
wherein the time when the 1 st received ranging signal is defined as t1Autonomous underwater robot at time t1Is at a position X1＝(x1,y1,z1)TAnd similarly defining the position of the autonomous underwater robot at the 2 nd, 3 rd, 4 th and 5 th time ranging moments as X2、X3、X4、X5The input signals are obtained by directly measuring a Doppler log, a deep water compass and a depth meter which are carried by the autonomous underwater robot, and are known input quantities. R1、R2、R3、R4、R5Respectively representing autonomous underwater vehicles at time t1、t2、t3、t4、t5Ranging with beacons, which are measured directly by a rangefinder.
H0And Y is an intermediate variable.
Definition P0Initial position estimate representing three-dimensional position of beacon
The variance estimate of (2) is calculated as follows:
wherein XLThree-dimensional position for representing autonomous underwater robot when laying down beaconAnd E (.) represents a mathematically expected computational symbol.
Thirdly, calculating the time t of the autonomous underwater robotkThe measured Jacobian matrix HkThe calculation method is as follows:
Hk＝[ρi-4,i-3 ρi-3,i-2 ρi-2,i-1 ρi-1,i]T (4)
where ρ isi-4,i-3Represents point Xi-4To point Xi-3The moving vector diameter of (1); rhoi-2,i-1Represents point Xi-2To point Xi-1The moving vector diameter of (1); rhoi-3,i-2Represents point Xi-3To point Xi-2The moving vector diameter of (1); rhoi-1,iRepresents point Xi-1To point XiAre intermediate variables, the intermediate variable pi-4,i-3、ρi-2,i-1、ρi-3,i-2、ρi-1,iThe calculation method of (2) is as follows:
u (t) represents the navigation speed of the autonomous underwater robot at the moment t, is directly measured by a Doppler log carried by the autonomous underwater robot, and is a known input quantity; psi (t) and theta (t) respectively represent a course angle and a pitch angle of the autonomous underwater robot at the moment t, and are directly measured by a deep water compass carried by the autonomous underwater robot and are known input quantities; z is a radical ofi、zi-1、zi-2、zi-3Respectively representing autonomous underwater vehicles at time tiTime ti-1Time ti-2Time ti-3The depth of (d) is directly measured by a depth sensor mounted on the autonomous underwater robot, and is a known input quantity.
The fourth step, calculate the beacon at time tkPosition estimate and variance estimate of
Definition of
And PkRespectively, indicates the beacon at time tkThe position estimate and variance estimate of (2), which are calculated as follows:
wherein P iskIndicating that the beacon is at time tkIs determined by the position of the mobile station,
indicating that the beacon is at time tkThe variance estimation of (2) is the variables to be solved of the equation, and the whole equation can be directly solved by a substitution method. In a special case, i.e. when k is 0 (indicating that the current time is the initial time), then PkAnd
position estimates respectively equal to the initial time
Sum variance estimate P0And the value is directly taken without solving the equation set. KkIndicating that the beacon is at time tkIs an intermediate variable; y iskRepresents a column vector consisting of beacon ranges at different time instants, which is an intermediate variable; rkRepresents t introduced by autonomous underwater robot due to body navigation motion in the beacon calibration processkTime of day measurement noise, which is an intermediate variable;
is to describe RkA conveniently introduced intermediate variable, which is a matrix of known input quantities; pk-1Representing the previous time tk-1For t, for the variance estimation ofkA set of equations at time of day, which are known quantities; u (t)k) Indicating autonomous underwater vehicle at tkThe navigation speed at the moment is directly measured by a Doppler log carried by the autonomous underwater robot and is a known input quantity; psi (t)k) And θ (t)k) Respectively representing the course angle and the pitch angle of the autonomous underwater robot at the moment, and directly measuring by a deepwater compass carried by the autonomous underwater robot, wherein the course angle and the pitch angle are known input quantities;
is a correlation matrix of the deep water compass and the Doppler log, and dialog represents a diagonal matrix operator, sigmauIs the speed measurement accuracy, sigma, of a Doppler logψCourse accuracy, sigma, of deep water compassuAnd σψThe parameters are known parameters and are directly provided by a manufacturer when the equipment leaves a factory;
the measurement variance of the range finder carried by the autonomous underwater robot is shown, and the parameter is directly provided by a manufacturer when the equipment leaves a factory and is a known parameter. Ri、Ri-1、Ri-2、Ri-3Indicating the range of the beacon from the autonomous underwater robot, which are provided by a range finder carried by the autonomous underwater robot, are known inputs. I denotes a 3 x 3 identity matrix, which is a constant.
Fifthly, repeating the third step to the fourth step until tkThe beacon location estimate calculated at the time satisfies the following discriminant and the beacon calibration process is stopped.
Wherein | · | purple2The 2-norm operator is represented by a 2-norm operator,
indicating the time t fromk-20To time tkEstimate a 2-norm expectation; alpha is alphacThe beacon calibration precision threshold value is set by a user, the suggested value is 10, and k represents the time tkFor is a key representing the desired calculation range.
The sixth step, according to tkDistance between autonomous underwater robot and beacon calibration position provided by time range finder
And (3) combining the self dead reckoning position of the autonomous underwater robot, constructing a single beacon integrated navigation filter according to a standard Kalman filter, and calculating the optimal estimation of the underwater position of the autonomous underwater robot in real time. This step is not claimed because the algorithm employs a standard kalman filter.
Claims (9)
1. The combined navigation method of the autonomous underwater robot suitable for the environment under the polar ice frame is characterized by comprising the following steps of:
1) the autonomous underwater robot carries out maneuvering ranging on the beacon according to the constraint condition of the planned track;
2) calculating an initial position and variance estimate for the beacon;
3) calculating a measured Jacobian matrix of the autonomous underwater robot at the current moment;
4) calculating the position estimation and the variance estimation of the beacon at the current moment according to the Jacobian matrix;
5) judging whether an ending condition is met or not according to the position estimation and the variance estimation of the beacon at the current moment, and if so, recording the position of the beacon at the current moment; otherwise, returning to the step 3);
6) and the underwater robot navigates according to the recorded beacon position as a reference point.
2. The integrated navigation method of the autonomous underwater vehicle applicable to the environment under the polar ice tray as claimed in claim 1, wherein the constraint conditions of the planned trajectory are as follows:
where ρ isi,i+1Represents point XiTo point Xi+1Position vector of (1), pi+1,i+2Represents point Xi+1To point Xi+2Position vector of (1), pcIs the linearized decision constant and fabs is the absolute value.
3. The integrated navigation method of the autonomous underwater vehicle suitable for the environment under the polar ice tray according to claim 1, characterized in that the step 2) comprises:
solving for initial position estimates for beacons
The method specifically comprises the following steps:
wherein the time of 1 st receiving the ranging signal is t1Autonomous underwater robot at time t1Is at a position X1＝(x1,y1,z1)TAnd similarly defining the position of the autonomous underwater robot at the 2 nd, 3 rd, 4 th and 5 th time ranging moments as X2、X3、X4、X5They are obtained by direct measurement of a Doppler log, a deep water compass and a depth meter carried by an autonomous underwater robot, R1、R2、R3、R4、R5Respectively representing autonomous underwater vehicles at time t1、t2、t3、t4、t5Ranging from beacons, obtained by direct measurement by a range finder, H0And Y is an intermediate variable;
P0indicating the initiation of a beaconPosition estimation
The variance estimation and calculation method comprises the following steps:
wherein, XLThe three-dimensional position of the autonomous underwater robot when the autonomous underwater robot lays down the beacon is represented, and E () represents a mathematically expected calculation symbol.
4. The integrated navigation method of an autonomous underwater vehicle applicable to the environment under an ice shelf on the polar region as claimed in claim 1, wherein the measured jacobian matrix is:
Hk＝[ρi-4,i-3 ρi-3,i-2 ρi-2,i-1 ρi-1,i]T
wherein HkFor autonomous underwater robot at time tkMeasurement of the Jacobian matrix, rhoi-4,i-3Represents point Xi-4To point Xi-3The moving vector diameter of (1); rhoi-3,i-2Represents point Xi-3To point Xi-2The moving vector diameter of (1); rhoi-2,i-1Represents point Xi-2To point Xi-1The moving vector diameter of (1); rhoi-1,iRepresents point Xi-1To point XiThe displacement of (2) is the dynamic vector diameter.
5. The integrated navigation method of the autonomous underwater vehicle suitable for the environment under the polar ice tray of claim 4, characterized in that ρi-4,i-3、ρi-2,i-1、ρi-3,i-2、ρi-1,iThe calculation method comprises the following steps:
wherein u (t) represents the navigation speed of the autonomous underwater robot at the moment t, and is directly measured by a Doppler log carried by the autonomous underwater robot; psi (t) and theta (t) respectively represent a course angle and a pitch angle of the autonomous underwater robot at the moment t, and are directly measured by a deep-water compass carried by the autonomous underwater robot; z is a radical ofi、zi-1、zi-2、zi-3、zi-4Respectively representing autonomous underwater vehicles at time tiTime ti-1Time ti-2Time ti-3Time ti-4The depth of (2) is directly measured by a depth sensor carried by the autonomous underwater robot.
6. The integrated navigation method of the autonomous underwater vehicle suitable for the environment under the polar ice tray according to claim 1, characterized in that the step 4) comprises:
calculating the beacon at time tkVariance estimate P ofkAnd location estimation
The calculation method comprises the following steps:
wherein, PkIndicating that the beacon is at time tkThe estimate of the variance of (a) is,
indicating that the beacon is at time tkThe location estimate of (2); pk-1Indicating that the beacon is at time tk-1The estimate of the variance of (a) is,
indicating that the beacon is at time tk-1The location estimate of (2); kkIndicating that the beacon is at time tkEstimated gain of, YkRepresenting a column vector consisting of beacon ranges at different times, RkRepresents t introduced by autonomous underwater robot due to body navigation motion in the beacon calibration processkMeasurement of noise at time HRkIs an intermediate variable, Pk-1Representing the previous time tk-1Estimate of variance of u (t)k) Indicating autonomous underwater vehicle at tkThe navigation speed at the moment is directly measured by a Doppler log carried by the autonomous underwater robot, psi (t)k) And θ (t)k) Respectively representing autonomous underwater vehicles at time tkThe course angle and the longitudinal inclination angle of the underwater robot are directly measured by a deepwater compass carried by the autonomous underwater robot,
is a matrix of deep water compass and Doppler log, dialog (r.) representing a diagonal matrix operator, σuIs the speed measurement accuracy, sigma, of a Doppler logψIs the course accuracy of the deepwater compass,
representing the measurement variance, R, of a rangefinder carried by the autonomous Underwater roboti、Ri-1、Ri-2、Ri-3、Ri-4Respectively representing the distance measurement of the beacon and the autonomous underwater robot, which are provided by a distance measuring instrument carried by the autonomous underwater robot, I represents an identity matrix which is a constant, and k represents the time tkThe time index of (c).
7. Set of autonomous underwater robots adapted to environments under polar ice racks according to claim 6The integrated navigation method is characterized in that when k is 0, namely the current time is an initial time, PkAnd
position estimates respectively equal to the initial time
Sum variance estimate P0。
8. The integrated navigation method of an autonomous underwater vehicle suitable for use in an environment under an ice bank of polar regions according to claim 1, characterized in that said end condition is:
wherein | · | purple2The 2-norm operator is represented by a 2-norm operator,
indicating the time t fromk-20To time tkEstimate 2 norm of the expectation, αcIs the beacon calibration accuracy threshold, k represents the time tkFor is a key representing the desired calculation range.
9. The integrated navigation method of the autonomous underwater vehicle suitable for the environment under the polar ice tray according to claim 1, characterized in that the step 6) comprises:
according to tkDistance between autonomous underwater robot and beacon calibration position provided by time range finder
And navigating the autonomous underwater robot by combining the dead reckoning position of the autonomous underwater robot.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010198740.6A CN111928850B (en) | 2020-03-20 | 2020-03-20 | Combined navigation method of autonomous underwater robot suitable for polar region ice frame environment |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010198740.6A CN111928850B (en) | 2020-03-20 | 2020-03-20 | Combined navigation method of autonomous underwater robot suitable for polar region ice frame environment |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN111928850A true CN111928850A (en) | 2020-11-13 |
| CN111928850B CN111928850B (en) | 2023-12-29 |
Family
ID=73317508
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202010198740.6A Active CN111928850B (en) | 2020-03-20 | 2020-03-20 | Combined navigation method of autonomous underwater robot suitable for polar region ice frame environment |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN111928850B (en) |
Cited By (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112986902A (en) * | 2021-02-23 | 2021-06-18 | 自然资源部第三海洋研究所 | Method for estimating azimuth of underwater broadband sound source by single detector across ice layer |
| CN113190025A (en) * | 2021-05-07 | 2021-07-30 | 中国科学院沈阳自动化研究所 | Motion control method suitable for variable-structure underwater robot |
| CN114088098A (en) * | 2021-11-16 | 2022-02-25 | 哈尔滨工程大学 | Auxiliary navigation path planning method for polar region underwater vehicle database |
| CN114200929A (en) * | 2021-11-24 | 2022-03-18 | 中国科学院沈阳自动化研究所 | Rapid comb type path planning method for maximum detection coverage rate of multiple underwater robots |
| CN114228959A (en) * | 2021-12-29 | 2022-03-25 | 中国科学院沈阳自动化研究所 | Underwater robot polar region under-ice recovery method based on acoustic road sign and optical road sign combined auxiliary navigation |
| CN114644087A (en) * | 2021-12-31 | 2022-06-21 | 自然资源部第二海洋研究所 | Short base line bearing structure suitable for ice district |
Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20030078706A1 (en) * | 2000-03-03 | 2003-04-24 | Larsen Mikael Bliksted | Methods and systems for navigating under water |
| WO2009039488A1 (en) * | 2007-09-21 | 2009-03-26 | Hydroid, Inc. | Autonomous underwater vehicle used to calibrate a long baseline navigation network |
| CN106679662A (en) * | 2015-11-06 | 2017-05-17 | 中国科学院沈阳自动化研究所 | Combined underwater robot navigation method based on TMA (target motion analysis) technology and single beacon |
| KR101789188B1 (en) * | 2016-05-11 | 2017-10-24 | 한국해양과학기술원 | An underwater integrated navigation system for tracking underwater moving objects |
| US20190204430A1 (en) * | 2017-12-31 | 2019-07-04 | Woods Hole Oceanographic Institution | Submerged Vehicle Localization System and Method |
| CN110207695A (en) * | 2019-05-28 | 2019-09-06 | 哈尔滨工程大学 | It is a kind of suitable for deep-sea AUV without velocity aid list beacon localization method |
| CN110646783A (en) * | 2019-09-30 | 2020-01-03 | 哈尔滨工程大学 | Underwater beacon positioning method of underwater vehicle |
| CN110779518A (en) * | 2019-11-18 | 2020-02-11 | 哈尔滨工程大学 | Underwater vehicle single beacon positioning method with global convergence |
-
2020
- 2020-03-20 CN CN202010198740.6A patent/CN111928850B/en active Active
Patent Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20030078706A1 (en) * | 2000-03-03 | 2003-04-24 | Larsen Mikael Bliksted | Methods and systems for navigating under water |
| WO2009039488A1 (en) * | 2007-09-21 | 2009-03-26 | Hydroid, Inc. | Autonomous underwater vehicle used to calibrate a long baseline navigation network |
| CN106679662A (en) * | 2015-11-06 | 2017-05-17 | 中国科学院沈阳自动化研究所 | Combined underwater robot navigation method based on TMA (target motion analysis) technology and single beacon |
| KR101789188B1 (en) * | 2016-05-11 | 2017-10-24 | 한국해양과학기술원 | An underwater integrated navigation system for tracking underwater moving objects |
| US20190204430A1 (en) * | 2017-12-31 | 2019-07-04 | Woods Hole Oceanographic Institution | Submerged Vehicle Localization System and Method |
| CN110207695A (en) * | 2019-05-28 | 2019-09-06 | 哈尔滨工程大学 | It is a kind of suitable for deep-sea AUV without velocity aid list beacon localization method |
| CN110646783A (en) * | 2019-09-30 | 2020-01-03 | 哈尔滨工程大学 | Underwater beacon positioning method of underwater vehicle |
| CN110779518A (en) * | 2019-11-18 | 2020-02-11 | 哈尔滨工程大学 | Underwater vehicle single beacon positioning method with global convergence |
Non-Patent Citations (3)
| Title |
|---|
| ZHANG JUCHENG ET AL.: "Navigation Calibration Algorithm Assisted by Single Beacon", 《2019 IEEE INTERNATIONAL CONFERENCE ON SIGNAL PROCESSING, COMMUNICATIONS AND COMPUTING (ICSPCC)》, pages 1 - 5 * |
| 刘明雍;董婷婷;张立川;: "基于随机信标的水下SLAM导航方法", 系统工程与电子技术, no. 12, pages 168 - 172 * |
| 曹俊: "基于单信标测距的水下载体定位研究", 《中国博士学位论文全文数据库 (工程科技Ⅱ辑)》, pages 028 - 27 * |
Cited By (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112986902A (en) * | 2021-02-23 | 2021-06-18 | 自然资源部第三海洋研究所 | Method for estimating azimuth of underwater broadband sound source by single detector across ice layer |
| CN112986902B (en) * | 2021-02-23 | 2022-07-19 | 自然资源部第三海洋研究所 | Method for estimating underwater broadband sound source frequency-band azimuth by single detector across ice layer |
| CN113190025A (en) * | 2021-05-07 | 2021-07-30 | 中国科学院沈阳自动化研究所 | Motion control method suitable for variable-structure underwater robot |
| CN113190025B (en) * | 2021-05-07 | 2023-09-12 | 中国科学院沈阳自动化研究所 | Motion control method suitable for variable-structure underwater robot |
| CN114088098A (en) * | 2021-11-16 | 2022-02-25 | 哈尔滨工程大学 | Auxiliary navigation path planning method for polar region underwater vehicle database |
| CN114200929A (en) * | 2021-11-24 | 2022-03-18 | 中国科学院沈阳自动化研究所 | Rapid comb type path planning method for maximum detection coverage rate of multiple underwater robots |
| CN114200929B (en) * | 2021-11-24 | 2023-10-20 | 中国科学院沈阳自动化研究所 | Rapid comb-type path planning method for maximum detection coverage rate of multi-underwater robot |
| CN114228959A (en) * | 2021-12-29 | 2022-03-25 | 中国科学院沈阳自动化研究所 | Underwater robot polar region under-ice recovery method based on acoustic road sign and optical road sign combined auxiliary navigation |
| CN114644087A (en) * | 2021-12-31 | 2022-06-21 | 自然资源部第二海洋研究所 | Short base line bearing structure suitable for ice district |
Also Published As
| Publication number | Publication date |
|---|---|
| CN111928850B (en) | 2023-12-29 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111928850B (en) | Combined navigation method of autonomous underwater robot suitable for polar region ice frame environment | |
| CN109443379B (en) | SINS/DV L underwater anti-shaking alignment method of deep-sea submersible vehicle | |
| RU2557361C2 (en) | Declination compensation in seismic survey | |
| CN111595348B (en) | Master-slave mode cooperative positioning method of autonomous underwater vehicle combined navigation system | |
| CN107990891B (en) | Underwater robot combined navigation method based on long baseline and beacon online calibration | |
| CN103697910B (en) | The correction method of autonomous underwater aircraft Doppler log installation error | |
| CN105823480A (en) | Underwater moving target positioning algorithm based on single beacon | |
| CN108614258B (en) | Underwater positioning method based on single underwater sound beacon distance measurement | |
| CN102829777A (en) | Integrated navigation system for autonomous underwater robot and method | |
| CN104316045A (en) | AUV (autonomous underwater vehicle) interactive auxiliary positioning system and AUV interactive auxiliary positioning method based on SINS (strapdown inertial navigation system)/LBL (long base line) | |
| CN1325932C (en) | Assembled navigation positioning method for manned submersible | |
| Hegrenæs et al. | Underwater transponder positioning and navigation of autonomous underwater vehicles | |
| CN111273298B (en) | Underwater acoustic target positioning and tracking method based on wave glider networking technology | |
| CN102636771A (en) | AUV (Autonomous Underwater Vehicle) underwater acoustic locating method based on double mobile beacons | |
| CN111596333B (en) | Underwater positioning navigation method and system | |
| CN102288170A (en) | Correction method of electronic compass in underwater vehicle | |
| CN107966145B (en) | AUV underwater navigation method based on sparse long baseline tight combination | |
| Xu et al. | A novel self-adapting filter based navigation algorithm for autonomous underwater vehicles | |
| CN111829512A (en) | AUV navigation positioning method and system based on multi-sensor data fusion | |
| CN113433553B (en) | Precise navigation method for multi-source acoustic information fusion of underwater robot | |
| CN110763872A (en) | Multi-parameter online calibration method for Doppler velocimeter | |
| Matsuda et al. | Accurate and efficient seafloor observations with multiple autonomous underwater vehicles: Theory and experiments in a hydrothermal vent field | |
| CN108871379B (en) | DVL speed measurement error online calibration method | |
| Zhou et al. | Underwater acoustic-based navigation towards multi-vehicle operation and adaptive oceanographic sampling | |
| CN111735455A (en) | Improved Gaussian distance iterative algorithm based butt joint recovery integrated navigation method |
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 |