CN109974706B - Master-slave mode multi-AUV collaborative navigation method based on double motion model - Google Patents

Master-slave mode multi-AUV collaborative navigation method based on double motion model Download PDF

Info

Publication number
CN109974706B
CN109974706B CN201910177062.2A CN201910177062A CN109974706B CN 109974706 B CN109974706 B CN 109974706B CN 201910177062 A CN201910177062 A CN 201910177062A CN 109974706 B CN109974706 B CN 109974706B
Authority
CN
China
Prior art keywords
auv
time
state
navigation
piloting
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
Application number
CN201910177062.2A
Other languages
Chinese (zh)
Other versions
CN109974706A (en
Inventor
徐博
李盛新
李金�
张勇刚
王潇雨
郭瑜
张大龙
李志鹏
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Harbin Engineering University
Original Assignee
Harbin Engineering University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Harbin Engineering University filed Critical Harbin Engineering University
Priority to CN201910177062.2A priority Critical patent/CN109974706B/en
Publication of CN109974706A publication Critical patent/CN109974706A/en
Application granted granted Critical
Publication of CN109974706B publication Critical patent/CN109974706B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Navigation (AREA)

Abstract

The invention belongs to the field of navigation research of underwater vehicles, and particularly relates to a master-slave mode multi-AUV collaborative navigation method based on a double-motion model, which comprises the following steps: the piloting AUV and the following AUV carry out underwater acoustic ranging, and simultaneously, the piloting AUV broadcasts and sends self position and speed information to the following AUV; establishing a relative motion state space model between the following AUV and the piloting AUV; estimating a velocity component difference value of the following AUV and the pilot AUV through the CKF; establishing a multi-AUV collaborative navigation state space model in a dual-pilot mode; the invention provides a method for combining an AUV relative motion state space model with a multi-AUV collaborative navigation state space model in a dual-pilot mode, so that the collaborative positioning performance of a multi-AUV collaborative navigation system is ensured; the invention follows the AUV without being equipped with inertial navigation equipment and DVL, thereby reducing the complexity of AUV system configuration, saving the internal space of the following AUV and reducing the weight.

Description

Master-slave mode multi-AUV collaborative navigation method based on double motion model
Technical Field
The invention belongs to the field of underwater vehicle navigation research, and particularly relates to a master-slave mode multi-AUV collaborative navigation method based on a double-motion model.
Background
The collaborative navigation is one of the most effective navigation methods of the multi-autonomous underwater vehicle in the middle layer area at present, and has wide application prospect. In general, multi-AUV collaborative navigation positioning has two forms: 1) the navigation system is parallel, namely, each aircraft in the system has the same function and structure, the navigation system is used for positioning, and the position information of other aircraft is obtained through underwater acoustic communication; 2) and the slave mode is also called a pilot mode, namely a small number of pilot AUVs are provided with high-precision navigation equipment in the system, a large number of following AUVs are provided with low-precision navigation equipment, the following AUVs improve the self-navigation precision by obtaining the position relation with the pilot AUVs, and the position of the self in the system is determined through underwater acoustic communication. The parallel mode has a simple structure, but each AUV is provided with high-precision navigation equipment, the cost is increased by many times, and the master-slave mode gives consideration to the navigation precision and the cost, so that the parallel mode becomes the main direction of multi-AUV collaborative navigation research. In the master-slave type collaborative navigation system, the piloting AUV is provided with high-precision inertial navigation equipment, a Doppler Velocity Log (DVL), a Differential Global Positioning System (DGPS), underwater acoustic communication equipment and the like, the navigation system takes the high-precision inertial navigation equipment as a main part, the initial position is obtained through the DGPS, the absolute speed measured by the DVL is taken as the external input of inertial navigation, the collaborative positioning precision is further improved, and the AUV is followed with low-precision inertial navigation equipment, the DVL, the underwater acoustic communication equipment and the like. Based on a sensing network built by high-precision and low-precision navigation equipment, the navigation and positioning performance of the whole formation is improved through relative measurement and information sharing among AUVs. However, under the condition of a large number of following AUVs, even if the following AUVs are equipped with low-precision dead reckoning navigation devices, the required inertial navigation devices and DVLs are still cost-prohibitive, so in practical engineering applications, if the following AUVs are not equipped with inertial navigation devices and DVLs, the positioning precision of the collaborative navigation system can be guaranteed to be within an allowable range, and the method has great research value.
Disclosure of Invention
The invention aims to provide a master-slave multi-AUV collaborative navigation method for estimating the position state of a following AUV by using CKF under the condition that the following AUV is not provided with inertial navigation equipment and DVL, thereby reducing the cost of a multi-AUV collaborative navigation system.
A master-slave mode multi-AUV collaborative navigation method based on a double-motion model comprises the following steps:
(1) the piloting AUV and the following AUV carry out underwater acoustic ranging, and simultaneously, the piloting AUV broadcasts and sends self position and speed information to the following AUV;
(2) establishing a relative motion state space model between the following AUV and the piloting AUV;
(3) estimating a velocity component difference value of the following AUV and the pilot AUV through CKF according to the relative motion state space model established in the step (2);
(4) establishing a multi-AUV collaborative navigation state space model in a dual-pilot mode;
(5) and (4) estimating the following AUV position information through CKF according to the collaborative navigation state space model established in the step (4).
The relative motion state space model comprises a state equation and a measurement equation;
the state equation is as follows:
following AUV at tkTime t andk+1the position vectors of the time are respectively
Figure GDA0002965668890000021
And
Figure GDA0002965668890000022
piloting AUV-1 at tkTime t andk+1the position vectors of the time are respectively
Figure GDA0002965668890000023
And
Figure GDA0002965668890000024
piloting AUV-2 at tkTime t andk+1the position vectors of the time are respectively
Figure GDA0002965668890000025
And
Figure GDA0002965668890000026
at t is respectively following AUV, piloting AUV-1 and piloting AUV-2kEast position of time;
Figure GDA0002965668890000027
at t is respectively following AUV, piloting AUV-1 and piloting AUV-2kThe north position of the time;
Figure GDA0002965668890000028
at t is respectively following AUV, piloting AUV-1 and piloting AUV-2k+1East position of time;
Figure GDA0002965668890000029
respectively a following AUV, a piloting AUV-1 and a piloting AUV-2 at tk+1The north position of the time;
following AUV at tkThe state vector at the moment of time is
Figure GDA00029656688900000210
Wherein the content of the first and second substances,
Figure GDA00029656688900000211
respectively following AUV at tkAn east and north velocity component of the time;
piloting AUV-1 at tkThe state vector at the moment of time is
Figure GDA00029656688900000212
Wherein the content of the first and second substances,
Figure GDA00029656688900000213
navigation AUV-1 at tkAn east and north velocity component of the time;
Figure GDA00029656688900000214
and
Figure GDA00029656688900000215
the expression of (a) is as follows:
Figure GDA00029656688900000216
wherein the content of the first and second substances,
Figure GDA00029656688900000217
at t for the piloting AUV-1 provided by the DVL, respectivelykStarboard and forward speeds at time;
Figure GDA00029656688900000218
piloting AUV-1 provided by high precision inertial navigation equipment at tkA course angle at a moment;
the position state equation following the AUV is:
Figure GDA00029656688900000219
wherein δ t is a sampling time interval;
the position state equation of the piloting AUV-1 is as follows:
Figure GDA0002965668890000031
following AUV and piloting AUV-1 at tkThe relative motion state vector at a time is:
Figure GDA0002965668890000032
wherein the content of the first and second substances,
Figure GDA0002965668890000033
to follow AUV relative navigation AUV-1 at tkEast distance of time;
Figure GDA0002965668890000034
to follow AUV relative navigation AUV-1 at tkThe north distance of the moment;
Figure GDA0002965668890000035
to follow AUV relative navigation AUV-1 at tkEast speed difference at time;
Figure GDA0002965668890000036
to follow AUV relative navigation AUV-1 at tkThe north velocity difference at that moment;
the discrete state equation for the system is:
Xk+1＝Fk+1|kXk
wherein the content of the first and second substances,
Figure GDA0002965668890000037
to follow between AUV and piloting AUV-1 at tkRelative motion state vectors at time; xk+1To follow between AUV and piloting AUV-1 at tk+1Relative motion state vectors at time; fk+1|kIs tkTime tk+1The state transition matrix of the time of day,
Figure GDA0002965668890000038
the measurement equation is as follows:
the coordinate position relation between the following AUV and the piloting AUV-1 is as follows:
Figure GDA0002965668890000039
the discrete time state space model following the relative motion of the AUV and the pilot AUV-1 is as follows:
Figure GDA00029656688900000310
wherein k +1 represents tk+1Time of day; measurement function
Figure GDA00029656688900000311
Δxk+1To follow AUV relative navigation AUV-1 at tk+1East distance of time; Δ yk+1To follow AUV relative navigation AUV-1 at tk+1The north distance of the moment; w is akIs a process noise vector, vk+1To measure the noise vector, and wk、vk+1Are all white gaussian noise, and the noise is,
Figure GDA00029656688900000312
for navigation between AUV-1 and following AUV at tkThe relative distance measurement information of the moment.
Estimating a velocity component difference value of the following AUV and the pilot AUV through CKF according to the relative motion state space model established in the step (2), wherein the velocity component difference value comprises the following velocity component difference values:
and (3) time updating:
state vector
Figure GDA0002965668890000041
At tkThe posterior probability state of the time system is
Figure GDA0002965668890000042
A posterior probability density function of
Figure GDA0002965668890000043
Covariance of state error P by Choleskyk|kThe decomposition is in the form:
Figure GDA0002965668890000044
wherein S isk|k＝chol{Pk|kDenotes Cholesky decomposition of the matrix,
Figure GDA0002965668890000045
is Sk|kTransposing;
the Cubature point is calculated as follows:
Figure GDA0002965668890000046
where (i ═ 1,2 …, m, m ═ 2n), n is the equation of state dimension, and the following variables are defined:
Figure GDA0002965668890000047
wherein m is 2n and is the number of the volume points; n is 4 as the dimension of the state equation; xiiIs the generated volume point; [1]iThe ith volume point in the point set is as follows:
Figure GDA0002965668890000048
withe weight value occupied by each volume point;
the Cubature point is transferred by a transition matrix function of the system state:
Xi,k+1|k＝Fk+1|kXi,k|k
tk+1the state predicted value at the moment is obtained by weighted summation:
Figure GDA0002965668890000049
calculating tk+1State error covariance prediction at time:
Figure GDA00029656688900000410
wherein, Xi,k+1|kTo transfer the Cubature point through the transition matrix function of the system state,
Figure GDA00029656688900000411
is Xi,k+1|kTransposing;
Figure GDA0002965668890000051
is tk+1The predicted value of the state at the moment,
Figure GDA0002965668890000052
is composed of
Figure GDA0002965668890000053
Transposing; system noise covariance matrix Qk＝E[wkwk T]，wkIs Gaussian white noise, wk TIs wkTransposition is carried out;
measurement updating:
the state error covariance predictor is decomposed by Cholesky into the following form:
Figure GDA0002965668890000054
wherein S isk+1|k＝chol{Pk+1|k}，
Figure GDA0002965668890000055
Is Sk+1|kTransposing;
the Cubature point is calculated as follows:
Figure GDA0002965668890000056
the Cubature point is passed through the system's measurement function:
Zi,k+1|k＝h(Xi,k+1|k,k+1)
tk+1the observation predicted value of the moment is obtained by weighted summation:
Figure GDA0002965668890000057
calculating tk+1The measurement error covariance predicted value at the moment is as follows:
Figure GDA0002965668890000058
wherein Z isi,k+1|kThe Cubature point is passed for the system's measurement function,
Figure GDA0002965668890000059
is Zi,k+1|kTransposing;
Figure GDA00029656688900000510
is tk+1The observed and predicted value of the time is,
Figure GDA00029656688900000511
is composed of
Figure GDA00029656688900000512
Transposing; covariance matrix for measuring noise
Figure GDA00029656688900000513
vk+1Is white gaussian noise, and is a noise,
Figure GDA00029656688900000514
is v isk+1Transposition is carried out;
cross covariance matrix:
Figure GDA00029656688900000515
and (3) estimating Kalman filtering gain:
Figure GDA00029656688900000516
status update procedure, obtaining tk+1State estimation at time:
Figure GDA00029656688900000517
obtaining t using a covariance update processk+1The state estimation error covariance matrix at the time instant, i.e.:
Figure GDA0002965668890000061
wherein the content of the first and second substances,
Figure GDA0002965668890000062
is Kk+1Transposing;
the process is carried out by successive recursion until the covariance of the state estimation error reaches a stable value, namely the relative state estimation of the following AUV and the pilot AUV-1 is obtained, and the process is carried out according to tk+1Relative state estimation of time of day
Figure GDA0002965668890000063
Get the following AUV at tk+1The east velocity estimation value and the north velocity estimation value at the moment are respectively
Figure GDA0002965668890000064
And
Figure GDA0002965668890000065
wherein following AUV at tk+1Estimated course angle at the moment of time of
Figure GDA0002965668890000066
Navigation AUV-1 at tk+1An east and north velocity component of the time;
Figure GDA0002965668890000067
to follow AUV relative navigation AUV-1 at tk+1An east distance estimate of the time;
Figure GDA0002965668890000068
to follow AUV relative navigation AUV-1 at tk+1A north distance estimate of the time;
Figure GDA0002965668890000069
to follow AUV relative navigation AUV-1 at tk+1An east velocity difference estimate of the time;
Figure GDA00029656688900000610
to follow AUV relative navigation AUV-1 at tk+1The north velocity difference estimate at time.
The establishing of the multi-AUV collaborative navigation state space model in the dual-pilot mode comprises the following steps:
the collaborative navigation system state equation based on the relative position measurement specifically comprises the following steps:
Figure GDA00029656688900000611
the coordinate position relations among the piloting AUV-1, the piloting AUV-2 and the following AUV are as follows:
Figure GDA00029656688900000612
the discrete time state space model of the multi-AUV cooperative positioning system is as follows:
Figure GDA00029656688900000613
wherein k +1 represents tk+1Time of day;
Figure GDA00029656688900000614
to follow AUV at tkA position state quantity of a time;
Figure GDA00029656688900000615
to follow AUV at tk+1A state quantity at a time;
Figure GDA00029656688900000616
for the calculation at t obtained by the third stepkThe time follows the east and north velocity estimates of the AUV,
Figure GDA0002965668890000071
to follow AUV relative navigation AUV-1 at tkAn east velocity difference estimate of the time;
Figure GDA0002965668890000072
to follow AUV relative navigation AUV-1 at tkA north velocity difference estimate of the time;
Figure GDA0002965668890000073
is tk+1A measurement vector of a moment;
Figure GDA0002965668890000074
AUV at t for pilotingk+1A position state quantity of a time;
Figure GDA0002965668890000075
in order to be a vector of the process noise,
Figure GDA0002965668890000076
to measure the noise vector, and
Figure GDA0002965668890000077
are both Gauss white noise; function of state
Figure GDA0002965668890000078
Measurement function
Figure GDA0002965668890000079
The collaborative navigation state space model established according to the step (4) estimates the following AUV position information through CKF, and the method comprises the following steps:
and (3) time updating:
state vector
Figure GDA00029656688900000710
At tkThe posterior probability of the time system is
Figure GDA00029656688900000711
A posterior probability density function of
Figure GDA00029656688900000712
Covariance of state error by Cholesky
Figure GDA00029656688900000713
The decomposition is in the form:
Figure GDA00029656688900000714
wherein the content of the first and second substances,
Figure GDA00029656688900000715
chol {. denotes performing Cholesky decomposition on the matrix,
Figure GDA00029656688900000716
is composed of
Figure GDA00029656688900000717
Transposing;
the Cubature point is calculated as follows:
Figure GDA00029656688900000718
wherein n is*Is the equation of state dimension and defines the following variables:
Figure GDA00029656688900000719
wherein m is*＝2n*The number of the volume points; n is*2 is the dimension of the state equation;
Figure GDA00029656688900000723
is the generated volume point;
Figure GDA00029656688900000720
the ith volume point in the point set is as follows:
Figure GDA00029656688900000721
the weight value occupied by each volume point;
the Cubature point is transferred by a transition matrix function of the system state:
Figure GDA00029656688900000722
tk+1the state predicted value at the moment is obtained by weighted summation:
Figure GDA0002965668890000081
calculating tk+1State error covariance prediction at time:
Figure GDA0002965668890000082
wherein the content of the first and second substances,
Figure GDA0002965668890000083
to transfer the Cubature point through the transition matrix function of the system state,
Figure GDA0002965668890000084
is composed of
Figure GDA0002965668890000085
Transposing;
Figure GDA0002965668890000086
is tk+1The predicted value of the state at the moment,
Figure GDA0002965668890000087
is composed of
Figure GDA0002965668890000088
Transposing; system noise covariance matrix
Figure GDA0002965668890000089
Figure GDA00029656688900000810
Is white gaussian noise, and is a noise,
Figure GDA00029656688900000811
is composed of
Figure GDA00029656688900000812
Transposition is carried out;
measurement updating:
the state error covariance predictor is decomposed by Cholesky into the following form:
Figure GDA00029656688900000813
wherein the content of the first and second substances,
Figure GDA00029656688900000814
is composed of
Figure GDA00029656688900000815
Transposing;
the Cubature point is calculated as follows:
Figure GDA00029656688900000816
the Cubature point is passed through the system's measurement function:
Figure GDA00029656688900000817
tk+1the observation predicted value of the moment is obtained by weighted summation:
Figure GDA00029656688900000818
calculating tk+1The measurement error covariance predicted value at the moment is as follows:
Figure GDA00029656688900000819
wherein the content of the first and second substances,
Figure GDA00029656688900000820
the Cubature point is passed for the system's measurement function,
Figure GDA00029656688900000821
is composed of
Figure GDA00029656688900000822
Transposing;
Figure GDA00029656688900000823
is tk+1The observed and predicted value of the time is,
Figure GDA00029656688900000824
is composed of
Figure GDA00029656688900000825
Transposing; covariance matrix for measuring noise
Figure GDA00029656688900000826
Is white gaussian noise, and is a noise,
Figure GDA00029656688900000827
is composed of
Figure GDA00029656688900000828
Transposition is carried out;
cross covariance matrix:
Figure GDA0002965668890000091
and (3) estimating Kalman filtering gain:
Figure GDA0002965668890000092
status update procedure, obtaining tk+1State estimation at time:
Figure GDA0002965668890000093
obtaining t using a covariance update processk+1The state estimation error covariance matrix at the time instant, i.e.:
Figure GDA0002965668890000094
wherein the content of the first and second substances,
Figure GDA0002965668890000095
is composed of
Figure GDA0002965668890000096
The transposing of (1).
The invention has the beneficial effects that:
1. the position, the speed and the course information of the following AUV are obtained by calculation only based on the relative measurement distance between the following AUV and the piloting AUV and the self position and speed information broadcasted by the piloting AUV, so that a large amount of inertial navigation equipment and DVL (dynamic Voltage Link) are saved, and the multi-AUV collaborative navigation cost is reduced;
2. the AUV is followed without an inertial navigation device and a DVL, so that the complexity of AUV system configuration is reduced, the internal space of the AUV is saved, and the weight is reduced;
3. the AUV relative motion state space model is combined with the multi-AUV collaborative navigation state space model in the double-pilot mode, so that the collaborative positioning performance of the multi-AUV collaborative navigation system is guaranteed.
Drawings
FIG. 1 is a flow chart of the present invention;
fig. 2 is a schematic diagram of underwater acoustic communication based on a dual-pilot AUV mode;
FIG. 3 is a diagram of the true sailing trajectory of the following AUV, the piloting AUV-1 and the piloting AUV-2;
FIG. 4 is a following AUV forward velocity estimation;
FIG. 5 is a following AUV course angle estimation;
FIG. 6 is an estimated trajectory followed by an AUV based on a dual motion model;
fig. 7 is an estimated positioning error following the AUV based on a dual motion model.
Detailed Description
The invention is further described below with reference to the accompanying drawings.
The collaborative navigation is one of the most effective navigation methods of the multi-autonomous underwater vehicle in the middle layer area at present, and has wide application prospect. In general, multi-AUV collaborative navigation positioning has two forms: 1) the navigation system is parallel, namely, each aircraft in the system has the same function and structure, the navigation system is used for positioning, and the position information of other aircraft is obtained through underwater acoustic communication; 2) and the slave mode is also called a pilot mode, namely a small number of pilot AUVs are provided with high-precision navigation equipment in the system, a large number of following AUVs are provided with low-precision navigation equipment, the following AUVs improve the self-navigation precision by obtaining the position relation with the pilot AUVs, and the position of the self in the system is determined through underwater acoustic communication. The parallel mode has a simple structure, but each AUV is provided with high-precision navigation equipment, the cost is increased by many times, and the master-slave mode gives consideration to the navigation precision and the cost, so that the parallel mode becomes the main direction of multi-AUV collaborative navigation research. In the master-slave type collaborative navigation system, the piloting AUV is provided with high-precision inertial navigation equipment, a Doppler Velocity Log (DVL), a Differential Global Positioning System (DGPS), underwater acoustic communication equipment and the like, the navigation system takes the high-precision inertial navigation equipment as a main part, the initial position is obtained through the DGPS, the absolute speed measured by the DVL is taken as the external input of inertial navigation, the collaborative positioning precision is further improved, and the AUV is followed with low-precision inertial navigation equipment, the DVL, the underwater acoustic communication equipment and the like. Based on a sensing network built by high-precision and low-precision navigation equipment, the navigation and positioning performance of the whole formation is improved through relative measurement and information sharing among AUVs. However, under the condition of a large number of following AUVs, even if the following AUVs are equipped with low-precision dead reckoning navigation devices, the required inertial navigation devices and DVLs are still cost-prohibitive, so in practical engineering applications, if the following AUVs are not equipped with inertial navigation devices and DVLs, the positioning precision of the collaborative navigation system can be guaranteed to be within an allowable range, and the method has great research value.
The purpose of the invention is realized as follows:
the method comprises the following steps: the piloting AUV and the following AUV carry out underwater acoustic ranging, and simultaneously, the piloting AUV broadcasts and sends self position and speed information to the following AUV;
step two: establishing a relative motion state space model between the following AUV and the piloting AUV;
step three: estimating a velocity component difference value of the following AUV and the pilot AUV through CKF according to the relative motion state space model established in the step two;
step four: establishing a multi-AUV collaborative navigation state space model in a dual-pilot mode;
step five: and estimating the following AUV position information through CKF according to the collaborative navigation state space model established in the step four.
In a master-slave mode multi-AUV collaborative navigation system, low-precision inertial navigation equipment, DVL (dynamic Voltage Link), underwater acoustic communication equipment and the like are arranged along with an AUV. However, in the case of a large number of AUVs, many inertial navigation devices and DVLs are required, the system configuration is complicated, and the cost is increased accordingly.
Aiming at the problems, the method aims at reducing the cost of the multi-AUV collaborative navigation system, and designs a master-slave multi-AUV collaborative navigation method based on a double-motion model on the basis of the traditional multi-AUV collaborative navigation method.
The method comprises the following steps: the piloting AUV and the following AUV carry out underwater acoustic ranging, and simultaneously, the piloting AUV broadcasts and sends self position and speed information to the following AUV
And in the dual-navigation mode, the navigation AUV-1 and the navigation AUV-2 send underwater acoustic signals to the following AUV, and the distances between the following AUV and the navigation AUV-1 and the navigation AUV-2 can be respectively obtained according to the time from the transmission of the underwater acoustic signals from the navigation AUV to the following AUV and the transmission speed of underwater acoustic communication. Meanwhile, the piloting AUV broadcasts and sends the position and speed information of the piloting AUV to the following AUV.
Step two: establishing a relative motion state space model between the following AUV and the piloting AUV
(1) Equation of state
In an actual underwater multi-AUV collaborative navigation system, the depth and the horizontal position of an AUV are mutually independent, and accurate depth information can be obtained through a pressure sensor, so that the three-dimensional collaborative navigation problem can be simplified into two dimensions, and a model is projected to a two-dimensional horizontal plane for analysis in discussion. Definition following AUV at tkTime t andk+1the position vectors of the time are respectively
Figure GDA0002965668890000111
And
Figure GDA0002965668890000112
piloting AUV-1 at tkTime t andk+1the position vectors of the time are respectively
Figure GDA0002965668890000113
And
Figure GDA0002965668890000114
piloting AUV-2 at tkTime t andk+1the position vectors of the time are respectively
Figure GDA0002965668890000115
And
Figure GDA0002965668890000116
wherein the content of the first and second substances,
Figure GDA0002965668890000117
at t is respectively following AUV, piloting AUV-1 and piloting AUV-2kEast position of time;
Figure GDA0002965668890000118
at t is respectively following AUV, piloting AUV-1 and piloting AUV-2kThe north position of the time;
Figure GDA0002965668890000119
at t is respectively following AUV, piloting AUV-1 and piloting AUV-2k+1East position of time;
Figure GDA00029656688900001110
at t is respectively following AUV, piloting AUV-1 and piloting AUV-2k+1The north position of the moment.
Definition following AUV at tkThe state vector at the moment of time is
Figure GDA00029656688900001111
Respectively following AUV at tkAn east and north velocity component of the time; piloting AUV-1 at tkThe state vector at the moment of time is
Figure GDA00029656688900001112
Navigation AUV-1 at tkThe east direction velocity component and the north direction velocity component of the time are expressed in the following specific forms:
Figure GDA00029656688900001113
in the formula (I), the compound is shown in the specification,
Figure GDA00029656688900001114
at t for the piloting AUV-1 provided by the DVL, respectivelykStarboard and forward speeds at time;
Figure GDA00029656688900001115
piloting AUV-1 provided by high precision inertial navigation equipment at tkThe heading angle at the moment.
The position state equation following the AUV is:
Figure GDA00029656688900001116
where δ t is the sampling time interval.
The position state equation of the piloting AUV-1 is as follows:
Figure GDA0002965668890000121
taking following AUV and pilot AUV-1 at tkThe relative motion state vector at a time is:
Figure GDA0002965668890000122
in the formula (I), the compound is shown in the specification,
Figure GDA0002965668890000123
to follow AUV relative navigation AUV-1 at tkEast distance of time;
Figure GDA0002965668890000124
to follow AUV relative navigation AUV-1 at tkThe north distance of the moment;
Figure GDA0002965668890000125
to follow AUV relative navigation AUV-1 at tkEast speed difference at time;
Figure GDA0002965668890000126
to follow AUV relative navigation AUV-1 at tkThe north velocity difference at that moment;
then, defined by the above vectors, the discrete state equations of the system can be described as:
Xk+1＝Fk+1|kXk
in the formula, Xk＝(Δxk,Δyk,Δvx,k,Δvy,k)TTo follow between AUV and piloting AUV-1 at tkRelative motion state vectors at time; xk+1To follow between AUV and piloting AUV-1 at tk+1Relative motion state vectors at time; fk+1|kIs tkTime tk+1The state transition matrix of the time of day,
Figure GDA0002965668890000127
(2) equation of measurement
Following AUV at tkRelative observation information obtained by underwater acoustic communication at the moment is pilot AUV-1 and pilot AUV-2 at tkLocation information of time of day
Figure GDA0002965668890000128
And
Figure GDA0002965668890000129
between piloting AUV-1 and AUV-2 and following AUV at tkTime of dayRelative distance measurement information of
Figure GDA00029656688900001210
And piloting AUV-1 at tkEast velocity component of time of day
Figure GDA00029656688900001211
And a north velocity component
Figure GDA00029656688900001212
The coordinate position relation between the following AUV and the piloting AUV-1 can be obtained by the information as follows:
Figure GDA00029656688900001213
based on the above formula, a discrete time state space model (state equation and measurement equation) of the relative motion of the following AUV and the pilot AUV-1 is established as follows:
Figure GDA0002965668890000131
wherein k +1 represents tk+1Time of day; measurement function
Figure GDA0002965668890000132
Δxk+1To follow AUV relative navigation AUV-1 at tk+1East distance of time; Δ yk+1To follow AUV relative navigation AUV-1 at tk+1The north distance of the moment; w is akIs a process noise vector, vk+1To measure the noise vector, and wk、vk+1Are all gaussian white noise.
Step three: estimating the velocity component difference value of the following AUV and the pilot AUV-1 through CKF according to the relative motion state space model established in the step two
The CKF filtering algorithm adopts a group of volume points with equal weight according to a Spherical-Radial rule, calculates the mean value and covariance of the nonlinear transformed random variables by using a statistical numerical integration principle, and can accurately obtain state update and state covariance matrix update.
(1) Time updating
Hypothesis state vector Xk＝(Δxk,Δyk,Δvx,k,Δvy,k)TAt tkPosterior probability state of time system
Figure GDA0002965668890000133
And a posterior probability density function
Figure GDA0002965668890000134
It is known to covariance the state error P by Choleskyk|kThe decomposition is in the form:
Figure GDA0002965668890000135
in the formula, Sk|k＝chol{Pk|kDenotes Cholesky decomposition of the matrix,
Figure GDA0002965668890000136
is Sk|kThe transposing of (1).
Calculate Cubature point (i ═ 1,2 …, m, m ═ 2 n):
Figure GDA0002965668890000137
where n is the equation of state dimension and defines the following variables:
Figure GDA0002965668890000138
wherein m is 2n and is the number of volume points; n is 4 as the dimension of the state equation; xiiIs the generated volume point; [1]iThe ith volume point in the point set is as follows:
Figure GDA0002965668890000139
withe weight occupied by each volume point.
The Cubature point is transferred by a transition matrix function of the system state:
Xi,k+1|k＝Fk+1|kXi,k|k
tk+1the state predicted value at the moment is obtained by weighted summation:
Figure GDA0002965668890000141
calculating tk+1State error covariance prediction at time:
Figure GDA0002965668890000142
in the formula, Xi,k+1|kTo transfer the Cubature point through the transition matrix function of the system state,
Figure GDA0002965668890000143
is Xi,k+1|kTransposing;
Figure GDA0002965668890000144
is tk+1The predicted value of the state at the moment,
Figure GDA0002965668890000145
is composed of
Figure GDA0002965668890000146
Transposing; system noise covariance matrix
Figure GDA0002965668890000147
wkIs Gaussian white noise, wk TIs wkAnd (4) transposition.
(2) Measurement update
The state error covariance predictor is decomposed by Cholesky into the following form:
Figure GDA0002965668890000148
in the formula, Sk+1|k＝chol{Pk+1|k}，
Figure GDA0002965668890000149
Is Sk+1|kThe transposing of (1).
Calculate Cubature point (i ═ 1,2 …, m, m ═ 2 n):
Figure GDA00029656688900001410
the Cubature point is passed through the system's measurement function:
Zi,k+1|k＝h(Xi,k+1|k,k+1)
tk+1the observation predicted value of the moment is obtained by weighted summation:
Figure GDA00029656688900001411
calculating tk+1The measurement error covariance predicted value at the moment is as follows:
Figure GDA00029656688900001412
in the formula, Zi,k+1|kThe Cubature point is passed for the system's measurement function,
Figure GDA00029656688900001413
is Zi,k+1|kTransposing;
Figure GDA00029656688900001414
is tk+1The observed and predicted value of the time is,
Figure GDA0002965668890000151
is composed of
Figure GDA0002965668890000152
Transposing; covariance matrix for measuring noise
Figure GDA0002965668890000153
vk+1Is white gaussian noise, and is a noise,
Figure GDA0002965668890000154
is v isk+1And (4) transposition.
Cross covariance matrix:
Figure GDA0002965668890000155
and (3) estimating Kalman filtering gain:
Figure GDA0002965668890000156
status update procedure, obtaining tk+1State estimation at time:
Figure GDA0002965668890000157
obtaining t using a covariance update processk+1The state estimation error covariance matrix at the time instant, i.e.:
Figure GDA0002965668890000158
in the formula (I), the compound is shown in the specification,
Figure GDA0002965668890000159
is Kk+1The transposing of (1).
The process is carried out by successive recursion until the covariance of the state estimation error reaches a stable value, namely the relative state estimation of the following AUV and the pilot AUV-1 is obtained, and the process is carried out according to tk+1Relative state estimation of time of day
Figure GDA00029656688900001510
Can obtain the following AUV at tk+1The east velocity estimation value and the north velocity estimation value at the moment are respectively
Figure GDA00029656688900001511
And
Figure GDA00029656688900001512
following AUV at tk+1Estimated course angle at the moment of time of
Figure GDA00029656688900001513
Figure GDA00029656688900001514
Navigation AUV-1 at tk+1An east and north velocity component of the time;
Figure GDA00029656688900001515
to follow AUV relative navigation AUV-1 at tk+1An east distance estimate of the time;
Figure GDA00029656688900001516
to follow AUV relative navigation AUV-1 at tk+1A north distance estimate of the time;
Figure GDA00029656688900001517
to follow AUV relative navigation AUV-1 at tk+1An east velocity difference estimate of the time;
Figure GDA00029656688900001518
to follow AUV relative navigation AUV-1 at tk+1The north velocity difference estimate at time.
Step four: establishing a multi-AUV collaborative navigation state space model in a dual-pilot mode;
following AUV defined by step two at tkTime t andk+1the position vectors of the time are respectively
Figure GDA00029656688900001519
And
Figure GDA00029656688900001524
piloting AUV-1 at tkTime t andk+1the position vectors of the time are respectively
Figure GDA00029656688900001520
And
Figure GDA00029656688900001521
piloting AUV-2 at tkTime t andk+1the position vectors of the time are respectively
Figure GDA00029656688900001522
And
Figure GDA00029656688900001523
between piloting AUV-1 and AUV-2 and following AUV at tkThe relative measurement distances at the time are respectively
Figure GDA0002965668890000161
And estimating the east speed and the north speed of the following AUV through filtering in the third step, and obtaining the collaborative navigation system state equation based on the relative position measurement as follows:
Figure GDA0002965668890000162
the coordinate position relations among the piloting AUV-1, the piloting AUV-2 and the following AUV are as follows:
Figure GDA0002965668890000163
based on the equations (8) and (9), a discrete time state space model (a state equation and a measurement equation) of the multi-AUV cooperative positioning system is established as follows:
Figure GDA0002965668890000164
wherein k +1 represents tk+1Time of day;
Figure GDA0002965668890000165
to follow AUV at tkA position state quantity of a time;
Figure GDA0002965668890000166
to follow AUV at tk+1A state quantity at a time;
Figure GDA0002965668890000167
for the calculation at t obtained by the third stepkThe time follows the east and north velocity estimates of the AUV,
Figure GDA0002965668890000168
to follow AUV relative navigation AUV-1 at tkAn east velocity difference estimate of the time;
Figure GDA0002965668890000169
to follow AUV relative navigation AUV-1 at tkA north velocity difference estimate of the time;
Figure GDA00029656688900001610
is tk+1A measurement vector of a moment;
Figure GDA00029656688900001611
AUV at t for pilotingk+1A position state quantity of a time;
Figure GDA00029656688900001612
in order to be a vector of the process noise,
Figure GDA00029656688900001613
to measure the noise vector, and
Figure GDA00029656688900001614
are both Gauss white noise; function of state
Figure GDA00029656688900001615
Measurement function
Figure GDA00029656688900001616
Step five: estimating the following AUV position information through CKF according to the collaborative navigation state space model established in the step three
(1) Time updating
Hypothesis state vector
Figure GDA00029656688900001617
At tkPosterior probability state of time system
Figure GDA00029656688900001618
And a posterior probability density function
Figure GDA00029656688900001619
It is known to covariance the state error by Cholesky
Figure GDA00029656688900001620
The decomposition is in the form:
Figure GDA00029656688900001621
in the formula (I), the compound is shown in the specification,
Figure GDA0002965668890000171
chol {. denotes performing Cholesky decomposition on the matrix,
Figure GDA0002965668890000172
is composed of
Figure GDA0002965668890000173
The transposing of (1).
Calculate the cubage point (i ═ 1,2 …, m*，m*＝2n*)：
Figure GDA0002965668890000174
Wherein n is*Is the equation of state dimension and defines the following variables:
Figure GDA0002965668890000175
in the formula, m*＝2n*The number of the volume points; n is*2 is the dimension of the state equation;
Figure GDA00029656688900001724
is the generated volume point;
Figure GDA0002965668890000176
the ith volume point in the point set is as follows:
Figure GDA0002965668890000177
the weight occupied by each volume point.
The Cubature point is transferred by a transition matrix function of the system state:
Figure GDA0002965668890000178
tk+1the state predicted value at the moment is obtained by weighted summation:
Figure GDA0002965668890000179
calculating tk+1State error covariance prediction at time:
Figure GDA00029656688900001710
in the formula (I), the compound is shown in the specification,
Figure GDA00029656688900001711
to transfer the Cubature point through the transition matrix function of the system state,
Figure GDA00029656688900001712
is composed of
Figure GDA00029656688900001713
Transposing;
Figure GDA00029656688900001714
is tk+1The predicted value of the state at the moment,
Figure GDA00029656688900001715
is composed of
Figure GDA00029656688900001716
Transposing; system noise covariance matrix
Figure GDA00029656688900001717
Figure GDA00029656688900001718
Is white gaussian noise, and is a noise,
Figure GDA00029656688900001719
is composed of
Figure GDA00029656688900001720
And (4) transposition.
(2) Measurement update
The state error covariance predictor is decomposed by Cholesky into the following form:
Figure GDA00029656688900001721
in the formula (I), the compound is shown in the specification,
Figure GDA00029656688900001722
is composed of
Figure GDA00029656688900001723
The transposing of (1).
Calculate Cubature point (i ═ 1,2 …, m, m ═ 2 n):
Figure GDA0002965668890000181
the Cubature point is passed through the system's measurement function:
Figure GDA0002965668890000182
tk+1the observation predicted value of the moment is obtained by weighted summation:
Figure GDA0002965668890000183
calculating tk+1The measurement error covariance predicted value at the moment is as follows:
Figure GDA0002965668890000184
in the formula (I), the compound is shown in the specification,
Figure GDA0002965668890000185
the Cubature point is passed for the system's measurement function,
Figure GDA0002965668890000186
is composed of
Figure GDA0002965668890000187
Transposing;
Figure GDA0002965668890000188
is tk+1The observed and predicted value of the time is,
Figure GDA0002965668890000189
is composed of
Figure GDA00029656688900001810
Transposing; covariance matrix for measuring noise
Figure GDA00029656688900001811
Is white gaussian noise, and is a noise,
Figure GDA00029656688900001812
is composed of
Figure GDA00029656688900001813
And (4) transposition.
Cross covariance matrix:
Figure GDA00029656688900001814
and (3) estimating Kalman filtering gain:
Figure GDA00029656688900001815
status update procedure, obtaining tk+1State estimation at time:
Figure GDA00029656688900001816
obtaining t using a covariance update processk+1The state estimation error covariance matrix at the time instant, i.e.:
Figure GDA00029656688900001817
in the formula (I), the compound is shown in the specification,
Figure GDA00029656688900001818
is composed of
Figure GDA00029656688900001819
The transposing of (1).
The process is carried out by recursion successively until the covariance of the state estimation error reaches a stable value, and the estimation of the system state is obtained, so that the following AUV is positioned.
In order to further explain the beneficial effects of the invention, the master-slave mode multi-AUV collaborative navigation scheme based on the dual-motion model is subjected to simulation verification:
the simulation time is 3600 s; the sampling period is 1 s;
(1) the initial position coordinate following the AUV is (0m, 1000m), the forward speed is 10kn, and the constant speed straight navigation is carried out along the direction with a course angle of 150 degrees;
(2) the initial position coordinates of the piloting AUV-1 are (0m, 0m), the forward speed is 8kn, the initial course angle is 30 degrees, the front 1200s performs rotary motion with the angular rate of 0.6 degrees/s, and the rear 1200s performs straight navigation at a constant speed;
(3) the initial position coordinate of the piloting AUV-2 is (0m, 1000m), the forward speed is 8kn, and the constant speed direct navigation is carried out along the direction with a course angle of 90 degrees;
measurement noises of underwater acoustic ranging between the pilot AUV-1, pilot AUV-2 and following AUV are also introduced in the simulation, and the variance is 10m2White Gaussian noise with a mean of zero, and east and north position errors of the two piloted AUVs, both with a variance of 5m2White gaussian noise with a mean value of zero. Assuming that the initial position coordinates of the following AUV, the pilot AUV-1 and the pilot AUV-2 are known, the initial state of the space model of the relative motion state is Xk＝(0-0,1000-0,0-8·sin(30°),0-8·cos(30°))TThe initial state of the multi-AUV collaborative navigation state space model in the double-pilot mode is
Figure GDA0002965668890000191
Simulation experiments were performed.
1. The position, the speed and the course information of the following AUV are obtained by calculation only based on the relative measurement distance between the following AUV and the piloting AUV and the self position and speed information broadcasted by the piloting AUV, so that a large amount of inertial navigation equipment and DVL (dynamic Voltage Link) are saved, and the multi-AUV collaborative navigation cost is reduced;
2. the AUV is followed without an inertial navigation device and a DVL, so that the complexity of AUV system configuration is reduced, the internal space of the AUV is saved, and the weight is reduced;
3. the AUV relative motion state space model is combined with the multi-AUV collaborative navigation state space model in the double-pilot mode, so that the collaborative positioning performance of the multi-AUV collaborative navigation system is guaranteed.

Claims (1)

1. A master-slave mode multi-AUV collaborative navigation method based on a double-motion model is characterized by comprising the following steps:
step 1: in the dual-navigation mode, the navigation AUV-1 and the navigation AUV-2 send underwater acoustic signals to the following AUV and broadcast and send self position and speed information to the following AUV; the distances between the following AUV and the navigation AUV-1 and the navigation AUV-2 can be respectively obtained according to the transmission time and the speed of the underwater sound signal;
step 2: establishing a relative motion state space model following the AUV and the piloting AUV-1;
Figure FDA0002965668880000011
Figure FDA0002965668880000012
Figure FDA0002965668880000013
wherein, Xk＝(Δxk,Δyk,Δvx,k,Δvy,k)TTo follow between AUV and piloting AUV-1 at tkRelative motion state vectors at time; fk+1|kIs tkTime tk+1The state transition matrix of the time of day,
Figure FDA0002965668880000014
Figure FDA0002965668880000015
to follow AUV at tkA state vector of a time;
Figure FDA0002965668880000016
to follow AUV at tkEast position of time;
Figure FDA0002965668880000017
to follow AUV at tkThe north position of the time;
Figure FDA0002965668880000018
to follow AUV at tkAn east velocity component of time;
Figure FDA0002965668880000019
to follow AUV at tkA north velocity component of time; δ t is the sampling time interval; piloting AUV-1 at tkThe state vector at the moment of time is
Figure FDA00029656688800000110
Figure FDA00029656688800000111
AUV-1 at t for pilotingkEast position of time;
Figure FDA00029656688800000112
AUV-1 at t for pilotingkThe north position of the time;
Figure FDA00029656688800000113
and
Figure FDA00029656688800000114
respectively, piloting AUV-1 at tkThe east and north velocity components of the time of day,
Figure FDA00029656688800000115
Figure FDA00029656688800000116
and
Figure FDA00029656688800000117
at t for the piloting AUV-1 provided by the DVL, respectivelykStarboard and forward speeds at time;
Figure FDA00029656688800000118
for piloting AUV-1 provided by inertial navigation equipment at tkA course angle at a moment; w is akIs a process noise vector, vk+1To measure the noise vector, and wk、vk+1Are all Gaussian white noise;
and step 3: obtaining following AUV and pilot AUV-1 t by CKF filtering algorithmk+1Relative state estimation of time of day
Figure FDA00029656688800000119
Wherein the content of the first and second substances,
Figure FDA00029656688800000120
to follow AUV relative navigation AUV-1 at tk+1An east distance estimate of the time;
Figure FDA00029656688800000121
to follow AUV relative navigation AUV-1 at tk+1A north distance estimate of the time;
Figure FDA0002965668880000021
to follow AUV relative navigation AUV-1 at tk+1An east velocity difference estimate of the time;
Figure FDA0002965668880000022
to follow AUV relative navigation AUV-1 at tk+1A north velocity difference estimate of the time;
step 3.1: state vector Xk＝(Δxk,Δyk,Δvx,k,Δvy,k)TAt tkPosterior probability state of time system
Figure FDA0002965668880000023
And a posterior probability density function
Figure FDA0002965668880000024
It is known to covariance the state error P by Choleskyk|kThe decomposition is in the form:
Figure FDA0002965668880000025
wherein S isk|k＝chol{Pk|kIndicates Cholesky decomposition of the matrix;
Figure FDA0002965668880000026
is Sk|kTransposing;
step 3.2: calculate the Cubature point:
Figure FDA0002965668880000027
Figure FDA0002965668880000028
wherein n is a dimension of a state equation; m is 2n and is the number of the volume points; xiiIs the generated volume point; [1]iThe ith volume point in the point set is as follows:
Figure FDA0002965668880000029
withe weight value occupied by each volume point;
step 3.3: transferring a Cubasic point through a transfer matrix function of a system state;
Xi,k+1|k＝Fk+1|kXi,k|k
step 3.4: calculating t by weighted summationk+1A state prediction value of a moment;
Figure FDA00029656688800000210
step 3.5: calculating tk+1Predicting the state error covariance at the moment;
Figure FDA00029656688800000211
wherein the content of the first and second substances,
Figure FDA00029656688800000212
is Xi,k+1|kTransposing;
Figure FDA00029656688800000213
is composed of
Figure FDA00029656688800000214
Transposing; qk＝E[wkwk T]，wk TIs wkTransposition is carried out;
step 3.6: the state error covariance predictor is decomposed by Cholesky into the following form:
Figure FDA0002965668880000031
wherein S isk+1|k＝chol{Pk+1|k}，
Figure FDA0002965668880000032
Is Sk+1|kTransposing;
step 3.7: calculate the Cubature point:
Figure FDA0002965668880000033
step 3.8: the Cubature point is passed through the system's measurement function:
Zi,k+1|k＝h(Xi,k+1|k,k+1)
step 3.9: calculating t by weighted summationk+1Observing and predicting values at the moment;
Figure FDA0002965668880000034
step 3.10: calculating tk+1Measuring error covariance predicted value of the moment;
Figure FDA0002965668880000035
wherein the content of the first and second substances,
Figure FDA0002965668880000036
is Zi,k+1|kTransposing;
Figure FDA0002965668880000037
is composed of
Figure FDA0002965668880000038
Transposing;
Figure FDA0002965668880000039
Figure FDA00029656688800000310
is v isk+1Transposition is carried out;
step 3.11: calculating a cross covariance matrix;
Figure FDA00029656688800000311
step 3.12: estimating a Kalman filtering gain;
Figure FDA00029656688800000312
step 3.13: calculating following AUV and piloting AUV-1 tk+1Estimating relative state of time;
Figure FDA00029656688800000313
step 3.14: calculating tk+1A state estimation error covariance matrix at the moment;
Figure FDA00029656688800000314
wherein the content of the first and second substances,
Figure FDA00029656688800000315
is Kk+1Transposing;
and 4, step 4: establishing a multi-AUV collaborative navigation state space model in a dual-pilot mode;
Figure FDA00029656688800000316
Figure FDA0002965668880000041
Figure FDA0002965668880000042
wherein the content of the first and second substances,
Figure FDA0002965668880000043
to follow AUV at tkA position state vector of a time; (ii) a
Figure FDA0002965668880000044
Is at tkEast and north velocity estimates of the AUV are followed at time;
Figure FDA0002965668880000045
in order to be a vector of the process noise,
Figure FDA0002965668880000046
to measure the noise vector, and
Figure FDA0002965668880000047
Figure FDA0002965668880000048
are both Gauss white noise;
Figure FDA0002965668880000049
at t is respectively following AUV, piloting AUV-1 and piloting AUV-2k+1East position of time;
Figure FDA00029656688800000410
at t is respectively following AUV, piloting AUV-1 and piloting AUV-2k+1The north position of the time;
and 5: obtaining t by CKF filtering algorithmk+1Position state estimation information that follows AUV at time
Figure FDA00029656688800000411
Step 5.1: state vector
Figure FDA00029656688800000412
At tkPosterior probability state of time system
Figure FDA00029656688800000413
And a posterior probability density function
Figure FDA00029656688800000414
It is known to covariance the state error by Cholesky
Figure FDA00029656688800000415
The decomposition is in the form:
Figure FDA00029656688800000416
wherein the content of the first and second substances,
Figure FDA00029656688800000417
chol {. is } represents performing Cholesky decomposition on the matrix;
Figure FDA00029656688800000418
is composed of
Figure FDA00029656688800000419
Transposing;
step 5.2: calculating a Cufoundation point;
Figure FDA00029656688800000420
Figure FDA00029656688800000421
wherein n is*Is the dimension of the state equation; m is*＝2n*The number of the volume points;
Figure FDA00029656688800000422
is the generated volume point;
Figure FDA00029656688800000423
the ith volume point in the point set is as follows:
Figure FDA00029656688800000424
Figure FDA00029656688800000425
the weight value occupied by each volume point;
step 5.3: transferring a Cubasic point through a transfer matrix function of a system state;
Figure FDA00029656688800000426
step 5.4: calculating t by weighted summationk+1A state prediction value of a moment;
Figure FDA0002965668880000051
step 5.5: calculating tk+1Predicting the state error covariance at the moment;
Figure FDA0002965668880000052
wherein the content of the first and second substances,
Figure FDA0002965668880000053
is composed of
Figure FDA0002965668880000054
Transposing;
Figure FDA0002965668880000055
is composed of
Figure FDA0002965668880000056
Transposing;
Figure FDA0002965668880000057
Figure FDA0002965668880000058
is composed of
Figure FDA0002965668880000059
Transposition is carried out;
step 5.6: the state error covariance predictor is decomposed by Cholesky into the following form:
Figure FDA00029656688800000510
wherein the content of the first and second substances,
Figure FDA00029656688800000511
Figure FDA00029656688800000512
is composed of
Figure FDA00029656688800000513
Transposing;
step 5.7: calculating a Cufoundation point;
Figure FDA00029656688800000514
step 5.8: transferring the Cubasic point through a measurement function of the system;
Figure FDA00029656688800000515
step 5.9: calculating t by weighted summationk+1Observing and predicting values at the moment;
Figure FDA00029656688800000516
step 5.10: calculating tk+1The measurement error covariance predicted value at the moment is as follows:
Figure FDA00029656688800000517
wherein the content of the first and second substances,
Figure FDA00029656688800000518
is composed of
Figure FDA00029656688800000519
Transposing;
Figure FDA00029656688800000520
is composed of
Figure FDA00029656688800000521
Transposing;
Figure FDA00029656688800000522
Figure FDA00029656688800000523
is composed of
Figure FDA00029656688800000524
Transposition is carried out;
step 5.11: calculating a cross covariance matrix;
Figure FDA00029656688800000525
step 5.12: estimating a Kalman filtering gain;
Figure FDA00029656688800000526
step 5.13: calculating tk+1Position state estimation information that follows AUV at time
Figure FDA00029656688800000527
Figure FDA0002965668880000061
CN201910177062.2A 2019-03-08 2019-03-08 Master-slave mode multi-AUV collaborative navigation method based on double motion model Active CN109974706B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910177062.2A CN109974706B (en) 2019-03-08 2019-03-08 Master-slave mode multi-AUV collaborative navigation method based on double motion model

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910177062.2A CN109974706B (en) 2019-03-08 2019-03-08 Master-slave mode multi-AUV collaborative navigation method based on double motion model

Publications (2)

Publication Number Publication Date
CN109974706A CN109974706A (en) 2019-07-05
CN109974706B true CN109974706B (en) 2021-05-11

Family

ID=67078338

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910177062.2A Active CN109974706B (en) 2019-03-08 2019-03-08 Master-slave mode multi-AUV collaborative navigation method based on double motion model

Country Status (1)

Country Link
CN (1) CN109974706B (en)

Families Citing this family (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111307136B (en) * 2020-02-12 2021-08-27 哈尔滨工程大学 Underwater navigation terrain matching navigation method for double intelligent underwater robots
CN111928851B (en) * 2020-04-17 2023-12-12 中国科学院沈阳自动化研究所 TMA technology-based multi-autonomous underwater robot cluster collaborative navigation method
CN111595348B (en) * 2020-06-23 2022-12-23 南京信息工程大学 Master-slave mode cooperative positioning method of autonomous underwater vehicle combined navigation system
CN112697145A (en) * 2020-11-17 2021-04-23 武汉第二船舶设计研究所(中国船舶重工集团公司第七一九研究所) Master-slave type multi-underwater unmanned submersible vehicle cooperative positioning method based on CKF
CN112818819B (en) * 2021-01-28 2022-08-26 青岛澎湃海洋探索技术有限公司 AUV state monitoring method based on dynamic model and complex network theory
CN112985391B (en) * 2021-04-19 2021-08-10 中国人民解放军国防科技大学 Multi-unmanned aerial vehicle collaborative navigation method and device based on inertia and binocular vision
CN113324547A (en) * 2021-05-25 2021-08-31 江苏科技大学 Multi-AUV (autonomous Underwater vehicle) cooperative positioning method based on iterative extended RTS (smooth Transmission) smoothing filter algorithm
CN114136323B (en) * 2021-11-29 2023-06-20 中国船舶重工集团公司第七0七研究所 AUV collaborative navigation method based on TDOA measurement under motion compensation
CN114370878B (en) * 2022-01-04 2023-10-27 江苏科技大学 Multi-AUV (autonomous Underwater vehicle) co-location method based on STACKF (space-time adaptive filter)
CN114995405A (en) * 2022-05-19 2022-09-02 同济大学 AGV cooperative handling method based on open dynamic environment multi-target cooperative theory
CN116680500B (en) * 2023-06-12 2024-03-22 哈尔滨工程大学 Position estimation method and system of underwater vehicle under non-Gaussian noise interference

Family Cites Families (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103398712B (en) * 2013-08-02 2016-12-28 中国人民解放军63983部队 The collaborative navigation method alternately navigated
CN103674029B (en) * 2013-12-19 2017-01-04 哈尔滨工程大学 A kind of method of many ships collaborative navigation formation configuration based on underwater sound communication
CN106441300B (en) * 2016-09-08 2019-10-18 哈尔滨工程大学 It is a kind of with adaptive collaborative navigation filtering method
CN107677272B (en) * 2017-09-08 2020-11-10 哈尔滨工程大学 AUV (autonomous Underwater vehicle) collaborative navigation method based on nonlinear information filtering
CN108827305A (en) * 2018-05-25 2018-11-16 哈尔滨工程大学 A kind of AUV collaborative navigation method based on robust information filtering
CN108489498A (en) * 2018-06-15 2018-09-04 哈尔滨工程大学 A kind of AUV collaborative navigation methods without mark particle filter based on maximum cross-correlation entropy

Also Published As

Publication number Publication date
CN109974706A (en) 2019-07-05

Similar Documents

Publication Publication Date Title
CN109974706B (en) Master-slave mode multi-AUV collaborative navigation method based on double motion model
CN109459040B (en) Multi-AUV (autonomous Underwater vehicle) cooperative positioning method based on RBF (radial basis function) neural network assisted volume Kalman filtering
CN110375730B (en) Indoor positioning navigation system based on IMU and UWB fusion
Mu et al. A practical INS/GPS/DVL/PS integrated navigation algorithm and its application on Autonomous Underwater Vehicle
Xiong et al. Fault-tolerant GNSS/SINS/DVL/CNS integrated navigation and positioning mechanism based on adaptive information sharing factors
WO2020062791A1 (en) Sins/dvl-based underwater anti-shaking alignment method for deep-sea underwater vehicle
Xiong et al. Robust GPS/INS/DVL navigation and positioning method using adaptive federated strong tracking filter based on weighted least square principle
CN110823217A (en) Integrated navigation fault-tolerant method based on self-adaptive federal strong tracking filtering
CN105737832A (en) Distributed SLAM (simultaneous localization and mapping) method on basis of global optimal data fusion
Li et al. An underwater integrated navigation algorithm to deal with DVL malfunctions based on deep learning
CN108444476B (en) Polar region collaborative navigation method of multi-underwater unmanned vehicle considering underwater acoustic communication delay
Xu et al. A novel measurement information anomaly detection method for cooperative localization
CN115307643A (en) Double-responder assisted SINS/USBL combined navigation method
Wiktor et al. Collaborative multi-robot localization in natural terrain
Du et al. An adaptive interactive multi-model navigation method based on UUV
Huang et al. Expectation maximization based GPS/INS integration for land-vehicle navigation
XU et al. Low-cost multi-AUV cooperative localization method based on dual-model
Yuan et al. Reaearch on underwater integrated navigation system based on SINS/DVL/magnetometer/depth-sensor
CN114577211A (en) Factor graph-based master-slave AUV (autonomous Underwater vehicle) cooperative positioning method considering ocean current influence
CN111964684B (en) SINS/LBL (strapdown inertial navigation system/location based language) tight combination-based underwater navigation hybrid positioning method and system
Zhao et al. A single beacon-aided cooperative localization algorithm based on maximum correntropy criterion
CN114111840A (en) DVL error parameter online calibration method based on integrated navigation
Ning et al. Research on fast calibration and moving base alignment of SINS/DVL integrated navigation system
Li et al. Research on the algorithm of multi-autonomous underwater vehicles navigation and localization based on the extended kalman filter
Zhang et al. Cooperative navigation and localization for multiple UUVs

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