CN111928850B - Combined navigation method of autonomous underwater robot suitable for polar region ice frame environment - Google Patents
Combined navigation method of autonomous underwater robot suitable for polar region ice frame environment Download PDFInfo
- Publication number
- CN111928850B CN111928850B CN202010198740.6A CN202010198740A CN111928850B CN 111928850 B CN111928850 B CN 111928850B CN 202010198740 A CN202010198740 A CN 202010198740A CN 111928850 B CN111928850 B CN 111928850B
- Authority
- CN
- China
- Prior art keywords
- autonomous underwater
- underwater robot
- beacon
- time
- representing
- 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.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 36
- 239000013598 vector Substances 0.000 claims description 25
- 239000011159 matrix material Substances 0.000 claims description 18
- 238000005259 measurement Methods 0.000 claims description 18
- 238000004364 calculation method Methods 0.000 claims description 12
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 description 4
- 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
- 238000010586 diagram Methods 0.000 description 1
- 230000008030 elimination Effects 0.000 description 1
- 238000003379 elimination reaction Methods 0.000 description 1
- 230000004927 fusion Effects 0.000 description 1
- 238000005457 optimization Methods 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 238000012876 topography Methods 0.000 description 1
- 238000012795 verification Methods 0.000 description 1
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
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
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 a polar ice frame of the autonomous underwater robot, which aims at solving the problem of accumulated navigation errors of long-time navigation of the autonomous underwater robot in a polar underwater long-range, realizes that the autonomous underwater robot autonomously carries an acoustic navigation beacon, the autonomous underwater robot autonomously distributes the acoustic navigation beacon, autonomously calibrates the acoustic beacon to distribute a three-dimensional space position, and finally performs 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 in a long-time remote under ice. The method mainly solves the problem of position calibration of the beacon placed under the ice, and after the beacon calibration is successful, the autonomous underwater robot under the ice is navigated according to the beacon calibration position and the 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 ice frame of a polar region.
Background
In the ocean engineering and ocean science investigation process, the autonomous underwater robot plays an increasingly important role, and the autonomous underwater robot is used for replacing human beings to complete the observation of the polar ice-ocean system in the polar environment, so that the autonomous underwater robot is a hot spot for the research of important engineering equipment for supporting the polar scientific investigation at present. The observation platforms for polar ice hockey frames-ocean systems can be divided into various types of platforms such as space-based, ice-based, autonomous underwater robots and the like. The space-based observation platform comprises satellite remote sensing, aircraft mounting observation equipment and the like, and has the advantages of realizing high-efficiency large-area observation, and being limited in that a fine structure in the ice frame cannot be obtained; the ice-based observation platform mainly comprises a radar, a hot water drill and the like, and has the advantages of being capable of observing an ice frame, but limited in that the space coverage rate is low and the observation flexibility is insufficient. Compared with the two types of platforms, the autonomous underwater robot platform is a high-efficiency under-ice moving platform, can reach the inside observation of the ice frame and can flexibly move and observe in a large range, so that the autonomous underwater robot becomes more and more important ocean equipment for polar under-ice ocean observation. The autonomous underwater robot needs to survey and draw the shape of the ice frame, the submarine topography under the ice frame and observe the marine water body under the ice frame for a long time, wherein the navigation system has the function of accurately guiding the autonomous underwater robot to reach the observation target position, realizing high-precision position matching of observation data and observation space and ensuring the quality of the marine observation data under the ice frame, so the navigation problem under the ice frame is a key technical problem faced by the application of the autonomous underwater robot under the ice frame. The difficulty of navigation under the polar ice frame is that the drift angle error of the deepwater compass in the polar region is amplified along with the increase of the latitude, namely, the heading accuracy of the deepwater compass is reduced along with the increase of the latitude, and the reduction of the heading accuracy further reduces the position accuracy of autonomous navigation dead reckoning, so that the realization of high-accuracy 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 integrated 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 length of the base line of the long base line is generally 100 meters to 6000 meters, more than 3 long base line positioning beacons are distributed by a mother ship, then the mother ship navigates and marks the position of the long base line positioning beacons by using a large-scale ultra-short base line and a differential GPS carried by the mother ship in a water surface navigation mode around the long base line acoustic positioning beacons, and finally the autonomous underwater robot navigates in the long base line array, continuously fuses long base line ranging information and improves the position precision of the integrated navigation system. The length of the short base line is generally 1 to 50 meters, a short base line beacon is generally carried on a mother ship, an autonomous underwater robot sails nearby the mother ship, and the position accuracy of the integrated navigation system is improved by continuously fusing short base line ranging information. The length of the base line of the ultra-short base line is smaller than 1 meter, the ultra-short base line base array is generally carried on a mother ship, an autonomous underwater robot sails in the coverage range of the ultra-short base array of the mother ship, and the position accuracy of the integrated navigation system is improved by data fusion and ultra-short base line azimuth/distance measurement. Under the environment under the polar region ice frame, especially the thickness of antarctic ice frame reaches the kilometer, the mother ship can't reach the ice frame top and uses short baseline, the autonomous underwater robot under the ultra-short baseline carries out acoustic assistance location to the ice frame, can't lay long baseline location beacon more can't carry out the position to the deployment beacon, so need autonomous underwater robot independently carry the beacon to reach the ocean below the ice frame from the entrance under the ice frame, autonomously deploy the beacon to the seabed, autonomously mark the position of beacon, autonomous underwater robot corrects self navigation error through measuring the distance with the beacon, wherein how to utilize autonomous underwater robot to mark the beacon position is the key step of polar region under the ice combination navigation, the external measurement precision of autonomous underwater robot is directly influenced to the precision of demarcating, and then the navigation precision of operation autonomous underwater robot under the ice is decided. In recent years, researchers have made certain progress in the aspect of carrying out position calibration on a deployment beacon under the working condition of not depending on a mother ship, but a platform of the unmanned ship still has an unmanned ship for receiving GPS signals in real time and an autonomous underwater robot which floats up to the water surface to receive the GPS beacon, but the autonomous underwater robot in the polar ice environment does not have the objective condition for receiving GPS positioning, and the autonomous underwater robot autonomous navigation precision under the polar ice frame is obviously reduced in consideration of the fact that the autonomous underwater robot autonomous navigation precision under the polar ice frame is obviously reduced, so that a new autonomous underwater robot single beacon combined navigation method suitable for the polar ice frame environment is required, the pure distance indirect observation of the beacon and the autonomous underwater robot is utilized, the mobile vector diameter calculated by an internal navigation sensor of the autonomous underwater robot is introduced into an observation equation, and the maneuvering path of planning observation is analyzed through maneuvering elements, so that the precision and the observability of the beacon calibration 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 ice frame of a polar region, aiming at the problem of accumulated navigation error elimination of long-time navigation of the autonomous underwater robot in a long navigation range under the polar region, the autonomous underwater robot autonomously carries an acoustic navigation beacon, the autonomous underwater robot autonomously distributes the acoustic navigation beacon, autonomously calibrates the acoustic beacon to distribute a three-dimensional space position, and finally performs 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 in a long-time remote under the ice. The key technical problem of single-beacon integrated navigation of the polar ice autonomous underwater robot is to solve the problem of precisely calibrating beacons of the autonomous underwater robot, and the beacon position calibration is also the basis of the single-beacon integrated navigation, so the key point of the invention is to solve the problem of the position calibration of the ice deployment beacons, and after the beacons are calibrated successfully, the autonomous underwater robot is navigated according to the beacon calibration positions and the single-beacon ranging. The invention comprises the following steps: after the autonomous underwater robot is laid under the ice frame, the autonomous underwater robot improves observability of a beacon calibration equation by utilizing dead reckoning positions of the autonomous underwater robot and ranging information of the beacon and a maneuvering navigation method of the autonomous underwater robot, and adjacent time sequence ranging information is introduced to recursively improve calibration calculation efficiency and obtain high-precision beacon position estimation. And finally, substituting the calibration result into a single beacon integrated navigation filter to realize high-precision navigation of the autonomous underwater robot sailing under the ice frame environment. The method can effectively solve the navigation problem of the autonomous underwater robot in the ice frame environment, adopts a maneuvering navigation optimization observability strategy and a movement vector path strategy, improves the observability of the system, saves the time cost of navigation calculation, and has stronger engineering application value; the method is convenient to transplant, has strong expansibility, and is also suitable for the combined navigation application fields of polar region gliders under ice, polar region cable remote control underwater robots and the like.
The technical scheme adopted by the invention for achieving the purpose is as follows:
the integrated 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 performs maneuvering ranging on the beacons according to the constraint conditions of the planned track;
2) Calculating initial position and variance estimation of the beacon;
3) Calculating a measuring jacobian matrix of the autonomous underwater robot at the current moment;
4) Calculating the position estimation and variance estimation of the beacon at the current moment according to the jacobian matrix;
5) Judging whether an ending condition is met 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) The underwater robot navigates according to the recorded beacon position as a reference point.
The constraint conditions of the planned track are as follows:
wherein ρ is i,i+1 Representing point X i To point X i+1 Position vector ρ of (2) i+1,i+2 Representing point X i+1 To point X i+2 Position vector ρ of (2) c Is a linearization determination constant, and fabs is taken as an absolute value.
Step 2) comprises:
solving initial position estimates for beaconsThe method comprises the following steps:
wherein the time of the 1 st time of receiving the ranging signal is t 1 Self-helpThe main underwater robot is at time t 1 Is X in position 1 ＝(x 1 ,y 1 ,z 1 ) T Similarly, the positions of the autonomous underwater robot at the ranging moments of times 2, 3, 4 and 5 are defined as X 2 、X 3 、X 4 、X 5 They are obtained by direct measurement of Doppler log, deepwater compass and depth gauge carried by autonomous underwater robot, R 1 、R 2 、R 3 、R 4 、R 5 Respectively represent the autonomous underwater robot at the time t 1 、t 2 、t 3 、t 4 、t 5 Ranging with beacons, obtained by direct measurement by rangefinders, H 0 And Y is an intermediate variable;
P 0 initial position estimation representing beaconsThe variance estimation of (2) is calculated by:
wherein X is L Representing the three-dimensional position of the autonomous underwater robot itself when deploying the beacon, E () represents a mathematically desired computational symbol.
The measured jacobian matrix is:
H k ＝[ρ i-4,i-3 ρ i-3,i-2 ρ i-2,i-1 ρ i-1,i ] T
wherein H is k At time t for autonomous underwater robot k Measured jacobian matrix, ρ i-4,i-3 Representing point X i-4 To point X i-3 Is provided for the moving vector path; ρ i-3,i-2 Representing point X i-3 To point X i-2 Is provided for the moving vector path; ρ i-2,i-1 Representing point X i-2 To point X i-1 Is provided for the moving vector path; ρ i-1,i Representing point X i-1 To point X i Is provided.
ρ i-4,i-3 、ρ i-2,i-1 、ρ i-3,i-2 、ρ i-1,i The calculation method of (1) is as follows:
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 the course angle and the pitch angle of the autonomous underwater robot at the moment t, and are directly measured by a deepwater compass carried by the autonomous underwater robot; z i 、z i-1 、z i-2 、z i-3 、z i-4 Respectively represent the autonomous underwater robot at the time t i Time t i-1 Time t i-2 Time t i-3 Time t i-4 Is directly measured by a depth sensor mounted on the autonomous underwater robot.
Step 4) comprises:
calculating the beacon at time t k Variance estimation P of (2) k Position estimationThe calculation method comprises the following steps:
wherein P is k Indicating that the beacon is at time t k Variance of (2)The number of times the sample is to be evaluated,indicating that the beacon is at time t k Is determined by the position estimation of (a); p (P) k-1 Indicating that the beacon is at time t k-1 Variance estimation of->Indicating that the beacon is at time t k-1 Is determined by the position estimation of (a); k (K) k Indicating that the beacon is at time t k Is the estimated gain of Y k Representing column vectors consisting of beacon ranging at different times, R k Representing t introduced by autonomous underwater robot due to body sailing movement in beacon calibration process k Measuring noise at all times->Is an intermediate variable, P k-1 Indicating the previous time t k-1 Variance estimation of u (t) k ) Representing an autonomous underwater robot at t k The navigational speed at moment is directly measured by a doppler log carried by an autonomous underwater robot, ψ (t k ) And θ (t) k ) Respectively represent the autonomous underwater robot at the time t k Is directly measured by a deepwater compass carried by an autonomous underwater robot, and the heading angle and the pitch angle of the robot are ∈10>Is a matrix of deepwater compass and doppler log, dialogs (i.e.) representing diagonal matrix operators, σ u Is the speed measuring precision sigma of Doppler log ψ Is heading accuracy of the deepwater compass, < + >>Representing the measurement variance of a range finder carried by an autonomous underwater robot, R i 、R i-1 、R i-2 、R i-3 、R i-4 Respectively representing the distance measurement of the beacon and the autonomous underwater robot, which are provided by a distance meter carried by the autonomous underwater robot, wherein I represents an identity matrix which is a constant, and k represents the time t k Time of (1)Inter index.
When k=0, i.e. the current time is the initial time, P k Andposition estimation equal to the initial moment, respectively +.>Sum-of-variance estimation P 0 。
The end condition is:
wherein I II 2 The operator of the 2-norm is represented,representing the slave time t k-20 By time t k Is estimated to be 2 norms, alpha c Is the beacon calibration accuracy threshold, k represents the time t k For is a key representing the desired calculation range.
Step 6) comprises:
according to t k Distance between autonomous underwater robot and beacon provided by time range finder and beacon calibration positionAnd 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 under-ice autonomous calibration beacon, and overcomes the defect that the traditional combined navigation method can only rely 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 method combines the maneuvering navigation strategy with the movement vector path model, and improves the real-time performance and reliability of the application of the autonomous underwater robot combined navigation method under the ice frame.
3. The invention has wide application range, and can be applied to the combined navigation of autonomous underwater robots in the environment under the ice frame, and can also be applied to the combined navigation of polar ice gliders and polar cable remote control underwater robots.
Drawings
FIG. 1 is a schematic diagram of the 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 deepwater compass, a depth gauge, a range finder and a beacon. The autonomous underwater robot is a body carrying a Doppler log, a deepwater compass, a depth gauge and a range finder and is also a carrier carrying a beacon. The Doppler log is used for measuring the navigation speed of the autonomous underwater robot; the function of the deepwater compass is to measure the attitude angle of the autonomous underwater robot; the deepwater meter is used for measuring the depth of the autonomous underwater robot; the range finder 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 arranged on the seabed below the ice bank, and has the function of providing an acoustic positioning response signal for the autonomous underwater robot, and the system is composed as shown in fig. 1.
The whole system works according to the following procedures:
for convenience of the following description, a definition is given herein for a part of the symbol variables: definition when autonomous underwater robots are respectively at time t i Time t i-1 Time t i-2 Time t i-3 When receiving the distance measurement between itself and the beacon, R is used for distance measurement corresponding to each moment i 、R i-1 、R i-2 、R i-3 A representation, wherein i represents a time sequence number of the ranging; definition X i Indicating the autonomous underwater robot at time t i X is the three-dimensional spatial position of (2) i ＝(x i ,y i ,z i ) T Wherein x is i ,，y i ,，z i Respectively representing the north position, the east position and the depth; definition ρ i-1,i Representing point X i-1 To point X i Is used for the position vector of (a).
A flow chart of the present invention is shown in fig. 2.
First, performing maneuvering ranging on a beacon according to a planned track
After the autonomous underwater robot autonomously lays the beacon, the autonomous underwater robot navigates dynamically and ranges the beacon. The requirements on the maneuvering navigation track are as follows, after the autonomous underwater robot distributes the beacon to the preset position, the autonomous underwater robot performs maneuvering navigation by taking the beacon as the center and simultaneously ranges the beacon, the maneuvering navigation track is required to meet maneuvering observable conditions, and the distinguishing basis of maneuvering observable is that the following distinguishing formula is met:
wherein ρ is defined as i,i+1 Representing point X i To point X i+1 Is a position vector of (2); ρ i+1,i+2 Representing point X i+1 To point X i+2 Is a position vector of (2); ρ i+2,i+3 Representing point X i+2 To point X i+3 Position vector ρ of (2) c Is a linearization determination constant ρ c The engineering verification value can be 0.9. The physical meaning of the discriminant is that when the adjacent position vector satisfies the discriminant then the adjacent position vector is linearly independent. Therefore, in the practical engineering application process, the maneuvering navigation is recommended to adopt a polygonal track (the number of sides is not less than 5), a circular track, an elliptic track, an Arabic numeral 8-shaped track and the like, and the three-dimensional distance from a track planning point to the initial laying position of the beacon is between 100 meters and 600 meters.
Second, calculate initial position and variance estimates for beacons
Definition of the definitionRepresenting the three-dimensional position of a beaconAn initial position estimate, which is an unknown quantity to be solved, is obtained by solving the following system of equations:
wherein the time t of the 1 st received ranging signal is defined as 1 Autonomous underwater robot at time t 1 Is X in position 1 ＝(x 1 ,y 1 ,z 1 ) T Similarly, the positions of the autonomous underwater robot at the ranging moments of times 2, 3, 4 and 5 are defined as X 2 、X 3 、X 4 、X 5 They are all obtained by direct measurement of a doppler log, a deepwater compass and a depth gauge carried by an autonomous underwater robot, and are known input quantities. R is R 1 、R 2 、R 3 、R 4 、R 5 Respectively represent the autonomous underwater robot at the time t 1 、t 2 、t 3 、t 4 、t 5 Ranging with beacons, which are obtained by direct measurement by a range finder.
H 0 And Y is an intermediate variable.
Definition P 0 Initial position estimation representing beacon three-dimensional positionThe variance estimation of (2) is calculated as follows:
wherein X is L Representing the three-dimensional position of the autonomous underwater robot itself when deploying the beacon, E () represents a mathematically desired computational symbol.
Thirdly, calculating the time t of the autonomous underwater robot k Measured jacobian matrix H of (a) k The calculation method is as follows:
H k ＝[ρ i-4,i-3 ρ i-3,i-2 ρ i-2,i-1 ρ i-1,i ] T (4)
wherein ρ is i-4,i-3 Representing point X i-4 To point X i-3 Is provided for the moving vector path; ρ i-2,i-1 Representing point X i-2 To point X i-1 Is provided for the moving vector path; ρ i-3,i-2 Representing point X i-3 To point X i-2 Is provided for the moving vector path; ρ i-1,i Representing point X i-1 To point X i Are intermediate variables, ρ i-4,i-3 、ρ i-2,i-1 、ρ i-3,i-2 、ρ i-1,i The calculation method of (2) is as follows:
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 and is a known input quantity; psi (t) and theta (t) respectively represent the course angle and the pitch angle of the autonomous underwater robot at the moment t, and the direct measurement of the deepwater compass carried by the autonomous underwater robot is a known input quantity; z i 、z i-1 、z i-2 、z i-3 Respectively represent the autonomous underwater robot at the time t i Time t i-1 Time t i-2 Time t i-3 Is a known input quantity, which is directly measured by a depth sensor mounted on an autonomous underwater robot.
Fourth step, calculating the beacon at time t k Position estimation and variance estimation of (a)
Definition of the definitionAnd P k Respectively indicate the beacon at time t k The position and variance estimates of (a) are calculated as follows:
wherein P is k Indicating that the beacon is at time t k Is provided for the location estimate of (a),indicating that the beacon is at time t k And the variance estimates of the equation are variables to be solved, and the whole equation is directly solved by a substitution method. In special cases, i.e. when k=0 (indicating that the current time is the initial time), then P k And->Position estimation equal to the initial moment, respectively +.>Sum-of-variance estimation P 0 The equation set does not need to be solved, and the value can be directly taken. K (K) k Indicating that the beacon is at time t k Is an intermediate variable; y is Y k Representing a column vector consisting of beacon ranging at different times, which is an intermediate variable; r is R k Representing t introduced by autonomous underwater robot due to body sailing movement in beacon calibration process k Time measurement noise, which is an intermediate variable; />Is for describing R k An intermediate variable which is conveniently introduced and is a matrix formed by known input quantities; p (P) k-1 Indicating the previous time t k-1 For t k A set of equations for time of day, which is a known quantity; u (t) k ) Self-expressionThe main underwater robot is at t k The navigation speed at 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 ) The heading angle and the pitch angle of the autonomous underwater robot at the moment are respectively expressed, and the deepwater compass carried by the autonomous underwater robot is used for directly measuring the input quantity;is a correlation matrix of a deepwater compass and a Doppler log, and dialogs (.) u Is the speed measuring precision sigma of Doppler log ψ Is the heading precision sigma of the deepwater compass u Sum sigma ψ The parameters are directly provided by factories when the equipment leaves the factory and are known parameters; />The measurement variance of the range finder carried by the autonomous underwater robot is represented, and the parameter is directly provided by a manufacturer when the equipment leaves the factory and is a known parameter. R is R i 、R i-1 、R i-2 、R i-3 The range of the beacon and the autonomous underwater robot are indicated, and they are provided by a range finder carried by the autonomous underwater robot, and are known input quantities. I represents the identity matrix of 3*3, which is a constant.
Fifth, repeating the third to fourth steps until t k And stopping the beacon calibration process when the beacon position estimation calculated at the moment meets the following discriminant.
Wherein I II 2 The operator of the 2-norm is represented,representing the slave time t k-20 By time t k Is expected for a 2-norm estimate of the neighboring locations of (a); alpha c The beacon calibration precision threshold value can be set by a user, the recommended value is 10, and k represents the time t k Time of (1)The inter index, for, is a key representing the desired calculation range.
Sixth step, according to t k Distance between autonomous underwater robot and beacon provided by time range finder and beacon calibration positionAnd constructing a single-beacon combined navigation filter according to a standard Kalman filter by combining the dead reckoning position of the autonomous underwater robot, and calculating the optimal estimation of the underwater position of the autonomous underwater robot in real time. This step is not a claim, as it 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 performs maneuvering ranging on the beacons according to the constraint conditions of the planned track;
2) Calculating initial position and variance estimation of the beacon;
3) Calculating a measuring jacobian matrix of the autonomous underwater robot at the current moment;
4) Calculating the position estimation and variance estimation of the beacon at the current moment according to the jacobian matrix;
5) Judging whether an ending condition is met 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) The underwater robot navigates according to the recorded beacon position as a reference point.
2. The method for integrated navigation of autonomous underwater vehicles adapted to an environment under an polar ice bank according to claim 1, characterized in that the constraints of the planned trajectory are:
wherein ρ is i,i+1 Representing point X i To point X i+1 Position vector ρ of (2) i+1,i+2 Representing point X i+1 To point X i+2 Position vector ρ of (2) c Is a linearization determination constant, and fabs is taken as an absolute value.
3. The method of integrated navigation of an autonomous underwater robot adapted for use in an polar ice bank environment of claim 1, wherein step 2) comprises:
solving initial position estimates for beaconsThe method comprises the following steps:
wherein the time of the 1 st time of receiving the ranging signal is t 1 Autonomous underwater robot at time t 1 Is X in position 1 ＝(x 1 ,y 1 ,z 1 ) T Similarly, the positions of the autonomous underwater robot at the ranging moments of times 2, 3, 4 and 5 are defined as X 2 、X 3 、X 4 、X 5 They are obtained by direct measurement of Doppler log, deepwater compass and depth gauge carried by autonomous underwater robot, R 1 、R 2 、R 3 、R 4 、R 5 Respectively represent the autonomous underwater robot at the time t 1 、t 2 、t 3 、t 4 、t 5 Ranging with beacons, obtained by direct measurement by rangefinders, H 0 And Y is an intermediate variable;
P 0 initial position estimation representing beaconsThe variance estimation of (2) is calculated by:
wherein X is L Representing the three-dimensional position of the autonomous underwater robot itself when deploying the beacon, E () represents a mathematically desired computational symbol.
4. The integrated navigation method of an autonomous underwater robot adapted to an environment under a polar ice bank according to claim 1, wherein the measurement jacobian matrix is:
H k ＝[ρ i-4,i-3 ρ i-3,i-2 ρ i-2,i-1 ρ i-1,i ] T
wherein H is k At time t for autonomous underwater robot k Measured jacobian matrix, ρ i-4,i-3 Representing point X i-4 To point X i-3 Is provided for the moving vector path; ρ i-3,i-2 Representing point X i-3 To point X i-2 Is provided for the moving vector path; ρ i-2,i-1 Representing point X i-2 To point X i-1 Is provided for the moving vector path; ρ i-1,i Representing point X i-1 To point X i Is provided.
5. The method for integrated navigation of an autonomous underwater vehicle adapted to an polar ice bank environment according to claim 4, characterized by ρ i-4,i-3 、ρ i-2,i-1 、ρ i-3,i-2 、ρ i-1,i The calculation method of (1) is as follows:
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 the course angle and the pitch angle of the autonomous underwater robot at the moment t, and are directly measured by a deepwater compass carried by the autonomous underwater robot; z i 、z i-1 、z i-2 、z i-3 、z i-4 Respectively represent the autonomous underwater robot at the time t i Time t i-1 Time t i-2 Time t i-3 Time t i-4 Is directly measured by a depth sensor mounted on the autonomous underwater robot.
6. The method of integrated navigation of an autonomous underwater robot adapted for use in an polar ice bank environment of claim 5, wherein step 4) comprises:
calculating the beacon at time t k Variance estimation P of (2) k Position estimationThe calculation method comprises the following steps:
wherein P is k Indicating that the beacon is at time t k Is a variance estimate of (a),indicating that the beacon is at time t k Is determined by the position estimation of (a); p (P) k-1 Indicating that the beacon is at time t k-1 Variance estimation of->Indicating that the beacon is at time t k-1 Is determined by the position estimation of (a); k (K) k Indicating that the beacon is at time t k Is the estimated gain of Y k Representing column vectors consisting of beacon ranging at different times, R k Representing t introduced by autonomous underwater robot due to body sailing movement in beacon calibration process k Measuring noise at all times->Is an intermediate variable, P k-1 Indicating the previous time t k-1 Variance estimation of u (t) k ) Representing an autonomous underwater robot at t k The navigational speed at moment is directly measured by a doppler log carried by an autonomous underwater robot, ψ (t k ) And θ (t) k ) Respectively represent the autonomous underwater robot at the time t k Is directly measured by a deepwater compass carried by an autonomous underwater robot, and the heading angle and the pitch angle of the robot are ∈10>Is a matrix of deepwater compass and doppler log, dialogs (i.e.) representing diagonal matrix operators, σ u Is the speed measuring precision sigma of Doppler log ψ Is heading accuracy of the deepwater compass, < + >>Representing the measurement variance of a range finder carried by an autonomous underwater robot, R i 、R i-1 、R i-2 、R i-3 、R i-4 Respectively representing the distance measurement of the beacon and the autonomous underwater robot, which are provided by a distance meter carried by the autonomous underwater robot, wherein I represents an identity matrix which is a constant, and k represents the time t k Is used for the time index of (a).
7. The integrated navigation method of an autonomous underwater vehicle adapted to an environment under an polar ice bank according to claim 6, wherein when k=0, i.e. the current moment is the initial moment, P k Andposition estimation equal to the initial moment, respectively +.>Sum-of-variance estimation P 0 。
8. The method for integrated navigation of autonomous underwater vehicles adapted to an environment under an polar ice bank according to claim 7, characterized in that said ending condition is:
wherein I II 2 The operator of the 2-norm is represented,representing the slave time t k-20 By time t k Is estimated to be 2 norms, alpha c Is the beacon calibration accuracy threshold, k represents the time t k For is a key representing the desired calculation range.
9. The method of integrated navigation of an autonomous underwater robot adapted for use in an polar ice bank environment of claim 1, wherein step 6) comprises:
according to t k Distance between autonomous underwater robot and beacon provided by time range finder and beacon calibration positionAnd 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 CN111928850A (en) | 2020-11-13 |
| CN111928850B true 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) |
Families Citing this family (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112986902B (en) * | 2021-02-23 | 2022-07-19 | 自然资源部第三海洋研究所 | Method for estimating underwater broadband sound source frequency-band azimuth by single detector across ice layer |
| CN113190025B (en) * | 2021-05-07 | 2023-09-12 | 中国科学院沈阳自动化研究所 | Motion control method suitable for variable-structure underwater robot |
| CN114234964B (en) * | 2021-11-05 | 2024-06-14 | 河北汉光重工有限责任公司 | Positioning method and system of integrated autonomous underwater vehicle under ice layer |
| CN114200929B (en) * | 2021-11-24 | 2023-10-20 | 中国科学院沈阳自动化研究所 | Rapid comb-type path planning method for maximum detection coverage rate of multi-underwater robot |
| CN114228959B (en) * | 2021-12-29 | 2023-03-21 | 中国科学院沈阳自动化研究所 | Underwater robot polar region under-ice recovery method based on acoustic road sign and optical road sign combined auxiliary navigation |
| CN114644087B (en) * | 2021-12-31 | 2023-03-28 | 自然资源部第二海洋研究所 | Short base line bearing structure suitable for ice district |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| 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 |
| 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 |
Family Cites Families (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| AU2001240470A1 (en) * | 2000-03-03 | 2001-09-12 | Mikael Bliksted Larsen | Methods and systems for navigating under water |
| US20190204430A1 (en) * | 2017-12-31 | 2019-07-04 | Woods Hole Oceanographic Institution | Submerged Vehicle Localization System and Method |
-
2020
- 2020-03-20 CN CN202010198740.6A patent/CN111928850B/en active Active
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| 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 |
| 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 |
|---|
| Navigation Calibration Algorithm Assisted by Single Beacon;Zhang Jucheng et al.;《2019 IEEE International Conference on Signal Processing, Communications and Computing (ICSPCC)》;1-5 * |
| 基于单信标测距的水下载体定位研究;曹俊;《中国博士学位论文全文数据库 (工程科技Ⅱ辑)》;C028-27 * |
| 基于随机信标的水下SLAM导航方法;刘明雍;董婷婷;张立川;;系统工程与电子技术(第12期);168-172 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN111928850A (en) | 2020-11-13 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111928850B (en) | Combined navigation method of autonomous underwater robot suitable for polar region ice frame environment | |
| CN105823480B (en) | Underwater moving target location algorithm based on single beacon | |
| CN102829777B (en) | Autonomous underwater vehicle combined navigation system and method | |
| CN104457754B (en) | SINS/LBL (strapdown inertial navigation systems/long base line) tight combination based AUV (autonomous underwater vehicle) underwater navigation positioning method | |
| Vaganay et al. | Homing by acoustic ranging to a single beacon | |
| CN111595348B (en) | Master-slave mode cooperative positioning method of autonomous underwater vehicle combined navigation system | |
| CN106017467B (en) | A kind of inertia/underwater sound Combinated navigation method based on more underwater transponders | |
| CN111829512B (en) | AUV navigation positioning method and system based on multi-sensor data fusion | |
| CN103697910B (en) | The correction method of autonomous underwater aircraft Doppler log installation error | |
| CN102288170B (en) | Correction method of electronic compass in underwater vehicle | |
| 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) | |
| Hegrenæs et al. | Underwater transponder positioning and navigation of autonomous underwater vehicles | |
| PT1275012E (en) | Methods and systems for navigating under water | |
| CN102052924A (en) | Combined navigation and positioning method of small underwater robot | |
| CN101900558A (en) | Combined navigation method of integrated sonar micro navigation autonomous underwater robot | |
| CN102636771A (en) | AUV (Autonomous Underwater Vehicle) underwater acoustic locating method based on double mobile beacons | |
| CN107966145B (en) | AUV underwater navigation method based on sparse long baseline tight combination | |
| CN107990891A (en) | Underwater robot Combinated navigation method based on Long baselines and beacon on-line proving | |
| Matsuda et al. | Accurate and efficient seafloor observations with multiple autonomous underwater vehicles: Theory and experiments in a hydrothermal vent field | |
| CN106679662A (en) | Combined underwater robot navigation method based on TMA (target motion analysis) technology and single beacon | |
| Baccou et al. | Simulation results, post-processing experimentations and comparisons results for navigation, homing and multiple vehicles operations with a new positioning method using on transponder | |
| CN112747748A (en) | Pilot AUV navigation data post-processing method based on reverse solution | |
| CN114509069B (en) | Indoor navigation positioning system based on Bluetooth AOA and IMU fusion | |
| CN111735455A (en) | Improved Gaussian distance iterative algorithm based butt joint recovery integrated navigation method | |
| CN111189446B (en) | Combined navigation method based on radio |
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 |